Freenect2: Fixed initRectifyMap error with custom calibration

Octomap: changed warning log "Did not find...in cahe" to a debug log
GUI: Fixed loaded parameters not actually modified from opened database
2026-10-04 00:57:46 +08:00 · 2016-07-21 12:40:25 -04:00 · 2016-07-21 10:14:22 -04:00 · 2016-07-20 17:07:35 -04:00 · 2016-07-20 16:37:32 -04:00 · 2016-07-20 15:17:59 -04:00
243 changed files with 13103 additions and 5757 deletions
@@ -0,0 +1,7 @@
+/lib
+.DS_Store
+.settings/language.settings.xml
+app/android/.classpath
+app/android/.project
+app/android/AndroidManifest.xml
+app/android/res/raw/
@@ -0,0 +1,29 @@
+sudo: true
+dist: trusty
+language: cpp
+
+compiler:
+  - gcc
+  - clang
+
+addons:
+  apt:
+    packages:
+      - cmake
+      - libopencv-dev
+      - libqt4-dev
+      - libsqlite3-dev
+
+install:
+    - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list'
+    - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
+    - sudo apt-get update
+    - sudo apt-get -y install libpcl-1.7-all libfreenect-dev
+
+script: 
+    - mkdir -p build && cd build
+    - cmake .. 
+    - make
+
+notifications:
+  email: false
@@ -6,9 +6,10 @@ SET(PROJECT_PREFIX rtabmap)

 # Catkin doesn't support multiarch library path, 
 # fix to "lib" if not set by user.
-IF(NOT DEFINED CMAKE_INSTALL_LIBDIR)
+OPTION(MULTI_ARCH "Activate multi-arch lib directory (debian)" OFF)
+IF(NOT MULTI_ARCH AND NOT DEFINED CMAKE_INSTALL_LIBDIR)
   set(CMAKE_INSTALL_LIBDIR "lib")
-ENDIF(NOT DEFINED CMAKE_INSTALL_LIBDIR)
+ENDIF(NOT MULTI_ARCH AND NOT DEFINED CMAKE_INSTALL_LIBDIR)

 INCLUDE(GNUInstallDirs)

@@ -20,7 +21,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 11)
-SET(RTABMAP_PATCH_VERSION 2)
+SET(RTABMAP_PATCH_VERSION 8)
 SET(RTABMAP_VERSION
  ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
  
@@ -32,8 +33,6 @@ SET(PROJECT_VERSION_PATCH ${RTABMAP_PATCH_VERSION})

 SET(PROJECT_SOVERSION "${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR}")

-SET(RTABMAP_QT_VERSION 4 CACHE STRING "Which QT version to use")
-
 ####### COMPILATION PARAMS #######
 # In case of Makefiles if the user does not setup CMAKE_BUILD_TYPE, assume it's Release:
 IF(${CMAKE_GENERATOR} MATCHES ".*Makefiles")
@@ -138,11 +137,19 @@ option(WITH_TORO          "Include TORO support"                 ON)
 option(WITH_VERTIGO       "Include Vertigo support"              ON)
 option(WITH_CVSBA         "Include cvsba support"                ON)
 option(WITH_FLYCAPTURE2   "Include FlyCapture2/Triclops support" ON)
+option(WITH_ZED           "Include ZED sdk support"              ON)
+option(WITH_OCTOMAP       "Include Octomap support"              ON)
+option(PCL_OMP            "With PCL OMP implementations"         ON)

 FIND_PACKAGE(OpenCV REQUIRED QUIET)
 FIND_PACKAGE(PCL 1.7 REQUIRED QUIET)
 FIND_PACKAGE(ZLIB REQUIRED QUIET)

+# fix libproj.so not found on Xenial
+if(NOT "${PCL_LIBRARIES}" STREQUAL "")
+   list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4")
+endif()
+
 # OpenMP ("-fopenmp" should be added for flann included in PCL)
 # the gcc-4.2.1 coming with MacOS X is not compatible with the OpenMP pragmas we use, so disabling OpenMP for it
 if((NOT APPLE) OR (NOT CMAKE_COMPILER_IS_GNUCXX) OR (GCC_VERSION VERSION_GREATER 4.2.1) OR (CMAKE_CXX_COMPILER_ID STREQUAL "Clang"))
@@ -152,6 +159,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()
@@ -167,14 +177,22 @@ IF(ZLIB_FOUND)
 ENDIF(ZLIB_FOUND)

 IF(WITH_QT)
-# If Qt is here, the GUI will be built
-    IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
+    FIND_PACKAGE(VTK)
+    IF(NOT VTK_FOUND)
+        MESSAGE(FATAL_ERROR "VTK is required when using Qt. Set -DWITH_QT=OFF if you don't want gui tools.")
+    ENDIF(NOT VTK_FOUND)
+
+    # If Qt is here, the GUI will be built
+    # look for Qt5 (if vtk>5 is installed) before Qt4
+    IF("${VTK_MAJOR_VERSION}" GREATER 5)
+        FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui Svg QUIET)
+    ENDIF("${VTK_MAJOR_VERSION}" GREATER 5)
+
+    IF(NOT Qt5_FOUND)
        FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg)
-    ELSE()
-        FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui Svg)
-    ENDIF()
+    ENDIF(NOT Qt5_FOUND)
+        
    IF(QT4_FOUND OR Qt5_FOUND)
-        FIND_PACKAGE(VTK REQUIRED)
        IF("${VTK_MAJOR_VERSION}" EQUAL 5)
            FIND_PACKAGE(QVTK REQUIRED) # only for VTK 5
        ENDIF("${VTK_MAJOR_VERSION}" EQUAL 5)
@@ -198,12 +216,13 @@ IF(WITH_FREENECT2)
    ENDIF(freenect2_FOUND)
 ENDIF(WITH_FREENECT2)

-IF(WITH_OPENNI2)
+# IF PCL depends on OpenNI2 (already found), ignore WITH_OPENNI2
+IF(WITH_OPENNI2 OR OpenNI2_FOUND)
    FIND_PACKAGE(OpenNI2 QUIET)
    IF(OpenNI2_FOUND)
       MESSAGE(STATUS "Found OpenNI2: ${OpenNI2_INCLUDE_DIRS}")
    ENDIF(OpenNI2_FOUND)
-ENDIF(WITH_OPENNI2)
+ENDIF(WITH_OPENNI2 OR OpenNI2_FOUND)

 IF(WITH_DC1394)
    FIND_PACKAGE(DC1394 QUIET)
@@ -223,7 +242,57 @@ IF(WITH_GTSAM)
    FIND_PACKAGE(GTSAM QUIET)
 ENDIF(WITH_GTSAM)

-IF(G2O_FOUND OR GTSAM_FOUND)
+IF(WITH_FLYCAPTURE2)
+    FIND_PACKAGE(FlyCapture2 QUIET)
+    IF(FlyCapture2_FOUND)
+       MESSAGE(STATUS "Found FlyCapture2: ${FlyCapture2_INCLUDE_DIRS}")
+    ENDIF(FlyCapture2_FOUND)
+ENDIF(WITH_FLYCAPTURE2)
+
+IF(WITH_CVSBA)
+	FIND_PACKAGE(cvsba QUIET)
+    IF(cvsba_FOUND)
+       MESSAGE(STATUS "Found cvsba: ${cvsba_INCLUDE_DIRS}")
+    ENDIF(cvsba_FOUND)
+ENDIF(WITH_CVSBA)
+
+IF(WITH_ZED)
+	IF(WIN32) # Windows
+		SET(ZED_INCLUDE_DIRS $ENV{ZED_INCLUDE_DIRS})
+		if (CMAKE_CL_64) # 64 bits
+			SET(ZED_LIBRARIES $ENV{ZED_LIBRARIES_64})
+		else(CMAKE_CL_64) # 32 bits
+			message("32bits compilation is no more available with CUDA7.0")
+		endif(CMAKE_CL_64)
+		SET(ZED_LIBRARY_DIR $ENV{ZED_LIBRARY_DIR})
+		IF(ZED_LIBRARIES AND ZED_INCLUDE_DIRS)
+			SET(ZED_FOUND TRUE)
+			LINK_DIRECTORIES( ${LINK_DIRECTORIES} ${ZED_LIBRARY_DIR})
+		ENDIF(ZED_LIBRARIES AND ZED_INCLUDE_DIRS)
+	ELSE() # Linux
+		find_package(ZED 1 QUIET)
+	ENDIF(WIN32)
+	
+	IF(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_OCTOMAP)
+	FIND_PACKAGE(OCTOMAP QUIET)
+    IF(OCTOMAP_FOUND)
+       MESSAGE(STATUS "Found octomap: ${OCTOMAP_INCLUDE_DIRS}")
+    ENDIF(OCTOMAP_FOUND)
+ENDIF(WITH_OCTOMAP)
+
+IF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND)
   #Newest versions require std11
   IF(NOT MSVC)
     include(CheckCXXCompilerFlag)
@@ -237,21 +306,7 @@ IF(G2O_FOUND OR GTSAM_FOUND)
 		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)
-       MESSAGE(STATUS "Found FlyCapture2: ${FlyCapture2_INCLUDE_DIRS}")
-    ENDIF(FlyCapture2_FOUND)
-ENDIF(WITH_FLYCAPTURE2)
-
-IF(WITH_CVSBA)
-    FIND_PACKAGE(cvsba QUIET)
-    IF(cvsba_FOUND)
-       MESSAGE(STATUS "Found cvsba: ${cvsba_INCLUDE_DIRS}")
-    ENDIF(cvsba_FOUND)
-ENDIF(WITH_CVSBA)
+ENDIF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND)

 ####### OSX BUNDLE CMAKE_INSTALL_PREFIX #######
 IF(APPLE AND BUILD_AS_BUNDLE)
@@ -282,15 +337,24 @@ ENDIF(APPLE AND BUILD_AS_BUNDLE)


 ####### SOURCES (Projects) #######
+
+# CONF_DEPENDENCIES contains only dependencies not required by the headers
+SET(CONF_DEPENDENCIES 
+	${ZLIB_LIBRARIES}
+)
 IF(NOT (OPENCV_NONFREE_FOUND OR OPENCV_XFEATURES2D_FOUND))
   SET(NONFREE "//")
 ENDIF(NOT (OPENCV_NONFREE_FOUND OR OPENCV_XFEATURES2D_FOUND))
 IF(NOT G2O_FOUND)
   SET(G2O "//")
-ENDIF(NOT G2O_FOUND)
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${G2O_LIBRARIES})
+ENDIF()
 IF(NOT GTSAM_FOUND)
   SET(GTSAM "//")
-ENDIF(NOT GTSAM_FOUND)
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${GTSAM_LIBRARIES})
+ENDIF()
 IF(NOT WITH_TORO)
   SET(TORO "//")
 ENDIF(NOT WITH_TORO)
@@ -299,22 +363,44 @@ IF(NOT WITH_VERTIGO)
 ENDIF(NOT WITH_VERTIGO)
 IF(NOT cvsba_FOUND)
   SET(CVSBA "//")
-ENDIF(NOT cvsba_FOUND)
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${cvsba_LIBRARIES})
+ENDIF()
 IF(NOT Freenect_FOUND)
   SET(FREENECT "//")
-ENDIF(NOT Freenect_FOUND)
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${Freenect_LIBRARIES})
+ENDIF()
 IF(NOT freenect2_FOUND)
   SET(FREENECT2 "//")
-ENDIF(NOT freenect2_FOUND)
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${freenect2_LIBRARIES})
+ENDIF()
 IF(NOT OpenNI2_FOUND)
   SET(OPENNI2 "//")
-ENDIF(NOT OpenNI2_FOUND)
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${OpenNI2_LIBRARIES})
+ENDIF()
 IF(NOT DC1394_FOUND)
   SET(DC1394 "//")
-ENDIF(NOT DC1394_FOUND)
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${DC1394_LIBRARIES})
+ENDIF()
 IF(NOT FlyCapture2_FOUND)
   SET(FLYCAPTURE2 "//")
-ENDIF(NOT FlyCapture2_FOUND)
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${FlyCapture2_LIBRARIES})
+ENDIF()
+IF(NOT ZED_FOUND)
+   SET(ZED "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${ZED_LIBRARIES} ${CUDA_LIBRARIES}) 
+ENDIF()
+IF(NOT OCTOMAP_FOUND)
+   SET(OCTOMAP "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${OCTOMAP_LIBRARIES}) 
+ENDIF()
 IF(NOT (OpenCV_FOUND AND OpenCV_VERSION_MAJOR EQUAL 3))
   SET(OPENCV3 "//")
 ENDIF(NOT (OpenCV_FOUND AND OpenCV_VERSION_MAJOR EQUAL 3))
@@ -365,9 +451,14 @@ file(RELATIVE_PATH REL_LIB_DIR "${CMAKE_INSTALL_PREFIX}/${INSTALL_CMAKE_DIR}" "$
 set(CONF_INCLUDE_DIRS "${PROJECT_SOURCE_DIR}/corelib/include" 
 					  "${PROJECT_SOURCE_DIR}/guilib/include" 
 					  "${PROJECT_SOURCE_DIR}/utilite/include")
-set(CONF_LIB_DIR "${CMAKE_ARCHIVE_OUTPUT_DIRECTORY}")
+set(CONF_LIB_DIR "${CMAKE_ARCHIVE_OUTPUT_DIRECTORY} ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}")
 IF(QT4_FOUND OR Qt5_FOUND)
   set(CONF_WITH_GUI ON)
+   IF(QT4_FOUND)
+      set(CONF_QT_VERSION 4)
+   ELSE()
+      set(CONF_QT_VERSION 5)
+   ENDIF()
 ELSE()
   set(CONF_WITH_GUI OFF)
 ENDIF()
@@ -478,16 +569,17 @@ MESSAGE(STATUS "--------------------------------------------")
 MESSAGE(STATUS "Info :")
 MESSAGE(STATUS "  Version : ${RTABMAP_VERSION}")
 MESSAGE(STATUS "  CMAKE_INSTALL_PREFIX = ${CMAKE_INSTALL_PREFIX}")
-MESSAGE(STATUS "  CMAKE_BUILD_TYPE = ${CMAKE_BUILD_TYPE}")
-MESSAGE(STATUS "  BUILD_APP = ${BUILD_APP}")
-MESSAGE(STATUS "  BUILD_TOOLS = ${BUILD_TOOLS}")
-MESSAGE(STATUS "  BUILD_EXAMPLES = ${BUILD_EXAMPLES}")
+MESSAGE(STATUS "  CMAKE_BUILD_TYPE =     ${CMAKE_BUILD_TYPE}")
+MESSAGE(STATUS "  CMAKE_INSTALL_LIBDIR = ${CMAKE_INSTALL_LIBDIR}")
+MESSAGE(STATUS "  BUILD_APP =            ${BUILD_APP}")
+MESSAGE(STATUS "  BUILD_TOOLS =          ${BUILD_TOOLS}")
+MESSAGE(STATUS "  BUILD_EXAMPLES =       ${BUILD_EXAMPLES}")
 IF(NOT WIN32)
 	# see comment above for the BUILD_SHARED_LIBS option on Windows
-	MESSAGE(STATUS "  BUILD_SHARED_LIBS = ${BUILD_SHARED_LIBS}")
+	MESSAGE(STATUS "  BUILD_SHARED_LIBS =    ${BUILD_SHARED_LIBS}")
 ENDIF(NOT WIN32)
 IF(APPLE)
-MESSAGE(STATUS "  BUILD_AS_BUNDLE = ${BUILD_AS_BUNDLE}")
+MESSAGE(STATUS "  BUILD_AS_BUNDLE =      ${BUILD_AS_BUNDLE}")
 ENDIF(APPLE)
 MESSAGE(STATUS "  CMAKE_CXX_FLAGS = ${CMAKE_CXX_FLAGS}")

@@ -583,6 +675,26 @@ ELSE()
 MESSAGE(STATUS "  With cvsba                = NO (cvsba not found)")
 ENDIF()

+IF(ZED_FOUND)
+IF(CUDA_FOUND)
+MESSAGE(STATUS "  With ZED                  = YES (With CUDA)")
+ELSE()
+MESSAGE(STATUS "  With ZED                  = YES (Without CUDA)")
+ENDIF()
+ELSEIF(NOT WITH_ZED)
+MESSAGE(STATUS "  With ZED                  = NO (WITH_ZED=OFF)")
+ELSE()
+MESSAGE(STATUS "  With ZED                  = NO (ZED sdk not found)")
+ENDIF()
+
+IF(OCTOMAP_FOUND)
+MESSAGE(STATUS "  With OCTOMAP              = YES (License: BSD)")
+ELSEIF(NOT WITH_OCTOMAP)
+MESSAGE(STATUS "  With OCTOMAP              = NO (WITH_OCTOMAP=OFF)")
+ELSE()
+MESSAGE(STATUS "  With OCTOMAP              = NO (octomap not found)")
+ENDIF()
+
 IF(QT4_FOUND)
 MESSAGE(STATUS "  With Qt4                  = YES (License: Open Source or Commercial)")
 ELSEIF(Qt5_FOUND)
@@ -590,7 +702,7 @@ MESSAGE(STATUS "  With Qt5                  = YES (License: Open Source or Comme
 ELSEIF(NOT WITH_QT)
 MESSAGE(STATUS "  With Qt                   = NO (WITH_QT=OFF)")
 ELSE()
-MESSAGE(STATUS "  With Qt                   = NO (Qt not found, to use Qt5 you should set -DRTABMAP_QT_VERSION=5)")
+MESSAGE(STATUS "  With Qt                   = NO (Qt not found)")
 ENDIF()

 MESSAGE(STATUS "--------------------------------------------")
@@ -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
@@ -1,4 +1,4 @@
-rtabmap
+rtabmap [![Build Status](https://travis-ci.org/introlab/rtabmap.svg?branch=master)](https://travis-ci.org/introlab/rtabmap)
 =======

 RTAB-Map library and standalone application.
@@ -11,41 +11,41 @@ get_filename_component(RTABMap_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH)
 set(RTABMap_INCLUDE_DIRS "@CONF_INCLUDE_DIRS@")

 #core lib
-find_library(RTABMap_CORE_RELEASE NAMES rtabmap_core NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@")
-find_library(RTABMap_CORE_DEBUG NAMES rtabmap_cored NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@")
+find_library(RTABMap_CORE_RELEASE NAMES rtabmap_core NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
+find_library(RTABMap_CORE_DEBUG NAMES rtabmap_cored NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)

 IF(RTABMap_CORE_DEBUG AND RTABMap_CORE_RELEASE)
   SET(RTABMap_CORE
      debug ${RTABMap_CORE_DEBUG}
      optimized ${RTABMap_CORE_RELEASE}
   )
-ELSEIF(RTABMap_CORE_RELEASE)
-   SET(RTABMap_CORE ${RTABMap_CORE_RELEASE})
 ELSEIF(RTABMap_CORE_DEBUG)
   SET(RTABMap_CORE ${RTABMap_CORE_DEBUG})
+ELSE()
+   SET(RTABMap_CORE ${RTABMap_CORE_RELEASE})
 ENDIF()

 #utilite lib
-find_library(RTABMap_UTILITE_RELEASE NAMES rtabmap_utilite NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@")
-find_library(RTABMap_UTILITE_DEBUG NAMES rtabmap_utilited NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@")
+find_library(RTABMap_UTILITE_RELEASE NAMES rtabmap_utilite NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
+find_library(RTABMap_UTILITE_DEBUG NAMES rtabmap_utilited NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)

 IF(RTABMap_UTILITE_DEBUG AND RTABMap_UTILITE_RELEASE)
   SET(RTABMap_UTILITE
      debug ${RTABMap_UTILITE_DEBUG}
      optimized ${RTABMap_UTILITE_RELEASE}
   )
-ELSEIF(RTABMap_UTILITE_RELEASE)
-   SET(RTABMap_UTILITE ${RTABMap_UTILITE_RELEASE})
 ELSEIF(RTABMap_UTILITE_DEBUG)
   SET(RTABMap_UTILITE ${RTABMap_UTILITE_DEBUG})
+ELSE()
+   SET(RTABMap_UTILITE ${RTABMap_UTILITE_RELEASE})
 ENDIF()

 set(RTABMap_LIBRARIES ${RTABMap_CORE} ${RTABMap_UTILITE})

 #gui lib (OFF if RTAB-Map is not built with Qt)
 if(@CONF_WITH_GUI@)
-   find_library(RTABMap_GUI_RELEASE NAMES rtabmap_gui NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@")
-   find_library(RTABMap_GUI_DEBUG NAMES rtabmap_guid NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@")
+   find_library(RTABMap_GUI_RELEASE NAMES rtabmap_gui NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
+   find_library(RTABMap_GUI_DEBUG NAMES rtabmap_guid NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
   
   IF(RTABMap_GUI_DEBUG AND RTABMap_GUI_RELEASE)
      SET(RTABMap_GUI
@@ -61,13 +61,13 @@ if(@CONF_WITH_GUI@)
   set(RTABMap_LIBRARIES ${RTABMap_LIBRARIES} ${RTABMap_GUI})
 endif(@CONF_WITH_GUI@)

+# Dependencies
+set(RTABMap_LIBRARIES ${RTABMap_LIBRARIES} @CONF_DEPENDENCIES@)
+
 #backward compatibilities
-if(RTABMap_CORE)
-   set(RTABMAP_CORE ${RTABMap_CORE})
-endif(RTABMap_CORE)
-if(RTABMap_UTILITE)
-   set(RTABMAP_UTILITE ${RTABMap_UTILITE})
-endif(RTABMap_UTILITE)
+set(RTABMAP_CORE ${RTABMap_CORE})
+set(RTABMAP_UTILITE ${RTABMap_UTILITE})
 if(RTABMap_GUI)
   set(RTABMAP_GUI ${RTABMap_GUI})
-endif(RTABMap_GUI)
+   set(RTABMAP_QT_VERSION @CONF_QT_VERSION@)
+endif(RTABMap_GUI)
@@ -49,6 +49,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@CVSBA@#define RTABMAP_CVSBA
@DC1394@#define RTABMAP_DC1394
@FLYCAPTURE2@#define RTABMAP_FLYCAPTURE2
+@ZED@#define RTABMAP_ZED
+@OCTOMAP@#define RTABMAP_OCTOMAP

 #endif /* VERSION_H_ */

@@ -0,0 +1,11 @@
+eclipse.preferences.version=1
+org.eclipse.jdt.core.compiler.codegen.inlineJsrBytecode=enabled
+org.eclipse.jdt.core.compiler.codegen.targetPlatform=1.6
+org.eclipse.jdt.core.compiler.codegen.unusedLocal=preserve
+org.eclipse.jdt.core.compiler.compliance=1.6
+org.eclipse.jdt.core.compiler.debug.lineNumber=generate
+org.eclipse.jdt.core.compiler.debug.localVariable=generate
+org.eclipse.jdt.core.compiler.debug.sourceFile=generate
+org.eclipse.jdt.core.compiler.problem.assertIdentifier=error
+org.eclipse.jdt.core.compiler.problem.enumIdentifier=error
+org.eclipse.jdt.core.compiler.source=1.6
@@ -2,8 +2,8 @@
 <!-- BEGIN_INCLUDE(manifest) -->
 <manifest xmlns:android="http://schemas.android.com/apk/res/android"
        package="com.introlab.rtabmap"
-        android:versionCode="1"
-        android:versionName="0.11.2">
+        android:versionCode="9"
+        android:versionName="@RTABMAP_VERSION@">

    <uses-permission android:name="android.permission.CAMERA" />
    <uses-permission android:name="android.permission.READ_EXTERNAL_STORAGE" />
@@ -11,7 +11,7 @@
    <uses-permission android:name="android.permission.READ_FRAME_BUFFER" />
    <uses-permission android:name="android.permission.ACCESS_SURFACE_FLINGER" />
    <uses-feature android:glEsVersion="0x00020000" />
-    <uses-library android:name="com.projecttango.libtango_device" android:required="true" />
+    <uses-library android:name="com.projecttango.libtango_device2" android:required="true" />

    <!-- This is the platform API where NativeActivity was introduced. -->
    <uses-sdk android:minSdkVersion="17" />
@@ -14,10 +14,19 @@ if(NOT ANDROID_EXECUTABLE)
  message(FATAL_ERROR "Can not find android command line tool: android")
 endif()

+configure_file(
+  "${CMAKE_CURRENT_SOURCE_DIR}/AndroidManifest.xml.in"
+  "${CMAKE_CURRENT_SOURCE_DIR}/AndroidManifest.xml")
+  
 configure_file(
  "${CMAKE_CURRENT_SOURCE_DIR}/AndroidManifest.xml"
  "${CMAKE_CURRENT_BINARY_DIR}/AndroidManifest.xml"
  COPYONLY)
+  
+configure_file(
+  "${CMAKE_CURRENT_SOURCE_DIR}/info.txt.in"
+  "${CMAKE_CURRENT_SOURCE_DIR}/res/raw/info.txt")
+  

 configure_file(
  "${CMAKE_CURRENT_SOURCE_DIR}/ant.properties.in"
@@ -1,5 +1,5 @@
 <h3>Real-Time Appearance-Based Mapping</h3>
-Version 0.11.2<br>
+Version @RTABMAP_VERSION@<br>
 Author: Mathieu Labb&eacute;<br>
 Copyright 2016<br>
 IntRoLab - Universit&eacute; de Sherbrooke<br>
@@ -99,8 +99,12 @@ static rtabmap::Transform opticalRotation(
 CameraTango::CameraTango(int decimation, bool autoExposure) :
 		Camera(0, opticalRotation),
 		tango_config_(0),
+		firstFrame_(true),
 		decimation_(decimation),
-		autoExposure_(autoExposure)
+		autoExposure_(autoExposure),
+		cloudStamp_(0),
+		tangoColorType_(0),
+		tangoColorStamp_(0)
 {
 	UASSERT(decimation >= 1);
 }
@@ -125,8 +129,7 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string

 	// Set auto-recovery for motion tracking as requested by the user.
 	bool is_atuo_recovery = true;
-	int ret = TangoConfig_setBool(tango_config_, "config_enable_auto_recovery",
-	is_atuo_recovery);
+	int ret = TangoConfig_setBool(tango_config_, "config_enable_auto_recovery", is_atuo_recovery);
 	if (ret != TANGO_SUCCESS)
 	{
 		LOGE("NativeRTABMap: config_enable_auto_recovery() failed with error code: %d", ret);
@@ -141,21 +144,30 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 		return false;
 	}

-	// disable auto exposure
+	// disable auto exposure (disabled, seems broken on latest Tango releases)
 	ret = TangoConfig_setBool(tango_config_, "config_color_mode_auto", autoExposure_);
 	if (ret != TANGO_SUCCESS)
 	{
 		LOGE("NativeRTABMap: config_color_mode_auto() failed with error code: %d", ret);
-		return false;
+		//return false;
 	}
-	if(!autoExposure_)
+	else
 	{
-		ret = TangoConfig_setInt32(tango_config_, "config_color_iso", 800);
-		if (ret != TANGO_SUCCESS)
+		if(!autoExposure_)
 		{
-			LOGE("NativeRTABMap: config_color_iso() failed with error code: %d", ret);
-			return false;
+			ret = TangoConfig_setInt32(tango_config_, "config_color_iso", 800);
+			if (ret != TANGO_SUCCESS)
+			{
+				LOGE("NativeRTABMap: config_color_iso() failed with error code: %d", ret);
+				return false;
+			}
 		}
+		bool verifyAutoExposureState;
+		int32_t verifyIso, verifyExp;
+		TangoConfig_getBool( tango_config_, "config_color_mode_auto", &verifyAutoExposureState );
+		TangoConfig_getInt32( tango_config_, "config_color_iso", &verifyIso );
+		TangoConfig_getInt32( tango_config_, "config_color_exp", &verifyExp );
+		LOGI( "NativeRTABMap: config_color autoExposure=%s %d %d", verifyAutoExposureState?"On" : "Off", verifyIso, verifyExp );
 	}

 	// Enable depth.
@@ -173,15 +185,13 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 	ret = TangoConfig_setBool(tango_config_, "config_enable_low_latency_imu_integration", true);
 	if (ret != TANGO_SUCCESS)
 	{
-		LOGE("Failed to enable low latency imu integration.");
+		LOGE("NativeRTABMap: Failed to enable low latency imu integration.");
 		return false;
 	}

 	// Get TangoCore version string from service.
 	char tango_core_version[kVersionStringLength];
-	ret = TangoConfig_getString(
-	tango_config_, "tango_service_library_version",
-	tango_core_version, kVersionStringLength);
+	ret = TangoConfig_getString(tango_config_, "tango_service_library_version", tango_core_version, kVersionStringLength);
 	if (ret != TANGO_SUCCESS)
 	{
 		LOGE("NativeRTABMap: get tango core version failed with error code: %d", ret);
@@ -197,14 +207,14 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 	ret = TangoService_connectOnXYZijAvailable(onPointCloudAvailableRouter);
 	if (ret != TANGO_SUCCESS)
 	{
-		LOGE("PointCloudApp: Failed to connect to point cloud callback with error code: %d", ret);
+		LOGE("NativeRTABMap: Failed to connect to point cloud callback with error code: %d", ret);
 		return false;
 	}

 	ret = TangoService_connectOnFrameAvailable(TANGO_CAMERA_COLOR, this, onFrameAvailableRouter);
 	if (ret != TANGO_SUCCESS)
 	{
-		LOGE("PointCloudApp: Failed to connect to color callback with error code: %d", ret);
+		LOGE("NativeRTABMap: Failed to connect to color callback with error code: %d", ret);
 		return false;
 	}

@@ -216,7 +226,7 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 	ret = TangoService_connectOnPoseAvailable(1, &pair, onPoseAvailableRouter);
 	if (ret != TANGO_SUCCESS)
 	{
-		LOGE("PointCloudApp: Failed to connect to pose callback with error code: %d", ret);
+		LOGE("NativeRTABMap: Failed to connect to pose callback with error code: %d", ret);
 		return false;
 	}

@@ -234,78 +244,78 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 	ret = TangoService_connect(this, tango_config_);
 	if (ret != TANGO_SUCCESS)
 	{
-		LOGE("PointCloudApp: Failed to connect to the Tango service with error code: %d", ret);
+		LOGE("NativeRTABMap: Failed to connect to the Tango service with error code: %d", ret);
 		return false;
 	}

 	// update extrinsics
-		LOGI("NativeRTABMap: Update extrinsics");
-		TangoPoseData pose_data;
-		TangoCoordinateFramePair frame_pair;
+	LOGI("NativeRTABMap: Update extrinsics");
+	TangoPoseData pose_data;
+	TangoCoordinateFramePair frame_pair;

-		// TangoService_getPoseAtTime function is used for query device extrinsics
-		// as well. We use timestamp 0.0 and the target frame pair to get the
-		// extrinsics from the sensors.
-		//
-		// Get device with respect to imu transformation matrix.
-		frame_pair.base = TANGO_COORDINATE_FRAME_IMU;
-		frame_pair.target = TANGO_COORDINATE_FRAME_DEVICE;
-		ret = TangoService_getPoseAtTime(0.0, frame_pair, &pose_data);
-		if (ret != TANGO_SUCCESS)
-		{
-			LOGE("PointCloudApp: Failed to get transform between the IMU frame and device frames");
-			return false;
-		}
-		imuTDevice_ = rtabmap::Transform(
-				pose_data.translation[0],
-				pose_data.translation[1],
-				pose_data.translation[2],
-				pose_data.orientation[0],
-				pose_data.orientation[1],
-				pose_data.orientation[2],
-				pose_data.orientation[3]);
+	// TangoService_getPoseAtTime function is used for query device extrinsics
+	// as well. We use timestamp 0.0 and the target frame pair to get the
+	// extrinsics from the sensors.
+	//
+	// Get device with respect to imu transformation matrix.
+	frame_pair.base = TANGO_COORDINATE_FRAME_IMU;
+	frame_pair.target = TANGO_COORDINATE_FRAME_DEVICE;
+	ret = TangoService_getPoseAtTime(0.0, frame_pair, &pose_data);
+	if (ret != TANGO_SUCCESS)
+	{
+		LOGE("NativeRTABMap: Failed to get transform between the IMU frame and device frames");
+		return false;
+	}
+	imuTDevice_ = rtabmap::Transform(
+			pose_data.translation[0],
+			pose_data.translation[1],
+			pose_data.translation[2],
+			pose_data.orientation[0],
+			pose_data.orientation[1],
+			pose_data.orientation[2],
+			pose_data.orientation[3]);

-		// Get color camera with respect to imu transformation matrix.
-		frame_pair.base = TANGO_COORDINATE_FRAME_IMU;
-		frame_pair.target = TANGO_COORDINATE_FRAME_CAMERA_DEPTH;
-		ret = TangoService_getPoseAtTime(0.0, frame_pair, &pose_data);
-		if (ret != TANGO_SUCCESS)
-		{
-			LOGE("PointCloudApp: Failed to get transform between the color camera frame and device frames");
-			return false;
-		}
-		imuTDepthCamera_ = rtabmap::Transform(
-				pose_data.translation[0],
-				pose_data.translation[1],
-				pose_data.translation[2],
-				pose_data.orientation[0],
-				pose_data.orientation[1],
-				pose_data.orientation[2],
-				pose_data.orientation[3]);
+	// Get color camera with respect to imu transformation matrix.
+	frame_pair.base = TANGO_COORDINATE_FRAME_IMU;
+	frame_pair.target = TANGO_COORDINATE_FRAME_CAMERA_DEPTH;
+	ret = TangoService_getPoseAtTime(0.0, frame_pair, &pose_data);
+	if (ret != TANGO_SUCCESS)
+	{
+		LOGE("NativeRTABMap: Failed to get transform between the color camera frame and device frames");
+		return false;
+	}
+	imuTDepthCamera_ = rtabmap::Transform(
+			pose_data.translation[0],
+			pose_data.translation[1],
+			pose_data.translation[2],
+			pose_data.orientation[0],
+			pose_data.orientation[1],
+			pose_data.orientation[2],
+			pose_data.orientation[3]);

-		deviceTDepth_ = imuTDevice_.inverse() * imuTDepthCamera_;
+	deviceTDepth_ = imuTDevice_.inverse() * imuTDepthCamera_;

-		// camera intrinsic
-		TangoCameraIntrinsics color_camera_intrinsics;
-		ret = TangoService_getCameraIntrinsics(TANGO_CAMERA_COLOR, &color_camera_intrinsics);
-		if (ret != TANGO_SUCCESS)
-		{
-			LOGE("SynchronizationApplication: Failed to get the intrinsics for the color camera with error code: %d.", ret);
-			return false;
-		}
-		model_ = CameraModel(
-				color_camera_intrinsics.fx,
-				color_camera_intrinsics.fy,
-				color_camera_intrinsics.cx,
-				color_camera_intrinsics.cy,
-				this->getLocalTransform());
-		model_.setImageSize(cv::Size(color_camera_intrinsics.width, color_camera_intrinsics.height));
+	// camera intrinsic
+	TangoCameraIntrinsics color_camera_intrinsics;
+	ret = TangoService_getCameraIntrinsics(TANGO_CAMERA_COLOR, &color_camera_intrinsics);
+	if (ret != TANGO_SUCCESS)
+	{
+		LOGE("NativeRTABMap: Failed to get the intrinsics for the color camera with error code: %d.", ret);
+		return false;
+	}
+	model_ = CameraModel(
+			color_camera_intrinsics.fx,
+			color_camera_intrinsics.fy,
+			color_camera_intrinsics.cx,
+			color_camera_intrinsics.cy,
+			this->getLocalTransform());
+	model_.setImageSize(cv::Size(color_camera_intrinsics.width, color_camera_intrinsics.height));

-		// optical rotation
-		model_.setLocalTransform(Transform(
-				0.0f, 0.0f, 1.0f, 0.0f,
-				-1.0f, 0.0f, 0.0f, 0.0f,
-				0.0f, -1.0f, 0.0f, 0.0f));
+	// optical rotation
+	model_.setLocalTransform(Transform(
+			0.0f, 0.0f, 1.0f, 0.0f,
+			-1.0f, 0.0f, 0.0f, 0.0f,
+			0.0f, -1.0f, 0.0f, 0.0f));

 	return true;
 }
@@ -318,6 +328,7 @@ void CameraTango::close()
 		tango_config_ = nullptr;
 		TangoService_disconnect();
 	}
+	firstFrame_ = true;
 }

 void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
@@ -374,7 +385,6 @@ void CameraTango::poseReceived(const Transform & pose)

 void CameraTango::tangoEventReceived(int type, const char * key, const char * value)
 {
-	LOGE("Tango event: %s:%s", key, value);
 	this->post(new CameraTangoEvent(type, key, value));
 }

@@ -454,7 +464,7 @@ rtabmap::Transform CameraTango::getPoseAtTimestamp(double timestamp, bool inOpen
 	return pose;
 }

-SensorData CameraTango::captureImage()
+SensorData CameraTango::captureImage(CameraInfo * info)
 {
 	LOGI("Capturing image...");

@@ -624,7 +634,9 @@ void CameraTango::mainLoop()
 		{
 			rtabmap::Transform pose = data.groundTruth();
 			data.setGroundTruth(Transform());
-			this->post(new OdometryEvent(data, pose, 0.0001, 0.0001));
+			LOGI("Publish odometry message (variance=%f)", firstFrame_?9999:0.0001);
+			this->post(new OdometryEvent(data, pose, firstFrame_?9999:0.0001, firstFrame_?9999:0.0001));
+			firstFrame_ = false;
 		}
 		else if(!this->isKilled())
 		{
@@ -77,6 +77,7 @@ public:
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
 	rtabmap::Transform tangoPoseToTransform(const TangoPoseData * tangoPose, bool inOpenGLFrame) const;
+	void setDecimation(int value) {decimation_ = value;}
 	void setAutoExposure(bool enabled) {autoExposure_ = enabled;}

 	void cloudReceived(const cv::Mat & cloud, double timestamp);
@@ -85,7 +86,7 @@ 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);
@@ -95,6 +96,7 @@ private:

 private:
 	void * tango_config_;
+	bool firstFrame_;
 	int decimation_;
 	bool autoExposure_;
 	cv::Mat cloud_;
@@ -36,63 +36,53 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Graph.h>
 #include <rtabmap/utilite/UEventsManager.h>
 #include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UDirectory.h>
+#include <rtabmap/utilite/UFile.h>
 #include <opencv2/opencv_modules.hpp>
 #include <rtabmap/core/util3d_surface.h>
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/core/ParamEvent.h>
+#include <rtabmap/core/Compression.h>
+#include <rtabmap/core/Optimizer.h>
+#include <rtabmap/core/VWDictionary.h>
+#include <rtabmap/core/Memory.h>
 #include <pcl/filters/extract_indices.h>
 #include <pcl/io/ply_io.h>
+#include <pcl/io/obj_io.h>

 const int kVersionStringLength = 128;
-const int cameraTangoDecimation = 2;
-const int renderingCloudDecimation = 8;
-const float renderingCloudMaxDepth = 4.0f;
-const int maxFeatures = 400;
-const float meshAngleTolerance = 0.1745; // 10 degrees
-const int meshTrianglePixels = 1;
-const bool substractFiltering = false;
-const float subtractRadius = 0.02;
-const float subtractMaxAngle = M_PI/4.0f;
-const int minNeighborsInRadius = 5;
-const float closeVerticesDistance = 0.02f;

 static JavaVM *jvm;
 static jobject RTABMapActivity = 0;

+namespace {
+constexpr int kTangoCoreMinimumVersion = 9377;
+}  // anonymous namespace.
+
 rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
 {
 	rtabmap::ParametersMap parameters;

-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapLoopThr(), "0.11"));
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDLoopClosureReextractFeatures(), std::string("false")));
+	parameters.insert(mappingParameters_.begin(), mappingParameters_.end());
+
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpDetectorStrategy(), std::string("6"))); // GFTT/BRIEF
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kGFTTQualityLevel(), std::string("0.0001")));
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kGFTTMinDistance(), std::string("10")));
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemImagePreDecimation(), std::string(fullResolution_?"2":"1")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kFASTThreshold(), std::string("1")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kBRIEFBytes(), std::string("64")));
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemImageKept(), "false"));
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapTimeThr(), std::string("800")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemBinDataKept(), uBool2Str(!trajectoryMode_)));
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemRawDescriptorsKept(), "true")); // for visual registration
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemNotLinkedNodesKept(), std::string("false")));
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpNNStrategy(), std::string("1"))); // Kd-tree
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpMaxFeatures(), std::string("200")));
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpNndrRatio(), std::string("0.8"))); // set the one for kd-tree
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpMaxFeatures(), !loopClosureDetection_?std::string("-1"):uNumber2Str(maxFeatures))); // Kd-tree
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), uNumber2Str(graphOptimization_?rtabmap::Parameters::defaultOptimizerIterations():0)));
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), graphOptimization_?"10":"0"));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), uBool2Str(!localizationMode_)));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapMaxRetrieved(), uBool2Str(!localizationMode_)));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpMaxDepth(), std::string("10"))); // to avoid extracting features in invalid depth (as we compute transformation directly from the words)
-	//parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemImageDecimation(), std::string("2")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDOptimizeFromGraphEnd(), std::string("true")));
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDOptimizeMaxError(), std::string("1")));
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerRobust(), std::string("false")));
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerVarianceIgnored(), std::string("false")));
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapTimeThr(), std::string("700")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), std::string("true")));
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemUseDepthAsMask(), std::string("true")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisMinInliers(), std::string("15")));
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisRefineIterations(), std::string("5")));
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisEstimationType(), std::string("0"))); // 0=3D-3D 1=PnP
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDOptimizeMaxError(), std::string("0.05")));

 	return parameters;
 }
