Update package.xml

Bump package version

.appveyor.yml

@@ -0,0 +1,107 @@
+
+branches:
+  only:
+    - master
+    - devel
+
+os: Visual Studio 2015
+
+clone_folder: c:\projects\rtabmap
+
+platform: x64
+configuration: Release
+
+init:
+  - cmake --version
+  - call "C:\Program Files\Microsoft SDKs\Windows\v7.1\Bin\SetEnv.cmd" /x64
+  - call "C:\Program Files (x86)\Microsoft Visual Studio 14.0\VC\vcvarsall.bat" x86_amd64
+
+install:
+  # Qt
+  - set QTDIR=C:\Qt\5.10.1\msvc2015_64
+  # make sure Qt bin path is before cmake bin path to avoid copying qt5 dlls from cmake before qt installation
+  - set PATH=%QTDIR%\bin;%PATH%
+  # Boost
+  - set PATH=%PATH%;C:\Libraries\boost_1_62_0\lib64-msvc-14.0
+  # Openni2
+  - ps: wget 'https://dl.dropboxusercontent.com/s/d98jv79l6oy9fxz/OpenNI2.exe?dl=0' -outfile OpenNI2.exe
+  - cmd: OpenNI2.exe -o"C:\Program Files" -y
+  - ECHO "Installed OpenNI2:"
+  - ps: "ls \"C:/Program Files/OpenNI2\""
+  - set PATH=%PATH%;C:\Program Files\OpenNI2\Redist
+  - set OPENNI2_INCLUDE64=C:\Program Files\OpenNI2\Include
+  - set OPENNI2_LIB64=C:\Program Files\OpenNI2\Lib
+  - set OPENNI2_REDIST64=C:\Program Files\OpenNI2\Redist
+  # OpenCV
+  - ps: wget 'http://kent.dl.sourceforge.net/project/opencvlibrary/opencv-win/3.3.1/opencv-3.3.1-vc14.exe' -outfile opencv-3.3.1-vc14.exe
+  - cmd: opencv-3.3.1-vc14.exe -o"C:\Program Files" -y
+  - ECHO "Installed OpenCV:"
+  - ps: "ls \"C:/Program Files/opencv/build\""
+  - set PATH=%PATH%;C:\Program Files\opencv\build\x64\vc14\bin
+  # VTK (including QVTK)
+  - ps: wget 'https://dl.dropboxusercontent.com/s/1l33b5l3f3y52gf/VTK-6_3-msvc140.exe?dl=0' -outfile VTK-6_3.exe
+  - cmd: VTK-6_3.exe -o"C:\Program Files" -y
+  - ECHO "Installed PCL:"
+  - ps: "ls \"C:/Program Files/VTK\""
+  - set PATH=%PATH%;C:\Program Files\VTK\bin
+  # QHull 
+  - ps: wget 'https://dl.dropboxusercontent.com/s/9widnk9msdsh2b8/Qhull-msvc140.exe?dl=0' -outfile Qhull.exe
+  - cmd: Qhull.exe -o"C:\Program Files" -y
+  - ECHO "Installed PCL:"
+  - ps: "ls \"C:/Program Files/Qhull\""
+  - set PATH=%PATH%;C:\Program Files\Qhull\bin
+  # FLANN 
+  - ps: wget 'https://dl.dropboxusercontent.com/s/7k58jbmqa51sxmh/FLANN-msvc140.exe?dl=0' -outfile FLANN.exe
+  - cmd: FLANN.exe -o"C:\Program Files" -y
+  - ECHO "Installed PCL:"
+  - ps: "ls \"C:/Program Files/FLANN\""
+  - set PATH=%PATH%;C:\Program Files\FLANN\bin
+  # Eigen 
+  - ps: wget 'https://dl.dropboxusercontent.com/s/3v6i9i8dxj4o8ji/Eigen.exe?dl=0' -outfile Eigen.exe
+  - cmd: Eigen.exe -o"C:\Program Files" -y
+  - ECHO "Installed PCL:"
+  - ps: "ls \"C:/Program Files/Eigen\""
+  # PCL
+  - ps: wget 'https://dl.dropboxusercontent.com/s/r9tvi9md54zlul2/PCL-1_8_1-July2018-msvc140.exe?dl=0' -outfile PCL_1.8.1.exe
+  - cmd: PCL_1.8.1.exe -o"C:\Program Files" -y
+  - ECHO "Installed PCL:"
+  - ps: "ls \"C:/Program Files/PCL\""
+  - set PATH=%PATH%;C:\Program Files\PCL\bin
+  # zlib
+  - ps: wget 'https://docs.google.com/uc?authuser=0&id=0B46akLGdg-uaYm9MTTI4MUtUcmc&export=download' -outfile zlib-1.2.8-vc2010-x64.zip
+  - ps: Expand-Archive zlib-1.2.8-vc2010-x64.zip -DestinationPath 'C:\Program Files'
+  - ECHO "Installed zlib:"
+  - ps: "ls \"C:/Program Files/zlib\""
+  - set PATH=%PATH%;C:\Program Files\zlib\bin
+  # g2o
+  - ps: wget 'https://dl.dropboxusercontent.com/s/ht74s5pa21wokzw/g2o.exe?dl=0' -outfile g2o.exe
+  - cmd: g2o.exe -o"C:\Program Files" -y
+  - ECHO "Installed g2o:"
+  - ps: "ls \"C:/Program Files/g2o\""
+  - set PATH=%PATH%;C:\Program Files\g2o\bin
+  # GTSAM
+  - ps: wget 'https://dl.dropboxusercontent.com/s/0fpr6r4cgsqmvhf/GTSAM-4_0_0_alpha2-msvc140.exe?dl=0' -outfile GTSAM.exe
+  - cmd: GTSAM.exe -o"C:\Program Files" -y
+  - ECHO "Installed GTSAM:"
+  - ps: "ls \"C:/Program Files/GTSAM\""
+  - set PATH=%PATH%;C:\Program Files\GTSAM\bin
+  
+before_build:
+  - cd c:\projects\rtabmap\build
+  - ECHO %PROGRAMFILES%
+  - ECHO %PATH%
+  - cmake -G "Visual Studio 14 2015 Win64" -DOpenCV_DIR="C:\Program Files\opencv\build" -DPCL_DIR="C:\Program Files\PCL\cmake" -DZLIB_ROOT="C:\Program Files\zlib" -DBUILD_AS_BUNDLE=ON ..
+  
+after_build :
+  - cmake --build . --config Release --target package
+
+artifacts:
+  - path: build\RTABMap-*
+  
+notifications:
+  - provider: Email
+    to:
+      - matlabbe@email.com
+    on_build_success: false
+    on_build_failure: false
+    on_build_status_changed: true

.gitignore

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

.travis.yml

@@ -1,6 +1,7 @@
 sudo: true
 dist: trusty
 language: cpp
+group: deprecated-2017Q3
 
 compiler:
   - gcc
@@ -13,17 +14,21 @@ addons:
       - libopencv-dev
       - libqt4-dev
       - libsqlite3-dev
+      - libyaml-cpp-dev
 
 install:
     - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list'
     - 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
+    - sudo apt-get update && sudo apt-get install dpkg
+    - sudo apt-get -y install libpcl-1.7-all libfreenect-dev ros-indigo-libg2o ros-indigo-octomap libopenni2-dev
 
 script: 
+    - source /opt/ros/indigo/setup.bash
     - mkdir -p build && cd build
     - cmake .. 
     - make
 
 notifications:
-  email: false
+  email:
+    - matlabbe@gmail.com

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)
-#   set(CMAKE_INSTALL_LIBDIR "lib")
-#ENDIF(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 MULTI_ARCH AND NOT DEFINED CMAKE_INSTALL_LIBDIR)
 
 INCLUDE(GNUInstallDirs)
 
@@ -19,8 +20,8 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 # VERSION
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
-SET(RTABMAP_MINOR_VERSION 11)
-SET(RTABMAP_PATCH_VERSION 5)
+SET(RTABMAP_MINOR_VERSION 17)
+SET(RTABMAP_PATCH_VERSION 6)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -45,18 +46,25 @@ IF(NOT ANDROID)
 ENDIF(NOT ANDROID)
 
 IF(WIN32 AND NOT MINGW)
-  	ADD_DEFINITIONS("-DNOMINMAX")
+    ADD_DEFINITIONS("-DNOMINMAX")
     ADD_DEFINITIONS("-wd4100 -wd4127 -wd4150 -wd4191 -wd4242 -wd4244 -wd4251 -wd4305 -wd4365 -wd4512 -wd4514 -wd4548 -wd4571 -wd4619 -wd4625 -wd4626 -wd4628 -wd4668 -wd4710 -wd4711 -wd4738 -wd4820 -wd4946 -wd4986")
 ELSE ()
-	ADD_DEFINITIONS( "-Wall" )
-	ADD_DEFINITIONS("-Wno-unknown-pragmas")
+    ADD_DEFINITIONS( "-Wall" )
+    ADD_DEFINITIONS("-Wno-unknown-pragmas")
 ENDIF()
 
+if(POLICY CMP0020)
+    cmake_policy(SET CMP0020 OLD)
+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")  
-	SET(CMAKE_MODULE_LINKER_FLAGS "-Wl,--enable-auto-import") 
-	SET(CMAKE_SHARED_LINKER_FLAGS "-Wl,--enable-auto-import") 
+    SET(CMAKE_EXE_LINKER_FLAGS "-Wl,--enable-auto-import")
+    SET(CMAKE_MODULE_LINKER_FLAGS "-Wl,--enable-auto-import")
+    SET(CMAKE_SHARED_LINKER_FLAGS "-Wl,--enable-auto-import")
 ENDIF(MINGW)
 
 # GCC 4 required
@@ -105,29 +113,40 @@ SET( CMAKE_ARCHIVE_OUTPUT_DIRECTORY_DEBUG "${CMAKE_ARCHIVE_OUTPUT_DIRECTORY}")
 SET( CMAKE_ARCHIVE_OUTPUT_DIRECTORY_RELEASE "${CMAKE_ARCHIVE_OUTPUT_DIRECTORY}")
 
 ####### INSTALL DIR #######
-set(INSTALL_INCLUDE_DIR include/${PROJECT_PREFIX}-${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION} CACHE PATH
-  "Installation directory for header files")
+set(INSTALL_INCLUDE_DIR include/${PROJECT_PREFIX}-${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION})
 if(WIN32 AND NOT CYGWIN)
   set(DEF_INSTALL_CMAKE_DIR CMake)
 else()
   set(DEF_INSTALL_CMAKE_DIR ${CMAKE_INSTALL_LIBDIR}/${PROJECT_PREFIX}-${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION})
 endif()
-set(INSTALL_CMAKE_DIR ${DEF_INSTALL_CMAKE_DIR} CACHE PATH
-  "Installation directory for CMake files")
+set(INSTALL_CMAKE_DIR ${DEF_INSTALL_CMAKE_DIR})
   
 ####### BUILD OPTIONS #######
-IF(APPLE)
-	OPTION(BUILD_AS_BUNDLE "Set to ON to build as bundle (DragNDrop)" OFF)
-ENDIF(APPLE)
+
+# ANDROID_PREBUILD (early exit if true)
+OPTION( ANDROID_PREBUILD "Set to ON to build rtabmap resource build tool (required for android build)" OFF )
+IF(ANDROID_PREBUILD)
+   MESSAGE("Option ANDROID_PREBUILD is set, only rtabmap resource tool will be built. You can use android toolchain after that.")
+   ADD_SUBDIRECTORY( utilite )
+   return()
+ENDIF(ANDROID_PREBUILD)
+
+IF(APPLE OR WIN32)
+    OPTION(BUILD_AS_BUNDLE "Set to ON to build as bundle with all embedded dependencies (DragNDrop for Mac, installer for Windows)" OFF)
+ENDIF(APPLE OR WIN32)
 OPTION(BUILD_APP "Build main application" ON)
 OPTION(BUILD_TOOLS "Build tools" ON)
 OPTION(BUILD_EXAMPLES "Build examples" ON)
 
 ####### 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_K4W2          "Include Kinect for Windows v2 support" ON)
 option(WITH_OPENNI2       "Include OpenNI2 support"              ON)
 option(WITH_DC1394        "Include dc1394 support"               ON)
 option(WITH_G2O           "Include g2o support"                  ON)
@@ -135,14 +154,46 @@ option(WITH_GTSAM         "Include GTSAM support"                ON)
 option(WITH_TORO          "Include TORO support"                 ON)
 option(WITH_VERTIGO       "Include Vertigo support"              ON)
 option(WITH_CVSBA         "Include cvsba support"                ON)
+option(WITH_POINTMATCHER  "Include libpointmatcher support"      ON)
+option(WITH_LOAM          "Include LOAM support"                 ON)
 option(WITH_FLYCAPTURE2   "Include FlyCapture2/Triclops support" ON)
+option(WITH_ZED           "Include ZED sdk support"              ON)
+option(WITH_REALSENSE     "Include RealSense support"            ON)
+option(WITH_REALSENSE_SLAM "Include RealSenseSlam support"       ON)
+option(WITH_REALSENSE2    "Include RealSense support"            ON)
+option(WITH_OCTOMAP       "Include Octomap support"              ON)
+option(WITH_CPUTSDF       "Include CPUTSDF support"              ON)
+option(WITH_OPENCHISEL    "Include open_chisel support"          ON)
+option(WITH_FOVIS         "Include FOVIS support"                ON)
+option(WITH_VISO2         "Include VISO2 support"                ON)
+option(WITH_DVO           "Include DVO support"                  ON)
+option(WITH_ORB_SLAM2     "Include ORB_SLAM2 support"            ON)
+option(WITH_OKVIS         "Include OKVIS support"                ON)
+option(WITH_MSCKF_VIO     "Include MSCKF_VIO support"            OFF)
+option(PCL_OMP            "With PCL OMP implementations"         ON)
 
 FIND_PACKAGE(OpenCV REQUIRED QUIET)
-FIND_PACKAGE(PCL 1.7 REQUIRED QUIET)
+
+IF(WITH_QT)
+FIND_PACKAGE(PCL 1.7 REQUIRED QUIET COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation visualization)
+ELSE()
+FIND_PACKAGE(PCL 1.7 REQUIRED QUIET COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation )
+ENDIF()
+if("${PCL_DEFINITIONS}" MATCHES "-march=native")
+  MESSAGE(WARNING "PCL definitions contain \"-march=native\", make sure all libraries using Eigen are also compiled with that flag to avoid some segmentation faults (with gdb referring to some Eigen functions).")
+else()
+  MESSAGE(STATUS "PCL definitions don't contain \"-march=native\", make sure all libraries using Eigen are also compiled without that flag to avoid some segmentation faults (with gdb referring to some Eigen functions).")
+endif()
+
 FIND_PACKAGE(ZLIB REQUIRED QUIET)
 
-# fix libproj.so not found on Xenial
+FIND_PACKAGE(Sqlite3 QUIET)
+IF(Sqlite3_FOUND)
+   MESSAGE(STATUS "Found Sqlite3: ${Sqlite3_INCLUDE_DIRS} ${Sqlite3_LIBRARIES}")
+ENDIF(Sqlite3_FOUND)
+
 if(NOT "${PCL_LIBRARIES}" STREQUAL "")
+   # fix libproj.so not found on Xenial
    list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4")
 endif()
 
@@ -155,6 +206,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()
@@ -169,6 +223,7 @@ 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)
     FIND_PACKAGE(VTK)
     IF(NOT VTK_FOUND)
@@ -178,17 +233,31 @@ IF(WITH_QT)
     # 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 Svg QUIET)
+        FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui QUIET)
+        IF(Qt5_FOUND)
+             FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui OPTIONAL_COMPONENTS Svg)
+        ENDIF(Qt5_FOUND)
     ENDIF("${VTK_MAJOR_VERSION}" GREATER 5)
 
     IF(NOT Qt5_FOUND)
-        FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg)
+        FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui OPTIONAL_COMPONENTS QtSvg)
     ENDIF(NOT Qt5_FOUND)
         
     IF(QT4_FOUND OR Qt5_FOUND)
         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)
+			MESSAGE(STATUS "VTK_RENDERING_BACKEND=${VTK_RENDERING_BACKEND}")
+			IF(VTK_RENDERING_BACKEND STREQUAL "OpenGL2")
+				ADD_DEFINITIONS("-DVTK_OPENGL2")
+			ENDIF(VTK_RENDERING_BACKEND STREQUAL "OpenGL2")
+        ENDIF()
+        ADD_DEFINITIONS(-DQT_NO_KEYWORDS) # To avoid conflicts with boost signals/foreach and Qt macros
     ENDIF(QT4_FOUND OR Qt5_FOUND)
 ENDIF(WITH_QT)
 
@@ -202,13 +271,28 @@ ENDIF(WITH_FREENECT)
 IF(WITH_FREENECT2)
     FIND_PACKAGE(freenect2 QUIET)
     IF(freenect2_FOUND)
-	   IF(NOT freenect2_INCLUDE_DIRS)
-	      SET(freenect2_INCLUDE_DIRS ${freenect2_INCLUDE_DIR})
-	   ENDIF(NOT freenect2_INCLUDE_DIRS)
+       IF(NOT freenect2_INCLUDE_DIRS)
+          SET(freenect2_INCLUDE_DIRS ${freenect2_INCLUDE_DIR})
+       ENDIF(NOT freenect2_INCLUDE_DIRS)
        MESSAGE(STATUS "Found freenect2: ${freenect2_INCLUDE_DIRS}")
+       # Explicitly link to OpenCL (in case of CUDA installed)
+       FIND_PACKAGE(OpenCL QUIET)
+       IF(OpenCL_FOUND)
+          SET(freenect2_LIBRARIES
+            ${OpenCL_LIBRARIES}
+            ${freenect2_LIBRARIES}
+          )
+       ENDIF(OpenCL_FOUND)
     ENDIF(freenect2_FOUND)
 ENDIF(WITH_FREENECT2)
 
+IF(WITH_K4W2 AND WIN32)
+    FIND_PACKAGE(KinectSDK2 QUIET)
+	IF(KinectSDK2_FOUND)
+       MESSAGE(STATUS "Found Kinect for Windows 2: ${KinectSDK2_INCLUDE_DIRS}")
+    ENDIF(KinectSDK2_FOUND)
+ENDIF(WITH_K4W2 AND WIN32)
+
 # IF PCL depends on OpenNI2 (already found), ignore WITH_OPENNI2
 IF(WITH_OPENNI2 OR OpenNI2_FOUND)
     FIND_PACKAGE(OpenNI2 QUIET)
@@ -235,22 +319,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)
@@ -265,6 +333,184 @@ IF(WITH_CVSBA)
     ENDIF(cvsba_FOUND)
 ENDIF(WITH_CVSBA)
 
+IF(WITH_POINTMATCHER)
+    find_package(libpointmatcher QUIET)
+    IF(libpointmatcher_FOUND)
+       MESSAGE(STATUS "Found libpointmatcher: ${libpointmatcher_INCLUDE_DIRS}")
+    ENDIF(libpointmatcher_FOUND)
+ENDIF(WITH_POINTMATCHER)
+
+IF(WITH_LOAM)
+    find_package(loam_velodyne QUIET)
+    IF(loam_velodyne_FOUND)
+       MESSAGE(STATUS "Found loam_velodyne: ${loam_velodyne_INCLUDE_DIRS}")
+    ENDIF(loam_velodyne_FOUND)
+ENDIF(WITH_LOAM)
+
+SET(ZED_FOUND FALSE)
+IF(WITH_ZED)
+    IF(WIN32) # Windows
+        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 2 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)
+    IF(WITH_REALSENSE_SLAM)
+       FIND_PACKAGE(RealSense QUIET COMPONENTS slam)
+    ELSE()
+       FIND_PACKAGE(RealSense QUIET)
+    ENDIF()
+    IF(RealSense_FOUND)
+       MESSAGE(STATUS "Found RealSense: ${RealSense_INCLUDE_DIRS}")
+    ENDIF(RealSense_FOUND)
+    IF(RealSenseSlam_FOUND)
+       MESSAGE(STATUS "Found RealSenseSlam: ${RealSense_INCLUDE_DIRS}")
+    ENDIF(RealSenseSlam_FOUND)
+ENDIF(WITH_REALSENSE)
+
+IF(WITH_REALSENSE2)
+    IF(WIN32)
+        FIND_PACKAGE(RealSense2 QUIET)
+	ELSE()
+        FIND_PACKAGE(realsense2 QUIET)
+    ENDIF()
+    IF(realsense2_FOUND)
+       MESSAGE(STATUS "Found RealSense2: ${realsense2_INCLUDE_DIRS}")
+    ENDIF(realsense2_FOUND)
+ENDIF(WITH_REALSENSE2)
+
+IF(WITH_OCTOMAP)
+    FIND_PACKAGE(OCTOMAP QUIET)
+    IF(OCTOMAP_FOUND)
+       MESSAGE(STATUS "Found octomap ${OCTOMAP_VERSION}: ${OCTOMAP_INCLUDE_DIRS}")
+       IF(OCTOMAP_VERSION VERSION_LESS 1.8)
+          ADD_DEFINITIONS("-DOCTOMAP_PRE_18")
+       ENDIF(OCTOMAP_VERSION VERSION_LESS 1.8)
+    ENDIF(OCTOMAP_FOUND)
+ENDIF(WITH_OCTOMAP)
+
+IF(WITH_CPUTSDF)
+    FIND_PACKAGE(CPUTSDF QUIET)
+    IF(CPUTSDF_FOUND)
+       MESSAGE(STATUS "Found CPUTSDF: ${CPUTSDF_INCLUDE_DIRS}")
+    ENDIF(CPUTSDF_FOUND)
+ENDIF(WITH_CPUTSDF)
+
+IF(WITH_OPENCHISEL)
+    find_package(open_chisel QUIET)
+    if(open_chisel_FOUND)
+        MESSAGE(STATUS "Found open_chisel: ${open_chisel_INCLUDE_DIRS}")
+    endif(open_chisel_FOUND)
+ENDIF(WITH_OPENCHISEL)
+
+IF(WITH_FOVIS)
+    FIND_PACKAGE(libfovis QUIET)
+    IF(libfovis_FOUND)
+       MESSAGE(STATUS "Found libfovis: ${libfovis_INCLUDE_DIRS}")
+    ENDIF(libfovis_FOUND)
+ENDIF(WITH_FOVIS)
+
+IF(WITH_VISO2)
+    FIND_PACKAGE(libviso2 QUIET)
+    IF(libviso2_FOUND)
+       MESSAGE(STATUS "Found libviso2: ${libviso2_INCLUDE_DIRS}")
+    ENDIF(libviso2_FOUND)
+ENDIF(WITH_VISO2)
+
+IF(WITH_DVO)
+    FIND_PACKAGE(dvo_core QUIET)
+    IF(dvo_core_FOUND)
+       MESSAGE(STATUS "Found dvo_core: ${dvo_core_INCLUDE_DIRS}")
+    ENDIF(dvo_core_FOUND)
+ENDIF(WITH_DVO)
+
+IF(WITH_OKVIS)
+    FIND_PACKAGE(okvis 1.1 QUIET)
+    IF(okvis_FOUND)
+       MESSAGE(STATUS "Found okvis: ${OKVIS_INCLUDE_DIRS}")
+       find_package(brisk 2 REQUIRED)
+       MESSAGE(STATUS "Found brisk: ${BRISK_INCLUDE_DIRS}")
+       find_package(opengv REQUIRED)
+       MESSAGE(STATUS "Found opengv: ${OPENGV_INCLUDE_DIRS}")
+       find_package(Ceres REQUIRED CONFIG PATHS ${OKVIS_CERES_CONFIG} NO_DEFAULT_PATH)
+       MESSAGE(STATUS "Found ceres: ${CERES_INCLUDE_DIRS}")
+    ENDIF(okvis_FOUND)
+ENDIF(WITH_OKVIS)
+
+IF(WITH_MSCKF_VIO)
+    FIND_PACKAGE(msckf_vio QUIET)
+    IF(msckf_vio_FOUND)
+       MESSAGE(STATUS "Found msckf_vio: ${msckf_vio_INCLUDE_DIRS}")
+    ENDIF(msckf_vio_FOUND)
+ENDIF(WITH_MSCKF_VIO)
+
+
+IF(WITH_ORB_SLAM2 AND NOT G2O_FOUND)
+    FIND_PACKAGE(ORB_SLAM2 QUIET)
+    IF(ORB_SLAM2_FOUND)
+       MESSAGE(STATUS "Found ORB_SLAM2: ${ORB_SLAM2_INCLUDE_DIRS}")
+       FIND_PACKAGE(Pangolin QUIET)
+       IF(NOT Pangolin_FOUND)
+          SET(ORB_SLAM2_FOUND FALSE)
+          MESSAGE(STATUS "Found ORB_SLAM2 but not Pangolin, disabling ORB_SLAM2.")
+       ELSE()
+          MESSAGE(STATUS "Found Pangolin: ${Pangolin_INCLUDE_DIRS}")
+          SET(ORB_SLAM2_INCLUDE_DIRS ${ORB_SLAM2_INCLUDE_DIRS} ${Pangolin_INCLUDE_DIRS})
+          SET(ORB_SLAM2_LIBRARIES ${ORB_SLAM2_LIBRARIES} ${Pangolin_LIBRARIES})
+       ENDIF()
+    ENDIF(ORB_SLAM2_FOUND)
+ENDIF(WITH_ORB_SLAM2 AND NOT G2O_FOUND)
+
+IF(loam_velodyne_FOUND)
+   #LOAM requires c++14
+   IF(NOT MSVC)
+     include(CheckCXXCompilerFlag)
+      CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
+      IF(COMPILER_SUPPORTS_CXX14)
+        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
+      ELSE()
+        message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler if you want to use LOAM (set \"-DWITH_LOAM=OFF\" to build without LOAM).")
+      ENDIF()
+   ENDIF()
+ELSEIF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND OR realsense2_FOUND OR ORB_SLAM2_FOUND OR okvis_FOUND OR open_chisel_FOUND OR msckf_vio_FOUND)
+   #Newest versions require std11
+   IF(NOT MSVC)
+     include(CheckCXXCompilerFlag)
+      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.")
+      ENDIF()
+   ENDIF()
+ENDIF()
+
 ####### 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))
@@ -294,15 +540,27 @@ 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})
+   IF(NOT G2O_CPP11)
+   SET(G2O_CPP_CONF "//")
+   ENDIF(NOT G2O_CPP11)
+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)
@@ -311,25 +569,121 @@ 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 libpointmatcher_FOUND)
+   SET(POINTMATCHER "//")
+ENDIF(NOT libpointmatcher_FOUND)
+IF(NOT loam_velodyne_FOUND)
+   SET(LOAM "//")
+ENDIF(NOT loam_velodyne_FOUND)
 IF(NOT Freenect_FOUND)
    SET(FREENECT "//")
