Merge branch 'jade-devel' of https://github.com/introlab/rtabmap into jade-devel

Update CMakeLists.txt

.gitignore

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

.travis.yml

@@ -0,0 +1,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

CMakeLists.txt

@@ -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)
+#IF(NOT DEFINED CMAKE_INSTALL_LIBDIR)
-   set(CMAKE_INSTALL_LIBDIR "lib")
+#   set(CMAKE_INSTALL_LIBDIR "lib")
-ENDIF(NOT DEFINED CMAKE_INSTALL_LIBDIR)
+#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 5)
 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")
@@ -143,6 +141,11 @@ 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 +170,22 @@ IF(ZLIB_FOUND)
 ENDIF(ZLIB_FOUND)
 
 IF(WITH_QT)
-# If Qt is here, the GUI will be built
+    FIND_PACKAGE(VTK)
-    IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
+    IF(NOT VTK_FOUND)
+        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()
+    ENDIF(NOT Qt5_FOUND)
-        FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui Svg)
+        
-    ENDIF()
     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 +209,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)
@@ -365,9 +377,14 @@ file(RELATIVE_PATH REL_LIB_DIR "${CMAKE_INSTALL_PREFIX}/${INSTALL_CMAKE_DIR}" "$
 set(CONF_INCLUDE_DIRS "${PROJECT_SOURCE_DIR}/corelib/include" 
 					  "${PROJECT_SOURCE_DIR}/guilib/include" 
 					  "${PROJECT_SOURCE_DIR}/utilite/include")
-set(CONF_LIB_DIR "${CMAKE_ARCHIVE_OUTPUT_DIRECTORY}")
+set(CONF_LIB_DIR "${CMAKE_ARCHIVE_OUTPUT_DIRECTORY} ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}")
 IF(QT4_FOUND OR Qt5_FOUND)
    set(CONF_WITH_GUI ON)
+   IF(QT4_FOUND)
+      set(CONF_QT_VERSION 4)
+   ELSE()
+      set(CONF_QT_VERSION 5)
+   ENDIF()
 ELSE()
    set(CONF_WITH_GUI OFF)
 ENDIF()
@@ -479,6 +496,7 @@ MESSAGE(STATUS "Info :")
 MESSAGE(STATUS "  Version : ${RTABMAP_VERSION}")
 MESSAGE(STATUS "  CMAKE_INSTALL_PREFIX = ${CMAKE_INSTALL_PREFIX}")
 MESSAGE(STATUS "  CMAKE_BUILD_TYPE =     ${CMAKE_BUILD_TYPE}")
+MESSAGE(STATUS "  CMAKE_INSTALL_LIBDIR = ${CMAKE_INSTALL_LIBDIR}")
 MESSAGE(STATUS "  BUILD_APP =            ${BUILD_APP}")
 MESSAGE(STATUS "  BUILD_TOOLS =          ${BUILD_TOOLS}")
 MESSAGE(STATUS "  BUILD_EXAMPLES =       ${BUILD_EXAMPLES}")
@@ -590,7 +608,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 "--------------------------------------------")

README.md

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

RTABMapConfig.cmake.in

@@ -11,41 +11,41 @@ get_filename_component(RTABMap_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH)
 set(RTABMap_INCLUDE_DIRS "@CONF_INCLUDE_DIRS@")
 
 #core lib
-find_library(RTABMap_CORE_RELEASE NAMES rtabmap_core NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@")
+find_library(RTABMap_CORE_RELEASE NAMES rtabmap_core NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
-find_library(RTABMap_CORE_DEBUG NAMES rtabmap_cored NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@")
+find_library(RTABMap_CORE_DEBUG NAMES rtabmap_cored NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
 
 IF(RTABMap_CORE_DEBUG AND RTABMap_CORE_RELEASE)
    SET(RTABMap_CORE
       debug ${RTABMap_CORE_DEBUG}
       optimized ${RTABMap_CORE_RELEASE}
    )
-ELSEIF(RTABMap_CORE_RELEASE)
-   SET(RTABMap_CORE ${RTABMap_CORE_RELEASE})
 ELSEIF(RTABMap_CORE_DEBUG)
    SET(RTABMap_CORE ${RTABMap_CORE_DEBUG})
+ELSE()
+   SET(RTABMap_CORE ${RTABMap_CORE_RELEASE})
 ENDIF()
 
 #utilite lib
-find_library(RTABMap_UTILITE_RELEASE NAMES rtabmap_utilite NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@")
+find_library(RTABMap_UTILITE_RELEASE NAMES rtabmap_utilite NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
-find_library(RTABMap_UTILITE_DEBUG NAMES rtabmap_utilited NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@")
+find_library(RTABMap_UTILITE_DEBUG NAMES rtabmap_utilited NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
 
 IF(RTABMap_UTILITE_DEBUG AND RTABMap_UTILITE_RELEASE)
    SET(RTABMap_UTILITE
       debug ${RTABMap_UTILITE_DEBUG}
       optimized ${RTABMap_UTILITE_RELEASE}
    )
-ELSEIF(RTABMap_UTILITE_RELEASE)
-   SET(RTABMap_UTILITE ${RTABMap_UTILITE_RELEASE})
 ELSEIF(RTABMap_UTILITE_DEBUG)
    SET(RTABMap_UTILITE ${RTABMap_UTILITE_DEBUG})
+ELSE()
+   SET(RTABMap_UTILITE ${RTABMap_UTILITE_RELEASE})
 ENDIF()
 
 set(RTABMap_LIBRARIES ${RTABMap_CORE} ${RTABMap_UTILITE})
 
 #gui lib (OFF if RTAB-Map is not built with Qt)
 if(@CONF_WITH_GUI@)
-   find_library(RTABMap_GUI_RELEASE NAMES rtabmap_gui NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@")
+   find_library(RTABMap_GUI_RELEASE NAMES rtabmap_gui NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
-   find_library(RTABMap_GUI_DEBUG NAMES rtabmap_guid NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@")
+   find_library(RTABMap_GUI_DEBUG NAMES rtabmap_guid NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
    
    IF(RTABMap_GUI_DEBUG AND RTABMap_GUI_RELEASE)
       SET(RTABMap_GUI
@@ -62,12 +62,9 @@ if(@CONF_WITH_GUI@)
 endif(@CONF_WITH_GUI@)
 
 #backward compatibilities
-if(RTABMap_CORE)
+set(RTABMAP_CORE ${RTABMap_CORE})
-   set(RTABMAP_CORE ${RTABMap_CORE})
+set(RTABMAP_UTILITE ${RTABMap_UTILITE})
-endif(RTABMap_CORE)
-if(RTABMap_UTILITE)
-   set(RTABMAP_UTILITE ${RTABMap_UTILITE})
-endif(RTABMap_UTILITE)
 if(RTABMap_GUI)
    set(RTABMAP_GUI ${RTABMap_GUI})
+   set(RTABMAP_QT_VERSION @CONF_QT_VERSION@)
 endif(RTABMap_GUI)

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

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

app/android/AndroidManifest.xml.in

@@ -2,8 +2,8 @@
 <!-- BEGIN_INCLUDE(manifest) -->
 <manifest xmlns:android="http://schemas.android.com/apk/res/android"
         package="com.introlab.rtabmap"
-        android:versionCode="1"
+        android:versionCode="9"
-        android:versionName="0.11.2">
+        android:versionName="@RTABMAP_VERSION@">
 
     <uses-permission android:name="android.permission.CAMERA" />
     <uses-permission android:name="android.permission.READ_EXTERNAL_STORAGE" />
@@ -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" />

app/android/CMakeLists.txt

@@ -14,11 +14,20 @@ if(NOT ANDROID_EXECUTABLE)
   message(FATAL_ERROR "Can not find android command line tool: android")
 endif()
 
+configure_file(
+  "${CMAKE_CURRENT_SOURCE_DIR}/AndroidManifest.xml.in"
+  "${CMAKE_CURRENT_SOURCE_DIR}/AndroidManifest.xml")
+  
 configure_file(
   "${CMAKE_CURRENT_SOURCE_DIR}/AndroidManifest.xml"
   "${CMAKE_CURRENT_BINARY_DIR}/AndroidManifest.xml"
   COPYONLY)
   
+configure_file(
+  "${CMAKE_CURRENT_SOURCE_DIR}/info.txt.in"
+  "${CMAKE_CURRENT_SOURCE_DIR}/res/raw/info.txt")
+  
+
 configure_file(
   "${CMAKE_CURRENT_SOURCE_DIR}/ant.properties.in"
   "${CMAKE_CURRENT_BINARY_DIR}/ant.properties"

app/android/info.txt.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>

app/android/jni/CameraTango.cpp

@@ -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",
+	int ret = TangoConfig_setBool(tango_config_, "config_enable_auto_recovery", is_atuo_recovery);
-	is_atuo_recovery);
 	if (ret != TANGO_SUCCESS)
 	{
 		LOGE("NativeRTABMap: config_enable_auto_recovery() failed with error code: %d", ret);
@@ -141,13 +144,15 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 		return false;
 	}
 
-	// disable auto exposure
+	// disable auto exposure (disabled, seems broken on latest Tango releases)
 	ret = TangoConfig_setBool(tango_config_, "config_color_mode_auto", autoExposure_);
 	if (ret != TANGO_SUCCESS)
 	{
 		LOGE("NativeRTABMap: config_color_mode_auto() failed with error code: %d", ret);
-		return false;
+		//return false;
 	}
+	else
+	{
 		if(!autoExposure_)
 		{
 			ret = TangoConfig_setInt32(tango_config_, "config_color_iso", 800);
@@ -157,6 +162,13 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 				return false;
 			}
 		}
+		bool verifyAutoExposureState;
+		int32_t verifyIso, verifyExp;
+		TangoConfig_getBool( tango_config_, "config_color_mode_auto", &verifyAutoExposureState );
+		TangoConfig_getInt32( tango_config_, "config_color_iso", &verifyIso );
+		TangoConfig_getInt32( tango_config_, "config_color_exp", &verifyExp );
+		LOGI( "NativeRTABMap: config_color autoExposure=%s %d %d", verifyAutoExposureState?"On" : "Off", verifyIso, verifyExp );
+	}
 
 	// Enable depth.
 	ret = TangoConfig_setBool(tango_config_, "config_enable_depth", true);
@@ -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(
+	ret = TangoConfig_getString(tango_config_, "tango_service_library_version", tango_core_version, kVersionStringLength);
-	tango_config_, "tango_service_library_version",
-	tango_core_version, kVersionStringLength);
 	if (ret != TANGO_SUCCESS)
 	{
 		LOGE("NativeRTABMap: get tango core version failed with error code: %d", ret);
@@ -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,7 +244,7 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 	ret = TangoService_connect(this, tango_config_);
 	if (ret != TANGO_SUCCESS)
 	{
-		LOGE("PointCloudApp: Failed to connect to the Tango service with error code: %d", ret);
+		LOGE("NativeRTABMap: Failed to connect to the Tango service with error code: %d", ret);
 		return false;
 	}
 
@@ -253,7 +263,7 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 	ret = TangoService_getPoseAtTime(0.0, frame_pair, &pose_data);
 	if (ret != TANGO_SUCCESS)
 	{
-			LOGE("PointCloudApp: Failed to get transform between the IMU frame and device frames");
+		LOGE("NativeRTABMap: Failed to get transform between the IMU frame and device frames");
 		return false;
 	}
 	imuTDevice_ = rtabmap::Transform(
@@ -271,7 +281,7 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 	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");
+		LOGE("NativeRTABMap: Failed to get transform between the color camera frame and device frames");
 		return false;
 	}
 	imuTDepthCamera_ = rtabmap::Transform(
@@ -290,7 +300,7 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 	ret = TangoService_getCameraIntrinsics(TANGO_CAMERA_COLOR, &color_camera_intrinsics);
 	if (ret != TANGO_SUCCESS)
 	{
-			LOGE("SynchronizationApplication: Failed to get the intrinsics for the color camera with error code: %d.", ret);
+		LOGE("NativeRTABMap: Failed to get the intrinsics for the color camera with error code: %d.", ret);
 		return false;
 	}
 	model_ = CameraModel(
@@ -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())
 		{

app/android/jni/CameraTango.h

@@ -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_;

app/android/jni/RTABMapApp.cpp

@@ -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(mappingParameters_.begin(), mappingParameters_.end());
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDLoopClosureReextractFeatures(), std::string("false")));
+
 	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::kOptimizerIterations(), graphOptimization_?"10":"0"));
-	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::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_);
+	camera_ = new rtabmap::CameraTango(fullResolution_?1:2, 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);
 }
 
 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);
+	rtabmap_->init(parameters, databasePath);
-	rtabmapThread_ = new rtabmap::RtabmapThread(rtabmap);
+	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
-
+			boost::mutex::scoped_lock  lock(meshesMutex_);
-			//filter poses?
-
-			// make sure the last pose is here though
-			//poses.insert(*rtabmapEvents.back().poses().rbegin());
-
 			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());
+							cloud = rtabmap::util3d::cloudRGBFromSensorData(data, data.imageRaw().rows/data.depthRaw().rows, maxCloudDepth_, 0, indices.get());
-
-							//compute normals
-							pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud = rtabmap::util3d::computeNormals(cloudWithoutNormals, 6);
 
 							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::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
-								pcl::ExtractIndices<pcl::PointXYZRGBNormal> filter;
+								pcl::ExtractIndices<pcl::PointXYZRGB> filter;
-								filter.setIndices(indicesKept);
+								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);
+								std::vector<pcl::Vertices> polygons = rtabmap::util3d::organizedFastMesh(output, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
-								pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr outputCloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+								pcl::PointCloud<pcl::PointXYZRGB>::Ptr outputCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 								std::vector<pcl::Vertices> outputPolygons;
-								rtabmap::util3d::filterNotUsedVerticesFromMesh(
+
-										*output,
+								outputCloud = output;
-										polygons,
+								outputPolygons = polygons;
-										*outputCloud,
+
-										outputPolygons);
 								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_);
+									std::pair<std::map<int, Mesh>::iterator, bool> inserted = createdMeshes_.insert(std::make_pair(id, Mesh()));
-									createdMeshes_.insert(std::make_pair(id, std::make_pair(std::make_pair(outputCloud, outputPolygons), iter->second)));
+									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);
+						LOGI("Created odom cloud (rgb=%dx%d depth=%dx%d cloud=%dx%d)",
-						main_scene_.addOrUpdateCloud(-1, cloud, polygons, opengl_world_T_rtabmap_world*event.pose());
+													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,54 +727,160 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
 	bool success = false;
 
 	//Assemble the meshes
-	UINFO("Organized fast mesh... ");
+	if(UFile::getExtension(filePath).compare("obj") == 0)
-
+	{
+		pcl::TextureMesh textureMesh;
+		std::vector<cv::Mat> textures;
+		int totalPolygons = 0;
 		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+		{
+			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)
+			{
+				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);
+				}
+			}
+			textureMesh.tex_materials.resize(oi);
+			textureMesh.tex_polygons.resize(oi);
+			textures.resize(oi);
+
+			if(textures.size())
+			{
+				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());
+				}
+			}
+		}
+	}
+	else
+	{
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGB>);
 		std::vector<pcl::Vertices> mergedPolygons;
 
 		{
 			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();
+			for(std::map<int, Mesh>::iterator iter=createdMeshes_.begin();
 				iter!= createdMeshes_.end();
 				++iter)
 			{
-			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformedCloud = rtabmap::util3d::transformPointCloud(iter->second.first.first, iter->second.second);
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+				std::vector<pcl::Vertices> densePolygons;
+
+				rtabmap::util3d::filterNotUsedVerticesFromMesh(
+						*iter->second.cloud,
+						iter->second.polygons,
+						*denseCloud,
+						densePolygons);
+
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformedCloud = rtabmap::util3d::transformPointCloud(denseCloud, iter->second.pose);
 				if(mergedClouds->size() == 0)
 				{
 					*mergedClouds = *transformedCloud;
-				mergedPolygons = iter->second.first.second;
+					mergedPolygons = densePolygons;
 				}
 				else
 				{
-				rtabmap::util3d::appendMesh(*mergedClouds, mergedPolygons, *transformedCloud, iter->second.first.second);
+					rtabmap::util3d::appendMesh(*mergedClouds, mergedPolygons, *transformedCloud, densePolygons);
 				}
 			}
 		}
-	if(closeVerticesDistance)
-	{
-		UINFO("Filtering assembled mesh (points=%d, polygons=%d, close vertices=%fm)...",
-				(int)mergedClouds->size(), (int)mergedPolygons.size(), closeVerticesDistance);
-
-		mergedPolygons = rtabmap::util3d::filterCloseVerticesFromMesh(
-				mergedClouds,
-				mergedPolygons,
-				closeVerticesDistance,
-				M_PI/4,
-				true);
-
-		// filter invalid polygons
-		unsigned int count = mergedPolygons.size();
-		mergedPolygons = rtabmap::util3d::filterInvalidPolygons(mergedPolygons);
-		UINFO("Filtered %d invalid polygons.", (int)count-mergedPolygons.size());
-
-		// 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;
-	}
 
 		if(mergedClouds->size() && mergedPolygons.size())
 		{
@@ -706,12 +888,85 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
 			pcl::toPCLPointCloud2(*mergedClouds, mesh.cloud);
 			mesh.polygons = mergedPolygons;
 
-		UINFO("Saving to %s.", filePath.c_str());
+			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!");
+			LOGI("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;
-			{
-				boost::mutex::scoped_lock  lock(rtabmapMutex_);
 			if(camera_->isRunning())
 			{
+				boost::mutex::scoped_lock lock(rtabmapMutex_);
 				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");
 			}
 		}
 	}
@@ -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");
+		}
+	}
 }
 

app/android/jni/RTABMapApp.h

@@ -50,14 +50,11 @@ class RTABMapApp : public UEventsHandler {
   RTABMapApp();
   ~RTABMapApp();
 
-  // Initialize the Tango Service, this function starts the communication
+  void onCreate(JNIEnv* env, jobject caller_activity);
-  // between the application and the Tango Service.
-  // The activity object is used for checking if the API version is outdated.
-  int TangoInitialize(JNIEnv* env, jobject caller_activity);
 
   void openDatabase(const std::string & databasePath);
 
-  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_;
 };

app/android/jni/jni_interface.cpp

@@ -48,11 +48,11 @@ void GetJStringContent(JNIEnv *AEnv, jstring AStr, std::string &ARes) {
   AEnv->ReleaseStringUTFChars(AStr,s);
 }
 
-JNIEXPORT jint JNICALL
+JNIEXPORT void JNICALL
-Java_com_introlab_rtabmap_RTABMapLib_initialize(
+Java_com_introlab_rtabmap_RTABMapLib_onCreate(
     JNIEnv* env, jobject, jobject activity)
 {
-	return app.TangoInitialize(env, activity);
+	return app.onCreate(env, activity);
 }
 
 JNIEXPORT void JNICALL
@@ -64,10 +64,10 @@ Java_com_introlab_rtabmap_RTABMapLib_openDatabase(
 	return app.openDatabase(databasePathC);
 }
 
-JNIEXPORT jint JNICALL
+JNIEXPORT bool JNICALL
-Java_com_introlab_rtabmap_RTABMapLib_onResume(
+Java_com_introlab_rtabmap_RTABMapLib_onTangoServiceConnected(
-    JNIEnv*, jobject) {
+		JNIEnv* env, jobject, jobject iBinder) {
-  return app.onResume();
+  return app.onTangoServiceConnected(env, iBinder);
 }
 
 JNIEXPORT void JNICALL
@@ -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

app/android/jni/point_cloud_drawable.cpp

@@ -1,104 +1,98 @@
-
 /*
- * Copyright 2014 Google Inc. All Rights Reserved.
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
- *
+All rights reserved.
- * Licensed under the Apache License, Version 2.0 (the "License");
+
- * you may not use this file except in compliance with the License.
+Redistribution and use in source and binary forms, with or without
- * You may obtain a copy of the License at
+modification, are permitted provided that the following conditions are met:
- *
+    * Redistributions of source code must retain the above copyright
- *      http://www.apache.org/licenses/LICENSE-2.0
+      notice, this list of conditions and the following disclaimer.
- *
+    * Redistributions in binary form must reproduce the above copyright
- * Unless required by applicable law or agreed to in writing, software
+      notice, this list of conditions and the following disclaimer in the
- * distributed under the License is distributed on an "AS IS" BASIS,
+      documentation and/or other materials provided with the distribution.
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+    * Neither the name of the Universite de Sherbrooke nor the
- * See the License for the specific language governing permissions and
+      names of its contributors may be used to endorse or promote products
- * limitations under the License.
+      derived from this software without specific prior written permission.
- */
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
 
 #include <sstream>
 
 #include "point_cloud_drawable.h"
 #include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UConversion.h"
+#include <opencv2/imgproc/imgproc.hpp>
 #include "util.h"
 
 #include <GLES2/gl2.h>
 
 PointCloudDrawable::PointCloudDrawable(
-		GLuint shaderProgram,
+		GLuint cloudShaderProgram,
-		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		GLuint textureShaderProgram,
-		const std::vector<pcl::Vertices> & indices) :
-		vertex_buffers_(0),
-		nPoints_(0),
-		pose_(1.0f),
-		visible_(true),
-		shader_program_(shaderProgram)
-{
-	UASSERT(!cloud->empty());
-
-	glGenBuffers(1, &vertex_buffers_);
-
-	if(vertex_buffers_)
-	{
-		LOGI("Creating cloud buffer %d", vertex_buffers_);
-		std::vector<float> vertices = std::vector<float>(cloud->size()*4);
-		for(unsigned int i=0; i<cloud->size(); ++i)
-		{
-			vertices[i*4] = cloud->at(i).x;
-			vertices[i*4+1] = cloud->at(i).y;
-			vertices[i*4+2] = cloud->at(i).z;
-			vertices[i*4+3] = cloud->at(i).rgb;
-		}
-
-		glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
-		glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * (int)vertices.size(), (const void *)vertices.data(), GL_STATIC_DRAW);
-		glBindBuffer(GL_ARRAY_BUFFER, 0);
-
-		GLint error = glGetError();
-		if(error != GL_NO_ERROR)
-		{
-			LOGI("OpenGL: Could not allocate point cloud (0x%x)\n", error);
-			vertex_buffers_ = 0;
-		}
-		else
-		{
-			nPoints_ = cloud->size();
-
-			if(indices.size())
-			{
-				int polygonSize = indices[0].vertices.size();
-				UASSERT(polygonSize == 3);
-				indices_.resize(indices.size() * polygonSize);
-				int oi = 0;
-				for(unsigned int i=0; i<indices.size(); ++i)
-				{
-					UASSERT((int)indices[i].vertices.size() == polygonSize);
-					for(int j=0; j<polygonSize; ++j)
-					{
-						indices_[oi++] = (unsigned short)indices[i].vertices[j];
-					}
-				}
-			}
-		}
-	}
-}
-PointCloudDrawable::PointCloudDrawable(
-		GLuint shaderProgram,
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		const std::vector<pcl::Vertices> & indices) :
+		const 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_)
 	{
+		LOGE("OpenGL: could not generate vertex buffers\n");
+		return;
+	}
+
+	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_)
+		{
+			vertex_buffers_ = 0;
+			LOGE("OpenGL: could not generate vertex buffers\n");
+			return;
+		}
+	}
+
 	LOGI("Creating cloud buffer %d", vertex_buffers_);