@@ -100,19 +90,26 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
 RTABMapApp::RTABMapApp() :
 		camera_(0),
 		rtabmapThread_(0),
+		rtabmap_(0),
 		logHandler_(0),
-		mapCloudShown_(true),
 		odomCloudShown_(true),
-		loopClosureDetection_(true),
 		graphOptimization_(true),
+		nodesFiltering_(false),
 		localizationMode_(false),
 		trajectoryMode_(false),
 		autoExposure_(false),
+		fullResolution_(false),
+		maxCloudDepth_(0.0),
+		meshTrianglePix_(1),
+		meshAngleToleranceDeg_(10.0),
 		clearSceneOnNextRender_(false),
 		totalPoints_(0),
 		totalPolygons_(0),
-		lastDrawnCloudsCount_(0)
+		lastDrawnCloudsCount_(0),
+		renderingFPS_(0.0f)
+
 {
+
 }

 RTABMapApp::~RTABMapApp() {
@@ -131,22 +128,19 @@ RTABMapApp::~RTABMapApp() {
  }
 }

-int RTABMapApp::TangoInitialize(JNIEnv* env, jobject caller_activity)
+void RTABMapApp::onCreate(JNIEnv* env, jobject caller_activity)
 {
-
 	env->GetJavaVM(&jvm);
 	RTABMapActivity = env->NewGlobalRef(caller_activity);

-	LOGI("RTABMapApp::TangoInitialize()");
+	LOGI("RTABMapApp::onCreate()");
 	createdMeshes_.clear();
-	previousCloud_.first = 0;
-	previousCloud_.second.first.reset();
-	previousCloud_.second.second.reset();
 	rawPoses_.clear();
 	clearSceneOnNextRender_ = true;
 	totalPoints_ = 0;
 	totalPolygons_ = 0;
 	lastDrawnCloudsCount_ = 0;
+	renderingFPS_ = 0.0f;

 	if(camera_)
 	{
@@ -157,6 +151,7 @@ int RTABMapApp::TangoInitialize(JNIEnv* env, jobject caller_activity)
 		rtabmapThread_->close(false);
 	    delete rtabmapThread_;
 	    rtabmapThread_ = 0;
+	    rtabmap_ = 0;
 	}

 	if(logHandler_ == 0)
@@ -168,13 +163,7 @@ int RTABMapApp::TangoInitialize(JNIEnv* env, jobject caller_activity)

 	this->registerToEventsManager();

-	camera_ = new rtabmap::CameraTango(cameraTangoDecimation, autoExposure_);
-
-
-  // The first thing we need to do for any Tango enabled application is to
-  // initialize the service. We'll do that here, passing on the JNI environment
-  // and jobject corresponding to the Android activity that is calling us.
-  return TangoService_initialize(env, caller_activity);
+	camera_ = new rtabmap::CameraTango(fullResolution_?1:2, autoExposure_);
 }

 void RTABMapApp::openDatabase(const std::string & databasePath)
@@ -187,20 +176,21 @@ void RTABMapApp::openDatabase(const std::string & databasePath)
 		rtabmapThread_->close(false);
 		delete rtabmapThread_;
 		rtabmapThread_ = 0;
+		rtabmap_ = 0;
 	}

 	//Rtabmap
-	rtabmap::Rtabmap * rtabmap = new rtabmap::Rtabmap();
+	rtabmap_ = new rtabmap::Rtabmap();
 	rtabmap::ParametersMap parameters = getRtabmapParameters();

-	rtabmap->init(parameters, databasePath);
-	rtabmapThread_ = new rtabmap::RtabmapThread(rtabmap);
+	rtabmap_->init(parameters, databasePath);
+	rtabmapThread_ = new rtabmap::RtabmapThread(rtabmap_);

 	// Generate all meshes
 	std::map<int, rtabmap::Signature> signatures;
 	std::map<int, rtabmap::Transform> poses;
 	std::multimap<int, rtabmap::Link> links;
-	rtabmap->get3DMap(
+	rtabmap_->get3DMap(
 			signatures,
 			poses,
 			links,
@@ -210,6 +200,9 @@ void RTABMapApp::openDatabase(const std::string & databasePath)
 	clearSceneOnNextRender_ = true;
 	rtabmap::Statistics stats;
 	stats.setSignatures(signatures);
+	stats.addStatistic(rtabmap::Statistics::kMemoryWorking_memory_size(), (float)rtabmap_->getWMSize());
+	stats.addStatistic(rtabmap::Statistics::kKeypointDictionary_size(), (float)rtabmap_->getMemory()->getVWDictionary()->getVisualWords().size());
+	stats.addStatistic(rtabmap::Statistics::kMemoryDatabase_memory_used(), (float)rtabmap_->getMemory()->getDatabaseMemoryUsed());
 	stats.setPoses(poses);
 	stats.setConstraints(links);
 	rtabmapEvents_.push_back(stats);
@@ -225,22 +218,27 @@ void RTABMapApp::openDatabase(const std::string & databasePath)
 	rtabmapMutex_.unlock();
 }

-int RTABMapApp::onResume()
+bool RTABMapApp::onTangoServiceConnected(JNIEnv* env, jobject iBinder)
 {
-	LOGW("onResume()");
+	LOGW("onTangoServiceConnected()");
 	if(camera_)
 	{
 		camera_->join(true);

+		if (TangoService_setBinder(env, iBinder) != TANGO_SUCCESS) {
+		    LOGE("TangoHandler::ConnectTango, TangoService_setBinder error");
+		    return false;
+		}
+
 		if(camera_->init())
 		{
 			LOGI("Start camera thread");
 			camera_->start();
-			return TANGO_SUCCESS;
+			return true;
 		}
 		LOGE("Failed camera initialization!");
 	}
-	return TANGO_ERROR;
+	return false;
 }

 void RTABMapApp::onPause()
@@ -272,6 +270,20 @@ void RTABMapApp::SetViewPort(int width, int height)
 	main_scene_.SetupViewPort(width, height);
 }

+class PostRenderEvent : public UEvent
+{
+public:
+	PostRenderEvent(const rtabmap::Statistics & stats) :
+		stats_(stats)
+	{
+
+	}
+	virtual std::string getClassName() const {return "PostRenderEvent";}
+	const rtabmap::Statistics & getStats() const {return stats_;}
+private:
+	rtabmap::Statistics stats_;
+};
+
 // OpenGL thread
 int RTABMapApp::Render()
 {
@@ -296,13 +308,11 @@ int RTABMapApp::Render()
 		main_scene_.clear();
 		clearSceneOnNextRender_ = false;
 		createdMeshes_.clear();
-		previousCloud_.first = 0;
-		previousCloud_.second.first.reset();
-		previousCloud_.second.second.reset();
 		rawPoses_.clear();
 		totalPoints_ = 0;
 		totalPolygons_ = 0;
 		lastDrawnCloudsCount_ = 0;
+		renderingFPS_ = 0.0f;
 	}

 	// Process events
@@ -374,20 +384,14 @@ int RTABMapApp::Render()
 			}
 		}

+		const std::multimap<int, rtabmap::Link> & links = rtabmapEvents.back().constraints();
 		if(poses.size())
 		{
-			const std::multimap<int, rtabmap::Link> & links = rtabmapEvents.back().constraints();
-
 			//update graph
 			main_scene_.updateGraph(poses, links);

 			// update clouds
-
-			//filter poses?
-
-			// make sure the last pose is here though
-			//poses.insert(*rtabmapEvents.back().poses().rbegin());
-
+			boost::mutex::scoped_lock  lock(meshesMutex_);
 			std::set<std::string> strIds;
 			for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 			{
@@ -399,6 +403,15 @@ int RTABMapApp::Render()
 						//just update pose
 						main_scene_.setCloudPose(id, iter->second);
 						main_scene_.setCloudVisible(id, true);
+						std::map<int, Mesh>::iterator meshIter = createdMeshes_.find(id);
+						if(meshIter!=createdMeshes_.end())
+						{
+							meshIter->second.pose = iter->second;
+						}
+						else
+						{
+							UERROR("Not found mesh %d !?!?", id);
+						}
 					}
 					else if(uContains(bufferedSensorData, id))
 					{
@@ -406,73 +419,46 @@ int RTABMapApp::Render()
 						if(!data.imageRaw().empty() && !data.depthRaw().empty())
 						{
 							// Voxelize and filter depending on the previous cloud?
-							pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudWithoutNormals;
+							pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 							pcl::IndicesPtr indices(new std::vector<int>);
 							LOGI("Creating node cloud %d (image size=%dx%d)", id, data.imageRaw().cols, data.imageRaw().rows);
-							cloudWithoutNormals = rtabmap::util3d::cloudRGBFromSensorData(data, renderingCloudDecimation, renderingCloudMaxDepth, 0, 0, indices.get());
-
-							//compute normals
-							pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud = rtabmap::util3d::computeNormals(cloudWithoutNormals, 6);
+							cloud = rtabmap::util3d::cloudRGBFromSensorData(data, data.imageRaw().rows/data.depthRaw().rows, maxCloudDepth_, 0, indices.get());

 							if(cloud->size() && indices->size())
 							{
 								UTimer time;

-								// substract? set points to NaN which are over previous cloud
-								pcl::IndicesPtr indicesKept = indices;
-								if(substractFiltering &&
-									subtractRadius > 0.0 &&
-									indices->size() &&
-									previousCloud_.first > 0 &&
-									previousCloud_.second.first.get() != 0 &&
-									previousCloud_.second.second.get() != 0 &&
-									previousCloud_.second.second->size() &&
-									poses.find(previousCloud_.first) != poses.end())
-								{
-									rtabmap::Transform t = iter->second.inverse() * poses.at(previousCloud_.first);
-									pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr previousCloud = rtabmap::util3d::transformPointCloud(previousCloud_.second.first, t);
-									indicesKept = rtabmap::util3d::subtractFiltering(
-											cloud,
-											indices,
-											previousCloud,
-											previousCloud_.second.second,
-											subtractRadius,
-											subtractMaxAngle,
-											minNeighborsInRadius);
-									UINFO("Subtraction %fs", time.ticks());
-								}
-
-								previousCloud_.first = id;
-								previousCloud_.second.first = cloud;
-								previousCloud_.second.second = indices;
-
 								// pcl::organizedFastMesh doesn't take indices, so set to NaN points we don't need to mesh
-								pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
-								pcl::ExtractIndices<pcl::PointXYZRGBNormal> filter;
-								filter.setIndices(indicesKept);
+								pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
+								pcl::ExtractIndices<pcl::PointXYZRGB> filter;
+								filter.setIndices(indices);
 								filter.setKeepOrganized(true);
 								filter.setInputCloud(cloud);
 								filter.filter(*output);
-								LOGE("Filtering %d from %d -> %d (%fs)", (int)indices->size(), (int)indices->size(), (int)indicesKept->size(), time.ticks());

-								std::vector<pcl::Vertices> polygons = rtabmap::util3d::organizedFastMesh(output, meshAngleTolerance, false, meshTrianglePixels);
-								pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr outputCloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+								std::vector<pcl::Vertices> polygons = rtabmap::util3d::organizedFastMesh(output, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
+								pcl::PointCloud<pcl::PointXYZRGB>::Ptr outputCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 								std::vector<pcl::Vertices> outputPolygons;
-								rtabmap::util3d::filterNotUsedVerticesFromMesh(
-										*output,
-										polygons,
-										*outputCloud,
-										outputPolygons);
+
+								outputCloud = output;
+								outputPolygons = polygons;
+
 								LOGI("Creating mesh, %d polygons (%fs)", (int)outputPolygons.size(), time.ticks());

-								if(outputCloud->size())
+								if(outputCloud->size() && outputPolygons.size())
 								{
 									totalPolygons_ += outputPolygons.size();
-									main_scene_.addOrUpdateCloud(id, outputCloud, outputPolygons, iter->second);
+
+									main_scene_.addCloud(id, outputCloud, outputPolygons, iter->second, data.imageRaw());
+

 									// protect createdMeshes_ used also by exportMesh() method
-									boost::mutex::scoped_lock  lock(meshesMutex_);
-									createdMeshes_.insert(std::make_pair(id, std::make_pair(std::make_pair(outputCloud, outputPolygons), iter->second)));
+									std::pair<std::map<int, Mesh>::iterator, bool> inserted = createdMeshes_.insert(std::make_pair(id, Mesh()));
+									UASSERT(inserted.second);
+									inserted.first->second.cloud = outputCloud;
+									inserted.first->second.polygons = outputPolygons;
+									inserted.first->second.pose = iter->second;
+									inserted.first->second.texture = data.imageCompressed();
 								}
 								else
 								{
@@ -486,13 +472,29 @@ int RTABMapApp::Render()
 			}
 		}

+		//filter poses?
+		if(poses.size() > 2)
+		{
+			if(nodesFiltering_)
+			{
+				for(std::multimap<int, rtabmap::Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
+				{
+					if(iter->second.type() != rtabmap::Link::kNeighbor)
+					{
+						int oldId = iter->second.to()>iter->second.from()?iter->second.from():iter->second.to();
+						poses.erase(oldId);
+					}
+				}
+			}
+		}
+
 		//update cloud visibility
 		std::set<int> addedClouds = main_scene_.getAddedClouds();
 		for(std::set<int>::const_iterator iter=addedClouds.begin();
 			iter!=addedClouds.end();
 			++iter)
 		{
-			if(*iter > 0 && (!mapCloudShown_ || poses.find(*iter) == poses.end()))
+			if(*iter > 0 && poses.find(*iter) == poses.end())
 			{
 				main_scene_.setCloudVisible(*iter, false);
 			}
@@ -513,23 +515,25 @@ int RTABMapApp::Render()
 			}
 		}

+		main_scene_.setCloudVisible(-1, odomCloudShown_ && !trajectoryMode_);
+
 		//just process the last one
 		if(set && !event.pose().isNull())
 		{
-			main_scene_.setCloudVisible(-1, false);
 			if(odomCloudShown_ && !trajectoryMode_)
 			{
 				if(!event.data().imageRaw().empty() && !event.data().depthRaw().empty())
 				{
-					LOGI("Creating Odom cloud (rgb=%dx%d depth=%dx%d)",
-							event.data().imageRaw().cols, event.data().imageRaw().rows,
-							event.data().depthRaw().cols, event.data().depthRaw().rows);
 					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
-					cloud = rtabmap::util3d::cloudRGBFromSensorData(event.data(), renderingCloudDecimation, renderingCloudMaxDepth);
+					cloud = rtabmap::util3d::cloudRGBFromSensorData(event.data(), event.data().imageRaw().rows/event.data().depthRaw().rows, maxCloudDepth_);
 					if(cloud->size())
 					{
-						std::vector<pcl::Vertices> polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleTolerance, false, meshTrianglePixels);
-						main_scene_.addOrUpdateCloud(-1, cloud, polygons, opengl_world_T_rtabmap_world*event.pose());
+						LOGI("Created odom cloud (rgb=%dx%d depth=%dx%d cloud=%dx%d)",
+													event.data().imageRaw().cols, event.data().imageRaw().rows,
+													event.data().depthRaw().cols, event.data().depthRaw().rows,
+													(int)cloud->width, (int)cloud->height);
+						std::vector<pcl::Vertices> polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
+						main_scene_.addCloud(-1, cloud, polygons, opengl_world_T_rtabmap_world*event.pose(), event.data().imageRaw());
 						main_scene_.setCloudVisible(-1, true);
 					}
 					else
@@ -545,7 +549,16 @@ int RTABMapApp::Render()
 		}
 	}

+	UTimer fpsTime;
    lastDrawnCloudsCount_ = main_scene_.Render();
+    renderingFPS_ = 1.0/fpsTime.elapsed();
+
+    if(rtabmapEvents.size())
+    {
+    	// send statistics to GUI
+		LOGI("Posting PostRenderEvent!");
+		UEventsManager::post(new PostRenderEvent(rtabmapEvents.back()));
+    }

    return notifyDataLoaded?1:0;
 }
@@ -579,7 +592,7 @@ void RTABMapApp::setPausedMapping(bool paused)
 }
 void RTABMapApp::setMapCloudShown(bool shown)
 {
-	mapCloudShown_ = shown;
+	main_scene_.setMapRendering(shown);
 }
 void RTABMapApp::setOdomCloudShown(bool shown)
 {
@@ -608,6 +621,27 @@ void RTABMapApp::setTrajectoryMode(bool enabled)
 void RTABMapApp::setGraphOptimization(bool enabled)
 {
 	graphOptimization_ = enabled;
+	if(!camera_->isRunning())
+	{
+		std::map<int, rtabmap::Transform> poses;
+		std::multimap<int, rtabmap::Link> links;
+		rtabmap_->getGraph(poses, links, true, true);
+		if(poses.size())
+		{
+			boost::mutex::scoped_lock  lock(rtabmapMutex_);
+			rtabmap::Statistics stats = rtabmap_->getStatistics();
+			stats.setPoses(poses);
+			stats.setConstraints(links);
+			rtabmapEvents_.push_back(stats);
+
+			rtabmap_->setOptimizedPoses(poses);
+		}
+	}
+}
+void RTABMapApp::setNodesFiltering(bool enabled)
+{
+	nodesFiltering_ = enabled;
+	setGraphOptimization(graphOptimization_); // this will resend the graph if paused
 }
 void RTABMapApp::setGraphVisible(bool visible)
 {
@@ -621,12 +655,54 @@ void RTABMapApp::setAutoExposure(bool enabled)
 		autoExposure_ = enabled;
 		if(camera_)
 		{
-			camera_->join(true);
-			camera_->close();
 			camera_->setAutoExposure(autoExposure_);
-			onResume();
 		}
-		resetMapping();
+	}
+}
+
+void RTABMapApp::setFullResolution(bool enabled)
+{
+	if(fullResolution_ != enabled)
+	{
+		fullResolution_ = enabled;
+		if(camera_)
+		{
+			camera_->setDecimation(fullResolution_?1:2);
+		}
+		rtabmap::ParametersMap parameters;
+		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemImagePreDecimation(), std::string(fullResolution_?"2":"1")));
+		this->post(new rtabmap::ParamEvent(parameters));
+	}
+}
+
+void RTABMapApp::setMaxCloudDepth(float value)
+{
+	maxCloudDepth_ = value;
+}
+
+void RTABMapApp::setMeshAngleTolerance(float value)
+{
+	meshAngleToleranceDeg_ = value;
+}
+
+void RTABMapApp::setMeshTriangleSize(int value)
+{
+	 meshTrianglePix_ = value;
+}
+
+int RTABMapApp::setMappingParameter(const std::string & key, const std::string & value)
+{
+	if(rtabmap::Parameters::getDefaultParameters().find(key) != rtabmap::Parameters::getDefaultParameters().end())
+	{
+		LOGI(uFormat("Setting param \"%s\"  to \"\"", key.c_str(), value.c_str()).c_str());
+		uInsert(mappingParameters_, rtabmap::ParametersPair(key, value));
+		UEventsManager::post(new rtabmap::ParamEvent(mappingParameters_));
+		return 0;
+	}
+	else
+	{
+		LOGE(uFormat("Key \"%s\" doesn't exist!", key.c_str()).c_str());
+		return -1;
 	}
 }