-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 KinectSDK2_FOUND)
+   SET(K4W2 "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${KinectSDK2_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)
-IF(NOT (OpenCV_FOUND AND OpenCV_VERSION_MAJOR EQUAL 3))
+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 RealSenseSlam_FOUND)
+   SET(REALSENSESLAM "//")
+ENDIF(NOT RealSenseSlam_FOUND)
+IF(NOT realsense2_FOUND)
+   SET(REALSENSE2 "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${realsense2_LIBRARIES}) 
+ENDIF()
+IF(NOT OCTOMAP_FOUND)
+   SET(OCTOMAP "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${OCTOMAP_LIBRARIES}) 
+ENDIF()
+IF(NOT CPUTSDF_FOUND)
+   SET(CPUTSDF "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${CPUTSDF_LIBRARIES}) 
+ENDIF()
+IF(NOT open_chisel_FOUND)
+   SET(OPENCHISEL "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${open_chisel_LIBRARIES}) 
+ENDIF()
+IF(NOT libfovis_FOUND)
+   SET(FOVIS "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${libfovis_LIBRARIES}) 
+ENDIF()
+IF(NOT libviso2_FOUND)
+   SET(VISO2 "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${libviso2_LIBRARIES}) 
+ENDIF()
+IF(NOT dvo_core_FOUND)
+   SET(DVO "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${dvo_core_LIBRARIES}) 
+ENDIF()
+IF(NOT okvis_FOUND)
+   SET(OKVIS "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${OKVIS_LIBRARIES}) 
+ENDIF()
+IF(NOT msckf_vio_FOUND)
+   SET(MSCKF_VIO "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${msckf_vio_LIBRARIES}) 
+ENDIF()
+IF(NOT ORB_SLAM2_FOUND)
+   SET(ORB_SLAM2 "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${ORB_SLAM2_LIBRARIES}) 
+ENDIF()
+IF(ADD_VTK_GUI_SUPPORT_QT_TO_CONF)
+   SET(CONF_VTK_QT true)
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} vtkGUISupportQt)
+ELSE()
+   SET(CONF_VTK_QT false)
+ENDIF()
+IF(VTK_USE_QVTK)
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${QVTK_LIBRARY}) 
+ENDIF(VTK_USE_QVTK)
+
+IF(NOT (OpenCV_FOUND AND NOT (OpenCV_VERSION_MAJOR LESS 3)))
    SET(OPENCV3 "//")
-ENDIF(NOT (OpenCV_FOUND AND OpenCV_VERSION_MAJOR EQUAL 3))
+ENDIF(NOT (OpenCV_FOUND AND NOT (OpenCV_VERSION_MAJOR LESS 3)))
 CONFIGURE_FILE(Version.h.in ${PROJECT_SOURCE_DIR}/corelib/include/${PROJECT_PREFIX}/core/Version.h)
 
 ADD_SUBDIRECTORY( utilite )
@@ -375,8 +729,8 @@ file(RELATIVE_PATH REL_LIB_DIR "${CMAKE_INSTALL_PREFIX}/${INSTALL_CMAKE_DIR}" "$
    
 # ... for the build tree
 set(CONF_INCLUDE_DIRS "${PROJECT_SOURCE_DIR}/corelib/include" 
-					  "${PROJECT_SOURCE_DIR}/guilib/include" 
-					  "${PROJECT_SOURCE_DIR}/utilite/include")
+                      "${PROJECT_SOURCE_DIR}/guilib/include" 
+                      "${PROJECT_SOURCE_DIR}/utilite/include")
 set(CONF_LIB_DIR "${CMAKE_ARCHIVE_OUTPUT_DIRECTORY} ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}")
 IF(QT4_FOUND OR Qt5_FOUND)
    set(CONF_WITH_GUI ON)
@@ -449,7 +803,11 @@ IF(WIN32)
   ELSE() 
     SET(CPACK_NSIS_INSTALL_ROOT "$PROGRAMFILES") 
   ENDIF()
-  SET(CPACK_GENERATOR "ZIP;NSIS")
+  IF(BUILD_AS_BUNDLE)
+    SET(CPACK_GENERATOR "ZIP;NSIS")
+  ELSE()
+    SET(CPACK_GENERATOR "ZIP")
+  ENDIF()
   SET(CPACK_SOURCE_GENERATOR "ZIP")
   SET(CPACK_NSIS_PACKAGE_NAME "${PROJECT_NAME}")
   SET(ICON_PATH "${PROJECT_SOURCE_DIR}/app/src/${PROJECT_NAME}.ico")
@@ -501,30 +859,38 @@ MESSAGE(STATUS "  BUILD_APP =            ${BUILD_APP}")
 MESSAGE(STATUS "  BUILD_TOOLS =          ${BUILD_TOOLS}")
 MESSAGE(STATUS "  BUILD_EXAMPLES =       ${BUILD_EXAMPLES}")
 IF(NOT WIN32)
-	# see comment above for the BUILD_SHARED_LIBS option on Windows
-	MESSAGE(STATUS "  BUILD_SHARED_LIBS =    ${BUILD_SHARED_LIBS}")
+    # see comment above for the BUILD_SHARED_LIBS option on Windows
+    MESSAGE(STATUS "  BUILD_SHARED_LIBS =    ${BUILD_SHARED_LIBS}")
 ENDIF(NOT WIN32)
-IF(APPLE)
+IF(APPLE OR WIN32)
 MESSAGE(STATUS "  BUILD_AS_BUNDLE =      ${BUILD_AS_BUNDLE}")
-ENDIF(APPLE)
+ENDIF(APPLE OR WIN32)
 MESSAGE(STATUS "  CMAKE_CXX_FLAGS = ${CMAKE_CXX_FLAGS}")
+MESSAGE(STATUS "  PCL_DEFINITIONS = ${PCL_DEFINITIONS}")
 
+MESSAGE(STATUS "Optional dependencies ('*' affects some default parameters) :")
 IF(OpenCV_FOUND)
   IF(OpenCV_VERSION_MAJOR EQUAL 2)
     IF(OPENCV_NONFREE_FOUND)
-      MESSAGE(STATUS "  With OpenCV 2 nonfree module (SIFT/SURF) = YES (License: Non commercial)")
+      MESSAGE(STATUS " *With OpenCV 2 nonfree module (SIFT/SURF) = YES (License: Non commercial)")
     ELSE()
-      MESSAGE(STATUS "  With OpenCV 2 nonfree module (SIFT/SURF) = NO (not found, License: BSD)")
+      MESSAGE(STATUS " *With OpenCV 2 nonfree module (SIFT/SURF) = NO (not found, License: BSD)")
     ENDIF()
   ELSE()
     IF(OPENCV_XFEATURES2D_FOUND)
-      MESSAGE(STATUS "  With OpenCV 3 xfeatures2d module (SIFT/SURF/BRIEF/FREAK) = YES (License: Non commercial)")
+      MESSAGE(STATUS " *With OpenCV 3 xfeatures2d module (SIFT/SURF/BRIEF/FREAK) = YES (License: Non commercial)")
     ELSE()
-      MESSAGE(STATUS "  With OpenCV 3 xfeatures2d module (SIFT/SURF/BRIEF/FREAK) = NO (not found, License: BSD)")
+      MESSAGE(STATUS " *With OpenCV 3 xfeatures2d module (SIFT/SURF/BRIEF/FREAK) = NO (not found, License: BSD)")
     ENDIF()
   ENDIF()
 ENDIF(OpenCV_FOUND)
 
+IF(Sqlite3_FOUND)
+MESSAGE(STATUS "  With external SQLite3     = YES (License: Public Domain)")
+ELSE()
+MESSAGE(STATUS "  With external SQLite3     = NO (sqlite3 not found, internal version is used for convenience)")
+ENDIF()
+
 IF(Freenect_FOUND)
 MESSAGE(STATUS "  With Freenect             = YES (License: Apache v2 and/or GPLv2)")
 ELSEIF(NOT WITH_FREENECT)
@@ -549,6 +915,14 @@ ELSE()
 MESSAGE(STATUS "  With Freenect2            = NO (libfreenect2 not found)")
 ENDIF()
 
+IF(KinectSDK2_FOUND)
+MESSAGE(STATUS "  With Kinect for Windows 2 = YES (License: Apache v2 and/or GPLv2)")
+ELSEIF(NOT WITH_K4W2)
+MESSAGE(STATUS "  With Kinect for Windows 2 = NO (WITH_K4W2=OFF)")
+ELSE()
+MESSAGE(STATUS "  With Kinect for Windows 2 = NO (Kinect for Windows 2 SDK not found)")
+ENDIF()
+
 IF(DC1394_FOUND)
 MESSAGE(STATUS "  With dc1394               = YES (License: LGPL)")
 ELSEIF(NOT WITH_DC1394)
@@ -572,26 +946,30 @@ MESSAGE(STATUS "  With TORO                 = NO (WITH_TORO=OFF)")
 ENDIF()
 
 IF(G2O_FOUND)
-MESSAGE(STATUS "  With g2o                  = YES (License: BSD)")
+MESSAGE(STATUS " *With g2o                  = YES (License: BSD)")
 ELSEIF(NOT WITH_G2O)
-MESSAGE(STATUS "  With g2o                  = NO (WITH_G2O=OFF)")
+MESSAGE(STATUS " *With g2o                  = NO (WITH_G2O=OFF)")
 ELSE()
-MESSAGE(STATUS "  With g2o                  = NO (g2o not found)")
+MESSAGE(STATUS " *With g2o                  = NO (g2o not found)")
 ENDIF()
 
 IF(GTSAM_FOUND)
-MESSAGE(STATUS "  With GTSAM                = YES (License: BSD)")
+MESSAGE(STATUS " *With GTSAM                = YES (License: BSD)")
 ELSEIF(NOT WITH_GTSAM)
-MESSAGE(STATUS "  With GTSAM                = NO (WITH_GTSAM=OFF)")
+MESSAGE(STATUS " *With GTSAM                = NO (WITH_GTSAM=OFF)")
 ELSE()
-MESSAGE(STATUS "  With GTSAM                = NO (GTSAM not found)")
+MESSAGE(STATUS " *With GTSAM                = NO (GTSAM not found)")
 ENDIF()
 
+IF(G2O_FOUND OR GTSAM_FOUND)
 IF(WITH_VERTIGO)
 MESSAGE(STATUS "  With VERTIGO              = YES (License: GPLv3)")
 ELSE()
 MESSAGE(STATUS "  With VERTIGO              = NO (WITH_VERTIGO=OFF)")
 ENDIF()
+ELSE()
+MESSAGE(STATUS "  With VERTIGO              = NO (GTSAM or g2o required)")
+ENDIF()
 
 IF(cvsba_FOUND)
 MESSAGE(STATUS "  With cvsba                = YES (License: GPLv2)")
@@ -601,6 +979,131 @@ ELSE()
 MESSAGE(STATUS "  With cvsba                = NO (cvsba not found)")
 ENDIF()
 
+IF(libpointmatcher_FOUND)
+MESSAGE(STATUS " *With libpointmatcher      = YES (License: BSD)")
+ELSEIF(NOT WITH_POINTMATCHER)
+MESSAGE(STATUS " *With libpointmatcher      = NO (WITH_POINTMATCHER=OFF)")
+ELSE()
+MESSAGE(STATUS " *With libpointmatcher      = NO (libpointmatcher not found)")
+ENDIF()
+
+IF(loam_velodyne_FOUND)
+MESSAGE(STATUS "  With loam_velodyne        = YES (License: BSD)")
+ELSEIF(NOT WITH_LOAM)
+MESSAGE(STATUS "  With loam_velodyne        = NO (WITH_LOAM=OFF)")
+ELSE()
+MESSAGE(STATUS "  With loam_velodyne        = NO (loam_velodyne not found)")
+ENDIF()
+
+IF(ZED_FOUND)
+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)")
+IF(RealSenseSlam_FOUND)
+MESSAGE(STATUS "  With RealSenseSlam        = YES")
+ELSEIF(NOT WITH_REALSENSE)
+MESSAGE(STATUS "  With RealSenseSlam        = NO (librealsense_slam not found)")
+ELSE()
+MESSAGE(STATUS "  With RealSenseSlam        = NO (WITH_REALSENSE_SLAM=OFF)")
+ENDIF()
+ELSEIF(NOT WITH_REALSENSE)
+MESSAGE(STATUS "  With RealSense            = NO (WITH_REALSENSE=OFF)")
+ELSE()
+MESSAGE(STATUS "  With RealSense            = NO (librealsense not found)")
+ENDIF()
+
+IF(realsense2_FOUND)
+MESSAGE(STATUS "  With RealSense2           = YES (License: Apache-2)")
+ELSEIF(NOT WITH_REALSENSE2)
+MESSAGE(STATUS "  With RealSense2           = NO (WITH_REALSENSE2=OFF)")
+ELSE()
+MESSAGE(STATUS "  With RealSense2           = NO (librealsense2 not found)")
+ENDIF()
+
+IF(OCTOMAP_FOUND)
+MESSAGE(STATUS "  With OCTOMAP              = YES (License: BSD)")
+ELSEIF(NOT WITH_OCTOMAP)
+MESSAGE(STATUS "  With OCTOMAP              = NO (WITH_OCTOMAP=OFF)")
+ELSE()
+MESSAGE(STATUS "  With OCTOMAP              = NO (octomap not found)")
+ENDIF()
+
+IF(CPUTSDF_FOUND)
+MESSAGE(STATUS "  With CPUTSDF              = YES (License: BSD)")
+ELSEIF(NOT WITH_CPUTSDF)
+MESSAGE(STATUS "  With CPUTSDF              = NO (WITH_CPUTSDF=OFF)")
+ELSE()
+MESSAGE(STATUS "  With CPUTSDF              = NO (CPUTSDF not found)")
+ENDIF()
+
+IF(open_chisel_FOUND)
+MESSAGE(STATUS "  With OpenChisel           = YES (License: ???)")
+ELSEIF(NOT WITH_OPENCHISEL)
+MESSAGE(STATUS "  With OpenChisel           = NO (WITH_OPENCHISEL=OFF)")
+ELSE()
+MESSAGE(STATUS "  With OpenChisel           = NO (open_chisel not found)")
+ENDIF()
+
+IF(libfovis_FOUND)
+MESSAGE(STATUS "  With libfovis             = YES (License: GPLv2)")
+ELSEIF(NOT WITH_FOVIS)
+MESSAGE(STATUS "  With libfovis             = NO (WITH_FOVIS=OFF)")
+ELSE()
+MESSAGE(STATUS "  With libfovis             = NO (libfovis not found)")
+ENDIF()
+
+IF(libviso2_FOUND)
+MESSAGE(STATUS "  With libviso2             = YES (License: GPLv3)")
+ELSEIF(NOT WITH_VISO2)
+MESSAGE(STATUS "  With libviso2             = NO (WITH_VISO2=OFF)")
+ELSE()
+MESSAGE(STATUS "  With libviso2             = NO (libviso2 not found)")
+ENDIF()
+
+IF(dvo_core_FOUND)
+MESSAGE(STATUS "  With dvo_core             = YES (License: GPLv3)")
+ELSEIF(NOT WITH_DVO)
+MESSAGE(STATUS "  With dvo_core             = NO (WITH_DVO=OFF)")
+ELSE()
+MESSAGE(STATUS "  With dvo_core             = NO (dvo_core not found)")
+ENDIF()
+
+IF(okvis_FOUND)
+MESSAGE(STATUS "  With okvis                = YES (License: BSD)")
+ELSEIF(NOT WITH_OKVIS)
+MESSAGE(STATUS "  With okvis                = NO (WITH_OKVIS=OFF)")
+ELSE()
+MESSAGE(STATUS "  With okvis                = NO (okvis not found)")
+ENDIF()
+
+IF(msckf_vio_FOUND)
+MESSAGE(STATUS "  With msckf_vio            = YES (License: Penn Software License)")
+ELSEIF(NOT WITH_MSCKF_VIO)
+MESSAGE(STATUS "  With msckf_vio            = NO (WITH_MSCKF_VIO=OFF)")
+ELSE()
+MESSAGE(STATUS "  With msckf_vio            = NO (msckf_vio not found)")
+ENDIF()
+
+IF(ORB_SLAM2_FOUND)
+MESSAGE(STATUS "  With ORB_SLAM2            = YES (License: GPLv3)")
+ELSEIF(NOT WITH_ORB_SLAM2)
+MESSAGE(STATUS "  With ORB_SLAM2            = NO (WITH_ORB_SLAM2=OFF)")
+ELSEIF(G2O_FOUND)
+MESSAGE(STATUS "  With ORB_SLAM2            = NO (WITH_G2O should be OFF as ORB_SLAM2 uses its own g2o version)")
+ELSE()
+MESSAGE(STATUS "  With ORB_SLAM2            = NO (ORB_SLAM2 not found, make sure environment variable ORB_SLAM2_ROOT_DIR is set)")
+ENDIF()
+
 IF(QT4_FOUND)
 MESSAGE(STATUS "  With Qt4                  = YES (License: Open Source or Commercial)")
 ELSEIF(Qt5_FOUND)
@@ -619,3 +1122,5 @@ MESSAGE(SEND_ERROR "No graph optimizer found! You should have at least one of th
        GTSAM (https://collab.cc.gatech.edu/borg/gtsam)
        set -DWITH_TORO=ON")
 ENDIF(NOT GTSAM_FOUND AND NOT G2O_FOUND AND NOT WITH_TORO)
+
+# vim: set et ft=cmake fenc=utf-8 ff=unix sts=0 sw=2 ts=2 :

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

RTABMapConfig.cmake.in

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

Version.h.in

@@ -40,15 +40,32 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @NONFREE@#define RTABMAP_NONFREE
 @TORO@#define RTABMAP_TORO
 @G2O@#define RTABMAP_G2O
+@G2O_CPP_CONF@#define RTABMAP_G2O_CPP11
 @GTSAM@#define RTABMAP_GTSAM
 @VERTIGO@#define RTABMAP_VERTIGO
 @OPENCV3@#define RTABMAP_OPENCV3
 @OPENNI2@#define RTABMAP_OPENNI2
 @FREENECT@#define RTABMAP_FREENECT
 @FREENECT2@#define RTABMAP_FREENECT2
+@K4W2@#define RTABMAP_K4W2
 @CVSBA@#define RTABMAP_CVSBA
+@POINTMATCHER@#define RTABMAP_POINTMATCHER
+@LOAM@#define RTABMAP_LOAM
 @DC1394@#define RTABMAP_DC1394
 @FLYCAPTURE2@#define RTABMAP_FLYCAPTURE2
+@ZED@#define RTABMAP_ZED
+@REALSENSE@#define RTABMAP_REALSENSE
+@REALSENSESLAM@#define RTABMAP_REALSENSE_SLAM
+@REALSENSE2@#define RTABMAP_REALSENSE2
+@OCTOMAP@#define RTABMAP_OCTOMAP
+@CPUTSDF@#define RTABMAP_CPUTSDF
+@OPENCHISEL@#define RTABMAP_OPENCHISEL
+@FOVIS@#define RTABMAP_FOVIS
+@VISO2@#define RTABMAP_VISO2
+@DVO@#define RTABMAP_DVO
+@OKVIS@#define RTABMAP_OKVIS
+@MSCKF_VIO@#define RTABMAP_MSCKF_VIO
+@ORB_SLAM2@#define RTABMAP_ORB_SLAM2
 
 #endif /* VERSION_H_ */
 

app/android/.gitignore

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

app/android/AndroidManifest.xml.in

@@ -2,7 +2,7 @@
 <!-- BEGIN_INCLUDE(manifest) -->
 <manifest xmlns:android="http://schemas.android.com/apk/res/android"
         package="com.introlab.rtabmap"
-        android:versionCode="8"
+        android:versionCode="70"
         android:versionName="@RTABMAP_VERSION@">
 
     <uses-permission android:name="android.permission.CAMERA" />
@@ -10,23 +10,31 @@
     <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-permission android:name="android.permission.ACCESS_FINE_LOCATION" />
+    <uses-feature android:name="android.hardware.location.gps" />
     <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">
+        android:icon="@drawable/ic_launcher"
+        android:debuggable="@ANDROID_DEBUGGABLE@">
+        
+        <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"
+		android:theme="@style/ThemeApp">
             <!-- Tell NativeActivity the name of our .so -->
             <meta-data android:name="android.app.lib_name"
                     android:value="NativeRTABMap" />
@@ -35,6 +43,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

@@ -1,4 +1,17 @@
 
+option(DISABLE_LOG        "Disable Android logging (should be true in release)"    ON)
+IF(DISABLE_LOG)
+  ADD_DEFINITIONS(-DDISABLE_LOG)
+ENDIF(DISABLE_LOG)
+
+MESSAGE(STATUS "DISABLE_LOG = ${DISABLE_LOG}")
+
+IF(DISABLE_LOG)
+   SET(ANDROID_DEBUGGABLE false)
+ELSE()
+   SET(ANDROID_DEBUGGABLE true)
+ENDIF()
+
 add_subdirectory(jni)
 
 
@@ -32,6 +45,11 @@ 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}"

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,13 @@
+<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>
+                <copy todir="${native.libs.dir}">
+                        <fileset dir="${srcdir}/jni/third-party/lib" includes="${android.abi}/*.so"/>
+                </copy>
+	</target>
+</project>

app/android/info.txt.in

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

app/android/jni/CameraTango.h

@@ -29,12 +29,15 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define CAMERATANGO_H_
 
 #include <rtabmap/core/Camera.h>
+#include <rtabmap/core/GeodeticCoords.h>
 #include <rtabmap/utilite/UMutex.h>
 #include <rtabmap/utilite/USemaphore.h>
 #include <rtabmap/utilite/UEventsSender.h>
 #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,16 +72,26 @@ 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(bool colorCamera, int decimation, bool publishRawScan, bool smoothing);
 	virtual ~CameraTango();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
 	void close(); // close Tango connection
+	void resetOrigin();
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
-	rtabmap::Transform tangoPoseToTransform(const TangoPoseData * tangoPose, bool inOpenGLFrame) const;
+	const CameraModel & getCameraModel() const {return model_;}
+	rtabmap::Transform tangoPoseToTransform(const TangoPoseData * tangoPose) const;
+	void setColorCamera(bool enabled) {if(!this->isRunning()) colorCamera_ = enabled;}
 	void setDecimation(int value) {decimation_ = value;}
-	void setAutoExposure(bool enabled) {autoExposure_ = enabled;}
+	void setSmoothing(bool enabled) {smoothing_ = enabled;}
+	void setRawScanPublished(bool enabled) {rawScanPublished_ = enabled;}
+	void setScreenRotation(TangoSupportRotation colorCameraToDisplayRotation) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
+	void setGPS(const GPS & gps);
 
 	void cloudReceived(const cv::Mat & cloud, double timestamp);
 	void rgbReceived(const cv::Mat & tangoImage, int type, double timestamp);
@@ -86,19 +99,24 @@ 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_;
+	Transform previousPose_;
+	double previousStamp_;
+	UTimer cameraStartedTime_;
+	double stampEpochOffset_;
+	bool colorCamera_;
 	int decimation_;
-	bool autoExposure_;
+	bool rawScanPublished_;
+	bool smoothing_;
 	cv::Mat cloud_;
 	double cloudStamp_;
 	cv::Mat tangoColor_;
@@ -106,10 +124,14 @@ private:
 	double tangoColorStamp_;
 	boost::mutex dataMutex_;
 	USemaphore dataReady_;
-	rtabmap::Transform imuTDevice_;
-	rtabmap::Transform imuTDepthCamera_;
-	rtabmap::Transform deviceTDepth_;
 	CameraModel model_;
+	Transform deviceTColorCamera_;
+	TangoSupportRotation colorCameraToDisplayRotation_;
+	cv::Mat fisheyeRectifyMapX_;
+	cv::Mat fisheyeRectifyMapY_;
+	GPS lastKnownGPS_;
+	Transform originOffset_;
+	bool originUpdate_;
 };
 
 } /* namespace rtabmap */

app/android/jni/ProgressionStatus.h

@@ -0,0 +1,124 @@
+/*
+ * 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), jvm_(0), rtabmap_(0)
+	{
+		registerToEventsManager();
+	}
+
+	void setJavaObjects(JavaVM * jvm, jobject rtabmap)
+	{
+		jvm_ = jvm;
+		rtabmap_ = rtabmap;
+	}
+
+	void reset(int max)
+	{
+		count_=-1;
+		max_ = max;
+		setCanceled(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(!isCanceled())
+		{
+			increment();
+		}
+
+		return ProgressState::callback(msg);
+	}
+	virtual ~ProgressionStatus(){}
+
+protected:
+	virtual bool 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");
+			}
+		}
+		return false;
+	}
+
+private:
+	int count_;
+	int max_;
+	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 <CameraTango.h>
-#include <util.h>
+#include "scene.h"
+#include "CameraTango.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
-  // 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 onCreate(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, const std::string & databaseSource=std::string());
+
+  bool onTangoServiceConnected(JNIEnv* env, jobject iBinder);
 
   // Explicitly reset motion tracking and restart the pipeline.
   // Note that this will cause motion tracking to re-initialize.
@@ -113,31 +114,74 @@ class RTABMapApp : public UEventsHandler {
                     float x0, float y0, float x1, float y1);
 
   void setPausedMapping(bool paused);
+  void setOnlineBlending(bool enabled);
   void setMapCloudShown(bool shown);
   void setOdomCloudShown(bool shown);
-  void setMeshRendering(bool enabled);
+  void setMeshRendering(bool enabled, bool withTexture);
+  void setPointSize(float value);
+  void setFOV(float angle);
+  void setOrthoCropFactor(float value);
+  void setGridRotation(float value);
+  void setLighting(bool enabled);
+  void setBackfaceCulling(bool enabled);
+  void setWireframe(bool enabled);
   void setLocalizationMode(bool enabled);
   void setTrajectoryMode(bool enabled);
   void setGraphOptimization(bool enabled);
   void setNodesFiltering(bool enabled);
   void setGraphVisible(bool visible);
-  void setAutoExposure(bool enabled);
+  void setGridVisible(bool visible);
+  void setRawScanSaved(bool enabled);
+  void setCameraColor(bool enabled);
   void setFullResolution(bool enabled);
+  void setSmoothing(bool enabled);
+  void setAppendMode(bool enabled);
+  void setDataRecorderMode(bool enabled);
   void setMaxCloudDepth(float value);
+  void setMinCloudDepth(float value);
+  void setCloudDensityLevel(int value);
   void setMeshAngleTolerance(float value);
   void setMeshTriangleSize(int value);
+  void setClusterRatio(float value);
+  void setMaxGainRadius(float value);
+  void setRenderingTextureDecimation(int value);
+  void setBackgroundColor(float gray);
   int setMappingParameter(const std::string & key, const std::string & value);
+  void setGPS(const rtabmap::GPS & gps);
 
   void resetMapping();
-  void save();
-  bool exportMesh(const std::string & filePath);
+  void save(const std::string & databasePath);
+  void cancelProcessing();
+  bool exportMesh(
+		  float cloudVoxelSize,
+		  bool regenerateCloud,
+		  bool meshing,
+		  int textureSize,
+		  int textureCount,
+		  int normalK,
+		  bool optimized,
+		  float optimizedVoxelSize,
+		  int optimizedDepth,
+		  int optimizedMaxPolygons,
+		  float optimizedColorRadius,
+		  bool optimizedCleanWhitePolygons,
+		  int optimizedMinClusterSize,
+		  float optimizedMaxTextureDistance,
+		  int optimizedMinTextureClusterSize,
+		  bool blockRendering);
+  bool postExportation(bool visualize);
+  bool writeExportedMesh(const std::string & directory, const std::string & name);
   int postProcessing(int approach);
 
  protected:
-  virtual void handleEvent(UEvent * event);
+  virtual bool handleEvent(UEvent * event);
 
  private:
   rtabmap::ParametersMap getRtabmapParameters();
+  bool smoothMesh(int id, Mesh & mesh);
+  void gainCompensation(bool full = false);
+  std::vector<pcl::Vertices> filterOrganizedPolygons(const std::vector<pcl::Vertices> & polygons, int cloudSize) const;
+  std::vector<pcl::Vertices> filterPolygons(const std::vector<pcl::Vertices> & polygons, int cloudSize) const;
 
  private:
   rtabmap::CameraTango * camera_;
@@ -150,45 +194,76 @@ class RTABMapApp : public UEventsHandler {
   bool nodesFiltering_;
   bool localizationMode_;
   bool trajectoryMode_;
-  bool autoExposure_;
+  bool rawScanSaved_;
+  bool smoothing_;
+  bool cameraColor_;
   bool fullResolution_;
+  bool appendMode_;
   float maxCloudDepth_;
+  float minCloudDepth_;
+  int cloudDensityLevel_;
   int meshTrianglePix_;
   float meshAngleToleranceDeg_;
+  float clusterRatio_;
+  float maxGainRadius_;
+  int renderingTextureDecimation_;
+  float backgroundColor_;
 
   rtabmap::ParametersMap mappingParameters_;
 
-
+  bool paused_;
+  bool dataRecorderMode_;
   bool clearSceneOnNextRender_;
+  bool openingDatabase_;
+  bool exporting_;
+  bool postProcessing_;
+  bool filterPolygonsOnNextRender_;
+  int gainCompensationOnNextRender_;
+  bool bilateralFilteringOnNextRender_;
+  bool takeScreenshotOnNextRender_;
+  bool cameraJustInitialized_;
+  int meshDecimation_;
   int totalPoints_;
   int totalPolygons_;
   int lastDrawnCloudsCount_;
+  float renderingTime_;
+  double lastPostRenderEventTime_;
+  double lastPoseEventTime_;
+  std::map<std::string, float> bufferedStatsData_;
+
+  bool visualizingMesh_;
+  bool exportedMeshUpdated_;
+  pcl::TextureMesh::Ptr optMesh_;
+  cv::Mat optTexture_;
+  int optRefId_;
+  rtabmap::Transform * optRefPose_; // App crashes when loading native library if not dynamic
 
   // main_scene_ includes all drawable object for visualizing Tango device's
   // movement and point cloud.
   Scene main_scene_;
 
-	std::list<rtabmap::Statistics> rtabmapEvents_;
+	std::list<rtabmap::RtabmapEvent*> rtabmapEvents_;
+	std::list<rtabmap::RtabmapEvent*> visLocalizationEvents_;
 	std::list<rtabmap::OdometryEvent> odomEvents_;
 	std::list<rtabmap::Transform> poseEvents_;
 
+	rtabmap::Transform mapToOdom_;
+
 	boost::mutex rtabmapMutex_;
+	boost::mutex visLocalizationMutex_;
 	boost::mutex meshesMutex_;
 	boost::mutex odomMutex_;
 	boost::mutex poseMutex_;
+	boost::mutex renderingMutex_;
 
-	struct Mesh
-	{
-		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
-		std::vector<pcl::Vertices> polygons;
-		rtabmap::Transform pose;
-		cv::Mat texture;
-	};
+	USemaphore screenshotReady_;
 
 	std::map<int, Mesh> createdMeshes_;
 	std::map<int, rtabmap::Transform> rawPoses_;
 
 	std::pair<rtabmap::RtabmapEventInit::Status, std::string> status_;
+
+	rtabmap::ProgressionStatus progressionStatus_;
 };
 
 #endif  // TANGO_POINT_CLOUD_POINT_CLOUD_APP_H_

app/android/jni/bounding_box_drawable.h

@@ -0,0 +1,131 @@
+/*
+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 BOUNDING_BOX_DRAWABLE_H_
+#define BOUNDING_BOX_DRAWABLE_H_
+
+#include "tango-gl/line.h"
+
+class BoundingBoxDrawable : public tango_gl::Line
+{
+public:
+	BoundingBoxDrawable() :
+		Line(3.0f, GL_LINES)
+	{
+		vec_vertices_.resize(24);
+	}
+
+	void updateVertices(const pcl::PointXYZ & min, const pcl::PointXYZ & max)
+	{
+		int index = 0;
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = min.z;
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = min.z;
+
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = min.z;
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = min.z;
+
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = min.z;
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = max.z;
+
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = max.z;
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = max.z;
+
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = max.z;
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = max.z;
+
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = max.z;
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = min.z;
+
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = min.z;
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = max.z;
+
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = min.z;
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = min.z;
+
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = min.z;
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = min.z;
+
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = min.z;
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = max.z;
+
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = max.z;
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = max.z;
+
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = max.z;
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = max.z;
+	}
+
+};
+#endif  // TANGO_GL_LINE_H_

app/android/jni/jni_interface.cpp

@@ -48,26 +48,44 @@ void GetJStringContent(JNIEnv *AEnv, jstring AStr, std::string &ARes) {
   AEnv->ReleaseStringUTFChars(AStr,s);
 }
 
-JNIEXPORT jint JNICALL
-Java_com_introlab_rtabmap_RTABMapLib_initialize(
+JNIEXPORT void JNICALL
+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
-Java_com_introlab_rtabmap_RTABMapLib_onResume(
-    JNIEnv*, jobject) {
-  return app.onResume();
+JNIEXPORT int JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_openDatabase2(
+    JNIEnv* env, jobject, jstring databaseSource, jstring databasePath, bool databaseInMemory, bool optimize)
+{
+	std::string databasePathC;
+	GetJStringContent(env,databasePath,databasePathC);
+	std::string databaseSourceC;
+	GetJStringContent(env,databaseSource,databaseSourceC);
+	return app.openDatabase(databasePathC, databaseInMemory, optimize, databaseSourceC);
+}
+
+JNIEXPORT bool JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_onTangoServiceConnected(
+		JNIEnv* env, jobject, jobject iBinder) {
+  return app.onTangoServiceConnected(env, iBinder);
 }
 
 JNIEXPORT void JNICALL
@@ -120,6 +138,12 @@ Java_com_introlab_rtabmap_RTABMapLib_setPausedMapping(
 	return app.setPausedMapping(paused);
 }
 JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setOnlineBlending(
+		JNIEnv*, jobject, bool enabled)
+{
+	return app.setOnlineBlending(enabled);
+}
+JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setMapCloudShown(
 		JNIEnv*, jobject, bool shown)
 {
@@ -133,9 +157,51 @@ 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_setFOV(
+		JNIEnv*, jobject, float fov)
+{
+	return app.setFOV(fov);
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setOrthoCropFactor(
+		JNIEnv*, jobject, float value)
+{
+	return app.setOrthoCropFactor(value);
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setGridRotation(
+		JNIEnv*, jobject, float value)
+{
+	return app.setGridRotation(value);
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setLighting(
 		JNIEnv*, jobject, bool enabled)
 {
-	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_setWireframe(
+		JNIEnv*, jobject, bool enabled)
+{
+	return app.setWireframe(enabled);
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setLocalizationMode(
@@ -168,10 +234,16 @@ Java_com_introlab_rtabmap_RTABMapLib_setGraphVisible(
 	return app.setGraphVisible(visible);
 }
 JNIEXPORT void JNICALL
-Java_com_introlab_rtabmap_RTABMapLib_setAutoExposure(
+Java_com_introlab_rtabmap_RTABMapLib_setGridVisible(
+		JNIEnv*, jobject, bool visible)
+{
+	return app.setGridVisible(visible);
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setRawScanSaved(
 		JNIEnv*, jobject, bool enabled)
 {
-	return app.setAutoExposure(enabled);
+	return app.setRawScanSaved(enabled);
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setFullResolution(
@@ -180,12 +252,48 @@ Java_com_introlab_rtabmap_RTABMapLib_setFullResolution(
 	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_setCameraColor(
+		JNIEnv*, jobject, bool enabled)
+{
+	return app.setCameraColor(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_setMinCloudDepth(
+		JNIEnv*, jobject, float value)
+{
+	return app.setMinCloudDepth(value);
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setCloudDensityLevel(
+		JNIEnv*, jobject, int value)
+{
+	return app.setCloudDensityLevel(value);
+}
+JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setMeshAngleTolerance(
 		JNIEnv*, jobject, float value)
 {
@@ -197,6 +305,30 @@ Java_com_introlab_rtabmap_RTABMapLib_setMeshTriangleSize(
 {
 	return app.setMeshTriangleSize(value);
 }
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setClusterRatio(
+		JNIEnv*, jobject, float value)
+{
+	return app.setClusterRatio(value);
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setMaxGainRadius(
+		JNIEnv*, jobject, float value)
+{
+	return app.setMaxGainRadius(value);
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setRenderingTextureDecimation(
+		JNIEnv*, jobject, int value)
+{
+	return app.setRenderingTextureDecimation(value);
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setBackgroundColor(
+		JNIEnv*, jobject, float value)
+{
+	return app.setBackgroundColor(value);
+}
 JNIEXPORT jint JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setMappingParameter(
     JNIEnv* env, jobject, jstring key, jstring value)
@@ -207,6 +339,24 @@ Java_com_introlab_rtabmap_RTABMapLib_setMappingParameter(
 	return app.setMappingParameter(keyC, valueC);
 }
 
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setGPS(
+		JNIEnv*, jobject,
+		double stamp,
+		double longitude,
+		double latitude,
+		double altitude,
+		double accuracy,
+		double bearing)
+{
+	return app.setGPS(rtabmap::GPS(stamp,
+			longitude,
+			latitude,
+			altitude,
+			accuracy,
+			bearing));
+}
+
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_resetMapping(
 		JNIEnv*, jobject)
@@ -216,20 +366,78 @@ 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,
+		float cloudVoxelSize,
+		bool regenerateCloud,
+		bool meshing,
+		int textureSize,
+		int textureCount,
+		int normalK,
+		bool optimized,
+		float optimizedVoxelSize,
+		int optimizedDepth,
+		int optimizedMaxPolygons,
+		float optimizedColorRadius,
+		bool optimizedCleanWhitePolygons,
+		int optimizedMinClusterSize,
+		float optimizedMaxTextureDistance,
+		int optimizedMinTextureClusterSize,
+		bool blockRendering)
 {
-	std::string filePathC;
-	GetJStringContent(env,filePath,filePathC);
-	return app.exportMesh(filePathC);
+	return app.exportMesh(
+			cloudVoxelSize,
+			regenerateCloud,
+			meshing,
+			textureSize,
+			textureCount,
+			normalK,
+			optimized,
+			optimizedVoxelSize,
+			optimizedDepth,
+			optimizedMaxPolygons,
+			optimizedColorRadius,
+			optimizedCleanWhitePolygons,
+			optimizedMinClusterSize,
+			optimizedMaxTextureDistance,
+			optimizedMinTextureClusterSize,
+			blockRendering);
 }
 
+JNIEXPORT bool JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_postExportation(
+		JNIEnv* env, jobject, bool visualize)
+{
+	return app.postExportation(visualize);
+}
+
+JNIEXPORT bool JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_writeExportedMesh(
+		JNIEnv* env, jobject, jstring directory, jstring name)
+{
+	std::string directoryC;
+	GetJStringContent(env,directory,directoryC);
+	std::string nameC;
+	GetJStringContent(env,name,nameC);
+	return app.writeExportedMesh(directoryC, nameC);
+}
+
+
 JNIEXPORT int JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_postProcessing(
 		JNIEnv* env, jobject, int approach)

app/android/jni/point_cloud_drawable.h

@@ -40,19 +40,41 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 // PointCloudDrawable is responsible for the point cloud rendering.
 class PointCloudDrawable {
+public:
+	static void createShaderPrograms();
+	static void releaseShaderPrograms();
+
+private:
+	static std::vector<GLuint> shaderPrograms_;
+
  public:
   PointCloudDrawable(
-		  GLuint cloudShaderProgram,
-		  GLuint textureShaderProgram,
   		  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-  		  const std::vector<pcl::Vertices> & polygons = std::vector<pcl::Vertices>(),
-		  const cv::Mat & image = cv::Mat());
+		  const pcl::IndicesPtr & indices,
+		  float gainR = 1.0f,
+		  float gainG = 1.0f,
+		  float gainB = 1.0f);
+  PointCloudDrawable(
+    	  const Mesh & mesh,
+		  bool createWireframe = false);
   virtual ~PointCloudDrawable();
 
+  void updatePolygons(const std::vector<pcl::Vertices> & polygons, const std::vector<pcl::Vertices> & polygonsLowRes = std::vector<pcl::Vertices>(), bool createWireframe = false);
+  void updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices);
+  void updateMesh(const Mesh & mesh, bool createWireframe = false);
   void setPose(const rtabmap::Transform & pose);
   void setVisible(bool visible) {visible_=visible;}
-  rtabmap::Transform getPose() const {return glmToTransform(pose_);}
+  void setGains(float gainR, float gainG, float gainB) {gainR_ = gainR; gainG_ = gainG; gainB_ = gainB;}
+  rtabmap::Transform getPose() const {return pose_;}
+  const glm::mat4 & getPoseGl() const {return poseGl_;}
   bool isVisible() const {return visible_;}
+  bool hasMesh() const {return polygons_.size()!=0;}
+  bool hasTexture() const {return textures_ != 0;}
+  float getMinHeight() const {return minHeight_;}
+  const pcl::PointXYZ & aabbMinModel() const {return aabbMinModel_;}
+  const pcl::PointXYZ & aabbMaxModel() const {return aabbMaxModel_;}
+  const pcl::PointXYZ & aabbMinWorld() const {return aabbMinWorld_;}
+  const pcl::PointXYZ & aabbMaxWorld() const {return aabbMaxWorld_;}
 
   // Update current point cloud data.
   //
@@ -60,19 +82,61 @@ 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,
+		  float distanceToCamSqr = 0.0f,
+		  const GLuint & depthTexture = 0,
+		  int screenWidth = 0,     // nonnull if depthTexture>0
+		  int screenHeight = 0,    // nonnull if depthTexture>0
+		  float nearClipPlane = 0, // nonnull if depthTexture>0
+		  float farClipPlane = 0,  // nonnull if depthTexture>0
+		  bool packDepthToColorChannel = false,
+		  bool wireFrame = false) const;
+
+ private:
+  template<class PointT>
+  void updateAABBMinMax(const PointT & pt, pcl::PointXYZ & min, pcl::PointXYZ & max)
+  {
+	  if(pt.x<min.x) min.x = pt.x;
+	  if(pt.y<min.y) min.y = pt.y;
+	  if(pt.z<min.z) min.z = pt.z;
+	  if(pt.x>max.x) max.x = pt.x;
+	  if(pt.y>max.y) max.y = pt.y;
+	  if(pt.z>max.z) max.z = pt.z;
+  }
+  void updateAABBWorld(const rtabmap::Transform & pose);
 
  private:
   // Vertex buffer of the point cloud geometry.
   GLuint vertex_buffers_;
   GLuint textures_;
-  std::vector<GLushort> polygons_;
+  std::vector<GLuint> polygons_;
+  std::vector<GLuint> polygonsLowRes_;
+  std::vector<GLuint> polygonLines_;
+  std::vector<GLuint> polygonLinesLowRes_;
+  std::vector<GLuint> verticesLowRes_;
+  std::vector<GLuint> verticesLowLowRes_;
   int nPoints_;
-  glm::mat4 pose_;
+  rtabmap::Transform pose_;
+  glm::mat4 poseGl_;
   bool visible_;
+  bool hasNormals_;
+  std::vector<unsigned int> organizedToDenseIndices_;
+  float minHeight_; // odom frame
 
-  GLuint cloud_shader_program_;
-  GLuint texture_shader_program_;
+  float gainR_;
+  float gainG_;
+  float gainB_;
+
+  pcl::PointXYZ aabbMinModel_;
+  pcl::PointXYZ aabbMaxModel_;
+  pcl::PointXYZ aabbMinWorld_;
+  pcl::PointXYZ aabbMaxWorld_;
 };
 
 #endif  // TANGO_POINT_CLOUD_POINT_CLOUD_DRAWABLE_H_

app/android/jni/scene.cpp

@@ -16,10 +16,18 @@
 
 #include <tango-gl/conversions.h>
 #include <tango-gl/gesture_camera.h>
+#include <tango-gl/util.h>
 
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/core/util3d_surface.h>
+#include <pcl/common/transforms.h>
+#include <pcl/common/common.h>
+
+#include <glm/gtx/transform.hpp>
 
 #include "scene.h"
 #include "util.h"
@@ -28,7 +36,7 @@
 // add an offset in z to our origin. We'll set this offset to 1.3 meters based
 // on the average height of a human standing with a Tango device. This allows us
 // to place a grid roughly on the ground for most users.
-const glm::vec3 kHeightOffset = glm::vec3(0.0f, 1.3f, 0.0f);
+const glm::vec3 kHeightOffset = glm::vec3(0.0f, -1.3f, 0.0f);
 
 // Color of the motion tracking trajectory.
 const tango_gl::Color kTraceColor(0.66f, 0.66f, 0.66f);
@@ -39,47 +47,6 @@ const tango_gl::Color kGridColor(0.85f, 0.85f, 0.85f);
 // Frustum scale.
 const glm::vec3 kFrustumScale = glm::vec3(0.4f, 0.3f, 0.5f);
 
-const std::string kPointCloudVertexShader =
-    "precision mediump float;\n"
-    "precision mediump int;\n"
-    "attribute vec3 vertex;\n"
-    "attribute vec3 color;\n"
-    "uniform mat4 mvp;\n"
-    "uniform float point_size;\n"
-    "varying vec3 v_color;\n"
-    "void main() {\n"
-    "  gl_Position = mvp*vec4(vertex.x, vertex.y, vertex.z, 1.0);\n"
-    "  gl_PointSize = point_size;\n"
-    "  v_color = color;\n"
-    "}\n";
-const std::string kPointCloudFragmentShader =
-    "precision mediump float;\n"
-    "precision mediump int;\n"
-    "varying vec3 v_color;\n"
-    "void main() {\n"
-    "  gl_FragColor = vec4(v_color.z, v_color.y, v_color.x, 1.0);\n"
-    "}\n";
-
-const std::string kTextureMeshVertexShader =
-    "precision mediump float;\n"
-    "precision mediump int;\n"
-    "attribute vec3 vertex;\n"
-    "attribute vec2 a_TexCoordinate;\n"
-    "uniform mat4 mvp;\n"
-    "varying vec2 v_TexCoordinate;\n"
-    "void main() {\n"
-    "  gl_Position = mvp*vec4(vertex.x, vertex.y, vertex.z, 1.0);\n"
-    "  v_TexCoordinate = a_TexCoordinate;\n"
-    "}\n";
-const std::string kTextureMeshFragmentShader =
-    "precision mediump float;\n"
-    "precision mediump int;\n"
-	"uniform sampler2D u_Texture;\n"
-    "varying vec2 v_TexCoordinate;\n"
-    "void main() {\n"
-    "  gl_FragColor = texture2D(u_Texture, v_TexCoordinate);\n"
-    "}\n";
-
 const std::string kGraphVertexShader =
     "precision mediump float;\n"
     "precision mediump int;\n"
@@ -106,91 +73,109 @@ Scene::Scene() :
 		axis_(0),
 		frustum_(0),
 		grid_(0),
+		box_(0),
 		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),
+		blending_(true),
 		mapRendering_(true),
 		meshRendering_(true),
-		pointSize_(3.0f) {}
+		meshRenderingTexture_(true),
+		pointSize_(5.0f),
+		boundingBoxRendering_(false),
+		lighting_(false),
+		backfaceCulling_(true),
+		wireFrame_(false),
+		r_(0.0f),
+		g_(0.0f),
+		b_(0.0f),
+		fboId_(0),
+		depthTexture_(0),
+		screenWidth_(0),
+		screenHeight_(0),
+		doubleTapOn_(false)
+{
+	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);
-
-  gesture_camera_ = new tango_gl::GestureCamera();
-  axis_ = new tango_gl::Axis();
-  frustum_ = new tango_gl::Frustum();
-  trace_ = new tango_gl::Trace();
-  grid_ = new tango_gl::Grid();
-  currentPose_ = new rtabmap::Transform();
+	UASSERT(axis_ == 0);
 
 
-  axis_->SetScale(glm::vec3(0.5f,0.5f,0.5f));
-  frustum_->SetColor(kTraceColor);
-  trace_->ClearVertexArray();
-  trace_->SetColor(kTraceColor);
-  grid_->SetColor(kGridColor);
-  grid_->SetPosition(-kHeightOffset);
-  gesture_camera_->SetCameraType(
-      tango_gl::GestureCamera::kFirstPerson);
+	axis_ = new tango_gl::Axis();
+	frustum_ = new tango_gl::Frustum();
+	trace_ = new tango_gl::Trace();
+	grid_ = new tango_gl::Grid();
+	box_ = new BoundingBoxDrawable();
+	currentPose_ = new rtabmap::Transform();
 
-  if(cloud_shader_program_ == 0)
-  {
-	  cloud_shader_program_ = tango_gl::util::CreateProgram(kPointCloudVertexShader.c_str(), kPointCloudFragmentShader.c_str());
-	  UASSERT(cloud_shader_program_ != 0);
-  }
-  if(texture_mesh_shader_program_ == 0)
-  {
-	  texture_mesh_shader_program_ = tango_gl::util::CreateProgram(kTextureMeshVertexShader.c_str(), kTextureMeshFragmentShader.c_str());
-	  UASSERT(texture_mesh_shader_program_ != 0);
-  }
-  if(graph_shader_program_ == 0)
-  {
-	  graph_shader_program_ = tango_gl::util::CreateProgram(kGraphVertexShader.c_str(), kGraphFragmentShader.c_str());
-	  UASSERT(graph_shader_program_ != 0);
-  }
+
+	axis_->SetScale(glm::vec3(0.5f,0.5f,0.5f));
+	frustum_->SetColor(kTraceColor);
+	trace_->ClearVertexArray();
+	trace_->SetColor(kTraceColor);
+	grid_->SetColor(kGridColor);
+	grid_->SetPosition(kHeightOffset);
+	box_->SetShader();
+	box_->SetColor(1,0,0);
+
+	PointCloudDrawable::createShaderPrograms();
+
+	if(graph_shader_program_ == 0)
+	{
+		graph_shader_program_ = tango_gl::util::CreateProgram(kGraphVertexShader.c_str(), kGraphFragmentShader.c_str());
+		UASSERT(graph_shader_program_ != 0);
+	}
 }
 
 //Should only be called in OpenGL thread!
 void Scene::DeleteResources() {
 
 	LOGI("Scene::DeleteResources()");
-	if(gesture_camera_)
+	if(axis_)
 	{
-	  delete gesture_camera_;
-	  delete axis_;
-	  delete frustum_;
-	  delete trace_;
-	  delete grid_;
-	  delete currentPose_;
-	  gesture_camera_ = 0;
+		delete axis_;
+		axis_ = 0;
+		delete frustum_;
+		delete trace_;
+		delete grid_;
+		delete currentPose_;
+		delete box_;
 	}
 
-	if (cloud_shader_program_) {
-		glDeleteShader(cloud_shader_program_);
-		cloud_shader_program_ = 0;
-	  }
-	if (texture_mesh_shader_program_) {
-		glDeleteShader(texture_mesh_shader_program_);
-		texture_mesh_shader_program_ = 0;
-	  }
+	PointCloudDrawable::releaseShaderPrograms();
+
 	if (graph_shader_program_) {
 		glDeleteShader(graph_shader_program_);
 		graph_shader_program_ = 0;
-	  }
+	}
+
+	if(fboId_>0)
+	{
+		glDeleteFramebuffers(1, &fboId_);
+		fboId_ = 0;
+		glDeleteTextures(1, &depthTexture_);
+		depthTexture_ = 0;
+	}
 
 	clear();
 }
@@ -213,142 +198,360 @@ void Scene::clear()
 		graph_ = 0;
 	}
 	pointClouds_.clear();
+	if(grid_)
+	{
+		grid_->SetPosition(kHeightOffset);
+	}
 }
 
 //Should only be called in OpenGL thread!
 void Scene::SetupViewPort(int w, int h) {
-  if (h == 0) {
-    LOGE("Setup graphic height not valid");
+	if (h == 0) {
+		LOGE("Setup graphic height not valid");
+	}
+	UASSERT(gesture_camera_ != 0);
+	gesture_camera_->SetWindowSize(static_cast<float>(w), static_cast<float>(h));
+	glViewport(0, 0, w, h);
+	if(screenWidth_ != w || fboId_ == 0)
+	{
+		if(fboId_>0)
+		{
+			glDeleteFramebuffers(1, &fboId_);
+			fboId_ = 0;
+			glDeleteTextures(1, &depthTexture_);
+			depthTexture_ = 0;
+		}
+
+		// Create depth texture
+		glGenTextures(1, &depthTexture_);
+		glBindTexture(GL_TEXTURE_2D, depthTexture_);
+		glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+		glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+		glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+		glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+		glTexImage2D(GL_TEXTURE_2D, 0, GL_DEPTH_COMPONENT, w, h, 0, GL_DEPTH_COMPONENT, GL_UNSIGNED_INT, NULL);
+		glBindTexture(GL_TEXTURE_2D, 0);
+
+		// regenerate fbo texture
+		// create a framebuffer object, you need to delete them when program exits.
+		glGenFramebuffers(1, &fboId_);
+		glBindFramebuffer(GL_FRAMEBUFFER, fboId_);
+
+		// Set the texture to be at the depth attachment point of the FBO
+		glFramebufferTexture2D(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_TEXTURE_2D, depthTexture_, 0);
+
+		GLuint status = glCheckFramebufferStatus(GL_FRAMEBUFFER);
+		if ( status != GL_FRAMEBUFFER_COMPLETE)
+		{
+			LOGE("Frame buffer cannot be generated! Status: %in", status);
+		}
+		glBindFramebuffer(GL_FRAMEBUFFER,0);
+	}
+	screenWidth_ = w;
+	screenHeight_ = h;
+}
+
+std::vector<glm::vec4> computeFrustumPlanes(const glm::mat4 & mat, bool normalize = true)
+{
+	// http://www.txutxi.com/?p=444
+	std::vector<glm::vec4> planes(6);
+
+	// Left Plane
+	// col4 + col1
+	planes[0].x = mat[0][3] + mat[0][0];
+	planes[0].y = mat[1][3] + mat[1][0];
+	planes[0].z = mat[2][3] + mat[2][0];
+	planes[0].w = mat[3][3] + mat[3][0];
+
+	// Right Plane
+	// col4 - col1
+	planes[1].x = mat[0][3] - mat[0][0];
+	planes[1].y = mat[1][3] - mat[1][0];
+	planes[1].z = mat[2][3] - mat[2][0];
+	planes[1].w = mat[3][3] - mat[3][0];
+
+	// Bottom Plane
+	// col4 + col2
+	planes[2].x = mat[0][3] + mat[0][1];
+	planes[2].y = mat[1][3] + mat[1][1];
+	planes[2].z = mat[2][3] + mat[2][1];
+	planes[2].w = mat[3][3] + mat[3][1];
+
+	// Top Plane
+	// col4 - col2
+	planes[3].x = mat[0][3] - mat[0][1];
+	planes[3].y = mat[1][3] - mat[1][1];
+	planes[3].z = mat[2][3] - mat[2][1];
+	planes[3].w = mat[3][3] - mat[3][1];
+
+	// Near Plane
+	// col4 + col3
+	planes[4].x = mat[0][3] + mat[0][2];
+	planes[4].y = mat[1][3] + mat[1][2];
+	planes[4].z = mat[2][3] + mat[2][2];
+	planes[4].w = mat[3][3] + mat[3][2];
+
+	// Far Plane
+	// col4 - col3
+	planes[5].x = mat[0][3] - mat[0][2];
+	planes[5].y = mat[1][3] - mat[1][2];
+	planes[5].z = mat[2][3] - mat[2][2];
+	planes[5].w = mat[3][3] - mat[3][2];
+
+	//if(normalize)
+	{
+		for(unsigned int i=0;i<planes.size(); ++i)
+		{
+			if(normalize)
+			{
+				float d = std::sqrt(planes[i].x * planes[i].x + planes[i].y * planes[i].y + planes[i].z * planes[i].z); // for normalizing the coordinates
+				planes[i].x/=d;
+				planes[i].y/=d;
+				planes[i].z/=d;
+				planes[i].w/=d;
+			}
+		}
+	}
+
+	return planes;
+}
+
+/**
+ * Tells whether or not b is intersecting f.
+ * http://www.txutxi.com/?p=584
+ * @param f Viewing frustum.
+ * @param b An axis aligned bounding box.
+ * @return True if b intersects f, false otherwise.
+ */
+bool intersectFrustumAABB(
+        const std::vector<glm::vec4> &planes,
+        const pcl::PointXYZ &boxMin,
+		const pcl::PointXYZ &boxMax)
+{
+  // Indexed for the 'index trick' later
+	const pcl::PointXYZ * box[] = {&boxMin, &boxMax};
+
+  // We only need to do 6 point-plane tests
+  for (unsigned int i = 0; i < planes.size(); ++i)
+  {
+    // This is the current plane
+    const glm::vec4 &p = planes[i];
+
+    // p-vertex selection (with the index trick)
+    // According to the plane normal we can know the
+    // indices of the positive vertex
+    const int px = p.x > 0.0f?1:0;
+    const int py = p.y > 0.0f?1:0;
+    const int pz = p.z > 0.0f?1:0;
+
+    // Dot product
+    // project p-vertex on plane normal
+    // (How far is p-vertex from the origin)
+    const float dp =
+        (p.x*box[px]->x) +
+        (p.y*box[py]->y) +
+        (p.z*box[pz]->z) + p.w;
+
+    // Doesn't intersect if it is behind the plane
+    if (dp < 0) {return false; }
   }
-  UASSERT(gesture_camera_ != 0);
-  gesture_camera_->SetAspectRatio(static_cast<float>(w) /
-                                  static_cast<float>(h));
-  glViewport(0, 0, w, h);
+  return true;
 }
 
 //Should only be called in OpenGL thread!
 int Scene::Render() {
 	UASSERT(gesture_camera_ != 0);
 
-  glEnable(GL_DEPTH_TEST);
-  glEnable(GL_CULL_FACE);
-
-  glClearColor(0.0f, 0.0f, 0.0f, 1.0f);
-  glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
-
-  if(!currentPose_->isNull())
-  {
-	  glm::vec3 position(currentPose_->x(), currentPose_->y(), currentPose_->z());
-	  Eigen::Quaternionf quat = currentPose_->getQuaternionf();
-	  glm::quat rotation(quat.w(), quat.x(), quat.y(), quat.z());
-
-	  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);
-	  }
-	  else
-	  {
-		// In third person or top down more, we follow the camera movement.
-		gesture_camera_->SetAnchorPosition(position, rotation);
-
-		frustum_->SetPosition(position);
-		frustum_->SetRotation(rotation);
-		// Set the frustum scale to 4:3, this doesn't necessarily match the physical
-		// camera's aspect ratio, this is just for visualization purposes.
-		frustum_->SetScale(kFrustumScale);
-		frustum_->Render(gesture_camera_->GetProjectionMatrix(),
-						 gesture_camera_->GetViewMatrix());
-
-		axis_->SetPosition(position);
-		axis_->SetRotation(rotation);
-		axis_->Render(gesture_camera_->GetProjectionMatrix(),
-					  gesture_camera_->GetViewMatrix());
-	  }
-
-	  trace_->UpdateVertexArray(position);
-	  if(traceVisible_)
-	  {
-		  trace_->Render(gesture_camera_->GetProjectionMatrix(),
-						 gesture_camera_->GetViewMatrix());
-	  }
-  }
-
-
-	grid_->Render(gesture_camera_->GetProjectionMatrix(),
-				  gesture_camera_->GetViewMatrix());
-
-	bool frustumCulling = true;
-	int cloudDrawn=0;
-	if(mapRendering_ && frustumCulling)
+	glm::vec3 position(currentPose_->x(), currentPose_->y(), currentPose_->z());
+	Eigen::Quaternionf quat = currentPose_->getQuaternionf();
+	glm::quat rotation(quat.w(), quat.x(), quat.y(), quat.z());
+	glm::mat4 rotateM;
+	if(!currentPose_->isNull())
 	{
-		//Use camera frustum to cull nodes that don't need to be drawn
-		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
-		std::vector<int> ids(pointClouds_.size());
+		rotateM = glm::rotate<float>(float(color_camera_to_display_rotation_)*-1.57079632679489661923132169163975144, glm::vec3(0.0f, 0.0f, 1.0f));
 
-		cloud->resize(pointClouds_.size());
-		ids.resize(pointClouds_.size());
-		int oi=0;
-		for(std::map<int, PointCloudDrawable*>::const_iterator iter=pointClouds_.begin(); iter!=pointClouds_.end(); ++iter)
+		if (gesture_camera_->GetCameraType() == tango_gl::GestureCamera::kFirstPerson)
 		{
-			if(!iter->second->getPose().isNull() && iter->second->isVisible())
-			{
-				(*cloud)[oi] = pcl::PointXYZ(iter->second->getPose().x(), iter->second->getPose().y(), iter->second->getPose().z());
-				ids[oi++] = iter->first;
-			}
+			// In first person mode, we directly control camera's motion.
+			gesture_camera_->SetPosition(position);
+			gesture_camera_->SetRotation(rotation*glm::quat(rotateM));
 		}
-		cloud->resize(oi);
-		ids.resize(oi);
-
-		if(oi)
+		else
 		{
-			float fov = 45.0f;
-			rtabmap::Transform openglCamera = GetOpenGLCameraPose(&fov)*rtabmap::Transform(0.0f, 0.0f, 3.0f, 0.0f, 0.0f, 0.0f);
-			// transform in same coordinate as frustum filtering
-			openglCamera *= rtabmap::Transform(
-				 0.0f,  0.0f,  1.0f, 0.0f,
-				 0.0f,  1.0f,  0.0f, 0.0f,
-				-1.0f,  0.0f,  0.0f, 0.0f);
-			pcl::IndicesPtr indices = rtabmap::util3d::frustumFiltering(
-					cloud,
-					pcl::IndicesPtr(new std::vector<int>),
-					openglCamera,
-					fov*2.0f,
-					fov*2.0f,
-					0.1f,
-					100.0f);
+			// In third person or top down mode, we follow the camera movement.
+			gesture_camera_->SetAnchorPosition(position, rotation*glm::quat(rotateM));
+		}
+	}
 
-			//LOGI("Frustum poses filtered = %d (showing %d/%d)",
-			//			(int)(pointClouds_.size()-indices->size()),
-			//			(int)indices->size(),
-			//			(int)pointClouds_.size());
+	glm::mat4 projectionMatrix = gesture_camera_->GetProjectionMatrix();
+	glm::mat4 viewMatrix = gesture_camera_->GetViewMatrix();
 
-			for(unsigned int i=0; i<indices->size(); ++i)
+	rtabmap::Transform openglCamera = GetOpenGLCameraPose();//*rtabmap::Transform(0.0f, 0.0f, 3.0f, 0.0f, 0.0f, 0.0f);
+	// transform in same coordinate as frustum filtering
+	openglCamera *= rtabmap::Transform(
+		 0.0f,  0.0f,  1.0f, 0.0f,
+		 0.0f,  1.0f,  0.0f, 0.0f,
+		-1.0f,  0.0f,  0.0f, 0.0f);
+
+	//Culling
+	std::vector<glm::vec4> planes = computeFrustumPlanes(projectionMatrix*viewMatrix, true);
+	std::vector<PointCloudDrawable*> cloudsToDraw(pointClouds_.size());
+	int oi=0;
+	for(std::map<int, PointCloudDrawable*>::const_iterator iter=pointClouds_.begin(); iter!=pointClouds_.end(); ++iter)
+	{
+		if(!mapRendering_ && iter->first > 0)
+		{
+			break;
+		}
+
+		if(iter->second->isVisible())
+		{
+			if(intersectFrustumAABB(planes,
+					iter->second->aabbMinWorld(),
+					iter->second->aabbMaxWorld()))
 			{
-				++cloudDrawn;
-				pointClouds_.find(ids[indices->at(i)])->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_);
+				cloudsToDraw[oi++] = iter->second;
 			}
 		}
 	}
+	cloudsToDraw.resize(oi);
+
+	// First rendering to get depth texture
+	glEnable(GL_DEPTH_TEST);
+	glDepthFunc(GL_LESS);
+	glDepthMask(GL_TRUE);
+	glColorMask(GL_TRUE, GL_TRUE, GL_TRUE, GL_TRUE);
+	glDisable (GL_BLEND);
+	glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+
+	if(backfaceCulling_)
+	{
+		glEnable(GL_CULL_FACE);
+	}
 	else
 	{
-		for(std::map<int, PointCloudDrawable*>::const_iterator iter=pointClouds_.begin(); iter!=pointClouds_.end(); ++iter)
+		glDisable(GL_CULL_FACE);
+	}
+
+	UTimer timer;
+
+	bool onlineBlending = blending_ && gesture_camera_->GetCameraType()!=tango_gl::GestureCamera::kTopOrtho && mapRendering_ && meshRendering_ && cloudsToDraw.size()>1;
+	if(onlineBlending && fboId_)
+	{
+		// set the rendering destination to FBO
+		glBindFramebuffer(GL_FRAMEBUFFER, fboId_);
+
+		glColorMask(GL_FALSE, GL_FALSE, GL_FALSE, GL_FALSE);
+		glClearColor(1, 1, 1, 1);
+		glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
+
+		// Draw scene
+		for(std::vector<PointCloudDrawable*>::const_iterator iter=cloudsToDraw.begin(); iter!=cloudsToDraw.end(); ++iter)
 		{
-			if((mapRendering_ || iter->first < 0) && iter->second->isVisible())
-			{
-				++cloudDrawn;
-				iter->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_);
-			}
+			// set large distance to cam to use low res polygons for fast processing
+			(*iter)->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, false, false, 999.0f);
+		}
+
+		// back to normal window-system-provided framebuffer
+		glBindFramebuffer(GL_FRAMEBUFFER, 0); // unbind
+		glColorMask(GL_TRUE, GL_TRUE, GL_TRUE, GL_TRUE);
+	}
+
+	if(doubleTapOn_ && gesture_camera_->GetCameraType() != tango_gl::GestureCamera::kFirstPerson)
+	{
+		glClearColor(0, 0, 0, 0);
+		glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
+
+		for(std::vector<PointCloudDrawable*>::const_iterator iter=cloudsToDraw.begin(); iter!=cloudsToDraw.end(); ++iter)
+		{
+			// set large distance to cam to use low res polygons for fast processing
+			(*iter)->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_*10.0f, false, false, 999.0f, 0, 0, 0, 0, 0, true);
+		}
+
+		GLubyte zValue[4];
+		glReadPixels(doubleTapPos_.x*screenWidth_, screenHeight_-doubleTapPos_.y*screenHeight_, 1, 1, GL_RGBA, GL_UNSIGNED_BYTE, zValue);
+		float fromFixed = 256.0f/255.0f;
+		float zValueF = float(zValue[0]/255.0f)*fromFixed + float(zValue[1]/255.0f)*fromFixed/255.0f + float(zValue[2]/255.0f)*fromFixed/65025.0f + float(zValue[3]/255.0f)*fromFixed/160581375.0f;
+
+		if(zValueF != 0.0f)
+		{
+			zValueF = zValueF*2.0-1.0;//NDC
+			glm::vec4 point = glm::inverse(projectionMatrix*viewMatrix)*glm::vec4(doubleTapPos_.x*2.0f-1.0f, (1.0f-doubleTapPos_.y)*2.0f-1.0f, zValueF, 1.0f);
+			point /= point.w;
+			gesture_camera_->SetAnchorOffset(glm::vec3(point.x, point.y, point.z) - position);
+		}
+	}
+	doubleTapOn_ = false;
+
+	glClearColor(r_, g_, b_, 1.0f);
+	glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
+
+	if(!currentPose_->isNull())
+	{
+		if (gesture_camera_->GetCameraType() != tango_gl::GestureCamera::kFirstPerson)
+		{
+			frustum_->SetPosition(position);
+			frustum_->SetRotation(rotation);
+			// Set the frustum scale to 4:3, this doesn't necessarily match the physical
+			// camera's aspect ratio, this is just for visualization purposes.
+			frustum_->SetScale(kFrustumScale);
+			frustum_->Render(projectionMatrix, viewMatrix);
+
+			axis_->SetPosition(position);
+			axis_->SetRotation(rotation);
+			axis_->Render(projectionMatrix, viewMatrix);
+		}
+
+		trace_->UpdateVertexArray(position);
+		if(traceVisible_)
+		{
+			trace_->Render(projectionMatrix, viewMatrix);
+		}
+
+		if(gridVisible_)
+		{
+			grid_->Render(projectionMatrix, viewMatrix);
 		}
 	}
 
 	if(graphVisible_ && graph_)
 	{
-		graph_->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix());
+		graph_->Render(projectionMatrix, viewMatrix);
 	}
 