-		std::vector<float> vertices = std::vector<float>(cloud->size()*4);
+	std::vector<float> vertices;
+	if(textures_)
+	{
+		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,6 +100,7 @@ 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);
@@ -114,27 +109,47 @@ PointCloudDrawable::PointCloudDrawable(
 	GLint error = glGetError();
 	if(error != GL_NO_ERROR)
 	{
-			LOGI("OpenGL: Could not allocate point cloud (0x%x)\n", error);
+		LOGE("OpenGL: Could not allocate point cloud (0x%x)\n", error);
 		vertex_buffers_ = 0;
+		return;
 	}
-		else
+
+	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)
+		{
+			LOGE("OpenGL: Could not allocate texture (0x%x)\n", error);
+			textures_ = 0;
+
+			glDeleteBuffers(1, &vertex_buffers_);
+			vertex_buffers_ = 0;
+			return;
+		}
+	}
+
 	nPoints_ = cloud->size();
 
-			if(indices.size())
+	if(polygons.size())
 	{
-				int polygonSize = indices[0].vertices.size();
+		int polygonSize = polygons[0].vertices.size();
 		UASSERT(polygonSize == 3);
-				indices_.resize(indices.size() * polygonSize);
+		polygons_.resize(polygons.size() * polygonSize);
 		int oi = 0;
-				for(unsigned int i=0; i<indices.size(); ++i)
+		for(unsigned int i=0; i<polygons.size(); ++i)
 		{
-					UASSERT((int)indices[i].vertices.size() == polygonSize);
+			UASSERT((int)polygons[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_);
+		if(meshRendering && textures_)
+		{
+			glUseProgram(texture_shader_program_);
 
-		GLuint mvp_handle_ = glGetUniformLocation(shader_program_, "mvp");
+			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));
 
-		GLuint point_size_handle_ = glGetUniformLocation(shader_program_, "point_size");
+			// 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(shader_program_, "vertex");
+			GLint attribute_vertex = glGetAttribLocation(cloud_shader_program_, "vertex");
-		GLint attribute_color = glGetAttribLocation(shader_program_, "color");
+			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 && indices_.size())
+			if(meshRendering && polygons_.size())
 			{
-			glDrawElements(GL_TRIANGLES, indices_.size(), GL_UNSIGNED_SHORT, indices_.data());
+				glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_SHORT, polygons_.data());
 			}
 			else
 			{
 				glDrawArrays(GL_POINTS, 0, nPoints_);
 			}
-
+		}
 		glDisableVertexAttribArray(0);
 		glBindBuffer(GL_ARRAY_BUFFER, 0);
 

app/android/jni/point_cloud_drawable.h

@@ -1,18 +1,29 @@
 /*
- * Copyright 2014 Google Inc. All Rights Reserved.
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
- *
+All rights reserved.
- * Licensed under the Apache License, Version 2.0 (the "License");
+
- * you may not use this file except in compliance with the License.
+Redistribution and use in source and binary forms, with or without
- * You may obtain a copy of the License at
+modification, are permitted provided that the following conditions are met:
- *
+    * Redistributions of source code must retain the above copyright
- *      http://www.apache.org/licenses/LICENSE-2.0
+      notice, this list of conditions and the following disclaimer.
- *
+    * Redistributions in binary form must reproduce the above copyright
- * Unless required by applicable law or agreed to in writing, software
+      notice, this list of conditions and the following disclaimer in the
- * distributed under the License is distributed on an "AS IS" BASIS,
+      documentation and/or other materials provided with the distribution.
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+    * Neither the name of the Universite de Sherbrooke nor the
- * See the License for the specific language governing permissions and
+      names of its contributors may be used to endorse or promote products
- * limitations under the License.
+      derived from this software without specific prior written permission.
- */
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
 
 #ifndef TANGO_POINT_CLOUD_POINT_CLOUD_DRAWABLE_H_
 #define TANGO_POINT_CLOUD_POINT_CLOUD_DRAWABLE_H_
@@ -31,13 +42,11 @@
 class PointCloudDrawable {
  public:
   PointCloudDrawable(
-		  GLuint shaderProgram,
+		  GLuint cloudShaderProgram,
-		  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		  GLuint textureShaderProgram,
-		  const std::vector<pcl::Vertices> & indices = std::vector<pcl::Vertices>());
-  PointCloudDrawable(
-  		  GLuint shaderProgram,
   		  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-  		  const std::vector<pcl::Vertices> & indices = std::vector<pcl::Vertices>());
+  		  const 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_

app/android/jni/scene.cpp

@@ -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>);
@@ -303,11 +334,14 @@ int Scene::Render() {
 	else
 	{
 		for(std::map<int, PointCloudDrawable*>::const_iterator iter=pointClouds_.begin(); iter!=pointClouds_.end(); ++iter)
+		{
+			if((mapRendering_ || iter->first < 0) && iter->second->isVisible())
 			{
 				++cloudDrawn;
 				iter->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_);
 			}
 		}
+	}
 
 	if(graphVisible_ && graph_)
 	{
@@ -372,31 +406,12 @@ void Scene::setTraceVisible(bool visible)
 }
 
 //Should only be called in OpenGL thread!
-void Scene::addOrUpdateCloud(
+void Scene::addCloud(
-		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(
 		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);
+	UASSERT(cloud_shader_program_ != 0 && texture_mesh_shader_program_!=0);
-	PointCloudDrawable * drawable = new PointCloudDrawable(cloud_shader_program_, cloud, polygons);
+	PointCloudDrawable * drawable = new PointCloudDrawable(
+			cloud_shader_program_,
+			texture_mesh_shader_program_,
+			cloud,
+			polygons,
+			image);
 	drawable->setPose(pose);
 	pointClouds_.insert(std::make_pair(id, drawable));
 }

app/android/jni/scene.h

@@ -96,22 +96,19 @@ class Scene {
   void setGraphVisible(bool visible);
   void setTraceVisible(bool visible);
 
-  void addOrUpdateCloud(
+  void addCloud(
-		  int id,
-		  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
-		  const std::vector<pcl::Vertices> & polygons,
-		  const rtabmap::Transform & pose);
-  void addOrUpdateCloud(
   		  int id,
   		  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
   		  const std::vector<pcl::Vertices> & polygons,
-  		  const 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_;
 };

app/android/res/layout/activity_rtabmap.xml

@@ -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>
 

app/android/res/menu/optionmenu.xml

@@ -6,26 +6,63 @@
     </group>
     
     <group android:id="@+id/group_actions">
-       <item android:id="@+id/open" android:title="Open"/>
+       <item android:id="@+id/post_processing" android:title="Post-Processing...">
-       <item android:id="@+id/save" android:title="Save"/>
+          <menu>
-       <item android:id="@+id/export" android:title="Export (*.ply)"/>
+	         <group android:id="@+id/group_post_processing">
-       <item android:id="@+id/reset" android:title="Reset"/>
+	            <item android:id="@+id/detect_more_loop_closures" android:title="Detect More Loop Closures" />
-       <item android:id="@+id/about" android:title="About"/>
+	            <item android:id="@+id/global_graph_optimization" android:title="Global Graph Optimization" />
-    </group>
+	            <item android:id="@+id/sba" android:title="Bundle Adjustement" />
-    
-    <item android:id="@+id/menu_settings" android:title="Options..." android:orderInCategory="2">
-        <menu >
-        <group android:id="@+id/group_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" />
 	         </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...">
+          <menu>
+	         <group android:id="@+id/group_export">
+	            <item android:id="@+id/export_ply" android:title="Mesh (.ply)" />
+	            <item android:id="@+id/export_obj" android:title="Mesh with texture (*.obj)" />
+	         </group>   
+          </menu>
+       </item>
+       <item android:id="@+id/reset" android:title="Reset"/>
+       
+       <item android:id="@+id/menu_rendering_settings" android:title="Rendering Options...">
+        <menu >
+        <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/mesh_rendering" android:checked="true" android:title="Mesh Rendering" />
+            <item android:id="@+id/map_shown" android:checked="true" android:title="Map Visible" />
+            <item android:id="@+id/odom_shown" android:checked="true" android:title="Odom Visible" />
+            <item android:id="@+id/graph_visible" android:checked="true" android:title="Graph Visible" />
+            <item android:id="@+id/auto_exposure" android:checked="false" android:title="Auto Exposure" />
+            <item android:id="@+id/max_depth" android:checkable="false" android:title="Cloud/Mesh Max Depth..." />
+            <item android:id="@+id/mesh_angle_tolerance" android:checkable="false" android:title="Mesh Angle Tolerance..." />
+            <item android:id="@+id/mesh_triangle_size" android:checkable="false" android:title="Mesh Triangle Size..." />
+        </group>   
+        </menu>
+	    </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>

app/android/res/values/strings.xml

@@ -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>

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

@@ -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);
@@ -89,6 +130,8 @@ public class RTABMapActivity extends Activity implements OnClickListener {
     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);
 
@@ -97,6 +140,8 @@ public class RTABMapActivity extends Activity implements OnClickListener {
     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!",
+  			mToast.makeText(this, "Motion Tracking Permissions Required!",
-  					Toast.LENGTH_SHORT).show();
+  					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);
-        }
+    		mItemPostProcessing.setEnabled(false);
-
-        if(RTABMapLib.onResume()!=0)
-        {
-        	Toast.makeText(getApplicationContext(), 
-    				String.format("Failed to connect with Tango!"), Toast.LENGTH_SHORT).show();
         }
 
     } 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.features)).setText(String.format("%d / %s", featuresExtracted, mMaxFeaturesValues[mParamMaxFeaturesIndex]));
-	  ((TextView)findViewById(R.id.update_time)).setText(String.format("%.3f", updateTime));
+	  ((TextView)findViewById(R.id.update_time)).setText(String.format("%.3f / %s", updateTime, mTimeThrValues[mParamTimeThrMsIndex]));
-	  ((TextView)findViewById(R.id.hypothesis)).setText(String.format("%.3f (%d)", hypothesis, loopClosureId));
+	  ((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())
+	  {
 		  if(loopClosureId > 0)
 		  {
 			  ++mTotalLoopClosures;
-		  ((TextView)findViewById(R.id.total_loop)).setText(String.valueOf(mTotalLoopClosures));
+	
-		  Toast.makeText(this, "Loop closure detected!", Toast.LENGTH_SHORT).show();
+			  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	
-			  Toast.makeText(this, String.format("Unknown Tango event detected!? (type=%d)", type), Toast.LENGTH_SHORT).show();
+	  {
+		  str = String.format("Unknown Tango event detected!? (type=%d)", type);
+	  }
+	  if(str!=null)
+	  {
+		  mToast.setText(str);
+		  mToast.show();
+	  }
   }
   
   //called from jni
@@ -503,14 +578,15 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		  final String key,
 		  final String value)
   {
-	  Log.i(TAG, String.format("tangoEventCallback()"));
+	  if(mItemPause != null && !mItemPause.isChecked())
-
+	  {
 		  runOnUiThread(new Runnable() {
 			    public void run() {
 			    	tangoEventUI(type, key, value);
 			    } 
 			});
 	  }
+  }
   
   private boolean CheckTangoCoreVersion(int minVersion) {
     int versionNumber = 0;
@@ -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)
       {
@@ -641,6 +973,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 				  public void onClick(DialogInterface dialog, int which)
 				  {
 					  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);
@@ -740,9 +1082,10 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			  public void onClick(DialogInterface dialog, int which)
 			  {
 				  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
 					                		    		{
-							                    		Toast.makeText(getActivity(), String.format("Exporting mesh to \"%s\" failed!", path), Toast.LENGTH_LONG).show();
+					                		    			mToast.makeText(getActivity(), String.format("Mesh \"%s\" successfully exported!", path), mToast.LENGTH_LONG).show();
+					                		    		}
+							                    	}
+							                    	else
+							                    	{
+							                    		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
 			                		    		{
-					                    		Toast.makeText(getActivity(), String.format("Exporting mesh to \"%s\" failed!", path), Toast.LENGTH_LONG).show();
+			                		    			mToast.makeText(getActivity(), String.format("Mesh \"%s\" successfully exported!", path), mToast.LENGTH_LONG).show();
+			                		    		}
+					                    	}
+					                    	else
+					                    	{
+					                    		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();
                 	  }
 

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

@@ -1,6 +1,8 @@
 
 package com.introlab.rtabmap;
+import android.os.IBinder;
 import android.view.KeyEvent;
+import android.util.Log;
 
 
 // Wrapper for native library
@@ -8,19 +10,29 @@ import android.view.KeyEvent;
 public class RTABMapLib
 {
 
-    static
+	static {
-    {
+        // This project depends on tango_client_api, so we need to make sure we load
+        // the correct library first.
+        if (TangoInitializationHelper.loadTangoSharedLibrary() ==
+                TangoInitializationHelper.ARCH_ERROR) {
+            Log.e(RTABMapActivity.class.getSimpleName(), "ERROR! Unable to load libtango_client_api.so!");
+        }
         System.loadLibrary("NativeRTABMap");
     }
 
  // Initialize the Tango Service, this function starts the communication
     // between the application and Tango Service.
     // The activity object is used for checking if the API version is outdated.
-    public static native int initialize(RTABMapActivity activity);
+    public static native void onCreate(RTABMapActivity activity);
     
     public static native void openDatabase(String databasePath);
     
-    public static native 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();

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

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

app/src/CMakeLists.txt

@@ -5,7 +5,7 @@ SET(headers_ui
 )
 
 #This will generate moc_* for Qt
-IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
+IF(QT4_FOUND)
     QT4_WRAP_CPP(moc_srcs ${headers_ui})
 ELSE()
     QT5_WRAP_CPP(moc_srcs ${headers_ui})
@@ -25,9 +25,9 @@ SET(INCLUDE_DIRS
     ${PCL_INCLUDE_DIRS}
 )
 
-IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
+IF(QT4_FOUND)
     INCLUDE(${QT_USE_FILE})
-ENDIF()
+ENDIF(QT4_FOUND)
 
 SET(LIBRARIES
     ${QT_LIBRARIES} 
@@ -73,9 +73,9 @@ ELSE()
   ADD_EXECUTABLE(rtabmap ${SRC_FILES})
 ENDIF()
 TARGET_LINK_LIBRARIES(rtabmap rtabmap_core rtabmap_gui rtabmap_utilite ${LIBRARIES})
-IF("${RTABMAP_QT_VERSION}" STREQUAL "5")
+IF(Qt5_FOUND)
     QT5_USE_MODULES(rtabmap Widgets Core Gui Svg PrintSupport)
-ENDIF()
+ENDIF(Qt5_FOUND)
 
 IF(APPLE AND BUILD_AS_BUNDLE)
   SET_TARGET_PROPERTIES(rtabmap PROPERTIES

app/src/main.cpp

@@ -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...");
 
@@ -65,6 +80,11 @@ int main(int argc, char* argv[])
 	rtabmap->start(); // start it not initialized... will be initialized by event from the gui
 	UEventsManager::addHandler(rtabmap);
     
+    if(!database.empty())
+    {
+    	QMetaObject::invokeMethod(mainWindow, "openDatabase", Qt::QueuedConnection, Q_ARG(QString, QString(database.c_str())));
+    }
+
 	// Now wait for application to finish
 	app->connect( app, SIGNAL( lastWindowClosed() ),
 				app, SLOT( quit() ) );

cmake_modules/FindG2O.cmake

@@ -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)

cmake_modules/MacOSXBundleInfo.plist.in

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

corelib/include/rtabmap/core/CameraModel.h

@@ -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
 

corelib/include/rtabmap/core/CameraThread.h

@@ -91,6 +91,7 @@ private:
 	bool _scanFromDepth;
 	int _scanDecimation;
 	float _scanMaxDepth;
+	float _scanMinDepth;
 	float _scanVoxelSize;
 	int _scanNormalsK;
 	StereoDense * _stereoDense;

corelib/include/rtabmap/core/DBDriver.h

@@ -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;

corelib/include/rtabmap/core/Memory.h

@@ -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;

corelib/include/rtabmap/core/Odometry.h

@@ -83,6 +83,7 @@ private:
 	bool _fillInfoData;
 	float _kalmanProcessNoise;
 	float _kalmanMeasurementNoise;
+	int _imageDecimation;
 	Transform _pose;
 	int _resetCurrentCount;
 	double previousStamp_;

corelib/include/rtabmap/core/Optimizer.h

@@ -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(),

corelib/include/rtabmap/core/OptimizerCVSBA.h

@@ -44,29 +44,18 @@ public:
 			int iterations         = Parameters::defaultOptimizerIterations(),
 			bool slam2d            = Parameters::defaultOptimizerSlam2D(),
 			bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored()) :
-		Optimizer(iterations, slam2d, covarianceIgnored),
+		Optimizer(iterations, slam2d, covarianceIgnored) {}
-		inlierDistance_(0.02),
-		minInliers_(10){}
 	OptimizerCVSBA(const ParametersMap & parameters) :
-		Optimizer(parameters),
+		Optimizer(parameters) {}
-		inlierDistance_(0.02),
-		minInliers_(10){}
 	virtual ~OptimizerCVSBA() {}
 
 	virtual Type type() const {return kTypeCVSBA;}
 
-	void setInlierDistance(float inlierDistance) {inlierDistance_ = inlierDistance;}
-	void setMinInliers(int minInliers) {minInliers_ = minInliers;}
-
 	virtual std::map<int, Transform> optimizeBA(
 			int rootId,
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & links,
 			const std::map<int, Signature> & signatures);
-
-private:
-	float inlierDistance_;
-	float minInliers_;
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/OptimizerG2O.h

@@ -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 */

corelib/include/rtabmap/core/Parameters.h

@@ -206,7 +206,8 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Mem, GenerateIds,             bool, true,     "True=Generate location IDs, False=use input image IDs.");
 	RTABMAP_PARAM(Mem, BadSignaturesIgnored,    bool, false,     "Bad signatures are ignored.");
 	RTABMAP_PARAM(Mem, InitWMWithAllNodes,      bool, false,     "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.");
-	RTABMAP_PARAM(Mem, ImageDecimation,         int, 1,          "Image decimation (>=1) when creating a signature.");
+	RTABMAP_PARAM(Mem, ImagePreDecimation,      int, 1,          "Image decimation (>=1) before features extraction.");
+	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.");
 
@@ -346,6 +347,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 +366,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.");

corelib/include/rtabmap/core/Rtabmap.h

@@ -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

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

@@ -23,10 +23,12 @@ 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)
 {
 	ground.reset(new std::vector<int>);
 	obstacles.reset(new std::vector<int>);
@@ -39,7 +41,7 @@ void segmentObstaclesFromGround(
 				indices,
 				groundNormalAngle,
 				Eigen::Vector4f(0,0,1,0),
-				normalRadiusSearch*2.0f,
+				normalKSearch,
 				Eigen::Vector4f(0,0,100,0));
 
 		if(segmentFlatObstacles)
@@ -48,17 +50,21 @@ 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
+			if(clusteredFlatSurfaces.size())
+			{
 				ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
 				Eigen::Vector4f min,max;
 				pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), min, max);
 
+				if(maxGroundHeight <= 0 || min[2] < maxGroundHeight)
+				{
 					for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
 					{
 						if((int)i!=biggestFlatSurfaceIndex)
@@ -73,6 +79,13 @@ void segmentObstaclesFromGround(
 					}
 				}
 				else
+				{
+					// reject ground!
+					ground.reset(new std::vector<int>);
+				}
+			}
+		}
+		else
 		{
 			ground = flatSurfaces;
 		}
@@ -86,7 +99,7 @@ void segmentObstaclesFromGround(
 			std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters(
 					cloud,
 					otherStuffIndices,
-					normalRadiusSearch*2.0f,
+					clusterRadius,
 					minClusterSize);
 
 			// merge indices
@@ -100,10 +113,12 @@ 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 indices(new std::vector<int>);
 	segmentObstaclesFromGround<PointT>(
@@ -111,10 +126,12 @@ void segmentObstaclesFromGround(
 			indices,
 			ground,
 			obstacles,
-			normalRadiusSearch,
+			normalKSearch,
 			groundNormalAngle,
+			clusterRadius,
 			minClusterSize,
-			segmentFlatObstacles);
+			segmentFlatObstacles,
+			maxGroundHeight);
 }
 
 template<typename PointT>
@@ -125,7 +142,9 @@ void occupancy2DFromCloud3D(
 		cv::Mat & obstacles,
 		float cellSize,
 		float groundNormalAngle,
-		int minClusterSize)
+		int minClusterSize,
+		bool segmentFlatObstacles,
+		float maxGroundHeight)
 {
 	if(cloud->size() == 0)
 	{
@@ -138,9 +157,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 +215,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);
 }
 
 }