@@ -651,67 +727,246 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
 	bool success = false;

 	//Assemble the meshes
-	UINFO("Organized fast mesh... ");
-
-	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
-	std::vector<pcl::Vertices> mergedPolygons;
-
+	if(UFile::getExtension(filePath).compare("obj") == 0)
 	{
-		boost::mutex::scoped_lock  lock(meshesMutex_);
-
-		for(std::map<int, std::pair<std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, std::vector<pcl::Vertices> >, rtabmap::Transform> >::iterator iter=createdMeshes_.begin();
-			iter!= createdMeshes_.end();
-			++iter)
+		pcl::TextureMesh textureMesh;
+		std::vector<cv::Mat> textures;
+		int totalPolygons = 0;
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 		{
-			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformedCloud = rtabmap::util3d::transformPointCloud(iter->second.first.first, iter->second.second);
-			if(mergedClouds->size() == 0)
+			boost::mutex::scoped_lock  lock(meshesMutex_);
+
+			textureMesh.tex_materials.resize(createdMeshes_.size());
+			textureMesh.tex_polygons.resize(createdMeshes_.size());
+			textureMesh.tex_coordinates.resize(createdMeshes_.size());
+			textures.resize(createdMeshes_.size());
+
+			int polygonsStep = 0;
+			int oi = 0;
+			for(std::map<int, Mesh>::iterator iter=createdMeshes_.begin();
+				iter!= createdMeshes_.end();
+				++iter)
 			{
-				*mergedClouds = *transformedCloud;
-				mergedPolygons = iter->second.first.second;
+				UASSERT(!iter->second.cloud->is_dense);
+
+				if(!iter->second.texture.empty() &&
+					iter->second.cloud->size() &&
+					iter->second.polygons.size())
+				{
+					// OBJ format requires normals
+					pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals;
+					cloudWithNormals = rtabmap::util3d::computeNormals(iter->second.cloud, 20);
+
+					// create dense cloud
+					pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+					std::vector<pcl::Vertices> densePolygons;
+					std::map<int, int> newToOldIndices;
+					newToOldIndices = rtabmap::util3d::filterNotUsedVerticesFromMesh(
+							*cloudWithNormals,
+							iter->second.polygons,
+							*denseCloud,
+							densePolygons);
+
+					// polygons
+					UASSERT(densePolygons.size());
+					unsigned int polygonSize = densePolygons.front().vertices.size();
+					textureMesh.tex_polygons[oi].resize(densePolygons.size());
+					textureMesh.tex_coordinates[oi].resize(densePolygons.size() * polygonSize);
+					for(unsigned int j=0; j<densePolygons.size(); ++j)
+					{
+						pcl::Vertices vertices = densePolygons[j];
+						UASSERT(polygonSize == vertices.vertices.size());
+						for(unsigned int k=0; k<vertices.vertices.size(); ++k)
+						{
+							//uv
+							std::map<int, int>::iterator jter = newToOldIndices.find(vertices.vertices[k]);
+							textureMesh.tex_coordinates[oi][j*vertices.vertices.size()+k] = Eigen::Vector2f(
+									float(jter->second % iter->second.cloud->width) / float(iter->second.cloud->width),   // u
+									float(iter->second.cloud->height - jter->second / iter->second.cloud->width) / float(iter->second.cloud->height)); // v
+
+							vertices.vertices[k] += polygonsStep;
+						}
+						textureMesh.tex_polygons[oi][j] = vertices;
+
+					}
+					totalPolygons += densePolygons.size();
+					polygonsStep += denseCloud->size();
+
+					pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformedCloud = rtabmap::util3d::transformPointCloud(denseCloud, iter->second.pose);
+					if(mergedClouds->size() == 0)
+					{
+						*mergedClouds = *transformedCloud;
+					}
+					else
+					{
+						*mergedClouds += *transformedCloud;
+					}
+
+					textures[oi] = iter->second.texture;
+					textureMesh.tex_materials[oi].tex_illum = 1;
+					textureMesh.tex_materials[oi].tex_name = uFormat("material_%d", iter->first);
+					++oi;
+				}
+				else
+				{
+					UERROR("Texture not set for mesh %d", iter->first);
+				}
 			}
-			else
+			textureMesh.tex_materials.resize(oi);
+			textureMesh.tex_polygons.resize(oi);
+			textures.resize(oi);
+
+			if(textures.size())
 			{
-				rtabmap::util3d::appendMesh(*mergedClouds, mergedPolygons, *transformedCloud, iter->second.first.second);
+				pcl::toPCLPointCloud2(*mergedClouds, textureMesh.cloud);
+
+				std::string textureDirectory = uSplit(filePath, '.').front();
+				UINFO("Saving %d textures to %s.", textures.size(), textureDirectory.c_str());
+				UDirectory::makeDir(textureDirectory);
+				for(unsigned int i=0;i<textures.size(); ++i)
+				{
+					cv::Mat rawImage = rtabmap::uncompressImage(textures[i]);
+					std::string texFile = textureDirectory+"/"+textureMesh.tex_materials[i].tex_name+".png";
+					cv::imwrite(texFile, rawImage);
+
+					UINFO("Saved %s (%d bytes).", texFile.c_str(), rawImage.total()*rawImage.channels());
+
+					// relative path
+					textureMesh.tex_materials[i].tex_file = uSplit(UFile::getName(filePath), '.').front()+"/"+textureMesh.tex_materials[i].tex_name+".png";
+				}
+
+				UINFO("Saving obj (%d vertices, %d polygons) to %s.", (int)mergedClouds->size(), totalPolygons, filePath.c_str());
+				success = pcl::io::saveOBJFile(filePath, textureMesh) == 0;
+				if(success)
+				{
+					UINFO("Saved obj to %s!", filePath.c_str());
+				}
+				else
+				{
+					UERROR("Failed saving obj to %s!", filePath.c_str());
+				}
 			}
 		}
 	}
-	if(closeVerticesDistance)
+	else
 	{
-		UINFO("Filtering assembled mesh (points=%d, polygons=%d, close vertices=%fm)...",
-				(int)mergedClouds->size(), (int)mergedPolygons.size(), closeVerticesDistance);
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGB>);
+		std::vector<pcl::Vertices> mergedPolygons;

-		mergedPolygons = rtabmap::util3d::filterCloseVerticesFromMesh(
-				mergedClouds,
-				mergedPolygons,
-				closeVerticesDistance,
-				M_PI/4,
-				true);
+		{
+			boost::mutex::scoped_lock  lock(meshesMutex_);

-		// filter invalid polygons
-		unsigned int count = mergedPolygons.size();
-		mergedPolygons = rtabmap::util3d::filterInvalidPolygons(mergedPolygons);
-		UINFO("Filtered %d invalid polygons.", (int)count-mergedPolygons.size());
+			for(std::map<int, Mesh>::iterator iter=createdMeshes_.begin();
+				iter!= createdMeshes_.end();
+				++iter)
+			{
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+				std::vector<pcl::Vertices> densePolygons;

-		// filter not used vertices
-		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr filteredCloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
-		std::vector<pcl::Vertices> filteredPolygons;
-		rtabmap::util3d::filterNotUsedVerticesFromMesh(*mergedClouds, mergedPolygons, *filteredCloud, filteredPolygons);
-		mergedClouds = filteredCloud;
-		mergedPolygons = filteredPolygons;
-	}
+				rtabmap::util3d::filterNotUsedVerticesFromMesh(
+						*iter->second.cloud,
+						iter->second.polygons,
+						*denseCloud,
+						densePolygons);

-	if(mergedClouds->size() && mergedPolygons.size())
-	{
-		pcl::PolygonMesh mesh;
-		pcl::toPCLPointCloud2(*mergedClouds, mesh.cloud);
-		mesh.polygons = mergedPolygons;
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformedCloud = rtabmap::util3d::transformPointCloud(denseCloud, iter->second.pose);
+				if(mergedClouds->size() == 0)
+				{
+					*mergedClouds = *transformedCloud;
+					mergedPolygons = densePolygons;
+				}
+				else
+				{
+					rtabmap::util3d::appendMesh(*mergedClouds, mergedPolygons, *transformedCloud, densePolygons);
+				}
+			}
+		}

-		UINFO("Saving to %s.", filePath.c_str());
-		success = pcl::io::savePLYFileBinary(filePath, mesh) == 0;
+		if(mergedClouds->size() && mergedPolygons.size())
+		{
+			pcl::PolygonMesh mesh;
+			pcl::toPCLPointCloud2(*mergedClouds, mesh.cloud);
+			mesh.polygons = mergedPolygons;
+
+			UINFO("Saving ply (%d vertices, %d polygons) to %s.", (int)mergedClouds->size(), (int)mergedPolygons.size(), filePath.c_str());
+			success = pcl::io::savePLYFileBinary(filePath, mesh) == 0;
+			if(success)
+			{
+				UINFO("Saved ply to %s!", filePath.c_str());
+			}
+			else
+			{
+				UERROR("Failed saving ply to %s!", filePath.c_str());
+			}
+		}
 	}
 	return success;
 }

+int RTABMapApp::postProcessing(int approach)
+{
+	int returnedValue = 0;
+	if(rtabmap_)
+	{
+		std::map<int, rtabmap::Transform> poses;
+		std::multimap<int, rtabmap::Link> links;
+		if(approach == 2 || approach == 0)
+		{
+			if(approach == 2)
+			{
+				// detect more loop closures
+				returnedValue = rtabmap_->detectMoreLoopClosures();
+			}
+
+			if(returnedValue >= 0)
+			{
+				// simple graph optmimization
+				rtabmap_->getGraph(poses, links, true, true);
+			}
+		}
+		else if (approach == 1)
+		{
+			if(rtabmap::Optimizer::isAvailable(rtabmap::Optimizer::kTypeG2O))
+			{
+				std::map<int, rtabmap::Signature> signatures;
+				rtabmap_->getGraph(poses, links, false, true, &signatures);
+
+				rtabmap::ParametersMap param;
+				param.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), "30"));
+                param.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerEpsilon(), "0"));
+				rtabmap::Optimizer * sba = rtabmap::Optimizer::create(rtabmap::Optimizer::kTypeG2O, param);
+				poses = sba->optimizeBA(poses.rbegin()->first, poses, links, signatures);
+				delete sba;
+			}
+			else
+			{
+				LOGE("g2o not available!");
+			}
+		}
+		else
+		{
+			LOGE("Invalid approach %d (should be 0 (graph optimization), 1 (sba) or 2 (detect more loop closures))", approach);
+			returnedValue = -1;
+		}
+
+		if(poses.size())
+		{
+			boost::mutex::scoped_lock  lock(rtabmapMutex_);
+			rtabmap::Statistics stats = rtabmap_->getStatistics();
+			stats.setPoses(poses);
+			stats.setConstraints(links);
+			rtabmapEvents_.push_back(stats);
+
+			rtabmap_->setOptimizedPoses(poses);
+		}
+		else
+		{
+			returnedValue = -1;
+		}
+	}
+	return returnedValue;
+}
+
 void RTABMapApp::handleEvent(UEvent * event)
 {
 	if(camera_ && camera_->isRunning())
@@ -719,7 +974,7 @@ void RTABMapApp::handleEvent(UEvent * event)
 		// called from events manager thread, so protect the data
 		if(event->getClassName().compare("OdometryEvent") == 0)
 		{
-			LOGI("GUI: Received OdometryEvent!");
+			LOGI("Received OdometryEvent!");
 			if(odomMutex_.try_lock())
 			{
 				odomEvents_.clear();
@@ -733,67 +988,11 @@ void RTABMapApp::handleEvent(UEvent * event)
 		if(status_.first == rtabmap::RtabmapEventInit::kInitialized &&
 		   event->getClassName().compare("RtabmapEvent") == 0)
 		{
-			LOGI("GUI: Received RtabmapEvent!");
-			int nodes =0;
-			int words = 0;
-			int loopClosureId = 0;
-			float updateTime = 0.0f;
-			int databaseMemoryUsed = 0;
-			int inliers = 0;
-			int featuresExtracted = 0;
-			float hypothesis = 0.0f;
+			LOGI("Received RtabmapEvent!");
+			if(camera_->isRunning())
 			{
-				boost::mutex::scoped_lock  lock(rtabmapMutex_);
-				if(camera_->isRunning())
-				{
-					rtabmapEvents_.push_back(((rtabmap::RtabmapEvent*)event)->getStats());
-					nodes = (int)uValue(rtabmapEvents_.back().data(), rtabmap::Statistics::kMemoryWorking_memory_size(), 0.0f) +
-							uValue(rtabmapEvents_.back().data(), rtabmap::Statistics::kMemoryShort_time_memory_size(), 0.0f);
-					words = (int)uValue(rtabmapEvents_.back().data(), rtabmap::Statistics::kKeypointDictionary_size(), 0.0f);
-					updateTime = uValue(rtabmapEvents_.back().data(), rtabmap::Statistics::kTimingTotal(), 0.0f);
-					loopClosureId = rtabmapEvents_.back().loopClosureId()>0?rtabmapEvents_.back().loopClosureId():rtabmapEvents_.back().localLoopClosureId()>0?rtabmapEvents_.back().localLoopClosureId():0;
-					databaseMemoryUsed = (int)uValue(rtabmapEvents_.back().data(), rtabmap::Statistics::kMemoryDatabase_memory_used(), 0.0f);
-					inliers = (int)uValue(rtabmapEvents_.back().data(), rtabmap::Statistics::kLoopVisual_inliers(), 0.0f);
-					featuresExtracted = rtabmapEvents_.back().getSignatures().size()?rtabmapEvents_.back().getSignatures().rbegin()->second.getWords().size():0;
-					hypothesis = uValue(rtabmapEvents_.back().data(), rtabmap::Statistics::kLoopHighest_hypothesis_value(), 0.0f);
-				}
-			}
-
-			// Call JAVA callback with some stats
-			bool success = false;
-			if(jvm && RTABMapActivity)
-			{
-				JNIEnv *env = 0;
-				jint rs = jvm->AttachCurrentThread(&env, NULL);
-				if(rs == JNI_OK && env)
-				{
-					jclass clazz = env->GetObjectClass(RTABMapActivity);
-					if(clazz)
-					{
-						jmethodID methodID = env->GetMethodID(clazz, "updateStatsCallback", "(IIIIFIIIIFI)V" );
-						if(methodID)
-						{
-							env->CallVoidMethod(RTABMapActivity, methodID,
-									nodes,
-									words,
-									totalPoints_,
-									totalPolygons_,
-									updateTime,
-									loopClosureId,
-									databaseMemoryUsed,
-									inliers,
-									featuresExtracted,
-									hypothesis,
-									lastDrawnCloudsCount_);
-							success = true;
-						}
-					}
-				}
-				jvm->DetachCurrentThread();
-			}
-			if(!success)
-			{
-				UERROR("Failed to call RTABMapActivity::updateStatsCallback");
+				boost::mutex::scoped_lock lock(rtabmapMutex_);
+				rtabmapEvents_.push_back(((rtabmap::RtabmapEvent*)event)->getStats());
 			}
 		}
 	}
@@ -844,7 +1043,7 @@ void RTABMapApp::handleEvent(UEvent * event)

 	if(event->getClassName().compare("RtabmapEventInit") == 0)
 	{
-		LOGI("GUI: Received RtabmapEventInit!");
+		LOGI("Received RtabmapEventInit!");
 		status_.first = ((rtabmap::RtabmapEventInit*)event)->getStatus();
 		status_.second = ((rtabmap::RtabmapEventInit*)event)->getInfo();

@@ -881,5 +1080,63 @@ void RTABMapApp::handleEvent(UEvent * event)
 			UERROR("Failed to call RTABMapActivity::rtabmapInitEventsCallback");
 		}
 	}
+
+	if(event->getClassName().compare("PostRenderEvent") == 0)
+	{
+		LOGI("Received PostRenderEvent!");
+		const rtabmap::Statistics & stats = ((PostRenderEvent*)event)->getStats();
+		int nodes = (int)uValue(stats.data(), rtabmap::Statistics::kMemoryWorking_memory_size(), 0.0f) +
+				uValue(stats.data(), rtabmap::Statistics::kMemoryShort_time_memory_size(), 0.0f);
+		int words = (int)uValue(stats.data(), rtabmap::Statistics::kKeypointDictionary_size(), 0.0f);
+		float updateTime = uValue(stats.data(), rtabmap::Statistics::kTimingTotal(), 0.0f);
+		int loopClosureId = stats.loopClosureId()>0?stats.loopClosureId():stats.proximityDetectionId()>0?stats.proximityDetectionId():0;
+		int highestHypId = (int)uValue(stats.data(), rtabmap::Statistics::kLoopHighest_hypothesis_id(), 0.0f);
+		int databaseMemoryUsed = (int)uValue(stats.data(), rtabmap::Statistics::kMemoryDatabase_memory_used(), 0.0f);
+		int inliers = (int)uValue(stats.data(), rtabmap::Statistics::kLoopVisual_inliers(), 0.0f);
+		int rejected = (int)uValue(stats.data(), rtabmap::Statistics::kLoopRejectedHypothesis(), 0.0f);
+		int featuresExtracted = stats.getSignatures().size()?stats.getSignatures().rbegin()->second.getWords().size():0;
+		float hypothesis = uValue(stats.data(), rtabmap::Statistics::kLoopHighest_hypothesis_value(), 0.0f);
+
+		// Call JAVA callback with some stats
+		UINFO("Send statistics to GUI");
+		bool success = false;
+		if(jvm && RTABMapActivity)
+		{
+			JNIEnv *env = 0;
+			jint rs = jvm->AttachCurrentThread(&env, NULL);
+			if(rs == JNI_OK && env)
+			{
+				jclass clazz = env->GetObjectClass(RTABMapActivity);
+				if(clazz)
+				{
+					jmethodID methodID = env->GetMethodID(clazz, "updateStatsCallback", "(IIIIFIIIIIFIFI)V" );
+					if(methodID)
+					{
+						env->CallVoidMethod(RTABMapActivity, methodID,
+								nodes,
+								words,
+								totalPoints_,
+								totalPolygons_,
+								updateTime,
+								loopClosureId,
+								highestHypId,
+								databaseMemoryUsed,
+								inliers,
+								featuresExtracted,
+								hypothesis,
+								lastDrawnCloudsCount_,
+								renderingFPS_,
+								rejected);
+						success = true;
+					}
+				}
+			}
+			jvm->DetachCurrentThread();
+		}
+		if(!success)
+		{
+			UERROR("Failed to call RTABMapActivity::updateStatsCallback");
+		}
+	}
 }

@@ -50,14 +50,11 @@ 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);

-  int onResume();
+  bool onTangoServiceConnected(JNIEnv* env, jobject iBinder);

  // Explicitly reset motion tracking and restart the pipeline.
  // Note that this will cause motion tracking to re-initialize.