-	return cloudDrawn;
+
+	if(onlineBlending)
+	{
+		glEnable (GL_BLEND);
+		glDepthMask(GL_FALSE);
+	}
+
+	for(std::vector<PointCloudDrawable*>::const_iterator iter=cloudsToDraw.begin(); iter!=cloudsToDraw.end(); ++iter)
+	{
+		PointCloudDrawable * cloud = *iter;
+
+		if(boundingBoxRendering_)
+		{
+			box_->updateVertices(cloud->aabbMinWorld(), cloud->aabbMaxWorld());
+			box_->Render(projectionMatrix, viewMatrix);
+		}
+
+		Eigen::Vector3f cloudToCamera(
+				cloud->getPose().x() - openglCamera.x(),
+				cloud->getPose().y() - openglCamera.y(),
+				cloud->getPose().z() - openglCamera.z());
+		float distanceToCameraSqr = cloudToCamera[0]*cloudToCamera[0] + cloudToCamera[1]*cloudToCamera[1] + cloudToCamera[2]*cloudToCamera[2];
+
+		cloud->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, meshRenderingTexture_, lighting_, distanceToCameraSqr, onlineBlending?depthTexture_:0, screenWidth_, screenHeight_, gesture_camera_->getNearClipPlane(), gesture_camera_->getFarClipPlane(), false, wireFrame_);
+	}
+
+	if(onlineBlending)
+	{
+		glDisable (GL_BLEND);
+		glDepthMask(GL_TRUE);
+	}
+
+	return (int)cloudsToDraw.size();
 }
 
 void Scene::SetCameraType(tango_gl::GestureCamera::CameraType camera_type) {
@@ -362,6 +565,24 @@ void Scene::SetCameraPose(const rtabmap::Transform & pose)
 	*currentPose_ = pose;
 }
 