corelib/include/rtabmap/core/util3d.h

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <pcl/pcl_base.h>
+#include <pcl/TextureMesh.h>
 #include <rtabmap/core/Transform.h>
 #include <rtabmap/core/SensorData.h>
 #include <opencv2/core/core.hpp>
@@ -78,6 +79,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 +89,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 +97,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 +106,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 +115,26 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromStereoImages(
 		const StereoCameraModel & model,
 		int decimation = 1,
 		float maxDepth = 0.0f,
+		float minDepth = 0.0f,
 		std::vector<int> * validIndices = 0);
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 		const SensorData & sensorData,
 		int decimation = 1,
 		float maxDepth = 0.0f,
-		float voxelSize = 0.0f,
+		float minDepth = 0.0f,
-		int samples = 0,
 		std::vector<int> * validIndices = 0);
 
 /**
- * Create an RGB cloud from the images contained in SensorData. If "voxelSize" and
+ * Create an RGB cloud from the images contained in SensorData. If there is only one camera,
- * "samples" are not set (0), the returned cloud is organized. Otherwise, all NaN
+ * 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 minDepth, minimum depth of the projected points (closer points are set to null in case of an organized cloud).
- * @param samples, random sampling filtering target cloud size.
  * @param validIndices, the indices of valid points in the cloud
  * @return a RGB cloud.
  */
@@ -140,8 +142,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 		const SensorData & sensorData,
 		int decimation = 1,
 		float maxDepth = 0.0f,
-		float voxelSize = 0.0f,
+		float minDepth = 0.0f,
-		int samples = 0,
 		std::vector<int> * validIndices = 0);
 
 pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImage(
@@ -151,6 +152,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 +203,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.
  *

corelib/include/rtabmap/core/util3d_filtering.h

@@ -344,13 +344,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 +365,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 +375,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);
 
 /**

corelib/include/rtabmap/core/util3d_mapping.h

@@ -86,19 +86,23 @@ 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);
 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);
 
 template<typename PointT>
 void occupancy2DFromCloud3D(
@@ -107,7 +111,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 +122,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

corelib/include/rtabmap/core/util3d_surface.h

@@ -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,

corelib/src/CameraThread.cpp

@@ -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)
 				{

corelib/src/DBDriver.cpp

@@ -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("");
+	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,

corelib/src/DBDriverSqlite3.cpp

@@ -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,

corelib/src/DBDriverSqlite3.h

@@ -48,8 +48,8 @@ public:
 	void setTempStore(int tempStore);
 
 private:
-	virtual bool connectDatabaseQuery(const std::string & url, bool overwirtten = false);
+	virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false);
-	virtual void disconnectDatabaseQuery();
+	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;

corelib/src/Features2d.cpp

@@ -505,7 +505,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

corelib/src/Graph.cpp

@@ -616,7 +616,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 +710,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

corelib/src/Memory.cpp

@@ -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);
 
@@ -2066,28 +2069,51 @@ Transform Memory::computeTransform(
 
 	if(fromS && toS)
 	{
+		return computeTransform(*fromS, *toS, guess, info);
+	}
+	else
+	{
+		std::string msg = uFormat("Did not find nodes %d and/or %d", fromId, toId);
+		if(info)
+		{
+			info->rejectedMsg = msg;
+		}
+		UWARN(msg.c_str());
+	}
+	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()) ||
+	if((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired() && fromS.sensorData().imageCompressed().empty()) ||
-		   (_registrationPipeline->isScanRequired() && fromS->sensorData().imageCompressed().empty() && fromS->sensorData().laserScanCompressed().empty()) ||
+	   (_registrationPipeline->isScanRequired() && fromS.sensorData().imageCompressed().empty() && fromS.sensorData().laserScanCompressed().empty()) ||
-		   (_registrationPipeline->isUserDataRequired() && fromS->sensorData().imageCompressed().empty() && fromS->sensorData().userDataCompressed().empty()))
+	   (_registrationPipeline->isUserDataRequired() && fromS.sensorData().imageCompressed().empty() && fromS.sensorData().userDataCompressed().empty()))
 	{
-			getNodeData(fromS->id());
+		fromS.sensorData() = getNodeData(fromS.id());
 	}
-		if((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired() && toS->sensorData().imageCompressed().empty()) ||
+	if((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired() && toS.sensorData().imageCompressed().empty()) ||
-		   (_registrationPipeline->isScanRequired() && toS->sensorData().imageCompressed().empty() && toS->sensorData().laserScanCompressed().empty()) ||
+	   (_registrationPipeline->isScanRequired() && toS.sensorData().imageCompressed().empty() && toS.sensorData().laserScanCompressed().empty()) ||
-		   (_registrationPipeline->isUserDataRequired() && toS->sensorData().imageCompressed().empty() && toS->sensorData().userDataCompressed().empty()))
+	   (_registrationPipeline->isUserDataRequired() && toS.sensorData().imageCompressed().empty() && toS.sensorData().userDataCompressed().empty()))
 	{
-			getNodeData(toS->id());
+		toS.sensorData() = getNodeData(toS.id());
 	}
 	// uncompress only what we need
 	cv::Mat imgBuf, depthBuf, laserBuf, userBuf;
-		fromS->sensorData().uncompressData(
+	fromS.sensorData().uncompressData(
 			(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&imgBuf:0,
 			(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&depthBuf:0,
 			_registrationPipeline->isScanRequired()?&laserBuf:0,
 			_registrationPipeline->isUserDataRequired()?&userBuf:0);
-		toS->sensorData().uncompressData(
+	toS.sensorData().uncompressData(
 			(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&imgBuf:0,
 			(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&depthBuf:0,
 			_registrationPipeline->isScanRequired()?&laserBuf:0,
@@ -2096,10 +2122,10 @@ Transform Memory::computeTransform(
 
 	// compute transform fromId -> toId
 	std::vector<int> inliersV;
-		if(_reextractLoopClosureFeatures || (fromS->getWords().size() && toS->getWords().size()))
+	if(_reextractLoopClosureFeatures || (fromS.getWords().size() && toS.getWords().size()))
 	{
-			Signature tmpFrom = *fromS;
+		Signature tmpFrom = fromS;
-			Signature tmpTo = *toS;
+		Signature tmpTo = toS;
 
 		// make a guess fast with known correspondences (if there are)
 		RegistrationVis regVis(parameters_);
@@ -2114,8 +2140,8 @@ Transform Memory::computeTransform(
 			tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
 			guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
 			// set back descriptors
-				tmpFrom.setWordsDescriptors(fromS->getWordsDescriptors());
+			tmpFrom.setWordsDescriptors(fromS.getWordsDescriptors());
-				tmpTo.setWordsDescriptors(toS->getWordsDescriptors());
+			tmpTo.setWordsDescriptors(toS.getWordsDescriptors());
 		}
 
 		if(_reextractLoopClosureFeatures)
@@ -2173,16 +2199,6 @@ Transform Memory::computeTransform(
 			}
 		}
 	}
-	}
-	else
-	{
-		std::string msg = uFormat("Did not find nodes %d and/or %d", fromId, toId);
-		if(info)
-		{
-			info->rejectedMsg = msg;
-		}
-		UWARN(msg.c_str());
-	}
 	return transform;
 }
 
@@ -2314,7 +2330,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 +2381,8 @@ bool Memory::addLink(const Link & link)
 				}
 			}
 		}
-		return true;
 	}
-	else
+	else if(!addInDatabase)
 	{
 		if(!fromS)
 		{
@@ -2377,8 +2392,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;
+	}
+	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 +2508,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->dumpDictionary((directory+"/DumpMemoryWordRef.txt").c_str(), (directory+"/DumpMemoryWordDesc.txt").c_str());
-	this->dumpSignatures((directory + "DumpMemorySign.txt").c_str(), false);
+	this->dumpSignatures((directory + "/DumpMemorySign.txt").c_str(), false);
-	this->dumpSignatures((directory + "DumpMemorySign3.txt").c_str(), true);
+	this->dumpSignatures((directory + "/DumpMemorySign3.txt").c_str(), true);
-	this->dumpMemoryTree((directory + "DumpMemoryTree.txt").c_str());
+	this->dumpMemoryTree((directory + "/DumpMemoryTree.txt").c_str());
 }
 
 void Memory::dumpDictionary(const char * fileNameRef, const char * fileNameDesc) const
@@ -2490,6 +2524,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 +2572,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,53 +2904,33 @@ 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();
 	}
-		}
-	}
 
 	return r;
 }
 
 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 +2938,7 @@ void Memory::getNodeWords(int nodeId,
 	{
 		words = s->getWords();
 		words3 = s->getWords3();
+		wordsDescriptors = s->getWordsDescriptors();
 	}
 	else if(_dbDriver)
 	{
@@ -2935,6 +2952,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 +2966,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 +3196,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 &&
+				if(imageMono.rows % decimatedData.depthRaw().rows == 0 &&
-					imageMono.cols % data.depthRaw().cols == 0 &&
+					imageMono.cols % decimatedData.depthRaw().cols == 0 &&
-					imageMono.rows/data.depthRaw().rows == imageMono.cols/data.depthRaw().cols)
+					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 +3262,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()) ||
+			else if((!decimatedData.depthRaw().empty() && decimatedData.cameraModels().size() && decimatedData.cameraModels()[0].isValidForProjection()) ||
-					(!data.rightRaw().empty() && data.stereoCameraModel().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 +3426,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(preDecimation)/log(2);
 		for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end() && i < keypoints.size(); ++iter, ++i)
-		{
-			if(_imageDecimation > 1)
 		{
 			cv::KeyPoint kpt = keypoints[i];
-				kpt.pt.x /= float(_imageDecimation);
+			if(preDecimation != _imagePostDecimation)
-				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 +3510,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);
+		image = util2d::decimate(image, _imagePostDecimation);
-		depthOrRightImage = util2d::decimate(depthOrRightImage, _imageDecimation);
+		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));
 		}
 	}
 

corelib/src/Odometry.cpp

@@ -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(_imageDecimation)/log(2);
+		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)
 	{

corelib/src/Optimizer.cpp

@@ -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((Optimizer::Type)optimizerTypeInt, parameters);
-	return create(type, 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 */

corelib/src/OptimizerCVSBA.cpp

@@ -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();
+			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>
+	std::map<int, std::map<int, cv::Point2f> > wordReferences; // <ID words, IDs frames + keypoint>
-	for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
+	computeBACorrespondences(frames, links, signatures, points3DMap, wordReferences);
-	{
-		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::vector<int> inliers;
+	UDEBUG("points=%d frames=%d", (int)wordReferences.size(), (int)frames.size());
-			Transform t = util3d::estimateMotion3DTo3D(
+	std::vector<cv::Point3f> points(wordReferences.size()); //npoints
-					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
 	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()));
+		imagePoints[i].resize(wordReferences.size(), cv::Point2f(std::numeric_limits<float>::quiet_NaN(), std::numeric_limits<float>::quiet_NaN()));
-		visibility[i].resize(wordReferencesKeys.size(), 0);
+		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);
+		for(std::map<int, cv::Point2f>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
-		while(jter->first == *iter && jter != wordReferences.end())
 		{
-			imagePoints[frameIdToIndex.at(jter->second.first)][i] = jter->second.second;
+			imagePoints[frameIdToIndex.at(jter->first)][i] = jter->second;
-			visibility[frameIdToIndex.at(jter->second.first)][i] = 1;
+			visibility[frameIdToIndex.at(jter->first)][i] = 1;
-			++jter;
 		}
-
 		++i;
 	}
 

corelib/src/OptimizerG2O.cpp

@@ -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,

corelib/src/OptimizerGTSAM.cpp

@@ -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;
 			}
 

corelib/src/Parameters.cpp

@@ -155,6 +155,9 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 	{
 		// removed parameters
 
+		// 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())));

corelib/src/RegistrationVis.cpp

@@ -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);
+						cv::cvtColor(imageFrom, tmp, cv::COLOR_BGR2GRAY);
-						fromSignature.sensorData().setImageRaw(tmp);
+						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 &&
+						if(imageFrom.rows % fromSignature.sensorData().depthRaw().rows == 0 &&
-							fromSignature.sensorData().imageRaw().cols % fromSignature.sensorData().depthRaw().cols == 0 &&
+						   imageFrom.cols % fromSignature.sensorData().depthRaw().cols == 0 &&
-							fromSignature.sensorData().imageRaw().rows/fromSignature.sensorData().depthRaw().rows == fromSignature.sensorData().imageRaw().cols/fromSignature.sensorData().depthRaw().cols)
+						   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() &&
+		   !imageFrom.empty() &&
-		   !toSignature.sensorData().imageRaw().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);
+				cv::cvtColor(imageFrom, tmp, cv::COLOR_BGR2GRAY);
-				fromSignature.sensorData().setImageRaw(tmp);
+				imageFrom = tmp;
 			}
-			if(toSignature.sensorData().imageRaw().channels() > 1)
+			if(imageTo.channels() > 1)
 			{
 				cv::Mat tmp;
-				cv::cvtColor(toSignature.sensorData().imageRaw(), tmp, cv::COLOR_BGR2GRAY);
+				cv::cvtColor(imageTo, tmp, cv::COLOR_BGR2GRAY);
-				toSignature.sensorData().setImageRaw(tmp);
+				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(),
+						imageFrom,
-						toSignature.sensorData().imageRaw(),
+						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].x, 0.0f, float(imageTo.cols)) &&
-					   uIsInBounds(cornersTo[i].y, 0.0f, float(toSignature.sensorData().imageRaw().rows)))
+					   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);
+						cv::cvtColor(imageTo, tmp, cv::COLOR_BGR2GRAY);
-						toSignature.sensorData().setImageRaw(tmp);
+						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 &&
+						if(imageTo.rows % toSignature.sensorData().depthRaw().rows == 0 &&
-							toSignature.sensorData().imageRaw().cols % toSignature.sensorData().depthRaw().cols == 0 &&
+						   imageTo.cols % toSignature.sensorData().depthRaw().cols == 0 &&
-							toSignature.sensorData().imageRaw().rows/toSignature.sensorData().depthRaw().rows == toSignature.sensorData().imageRaw().cols/toSignature.sensorData().depthRaw().cols)
+						   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);
+					cv::cvtColor(imageFrom, tmp, cv::COLOR_BGR2GRAY);
-					fromSignature.sensorData().setImageRaw(tmp);
+					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);
+						cv::cvtColor(imageTo, tmp, cv::COLOR_BGR2GRAY);
-						toSignature.sensorData().setImageRaw(tmp);
+						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];

corelib/src/Rtabmap.cpp

@@ -375,6 +375,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 +398,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);
@@ -1637,7 +1639,8 @@ bool Rtabmap::process(
 			++iter)
 		{
 			const Signature * s = _memory->getSignature(iter->second);
-			UASSERT(s!=0);
+			if(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();
@@ -1660,6 +1663,7 @@ bool Rtabmap::process(
 					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
 		if(retrievalLocalIds.size() < _maxLocalRetrieved)
 		{
@@ -1924,10 +1928,8 @@ bool Rtabmap::process(
 								{
 									// path filtering
 									std::map<int, Transform> filteredPath = graph::radiusPosesFiltering(path, _proximityFilteringRadius, 0, true);
-									// make sure the nearest and farthest poses are still here
+									// make sure the current pose is still here
 									filteredPath.insert(*path.find(nearestId));
-									filteredPath.insert(*path.begin());
-									filteredPath.insert(*path.rbegin());
 									path = filteredPath;
 								}
 							}
@@ -2683,6 +2685,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 +3167,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 +3180,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 +3241,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 +3268,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);

corelib/src/RtabmapThread.cpp

@@ -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;

corelib/src/SensorData.cpp

@@ -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;
 	}
 

corelib/src/VWDictionary.cpp

@@ -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!");
+							UASSERT_MSG(descriptor.type() == CV_32F, "To use KdTree dictionary, float descriptors are required!");
-							_flannIndex->build(w->getDescriptor(), rtflann::KDTreeIndexParams(), useDistanceL1_);
+							_flannIndex->build(descriptor, rtflann::KDTreeIndexParams(), useDistanceL1_);
 							break;
 						case kNNFlannLSH:
-							UASSERT_MSG(w->getDescriptor().type() == CV_8U, "To use LSH dictionary, binary descriptors are required!");
+							UASSERT_MSG(descriptor.type() == CV_8U, "To use LSH dictionary, binary descriptors are required!");
-							_flannIndex->build(w->getDescriptor(), rtflann::LshIndexParams(12, 20, 2), useDistanceL1_);
+							_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(descriptor.cols == _flannIndex->featuresDim());
-						UASSERT(w->getDescriptor().type() == _flannIndex->featuresType());
+						UASSERT(descriptor.type() == _flannIndex->featuresType());
-						index = _flannIndex->addPoint(w->getDescriptor());
+						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);
+					cv::Mat descriptor;
-					UASSERT(iter->second->getDescriptor().type() == type);
+					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;
 	}
-
+	dim = 0;
-	UDEBUG("id=%d descriptors=%d", signatureId, descriptors.rows);
+	type = -1;
-	UTimer timer;
+	if(_dataTree.rows || _flannIndex->isBuilt())
-	std::list<int> wordIds;
-	if(descriptors.rows == 0 || descriptors.cols == 0)
 	{
-		UERROR("Descriptors size is null!");
+		dim = _flannIndex->isBuilt()?_flannIndex->featuresDim():_dataTree.cols;
-		return wordIds;
+		type = _flannIndex->isBuilt()?_flannIndex->featuresType():_dataTree.type();
-	}
-	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);
 	}
 
@@ -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 = (*vws.begin())->getDescriptor().type();
-		int type = _visualWords.begin()->second->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.begin()->second->getDescriptor().type() != CV_32FC1)
-	if(_visualWords.at(0)->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

corelib/src/util3d.cpp

@@ -245,6 +245,7 @@ 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));
@@ -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,6 +587,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
 		const StereoCameraModel & model,
 		int decimation,
 		float maxDepth,
+		float minDepth,
 		std::vector<int> * validIndices)
 {
 	UASSERT(!imageLeft.empty() && !imageRight.empty());
@@ -622,6 +627,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
 			modelDecimation,
 			decimation,
 			maxDepth,
+			minDepth,
 			validIndices);
 }
 
@@ -629,8 +635,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 		const SensorData & sensorData,
 		int decimation,
 		float maxDepth,
-		float voxelSize,
+		float minDepth,
-		int samples,
 		std::vector<int> * validIndices)
 {
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
@@ -652,36 +657,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());
-					}
 
 					if(sensorData.cameraModels().size() > 1)
 					{
+						tmp = util3d::removeNaNFromPointCloud(tmp);
 						*cloud += *tmp;
 					}
 					else
@@ -695,11 +680,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())
 	{
@@ -720,15 +700,11 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 				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,8 +718,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 		const SensorData & sensorData,
 		int decimation,
 		float maxDepth,
-		float voxelSize,
+		float minDepth,
-		int samples,
 		std::vector<int> * validIndices)
 {
 	UASSERT(!sensorData.imageRaw().empty());
@@ -785,35 +760,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());
-					}
 
 					if(sensorData.cameraModels().size() > 1)
 					{
+						tmp = util3d::removeNaNFromPointCloud(tmp);
 						*cloud += *tmp;
 					}
 					else
@@ -828,11 +784,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,21 +803,14 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 				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());
 		}
 	}
-	}
 	return cloud;
 }
 
@@ -877,6 +821,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 +836,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 +1261,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

corelib/src/util3d_features.cpp

@@ -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;

corelib/src/util3d_filtering.cpp

@@ -1219,21 +1219,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 +1243,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 +1274,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 +1303,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 +1334,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 +1363,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>());

corelib/src/util3d_surface.cpp

@@ -194,7 +194,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 = cloudA.size();
+	cloudA += cloudB;
+
+	int sizePolygonsA = 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 +228,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 +244,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 +254,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 +492,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 +555,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);
 

examples/BOWMapping/CMakeLists.txt