@@ -119,12 +116,19 @@ class RTABMapApp : public UEventsHandler {
  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 setFullResolution(bool enabled);
+  void setMaxCloudDepth(float value);
+  void setMeshAngleTolerance(float value);
+  void setMeshTriangleSize(int value);
+  int setMappingParameter(const std::string & key, const std::string & value);

  void resetMapping();
  void save();
  bool exportMesh(const std::string & filePath);
+  int postProcessing(int approach);

 protected:
  virtual void handleEvent(UEvent * event);
@@ -135,20 +139,28 @@ class RTABMapApp : public UEventsHandler {
 private:
  rtabmap::CameraTango * camera_;
  rtabmap::RtabmapThread * rtabmapThread_;
+  rtabmap::Rtabmap * rtabmap_;
  LogHandler * logHandler_;

-  bool mapCloudShown_;
  bool odomCloudShown_;
-  bool loopClosureDetection_;
  bool graphOptimization_;
+  bool nodesFiltering_;
  bool localizationMode_;
  bool trajectoryMode_;
  bool autoExposure_;
+  bool fullResolution_;
+  float maxCloudDepth_;
+  int meshTrianglePix_;
+  float meshAngleToleranceDeg_;
+
+  rtabmap::ParametersMap mappingParameters_;
+

  bool clearSceneOnNextRender_;
  int totalPoints_;
  int totalPolygons_;
  int lastDrawnCloudsCount_;
+  float renderingFPS_;

  // main_scene_ includes all drawable object for visualizing Tango device's
  // movement and point cloud.
@@ -163,9 +175,16 @@ class RTABMapApp : public UEventsHandler {
 	boost::mutex odomMutex_;
 	boost::mutex poseMutex_;

-	std::map<int, std::pair<std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, std::vector<pcl::Vertices> >, rtabmap::Transform > > createdMeshes_;
+	struct Mesh
+	{
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
+		std::vector<pcl::Vertices> polygons;
+		rtabmap::Transform pose;
+		cv::Mat texture;
+	};
+
+	std::map<int, Mesh> createdMeshes_;
 	std::map<int, rtabmap::Transform> rawPoses_;
-	std::pair<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::IndicesPtr> > previousCloud_;

 	std::pair<rtabmap::RtabmapEventInit::Status, std::string> status_;
 };
@@ -48,11 +48,11 @@ 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
@@ -64,10 +64,10 @@ Java_com_introlab_rtabmap_RTABMapLib_openDatabase(
 	return app.openDatabase(databasePathC);
 }

-JNIEXPORT jint JNICALL
-Java_com_introlab_rtabmap_RTABMapLib_onResume(
-    JNIEnv*, jobject) {
-  return app.onResume();
+JNIEXPORT bool JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_onTangoServiceConnected(
+		JNIEnv* env, jobject, jobject iBinder) {
+  return app.onTangoServiceConnected(env, iBinder);
 }

 JNIEXPORT void JNICALL
@@ -156,6 +156,12 @@ Java_com_introlab_rtabmap_RTABMapLib_setGraphOptimization(
 	return app.setGraphOptimization(enabled);
 }
 JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setNodesFiltering(
+		JNIEnv*, jobject, bool enabled)
+{
+	return app.setNodesFiltering(enabled);
+}
+JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setGraphVisible(
 		JNIEnv*, jobject, bool visible)
 {
@@ -167,6 +173,39 @@ Java_com_introlab_rtabmap_RTABMapLib_setAutoExposure(
 {
 	return app.setAutoExposure(enabled);
 }
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setFullResolution(
+		JNIEnv*, jobject, bool enabled)
+{
+	return app.setFullResolution(enabled);
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setMaxCloudDepth(
+		JNIEnv*, jobject, float value)
+{
+	return app.setMaxCloudDepth(value);
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setMeshAngleTolerance(
+		JNIEnv*, jobject, float value)
+{
+	return app.setMeshAngleTolerance(value);
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setMeshTriangleSize(
+		JNIEnv*, jobject, int value)
+{
+	return app.setMeshTriangleSize(value);
+}
+JNIEXPORT jint JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setMappingParameter(
+    JNIEnv* env, jobject, jstring key, jstring value)
+{
+	std::string keyC, valueC;
+	GetJStringContent(env,key,keyC);
+	GetJStringContent(env,value,valueC);
+	return app.setMappingParameter(keyC, valueC);
+}

 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_resetMapping(
@@ -191,6 +230,15 @@ Java_com_introlab_rtabmap_RTABMapLib_exportMesh(
 	return app.exportMesh(filePathC);
 }

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

 #include <sstream>

 #include "point_cloud_drawable.h"
 #include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UConversion.h"
+#include <opencv2/imgproc/imgproc.hpp>
 #include "util.h"

 #include <GLES2/gl2.h>

 PointCloudDrawable::PointCloudDrawable(
-		GLuint shaderProgram,
-		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
-		const std::vector<pcl::Vertices> & indices) :
+		GLuint cloudShaderProgram,
+		GLuint textureShaderProgram,
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const std::vector<pcl::Vertices> & polygons,
+		const cv::Mat & image) :
 		vertex_buffers_(0),
+		textures_(0),
 		nPoints_(0),
 		pose_(1.0f),
 		visible_(true),
-		shader_program_(shaderProgram)
+		cloud_shader_program_(cloudShaderProgram),
+		texture_shader_program_(textureShaderProgram)
 {
 	UASSERT(!cloud->empty());

 	glGenBuffers(1, &vertex_buffers_);
-
-	if(vertex_buffers_)
+	if(!vertex_buffers_)
 	{
-		LOGI("Creating cloud buffer %d", vertex_buffers_);
-		std::vector<float> vertices = std::vector<float>(cloud->size()*4);
-		for(unsigned int i=0; i<cloud->size(); ++i)
-		{
-			vertices[i*4] = cloud->at(i).x;
-			vertices[i*4+1] = cloud->at(i).y;
-			vertices[i*4+2] = cloud->at(i).z;
-			vertices[i*4+3] = cloud->at(i).rgb;
-		}
+		LOGE("OpenGL: could not generate vertex buffers\n");
+		return;
+	}

-		glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
-		glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * (int)vertices.size(), (const void *)vertices.data(), GL_STATIC_DRAW);
-		glBindBuffer(GL_ARRAY_BUFFER, 0);
-
-		GLint error = glGetError();
-		if(error != GL_NO_ERROR)
+	if(!cloud->is_dense && !image.empty())
+	{
+		LOGI("cloud=%dx%d image=%dx%d\n", (int)cloud->width, (int)cloud->height, image.cols, image.rows);
+		UASSERT(polygons.size() && !cloud->is_dense && !image.empty() && image.type() == CV_8UC3);
+		glGenTextures(1, &textures_);
+		if(!textures_)
 		{
-			LOGI("OpenGL: Could not allocate point cloud (0x%x)\n", error);
 			vertex_buffers_ = 0;
-		}
-		else
-		{
-			nPoints_ = cloud->size();
-
-			if(indices.size())
-			{
-				int polygonSize = indices[0].vertices.size();
-				UASSERT(polygonSize == 3);
-				indices_.resize(indices.size() * polygonSize);
-				int oi = 0;
-				for(unsigned int i=0; i<indices.size(); ++i)
-				{
-					UASSERT((int)indices[i].vertices.size() == polygonSize);
-					for(int j=0; j<polygonSize; ++j)
-					{
-						indices_[oi++] = (unsigned short)indices[i].vertices[j];
-					}
-				}
-			}
+			LOGE("OpenGL: could not generate vertex buffers\n");
+			return;
 		}
 	}
-}
-PointCloudDrawable::PointCloudDrawable(
-		GLuint shaderProgram,
-		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		const std::vector<pcl::Vertices> & indices) :
-		vertex_buffers_(0),
-		nPoints_(0),
-		pose_(1.0f),
-		visible_(true),
-		shader_program_(shaderProgram)
-{
-	UASSERT(!cloud->empty());

-	glGenBuffers(1, &vertex_buffers_);
-
-	if(vertex_buffers_)
+	LOGI("Creating cloud buffer %d", vertex_buffers_);
+	std::vector<float> vertices;
+	if(textures_)
 	{
-		LOGI("Creating cloud buffer %d", vertex_buffers_);
-		std::vector<float> vertices = std::vector<float>(cloud->size()*4);
+		vertices = std::vector<float>(cloud->size()*6);
+		for(unsigned int i=0; i<cloud->size(); ++i)
+		{
+			vertices[i*6] = cloud->at(i).x;
+			vertices[i*6+1] = cloud->at(i).y;
+			vertices[i*6+2] = cloud->at(i).z;
+
+			// rgb
+			vertices[i*6+3] = cloud->at(i).rgb;
+
+			// texture uv
+			vertices[i*6+4] = float(i % cloud->width)/float(cloud->width); //u
+			vertices[i*6+5] = float(i/cloud->width)/float(cloud->height);  //v
+		}
+	}
+	else
+	{
+		vertices = std::vector<float>(cloud->size()*4);
 		for(unsigned int i=0; i<cloud->size(); ++i)
 		{
 			vertices[i*4] = cloud->at(i).x;
@@ -106,35 +100,56 @@ PointCloudDrawable::PointCloudDrawable(
 			vertices[i*4+2] = cloud->at(i).z;
 			vertices[i*4+3] = cloud->at(i).rgb;
 		}
+	}

-		glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
-		glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * (int)vertices.size(), (const void *)vertices.data(), GL_STATIC_DRAW);
-		glBindBuffer(GL_ARRAY_BUFFER, 0);
+	glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
+	glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * (int)vertices.size(), (const void *)vertices.data(), GL_STATIC_DRAW);
+	glBindBuffer(GL_ARRAY_BUFFER, 0);
+
+	GLint error = glGetError();
+	if(error != GL_NO_ERROR)
+	{
+		LOGE("OpenGL: Could not allocate point cloud (0x%x)\n", error);
+		vertex_buffers_ = 0;
+		return;
+	}
+
+	if(textures_)
+	{
+		// gen texture from image
+		glBindTexture(GL_TEXTURE_2D, textures_);
+		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+		cv::Mat rgbImage;
+		cv::cvtColor(image, rgbImage, CV_BGR2RGB);
+		glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, rgbImage.cols, rgbImage.rows, 0, GL_RGB, GL_UNSIGNED_BYTE, rgbImage.data);

 		GLint error = glGetError();
 		if(error != GL_NO_ERROR)
 		{
-			LOGI("OpenGL: Could not allocate point cloud (0x%x)\n", error);
-			vertex_buffers_ = 0;
-		}
-		else
-		{
-			nPoints_ = cloud->size();
+			LOGE("OpenGL: Could not allocate texture (0x%x)\n", error);
+			textures_ = 0;

-			if(indices.size())
+			glDeleteBuffers(1, &vertex_buffers_);
+			vertex_buffers_ = 0;
+			return;
+		}
+	}
+
+	nPoints_ = cloud->size();
+
+	if(polygons.size())
+	{
+		int polygonSize = polygons[0].vertices.size();
+		UASSERT(polygonSize == 3);
+		polygons_.resize(polygons.size() * polygonSize);
+		int oi = 0;
+		for(unsigned int i=0; i<polygons.size(); ++i)
+		{
+			UASSERT((int)polygons[i].vertices.size() == polygonSize);
+			for(int j=0; j<polygonSize; ++j)
 			{
-				int polygonSize = indices[0].vertices.size();
-				UASSERT(polygonSize == 3);
-				indices_.resize(indices.size() * polygonSize);
-				int oi = 0;
-				for(unsigned int i=0; i<indices.size(); ++i)
-				{
-					UASSERT((int)indices[i].vertices.size() == polygonSize);
-					for(int j=0; j<polygonSize; ++j)
-					{
-						indices_[oi++] = (unsigned short)indices[i].vertices[j];
-					}
-				}
+				polygons_[oi++] = (unsigned short)polygons[i].vertices[j];
 			}
 		}
 	}
@@ -149,6 +164,13 @@ PointCloudDrawable::~PointCloudDrawable()
 		tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
 		vertex_buffers_ = 0;
 	}
+
+	if (textures_)
+	{
+		glDeleteTextures(1, &textures_);
+		tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
+		textures_ = 0;
+	}
 }

 void PointCloudDrawable::setPose(const rtabmap::Transform & pose)
@@ -162,33 +184,69 @@ void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix, const glm::m

 	if(vertex_buffers_ && nPoints_ && visible_)
 	{
-		glUseProgram(shader_program_);
-
-		GLuint mvp_handle_ = glGetUniformLocation(shader_program_, "mvp");
-		glm::mat4 mvp_mat = projectionMatrix * viewMatrix * pose_;
-		glUniformMatrix4fv(mvp_handle_, 1, GL_FALSE, glm::value_ptr(mvp_mat));
-
-		GLuint point_size_handle_ = glGetUniformLocation(shader_program_, "point_size");
-		glUniform1f(point_size_handle_, pointSize);
-
-		GLint attribute_vertex = glGetAttribLocation(shader_program_, "vertex");
-		GLint attribute_color = glGetAttribLocation(shader_program_, "color");
-
-		glEnableVertexAttribArray(attribute_vertex);
-		glEnableVertexAttribArray(attribute_color);
-		glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
-		glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, 4*sizeof(GLfloat), 0);
-		glVertexAttribPointer(attribute_color, 3, GL_UNSIGNED_BYTE, GL_TRUE, 4*sizeof(GLfloat), (GLvoid*) (3 * sizeof(GLfloat)));
-
-		if(meshRendering && indices_.size())
+		if(meshRendering && textures_)
 		{
-			glDrawElements(GL_TRIANGLES, indices_.size(), GL_UNSIGNED_SHORT, indices_.data());
-		}
-		else
-		{
-			glDrawArrays(GL_POINTS, 0, nPoints_);
-		}
+			glUseProgram(texture_shader_program_);

+			GLuint mvp_handle_ = glGetUniformLocation(texture_shader_program_, "mvp");
+			glm::mat4 mvp_mat = projectionMatrix * viewMatrix * pose_;
+			glUniformMatrix4fv(mvp_handle_, 1, GL_FALSE, glm::value_ptr(mvp_mat));
+
+			// Texture activate unit 0
+			glActiveTexture(GL_TEXTURE0);
+			// Bind the texture to this unit.
+			glBindTexture(GL_TEXTURE_2D, textures_);
+			// Tell the texture uniform sampler to use this texture in the shader by binding to texture unit 0.
+			GLuint texture_handle = glGetUniformLocation(texture_shader_program_, "u_Texture");
+			glUniform1i(texture_handle, 0);
+
+			GLint attribute_vertex = glGetAttribLocation(texture_shader_program_, "vertex");
+			GLint attribute_texture = glGetAttribLocation(texture_shader_program_, "a_TexCoordinate");
+
+			glEnableVertexAttribArray(attribute_vertex);
+			glEnableVertexAttribArray(attribute_texture);
+			glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
+			glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, 6*sizeof(GLfloat), 0);
+			glVertexAttribPointer(attribute_texture, 2, GL_FLOAT, GL_FALSE, 6*sizeof(GLfloat), (GLvoid*) (4 * sizeof(GLfloat)));
+
+			glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_SHORT, polygons_.data());
+		}
+		else // point cloud or colored mesh
+		{
+			glUseProgram(cloud_shader_program_);
+
+			GLuint mvp_handle_ = glGetUniformLocation(cloud_shader_program_, "mvp");
+			glm::mat4 mvp_mat = projectionMatrix * viewMatrix * pose_;
+			glUniformMatrix4fv(mvp_handle_, 1, GL_FALSE, glm::value_ptr(mvp_mat));
+
+			GLuint point_size_handle_ = glGetUniformLocation(cloud_shader_program_, "point_size");
+			glUniform1f(point_size_handle_, pointSize);
+
+			GLint attribute_vertex = glGetAttribLocation(cloud_shader_program_, "vertex");
+			GLint attribute_color = glGetAttribLocation(cloud_shader_program_, "color");
+
+			glEnableVertexAttribArray(attribute_vertex);
+			glEnableVertexAttribArray(attribute_color);
+			glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
+			if(textures_)
+			{
+				glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, 6*sizeof(GLfloat), 0);
+				glVertexAttribPointer(attribute_color, 3, GL_UNSIGNED_BYTE, GL_TRUE, 6*sizeof(GLfloat), (GLvoid*) (3 * sizeof(GLfloat)));
+			}
+			else
+			{
+				glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, 4*sizeof(GLfloat), 0);
+				glVertexAttribPointer(attribute_color, 3, GL_UNSIGNED_BYTE, GL_TRUE, 4*sizeof(GLfloat), (GLvoid*) (3 * sizeof(GLfloat)));
+			}
+			if(meshRendering && polygons_.size())
+			{
+				glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_SHORT, polygons_.data());
+			}
+			else
+			{
+				glDrawArrays(GL_POINTS, 0, nPoints_);
+			}
+		}
 		glDisableVertexAttribArray(0);
 		glBindBuffer(GL_ARRAY_BUFFER, 0);

@@ -1,18 +1,29 @@
 /*
- * Copyright 2014 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.
- */
+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 TANGO_POINT_CLOUD_POINT_CLOUD_DRAWABLE_H_
 #define TANGO_POINT_CLOUD_POINT_CLOUD_DRAWABLE_H_
@@ -31,13 +42,11 @@
 class PointCloudDrawable {
 public:
  PointCloudDrawable(
-		  GLuint shaderProgram,
-		  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
-		  const std::vector<pcl::Vertices> & indices = std::vector<pcl::Vertices>());
-  PointCloudDrawable(
-  		  GLuint shaderProgram,
+		  GLuint cloudShaderProgram,
+		  GLuint textureShaderProgram,
  		  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-  		  const std::vector<pcl::Vertices> & indices = std::vector<pcl::Vertices>());
+  		  const std::vector<pcl::Vertices> & polygons = std::vector<pcl::Vertices>(),
+		  const cv::Mat & image = cv::Mat());
  virtual ~PointCloudDrawable();

  void setPose(const rtabmap::Transform & pose);
@@ -56,12 +65,14 @@ class PointCloudDrawable {
 private:
  // Vertex buffer of the point cloud geometry.
  GLuint vertex_buffers_;
-  std::vector<GLushort> indices_;
+  GLuint textures_;
+  std::vector<GLushort> polygons_;
  int nPoints_;
  glm::mat4 pose_;
  bool visible_;

-  GLuint shader_program_;
+  GLuint cloud_shader_program_;
+  GLuint texture_shader_program_;
 };

 #endif  // TANGO_POINT_CLOUD_POINT_CLOUD_DRAWABLE_H_
@@ -60,6 +60,26 @@ const std::string kPointCloudFragmentShader =
    "  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"
@@ -92,7 +112,9 @@ Scene::Scene() :
 		traceVisible_(true),
 		currentPose_(0),
 		cloud_shader_program_(0),
+		texture_mesh_shader_program_(0),
 		graph_shader_program_(0),
+		mapRendering_(true),
 		meshRendering_(true),
 		pointSize_(3.0f) {}

@@ -130,6 +152,11 @@ void Scene::InitGLContent()
 	  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());
@@ -156,6 +183,10 @@ void Scene::DeleteResources() {
 		glDeleteShader(cloud_shader_program_);
 		cloud_shader_program_ = 0;
 	  }