+void Scene::setFOV(float angle)
+{
+	gesture_camera_->SetFieldOfView(angle);
+}
+void Scene::setOrthoCropFactor(float value)
+{
+	gesture_camera_->SetOrthoCropFactor(value);
+}
+void Scene::setGridRotation(float angleDeg)
+{
+	float angleRad = angleDeg * DEGREE_2_RADIANS;
+	if(grid_)
+	{
+		glm::quat rot = glm::rotate(glm::quat(1,0,0,0), angleRad, glm::vec3(0, 1, 0));
+		grid_->SetRotation(rot);
+	}
+}
+
 rtabmap::Transform Scene::GetOpenGLCameraPose(float * fov) const
 {
 	if(fov)
@@ -369,14 +590,27 @@ rtabmap::Transform Scene::GetOpenGLCameraPose(float * fov) const
 		*fov = gesture_camera_->getFOV();
 	}
 	return glmToTransform(gesture_camera_->GetTransformationMatrix());
-
 }
 
 void Scene::OnTouchEvent(int touch_count,
                          tango_gl::GestureCamera::TouchEvent event, float x0,
                          float y0, float x1, float y1) {
 	UASSERT(gesture_camera_ != 0);
-  gesture_camera_->OnTouchEvent(touch_count, event, x0, y0, x1, y1);
+	if(touch_count == 3)
+	{
+		//doubletap
+		if(!doubleTapOn_)
+		{
+			doubleTapPos_.x = x0;
+			doubleTapPos_.y = y0;
+			doubleTapOn_ = true;
+		}
+	}
+	else
+	{
+		// rotate/translate/zoom
+		gesture_camera_->OnTouchEvent(touch_count, event, x0, y0, x1, y1);
+	}
 }
 
 void Scene::updateGraph(
@@ -391,8 +625,11 @@ void Scene::updateGraph(
 	}
 
 	//create
-	UASSERT(graph_shader_program_ != 0);
-	graph_ = new GraphDrawable(graph_shader_program_, poses, links);
+	if(graphVisible_)
+	{
+		UASSERT(graph_shader_program_ != 0);
+		graph_ = new GraphDrawable(graph_shader_program_, poses, links);
+	}
 }
 
 void Scene::setGraphVisible(bool visible)
@@ -400,6 +637,11 @@ void Scene::setGraphVisible(bool visible)
 	graphVisible_ = visible;
 }
 
+void Scene::setGridVisible(bool visible)
+{
+	gridVisible_ = visible;
+}
+
 void Scene::setTraceVisible(bool visible)
 {
 	traceVisible_ = visible;
@@ -409,11 +651,10 @@ void Scene::setTraceVisible(bool visible)
 void Scene::addCloud(
 		int id,
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		const std::vector<pcl::Vertices> & polygons,
-		const rtabmap::Transform & pose,
-		const cv::Mat & image)
+		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())
 	{
@@ -422,17 +663,83 @@ void Scene::addCloud(
 	}
 
 	//create
-	UASSERT(cloud_shader_program_ != 0 && texture_mesh_shader_program_!=0);
-	PointCloudDrawable * drawable = new PointCloudDrawable(
-			cloud_shader_program_,
-			texture_mesh_shader_program_,
-			cloud,
-			polygons,
-			image);
+	PointCloudDrawable * drawable = new PointCloudDrawable(cloud, indices);
 	drawable->setPose(pose);
 	pointClouds_.insert(std::make_pair(id, drawable));
 }
 
+void Scene::addMesh(
+		int id,
+		const Mesh & mesh,
+		const rtabmap::Transform & pose,
+		bool createWireframe)
+{
+	LOGI("add mesh %d", id);
+	std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
+	if(iter != pointClouds_.end())
+	{
+		delete iter->second;
+		pointClouds_.erase(iter);
+	}
+
+	//create
+	PointCloudDrawable * drawable = new PointCloudDrawable(mesh, createWireframe);
+	drawable->setPose(pose);
+	pointClouds_.insert(std::make_pair(id, drawable));
+
+	if(!mesh.pose.isNull() && mesh.cloud->size() && (!mesh.cloud->isOrganized() || mesh.indices->size()))
+	{
+		UTimer time;
+		float height = 0.0f;
+		Eigen::Affine3f affinePose = mesh.pose.toEigen3f();
+		if(mesh.polygons.size())
+		{
+			for(unsigned int i=0; i<mesh.polygons.size(); ++i)
+			{
+				for(unsigned int j=0; j<mesh.polygons[i].vertices.size(); ++j)
+				{
+					pcl::PointXYZRGB pt = pcl::transformPoint(mesh.cloud->at(mesh.polygons[i].vertices[j]), affinePose);
+					if(pt.z < height)
+					{
+						height = pt.z;
+					}
+				}
+			}
+		}
+		else
+		{
+			if(mesh.cloud->isOrganized())
+			{
+				for(unsigned int i=0; i<mesh.indices->size(); ++i)
+				{
+					pcl::PointXYZRGB pt = pcl::transformPoint(mesh.cloud->at(mesh.indices->at(i)), affinePose);
+					if(pt.z < height)
+					{
+						height = pt.z;
+					}
+				}
+			}
+			else
+			{
+				for(unsigned int i=0; i<mesh.cloud->size(); ++i)
+				{
+					pcl::PointXYZRGB pt = pcl::transformPoint(mesh.cloud->at(i), affinePose);
+					if(pt.z < height)
+					{
+						height = pt.z;
+					}
+				}
+			}
+		}
+
+		if(grid_->GetPosition().y == kHeightOffset.y || grid_->GetPosition().y > height)
+		{
+			grid_->SetPosition(glm::vec3(0,height,0));
+		}
+		LOGD("compute min height %f s", time.ticks());
+	}
+}
+
 
 void Scene::setCloudPose(int id, const rtabmap::Transform & pose)
 {
@@ -458,7 +765,52 @@ 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::updateGains(int id, float gainR, float gainG, float gainB)
+{
+	std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
+	if(iter != pointClouds_.end())
+	{
+		iter->second->setGains(gainR, gainG, gainB);
+	}
+}
+
+void Scene::setGridColor(float r, float g, float b)
+{
+	if(grid_)
+	{
+		grid_->SetColor(r, g, b);
+	}
+}

app/android/jni/scene.h

@@ -37,6 +37,7 @@
 
 #include <point_cloud_drawable.h>
 #include <graph_drawable.h>
+#include <bounding_box_drawable.h>
 
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
@@ -56,6 +57,10 @@ class Scene {
 
   // Setup GL view port.
   void SetupViewPort(int w, int h);
+  int getViewPortWidth() const {return screenWidth_;}
+  int getViewPortHeight() const {return screenHeight_;}
+
+  void setScreenRotation(TangoSupportRotation colorCameraToDisplayRotation) {color_camera_to_display_rotation_ = colorCameraToDisplayRotation;}
 
   void clear(); // removed all point clouds
 
@@ -75,7 +80,7 @@ class Scene {
 
   void SetCameraPose(const rtabmap::Transform & pose);
   rtabmap::Transform GetCameraPose() const {return currentPose_!=0?*currentPose_:rtabmap::Transform();}
-  rtabmap::Transform GetOpenGLCameraPose(float * fov) const;
+  rtabmap::Transform GetOpenGLCameraPose(float * fov = 0) const;
 
   // Touch event passed from android activity. This function only support two
   // touches.
@@ -94,23 +99,50 @@ class Scene {
   		const std::multimap<int, rtabmap::Link> & links);
 
   void setGraphVisible(bool visible);
+  void setGridVisible(bool visible);
   void setTraceVisible(bool visible);
 
   void addCloud(
   		  int id,
   		  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-  		  const std::vector<pcl::Vertices> & polygons,
-  		  const rtabmap::Transform & pose,
-		  const cv::Mat & image = cv::Mat());
+		  const pcl::IndicesPtr & indices,
+  		  const rtabmap::Transform & pose);
+  void addMesh(
+  		int id,
+  		const Mesh & mesh,
+  		const rtabmap::Transform & pose,
+		bool createWireframe = false);
 
   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 updateGains(int id, float gainR, float gainG, float gainB);
 
+  void setBlending(bool enabled) {blending_ = enabled;}
   void setMapRendering(bool enabled) {mapRendering_ = enabled;}
-  void setMeshRendering(bool enabled) {meshRendering_ = enabled;}
+  void setMeshRendering(bool enabled, bool withTexture) {meshRendering_ = enabled; meshRenderingTexture_ = withTexture;}
   void setPointSize(float size) {pointSize_ = size;}
+  void setFOV(float angle);
+  void setOrthoCropFactor(float value);
+  void setGridRotation(float angleDeg);
+  void setLighting(bool enabled) {lighting_ = enabled;}
+  void setBackfaceCulling(bool enabled) {backfaceCulling_ = enabled;}
+  void setWireframe(bool enabled) {wireFrame_ = enabled;}
+  void setBackgroundColor(float r, float g, float b) {r_=r; g_=g; b_=b;} // 0.0f <> 1.0f
+  void setGridColor(float r, float g, float b);
+
+  bool isBlending() const {return blending_;}
+  bool isMapRendering() const {return mapRendering_;}
+  bool isMeshRendering() const {return meshRendering_;}
+  bool isMeshTexturing() const {return meshRendering_ && meshRenderingTexture_;}
+  float getPointSize() const {return pointSize_;}
+  bool isLighting() const {return lighting_;}
+  bool isBackfaceCulling() const {return backfaceCulling_;}
 
  private:
   // Camera object that allows user to use touch input to interact with.
@@ -125,24 +157,43 @@ class Scene {
   // Ground grid.
   tango_gl::Grid* grid_;
 
+  // Bounding box
+  BoundingBoxDrawable * box_;
+
   // Trace of pose data.
   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 blending_;
   bool mapRendering_;
   bool meshRendering_;
+  bool meshRenderingTexture_;
   float pointSize_;
+  bool boundingBoxRendering_;
+  bool lighting_;
+  bool backfaceCulling_;
+  bool wireFrame_;
+  float r_;
+  float g_;
+  float b_;
+  GLuint fboId_;
+  GLuint depthTexture_;
+  GLsizei screenWidth_;
+  GLsizei screenHeight_;
+  bool doubleTapOn_;
+  cv::Point2f doubleTapPos_;
 };
 
 #endif  // TANGO_POINT_CLOUD_SCENE_H_

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

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

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

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

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

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

@@ -29,7 +29,8 @@ class GestureCamera : public Camera {
     kFirstPerson = 0,
     kThirdPersonFollow = 1,
     kTopDown = 2,
-    kThirdPerson = 3
+	kTopOrtho = 3,
+    kThirdPerson = 4
   };
 
   enum TouchEvent {
@@ -55,6 +56,11 @@ class GestureCamera : public Camera {
                               float touch_range);
 
   void SetAnchorPosition(const glm::vec3& pos, const glm::quat & rotation);
+  void SetAnchorOffset(const glm::vec3& pos) {anchor_offset_ = pos;}
+  const glm::vec3& GetAnchorOffset() const {return anchor_offset_;}
+
+  void SetCameraDistance(float cameraDistance) {cam_cur_dist_ = cameraDistance;}
+  float GetCameraDistance() const {return cam_cur_dist_;}
 
   // Set camera type, set render camera's parent position and rotation.
   void SetCameraType(CameraType camera_index);
@@ -75,7 +81,9 @@ class GestureCamera : public Camera {
   glm::quat cam_cur_target_rot_;
 
   float cam_start_dist_;
+  float cam_start_fov_;
   float cam_cur_dist_;
+  glm::vec3 anchor_offset_;
 
   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"
@@ -33,9 +34,15 @@
 #include "glm/gtx/matrix_decompose.hpp"
 
 #define LOG_TAG "rtabmap"
+#ifdef DISABLE_LOG
+#define LOGD(...) ;
+#define LOGI(...) ;
+#define LOGW(...) ;
+#else
 #define LOGD(...) __android_log_print(ANDROID_LOG_DEBUG,LOG_TAG,__VA_ARGS__)
 #define LOGI(...) __android_log_print(ANDROID_LOG_INFO,LOG_TAG,__VA_ARGS__)
 #define LOGW(...) __android_log_print(ANDROID_LOG_WARN,LOG_TAG,__VA_ARGS__)
+#endif
 #define LOGE(...) __android_log_print(ANDROID_LOG_ERROR,LOG_TAG,__VA_ARGS__)
 
 #ifndef M_PI
@@ -76,6 +83,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/third-party/lib/.gitignore

@@ -0,0 +1 @@
+ 

app/android/jni/util.h

@@ -32,17 +32,26 @@ 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::kDebug);
+		ULogger::setEventLevel(ULogger::kDebug);
+		ULogger::setPrintThreadId(true);
+
 		registerToEventsManager();
 	}
 protected:
-	virtual void handleEvent(UEvent * event)
+	virtual bool handleEvent(UEvent * event)
 	{
 		if(event->getClassName().compare("ULogEvent") == 0)
 		{
@@ -65,25 +74,31 @@ protected:
 			}
 
 		}
+		return false;
 	}
 };
 
 static const rtabmap::Transform opengl_world_T_tango_world(
-		1.0f, 0.0f,  0.0f, 0.0f,
-		0.0f, 0.0f, 1.0f, 0.0f,
+		1.0f,  0.0f,  0.0f, 0.0f,
+		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(
-		1.0f, 0.0f, 0.0f, 0.0f,
-		0.0f, -1.0f, 0.0f, 0.0f,
-		0.0f, 0.0f, -1.0f, 0.0f);
+static const rtabmap::Transform rtabmap_world_T_tango_world(
+		 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,
+		-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);
+		 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 +144,33 @@ 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)
+	{
+		gains[0] = gains[1] = gains[2] = 1.0f;
+	}
+
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud; // organized cloud
+	pcl::PointCloud<pcl::Normal>::Ptr normals;
+	pcl::IndicesPtr indices;
+	std::vector<pcl::Vertices> polygons;
+	std::vector<pcl::Vertices> polygonsLowRes;
+	rtabmap::Transform pose; // in rtabmap coordinates
+	bool visible;
+	rtabmap::CameraModel cameraModel;
+	double gains[3]; // RGB gains
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+    	std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > texCoords;
+#else
+    	std::vector<Eigen::Vector2f> texCoords;
+#endif
+	cv::Mat texture;
+};
+
 #endif /* UTIL_H_ */

