Fixed ICP-only registration with already provided guess (no need to do visual guess, as the guess can be already good)

Update VWDictionary.cpp
Fixed "HAVE_OPENCV_CUDAFEATURES2D" build error of https://github.com/introlab/rtabmap/issues/85
2026-10-05 17:47:49 +08:00 · 2016-06-23 11:08:20 -04:00 · 2016-06-22 19:28:22 -04:00 · 2016-06-15 15:48:56 -04:00 · 2016-06-14 11:55:58 -04:00 · 2016-06-09 15:49:17 -04:00
129 changed files with 7802 additions and 3095 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,9 @@ SET(PROJECT_PREFIX rtabmap)

 # Catkin doesn't support multiarch library path, 
 # fix to "lib" if not set by user.
-IF(NOT DEFINED CMAKE_INSTALL_LIBDIR)
-   set(CMAKE_INSTALL_LIBDIR "lib")
-ENDIF(NOT DEFINED CMAKE_INSTALL_LIBDIR)
+#IF(NOT DEFINED CMAKE_INSTALL_LIBDIR)
+#   set(CMAKE_INSTALL_LIBDIR "lib")
+#ENDIF(NOT DEFINED CMAKE_INSTALL_LIBDIR)

 INCLUDE(GNUInstallDirs)

@@ -20,7 +20,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 7)
 SET(RTABMAP_VERSION
  ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
  
@@ -32,8 +32,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 +136,17 @@ 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)

 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"))
@@ -167,14 +171,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 +210,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 +236,50 @@ 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 0.9 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(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND)
   #Newest versions require std11
   IF(NOT MSVC)
     include(CheckCXXCompilerFlag)
@@ -237,21 +293,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 +324,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 +350,39 @@ 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 (OpenCV_FOUND AND OpenCV_VERSION_MAJOR EQUAL 3))
   SET(OPENCV3 "//")
 ENDIF(NOT (OpenCV_FOUND AND OpenCV_VERSION_MAJOR EQUAL 3))
@@ -365,9 +433,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 +551,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 +657,18 @@ 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(QT4_FOUND)
 MESSAGE(STATUS "  With Qt4                  = YES (License: Open Source or Commercial)")
 ELSEIF(Qt5_FOUND)
@@ -590,7 +676,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 @@
-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,7 @@ 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

 #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));
 }

@@ -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);
@@ -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}\")
@@ -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>
@@ -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

@@ -39,6 +39,14 @@ namespace FlyCapture2
 class Camera;
 }

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

@@ -94,6 +102,52 @@ private:
 	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,
+			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,
+			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;
+
+protected:
+	virtual SensorData captureImage();
+
+private:
+	sl::zed::Camera * zed_;
+	StereoCameraModel stereoModel_;
+	CameraVideo::Source src_;
+	int usbDevice_;
+	std::string svoFilePath_;
+	int resolution_;
+	int quality_;
+	int sensingMode_;
+	int confidenceThr_;
+};
+
 /////////////////////////
 // CameraStereoImages
 /////////////////////////
@@ -147,6 +201,10 @@ public:
 			bool rectifyImages = false,
 			float imageRate=0.0f,
 			const Transform & localTransform = Transform::getIdentity());
+	CameraStereoVideo(
+			int device,
+			float imageRate = 0.0f,
+			const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraStereoVideo();

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -162,6 +220,8 @@ private:
 	bool rectifyImages_;
 	StereoCameraModel stereoModel_;
 	std::string cameraName_;
+	CameraVideo::Source src_;
+	int usbDevice_;
 };

 } // namespace rtabmap
@@ -91,6 +91,7 @@ private:
 	bool _scanFromDepth;
 	int _scanDecimation;
 	float _scanMaxDepth;
+	float _scanMinDepth;
 	float _scanVoxelSize;
 	int _scanNormalsK;
 	StereoDense * _stereoDense;
@@ -94,7 +94,7 @@ 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;
@@ -119,6 +119,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 +136,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;
@@ -169,6 +170,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;
@@ -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,
@@ -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;
@@ -83,6 +83,7 @@ private:
 	bool _fillInfoData;
 	float _kalmanProcessNoise;
 	float _kalmanMeasurementNoise;
+	int _imageDecimation;
 	Transform _pose;
 	int _resetCurrentCount;
 	double previousStamp_;
@@ -55,7 +55,7 @@ private:

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

 }
@@ -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(),
@@ -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 */
@@ -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 */
@@ -205,10 +205,11 @@ 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");
@@ -220,9 +221,9 @@ class RTABMAP_EXP Parameters
 	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.");
@@ -322,7 +323,6 @@ 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.");
@@ -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.0,     "Pixel variance used for SBA.");

 	// Odometry
 	RTABMAP_PARAM(Odom, Strategy,           	int, 0, 		"0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F)");
@@ -364,6 +365,7 @@ 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.");

 	// 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.");
@@ -394,7 +396,17 @@ class RTABMAP_EXP Parameters
 	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).");
@@ -115,6 +115,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 +126,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 +196,6 @@ private:
 	int _proximityMaxGraphDepth;
 	float _proximityFilteringRadius;
 	bool _proximityRawPosesUsed;
-	bool _proximityScansMerged;
 	float _proximityAngle;
 	std::string _databasePath;
 	bool _optimizeFromGraphEnd;
@@ -23,13 +23,20 @@ void segmentObstaclesFromGround(
 		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)
 {
 	ground.reset(new std::vector<int>);
 	obstacles.reset(new std::vector<int>);
+	if(flatObstacles)
+	{
+		flatObstacles->reset(new std::vector<int>);
+	}

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

 		if(segmentFlatObstacles)
@@ -48,26 +55,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 +109,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 +136,13 @@ 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)
 {
 	pcl::IndicesPtr indices(new std::vector<int>);
 	segmentObstaclesFromGround<PointT>(
@@ -111,10 +150,13 @@ void segmentObstaclesFromGround(
 			indices,
 			ground,
 			obstacles,
-			normalRadiusSearch,
+			normalKSearch,
 			groundNormalAngle,
+			clusterRadius,
 			minClusterSize,
-			segmentFlatObstacles);
+			segmentFlatObstacles,
+			maxGroundHeight,
+			flatObstacles);
 }

 template<typename PointT>
@@ -125,7 +167,9 @@ void occupancy2DFromCloud3D(
 		cv::Mat & obstacles,
 		float cellSize,
 		float groundNormalAngle,
-		int minClusterSize)
+		int minClusterSize,
+		bool segmentFlatObstacles,
+		float maxGroundHeight)
 {
 	if(cloud->size() == 0)
 	{
@@ -138,9 +182,12 @@ 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>);
@@ -193,10 +240,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);
 }

 }