+	if (texture_mesh_shader_program_) {
+		glDeleteShader(texture_mesh_shader_program_);
+		texture_mesh_shader_program_ = 0;
+	  }
 	if (graph_shader_program_) {
 		glDeleteShader(graph_shader_program_);
 		graph_shader_program_ = 0;
@@ -250,7 +281,7 @@ int Scene::Render() {

 	bool frustumCulling = true;
 	int cloudDrawn=0;
-	if(frustumCulling)
+	if(mapRendering_ && frustumCulling)
 	{
 		//Use camera frustum to cull nodes that don't need to be drawn
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
@@ -304,8 +335,11 @@ int Scene::Render() {
 	{
 		for(std::map<int, PointCloudDrawable*>::const_iterator iter=pointClouds_.begin(); iter!=pointClouds_.end(); ++iter)
 		{
-			++cloudDrawn;
-			iter->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_);
+			if((mapRendering_ || iter->first < 0) && iter->second->isVisible())
+			{
+				++cloudDrawn;
+				iter->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_);
+			}
 		}
 	}

@@ -372,31 +406,12 @@ void Scene::setTraceVisible(bool visible)
 }

 //Should only be called in OpenGL thread!
-void Scene::addOrUpdateCloud(
-		int id,
-		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
-		const std::vector<pcl::Vertices> & polygons,
-		const rtabmap::Transform & pose)
-{
-	LOGI("addOrUpdateCloud cloud %d", id);
-	std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
-	if(iter != pointClouds_.end())
-	{
-		delete iter->second;
-		pointClouds_.erase(iter);
-	}
-
-	//create
-	UASSERT(cloud_shader_program_ != 0);
-	PointCloudDrawable * drawable = new PointCloudDrawable(cloud_shader_program_, cloud, polygons);
-	drawable->setPose(pose);
-	pointClouds_.insert(std::make_pair(id, drawable));
-}
-void Scene::addOrUpdateCloud(
+void Scene::addCloud(
 		int id,
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const std::vector<pcl::Vertices> & polygons,
-		const rtabmap::Transform & pose)
+		const rtabmap::Transform & pose,
+		const cv::Mat & image)
 {
 	LOGI("addOrUpdateCloud cloud %d", id);
 	std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
@@ -407,8 +422,13 @@ void Scene::addOrUpdateCloud(
 	}

 	//create
-	UASSERT(cloud_shader_program_ != 0);
-	PointCloudDrawable * drawable = new PointCloudDrawable(cloud_shader_program_, cloud, polygons);
+	UASSERT(cloud_shader_program_ != 0 && texture_mesh_shader_program_!=0);
+	PointCloudDrawable * drawable = new PointCloudDrawable(
+			cloud_shader_program_,
+			texture_mesh_shader_program_,
+			cloud,
+			polygons,
+			image);
 	drawable->setPose(pose);
 	pointClouds_.insert(std::make_pair(id, drawable));
 }
@@ -96,22 +96,19 @@ class Scene {
  void setGraphVisible(bool visible);
  void setTraceVisible(bool visible);

-  void addOrUpdateCloud(
-		  int id,
-		  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
-		  const std::vector<pcl::Vertices> & polygons,
-		  const rtabmap::Transform & pose);
-  void addOrUpdateCloud(
+  void addCloud(
  		  int id,
  		  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
  		  const std::vector<pcl::Vertices> & polygons,
-  		  const rtabmap::Transform & pose);
+  		  const rtabmap::Transform & pose,
+		  const cv::Mat & image = cv::Mat());

  void setCloudPose(int id, const rtabmap::Transform & pose);
  void setCloudVisible(int id, bool visible);
  bool hasCloud(int id) const;
  std::set<int> getAddedClouds() const;

+  void setMapRendering(bool enabled) {mapRendering_ = enabled;}
  void setMeshRendering(bool enabled) {meshRendering_ = enabled;}
  void setPointSize(float size) {pointSize_ = size;}

@@ -140,8 +137,10 @@ class Scene {

  // Shader to display point cloud.
  GLuint cloud_shader_program_;
+  GLuint texture_mesh_shader_program_;
  GLuint graph_shader_program_;

+  bool mapRendering_;
  bool meshRendering_;
  float pointSize_;
 };
@@ -219,6 +219,21 @@
                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/fps" />
+
+            <TextView
+                android:id="@+id/fps"
+                android:layout_width="wrap_content"
+                android:layout_height="wrap_content" />
+        </LinearLayout>
           
    </LinearLayout>

@@ -6,26 +6,63 @@
    </group>
    
    <group android:id="@+id/group_actions">
+       <item android:id="@+id/post_processing" android:title="Post-Processing...">
+          <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>   
+          </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 (*.ply)"/>
+       <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/about" android:title="About"/>
-    </group>
-    
-    <item android:id="@+id/menu_settings" android:title="Options..." android:orderInCategory="2">
+       
+       <item android:id="@+id/menu_rendering_settings" android:title="Rendering Options...">
        <menu >
-        <group android:id="@+id/group_visibility" android:checkableBehavior="all">
+        <group android:id="@+id/group_rendering_visibility" android:checkableBehavior="all">
            <item android:id="@+id/debug" android:checked="false" android:title="Debug" />
-            <item android:id="@+id/localization_mode" android:checked="false" android:title="Localization Mode" />
-            <item android:id="@+id/trajectory_mode" android:checked="false" android:title="Trajectory Mode" />
            <item android:id="@+id/mesh_rendering" android:checked="true" android:title="Mesh Rendering" />
-            <item android:id="@+id/auto_exposure" android:checked="false" android:title="Auto Exposure" />
            <item android:id="@+id/map_shown" android:checked="true" android:title="Map Visible" />
            <item android:id="@+id/odom_shown" android:checked="true" android:title="Odom Visible" />
            <item android:id="@+id/graph_visible" android:checked="true" android:title="Graph Visible" />
-            <item android:id="@+id/graph_optimization" android:checked="true" android:title="Optimized Graph" />
+            <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..." />
        </group>   
        </menu>
-    </item>
+	    </item>
+	    <item android:id="@+id/menu_mapping_settings" android:title="Mapping Options...">
+	       <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>
+	       </group>   
+	       </menu>
+	   </item>
+       
+       <item android:id="@+id/about" android:title="About"/>
+    </group>
 </menu>
@@ -9,7 +9,7 @@
    <string name="third_person">Third</string>
    <string name="top_down">Top</string>
    <string name="start">Start</string>
-    <string name="nodes">"Nodes: "</string>
+    <string name="nodes">"Nodes (WM): "</string>
    <string name="points">"Number of points: "</string>
    <string name="update_time">"Update time (ms): "</string>
    <string name="loop_closure">"Loop closure ID: "</string>
@@ -20,5 +20,6 @@
    <string name="polygons">"Polygons: "</string>
    <string name="memory">"Memory (MB): "</string>
    <string name="hypothesis">"Hypothesis: "</string>
+    <string name="fps">"FPS (rendering): "</string>
    
 </resources>
@@ -9,8 +9,10 @@ import android.app.Notification;
 import android.app.NotificationManager;
 import android.app.PendingIntent;
 import android.app.ProgressDialog;
+import android.content.ComponentName;
 import android.content.DialogInterface;
 import android.content.Intent;
+import android.content.ServiceConnection;
 import android.content.pm.PackageInfo;
 import android.content.pm.PackageManager;
 import android.content.pm.PackageManager.NameNotFoundException;
@@ -20,6 +22,7 @@ import android.os.Bundle;
 import android.os.Environment;
 import android.os.Handler;
 import android.os.Debug;
+import android.os.IBinder;
 import android.text.Editable;
 import android.text.InputType;
 import android.util.Log;
@@ -30,6 +33,7 @@ import android.view.MenuInflater;
 import android.view.MotionEvent;
 import android.view.View;
 import android.view.View.OnClickListener;
+import android.view.WindowManager;
 import android.widget.EditText;
 import android.widget.LinearLayout;
 import android.widget.TextView;
@@ -66,6 +70,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
  private MenuItem mItemPause;
  private MenuItem mItemSave;
  private MenuItem mItemOpen;
+  private MenuItem mItemPostProcessing;
  private MenuItem mItemExport;
  private MenuItem mItemLocalizationMode;
  private MenuItem mItemTrajectoryMode;
@@ -75,10 +80,46 @@ public class RTABMapActivity extends Activity implements OnClickListener {
  private String mNewDatabasePath = "";
  private String mWorkingDirectory = "";
  
+  private int mMaxDepthIndex = 5;
+  private int mMeshAngleToleranceIndex = 1;
+  private int mMeshTriangleSizeIndex = 0;
+  
+  private int mParamUpdateRateHzIndex = 1;
+  private int mParamTimeThrMsIndex = 4;
+  private int mParamMaxFeaturesIndex = 4;
+  private int mParamLoopThrMsIndex = 1;
+  private int mParamOptimizeErrorIndex = 3;
+  
+  final String[] mUpdateRateValues = {"0.5", "1", "2", "Max"};
+  final String[] mTimeThrValues = {"400", "500", "600", "700", "800", "900", "1000", "1100", "1200", "1300", "1400", "1500", "No Limit"};
+  final String[] mMaxFeaturesValues = {"Disabled", "100", "200", "300", "400", "500", "600", "700", "800", "900", "1000", "No Limit"};
+  final String[] mLoopThrValues = {"Disabled", "0.11", "0.20", "0.30", "0.40", "0.50", "0.60", "0.70", "0.80", "0.90"};
+  final String[] mOptimizeErrorValues = {"Disabled", "0.01", "0.025", "0.05", "0.1", "0.2", "0.35", "0.5", "1"};
+  
  private LinearLayout mLayoutDebug;
  
  private int mTotalLoopClosures = 0;
  
+  private Toast mToast = null;
+  
+  //Tango Service connection.
+  ServiceConnection mTangoServiceConnection = new ServiceConnection() {
+     public void onServiceConnected(ComponentName name, IBinder service) {
+       if(!RTABMapLib.onTangoServiceConnected(service))
+       {
+    	   mToast.makeText(getApplicationContext(), 
+    				String.format("Failed to intialize Tango!"), mToast.LENGTH_SHORT).show();
+       }
+     }
+
+     public void onServiceDisconnected(ComponentName name) {
+       // Handle this if you need to gracefully shutdown/retry
+       // in the event that Tango itself crashes/gets upgraded while running.
+       mToast.makeText(getApplicationContext(), 
+ 				String.format("Tango disconnected!"), mToast.LENGTH_SHORT).show();
+     }
+   };
+  
  @Override
  protected void onCreate(Bundle savedInstanceState) {
    super.onCreate(savedInstanceState);
@@ -88,6 +129,8 @@ public class RTABMapActivity extends Activity implements OnClickListener {
    // touch point.
    Display display = getWindowManager().getDefaultDisplay();
    display.getSize(mScreenSize);
+    
+    getWindow().addFlags(WindowManager.LayoutParams.FLAG_KEEP_SCREEN_ON);

    // Setting content view of this activity.
    setContentView(R.layout.activity_rtabmap);
@@ -96,6 +139,8 @@ public class RTABMapActivity extends Activity implements OnClickListener {
    findViewById(R.id.first_person_button).setOnClickListener(this);
    findViewById(R.id.third_person_button).setOnClickListener(this);
    findViewById(R.id.top_down_button).setOnClickListener(this);
+    
+    mToast = Toast.makeText(getApplicationContext(), "", Toast.LENGTH_SHORT);

    // OpenGL view where all of the graphics are drawn.
    mGLView = (GLSurfaceView) findViewById(R.id.gl_surface_view);
@@ -115,7 +160,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {

    // Check if the Tango Core is out dated.
    if (!CheckTangoCoreVersion(MIN_TANGO_CORE_VERSION)) {
-      Toast.makeText(this, "Tango Core out dated, please update in Play Store", Toast.LENGTH_LONG).show();
+      mToast.makeText(this, "Tango Core out dated, please update in Play Store", mToast.LENGTH_LONG).show();
      finish();
      return;
    }   
@@ -143,12 +188,12 @@ public class RTABMapActivity extends Activity implements OnClickListener {
    else
    {
    	// show warning that data cannot be saved!
-		Toast.makeText(getApplicationContext(), 
+		mToast.makeText(getApplicationContext(), 
 				String.format("Failed to get external storage path (SD-CARD, state=%s). Saving disabled.", 
-						Environment.getExternalStorageState()), Toast.LENGTH_LONG).show();
+						Environment.getExternalStorageState()), mToast.LENGTH_LONG).show();
    }
    
-    RTABMapLib.initialize(this);
+    RTABMapLib.onCreate(this);
    RTABMapLib.openDatabase(mTempDatabasePath);
  }
  
@@ -158,8 +203,8 @@ public class RTABMapActivity extends Activity implements OnClickListener {
  	if (requestCode == Tango.TANGO_INTENT_ACTIVITYCODE) {
  		// Make sure the request was successful
  		if (resultCode == RESULT_CANCELED) {
-  			Toast.makeText(this, "Motion Tracking Permissions Required!",
-  					Toast.LENGTH_SHORT).show();
+  			mToast.makeText(this, "Motion Tracking Permissions Required!",
+  					mToast.LENGTH_SHORT).show();
  			finish();
  		}
  	}
@@ -169,6 +214,8 @@ public class RTABMapActivity extends Activity implements OnClickListener {
  protected void onResume() {
    super.onResume();
    
+    TangoInitializationHelper.bindTangoService(this, mTangoServiceConnection);
+    
    Log.i(TAG, String.format("onResume()"));
    
    if (Tango.hasPermission(this, Tango.PERMISSIONTYPE_MOTION_TRACKING)) {
@@ -182,12 +229,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
    	    mItemPause.setChecked(false);
    	    mItemSave.setEnabled(false);
    		mItemExport.setEnabled(false);
-        }
-
-        if(RTABMapLib.onResume()!=0)
-        {
-        	Toast.makeText(getApplicationContext(), 
-    				String.format("Failed to connect with Tango!"), Toast.LENGTH_SHORT).show();
+    		mItemPostProcessing.setEnabled(false);
        }

    } else {
@@ -207,6 +249,8 @@ public class RTABMapActivity extends Activity implements OnClickListener {
    RTABMapLib.onPause();
    mOpenedDatabasePath = "";
    RTABMapLib.openDatabase(mTempDatabasePath);
+    
+    unbindService(mTangoServiceConnection);
  }

  @Override
@@ -268,12 +312,14 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	  mItemPause = menu.findItem(R.id.pause);
 	  mItemSave = menu.findItem(R.id.save);
 	  mItemOpen = menu.findItem(R.id.open);
+	  mItemPostProcessing = menu.findItem(R.id.post_processing);
 	  mItemExport = menu.findItem(R.id.export);
 	  mItemLocalizationMode = menu.findItem(R.id.localization_mode);
 	  mItemTrajectoryMode = menu.findItem(R.id.trajectory_mode);
 	  mItemSave.setEnabled(false);
 	  mItemExport.setEnabled(false);
 	  mItemOpen.setEnabled(false);
+	  mItemPostProcessing.setEnabled(false);
 	  
 	  return true;
  }
@@ -285,15 +331,18 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		  int polygons,
 		  float updateTime, 
 		  int loopClosureId,
+		  int highestHypId,
 		  int databaseMemoryUsed,
 		  int inliers,
 		  int featuresExtracted,
 		  float hypothesis,
-		  int nodesDrawn)
+		  int nodesDrawn,
+		  float fps,
+		  int rejected)
  {
 	  if(mItemPause!=null)
 	  {
-		  ((TextView)findViewById(R.id.status)).setText(mItemPause.isChecked()?"Paused":mItemLocalizationMode.isChecked()?"Localization":"Mapping");
+		  ((TextView)findViewById(R.id.status)).setText(mItemPause.isChecked()?"Paused":mItemLocalizationMode.isChecked()?String.format("Localization (%s Hz)", mUpdateRateValues[mParamUpdateRateHzIndex]):String.format("Mapping (%s Hz)", mUpdateRateValues[mParamUpdateRateHzIndex]));
 	  }
 	  
 	  ((TextView)findViewById(R.id.points)).setText(String.valueOf(points));
@@ -303,15 +352,26 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	  ((TextView)findViewById(R.id.memory)).setText(String.valueOf(Debug.getNativeHeapAllocatedSize()/(1024*1024)));
 	  ((TextView)findViewById(R.id.db_size)).setText(String.valueOf(databaseMemoryUsed));
 	  ((TextView)findViewById(R.id.inliers)).setText(String.valueOf(inliers));
-	  ((TextView)findViewById(R.id.features)).setText(String.valueOf(featuresExtracted));
-	  ((TextView)findViewById(R.id.update_time)).setText(String.format("%.3f", updateTime));
-	  ((TextView)findViewById(R.id.hypothesis)).setText(String.format("%.3f (%d)", hypothesis, loopClosureId));
-	  if(loopClosureId > 0)
+	  ((TextView)findViewById(R.id.features)).setText(String.format("%d / %s", featuresExtracted, mMaxFeaturesValues[mParamMaxFeaturesIndex]));
+	  ((TextView)findViewById(R.id.update_time)).setText(String.format("%.3f / %s", updateTime, mTimeThrValues[mParamTimeThrMsIndex]));
+	  ((TextView)findViewById(R.id.hypothesis)).setText(String.format("%.3f / %s (%d)", hypothesis, mLoopThrValues[mParamLoopThrMsIndex], loopClosureId>0?loopClosureId:highestHypId));
+	  ((TextView)findViewById(R.id.fps)).setText(String.format("%.3f Hz", fps));
+	  if(mItemPause!=null && !mItemPause.isChecked())
 	  {
-		  ++mTotalLoopClosures;
-		  ((TextView)findViewById(R.id.total_loop)).setText(String.valueOf(mTotalLoopClosures));
-		  Toast.makeText(this, "Loop closure detected!", Toast.LENGTH_SHORT).show();
+		  if(loopClosureId > 0)
+		  {
+			  ++mTotalLoopClosures;
+	
+			  mToast.setText("Loop closure detected!");
+			  mToast.show();
+		  }
+		  else if(rejected > 0 && inliers >= 15)
+		  {
+			  mToast.setText("Loop closure rejected after graph optimization.");
+			  mToast.show();
+		  }
 	  }
+	  ((TextView)findViewById(R.id.total_loop)).setText(String.valueOf(mTotalLoopClosures));
  }
  
  // called from jni
@@ -322,17 +382,20 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		  final int polygons,
 		  final float updateTime, 
 		  final int loopClosureId,
+		  final int highestHypId,
 		  final int databaseMemoryUsed,
 		  final int inliers,
 		  final int features,
 		  final float hypothesis,
-		  final int nodesDrawn)
+		  final int nodesDrawn,
+		  final float fps,
+		  final int rejected)
  {
 	  Log.i(TAG, String.format("updateStatsCallback()"));

 	  runOnUiThread(new Runnable() {
 		    public void run() {
-		    	updateStatsUI(nodes, words, points, polygons, updateTime, loopClosureId, databaseMemoryUsed, inliers, features, hypothesis, nodesDrawn);
+		    	updateStatsUI(nodes, words, points, polygons, updateTime, loopClosureId, highestHypId, databaseMemoryUsed, inliers, features, hypothesis, nodesDrawn, fps, rejected);
 		    } 
 		});
  }
@@ -409,7 +472,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {

 			if(!msg.isEmpty())
 			{
-				Toast.makeText(this, msg, Toast.LENGTH_LONG).show();
+				mToast.makeText(this, msg, mToast.LENGTH_LONG).show();
 			}
 			
 			mOpenedDatabasePath = "";
@@ -428,6 +491,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			((TextView)findViewById(R.id.features)).setText(String.valueOf(0));
 			((TextView)findViewById(R.id.update_time)).setText(String.valueOf(0));
 			((TextView)findViewById(R.id.hypothesis)).setText(String.valueOf(0));
+			((TextView)findViewById(R.id.fps)).setText(String.valueOf(0));
 			mTotalLoopClosures = 0;
 			((TextView)findViewById(R.id.total_loop)).setText(String.valueOf(mTotalLoopClosures));
 			
@@ -437,6 +501,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 				{
 					mItemPause.setChecked(false);
 					mItemOpen.setEnabled(false);
+					mItemPostProcessing.setEnabled(false);
 					mItemSave.setEnabled(false);
 					mItemExport.setEnabled(false);
 					RTABMapLib.setPausedMapping(false); // resume mapping
@@ -476,25 +541,35 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	   * "AreaDescriptionSaveProgress:X" - ADF saving is X * 100 percent complete.
 	   * "Unknown"
 	   */
+	  String str = null;
 	  if(key.equals("TangoServiceException"))
-			  Toast.makeText(this, String.format("Tango service exception: %s", value), Toast.LENGTH_SHORT).show();
+		  str = String.format("Tango service exception: %s", value);
 	  else if(key.equals("FisheyeOverExposed"))
-			  ;//Toast.makeText(this, String.format("The fisheye image is over exposed with average pixel value %s px.", value), Toast.LENGTH_SHORT).show();
+		  ;//str = String.format("The fisheye image is over exposed with average pixel value %s px.", value);
 	  else if(key.equals("FisheyeUnderExposed"))
-			  ;//Toast.makeText(this, String.format("The fisheye image is under exposed with average pixel value %s px.", value), Toast.LENGTH_SHORT).show();
+		  ;//str = String.format("The fisheye image is under exposed with average pixel value %s px.", value);
 	  else if(key.equals("ColorOverExposed")) 
-			  ;//Toast.makeText(this, String.format("The color image is over exposed with average pixel value %s px.", value), Toast.LENGTH_SHORT).show();
+		  ;//str = String.format("The color image is over exposed with average pixel value %s px.", value);
 	  else if(key.equals("ColorUnderExposed")) 
-			  ;//Toast.makeText(this, String.format("The color image is under exposed with average pixel value %s px.", value), Toast.LENGTH_SHORT).show();
+		  ;//str = String.format("The color image is under exposed with average pixel value %s px.", value);
 	  else if(key.equals("CameraTango")) 
-		  	 Toast.makeText(this, value, Toast.LENGTH_SHORT).show();
+		  str = value;
 	  else if(key.equals("TooFewFeaturesTracked")) 
 	  {
 		  if(!value.equals("0"))
-			  Toast.makeText(this, String.format("Too few features (%s) were tracked in the fisheye image. This may result in poor odometry!", value), Toast.LENGTH_SHORT).show();
+		  {
+			  str = String.format("Too few features (%s) were tracked in the fisheye image. This may result in poor odometry!", value);
+		  }
+	  }
+	  else	
+	  {
+		  str = String.format("Unknown Tango event detected!? (type=%d)", type);
+	  }
+	  if(str!=null)
+	  {
+		  mToast.setText(str);
+		  mToast.show();
 	  }
-	  else
-			  Toast.makeText(this, String.format("Unknown Tango event detected!? (type=%d)", type), Toast.LENGTH_SHORT).show();
  }
  
  //called from jni
@@ -503,13 +578,14 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		  final String key,
 		  final String value)
  {
-	  Log.i(TAG, String.format("tangoEventCallback()"));
-
-	  runOnUiThread(new Runnable() {
-		    public void run() {
-		    	tangoEventUI(type, key, value);
-		    } 
-		});
+	  if(mItemPause != null && !mItemPause.isChecked())
+	  {
+		  runOnUiThread(new Runnable() {
+			    public void run() {
+			    	tangoEventUI(type, key, value);
+			    } 
+			});
+	  }
  }
  
  private boolean CheckTangoCoreVersion(int minVersion) {
@@ -542,7 +618,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	            @Override
 	            public boolean accept(File dir, String filename) {
 	                File sel = new File(dir, filename);
-	                return filename.endsWith(".db") || sel.isDirectory();
+	                return filename.endsWith(".db");
 	            }

 	        };
@@ -563,6 +639,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
    	  mItemSave.setEnabled(item.isChecked());
 		  mItemExport.setEnabled(item.isChecked());
    	  mItemOpen.setEnabled(item.isChecked());
+    	  mItemPostProcessing.setEnabled(item.isChecked());
    	 // mItemSave.setEnabled(item.isChecked() && !mWorkingDirectory.isEmpty());
    	  if(item.isChecked())
    	  {
@@ -575,6 +652,85 @@ public class RTABMapActivity extends Activity implements OnClickListener {
    		  ((TextView)findViewById(R.id.status)).setText(mItemLocalizationMode.isChecked()?"Localization":"Mapping");
    	  }
      } 
+      else if (itemId == R.id.detect_more_loop_closures)
+      {
+			mProgressDialog.setTitle("Post-Processing");
+			mProgressDialog.setMessage(String.format("Please wait while detecting more loop closures..."));
+			mProgressDialog.show();
+			
+			Thread workingThread = new Thread(new Runnable() {
+				    public void run() {
+				    	final int loopDetected = RTABMapLib.postProcessing(2);
+				    	runOnUiThread(new Runnable() {
+				    		public void run() {
+								mProgressDialog.dismiss();
+								if(loopDetected >= 0)
+								{
+									mTotalLoopClosures+=loopDetected;
+									mToast.makeText(getActivity(), String.format("Detection done! %d new loop closure(s) added.", loopDetected), mToast.LENGTH_SHORT).show();
+								}
+								else if(loopDetected < 0)
+								{
+									mToast.makeText(getActivity(), String.format("Detection failed!"), mToast.LENGTH_SHORT).show();
+								}
+				    		}
+				    	});
+				    } 
+			});
+			workingThread.start();
+      }
+      else if (itemId == R.id.global_graph_optimization)
+      {
+			mProgressDialog.setTitle("Post-Processing");
+			mProgressDialog.setMessage(String.format("Global graph optimization..."));
+			mProgressDialog.show();
+			
+			Thread workingThread = new Thread(new Runnable() {
+				    public void run() {
+				    	final int value = RTABMapLib.postProcessing(0);
+				    	runOnUiThread(new Runnable() {
+				    		public void run() {
+								mProgressDialog.dismiss();
+								if(value >= 0)
+								{
+									mToast.makeText(getActivity(), String.format("Optimization done!"), mToast.LENGTH_SHORT).show();
+								}
+								else if(value < 0)
+								{
+									mToast.makeText(getActivity(), String.format("Optimization failed!"), mToast.LENGTH_SHORT).show();
+								}
+				    		}
+				    	});
+				    } 
+			});
+			workingThread.start();
+      }
+      else if (itemId == R.id.sba)
+      {
+			mProgressDialog.setTitle("Post-Processing");
+			mProgressDialog.setMessage(String.format("Bundle adjustement..."));
+			mProgressDialog.show();
+			
+			Thread workingThread = new Thread(new Runnable() {
+				    public void run() {
+				    	final int value = RTABMapLib.postProcessing(1);
+				    	runOnUiThread(new Runnable() {
+				    		public void run() {
+								mProgressDialog.dismiss();
+								if(value >= 0)
+								{
+									mToast.makeText(getActivity(), String.format("Optimization done!"), mToast.LENGTH_SHORT).show();
+								}
+								else if(value < 0)
+								{
+									mToast.makeText(getActivity(), String.format("Optimization failed!"), mToast.LENGTH_SHORT).show();
+								}
+				    		}
+				    	});
+				    } 
+			});
+			workingThread.start();
+      }
      else if(itemId == R.id.debug)
      {
    	  item.setChecked(!item.isChecked());
@@ -617,6 +773,11 @@ public class RTABMapActivity extends Activity implements OnClickListener {
    	  item.setChecked(!item.isChecked());
    	  RTABMapLib.setGraphOptimization(item.isChecked());
      }
+      else if(itemId == R.id.nodes_filtering)
+      {
+    	  item.setChecked(!item.isChecked());
+    	  RTABMapLib.setNodesFiltering(item.isChecked());
+      }
      else if(itemId == R.id.graph_visible)
      {
    	  item.setChecked(!item.isChecked());
@@ -626,6 +787,177 @@ public class RTABMapActivity extends Activity implements OnClickListener {
      {
    	  item.setChecked(!item.isChecked());
    	  RTABMapLib.setAutoExposure(item.isChecked());
+    	  
+    	  // restart Tango service
+    	  onPause();
+    	  onResume();    	  
+      }
+      else if(itemId == R.id.resolution)
+      {
+    	  item.setChecked(!item.isChecked());
+    	  RTABMapLib.setFullResolution(item.isChecked());
+      }
+      else if(itemId == R.id.max_depth)
+      {
+    	  // get double
+		  AlertDialog.Builder builder = new AlertDialog.Builder(this);
+		  builder.setTitle("Max Depth (m)");
+		  final String[] values = {"1", "2", "3", "4", "5", "No Limit"};
+		  builder.setSingleChoiceItems(values, mMaxDepthIndex, new DialogInterface.OnClickListener() {
+		    @Override
+		    public void onClick(DialogInterface dialog, int which) {
+		        dialog.dismiss();
+		        if(which >=0 && which < 6)
+		        {
+			        mMaxDepthIndex = which;
+			        RTABMapLib.setMaxCloudDepth(which < 5?Float.parseFloat(values[which]):0);
+		        }
+		    }
+		  });
+		  builder.show();
+      }
+      else if(itemId == R.id.mesh_angle_tolerance)
+      {
+    	  // get double
+		  AlertDialog.Builder builder = new AlertDialog.Builder(this);
+		  builder.setTitle("Mesh Angle Tolerance (deg)");
+		  final String[] values = {"5", "10", "15", "20", "25", "30"};
+		  builder.setSingleChoiceItems(values, mMeshAngleToleranceIndex, new DialogInterface.OnClickListener() {
+		    @Override
+		    public void onClick(DialogInterface dialog, int which) {
+		        dialog.dismiss();
+		        if(which >=0 && which < 6)
+		        {
+		        	mMeshAngleToleranceIndex = which;
+			        RTABMapLib.setMeshAngleTolerance(Float.parseFloat(values[which]));
+		        }
+		    }
+		  });
+		  builder.show();
+      }
+      else if(itemId == R.id.mesh_triangle_size)
+      {
+    	  // get double
+		  AlertDialog.Builder builder = new AlertDialog.Builder(this);
+		  builder.setTitle("Mesh Triangle Size (pixels)");
+		  final String[] values = {"2", "3", "4", "5", "6"};
+		  builder.setSingleChoiceItems(values, mMeshTriangleSizeIndex, new DialogInterface.OnClickListener() {
+		    @Override
+		    public void onClick(DialogInterface dialog, int which) {
+		        dialog.dismiss();
+		        if(which >=0 && which < 5)
+		        {
+		        	mMeshTriangleSizeIndex = which;
+			        RTABMapLib.setMeshTriangleSize(Integer.parseInt(values[which]));
+		        }
+		    }
+		  });
+		  builder.show();
+      }
+      else if(itemId == R.id.update_rate)
+      {
+    	  // get double
+		  AlertDialog.Builder builder = new AlertDialog.Builder(this);
+		  builder.setTitle("Update Rate (Hz)");
+		  builder.setSingleChoiceItems(mUpdateRateValues, mParamUpdateRateHzIndex, new DialogInterface.OnClickListener() {
+		    @Override
+		    public void onClick(DialogInterface dialog, int which) {
+		        dialog.dismiss();
+		        if(which >=0 && which < mUpdateRateValues.length)
+		        {
+		        	mParamUpdateRateHzIndex = which;
+			        if(RTABMapLib.setMappingParameter("Rtabmap/DetectionRate", mUpdateRateValues[which]) != 0)
+			        {
+			        	mToast.makeText(getActivity(), "Failed to set parameter \"Rtabmap/DetectionRate\"!", mToast.LENGTH_LONG).show();
+			        }
+		        }
+		    }
+		  });
+		  builder.show();
+      }
+      else if(itemId == R.id.time_threshold)
+      {
+    	  // get double
+		  AlertDialog.Builder builder = new AlertDialog.Builder(this);
+		  builder.setTitle("Time Threshold (ms)");
+		  builder.setSingleChoiceItems(mTimeThrValues, mParamTimeThrMsIndex, new DialogInterface.OnClickListener() {
+		    @Override
+		    public void onClick(DialogInterface dialog, int which) {
+		        dialog.dismiss();
+		        if(which >=0 && which < mTimeThrValues.length)
+		        {
+		        	mParamTimeThrMsIndex = which;
+			        if(RTABMapLib.setMappingParameter("Rtabmap/TimeThr", which==mTimeThrValues.length-1?"0":mTimeThrValues[which]) != 0)
+			        {
+			        	mToast.makeText(getActivity(), "Failed to set parameter \"Rtabmap/TimeThr\"!", mToast.LENGTH_LONG).show();
+			        }
+		        }
+		    }
+		  });
+		  builder.show();
+      }
+      else if(itemId == R.id.features)
+      {
+    	  // get double
+		  AlertDialog.Builder builder = new AlertDialog.Builder(this);
+		  builder.setTitle("Max Features");
+		  builder.setSingleChoiceItems(mMaxFeaturesValues, mParamMaxFeaturesIndex, new DialogInterface.OnClickListener() {
+		    @Override
+		    public void onClick(DialogInterface dialog, int which) {
+		        dialog.dismiss();
+		        if(which >=0 && which < mMaxFeaturesValues.length)
+		        {
+		        	mParamMaxFeaturesIndex = which;
+			        if(RTABMapLib.setMappingParameter("Kp/MaxFeatures", which==0?"-1":which==mMaxFeaturesValues.length-1?"0":mMaxFeaturesValues[which]) != 0)
+			        {
+			        	mToast.makeText(getActivity(),"Failed to set parameter \"Kp/MaxFeatures\"!", mToast.LENGTH_LONG).show();
+			        }
+		        }
+		    }
+		  });
+		  builder.show();
+      }
+      else if(itemId == R.id.loop_threshold)
+      {
+    	  // get double
+		  AlertDialog.Builder builder = new AlertDialog.Builder(this);
+		  builder.setTitle("Loop Closure Threshold");
+		  builder.setSingleChoiceItems(mLoopThrValues, mParamLoopThrMsIndex, new DialogInterface.OnClickListener() {
+		    @Override
+		    public void onClick(DialogInterface dialog, int which) {
+		        dialog.dismiss();
+		        if(which >=0 && which < mLoopThrValues.length)
+		        {
+		        	mParamLoopThrMsIndex = which;
+			        if(RTABMapLib.setMappingParameter("Rtabmap/LoopThr", which==0?"1":mLoopThrValues[which]) != 0)
+			        {
+			        	mToast.makeText(getActivity(), "Failed to set parameter \"Rtabmap/LoopThr\"!", mToast.LENGTH_LONG).show();
+			        }
+		        }
+		    }
+		  });
+		  builder.show();
+      }
+      else if(itemId == R.id.optimize_error)
+      {
+    	  // get double
+		  AlertDialog.Builder builder = new AlertDialog.Builder(this);
+		  builder.setTitle("Max Optimization Error (m)");
+		  builder.setSingleChoiceItems(mOptimizeErrorValues, mParamOptimizeErrorIndex, new DialogInterface.OnClickListener() {
+		    @Override
+		    public void onClick(DialogInterface dialog, int which) {
+		        dialog.dismiss();
+		        if(which >=0 && which < mOptimizeErrorValues.length)
+		        {
+		        	mParamOptimizeErrorIndex = which;
+			        if(RTABMapLib.setMappingParameter("RGBD/OptimizeMaxError", which==0?"0":mOptimizeErrorValues[which]) != 0)
+			        {
+			        	mToast.makeText(getActivity(), "Failed to set parameter \"RGBD/OptimizeMaxError\"!", mToast.LENGTH_LONG).show();
+			        }
+		        }
+		    }
+		  });
+		  builder.show();
      }
      else if (itemId == R.id.save)
      {
@@ -640,7 +972,8 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 				  @Override
 				  public void onClick(DialogInterface dialog, int which)
 				  {
-					  final String fileName = input.getText().toString();      
+					  final String fileName = input.getText().toString();  
+					  dialog.dismiss();
 					  if(!fileName.isEmpty())
 					  {
 						  File newFile = new File(mWorkingDirectory + fileName + ".db");
@@ -661,6 +994,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 										//disable gui actions
 										mItemSave.setEnabled(false);
 										mItemOpen.setEnabled(false);
+										mItemPostProcessing.setEnabled(false);
 										mItemExport.setEnabled(false);
 				                    }
 				                })
@@ -683,6 +1017,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 							  //disable gui actions
 							  mItemSave.setEnabled(false);
 							  mItemOpen.setEnabled(false);
+							  mItemPostProcessing.setEnabled(false);
 							  mItemExport.setEnabled(false);
 						  }
 					  }
@@ -700,6 +1035,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
    		  //disable gui actions
 			  mItemSave.setEnabled(false);
 			  mItemOpen.setEnabled(false);
+			  mItemPostProcessing.setEnabled(false);
 			  mItemExport.setEnabled(false);
    	  }
      }
@@ -715,6 +1051,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
    	  ((TextView)findViewById(R.id.features)).setText(String.valueOf(0));
    	  ((TextView)findViewById(R.id.update_time)).setText(String.valueOf(0));
    	  ((TextView)findViewById(R.id.hypothesis)).setText(String.valueOf(0));
+    	  ((TextView)findViewById(R.id.fps)).setText(String.valueOf(0));
 		  mTotalLoopClosures = 0;
 		  ((TextView)findViewById(R.id.total_loop)).setText(String.valueOf(mTotalLoopClosures));
    	  
@@ -728,10 +1065,15 @@ public class RTABMapActivity extends Activity implements OnClickListener {
        	  RTABMapLib.openDatabase(mTempDatabasePath);
 		  }
      }
-      else if(itemId == R.id.export)
+      else if(itemId == R.id.export_obj || itemId == R.id.export_ply)
      {
+    	  final String extension = itemId == R.id.export_ply ? ".ply" : ".obj";
+    	  final boolean isOBJ = itemId == R.id.export_obj;
+    	  
+    	  final int polygons = Integer.parseInt(((TextView)findViewById(R.id.polygons)).getText().toString());
+    	  
    	  AlertDialog.Builder builder = new AlertDialog.Builder(this);
-		  builder.setTitle("File Name (*.ply):");
+		  builder.setTitle(String.format("File Name (*%s):", extension));
 		  final EditText input = new EditText(this);
 		  input.setInputType(InputType.TYPE_CLASS_TEXT);        
 		  builder.setView(input);
@@ -739,10 +1081,11 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			  @Override
 			  public void onClick(DialogInterface dialog, int which)
 			  {
-				  final String fileName = input.getText().toString();      
+				  final String fileName = input.getText().toString();    
+				  dialog.dismiss();
 				  if(!fileName.isEmpty())
 				  {
-					  File newFile = new File(mWorkingDirectory + fileName + ".ply");
+					  File newFile = new File(mWorkingDirectory + fileName + extension);
 					  if(newFile.exists())
 					  {
 						  new AlertDialog.Builder(getActivity())
@@ -750,12 +1093,26 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			                .setMessage("Do you want to overwrite the existing file?")
 			                .setPositiveButton("Yes", new DialogInterface.OnClickListener() {
 			                    public void onClick(DialogInterface dialog, int which) {
-			                    	final String path = mWorkingDirectory + fileName + ".ply";
+			                    	final String path = mWorkingDirectory + fileName + extension;
 			                    	
 			                    	mItemExport.setEnabled(false);
-			                    	
+			                    			                    	
 			                    	mProgressDialog.setTitle("Exporting");
-			                  	    mProgressDialog.setMessage(String.format("Please wait while exporting \"%s\"...", fileName+".ply"));
+			                    	if(polygons > 1000000)
+			                    	{
+			                    		mProgressDialog.setMessage(String.format(
+			                    				"Please wait while exporting \"%s\"...\n"
+			                    				+ "Tip: With more than 1M polygons, to reduce exporting time and file size, consider:\n"
+			                    				+ " a) increasing triangle size (Rendering options)\n"
+			                    				+ " b) decreasing maximum camera depth (Rendering options)\n"
+			                    				+ " c) activate Nodes Filtering (Mapping options)\n"
+			                    				+ "then save/open to refresh the meshes.", fileName+extension));
+			                    	}
+			                    	else
+			                    	{
+			                    		mProgressDialog.setMessage(String.format("Please wait while exporting \"%s\"...", fileName+extension));
+			                    	}
+			                  	    
 			                  	    mProgressDialog.show();
 			                  	  
 			                    	Thread exportThread = new Thread(new Runnable() {
@@ -765,11 +1122,18 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			                		    		public void run() {
 					                		    	if(success)
 							                    	{
-							                    		Toast.makeText(getActivity(), String.format("Mesh \"%s\" successfully exported!", path), Toast.LENGTH_LONG).show();
+					                		    		if(isOBJ)
+					                		    		{	
+					                		    			mToast.makeText(getActivity(), String.format("Mesh \"%s\" (with textures \"%s/\" and \"%s\") successfully exported!", path, fileName, fileName + ".mtl"), mToast.LENGTH_LONG).show();
+						                		    	}
+					                		    		else
+					                		    		{
+					                		    			mToast.makeText(getActivity(), String.format("Mesh \"%s\" successfully exported!", path), mToast.LENGTH_LONG).show();
+					                		    		}
 							                    	}
 							                    	else
 							                    	{
-							                    		Toast.makeText(getActivity(), String.format("Exporting mesh to \"%s\" failed!", path), Toast.LENGTH_LONG).show();
+							                    		mToast.makeText(getActivity(), String.format("Exporting mesh to \"%s\" failed!", path), mToast.LENGTH_LONG).show();
 							                    	}
 					                		    	mItemExport.setEnabled(true);
 					                		    	mProgressDialog.dismiss();
@@ -789,10 +1153,10 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 					  }
 					  else
 					  {
-						  final String path = mWorkingDirectory + fileName + ".ply";
+						  final String path = mWorkingDirectory + fileName + extension;
 						  mItemExport.setEnabled(false);
 						  mProgressDialog.setTitle("Exporting");
-	                  	  mProgressDialog.setMessage(String.format("Please wait while exporting \"%s\"...", fileName+".ply"));
+	                  	  mProgressDialog.setMessage(String.format("Please wait while exporting \"%s\"...", fileName+extension));
 	                  	  mProgressDialog.show();
 						  Thread exportThread = new Thread(new Runnable() {
 	                		    public void run() {
@@ -801,11 +1165,18 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	                		    		public void run() {
 			                		    	if(success)
 					                    	{
-					                    		Toast.makeText(getActivity(), String.format("Mesh \"%s\" successfully exported!", path), Toast.LENGTH_LONG).show();
+			                		    		if(isOBJ)
+			                		    		{	
+			                		    			mToast.makeText(getActivity(), String.format("Mesh \"%s\" (with textures \"%s/\" and \"%s\") successfully exported!", path, fileName, fileName + ".mtl"), mToast.LENGTH_LONG).show();
+			                		    		}
+			                		    		else
+			                		    		{
+			                		    			mToast.makeText(getActivity(), String.format("Mesh \"%s\" successfully exported!", path), mToast.LENGTH_LONG).show();
+			                		    		}
 					                    	}
 					                    	else
 					                    	{
-					                    		Toast.makeText(getActivity(), String.format("Exporting mesh to \"%s\" failed!", path), Toast.LENGTH_LONG).show();
+					                    		mToast.makeText(getActivity(), String.format("Exporting mesh to \"%s\" failed!", path), mToast.LENGTH_LONG).show();
 					                    	}
 			                		    	mItemExport.setEnabled(true);
 			                		    	mProgressDialog.dismiss();
@@ -834,7 +1205,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
                	  if(!mItemTrajectoryMode.isChecked())
                	  {
 	                	  mProgressDialog.setTitle("Loading");
-	                	  mProgressDialog.setMessage(String.format("Please wait while loading \"%s\"...", files[which]));
+	                	  mProgressDialog.setMessage(String.format("Database \"%s\" loaded. Please wait while creating point clouds and meshes...", files[which]));
 	                	  mProgressDialog.show();
                	  }

@@ -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,29 @@ 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 int onResume();
+    /*
+     * 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();
@@ -50,12 +62,19 @@ public class RTABMapLib
    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 setFullResolution(boolean enabled);
+    public static native void setMaxCloudDepth(float value);
+    public static native void setMeshAngleTolerance(float value);
+    public static native void setMeshTriangleSize(int value);
+    public static native int setMappingParameter(String key, String value);

    public static native void resetMapping();
    public static native void save();
    public static native boolean exportMesh(String filePath);
+    public static native int postProcessing(int approach);
    
    public static native String getStatus();
    public static native int getTotalNodes();
@@ -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;
+  }
+}
@@ -5,7 +5,7 @@ SET(headers_ui
 )

 #This will generate moc_* for Qt
-IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
+IF(QT4_FOUND)
    QT4_WRAP_CPP(moc_srcs ${headers_ui})
 ELSE()
    QT5_WRAP_CPP(moc_srcs ${headers_ui})
@@ -25,9 +25,9 @@ SET(INCLUDE_DIRS
    ${PCL_INCLUDE_DIRS}
 )

-IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
+IF(QT4_FOUND)
    INCLUDE(${QT_USE_FILE})
-ENDIF()
+ENDIF(QT4_FOUND)

 SET(LIBRARIES
    ${QT_LIBRARIES} 
@@ -73,9 +73,9 @@ ELSE()
  ADD_EXECUTABLE(rtabmap ${SRC_FILES})
 ENDIF()
 TARGET_LINK_LIBRARIES(rtabmap rtabmap_core rtabmap_gui rtabmap_utilite ${LIBRARIES})
-IF("${RTABMAP_QT_VERSION}" STREQUAL "5")
+IF(Qt5_FOUND)
    QT5_USE_MODULES(rtabmap Widgets Core Gui Svg PrintSupport)
-ENDIF()
+ENDIF(Qt5_FOUND)

 IF(APPLE AND BUILD_AS_BUNDLE)
  SET_TARGET_PROPERTIES(rtabmap PROPERTIES
@@ -137,11 +137,25 @@ IF((APPLE AND BUILD_AS_BUNDLE) OR WIN32)
  # Install needed Qt plugins by copying directories from the qt installation
  # One can cull what gets copied by using 'REGEX "..." EXCLUDE'
  # Exclude debug libraries
-  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
@@ -166,7 +180,7 @@ IF((APPLE AND BUILD_AS_BUNDLE) OR WIN32)
  # over.
  # To find dependencies, cmake use "otool" on Apple and "dumpbin" on Windows (make sure you have one of them).
  install(CODE "
-   file(GLOB_RECURSE QTPLUGINS \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${plugin_dest_dir}/plugins/*${CMAKE_SHARED_LIBRARY_SUFFIX}\")
+   file(GLOB_RECURSE QTPLUGINS \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${plugin_dest_dir}/plugins/*${CMAKE_SHARED_LIBRARY_SUFFIX}\")
    set(BU_CHMOD_BUNDLE_ITEMS ON)
    include(\"BundleUtilities\")
    fixup_bundle(\"${APPS}\" \"\${QTPLUGINS}\" \"${DIRS}\")
@@ -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
@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.

 Redistribution and use in source and binary forms, with or without
@@ -33,6 +33,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/gui/MainWindow.h"
 #include <QMessageBox>
 #include "rtabmap/utilite/UObjDeletionThread.h"
+#include "rtabmap/utilite/UFile.h"
+#include "rtabmap/utilite/UConversion.h"
 #include "ObjDeletionHandler.h"

 using namespace rtabmap;
@@ -46,6 +48,19 @@ int main(int argc, char* argv[])
 	/* Create tasks */
 	QApplication * app = new QApplication(argc, argv);
 	MainWindow * mainWindow = new MainWindow();
+    app->installEventFilter(mainWindow); // to catch FileOpen events.
+    
+    std::string database;
+    for(int i=1; i<argc; ++i)
+    {
+        std::string value = uReplaceChar(argv[i], '~', UDirectory::homeDir());
+        if(UFile::exists(value) &&
+           UFile::getExtension(value).compare("db") == 0)
+        {
+            database = value;
+            break;
+        }
+    }

 	UINFO("Program started...");

@@ -64,6 +79,11 @@ int main(int argc, char* argv[])
 	RtabmapThread * rtabmap = new RtabmapThread(new Rtabmap());
 	rtabmap->start(); // start it not initialized... will be initialized by event from the gui
 	UEventsManager::addHandler(rtabmap);
+    
+    if(!database.empty())
+    {
+    	QMetaObject::invokeMethod(mainWindow, "openDatabase", Qt::QueuedConnection, Q_ARG(QString, QString(database.c_str())));
+    }

 	// Now wait for application to finish
 	app->connect( app, SIGNAL( lastWindowClosed() ),
@@ -79,6 +79,7 @@ IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FO
 	${G2O_CORE_LIBRARY}
 	${G2O_TYPES_SLAM2D} 
 	${G2O_TYPES_SLAM3D}
+	${G2O_TYPES_SBA}
        ${G2O_STUFF_LIBRARY})
  
  IF(CSPARSE_FOUND)
@@ -0,0 +1,56 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE plist PUBLIC "-//Apple Computer//DTD PLIST 1.0//EN" "http://www.apple.com/DTDs/PropertyList-1.0.dtd">
+<plist version="1.0">
+<dict>
+	<key>CFBundleDevelopmentRegion</key>
+	<string>English</string>
+	<key>CFBundleExecutable</key>
+	<string>${MACOSX_BUNDLE_EXECUTABLE_NAME}</string>
+	<key>CFBundleGetInfoString</key>
+	<string>${MACOSX_BUNDLE_INFO_STRING}</string>
+	<key>CFBundleIconFile</key>
+	<string>${MACOSX_BUNDLE_ICON_FILE}</string>
+	<key>CFBundleIdentifier</key>
+	<string>${MACOSX_BUNDLE_GUI_IDENTIFIER}</string>
+	<key>CFBundleInfoDictionaryVersion</key>
+	<string>6.0</string>
+	<key>CFBundleLongVersionString</key>
+	<string>${MACOSX_BUNDLE_LONG_VERSION_STRING}</string>
+	<key>CFBundleName</key>
+	<string>${MACOSX_BUNDLE_BUNDLE_NAME}</string>
+	<key>CFBundlePackageType</key>
+	<string>APPL</string>
+	<key>CFBundleShortVersionString</key>
+	<string>${MACOSX_BUNDLE_SHORT_VERSION_STRING}</string>
+	<key>CFBundleSignature</key>
+	<string>????</string>
+	<key>CFBundleVersion</key>
+	<string>${MACOSX_BUNDLE_BUNDLE_VERSION}</string>
+	<key>CSResourcesFileMapped</key>
+	<true/>
+	<key>LSRequiresCarbon</key>
+	<true/>
+	<key>NSHumanReadableCopyright</key>
+	<string>${MACOSX_BUNDLE_COPYRIGHT}</string>
+
+<!-- File type associations -->
+<key>CFBundleDocumentTypes</key>
+<array>
+	<dict>
+		<key>CFBundleTypeExtensions</key>
+		<array>
+			<string>db</string>
+		</array>
+		<!-- <key>CFBundleTypeIconFile</key> -->
+		<!-- <string>rtabmap_db.icns</string> -->
+		<key>CFBundleTypeName</key>
+		<string>RTAB-Map Database</string>
+		<key>CFBundleTypeRole</key>
+		<string>Editor</string>
+		<key>LSIsAppleDefaultForType</key>
+		<string>Yes</string>
+	</dict>
+</array>
+
+</dict>
+</plist>
@@ -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
@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.

 Redistribution and use in source and binary forms, with or without
@@ -56,6 +56,7 @@ public:
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "") = 0;
 	virtual bool isCalibrated() const = 0;
 	virtual std::string getSerial() const = 0;
+	virtual bool odomProvided() const { return false; }

 	//getters
 	float getImageRate() const {return _imageRate;}
@@ -76,7 +77,7 @@ protected:
 	/**
 	 * returned rgb and depth images should be already rectified if calibration was loaded
 	 */
-	virtual SensorData captureImage() = 0;
+	virtual SensorData captureImage(CameraInfo * info = 0) = 0;

 	int getNextSeqID() {return ++_seq;}

@@ -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
@@ -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,27 @@ 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),
+		odomCovariance(cv::Mat::eye(6,6,CV_64FC1))
 	{
 	}
 	virtual ~CameraInfo() {}

 	std::string cameraName;
 	int id;
+	double stamp;
 	float timeCapture;
 	float timeDisparity;
 	float timeMirroring;
 	float timeImageDecimation;
 	float timeScanFromDepth;
+	Transform odomPose;
+	cv::Mat odomCovariance;
 };

 } // namespace rtabmap
@@ -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
@@ -99,7 +99,7 @@ public:
 	cv::Mat K_raw() const {return K_;} //intrinsic camera matrix (before rectification)
 	cv::Mat D_raw() const {return D_;} //intrinsic distorsion matrix (before rectification)
 	cv::Mat K() const {return !P_.empty()?P_.colRange(0,3):K_;} // if P exists, return rectified version
-	cv::Mat D() const {return P_.empty()&&!D_.empty()?D_:cv::Mat::zeros(1,4,CV_64FC1);} // if P exists, return rectified version
+	cv::Mat D() const {return P_.empty()&&!D_.empty()?D_:cv::Mat::zeros(1,5,CV_64FC1);} // if P exists, return rectified version
 	cv::Mat R() const {return R_;} //rectification matrix
 	cv::Mat P() const {return P_;} //projection matrix

@@ -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
@@ -119,7 +119,7 @@ public:
 	}

 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);

 private:
 	std::string _path;
@@ -178,6 +178,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 +194,7 @@ public:
 	const std::string & getFilePath() const {return _filePath;}

 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);

 private:
 	// File type
@@ -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
@@ -97,7 +97,7 @@ public:
    virtual std::string getSerial() const;

 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);

 private:
    pcl::Grabber* interface_;
@@ -131,7 +131,7 @@ public:
 	virtual std::string getSerial() const {return "";} // unknown with OpenCV

 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);

 private:
 	bool _asus;
@@ -168,7 +168,7 @@ public:
 	void setOpenNI2StampsAndIDsUsed(bool used) {_openNI2StampsAndIDsUsed = used;}

 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);

 private:
 	openni::Device * _device;
@@ -204,7 +204,7 @@ public:
 	virtual std::string getSerial() const;

 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);

 private:
 	int deviceId_;
@@ -249,7 +249,7 @@ public:
 	virtual std::string getSerial() const;

 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);

 private:
 	int deviceId_;
@@ -291,7 +291,7 @@ public:
 	virtual std::string getSerial() const;

 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);

 private:
 	CameraImages cameraDepth_;
@@ -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,14 @@ namespace FlyCapture2
 class Camera;
 }

+namespace sl
+{
+namespace zed
+{
+class Camera;
+}
+}
+
 namespace rtabmap
 {

@@ -62,7 +70,7 @@ public:
 	virtual std::string getSerial() const;

 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);

 private:
 	DC1394Device *device_;
@@ -87,13 +95,64 @@ public:
 	virtual std::string getSerial() const;

 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);

 private:
 	FlyCapture2::Camera * camera_;
 	void * triclopsCtx_; // TriclopsContext
 };

