Fixed a bug where camera facing polygons were not textured

Tango: Added Append Mode option (only start a new map after being localized, default true) and the old map is still shown for convenience until localization. Increased minInliers to 25. Increased ICP correspondenceRatio to 0.5. Decreased time to open a database. Saving doesn't close the database anymore and overwrite only if we save with the same name. Removed ICP refining from standard optimization.

.gitignore

@@ -0,0 +1,7 @@
+/lib
+.DS_Store
+.settings/language.settings.xml
+app/android/.classpath
+app/android/.project
+app/android/AndroidManifest.xml
+app/android/res/raw/

.travis.yml

@@ -0,0 +1,31 @@
+sudo: true
+dist: trusty
+language: cpp
+
+compiler:
+  - gcc
+  - clang
+
+addons:
+  apt:
+    packages:
+      - cmake
+      - libopencv-dev
+      - libqt4-dev
+      - libsqlite3-dev
+
+install:
+    - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list'
+    - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
+    - sudo apt-get update
+    - sudo apt-get -y install libpcl-1.7-all libfreenect-dev ros-indigo-libg2o ros-indigo-octomap libopenni2-dev
+
+script: 
+    - source /opt/ros/indigo/setup.bash
+    - mkdir -p build && cd build
+    - cmake .. 
+    - make
+
+notifications:
+  email:
+    - [email protected]

CMakeLists.txt

@@ -6,9 +6,10 @@ SET(PROJECT_PREFIX rtabmap)
 
 # Catkin doesn't support multiarch library path, 
 # fix to "lib" if not set by user.
-IF(NOT DEFINED CMAKE_INSTALL_LIBDIR)
+OPTION(MULTI_ARCH "Activate multi-arch lib directory (debian)" OFF)
+IF(NOT MULTI_ARCH AND NOT DEFINED CMAKE_INSTALL_LIBDIR)
    set(CMAKE_INSTALL_LIBDIR "lib")
-ENDIF(NOT DEFINED CMAKE_INSTALL_LIBDIR)
+ENDIF(NOT MULTI_ARCH AND NOT DEFINED CMAKE_INSTALL_LIBDIR)
 
 INCLUDE(GNUInstallDirs)
 
@@ -19,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 # VERSION
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
-SET(RTABMAP_MINOR_VERSION 11)
+SET(RTABMAP_MINOR_VERSION 12)
 SET(RTABMAP_PATCH_VERSION 2)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
@@ -32,8 +33,6 @@ SET(PROJECT_VERSION_PATCH ${RTABMAP_PATCH_VERSION})
 
 SET(PROJECT_SOVERSION "${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR}")
 
-SET(RTABMAP_QT_VERSION 4 CACHE STRING "Which QT version to use")
-
 ####### COMPILATION PARAMS #######
 # In case of Makefiles if the user does not setup CMAKE_BUILD_TYPE, assume it's Release:
 IF(${CMAKE_GENERATOR} MATCHES ".*Makefiles")
@@ -54,6 +53,13 @@ ELSE ()
 	ADD_DEFINITIONS("-Wno-unknown-pragmas")
 ENDIF()
 
+if(POLICY CMP0020)
+	cmake_policy(SET CMP0020 OLD)
+endif()
+if(POLICY CMP0043)
+	cmake_policy(SET CMP0043 OLD)
+endif()
+
 IF(MINGW)
     # Hide the --enable-auto-import warning
 	SET(CMAKE_EXE_LINKER_FLAGS "-Wl,--enable-auto-import")  
@@ -126,8 +132,11 @@ OPTION(BUILD_TOOLS "Build tools" ON)
 OPTION(BUILD_EXAMPLES "Build examples" ON)
 
 ####### DEPENDENCIES #######
-
+IF(ANDROID)
+option(WITH_QT            "Include Qt support"                   OFF)
+ELSE()
 option(WITH_QT            "Include Qt support"                   ON)
+ENDIF()
 option(WITH_FREENECT      "Include Freenect support"             ON)
 option(WITH_FREENECT2     "Include Freenect2 support"            ON)
 option(WITH_OPENNI2       "Include OpenNI2 support"              ON)
@@ -138,11 +147,20 @@ option(WITH_TORO          "Include TORO support"                 ON)
 option(WITH_VERTIGO       "Include Vertigo support"              ON)
 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_OCTOMAP       "Include Octomap 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 "")
+   list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4")
+endif()
+
 # OpenMP ("-fopenmp" should be added for flann included in PCL)
 # the gcc-4.2.1 coming with MacOS X is not compatible with the OpenMP pragmas we use, so disabling OpenMP for it
 if((NOT APPLE) OR (NOT CMAKE_COMPILER_IS_GNUCXX) OR (GCC_VERSION VERSION_GREATER 4.2.1) OR (CMAKE_CXX_COMPILER_ID STREQUAL "Clang"))
@@ -152,6 +170,9 @@ if(OPENMP_FOUND)
   set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
   message (STATUS "Found OpenMP")
+  if(PCL_OMP)
+    add_definitions(-DPCL_OMP)
+  endif(PCL_OMP)
 else(OPENMP_FOUND)
   message (STATUS "Not found OpenMP")
 endif()
@@ -166,18 +187,36 @@ IF(ZLIB_FOUND)
    MESSAGE(STATUS "Found ZLIB: ${ZLIB_INCLUDE_DIRS}")
 ENDIF(ZLIB_FOUND)
 
+SET(ADD_VTK_GUI_SUPPORT_QT_TO_CONF FALSE)
 IF(WITH_QT)
-# If Qt is here, the GUI will be built
+    FIND_PACKAGE(VTK)
-    IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
+    IF(NOT VTK_FOUND)
-        FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg)
+        MESSAGE(FATAL_ERROR "VTK is required when using Qt. Set -DWITH_QT=OFF if you don't want gui tools.")
-    ELSE()
+    ENDIF(NOT VTK_FOUND)
+
+    # If Qt is here, the GUI will be built
+    # look for Qt5 (if vtk>5 is installed) before Qt4
+    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)
-    ENDIF()
+        ENDIF(Qt5_FOUND)
+    ENDIF("${VTK_MAJOR_VERSION}" GREATER 5)
+
+    IF(NOT Qt5_FOUND)
+        FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg)
+    ENDIF(NOT Qt5_FOUND)
+        
     IF(QT4_FOUND OR Qt5_FOUND)
-        FIND_PACKAGE(VTK REQUIRED)
         IF("${VTK_MAJOR_VERSION}" EQUAL 5)
             FIND_PACKAGE(QVTK REQUIRED) # only for VTK 5
-        ENDIF("${VTK_MAJOR_VERSION}" EQUAL 5)
+        ELSE()
+            list(FIND PCL_LIBRARIES vtkGUISupportQt value)
+            IF(value EQUAL -1)
+                SET(PCL_LIBRARIES "${PCL_LIBRARIES};vtkGUISupportQt")
+                SET(ADD_VTK_GUI_SUPPORT_QT_TO_CONF TRUE)
+            ENDIF(value EQUAL -1)
+        ENDIF()
     ENDIF(QT4_FOUND OR Qt5_FOUND)
 ENDIF(WITH_QT)
 
@@ -195,15 +234,24 @@ IF(WITH_FREENECT2)
 	      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)
     ENDIF(freenect2_FOUND)
 ENDIF(WITH_FREENECT2)
 
-IF(WITH_OPENNI2)
+# IF PCL depends on OpenNI2 (already found), ignore WITH_OPENNI2
+IF(WITH_OPENNI2 OR OpenNI2_FOUND)
     FIND_PACKAGE(OpenNI2 QUIET)
     IF(OpenNI2_FOUND)
        MESSAGE(STATUS "Found OpenNI2: ${OpenNI2_INCLUDE_DIRS}")
     ENDIF(OpenNI2_FOUND)
-ENDIF(WITH_OPENNI2)
+ENDIF(WITH_OPENNI2 OR OpenNI2_FOUND)
 
 IF(WITH_DC1394)
     FIND_PACKAGE(DC1394 QUIET)
@@ -223,22 +271,6 @@ IF(WITH_GTSAM)
     FIND_PACKAGE(GTSAM QUIET)
 ENDIF(WITH_GTSAM)
 
-IF(G2O_FOUND OR GTSAM_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()
-   ENDIF()
-ENDIF(G2O_FOUND OR GTSAM_FOUND)
-
 IF(WITH_FLYCAPTURE2)
     FIND_PACKAGE(FlyCapture2 QUIET)
     IF(FlyCapture2_FOUND)
@@ -253,6 +285,65 @@ IF(WITH_CVSBA)
     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(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()
+    ENDIF(ZED_FOUND)
+ENDIF(WITH_ZED)
+
+IF(WITH_REALSENSE)
+	FIND_PACKAGE(RealSense QUIET)
+    IF(RealSense_FOUND)
+       MESSAGE(STATUS "Found RealSense: ${RealSense_INCLUDE_DIRS}")
+    ENDIF(RealSense_FOUND)
+ENDIF(WITH_REALSENSE)
+
+IF(WITH_OCTOMAP)
+	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)
+   #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()
+   ENDIF()
+ENDIF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND)
+
 ####### OSX BUNDLE CMAKE_INSTALL_PREFIX #######
 IF(APPLE AND BUILD_AS_BUNDLE)
   IF(Qt5_FOUND OR (QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND))
@@ -282,15 +373,24 @@ ENDIF(APPLE AND BUILD_AS_BUNDLE)
 
 
 ####### SOURCES (Projects) #######
+
+# CONF_DEPENDENCIES contains only dependencies not required by the headers
+SET(CONF_DEPENDENCIES 
+	${ZLIB_LIBRARIES}
+)
 IF(NOT (OPENCV_NONFREE_FOUND OR OPENCV_XFEATURES2D_FOUND))
    SET(NONFREE "//")
 ENDIF(NOT (OPENCV_NONFREE_FOUND OR OPENCV_XFEATURES2D_FOUND))
 IF(NOT G2O_FOUND)
    SET(G2O "//")
-ENDIF(NOT G2O_FOUND)
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${G2O_LIBRARIES})
+ENDIF()
 IF(NOT GTSAM_FOUND)
    SET(GTSAM "//")
-ENDIF(NOT GTSAM_FOUND)
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${GTSAM_LIBRARIES})
+ENDIF()
 IF(NOT WITH_TORO)
    SET(TORO "//")
 ENDIF(NOT WITH_TORO)
@@ -299,22 +399,55 @@ IF(NOT WITH_VERTIGO)
 ENDIF(NOT WITH_VERTIGO)
 IF(NOT cvsba_FOUND)
    SET(CVSBA "//")
-ENDIF(NOT cvsba_FOUND)
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${cvsba_LIBRARIES})
+ENDIF()
 IF(NOT Freenect_FOUND)
    SET(FREENECT "//")
-ENDIF(NOT Freenect_FOUND)
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${Freenect_LIBRARIES})
+ENDIF()
 IF(NOT freenect2_FOUND)
    SET(FREENECT2 "//")
-ENDIF(NOT freenect2_FOUND)
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${freenect2_LIBRARIES})
+ENDIF()
 IF(NOT OpenNI2_FOUND)
    SET(OPENNI2 "//")
-ENDIF(NOT OpenNI2_FOUND)
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${OpenNI2_LIBRARIES})
+ENDIF()
 IF(NOT DC1394_FOUND)
    SET(DC1394 "//")
-ENDIF(NOT DC1394_FOUND)
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${DC1394_LIBRARIES})
+ENDIF()
 IF(NOT FlyCapture2_FOUND)
    SET(FLYCAPTURE2 "//")
-ENDIF(NOT FlyCapture2_FOUND)
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${FlyCapture2_LIBRARIES})
+ENDIF()
+IF(NOT ZED_FOUND)
+   SET(ZED "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${ZED_LIBRARIES} ${CUDA_LIBRARIES}) 
+ENDIF()
+IF(NOT RealSense_FOUND)
+   SET(REALSENSE "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${RealSense_LIBRARIES}) 
+ENDIF()
+IF(NOT OCTOMAP_FOUND)
+   SET(OCTOMAP "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${OCTOMAP_LIBRARIES}) 
+ENDIF()
+IF(ADD_VTK_GUI_SUPPORT_QT_TO_CONF)
+   SET(CONF_VTK_QT true)
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} vtkGUISupportQt)
+ELSE()
+   SET(CONF_VTK_QT false)
+ENDIF()
 IF(NOT (OpenCV_FOUND AND OpenCV_VERSION_MAJOR EQUAL 3))
    SET(OPENCV3 "//")
 ENDIF(NOT (OpenCV_FOUND AND OpenCV_VERSION_MAJOR EQUAL 3))
@@ -365,9 +498,14 @@ file(RELATIVE_PATH REL_LIB_DIR "${CMAKE_INSTALL_PREFIX}/${INSTALL_CMAKE_DIR}" "$
 set(CONF_INCLUDE_DIRS "${PROJECT_SOURCE_DIR}/corelib/include" 
 					  "${PROJECT_SOURCE_DIR}/guilib/include" 
 					  "${PROJECT_SOURCE_DIR}/utilite/include")
-set(CONF_LIB_DIR "${CMAKE_ARCHIVE_OUTPUT_DIRECTORY}")
+set(CONF_LIB_DIR "${CMAKE_ARCHIVE_OUTPUT_DIRECTORY} ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}")
 IF(QT4_FOUND OR Qt5_FOUND)
    set(CONF_WITH_GUI ON)
+   IF(QT4_FOUND)
+      set(CONF_QT_VERSION 4)
+   ELSE()
+      set(CONF_QT_VERSION 5)
+   ENDIF()
 ELSE()
    set(CONF_WITH_GUI OFF)
 ENDIF()
@@ -479,6 +617,7 @@ MESSAGE(STATUS "Info :")
 MESSAGE(STATUS "  Version : ${RTABMAP_VERSION}")
 MESSAGE(STATUS "  CMAKE_INSTALL_PREFIX = ${CMAKE_INSTALL_PREFIX}")
 MESSAGE(STATUS "  CMAKE_BUILD_TYPE =     ${CMAKE_BUILD_TYPE}")
+MESSAGE(STATUS "  CMAKE_INSTALL_LIBDIR = ${CMAKE_INSTALL_LIBDIR}")
 MESSAGE(STATUS "  BUILD_APP =            ${BUILD_APP}")
 MESSAGE(STATUS "  BUILD_TOOLS =          ${BUILD_TOOLS}")
 MESSAGE(STATUS "  BUILD_EXAMPLES =       ${BUILD_EXAMPLES}")
@@ -583,6 +722,34 @@ ELSE()
 MESSAGE(STATUS "  With cvsba                = NO (cvsba not found)")
 ENDIF()
 
+IF(ZED_FOUND)
+IF(CUDA_FOUND)
+MESSAGE(STATUS "  With ZED                  = YES (With CUDA)")
+ELSE()
+MESSAGE(STATUS "  With ZED                  = YES (Without CUDA)")
+ENDIF()
+ELSEIF(NOT WITH_ZED)
+MESSAGE(STATUS "  With ZED                  = NO (WITH_ZED=OFF)")
+ELSE()
+MESSAGE(STATUS "  With ZED                  = NO (ZED sdk not found)")
+ENDIF()
+
+IF(RealSense_FOUND)
+MESSAGE(STATUS "  With RealSense            = YES (License: Apache-2)")
+ELSEIF(NOT WITH_REALSENSE)
+MESSAGE(STATUS "  With RealSense            = NO (WITH_REALSENSE=OFF)")
+ELSE()
+MESSAGE(STATUS "  With RealSense            = NO (librealsense not found)")
+ENDIF()
+
+IF(OCTOMAP_FOUND)
+MESSAGE(STATUS "  With OCTOMAP              = YES (License: BSD)")
+ELSEIF(NOT WITH_OCTOMAP)
+MESSAGE(STATUS "  With OCTOMAP              = NO (WITH_OCTOMAP=OFF)")
+ELSE()
+MESSAGE(STATUS "  With OCTOMAP              = NO (octomap not found)")
+ENDIF()
+
 IF(QT4_FOUND)
 MESSAGE(STATUS "  With Qt4                  = YES (License: Open Source or Commercial)")
 ELSEIF(Qt5_FOUND)
@@ -590,7 +757,7 @@ MESSAGE(STATUS "  With Qt5                  = YES (License: Open Source or Comme
 ELSEIF(NOT WITH_QT)
 MESSAGE(STATUS "  With Qt                   = NO (WITH_QT=OFF)")
 ELSE()
-MESSAGE(STATUS "  With Qt                   = NO (Qt not found, to use Qt5 you should set -DRTABMAP_QT_VERSION=5)")
+MESSAGE(STATUS "  With Qt                   = NO (Qt not found)")
 ENDIF()
 
 MESSAGE(STATUS "--------------------------------------------")

LICENSE

@@ -1,4 +1,4 @@
-Copyright (c) 2010-2015, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

Matlab/ShowLogs/showlogs.m

@@ -106,14 +106,14 @@ else
 end
 ylabel('Time (s)')
 xlabel('Node indexes')
-meanTime = mean(LogF(:,1))*1000
+meanTimeMS = mean(LogF(:,1))*1000
 plot([1 length(LogF(:,1))], [0.7 0.7], 'r')
 plot([1 length(LogF(:,1))], [1 1], 'k')
 %plot([1 length(LogF(:,1))], [350 350], 'r')
 %legend('Processing time', 'Time limit')%, 'Acquisition rate (1 Hz)')
 %title('Processing time')
 
-maxTime = max(sum(LogF(:,2:7),2)+LogF(:,17))
+maxTimeMS = max(sum(LogF(:,2:7),2)+LogF(:,17))*1000
 maxDict = max(LogI(:, 6))
 maxWM = max(LogI(:,7))
 %%

README.md

@@ -1,4 +1,4 @@
-rtabmap
+rtabmap [![Build Status](https://travis-ci.org/introlab/rtabmap.svg?branch=master)](https://travis-ci.org/introlab/rtabmap)
 =======
 
 RTAB-Map library and standalone application.

RTABMapConfig.cmake.in

@@ -11,41 +11,41 @@ get_filename_component(RTABMap_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH)
 set(RTABMap_INCLUDE_DIRS "@CONF_INCLUDE_DIRS@")
 
 #core lib
-find_library(RTABMap_CORE_RELEASE NAMES rtabmap_core NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@")
+find_library(RTABMap_CORE_RELEASE NAMES rtabmap_core NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
-find_library(RTABMap_CORE_DEBUG NAMES rtabmap_cored NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@")
+find_library(RTABMap_CORE_DEBUG NAMES rtabmap_cored NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
 
 IF(RTABMap_CORE_DEBUG AND RTABMap_CORE_RELEASE)
    SET(RTABMap_CORE
       debug ${RTABMap_CORE_DEBUG}
       optimized ${RTABMap_CORE_RELEASE}
    )
-ELSEIF(RTABMap_CORE_RELEASE)
-   SET(RTABMap_CORE ${RTABMap_CORE_RELEASE})
 ELSEIF(RTABMap_CORE_DEBUG)
    SET(RTABMap_CORE ${RTABMap_CORE_DEBUG})
+ELSE()
+   SET(RTABMap_CORE ${RTABMap_CORE_RELEASE})
 ENDIF()
 
 #utilite lib
-find_library(RTABMap_UTILITE_RELEASE NAMES rtabmap_utilite NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@")
+find_library(RTABMap_UTILITE_RELEASE NAMES rtabmap_utilite NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
-find_library(RTABMap_UTILITE_DEBUG NAMES rtabmap_utilited NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@")
+find_library(RTABMap_UTILITE_DEBUG NAMES rtabmap_utilited NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
 
 IF(RTABMap_UTILITE_DEBUG AND RTABMap_UTILITE_RELEASE)
    SET(RTABMap_UTILITE
       debug ${RTABMap_UTILITE_DEBUG}
       optimized ${RTABMap_UTILITE_RELEASE}
    )
-ELSEIF(RTABMap_UTILITE_RELEASE)
-   SET(RTABMap_UTILITE ${RTABMap_UTILITE_RELEASE})
 ELSEIF(RTABMap_UTILITE_DEBUG)
    SET(RTABMap_UTILITE ${RTABMap_UTILITE_DEBUG})
+ELSE()
+   SET(RTABMap_UTILITE ${RTABMap_UTILITE_RELEASE})
 ENDIF()
 
 set(RTABMap_LIBRARIES ${RTABMap_CORE} ${RTABMap_UTILITE})
 
 #gui lib (OFF if RTAB-Map is not built with Qt)
 if(@CONF_WITH_GUI@)
-   find_library(RTABMap_GUI_RELEASE NAMES rtabmap_gui NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@")
+   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@")
+   find_library(RTABMap_GUI_DEBUG NAMES rtabmap_guid NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
    
    IF(RTABMap_GUI_DEBUG AND RTABMap_GUI_RELEASE)
       SET(RTABMap_GUI
@@ -61,13 +61,16 @@ if(@CONF_WITH_GUI@)
    set(RTABMap_LIBRARIES ${RTABMap_LIBRARIES} ${RTABMap_GUI})
 endif(@CONF_WITH_GUI@)
 
+# Dependencies
+if(@CONF_VTK_QT@)
+   find_package(VTK COMPONENTS vtkGUISupportQt NO_MODULE) # to define vtkGUISupportQt target
+endif(@CONF_VTK_QT@)
+set(RTABMap_LIBRARIES ${RTABMap_LIBRARIES} @CONF_DEPENDENCIES@)
+
 #backward compatibilities
-if(RTABMap_CORE)
+set(RTABMAP_CORE ${RTABMap_CORE})
-   set(RTABMAP_CORE ${RTABMap_CORE})
+set(RTABMAP_UTILITE ${RTABMap_UTILITE})
-endif(RTABMap_CORE)
-if(RTABMap_UTILITE)
-   set(RTABMAP_UTILITE ${RTABMap_UTILITE})
-endif(RTABMap_UTILITE)
 if(RTABMap_GUI)
    set(RTABMAP_GUI ${RTABMap_GUI})
+   set(RTABMAP_QT_VERSION @CONF_QT_VERSION@)
 endif(RTABMap_GUI)

Version.h.in

@@ -49,6 +49,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @CVSBA@#define RTABMAP_CVSBA
 @DC1394@#define RTABMAP_DC1394
 @FLYCAPTURE2@#define RTABMAP_FLYCAPTURE2
+@ZED@#define RTABMAP_ZED
+@REALSENSE@#define RTABMAP_REALSENSE
+@OCTOMAP@#define RTABMAP_OCTOMAP
 
 #endif /* VERSION_H_ */
 

app/android/.gitignore

@@ -0,0 +1 @@
+/bin/

app/android/.settings/org.eclipse.jdt.core.prefs

@@ -0,0 +1,11 @@
+eclipse.preferences.version=1
+org.eclipse.jdt.core.compiler.codegen.inlineJsrBytecode=enabled
+org.eclipse.jdt.core.compiler.codegen.targetPlatform=1.6
+org.eclipse.jdt.core.compiler.codegen.unusedLocal=preserve
+org.eclipse.jdt.core.compiler.compliance=1.6
+org.eclipse.jdt.core.compiler.debug.lineNumber=generate
+org.eclipse.jdt.core.compiler.debug.localVariable=generate
+org.eclipse.jdt.core.compiler.debug.sourceFile=generate
+org.eclipse.jdt.core.compiler.problem.assertIdentifier=error
+org.eclipse.jdt.core.compiler.problem.enumIdentifier=error
+org.eclipse.jdt.core.compiler.source=1.6

app/android/AndroidManifest.xml.in

@@ -2,31 +2,35 @@
 <!-- BEGIN_INCLUDE(manifest) -->
 <manifest xmlns:android="http://schemas.android.com/apk/res/android"
         package="com.introlab.rtabmap"
-        android:versionCode="1"
+        android:versionCode="39"
-        android:versionName="0.11.2">
+        android:versionName="@RTABMAP_VERSION@">
 
     <uses-permission android:name="android.permission.CAMERA" />
     <uses-permission android:name="android.permission.READ_EXTERNAL_STORAGE" />
     <uses-permission android:name="android.permission.WRITE_EXTERNAL_STORAGE" />
     <uses-permission android:name="android.permission.READ_FRAME_BUFFER" />
     <uses-permission android:name="android.permission.ACCESS_SURFACE_FLINGER" />
+    <uses-permission android:name="android.permission.INTERNET" />
+    <uses-permission android:name="android.permission.ACCESS_NETWORK_STATE" />
     <uses-feature android:glEsVersion="0x00020000" />
-    <uses-library android:name="com.projecttango.libtango_device" android:required="true" />
 
     <!-- This is the platform API where NativeActivity was introduced. -->
-    <uses-sdk android:minSdkVersion="17" />
+    <uses-sdk android:minSdkVersion="19" />
 
     <!-- This .apk has no Java code itself, so set hasCode to false. -->
     <application 
         android:label="@string/app_name" 
         android:icon="@drawable/ic_launcher">
         
+        <uses-library android:name="com.projecttango.libtango_device2" android:required="true" />
+
         <!-- Our activity is the built-in NativeActivity framework class.
              This will take care of integrating with our NDK code. -->
         <activity android:name="RTABMapActivity"
                 android:label="@string/app_name"
                 android:launchMode="singleTask"
-                android:screenOrientation="nosensor">
+		android:screenOrientation="fullSensor"
+		android:configChanges="orientation|screenSize|keyboardHidden">
             <!-- Tell NativeActivity the name of our .so -->
             <meta-data android:name="android.app.lib_name"
                     android:value="NativeRTABMap" />
@@ -35,6 +39,9 @@
                 <category android:name="android.intent.category.LAUNCHER" />
             </intent-filter>
         </activity>
+
+        <activity android:name="SettingsActivity" android:label="@string/settings" android:screenOrientation="fullSensor"/>
+        <activity android:name="SketchfabActivity" android:label="@string/sketchfab" android:screenOrientation="fullSensor"/>
     </application>
 
 </manifest>

app/android/CMakeLists.txt

@@ -14,16 +14,30 @@ if(NOT ANDROID_EXECUTABLE)
   message(FATAL_ERROR "Can not find android command line tool: android")
 endif()
 
+configure_file(
+  "${CMAKE_CURRENT_SOURCE_DIR}/AndroidManifest.xml.in"
+  "${CMAKE_CURRENT_SOURCE_DIR}/AndroidManifest.xml")
+  
 configure_file(
   "${CMAKE_CURRENT_SOURCE_DIR}/AndroidManifest.xml"
   "${CMAKE_CURRENT_BINARY_DIR}/AndroidManifest.xml"
   COPYONLY)
   
+configure_file(
+  "${CMAKE_CURRENT_SOURCE_DIR}/info.txt.in"
+  "${CMAKE_CURRENT_SOURCE_DIR}/res/raw/info.txt")
+  
+
 configure_file(
   "${CMAKE_CURRENT_SOURCE_DIR}/ant.properties.in"
   "${CMAKE_CURRENT_BINARY_DIR}/ant.properties"
   @ONLY)
   
+configure_file(
+  "${CMAKE_CURRENT_SOURCE_DIR}/custom_rules.xml"
+  "${CMAKE_CURRENT_BINARY_DIR}/custom_rules.xml"
+  COPYONLY)
+
 add_custom_target(NativeRTABMap-ant-configure ALL
   COMMAND "${ANDROID_EXECUTABLE}"
           update project

app/android/ant.properties.in

@@ -1,5 +1,6 @@
 builddir=@CMAKE_CURRENT_BINARY_DIR@
 srcdir=@CMAKE_CURRENT_SOURCE_DIR@
+android.abi=@ANDROID_ABI@
 
 source.dir=${srcdir}/src
 gen.dir=${builddir}/gen

app/android/custom_rules.xml

@@ -0,0 +1,10 @@
+<project>
+    <target name="-pre-build">
+		<copy todir="${jar.libs.dir}">
+			<fileset dir="${srcdir}/libs" includes="**/*.jar" excludes="**/*sources.jar, **/*javadoc.jar" />
+		</copy>
+		<copy todir="${native.libs.dir}/${android.abi}">
+			<fileset dir="${srcdir}/jni/third-party/lib" includes="**/*.so"/>
+		</copy>
+	</target>
+</project>

app/android/info.txt.in

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

app/android/jni/CameraTango.cpp

@@ -29,22 +29,27 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "util.h"
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/core/util3d_transforms.h"
-#include "rtabmap/core/util2d.h"
 #include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/core/util2d.h"
 #include <tango_client_api.h>
+#include <tango_support_api.h>
 
 namespace rtabmap {
 
 #define nullptr 0
 const int kVersionStringLength = 128;
-const int holeSize = 10;
+const int holeSize = 5;
 const float maxDepthError = 0.10;
+const int scanDownsampling = 1;
 
 // Callbacks
-void onPointCloudAvailableRouter(void* context, const TangoXYZij* xyz_ij)
+void onPointCloudAvailableRouter(void* context, const TangoPointCloud* point_cloud)
 {
 	CameraTango* app = static_cast<CameraTango*>(context);
-	app->cloudReceived(cv::Mat(1, xyz_ij->xyz_count, CV_32FC3, xyz_ij->xyz[0]), xyz_ij->timestamp);
+	if(point_cloud->num_points>0)
+	{
+		app->cloudReceived(cv::Mat(1, point_cloud->num_points, CV_32FC4, point_cloud->points[0]), point_cloud->timestamp);
+	}
 }
 
 void onFrameAvailableRouter(void* context, TangoCameraId id, const TangoImageBuffer* color)
@@ -76,8 +81,11 @@ void onFrameAvailableRouter(void* context, TangoCameraId id, const TangoImageBuf
 
 void onPoseAvailableRouter(void* context, const TangoPoseData* pose)
 {
+	if(pose->status_code == TANGO_POSE_VALID)
+	{
 		CameraTango* app = static_cast<CameraTango*>(context);
-	app->poseReceived(app->tangoPoseToTransform(pose, true));
+		app->poseReceived(app->tangoPoseToTransform(pose));
+	}
 }
 
 void onTangoEventAvailableRouter(void* context, const TangoEvent* event)
@@ -86,21 +94,25 @@ void onTangoEventAvailableRouter(void* context, const TangoEvent* event)
 	app->tangoEventReceived(event->type, event->event_key, event->event_value);
 }
 
-// In OpenGL, axes are x->right, y->up and z->outScreen
-// Image is x->right, y->down and z->inScreen
-static rtabmap::Transform opticalRotation(
-		1.0f, 0.0f, 0.0f, 0.0f,
-		0.0f, -1.0f, 0.0f, 0.0f,
-		0.0f, 0.0f, -1.0f, 0.0f);
-
 //////////////////////////////
 // CameraTango
 //////////////////////////////
-CameraTango::CameraTango(int decimation, bool autoExposure) :
+const float CameraTango::bilateralFilteringSigmaS = 2.0f;
-		Camera(0, opticalRotation),
+const float CameraTango::bilateralFilteringSigmaR = 0.075f;
+
+CameraTango::CameraTango(int decimation, bool autoExposure, bool publishRawScan, bool smoothing) :
+		Camera(0),
 		tango_config_(0),
+		firstFrame_(true),
+		stampEpochOffset_(0.0),
 		decimation_(decimation),
-		autoExposure_(autoExposure)
+		autoExposure_(autoExposure),
+		rawScanPublished_(publishRawScan),
+		smoothing_(smoothing),
+		cloudStamp_(0),
+		tangoColorType_(0),
+		tangoColorStamp_(0),
+		colorCameraToDisplayRotation_(ROTATION_0)
 {
 	UASSERT(decimation >= 1);
 }
@@ -114,6 +126,8 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 {
 	close();
 
+	TangoSupport_initializeLibrary();
+
 	// Connect to Tango
 	LOGI("NativeRTABMap: Setup tango config");
 	tango_config_ = TangoService_getConfig(TANGO_CONFIG_DEFAULT);
@@ -125,8 +139,7 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 
 	// Set auto-recovery for motion tracking as requested by the user.
 	bool is_atuo_recovery = true;
-	int ret = TangoConfig_setBool(tango_config_, "config_enable_auto_recovery",
+	int ret = TangoConfig_setBool(tango_config_, "config_enable_auto_recovery", is_atuo_recovery);
-	is_atuo_recovery);
 	if (ret != TANGO_SUCCESS)
 	{
 		LOGE("NativeRTABMap: config_enable_auto_recovery() failed with error code: %d", ret);
@@ -141,13 +154,15 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 		return false;
 	}
 
-	// disable auto exposure
+	// 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;
+		//return false;
 	}
+	else
+	{
 		if(!autoExposure_)
 		{
 			ret = TangoConfig_setInt32(tango_config_, "config_color_iso", 800);
@@ -157,6 +172,13 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 				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.
 	ret = TangoConfig_setBool(tango_config_, "config_enable_depth", true);
@@ -166,6 +188,14 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 		return false;
 	}
 
+	// Need to specify the depth_mode as XYZC.
+	ret = TangoConfig_setInt32(tango_config_, "config_depth_mode", TANGO_POINTCLOUD_XYZC);
+	if (ret != TANGO_SUCCESS)
+	{
+		LOGE("Failed to set 'depth_mode' configuration flag with error code: %d", ret);
+		return false;
+	}
+
 	// Note that it's super important for AR applications that we enable low
 	// latency imu integration so that we have pose information available as
 	// quickly as possible. Without setting this flag, you'll often receive
@@ -173,15 +203,26 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 	ret = TangoConfig_setBool(tango_config_, "config_enable_low_latency_imu_integration", true);
 	if (ret != TANGO_SUCCESS)
 	{
-		LOGE("Failed to enable low latency imu integration.");
+		LOGE("NativeRTABMap: Failed to enable low latency imu integration.");
 		return false;
 	}
 
+	// Drift correction allows motion tracking to recover after it loses tracking.
+	//
+	// The drift corrected pose is is available through the frame pair with
+	// base frame AREA_DESCRIPTION and target frame DEVICE.
+	/*ret = TangoConfig_setBool(tango_config_, "config_enable_drift_correction", true);
+	if (ret != TANGO_SUCCESS) {
+		LOGE(
+			"NativeRTABMap: enabling config_enable_drift_correction "
+			"failed with error code: %d",
+			ret);
+		return false;
+	}*/
+
 	// Get TangoCore version string from service.
 	char tango_core_version[kVersionStringLength];
-	ret = TangoConfig_getString(
+	ret = TangoConfig_getString(tango_config_, "tango_service_library_version", tango_core_version, kVersionStringLength);
-	tango_config_, "tango_service_library_version",
-	tango_core_version, kVersionStringLength);
 	if (ret != TANGO_SUCCESS)
 	{
 		LOGE("NativeRTABMap: get tango core version failed with error code: %d", ret);
@@ -194,29 +235,30 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 	LOGI("NativeRTABMap: Setup callbacks");
 	// Attach the OnXYZijAvailable callback.
 	// The callback will be called after the service is connected.
-	ret = TangoService_connectOnXYZijAvailable(onPointCloudAvailableRouter);
+	ret = TangoService_connectOnPointCloudAvailable(onPointCloudAvailableRouter);
 	if (ret != TANGO_SUCCESS)
 	{
-		LOGE("PointCloudApp: Failed to connect to point cloud callback with error code: %d", ret);
+		LOGE("NativeRTABMap: Failed to connect to point cloud callback with error code: %d", ret);
 		return false;
 	}
 
 	ret = TangoService_connectOnFrameAvailable(TANGO_CAMERA_COLOR, this, onFrameAvailableRouter);
 	if (ret != TANGO_SUCCESS)
 	{
-		LOGE("PointCloudApp: Failed to connect to color callback with error code: %d", ret);
+		LOGE("NativeRTABMap: Failed to connect to color callback with error code: %d", ret);
 		return false;
 	}
 
 	// Attach the onPoseAvailable callback.
 	// The callback will be called after the service is connected.
 	TangoCoordinateFramePair pair;
+	//pair.base = TANGO_COORDINATE_FRAME_AREA_DESCRIPTION; // drift correction is enabled
 	pair.base = TANGO_COORDINATE_FRAME_START_OF_SERVICE;
 	pair.target = TANGO_COORDINATE_FRAME_DEVICE;
 	ret = TangoService_connectOnPoseAvailable(1, &pair, onPoseAvailableRouter);
 	if (ret != TANGO_SUCCESS)
 	{
-		LOGE("PointCloudApp: Failed to connect to pose callback with error code: %d", ret);
+		LOGE("NativeRTABMap: Failed to connect to pose callback with error code: %d", ret);
 		return false;
 	}
 
@@ -234,7 +276,7 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 	ret = TangoService_connect(this, tango_config_);
 	if (ret != TANGO_SUCCESS)
 	{
-		LOGE("PointCloudApp: Failed to connect to the Tango service with error code: %d", ret);
+		LOGE("NativeRTABMap: Failed to connect to the Tango service with error code: %d", ret);
 		return false;
 	}
 
@@ -247,16 +289,16 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 	// as well. We use timestamp 0.0 and the target frame pair to get the
 	// extrinsics from the sensors.
 	//
-		// Get device with respect to imu transformation matrix.
+	// Get color camera with respect to device transformation matrix.
-		frame_pair.base = TANGO_COORDINATE_FRAME_IMU;
+	frame_pair.base = TANGO_COORDINATE_FRAME_DEVICE;
-		frame_pair.target = TANGO_COORDINATE_FRAME_DEVICE;
+	frame_pair.target = TANGO_COORDINATE_FRAME_CAMERA_COLOR;
 	ret = TangoService_getPoseAtTime(0.0, frame_pair, &pose_data);
 	if (ret != TANGO_SUCCESS)
 	{
-			LOGE("PointCloudApp: Failed to get transform between the IMU frame and device frames");
+		LOGE("NativeRTABMap: Failed to get transform between the color camera frame and device frames");
 		return false;
 	}
-		imuTDevice_ = rtabmap::Transform(
+	deviceTColorCamera_ = rtabmap::Transform(
 			pose_data.translation[0],
 			pose_data.translation[1],
 			pose_data.translation[2],
@@ -265,32 +307,12 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 			pose_data.orientation[2],
 			pose_data.orientation[3]);
 
-		// Get color camera with respect to imu transformation matrix.
-		frame_pair.base = TANGO_COORDINATE_FRAME_IMU;
-		frame_pair.target = TANGO_COORDINATE_FRAME_CAMERA_DEPTH;
-		ret = TangoService_getPoseAtTime(0.0, frame_pair, &pose_data);
-		if (ret != TANGO_SUCCESS)
-		{
-			LOGE("PointCloudApp: Failed to get transform between the color camera frame and device frames");
-			return false;
-		}
-		imuTDepthCamera_ = rtabmap::Transform(
-				pose_data.translation[0],
-				pose_data.translation[1],
-				pose_data.translation[2],
-				pose_data.orientation[0],
-				pose_data.orientation[1],
-				pose_data.orientation[2],
-				pose_data.orientation[3]);
-
-		deviceTDepth_ = imuTDevice_.inverse() * imuTDepthCamera_;
-
 	// camera intrinsic
 	TangoCameraIntrinsics color_camera_intrinsics;
 	ret = TangoService_getCameraIntrinsics(TANGO_CAMERA_COLOR, &color_camera_intrinsics);
 	if (ret != TANGO_SUCCESS)
 	{
-			LOGE("SynchronizationApplication: Failed to get the intrinsics for the color camera with error code: %d.", ret);
+		LOGE("NativeRTABMap: Failed to get the intrinsics for the color camera with error code: %d.", ret);
 		return false;
 	}
 	model_ = CameraModel(
@@ -301,11 +323,13 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 			this->getLocalTransform());
 	model_.setImageSize(cv::Size(color_camera_intrinsics.width, color_camera_intrinsics.height));
 
-		// optical rotation
+	// device to camera optical rotation in rtabmap frame
-		model_.setLocalTransform(Transform(
+	model_.setLocalTransform(tango_device_T_rtabmap_device.inverse()*deviceTColorCamera_);
-				0.0f, 0.0f, 1.0f, 0.0f,
+
-				-1.0f, 0.0f, 0.0f, 0.0f,
+	LOGI("deviceTColorCameraTango  =%s", deviceTColorCamera_.prettyPrint().c_str());
-				0.0f, -1.0f, 0.0f, 0.0f));
+	LOGI("deviceTColorCameraRtabmap=%s", (tango_device_T_rtabmap_device.inverse()*deviceTColorCamera_).prettyPrint().c_str());
+
+	cameraStartedTime_.restart();
 
 	return true;
 }
@@ -318,63 +342,76 @@ void CameraTango::close()
 		tango_config_ = nullptr;
 		TangoService_disconnect();
 	}
+	firstFrame_ = true;
 }
 
 void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
 {
-	if(this->isRunning())
+	if(this->isRunning() && !cloud.empty())
 	{
-		UASSERT(cloud.type() == CV_32FC3);
+		//LOGD("Depth received! %fs (%d points)", timestamp, cloud.cols);
+
+		UASSERT(cloud.type() == CV_32FC4);
 		boost::mutex::scoped_lock  lock(dataMutex_);
 
+		// From post: http://stackoverflow.com/questions/29236110/timing-issues-with-tango-image-frames
+		// "In the current version of Project Tango Tablet RGB IR camera
+		//  is used for both depth and color images and it can only do one
+		//  or the other for each frame. So in the stream we get 4 RGB frames
+		//  followed by 1 Depth frame resulting in the pattern you observed. This
+		//  is more of a hardware limitation."
+		//
+		//  So, synchronize with the last RGB frame before the Depth is acquired
+		if(!tangoColor_.empty())
+		{
+			double dt = fabs(timestamp - tangoColorStamp_);
+
+			//LOGD("Depth: %f vs %f = %f", tangoColorStamp_, timestamp, dt);
+
+			if(dt >= 0.0 && dt < 0.5)
+			{
 				bool notify = cloud_.empty();
 				cloud_ = cloud.clone();
 				cloudStamp_ = timestamp;
-		LOGD("Depth received! (%d points)", cloud.cols);
+				if(notify)
-		if(!tangoColor_.empty() && notify)
 				{
-			LOGD("Cloud: Release semaphore");
+					//LOGD("Cloud: Release semaphore");
 					dataReady_.release();
 				}
 			}
+		}
+	}
 }
 
 void CameraTango::rgbReceived(const cv::Mat & tangoImage, int type, double timestamp)
 {
-	if(this->isRunning())
+	if(this->isRunning() && !tangoImage.empty())
 	{
+		//LOGD("RGB received! %fs", timestamp);
+
 		boost::mutex::scoped_lock  lock(dataMutex_);
 
-		if(!cloud_.empty())
-		{
-			if(!tangoImage.empty())
-			{
-				bool notify = tangoColor_.empty();
 		tangoColor_ = tangoImage.clone();
 		tangoColorStamp_ = timestamp;
 		tangoColorType_ = type;
-				LOGD("RGB received!");
-				if(!cloud_.empty() && notify)
-				{
-					LOGD("RGB: Release semaphore");
-					dataReady_.release();
-				}
-			}
-		}
 	}
 }
 
+static rtabmap::Transform opticalRotationTango(
+								1.0f,  0.0f,  0.0f, 0.0f,
+							    0.0f, -1.0f,  0.0f, 0.0f,
+								0.0f,  0.0f, -1.0f, 0.0f);
 void CameraTango::poseReceived(const Transform & pose)
 {
 	if(!pose.isNull() && pose.getNormSquared() < 100000)
 	{
-		this->post(new PoseEvent(pose));
+		// send pose of the camera (without optical rotation), not the device
+		this->post(new PoseEvent(pose*deviceTColorCamera_*opticalRotationTango));
 	}
 }
 
 void CameraTango::tangoEventReceived(int type, const char * key, const char * value)
 {
-	LOGE("Tango event: %s:%s", key, value);
 	this->post(new CameraTangoEvent(type, key, value));
 }
 
@@ -388,12 +425,11 @@ std::string CameraTango::getSerial() const
 	return "Tango";
 }
 
-rtabmap::Transform CameraTango::tangoPoseToTransform(const TangoPoseData * tangoPose, bool inOpenGLFrame) const
+rtabmap::Transform CameraTango::tangoPoseToTransform(const TangoPoseData * tangoPose) const
 {
 	UASSERT(tangoPose);
 	rtabmap::Transform pose;
-	if(!deviceTDepth_.isNull())
+
-	{
 	pose = rtabmap::Transform(
 			tangoPose->translation[0],
 			tangoPose->translation[1],
@@ -403,27 +439,13 @@ rtabmap::Transform CameraTango::tangoPoseToTransform(const TangoPoseData * tango
 			tangoPose->orientation[2],
 			tangoPose->orientation[3]);
 
-		// transform in OpenGL + extrinsics
-		// opengl_world_T_opengl_camera =
-		//      opengl_world_T_start_service *
-		//      start_service_T_device *
-		//      device_T_imu *
-		//      imu_T_depth_camera *
-		//      depth_camera_T_opengl_camera;
-		if(inOpenGLFrame)
-		{
-			pose = opengl_world_T_tango_world * pose * deviceTDepth_ * depth_camera_T_opengl_camera;
-		}
-	}
-
 	return pose;
 }
 
-rtabmap::Transform CameraTango::getPoseAtTimestamp(double timestamp, bool inOpenGLFrame)
+rtabmap::Transform CameraTango::getPoseAtTimestamp(double timestamp)
 {
 	rtabmap::Transform pose;
-	if(!deviceTDepth_.isNull())
+
-	{
 	TangoPoseData pose_start_service_T_device;
 	TangoCoordinateFramePair frame_pair;
 	frame_pair.base = TANGO_COORDINATE_FRAME_START_OF_SERVICE;
@@ -446,17 +468,15 @@ rtabmap::Transform CameraTango::getPoseAtTimestamp(double timestamp, bool inOpen
 	else
 	{
 
-			pose = tangoPoseToTransform(&pose_start_service_T_device, inOpenGLFrame);
+		pose = tangoPoseToTransform(&pose_start_service_T_device);
 	}
 
-
-	}
 	return pose;
 }
 
-SensorData CameraTango::captureImage()
+SensorData CameraTango::captureImage(CameraInfo * info)
 {
-	LOGI("Capturing image...");
+	//LOGI("Capturing image...");
 
 	SensorData data;
 	if(!dataReady_.acquire(1, 2000))
@@ -534,70 +554,180 @@ SensorData CameraTango::captureImage()
 		// Querying the depth image's frame transformation based on the depth image's
 		// timestamp.
 		cv::Mat depth;
-		Transform poseDepth = getPoseAtTimestamp(cloudStamp, false);
-		Transform poseColor = getPoseAtTimestamp(rgbStamp, false);
 
-		if(poseColor.getNormSquared() > 100000)
+		// Calculate the relative pose from color camera frame at timestamp
+		// color_timestamp t1 and depth
+		// camera frame at depth_timestamp t0.
+		Transform colorToDepth;
+		TangoPoseData pose_color_image_t1_T_depth_image_t0;
+		if (TangoSupport_calculateRelativePose(
+				rgbStamp, TANGO_COORDINATE_FRAME_CAMERA_COLOR, cloudStamp,
+			  TANGO_COORDINATE_FRAME_CAMERA_DEPTH,
+			  &pose_color_image_t1_T_depth_image_t0) == TANGO_SUCCESS)
 		{
-			LOGE("Very large odometry color pose detected (%s)! Ignoring this frame!", poseColor.prettyPrint().c_str());
+			colorToDepth = tangoPoseToTransform(&pose_color_image_t1_T_depth_image_t0);
-			poseColor.setNull();
 		}
-		if(poseDepth.getNormSquared() > 100000)
+		else
 		{
-			LOGE("Very large odometry depth pose detected (%s)! Ignoring this frame!", poseDepth.prettyPrint().c_str());
+			LOGE(
-			poseDepth.setNull();
+				"SynchronizationApplication: Could not find a valid relative pose at "
+				"time for color and "
+				" depth cameras.");
 		}
 
-		if(!poseDepth.isNull() && !poseColor.isNull())
+		if(colorToDepth.getNormSquared() > 100000)
+		{
+			LOGE("Very large color to depth error detected (%s)! Ignoring this frame!", colorToDepth.prettyPrint().c_str());
+			colorToDepth.setNull();
+		}
+		cv::Mat scan;
+		if(!colorToDepth.isNull())
 		{
 			// The Color Camera frame at timestamp t0 with respect to Depth
 			// Camera frame at timestamp t1.
-			Transform colorToDepth = deviceTDepth_.inverse() * poseColor.inverse() * poseDepth * deviceTDepth_;
+			//LOGD("colorToDepth=%s", colorToDepth.prettyPrint().c_str());
-			LOGI("colorToDepth=%s", colorToDepth.prettyPrint().c_str());
+			LOGD("rgb=%dx%d cloud size=%d", rgb.cols, rgb.rows, (int)cloud.total());
 
 			int pixelsSet = 0;
 			depth = cv::Mat::zeros(model_.imageHeight()/8, model_.imageWidth()/8, CV_16UC1); // mm
 			CameraModel depthModel = model_.scaled(1.0f/8.0f);
+			std::vector<cv::Point3f> scanData(rawScanPublished_?cloud.total():0);
+			int oi=0;
 			for(unsigned int i=0; i<cloud.total(); ++i)
 			{
-				cv::Vec3f & p = cloud.at<cv::Vec3f>(i);
+				float * p = cloud.ptr<float>(0,i);
 				cv::Point3f pt = util3d::transformPoint(cv::Point3f(p[0], p[1], p[2]), colorToDepth);
 
-				int pixel_x, pixel_y;
+				if(pt.z > 0.0f && i%scanDownsampling == 0 && rawScanPublished_)
+				{
+					scanData.at(oi++) = pt;
+				}
+
+				int pixel_x_l, pixel_y_l, pixel_x_h, pixel_y_h;
 				// get the coordinate on image plane.
-				pixel_x = static_cast<int>((depthModel.fx()) * (pt.x / pt.z) + depthModel.cx());
+				pixel_x_l = static_cast<int>((depthModel.fx()) * (pt.x / pt.z) + depthModel.cx());
-				pixel_y = static_cast<int>((depthModel.fy()) * (pt.y / pt.z) + depthModel.cy());
+				pixel_y_l = static_cast<int>((depthModel.fy()) * (pt.y / pt.z) + depthModel.cy());
+				pixel_x_h = static_cast<int>((depthModel.fx()) * (pt.x / pt.z) + depthModel.cx() + 0.5f);
+				pixel_y_h = static_cast<int>((depthModel.fy()) * (pt.y / pt.z) + depthModel.cy() + 0.5f);
 				unsigned short depth_value(pt.z * 1000.0f);
 
-				if(pixel_x>=0 && pixel_x<depth.cols &&
+				bool pixelSet = false;
-				   pixel_y>0 && pixel_y<depth.rows &&
+				if(pixel_x_l>=0 && pixel_x_l<depth.cols &&
+				   pixel_y_l>0 && pixel_y_l<depth.rows &&
 				   depth_value)
 				{
-					unsigned short & depthPixel = depth.at<unsigned short>(pixel_y, pixel_x);
+					unsigned short & depthPixel = depth.at<unsigned short>(pixel_y_l, pixel_x_l);
 					if(depthPixel == 0 || depthPixel > depth_value)
 					{
 						depthPixel = depth_value;
+						pixelSet = true;
+					}
+				}
+				if(pixel_x_h>=0 && pixel_x_h<depth.cols &&
+				   pixel_y_h>0 && pixel_y_h<depth.rows &&
+				   depth_value)
+				{
+					unsigned short & depthPixel = depth.at<unsigned short>(pixel_y_h, pixel_x_h);
+					if(depthPixel == 0 || depthPixel > depth_value)
+					{
+						depthPixel = depth_value;
+						pixelSet = true;
+					}
+				}
+				if(pixelSet)
+				{
 					pixelsSet += 1;
 				}
 			}
+
+			if(oi)
+			{
+				scan = cv::Mat(1, oi, CV_32FC3, scanData.data()).clone();
 			}
-			LOGI("pixels depth set= %d", pixelsSet);
+			//LOGD("pixels depth set= %d", pixelsSet);
 		}
 		else
 		{
-			LOGE("Poses are null?!? color=%d (stamp=%f) depth=%d (stamp=%f)", poseColor.isNull()?0:1, rgbStamp, poseDepth.isNull()?0:1, cloudStamp);
+			LOGE("color to depth pose is null?!? (rgb stamp=%f) (depth stamp=%f)", rgbStamp, cloudStamp);
 		}
 
 		if(!rgb.empty() && !depth.empty())
 		{
 			depth = rtabmap::util2d::fillDepthHoles(depth, holeSize, maxDepthError);
 
-			Transform poseColorOpenGL = getPoseAtTimestamp(rgbStamp, true);
+			Transform poseDevice = getPoseAtTimestamp(rgbStamp);
+
+			//LOGD("Local    = %s", model.localTransform().prettyPrint().c_str());
+			//LOGD("tango    = %s", poseDevice.prettyPrint().c_str());
+			//LOGD("opengl(t)= %s", (opengl_world_T_tango_world * poseDevice).prettyPrint().c_str());
 
 			//Rotate in RTAB-Map's coordinate
-			Transform odom = rtabmap_world_T_opengl_world * poseColorOpenGL * depth_camera_T_opengl_camera * model.localTransform().inverse();
+			Transform odom = rtabmap_world_T_tango_world * poseDevice * tango_device_T_rtabmap_device;
 
+			//LOGD("rtabmap  = %s", odom.prettyPrint().c_str());
+			//LOGD("opengl(r)= %s", (opengl_world_T_rtabmap_world * odom * rtabmap_device_T_opengl_device).prettyPrint().c_str());
+
+			// Rotate image depending on the camera orientation
+			if(colorCameraToDisplayRotation_ == ROTATION_90)
+			{
+				cv::Mat rgbt(rgb.cols, rgb.rows, rgb.type());
+				cv::flip(rgb,rgb,1);
+				cv::transpose(rgb,rgbt);
+				rgb = rgbt;
+				cv::Mat deptht(depth.cols, depth.rows, depth.type());
+				cv::flip(depth,depth,1);
+				cv::transpose(depth,deptht);
+				depth = deptht;
+				cv::Size sizet(model.imageHeight(), model.imageWidth());
+				model = CameraModel(model.fy(), model.fx(), model.cy(), model.cx()>0?model.imageWidth()-model.cx():0, model.localTransform()*rtabmap::Transform(0,0,0,0,0,1.57079632679489661923132169163975144));
+				model.setImageSize(sizet);
+			}
+			else if(colorCameraToDisplayRotation_ == ROTATION_180)
+			{
+				cv::flip(rgb,rgb,1);
+				cv::flip(rgb,rgb,0);
+				cv::flip(depth,depth,1);
+				cv::flip(depth,depth,0);
+				cv::Size sizet(model.imageWidth(), model.imageHeight());
+				model = CameraModel(
+						model.fx(),
+						model.fy(),
+						model.cx()>0?model.imageWidth()-model.cx():0,
+						model.cy()>0?model.imageHeight()-model.cy():0,
+						model.localTransform()*rtabmap::Transform(0,0,0,0,0,1.57079632679489661923132169163975144*2.0));
+				model.setImageSize(sizet);
+			}
+			else if(colorCameraToDisplayRotation_ == ROTATION_270)
+			{
+				cv::Mat rgbt(rgb.cols, rgb.rows, rgb.type());
+				cv::transpose(rgb,rgbt);
+				cv::flip(rgbt,rgbt,1);
+				rgb = rgbt;
+				cv::Mat deptht(depth.cols, depth.rows, depth.type());
+				cv::transpose(depth,deptht);
+				cv::flip(deptht,deptht,1);
+				depth = deptht;
+				cv::Size sizet(model.imageHeight(), model.imageWidth());
+				model = CameraModel(model.fy(), model.fx(), model.cy()>0?model.imageHeight()-model.cy():0, model.cx(), model.localTransform()*rtabmap::Transform(0,0,0,0,0,-1.57079632679489661923132169163975144));
+				model.setImageSize(sizet);
+			}
+
+			if(smoothing_)
+			{
+				//UTimer t;
+				depth = rtabmap::util2d::fastBilateralFiltering(depth, bilateralFilteringSigmaS, bilateralFilteringSigmaR);
+				data.setDepthOrRightRaw(depth);
+				//LOGD("Bilateral filtering, time=%fs", t.ticks());
+			}
+
+			if(rawScanPublished_)
+			{
+				data = SensorData(scan, LaserScanInfo(cloud.total()/scanDownsampling, 0, model.localTransform()), rgb, depth, model, this->getNextSeqID(), rgbStamp);
+			}
+			else
+			{
 				data = SensorData(rgb, depth, model, this->getNextSeqID(), rgbStamp);
+			}
 			data.setGroundTruth(odom);
 		}
 		else
@@ -611,7 +741,11 @@ SensorData CameraTango::captureImage()
 
 void CameraTango::mainLoopBegin()
 {
-	uSleep(2000); // just to make sure that the camera is started
+	double t = cameraStartedTime_.elapsed();
+	if(t < 5.0)
+	{
+		uSleep((5.0-t)*1000); // just to make sure that the camera is started
+	}
 }
 
 void CameraTango::mainLoop()
@@ -624,7 +758,15 @@ void CameraTango::mainLoop()
 		{
 			rtabmap::Transform pose = data.groundTruth();
 			data.setGroundTruth(Transform());
-			this->post(new OdometryEvent(data, pose, 0.0001, 0.0001));
+			// convert stamp to epoch
+			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;
 		}
 		else if(!this->isKilled())
 		{

app/android/jni/CameraTango.h

@@ -34,7 +34,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UEventsSender.h>
 #include <rtabmap/utilite/UThread.h>
 #include <rtabmap/utilite/UEvent.h>
+#include <rtabmap/utilite/UTimer.h>
 #include <boost/thread/mutex.hpp>
+#include <tango_support_api.h>
 
 class TangoPoseData;
 
@@ -69,15 +71,24 @@ private:
 
 class CameraTango : public Camera, public UThread, public UEventsSender {
 public:
-	CameraTango(int decimation, bool autoExposure);
+	static const float bilateralFilteringSigmaS;
+	static const float bilateralFilteringSigmaR;
+
+public:
+	CameraTango(int decimation, bool autoExposure, bool publishRawScan, bool smoothing);
 	virtual ~CameraTango();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
 	void close(); // close Tango connection
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
-	rtabmap::Transform tangoPoseToTransform(const TangoPoseData * tangoPose, bool inOpenGLFrame) const;
+	const CameraModel & getCameraModel() const {return model_;}
+	rtabmap::Transform tangoPoseToTransform(const TangoPoseData * tangoPose) const;
+	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;}
 
 	void cloudReceived(const cv::Mat & cloud, double timestamp);
 	void rgbReceived(const cv::Mat & tangoImage, int type, double timestamp);
@@ -85,18 +96,23 @@ public:
 	void tangoEventReceived(int type, const char * key, const char * value);
 
 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);
 
 private:
-	rtabmap::Transform getPoseAtTimestamp(double timestamp, bool inOpenGLFrame);
+	rtabmap::Transform getPoseAtTimestamp(double timestamp);
 
 	virtual void mainLoopBegin();
 	virtual void mainLoop();
 
 private:
 	void * tango_config_;
+	bool firstFrame_;
+	UTimer cameraStartedTime_;
+	double stampEpochOffset_;
 	int decimation_;
 	bool autoExposure_;
+	bool rawScanPublished_;
+	bool smoothing_;
 	cv::Mat cloud_;
 	double cloudStamp_;
 	cv::Mat tangoColor_;
@@ -104,10 +120,9 @@ private:
 	double tangoColorStamp_;
 	boost::mutex dataMutex_;
 	USemaphore dataReady_;
-	rtabmap::Transform imuTDevice_;
-	rtabmap::Transform imuTDepthCamera_;
-	rtabmap::Transform deviceTDepth_;
 	CameraModel model_;
+	Transform deviceTColorCamera_;
+	TangoSupportRotation colorCameraToDisplayRotation_;
 };
 
 } /* namespace rtabmap */

app/android/jni/ProgressionStatus.h

@@ -0,0 +1,134 @@
+/*
+ * ProgressionStatus.h
+ *
+ *  Created on: Feb 28, 2017
+ *      Author: mathieu
+ */
+
+#ifndef APP_ANDROID_JNI_PROGRESSIONSTATUS_H_
+#define APP_ANDROID_JNI_PROGRESSIONSTATUS_H_
+
+#include <rtabmap/core/ProgressState.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UEventsManager.h>
+#include <jni.h>
+
+namespace rtabmap {
+
+class ProgressEvent : public UEvent
+{
+public:
+	ProgressEvent(int count = 1) : count_(count){}
+	virtual std::string getClassName() const {return "ProgressEvent";}
+
+	int count_;
+};
+
+class ProgressionStatus: public ProgressState, public UEventsHandler
+{
+public:
+	ProgressionStatus() : count_(0), max_(100), canceled_(false), jvm_(0), rtabmap_(0)
+	{
+		registerToEventsManager();
+	}
+
+	void setJavaObjects(JavaVM * jvm, jobject rtabmap)
+	{
+		jvm_ = jvm;
+		rtabmap_ = rtabmap;
+	}
+
+	void reset(int max)
+	{
+		count_=-1;
+		max_ = max;
+		canceled_ = false;
+
+		increment();
+	}
+
+	void setMax(int max)
+	{
+		max_ = max;
+	}
+	int getMax() const {return max_;}
+
+	void increment(int count = 1) const
+	{
+		UEventsManager::post(new ProgressEvent(count));
+	}
+
+	void finish()
+	{
+
+	}
+
+	virtual bool callback(const std::string & msg) const
+	{
+		if(!canceled_)
+		{
+			increment();
+		}
+
+		return !canceled_;
+	}
+	virtual ~ProgressionStatus(){}
+
+	void cancel()
+	{
+		canceled_ = true;
+	}
+
+	bool isCanceled() const
+	{
+		return canceled_;
+	}
+
+protected:
+	virtual void handleEvent(UEvent * event)
+	{
+		if(event->getClassName().compare("ProgressEvent") == 0)
+		{
+			count_ += ((ProgressEvent*)event)->count_;
+			// Call JAVA callback
+			bool success = false;
+			if(jvm_ && rtabmap_)
+			{
+				JNIEnv *env = 0;
+				jint rs = jvm_->AttachCurrentThread(&env, NULL);
+				if(rs == JNI_OK && env)
+				{
+					jclass clazz = env->GetObjectClass(rtabmap_);
+					if(clazz)
+					{
+						jmethodID methodID = env->GetMethodID(clazz, "updateProgressionCallback", "(II)V" );
+						if(methodID)
+						{
+							env->CallVoidMethod(rtabmap_, methodID,
+									count_,
+									max_);
+							success = true;
+						}
+					}
+				}
+				jvm_->DetachCurrentThread();
+			}
+			if(!success)
+			{
+				UERROR("Failed to call rtabmap::updateProgressionCallback");
+			}
+		}
+	}
+
+private:
+	int count_;
+	int max_;
+	bool canceled_;
+	JavaVM *jvm_;
+	jobject rtabmap_;
+};
+
+}
+
+
+#endif /* APP_ANDROID_JNI_PROGRESSIONSTATUS_H_ */

app/android/jni/RTABMapApp.h

@@ -34,14 +34,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <tango_client_api.h>  // NOLINT
 #include <tango-gl/util.h>
 
-#include <scene.h>
+#include "scene.h"
-#include <CameraTango.h>
+#include "CameraTango.h"
-#include <util.h>
+#include "util.h"
+#include "ProgressionStatus.h"
 
 #include <rtabmap/core/RtabmapThread.h>
 #include <rtabmap/utilite/UEventsHandler.h>
 #include <boost/thread/mutex.hpp>
 #include <pcl/pcl_base.h>
+#include <pcl/TextureMesh.h>
 
 // RTABMapApp handles the application lifecycle and resources.
 class RTABMapApp : public UEventsHandler {
@@ -50,14 +52,13 @@ class RTABMapApp : public UEventsHandler {
   RTABMapApp();
   ~RTABMapApp();
 
-  // Initialize the Tango Service, this function starts the communication
+  void onCreate(JNIEnv* env, jobject caller_activity);
-  // between the application and the Tango Service.
-  // The activity object is used for checking if the API version is outdated.
-  int TangoInitialize(JNIEnv* env, jobject caller_activity);
 
-  void openDatabase(const std::string & databasePath);
+  void setScreenRotation(int displayRotation, int cameraRotation);
 
-  int onResume();
+  int openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize);
+
+  bool onTangoServiceConnected(JNIEnv* env, jobject iBinder);
 
   // Explicitly reset motion tracking and restart the pipeline.
   // Note that this will cause motion tracking to re-initialize.
@@ -115,40 +116,104 @@ class RTABMapApp : public UEventsHandler {
   void setPausedMapping(bool paused);
   void setMapCloudShown(bool shown);
   void setOdomCloudShown(bool shown);
-  void setMeshRendering(bool enabled);
+  void setMeshRendering(bool enabled, bool withTexture);
+  void setPointSize(float value);
+  void setLighting(bool enabled);
+  void setBackfaceCulling(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 setFullResolution(bool enabled);
+  void setSmoothing(bool enabled);
+  void setAppendMode(bool enabled);
+  void setDataRecorderMode(bool enabled);
+  void setMaxCloudDepth(float value);
+  void setMeshDecimation(int value);
+  void setMeshAngleTolerance(float value);
+  void setMeshTriangleSize(int value);
+  void setMinClusterSize(int value);
+  void setMaxGainRadius(float value);
+  int setMappingParameter(const std::string & key, const std::string & value);
 
   void resetMapping();
-  void save();
+  void save(const std::string & databasePath);
-  bool exportMesh(const std::string & filePath);
+  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 normalK,
+		  float maxTextureDistance,
+		  bool optimized,
+		  float optimizedVoxelSize,
+		  int optimizedDepth,
+		  int optimizedMaxPolygons,
+		  float optimizedColorRadius,
+		  bool optimizedCleanWhitePolygons,
+		  bool optimizedColorWhitePolygons,
+		  bool blockRendering);
+  bool postExportation(bool visualize);
+  int postProcessing(int approach);
 
  protected:
   virtual void handleEvent(UEvent * event);
 
  private:
   rtabmap::ParametersMap getRtabmapParameters();
+  bool smoothMesh(int id, Mesh & mesh);
 
  private:
   rtabmap::CameraTango * camera_;
   rtabmap::RtabmapThread * rtabmapThread_;
+  rtabmap::Rtabmap * rtabmap_;
   LogHandler * logHandler_;
 
-  bool mapCloudShown_;
   bool odomCloudShown_;
-  bool loopClosureDetection_;
   bool graphOptimization_;
+  bool nodesFiltering_;
   bool localizationMode_;
   bool trajectoryMode_;
   bool autoExposure_;
+  bool rawScanSaved_;
+  bool smoothing_;
+  bool fullResolution_;
+  bool appendMode_;
+  float maxCloudDepth_;
+  int meshDecimation_;
+  int meshTrianglePix_;
+  float meshAngleToleranceDeg_;
+  int minClusterSize_;
+  float maxGainRadius_;
 
+  rtabmap::ParametersMap mappingParameters_;
+
+  bool paused_;
+  bool dataRecorderMode_;
   bool clearSceneOnNextRender_;
+  bool optimizeOpenedDatabase_;
+  bool filterPolygonsOnNextRender_;
+  int gainCompensationOnNextRender_;
+  bool bilateralFilteringOnNextRender_;
+  bool cameraJustInitialized_;
   int totalPoints_;
   int totalPolygons_;
   int lastDrawnCloudsCount_;
+  float renderingTime_;
+  long processMemoryUsedBytes;
+  long processGPUMemoryUsedBytes;
+
+  bool visualizingMesh_;
+  bool exportedMeshUpdated_;
+  pcl::TextureMesh::Ptr exportedMesh_;
+  cv::Mat exportedTexture_;
 
   // main_scene_ includes all drawable object for visualizing Tango device's
   // movement and point cloud.
@@ -158,16 +223,20 @@ class RTABMapApp : public UEventsHandler {
 	std::list<rtabmap::OdometryEvent> odomEvents_;
 	std::list<rtabmap::Transform> poseEvents_;
 
+	rtabmap::Transform mapToOdom_;
+
 	boost::mutex rtabmapMutex_;
 	boost::mutex meshesMutex_;
 	boost::mutex odomMutex_;
 	boost::mutex poseMutex_;
+	boost::mutex renderingMutex_;
 
-	std::map<int, std::pair<std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, std::vector<pcl::Vertices> >, rtabmap::Transform > > createdMeshes_;
+	std::map<int, Mesh> createdMeshes_;
 	std::map<int, rtabmap::Transform> rawPoses_;
-	std::pair<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::IndicesPtr> > previousCloud_;
 
 	std::pair<rtabmap::RtabmapEventInit::Status, std::string> status_;
+
+	rtabmap::ProgressionStatus progressionStatus_;
 };
 
 #endif  // TANGO_POINT_CLOUD_POINT_CLOUD_APP_H_

app/android/jni/jni_interface.cpp

@@ -48,26 +48,33 @@ void GetJStringContent(JNIEnv *AEnv, jstring AStr, std::string &ARes) {
   AEnv->ReleaseStringUTFChars(AStr,s);
 }
 
-JNIEXPORT jint JNICALL
+JNIEXPORT void JNICALL
-Java_com_introlab_rtabmap_RTABMapLib_initialize(
+Java_com_introlab_rtabmap_RTABMapLib_onCreate(
     JNIEnv* env, jobject, jobject activity)
 {
-	return app.TangoInitialize(env, activity);
+	return app.onCreate(env, activity);
 }
 
 JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setScreenRotation(
+    JNIEnv* env, jobject, int displayRotation, int cameraRotation)
+{
+	return app.setScreenRotation(displayRotation, cameraRotation);
+}
+
+JNIEXPORT int JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_openDatabase(
-    JNIEnv* env, jobject, jstring databasePath)
+    JNIEnv* env, jobject, jstring databasePath, bool databaseInMemory, bool optimize)
 {
 	std::string databasePathC;
 	GetJStringContent(env,databasePath,databasePathC);
-	return app.openDatabase(databasePathC);
+	return app.openDatabase(databasePathC, databaseInMemory, optimize);
 }
 
-JNIEXPORT jint JNICALL
+JNIEXPORT bool JNICALL
-Java_com_introlab_rtabmap_RTABMapLib_onResume(
+Java_com_introlab_rtabmap_RTABMapLib_onTangoServiceConnected(
-    JNIEnv*, jobject) {
+		JNIEnv* env, jobject, jobject iBinder) {
-  return app.onResume();
+  return app.onTangoServiceConnected(env, iBinder);
 }
 
 JNIEXPORT void JNICALL
@@ -133,9 +140,27 @@ Java_com_introlab_rtabmap_RTABMapLib_setOdomCloudShown(
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setMeshRendering(
+		JNIEnv*, jobject, bool enabled, bool withTexture)
+{
+	return app.setMeshRendering(enabled, withTexture);
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setPointSize(
+		JNIEnv*, jobject, float value)
+{
+	return app.setPointSize(value);
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setLighting(
 		JNIEnv*, jobject, bool enabled)
 {
-	return app.setMeshRendering(enabled);
+	return app.setLighting(enabled);
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setBackfaceCulling(
+		JNIEnv*, jobject, bool enabled)
+{
+	return app.setBackfaceCulling(enabled);
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setLocalizationMode(
@@ -156,17 +181,104 @@ Java_com_introlab_rtabmap_RTABMapLib_setGraphOptimization(
 	return app.setGraphOptimization(enabled);
 }
 JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setNodesFiltering(
+		JNIEnv*, jobject, bool enabled)
+{
+	return app.setNodesFiltering(enabled);
+}
+JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setGraphVisible(
 		JNIEnv*, jobject, bool visible)
 {
 	return app.setGraphVisible(visible);
 }
 JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setGridVisible(
+		JNIEnv*, jobject, bool visible)
+{
+	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)
+{
+	return app.setRawScanSaved(enabled);
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setFullResolution(
+		JNIEnv*, jobject, bool enabled)
+{
+	return app.setFullResolution(enabled);
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setSmoothing(
+		JNIEnv*, jobject, bool enabled)
+{
+	return app.setSmoothing(enabled);
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setAppendMode(
+		JNIEnv*, jobject, bool enabled)
+{
+	return app.setAppendMode(enabled);
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setDataRecorderMode(
+		JNIEnv*, jobject, bool enabled)
+{
+	return app.setDataRecorderMode(enabled);
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setMaxCloudDepth(
+		JNIEnv*, jobject, float value)
+{
+	return app.setMaxCloudDepth(value);
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setMeshDecimation(
+		JNIEnv*, jobject, int value)
+{
+	return app.setMeshDecimation(value);
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setMeshAngleTolerance(
+		JNIEnv*, jobject, float value)
+{
+	return app.setMeshAngleTolerance(value);
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setMeshTriangleSize(
+		JNIEnv*, jobject, int value)
+{
+	return app.setMeshTriangleSize(value);
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setMinClusterSize(
+		JNIEnv*, jobject, int value)
+{
+	return app.setMinClusterSize(value);
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setMaxGainRadius(
+		JNIEnv*, jobject, float value)
+{
+	return app.setMaxGainRadius(value);
+}
+JNIEXPORT jint JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setMappingParameter(
+    JNIEnv* env, jobject, jstring key, jstring value)
+{
+	std::string keyC, valueC;
+	GetJStringContent(env,key,keyC);
+	GetJStringContent(env,value,valueC);
+	return app.setMappingParameter(keyC, valueC);
+}
 
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_resetMapping(
@@ -177,20 +289,75 @@ Java_com_introlab_rtabmap_RTABMapLib_resetMapping(
 
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_save(
+		JNIEnv* env, jobject, jstring databasePath)
+{
+	std::string databasePathC;
+	GetJStringContent(env,databasePath,databasePathC);
+	return app.save(databasePathC);
+}
+
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_cancelProcessing(
 		JNIEnv* env, jobject)
 {
-	return app.save();
+	return app.cancelProcessing();
 }
 
 JNIEXPORT bool JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_exportMesh(
-		JNIEnv* env, jobject, jstring filePath)
+		JNIEnv* env, jobject,
+		jstring filePath,
+		float cloudVoxelSize,
+		bool regenerateCloud,
+		bool meshing,
+		int textureSize,
+		int normalK,
+		float maxTextureDistance,
+		bool optimized,
+		float optimizedVoxelSize,
+		int optimizedDepth,
+		int optimizedMaxPolygons,
+		float optimizedColorRadius,
+		bool optimizedCleanWhitePolygons,
+		bool optimizedColorWhitePolygons,
+		bool blockRendering)
 {
 	std::string filePathC;
 	GetJStringContent(env,filePath,filePathC);
-	return app.exportMesh(filePathC);
+	return app.exportMesh(
+			filePathC,
+			cloudVoxelSize,
+			regenerateCloud,
+			meshing,
+			textureSize,
+			normalK,
+			maxTextureDistance,
+			optimized,
+			optimizedVoxelSize,
+			optimizedDepth,
+			optimizedMaxPolygons,
+			optimizedColorRadius,
+			optimizedCleanWhitePolygons,
+			optimizedColorWhitePolygons,
+			blockRendering);
 }
 
+JNIEXPORT bool JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_postExportation(
+		JNIEnv* env, jobject, bool visualize)
+{
+	return app.postExportation(visualize);
+}
+
+JNIEXPORT int JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_postProcessing(
+		JNIEnv* env, jobject, int approach)
+{
+	return app.postProcessing(approach);
+}
+
+
+
 #ifdef __cplusplus
 }
 #endif

app/android/jni/point_cloud_drawable.cpp

@@ -1,143 +1,74 @@
-
 /*
- * Copyright 2014 Google Inc. All Rights Reserved.
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
- *
+All rights reserved.
- * Licensed under the Apache License, Version 2.0 (the "License");
+
- * you may not use this file except in compliance with the License.
+Redistribution and use in source and binary forms, with or without
- * You may obtain a copy of the License at
+modification, are permitted provided that the following conditions are met:
- *
+    * Redistributions of source code must retain the above copyright
- *      http://www.apache.org/licenses/LICENSE-2.0
+      notice, this list of conditions and the following disclaimer.
- *
+    * Redistributions in binary form must reproduce the above copyright
- * Unless required by applicable law or agreed to in writing, software
+      notice, this list of conditions and the following disclaimer in the
- * distributed under the License is distributed on an "AS IS" BASIS,
+      documentation and/or other materials provided with the distribution.
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+    * Neither the name of the Universite de Sherbrooke nor the
- * See the License for the specific language governing permissions and
+      names of its contributors may be used to endorse or promote products
- * limitations under the License.
+      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 <sstream>
 
 #include "point_cloud_drawable.h"
 #include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UConversion.h"
+#include <opencv2/imgproc/imgproc.hpp>
 #include "util.h"
 
 #include <GLES2/gl2.h>
 
 PointCloudDrawable::PointCloudDrawable(
-		GLuint shaderProgram,
+		GLuint cloudShaderProgram,
-		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		GLuint textureShaderProgram,
-		const std::vector<pcl::Vertices> & indices) :
-		vertex_buffers_(0),
-		nPoints_(0),
-		pose_(1.0f),
-		visible_(true),
-		shader_program_(shaderProgram)
-{
-	UASSERT(!cloud->empty());
-
-	glGenBuffers(1, &vertex_buffers_);
-
-	if(vertex_buffers_)
-	{
-		LOGI("Creating cloud buffer %d", vertex_buffers_);
-		std::vector<float> vertices = std::vector<float>(cloud->size()*4);
-		for(unsigned int i=0; i<cloud->size(); ++i)
-		{
-			vertices[i*4] = cloud->at(i).x;
-			vertices[i*4+1] = cloud->at(i).y;
-			vertices[i*4+2] = cloud->at(i).z;
-			vertices[i*4+3] = cloud->at(i).rgb;
-		}
-
-		glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
-		glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * (int)vertices.size(), (const void *)vertices.data(), GL_STATIC_DRAW);
-		glBindBuffer(GL_ARRAY_BUFFER, 0);
-
-		GLint error = glGetError();
-		if(error != GL_NO_ERROR)
-		{
-			LOGI("OpenGL: Could not allocate point cloud (0x%x)\n", error);
-			vertex_buffers_ = 0;
-		}
-		else
-		{
-			nPoints_ = cloud->size();
-
-			if(indices.size())
-			{
-				int polygonSize = indices[0].vertices.size();
-				UASSERT(polygonSize == 3);
-				indices_.resize(indices.size() * polygonSize);
-				int oi = 0;
-				for(unsigned int i=0; i<indices.size(); ++i)
-				{
-					UASSERT((int)indices[i].vertices.size() == polygonSize);
-					for(int j=0; j<polygonSize; ++j)
-					{
-						indices_[oi++] = (unsigned short)indices[i].vertices[j];
-					}
-				}
-			}
-		}
-	}
-}
-PointCloudDrawable::PointCloudDrawable(
-		GLuint shaderProgram,
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		const std::vector<pcl::Vertices> & indices) :
+		const pcl::IndicesPtr & indices,
+		float gain) :
 		vertex_buffers_(0),
+		textures_(0),
 		nPoints_(0),
 		pose_(1.0f),
 		visible_(true),
-		shader_program_(shaderProgram)
+		hasNormals_(false),
+		cloud_shader_program_(cloudShaderProgram),
+		texture_shader_program_(textureShaderProgram),
+		gain_(1.0f)
 {
-	UASSERT(!cloud->empty());
+	updateCloud(cloud, indices, gain);
+}
 
-	glGenBuffers(1, &vertex_buffers_);
+PointCloudDrawable::PointCloudDrawable(
-
+		GLuint cloudShaderProgram,
-	if(vertex_buffers_)
+		GLuint textureShaderProgram,
-	{
+		const Mesh & mesh) :
-		LOGI("Creating cloud buffer %d", vertex_buffers_);
+		vertex_buffers_(0),
-		std::vector<float> vertices = std::vector<float>(cloud->size()*4);
+		textures_(0),
-		for(unsigned int i=0; i<cloud->size(); ++i)
+		nPoints_(0),
-		{
+		pose_(1.0f),
-			vertices[i*4] = cloud->at(i).x;
+		visible_(true),
-			vertices[i*4+1] = cloud->at(i).y;
+		hasNormals_(false),
-			vertices[i*4+2] = cloud->at(i).z;
+		cloud_shader_program_(cloudShaderProgram),
-			vertices[i*4+3] = cloud->at(i).rgb;
+		texture_shader_program_(textureShaderProgram),
-		}
+		gain_(1.0f)
-
+{
-		glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
+	updateMesh(mesh);
-		glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * (int)vertices.size(), (const void *)vertices.data(), GL_STATIC_DRAW);
-		glBindBuffer(GL_ARRAY_BUFFER, 0);
-
-		GLint error = glGetError();
-		if(error != GL_NO_ERROR)
-		{
-			LOGI("OpenGL: Could not allocate point cloud (0x%x)\n", error);
-			vertex_buffers_ = 0;
-		}
-		else
-		{
-			nPoints_ = cloud->size();
-
-			if(indices.size())
-			{
-				int polygonSize = indices[0].vertices.size();
-				UASSERT(polygonSize == 3);
-				indices_.resize(indices.size() * polygonSize);
-				int oi = 0;
-				for(unsigned int i=0; i<indices.size(); ++i)
-				{
-					UASSERT((int)indices[i].vertices.size() == polygonSize);
-					for(int j=0; j<polygonSize; ++j)
-					{
-						indices_[oi++] = (unsigned short)indices[i].vertices[j];
-					}
-				}
-			}
-		}
-	}
 }
 
 PointCloudDrawable::~PointCloudDrawable()
@@ -149,6 +80,355 @@ PointCloudDrawable::~PointCloudDrawable()
 		tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
 		vertex_buffers_ = 0;
 	}
+
+	if (textures_)
+	{
+		glDeleteTextures(1, &textures_);
+		tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
+		textures_ = 0;
+	}
+}
+
+void PointCloudDrawable::updatePolygons(const std::vector<pcl::Vertices> & polygons)
+{
+	LOGD("Update polygons");
+	polygons_.clear();
+	if(polygons.size() && organizedToDenseIndices_.size())
+	{
+		unsigned int polygonSize = polygons[0].vertices.size();
+		UASSERT(polygonSize == 3);
+		polygons_.resize(polygons.size() * polygonSize);
+		int oi = 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]);
+			}
+		}
+	}
+}
+
+void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, float gain)
+{
+	UASSERT(cloud.get() && !cloud->empty());
+	nPoints_ = 0;
+	polygons_.clear();
+	gain_ = gain;
+
+	if (vertex_buffers_)
+	{
+		glDeleteBuffers(1, &vertex_buffers_);
+		tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
+		vertex_buffers_ = 0;
+	}
+
+	if (textures_)
+	{
+		glDeleteTextures(1, &textures_);
+		tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
+		textures_ = 0;
+	}
+
+	glGenBuffers(1, &vertex_buffers_);
+	if(!vertex_buffers_)
+	{
+		LOGE("OpenGL: could not generate vertex buffers\n");
+		return;
+	}
+
+	LOGI("Creating cloud buffer %d", vertex_buffers_);
+	std::vector<float> vertices;
+	int totalPoints = 0;
+	if(indices.get() && indices->size())
+	{
+		totalPoints = indices->size();
+		vertices.resize(indices->size()*4);
+		for(unsigned int i=0; i<indices->size(); ++i)
+		{
+			vertices[i*4] = cloud->at(indices->at(i)).x;
+			vertices[i*4+1] = cloud->at(indices->at(i)).y;
+			vertices[i*4+2] = cloud->at(indices->at(i)).z;
+			vertices[i*4+3] = cloud->at(indices->at(i)).rgb;
+		}
+	}
+	else
+	{
+		totalPoints = cloud->size();
+		vertices.resize(cloud->size()*4);
+		for(unsigned int i=0; i<cloud->size(); ++i)
+		{
+			vertices[i*4] = cloud->at(i).x;
+			vertices[i*4+1] = cloud->at(i).y;
+			vertices[i*4+2] = cloud->at(i).z;
+			vertices[i*4+3] = cloud->at(i).rgb;
+		}
+	}
+
+	glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
+	glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * (int)vertices.size(), (const void *)vertices.data(), GL_STATIC_DRAW);
+	glBindBuffer(GL_ARRAY_BUFFER, 0);
+
+	GLint error = glGetError();
+	if(error != GL_NO_ERROR)
+	{
+		LOGE("OpenGL: Could not allocate point cloud (0x%x)\n", error);
+		vertex_buffers_ = 0;
+		return;
+	}
+
+	nPoints_ = totalPoints;
+}
+
+void PointCloudDrawable::updateMesh(const Mesh & mesh)
+{
+	UASSERT(mesh.cloud.get() && !mesh.cloud->empty());
+	nPoints_ = 0;
+
+	if (vertex_buffers_)
+	{
+		glDeleteBuffers(1, &vertex_buffers_);
+		tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
+		vertex_buffers_ = 0;
+	}
+
+	gain_ = mesh.gain;
+
+	bool textureUpdate = false;
+	if(!mesh.texture.empty() && mesh.texture.type() == CV_8UC3)
+	{
+		if (textures_)
+		{
+			glDeleteTextures(1, &textures_);
+			tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
+			textures_ = 0;
+		}
+		textureUpdate = true;
+	}
+
+	glGenBuffers(1, &vertex_buffers_);
+	if(!vertex_buffers_)
+	{
+		LOGE("OpenGL: could not generate vertex buffers\n");
+		return;
+	}
+
+	if(textureUpdate)
+	{
+		glGenTextures(1, &textures_);
+		if(!textures_)
+		{
+			vertex_buffers_ = 0;
+			LOGE("OpenGL: could not generate texture buffers\n");
+			return;
+		}
+	}
+
+	//LOGD("Creating cloud buffer %d", vertex_buffers_);
+	std::vector<float> vertices;
+	int totalPoints = 0;
+	std::vector<pcl::Vertices> polygons = mesh.polygons;
+	hasNormals_ = mesh.normals.get() && mesh.normals->size() == mesh.cloud->size();
+	UASSERT(!hasNormals_ || mesh.cloud->size() == mesh.normals->size());
+	if(mesh.cloud->isOrganized()) // assume organized mesh
+	{
+		organizedToDenseIndices_ = std::vector<unsigned int>(mesh.cloud->width*mesh.cloud->height, -1);
+		totalPoints = mesh.indices->size();
+		if(textures_ && polygons.size())
+		{
+			//LOGD("Organized mesh with texture");
+			int items = hasNormals_?9:6;
+			vertices = std::vector<float>(mesh.indices->size()*9);
+			for(unsigned int i=0; i<mesh.indices->size(); ++i)
+			{
+				vertices[i*items] = mesh.cloud->at(mesh.indices->at(i)).x;
+				vertices[i*items+1] = mesh.cloud->at(mesh.indices->at(i)).y;
+				vertices[i*items+2] = mesh.cloud->at(mesh.indices->at(i)).z;
+
+				// rgb
+				vertices[i*items+3] = mesh.cloud->at(mesh.indices->at(i)).rgb;
+
+				// texture uv
+				int index = mesh.indices->at(i);
+				vertices[i*items+4] = float(index % mesh.cloud->width)/float(mesh.cloud->width); //u
+				vertices[i*items+5] = float(index / mesh.cloud->width)/float(mesh.cloud->height);  //v
+
+				if(hasNormals_)
+				{
+					// normal
+					vertices[i*items+6] = mesh.normals->at(mesh.indices->at(i)).normal_x;
+					vertices[i*items+7] = mesh.normals->at(mesh.indices->at(i)).normal_y;
+					vertices[i*items+8] = mesh.normals->at(mesh.indices->at(i)).normal_z;
+				}
+
+				organizedToDenseIndices_[mesh.indices->at(i)] = i;
+			}
+		}
+		else
+		{
+			//LOGD("Organized mesh");
+			int items = hasNormals_?7:4;
+			vertices = std::vector<float>(mesh.indices->size()*items);
+			for(unsigned int i=0; i<mesh.indices->size(); ++i)
+			{
+				vertices[i*items] = mesh.cloud->at(mesh.indices->at(i)).x;
+				vertices[i*items+1] = mesh.cloud->at(mesh.indices->at(i)).y;
+				vertices[i*items+2] = mesh.cloud->at(mesh.indices->at(i)).z;
+				vertices[i*items+3] = mesh.cloud->at(mesh.indices->at(i)).rgb;
+
+				if(hasNormals_)
+				{
+					// normal
+					vertices[i*items+4] = mesh.normals->at(mesh.indices->at(i)).normal_x;
+					vertices[i*items+5] = mesh.normals->at(mesh.indices->at(i)).normal_y;
+					vertices[i*items+6] = mesh.normals->at(mesh.indices->at(i)).normal_z;
+				}
+
+				organizedToDenseIndices_[mesh.indices->at(i)] = i;
+			}
+		}
+	}
+	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)",
+			//		(int)mesh.texCoords.size(), (int)mesh.cloud->size(), (int)mesh.polygons.size(), texture.cols, texture.rows);
+
+			// Texturing issue:
+			//  tex_coordinates should be linked to points, not
+			//  polygon vertices. Points linked to multiple different texCoords (different textures) should
+			//  be duplicated.
+			vertices = std::vector<float>(mesh.texCoords.size()*9);
+			organizedToDenseIndices_ = std::vector<unsigned int>(mesh.texCoords.size(), -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());
+
+			int items = hasNormals_?9:6;
+			unsigned int oi=0;
+			for(unsigned int i=0; i<polygons.size(); ++i)
+			{
+				pcl::Vertices & v = polygons[i];
+				for(unsigned int j=0; j<v.vertices.size(); ++j)
+				{
+					UASSERT(oi < mesh.texCoords.size());
+					UASSERT(v.vertices[j] < mesh.cloud->size());
+
+					vertices[oi*items] = mesh.cloud->at(v.vertices[j]).x;
+					vertices[oi*items+1] = mesh.cloud->at(v.vertices[j]).y;
+					vertices[oi*items+2] = mesh.cloud->at(v.vertices[j]).z;
+
+					// rgb
+					vertices[oi*items+3] = mesh.cloud->at(v.vertices[j]).rgb;
+
+					// texture uv
+					if(mesh.texCoords[oi][0]>=0.0f)
+					{
+						vertices[oi*items+4] = mesh.texCoords[oi][0]; //u
+						vertices[oi*items+5] = 1.0f-mesh.texCoords[oi][1];  //v
+					}
+					else
+					{
+						vertices[oi*items+4] = vertices[oi*items+5] = -1.0f;
+					}
+
+					if(hasNormals_)
+					{
+						// normal
+						vertices[oi*items+6] = mesh.normals->at(v.vertices[j]).normal_x;
+						vertices[oi*items+7] = mesh.normals->at(v.vertices[j]).normal_y;
+						vertices[oi*items+8] = mesh.normals->at(v.vertices[j]).normal_z;
+					}
+
+					v.vertices[j] = (int)oi; // new vertex index
+
+					UASSERT(oi < organizedToDenseIndices_.size());
+					organizedToDenseIndices_[oi] = oi;
+
+					++oi;
+				}
+			}
+		}
+		else
+		{
+			//LOGD("Dense mesh");
+			int items = hasNormals_?7:4;
+			organizedToDenseIndices_ = std::vector<unsigned int>(mesh.cloud->size(), -1);
+			vertices = std::vector<float>(mesh.cloud->size()*items);
+			for(unsigned int i=0; i<mesh.cloud->size(); ++i)
+			{
+				vertices[i*items] = mesh.cloud->at(i).x;
+				vertices[i*items+1] = mesh.cloud->at(i).y;
+				vertices[i*items+2] = mesh.cloud->at(i).z;
+				vertices[i*items+3] = mesh.cloud->at(i).rgb;
+
+				if(hasNormals_)
+				{
+					vertices[i*items+4] = mesh.normals->at(i).normal_x;
+					vertices[i*items+5] = mesh.normals->at(i).normal_y;
+					vertices[i*items+6] = mesh.normals->at(i).normal_z;
+				}
+
+				organizedToDenseIndices_[i] = i;
+			}
+		}
+	}
+
+	glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
+	glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * (int)vertices.size(), (const void *)vertices.data(), GL_STATIC_DRAW);
+	glBindBuffer(GL_ARRAY_BUFFER, 0);
+
+	GLint error = glGetError();
+	if(error != GL_NO_ERROR)
+	{
+		LOGE("OpenGL: Could not allocate point cloud (0x%x)\n", error);
+		vertex_buffers_ = 0;
+		return;
+	}
+
+	if(textures_ && textureUpdate)
+	{
+		//GLint maxTextureSize = 0;
+		//glGetIntegerv(GL_MAX_TEXTURE_SIZE, &maxTextureSize);
+		//LOGI("maxTextureSize=%d", maxTextureSize);
+		//GLint maxTextureUnits = 0;
+		//glGetIntegerv(GL_MAX_TEXTURE_IMAGE_UNITS, &maxTextureUnits);
+		//LOGW("maxTextureUnits=%d", maxTextureUnits);
+
+		// gen texture from image
+		glBindTexture(GL_TEXTURE_2D, textures_);
+		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
+		cv::Mat rgbImage;
+		cv::cvtColor(mesh.texture, rgbImage, CV_BGR2RGB);
+
+		glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
+		//glPixelStorei(GL_UNPACK_ROW_LENGTH, 0);
+		//glPixelStorei(GL_UNPACK_SKIP_PIXELS, 0);
+		//glPixelStorei(GL_UNPACK_SKIP_ROWS, 0);
+		glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, rgbImage.cols, rgbImage.rows, 0, GL_RGB, GL_UNSIGNED_BYTE, rgbImage.data);
+
+		GLint error = glGetError();
+		if(error != GL_NO_ERROR)
+		{
+			LOGE("OpenGL: Could not allocate texture (0x%x)\n", error);
+			textures_ = 0;
+
+			glDeleteBuffers(1, &vertex_buffers_);
+			vertex_buffers_ = 0;
+			return;
+		}
+	}
+
+	nPoints_ = totalPoints;
+
+	if(polygons_.size() != polygons.size())
+	{
+		updatePolygons(polygons);
+	}
 }
 
 void PointCloudDrawable::setPose(const rtabmap::Transform & pose)
@@ -158,37 +438,163 @@ void PointCloudDrawable::setPose(const rtabmap::Transform & pose)
 	pose_ = glmFromTransform(pose);
 }
 
-void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix, const glm::mat4 & viewMatrix, bool meshRendering, float pointSize) {
+void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix,
+		const glm::mat4 & viewMatrix,
+		bool meshRendering,
+		float pointSize,
+		bool textureRendering,
+		bool lighting) {
 
 	if(vertex_buffers_ && nPoints_ && visible_)
 	{
-		glUseProgram(shader_program_);
+		if(meshRendering && textureRendering && textures_)
+		{
+			glUseProgram(texture_shader_program_);
 
-		GLuint mvp_handle_ = glGetUniformLocation(shader_program_, "mvp");
+			GLuint mvp_handle = glGetUniformLocation(texture_shader_program_, "uMVP");
-		glm::mat4 mvp_mat = projectionMatrix * viewMatrix * pose_;
+			glm::mat4 mv_mat = viewMatrix * pose_;
+			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(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
+			}
+
+			// 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);
+
+			GLuint gain_handle = glGetUniformLocation(texture_shader_program_, "uGain");
+			glUniform1f(gain_handle, gain_);
+
+			GLint attribute_vertex = glGetAttribLocation(texture_shader_program_, "aVertex");
+			GLint attribute_texture = glGetAttribLocation(texture_shader_program_, "aTexCoord");
+			GLint attribute_normal=0;
+			if(hasNormals_)
+			{
+				attribute_normal = glGetAttribLocation(texture_shader_program_, "aNormal");
+			}
+
+			glEnableVertexAttribArray(attribute_vertex);
+			glEnableVertexAttribArray(attribute_texture);
+			if(hasNormals_)
+			{
+				glEnableVertexAttribArray(attribute_normal);
+			}
+			glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
+			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_)
+			{
+				glVertexAttribPointer(attribute_normal, 3, GL_FLOAT, GL_FALSE, 9*sizeof(GLfloat), (GLvoid*) (6 * sizeof(GLfloat)));
+			}
+			glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_INT, polygons_.data());
+		}
+		else // point cloud or colored mesh
+		{
+			glUseProgram(cloud_shader_program_);
+
+			GLuint mvp_handle_ = glGetUniformLocation(cloud_shader_program_, "uMVP");
+			glm::mat4 mv_mat = viewMatrix * pose_;
+			glm::mat4 mvp_mat = projectionMatrix * mv_mat;
 			glUniformMatrix4fv(mvp_handle_, 1, GL_FALSE, glm::value_ptr(mvp_mat));
 
-		GLuint point_size_handle_ = glGetUniformLocation(shader_program_, "point_size");
+			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(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);
 
-		GLint attribute_vertex = glGetAttribLocation(shader_program_, "vertex");
+			GLuint gain_handle = glGetUniformLocation(cloud_shader_program_, "uGain");
-		GLint attribute_color = glGetAttribLocation(shader_program_, "color");
+			glUniform1f(gain_handle, gain_);
+
+			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);
-		glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
+			if(hasNormals_)
-		glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, 4*sizeof(GLfloat), 0);
-		glVertexAttribPointer(attribute_color, 3, GL_UNSIGNED_BYTE, GL_TRUE, 4*sizeof(GLfloat), (GLvoid*) (3 * sizeof(GLfloat)));
-
-		if(meshRendering && indices_.size())
 			{
-			glDrawElements(GL_TRIANGLES, indices_.size(), GL_UNSIGNED_SHORT, indices_.data());
+				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)));
+				}
+			}
+			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())
+			{
+				glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_INT, polygons_.data());
 			}
 			else
 			{
 				glDrawArrays(GL_POINTS, 0, nPoints_);
 			}
-
+		}
 		glDisableVertexAttribArray(0);
 		glBindBuffer(GL_ARRAY_BUFFER, 0);
 

app/android/jni/point_cloud_drawable.h

@@ -1,18 +1,29 @@
 /*
- * Copyright 2014 Google Inc. All Rights Reserved.
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
- *
+All rights reserved.
- * Licensed under the Apache License, Version 2.0 (the "License");
+
- * you may not use this file except in compliance with the License.
+Redistribution and use in source and binary forms, with or without
- * You may obtain a copy of the License at
+modification, are permitted provided that the following conditions are met:
- *
+    * Redistributions of source code must retain the above copyright
- *      http://www.apache.org/licenses/LICENSE-2.0
+      notice, this list of conditions and the following disclaimer.
- *
+    * Redistributions in binary form must reproduce the above copyright
- * Unless required by applicable law or agreed to in writing, software
+      notice, this list of conditions and the following disclaimer in the
- * distributed under the License is distributed on an "AS IS" BASIS,
+      documentation and/or other materials provided with the distribution.
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+    * Neither the name of the Universite de Sherbrooke nor the
- * See the License for the specific language governing permissions and
+      names of its contributors may be used to endorse or promote products
- * limitations under the License.
+      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 TANGO_POINT_CLOUD_POINT_CLOUD_DRAWABLE_H_
 #define TANGO_POINT_CLOUD_POINT_CLOUD_DRAWABLE_H_
@@ -31,19 +42,27 @@
 class PointCloudDrawable {
  public:
   PointCloudDrawable(
-		  GLuint shaderProgram,
+		  GLuint cloudShaderProgram,
-		  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		  GLuint textureShaderProgram,
-		  const std::vector<pcl::Vertices> & indices = std::vector<pcl::Vertices>());
-  PointCloudDrawable(
-  		  GLuint shaderProgram,
   		  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-  		  const std::vector<pcl::Vertices> & indices = std::vector<pcl::Vertices>());
+		  const pcl::IndicesPtr & indices,
+		  float gain);
+  PointCloudDrawable(
+  		  GLuint cloudShaderProgram,
+  		  GLuint textureShaderProgram,
+    	  const Mesh & mesh);
   virtual ~PointCloudDrawable();
 
+  void updatePolygons(const std::vector<pcl::Vertices> & polygons);
+  void updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, float gain);
+  void updateMesh(const Mesh & mesh);
   void setPose(const rtabmap::Transform & pose);
   void setVisible(bool visible) {visible_=visible;}
+  void setGain(float gain) {gain_ = gain;}
   rtabmap::Transform getPose() const {return glmToTransform(pose_);}
   bool isVisible() const {return visible_;}
+  bool hasMesh() const {return polygons_.size()!=0;}
+  bool hasTexture() const {return textures_ != 0;}
 
   // Update current point cloud data.
   //
@@ -51,17 +70,28 @@ 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, const glm::mat4 & viewMatrix, bool meshRendering = true, float pointSize = 3.0f);
+  void Render(const glm::mat4 & projectionMatrix,
+		  const glm::mat4 & viewMatrix,
+		  bool meshRendering = true,
+		  float pointSize = 3.0f,
+		  bool textureRendering = false,
+		  bool lighting = true);
 
  private:
   // Vertex buffer of the point cloud geometry.
   GLuint vertex_buffers_;
-  std::vector<GLushort> indices_;
+  GLuint textures_;
+  std::vector<GLuint> polygons_;
   int nPoints_;
   glm::mat4 pose_;
   bool visible_;
+  bool hasNormals_;
+  std::vector<unsigned int> organizedToDenseIndices_;
 
-  GLuint shader_program_;
+  GLuint cloud_shader_program_;
+  GLuint texture_shader_program_;
+
+  float gain_;
 };
 
 #endif  // TANGO_POINT_CLOUD_POINT_CLOUD_DRAWABLE_H_

app/android/jni/scene.cpp

@@ -21,6 +21,8 @@
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/core/util3d_filtering.h>
 
+#include <glm/gtx/transform.hpp>
+
 #include "scene.h"
 #include "util.h"
 
@@ -42,22 +44,87 @@ 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 vertex;\n"
+    "attribute vec3 aVertex;\n"
-    "attribute vec3 color;\n"
+	"attribute vec3 aNormal;\n"
-    "uniform mat4 mvp;\n"
+    "attribute vec3 aColor;\n"
-    "uniform float point_size;\n"
+
-    "varying vec3 v_color;\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 = mvp*vec4(vertex.x, vertex.y, vertex.z, 1.0);\n"
+    "  gl_Position = uMVP*vec4(aVertex.x, aVertex.y, aVertex.z, 1.0);\n"
-    "  gl_PointSize = point_size;\n"
+    "  gl_PointSize = uPointSize;\n"
-    "  v_color = color;\n"
+	"  if (!uUseLighting) {\n"
+	"    vLightWeighting = 1.0;\n"
+	"  } else {\n"
+	"    vec3 transformedNormal = uN * aNormal;\n"
+	"    vLightWeighting = max(dot(transformedNormal, uLightingDirection), 0.0);\n"
+	"    if(vLightWeighting<0.1) vLightWeighting=0.1;\n"
+	"  }\n"
+    "  vColor = aColor;\n"
     "}\n";
 const std::string kPointCloudFragmentShader =
     "precision mediump float;\n"
     "precision mediump int;\n"
-    "varying vec3 v_color;\n"
+	"uniform float uGain;\n"
+    "varying vec3 vColor;\n"
+	"varying float vLightWeighting;\n"
     "void main() {\n"
-    "  gl_FragColor = vec4(v_color.z, v_color.y, v_color.x, 1.0);\n"
+    "  vec4 textureColor = vec4(vColor.z*uGain, vColor.y*uGain, vColor.x*uGain, 1.0);\n"
+	"  gl_FragColor = vec4(textureColor.rgb * uGain * 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.0);\n"
+    "    if(vLightWeighting<0.1) vLightWeighting=0.1;\n"
+    "  }\n"
+    "}\n";
+const std::string kTextureMeshFragmentShader =
+    "precision mediump float;\n"
+    "precision mediump int;\n"
+	"uniform sampler2D uTexture;\n"
+	"uniform float uGain;\n"
+    "varying vec2 vTexCoord;\n"
+	"varying float vLightWeighting;\n"
+    "void main() {\n"
+    "  vec4 textureColor = texture2D(uTexture, vTexCoord);\n"
+	"  gl_FragColor = vec4(textureColor.rgb * uGain * vLightWeighting, textureColor.a);\n"
     "}\n";
 
 const std::string kGraphVertexShader =
@@ -89,26 +156,45 @@ Scene::Scene() :
 		trace_(0),
 		graph_(0),
 		graphVisible_(true),
+		gridVisible_(true),
 		traceVisible_(true),
+		color_camera_to_display_rotation_(ROTATION_0),
 		currentPose_(0),
 		cloud_shader_program_(0),
+		texture_mesh_shader_program_(0),
 		graph_shader_program_(0),
+		mapRendering_(true),
 		meshRendering_(true),
-		pointSize_(3.0f) {}
+		meshRenderingTexture_(true),
+		pointSize_(5.0f),
+		frustumCulling_(true),
+		lighting_(true),
+		backfaceCulling_(true),
+		r_(0.0f),
+		g_(0.0f),
+		b_(0.0f)
+{
+	gesture_camera_ = new tango_gl::GestureCamera();
+	gesture_camera_->SetCameraType(
+	      tango_gl::GestureCamera::kFirstPerson);
+}
 
-Scene::~Scene() {DeleteResources();}
+Scene::~Scene() {
+	DeleteResources();
+	delete gesture_camera_;
+}
 
 //Should only be called in OpenGL thread!
 void Scene::InitGLContent()
 {
-	if(gesture_camera_ != 0)
+	if(axis_ != 0)
 	{
 		DeleteResources();
 	}
 
-	UASSERT(gesture_camera_ == 0);
+	UASSERT(axis_ == 0);
+
 
-  gesture_camera_ = new tango_gl::GestureCamera();
   axis_ = new tango_gl::Axis();
   frustum_ = new tango_gl::Frustum();
   trace_ = new tango_gl::Trace();
@@ -122,14 +208,17 @@ void Scene::InitGLContent()
   trace_->SetColor(kTraceColor);
   grid_->SetColor(kGridColor);
   grid_->SetPosition(-kHeightOffset);
-  gesture_camera_->SetCameraType(
-      tango_gl::GestureCamera::kFirstPerson);
 
   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());
@@ -141,21 +230,24 @@ void Scene::InitGLContent()
 void Scene::DeleteResources() {
 
 	LOGI("Scene::DeleteResources()");
-	if(gesture_camera_)
+	if(axis_)
 	{
-	  delete gesture_camera_;
 	  delete axis_;
+	  axis_ = 0;
 	  delete frustum_;
 	  delete trace_;
 	  delete grid_;
 	  delete currentPose_;
-	  gesture_camera_ = 0;
 	}
 
 	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;
+	  }
 	if (graph_shader_program_) {
 		glDeleteShader(graph_shader_program_);
 		graph_shader_program_ = 0;
@@ -200,9 +292,16 @@ int Scene::Render() {
 	UASSERT(gesture_camera_ != 0);
 
 	glEnable(GL_DEPTH_TEST);
+	if(backfaceCulling_)
+	{
 		glEnable(GL_CULL_FACE);
+	}
+	else
+	{
+		glDisable(GL_CULL_FACE);
+	}
 
-  glClearColor(0.0f, 0.0f, 0.0f, 1.0f);
+	glClearColor(r_, g_, b_, 1.0f);
 	glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
 
 	if(!currentPose_->isNull())
@@ -211,16 +310,19 @@ int Scene::Render() {
 		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)
 		{
 			// In first person mode, we directly control camera's motion.
 			gesture_camera_->SetPosition(position);
-		gesture_camera_->SetRotation(rotation);
+			gesture_camera_->SetRotation(rotation*glm::quat(rotateM));
 		}
 		else
 		{
-		// In third person or top down more, we follow the camera movement.
+			// In third person or top down mode, we follow the camera movement.
-		gesture_camera_->SetAnchorPosition(position, rotation);
+			gesture_camera_->SetAnchorPosition(position, rotation*glm::quat(rotateM));
 
 			frustum_->SetPosition(position);
 			frustum_->SetRotation(rotation);
@@ -242,15 +344,16 @@ int Scene::Render() {
 			trace_->Render(gesture_camera_->GetProjectionMatrix(),
 					gesture_camera_->GetViewMatrix());
 		}
-  }
-
 
+		if(gridVisible_)
+		{
 			grid_->Render(gesture_camera_->GetProjectionMatrix(),
 					gesture_camera_->GetViewMatrix());
+		}
+	}
 
-	bool frustumCulling = true;
 	int cloudDrawn=0;
-	if(frustumCulling)
+	if(mapRendering_ && frustumCulling_)
 	{
 		//Use camera frustum to cull nodes that don't need to be drawn
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
@@ -296,16 +399,19 @@ int Scene::Render() {
 			for(unsigned int i=0; i<indices->size(); ++i)
 			{
 				++cloudDrawn;
-				pointClouds_.find(ids[indices->at(i)])->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_);
+				pointClouds_.find(ids[indices->at(i)])->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_, meshRenderingTexture_, lighting_);
 			}
 		}
 	}
 	else
 	{
 		for(std::map<int, PointCloudDrawable*>::const_iterator iter=pointClouds_.begin(); iter!=pointClouds_.end(); ++iter)
+		{
+			if((mapRendering_ || iter->first < 0) && iter->second->isVisible())
 			{
 				++cloudDrawn;
-			iter->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_);
+				iter->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_, meshRenderingTexture_, lighting_);
+			}
 		}
 	}
 
@@ -366,19 +472,24 @@ void Scene::setGraphVisible(bool visible)
 	graphVisible_ = visible;
 }
 
+void Scene::setGridVisible(bool visible)
+{
+	gridVisible_ = visible;
+}
+
 void Scene::setTraceVisible(bool visible)
 {
 	traceVisible_ = visible;
 }
 
 //Should only be called in OpenGL thread!
-void Scene::addOrUpdateCloud(
+void Scene::addCloud(
 		int id,
-		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		const std::vector<pcl::Vertices> & polygons,
+		const pcl::IndicesPtr & indices,
 		const rtabmap::Transform & pose)
 {
-	LOGI("addOrUpdateCloud cloud %d", id);
+	LOGI("add cloud %d (%d points %d indices)", id, (int)cloud->size(), indices.get()?(int)indices->size():0);
 	std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
 	if(iter != pointClouds_.end())
 	{
@@ -387,18 +498,23 @@ void Scene::addOrUpdateCloud(
 	}
 
 	//create
-	UASSERT(cloud_shader_program_ != 0);
+	UASSERT(cloud_shader_program_ != 0 && texture_mesh_shader_program_!=0);
-	PointCloudDrawable * drawable = new PointCloudDrawable(cloud_shader_program_, cloud, polygons);
+	PointCloudDrawable * drawable = new PointCloudDrawable(
+			cloud_shader_program_,
+			texture_mesh_shader_program_,
+			cloud,
+			indices,
+			1.0f);
 	drawable->setPose(pose);
 	pointClouds_.insert(std::make_pair(id, drawable));
 }
-void Scene::addOrUpdateCloud(
+
+void Scene::addMesh(
 		int id,
-		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const Mesh & mesh,
-		const std::vector<pcl::Vertices> & polygons,
 		const rtabmap::Transform & pose)
 {
-	LOGI("addOrUpdateCloud cloud %d", id);
+	LOGI("add mesh %d", id);
 	std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
 	if(iter != pointClouds_.end())
 	{
@@ -407,8 +523,11 @@ void Scene::addOrUpdateCloud(
 	}
 
 	//create
-	UASSERT(cloud_shader_program_ != 0);
+	UASSERT(cloud_shader_program_ != 0 && texture_mesh_shader_program_!=0);
-	PointCloudDrawable * drawable = new PointCloudDrawable(cloud_shader_program_, cloud, polygons);
+	PointCloudDrawable * drawable = new PointCloudDrawable(
+			cloud_shader_program_,
+			texture_mesh_shader_program_,
+			mesh);
 	drawable->setPose(pose);
 	pointClouds_.insert(std::make_pair(id, drawable));
 }
@@ -438,7 +557,44 @@ bool Scene::hasCloud(int id) const
 	return pointClouds_.find(id) != pointClouds_.end();
 }
 
+bool Scene::hasMesh(int id) const
+{
+	return pointClouds_.find(id) != pointClouds_.end() && pointClouds_.at(id)->hasMesh();
+}
+
+bool Scene::hasTexture(int id) const
+{
+	return pointClouds_.find(id) != pointClouds_.end() && pointClouds_.at(id)->hasTexture();
+}
+
 std::set<int> Scene::getAddedClouds() const
 {
 	return uKeysSet(pointClouds_);
 }
+
+void Scene::updateCloudPolygons(int id, const std::vector<pcl::Vertices> & polygons)
+{
+	std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
+	if(iter != pointClouds_.end())
+	{
+		iter->second->updatePolygons(polygons);
+	}
+}
+
+void Scene::updateMesh(int id, const Mesh & mesh)
+{
+	std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
+	if(iter != pointClouds_.end())
+	{
+		iter->second->updateMesh(mesh);
+	}
+}
+
+void Scene::updateGain(int id, float gain)
+{
+	std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
+	if(iter != pointClouds_.end())
+	{
+		iter->second->setGain(gain);
+	}
+}

app/android/jni/scene.h

@@ -57,6 +57,8 @@ class Scene {
   // Setup GL view port.
   void SetupViewPort(int w, int h);
 
+  void setScreenRotation(TangoSupportRotation colorCameraToDisplayRotation) {color_camera_to_display_rotation_ = colorCameraToDisplayRotation;}
+
   void clear(); // removed all point clouds
 
   // Render loop.
@@ -94,26 +96,43 @@ class Scene {
   		const std::multimap<int, rtabmap::Link> & links);
 
   void setGraphVisible(bool visible);
+  void setGridVisible(bool visible);
   void setTraceVisible(bool visible);
 
-  void addOrUpdateCloud(
+  void addCloud(
-		  int id,
-		  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
-		  const std::vector<pcl::Vertices> & polygons,
-		  const rtabmap::Transform & pose);
-  void addOrUpdateCloud(
   		  int id,
   		  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-  		  const std::vector<pcl::Vertices> & polygons,
+		  const pcl::IndicesPtr & indices,
+  		  const rtabmap::Transform & pose);
+  void addMesh(
+  		int id,
+  		const Mesh & mesh,
   		const rtabmap::Transform & pose);
 
   void setCloudPose(int id, const rtabmap::Transform & pose);
   void setCloudVisible(int id, bool visible);
   bool hasCloud(int id) const;
+  bool hasMesh(int id) const;
+  bool hasTexture(int id) const;
   std::set<int> getAddedClouds() const;
+  void updateCloudPolygons(int id, const std::vector<pcl::Vertices> & polygons);
+  void updateMesh(int id, const Mesh & mesh);
+  void updateGain(int id, float gain);
 
-  void setMeshRendering(bool enabled) {meshRendering_ = 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 setLighting(bool enabled) {lighting_ = enabled;}
+  void setBackfaceCulling(bool enabled) {backfaceCulling_ = enabled;}
+  void setBackgroundColor(float r, float g, float b) {r_=r; g_=g; b_=b;} // 0.0f <> 1.0f
+
+  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_;}
 
  private:
   // Camera object that allows user to use touch input to interact with.
@@ -132,18 +151,30 @@ class Scene {
   tango_gl::Trace* trace_;
   GraphDrawable * graph_;
   bool graphVisible_;
+  bool gridVisible_;
   bool traceVisible_;
 
+  TangoSupportRotation color_camera_to_display_rotation_;
+
   std::map<int, PointCloudDrawable*> pointClouds_;
 
   rtabmap::Transform * currentPose_;
 
   // Shader to display point cloud.
   GLuint cloud_shader_program_;
+  GLuint texture_mesh_shader_program_;
   GLuint graph_shader_program_;
 
+  bool mapRendering_;
   bool meshRendering_;
+  bool meshRenderingTexture_;
   float pointSize_;
+  bool frustumCulling_;
+  bool lighting_;
+  bool backfaceCulling_;
+  float r_;
+  float g_;
+  float b_;
 };
 
 #endif  // TANGO_POINT_CLOUD_SCENE_H_

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

@@ -16,6 +16,7 @@
 
 #include "tango-gl/gesture_camera.h"
 #include "tango-gl/util.h"
+#include "glm/gtx/quaternion.hpp"
 
 namespace {
 // Render camera observation distance in third person camera mode.
@@ -30,8 +31,14 @@ const float kTopDownCameraDist = 5.0f;
 // Zoom in speed.
 const float kZoomSpeed = 10.0f;
 
+// Move speed
+const float kMoveSpeed = 10.0f;
+
+// Rotation speed
+const float kRotationSpeed = 2.0f;
+
 // Min/max clamp value of camera observation distance.
-const float kCamViewMinDist = 1.0f;
+const float kCamViewMinDist = .1f;
 const float kCamViewMaxDist = 100.f;
 
 // FOV set up values.
@@ -70,8 +77,8 @@ void GestureCamera::OnTouchEvent(int touch_count, TouchEvent event, float x0,
         break;
       }
       case kTouchMove: {
-        float rotation_x = touch0_start_position_.y - y0;
+        float rotation_x = (touch0_start_position_.y - y0) * kRotationSpeed;
-        float rotation_y = touch0_start_position_.x - x0;
+        float rotation_y = (touch0_start_position_.x - x0) * kRotationSpeed;
 
         cam_cur_angle_.x = cam_start_angle_.x + rotation_x;
         cam_cur_angle_.y = cam_start_angle_.y + rotation_y;
@@ -88,17 +95,30 @@ void GestureCamera::OnTouchEvent(int touch_count, TouchEvent event, float x0,
         float abs_y = y0 - y1;
         start_touch_dist_ = std::sqrt(abs_x * abs_x + abs_y * abs_y);
         cam_start_dist_ = GetPosition().z;
+
+        // 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 =
+        float dist = start_touch_dist_ - std::sqrt(abs_x * abs_x + abs_y * abs_y);
-            start_touch_dist_ - std::sqrt(abs_x * abs_x + abs_y * abs_y);
+
-        cam_cur_dist_ =
+        cam_cur_dist_ = tango_gl::util::Clamp(cam_start_dist_ + dist * kZoomSpeed,
-            tango_gl::util::Clamp(cam_start_dist_ + dist * kZoomSpeed,
                                   kCamViewMinDist, kCamViewMaxDist);
+
+        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; }
@@ -126,7 +146,10 @@ Segment GestureCamera::GetSegmentFromTouch(float normalized_x,
 }
 
 void GestureCamera::SetAnchorPosition(const glm::vec3& pos, const glm::quat & rotation) {
-	cam_parent_transform_->SetPosition(pos);
+	// Anchor position
+	cam_parent_transform_->SetPosition(pos+anchor_offset_);
+
+	// Anchor rotation
 	if(camera_type_ == kThirdPersonFollow)
 	{
 		cam_cur_target_rot_ = rotation;
@@ -145,6 +168,7 @@ void GestureCamera::SetCameraType(CameraType camera_index) {
       SetPosition(glm::vec3(0.0f, 0.0f, 0.0f));
       SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
       cam_cur_dist_ = 0.0f;
+      anchor_offset_ = glm::vec3(0.0f,0.0f,0.0f);
       cam_cur_angle_.x = 0.0f;
       cam_cur_angle_.y = 0.0f;
       cam_cur_target_rot_ = glm::quat(1,0,0,0);
@@ -157,6 +181,7 @@ void GestureCamera::SetCameraType(CameraType camera_index) {
       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_.y = kThirdPersonFollow?0:M_PI / 4.0f;
       cam_cur_target_rot_ = glm::quat(1,0,0,0);
@@ -167,6 +192,7 @@ void GestureCamera::SetCameraType(CameraType camera_index) {
       SetPosition(glm::vec3(0.0f, 0.0f, 0.0f));
       SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.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);
@@ -179,9 +205,12 @@ void GestureCamera::SetCameraType(CameraType camera_index) {
 
 void GestureCamera::StartCameraToCurrentTransform()
 {
+  //Anchor rotation
   glm::quat parent_cam_rot = glm::rotate(cam_cur_target_rot_, cam_cur_angle_.y, glm::vec3(0, 1, 0));
   parent_cam_rot = glm::rotate(parent_cam_rot, cam_cur_angle_.x, glm::vec3(1, 0, 0));
-  SetPosition(glm::vec3(0.0f, 0.0f, cam_cur_dist_));
   cam_parent_transform_->SetRotation(parent_cam_rot);
+
+  //Camera position
+  SetPosition(glm::vec3(0, 0, cam_cur_dist_));
 }
 }  // namespace tango_gl

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

@@ -76,6 +76,7 @@ class GestureCamera : public Camera {
 
   float cam_start_dist_;
   float cam_cur_dist_;
+  glm::vec3 anchor_offset_;
 
   float start_touch_dist_;
   float cur_touch_dist_;

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

@@ -25,6 +25,7 @@
 #include <android/log.h>
 #include <GLES2/gl2.h>
 #include <GLES2/gl2ext.h>
+#include <tango_support_api.h>
 
 #include "glm/glm.hpp"
 #include "glm/gtc/matrix_transform.hpp"
@@ -76,6 +77,33 @@ namespace util {
                         const glm::vec3& start, const glm::vec3& end);
 
   glm::vec3 ApplyTransform(const glm::mat4& mat, const glm::vec3& vec);
+
+  // Get the Android rotation integer value from color camera to display.
+  // This function is used to compute the orientation difference to handle
+  // the portrait and landscape mode for color camera display.
+  //
+  // @param display: integer value of display orientation, values available
+  // are 0, 1, 2 ,3. Followed by Android display orientation standard:
+  // https://developer.android.com/reference/android/view/Display.html#getRotation()
+  // @param color_camera: integer value of color camera oreintation, values
+  // available are 0, 90, 180, 270. Followed by Android camera orientation
+  // standard:
+  // https://developer.android.com/reference/android/hardware/Camera.CameraInfo.html#orientation
+  TangoSupportRotation GetAndroidRotationFromColorCameraToDisplay(
+      int display_rotation, int color_camera_rotation);
+
+  // Get the Android rotation integer value from color camera to display.
+  // This function is used to compute the orientation difference to handle
+  // the portrait and landscape mode for color camera display.
+  //
+  // @param display: the device display orientation.
+  // @param color_camera: integer value of color camera oreintation, values
+  // available are 0, 90, 180, 270. Followed by Android camera orientation
+  // standard:
+  // https://developer.android.com/reference/android/hardware/Camera.CameraInfo.html#orientation
+  TangoSupportRotation GetAndroidRotationFromColorCameraToDisplay(
+      TangoSupportRotation display_rotation, int color_camera_rotation);
+
 }  // namespace util
 }  // namespace tango_gl
 #endif  // TANGO_GL_RENDERER_GL_UTIL

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

@@ -19,6 +19,27 @@
 
 namespace tango_gl {
 
+namespace {
+int NormalizedColorCameraRotation(int camera_rotation) {
+  int camera_n = 0;
+  switch (camera_rotation) {
+    case 90:
+      camera_n = 1;
+      break;
+    case 180:
+      camera_n = 2;
+      break;
+    case 270:
+      camera_n = 3;
+      break;
+    default:
+      camera_n = 0;
+      break;
+  }
+  return camera_n;
+}
+}  // annonymous namespace
+
 void util::CheckGlError(const char* operation) {
   for (GLint error = glGetError(); error; error = glGetError()) {
     LOGE("after %s() glError (0x%x)\n", operation, error);
@@ -217,4 +238,23 @@ glm::vec3 util::ApplyTransform(const glm::mat4& mat, const glm::vec3& vec) {
   return glm::vec3(mat * glm::vec4(vec, 1.0f));
 }
 
+TangoSupportRotation util::GetAndroidRotationFromColorCameraToDisplay(
+    int display_rotation, int color_camera_rotation) {
+  TangoSupportRotation r =
+      static_cast<TangoSupportRotation>(display_rotation);
+  return util::GetAndroidRotationFromColorCameraToDisplay(
+      r, color_camera_rotation);
+}
+
+TangoSupportRotation util::GetAndroidRotationFromColorCameraToDisplay(
+    TangoSupportRotation display_rotation, int color_camera_rotation) {
+  int color_camera_n = NormalizedColorCameraRotation(color_camera_rotation);
+
+  int ret = static_cast<int>(display_rotation) - color_camera_n;
+  if (ret < 0) {
+    ret += 4;
+  }
+  return static_cast<TangoSupportRotation>(ret % 4);
+}
+
 }  // namespace tango_gl

app/android/jni/util.h

@@ -32,13 +32,22 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <android/log.h>
 #include <rtabmap/utilite/UEventsHandler.h>
 #include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/core/CameraModel.h>
 #include <tango-gl/util.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/Vertices.h>
+#include <pcl/pcl_base.h>
 
 class LogHandler : public UEventsHandler
 {
 public:
 	LogHandler()
 	{
+		ULogger::setLevel(ULogger::kWarning);
+		ULogger::setEventLevel(ULogger::kWarning);
+		ULogger::setPrintThreadId(true);
+
 		registerToEventsManager();
 	}
 protected:
@@ -73,17 +82,22 @@ static const rtabmap::Transform opengl_world_T_tango_world(
 		0.0f,  0.0f,  1.0f, 0.0f,
 		0.0f, -1.0f,  0.0f, 0.0f);
 
-static const rtabmap::Transform depth_camera_T_opengl_camera(
+static const rtabmap::Transform rtabmap_world_T_tango_world(
-		1.0f, 0.0f, 0.0f, 0.0f,
+		 0.0f, 1.0f, 0.0f, 0.0f,
+	    -1.0f, 0.0f, 0.0f, 0.0f,
+		 0.0f, 0.0f, 1.0f, 0.0f);
+
+static const rtabmap::Transform tango_device_T_rtabmap_device(
 		 0.0f, -1.0f, 0.0f, 0.0f,
-		0.0f, 0.0f, -1.0f, 0.0f);
+	     0.0f,  0.0f, 1.0f, 0.0f,
+		-1.0f,  0.0f, 0.0f, 0.0f);
 
 static const rtabmap::Transform opengl_world_T_rtabmap_world(
 		 0.0f, -1.0f, 0.0f, 0.0f,
 		 0.0f,  0.0f, 1.0f, 0.0f,
 		-1.0f,  0.0f, 0.0f, 0.0f);
 
-static const rtabmap::Transform rtabmap_world_T_opengl_world(
+static const rtabmap::Transform rtabmap_device_T_opengl_device(
 		 0.0f, 0.0f, -1.0f, 0.0f,
 		-1.0f, 0.0f,  0.0f, 0.0f,
 		 0.0f, 1.0f,  0.0f, 0.0f);
@@ -129,4 +143,27 @@ inline rtabmap::Transform glmToTransform(const glm::mat4 & mat)
 	return transform;
 }
 
+class Mesh
+{
+public:
+	Mesh() :
+		cloud(new pcl::PointCloud<pcl::PointXYZRGB>),
+		normals(new pcl::PointCloud<pcl::Normal>),
+		indices(new std::vector<int>),
+		visible(true),
+		gain(1.0f)
+	{}
+
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud; // organized cloud
+	pcl::PointCloud<pcl::Normal>::Ptr normals;
+	pcl::IndicesPtr indices;
+	std::vector<pcl::Vertices> polygons;
+	rtabmap::Transform pose; // in rtabmap coordinates
+	bool visible;
+	rtabmap::CameraModel cameraModel;
+	float gain;
+	std::vector<Eigen::Vector2f> texCoords;
+	cv::Mat texture;
+};
+
 #endif /* UTIL_H_ */

app/android/res/layout/activity_rtabmap.xml

@@ -23,206 +23,33 @@
         android:layout_height="fill_parent"
         android:layout_gravity="top" />
     
-   <LinearLayout
+    <ToggleButton
-        android:id="@+id/debug_layout"
+        android:id="@+id/backface_button"
-        android:layout_width="wrap_content"
+        android:layout_width="100dp"
         android:layout_height="wrap_content"
-        android:layout_alignParentLeft="true"
+        android:layout_above="@+id/light_button"
-        android:layout_alignParentTop="true"
+        android:layout_alignLeft="@+id/light_button"
-        android:orientation="vertical"
+        android:layout_alignParentRight="true"
-        android:paddingLeft="5dp" >
+        android:layout_marginBottom="5dp"
+        android:layout_marginRight="5dp"
+        android:paddingRight="5dp"
+        android:textOff="@string/backface_off"
+        android:textOn="@string/backface_on" />
    
-        <LinearLayout
+    <ToggleButton
-            android:layout_width="wrap_content"
+        android:id="@+id/light_button"
+        android:layout_width="100dp"
         android:layout_height="wrap_content"
-            android:orientation="horizontal" >
+        android:layout_above="@+id/first_person_button"
+        android:layout_alignLeft="@+id/first_person_button"
+        android:layout_alignParentRight="true"
+        android:layout_marginBottom="15dp"
+        android:layout_marginRight="5dp"
+        android:paddingRight="5dp"
+        android:textOff="@string/light_off"
+        android:textOn="@string/light_on" />
    
-            <TextView
+    <ToggleButton
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/status" />
-
-            <TextView
-                android:id="@+id/status"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
-               
-        <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/nodes" />
-
-            <TextView
-                android:id="@+id/nodes"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
-        
-        <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/words" />
-
-            <TextView
-                android:id="@+id/words"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
-        
-        <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/memory" />
-
-            <TextView
-                android:id="@+id/memory"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
-        
-        <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/db_size" />
-
-            <TextView
-                android:id="@+id/db_size"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
-        
-         <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/points" />
-
-            <TextView
-                android:id="@+id/points"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
-        <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/polygons" />
-
-            <TextView
-                android:id="@+id/polygons"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
-        
-         <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/update_time" />
-
-            <TextView
-                android:id="@+id/update_time"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
-        <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/features" />
-
-            <TextView
-                android:id="@+id/features"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
-        <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/total_loop" />
-
-            <TextView
-                android:id="@+id/total_loop"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
-
-        <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/inliers" />
-
-            <TextView
-                android:id="@+id/inliers"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
-        <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/hypothesis" />
-
-            <TextView
-                android:id="@+id/hypothesis"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
-           
-    </LinearLayout>
-
-    <Button
         android:id="@+id/first_person_button"
         android:layout_width="100dp"
         android:layout_height="wrap_content"
@@ -232,19 +59,10 @@
         android:layout_marginBottom="5dp"
         android:layout_marginRight="5dp"
         android:paddingRight="5dp"
-        android:text="@string/first_person" />
+        android:textOff="@string/first_person"
+        android:textOn="@string/first_person" />
     
-    <Button
+    <ToggleButton
-        android:id="@+id/top_down_button"
-        android:layout_width="100dp"
-        android:layout_height="wrap_content"
-        android:layout_alignParentBottom="true"
-        android:layout_alignParentRight="true"
-        android:layout_marginRight="5dp"
-        android:paddingRight="5dp"
-        android:text="@string/top_down" />
-
-    <Button
         android:id="@+id/third_person_button"
         android:layout_width="100dp"
         android:layout_height="wrap_content"
@@ -253,6 +71,56 @@
         android:layout_marginBottom="5dp"
         android:layout_marginRight="5dp"
         android:paddingRight="5dp"
-        android:text="@string/third_person" />
+        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"
+        android:layout_alignParentBottom="true"
+        android:layout_alignParentRight="true"
+        android:layout_marginRight="5dp"
+        android:paddingRight="5dp"
+        android:textOff="@string/top_down"
+        android:textOn="@string/top_down" />
+
+    <ToggleButton
+        android:id="@+id/pause_button"
+        android:layout_width="100dp"
+        android:layout_height="wrap_content"
+        android:layout_alignLeft="@+id/first_person_button"
+        android:layout_alignParentTop="true"
+        android:layout_marginTop="61dp"
+        android:layout_alignParentRight="true"
+        android:paddingRight="5dp"
+        android:textOff="@string/pause"
+        android:textOn="@string/resume" />
+
+    <Button
+        android:id="@+id/button_shareToSketchfab"
+        android:layout_width="wrap_content"
+        android:layout_height="wrap_content"
+        android:layout_alignParentTop="true"
+        android:layout_alignRight="@+id/pause_button"
+        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: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_centerHorizontal="true"
+        android:paddingLeft="5dp"
+        android:text="@string/close_visualization" />
         
 </RelativeLayout>

app/android/res/layout/activity_settings.xml

@@ -0,0 +1,274 @@
+<PreferenceScreen xmlns:android="http://schemas.android.com/apk/res/android">
+    
+    <PreferenceCategory
+        android:title="@string/pref_title_rendering">
+        <ListPreference 
+            android:key="@string/pref_key_density"
+    		android:title="@string/pref_title_density"
+    		android:summary="@string/pref_summary_density"
+   			android:entries="@array/pref_density_keys"
+   			android:entryValues="@array/pref_density_values"
+   			android:defaultValue="@string/pref_default_density"/>
+        <ListPreference 
+            android:key="@string/pref_key_depth"
+    		android:title="@string/pref_title_depth"
+    		android:summary="@string/pref_summary_depth"
+   			android:entries="@array/pref_depth_keys"
+   			android:entryValues="@array/pref_depth_values"
+   			android:defaultValue="@string/pref_default_depth"/>
+        <ListPreference
+            android:key="@string/pref_key_point_size"
+    		android:title="@string/pref_title_point_size"
+    		android:summary="@string/pref_summary_point_size"
+   			android:entries="@array/pref_point_size_values"
+   			android:entryValues="@array/pref_point_size_values"
+   			android:defaultValue="@string/pref_default_point_size"/>
+        <ListPreference 
+            android:key="@string/pref_key_angle"
+    		android:title="@string/pref_title_angle"
+    		android:summary="@string/pref_summary_angle"
+   			android:entries="@array/pref_angle_keys"
+   			android:entryValues="@array/pref_angle_values"
+   			android:defaultValue="@string/pref_default_angle"/>
+        <ListPreference 
+            android:key="@string/pref_key_triangle"
+    		android:title="@string/pref_title_triangle"
+    		android:summary="@string/pref_summary_triangle"
+   			android:entries="@array/pref_triangle_keys"
+   			android:entryValues="@array/pref_triangle_values"
+   			android:defaultValue="@string/pref_default_triangle"/>
+        <SwitchPreference
+            android:key="@string/pref_key_nodes_filtering"
+            android:title="@string/pref_title_nodes_filtering"
+            android:summary="@string/pref_summary_nodes_filtering"
+            android:defaultValue="@string/pref_default_nodes_filtering"/>
+    </PreferenceCategory>
+ 	 	
+    <PreferenceCategory
+        android:title="@string/pref_title_general">
+        
+        <PreferenceScreen
+            android:key="pref_button_mapping"
+            android:title="@string/pref_title_mapping_sub"
+            android:summary="@string/pref_summary_mapping"
+            android:persistent="false">
+        
+		    <SwitchPreference
+	            android:key="@string/pref_key_append"
+	            android:title="@string/pref_title_append"
+	            android:summary="@string/pref_summary_append"
+	            android:defaultValue="@string/pref_default_append"/>
+		    <SwitchPreference
+	            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"
+	            android:defaultValue="@string/pref_default_resolution"/>
+		    <SwitchPreference
+		        android:key="@string/pref_key_smoothing"
+		        android:title="@string/pref_title_smoothing"
+		        android:summary="@string/pref_summary_smoothing"
+		        android:defaultValue="@string/pref_default_smoothing"/>
+	  
+		     <PreferenceCategory
+	        	android:title="@string/pref_title_mapping_core">
+		        <ListPreference
+		            android:key="@string/pref_key_update_rate"
+		            android:title="@string/pref_title_update_rate"
+		            android:summary="@string/pref_summary_update_rate"
+		            android:entries="@array/pref_update_rate_keys"
+		   			android:entryValues="@array/pref_update_rate_values"
+		   			android:defaultValue="@string/pref_default_update_rate"/>
+		        <ListPreference 
+		            android:key="@string/pref_key_time_thr"
+		    		android:title="@string/pref_title_time_thr"
+		    		android:summary="@string/pref_summary_time_thr"
+		   			android:entries="@array/pref_time_thr_keys"
+		   			android:entryValues="@array/pref_time_thr_values"
+		   			android:defaultValue="@string/pref_default_time_thr"/>
+		        <ListPreference 
+		            android:key="@string/pref_key_mem_thr"
+		    		android:title="@string/pref_title_mem_thr"
+		    		android:summary="@string/pref_summary_mem_thr"
+		   			android:entries="@array/pref_mem_thr_keys"
+		   			android:entryValues="@array/pref_mem_thr_values"
+		   			android:defaultValue="@string/pref_default_mem_thr"/>
+		        <ListPreference 
+		            android:key="@string/pref_key_loop_thr"
+		    		android:title="@string/pref_title_loop_thr"
+		    		android:summary="@string/pref_summary_loop_thr"
+		   			android:entries="@array/pref_loop_thr_keys"
+		   			android:entryValues="@array/pref_loop_thr_values"
+		   			android:defaultValue="@string/pref_default_loop_thr"/>
+		        <ListPreference 
+		            android:key="@string/pref_key_sim_thr"
+		    		android:title="@string/pref_title_sim_thr"
+		    		android:summary="@string/pref_summary_sim_thr"
+		   			android:entries="@array/pref_sim_thr_keys"
+		   			android:entryValues="@array/pref_sim_thr_values"
+		   			android:defaultValue="@string/pref_default_sim_thr"/>
+		        <ListPreference 
+		            android:key="@string/pref_key_min_inliers"
+		    		android:title="@string/pref_title_min_inliers"
+		    		android:summary="@string/pref_summary_min_inliers"
+		   			android:entries="@array/pref_min_inliers_keys"
+		   			android:entryValues="@array/pref_min_inliers_values"
+		   			android:defaultValue="@string/pref_default_min_inliers"/>
+		        <ListPreference 
+		            android:key="@string/pref_key_opt_error"
+		    		android:title="@string/pref_title_opt_error"
+		    		android:summary="@string/pref_summary_opt_error"
+		   			android:entries="@array/pref_opt_error_keys"
+		   			android:entryValues="@array/pref_opt_error_values"
+		   			android:defaultValue="@string/pref_default_opt_error"/>
+		        <ListPreference 
+		            android:key="@string/pref_key_features_voc"
+		    		android:title="@string/pref_title_features_voc"
+		    		android:summary="@string/pref_summary_features_voc"
+		   			android:entries="@array/pref_features_voc_keys"
+		   			android:entryValues="@array/pref_features_voc_values"
+		   			android:defaultValue="@string/pref_default_features_voc"/>
+		        <ListPreference 
+		            android:key="@string/pref_key_features"
+		    		android:title="@string/pref_title_features"
+		    		android:summary="@string/pref_summary_features"
+		   			android:entries="@array/pref_features_keys"
+		   			android:entryValues="@array/pref_features_values"
+		   			android:defaultValue="@string/pref_default_features"/>
+		        <ListPreference 
+		            android:key="@string/pref_key_features_type"
+		    		android:title="@string/pref_title_features_type"
+		    		android:summary="@string/pref_summary_features_type"
+		   			android:entries="@array/pref_features_type_keys"
+		   			android:entryValues="@array/pref_features_type_values"
+		   			android:defaultValue="@string/pref_default_features_type"/>
+		        <ListPreference 
+		            android:key="@string/pref_key_optimizer"
+		    		android:title="@string/pref_title_optimizer"
+		    		android:summary="@string/pref_summary_optimizer"
+		   			android:entries="@array/pref_optimizer_keys"
+		   			android:entryValues="@array/pref_optimizer_values"
+		   			android:defaultValue="@string/pref_default_optimizer"/>
+		        <SwitchPreference 
+		            android:key="@string/pref_key_optimize_end"
+		    		android:title="@string/pref_title_optimize_end"
+		    		android:summary="@string/pref_summary_optimize_end"
+		   			android:defaultValue="@string/pref_default_optimize_end"/>
+		    </PreferenceCategory>
+	        <PreferenceCategory
+	        	android:title="@string/pref_title_mapping_database">
+		        <SwitchPreference
+		            android:key="@string/pref_key_keep_all_db"
+		            android:title="@string/pref_title_keep_all_db"
+		            android:summary="@string/pref_summary_keep_all_db"
+		            android:defaultValue="@string/pref_default_keep_all_db"/>
+		        <SwitchPreference
+		            android:key="@string/pref_key_raw_scan_saved"
+		            android:title="@string/pref_title_raw_scan_saved"
+		            android:summary="@string/pref_summary_raw_scan_saved"
+		            android:defaultValue="@string/pref_default_raw_scan_saved"/>
+		        <SwitchPreference
+		            android:key="@string/pref_key_db_in_memory"
+		            android:title="@string/pref_title_db_in_memory"
+		            android:summary="@string/pref_summary_db_in_memory"
+		            android:defaultValue="@string/pref_default_db_in_memory"/>
+		    </PreferenceCategory>
+	    </PreferenceScreen>
+    
+	    <PreferenceScreen
+	            android:key="pref_button_export"
+	            android:title="@string/pref_title_export_sub"
+	            android:summary="@string/pref_summary_export"
+	            android:persistent="false">
+	        
+	        <ListPreference 
+	            android:key="@string/pref_key_cloud_voxel"
+	    		android:title="@string/pref_title_cloud_voxel"
+	    		android:summary="@string/pref_summary_cloud_voxel"
+	   			android:entries="@array/pref_cloud_voxel_keys"
+	   			android:entryValues="@array/pref_cloud_voxel_values"
+	   			android:defaultValue="@string/pref_default_cloud_voxel"/>
+	        
+	        <ListPreference 
+	            android:key="@string/pref_key_texture_size"
+	    		android:title="@string/pref_title_texture_size"
+	    		android:summary="@string/pref_summary_texture_size"
+	   			android:entries="@array/pref_texture_size_keys"
+	   			android:entryValues="@array/pref_texture_size_values"
+	   			android:defaultValue="@string/pref_default_texture_size"/>
+	        
+	        <ListPreference 
+	            android:key="@string/pref_key_normal_k"
+	    		android:title="@string/pref_title_normal_k"
+	    		android:summary="@string/pref_summary_normal_k"
+	   			android:entries="@array/pref_normal_k_values"
+	   			android:entryValues="@array/pref_normal_k_values"
+	   			android:defaultValue="@string/pref_default_normal_k"/>
+	        
+	        <ListPreference 
+	            android:key="@string/pref_key_max_texture_distance"
+	    		android:title="@string/pref_title_max_texture_distance"
+	    		android:summary="@string/pref_summary_max_texture_distance"
+	   			android:entries="@array/pref_max_texture_distance_keys"
+	   			android:entryValues="@array/pref_max_texture_distance_values"
+	   			android:defaultValue="@string/pref_default_max_texture_distance"/>
+	        
+		    <SwitchPreference
+	            android:key="@string/pref_key_block_render"
+	            android:title="@string/pref_title_block_render"
+	            android:summary="@string/pref_summary_block_render"
+	            android:defaultValue="@string/pref_default_block_render"/>
+		    
+		    <PreferenceCategory
+	        android:title="@string/pref_title_optimized">
+	        
+	        
+		        <ListPreference 
+		            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:entryValues="@array/pref_opt_depth_values"
+		   			android:defaultValue="@string/pref_default_opt_depth"/>
+		        
+
+		        <ListPreference 
+		            android:key="@string/pref_key_opt_color_radius"
+		    		android:title="@string/pref_title_opt_color_radius"
+		    		android:summary="@string/pref_summary_opt_color_radius"
+		   			android:entries="@array/pref_opt_color_radius_keys"
+		   			android:entryValues="@array/pref_opt_color_radius_values"
+		   			android:defaultValue="@string/pref_default_opt_color_radius"/>
+		        
+		        <SwitchPreference
+		            android:key="@string/pref_key_opt_clean_white"
+		            android:title="@string/pref_title_opt_clean_white"
+		            android:summary="@string/pref_summary_opt_clean_white"
+		            android:defaultValue="@string/pref_default_opt_clean_white"/>
+		        
+		       </PreferenceCategory>
+	 	</PreferenceScreen>
+        
+        <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"
+   			android:entries="@array/pref_gain_max_radius_keys"
+   			android:entryValues="@array/pref_gain_max_radius_values"
+   			android:defaultValue="@string/pref_default_gain_max_radius"/>
+        <ListPreference 
+            android:key="@string/pref_key_min_cluster_size"
+    		android:title="@string/pref_title_min_cluster_size"
+    		android:summary="@string/pref_summary_min_cluster_size"
+   			android:entries="@array/pref_min_cluster_size_values"
+   			android:entryValues="@array/pref_min_cluster_size_values"
+   			android:defaultValue="@string/pref_default_min_cluster_size"/>
+        
+	    <Preference android:title="@string/pref_title_reset_button"
+	                android:key="@string/pref_key_reset_button"/>
+    </PreferenceCategory>
+</PreferenceScreen>

app/android/res/layout/activity_sketchfab.xml

@@ -0,0 +1,81 @@
+<?xml version="1.0" encoding="utf-8"?>
+<ScrollView xmlns:android="http://schemas.android.com/apk/res/android" 
+    android:layout_width="match_parent"
+    android:layout_height="match_parent">
+<LinearLayout android:layout_width="match_parent"
+              android:layout_height="match_parent"
+              android:orientation="vertical"
+              android:focusableInTouchMode="true" >
+              
+    <TextView android:id="@+id/text"
+              android:layout_width="wrap_content"
+              android:layout_height="wrap_content"
+              android:text="Model Name*:" />
+
+    <EditText
+        android:id="@+id/editText_filename"
+        android:layout_width="match_parent"
+        android:layout_height="wrap_content"
+        android:imeOptions="flagNoExtractUi"
+        android:ems="10" />
+
+    <TextView
+        android:id="@+id/textView1"
+        android:layout_width="wrap_content"
+        android:layout_height="wrap_content"
+        android:text="Description:" />
+
+    <EditText
+        android:id="@+id/editText_description"
+        android:ems="10"
+        android:lines="4" 
+	    android:minLines="2" 
+	    android:gravity="top|left" 
+	    android:maxLines="8" 
+	    android:layout_weight="1" 
+	    android:layout_height="wrap_content" 
+	    android:layout_width="fill_parent" 
+	    android:scrollbars="vertical"
+	    android:imeOptions="flagNoExtractUi"
+        android:inputType="textMultiLine" >
+
+        <requestFocus />
+    </EditText>
+
+    <TextView
+        android:id="@+id/textView2"
+        android:layout_width="wrap_content"
+        android:layout_height="wrap_content"
+        android:text="Tags:" />
+
+    <EditText
+        android:id="@+id/editText_tags"
+        android:layout_width="match_parent"
+        android:layout_height="wrap_content"
+        android:imeOptions="flagNoExtractUi"
+        android:ems="10" />
+
+    <CheckBox
+        android:id="@+id/checkBox_draft"
+        android:layout_width="wrap_content"
+        android:layout_height="wrap_content"
+        android:checked="true"
+        android:text="Draft Mode" />
+
+    <LinearLayout
+        android:layout_width="match_parent"
+        android:layout_height="0dip"
+        android:layout_weight="1"
+        android:gravity="right|bottom"
+        android:orientation="horizontal" >
+
+        <Button
+            android:id="@+id/button_ok"
+            android:layout_width="wrap_content"
+            android:layout_height="wrap_content"
+            android:layout_weight="0.06"
+            android:text="Upload" />
+    </LinearLayout>
+
+</LinearLayout>
+</ScrollView>

app/android/res/layout/auth_dialog.xml

@@ -0,0 +1,11 @@
+<?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="match_parent"
+    android:orientation="vertical" >
+      <WebView
+        android:layout_width="wrap_content"
+        android:layout_height="wrap_content"
+        android:id="@+id/webv"/>
+
+</LinearLayout>

app/android/res/menu/optionmenu.xml

@@ -1,31 +1,80 @@
 <?xml version="1.0" encoding="utf-8"?>
 <menu xmlns:android="http://schemas.android.com/apk/res/android">
         
-    <group android:id="@+id/group_pause" android:checkableBehavior="all">
-        <item android:id="@+id/pause" android:checked="false" android:title="Pause"/>
-    </group>
-    
     <group android:id="@+id/group_actions">
-       <item android:id="@+id/open" android:title="Open"/>
+       <group android:checkableBehavior="single">
-       <item android:id="@+id/save" android:title="Save"/>
+	      <item android:id="@+id/point_cloud" android:title="Point Cloud" />
-       <item android:id="@+id/export" android:title="Export (*.ply)"/>
+		  <item android:id="@+id/mesh" android:title="Mesh" />
-       <item android:id="@+id/reset" android:title="Reset"/>
+		  <item android:id="@+id/texture_mesh" android:checked="true" android:title="Texture Mesh" />
-       <item android:id="@+id/about" android:title="About"/>
 	   </group>
+	   <item android:id="@+id/save" android:title="Save" android:showAsAction="ifRoom"/>
                      
-    <item android:id="@+id/menu_settings" android:title="Options..." android:orderInCategory="2">
+       <item android:id="@+id/export" android:showAsAction="ifRoom" android:title="Export">
+          <menu>
+              <item android:id="@+id/export_point_cloud_menu" android:title="Point Cloud">
+                  <menu>
+                  	<item android:id="@+id/export_point_cloud" android:title="Current Density" />
+                  	<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" />
+			          <item android:id="@+id/export_optimized_mesh_texture" android:title="Textured Mesh" />
+		          </menu>
+	          </item>
+          </menu>
+       </item>
+       
+        <item android:id="@+id/post_processing" android:title="Optimize" android:showAsAction="ifRoom">
+          <menu>
+             <item android:id="@+id/post_processing_standard" android:title="Standard Optimization" />
+             <item android:id="@+id/post_processing_advanced" android:title="Advanced..." >
+	            <menu>
+	              <item android:id="@+id/global_graph_optimization" android:title="Global Graph Optimization" />
+	              <item android:id="@+id/detect_more_loop_closures" android:title="Detect More Loop Closures" />
+	              <item android:id="@+id/gain_compensation_fast" android:title="Adjust Colors (Fast)" />
+	              <item android:id="@+id/gain_compensation_full" android:title="Adjust Colors (Full)" />
+	              <item android:id="@+id/bilateral_filtering" android:title="Mesh Smoothing" />
+	              <item android:id="@+id/sba" android:title="Bundle Adjustement" />
+	              <item android:id="@+id/polygons_filtering" android:title="Noise Filtering" />
+	            </menu>  
+	         </item>
+          </menu>
+       </item>
+       
+       <item android:id="@+id/open" android:title="Open" android:showAsAction="ifRoom"/>
+       
+       <item android:id="@+id/menu_rendering_settings" android:title="Visibility...">
         <menu >
-        <group android:id="@+id/group_visibility" android:checkableBehavior="all">
+        <group android:id="@+id/group_rendering_visibility" android:checkableBehavior="all">
+            <item android:id="@+id/status" android:checked="true" android:title="Status" />
             <item android:id="@+id/debug" android:checked="false" android:title="Debug" />
-            <item android:id="@+id/localization_mode" android:checked="false" android:title="Localization Mode" />
-            <item android:id="@+id/trajectory_mode" android:checked="false" android:title="Trajectory Mode" />
-            <item android:id="@+id/mesh_rendering" android:checked="true" android:title="Mesh Rendering" />
-            <item android:id="@+id/auto_exposure" android:checked="false" android:title="Auto Exposure" />
             <item android:id="@+id/map_shown" android:checked="true" android:title="Map Visible" />
             <item android:id="@+id/odom_shown" android:checked="true" android:title="Odom Visible" />
             <item android:id="@+id/graph_visible" android:checked="true" android:title="Graph Visible" />
+            <item android:id="@+id/grid_visible" android:checked="true" android:title="Grid Visible" />
             <item android:id="@+id/graph_optimization" android:checked="true" android:title="Optimized Graph" />
         </group>   
         </menu>
 	    </item>
+	    <item android:id="@+id/modes" android:title="Modes...">
+	       <menu >
+	       <group android:id="@+id/group_mapping_visibility" android:checkableBehavior="all">
+	           <item android:id="@+id/localization_mode" android:checked="false" android:title="Localization Mode" />
+	           <item android:id="@+id/trajectory_mode" android:checked="false" android:title="Trajectory Mode" />
+	           <item android:id="@+id/data_recorder" android:checked="false" android:title="Data Recorder Mode" />
+	       </group>   
+	       </menu>
+	   </item>
+	   <item android:id="@+id/settings" android:title="Settings"/>
+       <item android:id="@+id/reset" android:title="Reset"/>
+       <item android:id="@+id/about" android:title="About"/>
+    </group>
 </menu>

app/android/res/values/strings.xml

@@ -1,24 +1,626 @@
 <?xml version="1.0" encoding="utf-8"?>
 <resources>
+
     <string name="app_name">RTAB-Map</string>
     <string name="sys_name">RTAB-Map</string>
-    <string name="menu_name">Real-Time Appearance-Based Mapping</string>
+    <string name="menu_name">RTAB-Map</string>
+    <string name="settings">Settings</string>
+    <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="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="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>
     <string name="start">Start</string>
-    <string name="nodes">"Nodes: "</string>
+    <string name="nodes">"Nodes (WM): "</string>
     <string name="points">"Number of points: "</string>
     <string name="update_time">"Update time (ms): "</string>
     <string name="loop_closure">"Loop closure ID: "</string>
-    <string name="db_size">"Database (MB): "</string>
+    <string name="free_memory">"Free Memory (MB): "</string>
+    <string name="database_size">"Database (MB): "</string>
     <string name="total_loop">"Loop closures: "</string>
     <string name="inliers">"Inliers: "</string>
     <string name="features">"Features: "</string>
+    <string name="rehearsal">"Rehearsal (%): "</string>
     <string name="polygons">"Polygons: "</string>
-    <string name="memory">"Memory (MB): "</string>
+    <string name="memory">"Used Memory (MB): "</string>
-    <string name="hypothesis">"Hypothesis: "</string>
+    <string name="hypothesis">"Hypothesis (%): "</string>
+    <string name="fps">"FPS (rendering): "</string>
+
+    <!-- Preference keys: BEGIN -->
+    <string name="pref_key_tags">pref_key_tags</string>
+    <string name="pref_default_tags">rtabmap 3dscan</string>
+    <string name="pref_key_rendering">pref_key_rendering</string>
+    <string name="pref_default_rendering">2</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>
+    <string name="pref_key_depth">pref_key_depth</string>
+    <string name="pref_default_depth">2.5</string>
+    <string name="pref_key_point_size">pref_key_point_size</string>
+    <string name="pref_default_point_size">5</string>
+    <string name="pref_key_angle">pref_key_angle</string>
+    <string name="pref_default_angle">15</string>
+    <string name="pref_key_triangle">pref_key_triangle</string>
+    <string name="pref_default_triangle">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>
+    <string name="pref_default_smoothing">true</string>
+    
+    <string name="pref_key_update_rate">pref_key_update_rate</string>
+    <string name="pref_default_update_rate">1</string>
+    <string name="pref_key_time_thr">pref_key_time_thr</string>
+    <string name="pref_default_time_thr">1000</string>
+    <string name="pref_key_mem_thr">pref_key_mem_thr</string>
+    <string name="pref_default_mem_thr">0</string>
+    <string name="pref_key_loop_thr">pref_key_loop_thr</string>
+    <string name="pref_default_loop_thr">0.11</string>
+    <string name="pref_key_sim_thr">pref_key_sim_thr</string>
+    <string name="pref_default_sim_thr">0.3</string>
+    <string name="pref_key_min_inliers">pref_key_min_inliers</string>
+    <string name="pref_default_min_inliers">25</string>
+    <string name="pref_key_opt_error">pref_key_opt_error</string>
+    <string name="pref_default_opt_error">0.1</string>
+    <string name="pref_key_features_voc">pref_key_features_voc</string>
+    <string name="pref_default_features_voc">200</string>
+    <string name="pref_key_features">pref_key_features</string>
+    <string name="pref_default_features">400</string>
+    <string name="pref_key_features_type">pref_key_features_type</string>
+    <string name="pref_default_features_type">6</string>
+    <string name="pref_key_optimizer">pref_key_optimizer</string>
+    <string name="pref_default_optimizer">2</string>
+    <string name="pref_key_optimize_end">pref_key_optimize_end</string>
+    <string name="pref_default_optimize_end">true</string>
+    <string name="pref_key_keep_all_db">pref_key_keep_all_db</string>
+    <string name="pref_default_keep_all_db">true</string>
+    <string name="pref_key_raw_scan_saved">pref_key_raw_scan_saved</string>
+    <string name="pref_default_raw_scan_saved">false</string>
+    <string name="pref_key_db_in_memory">pref_key_db_in_memory</string>
+    <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_key_texture_size">pref_key_texture_size</string>
+    <string name="pref_default_texture_size">4096</string>
+    <string name="pref_key_normal_k">pref_key_normal_k</string>
+    <string name="pref_default_normal_k">6</string>
+    <string name="pref_key_max_texture_distance">pref_key_max_texture_distance</string>
+    <string name="pref_default_max_texture_distance">3</string>
+    <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_key_opt_color_radius">pref_key_opt_color_radius</string>
+    <string name="pref_default_opt_color_radius">0.025</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_gain_max_radius">pref_key_gain_max_radius</string>
+    <string name="pref_default_gain_max_radius">0.02</string>
+    <string name="pref_key_min_cluster_size">pref_key_min_cluster_size</string>
+    <string name="pref_default_min_cluster_size">200</string>
+    <!-- Preference keys: END -->
+
+    <string name="pref_title_rendering">Rendering</string>
+    <string name="pref_title_density">Point Cloud Density</string>
+    <string name="pref_summary_density">Decrease density to reduce rendering time and memory. Tip: To apply a different density to current map: save the map, change density and re-open the same map to regenerate the point clouds at this density.</string>
+    <string name="pref_title_angle">Mesh Angle Tolerance</string>
+    <string name="pref_summary_angle">Minimum polygon angle.</string>
+    <string name="pref_title_triangle">Mesh Triangle Size</string>
+    <string name="pref_summary_triangle">Size in pixels of the polygons created from the depth image.</string>
+    <string name="pref_title_depth">Max Depth</string>
+    <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_nodes_filtering">Nodes Filtering</string>
+    <string name="pref_summary_nodes_filtering">Render only the newest point cloud of a loop closure.</string>
+
+    <string-array name="pref_density_keys">
+        <item>"Maximum"</item>
+        <item>"High"</item>
+        <item>"Low"</item>
+        <item>"Very Low"</item>
+    </string-array>
+    <string-array name="pref_density_values">
+        <item>"0"</item>
+        <item>"1"</item>
+        <item>"2"</item>
+        <item>"3"</item>
+    </string-array>
+    <string-array name="pref_depth_keys">
+        <item>"No Limit"</item>
+        <item>"5"</item>
+        <item>"4.5"</item>
+        <item>"4"</item>
+        <item>"3.5"</item>
+        <item>"3"</item>
+        <item>"2.5"</item>
+        <item>"2"</item>
+        <item>"1.5"</item>
+        <item>"1"</item>
+    </string-array>
+    <string-array name="pref_depth_values">
+        <item>"0"</item>
+        <item>"5"</item>
+        <item>"4.5"</item>
+        <item>"4"</item>
+        <item>"3.5"</item>
+        <item>"3"</item>
+        <item>"2.5"</item>
+        <item>"2"</item>
+        <item>"1.5"</item>
+        <item>"1"</item>
+    </string-array>
+    <string-array name="pref_point_size_values">
+        <item>"50"</item>
+        <item>"30"</item>
+        <item>"25"</item>
+        <item>"15"</item>
+        <item>"5"</item>
+        <item>"1"</item>
+    </string-array>
+    <string-array name="pref_angle_keys">
+        <item>"30 deg"</item>
+        <item>"25 deg"</item>
+        <item>"20 deg"</item>
+        <item>"15 deg"</item>
+        <item>"10 deg"</item>
+        <item>"5 deg"</item>
+    </string-array>
+    <string-array name="pref_angle_values">
+        <item>"30"</item>
+        <item>"25"</item>
+        <item>"20"</item>
+        <item>"15"</item>
+        <item>"10"</item>
+        <item>"5"</item>
+    </string-array>
+    <string-array name="pref_triangle_keys">
+        <item>"6 pix"</item>
+        <item>"5 pix"</item>
+        <item>"4 pix"</item>
+        <item>"3 pix"</item>
+        <item>"2 pix"</item>
+    </string-array>
+    <string-array name="pref_triangle_values">
+        <item>"6"</item>
+        <item>"5"</item>
+        <item>"4"</item>
+        <item>"3"</item>
+        <item>"2"</item>
+    </string-array>
+
+    <string name="pref_title_mapping_sub">Mapping&#8230;</string>
+    <string name="pref_title_mapping">Mapping</string>
+    <string name="pref_summary_mapping">Advanced mapping parameters for fine tuning.</string>
+    <string name="pref_title_mapping_core">Core</string>
+    <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 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_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>
+    <string name="pref_summary_time_thr">Maximum time allowed for map updates. If time to add a new node is above this theshold, some old parts of the map are temporarly forgotten to reduce time of next updates.</string>
+    <string name="pref_title_mem_thr">Memory Limit</string>
+    <string name="pref_summary_mem_thr">Maximum nodes kept in working memory.</string>
+    <string name="pref_title_loop_thr">Loop Closure Threshold</string>
+    <string name="pref_summary_loop_thr">Threshold at which loop closure hypotheses are accepted. Higher means more robust to false loop closures while rejecting more good loop closures.</string>
+    <string name="pref_title_sim_thr">Similarity Threshold</string>
+    <string name="pref_summary_sim_thr">Threshold at which consecutive images are considered the same, so the corresponding node\'s weight is increased. The background turns dark blue when this happens.</string>
+    <string name="pref_title_min_inliers">Min Inliers</string>
+    <string name="pref_summary_min_inliers">Minimum visual inliers to accept a loop closure.</string>
+    <string name="pref_title_opt_error">Max Optimization Error</string>
+    <string name="pref_summary_opt_error">Reject any loop closures causing error corrections in the map higher than this threshold.</string>
+    <string name="pref_title_features_voc">Max Features Extracted (Vocabulary)</string>
+    <string name="pref_summary_features_voc">Extracting more features per image would result in better loop closure hypotheses but more processing time is required.</string>
+    <string name="pref_title_features">Max Features Extracted (Loop Closure)</string>
+    <string name="pref_summary_features">Extracting more features per image would result in better loop closure transforms but more processing time is required.</string>
+    <string name="pref_title_features_type">Feature Type</string>
+    <string name="pref_summary_features_type">BRIEF features are fast to compute but are not rotation invariant like FREAK. Warning: Changing feature type will automatically reset the map!</string>
+    <string name="pref_title_optimizer">Graph Optimizer</string>
+    <string name="pref_summary_optimizer">Graph optimization approach.</string>
+    <string name="pref_title_optimize_end">Optimization from Graph End</string>
+    <string name="pref_summary_optimize_end">The map\'s graph is optimized from the last node. The map is moved when a loop closure happens instead of jumping the current pose back to localized area. </string>
+    <string name="pref_title_keep_all_db">Save All Frames in Database</string>
+    <string name="pref_summary_keep_all_db">Discarded frames while not moving are still saved in database. Useful to replay exactly the scanning on RTAB-Map Desktop.</string>
+    <string name="pref_title_raw_scan_saved">Save Raw Scan</string>
+    <string name="pref_summary_raw_scan_saved">Save raw point clouds in database.</string>
+    <string name="pref_title_db_in_memory">Database In Memory</string>
+    <string name="pref_summary_db_in_memory">The database is kept in RAM for fast access. Set to false to reduce RAM used at the cost of slower access. This parameter is applied on reset or when a database is opened.</string>
+
+    <string-array name="pref_update_rate_keys">
+        <item>"Max"</item>
+        <item>"5 Hz"</item>
+        <item>"4 Hz"</item>
+        <item>"3 Hz"</item>
+        <item>"2 Hz"</item>
+        <item>"1 Hz"</item>
+        <item>"0.5 Hz"</item>
+    </string-array>
+    <string-array name="pref_update_rate_values">
+        <item>"0"</item>
+        <item>"5"</item>
+        <item>"4"</item>
+        <item>"3"</item>
+        <item>"2"</item>
+        <item>"1"</item>
+        <item>"0.5"</item>
+    </string-array>
+    <string-array name="pref_time_thr_keys">
+        <item>"No Limit"</item>
+        <item>"1500 ms"</item>
+        <item>"1400 ms"</item>
+        <item>"1300 ms"</item>
+        <item>"1200 ms"</item>
+        <item>"1100 ms"</item>
+        <item>"1000 ms"</item>
+        <item>"900 ms"</item>
+        <item>"800 ms"</item>
+        <item>"700 ms"</item>
+        <item>"600 ms"</item>
+        <item>"500 ms"</item>
+        <item>"400"</item>
+    </string-array>
+    <string-array name="pref_time_thr_values">
+        <item>"0"</item>
+        <item>"1500"</item>
+        <item>"1400"</item>
+        <item>"1300"</item>
+        <item>"1200"</item>
+        <item>"1100"</item>
+        <item>"1000"</item>
+        <item>"900"</item>
+        <item>"800"</item>
+        <item>"700"</item>
+        <item>"600"</item>
+        <item>"500"</item>
+        <item>"400"</item>
+    </string-array>
+    <string-array name="pref_mem_thr_keys">
+        <item>"No Limit"</item>
+        <item>"500 nodes"</item>
+        <item>"400 nodes"</item>
+        <item>"300 nodes"</item>
+        <item>"200 nodes"</item>
+        <item>"100 nodes"</item>
+        <item>"50 nodes"</item>
+    </string-array>
+    <string-array name="pref_mem_thr_values">
+        <item>"0"</item>
+        <item>"500"</item>
+        <item>"400"</item>
+        <item>"300"</item>
+        <item>"200"</item>
+        <item>"100"</item>
+        <item>"50"</item>
+    </string-array>
+    <string-array name="pref_loop_thr_keys">
+        <item>"0.90"</item>
+        <item>"0.80"</item>
+        <item>"0.70"</item>
+        <item>"0.60"</item>
+        <item>"0.50"</item>
+        <item>"0.40"</item>
+        <item>"0.30"</item>
+        <item>"0.20"</item>
+        <item>"0.15"</item>
+        <item>"0.11"</item>
+        <item>"0.10"</item>
+    </string-array>
+    <string-array name="pref_loop_thr_values">
+        <item>"0.90"</item>
+        <item>"0.80"</item>
+        <item>"0.70"</item>
+        <item>"0.60"</item>
+        <item>"0.50"</item>
+        <item>"0.40"</item>
+        <item>"0.30"</item>
+        <item>"0.20"</item>
+        <item>"0.15"</item>
+        <item>"0.11"</item>
+        <item>"0.10"</item>
+    </string-array>
+    <string-array name="pref_sim_thr_keys">
+        <item>"Disabled"</item>
+        <item>"0.60"</item>
+        <item>"0.50"</item>
+        <item>"0.40"</item>
+        <item>"0.30"</item>
+        <item>"0.20"</item>
+        <item>"0.10"</item>
+    </string-array>
+    <string-array name="pref_sim_thr_values">
+        <item>"0"</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>
+    </string-array>
+    <string-array name="pref_min_inliers_keys">
+        <item>"35"</item>
+        <item>"30"</item>
+        <item>"25"</item>
+        <item>"20"</item>
+        <item>"15"</item>
+        <item>"10"</item>
+    </string-array>
+    <string-array name="pref_min_inliers_values">
+        <item>"35"</item>
+        <item>"30"</item>
+        <item>"25"</item>
+        <item>"20"</item>
+        <item>"15"</item>
+        <item>"10"</item>
+    </string-array>
+    <string-array name="pref_opt_error_keys">
+        <item>"1.0 m"</item>
+        <item>"0.5 m"</item>
+        <item>"0.35 m"</item>
+        <item>"0.2 m"</item>
+        <item>"0.1 m"</item>
+        <item>"0.05 m"</item>
+        <item>"0.025 m"</item>
+        <item>"0.01 m"</item>
+        <item>"Disabled"</item>
+    </string-array>
+    <string-array name="pref_opt_error_values">
+        <item>"1.0"</item>
+        <item>"0.5"</item>
+        <item>"0.35"</item>
+        <item>"0.2"</item>
+        <item>"0.1"</item>
+        <item>"0.05"</item>
+        <item>"0.025"</item>
+        <item>"0.01"</item>
+        <item>"0"</item>
+    </string-array>
+    <string-array name="pref_features_voc_keys">
+        <item>"No Limit"</item>
+        <item>"1000"</item>
+        <item>"900"</item>
+        <item>"800"</item>
+        <item>"700"</item>
+        <item>"600"</item>
+        <item>"500"</item>
+        <item>"400"</item>
+        <item>"300"</item>
+        <item>"200"</item>
+        <item>"100"</item>
+        <item>"Disabled"</item>
+    </string-array>
+    <string-array name="pref_features_voc_values">
+        <item>"0"</item>
+        <item>"1000"</item>
+        <item>"900"</item>
+        <item>"800"</item>
+        <item>"700"</item>
+        <item>"600"</item>
+        <item>"500"</item>
+        <item>"400"</item>
+        <item>"300"</item>
+        <item>"200"</item>
+        <item>"100"</item>
+        <item>"-1"</item>
+    </string-array>
+    <string-array name="pref_features_keys">
+        <item>"No Limit"</item>
+        <item>"1000"</item>
+        <item>"900"</item>
+        <item>"800"</item>
+        <item>"700"</item>
+        <item>"600"</item>
+        <item>"500"</item>
+        <item>"400"</item>
+        <item>"300"</item>
+        <item>"200"</item>
+        <item>"100"</item>
+    </string-array>
+    <string-array name="pref_features_values">
+        <item>"0"</item>
+        <item>"1000"</item>
+        <item>"900"</item>
+        <item>"800"</item>
+        <item>"700"</item>
+        <item>"600"</item>
+        <item>"500"</item>
+        <item>"400"</item>
+        <item>"300"</item>
+        <item>"200"</item>
+        <item>"100"</item>
+    </string-array>
+    <string-array name="pref_features_type_keys">
+        <item>"BRIEF"</item>
+        <item>"FREAK"</item>
+    </string-array>
+    <string-array name="pref_features_type_values">
+        <item>"6"</item>
+        <item>"5"</item>
+    </string-array>
+    <string-array name="pref_optimizer_keys">
+        <item>"GTSAM"</item>
+        <item>"g2o"</item>
+        <item>"TORO"</item>
+    </string-array>
+    <string-array name="pref_optimizer_values">
+        <item>"2"</item>
+        <item>"1"</item>
+        <item>"0"</item>
+    </string-array>
+
+    <string name="pref_title_export_sub">Exporting&#8230;</string>
+    <string name="pref_title_export">Exporting</string>
+    <string name="pref_summary_export">Advanced parameters used when exporting the map.</string>
+    <string name="pref_title_cloud_voxel">Voxel Size</string>
+    <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_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>
+    <string name="pref_summary_max_texture_distance">Maximum distance from a camera for polygons to be textured by this camera.</string>
+    <string name="pref_title_block_render">Block Rendering Thread While Exporting</string>
+    <string name="pref_summary_block_render">This decreases exporting time, but freezes rendering while exporting. This also clears temporary the rendered clouds/meshes from memory during exporting, this can be useful to avoid out of memory errors.</string>
+
+    <string-array name="pref_cloud_voxel_keys">
+        <item>"0.2 m"</item>
+        <item>"0.1 m"</item>
+        <item>"0.05 m"</item>
+        <item>"0.02 m"</item>
+        <item>"0.01 m"</item>
+        <item>"0.005 m"</item>
+        <item>"Disabled"</item>
+    </string-array>
+    <string-array name="pref_cloud_voxel_values">
+        <item>"0.2"</item>
+        <item>"0.1"</item>
+        <item>"0.05"</item>
+        <item>"0.02"</item>
+        <item>"0.01"</item>
+        <item>"0.005"</item>
+        <item>"0"</item>
+    </string-array>
+    <string-array name="pref_texture_size_keys">
+        <item>"16384x16384"</item>
+        <item>"8192x8192"</item>
+        <item>"4096x4096"</item>
+        <item>"2048x2048"</item>
+        <item>"1024x1024"</item>
+    </string-array>
+    <string-array name="pref_texture_size_values">
+        <item>"16384"</item>
+        <item>"8192"</item>
+        <item>"4096"</item>
+        <item>"2048"</item>
+        <item>"1024"</item>
+    </string-array>
+    <string-array name="pref_normal_k_values">
+        <item>"30"</item>
+        <item>"24"</item>
+        <item>"18"</item>
+        <item>"12"</item>
+        <item>"6"</item>
+    </string-array>
+    <string-array name="pref_max_texture_distance_keys">
+        <item>"No Limit"</item>
+        <item>"5 m"</item>
+        <item>"4.5 m"</item>
+        <item>"4 m"</item>
+        <item>"3.5 m"</item>
+        <item>"3 m"</item>
+        <item>"2.5 m"</item>
+        <item>"2 m"</item>
+    </string-array>
+    <string-array name="pref_max_texture_distance_values">
+        <item>"0"</item>
+        <item>"5"</item>
+        <item>"4.5"</item>
+        <item>"4"</item>
+        <item>"3.5"</item>
+        <item>"3"</item>
+        <item>"2.5"</item>
+        <item>"2"</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_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-array name="pref_opt_depth_values">
+        <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_color_radius_keys">
+        <item>"Nearest"</item>
+        <item>"2 m"</item>
+        <item>"1 m"</item>
+        <item>"0.5 m"</item>
+        <item>"0.2 m"</item>
+        <item>"0.1 m"</item>
+        <item>"0.05 m"</item>
+        <item>"0.025 m"</item>
+        <item>"0.01 m"</item>
+        <item>"Disabled"</item>
+    </string-array>
+    <string-array name="pref_opt_color_radius_values">
+        <item>"0"</item>
+        <item>"2"</item>
+        <item>"1"</item>
+        <item>"0.5"</item>
+        <item>"0.2"</item>
+        <item>"0.1"</item>
+        <item>"0.05"</item>
+        <item>"0.025"</item>
+        <item>"0.01"</item>
+        <item>"-1"</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_min_cluster_size">Min Cluster Size</string>
+    <string name="pref_summary_min_cluster_size">Minimum number of polygons for a cluster to be kept after Noise Filtering optimization.</string>
+
+    <string-array name="pref_gain_max_radius_keys">
+        <item>"0.3 m"</item>
+        <item>"0.2 m"</item>
+        <item>"0.1 m"</item>
+        <item>"0.05 m"</item>
+        <item>"0.02 m"</item>
+        <item>"0.01 m"</item>
+    </string-array>
+    <string-array name="pref_gain_max_radius_values">
+        <item>"0.3"</item>
+        <item>"0.2"</item>
+        <item>"0.1"</item>
+        <item>"0.05"</item>
+        <item>"0.02"</item>
+        <item>"0.01"</item>
+    </string-array>
+    <string-array name="pref_min_cluster_size_values">
+        <item>"500"</item>
+        <item>"200"</item>
+        <item>"100"</item>
+        <item>"50"</item>
+        <item>"10"</item>
+    </string-array>
+
+    <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>
+    <string name="action_settings">Settings</string>
 
 </resources>

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

@@ -8,11 +8,13 @@ import java.io.BufferedReader;
 import java.io.IOException;
 import java.io.InputStream;
 import java.io.InputStreamReader;
+
 import android.app.Dialog;
 import android.content.Context;
 import android.graphics.Color;
 import android.os.Bundle;
 import android.text.Html;
+import android.text.method.LinkMovementMethod;
 import android.text.util.Linkify;
 import android.widget.TextView;
 
@@ -33,8 +35,10 @@ public class AboutDialog extends Dialog{
 		setContentView(R.layout.about);
 		TextView tv = (TextView)findViewById(R.id.info_text);
 		tv.setText(Html.fromHtml(readRawTextFile(R.raw.info)));
+		tv.setMovementMethod(LinkMovementMethod.getInstance());
 		tv.setLinkTextColor(Color.WHITE);
 		Linkify.addLinks(tv, Linkify.ALL);
+		tv.append(Html.fromHtml("<b><a href=http://introlab.github.io/rtabmap/index.html#privacy-policy>Privacy Policy</a></b>"));
 	}
 	
 	public static String readRawTextFile(int id) {

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

@@ -0,0 +1,148 @@
+package com.introlab.rtabmap;
+ 
+import java.io.BufferedReader;
+ 
+import java.io.File;
+import java.io.FileInputStream;
+import java.io.IOException;
+import java.io.InputStreamReader;
+import java.io.OutputStream;
+import java.io.OutputStreamWriter;
+import java.io.PrintWriter;
+import java.net.HttpURLConnection;
+import java.net.URL;
+import java.net.URLConnection;
+import java.util.ArrayList;
+import java.util.List;
+ 
+/**
+ * This utility class provides an abstraction layer for sending multipart HTTP
+ * POST requests to a web server.
+ * @author www.codejava.net
+ * http://www.codejava.net/java-se/networking/upload-files-by-sending-multipart-request-programmatically
+ *
+ */
+public class MultipartUtility {
+    private final String boundary;
+    private static final String LINE_FEED = "\r\n";
+    private HttpURLConnection httpConn;
+    private String charset;
+    private OutputStream outputStream;
+    private PrintWriter writer;
+ 
+    /**
+     * This constructor initializes a new HTTP POST request with content type
+     * is set to multipart/form-data
+     * @param requestURL
+     * @param charset
+     * @throws IOException
+     */
+    public MultipartUtility(String requestURL, String token, String charset)
+            throws IOException {
+        this.charset = charset;
+         
+        // creates a unique boundary based on time stamp
+        boundary = "===" + System.currentTimeMillis() + "===";
+         
+        URL url = new URL(requestURL);
+        httpConn = (HttpURLConnection) url.openConnection();
+        httpConn.setUseCaches(false);
+        httpConn.setDoOutput(true); // indicates POST method
+        httpConn.setDoInput(true);
+        httpConn.setRequestProperty("Content-Type", "multipart/form-data; boundary=" + boundary);
+        httpConn.setRequestProperty("Authorization", "Bearer " + token);
+        outputStream = httpConn.getOutputStream();
+        writer = new PrintWriter(new OutputStreamWriter(outputStream, charset),
+                true);
+    }
+ 
+    /**
+     * Adds a form field to the request
+     * @param name field name
+     * @param value field value
+     */
+    public void addFormField(String name, String value) {
+        writer.append("--" + boundary).append(LINE_FEED);
+        writer.append("Content-Disposition: form-data; name=\"" + name + "\"").append(LINE_FEED);
+        writer.append("Content-Type: text/plain; charset=" + charset).append(LINE_FEED);
+        writer.append(LINE_FEED);
+        writer.append(value).append(LINE_FEED);
+        writer.flush();
+    }
+ 
+    /**
+     * Adds a upload file section to the request
+     * @param fieldName name attribute in <input type="file" name="..." />
+     * @param uploadFile a File to be uploaded
+     * @throws IOException
+     */
+    public void addFilePart(String fieldName, File uploadFile)
+            throws IOException {
+        String fileName = uploadFile.getName();
+        writer.append("--" + boundary).append(LINE_FEED);
+        writer.append(
+                "Content-Disposition: form-data; name=\"" + fieldName
+                        + "\"; filename=\"" + fileName + "\"")
+                .append(LINE_FEED);
+        writer.append(
+                "Content-Type: "
+                        + URLConnection.guessContentTypeFromName(fileName))
+                .append(LINE_FEED);
+        writer.append("Content-Transfer-Encoding: binary").append(LINE_FEED);
+        writer.append(LINE_FEED);
+        writer.flush();
+ 
+        FileInputStream inputStream = new FileInputStream(uploadFile);
+        byte[] buffer = new byte[4096];
+        int bytesRead = -1;
+        while ((bytesRead = inputStream.read(buffer)) != -1) {
+            outputStream.write(buffer, 0, bytesRead);
+        }
+        outputStream.flush();
+        inputStream.close();
+         
+        writer.append(LINE_FEED);
+        writer.flush();    
+    }
+ 
+    /**
+     * Adds a header field to the request.
+     * @param name - name of the header field
+     * @param value - value of the header field
+     */
+    public void addHeaderField(String name, String value) {
+        writer.append(name + ": " + value).append(LINE_FEED);
+        writer.flush();
+    }
+     
+    /**
+     * Completes the request and receives response from the server.
+     * @return a list of Strings as response in case the server returned
+     * status OK, otherwise an exception is thrown.
+     * @throws IOException
+     */
+    public List<String> finish() throws IOException {
+        List<String> response = new ArrayList<String>();
+ 
+        writer.append(LINE_FEED).flush();
+        writer.append("--" + boundary + "--").append(LINE_FEED);
+        writer.close();
+ 
+        // checks server's status code first
+        int status = httpConn.getResponseCode();
+        if (status == 201) {
+            BufferedReader reader = new BufferedReader(new InputStreamReader(
+                    httpConn.getInputStream()));
+            String line = null;
+            while ((line = reader.readLine()) != null) {
+                response.add(line);
+            }
+            reader.close();
+            httpConn.disconnect();
+        } else {
+            throw new IOException("Server returned non-OK status: " + status + ", msg: " + httpConn.getResponseMessage());
+        }
+ 
+        return response;
+    }
+}

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

@@ -1,6 +1,8 @@
 
 package com.introlab.rtabmap;
+import android.os.IBinder;
 import android.view.KeyEvent;
+import android.util.Log;
 
 
 // Wrapper for native library
@@ -8,19 +10,31 @@ import android.view.KeyEvent;
 public class RTABMapLib
 {
 
-    static
+	static {
-    {
+        // This project depends on tango_client_api, so we need to make sure we load
+        // the correct library first.
+        if (TangoInitializationHelper.loadTangoSharedLibrary() ==
+                TangoInitializationHelper.ARCH_ERROR) {
+            Log.e(RTABMapActivity.class.getSimpleName(), "ERROR! Unable to load libtango_client_api.so!");
+        }
         System.loadLibrary("NativeRTABMap");
     }
 
  // Initialize the Tango Service, this function starts the communication
     // between the application and Tango Service.
     // The activity object is used for checking if the API version is outdated.
-    public static native int initialize(RTABMapActivity activity);
+    public static native void onCreate(RTABMapActivity activity);
     
-    public static native void openDatabase(String databasePath);
+    public static native void setScreenRotation(int displayRotation, int cameraRotation);
     
-    public static native int onResume();
+    public static native int openDatabase(String databasePath, boolean databaseInMemory, boolean optimize);
+    
+    /*
+     * Called when the Tango service is connected.
+     *
+     * @param binder The native binder object.
+     */
+    public static native boolean onTangoServiceConnected(IBinder binder);
 
     // Release all non OpenGl resources that are allocated from the program.
     public static native void onPause();
@@ -46,16 +60,51 @@ public class RTABMapLib
     public static native void setPausedMapping(boolean paused);
     public static native void setMapCloudShown(boolean shown);
     public static native void setOdomCloudShown(boolean shown);
-    public static native void setMeshRendering(boolean enabled);
+    public static native void setMeshRendering(boolean enabled, boolean withTexture);
     public static native void setLocalizationMode(boolean enabled);
     public static native void setTrajectoryMode(boolean enabled);
     public static native void setGraphOptimization(boolean enabled);
+    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);
+    public static native void setAppendMode(boolean enabled);
+    public static native void setDataRecorderMode(boolean enabled);
+    public static native void setMaxCloudDepth(float value);
+    public static native void setPointSize(float value);
+    public static native void setLighting(boolean enabled);
+    public static native void setBackfaceCulling(boolean enabled);
+    public static native void setMeshDecimation(int value);
+    public static native void setMeshAngleTolerance(float value);
+    public static native void setMeshTriangleSize(int value);
+    public static native void setMinClusterSize(int value);
+    public static native void setMaxGainRadius(float value);
+    public static native int setMappingParameter(String key, String value);
 
     public static native void resetMapping();
-    public static native void save();
+    public static native void save(String outputDatabasePath);
-    public static native boolean exportMesh(String filePath);
+    public static native void cancelProcessing();
+    public static native boolean exportMesh(
+    		String filePath,
+    		float cloudVoxelSize,
+    		boolean regenerateCloud,
+    		boolean meshing,
+    		int textureSize,
+    		int normalK,
+    		float maxTextureDistance,
+    		boolean optimized,
+    		float optimizedVoxelSize,
+    		int optimizedDepth,
+    		int optimizedMaxPolygons,
+    		float optimizedColorRadius,
+    		boolean optimizedCleanWhitePolygons,
+    		boolean optimizedColorWhitePolygons,
+    		boolean blockRendering);
+    public static native boolean postExportation(boolean visualize);
+    public static native int postProcessing(int approach);
     
     public static native String getStatus();
     public static native int getTotalNodes();

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

@@ -16,8 +16,16 @@
 
 package com.introlab.rtabmap;
 
+import java.util.Vector;
+import java.util.concurrent.locks.ReentrantLock;
+
+import android.app.Activity;
 import android.app.ProgressDialog;
+import android.opengl.GLES20;
 import android.opengl.GLSurfaceView;
+import android.opengl.Matrix;
+import android.util.Log;
+import android.widget.Toast;
 
 import javax.microedition.khronos.egl.EGLConfig;
 import javax.microedition.khronos.opengles.GL10;
@@ -27,29 +35,160 @@ import javax.microedition.khronos.opengles.GL10;
 // device's pose.
 public class Renderer implements GLSurfaceView.Renderer {
 
-	private ProgressDialog mProgressDialog;
+	private final float[] mtrxProjection = new float[16];
+	private final float[] mtrxView = new float[16];
+	private final float[] mtrxProjectionAndView = new float[16];
+
+	private TextManager mTextManager = null;
+	private float mSurfaceHeight = 0.0f;
+	
+	private Vector<TextObject> mTexts;
+
+	private static RTABMapActivity mActivity;
+	public Renderer(RTABMapActivity c) {
+		mActivity = c;
+	}
+
+	private ProgressDialog mProgressDialog = null;
+	private Toast mToast = null;
+	
+	private boolean mTextChanged = false;
+	private ReentrantLock mTextLock = new ReentrantLock();
 
 	public void setProgressDialog(ProgressDialog progressDialog)
 	{
 		mProgressDialog = progressDialog;
 	}
 
-  // Render loop of the Gl context.
+	public void setToast(Toast toast)
-  public void onDrawFrame(GL10 gl) {
-    int value = RTABMapLib.render();
-    if(value == 1 && mProgressDialog != null)
 	{
+		mToast = toast;
+	}
+
+	// Render loop of the Gl context.
+	public void onDrawFrame(GL10 useGLES20instead) {
+
+		try
+		{
+			final int value = RTABMapLib.render();
+
+			if(mTextManager!=null)
+			{
+				if(mTextChanged)
+				{
+					mTextChanged = false;
+					Vector<TextObject> txtcollection = new Vector<TextObject>();
+					
+					mTextLock.lock();
+				    try {
+				    	if(mTexts.size() > 0)
+				    	{
+				    		txtcollection.addAll(mTexts);
+				    	}
+				    } finally {
+				    	mTextLock.unlock();
+				    }
+				    
+					// Prepare the text for rendering
+					mTextManager.PrepareDraw(txtcollection);
+				}
+				
+				mTextManager.Draw(mtrxProjectionAndView);
+			}
+
+			mActivity.runOnUiThread(new Runnable() {
+				public void run() {
+					if(value != 0 && mProgressDialog != null && mProgressDialog.isShowing())
+					{
+						Log.i("RTABMapActivity", "Renderer: dismiss dialog, value received=" + String.valueOf(value));
 						mProgressDialog.dismiss();
+						mActivity.stopUpdateStatusThread();
+					}
+					if(value==-1 && mToast!=null)
+					{
+						mToast.makeText(mActivity, String.format("Out of Memory!"), Toast.LENGTH_SHORT).show();
+					}
+				} 
+			});
+		}
+		catch(final Exception e)
+		{
+			if(mToast!=null)
+			{
+				mActivity.runOnUiThread(new Runnable() {
+					public void run() {
+						mToast.makeText(mActivity, String.format("Rendering error! %s", e.getMessage()), Toast.LENGTH_SHORT).show();
+					}
+
+				});
+			}
 		}
 	}
 
 	// Called when the surface size changes.
-  public void onSurfaceChanged(GL10 gl, int width, int height) {
+	public void onSurfaceChanged(GL10 useGLES20instead, int width, int height) {
+		
 		RTABMapLib.setupGraphic(width, height);
+		
+		mSurfaceHeight = (float)height;
+
+		// Clear our matrices
+		for(int i=0;i<16;i++)
+		{
+			mtrxProjection[i] = 0.0f;
+			mtrxView[i] = 0.0f;
+			mtrxProjectionAndView[i] = 0.0f;
+		}
+
+		// Setup our screen width and height for normal sprite translation.
+		Matrix.orthoM(mtrxProjection, 0, 0f, width, 0.0f, height, 0, 50);
+
+		// Set the camera position (View matrix)
+		Matrix.setLookAtM(mtrxView, 0, 0f, 0f, 1f, 0f, 0f, 0f, 0f, 1.0f, 0.0f);
+
+		// Calculate the projection and view transformation
+		Matrix.multiplyMM(mtrxProjectionAndView, 0, mtrxProjection, 0, mtrxView, 0);
 	}
 
 	// Called when the surface is created or recreated.
-  public void onSurfaceCreated(GL10 gl, EGLConfig config) {
+	public void onSurfaceCreated(GL10 useGLES20instead, EGLConfig config) {
+
 		RTABMapLib.initGlContent();
+		
+		// Create our text manager
+		mTextManager = new TextManager(mActivity);
+		
+		GLES20.glEnable(GLES20.GL_BLEND);
+	    GLES20.glBlendFunc(GLES20.GL_ONE, GLES20.GL_ONE_MINUS_SRC_ALPHA);
+	}
+	
+	public void updateTexts(String[] texts)
+	{
+		if(mTextManager != null && mSurfaceHeight > 0.0f)
+		{
+			Vector<TextObject> textObjects = new Vector<TextObject>();
+			float offset = mSurfaceHeight-mTextManager.getMaxTextHeight();
+			if(texts != null)
+			{
+				for(int i=0;i<texts.length; ++i)
+				{
+					if(texts[i]!=null && texts[i].length()>0)
+					{
+						TextObject txt = new TextObject(texts[i], 0, offset);
+						textObjects.add(txt);
+					}
+					offset-=mTextManager.getMaxTextHeight();
+				}
+			}
+	
+			mTextLock.lock();
+			try {
+				mTexts = textObjects;
+			} finally {
+				mTextLock.unlock();
+			}
+	
+			mTextChanged = true;
+		}
 	}
 }

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

@@ -0,0 +1,112 @@
+package com.introlab.rtabmap;
+
+import android.app.Activity;
+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;
+
+public class SettingsActivity extends PreferenceActivity implements OnSharedPreferenceChangeListener {
+	
+    @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() {
+        	@Override
+        	public boolean onPreferenceClick(Preference preference) {   
+        		getPreferenceScreen().getSharedPreferences().edit().clear().commit();
+        		
+                recreate();
+        		
+        		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_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_update_rate))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_update_rate))).getEntry() + ") "+getString(R.string.pref_summary_update_rate));
+        ((Preference)findPreference(getString(R.string.pref_key_time_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_time_thr))).getEntry() + ") "+getString(R.string.pref_summary_time_thr));
+        ((Preference)findPreference(getString(R.string.pref_key_mem_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_mem_thr))).getEntry() + ") "+getString(R.string.pref_summary_mem_thr));
+        ((Preference)findPreference(getString(R.string.pref_key_loop_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_loop_thr))).getEntry() + ") "+getString(R.string.pref_summary_loop_thr));
+        ((Preference)findPreference(getString(R.string.pref_key_sim_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_sim_thr))).getEntry() + ") "+getString(R.string.pref_summary_sim_thr));
+        ((Preference)findPreference(getString(R.string.pref_key_min_inliers))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_min_inliers))).getEntry() + ") "+getString(R.string.pref_summary_min_inliers));
+        ((Preference)findPreference(getString(R.string.pref_key_opt_error))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_opt_error))).getEntry() + ") "+getString(R.string.pref_summary_opt_error));
+        ((Preference)findPreference(getString(R.string.pref_key_features_voc))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_features_voc))).getEntry() + ") "+getString(R.string.pref_summary_features_voc));
+        ((Preference)findPreference(getString(R.string.pref_key_features))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_features))).getEntry() + ") "+getString(R.string.pref_summary_features));
+        ((Preference)findPreference(getString(R.string.pref_key_features_type))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_features_type))).getEntry() + ") "+getString(R.string.pref_summary_features_type));
+        ((Preference)findPreference(getString(R.string.pref_key_optimizer))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_optimizer))).getEntry() + ") "+getString(R.string.pref_summary_optimizer));
+        
+        ((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_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_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_min_cluster_size))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_min_cluster_size))).getEntry() + ") "+getString(R.string.pref_summary_min_cluster_size));
+        ((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));
+    }
+    
+    public void onSharedPreferenceChanged(SharedPreferences sharedPreferences, String key) {
+        Preference pref = findPreference(key);
+
+        if (pref instanceof ListPreference) {
+        	if(key.compareTo(getString(R.string.pref_key_density))==0) pref.setSummary("("+ ((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_density));
+        	if(key.compareTo(getString(R.string.pref_key_depth))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_depth));
+        	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_update_rate))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_update_rate));
+        	if(key.compareTo(getString(R.string.pref_key_time_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_time_thr));
+        	if(key.compareTo(getString(R.string.pref_key_mem_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_mem_thr));
+        	if(key.compareTo(getString(R.string.pref_key_loop_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_loop_thr));
+        	if(key.compareTo(getString(R.string.pref_key_sim_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_sim_thr));
+        	if(key.compareTo(getString(R.string.pref_key_min_inliers))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_min_inliers));
+        	if(key.compareTo(getString(R.string.pref_key_opt_error))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_opt_error));
+        	if(key.compareTo(getString(R.string.pref_key_features_voc))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_features_voc));
+        	if(key.compareTo(getString(R.string.pref_key_features))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_features));
+        	if(key.compareTo(getString(R.string.pref_key_features_type))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_features_type));
+        	if(key.compareTo(getString(R.string.pref_key_optimizer))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_optimizer));
+        	
+        	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_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_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_min_cluster_size))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_min_cluster_size));
+        	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));
+        
+        }
+    }
+    
+    @Override
+    protected void onResume() {
+        super.onResume();
+        // Set up a listener whenever a key changes
+        getPreferenceScreen().getSharedPreferences()
+                .registerOnSharedPreferenceChangeListener(this);
+    }
+
+    @Override
+    protected void onPause() {
+        super.onPause();
+        // Unregister the listener whenever a key changes
+        getPreferenceScreen().getSharedPreferences()
+                .unregisterOnSharedPreferenceChangeListener(this);
+    }
+}

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

@@ -0,0 +1,504 @@
+package com.introlab.rtabmap;
+
+import java.io.BufferedInputStream;
+import java.io.BufferedOutputStream;
+import java.io.BufferedReader;
+import java.io.File;
+import java.io.FileInputStream;
+import java.io.FileOutputStream;
+import java.io.IOException;
+import java.io.InputStream;
+import java.io.InputStreamReader;
+import java.net.SocketTimeoutException;
+import java.text.SimpleDateFormat;
+import java.util.Date;
+import java.util.List;
+import java.util.zip.ZipEntry;
+import java.util.zip.ZipOutputStream;
+
+import org.apache.http.HttpEntity;
+import org.apache.http.HttpResponse;
+import org.apache.http.client.HttpClient;
+import org.apache.http.client.methods.HttpPatch;
+import org.apache.http.entity.StringEntity;
+import org.apache.http.impl.client.DefaultHttpClient;
+
+import android.app.Activity;
+import android.app.AlertDialog;
+import android.app.Dialog;
+import android.app.ProgressDialog;
+import android.content.Context;
+import android.content.DialogInterface;
+import android.content.Intent;
+import android.content.SharedPreferences;
+import android.net.ConnectivityManager;
+import android.net.NetworkInfo;
+import android.os.AsyncTask;
+import android.os.Bundle;
+import android.preference.PreferenceManager;
+import android.text.Editable;
+import android.text.InputType;
+import android.text.SpannableString;
+import android.text.TextWatcher;
+import android.text.method.LinkMovementMethod;
+import android.text.util.Linkify;
+import android.util.Log;
+import android.view.View;
+import android.view.View.OnClickListener;
+import android.webkit.WebView;
+import android.webkit.WebViewClient;
+import android.widget.Button;
+import android.widget.CheckBox;
+import android.widget.EditText;
+import android.widget.TextView;
+import android.widget.Toast;
+import android.widget.ToggleButton;
+
+public class SketchfabActivity extends Activity implements OnClickListener {
+	
+	private static final String AUTHORIZE_PATH	= "https://sketchfab.com/oauth2/authorize";
+	private static final String CLIENT_ID		= "RXrIJYAwlTELpySsyM8TrK9r3kOGQ5Qjj9VVDIfV";
+	private static final String REDIRECT_URI	= "https://introlab.github.io/rtabmap/oauth2_redirect";
+	
+	public static final int ZIP_BUFFER_SIZE = 1<<20; // 1MB
+	
+	ProgressDialog mProgressDialog;
+	
+	private Dialog mAuthDialog;
+	
+	private String mAuthToken;
+	private boolean mExportedOBJ;
+	private String mWorkingDirectory;
+	
+	EditText mFilename;
+	EditText mDescription;
+	EditText mTags;
+	CheckBox mDraft;
+	Button mButtonOk;
+	
+	private SketchfabActivity getActivity() {return this;}
+
+	@Override
+	protected void onCreate(Bundle savedInstanceState) {
+		super.onCreate(savedInstanceState);
+		setContentView(R.layout.activity_sketchfab);
+		
+		mFilename = (EditText)findViewById(R.id.editText_filename);
+		mDescription = (EditText)findViewById(R.id.editText_description);
+		mTags = (EditText)findViewById(R.id.editText_tags);
+		mDraft = (CheckBox)findViewById(R.id.checkBox_draft);
+		mButtonOk = (Button)findViewById(R.id.button_ok);
+		
+		mProgressDialog = new ProgressDialog(this);
+		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);
+		
+		SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(this);
+		String tags = sharedPref.getString(getString(R.string.pref_key_tags), getString(R.string.pref_default_tags));
+		if(tags.isEmpty())
+		{
+			tags = getString(R.string.pref_default_tags);
+		}
+		mTags.setText(tags);
+		
+		mButtonOk.setEnabled(mFilename.getText().toString().length()>0);
+		mButtonOk.setOnClickListener(this);
+		
+		mFilename.addTextChangedListener(new TextWatcher() {
+
+			   @Override
+			   public void afterTextChanged(Editable s) {}
+
+			   @Override    
+			   public void beforeTextChanged(CharSequence s, int start,
+			     int count, int after) {
+			   }
+
+			   @Override    
+			   public void onTextChanged(CharSequence s, int start,
+			     int before, int count) {
+			      mButtonOk.setEnabled(s.length() != 0);
+			   }
+			  });
+		
+		mFilename.setSelectAllOnFocus(true);
+		mFilename.requestFocus();
+	}
+	
+	@Override
+	public void onClick(View v) {
+		// Handle button clicks.
+		switch (v.getId()) {
+		case R.id.button_ok:
+			shareToSketchfab();
+			break;
+		default:
+			return;
+		}
+	}
+	
+	private void shareToSketchfab()
+	{
+		if(!mTags.getText().toString().isEmpty())
+		{
+			SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(this);
+			SharedPreferences.Editor editor = sharedPref.edit();
+			editor.putString(getString(R.string.pref_key_tags), mTags.getText().toString());
+			// Commit the edits!
+			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());
+		}
+	
+	}
+	
+	private void authorizeAndPublish(final String[] filesToZip, final String fileName)
+	{
+		if(!isNetworkAvailable())
+		{
+			// Visualize the result?
+			new AlertDialog.Builder(this)
+			.setTitle("Sharing to Sketchfab...")
+			.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);
+				}
+			})
+			.setNeutralButton("Abort", new DialogInterface.OnClickListener() {
+				public void onClick(DialogInterface dialog, int which) {
+				}
+			})
+			.show();
+			return;
+		}	
+
+		// get token the first time
+		if(mAuthToken == null)
+		{
+			Log.i(RTABMapActivity.TAG,"We don't have the token, get it!");
+
+			WebView web;
+			mAuthDialog = new Dialog(this);
+			mAuthDialog.setContentView(R.layout.auth_dialog);
+			web = (WebView)mAuthDialog.findViewById(R.id.webv);
+			web.getSettings().setJavaScriptEnabled(true);
+			String auth_url = AUTHORIZE_PATH+"?redirect_uri="+REDIRECT_URI+"&response_type=token&client_id="+CLIENT_ID;
+			Log.i(RTABMapActivity.TAG, "Auhorize url="+auth_url);
+			web.setWebViewClient(new WebViewClient() {
+
+				boolean authComplete = false;
+
+				@Override
+				public void onPageFinished(WebView view, String url) {
+					super.onPageFinished(view, url);
+
+					//Log.i(TAG,"onPageFinished url="+url);
+					if(url.contains("error=access_denied")){
+						Log.e(RTABMapActivity.TAG, "ACCESS_DENIED_HERE");
+						authComplete = true;
+						Toast.makeText(getApplicationContext(), "Error Occured", Toast.LENGTH_SHORT).show();
+						mAuthDialog.dismiss();
+					}
+					else if (url.startsWith(REDIRECT_URI) && url.contains("access_token") && authComplete != true) {
+						//Log.i(TAG,"onPageFinished received token="+url);
+						String[] sArray = url.split("access_token=");
+						mAuthToken = (sArray[1].split("&token_type=Bearer"))[0];
+						authComplete = true;
+						
+						mAuthDialog.dismiss();
+
+						zipAndPublish(filesToZip, fileName);
+					}
+				}
+			});
+			mAuthDialog.show();
+			mAuthDialog.setTitle("Authorize RTAB-Map");
+			mAuthDialog.setCancelable(true);
+			web.loadUrl(auth_url);
+		}
+		else
+		{
+			zipAndPublish(filesToZip, fileName);
+		}
+	}
+
+	private void zipAndPublish(final String[] filesToZip, final String fileName)
+	{		
+		final String zipOutput = mWorkingDirectory+fileName+".zip";
+
+		mProgressDialog.setTitle("Upload to Sketchfab");
+		mProgressDialog.setMessage(String.format("Compressing the files..."));
+		mProgressDialog.show();
+
+		Thread workingThread = new Thread(new Runnable() {
+			public void run() {
+				try{
+					zip(filesToZip, zipOutput);
+					runOnUiThread(new Runnable() {
+						public void run() {
+							mProgressDialog.dismiss();
+
+							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)
+							{
+								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. ":""));
+							}
+
+							
+							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();
+						}
+					});
+				}
+				catch(IOException ex) {
+					Log.e(RTABMapActivity.TAG, "Failed to zip", ex);
+				}
+			}
+		});
+
+		workingThread.start();
+	}
+	
+	private boolean isNetworkAvailable() {
+	    ConnectivityManager connectivityManager 
+	          = (ConnectivityManager) getSystemService(Context.CONNECTIVITY_SERVICE);
+	    NetworkInfo activeNetworkInfo = connectivityManager.getActiveNetworkInfo();
+	    return activeNetworkInfo != null && activeNetworkInfo.isConnected();
+	}
+
+	public static void zip(String file, String zipFile) throws IOException {
+		Log.i(RTABMapActivity.TAG, "Zipping " + file +" to " + zipFile);
+		String[] files = new String[1];
+		files[0] = file;
+		zip(files, zipFile);
+	}
+
+	public static void zip(String[] files, String zipFile) throws IOException {
+		Log.i(RTABMapActivity.TAG, "Zipping " + String.valueOf(files.length) +" files to " + zipFile);
+		BufferedInputStream origin = null;
+		ZipOutputStream out = new ZipOutputStream(new BufferedOutputStream(new FileOutputStream(zipFile)));
+		try { 
+			byte data[] = new byte[ZIP_BUFFER_SIZE];
+
+			for (int i = 0; i < files.length; i++) {
+				FileInputStream fi = new FileInputStream(files[i]);    
+				origin = new BufferedInputStream(fi, ZIP_BUFFER_SIZE);
+				try {
+					ZipEntry entry = new ZipEntry(files[i].substring(files[i].lastIndexOf("/") + 1));
+					out.putNextEntry(entry);
+					int count;
+					while ((count = origin.read(data, 0, ZIP_BUFFER_SIZE)) != -1) {
+						out.write(data, 0, count);
+					}
+				}
+				finally {
+					origin.close();
+				}
+			}
+		}
+		finally {
+			out.close();
+		}
+	}
+
+	private class uploadToSketchfabTask extends AsyncTask<String, Void, Void>
+	{
+		String mModelUri;
+		String mModelFilePath;
+		String error = "";
+		String mFileName;
+
+		protected void onPreExecute() {
+			//display progress dialog.
+		}
+
+		@Override
+		protected void onPostExecute(Void result) {
+			
+			mProgressDialog.dismiss();
+			//Task you want to do on UIThread after completing Network operation
+			//onPostExecute is called after doInBackground finishes its task.
+			if(mModelFilePath!= null)
+			{
+				File f = new File(mModelFilePath);
+				f.delete(); // cleanup
+
+				// See on sketchfab?
+				  final SpannableString s = new SpannableString(
+				        "Model \"" + mFileName + "\" is now processing on Sketchfab! You can click "
+				        + "on the link below to see it on Sketchfab.\n\nhttps://sketchfab.com/models/"+mModelUri);
+				  Linkify.addLinks(s, Linkify.WEB_URLS);
+				final AlertDialog d = new AlertDialog.Builder(getActivity())
+				.setTitle("Upload finished!")
+				.setCancelable(false)
+				.setMessage(s)
+				.setPositiveButton("Close", new DialogInterface.OnClickListener() {
+					public void onClick(DialogInterface dialog, int which) {
+						Intent resultIntent = new Intent();
+						resultIntent.putExtra(RTABMapActivity.RTABMAP_AUTH_TOKEN_KEY, mAuthToken); 
+						setResult(Activity.RESULT_OK, resultIntent);
+						finish();
+					}
+				}).create();
+				d.show();
+				((TextView)d.findViewById(android.R.id.message)).setMovementMethod(LinkMovementMethod.getInstance());
+			}
+			else
+			{
+				Toast.makeText(getApplicationContext(), String.format("Upload failed! Error=\"%s\"", error), Toast.LENGTH_SHORT).show();
+			}
+		}
+
+		@Override
+		protected Void doInBackground(String... files) {
+			String charset = "UTF-8";
+			File uploadFile = new File(files[0]);
+			mFileName = files[1];
+			String requestURL = "https://api.sketchfab.com/v3/models";
+
+			Log.i(RTABMapActivity.TAG, "Uploading " + files[0]);
+
+			try {
+				MultipartUtility multipart = new MultipartUtility(requestURL, mAuthToken, charset);
+
+				multipart.addFormField("name", mFileName);
+				multipart.addFormField("description", mDescription.getText().toString());
+				multipart.addFormField("tags", mTags.getText().toString());
+				multipart.addFormField("source", "RTAB-Map");
+				multipart.addFormField("isPublished", mDraft.isChecked()?"false":"true");	
+				multipart.addFilePart("modelFile", uploadFile);
+
+				Log.i(RTABMapActivity.TAG, "Starting multipart request");
+				List<String> response = multipart.finish();
+
+				Log.i(RTABMapActivity.TAG, "SERVER REPLIED:");
+
+				for (String line : response) {
+					Log.i(RTABMapActivity.TAG, line);
+					//{"uri":"https:\/\/api.sketchfab.com\/v3\/models\/XXXXXXXXX","uid":"XXXXXXXXXX"}
+					if(line.contains("\"uid\":\""))
+					{
+						String[] sArray = line.split("\"uid\":\"");
+						mModelUri = (sArray[1].split("\""))[0];
+						mModelFilePath = files[0];
+
+						//patch model for orientation
+						/*HttpClient httpClient = new DefaultHttpClient(); 
+						try {
+							String patchURL = "https://api.sketchfab.com/v3/models/"+ mModelUri +"/options";
+							HttpPatch request = new HttpPatch(patchURL);
+							String json = 
+											"{\n"+
+												"uid: "+ mModelUri + "\n" +
+												"shading: shadeless\n"+
+												//"orientation:\n"+
+												//"{\n"+
+												//	"axis : [1, 0, 0]\n"+
+												//	"angle : 0\n"+
+												//"}\n"+
+											"}";
+
+							request.setHeader("Authorization", "Bearer " + mAuthToken);
+							StringEntity params =new StringEntity(json, "UTF-8");
+							params.setContentType("application/json");
+							request.setEntity(params);
+							HttpResponse responsePatch = httpClient.execute(request);
+							int responseStatus = responsePatch.getStatusLine().getStatusCode();
+							Log.i(RTABMapActivity.TAG, "get data responseStatus: " + responseStatus);
+
+						}catch (Exception e) {
+							Log.e(RTABMapActivity.TAG, "Error while patching model", e);
+							error = e.getMessage();
+						}*/
+					}
+				}    		  
+			} catch (IOException ex) {
+				Log.e(RTABMapActivity.TAG, "Error while uploading", ex);
+				error = ex.getMessage();
+			}
+			return null;
+		}
+	}
+}

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

@@ -0,0 +1,136 @@
+/*
+ * Copyright 2016 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ * 
+ * Copied for convenience from https://github.com/googlesamples/tango-examples-c/blob/master/cpp_example_util/app/src/main/java/com/projecttango/examples/cpp/util/TangoInitializationHelper.java
+ */
+
+package com.introlab.rtabmap;
+
+import android.content.Context;
+import android.content.Intent;
+import android.content.ServiceConnection;
+import android.os.Build;
+import android.os.IBinder;
+import android.util.Log;
+
+import java.io.File;
+
+/**
+ * Functions for simplifying the process of initializing TangoService, and function
+ * handles loading correct libtango_client_api.so.
+ */
+public class TangoInitializationHelper {
+  public static final int ARCH_ERROR = -2;
+  public static final int ARCH_FALLBACK = -1;
+  public static final int ARCH_DEFAULT = 0;
+  public static final int ARCH_ARM64 = 1;
+  public static final int ARCH_ARM32 = 2;
+  public static final int ARCH_X86_64 = 3;
+  public static final int ARCH_X86 = 4;
+
+  /**
+   * Only for apps using the C API:
+   * Initializes the underlying TangoService for native apps.
+   *
+   * @return returns false if the device doesn't have the Tango running as Android Service.
+   *     Otherwise ture.
+   */
+  public static final boolean bindTangoService(final Context context,
+                                               ServiceConnection connection) {
+    Intent intent = new Intent();
+    intent.setClassName("com.google.tango", "com.google.atap.tango.TangoService");
+
+    boolean hasJavaService = (context.getPackageManager().resolveService(intent, 0) != null);
+
+    // User doesn't have the latest packagename for TangoCore, fallback to the previous name.
+    if (!hasJavaService) {
+      intent = new Intent();
+      intent.setClassName("com.projecttango.tango", "com.google.atap.tango.TangoService");
+      hasJavaService = (context.getPackageManager().resolveService(intent, 0) != null);
+    }
+
+    // User doesn't have a Java-fied TangoCore at all; fallback to the deprecated approach
+    // of doing nothing and letting the native side auto-init to the system-service version
+    // of Tango.
+    if (!hasJavaService) {
+      return false;
+    }
+
+    return context.bindService(intent, connection, Context.BIND_AUTO_CREATE);
+  }
+
+    
+  /**
+   * Load the libtango_client_api.so library based on different Tango device setup.
+   *
+   * @return returns the loaded architecture id.
+   */
+  public static final int loadTangoSharedLibrary() {
+    int loadedSoId = ARCH_ERROR;
+    String basePath = "/data/data/com.google.tango/libfiles/";
+    if (!(new File(basePath).exists())) {
+      basePath = "/data/data/com.projecttango.tango/libfiles/";
+    }
+    Log.i("TangoInitializationHelper", "basePath: " + basePath);
+
+    try {
+      System.load(basePath + "arm64-v8a/libtango_client_api.so");
+      loadedSoId = ARCH_ARM64;
+      Log.i("TangoInitializationHelper", "Success! Using arm64-v8a/libtango_client_api.");
+    } catch (UnsatisfiedLinkError e) {
+    }
+    if (loadedSoId < ARCH_DEFAULT) {
+      try {
+        System.load(basePath + "armeabi-v7a/libtango_client_api.so");
+        loadedSoId = ARCH_ARM32;
+        Log.i("TangoInitializationHelper", "Success! Using armeabi-v7a/libtango_client_api.");
+      } catch (UnsatisfiedLinkError e) {
+      }
+    }
+    if (loadedSoId < ARCH_DEFAULT) {
+      try {
+        System.load(basePath + "x86_64/libtango_client_api.so");
+        loadedSoId = ARCH_X86_64;
+        Log.i("TangoInitializationHelper", "Success! Using x86_64/libtango_client_api.");
+      } catch (UnsatisfiedLinkError e) {
+      }
+    }
+    if (loadedSoId < ARCH_DEFAULT) {
+      try {
+        System.load(basePath + "x86/libtango_client_api.so");
+        loadedSoId = ARCH_X86;
+        Log.i("TangoInitializationHelper", "Success! Using x86/libtango_client_api.");
+      } catch (UnsatisfiedLinkError e) {
+      }
+    }
+    if (loadedSoId < ARCH_DEFAULT) {
+      try {
+        System.load(basePath + "default/libtango_client_api.so");
+        loadedSoId = ARCH_DEFAULT;
+        Log.i("TangoInitializationHelper", "Success! Using default/libtango_client_api.");
+      } catch (UnsatisfiedLinkError e) {
+      }
+    }
+    if (loadedSoId < ARCH_DEFAULT) {
+      try {
+        System.loadLibrary("tango_client_api");
+        loadedSoId = ARCH_FALLBACK;
+        Log.i("TangoInitializationHelper", "Falling back to libtango_client_api.so symlink.");
+      } catch (UnsatisfiedLinkError e) {
+      }
+    }
+    return loadedSoId;
+  }
+}

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

@@ -0,0 +1,443 @@
+//package ri.blog.opengl008;
+package com.introlab.rtabmap;
+
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import java.nio.FloatBuffer;
+import java.nio.ShortBuffer;
+import java.util.Iterator;
+import java.util.Vector;
+
+import android.content.Context;
+import android.graphics.Bitmap;
+import android.graphics.Canvas;
+import android.graphics.Color;
+import android.graphics.Rect;
+import android.graphics.Typeface;
+import android.opengl.GLES20;
+import android.opengl.GLUtils;
+import android.text.TextPaint;
+
+public class TextManager {
+	
+	/* SHADER Text
+	 * 
+	 * This shader is for rendering 2D text textures straight from a texture
+	 * Color and alpha blended.
+	 * 
+	 */
+	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;" +
+	    "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;" +
+	    "}";
+	
+	public static int sp_Text;
+	
+	public static final int RI_TEXT_TEXTURE_SIZE = 512; // 512
+	public static final float RI_TEXT_HEIGHT_BASE = 32.0f;
+	public static final char RI_TEXT_START = ' ';
+	public static final char RI_TEXT_STOP = '~'+1;
+	
+	public float getMaxTextHeight() {return mTextHeight;}
+	
+	private float mUVWidth;
+	private float mUVHeight;
+	private float mTextHeight;
+	
+	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;
+	
+	private float uniformscale = 1.0f;
+	
+	float[] mCharacterWidth;
+	
+	public TextManager(Context context)
+	{
+		// Create the arrays
+		vecs = new float[3 * 10];
+		colors = new float[4 * 10];
+		uvs = new float[2 * 10];
+		indices = new short[10];
+		
+		// init as 0 as default
+		texturenr = 0;
+		
+		// Text shader
+		int vshadert = TextManager.loadShader(GLES20.GL_VERTEX_SHADER, TextManager.vs_Text);
+		int fshadert = TextManager.loadShader(GLES20.GL_FRAGMENT_SHADER, TextManager.fs_Text);
+
+		TextManager.sp_Text = GLES20.glCreateProgram();
+		GLES20.glAttachShader(TextManager.sp_Text, vshadert);
+		GLES20.glAttachShader(TextManager.sp_Text, fshadert); 		// add the fragment shader to program
+		GLES20.glLinkProgram(TextManager.sp_Text);                  // creates OpenGL ES program executables
+		
+		// Generate Textures, if more needed, alter these numbers.
+		mTextures = new int[1];
+		GLES20.glGenTextures(1, mTextures, 0);
+		
+		// Create an empty, mutable bitmap
+		Bitmap bitmap = Bitmap.createBitmap(RI_TEXT_TEXTURE_SIZE, RI_TEXT_TEXTURE_SIZE, Bitmap.Config.ARGB_4444);
+		// get a canvas to paint over the bitmap
+		Canvas canvas = new Canvas(bitmap);
+		bitmap.eraseColor(0);
+
+		// Draw the text
+		TextPaint textPaint = new TextPaint();
+		textPaint.setTextSize(RI_TEXT_HEIGHT_BASE);
+		textPaint.setColor(Color.WHITE);
+		textPaint.setAntiAlias(true);
+		textPaint.setTypeface(Typeface.create("Arial", Typeface.BOLD));
+		
+		// compute real maximum height
+		mTextHeight=0.0f;
+		for(char c=RI_TEXT_START; c<RI_TEXT_STOP; ++c)
+		{
+			Rect textBounds = new Rect();
+			textPaint.getTextBounds(String.valueOf(c), 0, 1, textBounds);
+			if(textBounds.height() + textBounds.bottom > mTextHeight)
+			{
+				mTextHeight = textBounds.height() + textBounds.bottom;
+			}
+		}
+		mUVWidth = (float)RI_TEXT_HEIGHT_BASE/(float)RI_TEXT_TEXTURE_SIZE;
+		mUVHeight = mTextHeight/(float)RI_TEXT_TEXTURE_SIZE;
+		
+		int colCount = RI_TEXT_TEXTURE_SIZE/(int)RI_TEXT_HEIGHT_BASE;
+		mCharacterWidth = new float[RI_TEXT_STOP-RI_TEXT_START];
+		int i=0;
+		for(char c=RI_TEXT_START; c<RI_TEXT_STOP; ++c)
+		{
+			Rect textBounds = new Rect();
+			textPaint.getTextBounds(String.valueOf(c), 0, 1, textBounds);
+			canvas.drawText(String.valueOf(c), (i%colCount)*RI_TEXT_HEIGHT_BASE, (i/colCount) * mTextHeight + RI_TEXT_HEIGHT_BASE, textPaint);
+			mCharacterWidth[i] = textPaint.measureText(String.valueOf(c));
+			++i;
+		}
+		
+		GLES20.glBindTexture(GLES20.GL_TEXTURE_2D, mTextures[0]);
+		GLES20.glTexParameteri(GLES20.GL_TEXTURE_2D, GLES20.GL_TEXTURE_MIN_FILTER, GLES20.GL_LINEAR);
+        GLES20.glTexParameteri(GLES20.GL_TEXTURE_2D, GLES20.GL_TEXTURE_MAG_FILTER, GLES20.GL_LINEAR);
+        GLUtils.texImage2D(GLES20.GL_TEXTURE_2D, 0, bitmap, 0);
+		
+		//Clean up
+		bitmap.recycle();
+	}
+	
+	public void setTextureID(int val)
+	{
+		texturenr = val;
+	}
+	
+	public static int loadShader(int type, String shaderCode){
+
+	    // create a vertex shader type (GLES20.GL_VERTEX_SHADER)
+	    // or a fragment shader type (GLES20.GL_FRAGMENT_SHADER)
+	    int shader = GLES20.glCreateShader(type);
+
+	    // add the source code to the shader and compile it
+	    GLES20.glShaderSource(shader, shaderCode);
+	    GLES20.glCompileShader(shader);
+	    
+	    // return the shader
+	    return shader;
+	}
+	
+	
+	public void AddCharRenderInformation(float[] vec, float[] cs, float[] uv, short[] indi)
+	{
+		// We need a base value because the object has indices related to 
+		// that object and not to this collection so basicly we need to 
+		// translate the indices to align with the vertexlocation in ou
+		// vecs array of vectors.
+		short base = (short) (index_vecs / 3);
+			
+		// We should add the vec, translating the indices to our saved vector
+		for(int i=0;i<vec.length;i++)
+		{
+			vecs[index_vecs] = vec[i];
+			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++)
+		{
+			uvs[index_uvs] = uv[i];
+			index_uvs++;
+		}
+
+		// We handle the indices
+		for(int j=0;j<indi.length;j++)
+		{
+			indices[index_indices] = (short) (base + indi[j]);
+			index_indices++;
+		}
+	}
+
+	public void PrepareDrawInfo(Vector<TextObject> txtcollection)
+	{
+		// Reset the indices.
+		index_vecs = 0;
+		index_indices = 0;
+		index_uvs = 0;
+		index_colors = 0;
+		
+		// Get the total amount of characters
+		int charcount = 0;
+		for (TextObject txt : txtcollection) {
+			if(txt!=null)
+			{
+				if(!(txt.text==null))
+				{
+					charcount += txt.text.length(); 
+				}
+			}
+		}
+		
+		// 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];
+
+	}
+	
+	public void PrepareDraw(Vector<TextObject> txtcollection)
+	{
+		// Setup all the arrays
+		PrepareDrawInfo(txtcollection);
+		
+		// Using the iterator protects for problems with concurrency
+		for( Iterator< TextObject > it = txtcollection.iterator(); it.hasNext() ; )
+	    {
+	    	TextObject txt = it.next();
+	    	if(txt!=null)
+			{
+		    	if(!(txt.text==null))
+				{
+					convertTextToTriangleInfo(txt);
+				}
+			}
+	    }
+	}
+	
+	public void Draw(float[] m)
+	{
+		if(vecs.length > 0)
+		{
+			GLES20.glDisable(GLES20.GL_DEPTH_TEST);
+			
+			// Set the correct shader for our grid object.
+			GLES20.glUseProgram(sp_Text);
+			
+			// The vertex buffer.
+			ByteBuffer bb = ByteBuffer.allocateDirect(vecs.length * 4);
+			bb.order(ByteOrder.nativeOrder());
+			vertexBuffer = bb.asFloatBuffer();
+			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());
+			textureBuffer = bb2.asFloatBuffer();
+			textureBuffer.put(uvs);
+			textureBuffer.position(0);
+			
+			// initialize byte buffer for the draw list
+			ByteBuffer dlb = ByteBuffer.allocateDirect(indices.length * 2);
+			dlb.order(ByteOrder.nativeOrder());
+			drawListBuffer = dlb.asShortBuffer();
+			drawListBuffer.put(indices);
+			drawListBuffer.position(0);
+			
+			// get handle to vertex shader's vPosition member
+		    int mPositionHandle = GLES20.glGetAttribLocation(sp_Text, "vPosition");
+		    
+		    // Enable a handle to the triangle vertices
+		    GLES20.glEnableVertexAttribArray(mPositionHandle);
+		 
+		    // Prepare the background coordinate data
+		    GLES20.glVertexAttribPointer(mPositionHandle, 3,
+		                                 GLES20.GL_FLOAT, false,
+		                                 0, vertexBuffer);
+		    
+		    int mTexCoordLoc = GLES20.glGetAttribLocation(sp_Text, "a_texCoord" );
+		    
+		    // Prepare the texturecoordinates
+		    GLES20.glVertexAttribPointer ( mTexCoordLoc, 2, GLES20.GL_FLOAT,
+	                false, 
+	                0, textureBuffer);
+	
+		    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);
+	        
+	        int mSamplerLoc = GLES20.glGetUniformLocation (sp_Text, "s_texture" );
+	        
+	        // Texture activate unit 0
+	        GLES20.glActiveTexture(GLES20.GL_TEXTURE0);
+			// Bind the texture to this unit.
+	        GLES20.glBindTexture(GLES20.GL_TEXTURE_2D, mTextures[0]);
+	        // Set the sampler texture unit to our selected id
+		    GLES20.glUniform1i ( mSamplerLoc, texturenr);
+		    
+	        // Draw the triangle
+	        GLES20.glDrawElements(GLES20.GL_TRIANGLES, indices.length, GLES20.GL_UNSIGNED_SHORT, drawListBuffer);
+	
+	        // Disable vertex array
+	        GLES20.glDisableVertexAttribArray(mPositionHandle);
+	        GLES20.glDisableVertexAttribArray(mTexCoordLoc);
+	        GLES20.glDisableVertexAttribArray(mColorHandle);
+		}
+	}
+	
+	private void convertTextToTriangleInfo(TextObject val)
+	{		
+		// Get attributes from text object
+		float x = val.x;
+		float y = val.y;
+		String text = val.text;
+		
+		// Create 
+		for(int j=0; j<text.length(); j++)
+		{
+			// get ascii value
+			char c = text.charAt(j);
+			int c_val = (int)c;
+			
+			int indx = c_val-RI_TEXT_START;
+			
+			if(indx<0 || indx>=mCharacterWidth.length) {
+				// unknown character, we will add a space for it to be save.
+				indx = 0;
+			}
+			
+			int colCount = RI_TEXT_TEXTURE_SIZE/(int)RI_TEXT_HEIGHT_BASE;
+			
+			// Calculate the uv parts
+			int row = indx / colCount;
+			int col = indx % colCount;
+			
+			float v = row * mUVHeight;
+			float v2 = v + mUVHeight;
+			float u = col * mUVWidth;
+			float u2 = u + mCharacterWidth[indx]/(float)RI_TEXT_TEXTURE_SIZE;
+			
+			// Creating the triangle information
+			float[] vec = new float[12];
+			float[] uv = new float[8];
+			float[] colors = new float[16];
+			
+			vec[0] = x;
+			vec[1] = y + (mTextHeight * uniformscale);
+			vec[2] = 0.99f;
+			vec[3] = x;
+			vec[4] = y;
+			vec[5] = 0.99f;
+			vec[6] = x + (mCharacterWidth[indx] * uniformscale);
+			vec[7] = y;
+			vec[8] = 0.99f;
+			vec[9] = x + (mCharacterWidth[indx] * uniformscale);
+			vec[10] = y + (mTextHeight * uniformscale);
+			vec[11] = 0.99f;
+		
+			colors = new float[]
+				    	{val.color[0], val.color[1], val.color[2], val.color[3],
+						val.color[0], val.color[1], val.color[2], val.color[3],
+						val.color[0], val.color[1], val.color[2], val.color[3],
+						val.color[0], val.color[1], val.color[2], val.color[3]
+				    			  		           };
+			// 0.001f = texture bleeding hack/fix
+			uv[0] = u+0.001f;
+			uv[1] = v+0.001f;
+			uv[2] = u+0.001f;
+			uv[3] = v2-0.001f;
+			uv[4] = u2-0.001f;
+			uv[5] = v2-0.001f;
+			uv[6] = u2-0.001f;
+			uv[7] = v+0.001f;
+			
+			short[] inds = {0, 1, 2, 0, 2, 3};
+			
+			// Add our triangle information to our collection for 1 render call.
+			AddCharRenderInformation(vec, colors, uv, inds);
+			
+			// Calculate the new position
+			x += (mCharacterWidth[indx]  * uniformscale);
+		}
+	}
+
+	public float getUniformscale() {
+		return uniformscale;
+	}
+
+	public void setUniformscale(float uniformscale) {
+		this.uniformscale = uniformscale;
+	}
+}

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

@@ -0,0 +1,33 @@
+//package ri.blog.opengl008;
+package com.introlab.rtabmap;
+
+public class TextObject {
+	
+	public String text;
+	public float x;
+	public float y;
+	public float[] color;
+	
+	public TextObject()
+	{
+		text = "default";
+		x = 0f;
+		y = 0f;
+		color = new float[] {1f, 1f, 1f, 1.0f};
+	}
+	
+	public TextObject(String txt, float xcoord, float ycoord)
+	{
+		text = txt;
+		x = xcoord;
+		y = ycoord;
+		color = new float[] {1f, 1f, 1f, 1.0f};
+	}
+	
+	/*public boolean validate()
+	{
+		if(text.compareTo("")==0) return false;
+		
+		return true;
+	}*/
+}

app/src/CMakeLists.txt

@@ -5,7 +5,7 @@ SET(headers_ui
 )
 
 #This will generate moc_* for Qt
-IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
+IF(QT4_FOUND)
     QT4_WRAP_CPP(moc_srcs ${headers_ui})
 ELSE()
     QT5_WRAP_CPP(moc_srcs ${headers_ui})
@@ -25,9 +25,9 @@ SET(INCLUDE_DIRS
     ${PCL_INCLUDE_DIRS}
 )
 
-IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
+IF(QT4_FOUND)
     INCLUDE(${QT_USE_FILE})
-ENDIF()
+ENDIF(QT4_FOUND)
 
 SET(LIBRARIES
     ${QT_LIBRARIES} 
@@ -73,9 +73,9 @@ ELSE()
   ADD_EXECUTABLE(rtabmap ${SRC_FILES})
 ENDIF()
 TARGET_LINK_LIBRARIES(rtabmap rtabmap_core rtabmap_gui rtabmap_utilite ${LIBRARIES})
-IF("${RTABMAP_QT_VERSION}" STREQUAL "5")
+IF(Qt5_FOUND)
     QT5_USE_MODULES(rtabmap Widgets Core Gui Svg PrintSupport)
-ENDIF()
+ENDIF(Qt5_FOUND)
 
 IF(APPLE AND BUILD_AS_BUNDLE)
   SET_TARGET_PROPERTIES(rtabmap PROPERTIES
@@ -137,11 +137,25 @@ IF((APPLE AND BUILD_AS_BUNDLE) OR WIN32)
   # Install needed Qt plugins by copying directories from the qt installation
   # One can cull what gets copied by using 'REGEX "..." EXCLUDE'
   # Exclude debug libraries
+  IF(QT_PLUGINS_DIR)
     INSTALL(DIRECTORY "${QT_PLUGINS_DIR}/imageformats"
           DESTINATION ${plugin_dest_dir}/plugins 
           COMPONENT runtime 
           REGEX ".*d4.dll" EXCLUDE
           REGEX ".*d4.a" EXCLUDE)
+  ELSE()
+	#Qt5
+	foreach(plugin ${Qt5Gui_PLUGINS})
+		get_target_property(plugin_loc ${plugin} LOCATION)
+		get_filename_component(plugin_dir ${plugin_loc} DIRECTORY)
+		string(REPLACE "plugins" ";" loc_list ${plugin_dir})
+		list(GET loc_list 1 plugin_type)
+	    #MESSAGE(STATUS "Qt5 plugin \"${plugin_loc}\" installed in \"${plugin_dest_dir}/plugins${plugin_type}\"")
+		INSTALL(FILES ${plugin_loc}
+          DESTINATION ${plugin_dest_dir}/plugins${plugin_type}
+          COMPONENT runtime)
+	endforeach()
+  ENDIF()
   
   # install a qt.conf file
   # this inserts some cmake code into the install script to write the file

app/src/ObjDeletionHandler.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

app/src/main.cpp

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -33,6 +33,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/gui/MainWindow.h"
 #include <QMessageBox>
 #include "rtabmap/utilite/UObjDeletionThread.h"
+#include "rtabmap/utilite/UFile.h"
+#include "rtabmap/utilite/UConversion.h"
 #include "ObjDeletionHandler.h"
 
 using namespace rtabmap;
@@ -45,7 +47,23 @@ int main(int argc, char* argv[])
 
 	/* Create tasks */
 	QApplication * app = new QApplication(argc, argv);
+	app->setStyleSheet("QMessageBox { messagebox-text-interaction-flags: 5; }"); // selectable message box
+
+	ParametersMap parameters = Parameters::parseArguments(argc, argv, true);
 	MainWindow * mainWindow = new MainWindow();
+    app->installEventFilter(mainWindow); // to catch FileOpen events.
+    
+    std::string database;
+    for(int i=1; i<argc; ++i)
+    {
+        std::string value = uReplaceChar(argv[i], '~', UDirectory::homeDir());
+        if(UFile::exists(value) &&
+           UFile::getExtension(value).compare("db") == 0)
+        {
+            database = value;
+            break;
+        }
+    }
 
 	UINFO("Program started...");
 
@@ -65,6 +83,15 @@ int main(int argc, char* argv[])
 	rtabmap->start(); // start it not initialized... will be initialized by event from the gui
 	UEventsManager::addHandler(rtabmap);
     
+    if(!database.empty())
+    {
+    	mainWindow->openDatabase(database.c_str());
+    }
+    if(parameters.size())
+    {
+    	mainWindow->updateParameters(parameters);
+    }
+
 	// Now wait for application to finish
 	app->connect( app, SIGNAL( lastWindowClosed() ),
 				app, SLOT( quit() ) );

cmake_modules/FindDC1394.cmake

@@ -13,9 +13,9 @@ IF (DC1394_INCLUDE_DIRS AND DC1394_LIBRARIES)
    SET(DC1394_FOUND TRUE)
 
    #On Mac OS X
-   if(CMAKE_SYSTEM_NAME MATCHES "Darwin")
+   #if(CMAKE_SYSTEM_NAME MATCHES "Darwin")
-     set(DC1394_LIBRARIES ${DC1394_LIBRARIES} "-framework CoreServices")
+   #  set(DC1394_LIBRARIES ${DC1394_LIBRARIES} "-framework CoreServices")
-   endif(CMAKE_SYSTEM_NAME MATCHES "Darwin")
+   #endif(CMAKE_SYSTEM_NAME MATCHES "Darwin")
 
 ENDIF (DC1394_INCLUDE_DIRS AND DC1394_LIBRARIES)
 

cmake_modules/FindG2O.cmake

@@ -18,8 +18,7 @@ FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h
   PATHS "C:\\Program Files\\g2o\\include")
 
 # Macro to unify finding both the debug and release versions of the
-# libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY
+# libraries; this is adapted from the rtabmap config
-# macro.
 
 MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME)
 
@@ -31,11 +30,14 @@ MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME)
     NAMES "g2o_${MYLIBRARYNAME}"
 	PATHS "C:\\Program Files\\g2o\\lib")
   
-  IF(NOT ${MYLIBRARY}_DEBUG)
+  IF(${MYLIBRARY}_DEBUG AND ${MYLIBRARY})
-    IF(MYLIBRARY)
+    SET(${MYLIBRARY}
-      SET(${MYLIBRARY}_DEBUG ${MYLIBRARY})
+      debug ${${MYLIBRARY}_DEBUG}
-    ENDIF(MYLIBRARY)
+      optimized ${${MYLIBRARY}}
-  ENDIF( NOT ${MYLIBRARY}_DEBUG)
+    )
+  ELSEIF(${MYLIBRARY}_DEBUG)
+    SET(${MYLIBRARY} ${${MYLIBRARY}_DEBUG})
+  ENDIF()  
   
 ENDMACRO(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME)
 
@@ -79,6 +81,7 @@ IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FO
 	${G2O_CORE_LIBRARY}
 	${G2O_TYPES_SLAM2D} 
 	${G2O_TYPES_SLAM3D}
+	${G2O_TYPES_SBA}
         ${G2O_STUFF_LIBRARY})
   
   IF(CSPARSE_FOUND)

cmake_modules/FindRealSense.cmake

@@ -0,0 +1,34 @@
+# - Find librealsense (https://github.com/IntelRealSense/librealsense)
+#
+#  RealSense_ROOT_DIR environment variable can be set to find the library.
+#
+# It sets the following variables:
+#  RealSense_FOUND       - Set to false, or undefined, if RealSense isn't found.
+#  RealSense_INCLUDE_DIRS - The RealSense include directory.
+#  RealSense_LIBRARIES     - The RealSense library to link against.
+
+#RealSense library
+find_path(RealSense_INCLUDE_DIRS NAMES librealsense/rs.hpp PATHS $ENV{RealSense_ROOT_DIR}/include)
+if(CMAKE_CL_64)
+find_library(RealSense_LIBRARY NAMES realsense PATHS $ENV{RealSense_ROOT_DIR}/lib  $ENV{RealSense_ROOT_DIR}/bin $ENV{RealSense_ROOT_DIR}/bin/x64)
+else()
+find_library(RealSense_LIBRARY NAMES realsense PATHS $ENV{RealSense_ROOT_DIR}/lib  $ENV{RealSense_ROOT_DIR}/bin $ENV{RealSense_ROOT_DIR}/bin/Win32)
+endif()
+
+IF (RealSense_INCLUDE_DIRS AND RealSense_LIBRARY)
+   SET(RealSense_FOUND TRUE)
+ENDIF (RealSense_INCLUDE_DIRS AND RealSense_LIBRARY)
+
+IF (RealSense_FOUND)
+   # show which RealSense was found only if not quiet
+   SET(RealSense_LIBRARIES ${RealSense_LIBRARY})
+   IF (NOT RealSense_FIND_QUIETLY)
+      MESSAGE(STATUS "Found RealSense: ${RealSense_LIBRARIES}")
+   ENDIF (NOT RealSense_FIND_QUIETLY)
+ELSE (RealSense_FOUND)
+   # fatal error if RealSense is required but not found
+   IF (RealSense_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find RealSense (librealsense)")
+   ENDIF (RealSense_FIND_REQUIRED)
+ENDIF (RealSense_FOUND)
+

cmake_modules/FindTango.cmake

@@ -9,12 +9,13 @@
 FIND_PATH(Tango_INCLUDE_DIR tango_client_api.h)
 
 FIND_LIBRARY(Tango_LIBRARY NAMES tango_client_api)
+FIND_LIBRARY(Tango_support_LIBRARY NAMES tango_support_api)
 
-IF (Tango_INCLUDE_DIR AND Tango_LIBRARY)
+IF (Tango_INCLUDE_DIR AND Tango_LIBRARY AND Tango_support_LIBRARY)
    SET(Tango_FOUND TRUE)
    SET(Tango_INCLUDE_DIRS ${Tango_INCLUDE_DIR})
-   SET(Tango_LIBRARIES ${Tango_LIBRARY})
+   SET(Tango_LIBRARIES ${Tango_LIBRARY} ${Tango_support_LIBRARY})
-ENDIF (Tango_INCLUDE_DIR AND Tango_LIBRARY)
+ENDIF (Tango_INCLUDE_DIR AND Tango_LIBRARY AND Tango_support_LIBRARY)
 
 IF (Tango_FOUND)
    # show which Tango was found only if not quiet
@@ -24,7 +25,7 @@ IF (Tango_FOUND)
 ELSE (Tango_FOUND)
    # fatal error if Tango is required but not found
    IF (Tango_FIND_REQUIRED)
-      MESSAGE(FATAL_ERROR "Could not find Tango")
+      MESSAGE(FATAL_ERROR "Could not find Tango (client and/or support libraries)")
    ENDIF (Tango_FIND_REQUIRED)
 ENDIF (Tango_FOUND)
 

cmake_modules/MacOSXBundleInfo.plist.in

@@ -0,0 +1,56 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE plist PUBLIC "-//Apple Computer//DTD PLIST 1.0//EN" "http://www.apple.com/DTDs/PropertyList-1.0.dtd">
+<plist version="1.0">
+<dict>
+	<key>CFBundleDevelopmentRegion</key>
+	<string>English</string>
+	<key>CFBundleExecutable</key>
+	<string>${MACOSX_BUNDLE_EXECUTABLE_NAME}</string>
+	<key>CFBundleGetInfoString</key>
+	<string>${MACOSX_BUNDLE_INFO_STRING}</string>
+	<key>CFBundleIconFile</key>
+	<string>${MACOSX_BUNDLE_ICON_FILE}</string>
+	<key>CFBundleIdentifier</key>
+	<string>${MACOSX_BUNDLE_GUI_IDENTIFIER}</string>
+	<key>CFBundleInfoDictionaryVersion</key>
+	<string>6.0</string>
+	<key>CFBundleLongVersionString</key>
+	<string>${MACOSX_BUNDLE_LONG_VERSION_STRING}</string>
+	<key>CFBundleName</key>
+	<string>${MACOSX_BUNDLE_BUNDLE_NAME}</string>
+	<key>CFBundlePackageType</key>
+	<string>APPL</string>
+	<key>CFBundleShortVersionString</key>
+	<string>${MACOSX_BUNDLE_SHORT_VERSION_STRING}</string>
+	<key>CFBundleSignature</key>
+	<string>????</string>
+	<key>CFBundleVersion</key>
+	<string>${MACOSX_BUNDLE_BUNDLE_VERSION}</string>
+	<key>CSResourcesFileMapped</key>
+	<true/>
+	<key>LSRequiresCarbon</key>
+	<true/>
+	<key>NSHumanReadableCopyright</key>
+	<string>${MACOSX_BUNDLE_COPYRIGHT}</string>
+
+<!-- File type associations -->
+<key>CFBundleDocumentTypes</key>
+<array>
+	<dict>
+		<key>CFBundleTypeExtensions</key>
+		<array>
+			<string>db</string>
+		</array>
+		<!-- <key>CFBundleTypeIconFile</key> -->
+		<!-- <string>rtabmap_db.icns</string> -->
+		<key>CFBundleTypeName</key>
+		<string>RTAB-Map Database</string>
+		<key>CFBundleTypeRole</key>
+		<string>Editor</string>
+		<key>LSIsAppleDefaultForType</key>
+		<string>Yes</string>
+	</dict>
+</array>
+
+</dict>
+</plist>

corelib/include/rtabmap/core/BayesFilter.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/Camera.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -56,6 +56,7 @@ public:
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "") = 0;
 	virtual bool isCalibrated() const = 0;
 	virtual std::string getSerial() const = 0;
+	virtual bool odomProvided() const { return false; }
 
 	//getters
 	float getImageRate() const {return _imageRate;}
@@ -76,7 +77,7 @@ protected:
 	/**
 	 * returned rgb and depth images should be already rectified if calibration was loaded
 	 */
-	virtual SensorData captureImage() = 0;
+	virtual SensorData captureImage(CameraInfo * info = 0) = 0;
 
 	int getNextSeqID() {return ++_seq;}
 

corelib/include/rtabmap/core/CameraEvent.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/CameraInfo.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -39,22 +39,33 @@ public:
 	CameraInfo() :
 		cameraName(""),
 		id(0),
+		stamp(0.0),
 		timeCapture(0.0f),
 		timeDisparity(0.0f),
 		timeMirroring(0.0f),
 		timeImageDecimation(0.0f),
-		timeScanFromDepth(0.0f)
+		timeScanFromDepth(0.0f),
+		timeUndistortDepth(0.0f),
+		timeBilateralFiltering(0.0f),
+		timeTotal(0.0f),
+		odomCovariance(cv::Mat::eye(6,6,CV_64FC1))
 	{
 	}
 	virtual ~CameraInfo() {}
 
 	std::string cameraName;
 	int id;
+	double stamp;
 	float timeCapture;
 	float timeDisparity;
 	float timeMirroring;
 	float timeImageDecimation;
 	float timeScanFromDepth;
+	float timeUndistortDepth;
+	float timeBilateralFiltering;
+	float timeTotal;
+	Transform odomPose;
+	cv::Mat odomCovariance;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/CameraModel.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -76,7 +76,8 @@ public:
 
 	void initRectificationMap();
 
-	bool isValidForProjection() const {return fx()>0.0 && fy()>0.0;}
+	bool isValidForProjection() const {return fx()>0.0 && fy()>0.0 && cx()>0.0 && cy()>0.0;}
+	bool isValidForReprojection() const {return fx()>0.0 && fy()>0.0 && cx()>0.0 && cy()>0.0 && imageWidth()>0 && imageHeight()>0;}
 	bool isValidForRectification() const
 	{
 		return imageSize_.width>0 &&
@@ -99,14 +100,14 @@ public:
 	cv::Mat K_raw() const {return K_;} //intrinsic camera matrix (before rectification)
 	cv::Mat D_raw() const {return D_;} //intrinsic distorsion matrix (before rectification)
 	cv::Mat K() const {return !P_.empty()?P_.colRange(0,3):K_;} // if P exists, return rectified version
-	cv::Mat D() const {return P_.empty()&&!D_.empty()?D_:cv::Mat::zeros(1,4,CV_64FC1);} // if P exists, return rectified version
+	cv::Mat D() const {return P_.empty()&&!D_.empty()?D_:cv::Mat::zeros(1,5,CV_64FC1);} // if P exists, return rectified version
 	cv::Mat R() const {return R_;} //rectification matrix
 	cv::Mat P() const {return P_;} //projection matrix
 
 	void setLocalTransform(const Transform & transform) {localTransform_ = transform;}
 	const Transform & localTransform() const {return localTransform_;}
 
-	void setImageSize(const cv::Size & size) {imageSize_ = size;}
+	void setImageSize(const cv::Size & size);
 	const cv::Size & imageSize() const {return imageSize_;}
 	int imageWidth() const {return imageSize_.width;}
 	int imageHeight() const {return imageSize_.height;}
@@ -115,6 +116,7 @@ public:
 	bool save(const std::string & directory) const;
 
 	CameraModel scaled(double scale) const;
+	CameraModel roi(const cv::Rect & roi) const;
 
 	double horizontalFOV() const; // in degrees
 	double verticalFOV() const;   // in degrees
@@ -123,6 +125,13 @@ public:
 	cv::Mat rectifyImage(const cv::Mat & raw, int interpolation = cv::INTER_LINEAR) const;
 	cv::Mat rectifyDepth(const cv::Mat & raw) const;
 
+	// Project 2D pixel to 3D (in /camera_link frame)
+	void project(float u, float v, float depth, float & x, float & y, float & z) const;
+	// Reproject 3D point (in /camera_link frame) to pixel
+	void reproject(float x, float y, float z, float & u, float & v) const;
+	void reproject(float x, float y, float z, int & u, int & v) const;
+	bool inFrame(int u, int v) const;
+
 private:
 	std::string name_;
 	cv::Size imageSize_;

corelib/include/rtabmap/core/CameraRGB.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -59,6 +59,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 { return odometry_.size() > 0; }
 	std::string getPath() const {return _path;}
 	unsigned int imagesCount() const;
 	std::vector<std::string> filenames() const;
@@ -75,8 +76,8 @@ public:
 	void setTimestamps(bool fileNamesAreStamps, const std::string & filePath = "", bool syncImageRateWithStamps=true)
 	{
 		_filenamesAreTimestamps = fileNamesAreStamps;
-		timestampsPath_=filePath;
+		_timestampsPath=filePath;
-		syncImageRateWithStamps_ = syncImageRateWithStamps;
+		_syncImageRateWithStamps = syncImageRateWithStamps;
 	}
 
 	void setScanPath(
@@ -106,9 +107,15 @@ public:
 		_depthFromScanFillHolesFromBorder = fillHolesFromBorder;
 	}
 
+	void setOdometryPath(const std::string & filePath, int format = 0)
+	{
+		_odometryPath = filePath;
+		_odometryFormat = format;
+	}
+
 	void setGroundTruthPath(const std::string & filePath, int format = 0)
 	{
-		groundTruthPath_ = filePath;
+		_groundTruthPath = filePath;
 		_groundTruthFormat = format;
 	}
 
@@ -119,7 +126,8 @@ public:
 	}
 
 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);
+	bool readPoses(std::list<Transform> & outputPoses, std::list<double> & stamps, const std::string & filePath, int format) const;
 
 private:
 	std::string _path;
@@ -150,13 +158,17 @@ private:
 	bool _depthFromScanFillHolesFromBorder;
 
 	bool _filenamesAreTimestamps;
-	std::string timestampsPath_;
+	std::string _timestampsPath;
-	bool syncImageRateWithStamps_;
+	bool _syncImageRateWithStamps;
 
-	std::string groundTruthPath_;
+	std::string _odometryPath;
+	int _odometryFormat;
+
+	std::string _groundTruthPath;
 	int _groundTruthFormat;
 
-	std::list<double> stamps_;
+	std::list<double> _stamps;
+	std::list<Transform> odometry_;
 	std::list<Transform> groundTruth_;
 	CameraModel _model;
 
@@ -178,6 +190,7 @@ public:
 
 public:
 	CameraVideo(int usbDevice = 0,
+			bool rectifyImages = false,
 			float imageRate = 0,
 			const Transform & localTransform = Transform::getIdentity());
 	CameraVideo(const std::string & filePath,
@@ -193,7 +206,7 @@ public:
 	const std::string & getFilePath() const {return _filePath;}
 
 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);
 
 private:
 	// File type

corelib/include/rtabmap/core/CameraRGBD.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/CameraModel.h"
 #include "rtabmap/core/Camera.h"
 #include "rtabmap/core/CameraRGB.h"
+#include "rtabmap/core/Version.h"
 
 #include <pcl/pcl_config.h>
 
@@ -64,6 +65,12 @@ class Registration;
 class PacketPipeline;
 }
 
+namespace rs
+{
+	class context;
+	class device;
+}
+
 typedef struct _freenect_context freenect_context;
 typedef struct _freenect_device freenect_device;
 
@@ -97,7 +104,7 @@ public:
     virtual std::string getSerial() const;
 
 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);
 
 private:
     pcl::Grabber* interface_;
@@ -131,7 +138,7 @@ public:
 	virtual std::string getSerial() const {return "";} // unknown with OpenCV
 
 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);
 
 private:
 	bool _asus;
@@ -149,9 +156,11 @@ class RTABMAP_EXP CameraOpenNI2 :
 public:
 	static bool available();
 	static bool exposureGainAvailable();
+	enum Type {kTypeColorDepth, kTypeIRDepth, kTypeIR};
 
 public:
 	CameraOpenNI2(const std::string & deviceId = "",
+					Type type = kTypeColorDepth,
 					float imageRate = 0,
 					const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraOpenNI2();
@@ -165,12 +174,14 @@ public:
 	bool setExposure(int value);
 	bool setGain(int value);
 	bool setMirroring(bool enabled);
-	void setOpenNI2StampsAndIDsUsed(bool used) {_openNI2StampsAndIDsUsed = used;}
+	void setOpenNI2StampsAndIDsUsed(bool used);
 
 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);
 
 private:
+#ifdef RTABMAP_OPENNI2
+	Type _type;
 	openni::Device * _device;
 	openni::VideoStream * _color;
 	openni::VideoStream * _depth;
@@ -178,6 +189,8 @@ private:
 	float _depthFy;
 	std::string _deviceId;
 	bool _openNI2StampsAndIDsUsed;
+	StereoCameraModel _stereoModel;
+#endif
 };
 
 
@@ -191,10 +204,12 @@ class RTABMAP_EXP CameraFreenect :
 {
 public:
 	static bool available();
+	enum Type {kTypeColorDepth, kTypeIRDepth};
 
 public:
 	// default local transform z in, x right, y down));
 	CameraFreenect(int deviceId= 0,
+					Type type = kTypeColorDepth,
 					float imageRate=0.0f,
 					const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraFreenect();
@@ -204,12 +219,16 @@ public:
 	virtual std::string getSerial() const;
 
 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);
 
 private:
+#ifdef RTABMAP_FREENECT
 	int deviceId_;
+	Type type_;
 	freenect_context * ctx_;
 	FreenectDevice * freenectDevice_;
+	StereoCameraModel stereoModel_;
+#endif
 };
 
 /////////////////////////
@@ -249,9 +268,10 @@ public:
 	virtual std::string getSerial() const;
 
 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);
 
 private:
+#ifdef RTABMAP_FREENECT2
 	int deviceId_;
 	Type type_;
 	StereoCameraModel stereoModel_;
@@ -264,6 +284,43 @@ private:
 	bool bilateralFiltering_;
 	bool edgeAwareFiltering_;
 	bool noiseFiltering_;
+#endif
+};
+
+/////////////////////////
+// CameraRealSense
+/////////////////////////
+class RTABMAP_EXP CameraRealSense :
+	public Camera
+{
+public:
+	static bool available();
+
+public:
+	// default local transform z in, x right, y down));
+	CameraRealSense(
+		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
+		float imageRate = 0,
+		const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraRealSense();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+
+protected:
+	virtual SensorData captureImage(CameraInfo * info = 0);
+
+private:
+#ifdef RTABMAP_REALSENSE
+	rs::context * ctx_;
+	rs::device * dev_;
+	int deviceId_;
+	int presetRGB_;
+	int presetDepth_;
+#endif
 };
 
 
@@ -291,7 +348,7 @@ public:
 	virtual std::string getSerial() const;
 
 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);
 
 private:
 	CameraImages cameraDepth_;

corelib/include/rtabmap/core/CameraStereo.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/CameraModel.h"
 #include "rtabmap/core/Camera.h"
 #include "rtabmap/core/CameraRGB.h"
+#include "rtabmap/core/Version.h"
 #include <list>
 
 namespace FlyCapture2
@@ -39,6 +40,14 @@ namespace FlyCapture2
 class Camera;
 }
 
+namespace sl
+{
+namespace zed
+{
+class Camera;
+}
+}
+
 namespace rtabmap
 {
 
@@ -62,11 +71,13 @@ public:
 	virtual std::string getSerial() const;
 
 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);
 
 private:
+#ifdef RTABMAP_DC1394
 	DC1394Device *device_;
 	StereoCameraModel stereoModel_;
+#endif
 };
 
 /////////////////////////
@@ -87,11 +98,69 @@ public:
 	virtual std::string getSerial() const;
 
 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);
 
 private:
+#ifdef RTABMAP_FLYCAPTURE2
 	FlyCapture2::Camera * camera_;
 	void * triclopsCtx_; // TriclopsContext
+#endif
+};
+
+/////////////////////////
+// CameraStereoZED
+/////////////////////////
+class RTABMAP_EXP CameraStereoZed :
+	public Camera
+{
+public:
+	static bool available();
+
+public:
+	CameraStereoZed(
+			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 confidenceThr = 100,
+			bool computeOdometry = false,
+			float imageRate=0.0f,
+			const Transform & localTransform = Transform::getIdentity(),
+			bool selfCalibration = false);
+	CameraStereoZed(
+			const std::string & svoFilePath,
+			int quality = 1,    // 0=NONE, 1=PERFORMANCE, 2=QUALITY
+			int sensingMode = 1,// 0=FULL, 1=RAW
+			int confidenceThr = 100,
+			bool computeOdometry = false,
+			float imageRate=0.0f,
+			const Transform & localTransform = Transform::getIdentity(),
+			bool selfCalibration = false);
+	virtual ~CameraStereoZed();
+
+	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);
+
+private:
+#ifdef RTABMAP_ZED
+	sl::zed::Camera * zed_;
+	StereoCameraModel stereoModel_;
+	CameraVideo::Source src_;
+	int usbDevice_;
+	std::string svoFilePath_;
+	int resolution_;
+	int quality_;
+	bool selfCalibration_;
+	int sensingMode_;
+	int confidenceThr_;
+	bool computeOdometry_;
+	bool lost_;
+#endif
 };
 
 /////////////////////////
@@ -123,7 +192,7 @@ public:
 	virtual std::string getSerial() const;
 
 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);
 
 private:
 	CameraImages * camera2_;
@@ -143,10 +212,21 @@ public:
 
 public:
 	CameraStereoVideo(
-			const std::string & path,
+			const std::string & pathSideBySide,
 			bool rectifyImages = false,
 			float imageRate=0.0f,
 			const Transform & localTransform = Transform::getIdentity());
+	CameraStereoVideo(
+			const std::string & pathLeft,
+			const std::string & pathRight,
+			bool rectifyImages = false,
+			float imageRate=0.0f,
+			const Transform & localTransform = Transform::getIdentity());
+	CameraStereoVideo(
+			int device,
+			bool rectifyImages = false,
+			float imageRate = 0.0f,
+			const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraStereoVideo();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -154,14 +234,18 @@ public:
 	virtual std::string getSerial() const;
 
 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);
 
 private:
 	cv::VideoCapture capture_;
+	cv::VideoCapture capture2_;
 	std::string path_;
+	std::string path2_;
 	bool rectifyImages_;
 	StereoCameraModel stereoModel_;
 	std::string cameraName_;
+	CameraVideo::Source src_;
+	int usbDevice_;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/CameraThread.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -33,6 +33,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UThread.h>
 #include <rtabmap/utilite/UEventsSender.h>
 
+namespace clams
+{
+class DiscreteDepthDistortionModel;
+}
+
 namespace rtabmap
 {
 
@@ -56,6 +61,10 @@ public:
 	void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;}
 	void setImageDecimation(int decimation) {_imageDecimation = decimation;}
 	void setStereoToDepth(bool enabled) {_stereoToDepth = enabled;}
+	void setImageRate(float imageRate);
+	void setDistortionModel(const std::string & path);
+	void enableBilateralFiltering(float sigmaS, float sigmaR);
+	void disableBilateralFiltering() {_bilateralFiltering = false;}
 
 	void setScanFromDepth(
 			bool enabled,
@@ -74,11 +83,11 @@ public:
 	//getters
 	bool isPaused() const {return !this->isRunning();}
 	bool isCapturing() const {return this->isRunning();}
-	void setImageRate(float imageRate);
 
 	Camera * camera() {return _camera;} // return null if not set, valid until CameraThread is deleted
 
 private:
+	virtual void mainLoopBegin();
 	virtual void mainLoop();
 	virtual void mainLoopKill();
 
@@ -91,9 +100,14 @@ private:
 	bool _scanFromDepth;
 	int _scanDecimation;
 	float _scanMaxDepth;
+	float _scanMinDepth;
 	float _scanVoxelSize;
 	int _scanNormalsK;
 	StereoDense * _stereoDense;
+	clams::DiscreteDepthDistortionModel * _distortionModel;
+	bool _bilateralFiltering;
+	float _bilateralSigmaS;
+	float _bilateralSigmaR;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/Compression.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/DBDriver.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UThreadNode.h"
 #include "rtabmap/core/Parameters.h"
 #include "rtabmap/core/SensorData.h"
+#include <rtabmap/core/Statistics.h>
 
 #include <rtabmap/core/Transform.h>
 #include <rtabmap/core/Link.h>
@@ -86,15 +87,22 @@ public:
 	void addLink(const Link & link);
 	void removeLink(int from, int to);
 	void updateLink(const Link & link);
+	void updateOccupancyGrid(
+				int nodeId,
+				const cv::Mat & ground,
+				const cv::Mat & obstacles,
+				float cellSize,
+				const cv::Point3f & viewpoint);
 
 public:
-	void addStatisticsAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed, int dictionarySize) const;
+	void addInfoAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed, int dictionarySize, const ParametersMap & parameters) const;
+	void addStatistics(const Statistics & statistics) const;
 
 public:
 	// Mutex-protected methods of abstract versions below
 
 	bool openConnection(const std::string & url, bool overwritten = false);
-	void closeConnection();
+	void closeConnection(bool save = true, const std::string & outputUrl = "");
 	bool isConnected() const;
 	long getMemoryUsed() const; // In bytes
 	std::string getDatabaseVersion() const;
@@ -107,6 +115,8 @@ public:
 	int getLastDictionarySize() const; // working memory
 	int getTotalNodesSize() const;
 	int getTotalDictionarySize() const;
+	ParametersMap getLastParameters() const;
+	std::map<std::string, float> getStatistics(int nodeId, double & stamp) const;
 
 	void executeNoResult(const std::string & sql) const;
 
@@ -117,8 +127,9 @@ public:
 	void loadWords(const std::set<int> & wordIds, std::list<VisualWord *> & vws);
 
 	// Specific queries...
-	void loadNodeData(std::list<Signature *> & signatures) const;
+	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) 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;
 	void loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
 	void getWeight(int signatureId, int & weight) const;
@@ -135,7 +146,7 @@ protected:
 
 private:
 	virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false) = 0;
-	virtual void disconnectDatabaseQuery() = 0;
+	virtual void disconnectDatabaseQuery(bool save = true, const std::string & outputUrl = "") = 0;
 	virtual bool isConnectedQuery() const = 0;
 	virtual long getMemoryUsedQuery() const = 0; // In bytes
 	virtual bool getDatabaseVersionQuery(std::string & version) const = 0;
@@ -148,6 +159,8 @@ private:
 	virtual int getLastDictionarySizeQuery() const = 0;
 	virtual int getTotalNodesSizeQuery() const = 0;
 	virtual int getTotalDictionarySizeQuery() const = 0;
+	virtual ParametersMap getLastParametersQuery() const = 0;
+	virtual std::map<std::string, float> getStatisticsQuery(int nodeId, double & stamp) const = 0;
 
 	virtual void executeNoResultQuery(const std::string & sql) const = 0;
 
@@ -161,6 +174,15 @@ private:
 	virtual void addLinkQuery(const Link & link) const = 0;
 	virtual void updateLinkQuery(const Link & link) const = 0;
 
+	virtual void updateOccupancyGridQuery(
+				int nodeId,
+				const cv::Mat & ground,
+				const cv::Mat & obstacles,
+				float cellSize,
+				const cv::Point3f & viewpoint) const = 0;
+
+	virtual void addStatisticsQuery(const Statistics & statistics) const = 0;
+
 	// Load objects
 	virtual void loadQuery(VWDictionary * dictionary) const = 0;
 	virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const = 0;
@@ -168,7 +190,8 @@ private:
 	virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const = 0;
 	virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const = 0;
 
-	virtual void loadNodeDataQuery(std::list<Signature *> & signatures) const = 0;
+	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 void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const = 0;
 	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const = 0;

corelib/include/rtabmap/core/DBReader.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -30,11 +30,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 
-#include <rtabmap/utilite/UThreadNode.h>
 #include <rtabmap/utilite/UTimer.h>
-#include <rtabmap/utilite/UEventsSender.h>
 #include <rtabmap/core/Transform.h>
-#include <rtabmap/core/OdometryEvent.h>
+#include <rtabmap/core/Camera.h>
 
 #include <opencv2/core/core.hpp>
 
@@ -45,41 +43,55 @@ namespace rtabmap {
 
 class DBDriver;
 
-class RTABMAP_EXP DBReader : public UThreadNode, public UEventsSender {
+class RTABMAP_EXP DBReader : public Camera {
 public:
 	DBReader(const std::string & databasePath,
-			 float frameRate = 0.0f,
+			 float frameRate = 0.0f, // -1 = use Database stamps, 0 = inf
 			 bool odometryIgnored = false,
 			 bool ignoreGoalDelay = false,
-			 bool goalsIgnored = false);
+			 bool goalsIgnored = false,
+			 int startIndex = 0,
+			 int cameraIndex = -1);
 	DBReader(const std::list<std::string> & databasePaths,
-			 float frameRate = 0.0f,
+			 float frameRate = 0.0f, // -1 = use Database stamps, 0 = inf
 			 bool odometryIgnored = false,
 			 bool ignoreGoalDelay = false,
-			 bool goalsIgnored = false);
+			 bool goalsIgnored = false,
+			 int startIndex = 0,
+			 int cameraIndex = -1);
 	virtual ~DBReader();
 
-	bool init(int startIndex=0);
+	virtual bool init(
-	void setFrameRate(float frameRate);
+			const std::string & calibrationFolder = ".",
-	OdometryEvent getNextData();
+			const std::string & cameraName = "");
+
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+	virtual bool odomProvided() const {return !_odometryIgnored;}
 
 protected:
-	virtual void mainLoopBegin();
+	virtual SensorData captureImage(CameraInfo * info = 0);
-	virtual void mainLoop();
+
+private:
+	SensorData getNextData(CameraInfo * info = 0);
 
 private:
 	std::list<std::string> _paths;
-	float _frameRate; // -1 = use Database stamps, 0 = inf
 	bool _odometryIgnored;
 	bool _ignoreGoalDelay;
 	bool _goalsIgnored;
+	int _startIndex;
+	int _cameraIndex;
 
 	DBDriver * _dbDriver;
 	UTimer _timer;
 	std::set<int> _ids;
 	std::set<int>::iterator _currentId;
+	int _previousMapId;
+	cv::Mat _previousInfMatrix;
 	double _previousStamp;
 	int _previousMapID;
+	bool _calibrated;
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/EpipolarGeometry.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -94,7 +94,8 @@ public:
 	static int findPairs(
 			const std::map<int, cv::KeyPoint> & wordsA,
 			const std::map<int, cv::KeyPoint> & wordsB,
-			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs);
+			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
+			bool ignoreNegativeIds = true);
 
 	/**
 	 * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (2,2) (4,4) (6a,6a) (6b,6b)]
@@ -103,7 +104,8 @@ public:
 	static int findPairs(
 			const std::multimap<int, cv::KeyPoint> & wordsA,
 			const std::multimap<int, cv::KeyPoint> & wordsB,
-			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs);
+			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
+			bool ignoreNegativeIds = true);
 
 	/**
 	 * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(2,2) (4,4)]
@@ -112,7 +114,8 @@ public:
 	static int findPairsUnique(
 			const std::multimap<int, cv::KeyPoint> & wordsA,
 			const std::multimap<int, cv::KeyPoint> & wordsB,
-			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs);
+			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
+			bool ignoreNegativeIds = true);
 
 	/**
 	 * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (1,1b) (2,2) (4,4) (6a,6a) (6a,6b) (6b,6a) (6b,6b)]
@@ -121,7 +124,8 @@ public:
 	static int findPairsAll(
 			const std::multimap<int, cv::KeyPoint> & wordsA,
 			const std::multimap<int, cv::KeyPoint> & wordsB,
-			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs);
+			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
+			bool ignoreNegativeIds = true);
 
 	static cv::Mat linearLSTriangulation(
 			cv::Point3d u,    //homogenous image point (u,v,1)

corelib/include/rtabmap/core/Features2d.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -104,7 +104,8 @@ public:
 		kFeatureGfttFreak=5,
 		kFeatureGfttBrief=6,
 		kFeatureBrisk=7,
-		kFeatureGfttOrb=8}; //new 0.10.11
+		kFeatureGfttOrb=8,  //new 0.10.11
+		kFeatureFreak=9};   //new 0.11.14
 
 	static Feature2D * create(const ParametersMap & parameters = ParametersMap());
 	static Feature2D * create(Feature2D::Type type, const ParametersMap & parameters = ParametersMap()); // for convenience
@@ -133,6 +134,8 @@ public:
 
 	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints);
 	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints);
+	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & keypoints3D, cv::Mat & descriptors, int maxKeypoints);
+	static void limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints);
 
 	static cv::Rect computeRoi(const cv::Mat & image, const std::string & roiRatios);
 	static cv::Rect computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios);
@@ -430,6 +433,29 @@ private:
 	cv::Ptr<CV_BRISK> brisk_;
 };
 
+//FREAK
+class RTABMAP_EXP FREAK : public Feature2D
+{
+public:
+	FREAK(const ParametersMap & parameters = ParametersMap());
+	virtual ~FREAK();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const { return kFeatureFreak; }
+
+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_;
+	int nOctaves_;
+
+	cv::Ptr<CV_FREAK> _freak;
+};
+
 
 }
 

corelib/include/rtabmap/core/FlannIndex.h

@@ -0,0 +1,114 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef CORELIB_SRC_FLANNINDEX_H_
+#define CORELIB_SRC_FLANNINDEX_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include <list>
+#include <opencv2/opencv.hpp>
+
+namespace rtabmap {
+
+class RTABMAP_EXP FlannIndex
+{
+public:
+	FlannIndex();
+	virtual ~FlannIndex();
+
+	void release();
+	unsigned int indexedFeatures() const;
+
+	// return KB
+	unsigned int memoryUsed() const;
+
+	// Note that useDistanceL1 doesn't have any effect if LSH is used
+	void buildLinearIndex(
+			const cv::Mat & features,
+			bool useDistanceL1 = false);
+	void buildKDTreeIndex(
+			const cv::Mat & features,
+			int trees = 4,
+			bool useDistanceL1 = false);
+	void buildKDTreeSingleIndex(
+			const cv::Mat & features,
+			int leafMaxSize = 10,
+			bool reorder = true,
+			bool useDistanceL1 = false);
+	void buildLSHIndex(
+			const cv::Mat & features,
+			unsigned int table_number = 12,
+			unsigned int key_size = 20,
+			unsigned int multi_probe_level = 2);
+
+	bool isBuilt();
+
+	int featuresType() const {return featuresType_;}
+	int featuresDim() const {return featuresDim_;}
+
+	unsigned int addPoints(const cv::Mat & features);
+
+	void removePoint(unsigned int index);
+
+	// return squared distances
+	void knnSearch(
+			const cv::Mat & query,
+			cv::Mat & indices,
+			cv::Mat & dists,
+	        int knn,
+			int checks = 32,
+			float eps = 0.0,
+			bool sorted = true) const;
+
+	// return squared distances
+	void radiusSearch(
+			const cv::Mat & query,
+			std::vector<std::vector<size_t> > & indices,
+			std::vector<std::vector<float> > & dists,
+			float radius,
+			int maxNeighbors = 0,
+			int checks = 32,
+			float eps = 0.0,
+			bool sorted = true) const;
+
+private:
+	void * index_;
+	unsigned int nextIndex_;
+	int featuresType_;
+	int featuresDim_;
+	bool isLSH_;
+	bool useDistanceL1_; // true=EUCLEDIAN_L2 false=MANHATTAN_L1
+
+	// keep feature in memory until the tree is rebuilt
+	// (in case the word is deleted when removed from the VWDictionary)
+	std::map<int, cv::Mat> addedDescriptors_;
+	std::list<int> removedIndexes_;
+};
+
+} /* namespace rtabmap */
+
+#endif /* CORELIB_SRC_FLANNINDEX_H_ */

corelib/include/rtabmap/core/GainCompensator.h

@@ -0,0 +1,104 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef CORELIB_SRC_GAINCOMPENSATOR_H_
+#define CORELIB_SRC_GAINCOMPENSATOR_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/pcl_base.h>
+#include <opencv2/opencv.hpp>
+#include <rtabmap/core/Link.h>
+
+namespace rtabmap {
+
+/**
+ * Works like cv::GainCompensator but with point clouds
+ */
+class RTABMAP_EXP GainCompensator {
+public:
+	GainCompensator(double maxCorrespondenceDistance = 0.02, double minOverlap = 0.05, double alpha = 0.01, double beta = 10);
+	virtual ~GainCompensator();
+
+	void feed(
+			const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloudA, // should not contain NaNs
+			const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloudB, // should not contain NaNs
+			const Transform & transformB);
+	void feed(
+			const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloudA,
+			const pcl::IndicesPtr & indicesA,
+			const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloudB,
+			const pcl::IndicesPtr & indicesB,
+			const Transform & transformB);
+	void feed(
+			const std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds, // should not contain NaNs
+			const std::multimap<int, Link> & links);
+	void feed(
+			const std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds,
+			const std::map<int, pcl::IndicesPtr> & indices,
+			const std::multimap<int, Link> & links);
+	void feed(
+			const std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> & clouds,
+			const std::map<int, pcl::IndicesPtr> & indices,
+			const std::multimap<int, Link> & links);
+	void feed(
+			const std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::IndicesPtr> > & clouds,
+			const std::multimap<int, Link> & links);
+
+	void apply(
+			int id,
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud) const;
+	void apply(
+			int id,
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+			const pcl::IndicesPtr & indices) const;
+	void apply(
+			int id,
+			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+			const pcl::IndicesPtr & indices) const;
+	void apply(
+			int id,
+			cv::Mat & image) const;
+
+	double getGain(int id) const;
+	int getIndex(int id) const;
+
+private:
+	cv::Mat_<double> gains_;
+	std::map<int, int> idToIndex_;
+	double maxCorrespondenceDistance_;
+	double minOverlap_;
+	double alpha_;
+	double beta_;
+
+};
+
+} /* namespace rtabmap */
+
+#endif /* CORELIB_SRC_GAINCOMPENSATOR_H_ */

corelib/include/rtabmap/core/Graph.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -79,6 +79,13 @@ std::multimap<int, int>::const_iterator RTABMAP_EXP findLink(
 		int to,
 		bool checkBothWays = true);
 
+std::multimap<int, Link> RTABMAP_EXP filterLinks(
+		const std::multimap<int, Link> & links,
+		Link::Type filteredType);
+std::map<int, Link> RTABMAP_EXP filterLinks(
+		const std::map<int, Link> & links,
+		Link::Type filteredType);
+
 //Note: This assumes a coordinate system where X is forward, * Y is up, and Z is right.
 std::map<int, Transform> RTABMAP_EXP frustumPosesFiltering(
 		const std::map<int, Transform> & poses,

corelib/include/rtabmap/core/Image.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/LaserScanInfo.h

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

corelib/include/rtabmap/core/Link.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -76,11 +76,7 @@ public:
 	void setTo(int to) {to_ = to;}
 	void setTransform(const Transform & transform) {transform_ = transform;}
 	void setType(Type type) {type_ = type;}
-	void setInfMatrix(const cv::Mat & infMatrix);
-	void setVariance(double rotVariance, double transVariance);
 
-	void setUserDataRaw(const cv::Mat & userDataRaw); // only set raw
-	void setUserData(const cv::Mat & userData); // detect automatically if raw or compressed. If raw, the data is compressed too.
 	const cv::Mat & userDataRaw() const {return _userDataRaw;}
 	const cv::Mat & userDataCompressed() const {return _userDataCompressed;}
 	void uncompressUserData();
@@ -89,6 +85,10 @@ public:
 	Link merge(const Link & link, Type outputType) const;
 	Link inverse() const;
 
+private:
+	void setInfMatrix(const cv::Mat & infMatrix);
+	void setVariance(double rotVariance, double transVariance);
+
 private:
 	int from_;
 	int to_;

corelib/include/rtabmap/core/Memory.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -55,6 +55,7 @@ class Registration;
 class RegistrationInfo;
 class RegistrationIcp;
 class Stereo;
+class OccupancyGrid;
 
 class RTABMAP_EXP Memory
 {
@@ -79,7 +80,7 @@ public:
 			bool dbOverwritten = false,
 			const ParametersMap & parameters = ParametersMap(),
 			bool postInitClosingEvents = false);
-	void close(bool databaseSaved = true, bool postInitClosingEvents = false);
+	void close(bool databaseSaved = true, bool postInitClosingEvents = false, const std::string & ouputDatabasePath = "");
 	std::map<int, float> computeLikelihood(const Signature * signature,
 			const std::list<int> & ids);
 	int incrementMapId(std::map<int, int> * reducedIds = 0);
@@ -89,11 +90,11 @@ public:
 	std::set<int> reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, double & timeDbAccess);
 
 	int cleanup();
+	void saveStatistics(const Statistics & statistics);
 	void emptyTrash();
 	void joinTrashThread();
-	bool addLink(const Link & link);
+	bool addLink(const Link & link, bool addInDatabase = false);
-	void updateLink(int fromId, int toId, const Transform & transform, float rotVariance, float transVariance);
+	void updateLink(const Link & link, bool updateInDatabase = false);
-	void updateLink(int fromId, int toId, const Transform & transform, const cv::Mat & covariance);
 	void removeAllVirtualLinks();
 	void removeVirtualLinks(int signatureId);
 	std::map<int, int> getNeighborsId(
@@ -132,6 +133,13 @@ public:
 	int getSignatureIdByLabel(const std::string & label, bool lookInDatabase = true) const;
 	bool labelSignature(int id, const std::string & label);
 	std::map<int, std::string> getAllLabels() const;
+	/**
+	 * Set user data. Detect automatically if raw or compressed. If raw, the data is
+	 * compressed too. A matrix of type CV_8UC1 with 1 row is considered as compressed.
+	 * If you have one dimension unsigned 8 bits raw data, make sure to transpose it
+	 * (to have multiple rows instead of multiple columns) in order to be detected as
+	 * not compressed.
+	 */
 	bool setUserData(int id, const cv::Mat & data);
 	int getDatabaseMemoryUsed() const; // in bytes
 	std::string getDatabaseVersion() const;
@@ -147,11 +155,15 @@ public:
 			Transform & groundTruth,
 			bool lookInDatabase = false) const;
 	cv::Mat getImageCompressed(int signatureId) const;
-	SensorData getNodeData(int nodeId, bool uncompressedData = false, bool keepLoadedDataInMemory = true);
+	SensorData getNodeData(int nodeId, bool uncompressedData = false) const;
 	void getNodeWords(int nodeId,
 			std::multimap<int, cv::KeyPoint> & words,
-			std::multimap<int, cv::Point3f> & words3);
+			std::multimap<int, cv::Point3f> & words3,
-	SensorData getSignatureDataConst(int locationId) const;
+			std::multimap<int, cv::Mat> & wordsDescriptors);
+	void getNodeCalibration(int nodeId,
+			std::vector<CameraModel> & models,
+			StereoCameraModel & stereoModel);
+	SensorData getSignatureDataConst(int locationId, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
 	std::set<int> getAllSignatureIds() const;
 	bool memoryChanged() const {return _memoryChanged;}
 	bool isIncremental() const {return _incrementalMemory;}
@@ -181,7 +193,8 @@ public:
 			std::multimap<int, Link> & links,
 			bool lookInDatabase = false);
 
-	Transform computeTransform(int fromId, int toId, Transform guess, RegistrationInfo * info = 0);
+	Transform computeTransform(Signature & fromS, Signature & toS, Transform guess, RegistrationInfo * info = 0, bool useKnownCorrespondencesIfPossible = false) const;
+	Transform computeTransform(int fromId, int toId, Transform guess, RegistrationInfo * info = 0, bool useKnownCorrespondencesIfPossible = false);
 	Transform computeIcpTransform(int fromId, int toId, Transform guess, RegistrationInfo * info = 0);
 	Transform computeIcpTransformMulti(
 			int newId,
@@ -193,6 +206,7 @@ private:
 	void preUpdate();
 	void addSignatureToStm(Signature * signature, const cv::Mat & covariance);
 	void clear();
+	void loadDataFromDb(bool postInitClosingEvents);
 	void moveToTrash(Signature * s, bool keepLinkedToGraph = true, std::list<int> * deletedWords = 0);
 
 	void moveSignatureToWMFromSTM(int id, int * reducedTo = 0);
@@ -239,13 +253,17 @@ private:
 	bool _generateIds;
 	bool _badSignaturesIgnored;
 	bool _mapLabelsAdded;
-	int _imageDecimation;
+	int _imagePreDecimation;
+	int _imagePostDecimation;
 	float _laserScanDownsampleStepSize;
+	int _laserScanNormalK;
 	bool _reextractLoopClosureFeatures;
 	float _rehearsalMaxDistance;
 	float _rehearsalMaxAngle;
 	bool _rehearsalWeightIgnoredWhileMoving;
 	bool _useOdometryFeatures;
+	bool _createOccupancyGrid;
+	int _visMaxFeatures;
 
 	int _idCount;
 	int _idMapCount;
@@ -268,6 +286,8 @@ private:
 
 	Registration * _registrationPipeline;
 	RegistrationIcp * _registrationIcp;
+
+	OccupancyGrid * _occupancy;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/OccupancyGrid.h

@@ -0,0 +1,121 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef CORELIB_SRC_OCCUPANCYGRID_H_
+#define CORELIB_SRC_OCCUPANCYGRID_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <pcl/point_cloud.h>
+#include <pcl/pcl_base.h>
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/Signature.h>
+
+namespace rtabmap {
+
+class RTABMAP_EXP OccupancyGrid
+{
+public:
+	OccupancyGrid(const ParametersMap & parameters = ParametersMap());
+	void parseParameters(const ParametersMap & parameters);
+	void setCellSize(float cellSize);
+	float getCellSize() const {return cellSize_;}
+	bool isGridFromDepth() const {return occupancyFromCloud_;}
+
+	template<typename PointT>
+	typename pcl::PointCloud<PointT>::Ptr segmentCloud(
+			const typename pcl::PointCloud<PointT>::Ptr & cloud,
+			const pcl::IndicesPtr & indices,
+			const Transform & pose,
+			const cv::Point3f & viewPoint,
+			pcl::IndicesPtr & groundIndices,        // output cloud indices
+			pcl::IndicesPtr & obstaclesIndices,     // output cloud indices
+			pcl::IndicesPtr * flatObstacles = 0) const; // output cloud indices
+
+	void createLocalMap(
+			const Signature & node,
+			cv::Mat & ground,
+			cv::Mat & obstacles,
+			cv::Point3f & viewPoint) const;
+
+	void clear();
+	void addToCache(
+			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_;
+	}
+
+private:
+	ParametersMap parameters_;
+	int cloudDecimation_;
+	float cloudMaxDepth_;
+	float cloudMinDepth_;
+	std::vector<float> roiRatios_;
+	float footprintLength_;
+	float footprintWidth_;
+	float footprintHeight_;
+	int scanDecimation_;
+	float cellSize_;
+	bool occupancyFromCloud_;
+	bool projMapFrame_;
+	float maxObstacleHeight_;
+	int normalKSearch_;
+	float maxGroundAngle_;
+	float clusterRadius_;
+	int minClusterSize_;
+	bool flatObstaclesDetected_;
+	float minGroundHeight_;
+	float maxGroundHeight_;
+	bool normalsSegmentation_;
+	bool grid3D_;
+	bool groundIsObstacle_;
+	float noiseFilteringRadius_;
+	int noiseFilteringMinNeighbors_;
+	bool scan2dUnknownSpaceFilled_;
+	double scan2dMaxUnknownSpaceFilledRange_;
+	bool projRayTracing_;
+
+	std::map<int, std::pair<cv::Mat, cv::Mat> > cache_;
+	cv::Mat map_;
+	cv::Mat mapInfo_;
+	std::map<int, std::pair<int, int> > cellCount_; //<node Id, cells>
+	float xMin_;
+	float yMin_;
+	std::map<int, Transform> addedNodes_;
+};
+
+}
+
+#include <rtabmap/core/impl/OccupancyGrid.hpp>
+
+#endif /* CORELIB_SRC_OCCUPANCYGRID_H_ */

corelib/include/rtabmap/core/OctoMap.h

@@ -0,0 +1,104 @@
+/*
+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 SRC_OCTOMAP_H_
+#define SRC_OCTOMAP_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <octomap/ColorOcTree.h>
+#include <octomap/OcTreeKey.h>
+
+#include <pcl/pcl_base.h>
+#include <pcl/point_types.h>
+
+#include <rtabmap/core/Transform.h>
+
+#include <map>
+#include <string>
+
+namespace rtabmap {
+
+class OcTreeNodeInfo
+{
+public:
+	OcTreeNodeInfo(int nodeRefId, const octomap::OcTreeKey & key, bool isObstacle) :
+		nodeRefId_(nodeRefId),
+		key_(key),
+		isObstacle_(isObstacle) {}
+	int nodeRefId_;
+	octomap::OcTreeKey key_;
+	bool isObstacle_;
+};
+
+class RTABMAP_EXP OctoMap {
+public:
+	OctoMap(float voxelSize = 0.1f, float occupancyThr = 0.5f);
+
+	const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
+	void addToCache(int nodeId,
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr & ground,
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstacles,
+			const pcl::PointXYZ & viewPoint);
+	void addToCache(int nodeId,
+			const cv::Mat & ground,
+			const cv::Mat & obstacles,
+			const cv::Point3f & viewPoint);
+	void update(const std::map<int, Transform> & poses);
+
+	const octomap::ColorOcTree * octree() const {return octree_;}
+
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr createCloud(
+			unsigned int treeDepth = 0,
+			std::vector<int> * obstacleIndices = 0,
+			std::vector<int> * emptyIndices = 0) const;
+
+	cv::Mat createProjectionMap(
+			float & xMin,
+			float & yMin,
+			float & gridCellSize,
+			float minGridSize);
+
+	bool writeBinary(const std::string & path);
+
+	virtual ~OctoMap();
+	void clear();
+
+private:
+	std::map<int, std::pair<cv::Mat, cv::Mat> > cache_;
+	std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> > cacheClouds_;
+	std::map<int, cv::Point3f> cacheViewPoints_;
+	octomap::ColorOcTree * octree_;
+	std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo> occupiedCells_;
+	std::map<int, Transform> addedNodes_;
+	octomap::KeyRay keyRay_;
+	bool hasColor_;
+};
+
+} /* namespace rtabmap */
+
+#endif /* SRC_OCTOMAP_H_ */

corelib/include/rtabmap/core/Odometry.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -44,7 +44,7 @@ class RTABMAP_EXP Odometry
 public:
 	enum Type {
 		kTypeUndef = -1,
-		kTypeLocalMap = 0,
+		kTypeF2M = 0,
 		kTypeF2F = 1
 	};
 
@@ -55,12 +55,15 @@ public:
 public:
 	virtual ~Odometry();
 	Transform process(SensorData & data, OdometryInfo * info = 0);
+	Transform process(SensorData & data, const Transform & guess, OdometryInfo * info = 0);
 	virtual void reset(const Transform & initialPose = Transform::getIdentity());
+	virtual Odometry::Type getType() = 0;
 
 	//getters
 	const Transform & getPose() const {return _pose;}
 	bool isInfoDataFilled() const {return _fillInfoData;}
 	const Transform & previousVelocityTransform() const {return previousVelocityTransform_;}
+	double previousStamp() const {return previousStamp_;}
 
 private:
 	virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0) = 0;
@@ -83,6 +86,8 @@ private:
 	bool _fillInfoData;
 	float _kalmanProcessNoise;
 	float _kalmanMeasurementNoise;
+	int _imageDecimation;
+	bool _alignWithGround;
 	Transform _pose;
 	int _resetCurrentCount;
 	double previousStamp_;

corelib/include/rtabmap/core/OdometryEvent.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/OdometryF2F.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -45,17 +45,20 @@ public:
 
 	const Signature & getRefFrame() const {return refFrame_;}
 
+	virtual Odometry::Type getType() {return Odometry::kTypeF2F;}
+
 private:
 	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
 
 private:
 	//Parameters:
 	float keyFrameThr_;
+	int visKeyFrameThr_;
 	float scanKeyFrameThr_;
 
 	Registration * registrationPipeline_;
 	Signature refFrame_;
-	Transform motionSinceLastKeyFrame_;
+	Transform lastKeyFramePose_;
 };
 
 }

corelib/include/rtabmap/core/OdometryF2M.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -31,11 +31,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Odometry.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
+#include <pcl/pcl_base.h>
+#include <rtabmap/core/Link.h>
 
 namespace rtabmap {
 
 class Signature;
 class Registration;
+class Optimizer;
 
 class RTABMAP_EXP OdometryF2M : public Odometry
 {
@@ -47,6 +50,8 @@ public:
 	const Signature & getMap() const {return *map_;}
 	const Signature & getLastFrame() const {return *lastFrame_;}
 
+	virtual Odometry::Type getType() {return Odometry::kTypeF2M;}
+
 private:
 	virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0);
 
@@ -54,16 +59,26 @@ private:
 	//Parameters
 	int maximumMapSize_;
 	float keyFrameThr_;
+	int visKeyFrameThr_;
 	int maxNewFeatures_;
 	float scanKeyFrameThr_;
 	int scanMaximumMapSize_;
-	float scanSubstractRadius_;
+	float scanSubtractRadius_;
-	std::string fixedMapPath_;
+	int bundleAdjustment_;
+	int bundleMaxFrames_;
 
 	Registration * regPipeline_;
 	Signature * map_;
 	Signature * lastFrame_;
-	std::map<int, pcl::PointCloud<pcl::PointNormal>::Ptr > scansBuffer_;
+	std::vector<std::pair<pcl::PointCloud<pcl::PointNormal>::Ptr, pcl::IndicesPtr> > scansBuffer_;
+
+	std::map<int, std::map<int, cv::Point3f> > bundleWordReferences_; //<WordId, <FrameId, pt2D+depth>>
+	std::map<int, Transform> bundlePoses_;
+	std::multimap<int, Link> bundleLinks_;
+	std::map<int, CameraModel> bundleModels_;
+	std::map<int, int> bundlePoseReferences_;
+	int bundleSeq_;
+	Optimizer * sba_;
 };
 
 }

corelib/include/rtabmap/core/OdometryInfo.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -42,10 +42,16 @@ public:
 		matches(0),
 		inliers(0),
 		icpInliersRatio(0.0f),
-		variance(0.0f),
+		varianceLin(0.0f),
+		varianceAng(0.0f),
 		features(0),
 		localMapSize(0),
 		localScanMapSize(0),
+		localKeyFrames(0),
+		localBundleOutliers(0),
+		localBundleConstraints(0),
+		localBundleTime(0),
+		keyFrameAdded(false),
 		timeEstimation(0.0f),
 		timeParticleFiltering(0.0f),
 		stamp(0),
@@ -61,10 +67,16 @@ public:
 		output.matches = matches;
 		output.inliers = inliers;
 		output.icpInliersRatio = icpInliersRatio;
-		output.variance = variance;
+		output.varianceLin = varianceLin;
+		output.varianceAng = varianceAng;
 		output.features = features;
 		output.localMapSize = localMapSize;
 		output.localScanMapSize = localScanMapSize;
+		output.localKeyFrames = localKeyFrames;
+		output.localBundleOutliers = localBundleOutliers;
+		output.localBundleConstraints = localBundleConstraints;
+		output.localBundleTime = localBundleTime;
+		output.keyFrameAdded = keyFrameAdded;
 		output.timeEstimation = timeEstimation;
 		output.timeParticleFiltering = timeParticleFiltering;
 		output.stamp = stamp;
@@ -72,6 +84,7 @@ public:
 		output.transformFiltered = transformFiltered;
 		output.transformGroundTruth = transformGroundTruth;
 		output.distanceTravelled = distanceTravelled;
+		output.type = type;
 		return output;
 	}
 
@@ -79,10 +92,16 @@ public:
 	int matches;
 	int inliers;
 	float icpInliersRatio;
-	float variance;
+	float varianceLin;
+	float varianceAng;
 	int features;
 	int localMapSize;
 	int localScanMapSize;
+	int localKeyFrames;
+	int localBundleOutliers;
+	int localBundleConstraints;
+	float localBundleTime;
+	bool keyFrameAdded;
 	float timeEstimation;
 	float timeParticleFiltering;
 	double stamp;

corelib/include/rtabmap/core/OdometryMono.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/OdometryThread.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -48,12 +48,13 @@ protected:
 	virtual void handleEvent(UEvent * event);
 
 private:
-	void mainLoopKill();
+	virtual void mainLoopBegin();
+	virtual void mainLoopKill();
 
 	//============================================================
 	// MAIN LOOP
 	//============================================================
-	void mainLoop();
+	virtual void mainLoop();
 	void addData(const SensorData & data);
 	bool getData(SensorData & data);
 

corelib/include/rtabmap/core/Optimizer.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -53,7 +53,7 @@ public:
 	};
 	static bool isAvailable(Optimizer::Type type);
 	static Optimizer * create(const ParametersMap & parameters);
-	static Optimizer * create(Optimizer::Type & type, const ParametersMap & parameters = ParametersMap());
+	static Optimizer * create(Optimizer::Type type, const ParametersMap & parameters = ParametersMap());
 
 	// Get connected poses and constraints from a set of links
 	static void getConnectedGraph(
@@ -83,6 +83,8 @@ public:
 	void setEpsilon(double epsilon) {epsilon_ = epsilon;}
 	void setRobust(bool enabled) {robust_ = enabled;}
 
+	virtual void parseParameters(const ParametersMap & parameters);
+
 	// inherited classes should implement one of these methods
 	virtual std::map<int, Transform> optimize(
 			int rootId,
@@ -92,17 +94,38 @@ public:
 			double * finalError = 0,
 			int * iterationsDone = 0);
 	virtual std::map<int, Transform> optimizeBA(
+			int rootId, // if negative, all other poses are fixed
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & links,
+			const std::map<int, CameraModel> & models, // in case of stereo, Tx should be set
+			std::map<int, cv::Point3f> & points3DMap,
+			const std::map<int, std::map<int, cv::Point3f> > & wordReferences, // <ID words, IDs frames + keypoint(x,y,depth)>
+			std::set<int> * outliers = 0);
+
+	std::map<int, Transform> optimizeBA(
 			int rootId,
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & links,
 			const std::map<int, Signature> & signatures);
 
-	virtual void parseParameters(const ParametersMap & parameters);
+	Transform optimizeBA(
+			const Link & link,
+			const CameraModel & model,
+			std::map<int, cv::Point3f> & points3DMap,
+			const std::map<int, std::map<int, cv::Point3f> > & wordReferences,
+			std::set<int> * outliers = 0);
+
+	void computeBACorrespondences(
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & links,
+			const std::map<int, Signature> & signatures,
+			std::map<int, cv::Point3f> & points3DMap,
+			std::map<int, std::map<int, cv::Point3f> > & wordReferences); // <ID words, IDs frames + keypoint/depth>
 
 protected:
 	Optimizer(
 			int iterations         = Parameters::defaultOptimizerIterations(),
-			bool slam2d            = Parameters::defaultOptimizerSlam2D(),
+			bool slam2d            = Parameters::defaultRegForce3DoF(),
 			bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored(),
 			double epsilon         = Parameters::defaultOptimizerEpsilon(),
 			bool robust            = Parameters::defaultOptimizerRobust());

corelib/include/rtabmap/core/OptimizerCVSBA.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -42,31 +42,23 @@ public:
 public:
 	OptimizerCVSBA(
 			int iterations         = Parameters::defaultOptimizerIterations(),
-			bool slam2d            = Parameters::defaultOptimizerSlam2D(),
+			bool slam2d            = Parameters::defaultRegForce3DoF(),
 			bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored()) :
-		Optimizer(iterations, slam2d, covarianceIgnored),
+		Optimizer(iterations, slam2d, covarianceIgnored) {}
-		inlierDistance_(0.02),
-		minInliers_(10){}
 	OptimizerCVSBA(const ParametersMap & parameters) :
-		Optimizer(parameters),
+		Optimizer(parameters) {}
-		inlierDistance_(0.02),
-		minInliers_(10){}
 	virtual ~OptimizerCVSBA() {}
 
 	virtual Type type() const {return kTypeCVSBA;}
 
-	void setInlierDistance(float inlierDistance) {inlierDistance_ = inlierDistance;}
-	void setMinInliers(int minInliers) {minInliers_ = minInliers;}
-
 	virtual std::map<int, Transform> optimizeBA(
 			int rootId,
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & links,
-			const std::map<int, Signature> & signatures);
+			const std::map<int, CameraModel> & models,
-
+			std::map<int, cv::Point3f> & points3DMap,
-private:
+			const std::map<int, std::map<int, cv::Point3f> > & wordReferences, // <ID words, IDs frames + keypoint(x,y,depth)>
-	float inlierDistance_;
+			std::set<int> * outliers = 0);
-	float minInliers_;
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/OptimizerG2O.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -50,7 +50,10 @@ public:
 	OptimizerG2O(const ParametersMap & parameters = ParametersMap()) :
 		Optimizer(parameters),
 		solver_(Parameters::defaultg2oSolver()),
-		optimizer_(Parameters::defaultg2oOptimizer())
+		optimizer_(Parameters::defaultg2oOptimizer()),
+        pixelVariance_(Parameters::defaultg2oPixelVariance()),
+		robustKernelDelta_(Parameters::defaultg2oRobustKernelDelta()),
+		baseline_(Parameters::defaultg2oBaseline())
 	{
 		parseParameters(parameters);
 	}
@@ -68,9 +71,21 @@ public:
 			double * finalError = 0,
 			int * iterationsDone = 0);
 
+	virtual std::map<int, Transform> optimizeBA(
+			int rootId,
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & links,
+			const std::map<int, CameraModel> & models, // in case of stereo, Tx should be set
+			std::map<int, cv::Point3f> & points3DMap,
+			const std::map<int, std::map<int, cv::Point3f> > & wordReferences, // <ID words, IDs frames + keypoint(x,y,depth)>
+			std::set<int> * outliers = 0);
+
 private:
 	int solver_;
 	int optimizer_;
+	double pixelVariance_;
+	double robustKernelDelta_;
+	double baseline_;
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/OptimizerGTSAM.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -40,20 +40,18 @@ public:
 	static bool available();
 
 public:
-	OptimizerGTSAM(
+	OptimizerGTSAM(const ParametersMap & parameters = ParametersMap()) :
-			int iterations         = Parameters::defaultOptimizerIterations(),
+		Optimizer(parameters),
-			bool slam2d            = Parameters::defaultOptimizerSlam2D(),
+		optimizer_(Parameters::defaultGTSAMOptimizer())
-			bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored(),
+	{
-			double epsilon         = Parameters::defaultOptimizerEpsilon(),
+		parseParameters(parameters);
-			bool robust            = Parameters::defaultOptimizerRobust()) :
+	}
-		Optimizer(iterations, slam2d, covarianceIgnored, epsilon, robust) {}
-
-	OptimizerGTSAM(const ParametersMap & parameters) :
-		Optimizer(parameters) {}
 	virtual ~OptimizerGTSAM() {}
 
 	virtual Type type() const {return kTypeGTSAM;}
 
+	virtual void parseParameters(const ParametersMap & parameters);
+
 	virtual std::map<int, Transform> optimize(
 			int rootId,
 			const std::map<int, Transform> & poses,
@@ -61,6 +59,9 @@ public:
 			std::list<std::map<int, Transform> > * intermediateGraphes = 0,
 			double * finalError = 0,
 			int * iterationsDone = 0);
+
+private:
+	int optimizer_;
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/OptimizerTORO.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -52,7 +52,7 @@ public:
 public:
 	OptimizerTORO(
 			int iterations         = Parameters::defaultOptimizerIterations(),
-			bool slam2d            = Parameters::defaultOptimizerSlam2D(),
+			bool slam2d            = Parameters::defaultRegForce3DoF(),
 			bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored(),
 			double epsilon         = Parameters::defaultOptimizerEpsilon()) :
 		Optimizer(iterations, slam2d, covarianceIgnored, epsilon) {}

corelib/include/rtabmap/core/ParamEvent.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/Parameters.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 // default parameters
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 #include "rtabmap/core/Version.h" // DLL export/import defines
+#include <rtabmap/utilite/UConversion.h>
 #include <string>
 #include <map>
 
@@ -172,11 +173,11 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Rtabmap, PublishLastSignature, 	     bool, true, "Publishing last signature.");
 	RTABMAP_PARAM(Rtabmap, PublishPdf, 	                 bool, true, "Publishing pdf.");
 	RTABMAP_PARAM(Rtabmap, PublishLikelihood, 	         bool, true, "Publishing likelihood.");
-	RTABMAP_PARAM(Rtabmap, TimeThr, 		             float, 0.0, "Maximum time allowed for the detector (ms) (0 means infinity).");
+	RTABMAP_PARAM(Rtabmap, TimeThr, 		             float, 0,   "Maximum time allowed for the detector (ms) (0 means infinity).");
 	RTABMAP_PARAM(Rtabmap, MemoryThr, 		             int, 0, 	 "Maximum signatures in the Working Memory (ms) (0 means infinity).");
-	RTABMAP_PARAM(Rtabmap, DetectionRate,                float, 1.0, "Detection rate. RTAB-Map will filter input images to satisfy this rate.");
+	RTABMAP_PARAM(Rtabmap, DetectionRate,                float, 1,   "Detection rate. RTAB-Map will filter input images to satisfy this rate.");
 	RTABMAP_PARAM(Rtabmap, ImageBufferSize,              unsigned int, 1, "Data buffer size (0 min inf).");
-	RTABMAP_PARAM(Rtabmap, CreateIntermediateNodes,      bool, false, "Create intermediate nodes between loop closure detection. Only used when Rtabmap/DetectionRate>0.");
+	RTABMAP_PARAM(Rtabmap, CreateIntermediateNodes,      bool, false, uFormat("Create intermediate nodes between loop closure detection. Only used when %s>0.", kRtabmapDetectionRate().c_str()));
 	RTABMAP_PARAM_STR(Rtabmap, WorkingDirectory,         "", "Working directory.");
 	RTABMAP_PARAM(Rtabmap, MaxRetrieved,                 unsigned int, 2, "Maximum locations retrieved at the same time from LTM.");
 	RTABMAP_PARAM(Rtabmap, StatisticLogsBufferedInRAM,   bool, true, "Statistic logs buffered in RAM instead of written to hard drive after each iteration.");
@@ -186,7 +187,7 @@ class RTABMAP_EXP Parameters
 
 	// Hypotheses selection
 	RTABMAP_PARAM(Rtabmap, LoopThr,    	     float, 0.11, 	"Loop closing threshold.");
-	RTABMAP_PARAM(Rtabmap, LoopRatio,    	 float, 0.0, 	"The loop closure hypothesis must be over LoopRatio x lastHypothesisValue.");
+	RTABMAP_PARAM(Rtabmap, LoopRatio,    	 float, 0, 	    "The loop closure hypothesis must be over LoopRatio x lastHypothesisValue.");
 
 	// Memory
 	RTABMAP_PARAM(Mem, RehearsalSimilarity,     float, 0.6, 	"Rehearsal similarity.");
@@ -194,7 +195,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Mem, BinDataKept, 		    bool, true, 	"Keep binary data in db.");
 	RTABMAP_PARAM(Mem, RawDescriptorsKept, 		bool, true, 	"Raw descriptors kept in memory.");
 	RTABMAP_PARAM(Mem, MapLabelsAdded, 		    bool, true, 	"Create map labels. The first node of a map will be labelled as \"map#\" where # is the map ID.");
-	RTABMAP_PARAM(Mem, SaveDepth16Format, 		bool, true, 	"Save depth image into 16 bits format to reduce memory used. Warning: values over ~65 meters are ignored (maximum 65535 millimeters).");
+	RTABMAP_PARAM(Mem, SaveDepth16Format, 		bool, false, 	"Save depth image into 16 bits format to reduce memory used. Warning: values over ~65 meters are ignored (maximum 65535 millimeters).");
 	RTABMAP_PARAM(Mem, NotLinkedNodesKept, 	    bool, true, 	"Keep not linked nodes in db (rehearsed nodes and deleted nodes).");
 	RTABMAP_PARAM(Mem, STMSize, 		        unsigned int, 10, "Short-term memory size.");
 	RTABMAP_PARAM(Mem, IncrementalMemory,       bool, true, 	"SLAM mode, otherwise it is Localization mode.");
@@ -206,23 +207,25 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Mem, GenerateIds,             bool, true,     "True=Generate location IDs, False=use input image IDs.");
 	RTABMAP_PARAM(Mem, BadSignaturesIgnored,    bool, false,     "Bad signatures are ignored.");
 	RTABMAP_PARAM(Mem, InitWMWithAllNodes,      bool, false,     "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.");
-	RTABMAP_PARAM(Mem, ImageDecimation,         int, 1,          "Image decimation (>=1) when creating a signature.");
+	RTABMAP_PARAM(Mem, ImagePreDecimation,      int, 1,          "Image decimation (>=1) before features extraction. Negative decimation is done from RGB size instead of depth size (if depth is smaller than RGB, it may be interpolated depending of the decimation value).");
+	RTABMAP_PARAM(Mem, ImagePostDecimation,     int, 1,          "Image decimation (>=1) of saved data in created signatures (after features extraction). Decimation is done from the original image. Negative decimation is done from RGB size instead of depth size (if depth is smaller than RGB, it may be interpolated depending of the decimation value).");
 	RTABMAP_PARAM(Mem, LaserScanDownsampleStepSize, int, 1,      "If > 1, downsample the laser scans when creating a signature.");
+	RTABMAP_PARAM(Mem, LaserScanNormalK,        int, 0,          "If > 0 and laser scans are 3D without normals, normals will be computed with K search neighbors when creating a signature.");
 	RTABMAP_PARAM(Mem, UseOdomFeatures,         bool, false, "Use odometry features.");
 
 	// KeypointMemory (Keypoint-based)
 	RTABMAP_PARAM(Kp, NNStrategy,            int, 1,            "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
 	RTABMAP_PARAM(Kp, IncrementalDictionary, bool, true, 		"");
 	RTABMAP_PARAM(Kp, IncrementalFlann,      bool, true, 		"When using FLANN based strategy, add/remove points to its index without always rebuilding the index (the index is built only when the dictionary doubles in size).");
-	RTABMAP_PARAM(Kp, MaxDepth,              float, 0.0, 		"Filter extracted keypoints by depth (0=inf).");
+	RTABMAP_PARAM(Kp, MaxDepth,              float, 0,   		"Filter extracted keypoints by depth (0=inf).");
-	RTABMAP_PARAM(Kp, MinDepth,              float, 0.0, 		"Filter extracted keypoints by depth.");
+	RTABMAP_PARAM(Kp, MinDepth,              float, 0,  		"Filter extracted keypoints by depth.");
 	RTABMAP_PARAM(Kp, MaxFeatures,           int, 400, 			"Maximum features extracted from the images (0 means not bounded, <0 means no extraction).");
 	RTABMAP_PARAM(Kp, BadSignRatio,          float, 0.5, 		"Bad signature ratio (less than Ratio x AverageWordsPerImage = bad).");
 	RTABMAP_PARAM(Kp, NndrRatio, 	         float, 0.8,		"NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)");
 #ifdef RTABMAP_NONFREE
-	RTABMAP_PARAM(Kp, DetectorStrategy,      int, 0,            "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
+	RTABMAP_PARAM(Kp, DetectorStrategy,      int, 0,            "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=FREAK.");
 #else
-	RTABMAP_PARAM(Kp, DetectorStrategy,      int, 2,            "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
+	RTABMAP_PARAM(Kp, DetectorStrategy,      int, 2,            "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=FREAK.");
 #endif
 	RTABMAP_PARAM(Kp, TfIdfLikelihoodUsed,   bool, true, 		"Use of the td-idf strategy to compute the likelihood.");
 	RTABMAP_PARAM(Kp, Parallelized,          bool, true, 		"If the dictionary update and signature creation were parallelized.");
@@ -242,7 +245,7 @@ class RTABMAP_EXP Parameters
 
 	// Keypoints descriptors/detectors
 	RTABMAP_PARAM(SURF, Extended, 		  bool, false, 	    "Extended descriptor flag (true - use extended 128-element descriptors; false - use 64-element descriptors).");
-	RTABMAP_PARAM(SURF, HessianThreshold, float, 500.0,		"Threshold for hessian keypoint detector used in SURF.");
+	RTABMAP_PARAM(SURF, HessianThreshold, float, 500,		"Threshold for hessian keypoint detector used in SURF.");
 	RTABMAP_PARAM(SURF, Octaves, 		  int, 4, 			"Number of pyramid octaves the keypoint detector will use.");
 	RTABMAP_PARAM(SURF, OctaveLayers, 	  int, 2, 			"Number of octave layers within each octave.");
 	RTABMAP_PARAM(SURF, Upright, 	      bool, false, 	"Up-right or rotated features flag (true - do not compute orientation of features; false - compute orientation).");
@@ -252,7 +255,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(SIFT, NFeatures,          int, 0,			"The number of best features to retain. The features are ranked by their scores (measured in SIFT algorithm as the local contrast).");
 	RTABMAP_PARAM(SIFT, NOctaveLayers,      int, 3, 		"The number of layers in each octave. 3 is the value used in D. Lowe paper. The number of octaves is computed automatically from the image resolution.");
 	RTABMAP_PARAM(SIFT, ContrastThreshold, 	double, 0.04, 	"The contrast threshold used to filter out weak features in semi-uniform (low-contrast) regions. The larger the threshold, the less features are produced by the detector.");
-	RTABMAP_PARAM(SIFT, EdgeThreshold,      double, 10.0, 	"The threshold used to filter out edge-like features. Note that the its meaning is different from the contrastThreshold, i.e. the larger the edgeThreshold, the less features are filtered out (more features are retained).");
+	RTABMAP_PARAM(SIFT, EdgeThreshold,      double, 10, 	"The threshold used to filter out edge-like features. Note that the its meaning is different from the contrastThreshold, i.e. the larger the edgeThreshold, the less features are filtered out (more features are retained).");
 	RTABMAP_PARAM(SIFT, Sigma,              double, 1.6, 	"The sigma of the Gaussian applied to the input image at the octave #0. If your image is captured with a weak camera with soft lenses, you might want to reduce the number.");
 
 	RTABMAP_PARAM(BRIEF, Bytes,         	   int, 32, 	"Bytes is a length of descriptor in bytes. It can be equal 16, 32 or 64 bytes.");
@@ -266,7 +269,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(FAST, GridRows,           int, 4,         "Grid rows (0 to disable). Adapts the detector to partition the source image into a grid and detect points in each cell.");
 	RTABMAP_PARAM(FAST, GridCols,           int, 4,         "Grid cols (0 to disable). Adapts the detector to partition the source image into a grid and detect points in each cell.");
 
-	RTABMAP_PARAM(GFTT, QualityLevel, double, 0.001, "");
+	RTABMAP_PARAM(GFTT, QualityLevel, double, 0.01, "");
 	RTABMAP_PARAM(GFTT, MinDistance, double, 5, "");
 	RTABMAP_PARAM(GFTT, BlockSize, int, 3, "");
 	RTABMAP_PARAM(GFTT, UseHarrisDetector, bool, false, "");
@@ -283,12 +286,12 @@ class RTABMAP_EXP Parameters
 
 	RTABMAP_PARAM(FREAK, OrientationNormalized, bool, true,   "Enable orientation normalization.");
 	RTABMAP_PARAM(FREAK, ScaleNormalized,       bool, true,   "Enable scale normalization.");
-	RTABMAP_PARAM(FREAK, PatternScale,          float, 22.0,  "Scaling of the description pattern.");
+	RTABMAP_PARAM(FREAK, PatternScale,          float, 22,    "Scaling of the description pattern.");
 	RTABMAP_PARAM(FREAK, NOctaves,              int, 4,        "Number of octaves covered by the detected keypoints.");
 
 	RTABMAP_PARAM(BRISK, Thresh,                int, 30,      "FAST/AGAST detection threshold score.");
 	RTABMAP_PARAM(BRISK, Octaves,               int, 3,       "Detection octaves. Use 0 to do single scale.");
-	RTABMAP_PARAM(BRISK, PatternScale,          float, 1.0,  "Apply this scale to the pattern used for sampling the neighbourhood of a keypoint.");
+	RTABMAP_PARAM(BRISK, PatternScale,          float, 1,    "Apply this scale to the pattern used for sampling the neighbourhood of a keypoint.");
 
 	// BayesFilter
 	RTABMAP_PARAM(Bayes, VirtualPlacePriorThr,           float, 0.9, "Virtual place prior");
@@ -297,7 +300,7 @@ class RTABMAP_EXP Parameters
 
 	// Verify hypotheses
 	RTABMAP_PARAM(VhEp, MatchCountMin, int, 8, 		"Minimum of matching visual words pairs to accept the loop hypothesis.");
-	RTABMAP_PARAM(VhEp, RansacParam1,  float, 3.0, 	"Fundamental matrix (see cvFindFundamentalMat()): Max distance (in pixels) from the epipolar line for a point to be inlier.");
+	RTABMAP_PARAM(VhEp, RansacParam1,  float, 3, 	"Fundamental matrix (see cvFindFundamentalMat()): Max distance (in pixels) from the epipolar line for a point to be inlier.");
 	RTABMAP_PARAM(VhEp, RansacParam2,  float, 0.99, "Fundamental matrix (see cvFindFundamentalMat()): Performance of the RANSAC.");
 
 	// RGB-D SLAM
@@ -306,46 +309,56 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(RGBD, AngularUpdate,            float, 0.1,  "Minimum angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
 	RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0,    "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
 	RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd,     bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
-	RTABMAP_PARAM(RGBD, OptimizeMaxError,         float, 1.0,  "Reject loop closures if optimization error is greater than this value (0=disabled). This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"Optimizer/Robust\" if enabled.");
+	RTABMAP_PARAM(RGBD, OptimizeMaxError,         float, 1,    uFormat("Reject loop closures if optimization error is greater than this value (0=disabled). This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"%s\" if enabled.", kOptimizerRobust().c_str()));
 	RTABMAP_PARAM(RGBD, GoalReachedRadius,        float, 0.5,  "Goal reached radius (m).");
 	RTABMAP_PARAM(RGBD, PlanStuckIterations,      int, 0,      "Mark the current goal node on the path as unreachable if it is not updated after X iterations (0=disabled). If all upcoming nodes on the path are unreachabled, the plan fails.");
-	RTABMAP_PARAM(RGBD, PlanLinearVelocity,       float, 0.0,  "Linear velocity (m/sec) used to compute path weights.");
+	RTABMAP_PARAM(RGBD, PlanLinearVelocity,       float, 0,    "Linear velocity (m/sec) used to compute path weights.");
-	RTABMAP_PARAM(RGBD, PlanAngularVelocity,      float, 0.0,  "Angular velocity (rad/sec) used to compute path weights.");
+	RTABMAP_PARAM(RGBD, PlanAngularVelocity,      float, 0,    "Angular velocity (rad/sec) used to compute path weights.");
 	RTABMAP_PARAM(RGBD, GoalsSavedInUserData,     bool, false, "When a goal is received and processed with success, it is saved in user data of the location with this format: \"GOAL:#\".");
 	RTABMAP_PARAM(RGBD, MaxLocalRetrieved,        unsigned int, 2, "Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority).");
 	RTABMAP_PARAM(RGBD, LocalRadius,              float, 10,   "Local radius (m) for nodes selection in the local map. This parameter is used in some approaches about the local map management.");
 	RTABMAP_PARAM(RGBD, LocalImmunizationRatio,   float, 0.25, "Ratio of working memory for which local nodes are immunized from transfer.");
 	RTABMAP_PARAM(RGBD, ScanMatchingIdsSavedInLinks, bool, true,      "Save scan matching IDs in link's user data.");
-	RTABMAP_PARAM(RGBD, NeighborLinkRefining,          bool, false,  "When a new node is added to the graph, the transformation of its neighbor link to the previous node is refined using ICP (laser scans required!).");
+	RTABMAP_PARAM(RGBD, NeighborLinkRefining,          bool, false,  uFormat("When a new node is added to the graph, the transformation of its neighbor link to the previous node is refined using registration approach selected (%s).", kRegStrategy().c_str()));
 	RTABMAP_PARAM(RGBD, LoopClosureReextractFeatures,  bool, false,  "Extract features even if there are some already in the nodes.");
+	RTABMAP_PARAM(RGBD, CreateOccupancyGrid,          bool, false,  "Create local occupancy grid maps. See \"Grid\" group for parameters.");
 
 	// Local/Proximity loop closure detection
 	RTABMAP_PARAM(RGBD, ProximityByTime,              bool, false, 	"Detection over all locations in STM.");
 	RTABMAP_PARAM(RGBD, ProximityBySpace,             bool, true, 	"Detection over locations (in Working Memory or STM) near in space.");
-	RTABMAP_PARAM(RGBD, ProximityPathScansMerged,     bool, true, 	"Merge close laser scans on each path. If false, only the nearest laser scan on the path is used for ICP.");
 	RTABMAP_PARAM(RGBD, ProximityMaxGraphDepth,       int, 50,      "Maximum depth from the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.");
-	RTABMAP_PARAM(RGBD, ProximityPathFilteringRadius, float, 0.5,   "Path filtering radius.");
+	RTABMAP_PARAM(RGBD, ProximityMaxPaths,            int, 3,       "Maximum paths compared (from the most recent) for proximity detection by space. 0 means no limit.");
+	RTABMAP_PARAM(RGBD, ProximityPathFilteringRadius, float, 0.5,   "Path filtering radius to reduce the number of nodes to compare in a path. A path should also be inside that radius to be considered for proximity detection.");
+	RTABMAP_PARAM(RGBD, ProximityPathMaxNeighbors,    int, 10,      "Maximum neighbor nodes compared on each path.");
 	RTABMAP_PARAM(RGBD, ProximityPathRawPosesUsed,    bool, true,   "When comparing to a local path, merge the scan using the odometry poses (with neighbor link optimizations) instead of the ones in the optimized local graph.");
-	RTABMAP_PARAM(RGBD, ProximityAngle,               float, 45.0,  "Maximum angle (degrees) for visual proximity detection.");
+	RTABMAP_PARAM(RGBD, ProximityAngle,               float, 45,    "Maximum angle (degrees) for visual proximity detection.");
 
 	// Graph optimization
 #ifdef RTABMAP_GTSAM
 	RTABMAP_PARAM(Optimizer, Strategy,          int, 2,          "Graph optimization strategy: 0=TORO, 1=g2o and 2=GTSAM.");
+	RTABMAP_PARAM(Optimizer, Iterations,        int, 20,          "Optimization iterations.");
+	RTABMAP_PARAM(Optimizer, Epsilon,           double, 0.00001,  "Stop optimizing when the error improvement is less than this value.");
 #else
 #ifdef RTABMAP_G2O
 	RTABMAP_PARAM(Optimizer, Strategy,          int, 1,          "Graph optimization strategy: 0=TORO, 1=g2o and 2=GTSAM.");
+	RTABMAP_PARAM(Optimizer, Iterations,        int, 20,          "Optimization iterations.");
+	RTABMAP_PARAM(Optimizer, Epsilon,           double, 0.0,     "Stop optimizing when the error improvement is less than this value.");
 #else
 	RTABMAP_PARAM(Optimizer, Strategy,          int, 0,          "Graph optimization strategy: 0=TORO, 1=g2o and 2=GTSAM.");
-#endif
-#endif
 	RTABMAP_PARAM(Optimizer, Iterations,        int, 100,        "Optimization iterations.");
-	RTABMAP_PARAM(Optimizer, Slam2D,            bool, false,     "If optimization is done only on x,y and theta (3DoF). Otherwise, it is done on full 6DoF poses.");
+	RTABMAP_PARAM(Optimizer, Epsilon,           double, 0.00001, "Stop optimizing when the error improvement is less than this value.");
+#endif
+#endif
 	RTABMAP_PARAM(Optimizer, VarianceIgnored,   bool, false,     "Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links.");
-	RTABMAP_PARAM(Optimizer, Epsilon,           double, 0.0001,  "Stop optimizing when the error improvement is less than this value.");
+	RTABMAP_PARAM(Optimizer, Robust,            bool, false,     uFormat("Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies). Not compatible with \"%s\" if enabled.", kRGBDOptimizeMaxError().c_str()));
-	RTABMAP_PARAM(Optimizer, Robust,            bool, false,      "Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies). Not compatible with \"RGBD/OptimizeMaxError\" if enabled.");
 
-	RTABMAP_PARAM(g2o, Solver,                  int, 0,          "0=csparse 1=pcg 2=cholmod");
+	RTABMAP_PARAM(g2o, Solver,                  int, 0,          "0=csparse 1=pcg 2=cholmod 3=Eigen");
 	RTABMAP_PARAM(g2o, Optimizer,               int, 0,          "0=Levenberg 1=GaussNewton");
+	RTABMAP_PARAM(g2o, PixelVariance,           double, 1.0,     "Pixel variance used for bundle adjustment.");
+	RTABMAP_PARAM(g2o, RobustKernelDelta,       double, 8,       "Robust kernel delta used for bundle adjustment (0 means don't use robust kernel). Observations with chi2 over this threshold will be ignored in the second optimization pass.");
+	RTABMAP_PARAM(g2o, Baseline,                double, 0.075,   "When doing bundle adjustment with RGB-D data, we can set a fake baseline (m) to do stereo bundle adjustment (if 0, mono bundle adjustment is done). For stereo data, the baseline in the calibration is used directly.");
+
+	RTABMAP_PARAM(GTSAM, Optimizer,             int, 1,          "0=Levenberg 1=GaussNewton 2=Dogleg");
 
 	// Odometry
 	RTABMAP_PARAM(Odom, Strategy,           	int, 0, 		"0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F)");
@@ -363,14 +376,18 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Odom, KalmanMeasurementNoise, float, 0.01,      "Process measurement covariance value.");
 	RTABMAP_PARAM(Odom, GuessMotion,            bool, false,      "Guess next transformation from the last motion computed.");
 	RTABMAP_PARAM(Odom, KeyFrameThr,            float, 0.3,       "[Visual] Create a new keyframe when the number of inliers drops under this ratio of features in last frame. Setting the value to 0 means that a keyframe is created for each processed frame.");
+	RTABMAP_PARAM(Odom, VisKeyFrameThr,         int, 100,         "[Visual] Create a new keyframe when the number of inliers drops under this threshold. Setting the value to 0 means that a keyframe is created for each processed frame.");
 	RTABMAP_PARAM(Odom, ScanKeyFrameThr,        float, 0.7,       "[Geometry] Create a new keyframe when the number of ICP inliers drops under this ratio of points in last frame's scan. Setting the value to 0 means that a keyframe is created for each processed frame.");
+	RTABMAP_PARAM(Odom, ImageDecimation,        int, 1,           "Decimation of the images before registration. Negative decimation is done from RGB size instead of depth size (if depth is smaller than RGB, it may be interpolated depending of the decimation value).");
+	RTABMAP_PARAM(Odom, AlignWithGround,        bool, false,      "Align odometry with the ground on initialization.");
 
 	// Odometry Bag-of-words
 	RTABMAP_PARAM(OdomF2M, MaxSize,             int, 2000,    "[Visual] Local map size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
 	RTABMAP_PARAM(OdomF2M, MaxNewFeatures,      int, 0,       "[Visual] Maximum features (sorted by keypoint response) added to local map from a new key-frame. 0 means no limit.");
 	RTABMAP_PARAM(OdomF2M, ScanMaxSize,         int, 2000,    "[Geometry] Maximum local scan map size.");
-	RTABMAP_PARAM(OdomF2M, ScanSubstractRadius, float, 0.05,  "[Geometry] Radius used to filter points of a new added scan to local map. This could match the voxel size of the scans.");
+	RTABMAP_PARAM(OdomF2M, ScanSubtractRadius,  float, 0.05,  "[Geometry] Radius used to filter points of a new added scan to local map. This could match the voxel size of the scans.");
-	RTABMAP_PARAM_STR(OdomF2M, FixedMapPath,    "",           "Path to a fixed map (RTAB-Map's database) to be used for odometry. Odometry will be constraint to this map. RGB-only images can be used if odometry PnP estimation is used.")
+	RTABMAP_PARAM(OdomF2M, BundleAdjustment,          int, 0, "Local bundle adjustment: 0=disabled, 1=g2o, 2=cvsba.");
+	RTABMAP_PARAM(OdomF2M, BundleAdjustmentMaxFrames, int, 0, "Maximum frames used for bundle adjustment (0=inf or all current frames in the local map).");
 
 	// Odometry Mono
 	RTABMAP_PARAM(OdomMono, InitMinFlow,            float, 100,  "Minimum optical flow required for the initialization step.");
@@ -386,39 +403,50 @@ class RTABMAP_EXP Parameters
 	// Visual registration parameters
 	RTABMAP_PARAM(Vis, EstimationType,           int, 0,    	"Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP), 2:2D->2D (Epipolar Geometry)");
 	RTABMAP_PARAM(Vis, ForwardEstOnly,           bool, true, 	"Forward estimation only (A->B). If false, a transformation is also computed in backward direction (B->A), then the two resulting transforms are merged (middle interpolation between the transforms).");
-	RTABMAP_PARAM(Vis, InlierDistance,           float, 0.1,    "[Vis/EstimationType = 0] Maximum distance for feature correspondences. Used by 3D->3D estimation approach.");
+	RTABMAP_PARAM(Vis, InlierDistance,           float, 0.1,    uFormat("[%s = 0] Maximum distance for feature correspondences. Used by 3D->3D estimation approach.", kVisEstimationType().c_str()));
-	RTABMAP_PARAM(Vis, RefineIterations,         int, 5,        "[Vis/EstimationType = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.");
+	RTABMAP_PARAM(Vis, RefineIterations,         int, 5,        uFormat("[%s = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.", kVisEstimationType().c_str()));
-	RTABMAP_PARAM(Vis, PnPReprojError, 	         float, 2.0,    "[Vis/EstimationType = 1] PnP reprojection error.");
+	RTABMAP_PARAM(Vis, PnPReprojError, 	         float, 2,      uFormat("[%s = 1] PnP reprojection error.", kVisEstimationType().c_str()));
-	RTABMAP_PARAM(Vis, PnPFlags,                 int, 1,        "[Vis/EstimationType = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P");
+	RTABMAP_PARAM(Vis, PnPFlags,                 int, 0,        uFormat("[%s = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P", kVisEstimationType().c_str()));
-	RTABMAP_PARAM(Vis, PnPRefineIterations,      int, 1,        "[Vis/EstimationType = 1] Refine iterations.");
+	RTABMAP_PARAM(Vis, PnPRefineIterations,      int, 1,        uFormat("[%s = 1] Refine iterations.", kVisEstimationType().c_str()));
-	RTABMAP_PARAM(Vis, EpipolarGeometryVar,      float, 0.02,   "[Vis/EstimationType = 2] Epipolar geometry maximum variance to accept the transformation.");
+	RTABMAP_PARAM(Vis, EpipolarGeometryVar,      float, 0.02,   uFormat("[%s = 2] Epipolar geometry maximum variance to accept the transformation.", kVisEstimationType().c_str()));
 	RTABMAP_PARAM(Vis, MinInliers,               int, 20, 		"Minimum feature correspondences to compute/accept the transformation.");
 	RTABMAP_PARAM(Vis, Iterations,               int, 100, 		"Maximum iterations to compute the transform.");
-	RTABMAP_PARAM(Vis, FeatureType,              int, 6,        "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
+#ifndef RTABMAP_NONFREE
+#ifdef RTABMAP_OPENCV3
+	// OpenCV 3 without xFeatures2D module doesn't have BRIEF
+	RTABMAP_PARAM(Vis, FeatureType, int, 8, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=FREAK.");
+#else
+	RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=FREAK.");
+#endif
+#else
+	RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=FREAK.");
+#endif
+	
 	RTABMAP_PARAM(Vis, MaxFeatures,              int, 1000,     "0 no limits.");
-	RTABMAP_PARAM(Vis, MaxDepth, 	             float, 0.0,    "Max depth of the features (0 means no limit).");
+	RTABMAP_PARAM(Vis, MaxDepth, 	             float, 0,      "Max depth of the features (0 means no limit).");
-	RTABMAP_PARAM(Vis, MinDepth, 	             float, 0.0,    "Min depth of the features (0 means no limit).");
+	RTABMAP_PARAM(Vis, MinDepth, 	             float, 0,      "Min depth of the features (0 means no limit).");
 	RTABMAP_PARAM_STR(Vis, RoiRatios,        "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
     RTABMAP_PARAM(Vis, SubPixWinSize,            int, 3,        "See cv::cornerSubPix().");
 	RTABMAP_PARAM(Vis, SubPixIterations,         int, 0,        "See cv::cornerSubPix(). 0 disables sub pixel refining.");
 	RTABMAP_PARAM(Vis, SubPixEps,                float, 0.02,   "See cv::cornerSubPix().");
 	RTABMAP_PARAM(Vis, CorType,                  int, 0,        "Correspondences computation approach: 0=Features Matching, 1=Optical Flow");
-	RTABMAP_PARAM(Vis, CorNNType, 	             int, 1,        "[Vis/CorrespondenceType=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4. Used for features matching approach.");
+	RTABMAP_PARAM(Vis, CorNNType, 	             int, 1,        uFormat("[%s=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4. Used for features matching approach.", kVisCorType().c_str()));
-	RTABMAP_PARAM(Vis, CorNNDR,                  float, 0.8,    "[Vis/CorrespondenceType=0] NNDR: nearest neighbor distance ratio. Used for features matching approach.");
+	RTABMAP_PARAM(Vis, CorNNDR,                  float, 0.6,    uFormat("[%s=0] NNDR: nearest neighbor distance ratio. Used for features matching approach.", kVisCorType().c_str()));
-	RTABMAP_PARAM(Vis, CorGuessWinSize,          int, 50,       "[Vis/CorrespondenceType=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences. 0 means disabled.");
+	RTABMAP_PARAM(Vis, CorGuessWinSize,          int, 20,       uFormat("[%s=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences. 0 means disabled.", kVisCorType().c_str()));
-	RTABMAP_PARAM(Vis, CorFlowWinSize,           int, 16,       "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.");
+	RTABMAP_PARAM(Vis, CorFlowWinSize,           int, 16,       uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
-	RTABMAP_PARAM(Vis, CorFlowIterations,        int, 30,       "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.");
+	RTABMAP_PARAM(Vis, CorFlowIterations,        int, 30,       uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
-	RTABMAP_PARAM(Vis, CorFlowEps,               float, 0.01,   "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.");
+	RTABMAP_PARAM(Vis, CorFlowEps,               float, 0.01,   uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
-	RTABMAP_PARAM(Vis, CorFlowMaxLevel,          int, 3,        "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.");
+	RTABMAP_PARAM(Vis, CorFlowMaxLevel,          int, 3,        uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
+	RTABMAP_PARAM(Vis, BundleAdjustment,         int, 0,        "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba.");
 
 	// ICP registration parameters
 	RTABMAP_PARAM(Icp, MaxTranslation,            float, 0.2,   "Maximum ICP translation correction accepted (m).");
 	RTABMAP_PARAM(Icp, MaxRotation,               float, 0.78,  "Maximum ICP rotation correction accepted (rad).");
-	RTABMAP_PARAM(Icp, VoxelSize,                 float, 0.025, "Uniform sampling voxel size (0=disabled).");
+	RTABMAP_PARAM(Icp, VoxelSize,                 float, 0.0,   "Uniform sampling voxel size (0=disabled).");
 	RTABMAP_PARAM(Icp, DownsamplingStep,          int, 1, 	    "Downsampling step size (1=no sampling). This is done before uniform sampling.");
 	RTABMAP_PARAM(Icp, MaxCorrespondenceDistance, float, 0.05,  "Max distance for point correspondences.");
 	RTABMAP_PARAM(Icp, Iterations,                int, 30, 		"Max iterations.");
-	RTABMAP_PARAM(Icp, Epsilon,                   float, 0.0,   "Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution.");
+	RTABMAP_PARAM(Icp, Epsilon,                   float, 0,     "Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution.");
 	RTABMAP_PARAM(Icp, CorrespondenceRatio,       float, 0.2,   "Ratio of matching correspondences to accept the transform.");
 	RTABMAP_PARAM(Icp, PointToPlane,              bool, false, 	"Use point to plane ICP.");
 	RTABMAP_PARAM(Icp, PointToPlaneNormalNeighbors, int, 20,    "Number of neighbors to compute normals for point to plane.");
@@ -429,14 +457,14 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Stereo, Iterations,            int, 30,       "Maximum iterations.");
 	RTABMAP_PARAM(Stereo, MaxLevel,              int, 3,        "Maximum pyramid level.");
 	RTABMAP_PARAM(Stereo, MinDisparity,          int, 1,        "Minimum disparity.");
-	RTABMAP_PARAM(Stereo, MaxDisparity,          int, 64,       "Maximum disparity.");
+	RTABMAP_PARAM(Stereo, MaxDisparity,          int, 128,      "Maximum disparity.");
 	RTABMAP_PARAM(Stereo, OpticalFlow,           bool, true,    "Use optical flow to find stereo correspondences, otherwise a simple block matching approach is used.");
-	RTABMAP_PARAM(Stereo, SSD,                   bool, true,    "[Stereo/OpticalFlow = false] Use Sum of Squared Differences (SSD) window, otherwise Sum of Absolute Differences (SAD) window is used.");
+	RTABMAP_PARAM(Stereo, SSD,                   bool, true,    uFormat("[%s=false] Use Sum of Squared Differences (SSD) window, otherwise Sum of Absolute Differences (SAD) window is used.", kStereoOpticalFlow().c_str()));
-	RTABMAP_PARAM(Stereo, Eps,                   double, 0.01,  "[Stereo/OpticalFlow = true] Epsilon stop criterion.");
+	RTABMAP_PARAM(Stereo, Eps,                   double, 0.01,  uFormat("[%s=true] Epsilon stop criterion.", kStereoOpticalFlow().c_str()));
 
 	RTABMAP_PARAM(StereoBM, BlockSize,           int, 15,       "See cv::StereoBM");
 	RTABMAP_PARAM(StereoBM, MinDisparity,        int, 0,        "See cv::StereoBM");
-	RTABMAP_PARAM(StereoBM, NumDisparities,      int, 64,       "See cv::StereoBM");
+	RTABMAP_PARAM(StereoBM, NumDisparities,      int, 128,      "See cv::StereoBM");
 	RTABMAP_PARAM(StereoBM, PreFilterSize,       int, 9,        "See cv::StereoBM");
 	RTABMAP_PARAM(StereoBM, PreFilterCap,        int, 31,       "See cv::StereoBM");
 	RTABMAP_PARAM(StereoBM, UniquenessRatio,     int, 15,       "See cv::StereoBM");
@@ -444,6 +472,39 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(StereoBM, SpeckleWindowSize,   int, 100,      "See cv::StereoBM");
 	RTABMAP_PARAM(StereoBM, SpeckleRange,        int, 4,        "See cv::StereoBM");
 
+	// Occupancy Grid
+	RTABMAP_PARAM(Grid, FromDepth,               bool,   true,    "Create occupancy grid from depth image(s), otherwise it is created from laser scan.");
+	RTABMAP_PARAM(Grid, DepthDecimation,         int,    4,       uFormat("[%s=true] Decimation of the depth image before creating cloud. Negative decimation is done from RGB size instead of depth size (if depth is smaller than RGB, it may be interpolated depending of the decimation value).", kGridDepthDecimation().c_str()));
+	RTABMAP_PARAM(Grid, DepthMin,                float,  0.0,     uFormat("[%s=true] Minimum cloud's depth from sensor.", kGridFromDepth().c_str()));
+	RTABMAP_PARAM(Grid, DepthMax,                float,  4.0,     uFormat("[%s=true] Maximum cloud's depth from sensor. 0=inf.", kGridFromDepth().c_str()));
+	RTABMAP_PARAM_STR(Grid, DepthRoiRatios,      "0.0 0.0 0.0 0.0", uFormat("[%s=true] Region of interest ratios [left, right, top, bottom].", kGridFromDepth().c_str()));
+	RTABMAP_PARAM(Grid, FootprintLength,         float,  0.0,     "Footprint length used to filter points over the footprint of the robot.");
+	RTABMAP_PARAM(Grid, FootprintWidth,          float,  0.0,     "Footprint width used to filter points over the footprint of the robot. Footprint length should be set.");
+	RTABMAP_PARAM(Grid, FootprintHeight,         float,  0.0,     "Footprint height used to filter points over the footprint of the robot. Footprint length and width should be set.");
+	RTABMAP_PARAM(Grid, ScanDecimation,          int,    1,       uFormat("[%s=false] Decimation of the laser scan before creating cloud.", kGridFromDepth().c_str()));
+	RTABMAP_PARAM(Grid, CellSize,                float,  0.05,    "Resolution of the occupancy grid.");
+	RTABMAP_PARAM(Grid, MapFrameProjection,      bool,   false,   "Projection in map frame. On a 3D terrain and a fixed local camera transform (the cloud is created relative to ground), you may want to disable this to do the projection in robot frame instead.");
+	RTABMAP_PARAM(Grid, NormalsSegmentation,     bool,   true,    "Segment ground from obstacles using point normals, otherwise a fast passthrough is used.");
+	RTABMAP_PARAM(Grid, MaxObstacleHeight,       float,  0.0,     "Maximum obstacles height (0=disabled).");
+	RTABMAP_PARAM(Grid, MinGroundHeight,         float,  0.0,     "Minimum ground height (0=disabled).");
+	RTABMAP_PARAM(Grid, MaxGroundHeight,         float,  0.0,     uFormat("Maximum ground height (0=disabled). Should be set if \"%s\" is true.", kGridNormalsSegmentation().c_str()));
+	RTABMAP_PARAM(Grid, MaxGroundAngle,          float,  45,      uFormat("[%s=true] Maximum angle (degrees) between point's normal to ground's normal to label it as ground. Points with higher angle difference are considered as obstacles.", kGridNormalsSegmentation().c_str()));
+	RTABMAP_PARAM(Grid, NormalK,                 int,    10,      uFormat("[%s=true] K neighbors to compute normals.", kGridNormalsSegmentation().c_str()));
+	RTABMAP_PARAM(Grid, ClusterRadius,           float,  0.1,     uFormat("[%s=true] Cluster maximum radius.", kGridNormalsSegmentation().c_str()));
+	RTABMAP_PARAM(Grid, MinClusterSize,          int,    10,      uFormat("[%s=true] Minimum cluster size to project the points.", kGridNormalsSegmentation().c_str()));
+	RTABMAP_PARAM(Grid, FlatObstacleDetected,    bool,   true,    uFormat("[%s=true] Flat obstacles detected.", kGridNormalsSegmentation().c_str()));
+#ifdef RTABMAP_OCTOMAP
+	RTABMAP_PARAM(Grid, 3D,                      bool,   true,    uFormat("A 3D occupancy grid is required if you want an OctoMap (3D ray tracing). Set to false if you want only a 2D map, the cloud will be projected on xy plane. A 2D map can be still generated if checked, but it requires more memory and time to generate it. Ignored if laser scan is 2D and \"%s\" is false.", kGridFromDepth().c_str()));
+#else
+	RTABMAP_PARAM(Grid, 3D,                      bool,   false,   uFormat("A 3D occupancy grid is required if you want an OctoMap (3D ray tracing). Set to false if you want only a 2D map, the cloud will be projected on xy plane. A 2D map can be still generated if checked, but it requires more memory and time to generate it. Ignored if laser scan is 2D and \"%s\" is false.", kGridFromDepth().c_str()));
+#endif
+	RTABMAP_PARAM(Grid, GroundIsObstacle,        bool,   false,   uFormat("[%s=true] Ground segmentation (%s) is ignored, all points are obstacles. Use this only if you want an OctoMap with ground identified as an obstacle (e.g., with an UAV).", kGrid3D().c_str(), kGridNormalsSegmentation().c_str()));
+	RTABMAP_PARAM(Grid, NoiseFilteringRadius,         float,   0.0,     "Noise filtering radius (0=disabled). Done after segmentation.");
+	RTABMAP_PARAM(Grid, NoiseFilteringMinNeighbors,   int,     5,       "Noise filtering minimum neighbors.");
+	RTABMAP_PARAM(Grid, Scan2dUnknownSpaceFilled,     bool,    false,   "Unknown space filled. Only used with 2D laser scans.");
+	RTABMAP_PARAM(Grid, Scan2dMaxFilledRange,         float,   4.0,     "Unknown space filled maximum range. If 0, the laser scan maximum range is used.");
+	RTABMAP_PARAM(Grid, ProjRayTracing,               bool,   false,    uFormat("[%s=false] 2D ray tracing is done for each projected obstacle, filling unknown space between the sensor and obstacles.", kGrid3D().c_str()));
+
 public:
 	virtual ~Parameters();
 
@@ -468,22 +529,25 @@ public:
 	 */
 	static std::string getDescription(const std::string & paramKey);
 
-	static void parse(const ParametersMap & parameters, const std::string & key, bool & value);
+	static bool parse(const ParametersMap & parameters, const std::string & key, bool & value);
-	static void parse(const ParametersMap & parameters, const std::string & key, int & value);
+	static bool parse(const ParametersMap & parameters, const std::string & key, int & value);
-	static void parse(const ParametersMap & parameters, const std::string & key, unsigned int & value);
+	static bool parse(const ParametersMap & parameters, const std::string & key, unsigned int & value);
-	static void parse(const ParametersMap & parameters, const std::string & key, float & value);
+	static bool parse(const ParametersMap & parameters, const std::string & key, float & value);
-	static void parse(const ParametersMap & parameters, const std::string & key, double & value);
+	static bool parse(const ParametersMap & parameters, const std::string & key, double & value);
-	static void parse(const ParametersMap & parameters, const std::string & key, std::string & value);
+	static bool parse(const ParametersMap & parameters, const std::string & key, std::string & value);
 	static void parse(const ParametersMap & parameters, ParametersMap & parametersOut);
 
 	static const char * showUsage();
-	static ParametersMap parseArguments(int argc, char * argv[]);
+	static ParametersMap parseArguments(int argc, char * argv[], bool onlyParameters = false);
 
 	static std::string getVersion();
 	static std::string getDefaultDatabaseName();
 	
+	static std::string serialize(const ParametersMap & parameters);
+	static ParametersMap deserialize(const std::string & parameters);
+
 	static bool isFeatureParameter(const std::string & param);
-	static ParametersMap getDefaultOdometryParameters(bool stereo = false);
+	static ParametersMap getDefaultOdometryParameters(bool stereo = false, bool vis = true, bool icp = false);
 	static ParametersMap getDefaultParameters(const std::string & group);
 	static ParametersMap filterParameters(const ParametersMap & parameters, const std::string & group);
 

corelib/include/rtabmap/core/ParticleFilter.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2015, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/ProgressState.h

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

corelib/include/rtabmap/core/Registration.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/RegistrationIcp.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/RegistrationInfo.h

@@ -1,9 +1,29 @@
 /*
- * RegistrationInfo.h
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
- *
+All rights reserved.
- *  Created on: Jan 5, 2016
+
- *      Author: mathieu
+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 REGISTRATIONINFO_H_
 #define REGISTRATIONINFO_H_
@@ -15,14 +35,18 @@ class RegistrationInfo
 {
 public:
 	RegistrationInfo() :
-		variance(0),
+		varianceLin(0),
+		varianceAng(0),
 		inliers(0),
 		matches(0),
-		icpInliersRatio(0)
+		icpInliersRatio(0),
+		icpTranslation(0.0f),
+		icpRotation(0.0f)
 	{
 	}
 
-	float variance;
+	float varianceLin;
+	float varianceAng;
 	std::string rejectedMsg;
 
 	// RegistrationVis
@@ -33,6 +57,8 @@ public:
 
 	// RegistrationIcp
 	float icpInliersRatio;
+	float icpTranslation;
+	float icpRotation;
 };
 
 }