@@ -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,
@@ -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>
@@ -78,6 +80,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDepth(
 		float fx, float fy,
 		int decimation = 1,
 		float maxDepth = 0.0f,
+		float minDepth = 0.0f,
 		std::vector<int> * validIndices = 0);

 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDepthRGB(
@@ -87,6 +90,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDepthRGB(
 		float fx, float fy,
 		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 +98,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 +107,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 +116,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 +145,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 +156,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 +207,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.
 *
@@ -108,6 +108,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 +358,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 +379,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 +389,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);

 /**
@@ -86,19 +86,25 @@ 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);
 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);

 template<typename PointT>
 void occupancy2DFromCloud3D(
@@ -107,7 +113,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 +124,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
@@ -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,
@@ -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,27 @@ 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)
+
 ####################################
 # Generate resources files
 ####################################
@@ -722,7 +722,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 +750,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
 			{
@@ -48,6 +48,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <fc2triclops.h>
 #endif

+#ifdef RTABMAP_ZED
+#include <zed/Camera.hpp>
+#endif
+
 namespace rtabmap
 {

@@ -731,6 +735,214 @@ SensorData CameraStereoFlyCapture2::captureImage()
 	return data;
 }

+//
+// CameraStereoZED
+//
+bool CameraStereoZed::available()
+{
+#ifdef RTABMAP_ZED
+	return true;
+#else
+	return false;
+#endif
+}
+
+CameraStereoZed::CameraStereoZed(
+		int deviceId,
+		int resolution,
+		int quality,
+		int sensingMode,
+		int confidenceThr,
+		float imageRate,
+		const Transform & localTransform) :
+	Camera(imageRate, localTransform),
+	zed_(0),
+	src_(CameraVideo::kUsbDevice),
+	usbDevice_(deviceId),
+	svoFilePath_(""),
+	resolution_(resolution),
+	quality_(quality),
+	sensingMode_(sensingMode),
+	confidenceThr_(confidenceThr)
+{
+#ifdef RTABMAP_ZED
+	UASSERT(resolution_ >= sl::zed::HD2K && resolution_ <=sl::zed::VGA);
+	UASSERT(quality_ >= sl::zed::NONE && quality_ <=sl::zed::QUALITY);
+	UASSERT(sensingMode_ >= sl::zed::FULL && sensingMode_ <=sl::zed::RAW);
+	UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
+#endif
+}
+
+CameraStereoZed::CameraStereoZed(
+		const std::string & filePath,
+		int quality,
+		int sensingMode,
+		int confidenceThr,
+		float imageRate,
+		const Transform & localTransform) :
+	Camera(imageRate, localTransform),
+	zed_(0),
+	src_(CameraVideo::kVideoFile),
+	usbDevice_(0),
+	svoFilePath_(filePath),
+	resolution_(2),
+	quality_(quality),
+	sensingMode_(sensingMode),
+	confidenceThr_(confidenceThr)
+{
+#ifdef RTABMAP_ZED
+	UASSERT(resolution_ >= sl::zed::HD2K && resolution_ <=sl::zed::VGA);
+	UASSERT(quality_ >= sl::zed::NONE && quality_ <=sl::zed::QUALITY);
+	UASSERT(sensingMode_ >= sl::zed::FULL && sensingMode_ <=sl::zed::RAW);
+	UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
+#endif
+}
+
+CameraStereoZed::~CameraStereoZed()
+{
+#ifdef RTABMAP_ZED
+	if(zed_)
+	{
+		delete zed_;
+	}
+#endif
+}
+
+bool CameraStereoZed::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+#ifdef RTABMAP_ZED
+	if(zed_)
+	{
+		delete zed_;
+		zed_ = 0;
+	}
+	
+
+	if(src_ == CameraVideo::kVideoFile)
+	{
+		zed_ = new sl::zed::Camera(svoFilePath_); // Use in SVO playback mode
+	}
+	else
+	{
+		if(zed_->isZEDconnected())
+		{
+			zed_ = new sl::zed::Camera((sl::zed::ZEDResolution_mode)resolution_, getImageRate(), usbDevice_); // Use in Live Mode
+		}
+		else
+		{
+			UERROR("ZED camera initialization failed: ZED is not connected!");
+			return false;
+		}
+	}
+
+	//init WITH self-calibration (- last parameter to false -)
+	sl::zed::ERRCODE err = zed_->init(
+			(sl::zed::MODE)quality_,
+			-1, // search for any GPU
+			true, false, false);
+
+	// Quit if an error occurred
+	if (err != sl::zed::SUCCESS)
+	{
+		UERROR("ZED camera initialization failed: %s", sl::zed::errcode2str(err).c_str());
+		delete zed_;
+		zed_ = 0;
+		return false;
+	}
+
+	zed_->setConfidenceThreshold(confidenceThr_);
+
+	sl::zed::StereoParameters * stereoParams = zed_->getParameters();
+	sl::zed::resolution res = zed_->getImageSize();
+				
+	stereoModel_ = StereoCameraModel(
+		stereoParams->LeftCam.fx, 
+		stereoParams->LeftCam.fy, 
+		stereoParams->LeftCam.cx, 
+		stereoParams->LeftCam.cy, 
+		stereoParams->baseline/1000.0f,
+		this->getLocalTransform(),
+		cv::Size(res.width, res.height));
+
+	return true;
+#else
+	UERROR("CameraStereoZED: RTAB-Map is not built with ZED sdk support!");
+#endif
+	return false;
+}
+
+bool CameraStereoZed::isCalibrated() const
+{
+	return stereoModel_.isValidForProjection();
+}
+
+std::string CameraStereoZed::getSerial() const
+{
+#ifdef RTABMAP_ZED
+	if(zed_)
+	{
+		return uFormat("%x", zed_->getZEDSerial());
+	}
+#endif
+	return "";
+}
+
+SensorData CameraStereoZed::captureImage()
+{
+	SensorData data;
+#ifdef RTABMAP_ZED
+	if(zed_)
+	{
+		UTimer timer;
+		bool res = zed_->grab((sl::zed::SENSING_MODE)sensingMode_, quality_ > 0, quality_ > 0, false);
+		while (src_ == CameraVideo::kUsbDevice && res && timer.elapsed() < 2.0)
+		{
+			// maybe there is a latency with the USB, try again in 10 ms (for the next 2 seconds)
+			uSleep(10);
+			res = zed_->grab((sl::zed::SENSING_MODE)sensingMode_, quality_ > 0, quality_ > 0, false);
+		}
+		if(!res)
+		{
+			// get left image
+			cv::Mat rgbaLeft = slMat2cvMat(zed_->retrieveImage(static_cast<sl::zed::SIDE> (sl::zed::STEREO_LEFT)));
+			cv::Mat left;
+			cv::cvtColor(rgbaLeft, left, cv::COLOR_BGRA2BGR);
+
+			if(quality_ > 0)
+			{
+				// get depth image
+				cv::Mat depth;
+				slMat2cvMat(zed_->retrieveMeasure(sl::zed::MEASURE::DEPTH)).copyTo(depth);
+				depth /= 1000.0; // to meters
+
+				data = SensorData(left, depth, stereoModel_.left(), this->getNextSeqID(), UTimer::now());
+			}
+			else
+			{
+				// get right image
+				cv::Mat rgbaRight = slMat2cvMat(zed_->retrieveImage(static_cast<sl::zed::SIDE> (sl::zed::STEREO_RIGHT)));
+				cv::Mat right;
+				cv::cvtColor(rgbaRight, right, cv::COLOR_BGRA2GRAY);
+			
+				data = SensorData(left, right, stereoModel_, this->getNextSeqID(), UTimer::now());
+			}
+		}
+		else if(src_ == CameraVideo::kUsbDevice)
+		{
+			UERROR("CameraStereoZed: Failed to grab images after 2 seconds!");
+		}
+		else
+		{
+			UWARN("CameraStereoZed: end of stream is reached!");
+		}
+	}
+#else
+	UERROR("CameraStereoZED: RTAB-Map is not built with ZED sdk support!");
+#endif
+	return data;
+}
+
+
 //
 // CameraStereoImages
 //
@@ -921,7 +1133,21 @@ CameraStereoVideo::CameraStereoVideo(
 		const Transform & localTransform) :
 		Camera(imageRate, localTransform),
 		path_(path),
-		rectifyImages_(rectifyImages)
+		rectifyImages_(rectifyImages),
+		src_(CameraVideo::kVideoFile),
+		usbDevice_(0)
+{
+}
+
+CameraStereoVideo::CameraStereoVideo(
+	int device,
+	float imageRate,
+	const Transform & localTransform) :
+	Camera(imageRate, localTransform),
+	path_(""),
+	rectifyImages_(false),
+	src_(CameraVideo::kUsbDevice),
+	usbDevice_(device)
 {
 }

@@ -932,29 +1158,51 @@ CameraStereoVideo::~CameraStereoVideo()

 bool CameraStereoVideo::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
+	cameraName_ = cameraName;
 	if(capture_.isOpened())
 	{
 		capture_.release();
 	}
-	ULOGGER_DEBUG("Camera: filename=\"%s\"", path_.c_str());
-	capture_.open(path_.c_str());
+
+	if (src_ == CameraVideo::kUsbDevice)
+	{
+		ULOGGER_DEBUG("CameraStereoVideo: Usb device initialization on device %d", usbDevice_);
+		capture_.open(usbDevice_);
+	}
+	else if (src_ == CameraVideo::kVideoFile)
+	{
+		ULOGGER_DEBUG("CameraStereoVideo: filename=\"%s\"", path_.c_str());
+		capture_.open(path_.c_str());
+	}
+	else
+	{
+		ULOGGER_ERROR("CameraStereoVideo: Unknown source...");
+	}

 	if(!capture_.isOpened())
 	{
-		ULOGGER_ERROR("Camera: Failed to create a capture object!");
+		ULOGGER_ERROR("CameraStereoVideo: Failed to create a capture object!");
 		capture_.release();
 		return false;
 	}
 	else
 	{
-		// look for calibration files
-		cameraName_ = cameraName;
-		if(!calibrationFolder.empty() && !cameraName.empty())
+		if (cameraName_.empty())
 		{
-			if(!stereoModel_.load(calibrationFolder, cameraName))
+			unsigned int guid = (unsigned int)capture_.get(CV_CAP_PROP_GUID);
+			if (guid != 0 && guid != 0xffffffff)
+			{
+				cameraName_ = uFormat("%08x", guid);
+			}
+		}
+
+		// look for calibration files
+		if(!calibrationFolder.empty() && !cameraName_.empty())
+		{
+			if(!stereoModel_.load(calibrationFolder, cameraName_))
 			{
 				UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
-						cameraName.c_str(), calibrationFolder.c_str());
+					cameraName_.c_str(), calibrationFolder.c_str());
 			}
 			else
 			{
@@ -1007,7 +1255,7 @@ SensorData CameraStereoVideo::captureImage()
 				rightCvt = true;
 			}

-			if(rectifyImages_ && stereoModel_.left().isValidForRectification() && stereoModel_.right().isValidForRectification())
+			if((src_ != CameraVideo::kVideoFile || rectifyImages_) && stereoModel_.left().isValidForRectification() && stereoModel_.right().isValidForRectification())
 			{
 				leftImage = stereoModel_.left().rectifyImage(leftImage);
 				rightImage = stereoModel_.right().rectifyImage(rightImage);
@@ -53,6 +53,7 @@ CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
 		_scanFromDepth(false),
 		_scanDecimation(4),
 		_scanMaxDepth(4.0f),
+		_scanMinDepth(0.0f),
 		_scanVoxelSize(0.0f),
 		_scanNormalsK(0),
 		_stereoDense(new StereoBM(parameters))
@@ -174,7 +175,7 @@ void CameraThread::mainLoop()
 				UASSERT(_scanDecimation >= 1);
 				UTimer timer;
 				pcl::IndicesPtr validIndices(new std::vector<int>);
-				pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(data, _scanDecimation, _scanMaxDepth, 0.0f, 0, validIndices.get());
+				pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(data, _scanDecimation, _scanMaxDepth, _scanMinDepth, validIndices.get());
 				float maxPoints = (data.depthRaw().rows/_scanDecimation)*(data.depthRaw().cols/_scanDecimation);
 				if(_scanVoxelSize>0.0f)
 				{
@@ -61,14 +61,24 @@ void DBDriver::parseParameters(const ParametersMap & parameters)
 {
 }

-void DBDriver::closeConnection()
+void DBDriver::closeConnection(bool save)
 {
 	UDEBUG("isRunning=%d", this->isRunning());
 	this->join(true);
 	UDEBUG("");
-	this->emptyTrashes();
+	if(save)
+	{
+		this->emptyTrashes();
+	}
+	else
+	{
+		_trashesMutex.lock();
+		_trashSignatures.clear();
+		_trashVisualWords.clear();
+		_trashesMutex.unlock();
+	}
 	_dbSafeAccessMutex.lock();
-	this->disconnectDatabaseQuery();
+	this->disconnectDatabaseQuery(save);
 	_dbSafeAccessMutex.unlock();
 	UDEBUG("");
 }
@@ -546,6 +556,31 @@ void DBDriver::getNodeData(
 	}
 }

+bool DBDriver::getCalibration(
+		int signatureId,
+		std::vector<CameraModel> & models,
+		StereoCameraModel & stereoModel) const
+{
+	bool found = false;
+	// look in the trash
+	_trashesMutex.lock();
+	if(uContains(_trashSignatures, signatureId))
+	{
+		models = _trashSignatures.at(signatureId)->sensorData().cameraModels();
+		stereoModel = _trashSignatures.at(signatureId)->sensorData().stereoCameraModel();
+		found = true;
+	}
+	_trashesMutex.unlock();
+
+	if(!found)
+	{
+		_dbSafeAccessMutex.lock();
+		found = this->getCalibrationQuery(signatureId, models, stereoModel);
+		_dbSafeAccessMutex.unlock();
+	}
+	return found;
+}
+
 bool DBDriver::getNodeInfo(
 		int signatureId,
 		Transform & pose,
@@ -381,7 +381,7 @@ bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwri

 	return true;
 }
-void DBDriverSqlite3::disconnectDatabaseQuery()
+void DBDriverSqlite3::disconnectDatabaseQuery(bool save)
 {
 	UDEBUG("");
 	if(_ppDb)
@@ -398,7 +398,7 @@ void DBDriverSqlite3::disconnectDatabaseQuery()
 			}
 		}

-		if(_dbInMemory)
+		if(save && _dbInMemory)
 		{
 			UTimer timer;
 			timer.start();
@@ -1009,6 +1009,156 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures) con
 	}
 }

+bool DBDriverSqlite3::getCalibrationQuery(
+		int signatureId,
+		std::vector<CameraModel> & models,
+		StereoCameraModel & stereoModel) const
+{
+	bool found = false;
+	if(_ppDb && signatureId)
+	{
+		int rc = SQLITE_OK;
+		sqlite3_stmt * ppStmt = 0;
+		std::stringstream query;
+
+		if(uStrNumCmp(_version, "0.10.0") >= 0)
+		{
+			query << "SELECT calibration "
+				  << "FROM Data "
+				  << "WHERE id = " << signatureId
+				  <<";";
+		}
+		else if(uStrNumCmp(_version, "0.7.0") >= 0)
+		{
+			query << "SELECT local_transform, fx, fy, cx, cy "
+				  << "FROM Depth "
+				  << "WHERE id = " << signatureId
+				  <<";";
+		}
+		else
+		{
+			query << "SELECT local_transform, constant "
+				  << "FROM Depth "
+				  << "WHERE id = " << signatureId
+				  <<";";
+		}
+
+		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		const void * data = 0;
+		int dataSize = 0;
+		Transform localTransform;
+
+		// Process the result if one
+		rc = sqlite3_step(ppStmt);
+		if(rc == SQLITE_ROW)
+		{
+			found = true;
+			int index = 0;
+
+			// calibration
+			if(uStrNumCmp(_version, "0.10.0") >= 0)
+			{
+				data = sqlite3_column_blob(ppStmt, index);
+				dataSize = sqlite3_column_bytes(ppStmt, index++);
+				// multi-cameras [fx,fy,cx,cy,[width,height],local_transform, ... ,fx,fy,cx,cy,[width,height],local_transform] (4or6+12)*float * numCameras
+				// stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
+				if(dataSize > 0 && data)
+				{
+					float * dataFloat = (float*)data;
+					if((unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0)
+					{
+						int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float));
+						UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
+						int max = cameraCount*(4+localTransform.size());
+						for(int i=0; i<max; i+=4+localTransform.size())
+						{
+							memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float));
+							models.push_back(CameraModel(
+									(double)dataFloat[i],
+									(double)dataFloat[i+1],
+									(double)dataFloat[i+2],
+									(double)dataFloat[i+3],
+									localTransform));
+						}
+					}
+					else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
+					{
+						UDEBUG("Loading calibration of a stereo camera");
+						memcpy(localTransform.data(), dataFloat+5, localTransform.size()*sizeof(float));
+						stereoModel = StereoCameraModel(
+								dataFloat[0],  // fx
+								dataFloat[1],  // fy
+								dataFloat[2],  // cx
+								dataFloat[3],  // cy
+								dataFloat[4], // baseline
+								localTransform);
+					}
+					else if((unsigned int)dataSize % (6+localTransform.size())*sizeof(float) == 0)
+					{
+						int cameraCount = dataSize / ((6+localTransform.size())*sizeof(float));
+						UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
+						int max = cameraCount*(6+localTransform.size());
+						for(int i=0; i<max; i+=6+localTransform.size())
+						{
+							memcpy(localTransform.data(), dataFloat+i+6, localTransform.size()*sizeof(float));
+							models.push_back(CameraModel(
+									(double)dataFloat[i],
+									(double)dataFloat[i+1],
+									(double)dataFloat[i+2],
+									(double)dataFloat[i+3],
+									localTransform));
+							models.back().setImageSize(cv::Size(dataFloat[i+4], dataFloat[i+5]));
+							UDEBUG("%f %f %f %f %f %f %s", dataFloat[i], dataFloat[i+1], dataFloat[i+2],
+									dataFloat[i+3], dataFloat[i+4], dataFloat[i+5],
+									localTransform.prettyPrint().c_str());
+						}
+					}
+					else
+					{
+						UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
+					}
+				}
+
+			}
+			else if(uStrNumCmp(_version, "0.7.0") >= 0)
+			{
+				double fx = sqlite3_column_double(ppStmt, index++);
+				double fyOrBaseline = sqlite3_column_double(ppStmt, index++);
+				double cx = sqlite3_column_double(ppStmt, index++);
+				double cy = sqlite3_column_double(ppStmt, index++);
+				if(fyOrBaseline < 1.0)
+				{
+					//it is a baseline
+					stereoModel = StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform);
+				}
+				else
+				{
+					models.push_back(CameraModel(fx, fyOrBaseline, cx, cy, localTransform));
+				}
+			}
+			else
+			{
+				float depthConstant = sqlite3_column_double(ppStmt, index++);
+				float fx = 1.0f/depthConstant;
+				float fy = 1.0f/depthConstant;
+				float cx = 0.0f;
+				float cy = 0.0f;
+				models.push_back(CameraModel(fx, fy, cx, cy, localTransform));
+			}
+
+			rc = sqlite3_step(ppStmt); // next result...
+		}
+		UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		// Finalize (delete) the statement
+		rc = sqlite3_finalize(ppStmt);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+	}
+	return found;
+}
+
 bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
 		Transform & pose,
 		int & mapId,
@@ -48,8 +48,8 @@ public:
 	void setTempStore(int tempStore);

 private:
-	virtual bool connectDatabaseQuery(const std::string & url, bool overwirtten = false);
-	virtual void disconnectDatabaseQuery();
+	virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false);
+	virtual void disconnectDatabaseQuery(bool save = true);
 	virtual bool isConnectedQuery() const;
 	virtual long getMemoryUsedQuery() const; // In bytes
 	virtual bool getDatabaseVersionQuery(std::string & version) const;
@@ -83,6 +83,7 @@ private:
 	virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;

 	virtual void loadNodeDataQuery(std::list<Signature *> & signatures) const;
+	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
 	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose) const;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const;
 	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const;
@@ -308,10 +308,9 @@ cv::Rect Feature2D::computeRoi(const cv::Mat & image, const std::vector<float> &
 		}

 		//right roi
-		roi.width = width - roi.x;
 		if(roiRatios[1] > 0 && roiRatios[1] < 1 - roiRatios[0])
 		{
-			roi.width -= width * roiRatios[1];
+			roi.width -= width * roiRatios[1] + width * roiRatios[0];
 		}

 		//top roi
@@ -321,10 +320,9 @@ cv::Rect Feature2D::computeRoi(const cv::Mat & image, const std::vector<float> &
 		}

 		//bottom roi
-		roi.height = height - roi.y;
 		if(roiRatios[3] > 0 && roiRatios[3] < 1 - roiRatios[2])
 		{
-			roi.height -= height * roiRatios[3];
+			roi.height -= height * roiRatios[3] + height * roiRatios[2];
 		}
 		UDEBUG("roi = %d, %d, %d, %d", roi.x, roi.y, roi.width, roi.height);

@@ -505,7 +503,7 @@ std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, co
 				if(((unsigned short*)maskIn.data)[i] > 0 &&
 				   ((unsigned short*)maskIn.data)[i] < std::numeric_limits<unsigned short>::max())
 				{
-					value = float(((unsigned short*)maskIn.data)[i])*0.0001f;
+					value = float(((unsigned short*)maskIn.data)[i])*0.001f;
 				}
 			}
 			else
@@ -561,6 +561,36 @@ std::multimap<int, int>::const_iterator findLink(
 	return links.end();
 }

+std::multimap<int, Link> filterLinks(
+		const std::multimap<int, Link> & links,
+		Link::Type filteredType)
+{
+	std::multimap<int, Link> output;
+	for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
+	{
+		if(iter->second.type() != filteredType)
+		{
+			output.insert(*iter);
+		}
+	}
+	return output;
+}
+
+std::map<int, Link> filterLinks(
+		const std::map<int, Link> & links,
+		Link::Type filteredType)
+{
+	std::map<int, Link> output;
+	for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
+	{
+		if(iter->second.type() != filteredType)
+		{
+			output.insert(*iter);
+		}
+	}
+	return output;
+}
+
 std::map<int, Transform> frustumPosesFiltering(
 		const std::map<int, Transform> & poses,
 		const Transform & cameraPose,
@@ -616,7 +646,7 @@ std::map<int, Transform> radiusPosesFiltering(
 		float angle,
 		bool keepLatest)
 {
-	if(poses.size() > 1 && radius > 0.0f)
+	if(poses.size() > 2 && radius > 0.0f)
 	{
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 		cloud->resize(poses.size());
@@ -710,6 +740,10 @@ std::map<int, Transform> radiusPosesFiltering(
 			keptPoses.insert(std::make_pair(names.at(*iter), transforms.at(*iter)));
 		}

+		// make sure the first and last poses are still here
+		keptPoses.insert(*poses.begin());
+		keptPoses.insert(*poses.rbegin());
+
 		return keptPoses;
 	}
 	else
@@ -83,7 +83,8 @@ Memory::Memory(const ParametersMap & parameters) :
 	_generateIds(Parameters::defaultMemGenerateIds()),
 	_badSignaturesIgnored(Parameters::defaultMemBadSignaturesIgnored()),
 	_mapLabelsAdded(Parameters::defaultMemMapLabelsAdded()),
-	_imageDecimation(Parameters::defaultMemImageDecimation()),
+	_imagePreDecimation(Parameters::defaultMemImagePreDecimation()),
+	_imagePostDecimation(Parameters::defaultMemImagePostDecimation()),
 	_laserScanDownsampleStepSize(Parameters::defaultMemLaserScanDownsampleStepSize()),
 	_reextractLoopClosureFeatures(Parameters::defaultRGBDLoopClosureReextractFeatures()),
 	_rehearsalMaxDistance(Parameters::defaultRGBDLinearUpdate()),
@@ -313,7 +314,7 @@ void Memory::close(bool databaseSaved, bool postInitClosingEvents)
 		if(_dbDriver)
 		{
 			if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Closing database \"%s\"...", _dbDriver->getUrl().c_str())));
-			_dbDriver->closeConnection();
+			_dbDriver->closeConnection(false);
 			delete _dbDriver;
 			_dbDriver = 0;
 			if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database, done!"));
@@ -397,7 +398,8 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kMemRecentWmRatio(), _recentWmRatio);
 	Parameters::parse(parameters, Parameters::kMemTransferSortingByWeightId(), _transferSortingByWeightId);
 	Parameters::parse(parameters, Parameters::kMemSTMSize(), _maxStMemSize);
-	Parameters::parse(parameters, Parameters::kMemImageDecimation(), _imageDecimation);
+	Parameters::parse(parameters, Parameters::kMemImagePreDecimation(), _imagePreDecimation);
+	Parameters::parse(parameters, Parameters::kMemImagePostDecimation(), _imagePostDecimation);
 	Parameters::parse(parameters, Parameters::kMemLaserScanDownsampleStepSize(), _laserScanDownsampleStepSize);
 	Parameters::parse(parameters, Parameters::kRGBDLoopClosureReextractFeatures(), _reextractLoopClosureFeatures);
 	Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rehearsalMaxDistance);
@@ -408,7 +410,8 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	UASSERT_MSG(_maxStMemSize >= 0, uFormat("value=%d", _maxStMemSize).c_str());
 	UASSERT_MSG(_similarityThreshold >= 0.0f && _similarityThreshold <= 1.0f, uFormat("value=%f", _similarityThreshold).c_str());
 	UASSERT_MSG(_recentWmRatio >= 0.0f && _recentWmRatio <= 1.0f, uFormat("value=%f", _recentWmRatio).c_str());
-	UASSERT(_imageDecimation >= 1);
+	UASSERT(_imagePreDecimation >= 1);
+	UASSERT(_imagePostDecimation >= 1);
 	UASSERT(_rehearsalMaxDistance >= 0.0f);
 	UASSERT(_rehearsalMaxAngle >= 0.0f);

@@ -492,7 +495,8 @@ void Memory::parseParameters(const ParametersMap & parameters)
 			if((_memoryChanged || _linksChanged) && _dbDriver)
 			{
 				UWARN("Switching from Mapping to Localization mode, the database will be saved and reloaded.");
-				this->init(_dbDriver->getUrl());
+				this->init(_dbDriver->getUrl());
+				UWARN("Switching from Mapping to Localization mode, the database is reloaded!");
 			}
 		}
 		_incrementalMemory = value;
@@ -1898,6 +1902,7 @@ bool Memory::labelSignature(int id, const std::string & label)
 		if(s)
 		{
 			s->setLabel(label);
+			_linksChanged = s->isSaved(); // HACK to get label updated in Localization mode
 			UWARN("Label \"%s\" set to node %d", label.c_str(), id);
 			return true;
 		}
@@ -2066,113 +2071,7 @@ Transform Memory::computeTransform(

 	if(fromS && toS)
 	{
-		// make sure we have all data needed
-		// load binary data from database if not in RAM (if image is already here, scan and userData should be or they are null)
-		if((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired() && fromS->sensorData().imageCompressed().empty()) ||
-		   (_registrationPipeline->isScanRequired() && fromS->sensorData().imageCompressed().empty() && fromS->sensorData().laserScanCompressed().empty()) ||
-		   (_registrationPipeline->isUserDataRequired() && fromS->sensorData().imageCompressed().empty() && fromS->sensorData().userDataCompressed().empty()))
-		{
-			getNodeData(fromS->id());
-		}
-		if((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired() && toS->sensorData().imageCompressed().empty()) ||
-		   (_registrationPipeline->isScanRequired() && toS->sensorData().imageCompressed().empty() && toS->sensorData().laserScanCompressed().empty()) ||
-		   (_registrationPipeline->isUserDataRequired() && toS->sensorData().imageCompressed().empty() && toS->sensorData().userDataCompressed().empty()))
-		{
-			getNodeData(toS->id());
-		}
-		// uncompress only what we need
-		cv::Mat imgBuf, depthBuf, laserBuf, userBuf;
-		fromS->sensorData().uncompressData(
-				(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&imgBuf:0,
-				(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&depthBuf:0,
-				_registrationPipeline->isScanRequired()?&laserBuf:0,
-				_registrationPipeline->isUserDataRequired()?&userBuf:0);
-		toS->sensorData().uncompressData(
-				(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&imgBuf:0,
-				(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&depthBuf:0,
-				_registrationPipeline->isScanRequired()?&laserBuf:0,
-				_registrationPipeline->isUserDataRequired()?&userBuf:0);
-
-
-		// compute transform fromId -> toId
-		std::vector<int> inliersV;
-		if(_reextractLoopClosureFeatures || (fromS->getWords().size() && toS->getWords().size()))
-		{
-			Signature tmpFrom = *fromS;
-			Signature tmpTo = *toS;
-
-			// make a guess fast with known correspondences (if there are)
-			RegistrationVis regVis(parameters_);
-			if(tmpFrom.getWords().size() &&
-				tmpTo.getWords().size() &&
-				tmpFrom.getWords3().size() &&
-				tmpTo.getWords3().size())
-			{
-				UDEBUG("");
-				// Remove descriptors, this will avoid recomputation of the correspondences in regVis
-				tmpFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
-				tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
-				guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
-				// set back descriptors
-				tmpFrom.setWordsDescriptors(fromS->getWordsDescriptors());
-				tmpTo.setWordsDescriptors(toS->getWordsDescriptors());
-			}
-
-			if(_reextractLoopClosureFeatures)
-			{
-				UDEBUG("");
-				tmpFrom.setWords(std::multimap<int, cv::KeyPoint>());
-				tmpFrom.setWords3(std::multimap<int, cv::Point3f>());
-				tmpFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
-				tmpFrom.sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
-				tmpTo.setWords(std::multimap<int, cv::KeyPoint>());
-				tmpTo.setWords3(std::multimap<int, cv::Point3f>());
-				tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
-				tmpTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
-			}
-
-			if(guess.isNull())
-			{
-				if(!_registrationPipeline->isImageRequired())
-				{
-					UDEBUG("");
-					// no visual in the pipeline, make visual registration for guess
-					guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
-				}
-				else
-				{
-					UDEBUG("");
-					guess.setIdentity();
-				}
-			}
-
-			if(!guess.isNull())
-			{
-				UDEBUG("");
-				transform = _registrationPipeline->computeTransformation(tmpFrom, tmpTo, guess, info);
-
-				if(!transform.isNull())
-				{
-					UDEBUG("");
-					// verify if it is a 180 degree transform, well verify > 90
-					float x,y,z, roll,pitch,yaw;
-					transform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
-					if(fabs(roll) > CV_PI/2 ||
-					   fabs(pitch) > CV_PI/2 ||
-					   fabs(yaw) > CV_PI/2)
-					{
-						transform.setNull();
-						std::string msg = uFormat("Too large rotation detected! (roll=%f, pitch=%f, yaw=%f)",
-								roll, pitch, yaw);
-						UINFO(msg.c_str());
-						if(info)
-						{
-							info->rejectedMsg = msg;
-						}
-					}
-				}
-			}
-		}
+		return computeTransform(*fromS, *toS, guess, info);
 	}
 	else
 	{
@@ -2186,6 +2085,99 @@ Transform Memory::computeTransform(
 	return transform;
 }

+// compute transform fromId -> toId
+Transform Memory::computeTransform(
+		Signature & fromS,
+		Signature & toS,
+		Transform guess,
+		RegistrationInfo * info) const
+{
+	Transform transform;
+
+	// make sure we have all data needed
+	// load binary data from database if not in RAM (if image is already here, scan and userData should be or they are null)
+	if((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired() && fromS.sensorData().imageCompressed().empty()) ||
+	   (_registrationPipeline->isScanRequired() && fromS.sensorData().imageCompressed().empty() && fromS.sensorData().laserScanCompressed().empty()) ||
+	   (_registrationPipeline->isUserDataRequired() && fromS.sensorData().imageCompressed().empty() && fromS.sensorData().userDataCompressed().empty()))
+	{
+		fromS.sensorData() = getNodeData(fromS.id());
+	}
+	if((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired() && toS.sensorData().imageCompressed().empty()) ||
+	   (_registrationPipeline->isScanRequired() && toS.sensorData().imageCompressed().empty() && toS.sensorData().laserScanCompressed().empty()) ||
+	   (_registrationPipeline->isUserDataRequired() && toS.sensorData().imageCompressed().empty() && toS.sensorData().userDataCompressed().empty()))
+	{
+		toS.sensorData() = getNodeData(toS.id());
+	}
+	// uncompress only what we need
+	cv::Mat imgBuf, depthBuf, laserBuf, userBuf;
+	fromS.sensorData().uncompressData(
+			(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&imgBuf:0,
+			(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&depthBuf:0,
+			_registrationPipeline->isScanRequired()?&laserBuf:0,
+			_registrationPipeline->isUserDataRequired()?&userBuf:0);
+	toS.sensorData().uncompressData(
+			(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&imgBuf:0,
+			(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&depthBuf:0,
+			_registrationPipeline->isScanRequired()?&laserBuf:0,
+			_registrationPipeline->isUserDataRequired()?&userBuf:0);
+
+
+	// compute transform fromId -> toId
+	std::vector<int> inliersV;
+	if(_reextractLoopClosureFeatures ||
+		(fromS.getWords().size() && toS.getWords().size()) ||
+		(!guess.isNull() && !_registrationPipeline->isImageRequired()))
+	{
+		Signature tmpFrom = fromS;
+		Signature tmpTo = toS;
+
+		if(_reextractLoopClosureFeatures)
+		{
+			UDEBUG("");
+			tmpFrom.setWords(std::multimap<int, cv::KeyPoint>());
+			tmpFrom.setWords3(std::multimap<int, cv::Point3f>());
+			tmpFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
+			tmpFrom.sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
+			tmpTo.setWords(std::multimap<int, cv::KeyPoint>());
+			tmpTo.setWords3(std::multimap<int, cv::Point3f>());
+			tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
+			tmpTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
+		}
+
+		if(guess.isNull() && !_registrationPipeline->isImageRequired())
+		{
+			UDEBUG("");
+			// no visual in the pipeline, make visual registration for guess
+			RegistrationVis regVis(parameters_);
+			guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
+		}
+
+		transform = _registrationPipeline->computeTransformation(tmpFrom, tmpTo, guess, info);
+
+		if(!transform.isNull())
+		{
+			UDEBUG("");
+			// verify if it is a 180 degree transform, well verify > 90
+			float x,y,z, roll,pitch,yaw;
+			transform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
+			if(fabs(roll) > CV_PI/2 ||
+			   fabs(pitch) > CV_PI/2 ||
+			   fabs(yaw) > CV_PI/2)
+			{
+				transform.setNull();
+				std::string msg = uFormat("Too large rotation detected! (roll=%f, pitch=%f, yaw=%f)",
+						roll, pitch, yaw);
+				UINFO(msg.c_str());
+				if(info)
+				{
+					info->rejectedMsg = msg;
+				}
+			}
+		}
+	}
+	return transform;
+}
+
 // compute transform fromId -> toId
 Transform Memory::computeIcpTransform(
 		int fromId,
@@ -2282,6 +2274,7 @@ Transform Memory::computeIcpTransformMulti(
 		SensorData assembledData;
 		Transform toPose = poses.at(toId);
 		std::string msg;
+		int maxPoints = fromScan.cols;
 		pcl::PointCloud<pcl::PointXYZ>::Ptr assembledToClouds(new pcl::PointCloud<pcl::PointXYZ>);
 		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 		{
@@ -2293,6 +2286,10 @@ Transform Memory::computeIcpTransformMulti(
 					cv::Mat scan;
 					s->sensorData().uncompressData(0, 0, &scan);
 					pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(scan, toPose.inverse() * iter->second);
+					if(scan.cols > maxPoints)
+					{
+						maxPoints = scan.cols;
+					}
 					*assembledToClouds += *cloud;
 				}
 				else
@@ -2303,7 +2300,7 @@ Transform Memory::computeIcpTransformMulti(
 		}
 		if(assembledToClouds->size())
 		{
-			assembledData.setLaserScanRaw(util3d::laserScanFromPointCloud(*assembledToClouds, Transform()), fromS->sensorData().laserScanMaxPts(), fromS->sensorData().laserScanMaxRange());
+			assembledData.setLaserScanRaw(util3d::laserScanFromPointCloud(*assembledToClouds, Transform()), fromS->sensorData().laserScanMaxPts()?fromS->sensorData().laserScanMaxPts():maxPoints, fromS->sensorData().laserScanMaxRange());
 		}

 		Transform guess = poses.at(fromId).inverse() * poses.at(toId);
@@ -2314,7 +2311,7 @@ Transform Memory::computeIcpTransformMulti(
 	return t;
 }

-bool Memory::addLink(const Link & link)
+bool Memory::addLink(const Link & link, bool addInDatabase)
 {
 	UASSERT(link.type() > Link::kNeighbor && link.type() != Link::kUndef);

@@ -2365,9 +2362,8 @@ bool Memory::addLink(const Link & link)
 				}
 			}
 		}
-		return true;
 	}
-	else
+	else if(!addInDatabase)
 	{
 		if(!fromS)
 		{
@@ -2377,8 +2373,27 @@ bool Memory::addLink(const Link & link)
 		{
 			UERROR("from=%d, to=%d, Signature %d not found in working/st memories", link.from(), link.to(), link.to());
 		}
+		return false;
 	}
-	return false;
+	else if(fromS)
+	{
+		UDEBUG("Add link between %d and %d (db)", link.from(), link.to());
+		fromS->addLink(link);
+		_dbDriver->addLink(link.inverse());
+	}
+	else if(toS)
+	{
+		UDEBUG("Add link between %d (db) and %d", link.from(), link.to());
+		_dbDriver->addLink(link);
+		toS->addLink(link.inverse());
+	}
+	else
+	{
+		UDEBUG("Add link between %d (db) and %d (db)", link.from(), link.to());
+		_dbDriver->addLink(link);
+		_dbDriver->addLink(link.inverse());
+	}
+	return true;
 }

 void Memory::updateLink(int fromId, int toId, const Transform & transform, float rotVariance, float transVariance)
@@ -2474,10 +2489,10 @@ void Memory::removeVirtualLinks(int signatureId)
 void Memory::dumpMemory(std::string directory) const
 {
 	UINFO("Dumping memory to directory \"%s\"", directory.c_str());
-	this->dumpDictionary((directory+"DumpMemoryWordRef.txt").c_str(), (directory+"DumpMemoryWordDesc.txt").c_str());
-	this->dumpSignatures((directory + "DumpMemorySign.txt").c_str(), false);
-	this->dumpSignatures((directory + "DumpMemorySign3.txt").c_str(), true);
-	this->dumpMemoryTree((directory + "DumpMemoryTree.txt").c_str());
+	this->dumpDictionary((directory+"/DumpMemoryWordRef.txt").c_str(), (directory+"/DumpMemoryWordDesc.txt").c_str());
+	this->dumpSignatures((directory + "/DumpMemorySign.txt").c_str(), false);
+	this->dumpSignatures((directory + "/DumpMemorySign3.txt").c_str(), true);
+	this->dumpMemoryTree((directory + "/DumpMemoryTree.txt").c_str());
 }

 void Memory::dumpDictionary(const char * fileNameRef, const char * fileNameDesc) const
@@ -2490,6 +2505,7 @@ void Memory::dumpDictionary(const char * fileNameRef, const char * fileNameDesc)

 void Memory::dumpSignatures(const char * fileNameSign, bool words3D) const
 {
+	UDEBUG("");
 	FILE* foutSign = 0;
 #ifdef _MSC_VER
 	fopen_s(&foutSign, fileNameSign, "w");
@@ -2537,6 +2553,7 @@ void Memory::dumpSignatures(const char * fileNameSign, bool words3D) const

 void Memory::dumpMemoryTree(const char * fileNameTree) const
 {
+	UDEBUG("");
 	FILE* foutTree = 0;
 	#ifdef _MSC_VER
 		fopen_s(&foutTree, fileNameTree, "w");
@@ -2868,45 +2885,24 @@ cv::Mat Memory::getImageCompressed(int signatureId) const
 	return image;
 }

-SensorData Memory::getNodeData(int nodeId, bool uncompressedData, bool keepLoadedDataInMemory)
+SensorData Memory::getNodeData(int nodeId, bool uncompressedData) const
 {
 	UDEBUG("nodeId=%d", nodeId);
 	SensorData r;
 	Signature * s = this->_getSignature(nodeId);
 	if(s && !s->sensorData().imageCompressed().empty())
 	{
-		if(keepLoadedDataInMemory && uncompressedData)
-		{
-			s->sensorData().uncompressData();
-		}
 		r = s->sensorData();
-		if(!keepLoadedDataInMemory && uncompressedData)
-		{
-			r.uncompressData();
-		}
 	}
 	else if(_dbDriver)
 	{
 		// load from database
-		if(s && keepLoadedDataInMemory)
-		{
-			std::list<Signature*> signatures;
-			signatures.push_back(s);
-			_dbDriver->loadNodeData(signatures);
-			if(uncompressedData)
-			{
-				s->sensorData().uncompressData();
-			}
-			r = s->sensorData();
-		}
-		else
-		{
-			_dbDriver->getNodeData(nodeId, r);
-			if(uncompressedData)
-			{
-				r.uncompressData();
-			}
-		}
+		_dbDriver->getNodeData(nodeId, r);
+	}
+
+	if(uncompressedData)
+	{
+		r.uncompressData();
 	}

 	return r;
@@ -2914,7 +2910,8 @@ SensorData Memory::getNodeData(int nodeId, bool uncompressedData, bool keepLoade

 void Memory::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)
 {
 	UDEBUG("nodeId=%d", nodeId);
 	Signature * s = this->_getSignature(nodeId);
@@ -2922,6 +2919,7 @@ void Memory::getNodeWords(int nodeId,
 	{
 		words = s->getWords();
 		words3 = s->getWords3();
+		wordsDescriptors = s->getWordsDescriptors();
 	}
 	else if(_dbDriver)
 	{
@@ -2935,6 +2933,7 @@ void Memory::getNodeWords(int nodeId,
 		{
 			words = signatures.front()->getWords();
 			words3 = signatures.front()->getWords3();
+			wordsDescriptors = signatures.front()->getWordsDescriptors();
 			if(loadedFromTrash.size())
 			{
 				//put back
@@ -2948,6 +2947,24 @@ void Memory::getNodeWords(int nodeId,
 	}
 }

+void Memory::getNodeCalibration(int nodeId,
+		std::vector<CameraModel> & models,
+		StereoCameraModel & stereoModel)
+{
+	UDEBUG("nodeId=%d", nodeId);
+	Signature * s = this->_getSignature(nodeId);
+	if(s)
+	{
+		models = s->sensorData().cameraModels();
+		stereoModel = s->sensorData().stereoCameraModel();
+	}
+	else if(_dbDriver)
+	{
+		// load from database
+		_dbDriver->getCalibration(nodeId, models, stereoModel);
+	}
+}
+
 SensorData Memory::getSignatureDataConst(int locationId) const
 {
 	UDEBUG("");
@@ -3160,31 +3177,52 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 		preUpdateThread.start();
 	}

+	int preDecimation = 1;
 	std::vector<cv::Point3f> keypoints3D;
 	if(!_useOdometryFeatures || data.keypoints().empty() || (int)data.keypoints().size() != data.descriptors().rows)
 	{
 		if(_feature2D->getMaxFeatures() >= 0 && !data.imageRaw().empty() && !isIntermediateNode)
 		{
+			SensorData decimatedData = data;
+			if(_imagePreDecimation > 1)
+			{
+				preDecimation = _imagePreDecimation;
+				decimatedData.setImageRaw(util2d::decimate(decimatedData.imageRaw(), _imagePreDecimation));
+				decimatedData.setDepthOrRightRaw(util2d::decimate(decimatedData.depthOrRightRaw(), _imagePreDecimation));
+				std::vector<CameraModel> cameraModels = decimatedData.cameraModels();
+				for(unsigned int i=0; i<cameraModels.size(); ++i)
+				{
+					cameraModels[i] = cameraModels[i].scaled(1.0/double(_imagePreDecimation));
+				}
+				decimatedData.setCameraModels(cameraModels);
+				StereoCameraModel stereoModel = decimatedData.stereoCameraModel();
+				if(stereoModel.isValidForProjection())
+				{
+					stereoModel.scale(1.0/double(_imagePreDecimation));
+				}
+				decimatedData.setStereoCameraModel(stereoModel);
+			}
+
 			UINFO("Extract features");
 			cv::Mat imageMono;
-			if(data.imageRaw().channels() == 3)
+			if(decimatedData.imageRaw().channels() == 3)
 			{
-				cv::cvtColor(data.imageRaw(), imageMono, CV_BGR2GRAY);
+				cv::cvtColor(decimatedData.imageRaw(), imageMono, CV_BGR2GRAY);
 			}
 			else
 			{
-				imageMono = data.imageRaw();
+				imageMono = decimatedData.imageRaw();
 			}

 			cv::Mat depthMask;
-			if(!data.depthRaw().empty() &&
+			if(!decimatedData.depthRaw().empty() &&
 			   _feature2D->getType() != Feature2D::kFeatureOrb) // ORB's mask pyramids don't seem to work well
 			{
-				if(imageMono.rows % data.depthRaw().rows == 0 &&
-					imageMono.cols % data.depthRaw().cols == 0 &&
-					imageMono.rows/data.depthRaw().rows == imageMono.cols/data.depthRaw().cols)
+				if(imageMono.rows % decimatedData.depthRaw().rows == 0 &&
+					imageMono.cols % decimatedData.depthRaw().cols == 0 &&
+					imageMono.rows/decimatedData.depthRaw().rows == imageMono.cols/decimatedData.depthRaw().cols)
 				{
-					depthMask = util2d::interpolate(data.depthRaw(), imageMono.rows/data.depthRaw().rows, 0.1f);
+					depthMask = util2d::interpolate(decimatedData.depthRaw(), imageMono.rows/decimatedData.depthRaw().rows, 0.1f);
 				}
 			}

@@ -3205,10 +3243,10 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 			{
 				descriptors = cv::Mat();
 			}
-			else if((!data.depthRaw().empty() && data.cameraModels().size() && data.cameraModels()[0].isValidForProjection()) ||
-					(!data.rightRaw().empty() && data.stereoCameraModel().isValidForProjection()))
+			else if((!decimatedData.depthRaw().empty() && decimatedData.cameraModels().size() && decimatedData.cameraModels()[0].isValidForProjection()) ||
+					(!decimatedData.rightRaw().empty() && decimatedData.stereoCameraModel().isValidForProjection()))
 			{
-				keypoints3D = _feature2D->generateKeypoints3D(data, keypoints);
+				keypoints3D = _feature2D->generateKeypoints3D(decimatedData, keypoints);
 				if(_feature2D->getMinDepth() > 0.0f || _feature2D->getMaxDepth() > 0.0f)
 				{
 					UDEBUG("");
@@ -3369,20 +3407,21 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 		UASSERT(wordIds.size() == keypoints.size());
 		UASSERT(keypoints3D.size() == 0 || keypoints3D.size() == wordIds.size());
 		unsigned int i=0;
+		float decimationRatio = preDecimation / _imagePostDecimation;
+		double log2value = log(double(preDecimation))/log(2.0);
 		for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end() && i < keypoints.size(); ++iter, ++i)
 		{
-			if(_imageDecimation > 1)
+			cv::KeyPoint kpt = keypoints[i];
+			if(preDecimation != _imagePostDecimation)
 			{
-				cv::KeyPoint kpt = keypoints[i];
-				kpt.pt.x /= float(_imageDecimation);
-				kpt.pt.y /= float(_imageDecimation);
-				kpt.size /= float(_imageDecimation);
-				words.insert(std::pair<int, cv::KeyPoint>(*iter, kpt));
-			}
-			else
-			{
-				words.insert(std::pair<int, cv::KeyPoint>(*iter, keypoints[i]));
+				// remap keypoints to final image size
+				kpt.pt.x *= decimationRatio;
+				kpt.pt.y *= decimationRatio;
+				kpt.size *= decimationRatio;
+				kpt.octave += log2value;
 			}
+			words.insert(std::pair<int, cv::KeyPoint>(*iter, kpt));
+
 			if(keypoints3D.size())
 			{
 				words3D.insert(std::pair<int, cv::Point3f>(*iter, keypoints3D.at(i)));
@@ -3452,17 +3491,17 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 	StereoCameraModel stereoCameraModel = data.stereoCameraModel();

 	// apply decimation?
-	if(_imageDecimation > 1)
+	if(_imagePostDecimation > 1)
 	{
-		image = util2d::decimate(image, _imageDecimation);
-		depthOrRightImage = util2d::decimate(depthOrRightImage, _imageDecimation);
+		image = util2d::decimate(image, _imagePostDecimation);
+		depthOrRightImage = util2d::decimate(depthOrRightImage, _imagePostDecimation);
 		for(unsigned int i=0; i<cameraModels.size(); ++i)
 		{
-			cameraModels[i] = cameraModels[i].scaled(1.0/double(_imageDecimation));
+			cameraModels[i] = cameraModels[i].scaled(1.0/double(_imagePostDecimation));
 		}
 		if(stereoCameraModel.isValidForProjection())
 		{
-			stereoCameraModel.scale(1.0/double(_imageDecimation));
+			stereoCameraModel.scale(1.0/double(_imagePostDecimation));
 		}
 	}

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UTimer.h"
 #include "rtabmap/utilite/UConversion.h"
 #include "rtabmap/core/ParticleFilter.h"
+#include "rtabmap/core/util2d.h"

 namespace rtabmap {

@@ -75,6 +76,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 		_fillInfoData(Parameters::defaultOdomFillInfoData()),
 		_kalmanProcessNoise(Parameters::defaultOdomKalmanProcessNoise()),
 		_kalmanMeasurementNoise(Parameters::defaultOdomKalmanMeasurementNoise()),
+		_imageDecimation(Parameters::defaultOdomImageDecimation()),
 		_resetCurrentCount(0),
 		previousStamp_(0),
 		distanceTravelled_(0)
@@ -97,6 +99,9 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 	UASSERT(_particleLambdaR>0);
 	Parameters::parse(parameters, Parameters::kOdomKalmanProcessNoise(), _kalmanProcessNoise);
 	Parameters::parse(parameters, Parameters::kOdomKalmanMeasurementNoise(), _kalmanMeasurementNoise);
+	Parameters::parse(parameters, Parameters::kOdomImageDecimation(), _imageDecimation);
+	UASSERT(_imageDecimation>=1);
+
 	if(_filteringStrategy == 2)
 	{
 		// Initialize the Particle filters
@@ -223,7 +228,64 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
 	}

 	UTimer time;
-	Transform t = this->computeTransform(data, guess, info);
+	Transform t;
+	if(_imageDecimation > 1)
+	{
+		// Decimation of images with calibrations
+		SensorData decimatedData = data;
+		decimatedData.setImageRaw(util2d::decimate(decimatedData.imageRaw(), _imageDecimation));
+		decimatedData.setDepthOrRightRaw(util2d::decimate(decimatedData.depthOrRightRaw(), _imageDecimation));
+		std::vector<CameraModel> cameraModels = decimatedData.cameraModels();
+		for(unsigned int i=0; i<cameraModels.size(); ++i)
+		{
+			cameraModels[i] = cameraModels[i].scaled(1.0/double(_imageDecimation));
+		}
+		decimatedData.setCameraModels(cameraModels);
+		StereoCameraModel stereoModel = decimatedData.stereoCameraModel();
+		if(stereoModel.isValidForProjection())
+		{
+			stereoModel.scale(1.0/double(_imageDecimation));
+		}
+		decimatedData.setStereoCameraModel(stereoModel);
+
+		// compute transform
+		t = this->computeTransform(decimatedData, guess, info);
+
+		// transform back the keypoints in the original image
+		std::vector<cv::KeyPoint> kpts = decimatedData.keypoints();
+		double log2value = log(double(_imageDecimation))/log(2.0);
+		for(unsigned int i=0; i<kpts.size(); ++i)
+		{
+			kpts[i].pt.x *= _imageDecimation;
+			kpts[i].pt.y *= _imageDecimation;
+			kpts[i].size *= _imageDecimation;
+			kpts[i].octave += log2value;
+		}
+		data.setFeatures(kpts, decimatedData.descriptors());
+
+		if(info)
+		{
+			UASSERT(info->newCorners.size() == info->refCorners.size());
+			for(unsigned int i=0; i<info->newCorners.size(); ++i)
+			{
+				info->refCorners[i].x *= _imageDecimation;
+				info->refCorners[i].y *= _imageDecimation;
+				info->newCorners[i].x *= _imageDecimation;
+				info->newCorners[i].y *= _imageDecimation;
+			}
+			for(std::multimap<int, cv::KeyPoint>::iterator iter=info->words.begin(); iter!=info->words.end(); ++iter)
+			{
+				iter->second.pt.x *= _imageDecimation;
+				iter->second.pt.y *= _imageDecimation;
+				iter->second.size *= _imageDecimation;
+				iter->second.octave += log2value;
+			}
+		}
+	}
+	else
+	{
+		t = this->computeTransform(data, guess, info);
+	}

 	if(info)
 	{
@@ -339,15 +401,7 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
 			else if(!_holonomic)
 			{
 				// arc trajectory around ICR
-				float tmpY = vyaw!=0.0f ? vx / tan((CV_PI-vyaw)/2.0f) : 0.0f;
-				if(fabs(tmpY) < fabs(vy) || (tmpY<=0 && vy >=0) || (tmpY>=0 && vy<=0))
-				{
-					vy = tmpY;
-				}
-				else
-				{
-					vyaw = (atan(vx/vy)*2.0f-CV_PI)*-1;
-				}
+				vy = vyaw!=0.0f ? vx / tan((CV_PI-vyaw)/2.0f) : 0.0f;
 				if(_force3DoF)
 				{
 					vz = 0.0f;
@@ -39,8 +39,7 @@ namespace rtabmap {
 OdometryF2F::OdometryF2F(const ParametersMap & parameters) :
 	Odometry(parameters),
 	keyFrameThr_(Parameters::defaultOdomKeyFrameThr()),
-	scanKeyFrameThr_(Parameters::defaultOdomScanKeyFrameThr()),
-	motionSinceLastKeyFrame_(Transform::getIdentity())
+	scanKeyFrameThr_(Parameters::defaultOdomScanKeyFrameThr())
 {
 	registrationPipeline_ = Registration::create(parameters);
 	Parameters::parse(parameters, Parameters::kOdomKeyFrameThr(), keyFrameThr_);
@@ -58,7 +57,7 @@ void OdometryF2F::reset(const Transform & initialPose)
 {
 	Odometry::reset(initialPose);
 	refFrame_ = Signature();
-	motionSinceLastKeyFrame_.setIdentity();
+	lastKeyFramePose_.setNull();
 }

 // return not null transform if odometry is correctly computed
@@ -83,6 +82,13 @@ Transform OdometryF2F::computeTransform(

 	RegistrationInfo regInfo;

+	UASSERT(!this->getPose().isNull());
+	if(lastKeyFramePose_.isNull())
+	{
+		lastKeyFramePose_ = this->getPose(); // reset to current pose
+	}
+	Transform motionSinceLastKeyFrame = lastKeyFramePose_.inverse()*this->getPose();
+
 	Signature newFrame(data);
 	if(refFrame_.sensorData().isValid())
 	{
@@ -90,7 +96,7 @@ Transform OdometryF2F::computeTransform(
 		output = registrationPipeline_->computeTransformationMod(
 				tmpRefFrame,
 				newFrame,
-				!guess.isNull()?motionSinceLastKeyFrame_*guess:Transform(),
+				!guess.isNull()?motionSinceLastKeyFrame*guess:Transform(),
 				&regInfo);

 		if(info && this->isInfoDataFilled())
@@ -117,7 +123,7 @@ Transform OdometryF2F::computeTransform(
 				info->cornerInliers[i] = idToIndex.at(regInfo.inliersIDs[i]);
 			}

-			Transform t = this->getPose()*motionSinceLastKeyFrame_.inverse();
+			Transform t = this->getPose()*motionSinceLastKeyFrame.inverse();
 			for(std::multimap<int, cv::Point3f>::const_iterator iter=tmpRefFrame.getWords3().begin(); iter!=tmpRefFrame.getWords3().end(); ++iter)
 			{
 				info->localMap.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, t)));
@@ -135,8 +141,7 @@ Transform OdometryF2F::computeTransform(

 	if(!output.isNull())
 	{
-		output = motionSinceLastKeyFrame_.inverse() * output;
-		motionSinceLastKeyFrame_ *= output;
+		output = motionSinceLastKeyFrame.inverse() * output;

 		// new key-frame?
 		if( (registrationPipeline_->isImageRequired() && (keyFrameThr_ == 0 || float(regInfo.inliers) <= keyFrameThr_*float(refFrame_.sensorData().keypoints().size()))) ||
@@ -167,7 +172,7 @@ Transform OdometryF2F::computeTransform(
 				refFrame_.setWordsDescriptors(std::multimap<int, cv::Mat>());

 				//reset motion
-				motionSinceLastKeyFrame_.setIdentity();
+				lastKeyFramePose_.setNull();
 			}
 			else
 			{
@@ -31,6 +31,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/core/Optimizer.h>
 #include <rtabmap/core/Graph.h>
+#include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/core/RegistrationVis.h>
 #include <set>
 #include <queue>

@@ -66,11 +68,10 @@ Optimizer * Optimizer::create(const ParametersMap & parameters)
 {
 	int optimizerTypeInt = Parameters::defaultOptimizerStrategy();
 	Parameters::parse(parameters, Parameters::kOptimizerStrategy(), optimizerTypeInt);
-	Optimizer::Type type = (Optimizer::Type)optimizerTypeInt;
-	return create(type, parameters);
+	return create((Optimizer::Type)optimizerTypeInt, parameters);
 }

-Optimizer * Optimizer::create(Optimizer::Type & type, const ParametersMap & parameters)
+Optimizer * Optimizer::create(Optimizer::Type type, const ParametersMap & parameters)
 {
 	UASSERT_MSG(OptimizerG2O::available() || OptimizerGTSAM::available() || OptimizerTORO::available(),
 			"RTAB-Map is not built with any graph optimization approach!");
@@ -270,4 +271,76 @@ void Optimizer::getConnectedGraph(
 	}
 }

+
+void Optimizer::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>
+{
+	int wordCount = 0;
+	int edgeWithWordsAdded = 0;
+	for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
+	{
+		Link link = iter->second;
+		if(link.to() < link.from())
+		{
+			link = link.inverse();
+		}
+		if(uContains(signatures, link.from()) &&
+		   uContains(signatures, link.to()) &&
+		   uContains(poses, link.from()))
+		{
+			Signature sFrom = signatures.at(link.from());
+			Signature sTo = signatures.at(link.to());
+
+			if(sFrom.getWords().size() &&
+				sTo.getWords().size() &&
+				sFrom.getWords3().size())
+			{
+				ParametersMap regParam;
+				regParam.insert(ParametersPair(Parameters::kVisEstimationType(), "1"));
+				regParam.insert(ParametersPair(Parameters::kVisPnPReprojError(), "5"));
+				regParam.insert(ParametersPair(Parameters::kVisMinInliers(), "5"));
+				regParam.insert(ParametersPair(Parameters::kVisCorNNDR(), "0.6"));
+				RegistrationVis reg(regParam);
+
+				//sFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
+				//sTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
+
+				RegistrationInfo info;
+				Transform t = reg.computeTransformationMod(sFrom, sTo, Transform(), &info);
+				//Transform t = reg.computeTransformationMod(sFrom, sTo, iter->second.transform(), &info);
+				UDEBUG("%d->%d, inliers=%d",sFrom.id(), sTo.id(), (int)info.inliersIDs.size());
+
+				if(!t.isNull())
+				{
+					Transform pose = poses.at(sFrom.id());
+					for(unsigned int i=0; i<info.inliersIDs.size(); ++i)
+					{
+						cv::Point3f p = sFrom.getWords3().lower_bound(info.inliersIDs[i])->second;
+						if(p.x > 0.0f) // make sure the point is valid
+						{
+							int wordId = ++wordCount;
+
+							p = util3d::transformPoint(p, pose);
+							points3DMap.insert(std::make_pair(wordId, p));
+							wordReferences.insert(std::make_pair(wordId, std::map<int, cv::Point2f>()));
+							wordReferences.at(wordId).insert(std::make_pair(sFrom.id(), sFrom.getWords().lower_bound(info.inliersIDs[i])->second.pt));
+							wordReferences.at(wordId).insert(std::make_pair(sTo.id(), sTo.getWords().lower_bound(info.inliersIDs[i])->second.pt));
+						}
+					}
+					++edgeWithWordsAdded;
+				}
+				else
+				{
+					UWARN("Not enough inliers (%d) between %d and %d", info.inliersIDs.size(), sFrom.id(), sTo.id());
+				}
+			}
+		}
+	}
+	UDEBUG("Added %d words (edges with words=%d/%d)", wordCount, edgeWithWordsAdded, links.size());
+}
+
 } /* namespace rtabmap */
@@ -69,7 +69,7 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
 	params.iterations = this->iterations();
 	params.minError = this->epsilon();
 	params.fixedIntrinsics = 5;
-	params.fixedDistortion = 5;
+	params.fixedDistortion = 5; // updated below
 	params.verbose=ULogger::level() <= ULogger::kInfo;
 	sba.setParams(params);

@@ -110,7 +110,15 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
 			frameIdToIndex.insert(std::make_pair(iter->first, oi));

 			cameraMatrix[oi] = model.K();
-			distCoeffs[oi] = model.D();
+			if(model.D().cols != 5)
+			{
+				distCoeffs[oi] = cv::Mat::zeros(1, 5, CV_64FC1);
+				UWARN("Camera model %d: Distortion coefficients are not 5, setting all them to 0 (assuming no distortion)", iter->first);
+			}
+			else
+			{
+				distCoeffs[oi] = model.D();
+			}

 			Transform t = (iter->second * model.localTransform()).inverse();

@@ -138,88 +146,28 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
 	distCoeffs.resize(oi);

 	std::map<int, cv::Point3f> points3DMap;
-	std::multimap<int, std::pair<int, cv::Point2f> > wordReferences; // <ID words, IDs frames + keypoint>
-	for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
-	{
-		Link link = iter->second;
-		if(link.to() < link.from())
-		{
-			link = link.inverse();
-		}
-		if(uContains(signatures, link.from()) &&
-		   uContains(signatures, link.to()) &&
-		   uContains(frames, link.from()))
-		{
-			const Signature & sFrom = signatures.at(link.from());
-			const Signature & sTo = signatures.at(link.to());
+	std::map<int, std::map<int, cv::Point2f> > wordReferences; // <ID words, IDs frames + keypoint>
+	computeBACorrespondences(frames, links, signatures, points3DMap, wordReferences);

-			std::vector<int> inliers;
-			Transform t = util3d::estimateMotion3DTo3D(
-					uMultimapToMapUnique(sFrom.getWords3()),
-					uMultimapToMapUnique(sTo.getWords3()),
-					minInliers_,
-					inlierDistance_,
-					100,
-					10,
-					0,
-					0,
-					&inliers);
-
-			if(!t.isNull())
-			{
-				Transform pose = frames.at(sFrom.id());
-				for(unsigned int i=0; i<inliers.size(); ++i)
-				{
-					cv::Point3f p = util3d::transformPoint(sFrom.getWords3().lower_bound(inliers[i])->second, pose);
-					std::map<int, cv::Point3f>::iterator jter = points3DMap.find(inliers[i]);
-					if(jter == points3DMap.end())
-					{
-						points3DMap.insert(std::make_pair(inliers[i], p));
-						wordReferences.insert(std::make_pair(inliers[i], std::make_pair(sFrom.id(), sFrom.getWords().lower_bound(inliers[i])->second.pt)));
-						wordReferences.insert(std::make_pair(inliers[i], std::make_pair(sTo.id(), sTo.getWords().lower_bound(inliers[i])->second.pt)));
-					}
-					else
-					{
-						float dist = uNorm(p.x - jter->second.x, p.y - jter->second.y, p.z - jter->second.z);
-						if(dist <= inlierDistance_)
-						{
-							// in case of loop closure links
-							wordReferences.insert(std::make_pair(inliers[i], std::make_pair(sFrom.id(), sFrom.getWords().lower_bound(inliers[i])->second.pt)));
-							wordReferences.insert(std::make_pair(inliers[i], std::make_pair(sTo.id(), sTo.getWords().lower_bound(inliers[i])->second.pt)));
-						}
-					}
-				}
-			}
-			else
-			{
-				UWARN("Not enough inliers (%d) between %d and %d", inliers.size(), sFrom.id(), sTo.id());
-			}
-		}
-	}
-
-	std::list<int> wordReferencesKeys = uUniqueKeys(wordReferences);
-	UDEBUG("points=%d frames=%d", (int)wordReferencesKeys.size(), (int)frames.size());
-	std::vector<cv::Point3f> points(wordReferencesKeys.size()); //npoints
+	UDEBUG("points=%d frames=%d", (int)wordReferences.size(), (int)frames.size());
+	std::vector<cv::Point3f> points(wordReferences.size()); //npoints
 	std::vector<std::vector<cv::Point2f> >  imagePoints(frames.size()); //nframes -> npoints
 	std::vector<std::vector<int> > visibility(frames.size()); //nframes -> npoints
 	for(unsigned int i=0; i<frames.size(); ++i)
 	{
-		imagePoints[i].resize(wordReferencesKeys.size(), cv::Point2f(std::numeric_limits<float>::quiet_NaN(), std::numeric_limits<float>::quiet_NaN()));
-		visibility[i].resize(wordReferencesKeys.size(), 0);
+		imagePoints[i].resize(wordReferences.size(), cv::Point2f(std::numeric_limits<float>::quiet_NaN(), std::numeric_limits<float>::quiet_NaN()));
+		visibility[i].resize(wordReferences.size(), 0);
 	}
 	int i=0;
-	for(std::list<int>::iterator iter = wordReferencesKeys.begin(); iter!=wordReferencesKeys.end(); ++iter)
+	for(std::map<int, std::map<int, cv::Point2f> >::iterator iter = wordReferences.begin(); iter!=wordReferences.end(); ++iter)
 	{
-		points[i] = points3DMap.at(*iter);
+		points[i] = points3DMap.at(iter->first);

-		std::multimap<int, std::pair<int, cv::Point2f> >::iterator jter = wordReferences.lower_bound(*iter);
-		while(jter->first == *iter && jter != wordReferences.end())
+		for(std::map<int, cv::Point2f>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
 		{
-			imagePoints[frameIdToIndex.at(jter->second.first)][i] = jter->second.second;
-			visibility[frameIdToIndex.at(jter->second.first)][i] = 1;
-			++jter;
+			imagePoints[frameIdToIndex.at(jter->first)][i] = jter->second;
+			visibility[frameIdToIndex.at(jter->first)][i] = 1;
 		}
-
 		++i;
 	}

@@ -33,6 +33,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <set>

 #include <rtabmap/core/OptimizerG2O.h>
+#include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/core/util3d_motion_estimation.h>

 #ifdef RTABMAP_G2O
 #include "g2o/config.h"
@@ -42,6 +44,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "g2o/core/optimization_algorithm_factory.h"
 #include "g2o/core/optimization_algorithm_gauss_newton.h"
 #include "g2o/core/optimization_algorithm_levenberg.h"
+#include "g2o/core/linear_solver.h"
+#include "g2o/types/sba/types_sba.h"
+#include "g2o/core/robust_kernel_impl.h"
 #ifdef G2O_HAVE_CSPARSE
 #include "g2o/solvers/csparse/linear_solver_csparse.h"
 #endif
@@ -107,6 +112,8 @@ void OptimizerG2O::parseParameters(const ParametersMap & parameters)

 	Parameters::parse(parameters, Parameters::kg2oSolver(), solver_);
 	Parameters::parse(parameters, Parameters::kg2oOptimizer(), optimizer_);
+	Parameters::parse(parameters, Parameters::kg2oPixelVariance(), pixelVariance_);
+	UASSERT(pixelVariance_ > 0.0);

 #ifndef G2O_HAVE_CHOLMOD
 	if(solver_ == 2)
@@ -152,12 +159,10 @@ std::map<int, Transform> OptimizerG2O::optimize(

 		g2o::SparseOptimizer optimizer;
 		optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
-		int solverApproach = 0;
-		int optimizationApproach = 1;

 		SlamBlockSolver * blockSolver = 0;

-		if(solverApproach == 2)
+		if(solver_ == 2)
 		{
 #ifdef G2O_HAVE_CHOLMOD
 			//chmold
@@ -166,7 +171,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
 			blockSolver = new SlamBlockSolver(linearSolver);
 #endif
 		}
-		else if(solverApproach == 0)
+		else if(solver_ == 0)
 		{
 #ifdef G2O_HAVE_CSPARSE
 			//csparse
@@ -183,7 +188,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
 			blockSolver = new SlamBlockSolver(linearSolver);
 		}

-		if(optimizationApproach == 1)
+		if(optimizer_ == 1)
 		{
 			optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(blockSolver));
 		}
@@ -538,6 +543,309 @@ std::map<int, Transform> OptimizerG2O::optimize(
 	return optimizedPoses;
 }

+std::map<int, Transform> OptimizerG2O::optimizeBA(
+		int rootId,
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & links,
+		const std::map<int, Signature> & signatures)
+{
+	std::map<int, Transform> optimizedPoses;
+#ifdef RTABMAP_G2O
+	UDEBUG("Optimizing graph...");
+
+	optimizedPoses.clear();
+	if(links.size()>=1 && poses.size()>=2 && iterations() > 0)
+	{
+		g2o::SparseOptimizer optimizer;
+		optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
+		g2o::BlockSolver_6_3::LinearSolverType * linearSolver = 0;
+		bool robustKernel = true;
+
+		if(solver_ == 2)
+		{
+#ifdef G2O_HAVE_CHOLMOD
+			//chmold
+			linearSolver = new g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>();
+#endif
+		}
+		else if(solver_ == 0)
+		{
+#ifdef G2O_HAVE_CSPARSE
+			//csparse
+			linearSolver = new g2o::LinearSolverCSparse<g2o::BlockSolver_6_3::PoseMatrixType>();
+#endif
+		}
+
+		if(linearSolver == 0)
+		{
+			//pcg
+			linearSolver = new g2o::LinearSolverPCG<g2o::BlockSolver_6_3::PoseMatrixType>();
+		}
+
+		g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
+
+		if(optimizer_ == 1)
+		{
+			optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(solver_ptr));
+		}
+		else
+		{
+			optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(solver_ptr));
+		}
+
+		std::map<int, Transform> frames = poses;
+
+		UDEBUG("fill poses to g2o...");
+		std::map<int, CameraModel> models;
+		for(std::map<int, Transform>::iterator iter=frames.begin(); iter!=frames.end(); )
+		{
+			// Get camera model
+			CameraModel model;
+			if(uContains(signatures, iter->first))
+			{
+				if(signatures.at(iter->first).sensorData().cameraModels().size() == 1 && signatures.at(iter->first).sensorData().cameraModels().at(0).isValidForProjection())
+				{
+					model = signatures.at(iter->first).sensorData().cameraModels()[0];
+				}
+				else if(signatures.at(iter->first).sensorData().stereoCameraModel().isValidForProjection())
+				{
+					model = signatures.at(iter->first).sensorData().stereoCameraModel().left();
+				}
+				else
+				{
+					UERROR("Missing calibration for node %d", iter->first);
+					return optimizedPoses;
+				}
+			}
+			else
+			{
+				UERROR("Did not find node %d in cache", iter->first);
+			}
+
+			if(model.isValidForProjection())
+			{
+				models.insert(std::make_pair(iter->first, model));
+				Transform camPose = iter->second * model.localTransform();
+				//iter->second = (iter->second * model.localTransform()).inverse();
+				UDEBUG("%d t=%s", iter->first, camPose.prettyPrint().c_str());
+
+				// Add node's pose
+				UASSERT(!camPose.isNull());
+				g2o::VertexCam * vCam = new g2o::VertexCam();
+
+				Eigen::Affine3d a = camPose.toEigen3d();
+				g2o::SBACam cam(Eigen::Quaterniond(a.rotation()), a.translation());
+				cam.setKcam(model.fx(), model.fy(), model.cx(), model.cy(), 0);
+				vCam->setEstimate(cam);
+				if(iter->first == rootId)
+				{
+					vCam->setFixed(true);
+				}
+				vCam->setId(iter->first);
+				std::cout << cam << std::endl;
+				UASSERT_MSG(optimizer.addVertex(vCam), uFormat("cannot insert vertex %d!?", iter->first).c_str());
+
+				++iter;
+			}
+			else
+			{
+				frames.erase(iter++);
+			}
+		}
+
+		UDEBUG("fill edges to g2o and associate each 3D point to all frames observing it...");
+		for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
+		{
+			Link link = iter->second;
+			if(link.to() < link.from())
+			{
+				link = link.inverse();
+			}
+			if(uContains(signatures, link.from()) &&
+			   uContains(signatures, link.to()) &&
+			   uContains(frames, link.from()) &&
+			   uContains(frames, link.to()))
+			{
+				// add edge
+				int id1 = iter->first;
+				int id2 = iter->second.to();
+
+				UASSERT(!iter->second.transform().isNull());
+
+				Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
+				if(!isCovarianceIgnored())
+				{
+					memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
+				}
+
+				// between cameras, not base_link
+				Transform camLink = models.at(id1).localTransform().inverse()*iter->second.transform()*models.at(id2).localTransform();
+				//Transform t = iter->second.transform();
+				UDEBUG("added edge %d=%s -> %d=%s",
+						id1,
+						iter->second.transform().prettyPrint().c_str(),
+						id2,
+						camLink.prettyPrint().c_str());
+				Eigen::Affine3d a = camLink.toEigen3d();
+
+				g2o::EdgeSBACam * e = new g2o::EdgeSBACam();
+				g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
+				g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2);
+				UASSERT(v1 != 0);
+				UASSERT(v2 != 0);
+				e->setVertex(0, v1);
+				e->setVertex(1, v2);
+				e->setMeasurement(g2o::SE3Quat(a.rotation(), a.translation()));
+				e->setInformation(information);
+
+				if (!optimizer.addEdge(e))
+				{
+					delete e;
+					UERROR("Map: Failed adding constraint between %d and %d, skipping", id1, id2);
+					return optimizedPoses;
+				}
+			}
+		}
+
+		std::map<int, cv::Point3f> points3DMap;
+		std::map<int, std::map<int, cv::Point2f> > wordReferences; // <ID words, IDs frames + keypoint>
+		this->computeBACorrespondences(frames, links, signatures, points3DMap, wordReferences);
+
+		UDEBUG("fill 3D points to g2o...");
+		int stepVertexId = frames.rbegin()->first+1;
+		for(std::map<int, std::map<int, cv::Point2f> >::iterator iter = wordReferences.begin(); iter!=wordReferences.end(); ++iter)
+		{
+			const cv::Point3f & pt3d = points3DMap.at(iter->first);
+			g2o::VertexSBAPointXYZ* vpt3d = new g2o::VertexSBAPointXYZ();
+
+			vpt3d->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
+			vpt3d->setId(stepVertexId + iter->first);
+			vpt3d->setMarginalized(true);
+			optimizer.addVertex(vpt3d);
+
+			// set observations
+			for(std::map<int, cv::Point2f>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
+			{
+				int camId = jter->first;
+
+				const cv::Point2f & pt = jter->second;
+
+				Eigen::Matrix<double,2,1> obs;
+				obs << pt.x, pt.y;
+
+				UDEBUG("Added observation pt=%d to cam=%d (%f,%f)", vpt3d->id(), camId, pt.x, pt.y);
+
+				g2o::EdgeProjectP2MC* e = new g2o::EdgeProjectP2MC();
+
+				e->setVertex(0, vpt3d);
+				e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(camId)));
+				e->setMeasurement(obs);
+				e->setInformation(Eigen::Matrix2d::Identity() / pixelVariance_);
+
+				if(robustKernel)
+				{
+					e->setRobustKernel(new g2o::RobustKernelHuber);
+				}
+
+				optimizer.addEdge(e);
+			}
+		}
+
+		UDEBUG("Initial optimization...");
+		optimizer.initializeOptimization();
+
+		UASSERT(optimizer.verifyInformationMatrices());
+
+		UINFO("g2o optimizing begin (max iterations=%d, epsilon=%f robustKernel=%d)", iterations(), this->epsilon(), robustKernel?1:0);
+
+		int it = 0;
+		UTimer timer;
+		double lastError = 0.0;
+		if(this->epsilon() > 0.0)
+		{
+			for(int i=0; i<iterations(); ++i)
+			{
+				it += optimizer.optimize(1);
+
+				// early stop condition
+				optimizer.computeActiveErrors();
+				double chi2 = optimizer.activeRobustChi2();
+				UDEBUG("iteration %d: %d nodes, %d edges, chi2: %f", i, (int)optimizer.vertices().size(), (int)optimizer.edges().size(), chi2);
+
+				if(i>0 && (optimizer.activeRobustChi2() > 1000000000000.0 || !uIsFinite(optimizer.activeRobustChi2())))
+				{
+					UWARN("g2o: Large optimization error detected (%f), aborting optimization!");
+					return optimizedPoses;
+				}
+
+				double errorDelta = lastError - chi2;
+				if(i>0 && errorDelta < this->epsilon())
+				{
+					if(errorDelta < 0)
+					{
+						UDEBUG("Negative improvement?! Ignore and continue optimizing... (%f < %f)", errorDelta, this->epsilon());
+					}
+					else
+					{
+						UINFO("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
+						break;
+					}
+				}
+				else if(i==0 && chi2 < this->epsilon())
+				{
+					UINFO("Stop optimizing, error is already under epsilon (%f < %f)", chi2, this->epsilon());
+					break;
+				}
+				lastError = chi2;
+			}
+		}
+		else
+		{
+			it = optimizer.optimize(iterations());
+			optimizer.computeActiveErrors();
+			UDEBUG("%d nodes, %d edges, chi2: %f", (int)optimizer.vertices().size(), (int)optimizer.edges().size(), optimizer.activeRobustChi2());
+		}
+		UINFO("g2o optimizing end (%d iterations done, error=%f, time = %f s)", it, optimizer.activeRobustChi2(), timer.ticks());
+
+		if(optimizer.activeRobustChi2() > 1000000000000.0)
+		{
+			UWARN("g2o: Large optimimzation error detected (%f), aborting optimization!");
+			return optimizedPoses;
+		}
+
+		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+		{
+			const g2o::VertexCam* v = (const g2o::VertexCam*)optimizer.vertex(iter->first);
+			if(v)
+			{
+				Transform t = Transform::fromEigen3d(v->estimate());
+				UDEBUG("%d t=%s", iter->first, t.prettyPrint().c_str());
+				// remove model local transform
+				t *= models.at(iter->first).localTransform().inverse();
+				optimizedPoses.insert(std::pair<int, Transform>(iter->first, t));
+				UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
+			}
+			else
+			{
+				UERROR("Vertex %d not found!?", iter->first);
+			}
+		}
+	}
+	else if(poses.size() == 1 || iterations() <= 0)
+	{
+		optimizedPoses = poses;
+	}
+	else
+	{
+		UWARN("This method should be called at least with 1 pose!");
+	}
+	UDEBUG("Optimizing graph...end!");
+#else
+	UERROR("Not built with G2O support!");
+#endif
+	return optimizedPoses;
+}
+
 bool OptimizerG2O::saveGraph(
 		const std::string & fileName,
 		const std::map<int, Transform> & poses,
@@ -262,7 +262,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 			}
 			catch(gtsam::IndeterminantLinearSystemException & e)
 			{
-				UERROR("GTSAM exception catched: %s", e.what());
+				UERROR("GTSAM exception caught: %s", e.what());
 				return optimizedPoses;
 			}