+/////////////////////////
+// CameraStereoZED
+/////////////////////////
+class RTABMAP_EXP CameraStereoZed :
+	public Camera
+{
+public:
+	static bool available();
+
+public:
+	CameraStereoZed(
+			int deviceId,
+			int resolution = 2, // 0=HD2K, 1=HD1080, 2=HD720, 3=VGA
+			int quality = 1,    // 0=NONE, 1=PERFORMANCE, 2=QUALITY
+			int sensingMode = 1,// 0=FULL, 1=RAW
+			int confidenceThr = 100,
+			bool computeOdometry = false,
+			float imageRate=0.0f,
+			const Transform & localTransform = Transform::getIdentity());
+	CameraStereoZed(
+			const std::string & svoFilePath,
+			int quality = 1,    // 0=NONE, 1=PERFORMANCE, 2=QUALITY
+			int sensingMode = 1,// 0=FULL, 1=RAW
+			int confidenceThr = 100,
+			bool computeOdometry = false,
+			float imageRate=0.0f,
+			const Transform & localTransform = Transform::getIdentity());
+	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 { return computeOdometry_; }
+
+protected:
+	virtual SensorData captureImage(CameraInfo * info = 0);
+
+private:
+	sl::zed::Camera * zed_;
+	StereoCameraModel stereoModel_;
+	CameraVideo::Source src_;
+	int usbDevice_;
+	std::string svoFilePath_;
+	int resolution_;
+	int quality_;
+	int sensingMode_;
+	int confidenceThr_;
+	bool computeOdometry_;
+	bool lost_;
+};
+
 /////////////////////////
 // CameraStereoImages
 /////////////////////////
@@ -123,7 +182,7 @@ public:
 	virtual std::string getSerial() const;

 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);

 private:
 	CameraImages * camera2_;
@@ -147,6 +206,11 @@ public:
 			bool rectifyImages = false,
 			float imageRate=0.0f,
 			const Transform & localTransform = Transform::getIdentity());
+	CameraStereoVideo(
+			int device,
+			bool rectifyImages = false,
+			float imageRate = 0.0f,
+			const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraStereoVideo();

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -154,7 +218,7 @@ public:
 	virtual std::string getSerial() const;

 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);

 private:
 	cv::VideoCapture capture_;
@@ -162,6 +226,8 @@ private:
 	bool rectifyImages_;
 	StereoCameraModel stereoModel_;
 	std::string cameraName_;
+	CameraVideo::Source src_;
+	int usbDevice_;
 };

 } // namespace rtabmap
@@ -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
@@ -91,6 +91,7 @@ private:
 	bool _scanFromDepth;
 	int _scanDecimation;
 	float _scanMaxDepth;
+	float _scanMinDepth;
 	float _scanVoxelSize;
 	int _scanNormalsK;
 	StereoDense * _stereoDense;
@@ -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
@@ -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
@@ -88,13 +88,13 @@ public:
 	void updateLink(const Link & link);

 public:
-	void addStatisticsAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed, int dictionarySize) const;
+	void addStatisticsAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed, int dictionarySize, const ParametersMap & parameters) const;

 public:
 	// Mutex-protected methods of abstract versions below

 	bool openConnection(const std::string & url, bool overwritten = false);
-	void closeConnection();
+	void closeConnection(bool save = true);
 	bool isConnected() const;
 	long getMemoryUsed() const; // In bytes
 	std::string getDatabaseVersion() const;
@@ -107,6 +107,7 @@ public:
 	int getLastDictionarySize() const; // working memory
 	int getTotalNodesSize() const;
 	int getTotalDictionarySize() const;
+	ParametersMap getLastParameters() const;

 	void executeNoResult(const std::string & sql) const;

@@ -119,6 +120,7 @@ public:
 	// Specific queries...
 	void loadNodeData(std::list<Signature *> & signatures) const;
 	void getNodeData(int signatureId, SensorData & data) const;
+	bool getCalibration(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
 	bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose) const;
 	void loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
 	void getWeight(int signatureId, int & weight) const;
@@ -135,7 +137,7 @@ protected:

 private:
 	virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false) = 0;
-	virtual void disconnectDatabaseQuery() = 0;
+	virtual void disconnectDatabaseQuery(bool save = true) = 0;
 	virtual bool isConnectedQuery() const = 0;
 	virtual long getMemoryUsedQuery() const = 0; // In bytes
 	virtual bool getDatabaseVersionQuery(std::string & version) const = 0;
@@ -148,6 +150,7 @@ private:
 	virtual int getLastDictionarySizeQuery() const = 0;
 	virtual int getTotalNodesSizeQuery() const = 0;
 	virtual int getTotalDictionarySizeQuery() const = 0;
+	virtual ParametersMap getLastParametersQuery() const = 0;

 	virtual void executeNoResultQuery(const std::string & sql) const = 0;

@@ -169,6 +172,7 @@ private:
 	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 bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const = 0;
 	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose) const = 0;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const = 0;
 	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const = 0;
@@ -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,34 +43,45 @@ 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;
@@ -80,6 +89,7 @@ private:
 	std::set<int>::iterator _currentId;
 	double _previousStamp;
 	int _previousMapID;
+	bool _calibrated;
 };

 } /* namespace rtabmap */
@@ -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
@@ -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
@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.

 Redistribution and use in source and binary forms, with or without
@@ -79,6 +79,13 @@ std::multimap<int, int>::const_iterator RTABMAP_EXP findLink(
 		int to,
 		bool checkBothWays = true);

+std::multimap<int, Link> RTABMAP_EXP filterLinks(
+		const std::multimap<int, Link> & links,
+		Link::Type filteredType);
+std::map<int, Link> RTABMAP_EXP filterLinks(
+		const std::map<int, Link> & links,
+		Link::Type filteredType);
+
 //Note: This assumes a coordinate system where X is forward, * Y is up, and Z is right.
 std::map<int, Transform> RTABMAP_EXP frustumPosesFiltering(
 		const std::map<int, Transform> & poses,
@@ -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
@@ -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
@@ -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
@@ -91,7 +91,7 @@ public:
 	int cleanup();
 	void emptyTrash();
 	void joinTrashThread();
-	bool addLink(const Link & link);
+	bool addLink(const Link & link, bool addInDatabase = false);
 	void updateLink(int fromId, int toId, const Transform & transform, float rotVariance, float transVariance);
 	void updateLink(int fromId, int toId, const Transform & transform, const cv::Mat & covariance);
 	void removeAllVirtualLinks();
@@ -147,10 +147,14 @@ public:
 			Transform & groundTruth,
 			bool lookInDatabase = false) const;
 	cv::Mat getImageCompressed(int signatureId) const;
-	SensorData getNodeData(int nodeId, bool uncompressedData = false, bool keepLoadedDataInMemory = true);
+	SensorData getNodeData(int nodeId, bool uncompressedData = false) const;
 	void getNodeWords(int nodeId,
 			std::multimap<int, cv::KeyPoint> & words,
-			std::multimap<int, cv::Point3f> & words3);
+			std::multimap<int, cv::Point3f> & words3,
+			std::multimap<int, cv::Mat> & wordsDescriptors);
+	void getNodeCalibration(int nodeId,
+			std::vector<CameraModel> & models,
+			StereoCameraModel & stereoModel);
 	SensorData getSignatureDataConst(int locationId) const;
 	std::set<int> getAllSignatureIds() const;
 	bool memoryChanged() const {return _memoryChanged;}
@@ -181,6 +185,7 @@ 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 computeIcpTransformMulti(
@@ -239,7 +244,8 @@ private:
 	bool _generateIds;
 	bool _badSignaturesIgnored;
 	bool _mapLabelsAdded;
-	int _imageDecimation;
+	int _imagePreDecimation;
+	int _imagePostDecimation;
 	float _laserScanDownsampleStepSize;
 	bool _reextractLoopClosureFeatures;
 	float _rehearsalMaxDistance;
@@ -0,0 +1,96 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef SRC_OCTOMAP_H_
+#define SRC_OCTOMAP_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <octomap/ColorOcTree.h>
+#include <octomap/OcTreeKey.h>
+
+#include <pcl/pcl_base.h>
+#include <pcl/point_types.h>
+
+#include <rtabmap/core/Transform.h>
+
+#include <map>
+#include <string>
+
+namespace rtabmap {
+
+class OcTreeNodeInfo
+{
+public:
+	OcTreeNodeInfo(int nodeRefId, const octomap::OcTreeKey & key, bool isObstacle) :
+		nodeRefId_(nodeRefId),
+		key_(key),
+		isObstacle_(isObstacle) {}
+	int nodeRefId_;
+	octomap::OcTreeKey key_;
+	bool isObstacle_;
+};
+
+class RTABMAP_EXP OctoMap {
+public:
+	OctoMap(float voxelSize = 0.1f);
+
+	const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
+	void addToCache(int nodeId,
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr & ground,
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstacles);
+	void update(const std::map<int, Transform> & poses);
+
+	const octomap::ColorOcTree * octree() const {return octree_;}
+
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr createCloud(
+			unsigned int treeDepth = 0,
+			std::vector<int> * obstacleIndices = 0,
+			std::vector<int> * emptyIndices = 0) const;
+
+	cv::Mat createProjectionMap(
+			float & xMin,
+			float & yMin,
+			float & gridCellSize,
+			float minGridSize);
+
+	bool writeBinary(const std::string & path);
+
+	virtual ~OctoMap();
+	void clear();
+
+private:
+	std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> > cache_;
+	octomap::ColorOcTree * octree_;
+	std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo> occupiedCells_;
+	std::map<int, Transform> addedNodes_;
+	octomap::KeyRay keyRay_;
+};
+
+} /* namespace rtabmap */
+
+#endif /* SRC_OCTOMAP_H_ */
@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.

 Redistribution and use in source and binary forms, with or without
@@ -55,12 +55,14 @@ public:
 public:
 	virtual ~Odometry();
 	Transform process(SensorData & data, OdometryInfo * info = 0);
+	Transform process(SensorData & data, const Transform & guess, OdometryInfo * info = 0);
 	virtual void reset(const Transform & initialPose = Transform::getIdentity());

 	//getters
 	const Transform & getPose() const {return _pose;}
 	bool isInfoDataFilled() const {return _fillInfoData;}
 	const Transform & previousVelocityTransform() const {return previousVelocityTransform_;}
+	double previousStamp() const {return previousStamp_;}

 private:
 	virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0) = 0;
@@ -83,6 +85,8 @@ private:
 	bool _fillInfoData;
 	float _kalmanProcessNoise;
 	float _kalmanMeasurementNoise;
+	int _imageDecimation;
+	bool _alignWithGround;
 	Transform _pose;
 	int _resetCurrentCount;
 	double previousStamp_;
@@ -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
@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.

 Redistribution and use in source and binary forms, with or without
@@ -55,7 +55,7 @@ private:

 	Registration * registrationPipeline_;
 	Signature refFrame_;
-	Transform motionSinceLastKeyFrame_;
+	Transform lastKeyFramePose_;
 };

 }
@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.

 Redistribution and use in source and binary forms, with or without
@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Odometry.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
+#include <pcl/pcl_base.h>

 namespace rtabmap {

@@ -57,13 +58,13 @@ private:
 	int maxNewFeatures_;
 	float scanKeyFrameThr_;
 	int scanMaximumMapSize_;
-	float scanSubstractRadius_;
+	float scanSubtractRadius_;
 	std::string fixedMapPath_;

 	Registration * regPipeline_;
 	Signature * map_;
 	Signature * lastFrame_;
-	std::map<int, pcl::PointCloud<pcl::PointNormal>::Ptr > scansBuffer_;
+	std::vector<std::pair<pcl::PointCloud<pcl::PointNormal>::Ptr, pcl::IndicesPtr> > scansBuffer_;
 };

 }
@@ -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
@@ -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
@@ -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
@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.

 Redistribution and use in source and binary forms, with or without
@@ -53,7 +53,7 @@ public:
 	};
 	static bool isAvailable(Optimizer::Type type);
 	static Optimizer * create(const ParametersMap & parameters);
-	static Optimizer * create(Optimizer::Type & type, const ParametersMap & parameters = ParametersMap());
+	static Optimizer * create(Optimizer::Type type, const ParametersMap & parameters = ParametersMap());

 	// Get connected poses and constraints from a set of links
 	static void getConnectedGraph(
@@ -99,6 +99,13 @@ public:

 	virtual void parseParameters(const ParametersMap & parameters);

+	void computeBACorrespondences(
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & links,
+			const std::map<int, Signature> & signatures,
+			std::map<int, cv::Point3f> & points3DMap,
+			std::map<int, std::map<int, cv::Point2f> > & wordReferences); // <ID words, IDs frames + keypoint>
+
 protected:
 	Optimizer(
 			int iterations         = Parameters::defaultOptimizerIterations(),
@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.

 Redistribution and use in source and binary forms, with or without
@@ -44,29 +44,18 @@ public:
 			int iterations         = Parameters::defaultOptimizerIterations(),
 			bool slam2d            = Parameters::defaultOptimizerSlam2D(),
 			bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored()) :
-		Optimizer(iterations, slam2d, covarianceIgnored),
-		inlierDistance_(0.02),
-		minInliers_(10){}
+		Optimizer(iterations, slam2d, covarianceIgnored) {}
 	OptimizerCVSBA(const ParametersMap & parameters) :
-		Optimizer(parameters),
-		inlierDistance_(0.02),
-		minInliers_(10){}
+		Optimizer(parameters) {}
 	virtual ~OptimizerCVSBA() {}

 	virtual Type type() const {return kTypeCVSBA;}

-	void setInlierDistance(float inlierDistance) {inlierDistance_ = inlierDistance;}
-	void setMinInliers(int minInliers) {minInliers_ = minInliers;}
-
 	virtual std::map<int, Transform> optimizeBA(
 			int rootId,
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & links,
 			const std::map<int, Signature> & signatures);
-
-private:
-	float inlierDistance_;
-	float minInliers_;
 };

 } /* namespace rtabmap */
@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.

 Redistribution and use in source and binary forms, with or without
@@ -50,7 +50,8 @@ public:
 	OptimizerG2O(const ParametersMap & parameters = ParametersMap()) :
 		Optimizer(parameters),
 		solver_(Parameters::defaultg2oSolver()),
-		optimizer_(Parameters::defaultg2oOptimizer())
+		optimizer_(Parameters::defaultg2oOptimizer()),
+        pixelVariance_(Parameters::defaultg2oPixelVariance())
 	{
 		parseParameters(parameters);
 	}
@@ -68,9 +69,16 @@ public:
 			double * finalError = 0,
 			int * iterationsDone = 0);

+	virtual std::map<int, Transform> optimizeBA(
+			int rootId,
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & links,
+			const std::map<int, Signature> & signatures);
+
 private:
 	int solver_;
 	int optimizer_;
+	double pixelVariance_;
 };

 } /* namespace rtabmap */
@@ -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
@@ -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
@@ -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
@@ -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
@@ -172,9 +172,9 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Rtabmap, PublishLastSignature, 	     bool, true, "Publishing last signature.");
 	RTABMAP_PARAM(Rtabmap, PublishPdf, 	                 bool, true, "Publishing pdf.");
 	RTABMAP_PARAM(Rtabmap, PublishLikelihood, 	         bool, true, "Publishing likelihood.");
-	RTABMAP_PARAM(Rtabmap, TimeThr, 		             float, 0.0, "Maximum time allowed for the detector (ms) (0 means infinity).");
+	RTABMAP_PARAM(Rtabmap, TimeThr, 		             float, 0,   "Maximum time allowed for the detector (ms) (0 means infinity).");
 	RTABMAP_PARAM(Rtabmap, MemoryThr, 		             int, 0, 	 "Maximum signatures in the Working Memory (ms) (0 means infinity).");
-	RTABMAP_PARAM(Rtabmap, DetectionRate,                float, 1.0, "Detection rate. RTAB-Map will filter input images to satisfy this rate.");
+	RTABMAP_PARAM(Rtabmap, DetectionRate,                float, 1,   "Detection rate. RTAB-Map will filter input images to satisfy this rate.");
 	RTABMAP_PARAM(Rtabmap, ImageBufferSize,              unsigned int, 1, "Data buffer size (0 min inf).");
 	RTABMAP_PARAM(Rtabmap, CreateIntermediateNodes,      bool, false, "Create intermediate nodes between loop closure detection. Only used when Rtabmap/DetectionRate>0.");
 	RTABMAP_PARAM_STR(Rtabmap, WorkingDirectory,         "", "Working directory.");
@@ -186,7 +186,7 @@ class RTABMAP_EXP Parameters

 	// Hypotheses selection
 	RTABMAP_PARAM(Rtabmap, LoopThr,    	     float, 0.11, 	"Loop closing threshold.");
-	RTABMAP_PARAM(Rtabmap, LoopRatio,    	 float, 0.0, 	"The loop closure hypothesis must be over LoopRatio x lastHypothesisValue.");
+	RTABMAP_PARAM(Rtabmap, LoopRatio,    	 float, 0, 	    "The loop closure hypothesis must be over LoopRatio x lastHypothesisValue.");

 	// Memory
 	RTABMAP_PARAM(Mem, RehearsalSimilarity,     float, 0.6, 	"Rehearsal similarity.");
@@ -194,7 +194,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Mem, BinDataKept, 		    bool, true, 	"Keep binary data in db.");
 	RTABMAP_PARAM(Mem, RawDescriptorsKept, 		bool, true, 	"Raw descriptors kept in memory.");
 	RTABMAP_PARAM(Mem, MapLabelsAdded, 		    bool, true, 	"Create map labels. The first node of a map will be labelled as \"map#\" where # is the map ID.");
-	RTABMAP_PARAM(Mem, SaveDepth16Format, 		bool, true, 	"Save depth image into 16 bits format to reduce memory used. Warning: values over ~65 meters are ignored (maximum 65535 millimeters).");
+	RTABMAP_PARAM(Mem, SaveDepth16Format, 		bool, false, 	"Save depth image into 16 bits format to reduce memory used. Warning: values over ~65 meters are ignored (maximum 65535 millimeters).");
 	RTABMAP_PARAM(Mem, NotLinkedNodesKept, 	    bool, true, 	"Keep not linked nodes in db (rehearsed nodes and deleted nodes).");
 	RTABMAP_PARAM(Mem, STMSize, 		        unsigned int, 10, "Short-term memory size.");
 	RTABMAP_PARAM(Mem, IncrementalMemory,       bool, true, 	"SLAM mode, otherwise it is Localization mode.");
@@ -205,24 +205,25 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Mem, RehearsalWeightIgnoredWhileMoving, bool, false, "When the robot is moving, weights are not updated on rehearsal.");
 	RTABMAP_PARAM(Mem, GenerateIds,             bool, true,     "True=Generate location IDs, False=use input image IDs.");
 	RTABMAP_PARAM(Mem, BadSignaturesIgnored,    bool, false,     "Bad signatures are ignored.");