@@ -1,11 +1,36 @@
-SET(INCLUDE_DIRS
+cmake_minimum_required(VERSION 2.8)
-	${PROJECT_SOURCE_DIR}/corelib/include
+
+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
+	${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 
+if(internal)
+  SET_TARGET_PROPERTIES( bow_mapping 
     PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-bow_mapping)
+endif(internal)

examples/NoEventsExample/CMakeLists.txt

@@ -1,17 +1,48 @@
+cmake_minimum_required(VERSION 2.8)
 
-SET(INCLUDE_DIRS
+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
+	${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})
 
-SET_TARGET_PROPERTIES( noEventsExample 
+if(internal)
+	SET_TARGET_PROPERTIES( noEventsExample 
 	  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-noEventsExample)
+endif(internal)
+
+
+
+

examples/RGBDMapping/CMakeLists.txt

@@ -1,17 +1,48 @@
+cmake_minimum_required(VERSION 2.8)
 
-SET(INCLUDE_DIRS
+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
+	${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 
+if(internal)
+  SET_TARGET_PROPERTIES( rgbd_mapping 
     PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-rgbd_mapping)
+endif(internal)

examples/RGBDMapping/main.cpp

@@ -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;
 }

examples/WifiMapping/CMakeLists.txt

@@ -1,20 +1,48 @@
+cmake_minimum_required(VERSION 2.8)
 
-SET(srcs
+IF(DEFINED PROJECT_NAME)
-    main.cpp)
+   set(internal TRUE)
+ENDIF(DEFINED PROJECT_NAME)
 
-SET(INCLUDE_DIRS
+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
+	${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")
+IF(QT4_FOUND)
-    QT4_WRAP_CPP(moc_srcs ../RGBDMapping/MapBuilder.h MapBuilderWifi.h)
+    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 
+if(internal)
+  SET_TARGET_PROPERTIES( wifi_mapping 
     PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-wifi_mapping)
+endif(internal)

examples/WifiMapping/MapBuilder.h

@@ -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_ */

examples/WifiMapping/MapBuilderWifi.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;

guilib/include/rtabmap/gui/CalibrationDialog.h

@@ -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);

guilib/include/rtabmap/gui/CloudViewer.h

@@ -206,6 +206,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,6 +277,7 @@ private:
     QAction * _aSetGridCellCount;
     QAction * _aSetGridCellSize;
     QAction * _aSetBackgroundColor;
+    QAction * _aSetRenderingRate;
     QMenu * _menu;
     std::set<std::string> _graphes;
     std::set<std::string> _coordinates;
@@ -297,6 +300,7 @@ private:
     QColor _currentBgColor;
     bool _backfaceCulling;
     bool _frontfaceCulling;
+    double _renderingRate;
 };
 
 } /* namespace rtabmap */

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -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_;

guilib/include/rtabmap/gui/GraphViewer.h

@@ -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 */

guilib/include/rtabmap/gui/LoopClosureViewer.h

@@ -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,7 +56,7 @@ public:
 public slots:
 	void setDecimation(int decimation) {decimation_ = decimation;}
 	void setMaxDepth(int maxDepth) {maxDepth_ = maxDepth;}
-	void setSamples(int samples) {samples_ = samples;}
+	void setMinDepth(int minDepth) {minDepth_ = minDepth;}
 	void updateView(const Transform & AtoB = Transform());
 
 protected:
@@ -71,7 +71,7 @@ private:
 
 	int decimation_;
 	float maxDepth_;
-	int samples_;
+	float minDepth_;
 };
 
 } /* namespace rtabmap */

guilib/include/rtabmap/gui/MainWindow.h

@@ -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);
@@ -306,6 +308,9 @@ private:
 
 	ProgressDialog * _initProgressDialog;
 
+	CloudViewer * _cloudViewer;
+	LoopClosureViewer * _loopClosureViewer;
+
 	QString _graphSavingFileName;
 	QMap<int, QString> _exportPosesFileName;
 	bool _autoScreenCaptureOdomSync;

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -148,6 +148,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
 
@@ -340,6 +341,7 @@ private:
 	QVector<QCheckBox*> _3dRenderingShowClouds;
 	QVector<QSpinBox*> _3dRenderingDecimation;
 	QVector<QDoubleSpinBox*> _3dRenderingMaxDepth;
+	QVector<QDoubleSpinBox*> _3dRenderingMinDepth;
 	QVector<QDoubleSpinBox*> _3dRenderingOpacity;
 	QVector<QSpinBox*> _3dRenderingPtSize;
 	QVector<QCheckBox*> _3dRenderingShowScans;

guilib/src/CMakeLists.txt

@@ -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

guilib/src/CalibrationDialog.cpp

@@ -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;

guilib/src/CloudViewer.cpp

@@ -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,12 +121,21 @@ 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());
 
@@ -134,7 +143,7 @@ CloudViewer::CloudViewer(QWidget *parent) :
 	// 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,
@@ -204,6 +213,7 @@ void CloudViewer::createMenu()
 	_aSetGridCellCount = new QAction("Set cell count...", this);
 	_aSetGridCellSize = new QAction("Set cell size...", 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 +249,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 +299,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 +341,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();
@@ -1040,6 +1055,7 @@ void CloudViewer::setFrustumShown(bool shown)
 	if(!shown)
 	{
 		this->removeFrustum("reference_frustum");
+		this->update();
 	}
 	_aShowFrustum->setChecked(shown);
 }
@@ -1133,6 +1149,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,
@@ -1435,6 +1457,10 @@ float CloudViewer::getGridCellSize() const
 {
 	return _gridCellSize;
 }
+double CloudViewer::getRenderingRate() const
+{
+	return _renderingRate;
+}
 
 void CloudViewer::setGridCellCount(unsigned int count)
 {
@@ -1552,7 +1578,7 @@ void CloudViewer::keyReleaseEvent(QKeyEvent * event) {
 	}
 	else
 	{
-		QVTKWidget::keyPressEvent(event);
+		QWidget::keyPressEvent(event);
 	}
 }
 
@@ -1652,7 +1678,7 @@ void CloudViewer::keyPressEvent(QKeyEvent * event)
 	}
 	else
 	{
-		QVTKWidget::keyPressEvent(event);
+		QWidget::keyPressEvent(event);
 	}
 }
 
@@ -1812,6 +1838,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())

guilib/src/DatabaseViewer.cpp

@@ -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);
+	// Create cloud viewers
-	ui_->constraintsViewer->setCameraFree();
+	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();
 	
@@ -1710,10 +1732,15 @@ 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());
 
 							if(assemble)
 							{
+								if(cloud->size())
+								{
+									cloud = util3d::voxelize(cloud, validIndices, 0.01);
+
 									if(cloud->size())
 									{
 										cloud = rtabmap::util3d::transformPointCloud(cloud, pose);
@@ -1726,6 +1753,7 @@ void DatabaseViewer::generate3DMap()
 											*assembledCloud += *cloud;
 										}
 									}
+								}
 								UINFO("Created cloud %d (%d points)", iter->first, (int)cloud->size());
 								progressDialog.appendText(QString("Created cloud %1 (%2 points)").arg(iter->first).arg(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();
 				}
 
@@ -2202,11 +2281,25 @@ void DatabaseViewer::update(int value,
 							{
 								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);
 								}
@@ -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();
 
 		}
 	}
@@ -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());
+		stereoViewer_->updateCameraTargetPosition(Transform::getIdentity());
-		ui_->stereoViewer->addCloud("stereo", cloud);
+		stereoViewer_->addCloud("stereo", cloud);
-		ui_->stereoViewer->update();
+		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;
 
@@ -2859,18 +2954,18 @@ void DatabaseViewer::updateConstraintView(
 
 				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");
+				constraintsViewer_->removeCloud("cloud0");
-				ui_->constraintsViewer->removeCloud("cloud1");
+				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");
+					constraintsViewer_->removeCloud("words0");
-					ui_->constraintsViewer->removeCloud("words1");
+					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");
+				constraintsViewer_->removeCloud("words0");
-				ui_->constraintsViewer->removeCloud("words1");
+				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");
+				constraintsViewer_->removeCloud("scan2");
-				ui_->constraintsViewer->removeGraph("scan2graph");
+				constraintsViewer_->removeGraph("scan2graph");
 				if(link.type() == Link::kLocalSpaceClosure &&
 				   !link.userDataCompressed().empty())
 				{
@@ -3070,11 +3165,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 +3182,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");
+				constraintsViewer_->removeCloud("scan0");
-				ui_->constraintsViewer->removeCloud("scan1");
+				constraintsViewer_->removeCloud("scan1");
-				ui_->constraintsViewer->removeCloud("scan2");
+				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);
+		constraintsViewer_->addOrUpdateCoordinate("from_coordinate", Transform::getIdentity(), 0.2);
-		ui_->constraintsViewer->addOrUpdateCoordinate("to_coordinate", t, 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 +3324,19 @@ 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());
+							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 +3658,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 +3775,13 @@ 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());
 		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());
 		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 +4224,8 @@ void DatabaseViewer::updateLoopClosuresSlider(int from, int to)
 	else
 	{
 		ui_->horizontalSlider_loops->setEnabled(false);
-		ui_->constraintsViewer->removeAllClouds();
+		constraintsViewer_->removeAllClouds();
-		ui_->constraintsViewer->update();
+		constraintsViewer_->update();
 		updateConstraintButtons();
 	}
 }

guilib/src/ExportCloudsDialog.cpp

@@ -75,6 +75,7 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
 	connect(_ui->groupBox_regenerate, SIGNAL(clicked(bool)), this, SIGNAL(configChanged()));
 	connect(_ui->spinBox_decimation, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->doubleSpinBox_maxDepth, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_minDepth, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
 
 	connect(_ui->groupBox_filtering, SIGNAL(clicked(bool)), this, SIGNAL(configChanged()));
 	connect(_ui->doubleSpinBox_filteringRadius, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
@@ -143,6 +144,7 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou
 	settings.setValue("regenerate", _ui->groupBox_regenerate->isChecked());
 	settings.setValue("regenerate_decimation", _ui->spinBox_decimation->value());
 	settings.setValue("regenerate_max_depth", _ui->doubleSpinBox_maxDepth->value());
+	settings.setValue("regenerate_min_depth", _ui->doubleSpinBox_minDepth->value());
 
 
 	settings.setValue("filtering", _ui->groupBox_filtering->isChecked());
@@ -197,6 +199,7 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou
 	_ui->groupBox_regenerate->setChecked(settings.value("regenerate", _ui->groupBox_regenerate->isChecked()).toBool());
 	_ui->spinBox_decimation->setValue(settings.value("regenerate_decimation", _ui->spinBox_decimation->value()).toInt());
 	_ui->doubleSpinBox_maxDepth->setValue(settings.value("regenerate_max_depth", _ui->doubleSpinBox_maxDepth->value()).toDouble());
+	_ui->doubleSpinBox_minDepth->setValue(settings.value("regenerate_min_depth", _ui->doubleSpinBox_minDepth->value()).toDouble());
 
 	_ui->groupBox_filtering->setChecked(settings.value("filtering", _ui->groupBox_filtering->isChecked()).toBool());
 	_ui->doubleSpinBox_filteringRadius->setValue(settings.value("filtering_radius", _ui->doubleSpinBox_filteringRadius->value()).toDouble());
@@ -245,6 +248,7 @@ void ExportCloudsDialog::restoreDefaults()
 	_ui->groupBox_regenerate->setChecked(false);
 	_ui->spinBox_decimation->setValue(1);
 	_ui->doubleSpinBox_maxDepth->setValue(4);
+	_ui->doubleSpinBox_minDepth->setValue(0);
 
 	_ui->groupBox_filtering->setChecked(false);
 	_ui->doubleSpinBox_filteringRadius->setValue(0.02);
@@ -671,6 +675,9 @@ bool ExportCloudsDialog::getExportedClouds(
 			QApplication::processEvents();
 		}
 
+		//used for organized texturing below
+		std::map<int, std::pair<std::map<int, int>, std::pair<int, int> > > organizedIndices;
+
 		//mesh
 		UDEBUG("Meshing=%d", _ui->groupBox_meshing->isChecked()?1:0);
 		if(_ui->groupBox_meshing->isChecked())
@@ -691,18 +698,62 @@ bool ExportCloudsDialog::getExportedClouds(
 					UASSERT(iter->second->isOrganized());
 					if(iter->second->size())
 					{
+						Eigen::Vector3f viewpoint(0.0f,0.0f,0.0f);
+						if(cachedSignatures.contains(iter->first))
+						{
+							const SensorData & data = cachedSignatures.find(iter->first)->sensorData();
+							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(
 								iter->second,
 								_ui->doubleSpinBox_mesh_angleTolerance->value()*M_PI/180.0,
 								_ui->checkBox_mesh_quad->isEnabled() && _ui->checkBox_mesh_quad->isChecked(),
-								_ui->spinBox_mesh_triangleSize->value());
+								_ui->spinBox_mesh_triangleSize->value(),
+								viewpoint);
 						_progressDialog->appendText(tr("Mesh %1 created with %2 polygons (%3/%4).").arg(iter->first).arg(polygons.size()).arg(++i).arg(clouds.size()));
 
 						pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 						std::vector<pcl::Vertices> densePolygons;
-						util3d::filterNotUsedVerticesFromMesh(*iter->second, polygons, *denseCloud, densePolygons);
+						std::map<int, int> newToOldIndices = util3d::filterNotUsedVerticesFromMesh(*iter->second, polygons, *denseCloud, densePolygons);
 
+						if(!_ui->checkBox_assemble->isChecked() ||
+							 (_ui->checkBox_textureMapping->isEnabled() &&
+							  _ui->checkBox_textureMapping->isChecked() &&
+							  _ui->doubleSpinBox_voxelSize_assembled->value() == 0.0)) // don't assemble now if we are texturing
+						{
 							if(_ui->checkBox_assemble->isChecked())
+							{
+								denseCloud = util3d::transformPointCloud(denseCloud, poses.at(iter->first));
+							}
+
+							pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);
+							pcl::toPCLPointCloud2(*denseCloud, mesh->cloud);
+							mesh->polygons = densePolygons;
+							if(_ui->doubleSpinBox_meshDecimationFactor->isEnabled() &&
+							   _ui->doubleSpinBox_meshDecimationFactor->value() > 0.0)
+							{
+								int count = mesh->polygons.size();
+								mesh = util3d::meshDecimation(mesh, (float)_ui->doubleSpinBox_meshDecimationFactor->value());
+								_progressDialog->appendText(tr("Mesh decimation (factor=%1) from %2 to %3 polygons").arg(_ui->doubleSpinBox_meshDecimationFactor->value()).arg(count).arg(mesh->polygons.size()));
+							}
+							else
+							{
+								organizedIndices.insert(std::make_pair(iter->first, std::make_pair(newToOldIndices, std::make_pair(iter->second->width, iter->second->height))));
+							}
+							meshes.insert(std::make_pair(iter->first, mesh));
+						}
+						else
 						{
 							denseCloud = util3d::transformPointCloud(denseCloud, poses.at(iter->first));
 							if(mergedClouds->size() == 0)
@@ -715,46 +766,6 @@ bool ExportCloudsDialog::getExportedClouds(
 								util3d::appendMesh(*mergedClouds, mergedPolygons, *denseCloud, densePolygons);
 							}
 						}
-						else
-						{
-							if(_ui->doubleSpinBox_voxelSize_assembled->value() == 0.0)
-							{
-								pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);
-								pcl::toPCLPointCloud2(*denseCloud, mesh->cloud);
-								mesh->polygons = densePolygons;
-								if(_ui->doubleSpinBox_meshDecimationFactor->isEnabled() &&
-								   _ui->doubleSpinBox_meshDecimationFactor->value() > 0.0)
-								{
-									int count = mesh->polygons.size();
-									mesh = util3d::meshDecimation(mesh, (float)_ui->doubleSpinBox_meshDecimationFactor->value());
-									_progressDialog->appendText(tr("Assembled mesh decimation (factor=%1) from %2 to %3 polygons").arg(_ui->doubleSpinBox_meshDecimationFactor->value()).arg(count).arg(mesh->polygons.size()));
-								}
-								meshes.insert(std::make_pair(iter->first, mesh));
-							}
-							else
-							{
-								densePolygons = util3d::filterCloseVerticesFromMesh(
-										denseCloud,
-										densePolygons,
-										_ui->doubleSpinBox_voxelSize_assembled->value(),
-										M_PI/4,
-										true);
-								pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr filteredCloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
-								std::vector<pcl::Vertices> filteredPolygons;
-								util3d::filterNotUsedVerticesFromMesh(*denseCloud, densePolygons, *filteredCloud, filteredPolygons);
-								pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);
-								pcl::toPCLPointCloud2(*filteredCloud, mesh->cloud);
-								mesh->polygons = filteredPolygons;
-								if(_ui->doubleSpinBox_meshDecimationFactor->isEnabled() &&
-								   _ui->doubleSpinBox_meshDecimationFactor->value() > 0.0)
-								{
-									int count = mesh->polygons.size();
-									mesh = util3d::meshDecimation(mesh, (float)_ui->doubleSpinBox_meshDecimationFactor->value());
-									_progressDialog->appendText(tr("Assembled mesh decimation (factor=%1) from %2 to %3 polygons").arg(_ui->doubleSpinBox_meshDecimationFactor->value()).arg(count).arg(mesh->polygons.size()));
-								}
-								meshes.insert(std::make_pair(iter->first, mesh));
-							}
-						}
 					}
 					else
 					{
@@ -769,7 +780,7 @@ bool ExportCloudsDialog::getExportedClouds(
 				{
 					if(_ui->doubleSpinBox_voxelSize_assembled->value())
 					{
-						_progressDialog->appendText(tr("Filtering assembled mesh (points=%1, polygons=%2)...").arg(mergedClouds->size()).arg(mergedPolygons.size()));
+						_progressDialog->appendText(tr("Filtering assembled mesh for close vertices (points=%1, polygons=%2)...").arg(mergedClouds->size()).arg(mergedPolygons.size()));
 						QApplication::processEvents();
 
 						mergedPolygons = util3d::filterCloseVerticesFromMesh(
@@ -865,7 +876,7 @@ bool ExportCloudsDialog::getExportedClouds(
 				else
 				{
 					UASSERT(uContains(poses, iter->first));
-					cameras.insert(std::make_pair(iter->first, Transform::getIdentity()));
+					cameras.insert(std::make_pair(iter->first, _ui->checkBox_assemble->isChecked()?poses.at(iter->first):Transform::getIdentity()));
 				}
 				std::map<int, Transform> cameraPoses;
 				std::map<int, CameraModel> cameraModels;
@@ -899,12 +910,67 @@ bool ExportCloudsDialog::getExportedClouds(
 				}
 				if(cameraPoses.size())
 				{
-					pcl::TextureMesh::Ptr textureMesh = util3d::createTextureMesh(
+					pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
+					std::map<int, std::pair<std::map<int, int>, std::pair<int, int> > >::iterator oter = organizedIndices.find(iter->first);
+					if(iter->first != 0 && oter != organizedIndices.end())
+					{
+						UDEBUG("Texture by pixels");
+						textureMesh->cloud = iter->second->cloud;
+						textureMesh->tex_polygons.push_back(iter->second->polygons);
+						int w = oter->second.second.first;
+						int h = oter->second.second.second;
+						UASSERT(w > 1 && h > 1);
+						UASSERT(textureMesh->tex_polygons.size() && textureMesh->tex_polygons[0].size());
+						textureMesh->tex_coordinates.resize(1);
+						int polygonSize = textureMesh->tex_polygons[0][0].vertices.size();
+						textureMesh->tex_coordinates[0].resize(polygonSize*textureMesh->tex_polygons[0].size());
+						for(unsigned int i=0; i<textureMesh->tex_polygons[0].size(); ++i)
+						{
+							const pcl::Vertices & vertices = textureMesh->tex_polygons[0][i];
+							UASSERT(polygonSize == (int)vertices.vertices.size());
+							for(int k=0; k<polygonSize; ++k)
+							{
+								//uv
+								std::map<int, int>::iterator vter = oter->second.first.find(vertices.vertices[k]);
+								UASSERT(vter != oter->second.first.end());
+								int originalVertex = vter->second;
+								textureMesh->tex_coordinates[0][i*polygonSize+k] = Eigen::Vector2f(
+										float(originalVertex % w) / float(w),   // u
+										float(h - originalVertex / w) / float(h)); // v
+							}
+						}
+						pcl::TexMaterial mesh_material;
+						mesh_material.tex_d = 1.0f;
+						mesh_material.tex_Ns = 75.0f;
+						mesh_material.tex_illum = 1;
+
+						std::stringstream tex_name;
+						tex_name << "material_" << iter->first;
+						tex_name >> mesh_material.tex_name;
+
+						std::string tmpDirectory = dir.filePath("tmp_textures").toStdString();
+						mesh_material.tex_file = uFormat("%s/%s%d.png", tmpDirectory.c_str(), "texture_", iter->first);
+						if(!cv::imwrite(mesh_material.tex_file, images.at(iter->first)))
+						{
+							UERROR("Cannot save texture of image %d", iter->first);
+						}
+						else
+						{
+							UINFO("Saved temporary texture: \"%s\"", mesh_material.tex_file.c_str());
+						}
+
+						textureMesh->tex_materials.push_back(mesh_material);
+					}
+					else
+					{
+						UDEBUG("Texture by projection");
+						textureMesh = util3d::createTextureMesh(
 								iter->second,
 								cameraPoses,
 								cameraModels,
 								images,
 								dir.filePath("tmp_textures").toStdString());
+					}
 
 					textureMeshes.insert(std::make_pair(iter->first, textureMesh));
 				}
@@ -918,6 +984,18 @@ bool ExportCloudsDialog::getExportedClouds(
 				QApplication::processEvents();
 			}
 
+			if(textureMeshes.size() > 1 && _ui->checkBox_assemble->isChecked())
+			{
+				_progressDialog->appendText(tr("Assembling %1 meshes...").arg(textureMeshes.size()));
+				QApplication::processEvents();
+				uSleep(100);
+				QApplication::processEvents();
+
+				pcl::TextureMesh::Ptr assembledMesh = util3d::concatenateTextureMeshes(uValuesList(textureMeshes));
+				textureMeshes.clear();
+				textureMeshes.insert(std::make_pair(0, assembledMesh));
+			}
+
 		}
 
 		return true;
@@ -959,11 +1037,19 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
 								d,
 								_ui->spinBox_decimation->value(),
 								_ui->doubleSpinBox_maxDepth->value(),
-								// Don't voxelize if we create organized mesh
+								_ui->doubleSpinBox_minDepth->value(),
-								_ui->comboBox_pipeline->currentIndex()==0 && _ui->groupBox_meshing->isChecked()?0:_ui->doubleSpinBox_voxelSize_assembled->value(),
-								0,
 								indices.get());
 
+						// Don't voxelize if we create organized mesh
+						if(!(_ui->comboBox_pipeline->currentIndex()==0 && _ui->groupBox_meshing->isChecked()))
+						{
+							cloudWithoutNormals = util3d::voxelize(cloudWithoutNormals, indices, _ui->doubleSpinBox_voxelSize_assembled->value());
+							indices->resize(cloudWithoutNormals->size());
+							for(unsigned int i=0; i<indices->size(); ++i)
+							{
+								indices->at(i) = i;
+							}
+						}
 
 						cloud = util3d::computeNormals(
 								cloudWithoutNormals,
@@ -1202,6 +1288,9 @@ void ExportCloudsDialog::saveMeshes(
 			if(meshes.begin()->second->polygons.size())
 			{
 				_progressDialog->appendText(tr("Saving the mesh (%1 polygons)...").arg(meshes.begin()->second->polygons.size()));
+				QApplication::processEvents();
+				uSleep(100);
+				QApplication::processEvents();
 
 				bool success =false;
 				if(QFileInfo(path).suffix() == "")
@@ -1350,6 +1439,9 @@ void ExportCloudsDialog::saveTextureMeshes(
 			if(meshes.begin()->second->tex_materials.size())
 			{
 				_progressDialog->appendText(tr("Saving the mesh (with %1 textures)...").arg(meshes.begin()->second->tex_materials.size()));
+				QApplication::processEvents();
+				uSleep(100);
+				QApplication::processEvents();
 
 				bool success =false;
 				if(QFileInfo(path).suffix() == "")
@@ -1424,7 +1516,8 @@ void ExportCloudsDialog::saveTextureMeshes(
 							QString fullPath = path+QDir::separator()+currentPrefix+QDir::separator()+info.fileName();
 							// relative path
 							mesh.tex_materials[i].tex_file=(currentPrefix+QDir::separator()+info.fileName()).toStdString();
-							if(!QFile::copy(iter->second->tex_materials[i].tex_file.c_str(), fullPath))
+							if(!QFile::copy(iter->second->tex_materials[i].tex_file.c_str(), fullPath) &&
+									info.fileName().compare("occluded.png") != 0)
 							{
 								_progressDialog->appendText(tr("Failed copying texture \"%1\" to \"%2\".")
 										.arg(iter->second->tex_materials[i].tex_file.c_str()).arg(fullPath), Qt::darkRed);

guilib/src/GraphViewer.cpp

@@ -195,10 +195,13 @@ GraphViewer::GraphViewer(QWidget * parent) :
 		_loopInterSessionColor(Qt::green),
 		_intraInterSessionColors(false),
 		_root(0),
+		_graphRoot(0),
+		_globalPathRoot(0),
 		_nodeRadius(0.01f),
 		_linkWidth(0),
 		_gridMap(0),
 		_referential(0),
+		_originReferential(0),
 		_gridCellSize(0.0f),
 		_localRadius(0),
 		_loopClosureOutlierThr(0),
@@ -245,6 +248,22 @@ GraphViewer::GraphViewer(QWidget * parent) :
 	_gridMap->setZValue(0);
 	_gridMap->setParentItem(_root);
 
+	_graphRoot = (QGraphicsItem *)this->scene()->addEllipse(QRectF(-0.0001,-0.0001,0.0001,0.0001));
+	_graphRoot->setZValue(2);
+	_graphRoot->setParentItem(_root);
+
+	_globalPathRoot = (QGraphicsItem *)this->scene()->addEllipse(QRectF(-0.0001,-0.0001,0.0001,0.0001));
+	_globalPathRoot->setZValue(3);
+	_globalPathRoot->setParentItem(_root);
+
+	_localPathRoot = (QGraphicsItem *)this->scene()->addEllipse(QRectF(-0.0001,-0.0001,0.0001,0.0001));
+	_localPathRoot->setZValue(4);
+	_localPathRoot->setParentItem(_root);
+
+	_gtGraphRoot = (QGraphicsItem *)this->scene()->addEllipse(QRectF(-0.0001,-0.0001,0.0001,0.0001));
+	_gtGraphRoot->setZValue(2);
+	_gtGraphRoot->setParentItem(_root);
+
 	this->restoreDefaults();
 
 	this->fitInView(this->sceneRect(), Qt::KeepAspectRatio);
@@ -258,6 +277,9 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 				 const std::multimap<int, Link> & constraints,
 				 const std::map<int, int> & mapIds)
 {
+	bool wasVisible = _graphRoot->isVisible();
+	_graphRoot->show();
+
 	bool wasEmpty = _nodeItems.size() == 0 && _linkItems.size() == 0;
 	UDEBUG("poses=%d constraints=%d", (int)poses.size(), (int)constraints.size());
 	//Hide nodes and links
@@ -289,7 +311,7 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 				this->scene()->addItem(item);
 				item->setZValue(20);
 				item->setColor(_nodeColor);
-				item->setParentItem(_root);
+				item->setParentItem(_graphRoot);
 				_nodeItems.insert(iter->first, item);
 			}
 		}
@@ -344,7 +366,7 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 					linkItem->setPen(p);
 					linkItem->setZValue(10);
 					this->scene()->addItem(linkItem);
-					linkItem->setParentItem(_root);
+					linkItem->setParentItem(_graphRoot);
 					_linkItems.insert(idFrom, linkItem);
 				}
 			}
@@ -464,11 +486,15 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 		this->fitInView(rect.adjusted(-rect.width()/2.0f, -rect.height()/2.0f, rect.width()/2.0f, rect.height()/2.0f), Qt::KeepAspectRatio);
 	}
 
+	_graphRoot->setVisible(wasVisible);
+
 	UDEBUG("_nodeItems=%d, _linkItems=%d", _nodeItems.size(), _linkItems.size());
 }
 
 void GraphViewer::updateGTGraph(const std::map<int, Transform> & poses)
 {
+	bool wasVisible = _gtGraphRoot->isVisible();
+	_gtGraphRoot->show();
 	bool wasEmpty = _gtNodeItems.size() == 0 && _gtLinkItems.size() == 0;
 	UDEBUG("poses=%d", (int)poses.size());
 	//Hide nodes and links
@@ -500,7 +526,7 @@ void GraphViewer::updateGTGraph(const std::map<int, Transform> & poses)
 				this->scene()->addItem(item);
 				item->setZValue(20);
 				item->setColor(_gtPathColor);
-				item->setParentItem(_root);
+				item->setParentItem(_gtGraphRoot);
 				_gtNodeItems.insert(iter->first, item);
 			}
 
@@ -537,7 +563,7 @@ void GraphViewer::updateGTGraph(const std::map<int, Transform> & poses)
 					linkItem->setPen(p);
 					linkItem->setZValue(10);
 					this->scene()->addItem(linkItem);
-					linkItem->setParentItem(_root);
+					linkItem->setParentItem(_gtGraphRoot);
 					_gtLinkItems.insert(iterPrevious->first, linkItem);
 				}
 				if(linkItem)
@@ -585,6 +611,8 @@ void GraphViewer::updateGTGraph(const std::map<int, Transform> & poses)
 		}
 	}
 
+	_gtGraphRoot->setVisible(wasVisible);
+
 	UDEBUG("_gtNodeItems=%d, _gtLinkItems=%d", _gtNodeItems.size(), _gtLinkItems.size());
 }
 
@@ -670,7 +698,7 @@ void GraphViewer::setGlobalPath(const std::vector<std::pair<int, Transform> > &
 			item->setColor(_globalPathColor);
 			this->scene()->addItem(item);
 			item->setZValue(15);
-			item->setParentItem(_root);
+			item->setParentItem(_globalPathRoot);
 			_globalPathLinkItems.insert(idFrom, item);
 		}
 	}
@@ -707,6 +735,9 @@ void GraphViewer::setLocalRadius(float radius)
 
 void GraphViewer::updateLocalPath(const std::vector<int> & localPath)
 {
+	bool wasVisible = _localPathRoot->isVisible();
+	_localPathRoot->show();
+
 	for(QMultiMap<int, LinkItem*>::iterator iter = _localPathLinkItems.begin(); iter!=_localPathLinkItems.end(); ++iter)
 	{
 		iter.value()->hide();
@@ -746,7 +777,7 @@ void GraphViewer::updateLocalPath(const std::vector<int> & localPath)
 					item->setColor(_localPathColor);
 					this->scene()->addItem(item);
 					item->setZValue(16); // just over the global path
-					item->setParentItem(_root);
+					item->setParentItem(_localPathRoot);
 					_localPathLinkItems.insert(idFrom, item);
 				}
 			}
@@ -766,6 +797,7 @@ void GraphViewer::updateLocalPath(const std::vector<int> & localPath)
 			++iter;
 		}
 	}