app/android/res/drawable/custom_seekbar.xml

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

app/android/res/layout/activity_rtabmap.xml

@@ -15,244 +15,130 @@
 -->
 <RelativeLayout xmlns:android="http://schemas.android.com/apk/res/android"
     android:layout_width="wrap_content"
-    android:layout_height="wrap_content" >
+    android:layout_height="wrap_content">
 
     <android.opengl.GLSurfaceView
         android:id="@+id/gl_surface_view"
         android:layout_width="fill_parent"
         android:layout_height="fill_parent"
-        android:layout_gravity="top" />
-
-   <LinearLayout
-        android:id="@+id/debug_layout"
+        android:layout_gravity="top"/>
+    
+    <RelativeLayout 
         android:layout_width="wrap_content"
-        android:layout_height="wrap_content"
-        android:layout_alignParentLeft="true"
-        android:layout_alignParentTop="true"
-        android:orientation="vertical"
-        android:paddingLeft="5dp" >
-
-        <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/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_height="match_parent"
+        android:fitsSystemWindows="true">
+    
+    <ToggleButton
+        android:id="@+id/wireframe_button"
         android:layout_width="100dp"
         android:layout_height="wrap_content"
-        android:layout_above="@+id/third_person_button"
-        android:layout_alignLeft="@+id/third_person_button"
+        android:layout_above="@+id/light_button"
+        android:layout_alignLeft="@+id/light_button"
         android:layout_alignParentRight="true"
         android:layout_marginBottom="5dp"
         android:layout_marginRight="5dp"
         android:paddingRight="5dp"
-        android:text="@string/first_person" />
-
-    <Button
-        android:id="@+id/top_down_button"
+        android:textOff="@string/wireframe"
+        android:textOn="@string/wireframe" />
+    
+    <ToggleButton
+        android:id="@+id/light_button"
         android:layout_width="100dp"
         android:layout_height="wrap_content"
+        android:layout_above="@+id/backface_button"
+        android:layout_alignLeft="@+id/backface_button"
+        android:layout_alignParentRight="true"
+        android:layout_marginBottom="5dp"
+        android:layout_marginRight="5dp"
+        android:paddingRight="5dp"
+        android:textOff="@string/light_off"
+        android:textOn="@string/light_on" />
+    
+    <ToggleButton
+        android:id="@+id/backface_button"
+        android:layout_width="100dp"
+        android:layout_height="wrap_content"
+        android:layout_above="@+id/camera_button"
+        android:layout_alignRight="@+id/camera_button"
+        android:layout_alignParentRight="true"
+        android:layout_marginBottom="15dp"
+        android:layout_marginRight="5dp"
+        android:paddingRight="5dp"
+        android:textOff="@string/backface_off"
+        android:textOn="@string/backface_on" />
+ 
+    <com.introlab.rtabmap.NDSpinner
+        android:id="@+id/camera_button"
+        android:layout_width="140dp"
+        android:layout_height="40dp"
         android:layout_alignParentBottom="true"
         android:layout_alignParentRight="true"
         android:layout_marginRight="5dp"
+        android:layout_marginBottom="10dp"
         android:paddingRight="5dp"
-        android:text="@string/top_down" />
+        android:paddingBottom="10dp"
+        android:text="@string/camera_button"
+        android:spinnerMode="dropdown"/>
 
-    <Button
-        android:id="@+id/third_person_button"
+    <ToggleButton
+        android:id="@+id/pause_button"
         android:layout_width="100dp"
         android:layout_height="wrap_content"
-        android:layout_above="@+id/top_down_button"
+		android:layout_marginRight="5dp"
+        android:layout_alignParentTop="true"
+        android:layout_marginTop="10dp"
         android:layout_alignParentRight="true"
-        android:layout_marginBottom="5dp"
-        android:layout_marginRight="5dp"
-        android:paddingRight="5dp"
-        android:text="@string/third_person" />
+        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_marginRight="5dp"
+        android:layout_marginTop="10dp"
+        android:layout_alignParentRight="true"
+        android:layout_below="@+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_alignRight="@+id/button_shareToSketchfab"
+        android:layout_below="@+id/button_shareToSketchfab"
+        android:text="@string/save_to_file" />
+
+    <Button
+        android:id="@+id/close_visualization_button"
+        android:layout_width="200dp"
+        android:layout_height="wrap_content"
+        android:layout_above="@+id/camera_button"
+        android:layout_centerHorizontal="true"
+        android:text="@string/close_visualization" />
+
+    <SeekBar
+        android:id="@+id/seekBar_ortho_cut"
+        android:layout_width="200dp"
+        android:layout_height="wrap_content"
+        android:rotation="270"
+        android:layout_above="@+id/light_button"
+        android:layout_alignParentLeft="true"
+        android:layout_gravity="center"
+        android:layout_marginLeft="-50dp"
+        android:progressDrawable="@drawable/custom_seekbar" />
+    
+    <SeekBar
+        android:id="@+id/seekBar_grid"
+        android:layout_width="200dp"
+        android:layout_height="wrap_content"
+        android:layout_alignParentLeft="true"
+        android:layout_alignBottom="@+id/camera_button"
+        android:layout_marginLeft="35dp"
+        android:paddingBottom="10dp"
+        android:progressDrawable="@drawable/custom_seekbar" />
+
+
+
+</RelativeLayout>        
 </RelativeLayout>

app/android/res/layout/activity_settings.xml

@@ -0,0 +1,348 @@
+<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_min_depth"
+    		android:title="@string/pref_title_min_depth"
+    		android:summary="@string/pref_summary_min_depth"
+   			android:entries="@array/pref_min_depth_keys"
+   			android:entryValues="@array/pref_min_depth_values"
+   			android:defaultValue="@string/pref_default_min_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"/>
+        <ListPreference 
+            android:key="@string/pref_key_rendering_texture_decimation"
+    		android:title="@string/pref_title_rendering_texture_decimation"
+    		android:summary="@string/pref_summary_rendering_texture_decimation"
+   			android:entries="@array/pref_rendering_texture_decimation_keys"
+   			android:entryValues="@array/pref_rendering_texture_decimation_values"
+   			android:defaultValue="@string/pref_default_rendering_texture_decimation"/>
+        <ListPreference 
+            android:key="@string/pref_key_background_color"
+    		android:title="@string/pref_title_background_color"
+    		android:summary="@string/pref_summary_background_color"
+   			android:entries="@array/pref_background_color_keys"
+   			android:entryValues="@array/pref_background_color_values"
+   			android:defaultValue="@string/pref_default_background_color"/>
+        <com.introlab.rtabmap.CustomSwitchPreference
+            android:key="@string/pref_key_blending"
+            android:title="@string/pref_title_blending"
+            android:summary="@string/pref_summary_blending"
+            android:defaultValue="@string/pref_default_blending"/>
+        <com.introlab.rtabmap.CustomSwitchPreference
+            android:key="@string/pref_key_nodes_filtering"
+            android:title="@string/pref_title_nodes_filtering"
+            android:summary="@string/pref_summary_nodes_filtering"
+            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">
+        
+		    <com.introlab.rtabmap.CustomSwitchPreference
+	            android:key="@string/pref_key_append"
+	            android:title="@string/pref_title_append"
+	            android:summary="@string/pref_summary_append"
+	            android:defaultValue="@string/pref_default_append"/>
+		    <com.introlab.rtabmap.CustomSwitchPreference
+	            android:key="@string/pref_key_resolution"
+	            android:title="@string/pref_title_resolution"
+	            android:summary="@string/pref_summary_resolution"
+	            android:defaultValue="@string/pref_default_resolution"/>
+		    <com.introlab.rtabmap.CustomSwitchPreference
+		        android:key="@string/pref_key_smoothing"
+		        android:title="@string/pref_title_smoothing"
+		        android:summary="@string/pref_summary_smoothing"
+		        android:defaultValue="@string/pref_default_smoothing"/>
+		    <com.introlab.rtabmap.CustomSwitchPreference
+		        android:key="@string/pref_key_fisheye"
+		        android:title="@string/pref_title_fisheye"
+		        android:summary="@string/pref_summary_fisheye"
+		        android:defaultValue="@string/pref_default_fisheye"/>
+	  
+		     <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_max_speed"
+		            android:title="@string/pref_title_max_speed"
+		            android:summary="@string/pref_summary_max_speed"
+		            android:entries="@array/pref_max_speed_keys"
+		   			android:entryValues="@array/pref_max_speed_values"
+		   			android:defaultValue="@string/pref_default_max_speed"/>
+		        <ListPreference 
+		            android:key="@string/pref_key_time_thr"
+		    		android:title="@string/pref_title_time_thr"
+		    		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"/>
+		        <com.introlab.rtabmap.CustomSwitchPreference 
+		            android:key="@string/pref_key_optimize_end"
+		    		android:title="@string/pref_title_optimize_end"
+		    		android:summary="@string/pref_summary_optimize_end"
+		   			android:defaultValue="@string/pref_default_optimize_end"/>
+		    </PreferenceCategory>
+	        <PreferenceCategory
+	        	android:title="@string/pref_title_mapping_database">
+		        <com.introlab.rtabmap.CustomSwitchPreference
+		            android:key="@string/pref_key_keep_all_db"
+		            android:title="@string/pref_title_keep_all_db"
+		            android:summary="@string/pref_summary_keep_all_db"
+		            android:defaultValue="@string/pref_default_keep_all_db"/>
+		        <com.introlab.rtabmap.CustomSwitchPreference
+		            android:key="@string/pref_key_raw_scan_saved"
+		            android:title="@string/pref_title_raw_scan_saved"
+		            android:summary="@string/pref_summary_raw_scan_saved"
+		            android:defaultValue="@string/pref_default_raw_scan_saved"/>
+		        <com.introlab.rtabmap.CustomSwitchPreference
+		            android:key="@string/pref_key_gps_saved"
+		            android:title="@string/pref_title_gps_saved"
+		            android:summary="@string/pref_summary_gps_saved"
+		            android:defaultValue="@string/pref_default_gps_saved"/>
+		        <com.introlab.rtabmap.CustomSwitchPreference
+		            android:key="@string/pref_key_db_in_memory"
+		            android:title="@string/pref_title_db_in_memory"
+		            android:summary="@string/pref_summary_db_in_memory"
+		            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_texture_count"
+	    		android:title="@string/pref_title_texture_count"
+	    		android:summary="@string/pref_summary_texture_count"
+	   			android:entries="@array/pref_texture_count_keys"
+	   			android:entryValues="@array/pref_texture_count_values"
+	   			android:defaultValue="@string/pref_default_texture_count"/>
+	        
+	        <ListPreference 
+	            android:key="@string/pref_key_normal_k"
+	    		android:title="@string/pref_title_normal_k"
+	    		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"/>
+	        
+	        <ListPreference 
+	            android:key="@string/pref_key_min_texture_cluster_size"
+	    		android:title="@string/pref_title_min_texture_cluster_size"
+	    		android:summary="@string/pref_summary_min_texture_cluster_size"
+	   			android:entries="@array/pref_min_texture_cluster_size_keys"
+	   			android:entryValues="@array/pref_min_texture_cluster_size_values"
+	   			android:defaultValue="@string/pref_default_min_texture_cluster_size"/>
+	        
+		    <com.introlab.rtabmap.CustomSwitchPreference
+	            android:key="@string/pref_key_block_render"
+	            android:title="@string/pref_title_block_render"
+	            android:summary="@string/pref_summary_block_render"
+	            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_keys"
+		   			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"/>
+		        
+		        <com.introlab.rtabmap.CustomSwitchPreference
+		            android:key="@string/pref_key_opt_clean_white"
+		            android:title="@string/pref_title_opt_clean_white"
+		            android:summary="@string/pref_summary_opt_clean_white"
+		            android:defaultValue="@string/pref_default_opt_clean_white"/>
+		        
+		        <ListPreference 
+		            android:key="@string/pref_key_opt_min_cluster_size"
+		    		android:title="@string/pref_title_opt_min_cluster_size"
+		    		android:summary="@string/pref_summary_opt_min_cluster_size"
+		   			android:entries="@array/pref_opt_min_cluster_size_keys"
+		   			android:entryValues="@array/pref_opt_min_cluster_size_values"
+		   			android:defaultValue="@string/pref_default_opt_min_cluster_size"/>
+		        
+		       </PreferenceCategory>
+	 	</PreferenceScreen>
+	 	<ListPreference 
+            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_cluster_ratio"
+    		android:title="@string/pref_title_cluster_ratio"
+    		android:summary="@string/pref_summary_cluster_ratio"
+   			android:entries="@array/pref_cluster_ratio_keys"
+   			android:entryValues="@array/pref_cluster_ratio_values"
+   			android:defaultValue="@string/pref_default_cluster_ratio"/>
+        <com.introlab.rtabmap.CustomSwitchPreference 
+            android:key="@string/pref_key_notification_sound"
+    		android:title="@string/pref_title_notification_sound"
+    		android:summary="@string/pref_summary_notification_sound"
+   			android:defaultValue="@string/pref_default_notification_sound"/>
+    </PreferenceCategory>
+	<PreferenceCategory
+        android:title="@string/pref_title_presets">
+        <Preference android:title="@string/pref_title_open_button"
+	                android:key="@string/pref_key_open_button"/>
+        <Preference android:title="@string/pref_title_save_button"
+	                android:key="@string/pref_key_save_button"/>
+        <Preference android:title="@string/pref_title_remove_button"
+	                android:key="@string/pref_key_remove_button"/>
+	    <Preference android:title="@string/pref_title_reset_button"
+	                android:key="@string/pref_key_reset_button"/>
+    </PreferenceCategory>
+</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/layout/database_list.xml

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

app/android/res/menu/optionmenu.xml

@@ -1,68 +1,74 @@
 <?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/post_processing" android:title="Post-Processing...">
+       <group android:checkableBehavior="single">
+	      <item android:id="@+id/point_cloud" android:title="Point Cloud" />
+		  <item android:id="@+id/mesh" android:title="Mesh" />
+		  <item android:id="@+id/texture_mesh" android:checked="true" android:title="Texture Mesh" />
+	   </group>
+	   <item android:id="@+id/save" android:title="Save" android:showAsAction="ifRoom"/>
+                     
+       <item android:id="@+id/export" android:showAsAction="ifRoom" android:title="Export">
           <menu>
-	         <group android:id="@+id/group_post_processing">
-	            <item android:id="@+id/detect_more_loop_closures" android:title="Detect More Loop Closures" />
-	            <item android:id="@+id/global_graph_optimization" android:title="Global Graph Optimization" />
-	            <item android:id="@+id/sba" android:title="Bundle Adjustement" />
-	         </group>   
+              <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_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/open" android:title="Open"/>
-       <item android:id="@+id/save" android:title="Save"/>
-       <item android:id="@+id/export" android:title="Export...">
-          <menu>
-	         <group android:id="@+id/group_export">
-	            <item android:id="@+id/export_ply" android:title="Mesh (.ply)" />
-	            <item android:id="@+id/export_obj" android:title="Mesh with texture (*.obj)" />
-	         </group>   
-          </menu>
-       </item>
-       <item android:id="@+id/reset" android:title="Reset"/>
        
-       <item android:id="@+id/menu_rendering_settings" android:title="Rendering Options...">
+        <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_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/mesh_rendering" android:checked="true" android:title="Mesh Rendering" />
             <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/auto_exposure" android:checked="false" android:title="Auto Exposure" />
-            <item android:id="@+id/max_depth" android:checkable="false" android:title="Cloud/Mesh Max Depth..." />
-            <item android:id="@+id/mesh_angle_tolerance" android:checkable="false" android:title="Mesh Angle Tolerance..." />
-            <item android:id="@+id/mesh_triangle_size" android:checkable="false" android:title="Mesh Triangle Size..." />
+            <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/menu_mapping_settings" android:title="Mapping Options...">
+	    <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/graph_optimization" android:checked="true" android:title="Optimized Graph" />
-	           <item android:id="@+id/nodes_filtering" android:checked="false" android:title="Nodes Filtering" />
-	           <item android:id="@+id/resolution" android:checked="false" android:title="720p Mode" />
-	           <item android:id="@+id/menu_param_settings" android:checkable="false" android:title="Parameters...">
-		           <menu >
-			           <item android:id="@+id/update_rate" android:title="Map Update Rate..." />
-			           <item android:id="@+id/time_threshold" android:title="Time Threshold..." />
-			           <item android:id="@+id/loop_threshold" android:title="Loop Closure Threshold..." />
-			           <item android:id="@+id/optimize_error" android:title="Max Optimization Error..." />
-			           <item android:id="@+id/features" android:title="Max Features Extracted..." />
-			       </menu>
-			    </item>
+	           <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/arrays.xml

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

app/android/res/values/dimens.xml

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

app/android/res/values/strings.xml

@@ -1,24 +1,846 @@
 <?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="camera_button">First View</string>
+    <string name="pause">Pause</string>
+    <string name="resume">Resume</string>
+    <string name="backface_on">Backface</string>
+    <string name="backface_off">Backface</string>
+    <string name="light_on">Lighting</string>
+    <string name="light_off">Lighting</string>
+    <string name="wireframe">Wireframe</string>
+    <string name="close_visualization">Close Visualization</string>
+    <string name="save_to_file">Export to File&#8230;</string>
+    <string name="share_to_sketchfab">Share to Sketchfab&#8230;</string>
     <string name="start">Start</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="hypothesis">"Hypothesis: "</string>
+    <string name="memory">"Used Memory (MB): "</string>
+    <string name="hypothesis">"Hypothesis (%): "</string>
+    <string name="fps">"FPS (rendering): "</string>
+    <string name="distance">"Distance travelled: "</string>
+    <string name="gps">"GPS (long,lat,alt,bearing,err): "</string>
+    <string name="time">"Time: "</string>
     