-	RTABMAP_PARAM(Mem, InitWMWithAllNodes,      bool, false,    "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.");
-	RTABMAP_PARAM(Mem, ImageDecimation,         int, 1,          "Image decimation (>=1) when creating a signature.");
-	RTABMAP_PARAM(Mem, LaserScanDownsampleStepSize, int, 1,  "If > 1, downsample the laser scans when creating a signature.");
-	RTABMAP_PARAM(Mem, UseOdomFeatures,         bool, false,   "Use odometry features.");
+	RTABMAP_PARAM(Mem, InitWMWithAllNodes,      bool, false,     "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.");
+	RTABMAP_PARAM(Mem, ImagePreDecimation,      int, 1,          "Image decimation (>=1) before features extraction.");
+	RTABMAP_PARAM(Mem, ImagePostDecimation,     int, 1,          "Image decimation (>=1) of saved data in created signatures (after features extraction). Decimation is done from the original image.");
+	RTABMAP_PARAM(Mem, LaserScanDownsampleStepSize, int, 1,      "If > 1, downsample the laser scans when creating a signature.");
+	RTABMAP_PARAM(Mem, UseOdomFeatures,             bool, false, "Use odometry features.");

 	// KeypointMemory (Keypoint-based)
 	RTABMAP_PARAM(Kp, NNStrategy,            int, 1,            "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
 	RTABMAP_PARAM(Kp, IncrementalDictionary, bool, true, 		"");
 	RTABMAP_PARAM(Kp, IncrementalFlann,      bool, true, 		"When using FLANN based strategy, add/remove points to its index without always rebuilding the index (the index is built only when the dictionary doubles in size).");
-	RTABMAP_PARAM(Kp, MaxDepth,              float, 0.0, 		"Filter extracted keypoints by depth (0=inf).");
-	RTABMAP_PARAM(Kp, MinDepth,              float, 0.0, 		"Filter extracted keypoints by depth.");
+	RTABMAP_PARAM(Kp, MaxDepth,              float, 0,   		"Filter extracted keypoints by depth (0=inf).");
+	RTABMAP_PARAM(Kp, MinDepth,              float, 0,  		"Filter extracted keypoints by depth.");
 	RTABMAP_PARAM(Kp, MaxFeatures,           int, 400, 			"Maximum features extracted from the images (0 means not bounded, <0 means no extraction).");
 	RTABMAP_PARAM(Kp, BadSignRatio,          float, 0.5, 		"Bad signature ratio (less than Ratio x AverageWordsPerImage = bad).");
 	RTABMAP_PARAM(Kp, NndrRatio, 	         float, 0.8,		"NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)");
 #ifdef RTABMAP_NONFREE
-	RTABMAP_PARAM(Kp, DetectorStrategy,      int, 0,            "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
+	RTABMAP_PARAM(Kp, DetectorStrategy,      int, 0,            "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB.");
 #else
-	RTABMAP_PARAM(Kp, DetectorStrategy,      int, 2,            "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
+	RTABMAP_PARAM(Kp, DetectorStrategy,      int, 2,            "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB.");
 #endif
 	RTABMAP_PARAM(Kp, TfIdfLikelihoodUsed,   bool, true, 		"Use of the td-idf strategy to compute the likelihood.");
 	RTABMAP_PARAM(Kp, Parallelized,          bool, true, 		"If the dictionary update and signature creation were parallelized.");
@@ -242,7 +243,7 @@ class RTABMAP_EXP Parameters

 	// Keypoints descriptors/detectors
 	RTABMAP_PARAM(SURF, Extended, 		  bool, false, 	    "Extended descriptor flag (true - use extended 128-element descriptors; false - use 64-element descriptors).");
-	RTABMAP_PARAM(SURF, HessianThreshold, float, 500.0,		"Threshold for hessian keypoint detector used in SURF.");
+	RTABMAP_PARAM(SURF, HessianThreshold, float, 500,		"Threshold for hessian keypoint detector used in SURF.");
 	RTABMAP_PARAM(SURF, Octaves, 		  int, 4, 			"Number of pyramid octaves the keypoint detector will use.");
 	RTABMAP_PARAM(SURF, OctaveLayers, 	  int, 2, 			"Number of octave layers within each octave.");
 	RTABMAP_PARAM(SURF, Upright, 	      bool, false, 	"Up-right or rotated features flag (true - do not compute orientation of features; false - compute orientation).");
@@ -252,7 +253,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(SIFT, NFeatures,          int, 0,			"The number of best features to retain. The features are ranked by their scores (measured in SIFT algorithm as the local contrast).");
 	RTABMAP_PARAM(SIFT, NOctaveLayers,      int, 3, 		"The number of layers in each octave. 3 is the value used in D. Lowe paper. The number of octaves is computed automatically from the image resolution.");
 	RTABMAP_PARAM(SIFT, ContrastThreshold, 	double, 0.04, 	"The contrast threshold used to filter out weak features in semi-uniform (low-contrast) regions. The larger the threshold, the less features are produced by the detector.");
-	RTABMAP_PARAM(SIFT, EdgeThreshold,      double, 10.0, 	"The threshold used to filter out edge-like features. Note that the its meaning is different from the contrastThreshold, i.e. the larger the edgeThreshold, the less features are filtered out (more features are retained).");
+	RTABMAP_PARAM(SIFT, EdgeThreshold,      double, 10, 	"The threshold used to filter out edge-like features. Note that the its meaning is different from the contrastThreshold, i.e. the larger the edgeThreshold, the less features are filtered out (more features are retained).");
 	RTABMAP_PARAM(SIFT, Sigma,              double, 1.6, 	"The sigma of the Gaussian applied to the input image at the octave #0. If your image is captured with a weak camera with soft lenses, you might want to reduce the number.");

 	RTABMAP_PARAM(BRIEF, Bytes,         	   int, 32, 	"Bytes is a length of descriptor in bytes. It can be equal 16, 32 or 64 bytes.");
@@ -283,12 +284,12 @@ class RTABMAP_EXP Parameters

 	RTABMAP_PARAM(FREAK, OrientationNormalized, bool, true,   "Enable orientation normalization.");
 	RTABMAP_PARAM(FREAK, ScaleNormalized,       bool, true,   "Enable scale normalization.");
-	RTABMAP_PARAM(FREAK, PatternScale,          float, 22.0,  "Scaling of the description pattern.");
+	RTABMAP_PARAM(FREAK, PatternScale,          float, 22,    "Scaling of the description pattern.");
 	RTABMAP_PARAM(FREAK, NOctaves,              int, 4,        "Number of octaves covered by the detected keypoints.");

 	RTABMAP_PARAM(BRISK, Thresh,                int, 30,      "FAST/AGAST detection threshold score.");
 	RTABMAP_PARAM(BRISK, Octaves,               int, 3,       "Detection octaves. Use 0 to do single scale.");
-	RTABMAP_PARAM(BRISK, PatternScale,          float, 1.0,  "Apply this scale to the pattern used for sampling the neighbourhood of a keypoint.");
+	RTABMAP_PARAM(BRISK, PatternScale,          float, 1,    "Apply this scale to the pattern used for sampling the neighbourhood of a keypoint.");

 	// BayesFilter
 	RTABMAP_PARAM(Bayes, VirtualPlacePriorThr,           float, 0.9, "Virtual place prior");
@@ -297,7 +298,7 @@ class RTABMAP_EXP Parameters

 	// Verify hypotheses
 	RTABMAP_PARAM(VhEp, MatchCountMin, int, 8, 		"Minimum of matching visual words pairs to accept the loop hypothesis.");
-	RTABMAP_PARAM(VhEp, RansacParam1,  float, 3.0, 	"Fundamental matrix (see cvFindFundamentalMat()): Max distance (in pixels) from the epipolar line for a point to be inlier.");
+	RTABMAP_PARAM(VhEp, RansacParam1,  float, 3, 	"Fundamental matrix (see cvFindFundamentalMat()): Max distance (in pixels) from the epipolar line for a point to be inlier.");
 	RTABMAP_PARAM(VhEp, RansacParam2,  float, 0.99, "Fundamental matrix (see cvFindFundamentalMat()): Performance of the RANSAC.");

 	// RGB-D SLAM
@@ -306,11 +307,11 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(RGBD, AngularUpdate,            float, 0.1,  "Minimum angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
 	RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0,    "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
 	RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd,     bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
-	RTABMAP_PARAM(RGBD, OptimizeMaxError,         float, 1.0,  "Reject loop closures if optimization error is greater than this value (0=disabled). This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"Optimizer/Robust\" if enabled.");
+	RTABMAP_PARAM(RGBD, OptimizeMaxError,         float, 1,    "Reject loop closures if optimization error is greater than this value (0=disabled). This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"Optimizer/Robust\" if enabled.");
 	RTABMAP_PARAM(RGBD, GoalReachedRadius,        float, 0.5,  "Goal reached radius (m).");
 	RTABMAP_PARAM(RGBD, PlanStuckIterations,      int, 0,      "Mark the current goal node on the path as unreachable if it is not updated after X iterations (0=disabled). If all upcoming nodes on the path are unreachabled, the plan fails.");
-	RTABMAP_PARAM(RGBD, PlanLinearVelocity,       float, 0.0,  "Linear velocity (m/sec) used to compute path weights.");
-	RTABMAP_PARAM(RGBD, PlanAngularVelocity,      float, 0.0,  "Angular velocity (rad/sec) used to compute path weights.");
+	RTABMAP_PARAM(RGBD, PlanLinearVelocity,       float, 0,    "Linear velocity (m/sec) used to compute path weights.");
+	RTABMAP_PARAM(RGBD, PlanAngularVelocity,      float, 0,    "Angular velocity (rad/sec) used to compute path weights.");
 	RTABMAP_PARAM(RGBD, GoalsSavedInUserData,     bool, false, "When a goal is received and processed with success, it is saved in user data of the location with this format: \"GOAL:#\".");
 	RTABMAP_PARAM(RGBD, MaxLocalRetrieved,        unsigned int, 2, "Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority).");
 	RTABMAP_PARAM(RGBD, LocalRadius,              float, 10,   "Local radius (m) for nodes selection in the local map. This parameter is used in some approaches about the local map management.");
@@ -322,11 +323,10 @@ class RTABMAP_EXP Parameters
 	// Local/Proximity loop closure detection
 	RTABMAP_PARAM(RGBD, ProximityByTime,              bool, false, 	"Detection over all locations in STM.");
 	RTABMAP_PARAM(RGBD, ProximityBySpace,             bool, true, 	"Detection over locations (in Working Memory or STM) near in space.");
-	RTABMAP_PARAM(RGBD, ProximityPathScansMerged,     bool, true, 	"Merge close laser scans on each path. If false, only the nearest laser scan on the path is used for ICP.");
 	RTABMAP_PARAM(RGBD, ProximityMaxGraphDepth,       int, 50,      "Maximum depth from the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.");
 	RTABMAP_PARAM(RGBD, ProximityPathFilteringRadius, float, 0.5,   "Path filtering radius.");
 	RTABMAP_PARAM(RGBD, ProximityPathRawPosesUsed,    bool, true,   "When comparing to a local path, merge the scan using the odometry poses (with neighbor link optimizations) instead of the ones in the optimized local graph.");
-	RTABMAP_PARAM(RGBD, ProximityAngle,               float, 45.0,  "Maximum angle (degrees) for visual proximity detection.");
+	RTABMAP_PARAM(RGBD, ProximityAngle,               float, 45,    "Maximum angle (degrees) for visual proximity detection.");

 	// Graph optimization
 #ifdef RTABMAP_GTSAM
@@ -346,6 +346,7 @@ class RTABMAP_EXP Parameters

 	RTABMAP_PARAM(g2o, Solver,                  int, 0,          "0=csparse 1=pcg 2=cholmod");
 	RTABMAP_PARAM(g2o, Optimizer,               int, 0,          "0=Levenberg 1=GaussNewton");
+	RTABMAP_PARAM(g2o, PixelVariance,           double, 1,       "Pixel variance used for SBA.");

 	// Odometry
 	RTABMAP_PARAM(Odom, Strategy,           	int, 0, 		"0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F)");
@@ -364,12 +365,14 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Odom, GuessMotion,            bool, false,      "Guess next transformation from the last motion computed.");
 	RTABMAP_PARAM(Odom, KeyFrameThr,            float, 0.3,       "[Visual] Create a new keyframe when the number of inliers drops under this ratio of features in last frame. Setting the value to 0 means that a keyframe is created for each processed frame.");
 	RTABMAP_PARAM(Odom, ScanKeyFrameThr,        float, 0.7,       "[Geometry] Create a new keyframe when the number of ICP inliers drops under this ratio of points in last frame's scan. Setting the value to 0 means that a keyframe is created for each processed frame.");
+	RTABMAP_PARAM(Odom, ImageDecimation,        int, 1,           "Decimation of the images before registration.");
+	RTABMAP_PARAM(Odom, AlignWithGround,        bool, false,      "Align odometry with the ground on initialization.");

 	// Odometry Bag-of-words
 	RTABMAP_PARAM(OdomF2M, MaxSize,             int, 2000,    "[Visual] Local map size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
 	RTABMAP_PARAM(OdomF2M, MaxNewFeatures,      int, 0,       "[Visual] Maximum features (sorted by keypoint response) added to local map from a new key-frame. 0 means no limit.");
 	RTABMAP_PARAM(OdomF2M, ScanMaxSize,         int, 2000,    "[Geometry] Maximum local scan map size.");
-	RTABMAP_PARAM(OdomF2M, ScanSubstractRadius, float, 0.05,  "[Geometry] Radius used to filter points of a new added scan to local map. This could match the voxel size of the scans.");
+	RTABMAP_PARAM(OdomF2M, ScanSubtractRadius,  float, 0.05,  "[Geometry] Radius used to filter points of a new added scan to local map. This could match the voxel size of the scans.");
 	RTABMAP_PARAM_STR(OdomF2M, FixedMapPath,    "",           "Path to a fixed map (RTAB-Map's database) to be used for odometry. Odometry will be constraint to this map. RGB-only images can be used if odometry PnP estimation is used.")

 	// Odometry Mono
@@ -388,16 +391,26 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Vis, ForwardEstOnly,           bool, true, 	"Forward estimation only (A->B). If false, a transformation is also computed in backward direction (B->A), then the two resulting transforms are merged (middle interpolation between the transforms).");
 	RTABMAP_PARAM(Vis, InlierDistance,           float, 0.1,    "[Vis/EstimationType = 0] Maximum distance for feature correspondences. Used by 3D->3D estimation approach.");
 	RTABMAP_PARAM(Vis, RefineIterations,         int, 5,        "[Vis/EstimationType = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.");
-	RTABMAP_PARAM(Vis, PnPReprojError, 	         float, 2.0,    "[Vis/EstimationType = 1] PnP reprojection error.");
+	RTABMAP_PARAM(Vis, PnPReprojError, 	         float, 2,      "[Vis/EstimationType = 1] PnP reprojection error.");
 	RTABMAP_PARAM(Vis, PnPFlags,                 int, 1,        "[Vis/EstimationType = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P");
 	RTABMAP_PARAM(Vis, PnPRefineIterations,      int, 1,        "[Vis/EstimationType = 1] Refine iterations.");
 	RTABMAP_PARAM(Vis, EpipolarGeometryVar,      float, 0.02,   "[Vis/EstimationType = 2] Epipolar geometry maximum variance to accept the transformation.");
 	RTABMAP_PARAM(Vis, MinInliers,               int, 20, 		"Minimum feature correspondences to compute/accept the transformation.");
 	RTABMAP_PARAM(Vis, Iterations,               int, 100, 		"Maximum iterations to compute the transform.");
-	RTABMAP_PARAM(Vis, FeatureType,              int, 6,        "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
+#ifndef RTABMAP_NONFREE
+#ifdef RTABMAP_OPENCV3
+	// OpenCV 3 without xFeatures2D module doesn't have BRIEF
+	RTABMAP_PARAM(Vis, FeatureType, int, 8, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB.");
+#else
+	RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB.");
+#endif
+#else
+	RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB.");
+#endif
+	
 	RTABMAP_PARAM(Vis, MaxFeatures,              int, 1000,     "0 no limits.");
-	RTABMAP_PARAM(Vis, MaxDepth, 	             float, 0.0,    "Max depth of the features (0 means no limit).");
-	RTABMAP_PARAM(Vis, MinDepth, 	             float, 0.0,    "Min depth of the features (0 means no limit).");
+	RTABMAP_PARAM(Vis, MaxDepth, 	             float, 0,      "Max depth of the features (0 means no limit).");
+	RTABMAP_PARAM(Vis, MinDepth, 	             float, 0,      "Min depth of the features (0 means no limit).");
 	RTABMAP_PARAM_STR(Vis, RoiRatios,        "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
    RTABMAP_PARAM(Vis, SubPixWinSize,            int, 3,        "See cv::cornerSubPix().");
 	RTABMAP_PARAM(Vis, SubPixIterations,         int, 0,        "See cv::cornerSubPix(). 0 disables sub pixel refining.");
@@ -418,7 +431,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Icp, DownsamplingStep,          int, 1, 	    "Downsampling step size (1=no sampling). This is done before uniform sampling.");
 	RTABMAP_PARAM(Icp, MaxCorrespondenceDistance, float, 0.05,  "Max distance for point correspondences.");
 	RTABMAP_PARAM(Icp, Iterations,                int, 30, 		"Max iterations.");
-	RTABMAP_PARAM(Icp, Epsilon,                   float, 0.0,   "Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution.");
+	RTABMAP_PARAM(Icp, Epsilon,                   float, 0,     "Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution.");
 	RTABMAP_PARAM(Icp, CorrespondenceRatio,       float, 0.2,   "Ratio of matching correspondences to accept the transform.");
 	RTABMAP_PARAM(Icp, PointToPlane,              bool, false, 	"Use point to plane ICP.");
 	RTABMAP_PARAM(Icp, PointToPlaneNormalNeighbors, int, 20,    "Number of neighbors to compute normals for point to plane.");
@@ -429,7 +442,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Stereo, Iterations,            int, 30,       "Maximum iterations.");
 	RTABMAP_PARAM(Stereo, MaxLevel,              int, 3,        "Maximum pyramid level.");
 	RTABMAP_PARAM(Stereo, MinDisparity,          int, 1,        "Minimum disparity.");
-	RTABMAP_PARAM(Stereo, MaxDisparity,          int, 64,       "Maximum disparity.");
+	RTABMAP_PARAM(Stereo, MaxDisparity,          int, 128,      "Maximum disparity.");
 	RTABMAP_PARAM(Stereo, OpticalFlow,           bool, true,    "Use optical flow to find stereo correspondences, otherwise a simple block matching approach is used.");
 	RTABMAP_PARAM(Stereo, SSD,                   bool, true,    "[Stereo/OpticalFlow = false] Use Sum of Squared Differences (SSD) window, otherwise Sum of Absolute Differences (SAD) window is used.");
 	RTABMAP_PARAM(Stereo, Eps,                   double, 0.01,  "[Stereo/OpticalFlow = true] Epsilon stop criterion.");
@@ -482,6 +495,9 @@ public:
 	static std::string getVersion();
 	static std::string getDefaultDatabaseName();
 	
+	static std::string serialize(const ParametersMap & parameters);
+	static ParametersMap deserialize(const std::string & parameters);
+
 	static bool isFeatureParameter(const std::string & param);
 	static ParametersMap getDefaultOdometryParameters(bool stereo = false);
 	static ParametersMap getDefaultParameters(const std::string & group);
@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2015, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.

 Redistribution and use in source and binary forms, with or without
@@ -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
@@ -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
@@ -1,9 +1,29 @@
 /*
- * RegistrationInfo.h
- *
- *  Created on: Jan 5, 2016
- *      Author: mathieu
- */
+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 REGISTRATIONINFO_H_
 #define REGISTRATIONINFO_H_
@@ -18,7 +38,9 @@ public:
 		variance(0),
 		inliers(0),
 		matches(0),
-		icpInliersRatio(0)
+		icpInliersRatio(0),
+		icpTranslation(0.0f),
+		icpRotation(0.0f)
 	{
 	}

@@ -33,6 +55,8 @@ public:

 	// RegistrationIcp
 	float icpInliersRatio;
+	float icpTranslation;
+	float icpRotation;
 };

 }
@@ -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
@@ -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
@@ -70,6 +70,7 @@ public:
 	void close(bool databaseSaved = true);

 	const std::string & getWorkingDir() const {return _wDir;}
+	bool isRGBDMode() const { return _rgbdSlamMode; }
 	int getLoopClosureId() const {return _loopClosureHypothesis.first;}
 	float getLoopClosureValue() const {return _loopClosureHypothesis.second;}
 	int getHighestHypothesisId() const {return _highestHypothesis.first;}
@@ -115,6 +116,7 @@ public:
 	const ParametersMap & getParameters() const {return _parameters;}
 	void setWorkingDirectory(std::string path);
 	void rejectLoopClosure(int oldId, int newId);
+	void setOptimizedPoses(const std::map<int, Transform> & poses);
 	void get3DMap(std::map<int, Signature> & signatures,
 			std::map<int, Transform> & poses,
 			std::multimap<int, Link> & constraints,
@@ -125,6 +127,7 @@ public:
 			bool optimized,
 			bool global,
 			std::map<int, Signature> * signatures = 0);
+	int detectMoreLoopClosures(float clusterRadius = 0.5f, float clusterAngle = M_PI/6.0f, int iterations = 1);

 	int getPathStatus() const {return _pathStatus;} // -1=failed 0=idle/executing 1=success
 	void clearPath(int status); // -1=failed 0=idle/executing 1=success
@@ -194,7 +197,6 @@ private:
 	int _proximityMaxGraphDepth;
 	float _proximityFilteringRadius;
 	bool _proximityRawPosesUsed;
-	bool _proximityScansMerged;
 	float _proximityAngle;
 	std::string _databasePath;
 	bool _optimizeFromGraphEnd;
@@ -244,6 +246,7 @@ private:
 	unsigned int _pathGoalIndex;
 	Transform _pathTransformToGoal;
 	int _pathStuckCount;
+	float _pathStuckDistance;

 };

@@ -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
@@ -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
@@ -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
@@ -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
@@ -194,6 +194,8 @@ public:
 	void setGroundTruth(const Transform & pose) {groundTruth_ = pose;}
 	const Transform & groundTruth() const {return groundTruth_;}

+	long getMemoryUsed() const; // Return memory usage in Bytes
+
 private:
 	int _id;
 	double _stamp;
@@ -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
@@ -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
@@ -145,6 +145,7 @@ public:
 	void setRefImageId(int refImageId) {_refImageId = refImageId;}
 	void setLoopClosureId(int loopClosureId) {_loopClosureId = loopClosureId;}
 	void setProximityDetectionId(int id) {_proximiyDetectionId = id;}
+	void setStamp(double stamp) {_stamp = stamp;}

 	void setSignatures(const std::map<int, Signature> & signatures) {_signatures = signatures;}

@@ -165,6 +166,7 @@ public:
 	int refImageId() const {return _refImageId;}
 	int loopClosureId() const {return _loopClosureId;}
 	int proximityDetectionId() const {return _proximiyDetectionId;}
+	double stamp() const {return _stamp;}

 	const std::map<int, Signature> & getSignatures() const {return _signatures;}

@@ -188,6 +190,7 @@ private:
 	int _refImageId;
 	int _loopClosureId;
 	int _proximiyDetectionId;
+	double _stamp;

 	std::map<int, Signature> _signatures;

@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -1,9 +1,29 @@
 /*
- * util3d_mapping.hpp
- *
- *  Created on: 2015-05-13
- *      Author: mathieu
- */
+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 UTIL3D_MAPPING_HPP_
 #define UTIL3D_MAPPING_HPP_
@@ -17,19 +37,40 @@
 namespace rtabmap{
 namespace util3d{

+template<typename PointT>
+typename pcl::PointCloud<PointT>::Ptr projectCloudOnXYPlane(
+		const typename pcl::PointCloud<PointT> & cloud)
+{
+	typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
+	*output = cloud;
+	for(unsigned int i=0; i<output->size(); ++i)
+	{
+		output->at(i).z = 0;
+	}
+	return output;
+}
+
 template<typename PointT>
 void segmentObstaclesFromGround(
 		const typename pcl::PointCloud<PointT>::Ptr & cloud,
 		const typename pcl::IndicesPtr & indices,
 		pcl::IndicesPtr & ground,
 		pcl::IndicesPtr & obstacles,
-		float normalRadiusSearch,
+		int normalKSearch,
 		float groundNormalAngle,
+		float clusterRadius,
 		int minClusterSize,
-		bool segmentFlatObstacles)
+		bool segmentFlatObstacles,
+		float maxGroundHeight,
+		pcl::IndicesPtr * flatObstacles,
+		const Eigen::Vector4f & viewPoint)
 {
 	ground.reset(new std::vector<int>);
 	obstacles.reset(new std::vector<int>);
+	if(flatObstacles)
+	{
+		flatObstacles->reset(new std::vector<int>);
+	}

 	if(cloud->size())
 	{
@@ -39,8 +80,8 @@ void segmentObstaclesFromGround(
 				indices,
 				groundNormalAngle,
 				Eigen::Vector4f(0,0,1,0),
-				normalRadiusSearch*2.0f,
-				Eigen::Vector4f(0,0,100,0));
+				normalKSearch,
+				viewPoint);

 		if(segmentFlatObstacles)
 		{
@@ -48,26 +89,46 @@ void segmentObstaclesFromGround(
 			std::vector<pcl::IndicesPtr> clusteredFlatSurfaces = extractClusters(
 					cloud,
 					flatSurfaces,
-					normalRadiusSearch*2.0f,
+					clusterRadius,
 					minClusterSize,
 					std::numeric_limits<int>::max(),
 					&biggestFlatSurfaceIndex);


 			// cluster all surfaces for which the centroid is in the Z-range of the bigger surface
-			ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
-			Eigen::Vector4f min,max;
-			pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), min, max);
-
-			for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
+			if(clusteredFlatSurfaces.size())
 			{
-				if((int)i!=biggestFlatSurfaceIndex)
+				ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
+				Eigen::Vector4f min,max;
+				pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), min, max);
+
+				if(maxGroundHeight <= 0 || min[2] < maxGroundHeight)
 				{
-					Eigen::Vector4f centroid;
-					pcl::compute3DCentroid(*cloud, *clusteredFlatSurfaces.at(i), centroid);
-					if(centroid[2] >= min[2] && centroid[2] <= max[2])
+					for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
 					{
-						ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
+						if((int)i!=biggestFlatSurfaceIndex)
+						{
+							Eigen::Vector4f centroid;
+							pcl::compute3DCentroid(*cloud, *clusteredFlatSurfaces.at(i), centroid);
+							if(centroid[2] >= min[2]-0.01 &&
+							  (centroid[2] <= max[2]+0.01 || (maxGroundHeight>0 && centroid[2] <= maxGroundHeight+0.01))) // epsilon
+							{
+								ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
+							}
+							else if(flatObstacles)
+							{
+								*flatObstacles = util3d::concatenate(*flatObstacles, clusteredFlatSurfaces.at(i));
+							}
+						}
+					}
+				}
+				else
+				{
+					// reject ground!
+					ground.reset(new std::vector<int>);
+					if(flatObstacles)
+					{
+						*flatObstacles = flatSurfaces;
 					}
 				}
 			}
@@ -82,15 +143,24 @@ void segmentObstaclesFromGround(
 			// Remove ground
 			pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, ground, true);

-			//Cluster remaining stuff (obstacles)
-			std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters(
-					cloud,
-					otherStuffIndices,
-					normalRadiusSearch*2.0f,
-					minClusterSize);
+			// If ground height is set, remove obstacles under it
+			if(maxGroundHeight > 0.0f)
+			{
+				otherStuffIndices = rtabmap::util3d::passThrough(cloud, otherStuffIndices, "z", maxGroundHeight, std::numeric_limits<float>::max());
+			}

-			// merge indices
-			obstacles = util3d::concatenate(clusteredObstaclesSurfaces);
+			//Cluster remaining stuff (obstacles)
+			if(otherStuffIndices->size())
+			{
+				std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters(
+						cloud,
+						otherStuffIndices,
+						clusterRadius,
+						minClusterSize);
+
+				// merge indices
+				obstacles = util3d::concatenate(clusteredObstaclesSurfaces);
+			}
 		}
 	}
 }