+	_localPathRoot->setVisible(wasVisible);
 }
 
 void GraphViewer::clearGraph()
@@ -838,6 +870,10 @@ void GraphViewer::saveSettings(QSettings & settings, const QString & group) cons
 	settings.setValue("local_radius_visible", this->isLocalRadiusVisible());
 	settings.setValue("loop_closure_outlier_thr", this->getLoopClosureOutlierThr());
 	settings.setValue("max_link_length", this->getMaxLinkLength());
+	settings.setValue("graph_visible", this->isGraphVisible());
+	settings.setValue("global_path_visible", this->isGlobalPathVisible());
+	settings.setValue("local_path_visible", this->isLocalPathVisible());
+	settings.setValue("gt_graph_visible", this->isGtGraphVisible());
 	if(!group.isEmpty())
 	{
 		settings.endGroup();
@@ -873,6 +909,10 @@ void GraphViewer::loadSettings(QSettings & settings, const QString & group)
 	this->setIntraInterSessionColorsEnabled(settings.value("intra_inter_session_colors_enabled", this->isIntraInterSessionColorsEnabled()).toBool());
 	this->setLoopClosureOutlierThr(settings.value("loop_closure_outlier_thr", this->getLoopClosureOutlierThr()).toDouble());
 	this->setMaxLinkLength(settings.value("max_link_length", this->getMaxLinkLength()).toDouble());
+	this->setGraphVisible(settings.value("graph_visible", this->isGraphVisible()).toBool());
+	this->setGlobalPathVisible(settings.value("global_path_visible", this->isGlobalPathVisible()).toBool());
+	this->setLocalPathVisible(settings.value("local_path_visible", this->isLocalPathVisible()).toBool());
+	this->setGtGraphVisible(settings.value("gt_graph_visible", this->isGtGraphVisible()).toBool());
 	if(!group.isEmpty())
 	{
 		settings.endGroup();
@@ -895,6 +935,22 @@ bool GraphViewer::isLocalRadiusVisible() const
 {
 	return _localRadius->isVisible();
 }
+bool GraphViewer::isGraphVisible() const
+{
+	return _graphRoot->isVisible();
+}
+bool GraphViewer::isGlobalPathVisible() const
+{
+	return _globalPathRoot->isVisible();
+}
+bool GraphViewer::isLocalPathVisible() const
+{
+	return _localPathRoot->isVisible();
+}
+bool GraphViewer::isGtGraphVisible() const
+{
+	return _gtGraphRoot->isVisible();
+}
 
 void GraphViewer::setWorkingDirectory(const QString & path)
 {
@@ -1114,6 +1170,22 @@ void GraphViewer::setMaxLinkLength(float value)
 {
 	_maxLinkLength = value;
 }
+void GraphViewer::setGraphVisible(bool visible)
+{
+	_graphRoot->setVisible(!_graphRoot->isVisible());
+}
+void GraphViewer::setGlobalPathVisible(bool visible)
+{
+	_globalPathRoot->setVisible(!_globalPathRoot->isVisible());
+}
+void GraphViewer::setLocalPathVisible(bool visible)
+{
+	_localPathRoot->setVisible(!_localPathRoot->isVisible());
+}
+void GraphViewer::setGtGraphVisible(bool visible)
+{
+	_gtGraphRoot->setVisible(!_gtGraphRoot->isVisible());
+}
 
 void GraphViewer::restoreDefaults()
 {
@@ -1127,9 +1199,12 @@ void GraphViewer::restoreDefaults()
 	setVirtualLoopClosureColor(Qt::magenta);
 	setNeighborMergedColor(QColor(255,170,0));
 	setGridMapVisible(true);
+	setGraphVisible(true);
+	setGlobalPathVisible(true);
+	setLocalPathVisible(true);
+	setGtGraphVisible(true);
 }
 
-
 void GraphViewer::wheelEvent ( QWheelEvent * event )
 {
 	if(event->delta() < 0)
@@ -1211,10 +1286,13 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 	QAction * aChangeMaxLinkLength = menu.addAction(tr("Set maximum link length..."));
 	menu.addSeparator();
 	QAction * aShowHideGridMap;
+	QAction * aShowHideGraph;
 	QAction * aShowHideOrigin;
 	QAction * aShowHideReferential;
 	QAction * aShowHideLocalRadius;
-	QAction * aClearGlobalPath;
+	QAction * aShowHideGlobalPath;
+	QAction * aShowHideLocalPath;
+	QAction * aShowHideGtGraph;
 	if(_gridMap->isVisible())
 	{
 		aShowHideGridMap = menu.addAction(tr("Hide grid map"));
@@ -1247,15 +1325,43 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 	{
 		aShowHideLocalRadius = menu.addAction(tr("Show local radius"));
 	}
-	if(_globalPathLinkItems.size() && _globalPathLinkItems.begin().value()->isVisible())
+	if(_graphRoot->isVisible())
 	{
-		aClearGlobalPath = menu.addAction(tr("Hide global path"));
+		aShowHideGraph = menu.addAction(tr("Hide graph"));
 	}
 	else
 	{
-		aClearGlobalPath = menu.addAction(tr("Show global path"));
+		aShowHideGraph = menu.addAction(tr("Show graph"));
 	}
-	aClearGlobalPath->setEnabled(_globalPathLinkItems.size());
+	if(_globalPathRoot->isVisible())
+	{
+		aShowHideGlobalPath = menu.addAction(tr("Hide global path"));
+	}
+	else
+	{
+		aShowHideGlobalPath = menu.addAction(tr("Show global path"));
+	}
+	if(_localPathRoot->isVisible())
+	{
+		aShowHideLocalPath = menu.addAction(tr("Hide local path"));
+	}
+	else
+	{
+		aShowHideLocalPath = menu.addAction(tr("Show local path"));
+	}
+	if(_gtGraphRoot->isVisible())
+	{
+		aShowHideGtGraph = menu.addAction(tr("Hide ground truth graph"));
+	}
+	else
+	{
+		aShowHideGtGraph = menu.addAction(tr("Show ground truth graph"));
+	}
+	aShowHideGridMap->setEnabled(!_gridMap->pixmap().isNull());
+	aShowHideGraph->setEnabled(_nodeItems.size());
+	aShowHideGlobalPath->setEnabled(_globalPathLinkItems.size());
+	aShowHideLocalPath->setEnabled(_localPathLinkItems.size());
+	aShowHideGtGraph->setEnabled(_gtNodeItems.size());
 	menu.addSeparator();
 	QAction * aRestoreDefaults = menu.addAction(tr("Restore defaults"));
 
@@ -1524,12 +1630,21 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 	{
 		this->restoreDefaults();
 	}
-	else if(r == aClearGlobalPath)
+	else if(r == aShowHideGraph)
 	{
-		for(QMap<int, LinkItem*>::iterator iter=_globalPathLinkItems.begin(); iter!=_globalPathLinkItems.end(); ++iter)
+		this->setGraphVisible(!this->isGraphVisible());
-		{
-			iter.value()->setVisible(!iter.value()->isVisible());
 	}
+	else if(r == aShowHideGlobalPath)
+	{
+		this->setGlobalPathVisible(!this->isGlobalPathVisible());
+	}
+	else if(r == aShowHideLocalPath)
+	{
+		this->setLocalPathVisible(!this->isLocalPathVisible());
+	}
+	else if(r == aShowHideGtGraph)
+	{
+		this->setGtGraphVisible(!this->isGtGraphVisible());
 	}
 	if(r)
 	{

guilib/src/LoopClosureViewer.cpp

@@ -44,7 +44,7 @@ LoopClosureViewer::LoopClosureViewer(QWidget * parent) :
 	QWidget(parent),
 	decimation_(1),
 	maxDepth_(0),
-	samples_(0)
+	minDepth_(0)
 {
 	ui_ = new Ui_loopClosureViewer();
 	ui_->setupUi(this);
@@ -73,17 +73,17 @@ void LoopClosureViewer::updateView(const Transform & transform)
 	{
 		int decimation = 1;
 		float maxDepth = 0;
-		int samples = 0;
+		float minDepth = 0;
 
 		if(!ui_->checkBox_rawCloud->isChecked())
 		{
			decimation = decimation_;
 			maxDepth = maxDepth_;
-			samples = samples_;
+			minDepth = minDepth_;
 		}
 
 		UDEBUG("decimation = %d", decimation);
 		UDEBUG("maxDepth = %f", maxDepth);
-		UDEBUG("samples = %d", samples);
+		UDEBUG("minDepth = %d", minDepth);
 
 		Transform t;
 		if(!transform.isNull())
@@ -106,8 +106,8 @@ void LoopClosureViewer::updateView(const Transform & transform)
 		{
 			//cloud 3d
 			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudA, cloudB;
-			cloudA = util3d::cloudRGBFromSensorData(sA_.sensorData(), decimation, maxDepth, 0.0f, samples);
+			cloudA = util3d::cloudRGBFromSensorData(sA_.sensorData(), decimation, maxDepth, minDepth);
-			cloudB = util3d::cloudRGBFromSensorData(sB_.sensorData(), decimation, maxDepth, 0.0f, samples);
+			cloudB = util3d::cloudRGBFromSensorData(sB_.sensorData(), decimation, maxDepth, minDepth);
 
 			//cloud 2d
 			pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;

guilib/src/MapVisibilityWidget.cpp

@@ -45,6 +45,10 @@ MapVisibilityWidget::MapVisibilityWidget(QWidget * parent) : QWidget(parent) {
 	QVBoxLayout * layout2 = new QVBoxLayout(scrollAreaWidgetContent);
 	scrollAreaWidgetContent->setLayout(layout2);
 	scrollArea->setWidget(scrollAreaWidgetContent);
+
+	QCheckBox * selectAll = new QCheckBox("Select all", this);
+	connect(selectAll, SIGNAL(toggled(bool)), this, SLOT(selectAll(bool)));
+	verticalLayout1->addWidget(selectAll);
 	verticalLayout1->addWidget(scrollArea);
 }
 
@@ -126,4 +130,14 @@ void MapVisibilityWidget::signalVisibility()
 	emit visibilityChanged(check->text().split('(').first().toInt(), check->isChecked());
 }
 
+void MapVisibilityWidget::selectAll(bool checked)
+{
+	QWidget * area = this->findChild<QWidget*>("area");
+	QList<QCheckBox*> checkboxes = area->findChildren<QCheckBox*>();
+	for(int i = 0; i<checkboxes.size(); ++i)
+	{
+		checkboxes[i]->setChecked(checked);
+	}
+}
+
 } /* namespace rtabmap */

guilib/src/MapVisibilityWidget.h

@@ -50,6 +50,7 @@ protected:
 
 private slots:
 	void signalVisibility();
+	void selectAll(bool);
 
 private:
 	void updateCheckBoxes();

guilib/src/OdometryViewer.cpp

@@ -235,11 +235,18 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
 		// visualization: buffering the clouds
 		// Create the new cloud
 		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
+		pcl::IndicesPtr validIndices(new std::vector<int>);
 		cloud = util3d::cloudRGBFromSensorData(
 				odom.data(),
 				validDecimationValue_,
-				0.0f,
+				0,
-				voxelSpin_->value());
+				0,
+				validIndices.get());
+
+		if(voxelSpin_->value())
+		{
+			cloud = util3d::voxelize(cloud, validIndices, voxelSpin_->value());
+		}
 
 		if(cloud->size())
 		{

guilib/src/PostProcessingDialog.cpp

@@ -39,11 +39,21 @@ PostProcessingDialog::PostProcessingDialog(QWidget * parent) :
 	_ui = new Ui_PostProcessingDialog();
 	_ui->setupUi(this);
 
-	if(!Optimizer::isAvailable(Optimizer::kTypeCVSBA))
+	if(!Optimizer::isAvailable(Optimizer::kTypeCVSBA) && !Optimizer::isAvailable(Optimizer::kTypeG2O))
 	{
 		_ui->sba->setEnabled(false);
 		_ui->sba->setChecked(false);
 	}
+	else if(!Optimizer::isAvailable(Optimizer::kTypeCVSBA))
+	{
+		_ui->comboBox_sbaType->setItemData(1, 0, Qt::UserRole - 1);
+		_ui->comboBox_sbaType->setCurrentIndex(0);
+	}
+	else if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
+	{
+		_ui->comboBox_sbaType->setItemData(0, 0, Qt::UserRole - 1);
+		_ui->comboBox_sbaType->setCurrentIndex(1);
+	}
 
 	restoreDefaults();
 
@@ -57,15 +67,16 @@ PostProcessingDialog::PostProcessingDialog(QWidget * parent) :
 	connect(_ui->clusterRadius, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
 	connect(_ui->clusterAngle, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
 	connect(_ui->iterations, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
-	connect(_ui->reextractFeatures, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->refineNeighborLinks, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->refineLoopClosureLinks, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
 
 	connect(_ui->sba, SIGNAL(clicked(bool)), this, SIGNAL(configChanged()));
 	connect(_ui->sba_iterations, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->sba_epsilon, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
-	connect(_ui->sba_minInlierDistance, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->comboBox_sbaType, SIGNAL(currentIndexChanged(int)), this, SIGNAL(configChanged()));
-	connect(_ui->sba_minInliers, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->comboBox_sbaType, SIGNAL(currentIndexChanged(int)), this, SLOT(updateVisibility()));
+
+	updateVisibility();
 }
 
 PostProcessingDialog::~PostProcessingDialog()
@@ -73,6 +84,12 @@ PostProcessingDialog::~PostProcessingDialog()
 	delete _ui;
 }
 
+void PostProcessingDialog::updateVisibility()
+{
+	_ui->sba_variance->setVisible(_ui->comboBox_sbaType->currentIndex() == 0);
+	_ui->label_variance->setVisible(_ui->comboBox_sbaType->currentIndex() == 0);
+}
+
 void PostProcessingDialog::saveSettings(QSettings & settings, const QString & group) const
 {
 	if(!group.isEmpty())
@@ -83,14 +100,13 @@ void PostProcessingDialog::saveSettings(QSettings & settings, const QString & gr
 	settings.setValue("cluster_radius", this->clusterRadius());
 	settings.setValue("cluster_angle", this->clusterAngle());
 	settings.setValue("iterations", this->iterations());
-	settings.setValue("reextract_features", this->isReextractFeatures());
 	settings.setValue("refine_neigbors", this->isRefineNeighborLinks());
 	settings.setValue("refine_lc", this->isRefineLoopClosureLinks());
 	settings.setValue("sba", this->isSBA());
 	settings.setValue("sba_iterations", this->sbaIterations());
 	settings.setValue("sba_epsilon", this->sbaEpsilon());
-	settings.setValue("sba_inlier_distance", this->sbaInlierDistance());
+	settings.setValue("sba_type", this->sbaType());
-	settings.setValue("sba_min_inliers", this->sbaMinInliers());
+	settings.setValue("sba_variance", this->sbaVariance());
 	if(!group.isEmpty())
 	{
 		settings.endGroup();
@@ -107,14 +123,13 @@ void PostProcessingDialog::loadSettings(QSettings & settings, const QString & gr
 	this->setClusterRadius(settings.value("cluster_radius", this->clusterRadius()).toDouble());
 	this->setClusterAngle(settings.value("cluster_angle", this->clusterAngle()).toDouble());
 	this->setIterations(settings.value("iterations", this->iterations()).toInt());
-	this->setReextractFeatures(settings.value("reextract_features", this->isReextractFeatures()).toBool());
 	this->setRefineNeighborLinks(settings.value("refine_neigbors", this->isRefineNeighborLinks()).toBool());
 	this->setRefineLoopClosureLinks(settings.value("refine_lc", this->isRefineLoopClosureLinks()).toBool());
 	this->setSBA(settings.value("sba", this->isSBA()).toBool());
 	this->setSBAIterations(settings.value("sba_iterations", this->sbaIterations()).toInt());
 	this->setSBAEpsilon(settings.value("sba_epsilon", this->sbaEpsilon()).toDouble());
-	this->setSBAInlierDistance(settings.value("sba_inlier_distance", this->sbaInlierDistance()).toDouble());
+	this->setSBAType((Optimizer::Type)settings.value("sba_type", this->sbaType()).toInt());
-	this->setSBAMinInliers(settings.value("sba_min_inliers", this->sbaMinInliers()).toInt());
+	this->setSBAVariance(settings.value("sba_variance", this->sbaVariance()).toDouble());
 	if(!group.isEmpty())
 	{
 		settings.endGroup();
@@ -127,14 +142,13 @@ void PostProcessingDialog::restoreDefaults()
 	setClusterRadius(0.5);
 	setClusterAngle(30);
 	setIterations(1);
-	setReextractFeatures(false);
 	setRefineNeighborLinks(false);
 	setRefineLoopClosureLinks(false);
 	setSBA(false);
 	setSBAIterations(20);
-	setSBAEpsilon(0.0001);
+	setSBAEpsilon(0.0);
-	setSBAInlierDistance(0.05);
+	setSBAType(!Optimizer::isAvailable(Optimizer::kTypeG2O)&&Optimizer::isAvailable(Optimizer::kTypeCVSBA)?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O);
-	setSBAMinInliers(10);
+	setSBAVariance(1.0);
 }
 
 void PostProcessingDialog::updateButtonBox()
@@ -163,11 +177,6 @@ int PostProcessingDialog::iterations() const
 	return _ui->iterations->value();
 }
 
-bool PostProcessingDialog::isReextractFeatures() const
-{
-	return _ui->reextractFeatures->isChecked();
-}
-
 bool PostProcessingDialog::isRefineNeighborLinks() const
 {
 	return _ui->refineNeighborLinks->isChecked();
@@ -191,13 +200,13 @@ double PostProcessingDialog::sbaEpsilon() const
 {
 	return _ui->sba_epsilon->value();
 }
-double PostProcessingDialog::sbaInlierDistance() const
+double PostProcessingDialog::sbaVariance() const
 {
-	return _ui->sba_minInlierDistance->value();
+	return _ui->sba_variance->value();
 }
-int PostProcessingDialog::sbaMinInliers() const
+Optimizer::Type PostProcessingDialog::sbaType() const
 {
-	return _ui->sba_minInliers->value();
+	return _ui->comboBox_sbaType->currentIndex()==0?Optimizer::kTypeG2O:Optimizer::kTypeCVSBA;
 }
 
 //setters
@@ -217,10 +226,6 @@ void PostProcessingDialog::setIterations(int iterations)
 {
 	_ui->iterations->setValue(iterations);
 }
-void PostProcessingDialog::setReextractFeatures(bool on)
-{
-	_ui->reextractFeatures->setChecked(on);
-}
 void PostProcessingDialog::setRefineNeighborLinks(bool on)
 {
 	_ui->refineNeighborLinks->setChecked(on);
@@ -241,13 +246,20 @@ void PostProcessingDialog::setSBAEpsilon(double epsilon)
 {
 	_ui->sba_epsilon->setValue(epsilon);
 }
-void PostProcessingDialog::setSBAInlierDistance(double inlierDistance)
+void PostProcessingDialog::setSBAVariance(double variance)
 {
-	_ui->sba_minInlierDistance->setValue(inlierDistance);
+	_ui->sba_variance->setValue(variance);
 }
-void PostProcessingDialog::setSBAMinInliers(int minInliers)
+void PostProcessingDialog::setSBAType(Optimizer::Type type)
 {
-	_ui->sba_minInliers->setValue(minInliers);
+	if(type == Optimizer::kTypeCVSBA)
+	{
+		_ui->comboBox_sbaType->setCurrentIndex(1);
+	}
+	else
+	{
+		_ui->comboBox_sbaType->setCurrentIndex(0);
+	}
 }
 
 

guilib/src/PostProcessingDialog.h

@@ -31,6 +31,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QDialog>
 #include <QtCore/QSettings>
 
+#include <rtabmap/core/Optimizer.h>
+
 class Ui_PostProcessingDialog;
 class QAbstractButton;
 
@@ -53,28 +55,26 @@ public:
 	double clusterRadius() const;
 	double clusterAngle() const;
 	int iterations() const;
-	bool isReextractFeatures() const;
 	bool isRefineNeighborLinks() const;
 	bool isRefineLoopClosureLinks() const;
 	bool isSBA() const;
 	int sbaIterations() const;
 	double sbaEpsilon() const;
-	double sbaInlierDistance() const;
+	double sbaVariance() const;
-	int sbaMinInliers() const;
+	Optimizer::Type sbaType() const;
 
 	//setters
 	void setDetectMoreLoopClosures(bool on);
 	void setClusterRadius(double radius);
 	void setClusterAngle(double angle);
 	void setIterations(int iterations);
-	void setReextractFeatures(bool on);
 	void setRefineNeighborLinks(bool on);
 	void setRefineLoopClosureLinks(bool on);
 	void setSBA(bool on);
 	void setSBAIterations(int iterations);
 	void setSBAEpsilon(double epsilon);
-	void setSBAInlierDistance(double inlierDistance);
+	void setSBAVariance(double variance);
-	void setSBAMinInliers(int minInliers);
+	void setSBAType(Optimizer::Type type);
 
 signals:
 	void configChanged();
@@ -83,6 +83,7 @@ public slots:
 	void restoreDefaults();
 
 private slots:
+	void updateVisibility();
 	void updateButtonBox();
 
 

guilib/src/PreferencesDialog.cpp

@@ -98,6 +98,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui(0),
 	_indexModel(0),
 	_initialized(false),
+	_progressDialog(new QProgressDialog(this)),
 	_calibrationDialog(new CalibrationDialog(false, ".", this)),
 	_createCalibrationDialog(new CreateSimpleCalibrationDialog(".", "", this))
 {
@@ -105,6 +106,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_calibrationDialog->setWindowFlags(Qt::Window);
 	_calibrationDialog->setWindowTitle(tr("Calibration"));
 
+	_progressDialog->setWindowTitle(tr("Read parameters..."));
+	_progressDialog->setMaximum(2);
+	_progressDialog->setValue(2);
+
 	_ui = new Ui_preferencesDialog();
 	_ui->setupUi(this);
 
@@ -281,6 +286,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_3dRenderingMaxDepth[0] = _ui->doubleSpinBox_maxDepth;
 	_3dRenderingMaxDepth[1] = _ui->doubleSpinBox_maxDepth_odom;
 
+	_3dRenderingMinDepth.resize(2);
+	_3dRenderingMinDepth[0] = _ui->doubleSpinBox_minDepth;
+	_3dRenderingMinDepth[1] = _ui->doubleSpinBox_minDepth_odom;
+
 	_3dRenderingOpacity.resize(2);
 	_3dRenderingOpacity[0] = _ui->doubleSpinBox_opacity;
 	_3dRenderingOpacity[1] = _ui->doubleSpinBox_opacity_odom;
@@ -322,6 +331,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 		connect(_3dRenderingShowClouds[i], SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 		connect(_3dRenderingDecimation[i], SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 		connect(_3dRenderingMaxDepth[i], SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+		connect(_3dRenderingMinDepth[i], SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 		connect(_3dRenderingShowScans[i], SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 		connect(_3dRenderingShowFeatures[i], SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 
@@ -530,7 +540,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->general_checkBox_createMapLabels->setObjectName(Parameters::kMemMapLabelsAdded().c_str());
 	_ui->general_checkBox_initWMWithAllNodes->setObjectName(Parameters::kMemInitWMWithAllNodes().c_str());
 	_ui->checkBox_localSpaceScanMatchingIDsSaved->setObjectName(Parameters::kRGBDScanMatchingIdsSavedInLinks().c_str());
-	_ui->spinBox_imageDecimation->setObjectName(Parameters::kMemImageDecimation().c_str());
+	_ui->spinBox_imagePreDecimation->setObjectName(Parameters::kMemImagePreDecimation().c_str());
+	_ui->spinBox_imagePostDecimation->setObjectName(Parameters::kMemImagePostDecimation().c_str());
 	_ui->general_spinBox_laserScanDownsample->setObjectName(Parameters::kMemLaserScanDownsampleStepSize().c_str());
 	_ui->checkBox_useOdomFeatures->setObjectName(Parameters::kMemUseOdomFeatures().c_str());
 
@@ -658,6 +669,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 
 	_ui->comboBox_g2o_solver->setObjectName(Parameters::kg2oSolver().c_str());
 	_ui->comboBox_g2o_optimizer->setObjectName(Parameters::kg2oOptimizer().c_str());
+	_ui->doubleSpinBox_g2o_variance->setObjectName(Parameters::kg2oPixelVariance().c_str());
 
 	_ui->graphPlan_goalReachedRadius->setObjectName(Parameters::kRGBDGoalReachedRadius().c_str());
 	_ui->graphPlan_goalsSavedInUserData->setObjectName(Parameters::kRGBDGoalsSavedInUserData().c_str());
@@ -737,6 +749,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->odom_flow_keyframeThr->setObjectName(Parameters::kOdomKeyFrameThr().c_str());
 	_ui->odom_flow_scanKeyframeThr->setObjectName(Parameters::kOdomScanKeyFrameThr().c_str());
 	_ui->odom_flow_guessMotion->setObjectName(Parameters::kOdomGuessMotion().c_str());
+	_ui->odom_imageDecimation->setObjectName(Parameters::kOdomImageDecimation().c_str());
 
 	//Odometry Frame to Map
 	_ui->odom_localHistory->setObjectName(Parameters::kOdomF2MMaxSize().c_str());
@@ -809,10 +822,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	this->setupTreeView();
 
 	_obsoletePanels = kPanelAll;
-
-	_progressDialog = new QProgressDialog(this);
-	_progressDialog->setWindowTitle(tr("Read parameters..."));
-	_progressDialog->setMaximum(2);
 }
 
 PreferencesDialog::~PreferencesDialog() {
@@ -1135,6 +1144,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 			_3dRenderingShowClouds[i]->setChecked(true);
 			_3dRenderingDecimation[i]->setValue(8);
 			_3dRenderingMaxDepth[i]->setValue(0.0);
+			_3dRenderingMinDepth[i]->setValue(0.0);
 			_3dRenderingShowScans[i]->setChecked(true);
 			_3dRenderingShowFeatures[i]->setChecked(i==0?false:true);
 
@@ -1474,6 +1484,7 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 		_3dRenderingShowClouds[i]->setChecked(settings.value(QString("showClouds%1").arg(i), _3dRenderingShowClouds[i]->isChecked()).toBool());
 		_3dRenderingDecimation[i]->setValue(settings.value(QString("decimation%1").arg(i), _3dRenderingDecimation[i]->value()).toInt());
 		_3dRenderingMaxDepth[i]->setValue(settings.value(QString("maxDepth%1").arg(i), _3dRenderingMaxDepth[i]->value()).toDouble());
+		_3dRenderingMinDepth[i]->setValue(settings.value(QString("minDepth%1").arg(i), _3dRenderingMinDepth[i]->value()).toDouble());
 		_3dRenderingShowScans[i]->setChecked(settings.value(QString("showScans%1").arg(i), _3dRenderingShowScans[i]->isChecked()).toBool());
 		_3dRenderingShowFeatures[i]->setChecked(settings.value(QString("showFeatures%1").arg(i), _3dRenderingShowFeatures[i]->isChecked()).toBool());
 
@@ -1856,6 +1867,7 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
 		settings.setValue(QString("showClouds%1").arg(i), _3dRenderingShowClouds[i]->isChecked());
 		settings.setValue(QString("decimation%1").arg(i), _3dRenderingDecimation[i]->value());
 		settings.setValue(QString("maxDepth%1").arg(i), _3dRenderingMaxDepth[i]->value());
+		settings.setValue(QString("minDepth%1").arg(i), _3dRenderingMinDepth[i]->value());
 		settings.setValue(QString("showScans%1").arg(i), _3dRenderingShowScans[i]->isChecked());
 		settings.setValue(QString("showFeatures%1").arg(i), _3dRenderingShowFeatures[i]->isChecked());
 
@@ -2258,10 +2270,6 @@ void PreferencesDialog::showEvent ( QShowEvent * event )
 	if(_monitoringState)
 	{
 		// In monitoring state, you cannot change remote paths
-		_ui->lineEdit_workingDirectory->setEnabled(false);
-		_ui->toolButton_workingDirectory->setEnabled(false);
-		_ui->label_workingDirectory->setEnabled(false);
-
 		_ui->lineEdit_dictionaryPath->setEnabled(false);
 		_ui->toolButton_dictionaryPath->setEnabled(false);
 		_ui->label_dictionaryPath->setEnabled(false);
@@ -2276,10 +2284,6 @@ void PreferencesDialog::showEvent ( QShowEvent * event )
 	}
 	else
 	{
-		_ui->lineEdit_workingDirectory->setEnabled(true);
-		_ui->toolButton_workingDirectory->setEnabled(true);
-		_ui->label_workingDirectory->setEnabled(true);
-
 		_ui->lineEdit_dictionaryPath->setEnabled(true);
 		_ui->toolButton_dictionaryPath->setEnabled(true);
 		_ui->label_dictionaryPath->setEnabled(true);
@@ -2863,18 +2867,6 @@ void PreferencesDialog::setParameter(const std::string & key, const std::string
 						  combo->currentText().toStdString().c_str());
 					ok = false;
 				}
-				else if(valueInt==1 &&
-						(combo->objectName().toStdString().compare(Parameters::kKpNNStrategy()) == 0 ||
-						 combo->objectName().toStdString().compare(Parameters::kVisCorNNType()) == 0))
-
-				{
-					UWARN("Trying to set \"%s\" to KdTree but RTAB-Map isn't built "
-						  "with the nonfree module from OpenCV and kdTree cannot be used "
-						  "with binary descriptors. Keeping default combo value: %s.",
-						  combo->objectName().toStdString().c_str(),
-						  combo->currentText().toStdString().c_str());
-					ok = false;
-				}
 #endif
 				if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
 				{
@@ -3527,7 +3519,7 @@ bool PreferencesDialog::isCloudMeshing() const
 }
 double PreferencesDialog::getCloudMeshingAngle() const
 {
-	return _ui->doubleSpinBox_mesh_angleTolerance->value()*M_PI/180.0f;
+	return _ui->doubleSpinBox_mesh_angleTolerance->value()*M_PI/180.0;
 }
 bool PreferencesDialog::isCloudMeshingQuad() const
 {
@@ -3547,6 +3539,11 @@ double PreferencesDialog::getCloudMaxDepth(int index) const
 	UASSERT(index >= 0 && index <= 1);
 	return _3dRenderingMaxDepth[index]->value();
 }
+double PreferencesDialog::getCloudMinDepth(int index) const
+{
+	UASSERT(index >= 0 && index <= 1);
+	return _3dRenderingMinDepth[index]->value();
+}
 double PreferencesDialog::getCloudOpacity(int index) const
 {
 	UASSERT(index >= 0 && index <= 1);

guilib/src/ui/DatabaseViewer.ui

@@ -21,7 +21,16 @@
     <property name="spacing">
      <number>0</number>
     </property>
-    <property name="margin">
+    <property name="leftMargin">
+     <number>0</number>
+    </property>
+    <property name="topMargin">
+     <number>0</number>
+    </property>
+    <property name="rightMargin">
+     <number>0</number>
+    </property>
+    <property name="bottomMargin">
      <number>0</number>
     </property>
     <item>
@@ -53,36 +62,18 @@
            <x>0</x>
            <y>0</y>
            <width>205</width>
-           <height>184</height>
+           <height>196</height>
           </rect>
          </property>
-         <layout class="QGridLayout" name="gridLayout" rowstretch="0,0,0,0,0,0,0" columnstretch="1,1">
+         <layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
-          <item row="0" column="0">
-           <widget class="QLabel" name="label_parentsA_2">
-            <property name="text">
-             <string>Parents</string>
-            </property>
-           </widget>
-          </item>
-          <item row="0" column="1">
-           <widget class="QLabel" name="label_parentsA">
-            <property name="text">
-             <string/>
-            </property>
-           </widget>
-          </item>
-          <item row="1" column="0">
-           <widget class="QLabel" name="label_childrenA_2">
-            <property name="text">
-             <string>Children</string>
-            </property>
-           </widget>
-          </item>
           <item row="1" column="1">
            <widget class="QLabel" name="label_childrenA">
             <property name="text">
              <string/>
             </property>
+            <property name="textInteractionFlags">
+             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
            </widget>
           </item>
           <item row="3" column="0">
@@ -97,6 +88,9 @@
             <property name="text">
              <string/>
             </property>
+            <property name="textInteractionFlags">
+             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
            </widget>
           </item>
           <item row="6" column="0">
@@ -111,6 +105,9 @@
             <property name="text">
              <string/>
             </property>
+            <property name="textInteractionFlags">
+             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
            </widget>
           </item>
           <item row="2" column="0">
@@ -125,6 +122,9 @@
             <property name="text">
              <string/>
             </property>
+            <property name="textInteractionFlags">
+             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
            </widget>
           </item>
           <item row="4" column="0">
@@ -139,6 +139,9 @@
             <property name="text">
              <string/>
             </property>
+            <property name="textInteractionFlags">
+             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
            </widget>
           </item>
           <item row="5" column="0">
@@ -148,11 +151,55 @@
             </property>
            </widget>
           </item>
+          <item row="0" column="0">
+           <widget class="QLabel" name="label_parentsA_2">
+            <property name="text">
+             <string>Parents</string>
+            </property>
+           </widget>
+          </item>
+          <item row="0" column="1">
+           <widget class="QLabel" name="label_parentsA">
+            <property name="text">
+             <string/>
+            </property>
+            <property name="textInteractionFlags">
+             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
+           </widget>
+          </item>
+          <item row="1" column="0">
+           <widget class="QLabel" name="label_childrenA_2">
+            <property name="text">
+             <string>Children</string>
+            </property>
+           </widget>
+          </item>
           <item row="5" column="1">
            <widget class="QLabel" name="label_poseA">
             <property name="text">
              <string/>
             </property>
+            <property name="textInteractionFlags">
+             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
+           </widget>
+          </item>
+          <item row="7" column="0">
+           <widget class="QLabel" name="label_childrenA_14">
+            <property name="text">
+             <string>Calib</string>
+            </property>
+           </widget>
+          </item>
+          <item row="7" column="1">
+           <widget class="QLabel" name="label_calibA">
+            <property name="text">
+             <string/>
+            </property>
+            <property name="textInteractionFlags">
+             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
            </widget>
           </item>
          </layout>
@@ -172,8 +219,8 @@
           <rect>
            <x>0</x>
            <y>0</y>
-           <width>203</width>
+           <width>204</width>
-           <height>184</height>
+           <height>196</height>
           </rect>
          </property>
          <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1">
@@ -182,6 +229,9 @@
             <property name="text">
              <string/>
             </property>
+            <property name="textInteractionFlags">
+             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
            </widget>
           </item>
           <item row="0" column="0">
@@ -203,6 +253,9 @@
             <property name="text">
              <string/>
             </property>
+            <property name="textInteractionFlags">
+             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
            </widget>
           </item>
           <item row="3" column="0">
@@ -217,6 +270,9 @@
             <property name="text">
              <string/>
             </property>
+            <property name="textInteractionFlags">
+             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
            </widget>
           </item>
           <item row="6" column="0">
@@ -231,6 +287,9 @@
             <property name="text">
              <string/>
             </property>
+            <property name="textInteractionFlags">
+             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
            </widget>
           </item>
           <item row="2" column="0">
@@ -245,6 +304,9 @@
             <property name="text">
              <string/>
             </property>
+            <property name="textInteractionFlags">
+             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
            </widget>
           </item>
           <item row="4" column="0">
@@ -259,6 +321,9 @@
             <property name="text">
              <string/>
             </property>
+            <property name="textInteractionFlags">
+             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
            </widget>
           </item>
           <item row="5" column="0">
@@ -273,6 +338,26 @@
             <property name="text">
              <string/>
             </property>
+            <property name="textInteractionFlags">
+             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
+           </widget>
+          </item>
+          <item row="7" column="0">
+           <widget class="QLabel" name="label_childrenA_15">
+            <property name="text">
+             <string>Calib</string>
+            </property>
+           </widget>
+          </item>
+          <item row="7" column="1">
+           <widget class="QLabel" name="label_calibB">
+            <property name="text">
+             <string/>
+            </property>
+            <property name="textInteractionFlags">
+             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
            </widget>
           </item>
          </layout>
@@ -281,7 +366,16 @@
       </item>
       <item row="1" column="0">
        <layout class="QHBoxLayout" name="horizontalLayout">
-        <property name="margin">
+        <property name="leftMargin">
+         <number>12</number>
+        </property>
+        <property name="topMargin">
+         <number>12</number>
+        </property>
+        <property name="rightMargin">
+         <number>12</number>
+        </property>
+        <property name="bottomMargin">
          <number>12</number>
         </property>
         <item>
@@ -337,7 +431,16 @@
       </item>
       <item row="1" column="1">
        <layout class="QHBoxLayout" name="horizontalLayout_2">
-        <property name="margin">
+        <property name="leftMargin">
+         <number>12</number>
+        </property>
+        <property name="topMargin">
+         <number>12</number>
+        </property>
+        <property name="rightMargin">
+         <number>12</number>
+        </property>
+        <property name="bottomMargin">
          <number>12</number>
         </property>
         <item>
@@ -408,7 +511,7 @@
      <x>0</x>
      <y>0</y>
      <width>1285</width>
-     <height>22</height>
+     <height>25</height>
     </rect>
    </property>
    <widget class="QMenu" name="menuFile">
@@ -465,7 +568,11 @@
    <widget class="QWidget" name="dockWidgetContents">
     <layout class="QVBoxLayout" name="verticalLayout_8" stretch="1,0,0,0">
      <item>
-      <widget class="rtabmap::CloudViewer" name="constraintsViewer" native="true"/>
+      <layout class="QVBoxLayout" name="layout_constraintsViewer">
+       <property name="spacing">
+        <number>0</number>
+       </property>
+      </layout>
      </item>
      <item>
       <layout class="QHBoxLayout" name="horizontalLayout_10">
@@ -565,6 +672,9 @@
          <property name="text">
           <string/>
          </property>
+         <property name="textInteractionFlags">
+          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+         </property>
         </widget>
        </item>
        <item row="3" column="0">
@@ -579,6 +689,9 @@
          <property name="text">
           <string/>
          </property>
+         <property name="textInteractionFlags">
+          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+         </property>
         </widget>
        </item>
        <item row="4" column="0">
@@ -593,6 +706,9 @@
          <property name="text">
           <string/>
          </property>
+         <property name="textInteractionFlags">
+          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+         </property>
         </widget>
        </item>
        <item row="5" column="0">
@@ -607,6 +723,9 @@
          <property name="text">
           <string/>
          </property>
+         <property name="textInteractionFlags">
+          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+         </property>
         </widget>
        </item>
       </layout>
@@ -709,6 +828,9 @@
          <property name="text">
           <string/>
          </property>
+         <property name="textInteractionFlags">
+          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+         </property>
         </widget>
        </item>
        <item row="4" column="0">
@@ -716,6 +838,9 @@
          <property name="text">
           <string/>
          </property>
+         <property name="textInteractionFlags">
+          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+         </property>
         </widget>
        </item>
        <item row="3" column="0">
@@ -723,6 +848,9 @@
          <property name="text">
           <string/>
          </property>
+         <property name="textInteractionFlags">
+          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+         </property>
         </widget>
        </item>
        <item row="0" column="0">
@@ -768,6 +896,9 @@
          <property name="text">
           <string/>
          </property>
+         <property name="textInteractionFlags">
+          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+         </property>
         </widget>
        </item>
        <item row="0" column="1">
@@ -796,6 +927,9 @@
          <property name="text">
           <string/>
          </property>
+         <property name="textInteractionFlags">
+          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+         </property>
         </widget>
        </item>
       </layout>
@@ -873,15 +1007,15 @@
      <item>
       <widget class="QToolBox" name="toolBox">
        <property name="currentIndex">
-        <number>1</number>
+        <number>3</number>
        </property>
        <widget class="QWidget" name="page_3">
         <property name="geometry">
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>324</width>
+          <width>340</width>
-          <height>188</height>
+          <height>186</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1016,8 +1150,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>280</width>
+          <width>282</width>
-          <height>628</height>
+          <height>611</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1126,7 +1260,7 @@
                <double>0.400000000000000</double>
               </property>
               <property name="maximum">
-               <double>15.000000000000000</double>
+               <double>999.000000000000000</double>
               </property>
               <property name="singleStep">
                <double>1.000000000000000</double>
@@ -1143,21 +1277,21 @@
               </property>
              </widget>
             </item>
-            <item row="2" column="1">
+            <item row="3" column="1">
              <widget class="QLabel" name="label_66">
               <property name="text">
                <string>Cluster size</string>
               </property>
              </widget>
             </item>
-            <item row="3" column="1">
+            <item row="4" column="1">
              <widget class="QLabel" name="label_67">
               <property name="text">
                <string>Max ground angle</string>
               </property>
              </widget>
             </item>
-            <item row="3" column="0">
+            <item row="4" column="0">
              <widget class="QDoubleSpinBox" name="doubleSpinBox_projMaxAngle">
               <property name="suffix">
                <string> rad</string>
@@ -1179,7 +1313,7 @@
               </property>
              </widget>
             </item>
-            <item row="2" column="0">
+            <item row="3" column="0">
              <widget class="QSpinBox" name="spinBox_projClusterSize">
               <property name="minimum">
                <number>1</number>
@@ -1192,6 +1326,35 @@
               </property>
              </widget>
             </item>
+            <item row="2" column="1">
+             <widget class="QLabel" name="label_40">
+              <property name="text">
+               <string>Cloud min depth</string>
+              </property>
+             </widget>
+            </item>
+            <item row="2" column="0">
+             <widget class="QDoubleSpinBox" name="doubleSpinBox_projMinDepth">
+              <property name="suffix">
+               <string> m</string>
+              </property>
+              <property name="decimals">
+               <number>1</number>
+              </property>
+              <property name="minimum">
+               <double>0.000000000000000</double>
+              </property>
+              <property name="maximum">
+               <double>999.000000000000000</double>
+              </property>
+              <property name="singleStep">
+               <double>1.000000000000000</double>
+              </property>
+              <property name="value">
+               <double>0.000000000000000</double>
+              </property>
+             </widget>
+            </item>
            </layout>
           </widget>
          </item>
@@ -1398,8 +1561,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>201</width>
+          <width>205</width>
-          <height>126</height>
+          <height>117</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1498,8 +1661,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>173</width>
+          <width>267</width>
-          <height>166</height>
+          <height>157</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1553,7 +1716,7 @@
            </property>
           </widget>
          </item>
-         <item row="3" column="0">
+         <item row="4" column="0">
           <spacer name="verticalSpacer">
            <property name="orientation">
             <enum>Qt::Vertical</enum>
@@ -1566,6 +1729,19 @@
            </property>
           </spacer>
          </item>
+         <item row="3" column="2">
+          <widget class="QLabel" name="label_19">
+           <property name="text">
+            <string>Min depth</string>
+           </property>
+           <property name="wordWrap">
+            <bool>true</bool>
+           </property>
+          </widget>
+         </item>
+         <item row="3" column="0">
+          <widget class="QDoubleSpinBox" name="doubleSpinBox_icp_minDepth"/>
+         </item>
         </layout>
        </widget>
       </widget>
@@ -1580,22 +1756,28 @@
    <attribute name="dockWidgetArea">
     <number>4</number>
    </attribute>
-   <widget class="QWidget" name="dockWidgetContents_5">
+   <widget class="QWidget" name="dockWidgetContents_stereo">
     <layout class="QVBoxLayout" name="verticalLayout_11" stretch="1,0">
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="margin">
+     <property name="leftMargin">
+      <number>0</number>
+     </property>
+     <property name="topMargin">
+      <number>0</number>
+     </property>
+     <property name="rightMargin">
+      <number>0</number>
+     </property>
+     <property name="bottomMargin">
       <number>0</number>
      </property>
      <item>
-      <layout class="QHBoxLayout" name="horizontalLayout_9" stretch="1,1">
+      <layout class="QHBoxLayout" name="horizontalLayout_stereo" stretch="1">
        <item>
         <widget class="rtabmap::ImageView" name="graphicsView_stereo" native="true"/>
        </item>
-       <item>
-        <widget class="rtabmap::CloudViewer" name="stereoViewer" native="true"/>
-       </item>
       </layout>
      </item>
      <item>
@@ -1696,25 +1878,28 @@
    <attribute name="dockWidgetArea">
     <number>4</number>
    </attribute>
-   <widget class="QWidget" name="dockWidgetContents_6">
+   <widget class="QWidget" name="dockWidgetContents_3dviews">
     <layout class="QVBoxLayout" name="verticalLayout_5" stretch="1,0">
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="margin">
+     <property name="leftMargin">
+      <number>0</number>
+     </property>
+     <property name="topMargin">
+      <number>0</number>
+     </property>
+     <property name="rightMargin">
+      <number>0</number>
+     </property>
+     <property name="bottomMargin">
       <number>0</number>
      </property>
      <item>
-      <layout class="QHBoxLayout" name="horizontalLayout_3dviews" stretch="1,1">
+      <layout class="QHBoxLayout" name="horizontalLayout_3dviews">
        <property name="spacing">
         <number>1</number>
        </property>
-       <item>
-        <widget class="rtabmap::CloudViewer" name="widget_cloudA" native="true"/>
-       </item>
-       <item>
-        <widget class="rtabmap::CloudViewer" name="widget_cloudB" native="true"/>
-       </item>
       </layout>
      </item>
      <item>
@@ -1761,7 +1946,16 @@
    </attribute>
    <widget class="QWidget" name="dockWidgetContents_7">
     <layout class="QVBoxLayout" name="verticalLayout_13">
-     <property name="margin">
+     <property name="leftMargin">
+      <number>0</number>
+     </property>
+     <property name="topMargin">
+      <number>0</number>
+     </property>
+     <property name="rightMargin">
+      <number>0</number>
+     </property>
+     <property name="bottomMargin">
       <number>0</number>
      </property>
      <item>
@@ -1887,12 +2081,6 @@
   </action>
  </widget>
  <customwidgets>
-  <customwidget>
-   <class>rtabmap::CloudViewer</class>
-   <extends>QWidget</extends>
-   <header>rtabmap/gui/CloudViewer.h</header>
-   <container>1</container>
-  </customwidget>
   <customwidget>
    <class>rtabmap::ImageView</class>
    <extends>QWidget</extends>

guilib/src/ui/calibrationDialog.ui

@@ -232,6 +232,29 @@
             </property>
            </widget>
           </item>
+          <item row="4" column="1">
+           <widget class="QSpinBox" name="spinBox_maxScale">
+            <property name="minimum">
+             <number>1</number>
+            </property>
+            <property name="maximum">
+             <number>3</number>
+            </property>
+            <property name="value">
+             <number>1</number>
+            </property>
+           </widget>
+          </item>
+          <item row="4" column="0">
+           <widget class="QLabel" name="label_22">
+            <property name="toolTip">
+             <string>Number of inner squares on the board</string>
+            </property>
+            <property name="text">
+             <string>Max scale</string>
+            </property>
+           </widget>
+          </item>
          </layout>
         </widget>
        </item>

guilib/src/ui/exportCloudsDialog.ui

@@ -24,14 +24,14 @@
        <rect>
         <x>0</x>
         <y>0</y>
-        <width>773</width>
+        <width>778</width>
-        <height>1422</height>
+        <height>1318</height>
        </rect>
       </property>
       <layout class="QVBoxLayout" name="verticalLayout_13">
        <item>
         <layout class="QGridLayout" name="gridLayout_8" columnstretch="0,1">
-         <item row="1" column="1">
+         <item row="2" column="1">
           <widget class="QLabel" name="label_binaryFile_2">
            <property name="text">
             <string>Assemble clouds/meshes to a single output cloud/mesh.</string>
@@ -41,24 +41,7 @@
            </property>
           </widget>
          </item>
-         <item row="4" column="1">
+         <item row="3" column="0">
-          <widget class="QLabel" name="label_12">
-           <property name="text">
-            <string>Reconstruction flavor.</string>
-           </property>
-          </widget>
-         </item>
-         <item row="2" column="1">
-          <widget class="QLabel" name="label_111">
-           <property name="text">
-            <string>Set the number of k nearest neighbors to use for the normal estimation.</string>
-           </property>
-           <property name="wordWrap">
-            <bool>true</bool>
-           </property>
-          </widget>
-         </item>
-         <item row="2" column="0">
           <widget class="QSpinBox" name="spinBox_normalKSearch">
            <property name="minimum">
             <number>0</number>
@@ -68,7 +51,7 @@
            </property>
           </widget>
          </item>
-         <item row="0" column="1">
+         <item row="1" column="1">
           <widget class="QLabel" name="label_binaryFile">
            <property name="text">
             <string>Binary file.</string>
@@ -78,7 +61,7 @@
            </property>
           </widget>
          </item>
-         <item row="0" column="0">
+         <item row="1" column="0">
           <widget class="QCheckBox" name="checkBox_binary">
            <property name="text">
             <string/>
@@ -88,29 +71,15 @@
            </property>
           </widget>
          </item>
-         <item row="4" column="0">
+         <item row="2" column="0">
-          <widget class="QComboBox" name="comboBox_pipeline">
-           <item>
-            <property name="text">
-             <string>Organized Point Cloud</string>
-            </property>
-           </item>
-           <item>
-            <property name="text">
-             <string>Dense Point Cloud</string>
-            </property>
-           </item>
-          </widget>
-         </item>
-         <item row="1" column="0">
           <widget class="QCheckBox" name="checkBox_assemble">
            <property name="text">
             <string/>
            </property>
           </widget>
          </item>
-         <item row="3" column="1">
+         <item row="4" column="1">
-          <widget class="QLabel" name="label_109">
+          <widget class="QLabel" name="label_voxel">
            <property name="text">
             <string>Voxel size. Set 0 to disable. When organized meshes are assembled, this is the radius in which the vertices of the polygons are merged.</string>
            </property>
@@ -119,7 +88,7 @@
            </property>
           </widget>
          </item>
-         <item row="3" column="0">
+         <item row="4" column="0">
           <widget class="QDoubleSpinBox" name="doubleSpinBox_voxelSize_assembled">
            <property name="suffix">
             <string> m</string>
@@ -138,6 +107,37 @@
            </property>
           </widget>
          </item>
+         <item row="3" column="1">
+          <widget class="QLabel" name="label_normal">
+           <property name="text">
+            <string>Set the number of k nearest neighbors to use for the normal estimation.</string>
+           </property>
+           <property name="wordWrap">
+            <bool>true</bool>
+           </property>
+          </widget>
+         </item>
+         <item row="0" column="1">
+          <widget class="QLabel" name="label_12">
+           <property name="text">
+            <string>Reconstruction flavor.</string>
+           </property>
+          </widget>
+         </item>
+         <item row="0" column="0">
+          <widget class="QComboBox" name="comboBox_pipeline">
+           <item>
+            <property name="text">
+             <string>Organized Point Cloud</string>
+            </property>
+           </item>
+           <item>
+            <property name="text">
+             <string>Dense Point Cloud</string>
+            </property>
+           </item>
+          </widget>
+         </item>
         </layout>
        </item>
        <item>
@@ -203,6 +203,35 @@
               </property>
              </widget>
             </item>
+            <item row="2" column="1">
+             <widget class="QLabel" name="label_133">
+              <property name="text">
+               <string>3D cloud minimum depth.</string>
+              </property>
+              <property name="wordWrap">
+               <bool>true</bool>
+              </property>
+             </widget>
+            </item>
+            <item row="2" column="0">
+             <widget class="QDoubleSpinBox" name="doubleSpinBox_minDepth">
+              <property name="suffix">
+               <string> m</string>
+              </property>
+              <property name="decimals">
+               <number>1</number>
+              </property>
+              <property name="maximum">
+               <double>100.000000000000000</double>
+              </property>
+              <property name="singleStep">
+               <double>0.100000000000000</double>
+              </property>
+              <property name="value">
+               <double>0.000000000000000</double>
+              </property>
+             </widget>
+            </item>
            </layout>
           </item>
           <item>

guilib/src/ui/mainWindow.ui

@@ -285,7 +285,16 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="margin">
+     <property name="leftMargin">
+      <number>0</number>
+     </property>
+     <property name="topMargin">
+      <number>0</number>
+     </property>
+     <property name="rightMargin">
+      <number>0</number>
+     </property>
+     <property name="bottomMargin">
       <number>0</number>
      </property>
      <item>
@@ -323,7 +332,16 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="margin">
+     <property name="leftMargin">
+      <number>0</number>
+     </property>
+     <property name="topMargin">
+      <number>0</number>
+     </property>
+     <property name="rightMargin">
+      <number>0</number>
+     </property>
+     <property name="bottomMargin">
       <number>0</number>
      </property>
      <item>
@@ -566,7 +584,16 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="margin">
+     <property name="leftMargin">
+      <number>0</number>
+     </property>
+     <property name="topMargin">
+      <number>0</number>
+     </property>
+     <property name="rightMargin">
+      <number>0</number>
+     </property>
+     <property name="bottomMargin">
       <number>0</number>
      </property>
      <item>
@@ -587,7 +614,16 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="margin">
+     <property name="leftMargin">
+      <number>0</number>
+     </property>
+     <property name="topMargin">
+      <number>0</number>
+     </property>
+     <property name="rightMargin">
+      <number>0</number>
+     </property>
+     <property name="bottomMargin">
       <number>0</number>
      </property>
      <item>
@@ -608,7 +644,16 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="margin">
+     <property name="leftMargin">
+      <number>0</number>
+     </property>
+     <property name="topMargin">
+      <number>0</number>
+     </property>
+     <property name="rightMargin">
+      <number>0</number>
+     </property>
+     <property name="bottomMargin">
       <number>0</number>
      </property>
      <item>
@@ -624,17 +669,23 @@
    <attribute name="dockWidgetArea">
     <number>2</number>
    </attribute>
-   <widget class="QWidget" name="dockWidgetContents_5">
+   <widget class="QWidget" name="layout_cloudViewer">
     <layout class="QVBoxLayout" name="verticalLayout_3">
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="margin">
+     <property name="leftMargin">
+      <number>0</number>
+     </property>
+     <property name="topMargin">
+      <number>0</number>
+     </property>
+     <property name="rightMargin">
+      <number>0</number>
+     </property>
+     <property name="bottomMargin">
       <number>0</number>
      </property>
-     <item>
-      <widget class="rtabmap::CloudViewer" name="widget_cloudViewer" native="true"/>
-     </item>
     </layout>
    </widget>
   </widget>
@@ -645,17 +696,23 @@
    <attribute name="dockWidgetArea">
     <number>2</number>
    </attribute>
-   <widget class="QWidget" name="dockWidgetContents_7">
+   <widget class="QWidget" name="layout_loopClosureViewer">
     <layout class="QVBoxLayout" name="verticalLayout_7">
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="margin">
+     <property name="leftMargin">
+      <number>0</number>
+     </property>
+     <property name="topMargin">
+      <number>0</number>
+     </property>
+     <property name="rightMargin">
+      <number>0</number>
+     </property>
+     <property name="bottomMargin">
       <number>0</number>
      </property>
-     <item>
-      <widget class="rtabmap::LoopClosureViewer" name="widget_loopClosureViewer" native="true"/>
-     </item>
     </layout>
    </widget>
   </widget>
@@ -674,7 +731,16 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="margin">
+     <property name="leftMargin">
+      <number>0</number>
+     </property>
+     <property name="topMargin">
+      <number>0</number>
+     </property>
+     <property name="rightMargin">
+      <number>0</number>
+     </property>
+     <property name="bottomMargin">
       <number>0</number>
      </property>
      <item>
@@ -695,7 +761,16 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="margin">
+     <property name="leftMargin">
+      <number>0</number>
+     </property>
+     <property name="topMargin">
+      <number>0</number>
+     </property>
+     <property name="rightMargin">
+      <number>0</number>
+     </property>
+     <property name="bottomMargin">
       <number>0</number>
      </property>
      <item>
@@ -719,7 +794,16 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="margin">
+     <property name="leftMargin">
+      <number>0</number>
+     </property>
+     <property name="topMargin">
+      <number>0</number>
+     </property>
+     <property name="rightMargin">
+      <number>0</number>
+     </property>
+     <property name="bottomMargin">
       <number>0</number>
      </property>
      <item>
@@ -789,7 +873,16 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="margin">
+     <property name="leftMargin">
+      <number>0</number>
+     </property>
+     <property name="topMargin">
+      <number>0</number>
+     </property>
+     <property name="rightMargin">
+      <number>0</number>
+     </property>
+     <property name="bottomMargin">
       <number>0</number>
      </property>
      <item>
@@ -1323,18 +1416,6 @@
    <header>../include/rtabmap/gui/ConsoleWidget.h</header>
    <container>1</container>
   </customwidget>
-  <customwidget>
-   <class>rtabmap::CloudViewer</class>
-   <extends>QWidget</extends>
-   <header>../include/rtabmap/gui/CloudViewer.h</header>
-   <container>1</container>
-  </customwidget>
-  <customwidget>
-   <class>rtabmap::LoopClosureViewer</class>
-   <extends>QWidget</extends>
-   <header>../include/rtabmap/gui/LoopClosureViewer.h</header>
-   <container>1</container>
-  </customwidget>
   <customwidget>
    <class>rtabmap::MapVisibilityWidget</class>
    <extends>QWidget</extends>

guilib/src/ui/postProcessingDialog.ui

@@ -87,26 +87,6 @@
           </property>
          </widget>
         </item>
-        <item row="3" column="1">
-         <widget class="QLabel" name="label_5">
-          <property name="text">
-           <string>Re-extract features</string>
-          </property>
-          <property name="wordWrap">
-           <bool>true</bool>
-          </property>
-         </widget>
-        </item>
-        <item row="3" column="0">
-         <widget class="QCheckBox" name="reextractFeatures">
-          <property name="text">
-           <string/>
-          </property>
-          <property name="checked">
-           <bool>false</bool>
-          </property>
-         </widget>
-        </item>
         <item row="2" column="1">
          <widget class="QLabel" name="label_6">
           <property name="text">
@@ -201,7 +181,56 @@
      <layout class="QVBoxLayout" name="verticalLayout_4">
       <item>
        <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1">
-        <item row="0" column="1">
+        <item row="1" column="0">
+         <widget class="QSpinBox" name="sba_iterations">
+          <property name="minimum">
+           <number>1</number>
+          </property>
+          <property name="maximum">
+           <number>999999</number>
+          </property>
+          <property name="value">
+           <number>100</number>
+          </property>
+         </widget>
+        </item>
+        <item row="0" column="0">
+         <widget class="QComboBox" name="comboBox_sbaType">
+          <item>
+           <property name="text">
+            <string>g2o</string>
+           </property>
+          </item>
+          <item>
+           <property name="text">
+            <string>cvsba</string>
+           </property>
+          </item>
+         </widget>
+        </item>
+        <item row="2" column="0">
+         <widget class="QDoubleSpinBox" name="sba_epsilon">
+          <property name="suffix">
+           <string/>
+          </property>
+          <property name="decimals">
+           <number>4</number>
+          </property>
+          <property name="minimum">
+           <double>0.000000000000000</double>
+          </property>
+          <property name="maximum">
+           <double>1.000000000000000</double>
+          </property>
+          <property name="singleStep">
+           <double>0.000100000000000</double>
+          </property>
+          <property name="value">
+           <double>0.000100000000000</double>
+          </property>
+         </widget>
+        </item>
+        <item row="1" column="1">
          <widget class="QLabel" name="label_8">
           <property name="text">
            <string>Iterations</string>
@@ -212,74 +241,6 @@
          </widget>
         </item>
         <item row="2" column="1">
-         <widget class="QLabel" name="label_10">
-          <property name="text">
-           <string>Inlier distance of corresponding 3D words</string>
-          </property>
-          <property name="wordWrap">
-           <bool>true</bool>
-          </property>
-         </widget>
-        </item>
-        <item row="2" column="0">
-         <widget class="QDoubleSpinBox" name="sba_minInlierDistance">
-          <property name="suffix">
-           <string> m</string>
-          </property>
-          <property name="decimals">
-           <number>3</number>
-          </property>
-          <property name="minimum">
-           <double>0.001000000000000</double>
-          </property>
-          <property name="maximum">
-           <double>10.000000000000000</double>
-          </property>
-          <property name="singleStep">
-           <double>0.010000000000000</double>
-          </property>
-          <property name="value">
-           <double>0.020000000000000</double>
-          </property>
-         </widget>
-        </item>
-        <item row="3" column="1">
-         <widget class="QLabel" name="label_11">
-          <property name="text">
-           <string>Minimum inliers</string>
-          </property>
-          <property name="wordWrap">
-           <bool>true</bool>
-          </property>
-         </widget>
-        </item>
-        <item row="3" column="0">
-         <widget class="QSpinBox" name="sba_minInliers">
-          <property name="minimum">
-           <number>1</number>
-          </property>
-          <property name="maximum">
-           <number>1000</number>
-          </property>
-          <property name="value">
-           <number>10</number>
-          </property>
-         </widget>
-        </item>
-        <item row="0" column="0">
-         <widget class="QSpinBox" name="sba_iterations">
-          <property name="minimum">
-           <number>1</number>
-          </property>
-          <property name="maximum">
-           <number>1000</number>
-          </property>
-          <property name="value">
-           <number>100</number>
-          </property>
-         </widget>
-        </item>
-        <item row="1" column="1">
          <widget class="QLabel" name="label_9">
           <property name="text">
            <string>Stop optimizing when the error improvement is less than this value.</string>
@@ -289,8 +250,28 @@
           </property>
          </widget>
         </item>
-        <item row="1" column="0">
+        <item row="0" column="1">
-         <widget class="QDoubleSpinBox" name="sba_epsilon">
+         <widget class="QLabel" name="label_10">
+          <property name="text">
+           <string>Type</string>
+          </property>
+          <property name="wordWrap">
+           <bool>true</bool>
+          </property>
+         </widget>
+        </item>
+        <item row="3" column="1">
+         <widget class="QLabel" name="label_variance">
+          <property name="text">
+           <string>Pixel variance used by g2o.</string>
+          </property>
+          <property name="wordWrap">
+           <bool>true</bool>
+          </property>
+         </widget>
+        </item>
+        <item row="3" column="0">
+         <widget class="QDoubleSpinBox" name="sba_variance">
           <property name="suffix">
            <string/>
           </property>
@@ -298,16 +279,16 @@
            <number>4</number>
           </property>
           <property name="minimum">
-           <double>0.000100000000000</double>
+           <double>0.000000000000000</double>
           </property>
           <property name="maximum">
-           <double>1.000000000000000</double>
+           <double>10.000000000000000</double>
           </property>
           <property name="singleStep">
-           <double>0.000100000000000</double>
+           <double>0.010000000000000</double>
           </property>
           <property name="value">
-           <double>0.000100000000000</double>
+           <double>1.000000000000000</double>
           </property>
          </widget>
         </item>

guilib/src/ui/preferencesDialog.ui

@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-1161</y>
+            <y>-466</y>
-            <width>681</width>
+            <width>686</width>
-            <height>2010</height>
+            <height>2023</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>3</number>
+                <number>1</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -773,7 +773,17 @@ Show a yellow background when the number of odometry inliers goes under this thr
                        <enum>QFrame::Raised</enum>
                       </property>
                       <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,0,1">
-                       <item row="11" column="0">
+                       <item row="9" column="0">
+                        <widget class="QCheckBox" name="checkBox_showScans">
+                         <property name="text">
+                          <string/>
+                         </property>
+                         <property name="checked">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="12" column="0">
                         <widget class="QDoubleSpinBox" name="doubleSpinBox_opacity_scan">
                          <property name="suffix">
                           <string/>
@@ -792,7 +802,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="8" column="2">
+                       <item row="9" column="2">
                         <widget class="QLabel" name="label_110">
                          <property name="text">
                           <string>Show scans.</string>
@@ -818,7 +828,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="13" column="1">
+                       <item row="14" column="1">
                         <widget class="QCheckBox" name="checkBox_showOdomFeatures">
                          <property name="text">
                           <string/>
@@ -828,7 +838,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="13" column="0">
+                       <item row="14" column="0">
                         <widget class="QCheckBox" name="checkBox_showFeatures">
                          <property name="text">
                           <string/>
@@ -902,7 +912,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="6" column="0">
+                       <item row="7" column="0">
                         <widget class="QSpinBox" name="spinBox_ptsize">
                          <property name="minimum">
                           <number>1</number>
@@ -915,7 +925,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="11" column="1">
+                       <item row="12" column="1">
                         <widget class="QDoubleSpinBox" name="doubleSpinBox_opacity_odom_scan">
                          <property name="suffix">
                           <string/>
@@ -934,16 +944,6 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="8" column="0">
-                        <widget class="QCheckBox" name="checkBox_showScans">
-                         <property name="text">
-                          <string/>
-                         </property>
-                         <property name="checked">
-                          <bool>true</bool>
-                         </property>
-                        </widget>
-                       </item>
                        <item row="3" column="2">
                         <widget class="QLabel" name="label_108">
                          <property name="text">
@@ -970,7 +970,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="5" column="2">
+                       <item row="6" column="2">
                         <widget class="QLabel" name="label_155">
                          <property name="text">
                           <string>3D cloud opacity.</string>
@@ -983,7 +983,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="6" column="1">
+                       <item row="7" column="1">
                         <widget class="QSpinBox" name="spinBox_ptsize_odom">
                          <property name="minimum">
                           <number>1</number>
@@ -996,7 +996,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="12" column="1">
+                       <item row="13" column="1">
                         <widget class="QSpinBox" name="spinBox_ptsize_odom_scan">
                          <property name="minimum">
                           <number>1</number>
@@ -1006,7 +1006,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="12" column="2">
+                       <item row="13" column="2">
                         <widget class="QLabel" name="label_158">
                          <property name="text">
                           <string>Scan point size (1..64).</string>
@@ -1019,7 +1019,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="10" column="0">
+                       <item row="11" column="0">
                         <widget class="QDoubleSpinBox" name="doubleSpinBox_voxelSizeScan">
                          <property name="suffix">
                           <string> m</string>
@@ -1038,7 +1038,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="10" column="2">
+                       <item row="11" column="2">
                         <widget class="QLabel" name="label_271">
                          <property name="text">
                           <string>Scan voxel size.</string>
@@ -1051,7 +1051,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="9" column="1">
+                       <item row="10" column="1">
                         <widget class="QSpinBox" name="spinBox_downsamplingScan_odom">
                          <property name="minimum">
                           <number>1</number>
@@ -1061,7 +1061,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="11" column="2">
+                       <item row="12" column="2">
                         <widget class="QLabel" name="label_156">
                          <property name="text">
                           <string>Scan opacity.</string>
@@ -1097,7 +1097,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="5" column="0">
+                       <item row="6" column="0">
                         <widget class="QDoubleSpinBox" name="doubleSpinBox_opacity">
                          <property name="suffix">
                           <string/>
@@ -1116,7 +1116,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="8" column="1">
+                       <item row="9" column="1">
                         <widget class="QCheckBox" name="checkBox_showOdomScans">
                          <property name="text">
                           <string/>
@@ -1126,7 +1126,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="15" column="2">
+                       <item row="16" column="2">
                         <widget class="QLabel" name="label_213">
                          <property name="text">
                           <string>Show graphs.</string>
@@ -1139,7 +1139,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="15" column="0">
+                       <item row="16" column="0">
                         <widget class="QCheckBox" name="checkBox_showGraphs">
                          <property name="text">
                           <string/>
@@ -1149,7 +1149,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="5" column="1">
+                       <item row="6" column="1">
                         <widget class="QDoubleSpinBox" name="doubleSpinBox_opacity_odom">
                          <property name="suffix">
                           <string/>
@@ -1168,7 +1168,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="12" column="0">
+                       <item row="13" column="0">
                         <widget class="QSpinBox" name="spinBox_ptsize_scan">
                          <property name="minimum">
                           <number>1</number>
@@ -1188,7 +1188,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="16" column="2">
+                       <item row="17" column="2">
                         <widget class="QLabel" name="label_243">
                          <property name="text">
                           <string>Show labels.</string>
@@ -1201,7 +1201,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="16" column="0">
+                       <item row="17" column="0">
                         <widget class="QCheckBox" name="checkBox_showLabels">
                          <property name="text">
                           <string/>
@@ -1211,7 +1211,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="10" column="1">
+                       <item row="11" column="1">
                         <widget class="QDoubleSpinBox" name="doubleSpinBox_voxelSizeScan_odom">
                          <property name="suffix">
                           <string> m</string>
@@ -1230,7 +1230,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="9" column="2">
+                       <item row="10" column="2">
                         <widget class="QLabel" name="label_273">
                          <property name="text">
                           <string>Scan downsampling step size.</string>
@@ -1243,7 +1243,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="9" column="0">
+                       <item row="10" column="0">
                         <widget class="QSpinBox" name="spinBox_downsamplingScan">
                          <property name="minimum">
                           <number>1</number>
@@ -1253,7 +1253,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="6" column="2">
+                       <item row="7" column="2">
                         <widget class="QLabel" name="label_157">
                          <property name="text">
                           <string>3D cloud point size (1..64).</string>
@@ -1279,7 +1279,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="13" column="2">
+                       <item row="14" column="2">
                         <widget class="QLabel" name="label_123">
                          <property name="text">
                           <string>Show 3D features.</string>
@@ -1292,7 +1292,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="14" column="2">
+                       <item row="15" column="2">
                         <widget class="QLabel" name="label_166">
                          <property name="text">
                           <string>Feature point size.</string>
@@ -1305,7 +1305,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="14" column="1">
+                       <item row="15" column="1">
                         <widget class="QSpinBox" name="spinBox_ptsize_odom_features">
                          <property name="minimum">
                           <number>1</number>
@@ -1315,7 +1315,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="14" column="0">
+                       <item row="15" column="0">
                         <widget class="QSpinBox" name="spinBox_ptsize_features">
                          <property name="minimum">
                           <number>1</number>
@@ -1325,6 +1325,60 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
+                       <item row="5" column="2">
+                        <widget class="QLabel" name="label_150">
+                         <property name="text">
+                          <string>3D cloud minimum depth.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="5" column="0">
+                        <widget class="QDoubleSpinBox" name="doubleSpinBox_minDepth">
+                         <property name="suffix">
+                          <string> m</string>
+                         </property>
+                         <property name="decimals">
+                          <number>1</number>
+                         </property>
+                         <property name="maximum">
+                          <double>100.000000000000000</double>
+                         </property>
+                         <property name="singleStep">
+                          <double>0.100000000000000</double>
+                         </property>
+                         <property name="value">
+                          <double>0.000000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="5" column="1">
+                        <widget class="QDoubleSpinBox" name="doubleSpinBox_minDepth_odom">
+                         <property name="suffix">
+                          <string> m</string>
+                         </property>
+                         <property name="decimals">
+                          <number>1</number>
+                         </property>
+                         <property name="minimum">
+                          <double>0.000000000000000</double>
+                         </property>
+                         <property name="maximum">
+                          <double>100.000000000000000</double>
+                         </property>
+                         <property name="singleStep">
+                          <double>0.100000000000000</double>
+                         </property>
+                         <property name="value">
+                          <double>0.000000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
                       </layout>
                      </widget>
                     </item>
@@ -4662,6 +4716,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
+                      <item row="13" column="0">
+                       <widget class="QSpinBox" name="spinBox_imagePostDecimation">
+                        <property name="minimum">
+                         <number>1</number>
+                        </property>
+                        <property name="maximum">
+                         <number>16</number>
+                        </property>
+                       </widget>
+                      </item>
                       <item row="7" column="1">
                        <widget class="QLabel" name="label_retrieved_10">
                         <property name="text">
@@ -4802,10 +4866,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="12" column="1">
+                      <item row="13" column="1">
                        <widget class="QLabel" name="label_retrieved_6">
                         <property name="text">
-                         <string>Image decimation. This feature can be used to save images in lower resolution (size/decimation).</string>
+                         <string>Image post decimation. This option can be used to save images in lower resolution (size/decimation). It is done on the original image.</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
@@ -4828,7 +4892,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="13" column="1">
+                      <item row="14" column="1">
                        <widget class="QLabel" name="label_retrieved_8">
                         <property name="text">
                          <string>If &gt; 1, downsample the laser scans when creating a location. This feature can be used to save laser scans already downsampled.</string>
@@ -4874,7 +4938,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="13" column="0">
+                      <item row="14" column="0">
                        <widget class="QSpinBox" name="general_spinBox_laserScanDownsample">
                         <property name="minimumSize">
                          <size>
@@ -4890,16 +4954,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="12" column="0">
-                       <widget class="QSpinBox" name="spinBox_imageDecimation">
-                        <property name="minimum">
-                         <number>1</number>
-                        </property>
-                        <property name="maximum">
-                         <number>16</number>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="11" column="0">
                        <widget class="QCheckBox" name="general_checkBox_saveDepth16bits">
                         <property name="text">
@@ -4933,6 +4987,29 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
+                      <item row="12" column="0">
+                       <widget class="QSpinBox" name="spinBox_imagePreDecimation">
+                        <property name="minimum">
+                         <number>1</number>
+                        </property>
+                        <property name="maximum">
+                         <number>16</number>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="12" column="1">
+                       <widget class="QLabel" name="label_retrieved_13">
+                        <property name="text">
+                         <string>Image pre decimation. This option can be used to reduce image size before features extraction.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
                      </layout>
                     </item>
                     <item>
@@ -7060,6 +7137,35 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
+                       <item row="2" column="0">
+                        <widget class="QDoubleSpinBox" name="doubleSpinBox_g2o_variance">
+                         <property name="enabled">
+                          <bool>false</bool>
+                         </property>
+                         <property name="decimals">
+                          <number>4</number>
+                         </property>
+                         <property name="minimum">
+                          <double>0.000100000000000</double>
+                         </property>
+                         <property name="maximum">
+                          <double>10.000000000000000</double>
+                         </property>
+                         <property name="value">
+                          <double>1.000000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="2" column="1">
+                        <widget class="QLabel" name="label_148">
+                         <property name="enabled">
+                          <bool>false</bool>
+                         </property>
+                         <property name="text">
+                          <string>Pixel variance for SBA.</string>
+                         </property>
+                        </widget>
+                       </item>
                       </layout>
                      </widget>
                     </item>
@@ -7262,14 +7368,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                     </item>
                     <item>
                      <layout class="QGridLayout" name="gridLayout_27" columnstretch="0,1">
-                      <item row="6" column="0">
+                      <item row="8" column="0">
-                       <widget class="QCheckBox" name="odom_flow_guessMotion">
-                        <property name="text">
-                         <string/>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="7" column="0">
                        <widget class="QDoubleSpinBox" name="odom_flow_keyframeThr">
                         <property name="maximum">
                          <double>1.000000000000000</double>
@@ -7282,6 +7381,13 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
+                      <item row="6" column="0">
+                       <widget class="QCheckBox" name="odom_flow_guessMotion">
+                        <property name="text">
+                         <string/>
+                        </property>
+                       </widget>
+                      </item>
                       <item row="4" column="1">
                        <widget class="QLabel" name="label_233">
                         <property name="text">
@@ -7322,7 +7428,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="9" column="0">
+                      <item row="10" column="0">
                        <widget class="QSpinBox" name="odom_dataBufferSize">
                         <property name="maximum">
                          <number>999999</number>
@@ -7359,7 +7465,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </item>
                        </widget>
                       </item>
-                      <item row="10" column="0">
+                      <item row="11" column="0">
                        <widget class="QPushButton" name="pushButton_testOdometry">
                         <property name="text">
                          <string>Test selected odometry</string>
@@ -7379,7 +7485,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="9" column="1">
+                      <item row="10" column="1">
                        <widget class="QLabel" name="label_232">
                         <property name="text">
                          <string>Data buffer size (0 means inf).</string>
@@ -7447,7 +7553,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="7" column="1">
+                      <item row="8" column="1">
                        <widget class="QLabel" name="label_196">
                         <property name="text">
                          <string>[Visual] Create a new keyframe when the number of inliers drops under this threshold. Setting value to 0 means that a keyframe is created for each processed frame.</string>
@@ -7460,7 +7566,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="8" column="1">
+                      <item row="9" column="1">
                        <widget class="QLabel" name="label_246">
                         <property name="text">
                          <string>[Geometry] Create a new keyframe when the number of inliers drops under this threshold. Setting value to 0 means that a keyframe is created for each processed frame.</string>
@@ -7473,7 +7579,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="8" column="0">
+                      <item row="9" column="0">
                        <widget class="QDoubleSpinBox" name="odom_flow_scanKeyframeThr">
                         <property name="maximum">
                          <double>1.000000000000000</double>
@@ -7486,6 +7592,26 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
+                      <item row="7" column="1">
+                       <widget class="QLabel" name="label_248">
+                        <property name="text">
+                         <string>Decimation of images before features extraction.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="7" column="0">
+                       <widget class="QSpinBox" name="odom_imageDecimation">
+                        <property name="minimum">
+                         <number>1</number>
+                        </property>
+                       </widget>
+                      </item>
                      </layout>
                     </item>
                     <item>

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rtabmap</name>
-  <version>0.10.11</version>
+  <version>0.11.5</version>
   <description>RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="[email protected]">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>
@@ -12,24 +12,26 @@
 
   <buildtool_depend>cmake</buildtool_depend>
   
+  <build_depend>qt_gui_cpp</build_depend> <!-- libqt4-dev or libqt5-dev -->
   <build_depend>libpcl-all-dev</build_depend>
-  <build_depend>libqt4-dev</build_depend>
-  <build_depend>libvtk-qt</build_depend>
   <build_depend>libsqlite3-dev</build_depend>
   <build_depend>zlib</build_depend>
-  <build_depend>libfreenect</build_depend>
+  <build_depend>libfreenect-dev</build_depend>
   <build_depend>libopenni-dev</build_depend>
-  <build_depend>libopenni2-dev</build_depend>
+  <build_depend>libvtk-qt</build_depend>
+  <!--<build_depend>libopenni2-dev</build_depend> --> <!-- not available on Jessie -->
   <build_depend>cv_bridge</build_depend>
+  <!-- libproj-dev needed due to error in vtk6 (kinetic)-->
+  <build_depend>proj</build_depend>
   
+  <run_depend>qt_gui_cpp</run_depend>
   <run_depend>libpcl-all-dev</run_depend>
-  <run_depend>libqt4-dev</run_depend>
-  <run_depend>libvtk-qt</run_depend>
   <run_depend>libsqlite3-dev</run_depend>
   <run_depend>zlib</run_depend>
-  <run_depend>libfreenect</run_depend>
+  <run_depend>libfreenect-dev</run_depend>
   <run_depend>libopenni-dev</run_depend>
-  <run_depend>libopenni2-dev</run_depend>
+  <run_depend>libvtk-qt</run_depend>
+  <!-- <run_depend>libopenni2-dev</run_depend> -->
   <run_depend>cv_bridge</run_depend>
 
   <export>