+    <!-- Preference keys: BEGIN -->
+    <string name="pref_key_tags">pref_key_tags</string>
+    <string name="pref_default_tags">rtabmap 3dscan tango</string>
+    <string name="pref_key_rendering">pref_key_rendering</string>
+    <string name="pref_default_rendering">2</string>
+    <string name="pref_key_open_button">pref_key_open_button</string>
+    <string name="pref_key_save_button">pref_key_save_button</string>
+    <string name="pref_key_remove_button">pref_key_remove_button</string>
+    <string name="pref_key_reset_button">pref_key_reset_button</string>
+    <string name="pref_key_density">pref_key_density</string>
+    <string name="pref_default_density">1</string>
+    <string name="pref_key_min_depth">pref_key_min_depth</string>
+    <string name="pref_default_min_depth">0</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">10</string>
+    <string name="pref_key_angle">pref_key_angle</string>
+    <string name="pref_default_angle">20</string>
+    <string name="pref_key_triangle">pref_key_triangle</string>
+    <string name="pref_default_triangle">2</string>
+    <string name="pref_key_rendering_texture_decimation">pref_key_rendering_texture_decimation</string>
+    <string name="pref_default_rendering_texture_decimation">4</string>
+    <string name="pref_key_blending">pref_key_blending</string>
+    <string name="pref_default_blending">true</string>
+    <string name="pref_key_background_color">pref_key_background_color</string>
+    <string name="pref_default_background_color">0.2</string>
+    <string name="pref_key_nodes_filtering">pref_key_nodes_filtering</string>
+    <string name="pref_default_nodes_filtering">false</string>
+    <string name="pref_key_append">pref_key_append</string>
+    <string name="pref_default_append">true</string>
+    <string name="pref_key_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_fisheye">pref_key_fisheye</string>
+    <string name="pref_default_fisheye">false</string>
+    
+    <string name="pref_key_update_rate">pref_key_update_rate</string>
+    <string name="pref_default_update_rate">1</string>
+    <string name="pref_key_max_speed">pref_key_max_speed</string>
+    <string name="pref_default_max_speed">0</string>
+    <string name="pref_key_time_thr">pref_key_time_thr</string>
+    <string name="pref_default_time_thr">1000</string>
+    <string name="pref_key_mem_thr">pref_key_mem_thr</string>
+    <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">2</string>
+    <string name="pref_key_features_voc">pref_key_features_voc</string>
+    <string name="pref_default_features_voc">200</string>
+    <string name="pref_key_features">pref_key_features</string>
+    <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_gps_saved">pref_key_gps_saved</string>
+    <string name="pref_default_gps_saved">false</string>
+    <string name="pref_key_db_in_memory">pref_key_db_in_memory</string>
+    <string name="pref_default_db_in_memory">false</string>
+    
+    <string name="pref_key_cloud_voxel">pref_key_cloud_voxel</string>
+    <string name="pref_default_cloud_voxel">0.01</string>
+    <string name="pref_key_texture_size">pref_key_texture_size</string>
+    <string name="pref_default_texture_size">4096</string>
+    <string name="pref_key_texture_count">pref_key_texture_count</string>
+    <string name="pref_default_texture_count">1</string>
+    <string name="pref_key_normal_k">pref_key_normal_k</string>
+    <string name="pref_default_normal_k">18</string>
+    <string name="pref_key_max_texture_distance">pref_key_max_texture_distance</string>
+    <string name="pref_default_max_texture_distance">3</string>
+    <string name="pref_key_min_texture_cluster_size">pref_key_min_texture_cluster_size</string>
+    <string name="pref_default_min_texture_cluster_size">50</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">0</string>
+    <string name="pref_key_opt_color_radius">pref_key_opt_color_radius</string>
+    <string name="pref_default_opt_color_radius">0.05</string>
+    <string name="pref_key_opt_clean_white">pref_key_opt_clean_white</string>
+    <string name="pref_default_opt_clean_white">true</string>
+    <string name="pref_key_opt_min_cluster_size">pref_key_opt_min_cluster_size</string>
+    <string name="pref_default_opt_min_cluster_size">0</string>
+    <string name="pref_key_gain_max_radius">pref_key_gain_max_radius</string>
+    <string name="pref_default_gain_max_radius">0.02</string>
+    <string name="pref_key_cluster_ratio">pref_key_cluster_ratio</string>
+    <string name="pref_default_cluster_ratio">0.05</string>
+    <string name="pref_key_notification_sound">pref_key_notification_sound</string>
+    <string name="pref_default_notification_sound">true</string>
+    <!-- Preference keys: END -->
+
+    <string name="pref_title_rendering">Rendering</string>
+    <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. Increase to force scanning perpendicular to surfaces.</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_rendering_texture_decimation">Texture Resolution</string>
+    <string name="pref_summary_rendering_texture_decimation">Resolution of the texture for online rendering. This doesn\'t affect Export results.</string>
+    <string name="pref_title_min_depth">Min Depth</string>
+    <string name="pref_summary_min_depth">Points under the minimum depth are not rendered.</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_blending">Blending</string>
+    <string name="pref_summary_blending">Blend close surfaces together to get more smooth colors on overlapping surfaces. May decrease rendering frame rate.</string>
+    <string name="pref_title_background_color">Background Color</string>
+    <string name="pref_summary_background_color"></string>
+    <string name="pref_title_nodes_filtering">Nodes Filtering</string>
+    <string name="pref_summary_nodes_filtering">Render only the newest point cloud of a loop closure.</string>
+
+    <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_min_depth_keys">
+        <item>"0 m"</item>
+        <item>"0.3 m"</item>
+        <item>"0.5 m"</item>
+        <item>"0.75 m"</item>
+        <item>"1 m"</item>
+        <item>"1.5 m"</item>
+        <item>"2 m"</item>
+        <item>"2.5 m"</item>
+        <item>"3 m"</item>
+    </string-array>
+    <string-array name="pref_min_depth_values">
+        <item>"0"</item>
+        <item>"0.3"</item>
+        <item>"0.5"</item>
+        <item>"0.75"</item>
+        <item>"1"</item>
+        <item>"1.5"</item>
+        <item>"2"</item>
+        <item>"2.5"</item>
+        <item>"3"</item>
+    </string-array>
+    <string-array name="pref_depth_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>
+        <item>"1.5 m"</item>
+        <item>"1 m"</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>"10"</item>
+        <item>"5"</item>
+        <item>"1"</item>
+    </string-array>
+    <string-array name="pref_angle_keys">
+        <item>"60 deg"</item>
+        <item>"45 deg"</item>
+        <item>"35 deg"</item>
+        <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>"60"</item>
+        <item>"45"</item>
+        <item>"35"</item>
+        <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-array name="pref_rendering_texture_decimation_keys">
+        <item>"Maximum"</item>
+        <item>"High"</item>
+        <item>"Low"</item>
+        <item>"Very Low"</item>
+    </string-array>
+    <string-array name="pref_rendering_texture_decimation_values">
+        <item>"1"</item>
+        <item>"2"</item>
+        <item>"4"</item>
+        <item>"8"</item>
+    </string-array>
+    <string-array name="pref_background_color_keys">
+        <item>"White"</item>
+        <item>"0.9"</item>
+        <item>"Light Gray"</item>
+        <item>"0.7"</item>
+        <item>"0.6"</item>
+        <item>"Gray"</item>
+        <item>"0.4"</item>
+        <item>"0.3"</item>
+        <item>"Dark Gray"</item>
+        <item>"0.1"</item>
+        <item>"Black"</item>
+    </string-array>
+    <string-array name="pref_background_color_values">
+        <item>"1.0"</item>
+        <item>"0.9"</item>
+        <item>"0.8"</item>
+        <item>"0.7"</item>
+        <item>"0.6"</item>
+        <item>"0.5"</item>
+        <item>"0.4"</item>
+        <item>"0.3"</item>
+        <item>"0.2"</item>
+        <item>"0.1"</item>
+        <item>"0.0"</item>
+    </string-array>
+
+    <string name="pref_title_mapping_sub">Mapping&#8230;</string>
+    <string name="pref_title_mapping">Mapping</string>
+    <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_resolution">HD Mode</string>
+    <string name="pref_summary_resolution">Save HD images of the color camera if you want very detailed textures. More memory will be required.</string>
+    <string name="pref_title_smoothing">Smoothing</string>
+    <string name="pref_summary_smoothing">Smooth the point clouds.</string>
+    <string name="pref_title_fisheye">Fish Eye Camera</string>
+    <string name="pref_summary_fisheye">Use fish eye camera instead of the color camera. May not work on some devices.</string>
+    <string name="pref_title_update_rate">Update Rate</string>
+    <string name="pref_summary_update_rate">Rate at which a new node is added to map.</string>
+    <string name="pref_title_max_speed">Maximum Motion Speed</string>
+    <string name="pref_summary_max_speed">Images taken when the camera is moving too fast are ignored to avoid blurry textures.</string>
+    <string name="pref_title_time_thr">Time Limit</string>
+    <string name="pref_summary_time_thr">Maximum time allowed for map updates. If time to add a new node is above this theshold, some old parts of the map are temporarly forgotten to reduce time of next updates.</string>
+    <string name="pref_title_mem_thr">Memory Limit</string>
+    <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 factor of the link\'s variance.</string>
+    <string name="pref_title_features_voc">Max Features Extracted (Vocabulary)</string>
+    <string name="pref_summary_features_voc">Extracting more features per image would result in better loop closure hypotheses but more processing time is required.</string>
+    <string name="pref_title_features">Max Features Extracted (Loop Closure)</string>
+    <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_gps_saved">Save GPS</string>
+    <string name="pref_summary_gps_saved">Save GPS in database.</string>
+    <string name="pref_title_db_in_memory">Database In Memory</string>
+    <string name="pref_summary_db_in_memory">The database is kept in RAM for fast access. Set to false to reduce RAM used at the cost of slower access. This parameter is applied on reset or when a database is opened.</string>
+
+    <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_max_speed_keys">
+        <item>"No Limit"</item>
+        <item>"High"</item>
+        <item>"Medium"</item>
+        <item>"Low"</item>
+        <item>"Very Low"</item>
+    </string-array>
+    <string-array name="pref_max_speed_values">
+        <item>"0"</item>
+        <item>"0.4"</item>
+        <item>"0.3"</item>
+        <item>"0.2"</item>
+        <item>"0.1"</item>
+    </string-array>
+    <string-array name="pref_time_thr_keys">
+        <item>"No Limit"</item>
+        <item>"1500 ms"</item>
+        <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>"10x"</item>
+        <item>"9x"</item>
+        <item>"8x"</item>
+        <item>"7x"</item>
+        <item>"6x"</item>
+        <item>"5x"</item>
+        <item>"4x"</item>
+        <item>"3x"</item>
+        <item>"2x"</item>
+        <item>"1x"</item>
+        <item>"Disabled"</item>
+    </string-array>
+    <string-array name="pref_opt_error_values">
+        <item>"10"</item>
+        <item>"9"</item>
+        <item>"8"</item>
+        <item>"7"</item>
+        <item>"6"</item>
+        <item>"5"</item>
+        <item>"4"</item>
+        <item>"3"</item>
+        <item>"2"</item>
+        <item>"1"</item>
+        <item>"0"</item>
+    </string-array>
+    <string-array name="pref_features_voc_keys">
+        <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_texture_count">Maximum Output Textures</string>
+    <string name="pref_summary_texture_count">Note that all textures are still merged into one for visualization, but exported in multiple files.</string>
+    <string name="pref_title_normal_k">Normal K</string>
+    <string name="pref_summary_normal_k">K-nearest neighbors used for normal computation when a mesh is created.</string>
+    <string name="pref_title_max_texture_distance">Max Texture Distance</string>
+    <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_min_texture_cluster_size">Min Texture Cluster Size</string>
+    <string name="pref_summary_min_texture_cluster_size">Minimum polygon cluster size to be textured by a camera. This helps to filter sparse textured polygons.</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_texture_count_keys">
+        <item>"8"</item>
+        <item>"7"</item>
+        <item>"6"</item>
+        <item>"5"</item>
+        <item>"4"</item>
+        <item>"3"</item>
+        <item>"2"</item>
+        <item>"1"</item>
+    </string-array>
+    <string-array name="pref_texture_count_values">
+        <item>"8"</item>
+        <item>"7"</item>
+        <item>"6"</item>
+        <item>"5"</item>
+        <item>"4"</item>
+        <item>"3"</item>
+        <item>"2"</item>
+        <item>"1"</item>
+    </string-array>
+    <string-array name="pref_normal_k_values">
+        <item>"30"</item>
+        <item>"24"</item>
+        <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-array name="pref_min_texture_cluster_size_keys">
+        <item>"1000"</item>
+        <item>"500"</item>
+        <item>"200"</item>
+        <item>"100"</item>
+        <item>"50"</item>
+        <item>"10"</item>
+        <item>"Disabled"</item>
+    </string-array>
+    <string-array name="pref_min_texture_cluster_size_values">
+        <item>"1000"</item>
+        <item>"500"</item>
+        <item>"200"</item>
+        <item>"100"</item>
+        <item>"50"</item>
+        <item>"10"</item>
+        <item>"0"</item>
+    </string-array>
+    
+    <string name="pref_title_optimized">Optimized</string>
+    <string name="pref_title_opt_voxel">Voxel Size</string>
+    <string name="pref_summary_opt_voxel">Increasing this can reduce reconstruction time at the cost of less geometry precision.</string>
+    <string name="pref_title_opt_depth">Reconstruction Depth</string>
+    <string name="pref_summary_opt_depth">Lowering this parameter decreases reconstruction time, but geometry precision is lower. Minimum polygon size: map length / 2^depth). \"Auto\" means that depth is chosen so that polygon size is just under 3 cm.</string>
+    <string name="pref_title_opt_color_radius">Color Radius</string>
+    <string name="pref_summary_opt_color_radius">Radius used to transfer nearest color from the point cloud to reconstructed mesh. When exporting with texture, if Clean Mesh is also enabled, this will limit the number of polygons textured in holes.</string>
+    <string name="pref_title_opt_clean_white">Clean Mesh</string>
+    <string name="pref_summary_opt_clean_white">Clean mesh from textureless or colorless reconstructed polygons.</string>
+	<string name="pref_summary_opt_min_cluster_size">This can be used to filter polygons before texturing.</string>
+    <string name="pref_title_opt_min_cluster_size">Polygon Filtering</string>
+    
+    <string-array name="pref_opt_depth_keys">
+        <item>"Auto"</item>
+        <item>"12"</item>
+        <item>"11"</item>
+        <item>"10"</item>
+        <item>"9"</item>
+        <item>"8"</item>
+        <item>"7"</item>
+        <item>"6"</item>
+    </string-array>
+    <string-array name="pref_opt_depth_values">
+        <item>"0"</item>
+        <item>"12"</item>
+        <item>"11"</item>
+        <item>"10"</item>
+        <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"</item>
+        <item>"0.02"</item>
+        <item>"0.015"</item>
+        <item>"0.01"</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.02"</item>
+        <item>"0.015"</item>
+        <item>"0.01"</item>
+        <item>"-1"</item>
+    </string-array>
+    <string-array name="pref_opt_min_cluster_size_keys">
+        <item>"Only biggest cluster kept"</item>
+        <item>"Keep all polygons"</item>
+    </string-array>
+    <string-array name="pref_opt_min_cluster_size_values">
+        <item>"-1"</item>
+        <item>"0"</item>
+    </string-array>
+
+    <string name="pref_title_general">General</string>
+    <string name="pref_title_gain_max_radius">Color Correction Radius</string>
+    <string name="pref_summary_gain_max_radius">Radius used to find pixel correspondences for Adjust Colors optimization.</string>
+    <string name="pref_title_cluster_ratio">Noise Filtering Ratio</string>
+    <string name="pref_summary_cluster_ratio">Polygon clusters with size smaller than this ratio of the largest cluster are removed by the Noise Filtering optimization.</string>
+    <string name="pref_title_notification_sound">Notification Sound</string>
+    <string name="pref_summary_notification_sound">After saving database or preparing data to export, a notification sound is played.</string>
+
+    <string-array name="pref_gain_max_radius_keys">
+        <item>"0.3 m"</item>
+        <item>"0.2 m"</item>
+        <item>"0.1 m"</item>
+        <item>"0.05 m"</item>
+        <item>"0.025 m"</item>
+        <item>"0.02 m"</item>
+        <item>"0.015 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.025"</item>
+        <item>"0.02"</item>
+        <item>"0.015"</item>
+        <item>"0.01"</item>
+    </string-array>
+    <string-array name="pref_cluster_ratio_keys">
+        <item>"Keep Largest Only"</item>
+        <item>"0.9"</item>
+        <item>"0.8"</item>
+        <item>"0.7"</item>
+        <item>"0.6"</item>
+        <item>"0.5"</item>
+        <item>"0.4"</item>
+        <item>"0.3"</item>
+        <item>"0.2"</item>
+        <item>"0.1"</item>
+        <item>"0.05"</item>
+        <item>"0.01"</item>
+    </string-array>
+    <string-array name="pref_cluster_ratio_values">
+        <item>"1.0"</item>
+        <item>"0.9"</item>
+        <item>"0.8"</item>
+        <item>"0.7"</item>
+        <item>"0.6"</item>
+        <item>"0.5"</item>
+        <item>"0.4"</item>
+        <item>"0.3"</item>
+        <item>"0.2"</item>
+        <item>"0.1"</item>
+        <item>"0.05"</item>
+        <item>"0.01"</item>
+    </string-array>
+
+    <string name="pref_title_presets">Presets</string>
+    <string name="pref_title_open_button">Open</string>
+    <string name="pref_title_save_button">Save</string>
+    <string name="pref_title_remove_button">Remove</string>
+    <string name="pref_title_reset_button">Restore All Default Settings</string>
+    <string name="title_activity_sketchfab">SketchfabActivity</string>
+    <string name="hello_world">Hello world!</string>
+    <string name="action_settings">Settings</string>
+
 </resources>

app/android/res/values/styles.xml

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

app/android/src/com/introlab/rtabmap/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/CustomSwitchPreference.java

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

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

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

app/android/src/com/introlab/rtabmap/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/NDSpinner.java

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

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

@@ -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,32 @@ import android.view.KeyEvent;
 public class RTABMapLib
 {
 
-    static
-    {
-      System.loadLibrary("NativeRTABMap");
+	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);
+    public static native int openDatabase2(String databaseSource, 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();
@@ -44,24 +59,69 @@ public class RTABMapLib
     
 
     public static native void setPausedMapping(boolean paused);
+    public static native void setOnlineBlending(boolean enabled);
     public static native void setMapCloudShown(boolean shown);
     public static native void setOdomCloudShown(boolean shown);
-    public static native void setMeshRendering(boolean enabled);
+    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 setAutoExposure(boolean enabled);
+    public static native void setGridVisible(boolean visible);
+    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 setCameraColor(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 setMinCloudDepth(float value);
+    public static native void setPointSize(float value);
+    public static native void setFOV(float value);
+    public static native void setOrthoCropFactor(float value);
+    public static native void setGridRotation(float value);
+    public static native void setLighting(boolean enabled);
+    public static native void setBackfaceCulling(boolean enabled);
+    public static native void setWireframe(boolean enabled);
+    public static native void setCloudDensityLevel(int value);
     public static native void setMeshAngleTolerance(float value);
     public static native void setMeshTriangleSize(int value);
+    public static native void setClusterRatio(float value);
+    public static native void setMaxGainRadius(float value);
+    public static native void setRenderingTextureDecimation(int value);
+    public static native void setBackgroundColor(float gray);
     public static native int setMappingParameter(String key, String value);
+    public static native void setGPS(
+			double stamp,
+			double longitude, 
+			double latitude, 
+			double altitude,  
+			double accuracy,
+			double bearing);
 
     public static native void resetMapping();
-    public static native void save();
-    public static native boolean exportMesh(String filePath);
+    public static native void save(String outputDatabasePath);
+    public static native void cancelProcessing();
+    public static native boolean exportMesh(
+    		float cloudVoxelSize,
+    		boolean regenerateCloud,
+    		boolean meshing,
+    		int textureSize,
+    		int textureCount,
+    		int normalK,
+    		boolean optimized,
+    		float optimizedVoxelSize,
+    		int optimizedDepth,
+    		int optimizedMaxPolygons,
+    		float optimizedColorRadius,
+    		boolean optimizedCleanWhitePolygons,
+    		int optimizedMinClusterSize,
+    		float optimizedMaxTextureDistance,
+    		int optimizedMinTextureClusterSize,
+    		boolean blockRendering);
+    public static native boolean writeExportedMesh(String directory, String name);
+    public static native boolean postExportation(boolean visualize);
     public static native int postProcessing(int approach);
     
     public static native String getStatus();

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;
@@ -26,30 +34,195 @@ import javax.microedition.khronos.opengles.GL10;
 // ground grid, camera frustum, camera axis, and trajectory based on the Tango
 // device's pose.
 public class Renderer implements GLSurfaceView.Renderer {
+
+	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 float mTextColor = 1.0f;
+	private int mOffset = 0;
 	
-	private ProgressDialog mProgressDialog;
+	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;
 	}
+
+	public void setToast(Toast toast)
+	{
+		mToast = toast;
+	}
 	
-  // Render loop of the Gl context.
-  public void onDrawFrame(GL10 gl) {
-    int value = RTABMapLib.render();
-    if(value == 1 && mProgressDialog != null)
-    {
-    	mProgressDialog.dismiss();
-    }
-  }
+	public void setOffset(int offset)
+	{
+		mOffset = offset;
+	}
 
-  // Called when the surface size changes.
-  public void onSurfaceChanged(GL10 gl, int width, int height) {
-	  RTABMapLib.setupGraphic(width, height);
-  }
+	// Render loop of the Gl context.
+	public void onDrawFrame(GL10 useGLES20instead) {
 
-  // Called when the surface is created or recreated.
-  public void onSurfaceCreated(GL10 gl, EGLConfig config) {
-	  RTABMapLib.initGlContent();
-  }
+		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);
+				}
+				
+				float[] mvp = new float[16];
+				Matrix.translateM(mvp, 0, mtrxProjectionAndView, 0, 0, mOffset, 0);
+				mTextManager.Draw(mvp);
+			}
+
+			if(value != 0 && mProgressDialog != null && mProgressDialog.isShowing())
+			{
+				mActivity.runOnUiThread(new Runnable() {
+					public void run() {
+						if(!RTABMapActivity.DISABLE_LOG) Log.i("RTABMapActivity", "Renderer: dismiss dialog, value received=" + String.valueOf(value));
+						mProgressDialog.dismiss();
+						mActivity.resetNoTouchTimer();
+					}
+				});
+			}
+			if(value==-1)
+			{
+				mActivity.runOnUiThread(new Runnable() {
+					public void run() {
+						if(mToast!=null)
+						{
+							mToast.makeText(mActivity, String.format("Out of Memory!"), Toast.LENGTH_SHORT).show();
+						}
+					}
+				});
+			}
+			else if(value==-2)
+			{
+				mActivity.runOnUiThread(new Runnable() {
+					public void run() {
+						if(mToast!=null)
+						{
+							mToast.makeText(mActivity, String.format("Rendering Error!"), Toast.LENGTH_SHORT).show();
+						}
+					}
+				});
+			}
+		}
+		catch(final Exception e)
+		{
+			mActivity.runOnUiThread(new Runnable() {
+				public void run() {
+					if(mToast!=null)
+					{
+						mToast.makeText(mActivity, String.format("Rendering error! %s", e.getMessage()), Toast.LENGTH_SHORT).show();
+					}
+				}
+
+			});
+		}
+	}
+
+	// Called when the surface size changes.
+	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 useGLES20instead, EGLConfig config) {
+
+		RTABMapLib.initGlContent();
+		
+		// Create our text manager
+		mTextManager = new TextManager(mActivity);
+		mTextManager.setColor(mTextColor);
+	}
+	
+	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;
+		}
+	}
+	
+	public void setTextColor(float color) 
+	{
+		mTextColor = color;
+		if(mTextManager != null)
+		{
+			mTextManager.setColor(mTextColor);
+		}
+	}
 }

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

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

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

@@ -0,0 +1,453 @@
+package com.introlab.rtabmap;
+
+import java.io.File;
+import java.io.IOException;
+import java.util.List;
+
+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.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;
+
+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";
+	
+	ProgressDialog mProgressDialog;
+	
+	private Dialog mAuthDialog;
+	
+	private String mAuthToken;
+	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);
+		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();
+		}
+		
+		authorizeAndPublish(mFilename.getText().toString());	
+	}
+	
+	private boolean isNetworkAvailable() {
+	    ConnectivityManager connectivityManager 
+	          = (ConnectivityManager) getSystemService(Context.CONNECTIVITY_SERVICE);
+	    NetworkInfo activeNetworkInfo = connectivityManager.getActiveNetworkInfo();
+	    return activeNetworkInfo != null && activeNetworkInfo.isConnected();
+	}
+	
+	private void authorizeAndPublish(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(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.setWebContentsDebuggingEnabled(!RTABMapActivity.DISABLE_LOG);
+			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(fileName);
+					}
+				}
+			});
+			mAuthDialog.show();
+			mAuthDialog.setTitle("Authorize RTAB-Map");
+			mAuthDialog.setCancelable(true);
+			web.loadUrl(auth_url);
+		}
+		else
+		{
+			zipAndPublish(fileName);
+		}
+	}
+
+	private void zipAndPublish(final String fileName)
+	{		
+		mProgressDialog.setTitle("Upload to Sketchfab");
+		mProgressDialog.setMessage(String.format("Compressing the files..."));
+		mProgressDialog.show();
+
+		Thread workingThread = new Thread(new Runnable() {
+			public void run() {
+				try{
+					
+					File tmpDir = new File(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR);
+					tmpDir.mkdirs();
+					String[] fileNames = Util.loadFileList(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR, false);
+					if(!RTABMapActivity.DISABLE_LOG) Log.i(RTABMapActivity.TAG, String.format("Deleting %d files in \"%s\"", fileNames.length, mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR));
+					for(int i=0; i<fileNames.length; ++i)
+					{
+						File f = new File(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR + "/" + fileNames[i]);
+						if(f.delete())
+						{
+							if(!RTABMapActivity.DISABLE_LOG) Log.i(RTABMapActivity.TAG, String.format("Deleted \"%s\"", f.getPath()));
+						}
+						else
+						{
+							if(!RTABMapActivity.DISABLE_LOG) Log.i(RTABMapActivity.TAG, String.format("Failed deleting \"%s\"", f.getPath()));
+						}
+					}
+					File exportDir = new File(mWorkingDirectory + RTABMapActivity.RTABMAP_EXPORT_DIR);
+					exportDir.mkdirs();
+					
+					if(RTABMapLib.writeExportedMesh(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR, RTABMapActivity.RTABMAP_TMP_FILENAME))
+					{
+						String[] files = new String[0];
+						// verify if we have all files
+				
+						fileNames = Util.loadFileList(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR, false);
+						if(fileNames.length > 0)
+						{
+							files = new String[fileNames.length];
+							for(int i=0; i<fileNames.length; ++i)
+							{
+								files[i] = mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR + "/" + fileNames[i];
+							}
+						}
+						else
+						{
+							if(!RTABMapActivity.DISABLE_LOG) Log.i(RTABMapActivity.TAG, "Missing files!");
+						}
+				
+						if(files.length > 0)
+						{
+							final String[] filesToZip = files;
+							final String zipOutput = mWorkingDirectory+fileName+".zip";
+							Util.zip(filesToZip, zipOutput);
+							runOnUiThread(new Runnable() {
+								public void run() {
+									mProgressDialog.dismiss();
+
+									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();
+								}
+							});
+						}
+					}
+					else
+					{
+						runOnUiThread(new Runnable() {
+							public void run() {
+								mProgressDialog.dismiss();
+								Toast.makeText(getActivity(), String.format("Failed writing files!"), Toast.LENGTH_LONG).show();
+							}
+						});
+					}
+				}
+				catch(IOException ex) {
+					Log.e(RTABMapActivity.TAG, "Failed to zip", ex);
+				}
+			}
+		});
+
+		workingThread.start();
+	}
+	
+	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,419 @@
+//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 vec2 a_texCoord;" +
+		"varying vec2 v_texCoord;" +
+	    "void main() {" +
+	    "  gl_Position = uMVPMatrix * vPosition;" +
+	    "  v_texCoord = a_texCoord;" +
+	    "}";
+	public static final String fs_Text =
+	    "precision mediump float;" +
+	    "uniform float uColor;" +
+	    "varying vec2 v_texCoord;" +
+        "uniform sampler2D s_texture;" +
+	    "void main() {" +
+	    "  gl_FragColor = texture2D( s_texture, v_texCoord );" +
+	    "  gl_FragColor.rgb *= uColor;" +
+	    "}";
+	
+	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 float mColor;
+	
+	private FloatBuffer vertexBuffer;
+	private FloatBuffer textureBuffer;
+	private ShortBuffer drawListBuffer;
+	
+	private float[] vecs;
+	private float[] uvs;
+	private short[] indices;
+	
+	private int index_vecs;
+	private int index_indices;
+	private int index_uvs;
+
+	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];
+		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;
+		mColor = 1.0f;
+		
+		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 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;
+		
+		// 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;
+		uvs = null;
+		indices = null;
+		
+		vecs = new float[charcount * 12];
+		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);
+			GLES20.glEnable(GLES20.GL_BLEND);
+			
+			// 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 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 );
+	
+		    // get handle to shape's transformation matrix
+	        int mtrxhandle = GLES20.glGetUniformLocation(sp_Text, "uMVPMatrix");
+	        
+	    	// Apply the projection and view transformation
+	        GLES20.glUniformMatrix4fv(mtrxhandle, 1, false, m, 0);
+	        
+	        // get handle to color value
+	        int colorhandle = GLES20.glGetUniformLocation(sp_Text, "uColor");
+	        GLES20.glUniform1f(colorhandle, mColor);
+	        
+	        int mSamplerLoc = GLES20.glGetUniformLocation (sp_Text, "s_texture" );
+	        
+	        // Texture activate unit 0
+	        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);
+		}
+	}
+	
+	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;
+	}
+	
+	public void setColor(float color) {
+		mColor = color;
+	}
+}

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/android/src/com/introlab/rtabmap/Util.java

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

app/src/CMakeLists.txt

@@ -74,7 +74,11 @@ ELSE()
 ENDIF()
 TARGET_LINK_LIBRARIES(rtabmap rtabmap_core rtabmap_gui rtabmap_utilite ${LIBRARIES})
 IF(Qt5_FOUND)
-    QT5_USE_MODULES(rtabmap Widgets Core Gui Svg PrintSupport)
+    IF(Qt5Svg_FOUND)
+        QT5_USE_MODULES(rtabmap Widgets Core Gui Svg PrintSupport)
+    ELSE()
+        QT5_USE_MODULES(rtabmap Widgets Core Gui PrintSupport)
+    ENDIF()
 ENDIF(Qt5_FOUND)
 
 IF(APPLE AND BUILD_AS_BUNDLE)
@@ -100,7 +104,7 @@ INSTALL(CODE "execute_process(COMMAND ln -s \"../MacOS/${CMAKE_BUNDLE_NAME}\" ${
         WORKING_DIRECTORY \$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/bin)")
 ENDIF(APPLE AND BUILD_AS_BUNDLE)
        
-IF((APPLE AND BUILD_AS_BUNDLE) OR WIN32)
+IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
   SET(APPS "\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/bin/${PROJECT_NAME}${CMAKE_EXECUTABLE_SUFFIX}")
   SET(plugin_dest_dir bin)
   SET(qtconf_dest_dir bin)