@@ -100,10 +170,14 @@ void segmentObstaclesFromGround(
 		const typename pcl::PointCloud<PointT>::Ptr & cloud,
 		pcl::IndicesPtr & ground,
 		pcl::IndicesPtr & obstacles,
-		float normalRadiusSearch,
+		int normalKSearch,
 		float groundNormalAngle,
+		float clusterRadius,
 		int minClusterSize,
-		bool segmentFlatObstacles)
+		bool segmentFlatObstacles,
+		float maxGroundHeight,
+		pcl::IndicesPtr * flatObstacles,
+		const Eigen::Vector4f & viewPoint)
 {
 	pcl::IndicesPtr indices(new std::vector<int>);
 	segmentObstaclesFromGround<PointT>(
@@ -111,10 +185,87 @@ void segmentObstaclesFromGround(
 			indices,
 			ground,
 			obstacles,
-			normalRadiusSearch,
+			normalKSearch,
 			groundNormalAngle,
+			clusterRadius,
 			minClusterSize,
-			segmentFlatObstacles);
+			segmentFlatObstacles,
+			maxGroundHeight,
+			flatObstacles,
+			viewPoint);
+}
+
+template<typename PointT>
+void occupancy2DFromGroundObstacles(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		const pcl::IndicesPtr & groundIndices,
+		const pcl::IndicesPtr & obstaclesIndices,
+		cv::Mat & ground,
+		cv::Mat & obstacles,
+		float cellSize)
+{
+	typename pcl::PointCloud<PointT>::Ptr groundCloud(new pcl::PointCloud<PointT>);
+	typename pcl::PointCloud<PointT>::Ptr obstaclesCloud(new pcl::PointCloud<PointT>);
+
+	if(groundIndices->size())
+	{
+		pcl::copyPointCloud(*cloud, *groundIndices, *groundCloud);
+	}
+
+	if(obstaclesIndices->size())
+	{
+		pcl::copyPointCloud(*cloud, *obstaclesIndices, *obstaclesCloud);
+	}
+
+	occupancy2DFromGroundObstacles<PointT>(
+			groundCloud,
+			obstaclesCloud,
+			ground,
+			obstacles,
+			cellSize);
+}
+
+template<typename PointT>
+void occupancy2DFromGroundObstacles(
+		const typename pcl::PointCloud<PointT>::Ptr & groundCloud,
+		const typename pcl::PointCloud<PointT>::Ptr & obstaclesCloud,
+		cv::Mat & ground,
+		cv::Mat & obstacles,
+		float cellSize)
+{
+	ground = cv::Mat();
+	if(groundCloud->size())
+	{
+		//project on XY plane
+		typename pcl::PointCloud<PointT>::Ptr groundCloudProjected;
+		groundCloudProjected = util3d::projectCloudOnXYPlane(*groundCloud);
+		//voxelize to grid cell size
+		groundCloudProjected = util3d::voxelize(groundCloudProjected, cellSize);
+
+		ground = cv::Mat((int)groundCloudProjected->size(), 1, CV_32FC2);
+		for(unsigned int i=0;i<groundCloudProjected->size(); ++i)
+		{
+			ground.at<cv::Vec2f>(i)[0] = groundCloudProjected->at(i).x;
+			ground.at<cv::Vec2f>(i)[1] = groundCloudProjected->at(i).y;
+		}
+	}
+
+	obstacles = cv::Mat();
+	if(obstaclesCloud->size())
+	{
+		//project on XY plane
+		typename pcl::PointCloud<PointT>::Ptr obstaclesCloudProjected;
+		obstaclesCloudProjected = util3d::projectCloudOnXYPlane(*obstaclesCloud);
+		//voxelize to grid cell size
+		obstaclesCloudProjected = util3d::voxelize(obstaclesCloudProjected, cellSize);
+
+		obstacles = cv::Mat((int)obstaclesCloudProjected->size(), 1, CV_32FC2);
+		for(unsigned int i=0;i<obstaclesCloudProjected->size(); ++i)
+		{
+			obstacles.at<cv::Vec2f>(i)[0] = obstaclesCloudProjected->at(i).x;
+			obstacles.at<cv::Vec2f>(i)[1] = obstaclesCloudProjected->at(i).y;
+		}
+	}
 }

 template<typename PointT>
@@ -125,7 +276,9 @@ void occupancy2DFromCloud3D(
 		cv::Mat & obstacles,
 		float cellSize,
 		float groundNormalAngle,
-		int minClusterSize)
+		int minClusterSize,
+		bool segmentFlatObstacles,
+		float maxGroundHeight)
 {
 	if(cloud->size() == 0)
 	{
@@ -138,52 +291,20 @@ void occupancy2DFromCloud3D(
 			indices,
 			groundIndices,
 			obstaclesIndices,
-			cellSize,
+			20,
 			groundNormalAngle,
-			minClusterSize);
+			cellSize*2.0f,
+			minClusterSize,
+			segmentFlatObstacles,
+			maxGroundHeight);

-	pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
-	pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
-
-	if(groundIndices->size())
-	{
-		pcl::copyPointCloud(*cloud, *groundIndices, *groundCloud);
-		//project on XY plane
-		util3d::projectCloudOnXYPlane(groundCloud);
-		//voxelize to grid cell size
-		groundCloud = util3d::voxelize(groundCloud, cellSize);
-	}
-
-	if(obstaclesIndices->size())
-	{
-		pcl::copyPointCloud(*cloud, *obstaclesIndices, *obstaclesCloud);
-		//project on XY plane
-		util3d::projectCloudOnXYPlane(obstaclesCloud);
-		//voxelize to grid cell size
-		obstaclesCloud = util3d::voxelize(obstaclesCloud, cellSize);
-	}
-
-	ground = cv::Mat();
-	if(groundCloud->size())
-	{
-		ground = cv::Mat((int)groundCloud->size(), 1, CV_32FC2);
-		for(unsigned int i=0;i<groundCloud->size(); ++i)
-		{
-			ground.at<cv::Vec2f>(i)[0] = groundCloud->at(i).x;
-			ground.at<cv::Vec2f>(i)[1] = groundCloud->at(i).y;
-		}
-	}
-
-	obstacles = cv::Mat();
-	if(obstaclesCloud->size())
-	{
-		obstacles = cv::Mat((int)obstaclesCloud->size(), 1, CV_32FC2);
-		for(unsigned int i=0;i<obstaclesCloud->size(); ++i)
-		{
-			obstacles.at<cv::Vec2f>(i)[0] = obstaclesCloud->at(i).x;
-			obstacles.at<cv::Vec2f>(i)[1] = obstaclesCloud->at(i).y;
-		}
-	}
+	occupancy2DFromGroundObstacles<PointT>(
+			cloud,
+			groundIndices,
+			obstaclesIndices,
+			ground,
+			obstacles,
+			cellSize);
 }

 template<typename PointT>
@@ -193,10 +314,12 @@ void occupancy2DFromCloud3D(
 		cv::Mat & obstacles,
 		float cellSize,
 		float groundNormalAngle,
-		int minClusterSize)
+		int minClusterSize,
+		bool segmentFlatObstacles,
+		float maxGroundHeight)
 {
 	pcl::IndicesPtr indices(new std::vector<int>);
-	occupancy2DFromCloud3D<PointT>(cloud, indices, ground, obstacles, cellSize, groundNormalAngle, minClusterSize);
+	occupancy2DFromCloud3D<PointT>(cloud, indices, ground, obstacles, cellSize, groundNormalAngle, minClusterSize, segmentFlatObstacles, maxGroundHeight);
 }

 }
@@ -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 <opencv2/core/core.hpp>
 #include <rtabmap/core/Transform.h>
+#include <rtabmap/core/Parameters.h>

 namespace rtabmap
 {
@@ -68,7 +69,7 @@ void RTABMAP_EXP calcOpticalFlowPyrLKStereo( cv::InputArray _prevImg, cv::InputA
 cv::Mat RTABMAP_EXP disparityFromStereoImages(
 		const cv::Mat & leftImage,
 		const cv::Mat & rightImage,
-		int type = CV_32FC1); // CV_32FC1 or CV_16SC1
+	    const ParametersMap & parameters = ParametersMap());

 cv::Mat RTABMAP_EXP depthFromDisparity(const cv::Mat & disparity,
 		float fx, float baseline,
@@ -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,8 +33,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <pcl/pcl_base.h>
+#include <pcl/TextureMesh.h>
 #include <rtabmap/core/Transform.h>
 #include <rtabmap/core/SensorData.h>
+#include <rtabmap/core/Parameters.h>
 #include <opencv2/core/core.hpp>
 #include <map>
 #include <list>
@@ -72,21 +74,38 @@ pcl::PointXYZ RTABMAP_EXP projectDepthTo3D(
 		bool smoothing,
 		float maxZError = 0.02f);

-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDepth(
+RTABMAP_DEPRECATED (pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDepth(
 		const cv::Mat & imageDepth,
 		float cx, float cy,
 		float fx, float fy,
 		int decimation = 1,
 		float maxDepth = 0.0f,
+		float minDepth = 0.0f,
+		std::vector<int> * validIndices = 0), "Use cloudFromDepth with CameraModel interface.");
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDepth(
+		const cv::Mat & imageDepth,
+		const CameraModel & model,
+		int decimation = 1,
+		float maxDepth = 0.0f,
+		float minDepth = 0.0f,
 		std::vector<int> * validIndices = 0);

-pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDepthRGB(
+RTABMAP_DEPRECATED (pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDepthRGB(
 		const cv::Mat & imageRgb,
 		const cv::Mat & imageDepth,
 		float cx, float cy,
 		float fx, float fy,
 		int decimation = 1,
 		float maxDepth = 0.0f,
+		float minDepth = 0.0f,
+		std::vector<int> * validIndices = 0), "Use cloudFromDepthRGB with CameraModel interface.");
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDepthRGB(
+		const cv::Mat & imageRgb,
+		const cv::Mat & imageDepth,
+		const CameraModel & model,
+		int decimation = 1,
+		float maxDepth = 0.0f,
+		float minDepth = 0.0f,
 		std::vector<int> * validIndices = 0);

 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDisparity(
@@ -94,6 +113,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDisparity(
 		const StereoCameraModel & model,
 		int decimation = 1,
 		float maxDepth = 0.0f,
+		float minDepth = 0.0f,
 		std::vector<int> * validIndices = 0);

 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDisparityRGB(
@@ -102,6 +122,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDisparityRGB(
 		const StereoCameraModel & model,
 		int decimation = 1,
 		float maxDepth = 0.0f,
+		float minDepth = 0.0f,
 		std::vector<int> * validIndices = 0);

 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromStereoImages(
@@ -110,29 +131,28 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromStereoImages(
 		const StereoCameraModel & model,
 		int decimation = 1,
 		float maxDepth = 0.0f,
-		std::vector<int> * validIndices = 0);
+		float minDepth = 0.0f,
+		std::vector<int> * validIndices = 0,
+		const ParametersMap & parameters = ParametersMap());

 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 		const SensorData & sensorData,
 		int decimation = 1,
 		float maxDepth = 0.0f,
-		float voxelSize = 0.0f,
-		int samples = 0,
-		std::vector<int> * validIndices = 0);
+		float minDepth = 0.0f,
+		std::vector<int> * validIndices = 0,
+		const ParametersMap & parameters = ParametersMap());

 /**
- * Create an RGB cloud from the images contained in SensorData. If "voxelSize" and
- * "samples" are not set (0), the returned cloud is organized. Otherwise, all NaN
+ * Create an RGB cloud from the images contained in SensorData. If there is only one camera,
+ * the returned cloud is organized. Otherwise, all NaN
 * points are removed and the cloud will be dense.
 *
- * Note that multiple RGB-D camera images will result in a dense cloud.
- *
 * @param sensorData, the sensor data.
 * @param decimation, images are decimated by this factor before projecting points to 3D. The factor
 * should be a factor of the image width and height.
 * @param maxDepth, maximum depth of the projected points (farther points are set to null in case of an organized cloud).
- * @param voxelSize, use a voxel grid filter with this size of voxel.
- * @param samples, random sampling filtering target cloud size.
+ * @param minDepth, minimum depth of the projected points (closer points are set to null in case of an organized cloud).
 * @param validIndices, the indices of valid points in the cloud
 * @return a RGB cloud.
 */
@@ -140,9 +160,9 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 		const SensorData & sensorData,
 		int decimation = 1,
 		float maxDepth = 0.0f,
-		float voxelSize = 0.0f,
-		int samples = 0,
-		std::vector<int> * validIndices = 0);
+		float minDepth = 0.0f,
+		std::vector<int> * validIndices = 0,
+		const ParametersMap & parameters = ParametersMap());

 pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImage(
 					const cv::Mat & depthImage,
@@ -151,6 +171,7 @@ pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImage(
 					float cx,
 					float cy,
 					float maxDepth = 0,
+					float minDepth = 0,
 					const Transform & localTransform = Transform::getIdentity());

 // return CV_32FC3
@@ -201,6 +222,9 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP concatenateClouds(
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP concatenateClouds(
 		const std::list<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds);

+pcl::TextureMesh::Ptr RTABMAP_EXP concatenateTextureMeshes(
+		const std::list<pcl::TextureMesh::Ptr> & meshes);
+
 /**
 * @brief Concatenate a vector of indices to a single vector.
 *
@@ -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
@@ -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
@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.

 Redistribution and use in source and binary forms, with or without
@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <pcl/pcl_base.h>
+#include <pcl/ModelCoefficients.h>

 namespace rtabmap
 {
@@ -108,6 +109,20 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP randomSampling(
 		int samples);


+pcl::IndicesPtr RTABMAP_EXP passThrough(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::string & axis,
+		float min,
+		float max,
+		bool negative = false);
+pcl::IndicesPtr RTABMAP_EXP passThrough(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::string & axis,
+		float min,
+		float max,
+		bool negative = false);
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP passThrough(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const std::string & axis,
@@ -344,13 +359,13 @@ pcl::IndicesPtr RTABMAP_EXP normalFiltering(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		float angleMax,
 		const Eigen::Vector4f & normal,
-		float radiusSearch,
+		int normalKSearch,
 		const Eigen::Vector4f & viewpoint);
 pcl::IndicesPtr RTABMAP_EXP normalFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		float angleMax,
 		const Eigen::Vector4f & normal,
-		float radiusSearch,
+		int normalKSearch,
 		const Eigen::Vector4f & viewpoint);

 /**
@@ -365,7 +380,7 @@ pcl::IndicesPtr RTABMAP_EXP normalFiltering(
 * @param indices the input indices of the cloud to process, if empty, all points in the cloud are processed.
 * @param angleMax the maximum angle.
 * @param normal the normal to which each point's normal is compared.
- * @param radiusSearch radius parameter used for normal estimation (see pcl::NormalEstimation).
+ * @param normalKSearch number of neighbor points used for normal estimation (see pcl::NormalEstimation).
 * @param viewpoint from which viewpoint the normals should be estimated (see pcl::NormalEstimation).
 * @return the indices of the points which respect the normal constraint.
 */
@@ -375,21 +390,21 @@ pcl::IndicesPtr RTABMAP_EXP normalFiltering(
 		const pcl::IndicesPtr & indices,
 		float angleMax,
 		const Eigen::Vector4f & normal,
-		float radiusSearch,
+		int normalKSearch,
 		const Eigen::Vector4f & viewpoint);
 pcl::IndicesPtr RTABMAP_EXP normalFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		float angleMax,
 		const Eigen::Vector4f & normal,
-		float radiusSearch,
+		int normalKSearch,
 		const Eigen::Vector4f & viewpoint);
 pcl::IndicesPtr RTABMAP_EXP normalFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		float angleMax,
 		const Eigen::Vector4f & normal,
-		float radiusSearch,
+		int normalKSearch,
 		const Eigen::Vector4f & viewpoint);

 /**
@@ -471,6 +486,18 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP extractIndices(
 		bool negative,
 		bool keepOrganized);

+pcl::IndicesPtr extractPlane(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		float distanceThreshold,
+		int maxIterations = 100,
+		pcl::ModelCoefficients * coefficientsOut = 0);
+pcl::IndicesPtr extractPlane(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float distanceThreshold,
+		int maxIterations = 100,
+		pcl::ModelCoefficients * coefficientsOut = 0);
+
 } // namespace util3d
 } // namespace rtabmap

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.

 Redistribution and use in source and binary forms, with or without
@@ -76,8 +76,9 @@ void RTABMAP_EXP rayTrace(const cv::Point2i & start,

 cv::Mat RTABMAP_EXP convertMap2Image8U(const cv::Mat & map8S);

-void RTABMAP_EXP projectCloudOnXYPlane(
-		pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud);
+template<typename PointT>
+typename pcl::PointCloud<PointT>::Ptr projectCloudOnXYPlane(
+		const typename pcl::PointCloud<PointT> & cloud);

 // templated methods
 template<typename PointT>
@@ -86,19 +87,44 @@ void segmentObstaclesFromGround(
 		const pcl::IndicesPtr & indices,
 		pcl::IndicesPtr & ground,
 		pcl::IndicesPtr & obstacles,
-		float normalRadiusSearch,
+		int normalKSearch,
 		float groundNormalAngle,
+		float clusterRadius,
 		int minClusterSize,
-		bool segmentFlatObstacles = false);
+		bool segmentFlatObstacles = false,
+		float maxGroundHeight = 0.0f,
+		pcl::IndicesPtr * flatObstacles = 0,
+		const Eigen::Vector4f & viewPoint = Eigen::Vector4f(0,0,100,0));
 template<typename PointT>
 void segmentObstaclesFromGround(
 		const typename pcl::PointCloud<PointT>::Ptr & cloud,
 		pcl::IndicesPtr & ground,
 		pcl::IndicesPtr & obstacles,
-		float normalRadiusSearch,
+		int normalKSearch,
 		float groundNormalAngle,
+		float clusterRadius,
 		int minClusterSize,
-		bool segmentFlatObstacles = false);
+		bool segmentFlatObstacles = false,
+		float maxGroundHeight = 0.0f,
+		pcl::IndicesPtr * flatObstacles = 0,
+		const Eigen::Vector4f & viewPoint = Eigen::Vector4f(0,0,100,0));
+
+template<typename PointT>
+void occupancy2DFromGroundObstacles(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		const pcl::IndicesPtr & groundIndices,
+		const pcl::IndicesPtr & obstaclesIndices,
+		cv::Mat & ground,
+		cv::Mat & obstacles,
+		float cellSize);
+
+template<typename PointT>
+void occupancy2DFromGroundObstacles(
+		const typename pcl::PointCloud<PointT>::Ptr & groundCloud,
+		const typename pcl::PointCloud<PointT>::Ptr & obstaclesCloud,
+		cv::Mat & ground,
+		cv::Mat & obstacles,
+		float cellSize);

 template<typename PointT>
 void occupancy2DFromCloud3D(
@@ -107,7 +133,9 @@ void occupancy2DFromCloud3D(
 		cv::Mat & obstacles,
 		float cellSize = 0.05f,
 		float groundNormalAngle = M_PI_4,
-		int minClusterSize = 20);
+		int minClusterSize = 20,
+		bool segmentFlatObstacles = false,
+		float maxGroundHeight = 0.0f);
 template<typename PointT>
 void occupancy2DFromCloud3D(
 		const typename pcl::PointCloud<PointT>::Ptr & cloud,
@@ -116,7 +144,9 @@ void occupancy2DFromCloud3D(
 		cv::Mat & obstacles,
 		float cellSize = 0.05f,
 		float groundNormalAngle = M_PI_4,
-		int minClusterSize = 20);
+		int minClusterSize = 20,
+		bool segmentFlatObstacles = false,
+		float maxGroundHeight = 0.0f);

 } // namespace util3d
 } // namespace rtabmap
@@ -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
@@ -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
@@ -92,18 +92,6 @@ Transform RTABMAP_EXP icpPointToPlane(
 		float epsilon = 0.0f,
 		bool icp2D = false);

-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP getICPReadyCloud(
-		const cv::Mat & depth,
-		float fx,
-		float fy,
-		float cx,
-		float cy,
-		int decimation,
-		double maxDepth,
-		float voxel,
-		int samples,
-		const Transform & transform = Transform::getIdentity());
-
 } // namespace util3d
 } // namespace rtabmap

@@ -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
@@ -78,12 +78,23 @@ void RTABMAP_EXP appendMesh(
 		std::vector<pcl::Vertices> & polygonsA,
 		const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloudB,
 		const std::vector<pcl::Vertices> & polygonsB);
+void RTABMAP_EXP appendMesh(
+		pcl::PointCloud<pcl::PointXYZRGB> & cloudA,
+		std::vector<pcl::Vertices> & polygonsA,
+		const pcl::PointCloud<pcl::PointXYZRGB> & cloudB,
+		const std::vector<pcl::Vertices> & polygonsB);

-void RTABMAP_EXP filterNotUsedVerticesFromMesh(
+// return map from new to old polygon indices
+std::map<int, int> RTABMAP_EXP filterNotUsedVerticesFromMesh(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
 		const std::vector<pcl::Vertices> & polygons,
 		pcl::PointCloud<pcl::PointXYZRGBNormal> & outputCloud,
 		std::vector<pcl::Vertices> & outputPolygons);
+std::map<int, int> RTABMAP_EXP filterNotUsedVerticesFromMesh(
+		const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
+		const std::vector<pcl::Vertices> & polygons,
+		pcl::PointCloud<pcl::PointXYZRGB> & outputCloud,
+		std::vector<pcl::Vertices> & outputPolygons);

 std::vector<pcl::Vertices> RTABMAP_EXP filterCloseVerticesFromMesh(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud,
@@ -110,32 +121,39 @@ pcl::TextureMesh::Ptr RTABMAP_EXP createTextureMesh(
 		const std::map<int, Transform> & poses,
 		const std::map<int, CameraModel> & cameraModels,
 		const std::map<int, cv::Mat> & images,
-		const std::string & tmpDirectory = ".");
+		const std::string & tmpDirectory = ".",
+		int kNormalSearch = 20); // if mesh doesn't have normals, compute them with k neighbors

-pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP computeNormals(
+pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
-		int normalKSearch = 20);
-pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP computeNormals(
+		int normalKSearch = 20,
+		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
+pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		int normalKSearch = 20);
-pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP computeNormals(
+		int normalKSearch = 20,
+		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
+pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
-		int normalKSearch = 20);
-pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP computeNormals(
+		int normalKSearch = 20,
+		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
+pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
-		int normalKSearch = 20);
+		int normalKSearch = 20,
+		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));

-pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeFastOrganizedNormals(
+pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		float maxDepthChangeFactor = 0.02f,
-		float normalSmoothingSize = 10.0f);
-pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeFastOrganizedNormals(
+		float normalSmoothingSize = 10.0f,
+		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
+pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		float maxDepthChangeFactor = 0.02f,
-		float normalSmoothingSize = 10.0f);
+		float normalSmoothingSize = 10.0f,
+		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));

 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP mls(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
@@ -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
@@ -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
@@ -188,10 +188,17 @@ IF(G2O_FOUND)
 ENDIF(G2O_FOUND)

 IF(GTSAM_FOUND)
-	SET(INCLUDE_DIRS 
-		${INCLUDE_DIRS} 
-		${GTSAM_INCLUDE_DIRS}
-	)
+	IF(GTSAM_INCLUDE_DIR)
+		SET(INCLUDE_DIRS 
+			${GTSAM_INCLUDE_DIR} # place it in front to use Eigen installed by GTSAM
+			${INCLUDE_DIRS} 
+		)
+	ELSE()
+		SET(INCLUDE_DIRS 
+			${GTSAM_INCLUDE_DIRS} # cmake standard
+			${INCLUDE_DIRS} 
+		)
+	ENDIF()
 	SET(LIBRARIES
 		${LIBRARIES}
 		gtsam
@@ -209,6 +216,42 @@ IF(cvsba_FOUND)
 	)
 ENDIF(cvsba_FOUND)

+IF(ZED_FOUND)
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${ZED_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${ZED_LIBRARIES}
+	)
+	IF(CUDA_FOUND)
+		SET(INCLUDE_DIRS
+			${INCLUDE_DIRS}
+			${CUDA_INCLUDE_DIRS}
+		)
+		SET(LIBRARIES
+			${LIBRARIES}
+			${CUDA_LIBRARIES}
+		)
+	ENDIF(CUDA_FOUND)
+ENDIF(ZED_FOUND)
+
+IF(OCTOMAP_FOUND)
+    SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${OCTOMAP_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${OCTOMAP_LIBRARIES}
+	)
+	SET(SRC_FILES
+    	${SRC_FILES}
+		OctoMap.cpp
+	)
+ENDIF(OCTOMAP_FOUND)
+
 ####################################
 # Generate resources files
 ####################################
@@ -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
@@ -88,7 +88,7 @@ SensorData Camera::takeImage(CameraInfo * info)
 	}

 	UTimer timer;
-	SensorData data  = this->captureImage();
+	SensorData data  = this->captureImage(info);
 	double captureTime = timer.ticks();
 	if(warnFrameRateTooHigh)
 	{
@@ -102,6 +102,7 @@ SensorData Camera::takeImage(CameraInfo * info)
 	if(info)
 	{
 		info->id = data.id();
+		info->stamp = data.stamp();
 		info->timeCapture = captureTime;
 	}
 	return data;
@@ -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
@@ -371,6 +371,8 @@ CameraModel CameraModel::scaled(double scale) const
 			P.at<double>(1,1) *= scale;
 			P.at<double>(0,2) *= scale;
 			P.at<double>(1,2) *= scale;
+			P.at<double>(0,3) *= scale;
+			P.at<double>(1,3) *= scale;
 		}
 		scaledModel = CameraModel(name_, cv::Size(double(imageSize_.width)*scale, double(imageSize_.height)*scale), K, D_, R_, P, localTransform_);
 	}
@@ -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,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/util3d_filtering.h>
 #include <rtabmap/core/util3d_surface.h>

+#include <pcl/common/io.h>
+
 #include <iostream>
 #include <fstream>
 #include <cmath>
@@ -436,7 +438,7 @@ std::vector<std::string> CameraImages::filenames() const
 	return std::vector<std::string>();
 }

-SensorData CameraImages::captureImage()
+SensorData CameraImages::captureImage(CameraInfo * info)
 {
 	if(syncImageRateWithStamps_ && _captureDelay>0.0)
 	{
@@ -661,7 +663,9 @@ SensorData CameraImages::captureImage()
 			}
 			if(_scanNormalsK > 0 && cloud->size())
 			{
-				pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals = util3d::computeNormals(cloud, _scanNormalsK);
+				pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK);
+				pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
+				pcl::concatenateFields(*cloud, *normals, *cloudNormals);
 				scan = util3d::laserScanFromPointCloud(*cloudNormals);
 			}
 			else
@@ -692,10 +696,11 @@ SensorData CameraImages::captureImage()
 /////////////////////////
 CameraVideo::CameraVideo(
 		int usbDevice,
+		bool rectifyImages,
 		float imageRate,
 		const Transform & localTransform) :
 	Camera(imageRate, localTransform),
-	_rectifyImages(false),
+	_rectifyImages(rectifyImages),
 	_src(kUsbDevice),
 	_usbDevice(usbDevice)
 {
@@ -722,7 +727,7 @@ CameraVideo::~CameraVideo()

 bool CameraVideo::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
-	_guid.clear();
+	_guid = cameraName;
 	if(_capture.isOpened())
 	{
 		_capture.release();
@@ -750,19 +755,22 @@ bool CameraVideo::init(const std::string & calibrationFolder, const std::string
 	}
 	else
 	{
-		unsigned int guid = (unsigned int)_capture.get(CV_CAP_PROP_GUID);
-		if(guid != 0 && guid != 0xffffffff)
+		if (_guid.empty())
 		{
-			_guid = uFormat("%08x", guid);
+			unsigned int guid = (unsigned int)_capture.get(CV_CAP_PROP_GUID);
+			if (guid != 0 && guid != 0xffffffff)
+			{
+				_guid = uFormat("%08x", guid);
+			}
 		}

 		// look for calibration files
-		if(!calibrationFolder.empty() && (!_guid.empty() || !cameraName.empty()))
+		if(!calibrationFolder.empty() && !_guid.empty())
 		{
-			if(!_model.load(calibrationFolder, (cameraName.empty()?_guid:cameraName)))
+			if(!_model.load(calibrationFolder, _guid))
 			{
 				UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
-						cameraName.empty()?_guid.c_str():cameraName.c_str(), calibrationFolder.c_str());
+						_guid.c_str(), calibrationFolder.c_str());
 			}
 			else
 			{
@@ -793,7 +801,7 @@ std::string CameraVideo::getSerial() const
 	return _guid;
 }

-SensorData CameraVideo::captureImage()
+SensorData CameraVideo::captureImage(CameraInfo * info)
 {
 	cv::Mat img;
 	if(_capture.isOpened())
@@ -805,7 +813,7 @@ SensorData CameraVideo::captureImage()
 				_model.setImageSize(img.size());
 			}

-			if(_model.isValidForRectification() && (_src != kVideoFile || _rectifyImages))
+			if(_model.isValidForRectification() && _rectifyImages)
 			{
 				img = _model.rectifyImage(img);
 			}
--- a/Show More
+++ b/Show More