@@ -155,11 +155,20 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 	{
 		// removed parameters

+		// 0.11.8
+		removedParameters_.insert(std::make_pair("Reg/Force2D",                   std::make_pair(true,  Parameters::kRegForce3DoF())));
+
+		// 0.11.6
+		removedParameters_.insert(std::make_pair("RGBD/ProximityPathScansMerged", std::make_pair(false,  "")));
+
+		// 0.11.3
+		removedParameters_.insert(std::make_pair("Mem/ImageDecimation",           std::make_pair(true, Parameters::kMemImagePostDecimation())));
+
 		// 0.11.2
 		removedParameters_.insert(std::make_pair("OdomLocalMap/HistorySize",      std::make_pair(true, Parameters::kOdomF2MMaxSize())));
 		removedParameters_.insert(std::make_pair("OdomLocalMap/FixedMapPath",     std::make_pair(true, Parameters::kOdomF2MFixedMapPath())));
 		removedParameters_.insert(std::make_pair("OdomF2F/GuessMotion",           std::make_pair(true, Parameters::kOdomGuessMotion())));
-		removedParameters_.insert(std::make_pair("OdomF2F/KeyFrameThr",          std::make_pair(false, Parameters::kOdomKeyFrameThr())));
+		removedParameters_.insert(std::make_pair("OdomF2F/KeyFrameThr",           std::make_pair(false, Parameters::kOdomKeyFrameThr())));

 		// 0.11.0
 		removedParameters_.insert(std::make_pair("OdomBow/LocalHistorySize",      std::make_pair(true, Parameters::kOdomF2MMaxSize())));
@@ -241,7 +250,7 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 		removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionBySpace",             std::make_pair(true,  Parameters::kRGBDProximityBySpace())));
 		removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionTime",                std::make_pair(true,  Parameters::kRGBDProximityByTime())));
 		removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionSpace",               std::make_pair(true,  Parameters::kRGBDProximityBySpace())));
-		removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionPathScansMerged",     std::make_pair(true,  Parameters::kRGBDProximityPathScansMerged())));
+		removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionPathScansMerged",     std::make_pair(false,  "")));
 		removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionMaxGraphDepth",       std::make_pair(true,  Parameters::kRGBDProximityMaxGraphDepth())));
 		removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionPathFilteringRadius", std::make_pair(true,  Parameters::kRGBDProximityPathFilteringRadius())));
 		removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionPathRawPosesUsed",    std::make_pair(true,  Parameters::kRGBDProximityPathRawPosesUsed())));
@@ -186,6 +186,11 @@ Transform Registration::computeTransformationMod(
 		info = *infoOut;
 	}

+	if(!guess.isNull() && force3DoF_)
+	{
+		guess = guess.to3DoF();
+	}
+
 	Transform t = computeTransformationImpl(from, to, guess, info);

 	if(varianceFromInliersCount_)
@@ -207,6 +212,11 @@ Transform Registration::computeTransformationMod(
 		{
 			t = child_->computeTransformationMod(from, to, force3DoF_?t.to3DoF():t, &info);
 		}
+		else if(!guess.isNull())
+		{
+			UDEBUG("This registration approach failed, continue with the guess for the next registration");
+			t = child_->computeTransformationMod(from, to, guess, &info);
+		}
 	}
 	else if(!t.isNull() && force3DoF_)
 	{
@@ -113,7 +113,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 	if(!guess.isNull() && !dataFrom.laserScanRaw().empty() && !dataTo.laserScanRaw().empty())
 	{
 		// ICP with guess transform
-		int maxLaserScans = dataTo.laserScanMaxPts();
+		int maxLaserScans = dataTo.laserScanMaxPts()?dataTo.laserScanMaxPts():dataFrom.laserScanMaxPts();
 		cv::Mat fromScan = dataFrom.laserScanRaw();
 		cv::Mat toScan = dataTo.laserScanRaw();
 		if(_downsamplingStep>1)
@@ -309,8 +309,13 @@ Transform RegistrationIcp::computeTransformationImpl(
 					}
 					else
 					{
-						UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set relative instead of absolute!",
-								dataTo.id());
+						static bool warningShown = false;
+						if(!warningShown)
+						{
+							UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set relative instead of absolute! This message will only appear once.",
+									dataTo.id());
+							warningShown = true;
+						}
 						correspondencesRatio = float(correspondences)/float(toScan.cols>fromScan.cols?toScan.cols:fromScan.cols);
 					}

@@ -243,33 +243,36 @@ Transform RegistrationVis::computeTransformationImpl(

 		Feature2D * detector = createFeatureDetector();
 		std::vector<cv::KeyPoint> kptsFrom;
+		cv::Mat imageFrom = fromSignature.sensorData().imageRaw();
+		cv::Mat imageTo = toSignature.sensorData().imageRaw();
+
 		if(fromSignature.getWords().empty())
 		{
 			if(fromSignature.sensorData().keypoints().empty())
 			{
-				if(!fromSignature.sensorData().imageRaw().empty())
+				if(!imageFrom.empty())
 				{
-					if(fromSignature.sensorData().imageRaw().channels() > 1)
+					if(imageFrom.channels() > 1)
 					{
 						cv::Mat tmp;
-						cv::cvtColor(fromSignature.sensorData().imageRaw(), tmp, cv::COLOR_BGR2GRAY);
-						fromSignature.sensorData().setImageRaw(tmp);
+						cv::cvtColor(imageFrom, tmp, cv::COLOR_BGR2GRAY);
+						imageFrom = tmp;
 					}

 					cv::Mat depthMask;
 					if(!fromSignature.sensorData().depthRaw().empty() &&
 					   detector->getType() != Feature2D::kFeatureOrb) // ORB's mask pyramids don't seem to work well
 					{
-						if(fromSignature.sensorData().imageRaw().rows % fromSignature.sensorData().depthRaw().rows == 0 &&
-							fromSignature.sensorData().imageRaw().cols % fromSignature.sensorData().depthRaw().cols == 0 &&
-							fromSignature.sensorData().imageRaw().rows/fromSignature.sensorData().depthRaw().rows == fromSignature.sensorData().imageRaw().cols/fromSignature.sensorData().depthRaw().cols)
+						if(imageFrom.rows % fromSignature.sensorData().depthRaw().rows == 0 &&
+						   imageFrom.cols % fromSignature.sensorData().depthRaw().cols == 0 &&
+						   imageFrom.rows/fromSignature.sensorData().depthRaw().rows == fromSignature.sensorData().imageRaw().cols/fromSignature.sensorData().depthRaw().cols)
 						{
 							depthMask = util2d::interpolate(fromSignature.sensorData().depthRaw(), fromSignature.sensorData().imageRaw().rows/fromSignature.sensorData().depthRaw().rows, 0.1f);
 						}
 					}

 					kptsFrom = detector->generateKeypoints(
-							fromSignature.sensorData().imageRaw(),
+							imageFrom,
 							depthMask);
 				}
 			}
@@ -290,22 +293,22 @@ Transform RegistrationVis::computeTransformationImpl(
 		std::multimap<int, cv::Mat> wordsDescFrom;
 		std::multimap<int, cv::Mat> wordsDescTo;
 		if(_correspondencesApproach == 1 && //Optical Flow
-		   !fromSignature.sensorData().imageRaw().empty() &&
-		   !toSignature.sensorData().imageRaw().empty())
+		   !imageFrom.empty() &&
+		   !imageTo.empty())
 		{
 			UDEBUG("");
 			// convert to grayscale
-			if(fromSignature.sensorData().imageRaw().channels() > 1)
+			if(imageFrom.channels() > 1)
 			{
 				cv::Mat tmp;
-				cv::cvtColor(fromSignature.sensorData().imageRaw(), tmp, cv::COLOR_BGR2GRAY);
-				fromSignature.sensorData().setImageRaw(tmp);
+				cv::cvtColor(imageFrom, tmp, cv::COLOR_BGR2GRAY);
+				imageFrom = tmp;
 			}
-			if(toSignature.sensorData().imageRaw().channels() > 1)
+			if(imageTo.channels() > 1)
 			{
 				cv::Mat tmp;
-				cv::cvtColor(toSignature.sensorData().imageRaw(), tmp, cv::COLOR_BGR2GRAY);
-				toSignature.sensorData().setImageRaw(tmp);
+				cv::cvtColor(imageTo, tmp, cv::COLOR_BGR2GRAY);
+				imageTo = tmp;
 			}

 			std::vector<cv::Point3f> kptsFrom3D;
@@ -318,7 +321,7 @@ Transform RegistrationVis::computeTransformationImpl(
 				kptsFrom3D = uValues(fromSignature.getWords3());
 			}

-			if(!toSignature.sensorData().imageRaw().empty())
+			if(!imageTo.empty())
 			{
 				std::vector<cv::Point2f> cornersFrom;
 				cv::KeyPoint::convert(kptsFrom, cornersFrom);
@@ -345,8 +348,8 @@ Transform RegistrationVis::computeTransformationImpl(
 				std::vector<float> err;
 				UDEBUG("cv::calcOpticalFlowPyrLK() begin");
 				cv::calcOpticalFlowPyrLK(
-						fromSignature.sensorData().imageRaw(),
-						toSignature.sensorData().imageRaw(),
+						imageFrom,
+						imageTo,
 						cornersFrom,
 						cornersTo,
 						status,
@@ -364,8 +367,8 @@ Transform RegistrationVis::computeTransformationImpl(
 				for(unsigned int i=0; i<status.size(); ++i)
 				{
 					if(status[i] &&
-					   uIsInBounds(cornersTo[i].x, 0.0f, float(toSignature.sensorData().imageRaw().cols)) &&
-					   uIsInBounds(cornersTo[i].y, 0.0f, float(toSignature.sensorData().imageRaw().rows)))
+					   uIsInBounds(cornersTo[i].x, 0.0f, float(imageTo.cols)) &&
+					   uIsInBounds(cornersTo[i].y, 0.0f, float(imageTo.rows)))
 					{
 						kptsFrom[ki] = cv::KeyPoint(cornersFrom[i], 1);
 						kptsFrom3DKept[ki] = kptsFrom3D[i];
@@ -420,29 +423,29 @@ Transform RegistrationVis::computeTransformationImpl(
 			if(toSignature.getWords().empty())
 			{
 				if(toSignature.sensorData().keypoints().empty() &&
-				   !toSignature.sensorData().imageRaw().empty())
+				   !imageTo.empty())
 				{
-					if(toSignature.sensorData().imageRaw().channels() > 1)
+					if(imageTo.channels() > 1)
 					{
 						cv::Mat tmp;
-						cv::cvtColor(toSignature.sensorData().imageRaw(), tmp, cv::COLOR_BGR2GRAY);
-						toSignature.sensorData().setImageRaw(tmp);
+						cv::cvtColor(imageTo, tmp, cv::COLOR_BGR2GRAY);
+						imageTo = tmp;
 					}

 					cv::Mat depthMask;
 					if(!toSignature.sensorData().depthRaw().empty() &&
 						detector->getType() != Feature2D::kFeatureOrb) // ORB's mask pyramids don't seem to work well
 					{
-						if(toSignature.sensorData().imageRaw().rows % toSignature.sensorData().depthRaw().rows == 0 &&
-							toSignature.sensorData().imageRaw().cols % toSignature.sensorData().depthRaw().cols == 0 &&
-							toSignature.sensorData().imageRaw().rows/toSignature.sensorData().depthRaw().rows == toSignature.sensorData().imageRaw().cols/toSignature.sensorData().depthRaw().cols)
+						if(imageTo.rows % toSignature.sensorData().depthRaw().rows == 0 &&
+						   imageTo.cols % toSignature.sensorData().depthRaw().cols == 0 &&
+						   imageTo.rows/toSignature.sensorData().depthRaw().rows == imageTo.cols/toSignature.sensorData().depthRaw().cols)
 						{
-							depthMask = util2d::interpolate(toSignature.sensorData().depthRaw(), toSignature.sensorData().imageRaw().rows/toSignature.sensorData().depthRaw().rows, 0.1f);
+							depthMask = util2d::interpolate(toSignature.sensorData().depthRaw(), imageTo.rows/toSignature.sensorData().depthRaw().rows, 0.1f);
 						}
 					}

 					kptsTo = detector->generateKeypoints(
-							toSignature.sensorData().imageRaw(),
+							imageTo,
 							depthMask);
 				}
 				else
@@ -477,15 +480,15 @@ Transform RegistrationVis::computeTransformationImpl(
 			{
 				descriptorsFrom = fromSignature.sensorData().descriptors();
 			}
-			else if(!fromSignature.sensorData().imageRaw().empty())
+			else if(!imageFrom.empty())
 			{
-				if(fromSignature.sensorData().imageRaw().channels() > 1)
+				if(imageFrom.channels() > 1)
 				{
 					cv::Mat tmp;
-					cv::cvtColor(fromSignature.sensorData().imageRaw(), tmp, cv::COLOR_BGR2GRAY);
-					fromSignature.sensorData().setImageRaw(tmp);
+					cv::cvtColor(imageFrom, tmp, cv::COLOR_BGR2GRAY);
+					imageFrom = tmp;
 				}
-				descriptorsFrom = detector->generateDescriptors(fromSignature.sensorData().imageRaw(), kptsFrom);
+				descriptorsFrom = detector->generateDescriptors(imageFrom, kptsFrom);
 			}

 			cv::Mat descriptorsTo;
@@ -508,16 +511,16 @@ Transform RegistrationVis::computeTransformationImpl(
 				{
 					descriptorsTo = toSignature.sensorData().descriptors();
 				}
-				else if(!toSignature.sensorData().imageRaw().empty())
+				else if(!imageTo.empty())
 				{
-					if(toSignature.sensorData().imageRaw().channels() > 1)
+					if(imageTo.channels() > 1)
 					{
 						cv::Mat tmp;
-						cv::cvtColor(toSignature.sensorData().imageRaw(), tmp, cv::COLOR_BGR2GRAY);
-						toSignature.sensorData().setImageRaw(tmp);
+						cv::cvtColor(imageTo, tmp, cv::COLOR_BGR2GRAY);
+						imageTo = tmp;
 					}

-					descriptorsTo = detector->generateDescriptors(toSignature.sensorData().imageRaw(), kptsTo);
+					descriptorsTo = detector->generateDescriptors(imageTo, kptsTo);
 				}
 			}

@@ -618,7 +621,7 @@ Transform RegistrationVis::computeTransformationImpl(
 			// We have all data we need here, so match!
 			if(descriptorsFrom.rows > 0 && descriptorsTo.rows > 0)
 			{
-				cv::Size imageSize = toSignature.sensorData().imageRaw().size();
+				cv::Size imageSize = imageTo.size();
 				bool isCalibrated = false;
 				if(imageSize.height == 0 || imageSize.width == 0)
 				{
@@ -931,7 +934,7 @@ Transform RegistrationVis::computeTransformationImpl(
 	float variance = 1.0f;
 	int inliersCount = 0;
 	int matchesCount = 0;
-	if(toSignature.getWords().size() || !toSignature.sensorData().imageRaw().empty())
+	if(toSignature.getWords().size())
 	{
 		Transform transforms[2];
 		std::vector<int> inliers[2];
@@ -100,7 +100,6 @@ Rtabmap::Rtabmap() :
 	_proximityMaxGraphDepth(Parameters::defaultRGBDProximityMaxGraphDepth()),
 	_proximityFilteringRadius(Parameters::defaultRGBDProximityPathFilteringRadius()),
 	_proximityRawPosesUsed(Parameters::defaultRGBDProximityPathRawPosesUsed()),
-	_proximityScansMerged(Parameters::defaultRGBDProximityPathScansMerged()),
 	_proximityAngle(Parameters::defaultRGBDProximityAngle()*M_PI/180.0f),
 	_databasePath(""),
 	_optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()),
@@ -375,6 +374,11 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 {
 	uInsert(_parameters, parameters);

+	// place this before changing working directory
+	Parameters::parse(parameters, Parameters::kRtabmapStatisticLogsBufferedInRAM(), _statisticLogsBufferedInRAM);
+	Parameters::parse(parameters, Parameters::kRtabmapStatisticLogged(), _statisticLogged);
+	Parameters::parse(parameters, Parameters::kRtabmapStatisticLoggedHeaders(), _statisticLoggedHeaders);
+
 	ULOGGER_DEBUG("");
 	ParametersMap::const_iterator iter;
 	if((iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end())
@@ -393,9 +397,6 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRtabmapMaxRetrieved(), _maxRetrieved);
 	Parameters::parse(parameters, Parameters::kRGBDMaxLocalRetrieved(), _maxLocalRetrieved);
 	Parameters::parse(parameters, Parameters::kMemImageKept(), _rawDataKept);
-	Parameters::parse(parameters, Parameters::kRtabmapStatisticLogsBufferedInRAM(), _statisticLogsBufferedInRAM);
-	Parameters::parse(parameters, Parameters::kRtabmapStatisticLogged(), _statisticLogged);
-	Parameters::parse(parameters, Parameters::kRtabmapStatisticLoggedHeaders(), _statisticLoggedHeaders);
 	Parameters::parse(parameters, Parameters::kRGBDEnabled(), _rgbdSlamMode);
 	Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rgbdLinearUpdate);
 	Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rgbdAngularUpdate);
@@ -409,7 +410,6 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDProximityMaxGraphDepth(), _proximityMaxGraphDepth);
 	Parameters::parse(parameters, Parameters::kRGBDProximityPathFilteringRadius(), _proximityFilteringRadius);
 	Parameters::parse(parameters, Parameters::kRGBDProximityPathRawPosesUsed(), _proximityRawPosesUsed);
-	Parameters::parse(parameters, Parameters::kRGBDProximityPathScansMerged(), _proximityScansMerged);
 	Parameters::parse(parameters, Parameters::kRGBDProximityAngle(), _proximityAngle);
 	_proximityAngle *= M_PI/180.0f;
 	Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
@@ -1011,17 +1011,16 @@ bool Rtabmap::process(
 			else
 			{
 				//============================================================
-				// Scan matching
+				// Refine neighbor links
 				//============================================================
 				if(!signature->sensorData().laserScanCompressed().empty())
 				{
-					UINFO("Odometry correction by scan matching");
-					Transform guess = signature->getLinks().begin()->second.transform().inverse();
+					UINFO("Odometry refining: guess = %s", guess.prettyPrint().c_str());
 					RegistrationInfo info;
-					Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, &info);
+					Transform t = _memory->computeTransform(oldId, signature->id(), guess, &info);
 					if(!t.isNull())
 					{
-						UINFO("Scan matching: update neighbor link (%d->%d, variance=%f) from %s to %s",
+						UINFO("Odometry refining: update neighbor link (%d->%d, variance=%f) from %s to %s",
 								oldId,
 								signature->id(),
 								info.variance,
@@ -1048,7 +1047,7 @@ bool Rtabmap::process(
 					}
 					else
 					{
-						UINFO("Scan matching rejected: %s", info.rejectedMsg.c_str());
+						UINFO("Odometry refining rejected: %s", info.rejectedMsg.c_str());
 						if(info.variance > 0)
 						{
 							double sqrtVar = sqrt(info.variance);
@@ -1063,7 +1062,7 @@ bool Rtabmap::process(
 				}
 			}
 			timeNeighborLinkRefining = timer.ticks();
-			ULOGGER_INFO("timeScanMatching=%fs", timeNeighborLinkRefining);
+			ULOGGER_INFO("timeOdometryRefining=%fs", timeNeighborLinkRefining);

 			UASSERT(oldS->hasLink(signature->id()));
 			UASSERT(uContains(_optimizedPoses, oldId));
@@ -1637,27 +1636,29 @@ bool Rtabmap::process(
 			++iter)
 		{
 			const Signature * s = _memory->getSignature(iter->second);
-			UASSERT(s!=0);
-			// If there is a change of direction, better to be retrieving
-			// ALL nearest signatures than only newest neighbors
-			const std::map<int, Link> & links = s->getLinks();
-			for(std::map<int, Link>::const_reverse_iterator jter=links.rbegin();
-				jter!=links.rend() && retrievalLocalIds.size() < _maxLocalRetrieved;
-				++jter)
+			if(s!=0)
 			{
-				if(_memory->getSignature(jter->first) == 0)
+				// If there is a change of direction, better to be retrieving
+				// ALL nearest signatures than only newest neighbors
+				const std::map<int, Link> & links = s->getLinks();
+				for(std::map<int, Link>::const_reverse_iterator jter=links.rbegin();
+					jter!=links.rend() && retrievalLocalIds.size() < _maxLocalRetrieved;
+					++jter)
 				{
-					UINFO("retrieval of node %d on local map", jter->first);
-					retrievalLocalIds.push_back(jter->first);
+					if(_memory->getSignature(jter->first) == 0)
+					{
+						UINFO("retrieval of node %d on local map", jter->first);
+						retrievalLocalIds.push_back(jter->first);
+					}
 				}
-			}
-			if(!_memory->isInSTM(s->id()) && immunizedLocally < maxLocalLocationsImmunized)
-			{
-				if(immunizedLocations.insert(s->id()).second)
+				if(!_memory->isInSTM(s->id()) && immunizedLocally < maxLocalLocationsImmunized)
 				{
-					++immunizedLocally;
+					if(immunizedLocations.insert(s->id()).second)
+					{
+						++immunizedLocally;
+					}
+					UDEBUG("local node %d (%f m) immunized=1", iter->second, iter->first);
 				}
-				UDEBUG("local node %d (%f m) immunized=1", iter->second, iter->first);
 			}
 		}
 		// well, if the maximum retrieved is not reached, look for neighbors in database
@@ -1833,7 +1834,8 @@ bool Rtabmap::process(
 							 _optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _proximityFilteringRadius*_proximityFilteringRadius))
 						{
 							RegistrationInfo info;
-							Transform transform = _memory->computeTransform(signature->id(), nearestId, Transform(), &info);
+							Transform guess = _optimizedPoses.at(signature->id()).inverse() * _optimizedPoses.at(nearestId);
+							Transform transform = _memory->computeTransform(signature->id(), nearestId, guess, &info);
 							if(!transform.isNull())
 							{
 								if(_proximityFilteringRadius <= 0 || transform.getNormSquared() <= _proximityFilteringRadius*_proximityFilteringRadius)
@@ -1898,49 +1900,37 @@ bool Rtabmap::process(
 						   (_proximityFilteringRadius <= 0.0f ||
 							_optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _proximityFilteringRadius*_proximityFilteringRadius))
 						{
-							if(!_proximityScansMerged)
-							{
-								//only keep the nearest node
-								std::map<int, Transform> tmp;
-								tmp.insert(*path.find(nearestId));
-								path = tmp;
-							}
-							else
+							// Assemble scans in the path and do ICP only
+							if(_proximityRawPosesUsed)
 							{
-								// Assemble scans in the path and do ICP only
-								if(_proximityRawPosesUsed)
+								//optimize the path's poses locally
+								path = optimizeGraph(nearestId, uKeysSet(path), std::map<int, Transform>(), false);
+								// transform local poses in optimized graph referential
+								UASSERT(uContains(path, nearestId));
+								Transform t = _optimizedPoses.at(nearestId) * path.at(nearestId).inverse();
+								for(std::map<int, Transform>::iterator jter=path.begin(); jter!=path.end(); ++jter)
 								{
-									//optimize the path's poses locally
-									path = optimizeGraph(nearestId, uKeysSet(path), std::map<int, Transform>(), false);
-									// transform local poses in optimized graph referential
-									UASSERT(uContains(path, nearestId));
-									Transform t = _optimizedPoses.at(nearestId) * path.at(nearestId).inverse();
-									for(std::map<int, Transform>::iterator jter=path.begin(); jter!=path.end(); ++jter)
-									{
-										jter->second = t * jter->second;
-									}
-								}
-								if(path.size() > 2 && _proximityFilteringRadius > 0.0f)
-								{
-									// path filtering
-									std::map<int, Transform> filteredPath = graph::radiusPosesFiltering(path, _proximityFilteringRadius, 0, true);
-									// make sure the nearest and farthest poses are still here
-									filteredPath.insert(*path.find(nearestId));
-									filteredPath.insert(*path.begin());
-									filteredPath.insert(*path.rbegin());
-									path = filteredPath;
+									jter->second = t * jter->second;
 								}
 							}
+							std::map<int, Transform> filteredPath = path;
+							if(path.size() > 2 && _proximityFilteringRadius > 0.0f)
+							{
+								// path filtering
+								filteredPath = graph::radiusPosesFiltering(path, _proximityFilteringRadius, 0, true);
+								// make sure the current pose is still here
+								filteredPath.insert(*path.find(nearestId));
+							}

-							if(path.size() > 0)
+							if(filteredPath.size() > 0)
 							{
 								// add current node to poses
-								path.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
+								filteredPath.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
 								//The nearest will be the reference for a loop closure transform
 								if(signature->getLinks().find(nearestId) == signature->getLinks().end())
 								{
 									RegistrationInfo info;
-									Transform transform = _memory->computeIcpTransformMulti(signature->id(), nearestId, path, &info);
+									Transform transform = _memory->computeIcpTransformMulti(signature->id(), nearestId, filteredPath, &info);
 									if(!transform.isNull())
 									{
 										if(_proximityFilteringRadius <= 0 || transform.getNormSquared() <= _proximityFilteringRadius*_proximityFilteringRadius)
@@ -1957,14 +1947,11 @@ bool Rtabmap::process(
 												stream << "SCANS:";
 												for(std::map<int, Transform>::iterator iter=path.begin(); iter!=path.end(); ++iter)
 												{
-													if(iter->first!=signature->id())
+													if(iter != path.begin())
 													{
-														if(iter != path.begin())
-														{
-															stream << ";";
-														}
-														stream << uNumber2Str(iter->first);
+														stream << ";";
 													}
+													stream << uNumber2Str(iter->first);
 												}
 												std::string scansStr = stream.str();
 												scanMatchingIds = cv::Mat(1, int(scansStr.size()+1), CV_8SC1, (void *)scansStr.c_str());
@@ -2049,14 +2036,17 @@ bool Rtabmap::process(
 	{
 		UASSERT(uContains(_optimizedPoses, signature->id()));

+		//used in localization mode: filter virtual links
+		std::map<int, Link> localizationLinks = graph::filterLinks(signature->getLinks(), Link::kVirtualClosure);
+
 		// Note that in localization mode, we don't re-optimize the graph
 		// if:
 		//  1- there are no signatures retrieved,
 		//  2- we are relocalizing on a node already in the optimized graph
 		if(!_memory->isIncremental() &&
 		   signaturesRetrieved.size() == 0 &&
-		   signature->getLinks().size() &&
-		   uContains(_optimizedPoses, signature->getLinks().begin()->first))
+		   localizationLinks.size() &&
+		   uContains(_optimizedPoses, localizationLinks.begin()->first))
 		{
 			// If there are no signatures retrieved, we don't
 			// need to re-optimize the graph. Just update the last
@@ -2068,8 +2058,8 @@ bool Rtabmap::process(
 				// update all previous nodes
 				// Normally _mapCorrection should be identity, but if _optimizeFromGraphEnd
 				// parameters just changed state, we should put back all poses without map correction.
-				Transform oldPose = _optimizedPoses.at(signature->getLinks().begin()->first);
-				Transform u = signature->getPose() * signature->getLinks().begin()->second.transform();
+				Transform oldPose = _optimizedPoses.at(localizationLinks.begin()->first);
+				Transform u = signature->getPose() * localizationLinks.begin()->second.transform();
 				Transform up = u * oldPose.inverse();
 				Transform mapCorrectionInv = _mapCorrection.inverse();
 				for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
@@ -2080,7 +2070,7 @@ bool Rtabmap::process(
 			}
 			else
 			{
-				_optimizedPoses.at(signature->id()) = _optimizedPoses.at(signature->getLinks().begin()->first) * signature->getLinks().begin()->second.transform().inverse();
+				_optimizedPoses.at(signature->id()) = _optimizedPoses.at(localizationLinks.begin()->first) * localizationLinks.begin()->second.transform().inverse();
 			}
 		}
 		else
@@ -2683,6 +2673,11 @@ void Rtabmap::rejectLoopClosure(int oldId, int newId)
 	}
 }

+void Rtabmap::setOptimizedPoses(const std::map<int, Transform> & poses)
+{
+	_optimizedPoses = poses;
+}
+
 void Rtabmap::dumpData() const
 {
 	UDEBUG("");
@@ -3160,7 +3155,8 @@ void Rtabmap::get3DMap(
 			data.setId(*iter);
 			std::multimap<int, cv::KeyPoint> words;
 			std::multimap<int, cv::Point3f> words3;
-			_memory->getNodeWords(*iter, words, words3);
+			std::multimap<int, cv::Mat> wordsDescriptors;
+			_memory->getNodeWords(*iter, words, words3, wordsDescriptors);
 			signatures.insert(std::make_pair(*iter,
 					Signature(*iter,
 							mapId,
@@ -3172,6 +3168,7 @@ void Rtabmap::get3DMap(
 							data)));
 			signatures.at(*iter).setWords(words);
 			signatures.at(*iter).setWords3(words3);
+			signatures.at(*iter).setWordsDescriptors(wordsDescriptors);
 		}
 	}
 	else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size() > 1))
@@ -3232,6 +3229,20 @@ void Rtabmap::getGraph(
 							label,
 							odomPose,
 							groundTruth)));
+
+				std::multimap<int, cv::KeyPoint> words;
+				std::multimap<int, cv::Point3f> words3;
+				std::multimap<int, cv::Mat> wordsDescriptors;
+				_memory->getNodeWords(iter->first, words, words3, wordsDescriptors);
+				signatures->at(iter->first).setWords(words);
+				signatures->at(iter->first).setWords3(words3);
+				signatures->at(iter->first).setWordsDescriptors(wordsDescriptors);
+
+				std::vector<CameraModel> models;
+				StereoCameraModel stereoModel;
+				_memory->getNodeCalibration(iter->first, models, stereoModel);
+				signatures->at(iter->first).sensorData().setCameraModels(models);
+				signatures->at(iter->first).sensorData().setStereoCameraModel(stereoModel);
 			}
 		}
 	}
@@ -3245,6 +3256,114 @@ void Rtabmap::getGraph(
 	}
 }

+int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int iterations)
+{
+	UASSERT(iterations>0);
+
+	if(_graphOptimizer->iterations() <= 0)
+	{
+		UERROR("Cannot detect more loop closures if graph optimization iterations = 0");
+		return -1;
+	}
+	if(!_rgbdSlamMode)
+	{
+		UERROR("Detecting more loop closures can be done only in RGBD-SLAM mode.");
+		return -1;
+	}
+
+	std::list<Link> loopClosuresAdded;
+	std::multimap<int, int> checkedLoopClosures;
+
+	std::map<int, Transform> poses;
+	std::multimap<int, Link> links;
+	std::map<int, Signature> signatures;
+	this->getGraph(poses, links, true, true, &signatures);
+
+	for(int n=0; n<iterations; ++n)
+	{
+		UINFO("Looking for more loop closures, clustering poses... (iteration=%d/%d, radius=%f m angle=%f rad)",
+				n+1, iterations, clusterRadius, clusterAngle);
+
+		std::multimap<int, int> clusters = graph::radiusPosesClustering(
+				poses,
+				clusterRadius,
+				clusterAngle);
+
+		UINFO("Looking for more loop closures, clustering poses... found %d clusters.", (int)clusters.size());
+
+		int i=0;
+		std::set<int> addedLinks;
+		for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!= clusters.end(); ++iter, ++i)
+		{
+			int from = iter->first;
+			int to = iter->second;
+			if(iter->first < iter->second)
+			{
+				from = iter->second;
+				to = iter->first;
+			}
+
+			if(rtabmap::graph::findLink(checkedLoopClosures, from, to) == checkedLoopClosures.end())
+			{
+				// only add new links and one per cluster per iteration
+				if(addedLinks.find(from) == addedLinks.end() &&
+				   addedLinks.find(to) == addedLinks.end() &&
+				   rtabmap::graph::findLink(links, from, to) == links.end())
+				{
+					checkedLoopClosures.insert(std::make_pair(from, to));
+
+					UASSERT(signatures.find(from) != signatures.end());
+					UASSERT(signatures.find(to) != signatures.end());
+
+					RegistrationInfo info;
+					// use signatures instead of IDs because some signatures may not be in WM
+					Transform t = _memory->computeTransform(signatures.at(from), signatures.at(to), Transform(), &info);
+
+					if(!t.isNull())
+					{
+						UINFO("Added new loop closure between %d and %d.", from, to);
+						addedLinks.insert(from);
+						addedLinks.insert(to);
+						links.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, info.variance, info.variance)));
+						loopClosuresAdded.push_back(Link(from, to, Link::kUserClosure, t, info.variance, info.variance));
+						UINFO("Detected loop closure %d->%d! (%d/%d)", from, to, i+1, (int)clusters.size());
+					}
+				}
+			}
+		}
+		UINFO("Iteration %d/%d: Detected %d loop closures!", n+1, iterations, (int)addedLinks.size()/2);
+		if(addedLinks.size() == 0)
+		{
+			break;
+		}
+
+		if(n+1 < iterations)
+		{
+			UINFO("Optimizing graph with new links (%d nodes, %d constraints)...",
+					(int)poses.size(), (int)links.size());
+			int fromId = _optimizeFromGraphEnd?poses.rbegin()->first:poses.begin()->first;
+			poses = _graphOptimizer->optimize(fromId, poses, links, 0);
+			if(poses.size() == 0)
+			{
+				UERROR("Optimization failed! Rejecting all loop closures...");
+				loopClosuresAdded.clear();
+				return -1;
+			}
+			UINFO("Optimizing graph with new links... done!");
+		}
+	}
+	UINFO("Total added %d loop closures.", (int)loopClosuresAdded.size());
+
+	if(loopClosuresAdded.size())
+	{
+		for(std::list<Link>::iterator iter=loopClosuresAdded.begin(); iter!=loopClosuresAdded.end(); ++iter)
+		{
+			_memory->addLink(*iter, true);
+		}
+	}
+	return (int)loopClosuresAdded.size();
+}
+
 void Rtabmap::clearPath(int status)
 {
 	UINFO("status=%d", status);
@@ -544,14 +544,31 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 				ignoreFrame = true;
 			}
 		}
-		if(_dataBufferMaxSize > 0 && !lastPose_.isIdentity() && (odomEvent.pose().isIdentity() || odomEvent.info().variance>=9999))
+		if(_dataBufferMaxSize > 0 &&
+				(!lastPose_.isIdentity() &&
+					   (odomEvent.pose().isIdentity() ||
+						odomEvent.info().variance>=9999 ||
+						odomEvent.rotVariance()>=9999 ||
+						odomEvent.transVariance()>=9999)))
 		{
-			UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", odomEvent.info().variance);
+			UWARN("Odometry is reset (identity pose or high variance >=9999 detected). Increment map id!");
 			pushNewState(kStateTriggeringMap);
 			_rotVariance = 0;
 			_transVariance = 0;
 		}

+		double maxRotVar = odomEvent.rotVariance();
+		double maxTransVar = odomEvent.transVariance();
+		// FIXME: should merge the transformations/variances like Link::merge();
+		if(maxRotVar > _rotVariance)
+		{
+			_rotVariance = maxRotVar;
+		}
+		if(maxTransVar > _transVariance)
+		{
+			_transVariance = maxTransVar;
+		}
+
 		if(ignoreFrame && !_createIntermediateNodes)
 		{
 			return;
@@ -563,17 +580,6 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 		}

 		lastPose_ = odomEvent.pose();
-		double maxRotVar = odomEvent.rotVariance();
-		double maxTransVar = odomEvent.transVariance();
-		// FIXME: should merge the transformations/variances like Link::merge();
-		if(maxRotVar > _rotVariance)
-		{
-			_rotVariance = maxRotVar;
-		}
-		if(maxTransVar > _transVariance)
-		{
-			_transVariance = maxTransVar;
-		}

 		bool notify = true;
 		_dataMutex.lock();
@@ -598,7 +604,7 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 			{
 				_dataBuffer.push_back(OdometryEvent(odomEvent.data(), odomEvent.pose(), _rotVariance, _transVariance));
 			}
-			UDEBUG("Added data %d", odomEvent.data().id());
+			UINFO("Added data %d (variance=%f)", odomEvent.data().id(), _rotVariance);

 			_rotVariance = 0;
 			_transVariance = 0;
@@ -348,7 +348,8 @@ SensorData::SensorData(
 	else if(!left.empty())
 	{
 		UASSERT(left.type() == CV_8UC1 || // Mono
-				left.type() == CV_8UC3);  // RGB
+				left.type() == CV_8UC3 || // RGB
+				left.type() == CV_16UC1); // IR
 		_imageRaw = left;
 	}
 	if(right.rows == 1)
@@ -358,7 +359,8 @@ SensorData::SensorData(
 	}
 	else if(!right.empty())
 	{
-		UASSERT(right.type() == CV_8UC1); // Mono
+		UASSERT(right.type() == CV_8UC1 || // Mono
+				right.type() == CV_16UC1);  // IR
 		_depthOrRightRaw = right;
 	}

@@ -150,7 +150,13 @@ std::vector<cv::Point2f> StereoOpticalFlow::computeCorrespondences(

 	if(countFlowRejected + countDisparityRejected > (int)status.size()/2)
 	{
-		UWARN("A large number (%d/%d) of stereo correspondences are rejected! Optical flow may have failed, images are not calibrated or the background is too far (no disparity between the images).", countFlowRejected+countDisparityRejected, (int)status.size());
+		UWARN("A large number (%d/%d) of stereo correspondences are rejected! "
+				"Optical flow may have failed, images are not calibrated, "
+				"the background is too far (no disparity between the images) or "
+				"maximum disparity may be too small (%d).",
+				countFlowRejected+countDisparityRejected,
+				(int)status.size(),
+				this->maxDisparity());
 	}

 	return rightCorners;
@@ -542,7 +542,7 @@ void VWDictionary::setNNStrategy(NNStrategy strategy)
 		}
 #endif
 #else
-#if HAVE_OPENCV_CUDAFEATURES2D
+#ifdef HAVE_OPENCV_CUDAFEATURES2D
 		if(strategy == kNNBruteForceGPU && !cv::cuda::getCudaEnabledDeviceCount())
 		{
 			UERROR("Nearest neighobr strategy \"kNNBruteForceGPU\" chosen but no CUDA devices found! Doing \"kNNBruteForce\" instead.");
@@ -626,6 +626,25 @@ void VWDictionary::update()
 				{
 					VisualWord* w = uValue(_visualWords, *iter, (VisualWord*)0);
 					UASSERT(w);
+
+					cv::Mat descriptor;
+					if(w->getDescriptor().type() == CV_8U)
+					{
+						useDistanceL1_ = true;
+						if(_strategy == kNNFlannKdTree || _strategy == kNNFlannNaive)
+						{
+							w->getDescriptor().convertTo(descriptor, CV_32F);
+						}
+						else
+						{
+							descriptor = w->getDescriptor();
+						}
+					}
+					else
+					{
+						descriptor = w->getDescriptor();
+					}
+
 					int index = 0;
 					if(!_flannIndex->isBuilt())
 					{
@@ -633,15 +652,15 @@ void VWDictionary::update()
 						switch(_strategy)
 						{
 						case kNNFlannNaive:
-							_flannIndex->build(w->getDescriptor(), rtflann::LinearIndexParams(), useDistanceL1_);
+							_flannIndex->build(descriptor, rtflann::LinearIndexParams(), useDistanceL1_);
 							break;
 						case kNNFlannKdTree:
-							UASSERT_MSG(w->getDescriptor().type() == CV_32F, "To use KdTree dictionary, float descriptors are required!");
-							_flannIndex->build(w->getDescriptor(), rtflann::KDTreeIndexParams(), useDistanceL1_);
+							UASSERT_MSG(descriptor.type() == CV_32F, "To use KdTree dictionary, float descriptors are required!");
+							_flannIndex->build(descriptor, rtflann::KDTreeIndexParams(), useDistanceL1_);
 							break;
 						case kNNFlannLSH:
-							UASSERT_MSG(w->getDescriptor().type() == CV_8U, "To use LSH dictionary, binary descriptors are required!");
-							_flannIndex->build(w->getDescriptor(), rtflann::LshIndexParams(12, 20, 2), useDistanceL1_);
+							UASSERT_MSG(descriptor.type() == CV_8U, "To use LSH dictionary, binary descriptors are required!");
+							_flannIndex->build(descriptor, rtflann::LshIndexParams(12, 20, 2), useDistanceL1_);
 							break;
 						default:
 							UFATAL("Not supposed to be here!");
@@ -651,9 +670,9 @@ void VWDictionary::update()
 					}
 					else
 					{
-						UASSERT(w->getDescriptor().cols == _flannIndex->featuresDim());
-						UASSERT(w->getDescriptor().type() == _flannIndex->featuresType());
-						index = _flannIndex->addPoint(w->getDescriptor());
+						UASSERT(descriptor.cols == _flannIndex->featuresDim());
+						UASSERT(descriptor.type() == _flannIndex->featuresType());
+						index = _flannIndex->addPoint(descriptor);
 					}
 					std::pair<std::map<int, int>::iterator, bool> inserted;
 					inserted = _mapIndexId.insert(std::pair<int, int>(index, w->id()));
@@ -698,7 +717,23 @@ void VWDictionary::update()
 				UTimer timer;
 				timer.start();

-				int type = _visualWords.begin()->second->getDescriptor().type();
+				int type;
+				if(_visualWords.begin()->second->getDescriptor().type() == CV_8U)
+				{
+					useDistanceL1_ = true;
+					if(_strategy == kNNFlannKdTree || _strategy == kNNFlannNaive)
+					{
+						type = CV_32F;
+					}
+					else
+					{
+						type = _visualWords.begin()->second->getDescriptor().type();
+					}
+				}
+				else
+				{
+					type = _visualWords.begin()->second->getDescriptor().type();
+				}
 				int dim = _visualWords.begin()->second->getDescriptor().cols;

 				UASSERT(type == CV_32F || type == CV_8U);
@@ -709,10 +744,27 @@ void VWDictionary::update()
 				std::map<int, VisualWord*>::const_iterator iter = _visualWords.begin();
 				for(unsigned int i=0; i < _visualWords.size(); ++i, ++iter)
 				{
-					UASSERT(iter->second->getDescriptor().cols == dim);
-					UASSERT(iter->second->getDescriptor().type() == type);
+					cv::Mat descriptor;
+					if(iter->second->getDescriptor().type() == CV_8U)
+					{
+						if(_strategy == kNNFlannKdTree || _strategy == kNNFlannNaive)
+						{
+							iter->second->getDescriptor().convertTo(descriptor, CV_32F);
+						}
+						else
+						{
+							descriptor = iter->second->getDescriptor();
+						}
+					}
+					else
+					{
+						descriptor = iter->second->getDescriptor();
+					}

-					iter->second->getDescriptor().copyTo(_dataTree.row(i));
+					UASSERT(descriptor.cols == dim);
+					UASSERT(descriptor.type() == type);
+
+					descriptor.copyTo(_dataTree.row(i));
 					_mapIndexId.insert(_mapIndexId.end(), std::pair<int, int>(i, iter->second->id()));
 					_mapIdIndex.insert(_mapIdIndex.end(), std::pair<int, int>(iter->second->id(), i));
 				}
@@ -827,6 +879,42 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptorsIn,
 {
 	UASSERT(signatureId > 0);

+	UDEBUG("id=%d descriptors=%d", signatureId, descriptorsIn.rows);
+	UTimer timer;
+	std::list<int> wordIds;
+	if(descriptorsIn.rows == 0 || descriptorsIn.cols == 0)
+	{
+		UERROR("Descriptors size is null!");
+		return wordIds;
+	}
+
+	if(!_incrementalDictionary && _visualWords.empty())
+	{
+		UERROR("Dictionary mode is set to fixed but no words are in it!");
+		return wordIds;
+	}
+
+	// verify we have the same features
+	int dim = 0;
+	int type = -1;
+	if(_visualWords.size())
+	{
+		dim = _visualWords.begin()->second->getDescriptor().cols;
+		type = _visualWords.begin()->second->getDescriptor().type();
+		UASSERT(type == CV_32F || type == CV_8U);
+	}
+	if(dim && dim != descriptorsIn.cols)
+	{
+		UERROR("Descriptors (size=%d) are not the same size as already added words in dictionary(size=%d)", descriptorsIn.cols, dim);
+		return wordIds;
+	}
+	if(type>=0 && type != descriptorsIn.type())
+	{
+		UERROR("Descriptors (type=%d) are not the same type as already added words in dictionary(type=%d)", descriptorsIn.type(), type);
+		return wordIds;
+	}
+
+	// now compare with the actual index
 	cv::Mat descriptors;
 	if(descriptorsIn.type() == CV_8U)
 	{
@@ -844,21 +932,12 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptorsIn,
 	{
 		descriptors = descriptorsIn;
 	}
-
-	UDEBUG("id=%d descriptors=%d", signatureId, descriptors.rows);
-	UTimer timer;
-	std::list<int> wordIds;
-	if(descriptors.rows == 0 || descriptors.cols == 0)
+	dim = 0;
+	type = -1;
+	if(_dataTree.rows || _flannIndex->isBuilt())
 	{
-		UERROR("Descriptors size is null!");
-		return wordIds;
-	}
-	int dim = 0;
-	int type = -1;
-	if(_visualWords.size())
-	{
-		dim = _visualWords.begin()->second->getDescriptor().cols;
-		type = _visualWords.begin()->second->getDescriptor().type();
+		dim = _flannIndex->isBuilt()?_flannIndex->featuresDim():_dataTree.cols;
+		type = _flannIndex->isBuilt()?_flannIndex->featuresType():_dataTree.type();
 		UASSERT(type == CV_32F || type == CV_8U);
 	}

@@ -867,20 +946,12 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptorsIn,
 		UERROR("Descriptors (size=%d) are not the same size as already added words in dictionary(size=%d)", descriptors.cols, dim);
 		return wordIds;
 	}
-	dim = descriptors.cols;

 	if(type>=0 && type != descriptors.type())
 	{
 		UERROR("Descriptors (type=%d) are not the same type as already added words in dictionary(type=%d)", descriptors.type(), type);
 		return wordIds;
 	}
-	type = descriptors.type();
-
-	if(!_incrementalDictionary && _visualWords.empty())
-	{
-		UERROR("Dictionary mode is set to fixed but no words are in it!");
-		return wordIds;
-	}

 	int dupWordsCountFromDict= 0;
 	int dupWordsCountFromLast= 0;
@@ -910,7 +981,7 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptorsIn,
 		else if(_strategy == kNNBruteForce)
 		{
 			bruteForce = true;
-			cv::BFMatcher matcher(type==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
+			cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
 			matcher.knnMatch(descriptors, _dataTree, matches, k);
 		}
 		else if(_strategy == kNNBruteForceGPU)
@@ -920,7 +991,7 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptorsIn,
 #ifdef HAVE_OPENCV_GPU
 			cv::gpu::GpuMat newDescriptorsGpu(descriptors);
 			cv::gpu::GpuMat lastDescriptorsGpu(_dataTree);
-			if(type==CV_8U)
+			if(descriptors.type()==CV_8U)
 			{
 				cv::gpu::BruteForceMatcher_GPU<cv::Hamming> gpuMatcher;
 				gpuMatcher.knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k);
@@ -938,7 +1009,7 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptorsIn,
 			cv::cuda::GpuMat newDescriptorsGpu(descriptors);
 			cv::cuda::GpuMat lastDescriptorsGpu(_dataTree);
 			cv::Ptr<cv::cuda::DescriptorMatcher> gpuMatcher;
-			if(type==CV_8U)
+			if(descriptors.type()==CV_8U)
 			{
 				gpuMatcher = cv::cuda::DescriptorMatcher::createBFMatcher(cv::NORM_HAMMING);
 				gpuMatcher->knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k);
@@ -1010,7 +1081,8 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptorsIn,
 		if(_newWordsComparedTogether && newWords.rows)
 		{
 			std::vector<std::vector<cv::DMatch> > matchesNewWords;
-			cv::BFMatcher matcher(type==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
+			cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:useDistanceL1_?cv::NORM_L1:cv::NORM_L2SQR);
+			UASSERT(descriptors.cols == newWords.cols && descriptors.type() == newWords.type());
 			matcher.knnMatch(descriptors.row(i), newWords, matchesNewWords, newWords.rows>1?2:1);
 			UASSERT(matchesNewWords.size() == 1);
 			for(unsigned int j=0; j<matchesNewWords.at(0).size(); ++j)
@@ -1053,10 +1125,11 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptorsIn,

 			if(badDist)
 			{
-				VisualWord * vw = new VisualWord(getNextId(), descriptors.row(i), signatureId);
+				// use original descriptor
+				VisualWord * vw = new VisualWord(getNextId(), descriptorsIn.row(i), signatureId);
 				_visualWords.insert(_visualWords.end(), std::pair<int, VisualWord *>(vw->id(), vw));
 				_notIndexedWords.insert(_notIndexedWords.end(), vw->id());
-				newWords.push_back(vw->getDescriptor());
+				newWords.push_back(descriptors.row(i));
 				newWordsId.push_back(vw->id());
 				wordIds.push_back(vw->id());
 				UASSERT(vw->id()>0);
@@ -1103,16 +1176,16 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws) const

 	if(_visualWords.size() && vws.size())
 	{
-		int dim = _visualWords.begin()->second->getDescriptor().cols;
-		int type = _visualWords.begin()->second->getDescriptor().type();
+		int type = (*vws.begin())->getDescriptor().type();
+		int dim = (*vws.begin())->getDescriptor().cols;

-		if(dim != (*vws.begin())->getDescriptor().cols)
+		if(dim != _visualWords.begin()->second->getDescriptor().cols)
 		{
 			UERROR("Descriptors (size=%d) are not the same size as already added words in dictionary(size=%d)", (*vws.begin())->getDescriptor().cols, dim);
 			return std::vector<int>(vws.size(), 0);
 		}

-		if(type != (*vws.begin())->getDescriptor().type())
+		if(type != _visualWords.begin()->second->getDescriptor().type())
 		{
 			UERROR("Descriptors (type=%d) are not the same type as already added words in dictionary(type=%d)", (*vws.begin())->getDescriptor().type(), type);
 			return std::vector<int>(vws.size(), 0);
@@ -1126,6 +1199,7 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws) const
 		{
 			vw = *iter;
 			UASSERT(vw);
+
 			UASSERT(vw->getDescriptor().cols == dim);
 			UASSERT(vw->getDescriptor().type() == type);

@@ -1137,25 +1211,64 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws) const
 	}
 	return std::vector<int>(vws.size(), 0);
 }
-std::vector<int> VWDictionary::findNN(const cv::Mat & query) const
+std::vector<int> VWDictionary::findNN(const cv::Mat & queryIn) const
 {
 	UTimer timer;
 	timer.start();
-	std::vector<int> resultIds(query.rows, 0);
+	std::vector<int> resultIds(queryIn.rows, 0);
 	unsigned int k=2; // k nearest neighbor

-	if(_visualWords.size() && query.rows)
+	if(_visualWords.size() && queryIn.rows)
 	{
+		// verify we have the same features
 		int dim = _visualWords.begin()->second->getDescriptor().cols;
 		int type = _visualWords.begin()->second->getDescriptor().type();
+		UASSERT(type == CV_32F || type == CV_8U);

-		if(dim != query.cols)
+		if(dim != queryIn.cols)
+		{
+			UERROR("Descriptors (size=%d) are not the same size as already added words in dictionary(size=%d)", queryIn.cols, dim);
+			return resultIds;
+		}
+		if(type != queryIn.type())
+		{
+			UERROR("Descriptors (type=%d) are not the same type as already added words in dictionary(type=%d)", queryIn.type(), type);
+			return resultIds;
+		}
+
+		// now compare with the actual index
+		cv::Mat query;
+		if(queryIn.type() == CV_8U)
+		{
+			if(_strategy == kNNFlannKdTree || _strategy == kNNFlannNaive)
+			{
+				queryIn.convertTo(query, CV_32F);
+			}
+			else
+			{
+				query = queryIn;
+			}
+		}
+		else
+		{
+			query = queryIn;
+		}
+		dim = 0;
+		type = -1;
+		if(_dataTree.rows || _flannIndex->isBuilt())
+		{
+			dim = _flannIndex->isBuilt()?_flannIndex->featuresDim():_dataTree.cols;
+			type = _flannIndex->isBuilt()?_flannIndex->featuresType():_dataTree.type();
+			UASSERT(type == CV_32F || type == CV_8U);
+		}
+
+		if(dim && dim != query.cols)
 		{
 			UERROR("Descriptors (size=%d) are not the same size as already added words in dictionary(size=%d)", query.cols, dim);
 			return resultIds;
 		}

-		if(type != query.type())
+		if(type>=0 && type != query.type())
 		{
 			UERROR("Descriptors (type=%d) are not the same type as already added words in dictionary(type=%d)", query.type(), type);
 			return resultIds;
@@ -1178,7 +1291,7 @@ std::vector<int> VWDictionary::findNN(const cv::Mat & query) const
 			else if(_strategy == kNNBruteForce)
 			{
 				bruteForce = true;
-				cv::BFMatcher matcher(type==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
+				cv::BFMatcher matcher(query.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
 				matcher.knnMatch(query, _dataTree, matches, k);
 			}
 			else if(_strategy == kNNBruteForceGPU)
@@ -1188,7 +1301,7 @@ std::vector<int> VWDictionary::findNN(const cv::Mat & query) const
 #ifdef HAVE_OPENCV_GPU
 				cv::gpu::GpuMat newDescriptorsGpu(query);
 				cv::gpu::GpuMat lastDescriptorsGpu(_dataTree);
-				if(type==CV_8U)
+				if(query.type()==CV_8U)
 				{
 					cv::gpu::BruteForceMatcher_GPU<cv::Hamming> gpuMatcher;
 					gpuMatcher.knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k);
@@ -1206,7 +1319,7 @@ std::vector<int> VWDictionary::findNN(const cv::Mat & query) const
 				cv::cuda::GpuMat newDescriptorsGpu(query);
 				cv::cuda::GpuMat lastDescriptorsGpu(_dataTree);
 				cv::Ptr<cv::cuda::DescriptorMatcher> gpuMatcher;
-				if(type==CV_8U)
+				if(query.type()==CV_8U)
 				{
 					gpuMatcher = cv::cuda::DescriptorMatcher::createBFMatcher(cv::NORM_HAMMING);
 					gpuMatcher->knnMatchAsync(newDescriptorsGpu, lastDescriptorsGpu, matches, k);
@@ -1240,20 +1353,38 @@ std::vector<int> VWDictionary::findNN(const cv::Mat & query) const
 		std::vector<std::vector<cv::DMatch> > matchesNotIndexed;
 		if(_notIndexedWords.size())
 		{
-			cv::Mat dataNotIndexed = cv::Mat::zeros(_notIndexedWords.size(), dim, type);
+			cv::Mat dataNotIndexed = cv::Mat::zeros(_notIndexedWords.size(), query.cols, query.type());
 			unsigned int index = 0;
 			VisualWord * vw;
 			for(std::set<int>::iterator iter = _notIndexedWords.begin(); iter != _notIndexedWords.end(); ++iter, ++index)
 			{
 				vw = _visualWords.at(*iter);
-				UASSERT(vw != 0 && vw->getDescriptor().cols == dim && vw->getDescriptor().type() == type);
+
+				cv::Mat descriptor;
+				if(vw->getDescriptor().type() == CV_8U)
+				{
+					if(_strategy == kNNFlannKdTree || _strategy == kNNFlannNaive)
+					{
+						vw->getDescriptor().convertTo(descriptor, CV_32F);
+					}
+					else
+					{
+						descriptor = vw->getDescriptor();
+					}
+				}
+				else
+				{
+					descriptor = vw->getDescriptor();
+				}
+
+				UASSERT(vw != 0 && descriptor.cols == query.cols && descriptor.type() == query.type());
 				vw->getDescriptor().copyTo(dataNotIndexed.row(index));
 				mapIndexIdNotIndexed.insert(mapIndexIdNotIndexed.end(), std::pair<int,int>(index, vw->id()));
 			}

 			// Find nearest neighbor
 			ULOGGER_DEBUG("Searching in words not indexed...");
-			cv::BFMatcher matcher(type==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
+			cv::BFMatcher matcher(query.type()==CV_8U?cv::NORM_HAMMING:useDistanceL1_?cv::NORM_L1:cv::NORM_L2SQR);
 			matcher.knnMatch(query, dataNotIndexed, matchesNotIndexed, dataNotIndexed.rows>1?2:1);
 		}
 		ULOGGER_DEBUG("Search not yet indexed words time = %fs", timer.ticks());
@@ -1415,18 +1546,17 @@ void VWDictionary::deleteUnusedWords()

 void VWDictionary::exportDictionary(const char * fileNameReferences, const char * fileNameDescriptors) const
 {
+	UDEBUG("");
 	if(_visualWords.empty())
 	{
 		UWARN("Dictionary is empty, cannot export it!");
 		return;
 	}
-
-	if(_visualWords.at(0)->getDescriptor().type() != CV_32FC1)
+	if(_visualWords.begin()->second->getDescriptor().type() != CV_32FC1)
 	{
 		UERROR("Exporting binary descriptors is not implemented!");
 		return;
 	}
-
    FILE* foutRef = 0;
    FILE* foutDesc = 0;
 #ifdef _MSC_VER
@@ -1449,10 +1579,12 @@ void VWDictionary::exportDictionary(const char * fileNameReferences, const char
 		}
 		else
 		{
+			UDEBUG("");
 			fprintf(foutDesc, "WordID Descriptors...%d\n", (*_visualWords.begin()).second->getDescriptor().cols);
 		}
 	}

+	UDEBUG("Export %d words...", _visualWords.size());
    for(std::map<int, VisualWord *>::const_iterator iter=_visualWords.begin(); iter!=_visualWords.end(); ++iter)
    {
    	// References
@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/core/StereoDense.h>
 #include <opencv2/calib3d/calib3d.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
 #include <opencv2/video/tracking.hpp>
@@ -724,12 +725,11 @@ void calcOpticalFlowPyrLKStereo( cv::InputArray _prevImg, cv::InputArray _nextIm
 cv::Mat disparityFromStereoImages(
 		const cv::Mat & leftImage,
 		const cv::Mat & rightImage,
-		int type)
+		const ParametersMap & parameters)
 {
 	UASSERT(!leftImage.empty() && !rightImage.empty());
 	UASSERT(leftImage.cols == rightImage.cols && leftImage.rows == rightImage.rows);
 	UASSERT((leftImage.type() == CV_8UC1 || leftImage.type() == CV_8UC3) && rightImage.type() == CV_8UC1);
-	UASSERT(type == CV_32FC1 || type == CV_16SC1);

 	cv::Mat leftMono;
 	if(leftImage.channels() == 3)
@@ -741,32 +741,9 @@ cv::Mat disparityFromStereoImages(
 		leftMono = leftImage;
 	}
 	cv::Mat disparity;
-#if CV_MAJOR_VERSION < 3
-	cv::StereoBM stereo(cv::StereoBM::BASIC_PRESET);
-	stereo.state->SADWindowSize = 15;
-	stereo.state->minDisparity = 0;
-	stereo.state->numberOfDisparities = 64;
-	stereo.state->preFilterSize = 9;
-	stereo.state->preFilterCap = 31;
-	stereo.state->uniquenessRatio = 15;
-	stereo.state->textureThreshold = 10;
-	stereo.state->speckleWindowSize = 100;
-	stereo.state->speckleRange = 4;
-	stereo(leftMono, rightImage, disparity, type);
-#else
-	cv::Ptr<cv::StereoBM> stereo = cv::StereoBM::create();
-	stereo->setBlockSize(15);
-	stereo->setMinDisparity(0);
-	stereo->setNumDisparities(64);
-	stereo->setPreFilterSize(9);
-	stereo->setPreFilterCap(31);
-	stereo->setUniquenessRatio(15);
-	stereo->setTextureThreshold(10);
-	stereo->setSpeckleWindowSize(100);
-	stereo->setSpeckleRange(4);
-	stereo->compute(leftMono, rightImage, disparity);
-#endif
-	return disparity;
+
+	StereoBM stereo(parameters);
+	return stereo.computeDisparity(leftMono, rightImage);
 }

 cv::Mat depthFromDisparity(const cv::Mat & disparity,
@@ -245,11 +245,12 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
 		float fx, float fy,
 		int decimation,
 		float maxDepth,
+		float minDepth,
 		std::vector<int> * validIndices)
 {
 	UASSERT(!imageDepth.empty() && (imageDepth.type() == CV_16UC1 || imageDepth.type() == CV_32FC1));
-	UASSERT(imageDepth.rows % decimation == 0);
-	UASSERT(imageDepth.cols % decimation == 0);
+	UASSERT_MSG(imageDepth.rows % decimation == 0, uFormat("rows=%d decimation=%d", imageDepth.rows, decimation).c_str());
+	UASSERT_MSG(imageDepth.cols % decimation == 0, uFormat("cols=%d decimation=%d", imageDepth.cols, decimation).c_str());

 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 	if(decimation < 1)
@@ -275,7 +276,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
 			pcl::PointXYZ & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));

 			pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, cx, cy, fx, fy, false);
-			if(maxDepth<=0.0f || ptXYZ.z <= maxDepth)
+			if(pcl::isFinite(ptXYZ) && ptXYZ.z>=minDepth && (maxDepth<=0.0f || ptXYZ.z <= maxDepth))
 			{
 				pt.x = ptXYZ.x;
 				pt.y = ptXYZ.y;
@@ -307,6 +308,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
 		float fx, float fy,
 		int decimation,
 		float maxDepth,
+		float minDepth,
 		std::vector<int> * validIndices)
 {
 	UDEBUG("");
@@ -381,7 +383,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
 			}

 			pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w*rgbToDepthFactorX, h*rgbToDepthFactorY, depthCx, depthCy, depthFx, depthFy, false);
-			if(pcl::isFinite(ptXYZ) && (maxDepth<=0.0f || ptXYZ.z <= maxDepth))
+			if(pcl::isFinite(ptXYZ) && ptXYZ.z>=minDepth && (maxDepth<=0.0f || ptXYZ.z <= maxDepth))
 			{
 				pt.x = ptXYZ.x;
 				pt.y = ptXYZ.y;
@@ -415,6 +417,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDisparity(
 		const StereoCameraModel & model,
 		int decimation,
 		float maxDepth,
+		float minDepth,
 		std::vector<int> * validIndices)
 {
 	UASSERT(imageDisparity.type() == CV_32FC1 || imageDisparity.type()==CV_16SC1);
@@ -443,7 +446,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDisparity(
 			{
 				float disp = float(imageDisparity.at<short>(h,w))/16.0f;
 				cv::Point3f pt = projectDisparityTo3D(cv::Point2f(w, h), disp, model);
-				if(maxDepth <= 0.0f || pt.z <= maxDepth)
+				if(pt.z >= minDepth && (maxDepth <= 0.0f || pt.z <= maxDepth))
 				{
 					cloud->at((h/decimation)*cloud->width + (w/decimation)) = pcl::PointXYZ(pt.x, pt.y, pt.z);
 					if(validIndices)
@@ -469,7 +472,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDisparity(
 			{
 				float disp = imageDisparity.at<float>(h,w);
 				cv::Point3f pt = projectDisparityTo3D(cv::Point2f(w, h), disp, model);
-				if(maxDepth <= 0.0f || pt.z <= maxDepth)
+				if(pt.z > minDepth && (maxDepth <= 0.0f || pt.z <= maxDepth))
 				{
 					cloud->at((h/decimation)*cloud->width + (w/decimation)) = pcl::PointXYZ(pt.x, pt.y, pt.z);
 					if(validIndices)
@@ -500,6 +503,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDisparityRGB(
 		const StereoCameraModel & model,
 		int decimation,
 		float maxDepth,
+		float minDepth,
 		std::vector<int> * validIndices)
 {
 	UASSERT(!imageRgb.empty() && !imageDisparity.empty());
@@ -554,7 +558,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDisparityRGB(

 			float disp = imageDisparity.type()==CV_16SC1?float(imageDisparity.at<short>(h,w))/16.0f:imageDisparity.at<float>(h,w);
 			cv::Point3f ptXYZ = projectDisparityTo3D(cv::Point2f(w, h), disp, model);
-			if(util3d::isFinite(ptXYZ) && (maxDepth<=0.0f || ptXYZ.z <= maxDepth))
+			if(util3d::isFinite(ptXYZ) && ptXYZ.z >= minDepth && (maxDepth<=0.0f || ptXYZ.z <= maxDepth))
 			{
 				pt.x = ptXYZ.x;
 				pt.y = ptXYZ.y;
@@ -583,7 +587,9 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
 		const StereoCameraModel & model,
 		int decimation,
 		float maxDepth,
-		std::vector<int> * validIndices)
+		float minDepth,
+		std::vector<int> * validIndices,
+		const ParametersMap & parameters)
 {
 	UASSERT(!imageLeft.empty() && !imageRight.empty());
 	UASSERT(imageRight.type() == CV_8UC1);
@@ -618,10 +624,11 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(

 	return cloudFromDisparityRGB(
 			leftColor,
-			util2d::disparityFromStereoImages(leftMono, rightMono),
+			util2d::disparityFromStereoImages(leftMono, rightMono, parameters),
 			modelDecimation,
 			decimation,
 			maxDepth,
+			minDepth,
 			validIndices);
 }

@@ -629,9 +636,9 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 		const SensorData & sensorData,
 		int decimation,
 		float maxDepth,
-		float voxelSize,
-		int samples,
-		std::vector<int> * validIndices)
+		float minDepth,
+		std::vector<int> * validIndices,
+		const ParametersMap & parameters)
 {
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

@@ -652,36 +659,16 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 						sensorData.cameraModels()[i].fy(),
 						decimation,
 						maxDepth,
+						minDepth,
 						sensorData.cameraModels().size()==1?validIndices:0);

 				if(tmp->size())
 				{
-					bool filtered = false;
-
-					if(tmp->size() && voxelSize)
-					{
-						tmp = util3d::voxelize(tmp, voxelSize);
-						filtered = true;
-					}
-
-					if(tmp->size() && samples)
-					{
-						tmp = util3d::randomSampling(tmp, samples);
-						filtered = true;
-					}
-
-					if(tmp->size() && !filtered && sensorData.cameraModels().size() > 1)
-					{
-						tmp = util3d::removeNaNFromPointCloud(tmp);
-					}
-
-					if(tmp->size())
-					{
-						tmp = util3d::transformPointCloud(tmp, sensorData.cameraModels()[i].localTransform());
-					}
+					tmp = util3d::transformPointCloud(tmp, sensorData.cameraModels()[i].localTransform());

 					if(sensorData.cameraModels().size() > 1)
 					{
+						tmp = util3d::removeNaNFromPointCloud(tmp);
 						*cloud += *tmp;
 					}
 					else
@@ -695,11 +682,6 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 				UERROR("Camera model %d is invalid", i);
 			}
 		}
-
-		if(cloud->size() && voxelSize && sensorData.cameraModels().size() > 1)
-		{
-			cloud = util3d::voxelize(cloud, voxelSize);
-		}
 	}
 	else if(!sensorData.imageRaw().empty() && !sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValidForProjection())
 	{
@@ -716,19 +698,15 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 			leftMono = sensorData.imageRaw();
 		}
 		cloud = cloudFromDisparity(
-				util2d::disparityFromStereoImages(leftMono, sensorData.rightRaw()),
+				util2d::disparityFromStereoImages(leftMono, sensorData.rightRaw(), parameters),
 				sensorData.stereoCameraModel(),
 				decimation,
 				maxDepth,
+				minDepth,
 				validIndices);

 		if(cloud->size())
 		{
-			if(cloud->size() && voxelSize)
-			{
-				cloud = util3d::voxelize(cloud, voxelSize);
-			}
-
 			if(cloud->size())
 			{
 				cloud = util3d::transformPointCloud(cloud, sensorData.stereoCameraModel().left().localTransform());
@@ -742,9 +720,9 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 		const SensorData & sensorData,
 		int decimation,
 		float maxDepth,
-		float voxelSize,
-		int samples,
-		std::vector<int> * validIndices)
+		float minDepth,
+		std::vector<int> * validIndices,
+		const ParametersMap & parameters)
 {
 	UASSERT(!sensorData.imageRaw().empty());
 	UASSERT((!sensorData.depthRaw().empty() && sensorData.cameraModels().size()) ||
@@ -785,35 +763,16 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 						sensorData.cameraModels()[i].fy(),
 						decimation,
 						maxDepth,
+						minDepth,
 						sensorData.cameraModels().size() == 1?validIndices:0);

 				if(tmp->size())
 				{
-					bool filtered = false;
-					if(tmp->size() && voxelSize)
-					{
-						tmp = util3d::voxelize(tmp, voxelSize);
-						filtered = true;
-					}
-
-					if(tmp->size() && samples)
-					{
-						tmp = util3d::randomSampling(tmp, samples);
-						filtered = true;
-					}
-
-					if(tmp->size() && !filtered && sensorData.cameraModels().size() > 1)
-					{
-						tmp = util3d::removeNaNFromPointCloud(tmp);
-					}
-
-					if(tmp->size())
-					{
-						tmp = util3d::transformPointCloud(tmp, sensorData.cameraModels()[i].localTransform());
-					}
+					tmp = util3d::transformPointCloud(tmp, sensorData.cameraModels()[i].localTransform());

 					if(sensorData.cameraModels().size() > 1)
 					{
+						tmp = util3d::removeNaNFromPointCloud(tmp);
 						*cloud += *tmp;
 					}
 					else
@@ -828,11 +787,6 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 			}
 		}

-		if(cloud->size() && voxelSize && sensorData.cameraModels().size() > 1)
-		{
-			cloud = util3d::voxelize(cloud, voxelSize);
-		}
-
 		if(cloud->is_dense && validIndices)
 		{
 			//generate indices for all points (they are all valid)
@@ -852,19 +806,13 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 				sensorData.stereoCameraModel(),
 				decimation,
 				maxDepth,
-				validIndices);
+				minDepth,
+				validIndices,
+				parameters);

 		if(cloud->size())
 		{
-			if(cloud->size() && voxelSize)
-			{
-				cloud = util3d::voxelize(cloud, voxelSize);
-			}
-
-			if(cloud->size())
-			{
-				cloud = util3d::transformPointCloud(cloud, sensorData.stereoCameraModel().left().localTransform());
-			}
+			cloud = util3d::transformPointCloud(cloud, sensorData.stereoCameraModel().left().localTransform());
 		}
 	}
 	return cloud;
@@ -877,6 +825,7 @@ pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImage(
 					float cx,
 					float cy,
 					float maxDepth,
+					float minDepth,
 					const Transform & localTransform)
 {
 	UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);
@@ -891,7 +840,7 @@ pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImage(
 		for(int i=0; i<depthImage.cols; ++i)
 		{
 			pcl::PointXYZ pt = util3d::projectDepthTo3D(depthImage, i, middle, cx, cy, fx, fy, false);
-			if(pcl::isFinite(pt) && (maxDepth == 0 || pt.z < maxDepth))
+			if(pcl::isFinite(pt) && pt.z >= minDepth && (maxDepth == 0 || pt.z < maxDepth))
 			{
 				if(!localTransform.isIdentity())
 				{
@@ -1316,6 +1265,65 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr concatenateClouds(const std::list<pcl::Po
 	return cloud;
 }

+pcl::TextureMesh::Ptr concatenateTextureMeshes(const std::list<pcl::TextureMesh::Ptr> & meshes)
+{
+	pcl::TextureMesh::Ptr output(new pcl::TextureMesh);
+	std::map<std::string, int> addedMaterials; //<file, index>
+	for(std::list<pcl::TextureMesh::Ptr>::const_iterator iter = meshes.begin(); iter!=meshes.end(); ++iter)
+	{
+		// append point cloud
+		int polygonStep = output->cloud.height * output->cloud.width;
+		pcl::PCLPointCloud2 tmp;
+		pcl::concatenatePointCloud(output->cloud, iter->get()->cloud, tmp);
+		output->cloud = tmp;
+
+		UASSERT((*iter)->tex_polygons.size() == (*iter)->tex_coordinates.size() &&
+				(*iter)->tex_polygons.size() == (*iter)->tex_materials.size());
+
+		int materialCount = (*iter)->tex_polygons.size();
+		for(int i=0; i<materialCount; ++i)
+		{
+			std::map<std::string, int>::iterator jter = addedMaterials.find((*iter)->tex_materials[i].tex_file);
+			int index;
+			if(jter != addedMaterials.end())
+			{
+				index = jter->second;
+			}
+			else
+			{
+				addedMaterials.insert(std::make_pair((*iter)->tex_materials[i].tex_file, output->tex_materials.size()));
+				index = output->tex_materials.size();
+				output->tex_materials.push_back((*iter)->tex_materials[i]);
+				output->tex_materials.back().tex_name = uFormat("material_%d", index);
+				output->tex_polygons.resize(output->tex_polygons.size() + 1);
+				output->tex_coordinates.resize(output->tex_coordinates.size() + 1);
+			}
+
+			// update and append polygon indices
+			int oi = output->tex_polygons[index].size();
+			output->tex_polygons[index].resize(output->tex_polygons[index].size() + (*iter)->tex_polygons[i].size());
+			for(unsigned int j=0; j<(*iter)->tex_polygons[i].size(); ++j)
+			{
+				pcl::Vertices polygon = (*iter)->tex_polygons[i][j];
+				for(unsigned int k=0; k<polygon.vertices.size(); ++k)
+				{
+					polygon.vertices[k] += polygonStep;
+				}
+				output->tex_polygons[index][oi+j] = polygon;
+			}
+
+			// append uv coordinates
+			oi = output->tex_coordinates[index].size();
+			output->tex_coordinates[index].resize(output->tex_coordinates[index].size() + (*iter)->tex_coordinates[i].size());
+			for(unsigned int j=0; j<(*iter)->tex_coordinates[i].size(); ++j)
+			{
+				output->tex_coordinates[index][oi+j] = (*iter)->tex_coordinates[i][j];
+			}
+		}
+	}
+	return output;
+}
+
 pcl::IndicesPtr concatenate(const std::vector<pcl::IndicesPtr> & indices)
 {
 	//compute total size
@@ -100,7 +100,7 @@ std::vector<cv::Point3f> generateKeypoints3DDepth(

 			cv::Point3f pt(bad_point, bad_point, bad_point);
 			if(pcl::isFinite(ptXYZ) &&
-				(minDepth <= 0.0f || ptXYZ.z >= minDepth) &&
+				(minDepth < 0.0f || ptXYZ.z > minDepth) &&
 				(maxDepth <= 0.0f || ptXYZ.z <= maxDepth))
 			{
 				pt = cv::Point3f(ptXYZ.x, ptXYZ.y, ptXYZ.z);
@@ -137,7 +137,7 @@ std::vector<cv::Point3f> generateKeypoints3DDisparity(

 		cv::Point3f pt(bad_point, bad_point, bad_point);
 		if(util3d::isFinite(tmpPt) &&
-			(minDepth <= 0.0f || tmpPt.z >= minDepth) &&
+			(minDepth < 0.0f || tmpPt.z > minDepth) &&
 			(maxDepth <= 0.0f || tmpPt.z <= maxDepth))
 		{
 			pt = tmpPt;
@@ -181,7 +181,7 @@ std::vector<cv::Point3f> generateKeypoints3DStereo(
 						model);

 				if(util3d::isFinite(tmpPt) &&
-				   (minDepth <= 0.0f || tmpPt.z >= minDepth) &&
+				   (minDepth < 0.0f || tmpPt.z > minDepth) &&
 				   (maxDepth <= 0.0f || tmpPt.z <= maxDepth))
 				{
 					pt = tmpPt;
@@ -221,7 +221,7 @@ std::map<int, cv::Point3f> generateWords3DMono(
 	std::map<int, cv::Point3f> words3D;
 	std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
 	int pairsFound = EpipolarGeometry::findPairs(refWords, nextWords, pairs);
-	UDEBUG("pairsFound=%d", pairsFound);
+	UDEBUG("pairsFound=%d/%d", pairsFound, int(refWords.size()>nextWords.size()?refWords.size():nextWords.size()));
 	if(pairsFound > 8)
 	{
 		std::vector<unsigned char> status;
@@ -45,6 +45,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UMath.h>
 #include <rtabmap/utilite/UConversion.h>

+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+#include <pcl/impl/instantiate.hpp>
+#include <pcl/point_types.h>
+#include <pcl/segmentation/impl/extract_clusters.hpp>
+#include <pcl/segmentation/extract_labeled_clusters.h>
+#include <pcl/segmentation/impl/extract_labeled_clusters.hpp>
+
+PCL_INSTANTIATE(EuclideanClusterExtraction, (pcl::PointXYZRGBNormal))
+PCL_INSTANTIATE(extractEuclideanClusters, (pcl::PointXYZRGBNormal))
+PCL_INSTANTIATE(extractEuclideanClusters_indices, (pcl::PointXYZRGBNormal))
+#endif
+
 namespace rtabmap
 {

@@ -88,7 +100,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr downsample(
 	}
 	else
 	{
-		int finalSize = cloud->size()/step;
+		int finalSize = int(cloud->size())/step;
 		output->resize(finalSize);
 		int oi = 0;
 		for(unsigned int i=0; i<cloud->size()-step+1; i+=step)
@@ -111,7 +123,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr downsample(
 	}
 	else
 	{
-		int finalSize = cloud->size()/step;
+		int finalSize = int(cloud->size())/step;
 		output->resize(finalSize);
 		int oi = 0;
 		for(int i=0; i<(int)cloud->size()-step+1; i+=step)
@@ -244,6 +256,48 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr randomSampling(
 	return output;
 }

+pcl::IndicesPtr passThrough(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::string & axis,
+		float min,
+		float max,
+		bool negative)
+{
+	UASSERT(max > min);
+	UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
+
+	pcl::IndicesPtr output(new std::vector<int>);
+	pcl::PassThrough<pcl::PointXYZ> filter;
+	filter.setNegative(negative);
+	filter.setFilterFieldName(axis);
+	filter.setFilterLimits(min, max);
+	filter.setInputCloud(cloud);
+	filter.setIndices(indices);
+	filter.filter(*output);
+	return output;
+}
+pcl::IndicesPtr passThrough(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::string & axis,
+		float min,
+		float max,
+		bool negative)
+{
+	UASSERT(max > min);
+	UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
+
+	pcl::IndicesPtr output(new std::vector<int>);
+	pcl::PassThrough<pcl::PointXYZRGB> filter;
+	filter.setNegative(negative);
+	filter.setFilterFieldName(axis);
+	filter.setFilterLimits(min, max);
+	filter.setInputCloud(cloud);
+	filter.setIndices(indices);
+	filter.filter(*output);
+	return output;
+}

 pcl::PointCloud<pcl::PointXYZ>::Ptr passThrough(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
@@ -1219,21 +1273,21 @@ pcl::IndicesPtr normalFiltering(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		float angleMax,
 		const Eigen::Vector4f & normal,
-		float radiusSearch,
+		int normalKSearch,
 		const Eigen::Vector4f & viewpoint)
 {
 	pcl::IndicesPtr indices(new std::vector<int>);
-	return normalFiltering(cloud, indices, angleMax, normal, radiusSearch, viewpoint);
+	return normalFiltering(cloud, indices, angleMax, normal, normalKSearch, viewpoint);
 }
 pcl::IndicesPtr normalFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		float angleMax,
 		const Eigen::Vector4f & normal,
-		float radiusSearch,
+		int normalKSearch,
 		const Eigen::Vector4f & viewpoint)
 {
 	pcl::IndicesPtr indices(new std::vector<int>);
-	return normalFiltering(cloud, indices, angleMax, normal, radiusSearch, viewpoint);
+	return normalFiltering(cloud, indices, angleMax, normal, normalKSearch, viewpoint);
 }


@@ -1243,7 +1297,7 @@ pcl::IndicesPtr normalFiltering(
 		const pcl::IndicesPtr & indices,
 		float angleMax,
 		const Eigen::Vector4f & normal,
-		float radiusSearch,
+		int normalKSearch,
 		const Eigen::Vector4f & viewpoint)
 {
 	pcl::IndicesPtr output(new std::vector<int>());
@@ -1274,7 +1328,7 @@ pcl::IndicesPtr normalFiltering(

 		pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);

-		ne.setRadiusSearch (radiusSearch);
+		ne.setKSearch(normalKSearch);
 		if(viewpoint[0] != 0 || viewpoint[1] != 0 || viewpoint[2] != 0)
 		{
 			ne.setViewPoint(viewpoint[0], viewpoint[1], viewpoint[2]);
@@ -1303,7 +1357,7 @@ pcl::IndicesPtr normalFiltering(
 		const pcl::IndicesPtr & indices,
 		float angleMax,
 		const Eigen::Vector4f & normal,
-		float radiusSearch,
+		int normalKSearch,
 		const Eigen::Vector4f & viewpoint)
 {
 	pcl::IndicesPtr output(new std::vector<int>());
@@ -1334,7 +1388,7 @@ pcl::IndicesPtr normalFiltering(

 		pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);

-		ne.setRadiusSearch (radiusSearch);
+		ne.setKSearch (normalKSearch);
 		if(viewpoint[0] != 0 || viewpoint[1] != 0 || viewpoint[2] != 0)
 		{
 			ne.setViewPoint(viewpoint[0], viewpoint[1], viewpoint[2]);
@@ -1363,7 +1417,7 @@ pcl::IndicesPtr normalFiltering(
 		const pcl::IndicesPtr & indices,
 		float angleMax,
 		const Eigen::Vector4f & normal,
-		float radiusSearch,
+		int normalKSearch,
 		const Eigen::Vector4f & viewpoint)
 {
 	pcl::IndicesPtr output(new std::vector<int>());
@@ -46,6 +46,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "pcl18/surface/organized_fast_mesh.h"
 #else
 #include <pcl/surface/organized_fast_mesh.h>
+#include <pcl/surface/impl/marching_cubes.hpp>
+#include <pcl/surface/impl/organized_fast_mesh.hpp>
+#include <pcl/impl/instantiate.hpp>
+#include <pcl/point_types.h>
+
+// Instantiations of specific point types
+PCL_INSTANTIATE(OrganizedFastMesh, (pcl::PointXYZRGBNormal))
+
+#include <pcl/features/impl/normal_3d_omp.hpp>
+PCL_INSTANTIATE_PRODUCT(NormalEstimationOMP, ((pcl::PointXYZRGB))((pcl::Normal)))
 #endif

 namespace rtabmap
@@ -177,10 +187,10 @@ void appendMesh(
 	UDEBUG("cloudA=%d polygonsA=%d cloudB=%d polygonsB=%d", (int)cloudA.size(), (int)polygonsA.size(), (int)cloudB.size(), (int)polygonsB.size());
 	UASSERT(!cloudA.isOrganized() && !cloudB.isOrganized());

-	int sizeA = cloudA.size();
+	int sizeA = (int)cloudA.size();
 	cloudA += cloudB;

-	int sizePolygonsA = polygonsA.size();
+	int sizePolygonsA = (int)polygonsA.size();
 	polygonsA.resize(sizePolygonsA+polygonsB.size());

 	for(unsigned int i=0; i<polygonsB.size(); ++i)
@@ -194,7 +204,33 @@ void appendMesh(
 	}
 }

-void filterNotUsedVerticesFromMesh(
+void appendMesh(
+		pcl::PointCloud<pcl::PointXYZRGB> & cloudA,
+		std::vector<pcl::Vertices> & polygonsA,
+		const pcl::PointCloud<pcl::PointXYZRGB> & cloudB,
+		const std::vector<pcl::Vertices> & polygonsB)
+{
+	UDEBUG("cloudA=%d polygonsA=%d cloudB=%d polygonsB=%d", (int)cloudA.size(), (int)polygonsA.size(), (int)cloudB.size(), (int)polygonsB.size());
+	UASSERT(!cloudA.isOrganized() && !cloudB.isOrganized());
+
+	int sizeA = (int)cloudA.size();
+	cloudA += cloudB;
+
+	int sizePolygonsA = (int)polygonsA.size();
+	polygonsA.resize(sizePolygonsA+polygonsB.size());
+
+	for(unsigned int i=0; i<polygonsB.size(); ++i)
+	{
+		pcl::Vertices vertices = polygonsB[i];
+		for(unsigned int j=0; j<vertices.vertices.size(); ++j)
+		{
+			vertices.vertices[j] += sizeA;
+		}
+		polygonsA[i+sizePolygonsA] = vertices;
+	}
+}
+
+std::map<int, int> filterNotUsedVerticesFromMesh(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
 		const std::vector<pcl::Vertices> & polygons,
 		pcl::PointCloud<pcl::PointXYZRGBNormal> & outputCloud,
@@ -202,7 +238,9 @@ void filterNotUsedVerticesFromMesh(
 {
 	UDEBUG("size=%d polygons=%d", (int)cloud.size(), (int)polygons.size());
 	std::map<int, int> addedVertices; //<oldIndex, newIndex>
+	std::map<int, int> output; //<newIndex, oldIndex>
 	outputCloud.resize(cloud.size());
+	outputCloud.is_dense = true;
 	outputPolygons.resize(polygons.size());
 	int oi = 0;
 	for(unsigned int i=0; i<polygons.size(); ++i)
@@ -216,6 +254,7 @@ void filterNotUsedVerticesFromMesh(
 			{
 				outputCloud[oi] = cloud.at(polygons[i].vertices[j]);
 				addedVertices.insert(std::make_pair(polygons[i].vertices[j], oi));
+				output.insert(std::make_pair(oi, polygons[i].vertices[j]));
 				v.vertices[j] = oi++;
 			}
 			else
@@ -225,13 +264,53 @@ void filterNotUsedVerticesFromMesh(
 		}
 	}
 	outputCloud.resize(oi);
+
+	return output;
+}
+
+std::map<int, int> filterNotUsedVerticesFromMesh(
+		const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
+		const std::vector<pcl::Vertices> & polygons,
+		pcl::PointCloud<pcl::PointXYZRGB> & outputCloud,
+		std::vector<pcl::Vertices> & outputPolygons)
+{
+	UDEBUG("size=%d polygons=%d", (int)cloud.size(), (int)polygons.size());
+	std::map<int, int> addedVertices; //<oldIndex, newIndex>
+	std::map<int, int> output; //<newIndex, oldIndex>
+	outputCloud.resize(cloud.size());
+	outputCloud.is_dense = true;
+	outputPolygons.resize(polygons.size());
+	int oi = 0;
+	for(unsigned int i=0; i<polygons.size(); ++i)
+	{
+		pcl::Vertices & v = outputPolygons[i];
+		v.vertices.resize(polygons[i].vertices.size());
+		for(unsigned int j=0; j<polygons[i].vertices.size(); ++j)
+		{
+			std::map<int, int>::iterator iter = addedVertices.find(polygons[i].vertices[j]);
+			if(iter == addedVertices.end())
+			{
+				outputCloud[oi] = cloud.at(polygons[i].vertices[j]);
+				addedVertices.insert(std::make_pair(polygons[i].vertices[j], oi));
+				output.insert(std::make_pair(oi, polygons[i].vertices[j]));
+				v.vertices[j] = oi++;
+			}
+			else
+			{
+				v.vertices[j] = iter->second;
+			}
+		}
+	}
+	outputCloud.resize(oi);
+
+	return output;
 }

 std::vector<pcl::Vertices> filterCloseVerticesFromMesh(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud,
 		const std::vector<pcl::Vertices> & polygons,
 		float radius,
-		float angle,
+		float angle, // FIXME angle not used
 		bool keepLatestInRadius)
 {
 	UDEBUG("size=%d polygons=%d radius=%f angle=%f keepLatest=%d",
@@ -423,21 +502,14 @@ pcl::TextureMesh::Ptr createTextureMesh(
 		const std::map<int, cv::Mat> & images,
 		const std::string & tmpDirectory)
 {
+	pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
+	textureMesh->cloud = mesh->cloud;
+	textureMesh->tex_polygons.push_back(mesh->polygons);
+
 	// Original from pcl/gpu/kinfu_large_scale/tools/standalone_texture_mapping.cpp:
 	// Author: Raphael Favier, Technical University Eindhoven, (r.mysurname <aT> tue.nl)

 	// Create the texturemesh object that will contain our UV-mapped mesh
-	pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
-	textureMesh->cloud = mesh->cloud;
-	std::vector< pcl::Vertices> polygons;
-
-	// push faces into the texturemesh object
-	polygons.resize (mesh->polygons.size ());
-	for(size_t i =0; i < mesh->polygons.size (); ++i)
-	{
-		polygons[i] = mesh->polygons[i];
-	}
-	textureMesh->tex_polygons.push_back(polygons);

 	// create cameras
 	pcl::texture_mapping::CameraVector cameras = createTextureCameras(
@@ -493,7 +565,7 @@ pcl::TextureMesh::Ptr createTextureMesh(
 		textureMesh->tex_materials[i] = mesh_material;
 	}

-	// Sort faces
+	// Texture by projection
 	pcl::TextureMapping<pcl::PointXYZ> tm; // TextureMapping object that will perform the sort
 	tm.textureMeshwithMultipleCameras(*textureMesh, cameras);

@@ -1,11 +1,36 @@
+cmake_minimum_required(VERSION 2.8)
+
+IF(DEFINED PROJECT_NAME)
+   set(internal TRUE)
+ENDIF(DEFINED PROJECT_NAME)
+
+if(internal)
+	# inside rtabmap project (see below for external build)
+	SET(RTABMap_INCLUDE_DIRS 
+	    ${PROJECT_SOURCE_DIR}/utilite/include
+		${PROJECT_SOURCE_DIR}/corelib/include
+	)
+	SET(RTABMap_LIBRARIES 
+	    rtabmap_core
+		rtabmap_utilite
+	)  
+else()
+	# external build
+	PROJECT( MyProject )
+	
+	FIND_PACKAGE(RTABMap REQUIRED)
+	FIND_PACKAGE(OpenCV REQUIRED)
+	FIND_PACKAGE(PCL 1.7 REQUIRED)
+endif()
+
 SET(INCLUDE_DIRS
-	${PROJECT_SOURCE_DIR}/corelib/include
-	${PROJECT_SOURCE_DIR}/utilite/include
+	${RTABMap_INCLUDE_DIRS}
    ${OpenCV_INCLUDE_DIRS}
    ${PCL_INCLUDE_DIRS}
 )

 SET(LIBRARIES
+    ${RTABMap_LIBRARIES}
 	${OpenCV_LIBRARIES} 
 	${PCL_LIBRARIES}
 )
@@ -13,7 +38,9 @@ SET(LIBRARIES
 INCLUDE_DIRECTORIES(${INCLUDE_DIRS})

 ADD_EXECUTABLE(bow_mapping main.cpp)
-TARGET_LINK_LIBRARIES(bow_mapping rtabmap_core rtabmap_utilite ${LIBRARIES})
+TARGET_LINK_LIBRARIES(bow_mapping ${LIBRARIES})

-SET_TARGET_PROPERTIES( bow_mapping 
-  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-bow_mapping)
+if(internal)
+  SET_TARGET_PROPERTIES( bow_mapping 
+    PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-bow_mapping)
+endif(internal)
@@ -1,17 +1,48 @@
+cmake_minimum_required(VERSION 2.8)
+
+IF(DEFINED PROJECT_NAME)
+   set(internal TRUE)
+ENDIF(DEFINED PROJECT_NAME)
+
+if(internal)
+	# inside rtabmap project (see below for external build)
+	SET(RTABMap_INCLUDE_DIRS 
+	    ${PROJECT_SOURCE_DIR}/utilite/include
+		${PROJECT_SOURCE_DIR}/corelib/include
+		${PROJECT_SOURCE_DIR}/guilib/include
+	)
+	SET(RTABMap_LIBRARIES 
+	    rtabmap_core
+		rtabmap_gui
+		rtabmap_utilite
+	)  
+else()
+	# external build
+	PROJECT( MyProject )
+	
+	FIND_PACKAGE(RTABMap REQUIRED)
+	FIND_PACKAGE(OpenCV REQUIRED)
+	FIND_PACKAGE(PCL 1.7 REQUIRED)
+	
+	# Find Qt5 first
+	FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui Svg QUIET)
+    	IF(NOT Qt5_FOUND)
+       		FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg)
+    	ENDIF(NOT Qt5_FOUND)
+endif()

 SET(INCLUDE_DIRS
-	${PROJECT_SOURCE_DIR}/utilite/include
-	${PROJECT_SOURCE_DIR}/corelib/include
-	${PROJECT_SOURCE_DIR}/guilib/include
+	${RTABMap_INCLUDE_DIRS}
    ${OpenCV_INCLUDE_DIRS}
    ${PCL_INCLUDE_DIRS}
 )

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

 SET(LIBRARIES
+	${RTABMap_LIBRARIES}
 	${OpenCV_LIBRARIES}
 	${QT_LIBRARIES} 
 	${PCL_LIBRARIES}
@@ -19,7 +50,7 @@ SET(LIBRARIES

 INCLUDE_DIRECTORIES(${INCLUDE_DIRS})

-IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
+IF(QT4_FOUND)
    QT4_WRAP_CPP(moc_srcs MapBuilder.h)
 ELSE()
    QT5_WRAP_CPP(moc_srcs MapBuilder.h)
@@ -27,7 +58,13 @@ ENDIF()

 ADD_EXECUTABLE(noEventsExample main.cpp ${moc_srcs})
  
-TARGET_LINK_LIBRARIES(noEventsExample rtabmap_core rtabmap_gui rtabmap_utilite ${LIBRARIES})
+TARGET_LINK_LIBRARIES(noEventsExample ${LIBRARIES})
+
+if(internal)
+	SET_TARGET_PROPERTIES( noEventsExample 
+	  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-noEventsExample)
+endif(internal)
+
+
+

-SET_TARGET_PROPERTIES( noEventsExample 
-  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-noEventsExample)
@@ -1,17 +1,48 @@
+cmake_minimum_required(VERSION 2.8)
+
+IF(DEFINED PROJECT_NAME)
+   set(internal TRUE)
+ENDIF(DEFINED PROJECT_NAME)
+
+if(internal)
+	# inside rtabmap project (see below for external build)
+	SET(RTABMap_INCLUDE_DIRS 
+	    ${PROJECT_SOURCE_DIR}/utilite/include
+		${PROJECT_SOURCE_DIR}/corelib/include
+		${PROJECT_SOURCE_DIR}/guilib/include
+	)
+	SET(RTABMap_LIBRARIES 
+	    rtabmap_core
+		rtabmap_gui
+		rtabmap_utilite
+	)  
+else()
+	# external build
+	PROJECT( MyProject )
+	
+	FIND_PACKAGE(RTABMap REQUIRED)
+	FIND_PACKAGE(OpenCV REQUIRED)
+	FIND_PACKAGE(PCL 1.7 REQUIRED)
+	
+	# Find Qt5 first
+	FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui Svg QUIET)
+    	IF(NOT Qt5_FOUND)
+       		FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg)
+    	ENDIF(NOT Qt5_FOUND)
+endif()

 SET(INCLUDE_DIRS
-	${PROJECT_SOURCE_DIR}/utilite/include
-	${PROJECT_SOURCE_DIR}/corelib/include
-	${PROJECT_SOURCE_DIR}/guilib/include
+	${RTABMap_INCLUDE_DIRS}
    ${OpenCV_INCLUDE_DIRS}
    ${PCL_INCLUDE_DIRS}
 )

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

 SET(LIBRARIES
+	${RTABMap_LIBRARIES}
 	${OpenCV_LIBRARIES}
 	${QT_LIBRARIES} 
 	${PCL_LIBRARIES}
@@ -19,7 +50,7 @@ SET(LIBRARIES

 INCLUDE_DIRECTORIES(${INCLUDE_DIRS})

-IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
+IF(QT4_FOUND)
    QT4_WRAP_CPP(moc_srcs MapBuilder.h)
 ELSE()
    QT5_WRAP_CPP(moc_srcs MapBuilder.h)
@@ -27,7 +58,9 @@ ENDIF()

 ADD_EXECUTABLE(rgbd_mapping main.cpp ${moc_srcs})
  
-TARGET_LINK_LIBRARIES(rgbd_mapping rtabmap_core rtabmap_gui rtabmap_utilite ${LIBRARIES})
+TARGET_LINK_LIBRARIES(rgbd_mapping ${LIBRARIES})

-SET_TARGET_PROPERTIES( rgbd_mapping 
-  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-rgbd_mapping)
+if(internal)
+  SET_TARGET_PROPERTIES( rgbd_mapping 
+    PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-rgbd_mapping)
+endif(internal)
@@ -31,9 +31,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/CameraRGBD.h"
 #include "rtabmap/core/CameraThread.h"
 #include "rtabmap/core/OdometryThread.h"
+#include "rtabmap/core/Graph.h"
 #include "rtabmap/utilite/UEventsManager.h"
 #include <QApplication>
 #include <stdio.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/io/ply_io.h>

 #include "MapBuilder.h"

@@ -167,5 +170,51 @@ int main(int argc, char * argv[])
 	odomThread.join(true);
 	rtabmapThread.join(true);

+	// Save 3D map
+	printf("Saving rtabmap_cloud.pcd...\n");
+	std::map<int, Signature> nodes;
+	std::map<int, Transform> optimizedPoses;
+	std::multimap<int, Link> links;
+	rtabmap->get3DMap(nodes, optimizedPoses, links, true, true);
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+	for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
+	{
+		Signature node = nodes.find(iter->first)->second;
+
+		// uncompress data
+		node.sensorData().uncompressData();
+
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp = util3d::cloudRGBFromSensorData(
+				node.sensorData(),
+				4,           // image decimation before creating the clouds
+				4.0f,        // maximum depth of the cloud
+				0.01f);  // Voxel grid filtering
+		*cloud += *util3d::transformPointCloud(tmp, iter->second); // transform the point cloud to its pose
+	}
+	if(cloud->size())
+	{
+		printf("Voxel grid filtering of the assembled cloud (voxel=%f, %d points)\n", 0.01f, (int)cloud->size());
+		cloud = util3d::voxelize(cloud, 0.01f);
+
+		printf("Saving rtabmap_cloud.pcd... done! (%d points)\n", (int)cloud->size());
+		pcl::io::savePCDFile("rtabmap_cloud.pcd", *cloud);
+		//pcl::io::savePLYFile("rtabmap_cloud.ply", *cloud); // to save in PLY format
+	}
+	else
+	{
+		printf("Saving rtabmap_cloud.pcd... failed! The cloud is empty.\n");
+	}
+
+	// Save trajectory
+	printf("Saving rtabmap_trajectory.txt ...\n");
+	if(optimizedPoses.size() && graph::exportPoses("rtabmap_trajectory.txt", 0, optimizedPoses, links))
+	{
+		printf("Saving rtabmap_trajectory.txt... done!\n");
+	}
+	else
+	{
+		printf("Saving rtabmap_trajectory.txt... failed!\n");
+	}
+
 	return 0;
 }
@@ -1,20 +1,48 @@
+cmake_minimum_required(VERSION 2.8)

-SET(srcs
-    main.cpp)
+IF(DEFINED PROJECT_NAME)
+   set(internal TRUE)
+ENDIF(DEFINED PROJECT_NAME)
+
+if(internal)
+	# inside rtabmap project (see below for external build)
+	SET(RTABMap_INCLUDE_DIRS 
+	    ${PROJECT_SOURCE_DIR}/utilite/include
+		${PROJECT_SOURCE_DIR}/corelib/include
+		${PROJECT_SOURCE_DIR}/guilib/include
+	)
+	SET(RTABMap_LIBRARIES 
+	    rtabmap_core
+		rtabmap_gui
+		rtabmap_utilite
+	)  
+else()
+	# external build
+	PROJECT( MyProject )
+	
+	FIND_PACKAGE(RTABMap REQUIRED)
+	FIND_PACKAGE(OpenCV REQUIRED)
+	FIND_PACKAGE(PCL 1.7 REQUIRED)
+	
+	# Find Qt5 first
+	FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui Svg QUIET)
+    	IF(NOT Qt5_FOUND)
+       		FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg)
+    	ENDIF(NOT Qt5_FOUND)
+endif()

 SET(INCLUDE_DIRS
-	${PROJECT_SOURCE_DIR}/utilite/include
-	${PROJECT_SOURCE_DIR}/corelib/include
-	${PROJECT_SOURCE_DIR}/guilib/include
+	${RTABMap_INCLUDE_DIRS}
    ${OpenCV_INCLUDE_DIRS}
    ${PCL_INCLUDE_DIRS}
 )

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

 SET(LIBRARIES
+	${RTABMap_LIBRARIES}
 	${OpenCV_LIBRARIES}
 	${QT_LIBRARIES} 
 	${PCL_LIBRARIES}
@@ -22,12 +50,15 @@ SET(LIBRARIES

 INCLUDE_DIRECTORIES(${INCLUDE_DIRS})

-IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
-    QT4_WRAP_CPP(moc_srcs ../RGBDMapping/MapBuilder.h MapBuilderWifi.h)
+IF(QT4_FOUND)
+    QT4_WRAP_CPP(moc_srcs MapBuilder.h MapBuilderWifi.h)
 ELSE()
-    QT5_WRAP_CPP(moc_srcs ../RGBDMapping/MapBuilder.h MapBuilderWifi.h)
+    QT5_WRAP_CPP(moc_srcs MapBuilder.h MapBuilderWifi.h)
 ENDIF()

+SET(srcs
+    main.cpp)
+
 IF(APPLE)
    FIND_LIBRARY(CoreWLAN_LIBRARY CoreWLAN)
    FIND_LIBRARY(Foundation_LIBRARY Foundation)
@@ -44,7 +75,9 @@ IF(APPLE)
 ENDIF(APPLE)

 ADD_EXECUTABLE(wifi_mapping ${srcs} ${moc_srcs})
-TARGET_LINK_LIBRARIES(wifi_mapping rtabmap_core rtabmap_gui rtabmap_utilite ${LIBRARIES})
+TARGET_LINK_LIBRARIES(wifi_mapping ${LIBRARIES})

-SET_TARGET_PROPERTIES( wifi_mapping 
-  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-wifi_mapping)
+if(internal)
+  SET_TARGET_PROPERTIES( wifi_mapping 
+    PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-wifi_mapping)
+endif(internal)
@@ -0,0 +1,288 @@
+/*
+Copyright (c) 2010-2014, 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 MAPBUILDER_H_
+#define MAPBUILDER_H_
+
+#include <QVBoxLayout>
+#include <QtCore/QMetaType>
+#include <QAction>
+
+#ifndef Q_MOC_RUN // Mac OS X issue
+#include "rtabmap/gui/CloudViewer.h"
+#include "rtabmap/core/util3d.h"
+#include "rtabmap/core/util3d_filtering.h"
+#include "rtabmap/core/util3d_transforms.h"
+#include "rtabmap/core/RtabmapEvent.h"
+#endif
+#include "rtabmap/utilite/UStl.h"
+#include "rtabmap/utilite/UConversion.h"
+#include "rtabmap/utilite/UEventsHandler.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/core/CameraThread.h"
+
+using namespace rtabmap;
+
+// This class receives RtabmapEvent and construct/update a 3D Map
+class MapBuilder : public QWidget, public UEventsHandler
+{
+	Q_OBJECT
+public:
+	//Camera ownership is not transferred!
+	MapBuilder(CameraThread * camera = 0) :
+		camera_(camera),
+		odometryCorrection_(Transform::getIdentity()),
+		processingStatistics_(false),
+		lastOdometryProcessed_(true)
+	{
+		this->setWindowFlags(Qt::Dialog);
+		this->setWindowTitle(tr("3D Map"));
+		this->setMinimumWidth(800);
+		this->setMinimumHeight(600);
+
+		cloudViewer_ = new CloudViewer(this);
+
+		QVBoxLayout *layout = new QVBoxLayout();
+		layout->addWidget(cloudViewer_);
+		this->setLayout(layout);
+
+		qRegisterMetaType<rtabmap::OdometryEvent>("rtabmap::OdometryEvent");
+		qRegisterMetaType<rtabmap::Statistics>("rtabmap::Statistics");
+
+		QAction * pause = new QAction(this);
+		this->addAction(pause);
+		pause->setShortcut(Qt::Key_Space);
+		connect(pause, SIGNAL(triggered()), this, SLOT(pauseDetection()));
+	}
+
+	virtual ~MapBuilder()
+	{
+		this->unregisterFromEventsManager();
+	}
+
+protected slots:
+	virtual void pauseDetection()
+	{
+		UWARN("");
+		if(camera_)
+		{
+			if(camera_->isCapturing())
+			{
+				camera_->join(true);
+			}
+			else
+			{
+				camera_->start();
+			}
+		}
+	}
+
+	virtual void processOdometry(const rtabmap::OdometryEvent & odom)
+	{
+		if(!this->isVisible())
+		{
+			return;
+		}
+
+		Transform pose = odom.pose();
+		if(pose.isNull())
+		{
+			//Odometry lost
+			cloudViewer_->setBackgroundColor(Qt::darkRed);
+
+			pose = lastOdomPose_;
+		}
+		else
+		{
+			cloudViewer_->setBackgroundColor(cloudViewer_->getDefaultBackgroundColor());
+		}
+		if(!pose.isNull())
+		{
+			lastOdomPose_ = pose;
+
+			// 3d cloud
+			if(odom.data().depthOrRightRaw().cols == odom.data().imageRaw().cols &&
+			   odom.data().depthOrRightRaw().rows == odom.data().imageRaw().rows &&
+			   !odom.data().depthOrRightRaw().empty() &&
+			   (odom.data().stereoCameraModel().isValidForProjection() || odom.data().cameraModels().size()))
+			{
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(
+					odom.data(),
+					2,     // decimation
+					4.0f); // max depth
+				if(cloud->size())
+				{
+					if(!cloudViewer_->addCloud("cloudOdom", cloud, odometryCorrection_*pose))
+					{
+						UERROR("Adding cloudOdom to viewer failed!");
+					}
+				}
+				else
+				{
+					cloudViewer_->setCloudVisibility("cloudOdom", false);
+					UWARN("Empty cloudOdom!");
+				}
+			}
+
+			if(!odom.pose().isNull())
+			{
+				// update camera position
+				cloudViewer_->updateCameraTargetPosition(odometryCorrection_*odom.pose());
+			}
+		}
+		cloudViewer_->update();
+
+		lastOdometryProcessed_ = true;
+	}
+
+
+	virtual void processStatistics(const rtabmap::Statistics & stats)
+	{
+		processingStatistics_ = true;
+
+		//============================
+		// Add RGB-D clouds
+		//============================
+		const std::map<int, Transform> & poses = stats.poses();
+		QMap<std::string, Transform> clouds = cloudViewer_->getAddedClouds();
+		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+		{
+			if(!iter->second.isNull())
+			{
+				std::string cloudName = uFormat("cloud%d", iter->first);
+
+				// 3d point cloud
+				if(clouds.contains(cloudName))
+				{
+					// Update only if the pose has changed
+					Transform tCloud;
+					cloudViewer_->getPose(cloudName, tCloud);
+					if(tCloud.isNull() || iter->second != tCloud)
+					{
+						if(!cloudViewer_->updateCloudPose(cloudName, iter->second))
+						{
+							UERROR("Updating pose cloud %d failed!", iter->first);
+						}
+					}
+					cloudViewer_->setCloudVisibility(cloudName, true);
+				}
+				else if(uContains(stats.getSignatures(), iter->first))
+				{
+					Signature s = stats.getSignatures().at(iter->first);
+					s.sensorData().uncompressData(); // make sure data is uncompressed
+					// Add the new cloud
+					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(
+							s.sensorData(),
+						    4,     // decimation
+						    4.0f); // max depth
+					if(cloud->size())
+					{
+						if(!cloudViewer_->addCloud(cloudName, cloud, iter->second))
+						{
+							UERROR("Adding cloud %d to viewer failed!", iter->first);
+						}
+					}
+					else
+					{
+						UWARN("Empty cloud %d!", iter->first);
+					}
+				}
+			}
+			else
+			{
+				UWARN("Null pose for %d ?!?", iter->first);
+			}
+		}
+
+		//============================
+		// Add 3D graph (show all poses)
+		//============================
+		cloudViewer_->removeAllGraphs();
+		cloudViewer_->removeCloud("graph_nodes");
+		if(poses.size())
+		{
+			// Set graph
+			pcl::PointCloud<pcl::PointXYZ>::Ptr graph(new pcl::PointCloud<pcl::PointXYZ>);
+			pcl::PointCloud<pcl::PointXYZ>::Ptr graphNodes(new pcl::PointCloud<pcl::PointXYZ>);
+			for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+			{
+				graph->push_back(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()));
+			}
+			*graphNodes = *graph;
+
+
+			// add graph
+			cloudViewer_->addOrUpdateGraph("graph", graph, Qt::gray);
+			cloudViewer_->addCloud("graph_nodes", graphNodes, Transform::getIdentity(), Qt::green);
+			cloudViewer_->setCloudPointSize("graph_nodes", 5);
+		}
+
+		odometryCorrection_ = stats.mapCorrection();
+
+		cloudViewer_->update();
+
+		processingStatistics_ = false;
+	}
+
+	virtual void handleEvent(UEvent * event)
+	{
+		if(event->getClassName().compare("RtabmapEvent") == 0)
+		{
+			RtabmapEvent * rtabmapEvent = (RtabmapEvent *)event;
+			const Statistics & stats = rtabmapEvent->getStats();
+			// Statistics must be processed in the Qt thread
+			if(this->isVisible())
+			{
+				QMetaObject::invokeMethod(this, "processStatistics", Q_ARG(rtabmap::Statistics, stats));
+			}
+		}
+		else if(event->getClassName().compare("OdometryEvent") == 0)
+		{
+			OdometryEvent * odomEvent = (OdometryEvent *)event;
+			// Odometry must be processed in the Qt thread
+			if(this->isVisible() &&
+			   lastOdometryProcessed_ &&
+			   !processingStatistics_)
+			{
+				lastOdometryProcessed_ = false; // if we receive too many odometry events!
+				QMetaObject::invokeMethod(this, "processOdometry", Q_ARG(rtabmap::OdometryEvent, *odomEvent));
+			}
+		}
+	}
+
+protected:
+	CloudViewer * cloudViewer_;
+	CameraThread * camera_;
+	Transform lastOdomPose_;
+	Transform odometryCorrection_;
+	bool processingStatistics_;
+	bool lastOdometryProcessed_;
+};
+
+
+#endif /* MAPBUILDER_H_ */
@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef MAPBUILDERWIFI_H_
 #define MAPBUILDERWIFI_H_

-#include "../RGBDMapping/MapBuilder.h"
+#include "MapBuilder.h"
 #include "rtabmap/core/UserDataEvent.h"

 using namespace rtabmap;
@@ -55,6 +55,7 @@ public:
 	const rtabmap::CameraModel & getLeftCameraModel() const {return models_[0];}
 	const rtabmap::CameraModel & getRightCameraModel() const {return models_[1];}
 	const rtabmap::StereoCameraModel & getStereoCameraModel() const {return stereoModel_;}
+	bool isProcessing() const {return processingData_;}

 	void saveSettings(QSettings & settings, const QString & group = "") const;
 	void loadSettings(QSettings & settings, const QString & group = "");
@@ -68,6 +69,7 @@ public slots:
 	void setBoardWidth(int width);
 	void setBoardHeight(int height);
 	void setSquareSize(double size);
+	void setMaxScale(int scale);

 private slots:
 	void processImages(const cv::Mat & imageLeft, const cv::Mat & imageRight, const QString & cameraName);
@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UEventsHandler.h>
 #include <QDialog>
 #include <rtabmap/core/SensorData.h>
+#include <rtabmap/core/Parameters.h>

 class QSpinBox;
 class QCheckBox;
@@ -47,7 +48,9 @@ class RTABMAPGUI_EXP CameraViewer : public QDialog, public UEventsHandler
 {
 	Q_OBJECT
 public:
-	CameraViewer(QWidget * parent = 0);
+	CameraViewer(
+		QWidget * parent = 0,
+		const ParametersMap & parameters = ParametersMap());
 	virtual ~CameraViewer();

 public slots:
@@ -61,6 +64,7 @@ private:
 	bool processingImages_;
 	QSpinBox * decimationSpin_;
 	int validDecimationValue_;
+	ParametersMap parameters_;
 	QPushButton * pause_;
 	QCheckBox * showCloudCheckbox_;
 	QCheckBox * showScanCheckbox_;
@@ -142,7 +142,8 @@ public:
 	void removeOccupancyGridMap();

 	void updateCameraTargetPosition(
-		const Transform & pose);
+		const Transform & pose,
+		const Transform & localTransform = Transform::getIdentity());

 	void addOrUpdateCoordinate(
 			const std::string & id,
@@ -154,6 +155,15 @@ public:
 	void removeCoordinate(const std::string & id);
 	void removeAllCoordinates();

+	void addOrUpdateLine(
+				const std::string & id,
+				const Transform & from,
+				const Transform & to,
+				const QColor & color,
+				bool arrow = false);
+	void removeLine(const std::string & id);
+	void removeAllLines();
+
 	void addOrUpdateFrustum(
 			const std::string & id,
 			const Transform & transform,
@@ -206,6 +216,8 @@ public:
 	Transform getTargetPose() const;

 	void setBackfaceCulling(bool enabled, bool frontfaceCulling);
+	void setRenderingRate(double rate);
+	double getRenderingRate() const;

 	void getCameraPosition(
 			float & x, float & y, float & z,
@@ -275,10 +287,12 @@ private:
    QAction * _aSetGridCellCount;
    QAction * _aSetGridCellSize;
    QAction * _aSetBackgroundColor;
+    QAction * _aSetRenderingRate;
    QMenu * _menu;
    std::set<std::string> _graphes;
    std::set<std::string> _coordinates;
    std::set<std::string> _texts;
+    std::set<std::string> _lines;
    std::set<std::string> _frustums;
    pcl::PointCloud<pcl::PointXYZ>::Ptr _trajectory;
    unsigned int _maxTrajectorySize;
@@ -297,6 +311,7 @@ private:
    QColor _currentBgColor;
    bool _backfaceCulling;
    bool _frontfaceCulling;
+    double _renderingRate;
 };

 } /* namespace rtabmap */
@@ -131,6 +131,7 @@ private:
 				QLabel * labelId,
 				QLabel * labelMapId,
 				QLabel * labelPose,
+				QLabel * labeCalib,
 				bool updateConstraintView);
 	void updateStereo(const SensorData * data);
 	void updateWordsMatching();
@@ -156,6 +157,10 @@ private:

 private:
 	Ui_DatabaseViewer * ui_;
+	CloudViewer * constraintsViewer_;
+	CloudViewer * cloudViewerA_;
+	CloudViewer * cloudViewerB_;
+	CloudViewer * stereoViewer_;
 	QList<int> ids_;
 	std::map<int, int> mapIds_;
 	QMap<int, int> idToIndex_;
@@ -98,6 +98,10 @@ public:
 	bool isLocalRadiusVisible() const;
 	float getLoopClosureOutlierThr() const {return _loopClosureOutlierThr;}
 	float getMaxLinkLength() const {return _maxLinkLength;}
+	bool isGraphVisible() const;
+	bool isGlobalPathVisible() const;
+	bool isLocalPathVisible() const;
+	bool isGtGraphVisible() const;

 	// setters
 	void setWorkingDirectory(const QString & path);
@@ -124,6 +128,10 @@ public:
 	void setLocalRadiusVisible(bool visible);
 	void setLoopClosureOutlierThr(float value);
 	void setMaxLinkLength(float value);
+	void setGraphVisible(bool visible);
+	void setGlobalPathVisible(bool visible);
+	void setLocalPathVisible(bool visible);
+	void setGtGraphVisible(bool visible);

 signals:
 	void configChanged();
@@ -153,6 +161,10 @@ private:
 	QColor _loopInterSessionColor;
 	bool _intraInterSessionColors;
 	QGraphicsItem * _root;
+	QGraphicsItem * _graphRoot;
+	QGraphicsItem * _globalPathRoot;
+	QGraphicsItem * _localPathRoot;
+	QGraphicsItem * _gtGraphRoot;
 	QMap<int, NodeItem*> _nodeItems;
 	QMultiMap<int, LinkItem*> _linkItems;
 	QMap<int, NodeItem*> _gtNodeItems;
@@ -168,7 +180,6 @@ private:
 	QGraphicsEllipseItem * _localRadius;
 	float _loopClosureOutlierThr;
 	float _maxLinkLength;
-	int _coordinateSystem;
 };

 } /* namespace rtabmap */
@@ -45,7 +45,7 @@ class RTABMAPGUI_EXP LoopClosureViewer : public QWidget {
 	Q_OBJECT

 public:
-	LoopClosureViewer(QWidget * parent);
+	LoopClosureViewer(QWidget * parent = 0);
 	virtual ~LoopClosureViewer();

 	void setData(const Signature & sA, const Signature & sB); // sB contains loop transform as pose() from sA
@@ -56,8 +56,8 @@ public:
 public slots:
 	void setDecimation(int decimation) {decimation_ = decimation;}
 	void setMaxDepth(int maxDepth) {maxDepth_ = maxDepth;}
-	void setSamples(int samples) {samples_ = samples;}
-	void updateView(const Transform & AtoB = Transform());
+	void setMinDepth(int minDepth) {minDepth_ = minDepth;}
+	void updateView(const Transform & AtoB = Transform(), const ParametersMap & parameters = ParametersMap());

 protected:
 	virtual void showEvent(QShowEvent * event);
@@ -71,7 +71,7 @@ private:

 	int decimation_;
 	float maxDepth_;
-	int samples_;
+	float minDepth_;
 };

 } /* namespace rtabmap */
@@ -52,6 +52,7 @@ class CameraOpenni;
 class CameraFreenect;
 class OdometryThread;
 class CloudViewer;
+class LoopClosureViewer;
 }

 class QGraphicsScene;
@@ -108,6 +109,7 @@ public:
 public slots:
 	void processStats(const rtabmap::Statistics & stat);
 	void updateCacheFromDatabase(const QString & path);
+	void openDatabase(const QString & path);

 protected:
 	virtual void closeEvent(QCloseEvent* event);
@@ -151,6 +153,8 @@ private slots:
 	void selectFreenect2();
 	void selectStereoDC1394();
 	void selectStereoFlyCapture2();
+	void selectStereoZed();
+	void selectStereoUsb();
 	void dumpTheMemory();
 	void dumpThePrediction();
 	void sendGoal();
@@ -227,7 +231,6 @@ private:
 	void update3DMapVisibility(bool cloudsShown, bool scansShown);
 	void updateMapCloud(
 			const std::map<int, Transform> & poses,
-			const Transform & pose,
 			const std::multimap<int, Link> & constraints,
 			const std::map<int, int> & mapIds,
 			const std::map<int, std::string> & labels,
@@ -306,6 +309,9 @@ private:

 	ProgressDialog * _initProgressDialog;

+	CloudViewer * _cloudViewer;
+	LoopClosureViewer * _loopClosureViewer;
+
 	QString _graphSavingFileName;
 	QMap<int, QString> _exportPosesFileName;
 	bool _autoScreenCaptureOdomSync;
@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines

 #include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/core/Parameters.h"
 #include <QDialog>
 #include "rtabmap/utilite/UEventsHandler.h"

@@ -49,7 +50,14 @@ class RTABMAPGUI_EXP OdometryViewer : public QDialog, public UEventsHandler
 	Q_OBJECT

 public:
-	OdometryViewer(int maxClouds = 10, int decimation = 2, float voxelSize = 0.0f, float maxDepth = 0, int qualityWarningThr=0, QWidget * parent = 0);
+	OdometryViewer(
+		int maxClouds = 10, 
+		int decimation = 2, 
+		float voxelSize = 0.0f, 
+		float maxDepth = 0, 
+		int qualityWarningThr=0, 
+		QWidget * parent = 0,
+		const ParametersMap & parameters = ParametersMap());
 	virtual ~OdometryViewer();

 public slots:
@@ -83,6 +91,7 @@ private:
 	QCheckBox * featuresShown_;
 	QLabel * timeLabel_;
 	int validDecimationValue_;
+	ParametersMap parameters_;
 };

 } /* namespace rtabmap */
@@ -96,6 +96,8 @@ public:
 		kSrcFlyCapture2    = 101,
 		kSrcStereoImages   = 102,
 		kSrcStereoVideo    = 103,
+		kSrcStereoZed      = 104,
+		kSrcStereoUsb      = 105,

 		kSrcRGB            = 200,
 		kSrcUsbDevice      = 200,
@@ -148,6 +150,7 @@ public:
 	bool isCloudsShown(int index) const;      // 0=map, 1=odom
 	int getCloudDecimation(int index) const;   // 0=map, 1=odom
 	double getCloudMaxDepth(int index) const;  // 0=map, 1=odom
+	double getCloudMinDepth(int index) const;  // 0=map, 1=odom
 	double getCloudOpacity(int index) const;   // 0=map, 1=odom
 	int getCloudPointSize(int index) const;    // 0=map, 1=odom

@@ -282,6 +285,7 @@ private slots:
 	void selectSourceStereoVideoPath();
 	void selectSourceOniPath();
 	void selectSourceOni2Path();
+	void selectSourceSvoPath();
 	void updateSourceGrpVisibility();
 	void testOdometry();
 	void testCamera();
@@ -340,6 +344,7 @@ private:
 	QVector<QCheckBox*> _3dRenderingShowClouds;
 	QVector<QSpinBox*> _3dRenderingDecimation;
 	QVector<QDoubleSpinBox*> _3dRenderingMaxDepth;
+	QVector<QDoubleSpinBox*> _3dRenderingMinDepth;
 	QVector<QDoubleSpinBox*> _3dRenderingOpacity;
 	QVector<QSpinBox*> _3dRenderingPtSize;
 	QVector<QCheckBox*> _3dRenderingShowScans;
@@ -48,7 +48,7 @@ SET(qrc
    ./GuiLib.qrc
 )

-IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
+IF(QT4_FOUND)
    # generate rules for building source files from the resources
    QT4_ADD_RESOURCES(srcs_qrc ${qrc})

@@ -106,9 +106,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} 
@@ -131,9 +131,9 @@ ADD_LIBRARY(rtabmap_gui ${SRC_FILES})
 # Linking with Qt libraries

 TARGET_LINK_LIBRARIES(rtabmap_gui rtabmap_core rtabmap_utilite ${LIBRARIES})
-IF("${RTABMAP_QT_VERSION}" STREQUAL "5")
+IF(Qt5_FOUND)
    QT5_USE_MODULES(rtabmap_gui Widgets Core Gui Svg PrintSupport)
-ENDIF()
+ENDIF(Qt5_FOUND)

 SET_TARGET_PROPERTIES(
     rtabmap_gui
@@ -80,6 +80,7 @@ CalibrationDialog::CalibrationDialog(bool stereo, const QString & savingDirector
 	connect(ui_->spinBox_boardWidth, SIGNAL(valueChanged(int)), this, SLOT(setBoardWidth(int)));
 	connect(ui_->spinBox_boardHeight, SIGNAL(valueChanged(int)), this, SLOT(setBoardHeight(int)));
 	connect(ui_->doubleSpinBox_squareSize, SIGNAL(valueChanged(double)), this, SLOT(setSquareSize(double)));
+	connect(ui_->spinBox_maxScale, SIGNAL(valueChanged(int)), this, SLOT(setMaxScale(int)));

 	connect(ui_->buttonBox, SIGNAL(rejected()), this, SLOT(close()));

@@ -112,6 +113,7 @@ void CalibrationDialog::saveSettings(QSettings & settings, const QString & group
 	settings.setValue("board_width", ui_->spinBox_boardWidth->value());
 	settings.setValue("board_height", ui_->spinBox_boardHeight->value());
 	settings.setValue("board_square_size", ui_->doubleSpinBox_squareSize->value());
+	settings.setValue("max_scale", ui_->spinBox_maxScale->value());
 	settings.setValue("geometry", this->saveGeometry());
 	if(!group.isEmpty())
 	{
@@ -128,6 +130,7 @@ void CalibrationDialog::loadSettings(QSettings & settings, const QString & group
 	this->setBoardWidth(settings.value("board_width", ui_->spinBox_boardWidth->value()).toInt());
 	this->setBoardHeight(settings.value("board_height", ui_->spinBox_boardHeight->value()).toInt());
 	this->setSquareSize(settings.value("board_square_size", ui_->doubleSpinBox_squareSize->value()).toDouble());
+	this->setMaxScale(settings.value("max_scale", ui_->spinBox_maxScale->value()).toDouble());
 	QByteArray bytes = settings.value("geometry", QByteArray()).toByteArray();
 	if(!bytes.isEmpty())
 	{
@@ -205,6 +208,14 @@ void CalibrationDialog::setSquareSize(double size)
 	}
 }

+void CalibrationDialog::setMaxScale(int scale)
+{
+	if(scale != ui_->spinBox_maxScale->value())
+	{
+		ui_->spinBox_maxScale->setValue(scale);
+	}
+}
+
 void CalibrationDialog::closeEvent(QCloseEvent* event)
 {
 	if(!savedCalibration_ && models_[0].isValidForRectification() &&
@@ -260,6 +271,7 @@ void CalibrationDialog::handleEvent(UEvent * event)

 void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat & imageRight, const QString & cameraName)
 {
+	UDEBUG("Processing images");
 	processingData_ = true;
 	if(cameraName_.isEmpty())
 	{
@@ -306,6 +318,18 @@ void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat &
 		{
 			if(images[id].type() == CV_16UC1)
 			{
+				double min, max;
+				cv::minMaxLoc(images[id], &min, &max);
+				UDEBUG("Camera IR %d: min=%f max=%f", id, min, max);
+				if(minIrs_[id] == 0)
+				{
+					minIrs_[id] = min;
+				}
+				if(maxIrs_[id] == 0x7fff)
+				{
+					maxIrs_[id] = max;
+				}
+
 				depthDetected = true;
 				//assume IR image: convert to gray scaled
 				const float factor = 255.0f / float((maxIrs_[id] - minIrs_[id]));
@@ -347,7 +371,7 @@ void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat &

 				if(!viewGray.empty())
 				{
-					int maxScale = viewGray.cols < 640?2:1;
+					int maxScale = ui_->spinBox_maxScale->value();
 					for( int scale = 1; scale <= maxScale; scale++ )
 					{
 						cv::Mat timg;
@@ -698,8 +722,8 @@ void CalibrationDialog::calibrate()
 		P.at<double>(2,3) = 1;
 		K.copyTo(P.colRange(0,3).rowRange(0,3));

-		std::cout << "cameraMatrix = " << K << std::endl;
-		std::cout << "distCoeffs = " << D << std::endl;
+		std::cout << "K = " << K << std::endl;
+		std::cout << "D = " << D << std::endl;
 		std::cout << "width = " << imageSize_[id].width << std::endl;
 		std::cout << "height = " << imageSize_[id].height << std::endl;

@@ -714,8 +738,8 @@ void CalibrationDialog::calibrate()
 			ui_->label_error->setNum(totalAvgErr);

 			std::stringstream strK, strD, strR, strP;
-			strK << models_[id].K();
-			strD << models_[id].D();
+			strK << models_[id].K_raw();
+			strD << models_[id].D_raw();
 			strR << models_[id].R();
 			strP << models_[id].P();
 			ui_->lineEdit_K->setText(strK.str().c_str());
@@ -732,8 +756,8 @@ void CalibrationDialog::calibrate()
 			ui_->label_error_2->setNum(totalAvgErr);

 			std::stringstream strK, strD, strR, strP;
-			strK << models_[id].K();
-			strD << models_[id].D();
+			strK << models_[id].K_raw();
+			strD << models_[id].D_raw();
 			strR << models_[id].R();
 			strP << models_[id].P();
 			ui_->lineEdit_K_2->setText(strK.str().c_str());
@@ -782,6 +806,11 @@ void CalibrationDialog::calibrate()
 #endif
 		UINFO("stereo calibration... done with RMS error=%f", rms);

+		std::cout << "R = " << R << std::endl;
+		std::cout << "T = " << T << std::endl;
+		std::cout << "E = " << E << std::endl;
+		std::cout << "F = " << F << std::endl;
+
 		if(imageSize_[0] == imageSize_[1])
 		{
 			//Stereo, compute stereo rectification
@@ -47,12 +47,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap {


-CameraViewer::CameraViewer(QWidget * parent) :
+CameraViewer::CameraViewer(QWidget * parent, const ParametersMap & parameters) :
 		QDialog(parent),
 	imageView_(new ImageView(this)),
 	cloudView_(new CloudViewer(this)),
 	processingImages_(false),
-	validDecimationValue_(1)
+	validDecimationValue_(1),
+	parameters_(parameters)
 {
 	qRegisterMetaType<rtabmap::SensorData>("rtabmap::SensorData");

@@ -146,12 +147,12 @@ void CameraViewer::showImage(const rtabmap::SensorData & data)
 			if(!data.imageRaw().empty() && !data.depthOrRightRaw().empty())
 			{
 				showCloudCheckbox_->setEnabled(true);
-				cloudView_->addCloud("cloud", util3d::cloudRGBFromSensorData(data, validDecimationValue_));
+				cloudView_->addCloud("cloud", util3d::cloudRGBFromSensorData(data, validDecimationValue_, 0, 0, 0, parameters_));
 			}
 			else if(!data.depthOrRightRaw().empty())
 			{
 				showCloudCheckbox_->setEnabled(true);
-				cloudView_->addCloud("cloud", util3d::cloudFromSensorData(data, validDecimationValue_));
+				cloudView_->addCloud("cloud", util3d::cloudFromSensorData(data, validDecimationValue_, 0, 0, 0, parameters_));
 			}
 		}
 	}
@@ -31,7 +31,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UMath.h>
 #include <rtabmap/utilite/UConversion.h>
-#include <rtabmap/core/util3d.h>
 #include <pcl/visualization/pcl_visualizer.h>
 #include <pcl/common/transforms.h>
 #include <QMenu>
@@ -42,6 +41,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QtGui/QKeyEvent>
 #include <QColorDialog>
 #include <QtGui/QVector3D>
+#include <QMainWindow>
 #include <set>

 #include <vtkCamera.h>
@@ -121,20 +121,29 @@ CloudViewer::CloudViewer(QWidget *parent) :
 		_defaultBgColor(Qt::black),
 		_currentBgColor(Qt::black),
 		_backfaceCulling(false),
-		_frontfaceCulling(false)
+		_frontfaceCulling(false),
+		_renderingRate(5.0)
 {
+	UDEBUG("");
 	this->setMinimumSize(200, 200);

 	int argc = 0;
-	_visualizer = new pcl::visualization::PCLVisualizer(argc, 0, "PCLVisualizer", vtkSmartPointer<MyInteractorStyle>(new MyInteractorStyle()), false);
+	_visualizer = new pcl::visualization::PCLVisualizer(
+		argc, 
+		0, 
+		"PCLVisualizer", 
+		vtkSmartPointer<MyInteractorStyle>(new MyInteractorStyle()), 
+		false);

+	_visualizer->setShowFPS(false);
+	
 	this->SetRenderWindow(_visualizer->getRenderWindow());

 	// Replaced by the second line, to avoid a crash in Mac OS X on close, as well as
 	// the "Invalid drawable" warning when the view is not visible.
 	//_visualizer->setupInteractor(this->GetInteractor(), this->GetRenderWindow());
 	this->GetInteractor()->SetInteractorStyle (_visualizer->getInteractorStyle());
-	_visualizer->getInteractorStyle()->GetInteractor()->SetDesiredUpdateRate(5.0);
+	setRenderingRate(_renderingRate);

 	_visualizer->setCameraPosition(
 				-1, 0, 0,
@@ -163,6 +172,7 @@ void CloudViewer::clear()
 	this->removeAllClouds();
 	this->removeAllGraphs();
 	this->removeAllCoordinates();
+	this->removeAllLines();
 	this->removeAllFrustums();
 	this->removeAllTexts();
 	this->clearTrajectory();
@@ -203,7 +213,8 @@ void CloudViewer::createMenu()
 	_aShowGrid->setCheckable(true);
 	_aSetGridCellCount = new QAction("Set cell count...", this);
 	_aSetGridCellSize = new QAction("Set cell size...", this);
-	_aSetBackgroundColor = new QAction("Set background color...", this);
+	_aSetBackgroundColor = new QAction("Set background color...", this);	
+	_aSetRenderingRate = new QAction("Set rendering rate...", this);

 	QMenu * cameraMenu = new QMenu("Camera", this);
 	cameraMenu->addAction(_aLockCamera);
@@ -239,6 +250,7 @@ void CloudViewer::createMenu()
 	_menu->addMenu(frustumMenu);
 	_menu->addMenu(gridMenu);
 	_menu->addAction(_aSetBackgroundColor);
+	_menu->addAction(_aSetRenderingRate);
 }

 void CloudViewer::saveSettings(QSettings & settings, const QString & group) const
@@ -288,6 +300,7 @@ void CloudViewer::saveSettings(QSettings & settings, const QString & group) cons
 	settings.setValue("camera_lockZ", this->isCameraLockZ());

 	settings.setValue("bg_color", this->getDefaultBackgroundColor());
+	settings.setValue("rendering_rate", this->getRenderingRate());
 	if(!group.isEmpty())
 	{
 		settings.endGroup();
@@ -329,6 +342,9 @@ void CloudViewer::loadSettings(QSettings & settings, const QString & group)
 	this->setCameraLockZ(settings.value("camera_lockZ", this->isCameraLockZ()).toBool());

 	this->setDefaultBackgroundColor(settings.value("bg_color", this->getDefaultBackgroundColor()).value<QColor>());
+	
+	this->setRenderingRate(settings.value("rendering_rate", this->getRenderingRate()).toDouble());
+
 	if(!group.isEmpty())
 	{
 		settings.endGroup();
@@ -777,6 +793,70 @@ void CloudViewer::removeAllCoordinates()
 	UASSERT(_coordinates.empty());
 }

+void CloudViewer::addOrUpdateLine(
+			const std::string & id,
+			const Transform & from,
+			const Transform & to,
+			const QColor & color,
+			bool arrow)
+{
+	if(id.empty())
+	{
+		UERROR("id should not be empty!");
+		return;
+	}
+
+	removeLine(id);
+
+	if(!from.isNull() && !to.isNull())
+	{
+		_lines.insert(id);
+
+		QColor c = Qt::gray;
+		if(color.isValid())
+		{
+			c = color;
+		}
+
+		pcl::PointXYZ pt1(from.x(), from.y(), from.z());
+		pcl::PointXYZ pt2(to.x(), to.y(), to.z());
+
+		if(arrow)
+		{
+			_visualizer->addArrow(pt2, pt1, c.redF(), c.greenF(), c.blueF(), false, id);
+		}
+		else
+		{
+			_visualizer->addLine(pt2, pt1, c.redF(), c.greenF(), c.blueF(), id);
+		}
+	}
+}
+
+void CloudViewer::removeLine(const std::string & id)
+{
+	if(id.empty())
+	{
+		UERROR("id should not be empty!");
+		return;
+	}
+
+	if(_lines.find(id) != _lines.end())
+	{
+		_visualizer->removeShape(id);
+		_lines.erase(id);
+	}
+}
+
+void CloudViewer::removeAllLines()
+{
+	std::set<std::string> arrows = _lines;
+	for(std::set<std::string>::iterator iter = arrows.begin(); iter!=arrows.end(); ++iter)
+	{
+		this->removeLine(*iter);
+	}
+	UASSERT(_lines.empty());
+}
+
 static const float frustum_vertices[] = {
    0.0f,  0.0f, 0.0f,
 	1.0f, 1.0f, 1.0f,
@@ -1040,6 +1120,8 @@ void CloudViewer::setFrustumShown(bool shown)
 	if(!shown)
 	{
 		this->removeFrustum("reference_frustum");
+		this->removeLine("reference_frustum_line");
+		this->update();
 	}
 	_aShowFrustum->setChecked(shown);
 }
@@ -1058,8 +1140,8 @@ void CloudViewer::setFrustumColor(QColor value)
 	if(_frustums.find("reference_frustum") != _frustums.end())
 	{
 		_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, value.redF(), value.greenF(), value.blueF(), "reference_frustum");
-		this->update();
 	}
+	this->update();
 	_frustumColor = value;
 }

@@ -1133,6 +1215,12 @@ void CloudViewer::setBackfaceCulling(bool enabled, bool frontfaceCulling)
 	_frontfaceCulling = frontfaceCulling;
 }

+void CloudViewer::setRenderingRate(double rate)
+{
+	_renderingRate = rate;
+	_visualizer->getInteractorStyle()->GetInteractor()->SetDesiredUpdateRate(_renderingRate);
+}
+
 void CloudViewer::getCameraPosition(
 		float & x, float & y, float & z,
 		float & focalX, float & focalY, float & focalZ,
@@ -1167,7 +1255,7 @@ void CloudViewer::setCameraPosition(
 	_visualizer->setCameraPosition(x,y,z, focalX,focalY,focalX, upX,upY,upZ);
 }

-void CloudViewer::updateCameraTargetPosition(const Transform & pose)
+void CloudViewer::updateCameraTargetPosition(const Transform & pose, const Transform & localTransform)
 {
 	if(!pose.isNull())
 	{
@@ -1283,7 +1371,17 @@ void CloudViewer::updateCameraTargetPosition(const Transform & pose)
 			}
 			else */ if(_aShowFrustum->isChecked())
 			{
-				this->addOrUpdateFrustum("reference_frustum", pose, _frustumScale, _frustumColor);
+				Transform baseToCamera = Transform::getIdentity();
+				Transform opticalRot(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
+				if(!localTransform.isNull() && !localTransform.isIdentity())
+				{
+					baseToCamera = localTransform*opticalRot.inverse();
+				}
+				this->addOrUpdateFrustum("reference_frustum", pose * baseToCamera, _frustumScale, _frustumColor);
+				if(!baseToCamera.isIdentity())
+				{
+					this->addOrUpdateLine("reference_frustum_line", pose, pose * baseToCamera, _frustumColor);
+				}
 			}

 			vtkRenderer* renderer = _visualizer->getRendererCollection()->GetFirstRenderer();
@@ -1435,6 +1533,10 @@ float CloudViewer::getGridCellSize() const
 {
 	return _gridCellSize;
 }
+double CloudViewer::getRenderingRate() const
+{
+	return _renderingRate;
+}

 void CloudViewer::setGridCellCount(unsigned int count)
 {
@@ -1812,6 +1914,15 @@ void CloudViewer::handleAction(QAction * a)
 			this->update();
 		}
 	}
+	else if(a == _aSetRenderingRate)
+	{
+		bool ok;
+		double value = QInputDialog::getDouble(this, tr("Rendering rate"), tr("Rate (hz)"), _renderingRate, 0, 60, 0, &ok);
+		if(ok)
+		{
+			this->setRenderingRate(value);
+		}
+	}
 	else if(a == _aLockViewZ)
 	{
 		if(_aLockViewZ->isChecked())
@@ -47,6 +47,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UFile.h>
 #include "rtabmap/core/DBDriver.h"
 #include "rtabmap/gui/KeypointItem.h"
+#include "rtabmap/gui/CloudViewer.h"
 #include "rtabmap/utilite/UCv2Qt.h"
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/util3d_transforms.h"
@@ -113,8 +114,22 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	ui_->dockWidget_stereoView->setVisible(false);
 	ui_->dockWidget_view3d->setVisible(false);

-	ui_->constraintsViewer->setCameraLockZ(false);
-	ui_->constraintsViewer->setCameraFree();
+	// Create cloud viewers
+	constraintsViewer_ = new CloudViewer(ui_->dockWidgetContents);
+	cloudViewerA_ = new CloudViewer(ui_->dockWidgetContents_3dviews);
+	cloudViewerB_ = new CloudViewer(ui_->dockWidgetContents_3dviews);
+	stereoViewer_ = new CloudViewer(ui_->dockWidgetContents_stereo);
+	constraintsViewer_->setObjectName("constraintsViewer");
+	cloudViewerA_->setObjectName("cloudViewerA");
+	cloudViewerB_->setObjectName("cloudViewerB");
+	stereoViewer_->setObjectName("stereoViewer");
+	ui_->layout_constraintsViewer->addWidget(constraintsViewer_);
+	ui_->horizontalLayout_3dviews->addWidget(cloudViewerA_, 1);
+	ui_->horizontalLayout_3dviews->addWidget(cloudViewerB_, 1);
+	ui_->horizontalLayout_stereo->addWidget(stereoViewer_, 1);
+
+	constraintsViewer_->setCameraLockZ(false);
+	constraintsViewer_->setCameraFree();

 	ui_->graphicsView_stereo->setAlpha(255);

@@ -239,6 +254,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->doubleSpinBox_gridCellSize, SIGNAL(editingFinished()), this, SLOT(updateGrid()));
 	connect(ui_->spinBox_projDecimation, SIGNAL(editingFinished()), this, SLOT(updateGrid()));
 	connect(ui_->doubleSpinBox_projMaxDepth, SIGNAL(editingFinished()), this, SLOT(updateGrid()));
+	connect(ui_->doubleSpinBox_projMinDepth, SIGNAL(editingFinished()), this, SLOT(updateGrid()));
 	connect(ui_->doubleSpinBox_projMaxAngle, SIGNAL(editingFinished()), this, SLOT(updateGrid()));
 	connect(ui_->spinBox_projClusterSize, SIGNAL(editingFinished()), this, SLOT(updateGrid()));

@@ -268,6 +284,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->doubleSpinBox_gridCellSize, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
 	connect(ui_->spinBox_projDecimation, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
 	connect(ui_->doubleSpinBox_projMaxDepth, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
+	connect(ui_->doubleSpinBox_projMinDepth, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
 	connect(ui_->doubleSpinBox_projMaxAngle, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
 	connect(ui_->spinBox_projClusterSize, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
 	connect(ui_->groupBox_posefiltering, SIGNAL(clicked(bool)), this, SLOT(configModified()));
@@ -276,6 +293,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :

 	connect(ui_->spinBox_icp_decimation, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
 	connect(ui_->doubleSpinBox_icp_maxDepth, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
+	connect(ui_->doubleSpinBox_icp_minDepth, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
 	connect(ui_->checkBox_icp_laserScan, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
 	
 	connect(ui_->doubleSpinBox_detectMore_radius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
@@ -388,6 +406,7 @@ void DatabaseViewer::readSettings()
 	ui_->doubleSpinBox_gridCellSize->setValue(settings.value("gridCellSize", ui_->doubleSpinBox_gridCellSize->value()).toDouble());
 	ui_->spinBox_projDecimation->setValue(settings.value("projDecimation", ui_->spinBox_projDecimation->value()).toInt());
 	ui_->doubleSpinBox_projMaxDepth->setValue(settings.value("projMaxDepth", ui_->doubleSpinBox_projMaxDepth->value()).toDouble());
+	ui_->doubleSpinBox_projMinDepth->setValue(settings.value("projMinDepth", ui_->doubleSpinBox_projMinDepth->value()).toDouble());
 	ui_->doubleSpinBox_projMaxAngle->setValue(settings.value("projMaxAngle", ui_->doubleSpinBox_projMaxAngle->value()).toDouble());
 	ui_->spinBox_projClusterSize->setValue(settings.value("projClusterSize", ui_->spinBox_projClusterSize->value()).toInt());
 	ui_->groupBox_posefiltering->setChecked(settings.value("poseFiltering", ui_->groupBox_posefiltering->isChecked()).toBool());
@@ -411,6 +430,7 @@ void DatabaseViewer::readSettings()
 	settings.beginGroup("icp");
 	ui_->spinBox_icp_decimation->setValue(settings.value("decimation", ui_->spinBox_icp_decimation->value()).toInt());
 	ui_->doubleSpinBox_icp_maxDepth->setValue(settings.value("maxDepth", ui_->doubleSpinBox_icp_maxDepth->value()).toDouble());
+	ui_->doubleSpinBox_icp_minDepth->setValue(settings.value("minDepth", ui_->doubleSpinBox_icp_minDepth->value()).toDouble());
 	ui_->checkBox_icp_laserScan->setChecked(settings.value("icpLaserScan", ui_->checkBox_icp_laserScan->isChecked()).toBool());
 	settings.endGroup();
 	// Visual parameters
@@ -474,6 +494,7 @@ void DatabaseViewer::writeSettings()
 	settings.setValue("gridCellSize", ui_->doubleSpinBox_gridCellSize->value());
 	settings.setValue("projDecimation", ui_->spinBox_projDecimation->value());
 	settings.setValue("projMaxDepth", ui_->doubleSpinBox_projMaxDepth->value());
+	settings.setValue("projMinDepth", ui_->doubleSpinBox_projMinDepth->value());
 	settings.setValue("projMaxAngle", ui_->doubleSpinBox_projMaxAngle->value());
 	settings.setValue("projClusterSize", ui_->spinBox_projClusterSize->value());
 	settings.setValue("poseFiltering", ui_->groupBox_posefiltering->isChecked());
@@ -497,6 +518,7 @@ void DatabaseViewer::writeSettings()
 	settings.beginGroup("icp");
 	settings.setValue("decimation", ui_->spinBox_icp_decimation->value());
 	settings.setValue("maxDepth", ui_->doubleSpinBox_icp_maxDepth->value());
+	settings.setValue("minDepth", ui_->doubleSpinBox_icp_minDepth->value());
 	settings.setValue("icpLaserScan", ui_->checkBox_icp_laserScan->isChecked());
 	settings.endGroup();
 	
@@ -1442,8 +1464,8 @@ void DatabaseViewer::view3DMap()
 	QString item = QInputDialog::getItem(this, tr("Decimation?"), tr("Image decimation"), items, 2, false, &ok);
 	if(ok)
 	{
-		int decimation = item.toInt();
-		double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 100, 2, &ok);
+		int decimation = item.toInt();
+		double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 100, 2, &ok);
 		if(ok)
 		{
 			std::map<int, Transform> optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
@@ -1487,7 +1509,7 @@ void DatabaseViewer::view3DMap()
 						pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 						UASSERT(data.imageRaw().empty() || data.imageRaw().type()==CV_8UC3 || data.imageRaw().type() == CV_8UC1);
 						UASSERT(data.depthOrRightRaw().empty() || data.depthOrRightRaw().type()==CV_8UC1 || data.depthOrRightRaw().type() == CV_16UC1 || data.depthOrRightRaw().type() == CV_32FC1);
-						cloud = util3d::cloudRGBFromSensorData(data, decimation, maxDepth);
+						cloud = util3d::cloudRGBFromSensorData(data, decimation, maxDepth, 0, 0, ui_->parameters_toolbox->getParameters());

 						if(cloud->size())
 						{
@@ -1660,7 +1682,7 @@ void DatabaseViewer::generate3DMap()
 	QString item = QInputDialog::getItem(this, tr("Decimation?"), tr("Image decimation"), items, 2, false, &ok);
 	if(ok)
 	{
-		int decimation = item.toInt();
+		int decimation = item.toInt();
 		double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 100, 2, &ok);
 		if(ok)
 		{
@@ -1710,20 +1732,26 @@ void DatabaseViewer::generate3DMap()
 							pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 							UASSERT(data.imageRaw().empty() || data.imageRaw().type()==CV_8UC3 || data.imageRaw().type() == CV_8UC1);
 							UASSERT(data.depthOrRightRaw().empty() || data.depthOrRightRaw().type()==CV_8UC1 || data.depthOrRightRaw().type() == CV_16UC1 || data.depthOrRightRaw().type() == CV_32FC1);
-							cloud = util3d::cloudRGBFromSensorData(data, decimation, maxDepth, assemble?0.01:0);
+							pcl::IndicesPtr validIndices(new std::vector<int>);
+							cloud = util3d::cloudRGBFromSensorData(data, decimation, maxDepth, 0, validIndices.get(), ui_->parameters_toolbox->getParameters());

 							if(assemble)
 							{
 								if(cloud->size())
 								{
-									cloud = rtabmap::util3d::transformPointCloud(cloud, pose);
-									if(assembledCloud->size() == 0)
+									cloud = util3d::voxelize(cloud, validIndices, 0.01);
+
+									if(cloud->size())
 									{
-										*assembledCloud = *cloud;
-									}
-									else
-									{
-										*assembledCloud += *cloud;
+										cloud = rtabmap::util3d::transformPointCloud(cloud, pose);
+										if(assembledCloud->size() == 0)
+										{
+											*assembledCloud = *cloud;
+										}
+										else
+										{
+											*assembledCloud += *cloud;
+										}
 									}
 								}
 								UINFO("Created cloud %d (%d points)", iter->first, (int)cloud->size());
@@ -2055,10 +2083,11 @@ void DatabaseViewer::sliderAValueChanged(int value)
 			ui_->label_labelA,
 			ui_->label_stampA,
 			ui_->graphicsView_A,
-			ui_->widget_cloudA,
+			cloudViewerA_,
 			ui_->label_idA,
 			ui_->label_mapA,
 			ui_->label_poseA,
+			ui_->label_calibA,
 			true);
 }

@@ -2072,10 +2101,11 @@ void DatabaseViewer::sliderBValueChanged(int value)
 			ui_->label_labelB,
 			ui_->label_stampB,
 			ui_->graphicsView_B,
-			ui_->widget_cloudB,
+			cloudViewerB_,
 			ui_->label_idB,
 			ui_->label_mapB,
 			ui_->label_poseB,
+			ui_->label_calibB,
 			true);
 }

@@ -2091,6 +2121,7 @@ void DatabaseViewer::update(int value,
 						QLabel * labelId,
 						QLabel * labelMapId,
 						QLabel * labelPose,
+						QLabel * labelCalib,
 						bool updateConstraintView)
 {
 	UTimer timer;
@@ -2102,6 +2133,7 @@ void DatabaseViewer::update(int value,
 	labelMapId->clear();
 	labelPose->clear();
 	stamp->clear();
+	labelCalib->clear();
 	QRectF rect;
 	if(value >= 0 && value < ids_.size())
 	{
@@ -2163,6 +2195,53 @@ void DatabaseViewer::update(int value,
 				{
 					stamp->setText(QDateTime::fromMSecsSinceEpoch(s*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz"));
 				}
+				if(data.cameraModels().size() || data.stereoCameraModel().isValidForProjection())
+				{
+					if(data.cameraModels().size())
+					{
+						if(!data.depthRaw().empty() && data.depthRaw().cols!=data.imageRaw().cols && data.imageRaw().cols)
+						{
+							labelCalib->setText(tr("%1 %2x%3 [%8x%9] fx=%4 fy=%5 cx=%6 cy=%7")
+									.arg(data.cameraModels().size())
+									.arg(data.cameraModels()[0].imageWidth()>0?data.cameraModels()[0].imageWidth():data.imageRaw().cols/data.cameraModels().size())
+									.arg(data.cameraModels()[0].imageHeight()>0?data.cameraModels()[0].imageHeight():data.imageRaw().rows)
+									.arg(data.cameraModels()[0].fx())
+									.arg(data.cameraModels()[0].fy())
+									.arg(data.cameraModels()[0].cx())
+									.arg(data.cameraModels()[0].cy())
+									.arg(data.depthRaw().cols/data.cameraModels().size())
+									.arg(data.depthRaw().rows));
+						}
+						else
+						{
+							labelCalib->setText(tr("%1 %2x%3 fx=%4 fy=%5 cx=%6 cy=%7")
+									.arg(data.cameraModels().size())
+									.arg(data.cameraModels()[0].imageWidth()>0?data.cameraModels()[0].imageWidth():data.imageRaw().cols/data.cameraModels().size())
+									.arg(data.cameraModels()[0].imageHeight()>0?data.cameraModels()[0].imageHeight():data.imageRaw().rows)
+									.arg(data.cameraModels()[0].fx())
+									.arg(data.cameraModels()[0].fy())
+									.arg(data.cameraModels()[0].cx())
+									.arg(data.cameraModels()[0].cy()));
+						}
+					}
+					else
+					{
+						//stereo
+						labelCalib->setText(tr("%1x%2 fx=%3 fy=%4 cx=%5 cy=%6 baseline=%7m")
+									.arg(data.stereoCameraModel().left().imageWidth()>0?data.stereoCameraModel().left().imageWidth():data.imageRaw().cols)
+									.arg(data.stereoCameraModel().left().imageHeight()>0?data.stereoCameraModel().left().imageHeight():data.imageRaw().rows)
+									.arg(data.stereoCameraModel().left().fx())
+									.arg(data.stereoCameraModel().left().fy())
+									.arg(data.stereoCameraModel().left().cx())
+									.arg(data.stereoCameraModel().left().cy())
+									.arg(data.stereoCameraModel().baseline()));
+					}
+
+				}
+				else
+				{
+					labelCalib->setText("NA");
+				}

 				//stereo
 				if(!data.depthOrRightRaw().empty() && data.depthOrRightRaw().type() == CV_8UC1)
@@ -2171,7 +2250,7 @@ void DatabaseViewer::update(int value,
 				}
 				else
 				{
-					ui_->stereoViewer->clear();
+					stereoViewer_->clear();
 					ui_->graphicsView_stereo->clear();
 				}

@@ -2196,17 +2275,31 @@ void DatabaseViewer::update(int value,
 							}
 							else
 							{
-								cloud = util3d::cloudRGBFromSensorData(data);
+								cloud = util3d::cloudRGBFromSensorData(data, 1, 0, 0, 0, ui_->parameters_toolbox->getParameters());
 							}
 							if(cloud->size())
 							{
 								if(ui_->checkBox_showMesh->isChecked() && !cloud->is_dense)
 								{
+									Eigen::Vector3f viewpoint(0.0f,0.0f,0.0f);
+									if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull())
+									{
+										viewpoint[0] = data.cameraModels()[0].localTransform().x();
+										viewpoint[1] = data.cameraModels()[0].localTransform().y();
+										viewpoint[2] = data.cameraModels()[0].localTransform().z();
+									}
+									else if(!data.stereoCameraModel().localTransform().isNull())
+									{
+										viewpoint[0] = data.stereoCameraModel().localTransform().x();
+										viewpoint[1] = data.stereoCameraModel().localTransform().y();
+										viewpoint[2] = data.stereoCameraModel().localTransform().z();
+									}
 									std::vector<pcl::Vertices> polygons = util3d::organizedFastMesh(
 											cloud,
 											float(ui_->spinBox_mesh_angleTolerance->value())*M_PI/180.0f,
 											ui_->checkBox_mesh_quad->isChecked(),
-											ui_->spinBox_mesh_triangleSize->value());
+											ui_->spinBox_mesh_triangleSize->value(),
+											viewpoint);
 									view3D->removeCloud("0");
 									view3D->addCloudMesh("0", cloud, polygons);
 								}
@@ -2219,7 +2312,7 @@ void DatabaseViewer::update(int value,
 						else
 						{
 							pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
-							cloud = util3d::cloudFromSensorData(data);
+							cloud = util3d::cloudFromSensorData(data, 1, 0, 0, 0, ui_->parameters_toolbox->getParameters());
 							if(cloud->size())
 							{
 								view3D->addCloud("0", cloud);
@@ -2352,7 +2445,7 @@ void DatabaseViewer::update(int value,
 			ui_->horizontalSlider_loops->blockSignals(false);
 			ui_->horizontalSlider_neighbors->blockSignals(false);

-			ui_->constraintsViewer->removeAllClouds();
+			constraintsViewer_->removeAllClouds();

 			// make a fake link using globally optimized poses
 			if(graphes_.size())
@@ -2371,7 +2464,7 @@ void DatabaseViewer::update(int value,
 				}
 			}

-			ui_->constraintsViewer->update();
+			constraintsViewer_->update();

 		}
 	}
@@ -2389,7 +2482,7 @@ void DatabaseViewer::updateLoggerLevel()
 		ULogger::setLevel((ULogger::Level)ui_->comboBox_logger_level->currentIndex());
 	}
 }
-
+
 void DatabaseViewer::updateStereo()
 {
 	if(ui_->horizontalSlider_A->maximum())
@@ -2402,7 +2495,7 @@ void DatabaseViewer::updateStereo()
 	}
 }

-void DatabaseViewer::updateStereo(const SensorData * data)
+void DatabaseViewer::updateStereo(const SensorData * data)
 {
 	if(data &&
 		ui_->dockWidget_stereoView->isVisible() &&
@@ -2489,10 +2582,10 @@ void DatabaseViewer::updateStereo(const SensorData * data)
 							data->stereoCameraModel());

 					if(util3d::isFinite(tmpPt))
-					{
+					{
 						pt = util3d::transformPoint(tmpPt, data->stereoCameraModel().left().localTransform());
 						status[i] = 100; //blue
-						++inliers;
+						++inliers;
 						cloud->at(oi++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
 					}
 				}
@@ -2512,9 +2605,9 @@ void DatabaseViewer::updateStereo(const SensorData * data)
 		UINFO("correspondences = %d/%d (%f) (time kpt=%fs stereo=%fs)",
 				(int)cloud->size(), (int)leftCorners.size(), float(cloud->size())/float(leftCorners.size()), timeKpt, timeStereo);

-		ui_->stereoViewer->updateCameraTargetPosition(Transform::getIdentity());
-		ui_->stereoViewer->addCloud("stereo", cloud);
-		ui_->stereoViewer->update();
+		stereoViewer_->updateCameraTargetPosition(Transform::getIdentity());
+		stereoViewer_->addCloud("stereo", cloud);
+		stereoViewer_->update();

 		ui_->label_stereo_inliers->setNum(inliers);
 		ui_->label_stereo_flowOutliers->setNum(flowOutliers);
@@ -2807,10 +2900,11 @@ void DatabaseViewer::updateConstraintView(
 					ui_->label_labelA,
 					ui_->label_stampA,
 					ui_->graphicsView_A,
-					ui_->widget_cloudA,
+					cloudViewerA_,
 					ui_->label_idA,
 					ui_->label_mapA,
 					ui_->label_poseA,
+					ui_->label_calibA,
 					false); // don't update constraints view!
 		this->update(idToIndex_.value(link.to()),
 					ui_->label_indexB,
@@ -2820,14 +2914,15 @@ void DatabaseViewer::updateConstraintView(
 					ui_->label_labelB,
 					ui_->label_stampB,
 					ui_->graphicsView_B,
-					ui_->widget_cloudB,
+					cloudViewerB_,
 					ui_->label_idB,
 					ui_->label_mapB,
 					ui_->label_poseB,
+					ui_->label_calibB,
 					false); // don't update constraints view!
 	}

-	if(ui_->constraintsViewer->isVisible())
+	if(constraintsViewer_->isVisible())
 	{
 		SensorData dataFrom, dataTo;

@@ -2850,27 +2945,27 @@ void DatabaseViewer::updateConstraintView(
 				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFrom, cloudTo;
 				if(!dataFrom.imageRaw().empty() && !dataFrom.depthOrRightRaw().empty())
 				{
-					cloudFrom=util3d::cloudRGBFromSensorData(dataFrom, 1);
+					cloudFrom=util3d::cloudRGBFromSensorData(dataFrom, 1, 0, 0, 0, ui_->parameters_toolbox->getParameters());
 				}
 				if(!dataTo.imageRaw().empty() && !dataTo.depthOrRightRaw().empty())
 				{
-					cloudTo=util3d::cloudRGBFromSensorData(dataTo, 1);
+					cloudTo=util3d::cloudRGBFromSensorData(dataTo, 1, 0, 0, 0, ui_->parameters_toolbox->getParameters());
 				}

 				if(cloudFrom.get() && cloudFrom->size())
 				{
-					ui_->constraintsViewer->addCloud("cloud0", cloudFrom, Transform::getIdentity(), Qt::red);
+					constraintsViewer_->addCloud("cloud0", cloudFrom, Transform::getIdentity(), Qt::red);
 				}
 				if(cloudTo.get() && cloudTo->size())
 				{
 					cloudTo = rtabmap::util3d::transformPointCloud(cloudTo, t);
-					ui_->constraintsViewer->addCloud("cloud1", cloudTo, Transform::getIdentity(), Qt::cyan);
+					constraintsViewer_->addCloud("cloud1", cloudTo, Transform::getIdentity(), Qt::cyan);
 				}
 			}
 			else
 			{
-				ui_->constraintsViewer->removeCloud("cloud0");
-				ui_->constraintsViewer->removeCloud("cloud1");
+				constraintsViewer_->removeCloud("cloud0");
+				constraintsViewer_->removeCloud("cloud1");
 			}
 			if(ui_->checkBox_show3DWords->isChecked())
 			{
@@ -2918,28 +3013,28 @@ void DatabaseViewer::updateConstraintView(

 					if(cloudFrom->size())
 					{
-						ui_->constraintsViewer->addCloud("words0", cloudFrom, Transform::getIdentity(), Qt::red);
+						constraintsViewer_->addCloud("words0", cloudFrom, Transform::getIdentity(), Qt::red);
 					}
 					else
 					{
 						UWARN("Empty 3D words for node %d", link.from());
-						ui_->constraintsViewer->removeCloud("words0");
+						constraintsViewer_->removeCloud("words0");
 					}
 					if(cloudTo->size())
 					{
-						ui_->constraintsViewer->addCloud("words1", cloudTo, Transform::getIdentity(), Qt::cyan);
+						constraintsViewer_->addCloud("words1", cloudTo, Transform::getIdentity(), Qt::cyan);
 					}
 					else
 					{
 						UWARN("Empty 3D words for node %d", link.to());
-						ui_->constraintsViewer->removeCloud("words1");
+						constraintsViewer_->removeCloud("words1");
 					}
 				}
 				else
 				{
 					UERROR("Not found signature %d or %d in RAM", link.from(), link.to());
-					ui_->constraintsViewer->removeCloud("words0");
-					ui_->constraintsViewer->removeCloud("words1");
+					constraintsViewer_->removeCloud("words0");
+					constraintsViewer_->removeCloud("words1");
 				}
 				//cleanup
 				for(std::list<Signature*>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
@@ -2949,19 +3044,19 @@ void DatabaseViewer::updateConstraintView(
 			}
 			else
 			{
-				ui_->constraintsViewer->removeCloud("words0");
-				ui_->constraintsViewer->removeCloud("words1");
+				constraintsViewer_->removeCloud("words0");
+				constraintsViewer_->removeCloud("words1");
 			}
 		}
 		else
 		{
 			if(cloudFrom->size())
 			{
-				ui_->constraintsViewer->addCloud("cloud0", cloudFrom, Transform::getIdentity(), Qt::red);
+				constraintsViewer_->addCloud("cloud0", cloudFrom, Transform::getIdentity(), Qt::red);
 			}
 			if(cloudTo->size())
 			{
-				ui_->constraintsViewer->addCloud("cloud1", cloudTo, Transform::getIdentity(), Qt::cyan);
+				constraintsViewer_->addCloud("cloud1", cloudTo, Transform::getIdentity(), Qt::cyan);
 			}
 		}

@@ -2971,8 +3066,8 @@ void DatabaseViewer::updateConstraintView(
 			{
 				//cloud 2d

-				ui_->constraintsViewer->removeCloud("scan2");
-				ui_->constraintsViewer->removeGraph("scan2graph");
+				constraintsViewer_->removeCloud("scan2");
+				constraintsViewer_->removeGraph("scan2graph");
 				if(link.type() == Link::kLocalSpaceClosure &&
 				   !link.userDataCompressed().empty())
 				{
@@ -2985,6 +3080,7 @@ void DatabaseViewer::updateConstraintView(
 					   memcmp(userData.data, "SCANS:", 6) == 0)
 					{
 						std::string scansStr = (const char *)userData.data;
+						UINFO("Detected \"%s\" in links's user data", scansStr.c_str());
 						if(!scansStr.empty())
 						{
 							std::list<std::string> strs = uSplit(scansStr, ':');
@@ -3033,6 +3129,22 @@ void DatabaseViewer::updateConstraintView(
 									posesOut,
 									linksOut);

+							if(poses.size() != posesOut.size())
+							{
+								UWARN("Scan poses input and output are different! %d vs %d", (int)poses.size(), (int)posesOut.size());
+								UWARN("Input poses: ");
+								for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+								{
+									UWARN(" %d", iter->first);
+								}
+								UWARN("Input links: ");
+								std::multimap<int, Link> modifiedLinks = updateLinksWithModifications(links_);
+								for(std::multimap<int, Link>::iterator iter=modifiedLinks.begin(); iter!=modifiedLinks.end(); ++iter)
+								{
+									UWARN(" %d->%d", iter->second.from(), iter->second.to());
+								}
+							}
+
 							QTime time;
 							time.start();
 							std::map<int, rtabmap::Transform> finalPoses = optimizer->optimize(link.to(), posesOut, linksOut);
@@ -3070,11 +3182,11 @@ void DatabaseViewer::updateConstraintView(

 							if(assembledScans->size())
 							{
-								ui_->constraintsViewer->addCloud("scan2", assembledScans, Transform::getIdentity(), Qt::cyan);
+								constraintsViewer_->addCloud("scan2", assembledScans, Transform::getIdentity(), Qt::cyan);
 							}
 							if(graph->size())
 							{
-								ui_->constraintsViewer->addOrUpdateGraph("scan2graph", graph, Qt::cyan);
+								constraintsViewer_->addOrUpdateGraph("scan2graph", graph, Qt::cyan);
 							}
 						}
 					}
@@ -3087,62 +3199,62 @@ void DatabaseViewer::updateConstraintView(
 				scanB = rtabmap::util3d::transformPointCloud(scanB, t);
 				if(scanA->size())
 				{
-					ui_->constraintsViewer->addCloud("scan0", scanA, Transform::getIdentity(), Qt::yellow);
+					constraintsViewer_->addCloud("scan0", scanA, Transform::getIdentity(), Qt::yellow);
 				}
 				else
 				{
-					ui_->constraintsViewer->removeCloud("scan0");
+					constraintsViewer_->removeCloud("scan0");
 				}
 				if(scanB->size())
 				{
-					ui_->constraintsViewer->addCloud("scan1", scanB, Transform::getIdentity(), Qt::magenta);
+					constraintsViewer_->addCloud("scan1", scanB, Transform::getIdentity(), Qt::magenta);
 				}
 				else
 				{
-					ui_->constraintsViewer->removeCloud("scan1");
+					constraintsViewer_->removeCloud("scan1");
 				}
 			}
 			else
 			{
-				ui_->constraintsViewer->removeCloud("scan0");
-				ui_->constraintsViewer->removeCloud("scan1");
-				ui_->constraintsViewer->removeCloud("scan2");
+				constraintsViewer_->removeCloud("scan0");
+				constraintsViewer_->removeCloud("scan1");
+				constraintsViewer_->removeCloud("scan2");
 			}
 		}
 		else
 		{
 			if(scanFrom->size())
 			{
-				ui_->constraintsViewer->addCloud("scan0", scanFrom, Transform::getIdentity(), Qt::yellow);
+				constraintsViewer_->addCloud("scan0", scanFrom, Transform::getIdentity(), Qt::yellow);
 			}
 			else
 			{
-				ui_->constraintsViewer->removeCloud("scan0");
+				constraintsViewer_->removeCloud("scan0");
 			}
 			if(scanTo->size())
 			{
-				ui_->constraintsViewer->addCloud("scan1", scanTo, Transform::getIdentity(), Qt::magenta);
+				constraintsViewer_->addCloud("scan1", scanTo, Transform::getIdentity(), Qt::magenta);
 			}
 			else
 			{
-				ui_->constraintsViewer->removeCloud("scan1");
+				constraintsViewer_->removeCloud("scan1");
 			}
-			ui_->constraintsViewer->removeCloud("scan2");
+			constraintsViewer_->removeCloud("scan2");
 		}

 		//update coordinate

-		ui_->constraintsViewer->addOrUpdateCoordinate("from_coordinate", Transform::getIdentity(), 0.2);
-		ui_->constraintsViewer->addOrUpdateCoordinate("to_coordinate", t, 0.2);
+		constraintsViewer_->addOrUpdateCoordinate("from_coordinate", Transform::getIdentity(), 0.2);
+		constraintsViewer_->addOrUpdateCoordinate("to_coordinate", t, 0.2);
 		if(uContains(groundTruthPoses_, link.from()) && uContains(groundTruthPoses_, link.to()))
 		{
-			ui_->constraintsViewer->addOrUpdateCoordinate("to_coordinate_gt",
+			constraintsViewer_->addOrUpdateCoordinate("to_coordinate_gt",
 					groundTruthPoses_.at(link.from()).inverse()*groundTruthPoses_.at(link.to()), 0.1);
 		}

-		ui_->constraintsViewer->clearTrajectory();
+		constraintsViewer_->clearTrajectory();

-		ui_->constraintsViewer->update();
+		constraintsViewer_->update();
 	}

 	// update buttons
@@ -3229,10 +3341,20 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 						if(!data.depthOrRightRaw().empty())
 						{
 							pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
+							pcl::IndicesPtr validIndices(new std::vector<int>);
 							cloud = util3d::cloudFromSensorData(data,
 									ui_->spinBox_projDecimation->value(),
 									ui_->doubleSpinBox_projMaxDepth->value(),
-									ui_->doubleSpinBox_gridCellSize->value());
+									ui_->doubleSpinBox_projMinDepth->value(),
+									validIndices.get(),
+									ui_->parameters_toolbox->getParameters());
+							UASSERT(ui_->doubleSpinBox_gridCellSize->value() > 0);
+							cloud = util3d::voxelize(cloud, validIndices, ui_->doubleSpinBox_gridCellSize->value());
+
+							// add pose rotation without yaw
+							float roll, pitch, yaw;
+							graphFiltered.at(ids[i]).getEulerAngles(roll, pitch, yaw);
+							cloud = util3d::transformPointCloud(cloud, Transform(0,0,0, roll, pitch, 0));

 							if(cloud->size())
 							{
@@ -3554,9 +3676,10 @@ void DatabaseViewer::updateGraphView()

 void DatabaseViewer::updateGrid()
 {
-	if((sender() != ui_->spinBox_projDecimation && sender() != ui_->doubleSpinBox_projMaxDepth && sender()!=ui_->doubleSpinBox_projMaxAngle && sender()!=ui_->spinBox_projClusterSize) ||
+	if((sender() != ui_->spinBox_projDecimation && sender() != ui_->doubleSpinBox_projMaxDepth && sender() != ui_->doubleSpinBox_projMinDepth && sender()!=ui_->doubleSpinBox_projMaxAngle && sender()!=ui_->spinBox_projClusterSize) ||
 	   (sender() == ui_->spinBox_projDecimation && ui_->groupBox_gridFromProjection->isChecked()) ||
 	   (sender() == ui_->doubleSpinBox_projMaxDepth && ui_->groupBox_gridFromProjection->isChecked()) ||
+	   (sender() == ui_->doubleSpinBox_projMinDepth && ui_->groupBox_gridFromProjection->isChecked()) ||
 	   (sender() == ui_->doubleSpinBox_projMaxAngle && ui_->groupBox_gridFromProjection->isChecked()) ||
 	   (sender() == ui_->spinBox_projClusterSize && ui_->groupBox_gridFromProjection->isChecked()))
 	{
@@ -3670,11 +3793,17 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFrom = util3d::cloudFromSensorData(
 				dataFrom,
 				ui_->spinBox_icp_decimation->value(),
-				ui_->doubleSpinBox_icp_maxDepth->value());
+				ui_->doubleSpinBox_icp_maxDepth->value(),
+				ui_->doubleSpinBox_icp_minDepth->value(),
+				0,
+				ui_->parameters_toolbox->getParameters());
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTo = util3d::cloudFromSensorData(
 				dataTo,
 				ui_->spinBox_icp_decimation->value(),
-				ui_->doubleSpinBox_icp_maxDepth->value());
+				ui_->doubleSpinBox_icp_maxDepth->value(),
+				ui_->doubleSpinBox_icp_minDepth->value(),
+				0,
+				ui_->parameters_toolbox->getParameters());
 		int maxLaserScans = cloudFrom->size();
 		dataFrom.setLaserScanRaw(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudFrom), Transform()), maxLaserScans, 0);
 		dataTo.setLaserScanRaw(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudTo), Transform()), maxLaserScans, 0);
@@ -4117,8 +4246,8 @@ void DatabaseViewer::updateLoopClosuresSlider(int from, int to)
 	else
 	{
 		ui_->horizontalSlider_loops->setEnabled(false);
-		ui_->constraintsViewer->removeAllClouds();
-		ui_->constraintsViewer->update();
+		constraintsViewer_->removeAllClouds();
+		constraintsViewer_->update();
 		updateConstraintButtons();
 	}
 }
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
matlabbe	f2d48cb894	Fixed ICP-only registration with already provided guess (no need to do visual guess, as the guess can be already good)	2016-06-23 11:08:20 -04:00
matlabbe	22766e958f	Update VWDictionary.cpp Fixed "HAVE_OPENCV_CUDAFEATURES2D" build error of https://github.com/introlab/rtabmap/issues/85	2016-06-22 19:28:22 -04:00
matlabbe	8e76de7d34	Fixed GTSAM/Eigen include dir	2016-06-15 15:48:56 -04:00
matlabbe	abd376a44c	segmentObstaclesFromGround() fixed identical ground and obstacles indices	2016-06-14 11:55:58 -04:00
matlabbe	4a3f490814	Added Reg/Force2D compatibility name for Reg/Force3DoF	2016-06-09 15:49:17 -04:00
matlabbe	cbf348fafa	labels can be saved in localization mode	2016-06-08 18:19:51 -04:00
matlabbe	e205883de5	Export: set 0 voxel size by default	2016-06-08 11:53:25 -04:00
matlabbe	ce04336648	fixed cmake warning, removed a .DS_Store from repository	2016-06-04 16:21:00 -04:00
matlabbe	a8bf7e5d5f	Fixed Qt5 plugins release. Zed driver: sdded 2 seconds delay before sending grab error.	2016-06-04 15:17:13 -04:00
matlabbe	69e1973544	Update MainWindow.cpp	2016-06-03 15:06:18 -04:00
matlabbe	2f817568e2	MainWindow: Don't show an error if a created cloud is empty	2016-06-02 17:52:22 -04:00
matlabbe	d9611f784c	Added ZED parameters	2016-06-02 17:27:12 -04:00
matlabbe	9430bcbf2e	Increased ROS package version to 0.11.7	2016-06-01 14:53:00 -04:00
matlabbe	a6f7062f92	Export intern dependencies in RTABMapConfig.cmake	2016-06-01 13:13:30 -04:00
matlabbe	e234717129	Fixed ZED sdk build on Linux (added c++11)	2016-05-31 20:19:04 -04:00
matlabbe	6f1f490370	0.11.7: Added ZED sdk support	2016-05-31 19:09:49 -04:00
matlabbe	b2bb421063	Fixed pcl:OrganizedFastMesh link error for type pcl::PointXYZRGBNormal with PCL 1.8 (issue #75 )	2016-05-31 12:23:13 -04:00
matlabbe	be13a9b967	Fixed localization bug when virtual links are added	2016-05-26 17:28:06 -04:00
matlabbe	0fa41d317a	Added log msg to tell when switching from Mapping to Localization is finished	2016-05-26 12:51:32 -04:00
matlabbe	a4d36e0212	MainWindow: fixed same bug as previous commit `30fecd412c` but on runtime	2016-05-26 12:38:18 -04:00
matlabbe	30fecd412c	MainWindow: Fixed clouds shown (and should not) after refreshing map with grid from projection enabled and cloud map visualization unchecked	2016-05-26 12:35:02 -04:00
matlabbe	0d61c12dcd	Create map from projection: Removed debug cloud saved	2016-05-26 12:23:44 -04:00
matlabbe	7f2a899c6f	Updated default decimation to 4 instead of 8	2016-05-26 12:13:59 -04:00
matlabbe	0bbb773e95	fixed passthrough not using input indices	2016-05-21 16:02:57 -04:00
matlabbe	7aa9c92971	Added util3d::pasthrough returning indices for convenience. segmentObstaclesFromGound(): filtering obstacles under maxGroundHeight if set	2016-05-21 15:54:37 -04:00
Mathieu Labbe	904f4bb4d8	Features2d: updated computeROI() to be more precise	2016-05-21 14:56:25 -04:00
matlabbe	09696195f4	Version 0.11.6	2016-05-20 17:27:03 -04:00
matlabbe	971c96f566	MainWindow projected map: fixed bad occupancy from wrong normals after voxel filtering. util3d::segmentObstaclesFromGround(): added flatObstacles argument.	2016-05-20 17:25:52 -04:00
matlabbe	4fdaa2b708	CloudViewer: fixed numpad color not working -> reverted changes from https://github.com/introlab/rtabmap/commit/ad0afc58c0d404b420108e527e454201d89a840e#diff-d7a550026127f42ccabdb5e42979eeeb	2016-05-20 16:38:35 -04:00
matlabbe	9f6af75f79	Local scan matching: set larger scan points for max scan points when it is not set. Fixed missing scan Ids in links' user data to correctly visualize proximity links by space in DatabaseViewer. CloudViewer: using line instead of arrow between the referential and the frustum. removed parameter "RGBD/ProximityPathScansMerged" as visual proximity by space already does that.	2016-05-20 11:44:50 -04:00
matlabbe	b29ce28877	CloudViewer: Fixed crash when changing frustum's color	2016-05-19 17:19:11 -04:00
matlabbe	ff7406a755	Memory::computeTransform(): removed setting guess to identity if input guess is null	2016-05-19 17:01:46 -04:00
matlabbe	a8be08a19a	Registration: if parent registration fails, continue with the child if the prior guess is not null	2016-05-19 16:11:26 -04:00
matlabbe	1ecaae364c	Generalized neighbor link refining using registration done in Memory (can be visual, visual+icp or, icp)	2016-05-19 15:35:30 -04:00
Mathieu Labbe	2637f74094	Added some debug info	2016-05-19 14:31:29 -04:00
Mathieu Labbe	02d944aa67	CalibrationDialog: fixed D not shown properly, add scroll area for small screens	2016-05-19 11:40:48 -04:00
matlabbe	a6bad6d2a5	F2F nonholonomic bad transforms fixed (https://github.com/introlab/rtabmap_ros/issues/74 )	2016-05-17 18:10:24 -04:00
matlabbe	0ae131108d	CloudViewer: Added local transformation between base frame and camera frame (when they are not the same)	2016-05-17 16:32:21 -04:00
matlabbe	2db6b2ceef	segmentObstaclesFromGround: Added epsilon for ground surface inclusion inside maximum and minimum heights	2016-05-16 16:26:16 -04:00
matlabbe	f19c058634	Merge branch 'jade-devel' of https://github.com/introlab/rtabmap into jade-devel	2016-05-14 12:11:10 -04:00
matlabbe	ceb4acd749	Merge branch 'master' of https://github.com/introlab/rtabmap into jade-devel	2016-05-14 12:09:29 -04:00
matlabbe	dde0e26110	Tango: C-API Code Migration to Mira release	2016-05-12 14:34:51 -04:00
matlabbe	1fbcc2319a	0.11.5: added RTABMAP_QT_VERSION to RTABMapConfig.cmake (used by rtabmap_ros to know to which Qt version it should link)	2016-05-09 11:17:51 -04:00
matlabbe	c7670c505d	ROS: Added qt_gui_core dependency to get libqt4-dev or libqt5-dev installed	2016-05-08 11:56:25 -04:00
matlabbe	80c84a66fe	package.xml: added dependency to libvtk-qt (fixing Kinetic on Wily build)	2016-05-06 15:33:10 -04:00
matlabbe	5e4111d67e	ROS: removed libopenni2-dev dependency as it is not yet available on Jessie	2016-05-06 13:07:38 -04:00
matlabbe	c8ed731809	Fixed package dependencies for Kinetic	2016-05-06 12:55:19 -04:00
matlabbe	22577bdf71	ROS: updated package version to 0.11.4	2016-05-06 12:36:11 -04:00
matlabbe	622dfb370d	Merge branch 'master' of https://github.com/introlab/rtabmap	2016-05-05 18:25:46 -04:00
matlabbe	ad0afc58c0	Fixed crash on startup with vtk6+qt5. All cloud viewers in QDockWidget are now created manually instead of being defined in the *.ui files. The generated ui didn't pass the top level parent window to constructor of CloudViewer, which caused a problem when initializing the QVTKWidget.	2016-05-05 18:25:26 -04:00
matlabbe	7c8583f025	fixed crash from #79	2016-05-05 11:53:09 -04:00
matlabbe	74d0bfa1bb	Qt5: fixed "read parameters..." progress dialog showing up on start	2016-05-04 19:03:22 -04:00
matlabbe	e1866d38af	Fixed cmake erros on system with Qt4	2016-05-04 16:51:41 -04:00
matlabbe	b8c2cd9f94	Auto detect which Qt version to use (Qt5 has priority if both are installed). ROS Kinetic fix for vtk libproj.so not found bug.	2016-05-04 16:45:19 -04:00
matlabbe	d2882a88f3	fixed isnan() not defined	2016-05-03 20:44:06 -04:00
matlabbe	7230474f98	MainWindow: update pose in 3D Map view even if odometry has no images	2016-04-29 14:29:55 -04:00
matlabbe	b2c36742ee	travis: disabled email notifications	2016-04-28 10:11:44 -04:00
matlabbe	427f44dca3	Update README.md	2016-04-28 10:01:58 -04:00
matlabbe	fc7659888c	travis: auto answer yes on apt-get	2016-04-28 09:49:33 -04:00
matlabbe	f81734a8e5	travis: added ros keys	2016-04-28 09:44:09 -04:00
matlabbe	d92c3d224c	travis updated	2016-04-28 09:36:44 -04:00
matlabbe	70fd054f67	travis pcl	2016-04-28 09:31:19 -04:00
matlabbe	5a3c7c7a24	updated .travis.yml	2016-04-28 09:27:28 -04:00
matlabbe	1091960282	Added .travis.yml	2016-04-28 09:15:36 -04:00
matlabbe	a78587e505	Hidden FPS shown in CloudViewer	2016-04-26 14:24:09 -04:00
matlabbe	f408ef8922	Calibration: added max scale parameter	2016-04-25 17:02:25 -04:00
matlabbe	8e1fc0df56	Calibration: added support for stereo IR cameras	2016-04-25 09:52:03 -04:00
Mathieu Labbe	4cc1c66f09	Segment ground/obstacles: added max ground height parameter	2016-04-15 17:41:40 -04:00
Mathieu Labbe	8bb5f0d905	Fixed Vis/MinDepth not working (with depth image in mm)	2016-04-14 11:24:36 -04:00
Mathieu Labbe	d5b385ec69	PreferencesDialog: Removed NN warning when RTABMAP_NONFREE=0	2016-04-14 11:01:24 -04:00
matlabbe	7c0db617cf	GraphViewer: added hide/show graphs and paths options	2016-04-13 12:57:28 -04:00
matlabbe	809f5dd4df	MainWindow: working directory of GUI widgets can be updated when running	2016-04-13 11:58:54 -04:00
matlabbe	9339b86633	API change: segmentObstaclesFromGround() and normalFiltering()	2016-04-12 18:57:04 -04:00
matlabbe	fc76e5b8f3	3D projection: adding pose rotation (roll, pitch) before projection	2016-04-12 17:45:50 -04:00
matlabbe	f511896c43	MainWindow: Fixed "Invalid (NaN, Inf) point coordinates given to radiusSearch" error when creating occupancy map from projection	2016-04-12 17:16:13 -04:00
matlabbe	b53b861342	GUI: Fixed viewpoint including local transform when generating organized mesh	2016-04-12 16:47:55 -04:00
matlabbe	684e9fb8b9	0.11.4: API change: added minDepth parameter to cloudFromXXXXX() methods and removed voxel parameter	2016-04-12 15:14:04 -04:00
matlabbe	f185a4d739	Updated default RGBD/OptimizeMaxError to 0.05 m for Tango app	2016-04-11 10:43:28 -04:00
matlabbe	29dd529762	Main app: Open database as argument, file association *.db on Mac OS X, automatically ask to download clouds when opening a database	2016-04-10 14:52:14 -04:00
matlabbe	72fc714cb4	Tango: fixed export with optimization, added "Nodes Filtering" option	2016-04-09 18:57:49 -04:00
matlabbe	656431a03f	Tango: Fixed Mem/ImagePreDecimation when changing to/from 720p mode	2016-04-09 12:08:30 -04:00
matlabbe	cc9c9d552c	Tango: Fixed auto-exposure config error (now ignoring it), updated log info	2016-04-08 18:58:18 -04:00
matlabbe	ab35106359	Fixed build errors with log2 not defined on some os	2016-04-08 17:20:13 -04:00
matlabbe	dece54ca3e	Added parameters: Mem/ImagePreDecimation Mem/ImagePostDecimation Odom/ImageDecimation	2016-04-08 16:15:08 -04:00
matlabbe	0b3da6b246	DbViewer: labels are now selectable	2016-04-07 12:26:24 -04:00
matlabbe	d5553db9eb	DbViewer: Added calibration info	2016-04-07 12:15:58 -04:00
matlabbe	748360aac7	Vocabulary: Binary descriptors are saved as is even if there is a float conversion for flann	2016-04-07 11:48:07 -04:00
matlabbe	0bb82af91c	Tango: moved Auto-exposure option in Rendering menu, updated pop-up time "Loop closure detected"	2016-04-06 18:07:45 -04:00
matlabbe	9270fc9ca9	Fixed OptimizerG2O::pixelVariance_ not initialized	2016-04-06 17:02:31 -04:00
matlabbe	d2ebdea96d	Added parameter "g2o/PixelVariance" (default 1)	2016-04-06 15:24:36 -04:00
matlabbe	000a2727ef	Added .gitignore	2016-04-05 17:10:08 -04:00
matlabbe	e3a44bbb08	Tango: increased package version to 2	2016-04-04 13:24:25 -04:00
matlabbe	af405e69b1	Tango #57 : Increased version to 0.11.3, updated Post-Processing actions, added mesh rendering actions, fixed point cloud rendering	2016-04-04 13:03:43 -04:00
matlabbe	ff32f54aa8	Rtabmap::detectMoreLoopClosures(): use Memory::computeTransform() with signature parameters	2016-04-03 21:49:12 -04:00
matlabbe	5d9522c901	Tango #57 : Global optimization / Post-processing on pause	2016-04-03 21:35:30 -04:00
matlabbe	30e52b785a	Tango #57 : Added 720p option, Export PLY or OBJ, Added Rendering and Mapping menus, RtabmapThread: Fixed large covariance (9999) detection	2016-04-02 15:17:40 -04:00
matlabbe	d489cd48e9	Frustum hiding on action click	2016-03-31 12:44:03 -04:00
matlabbe	30777b630a	Fixed issue #10 (missing one iteration on rtabmap-console)	2016-03-29 13:07:32 -04:00
matlabbe	8030d89634	Implemented OptimizerG2O::optimimzeBA(). Updated PostProcessingDialog (g2o sba option). Fixed words descriptors not filled in rtabmap::getMap3D(). CameraModelD(): return 5 null coeff distorsions if not set	2016-03-28 18:19:17 -04:00
matlabbe	18759e7197	CMake: Added info about CMAKE_INSTALL_LIBDIR	2016-03-23 19:56:45 -04:00
matlabbe	d74cb1b232	Updated pull request (supporting internal/external build)	2016-03-23 19:07:44 -04:00
matlabbe	cee3a77a4c	Merge pull request #61 from Wade5566/patch-1 Update CMakeLists.txt	2016-03-23 19:06:40 -04:00
matlabbe	1120a74b13	Updated RGB-D mapping C++ example	2016-03-23 14:18:22 -04:00
Wade5566	6a787670a1	Update CMakeLists.txt Fix the CMakeLists.txt which cannot be used	2016-03-23 17:48:16 +08:00
matlabbe	a01fb82f51	ExportCloudsDialog: Updated TextureMesh export. MainWindow: fixed exporting only visible clouds.	2016-03-22 20:44:18 -04:00
matlabbe	c43fd6a2a7	Optimizer::create() updated interface	2016-03-22 11:06:45 -04:00
matlabbe	9385aa2332	Tango: Added OBJ export (with texture)	2016-03-21 20:03:03 -04:00
matlabbe	bc78f789eb	Tango: Added texture to meshes #57	2016-03-21 15:45:26 -04:00
matlabbe	b0629d626e	Merge branch 'master' of github.com:introlab/rtabmap into jade-devel	2015-10-17 14:16:14 -04:00
matlabbe	ec09d69145	jade package: libfreenect -> libfreenect-dev	2015-08-04 15:52:13 -04:00
matlabbe	8ddbc6bf96	Merge branch 'master' of https://github.com/introlab/rtabmap into jade-devel	2015-05-12 08:37:38 -04:00
matlabbe	b608e50296	Merge branch 'master' of https://github.com/introlab/rtabmap into jade-devel	2015-05-12 08:29:41 -04:00
matlabbe	7fa791992c	Jade branch: changed libfreenect to libfreenect-dev ROS dependency (run-depend)	2015-05-10 21:22:59 -04:00
matlabbe	fae21132ee	Jade branch: changed libfreenect to libfreenect-dev ROS dependency	2015-05-10 21:13:52 -04:00