@@ -137,11 +141,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
-  INSTALL(DIRECTORY "${QT_PLUGINS_DIR}/imageformats" 
+  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
@@ -171,5 +189,5 @@ IF((APPLE AND BUILD_AS_BUNDLE) OR WIN32)
     include(\"BundleUtilities\")
     fixup_bundle(\"${APPS}\" \"\${QTPLUGINS}\" \"${DIRS}\")
   " COMPONENT runtime)
-ENDIF((APPLE AND BUILD_AS_BUNDLE) OR WIN32)
+ENDIF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
   

app/src/ObjDeletionHandler.h

@@ -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
@@ -46,17 +46,18 @@ public:
 	}
 	virtual ~ObjDeletionHandler() {}
 
-signals:
+Q_SIGNALS:
 	void objDeletionEventReceived(int);
 
 protected:
-	virtual void handleEvent(UEvent * event)
+	virtual bool handleEvent(UEvent * event)
 	{
 		if(event->getClassName().compare("UObjDeletedEvent") == 0 &&
 		   event->getCode() == _watchedId)
 		{
-			emit objDeletionEventReceived(_watchedId);
+			Q_EMIT objDeletionEventReceived(_watchedId);
 		}
+		return false;
 	}
 private:
 	int _watchedId;

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
@@ -47,6 +47,9 @@ 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, false);
 	MainWindow * mainWindow = new MainWindow();
     app->installEventFilter(mainWindow); // to catch FileOpen events.
     
@@ -82,7 +85,11 @@ int main(int argc, char* argv[])
     
     if(!database.empty())
     {
-    	QMetaObject::invokeMethod(mainWindow, "openDatabase", Qt::QueuedConnection, Q_ARG(QString, QString(database.c_str())));
+    	mainWindow->openDatabase(database.c_str(), parameters);
+    }
+    else if(parameters.size())
+    {
+    	mainWindow->updateParameters(parameters);
     }
 
 	// Now wait for application to finish

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")
-     set(DC1394_LIBRARIES ${DC1394_LIBRARIES} "-framework CoreServices")
-   endif(CMAKE_SYSTEM_NAME MATCHES "Darwin")
+   #if(CMAKE_SYSTEM_NAME MATCHES "Darwin")
+   #  set(DC1394_LIBRARIES ${DC1394_LIBRARIES} "-framework CoreServices")
+   #endif(CMAKE_SYSTEM_NAME MATCHES "Darwin")
 
 ENDIF (DC1394_INCLUDE_DIRS AND DC1394_LIBRARIES)
 

cmake_modules/FindFlyCapture2.cmake

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

cmake_modules/FindG2O.cmake

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

cmake_modules/FindKinectSDK2.cmake

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

cmake_modules/FindORB_SLAM2.cmake

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

cmake_modules/FindRealSense.cmake

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

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

cmake_modules/FindSqlite3.cmake

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

cmake_modules/FindTango.cmake

@@ -8,13 +8,14 @@
 
 FIND_PATH(Tango_INCLUDE_DIR tango_client_api.h)
 
-FIND_LIBRARY(Tango_LIBRARY NAMES tango_client_api)
+FIND_LIBRARY(Tango_LIBRARY NAMES tango_client_api PATH_SUFFIXES ${ANDROID_ABI})
+FIND_LIBRARY(Tango_support_LIBRARY NAMES tango_support_api PATH_SUFFIXES ${ANDROID_ABI})
 
-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})
-ENDIF (Tango_INCLUDE_DIR AND Tango_LIBRARY)
+   SET(Tango_LIBRARIES ${Tango_LIBRARY} ${Tango_support_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)
 

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

corelib/include/rtabmap/core/Camera.h

@@ -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;}
@@ -65,6 +66,7 @@ public:
 	void setImageRate(float imageRate) {_imageRate = imageRate;}
 	void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;}
 
+	void resetTimer();
 protected:
 	/**
 	 * Constructor
@@ -76,7 +78,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,35 @@ 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 timeStereoExposureCompensation;
 	float timeImageDecimation;
 	float timeScanFromDepth;
+	float timeUndistortDepth;
+	float timeBilateralFiltering;
+	float timeTotal;
+	Transform odomPose;
+	cv::Mat odomCovariance;
+	std::vector<float> odomVelocity;
 };
 
 } // 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
@@ -75,8 +75,10 @@ public:
 	virtual ~CameraModel() {}
 
 	void initRectificationMap();
+	bool isRectificationMapInitialized() {return !mapX_.empty() && !mapY_.empty();}
 
-	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 &&
@@ -106,7 +108,7 @@ public:
 	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 +117,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 +126,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;
@@ -67,7 +68,7 @@ public:
 	const CameraModel & cameraModel() const {return _model;}
 
 	void setPath(const std::string & dir) {_path=dir;}
-	void setStartIndex(int index) {_startAt = index;} // negative means last
+	virtual void setStartIndex(int index) {_startAt = index;} // negative means last
 	void setDirRefreshed(bool enabled) {_refreshDir = enabled;}
 	void setImagesRectified(bool enabled) {_rectifyImages = enabled;}
 	void setBayerMode(int mode) {_bayerMode = mode;} // -1=disabled (default) 0=BayerBG, 1=BayerGB, 2=BayerRG, 3=BayerGR
@@ -75,8 +76,8 @@ public:
 	void setTimestamps(bool fileNamesAreStamps, const std::string & filePath = "", bool syncImageRateWithStamps=true)
 	{
 		_filenamesAreTimestamps = fileNamesAreStamps;
-		timestampsPath_=filePath;
-		syncImageRateWithStamps_ = syncImageRateWithStamps;
+		_timestampsPath=filePath;
+		_syncImageRateWithStamps = syncImageRateWithStamps;
 	}
 
 	void setScanPath(
@@ -84,19 +85,19 @@ public:
 			int maxScanPts = 0,
 			int downsampleStep = 1,
 			float voxelSize = 0.0f,
-			int normalsK = 0, // compute normals if > 0
-			const Transform & localTransform=Transform::getIdentity())
+			int normalsK = 0,        // compute normals if > 0
+			float normalsRadius = 0, // compute normals if > 0
+			const Transform & localTransform=Transform::getIdentity(),
+			bool forceGroundNormalsUp = false)
 	{
 		_scanPath = dir;
 		_scanLocalTransform = localTransform;
 		_scanMaxPts = maxScanPts;
 		_scanDownsampleStep = downsampleStep;
 		_scanNormalsK = normalsK;
+		_scanNormalsRadius = normalsRadius;
 		_scanVoxelSize = voxelSize;
-		if(_scanDownsampleStep>1)
-		{
-			_scanMaxPts /= _scanDownsampleStep;
-		}
+		_scanForceGroundNormalsUp = forceGroundNormalsUp;
 	}
 
 	void setDepthFromScan(bool enabled, int fillHoles = 1, bool fillHolesFromBorder = false)
@@ -106,12 +107,23 @@ public:
 		_depthFromScanFillHolesFromBorder = fillHolesFromBorder;
 	}
 
+	// Format: 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe
+	void setOdometryPath(const std::string & filePath, int format = 0)
+	{
+		_odometryPath = filePath;
+		_odometryFormat = format;
+	}
+
+	// Format: 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe
 	void setGroundTruthPath(const std::string & filePath, int format = 0)
 	{
-		groundTruthPath_ = filePath;
+		_groundTruthPath = filePath;
 		_groundTruthFormat = format;
 	}
 
+	void setMaxPoseTimeDiff(double diff) {_maxPoseTimeDiff = diff;}
+	double getMaxPoseTimeDiff() const {return _maxPoseTimeDiff;}
+
 	void setDepth(bool isDepth, float depthScaleFactor = 1.0f)
 	{
 		_isDepth = isDepth;
@@ -119,7 +131,13 @@ 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,
+			double maxTimeDiff) const;
 
 private:
 	std::string _path;
@@ -144,19 +162,25 @@ private:
 	int _scanDownsampleStep;
 	float _scanVoxelSize;
 	int _scanNormalsK;
+	float _scanNormalsRadius;
+	bool _scanForceGroundNormalsUp;
 
 	bool _depthFromScan;
 	int _depthFromScanFillHoles; // <0:horizontal 0:disabled >0:vertical
 	bool _depthFromScanFillHolesFromBorder;
 
 	bool _filenamesAreTimestamps;
-	std::string timestampsPath_;
-	bool syncImageRateWithStamps_;
+	std::string _timestampsPath;
+	bool _syncImageRateWithStamps;
 
-	std::string groundTruthPath_;
+	std::string _odometryPath;
+	int _odometryFormat;
+	std::string _groundTruthPath;
 	int _groundTruthFormat;
+	double _maxPoseTimeDiff;
 
-	std::list<double> stamps_;
+	std::list<double> _stamps;
+	std::list<Transform> odometry_;
 	std::list<Transform> groundTruth_;
 	CameraModel _model;
 
@@ -178,6 +202,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 +218,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,13 +34,19 @@ 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>
 
 #ifdef HAVE_OPENNI
+#if __linux__ && __i386__ && __cplusplus >= 201103L
+#warning "Openni driver is not available on i386 when building with c++11 support"
+#else
+#define RTABMAP_OPENNI
 #include <pcl/io/openni_camera/openni_depth_image.h>
 #include <pcl/io/openni_camera/openni_image.h>
 #endif
+#endif
 
 #include <boost/signals2/connection.hpp>
 
@@ -64,9 +70,34 @@ class Registration;
 class PacketPipeline;
 }
 
+namespace rs
+{
+	class context;
+	class device;
+	namespace slam {
+	class slam;
+	}
+}
+
+namespace rs2
+{
+	class context;
+	class device;
+	class syncer;
+}
+struct rs2_intrinsics;
+struct rs2_extrinsics;
+
 typedef struct _freenect_context freenect_context;
 typedef struct _freenect_device freenect_device;
 
+typedef struct IKinectSensor IKinectSensor;
+typedef struct ICoordinateMapper ICoordinateMapper;
+typedef struct _DepthSpacePoint DepthSpacePoint;
+typedef struct _ColorSpacePoint ColorSpacePoint;
+typedef struct tagRGBQUAD RGBQUAD;
+typedef struct IMultiSourceFrameReader IMultiSourceFrameReader;
+
 namespace rtabmap
 {
 
@@ -85,7 +116,7 @@ public:
 			float imageRate = 0,
 			const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraOpenni();
-#ifdef HAVE_OPENNI
+#ifdef RTABMAP_OPENNI
     void image_cb (
     		const boost::shared_ptr<openni_wrapper::Image>& rgb,
 			const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
@@ -97,7 +128,7 @@ public:
     virtual std::string getSerial() const;
 
 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);
 
 private:
     pcl::Grabber* interface_;
@@ -131,7 +162,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 +180,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 +198,15 @@ public:
 	bool setExposure(int value);
 	bool setGain(int value);
 	bool setMirroring(bool enabled);
-	void setOpenNI2StampsAndIDsUsed(bool used) {_openNI2StampsAndIDsUsed = used;}
+	void setOpenNI2StampsAndIDsUsed(bool used);
+	void setIRDepthShift(int horizontal, int vertical);
 
 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 +214,10 @@ private:
 	float _depthFy;
 	std::string _deviceId;
 	bool _openNI2StampsAndIDsUsed;
+	StereoCameraModel _stereoModel;
+	int _depthHShift;
+	int _depthVShift;
+#endif
 };
 
 
@@ -191,10 +231,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 +246,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
 };
 
 /////////////////////////
@@ -234,14 +280,15 @@ public:
 public:
 	// default local transform z in, x right, y down));
 	CameraFreenect2(int deviceId= 0,
-					Type type = kTypeColor2DepthSD,
+					Type type = kTypeDepth2ColorSD,
 					float imageRate=0.0f,
 					const Transform & localTransform = Transform::getIdentity(),
 					float minDepth = 0.3f,
 					float maxDepth = 12.0f,
 					bool bilateralFiltering = true,
 					bool edgeAwareFiltering = true,
-					bool noiseFiltering = true);
+					bool noiseFiltering = true,
+					const std::string & pipelineName = "");
 	virtual ~CameraFreenect2();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -249,9 +296,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 +312,165 @@ private:
 	bool bilateralFiltering_;
 	bool edgeAwareFiltering_;
 	bool noiseFiltering_;
+	std::string pipelineName_;
+#endif
+};
+
+/////////////////////////
+// CameraK4W2
+/////////////////////////
+
+class RTABMAP_EXP CameraK4W2 :
+	public Camera
+{
+public:
+	static bool available();
+
+	enum Type {
+		kTypeColor2DepthSD,
+		kTypeDepth2ColorSD,
+		kTypeDepth2ColorHD
+	};
+
+public:
+	static const int        cDepthWidth = 512;
+	static const int        cDepthHeight = 424;
+	static const int        cColorWidth = 1920;
+	static const int        cColorHeight = 1080;
+
+public:
+	// default local transform z in, x right, y down));
+	CameraK4W2(int deviceId = 0, // not used
+		Type type = kTypeDepth2ColorSD,
+		float imageRate = 0.0f,
+		const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraK4W2();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+
+protected:
+	virtual SensorData captureImage(CameraInfo * info = 0);
+
+private:
+	void close();
+
+private:
+#ifdef RTABMAP_K4W2
+	Type type_;
+	IKinectSensor*          pKinectSensor_;
+	ICoordinateMapper*      pCoordinateMapper_;
+	DepthSpacePoint*        pDepthCoordinates_;
+	ColorSpacePoint*        pColorCoordinates_;
+	IMultiSourceFrameReader* pMultiSourceFrameReader_;
+	RGBQUAD * pColorRGBX_;
+	INT_PTR hMSEvent;
+	CameraModel colorCameraModel_;
+#endif
+};
+
+/////////////////////////
+// CameraRealSense
+/////////////////////////
+class slam_event_handler;
+class RTABMAP_EXP CameraRealSense :
+	public Camera
+{
+public:
+	static bool available();
+	enum RGBSource {kColor, kInfrared, kFishEye};
+
+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
+		bool computeOdometry = false,
+		float imageRate = 0,
+		const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraRealSense();
+
+	void setDepthScaledToRGBSize(bool enabled);
+	void setRGBSource(RGBSource source);
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+	virtual bool odomProvided() const;
+
+protected:
+	virtual SensorData captureImage(CameraInfo * info = 0);
+
+private:
+#ifdef RTABMAP_REALSENSE
+	rs::context * ctx_;
+	rs::device * dev_;
+	int deviceId_;
+	int presetRGB_;
+	int presetDepth_;
+	bool computeOdometry_;
+	bool depthScaledToRGBSize_;
+	RGBSource rgbSource_;
+	CameraModel cameraModel_;
+	std::vector<int> rsRectificationTable_;
+
+	int motionSeq_[2];
+	rs::slam::slam * slam_;
+	UMutex slamLock_;
+
+	std::map<double, std::pair<cv::Mat, cv::Mat> > bufferedFrames_;
+	std::pair<cv::Mat, cv::Mat> lastSyncFrames_;
+	UMutex dataMutex_;
+	USemaphore dataReady_;
+#endif
+};
+/////////////////////////
+// CameraRealSense2
+/////////////////////////
+class slam_event_handler;
+class RTABMAP_EXP CameraRealSense2 :
+	public Camera
+{
+public:
+	static bool available();
+
+public:
+	// default local transform z in, x right, y down));
+	CameraRealSense2(
+		const std::string & deviceId = "",
+		float imageRate = 0,
+		const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraRealSense2();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+
+	// parameters are set during initialization
+	void setEmitterEnabled(bool enabled);
+	void setIRDepthFormat(bool enabled);
+
+protected:
+	virtual SensorData captureImage(CameraInfo * info = 0);
+
+private:
+#ifdef RTABMAP_REALSENSE2
+	rs2::context * ctx_;
+	rs2::device * dev_;
+	std::string deviceId_;
+	rs2::syncer * syncer_;
+	float depth_scale_meters_;
+	rs2_intrinsics * depthIntrinsics_;
+	rs2_intrinsics * rgbIntrinsics_;
+	rs2_extrinsics * depthToRGBExtrinsics_;
+	cv::Mat depthBuffer_;
+	cv::Mat rgbBuffer_;
+	CameraModel model_;
+
+	bool emitterEnabled_;
+	bool irDepth_;
+#endif
 };
 
 
@@ -290,8 +497,10 @@ public:
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
 
+	virtual void setStartIndex(int index) {CameraImages::setStartIndex(index);cameraDepth_.setStartIndex(index);} // negative means last
+
 protected:
-	virtual SensorData captureImage();
+	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,11 @@ namespace FlyCapture2
 class Camera;
 }
 
+namespace sl
+{
+class Camera;
+}
+
 namespace rtabmap
 {
 
@@ -62,11 +68,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 +95,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 = 0,// 0=STANDARD, 1=FILL
+			int confidenceThr = 100,
+			bool computeOdometry = false,
+			float imageRate=0.0f,
+			const Transform & localTransform = Transform::getIdentity(),
+			bool selfCalibration = true);
+	CameraStereoZed(
+			const std::string & svoFilePath,
+			int quality = 1,    // 0=NONE, 1=PERFORMANCE, 2=QUALITY
+			int sensingMode = 0,// 0=STANDARD, 1=FILL
+			int confidenceThr = 100,
+			bool computeOdometry = false,
+			float imageRate=0.0f,
+			const Transform & localTransform = Transform::getIdentity(),
+			bool selfCalibration = true);
+	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::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
 };
 
 /////////////////////////
@@ -122,8 +188,10 @@ public:
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
 
+	virtual void setStartIndex(int index) {CameraImages::setStartIndex(index);camera2_->setStartIndex(index);} // negative means last
+
 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);
 
 private:
 	CameraImages * camera2_;
@@ -143,10 +211,27 @@ 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());
+	CameraStereoVideo(
+			int deviceLeft,
+			int deviceRight,
+			bool rectifyImages = false,
+			float imageRate = 0.0f,
+			const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraStereoVideo();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -154,14 +239,19 @@ 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_;
+	int usbDevice2_;
 };
 
 } // 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,10 +33,17 @@ 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
 {
 
 class Camera;
+class CameraInfo;
+class SensorData;
 class StereoDense;
 
 /**
@@ -53,38 +60,48 @@ public:
 	virtual ~CameraThread();
 
 	void setMirroringEnabled(bool enabled) {_mirroring = enabled;}
+	void setStereoExposureCompensation(bool enabled) {_stereoExposureCompensation = enabled;}
 	void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;}
 	void setImageDecimation(int decimation) {_imageDecimation = decimation;}
 	void setStereoToDepth(bool enabled) {_stereoToDepth = enabled;}
+	void setImageRate(float imageRate);
+	void setDistortionModel(const std::string & path);
+	void enableBilateralFiltering(float sigmaS, float sigmaR);
+	void disableBilateralFiltering() {_bilateralFiltering = false;}
 
 	void setScanFromDepth(
 			bool enabled,
 			int decimation=4,
 			float maxDepth=4.0f,
 			float voxelSize = 0.0f,
-			int normalsK = 0)
+			int normalsK = 0,
+			int normalsRadius = 0.0f)
 	{
 		_scanFromDepth = enabled;
 		_scanDecimation=decimation;
 		_scanMaxDepth = maxDepth;
 		_scanVoxelSize = voxelSize;
 		_scanNormalsK = normalsK;
+		_scanNormalsRadius = normalsRadius;
 	}
 
+	void postUpdate(SensorData * data, CameraInfo * info = 0) const;
+
 	//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();
 
 private:
 	Camera * _camera;
 	bool _mirroring;
+	bool _stereoExposureCompensation;
 	bool _colorOnly;
 	int _imageDecimation;
 	bool _stereoToDepth;
@@ -94,7 +111,12 @@ private:
 	float _scanMinDepth;
 	float _scanVoxelSize;
 	int _scanNormalsK;
+	float _scanNormalsRadius;
 	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
@@ -83,5 +83,8 @@ cv::Mat RTABMAP_EXP uncompressData(const cv::Mat & bytes);
 cv::Mat RTABMAP_EXP uncompressData(const std::vector<unsigned char> & bytes);
 cv::Mat RTABMAP_EXP uncompressData(const unsigned char * bytes, unsigned long size);
 
+cv::Mat RTABMAP_EXP compressString(const std::string & str);
+std::string RTABMAP_EXP uncompressString(const cv::Mat & bytes);
+
 } /* namespace rtabmap */
 #endif /* COMPRESSION_H_ */

corelib/include/rtabmap/core/DBDriver.h

@@ -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>
@@ -67,6 +68,7 @@ public:
 	virtual ~DBDriver();
 
 	virtual void parseParameters(const ParametersMap & parameters);
+	virtual bool isInMemory() const {return _url.empty();}
 	const std::string & getUrl() const {return _url;}
 
 	void beginTransaction() const;
@@ -86,41 +88,84 @@ 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,
+				const cv::Mat & empty,
+				float cellSize,
+				const cv::Point3f & viewpoint);
+	void updateDepthImage(int nodeId, const cv::Mat & image);
 
 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;
+	void savePreviewImage(const cv::Mat & image) const;
+	cv::Mat loadPreviewImage() const;
+	void saveOptimizedPoses(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const;
+	std::map<int, Transform> loadOptimizedPoses(Transform * lastlocalizationPose) const;
+	void save2DMap(const cv::Mat & map, float xMin, float yMin, float cellSize) const;
+	cv::Mat load2DMap(float & xMin, float & yMin, float & cellSize) const;
+	void saveOptimizedMesh(
+			const cv::Mat & cloud,
+			const std::vector<std::vector<std::vector<unsigned int> > > & polygons = std::vector<std::vector<std::vector<unsigned int> > >(),      // Textures -> polygons -> vertices
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords = std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > >(), // Textures -> uv coords for each vertex of the polygons
+#else
+			const std::vector<std::vector<Eigen::Vector2f> > & texCoords = std::vector<std::vector<Eigen::Vector2f> >(), // Textures -> uv coords for each vertex of the polygons
+#endif
+			const cv::Mat & textures = cv::Mat()) const; // concatenated textures (assuming square textures with all same size);
+	cv::Mat loadOptimizedMesh(
+			std::vector<std::vector<std::vector<unsigned int> > > * polygons = 0,
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords = 0,
+#else
+			std::vector<std::vector<Eigen::Vector2f> > * texCoords = 0,
+#endif
+			cv::Mat * textures = 0) const;
 
 public:
 	// Mutex-protected methods of abstract versions below
 
 	bool openConnection(const std::string & url, bool overwritten = false);
-	void closeConnection(bool save = true);
+	void closeConnection(bool save = true, const std::string & outputUrl = "");
 	bool isConnected() const;
 	long getMemoryUsed() const; // In bytes
 	std::string getDatabaseVersion() const;
+	long getNodesMemoryUsed() const;
+	long getLinksMemoryUsed() const;
 	long getImagesMemoryUsed() const;
 	long getDepthImagesMemoryUsed() const;
+	long getCalibrationsMemoryUsed() const;
+	long getGridsMemoryUsed() const;
 	long getLaserScansMemoryUsed() const;
 	long getUserDataMemoryUsed() const;
 	long getWordsMemoryUsed() const;
+	long getFeaturesMemoryUsed() const;
+	long getStatisticsMemoryUsed() const;
 	int getLastNodesSize() const; // working memory
 	int getLastDictionarySize() const; // working memory
 	int getTotalNodesSize() const;
 	int getTotalDictionarySize() const;
+	ParametersMap getLastParameters() const;
+	std::map<std::string, float> getStatistics(int nodeId, double & stamp, std::vector<int> * wmState=0) const;
+	std::map<int, std::pair<std::map<std::string, float>, double> > getAllStatistics() const;
+	std::map<int, std::vector<int> > getAllStatisticsWmStates() const;
 
 	void executeNoResult(const std::string & sql) const;
 
 	// Load objects
-	void load(VWDictionary * dictionary) const;
+	void load(VWDictionary * dictionary, bool lastStateOnly = true) const;
 	void loadLastNodes(std::list<Signature *> & signatures) const;
 	void loadSignatures(const std::list<int> & ids, std::list<Signature *> & signatures, std::set<int> * loadedFromTrash = 0);
 	void loadWords(const std::set<int> & wordIds, std::list<VisualWord *> & vws);
 
 	// Specific queries...
-	void loadNodeData(std::list<Signature *> & signatures) const;
-	void getNodeData(int signatureId, SensorData & data) 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, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
 	bool getCalibration(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
-	bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose) const;
+	bool getLaserScanInfo(int signatureId, LaserScan & info) const;
+	bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps) const;
 	void loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
 	void getWeight(int signatureId, int & weight) const;
 	void getAllNodeIds(std::set<int> & ids, bool ignoreChildren = false, bool ignoreBadSignatures = false) const;
@@ -134,27 +179,36 @@ public:
 protected:
 	DBDriver(const ParametersMap & parameters = ParametersMap());
 
-private:
 	virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false) = 0;
-	virtual void disconnectDatabaseQuery(bool save = true) = 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;
+	virtual long getNodesMemoryUsedQuery() const = 0;
+	virtual long getLinksMemoryUsedQuery() const = 0;
 	virtual long getImagesMemoryUsedQuery() const = 0;
 	virtual long getDepthImagesMemoryUsedQuery() const = 0;
+	virtual long getCalibrationsMemoryUsedQuery() const = 0;
+	virtual long getGridsMemoryUsedQuery() const = 0;
 	virtual long getLaserScansMemoryUsedQuery() const = 0;
 	virtual long getUserDataMemoryUsedQuery() const = 0;
 	virtual long getWordsMemoryUsedQuery() const = 0;
+	virtual long getFeaturesMemoryUsedQuery() const = 0;
+	virtual long getStatisticsMemoryUsedQuery() const = 0;
 	virtual int getLastNodesSizeQuery() const = 0;
 	virtual int getLastDictionarySizeQuery() const = 0;
 	virtual int getTotalNodesSizeQuery() const = 0;
 	virtual int getTotalDictionarySizeQuery() const = 0;
+	virtual ParametersMap getLastParametersQuery() const = 0;
+	virtual std::map<std::string, float> getStatisticsQuery(int nodeId, double & stamp, std::vector<int> * wmState) const = 0;
+	virtual std::map<int, std::pair<std::map<std::string, float>, double> > getAllStatisticsQuery() const = 0;
+	virtual std::map<int, std::vector<int> > getAllStatisticsWmStatesQuery() const = 0;
 
 	virtual void executeNoResultQuery(const std::string & sql) const = 0;
 
 	virtual void getWeightQuery(int signatureId, int & weight) const = 0;
 
-	virtual void saveQuery(const std::list<Signature *> & signatures) const = 0;
+	virtual void saveQuery(const std::list<Signature *> & signatures) = 0;
 	virtual void saveQuery(const std::list<VisualWord *> & words) const = 0;
 	virtual void updateQuery(const std::list<Signature *> & signatures, bool updateTimestamp) const = 0;
 	virtual void updateQuery(const std::list<VisualWord *> & words, bool updateTimestamp) const = 0;
@@ -162,16 +216,54 @@ 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,
+				const cv::Mat & empty,
+				float cellSize,
+				const cv::Point3f & viewpoint) const = 0;
+
+	virtual void updateDepthImageQuery(
+					int nodeId,
+					const cv::Mat & image) const = 0;
+
+	virtual void addStatisticsQuery(const Statistics & statistics) const = 0;
+	virtual void savePreviewImageQuery(const cv::Mat & image) const = 0;
+	virtual cv::Mat loadPreviewImageQuery() const = 0;
+	virtual void saveOptimizedPosesQuery(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const = 0;
+	virtual std::map<int, Transform> loadOptimizedPosesQuery(Transform * lastlocalizationPose) const = 0;
+	virtual void save2DMapQuery(const cv::Mat & map, float xMin, float yMin, float cellSize) const = 0;
+	virtual cv::Mat load2DMapQuery(float & xMin, float & yMin, float & cellSize) const = 0;
+	virtual void saveOptimizedMeshQuery(
+				const cv::Mat & cloud,
+				const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+				const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
+#else
+				const std::vector<std::vector<Eigen::Vector2f> > & texCoords,
+#endif
+				const cv::Mat & textures) const = 0;
+	virtual cv::Mat loadOptimizedMeshQuery(
+				std::vector<std::vector<std::vector<unsigned int> > > * polygons,
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+				std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
+#else
+				std::vector<std::vector<Eigen::Vector2f> > * texCoords,
+#endif
+				cv::Mat * textures) const = 0;
+
 	// Load objects
-	virtual void loadQuery(VWDictionary * dictionary) const = 0;
+	virtual void loadQuery(VWDictionary * dictionary, bool lastStateOnly = true) const = 0;
 	virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const = 0;
 	virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const = 0;
 	virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const = 0;
 	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 bool getLaserScanInfoQuery(int signatureId, LaserScan & info) const = 0;
+	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps) const = 0;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const = 0;
 	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const = 0;
 	virtual void getLastIdQuery(const std::string & tableName, int & id) const = 0;
@@ -181,7 +273,7 @@ private:
 
 private:
 	//non-abstract methods
-	void saveOrUpdate(const std::vector<Signature *> & signatures) const;
+	void saveOrUpdate(const std::vector<Signature *> & signatures);
 	void saveOrUpdate(const std::vector<VisualWord *> & words) const;
 
 	//thread stuff

corelib/include/rtabmap/core/DBDriverSqlite3.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,7 +31,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 #include "rtabmap/core/DBDriver.h"
 #include <opencv2/features2d/features2d.hpp>
-#include "sqlite3/sqlite3.h"
+
+typedef struct sqlite3_stmt sqlite3_stmt;
+typedef struct sqlite3 sqlite3;
 
 namespace rtabmap {
 
@@ -41,33 +43,44 @@ public:
 	virtual ~DBDriverSqlite3();
 
 	virtual void parseParameters(const ParametersMap & parameters);
+	virtual bool isInMemory() const {return getUrl().empty() || _dbInMemory;}
 	void setDbInMemory(bool dbInMemory);
 	void setJournalMode(int journalMode);
 	void setCacheSize(unsigned int cacheSize);
 	void setSynchronous(int synchronous);
 	void setTempStore(int tempStore);
 
-private:
+protected:
 	virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false);
-	virtual void disconnectDatabaseQuery(bool save = true);
+	virtual void disconnectDatabaseQuery(bool save = true, const std::string & outputUrl = "");
 	virtual bool isConnectedQuery() const;
 	virtual long getMemoryUsedQuery() const; // In bytes
 	virtual bool getDatabaseVersionQuery(std::string & version) const;
+	virtual long getNodesMemoryUsedQuery() const;
+	virtual long getLinksMemoryUsedQuery() const;
 	virtual long getImagesMemoryUsedQuery() const;
 	virtual long getDepthImagesMemoryUsedQuery() const;
+	virtual long getCalibrationsMemoryUsedQuery() const;
+	virtual long getGridsMemoryUsedQuery() const;
 	virtual long getLaserScansMemoryUsedQuery() const;
 	virtual long getUserDataMemoryUsedQuery() const;
 	virtual long getWordsMemoryUsedQuery() const;
+	virtual long getFeaturesMemoryUsedQuery() const;
+	virtual long getStatisticsMemoryUsedQuery() const;
 	virtual int getLastNodesSizeQuery() const;
 	virtual int getLastDictionarySizeQuery() const;
 	virtual int getTotalNodesSizeQuery() const;
 	virtual int getTotalDictionarySizeQuery() const;
+	virtual ParametersMap getLastParametersQuery() const;
+	virtual std::map<std::string, float> getStatisticsQuery(int nodeId, double & stamp, std::vector<int> * wmState) const;
+	virtual std::map<int, std::pair<std::map<std::string, float>, double> > getAllStatisticsQuery() const;
+	virtual std::map<int, std::vector<int> > getAllStatisticsWmStatesQuery() const;
 
 	virtual void executeNoResultQuery(const std::string & sql) const;
 
 	virtual void getWeightQuery(int signatureId, int & weight) const;
 
-	virtual void saveQuery(const std::list<Signature *> & signatures) const;
+	virtual void saveQuery(const std::list<Signature *> & signatures);
 	virtual void saveQuery(const std::list<VisualWord *> & words) const;
 	virtual void updateQuery(const std::list<Signature *> & signatures, bool updateTimestamp) const;
 	virtual void updateQuery(const std::list<VisualWord *> & words, bool updateTimestamp) const;
@@ -75,16 +88,54 @@ private:
 	virtual void addLinkQuery(const Link & link) const;
 	virtual void updateLinkQuery(const Link & link) const;
 
+	virtual void updateOccupancyGridQuery(
+			int nodeId,
+			const cv::Mat & ground,
+			const cv::Mat & obstacles,
+			const cv::Mat & empty,
+			float cellSize,
+			const cv::Point3f & viewpoint) const;
+
+	virtual void updateDepthImageQuery(
+			int nodeId,
+			const cv::Mat & image) const;
+
+	virtual void addStatisticsQuery(const Statistics & statistics) const;
+	virtual void savePreviewImageQuery(const cv::Mat & image) const;
+	virtual cv::Mat loadPreviewImageQuery() const;
+	virtual void saveOptimizedPosesQuery(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const;
+	virtual std::map<int, Transform> loadOptimizedPosesQuery(Transform * lastlocalizationPose) const;
+	virtual void save2DMapQuery(const cv::Mat & map, float xMin, float yMin, float cellSize) const;
+	virtual cv::Mat load2DMapQuery(float & xMin, float & yMin, float & cellSize) const;
+	virtual void saveOptimizedMeshQuery(
+			const cv::Mat & cloud,
+			const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
+#else
+			const std::vector<std::vector<Eigen::Vector2f> > & texCoords,
+#endif
+			const cv::Mat & textures) const;
+	virtual cv::Mat loadOptimizedMeshQuery(
+			std::vector<std::vector<std::vector<unsigned int> > > * polygons,
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
+#else
+			std::vector<std::vector<Eigen::Vector2f> > * texCoords,
+#endif
+			cv::Mat * textures) const;
+
 	// Load objects
-	virtual void loadQuery(VWDictionary * dictionary) const;
+	virtual void loadQuery(VWDictionary * dictionary, bool lastStateOnly = true) const;
 	virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const;
 	virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const;
 	virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const;
 	virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
 
-	virtual void loadNodeDataQuery(std::list<Signature *> & signatures) const;
+	virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const;
 	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
-	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose) const;
+	virtual bool getLaserScanInfoQuery(int signatureId, LaserScan & info) const;
+	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps) const;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const;
 	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const;
 	virtual void getLastIdQuery(const std::string & tableName, int & id) const;
@@ -96,29 +147,42 @@ private:
 	std::string queryStepNode() const;
 	std::string queryStepImage() const;
 	std::string queryStepDepth() const;
+	std::string queryStepDepthUpdate() const;
 	std::string queryStepSensorData() const;
 	std::string queryStepLinkUpdate() const;
 	std::string queryStepLink() const;
 	std::string queryStepWordsChanged() const;
 	std::string queryStepKeypoint() const;
+	std::string queryStepOccupancyGridUpdate() const;
 	void stepNode(sqlite3_stmt * ppStmt, const Signature * s) const;
 	void stepImage(
 			sqlite3_stmt * ppStmt,
 			int id,
 			const cv::Mat & imageBytes) const;
 	void stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
+	void stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const cv::Mat & imageCompressed) const;
 	void stepSensorData(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
 	void stepLink(sqlite3_stmt * ppStmt, const Link & link) const;
 	void stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
 	void stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp, const cv::Point3f & pt, const cv::Mat & descriptor) const;
+	void stepOccupancyGridUpdate(sqlite3_stmt * ppStmt,
+			int nodeId,
+			const cv::Mat & ground,
+			const cv::Mat & obstacles,
+			const cv::Mat & empty,
+			float cellSize,
+			const cv::Point3f & viewpoint) const;
 
 private:
 	void loadLinksQuery(std::list<Signature *> & signatures) const;
 	int loadOrSaveDb(sqlite3 *pInMemory, const std::string & fileName, int isSave) const;
 
-private:
+protected:
 	sqlite3 * _ppDb;
 	std::string _version;
+
+private:
+	long _memoryUsedEstimate;
 	bool _dbInMemory;
 	unsigned int _cacheSize;
 	int _journalMode;

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);
-	void setFrameRate(float frameRate);
-	OdometryEvent getNextData();
+	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 !_odometryIgnored;}
 
 protected:
-	virtual void mainLoopBegin();
-	virtual void mainLoop();
+	virtual SensorData captureImage(CameraInfo * info = 0);
+
+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
+		kFeatureKaze=9};    //new 0.13.2
 
 	static Feature2D * create(const ParametersMap & parameters = ParametersMap());
 	static Feature2D * create(Feature2D::Type type, const ParametersMap & parameters = ParametersMap()); // for convenience
@@ -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);
@@ -174,6 +177,8 @@ private:
 	int _subPixWinSize;
 	int _subPixIterations;
 	double _subPixEps;
+	int gridRows_;
+	int gridCols_;
 	// Stereo stuff
 	Stereo * _stereo;
 };
@@ -430,6 +435,33 @@ private:
 	cv::Ptr<CV_BRISK> brisk_;
 };
 
+//KAZE
+class RTABMAP_EXP KAZE : public Feature2D
+{
+public:
+	KAZE(const ParametersMap & parameters = ParametersMap());
+	virtual ~KAZE();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const { return kFeatureKaze; }
+
+private:
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) const;
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+private:
+	bool extended_;
+	bool upright_;
+	float threshold_;
+	int nOctaves_;
+	int nOctaveLayers_;
+	int diffusivity_;
+
+#if CV_MAJOR_VERSION > 2
+	cv::Ptr<cv::KAZE> kaze_;
+#endif
+};
+
 
 }
 

corelib/include/rtabmap/core/FlannIndex.h

@@ -0,0 +1,119 @@
+/*
+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,
+			float rebalancingFactor = 2.0f);
+	void buildKDTreeIndex(
+			const cv::Mat & features,
+			int trees = 4,
+			bool useDistanceL1 = false,
+			float rebalancingFactor = 2.0f);
+	void buildKDTreeSingleIndex(
+			const cv::Mat & features,
+			int leafMaxSize = 10,
+			bool reorder = true,
+			bool useDistanceL1 = false,
+			float rebalancingFactor = 2.0f);
+	void buildLSHIndex(
+			const cv::Mat & features,
+			unsigned int table_number = 12,
+			unsigned int key_size = 20,
+			unsigned int multi_probe_level = 2,
+			float rebalancingFactor = 2.0f);
+
+	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 (indices should be casted in size_t)
+	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
+	float rebalancingFactor_;
+
+	// 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,108 @@
+/*
+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.0, 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,
+			bool rgb = true) const;
+	void apply(
+			int id,
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+			const pcl::IndicesPtr & indices,
+			bool rgb = true) const;
+	void apply(
+			int id,
+			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+			const pcl::IndicesPtr & indices,
+			bool rgb = true) const;
+	void apply(
+			int id,
+			cv::Mat & image,
+			bool rgb = true) const;
+
+	double getGain(int id, double * r=0, double * g=0, double * b=0) const;
+	int getIndex(int id) const;
+
+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/GeodeticCoords.h

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

corelib/include/rtabmap/core/Graph.h

@@ -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,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <map>
 #include <list>
 #include <rtabmap/core/Link.h>
+#include <rtabmap/core/GeodeticCoords.h>
 
 namespace rtabmap {
 class Memory;
@@ -53,11 +54,54 @@ bool RTABMAP_EXP exportPoses(
 
 bool RTABMAP_EXP importPoses(
 		const std::string & filePath,
-		int format, // 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, GPS (t,x,y)
+		int format, // 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe
 		std::map<int, Transform> & poses,
 		std::multimap<int, Link> * constraints = 0, // optional for formats 3 and 4
 		std::map<int, double> * stamps = 0); // optional for format 1
 
+bool RTABMAP_EXP exportGPS(
+		const std::string & filePath,
+		const std::map<int, GPS> & gpsValues,
+		unsigned int rgba = 0xFFFFFFFF);
+
+/**
+ * Compute translation and rotation errors for KITTI datasets.
+ * See http://www.cvlibs.net/datasets/kitti/eval_odometry.php.
+ * @param poses_gt, Ground Truth poses
+ * @param poses_result, Estimated poses
+ * @param t_err, Output translation error (%)
+ * @param r_err, Output rotation error (deg/m)
+ */
+void RTABMAP_EXP calcKittiSequenceErrors(
+		const std::vector<Transform> &poses_gt,
+		const std::vector<Transform> &poses_result,
+		float & t_err,
+		float & r_err);
+
+/**
+ * Compute root-mean-square error (RMSE) like the TUM RGBD
+ * dataset's evaluation tool (absolute trajectory error).
+ * See https://vision.in.tum.de/data/datasets/rgbd-dataset
+ * @param groundTruth, Ground Truth poses
+ * @param poses, Estimated poses
+ * @return Gt to Map transform
+ */
+Transform RTABMAP_EXP calcRMSE(
+		const std::map<int, Transform> &groundTruth,
+		const std::map<int, Transform> &poses,
+		float & translational_rmse,
+		float & translational_mean,
+		float & translational_median,
+		float & translational_std,
+		float & translational_min,
+		float & translational_max,
+		float & rotational_rmse,
+		float & rotational_mean,
+		float & rotational_median,
+		float & rotational_std,
+		float & rotational_min,
+		float & rotational_max);
+
 std::multimap<int, Link>::iterator RTABMAP_EXP findLink(
 		std::multimap<int, Link> & links,
 		int from,
@@ -79,6 +123,15 @@ std::multimap<int, int>::const_iterator RTABMAP_EXP findLink(
 		int to,
 		bool checkBothWays = true);
 
+std::multimap<int, Link> RTABMAP_EXP filterDuplicateLinks(
+		const std::multimap<int, Link> & links);
+std::multimap<int, Link> RTABMAP_EXP filterLinks(
+		const std::multimap<int, Link> & links,
+		Link::Type filteredType);
+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,
@@ -176,6 +229,11 @@ int RTABMAP_EXP findNearestNode(
 		const std::map<int, rtabmap::Transform> & nodes,
 		const rtabmap::Transform & targetPose);
 
+std::vector<int> RTABMAP_EXP findNearestNodes(
+		const std::map<int, rtabmap::Transform> & nodes,
+		const rtabmap::Transform & targetPose,
+		int k);
+
 /**
  * Get nodes near the query
  * @param nodeId the query id
@@ -198,6 +256,10 @@ float RTABMAP_EXP computePathLength(
 		unsigned int fromIndex = 0,
 		unsigned int toIndex = 0);
 
+// assuming they are all linked in map order
+float RTABMAP_EXP computePathLength(
+		const std::map<int, Transform> & path);
+
 std::list<std::map<int, Transform> > RTABMAP_EXP getPaths(
 		std::map<int, Transform> poses,
 		const std::multimap<int, Link> & links);

corelib/include/rtabmap/core/IMU.h

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

corelib/include/rtabmap/core/IMUThread.h

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

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

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

corelib/include/rtabmap/core/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
@@ -46,20 +46,14 @@ public:
 		kUserClosure,
 		kVirtualClosure,
 		kNeighborMerged,
-		kUndef};
+		kPosePrior,
+		kUndef = 99};
 	Link();
 	Link(int from,
 			int to,
 			Type type,
 			const Transform & transform,
-			const cv::Mat & infMatrix = cv::Mat::eye(6,6,CV_64FC1),
-			const cv::Mat & userData = cv::Mat());
-	Link(int from,
-			int to,
-			Type type,
-			const Transform & transform,
-			double rotVariance,
-			double transVariance,
+			const cv::Mat & infMatrix = cv::Mat::eye(6,6,CV_64FC1), // information matrix: inverse of covariance matrix
 			const cv::Mat & userData = cv::Mat());
 
 	bool isValid() const {return from_ > 0 && to_ > 0 && !transform_.isNull() && type_!=kUndef;}
@@ -76,11 +70,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 +79,9 @@ public:
 	Link merge(const Link & link, Type outputType) const;
 	Link inverse() const;
 
+private:
+	void setInfMatrix(const cv::Mat & infMatrix);
+
 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
@@ -42,6 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UStl.h"
 #include <opencv2/core/core.hpp>
 #include <opencv2/features2d/features2d.hpp>
+#include <pcl/pcl_config.h>
 
 namespace rtabmap {
 
@@ -55,6 +56,7 @@ class Registration;
 class RegistrationInfo;
 class RegistrationIcp;
 class Stereo;
+class OccupancyGrid;
 
 class RTABMAP_EXP Memory
 {
@@ -74,12 +76,13 @@ public:
 	bool update(const SensorData & data,
 			const Transform & pose,
 			const cv::Mat & covariance,
+			const std::vector<float> & velocity = std::vector<float>(), // vx,vy,vz,vroll,vpitch,vyaw
 			Statistics * stats = 0);
 	bool init(const std::string & dbUrl,
 			bool dbOverwritten = false,
 			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 +92,34 @@ public:
 	std::set<int> reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, double & timeDbAccess);
 
 	int cleanup();
+	void saveStatistics(const Statistics & statistics);
+	void savePreviewImage(const cv::Mat & image) const;
+	cv::Mat loadPreviewImage() const;
+	void saveOptimizedPoses(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const;
+	std::map<int, Transform> loadOptimizedPoses(Transform * lastlocalizationPose) const;
+	void save2DMap(const cv::Mat & map, float xMin, float yMin, float cellSize) const;
+	cv::Mat load2DMap(float & xMin, float & yMin, float & cellSize) const;
+	void saveOptimizedMesh(
+			const cv::Mat & cloud,
+			const std::vector<std::vector<std::vector<unsigned int> > > & polygons = std::vector<std::vector<std::vector<unsigned int> > >(),      // Textures -> polygons -> vertices
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords = std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > >(), // Textures -> uv coords for each vertex of the polygons
+#else
+			const std::vector<std::vector<Eigen::Vector2f> > & texCoords = std::vector<std::vector<Eigen::Vector2f> >(), // Textures -> uv coords for each vertex of the polygons
+#endif
+			const cv::Mat & textures = cv::Mat()) const; // concatenated textures (assuming square textures with all same size)
+	cv::Mat loadOptimizedMesh(
+			std::vector<std::vector<std::vector<unsigned int> > > * polygons = 0,
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords = 0,
+#else
+			std::vector<std::vector<Eigen::Vector2f> > * texCoords = 0,
+#endif
+			cv::Mat * textures = 0) const;
 	void emptyTrash();
 	void joinTrashThread();
 	bool addLink(const Link & link, bool addInDatabase = false);
-	void updateLink(int fromId, int toId, const Transform & transform, float rotVariance, float transVariance);
-	void updateLink(int fromId, int toId, const Transform & transform, const cv::Mat & covariance);
+	void updateLink(const Link & link, bool updateInDatabase = false);
 	void removeAllVirtualLinks();
 	void removeVirtualLinks(int signatureId);
 	std::map<int, int> getNeighborsId(
@@ -103,6 +129,8 @@ public:
 			bool incrementMarginOnLoop = false,
 			bool ignoreLoopIds = false,
 			bool ignoreIntermediateNodes = false,
+			bool ignoreLocalSpaceLoopIds = false,
+			const std::set<int> & nodesSet = std::set<int>(),
 			double * dbAccessTime = 0) const;
 	std::map<int, float> getNeighborsIdRadius(
 			int signatureId,
@@ -110,6 +138,7 @@ public:
 			const std::map<int, Transform> & optimizedPoses,
 			int maxGraphDepth) const;
 	void deleteLocation(int locationId, std::list<int> * deletedWords = 0);
+	void saveLocationData(int locationId);
 	void removeLink(int idA, int idB);
 	void removeRawData(int id, bool image = true, bool scan = true, bool userData = true);
 
@@ -132,9 +161,17 @@ 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;
+	std::string getDatabaseUrl() const;
 	double getDbSavingTime() const;
 	Transform getOdomPose(int signatureId, bool lookInDatabase = false) const;
 	Transform getGroundTruthPose(int signatureId, bool lookInDatabase = false) const;
@@ -145,6 +182,8 @@ public:
 			std::string & label,
 			double & stamp,
 			Transform & groundTruth,
+			std::vector<float> & velocity,
+			GPS & gps,
 			bool lookInDatabase = false) const;
 	cv::Mat getImageCompressed(int signatureId) const;
 	SensorData getNodeData(int nodeId, bool uncompressedData = false) const;
@@ -155,7 +194,7 @@ public:
 	void getNodeCalibration(int nodeId,
 			std::vector<CameraModel> & models,
 			StereoCameraModel & stereoModel);
-	SensorData getSignatureDataConst(int locationId) const;
+	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;}
@@ -185,9 +224,8 @@ public:
 			std::multimap<int, Link> & links,
 			bool lookInDatabase = false);
 
-	Transform computeTransform(Signature & fromS, Signature & toS, Transform guess, RegistrationInfo * info = 0) const;
-	Transform computeTransform(int fromId, int toId, Transform guess, RegistrationInfo * info = 0);
-	Transform computeIcpTransform(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 computeIcpTransformMulti(
 			int newId,
 			int oldId,
@@ -198,6 +236,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);
@@ -235,6 +274,7 @@ private:
 	bool _rawDescriptorsKept;
 	bool _saveDepth16Format;
 	bool _notLinkedNodesKeptInDb;
+	bool _saveIntermediateNodeData;
 	bool _incrementalMemory;
 	bool _reduceGraph;
 	int _maxStMemSize;
@@ -244,14 +284,26 @@ private:
 	bool _generateIds;
 	bool _badSignaturesIgnored;
 	bool _mapLabelsAdded;
+	bool _depthAsMask;
 	int _imagePreDecimation;
 	int _imagePostDecimation;
+	bool _compressionParallelized;
 	float _laserScanDownsampleStepSize;
+	float _laserScanVoxelSize;
+	int _laserScanNormalK;
+	float _laserScanNormalRadius;
 	bool _reextractLoopClosureFeatures;
+	bool _localBundleOnLoopClosure;
 	float _rehearsalMaxDistance;
 	float _rehearsalMaxAngle;
 	bool _rehearsalWeightIgnoredWhileMoving;
 	bool _useOdometryFeatures;
+	bool _createOccupancyGrid;
+	int _visMaxFeatures;
+	int _visCorType;
+	bool _imagesAlreadyRectified;
+	bool _rectifyOnlyFeatures;
+	bool _covOffDiagonalIgnored;
 
 	int _idCount;
 	int _idMapCount;
@@ -260,6 +312,9 @@ private:
 	bool _memoryChanged; // False by default, become true only when Memory::update() is called.
 	bool _linksChanged; // False by default, become true when links are modified.
 	int _signaturesAdded;
+	GPS _gpsOrigin;
+	std::vector<CameraModel> _rectCameraModels;
+	StereoCameraModel _rectStereoCameraModel;
 
 	std::map<int, Signature *> _signatures; // TODO : check if a signature is already added? although it is not supposed to occur...
 	std::set<int> _stMem; // id
@@ -273,7 +328,9 @@ private:
 	bool _parallelized;
 
 	Registration * _registrationPipeline;
-	RegistrationIcp * _registrationIcp;
+	RegistrationIcp * _registrationIcpMulti;
+
+	OccupancyGrid * _occupancy;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/OccupancyGrid.h

@@ -0,0 +1,165 @@
+/*
+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:
+	inline static float logodds(double probability)
+	{
+		return (float) log(probability/(1-probability));
+	}
+
+	inline static double probability(double logodds)
+	{
+		return 1. - ( 1. / (1. + exp(logodds)));
+	}
+
+public:
+	OccupancyGrid(const ParametersMap & parameters = ParametersMap());
+	void parseParameters(const ParametersMap & parameters);
+	void setMap(const cv::Mat & map, float xMin, float yMin, float cellSize, const std::map<int, Transform> & poses);
+	void setCellSize(float cellSize);
+	float getCellSize() const {return cellSize_;}
+	void setCloudAssembling(bool enabled);
+	float getMinMapSize() const {return minMapSize_;}
+	bool isGridFromDepth() const {return occupancyFromDepth_;}
+	bool isFullUpdate() const {return fullUpdate_;}
+	float getUpdateError() const {return updateError_;}
+	bool isMapFrameProjection() const {return projMapFrame_;}
+	const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
+	int cacheSize() const {return (int)cache_.size();}
+	const std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > & getCache() const {return cache_;}
+
+	template<typename PointT>
+	typename pcl::PointCloud<PointT>::Ptr segmentCloud(
+			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 & groundCells,
+			cv::Mat & obstacleCells,
+			cv::Mat & emptyCells,
+			cv::Point3f & viewPoint) const;
+
+	void createLocalMap(
+			const LaserScan & cloud,
+			const Transform & pose,
+			cv::Mat & groundCells,
+			cv::Mat & obstacleCells,
+			cv::Mat & emptyCells,
+			cv::Point3f & viewPointInOut) const;
+
+	void clear();
+	void addToCache(
+			int nodeId,
+			const cv::Mat & ground,
+			const cv::Mat & obstacles,
+			const cv::Mat & empty);
+	void update(const std::map<int, Transform> & poses);
+	cv::Mat getMap(float & xMin, float & yMin) const;
+	cv::Mat getProbMap(float & xMin, float & yMin) const;
+	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapGround() const {return assembledGround_;}
+	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapObstacles() const {return assembledObstacles_;}
+	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapEmptyCells() const {return assembledEmptyCells_;}
+
+private:
+	ParametersMap parameters_;
+	int cloudDecimation_;
+	float cloudMaxDepth_;
+	float cloudMinDepth_;
+	std::vector<float> roiRatios_;
+	float footprintLength_;
+	float footprintWidth_;
+	float footprintHeight_;
+	int scanDecimation_;
+	float cellSize_;
+	bool preVoxelFiltering_;
+	bool occupancyFromDepth_;
+	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_;
+	bool rayTracing_;
+	bool fullUpdate_;
+	float minMapSize_;
+	bool erode_;
+	float footprintRadius_;
+	float updateError_;
+	float occupancyThr_;
+	float probHit_;
+	float probMiss_;
+	float probClampingMin_;
+	float probClampingMax_;
+
+	std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > cache_; //<node id, < <ground, obstacles>, empty> >
+	cv::Mat map_;
+	cv::Mat mapInfo_;
+	std::map<int, std::pair<int, int> > cellCount_; //<node Id, cells>
+	float xMin_;
+	float yMin_;
+	std::map<int, Transform> addedNodes_;
+
+	bool cloudAssembling_;
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledGround_;
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledObstacles_;
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledEmptyCells_;
+};
+
+}
+
+#include <rtabmap/core/impl/OccupancyGrid.hpp>
+
+#endif /* CORELIB_SRC_OCCUPANCYGRID_H_ */