UCv2Qt: fixed min/max of depth to grayscale conversion for CV_32FC1 type

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

.travis.yml

@@ -18,12 +18,14 @@ install:
     - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list'
     - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
     - sudo apt-get update
-    - sudo apt-get -y install libpcl-1.7-all libfreenect-dev
+    - sudo apt-get -y install libpcl-1.7-all libfreenect-dev ros-indigo-libg2o ros-indigo-octomap libopenni2-dev
 
 script: 
+    - source /opt/ros/indigo/setup.bash
     - mkdir -p build && cd build
     - cmake .. 
     - make
 
 notifications:
-  email: false
+  email:
+    - matlabbe@gmail.com

CMakeLists.txt

@@ -6,9 +6,10 @@ SET(PROJECT_PREFIX rtabmap)
 
 # Catkin doesn't support multiarch library path, 
 # fix to "lib" if not set by user.
-#IF(NOT DEFINED CMAKE_INSTALL_LIBDIR)
+OPTION(MULTI_ARCH "Activate multi-arch lib directory (debian)" OFF)
-#   set(CMAKE_INSTALL_LIBDIR "lib")
+IF(NOT MULTI_ARCH AND NOT DEFINED CMAKE_INSTALL_LIBDIR)
-#ENDIF(NOT DEFINED CMAKE_INSTALL_LIBDIR)
+   set(CMAKE_INSTALL_LIBDIR "lib")
+ENDIF(NOT MULTI_ARCH AND NOT DEFINED CMAKE_INSTALL_LIBDIR)
 
 INCLUDE(GNUInstallDirs)
 
@@ -20,7 +21,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 11)
-SET(RTABMAP_PATCH_VERSION 7)
+SET(RTABMAP_PATCH_VERSION 13)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -52,6 +53,13 @@ ELSE ()
 	ADD_DEFINITIONS("-Wno-unknown-pragmas")
 ENDIF()
 
+if(POLICY CMP0020)
+	cmake_policy(SET CMP0020 OLD)
+endif()
+if(POLICY CMP0043)
+	cmake_policy(SET CMP0043 OLD)
+endif()
+
 IF(MINGW)
     # Hide the --enable-auto-import warning
 	SET(CMAKE_EXE_LINKER_FLAGS "-Wl,--enable-auto-import")  
@@ -124,8 +132,11 @@ OPTION(BUILD_TOOLS "Build tools" ON)
 OPTION(BUILD_EXAMPLES "Build examples" ON)
 
 ####### DEPENDENCIES #######
-
+IF(ANDROID)
+option(WITH_QT            "Include Qt support"                   OFF)
+ELSE()
 option(WITH_QT            "Include Qt support"                   ON)
+ENDIF()
 option(WITH_FREENECT      "Include Freenect support"             ON)
 option(WITH_FREENECT2     "Include Freenect2 support"            ON)
 option(WITH_OPENNI2       "Include OpenNI2 support"              ON)
@@ -137,6 +148,9 @@ option(WITH_VERTIGO       "Include Vertigo support"              ON)
 option(WITH_CVSBA         "Include cvsba support"                ON)
 option(WITH_FLYCAPTURE2   "Include FlyCapture2/Triclops support" ON)
 option(WITH_ZED           "Include ZED sdk support"              ON)
+option(WITH_REALSENSE     "Include RealSense support"            ON)
+option(WITH_OCTOMAP       "Include Octomap support"              ON)
+option(PCL_OMP            "With PCL OMP implementations"         ON)
 
 FIND_PACKAGE(OpenCV REQUIRED QUIET)
 FIND_PACKAGE(PCL 1.7 REQUIRED QUIET)
@@ -156,6 +170,9 @@ if(OPENMP_FOUND)
   set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
   message (STATUS "Found OpenMP")
+  if(PCL_OMP)
+    add_definitions(-DPCL_OMP)
+  endif(PCL_OMP)
 else(OPENMP_FOUND)
   message (STATUS "Not found OpenMP")
 endif()
@@ -170,6 +187,7 @@ IF(ZLIB_FOUND)
    MESSAGE(STATUS "Found ZLIB: ${ZLIB_INCLUDE_DIRS}")
 ENDIF(ZLIB_FOUND)
 
+SET(ADD_VTK_GUI_SUPPORT_QT_TO_CONF FALSE)
 IF(WITH_QT)
     FIND_PACKAGE(VTK)
     IF(NOT VTK_FOUND)
@@ -179,7 +197,10 @@ IF(WITH_QT)
     # If Qt is here, the GUI will be built
     # look for Qt5 (if vtk>5 is installed) before Qt4
     IF("${VTK_MAJOR_VERSION}" GREATER 5)
-        FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui Svg QUIET)
+        FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui QUIET)
+        IF(Qt5_FOUND)
+             FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui Svg)
+        ENDIF(Qt5_FOUND)
     ENDIF("${VTK_MAJOR_VERSION}" GREATER 5)
 
     IF(NOT Qt5_FOUND)
@@ -189,7 +210,13 @@ IF(WITH_QT)
     IF(QT4_FOUND OR Qt5_FOUND)
         IF("${VTK_MAJOR_VERSION}" EQUAL 5)
             FIND_PACKAGE(QVTK REQUIRED) # only for VTK 5
-        ENDIF("${VTK_MAJOR_VERSION}" EQUAL 5)
+        ELSE()
+            list(FIND PCL_LIBRARIES vtkGUISupportQt value)
+            IF(value EQUAL -1)
+                SET(PCL_LIBRARIES "${PCL_LIBRARIES};vtkGUISupportQt")
+                SET(ADD_VTK_GUI_SUPPORT_QT_TO_CONF TRUE)
+            ENDIF(value EQUAL -1)
+        ENDIF()
     ENDIF(QT4_FOUND OR Qt5_FOUND)
 ENDIF(WITH_QT)
 
@@ -207,6 +234,14 @@ IF(WITH_FREENECT2)
 	      SET(freenect2_INCLUDE_DIRS ${freenect2_INCLUDE_DIR})
 	   ENDIF(NOT freenect2_INCLUDE_DIRS)
        MESSAGE(STATUS "Found freenect2: ${freenect2_INCLUDE_DIRS}")
+       # Explicitly link to OpenCL (in case of CUDA installed)
+	   FIND_PACKAGE(OpenCL QUIET)
+	   IF(OpenCL_FOUND)
+		  SET(freenect2_LIBRARIES
+			${OpenCL_LIBRARIES}
+			${freenect2_LIBRARIES}
+		  )
+	   ENDIF(OpenCL_FOUND)
     ENDIF(freenect2_FOUND)
 ENDIF(WITH_FREENECT2)
 
@@ -264,7 +299,7 @@ IF(WITH_ZED)
 			LINK_DIRECTORIES( ${LINK_DIRECTORIES} ${ZED_LIBRARY_DIR})
 		ENDIF(ZED_LIBRARIES AND ZED_INCLUDE_DIRS)
 	ELSE() # Linux
-		find_package(ZED 0.9 QUIET)
+		find_package(ZED 1 QUIET)
 	ENDIF(WIN32)
 	
 	IF(ZED_FOUND)
@@ -279,7 +314,21 @@ IF(WITH_ZED)
     ENDIF(ZED_FOUND)
 ENDIF(WITH_ZED)
 
-IF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND)
+IF(WITH_REALSENSE)
+	FIND_PACKAGE(RealSense QUIET)
+    IF(RealSense_FOUND)
+       MESSAGE(STATUS "Found RealSense: ${RealSense_INCLUDE_DIRS}")
+    ENDIF(RealSense_FOUND)
+ENDIF(WITH_REALSENSE)
+
+IF(WITH_OCTOMAP)
+	FIND_PACKAGE(OCTOMAP QUIET)
+    IF(OCTOMAP_FOUND)
+       MESSAGE(STATUS "Found octomap: ${OCTOMAP_INCLUDE_DIRS}")
+    ENDIF(OCTOMAP_FOUND)
+ENDIF(WITH_OCTOMAP)
+
+IF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND)
    #Newest versions require std11
    IF(NOT MSVC)
      include(CheckCXXCompilerFlag)
@@ -293,7 +342,7 @@ IF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND)
 		message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler if you want to use g2o or gtsam (set \"-DWITH_G2O=OFF -DWITH_GTSAM=OFF\" to build without g2o and gtsam).")
 	  ENDIF()
    ENDIF()
-ENDIF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND)
+ENDIF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND)
 
 ####### OSX BUNDLE CMAKE_INSTALL_PREFIX #######
 IF(APPLE AND BUILD_AS_BUNDLE)
@@ -383,6 +432,22 @@ IF(NOT ZED_FOUND)
 ELSE()
    SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${ZED_LIBRARIES} ${CUDA_LIBRARIES}) 
 ENDIF()
+IF(NOT RealSense_FOUND)
+   SET(REALSENSE "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${RealSense_LIBRARIES}) 
+ENDIF()
+IF(NOT OCTOMAP_FOUND)
+   SET(OCTOMAP "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${OCTOMAP_LIBRARIES}) 
+ENDIF()
+IF(ADD_VTK_GUI_SUPPORT_QT_TO_CONF)
+   SET(CONF_VTK_QT true)
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} vtkGUISupportQt)
+ELSE()
+   SET(CONF_VTK_QT false)
+ENDIF()
 IF(NOT (OpenCV_FOUND AND OpenCV_VERSION_MAJOR EQUAL 3))
    SET(OPENCV3 "//")
 ENDIF(NOT (OpenCV_FOUND AND OpenCV_VERSION_MAJOR EQUAL 3))
@@ -669,6 +734,22 @@ ELSE()
 MESSAGE(STATUS "  With ZED                  = NO (ZED sdk not found)")
 ENDIF()
 
+IF(RealSense_FOUND)
+MESSAGE(STATUS "  With RealSense            = YES (License: Apache-2)")
+ELSEIF(NOT WITH_REALSENSE)
+MESSAGE(STATUS "  With RealSense            = NO (WITH_REALSENSE=OFF)")
+ELSE()
+MESSAGE(STATUS "  With RealSense            = NO (librealsense not found)")
+ENDIF()
+
+IF(OCTOMAP_FOUND)
+MESSAGE(STATUS "  With OCTOMAP              = YES (License: BSD)")
+ELSEIF(NOT WITH_OCTOMAP)
+MESSAGE(STATUS "  With OCTOMAP              = NO (WITH_OCTOMAP=OFF)")
+ELSE()
+MESSAGE(STATUS "  With OCTOMAP              = NO (octomap not found)")
+ENDIF()
+
 IF(QT4_FOUND)
 MESSAGE(STATUS "  With Qt4                  = YES (License: Open Source or Commercial)")
 ELSEIF(Qt5_FOUND)

LICENSE

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

Matlab/ShowLogs/showlogs.m

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

RTABMapConfig.cmake.in

@@ -62,6 +62,9 @@ if(@CONF_WITH_GUI@)
 endif(@CONF_WITH_GUI@)
 
 # Dependencies
+if(@CONF_VTK_QT@)
+   find_package(VTK COMPONENTS vtkGUISupportQt NO_MODULE) # to define vtkGUISupportQt target
+endif(@CONF_VTK_QT@)
 set(RTABMap_LIBRARIES ${RTABMap_LIBRARIES} @CONF_DEPENDENCIES@)
 
 #backward compatibilities

Version.h.in

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

app/android/AndroidManifest.xml.in

@@ -2,7 +2,7 @@
 <!-- BEGIN_INCLUDE(manifest) -->
 <manifest xmlns:android="http://schemas.android.com/apk/res/android"
         package="com.introlab.rtabmap"
-        android:versionCode="9"
+        android:versionCode="29"
         android:versionName="@RTABMAP_VERSION@">
 
     <uses-permission android:name="android.permission.CAMERA" />
@@ -11,7 +11,6 @@
     <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_device2" android:required="true" />
 
     <!-- This is the platform API where NativeActivity was introduced. -->
     <uses-sdk android:minSdkVersion="17" />
@@ -20,13 +19,15 @@
     <application 
         android:label="@string/app_name" 
         android:icon="@drawable/ic_launcher">
+        
+        <uses-library android:name="com.projecttango.libtango_device2" android:required="true" />
 
         <!-- Our activity is the built-in NativeActivity framework class.
              This will take care of integrating with our NDK code. -->
         <activity android:name="RTABMapActivity"
                 android:label="@string/app_name"
                 android:launchMode="singleTask"
-                android:screenOrientation="nosensor">
+                android:screenOrientation="landscape">
             <!-- Tell NativeActivity the name of our .so -->
             <meta-data android:name="android.app.lib_name"
                     android:value="NativeRTABMap" />

app/android/jni/CameraTango.cpp

@@ -29,22 +29,24 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "util.h"
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/core/util3d_transforms.h"
-#include "rtabmap/core/util2d.h"
 #include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/core/util2d.h"
 #include <tango_client_api.h>
+#include <tango_support_api.h>
 
 namespace rtabmap {
 
 #define nullptr 0
 const int kVersionStringLength = 128;
-const int holeSize = 10;
+const int holeSize = 5;
 const float maxDepthError = 0.10;
+const int scanDownsampling = 10;
 
 // Callbacks
-void onPointCloudAvailableRouter(void* context, const TangoXYZij* xyz_ij)
+void onPointCloudAvailableRouter(void* context, const TangoPointCloud* point_cloud)
 {
 	CameraTango* app = static_cast<CameraTango*>(context);
-	app->cloudReceived(cv::Mat(1, xyz_ij->xyz_count, CV_32FC3, xyz_ij->xyz[0]), xyz_ij->timestamp);
+	app->cloudReceived(cv::Mat(1, point_cloud->num_points, CV_32FC4, point_cloud->points[0]), point_cloud->timestamp);
 }
 
 void onFrameAvailableRouter(void* context, TangoCameraId id, const TangoImageBuffer* color)
@@ -76,8 +78,11 @@ void onFrameAvailableRouter(void* context, TangoCameraId id, const TangoImageBuf
 
 void onPoseAvailableRouter(void* context, const TangoPoseData* pose)
 {
-	CameraTango* app = static_cast<CameraTango*>(context);
+	if(pose->status_code == TANGO_POSE_VALID)
-	app->poseReceived(app->tangoPoseToTransform(pose, true));
+	{
+		CameraTango* app = static_cast<CameraTango*>(context);
+		app->poseReceived(app->tangoPoseToTransform(pose));
+	}
 }
 
 void onTangoEventAvailableRouter(void* context, const TangoEvent* event)
@@ -86,18 +91,11 @@ void onTangoEventAvailableRouter(void* context, const TangoEvent* event)
 	app->tangoEventReceived(event->type, event->event_key, event->event_value);
 }
 
-// In OpenGL, axes are x->right, y->up and z->outScreen
-// Image is x->right, y->down and z->inScreen
-static rtabmap::Transform opticalRotation(
-		1.0f, 0.0f, 0.0f, 0.0f,
-		0.0f, -1.0f, 0.0f, 0.0f,
-		0.0f, 0.0f, -1.0f, 0.0f);
-
 //////////////////////////////
 // CameraTango
 //////////////////////////////
 CameraTango::CameraTango(int decimation, bool autoExposure) :
-		Camera(0, opticalRotation),
+		Camera(0),
 		tango_config_(0),
 		firstFrame_(true),
 		decimation_(decimation),
@@ -118,6 +116,8 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 {
 	close();
 
+	TangoSupport_initializeLibrary();
+
 	// Connect to Tango
 	LOGI("NativeRTABMap: Setup tango config");
 	tango_config_ = TangoService_getConfig(TANGO_CONFIG_DEFAULT);
@@ -178,6 +178,14 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 		return false;
 	}
 
+	// Need to specify the depth_mode as XYZC.
+	ret = TangoConfig_setInt32(tango_config_, "config_depth_mode", TANGO_POINTCLOUD_XYZC);
+	if (ret != TANGO_SUCCESS)
+	{
+		LOGE("Failed to set 'depth_mode' configuration flag with error code: %d", ret);
+		return false;
+	}
+
 	// Note that it's super important for AR applications that we enable low
 	// latency imu integration so that we have pose information available as
 	// quickly as possible. Without setting this flag, you'll often receive
@@ -189,6 +197,19 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 		return false;
 	}
 
+	// Drift correction allows motion tracking to recover after it loses tracking.
+	//
+	// The drift corrected pose is is available through the frame pair with
+	// base frame AREA_DESCRIPTION and target frame DEVICE.
+	/*ret = TangoConfig_setBool(tango_config_, "config_enable_drift_correction", true);
+	if (ret != TANGO_SUCCESS) {
+		LOGE(
+			"NativeRTABMap: enabling config_enable_drift_correction "
+			"failed with error code: %d",
+			ret);
+		return false;
+	}*/
+
 	// Get TangoCore version string from service.
 	char tango_core_version[kVersionStringLength];
 	ret = TangoConfig_getString(tango_config_, "tango_service_library_version", tango_core_version, kVersionStringLength);
@@ -204,7 +225,7 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 	LOGI("NativeRTABMap: Setup callbacks");
 	// Attach the OnXYZijAvailable callback.
 	// The callback will be called after the service is connected.
-	ret = TangoService_connectOnXYZijAvailable(onPointCloudAvailableRouter);
+	ret = TangoService_connectOnPointCloudAvailable(onPointCloudAvailableRouter);
 	if (ret != TANGO_SUCCESS)
 	{
 		LOGE("NativeRTABMap: Failed to connect to point cloud callback with error code: %d", ret);
@@ -221,6 +242,7 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 	// Attach the onPoseAvailable callback.
 	// The callback will be called after the service is connected.
 	TangoCoordinateFramePair pair;
+	//pair.base = TANGO_COORDINATE_FRAME_AREA_DESCRIPTION; // drift correction is enabled
 	pair.base = TANGO_COORDINATE_FRAME_START_OF_SERVICE;
 	pair.target = TANGO_COORDINATE_FRAME_DEVICE;
 	ret = TangoService_connectOnPoseAvailable(1, &pair, onPoseAvailableRouter);
@@ -257,34 +279,16 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 	// as well. We use timestamp 0.0 and the target frame pair to get the
 	// extrinsics from the sensors.
 	//
-	// Get device with respect to imu transformation matrix.
+	// Get color camera with respect to device transformation matrix.
-	frame_pair.base = TANGO_COORDINATE_FRAME_IMU;
+	frame_pair.base = TANGO_COORDINATE_FRAME_DEVICE;
-	frame_pair.target = TANGO_COORDINATE_FRAME_DEVICE;
+	frame_pair.target = TANGO_COORDINATE_FRAME_CAMERA_COLOR;
-	ret = TangoService_getPoseAtTime(0.0, frame_pair, &pose_data);
-	if (ret != TANGO_SUCCESS)
-	{
-		LOGE("NativeRTABMap: Failed to get transform between the IMU frame and device frames");
-		return false;
-	}
-	imuTDevice_ = rtabmap::Transform(
-			pose_data.translation[0],
-			pose_data.translation[1],
-			pose_data.translation[2],
-			pose_data.orientation[0],
-			pose_data.orientation[1],
-			pose_data.orientation[2],
-			pose_data.orientation[3]);
-
-	// Get color camera with respect to imu transformation matrix.
-	frame_pair.base = TANGO_COORDINATE_FRAME_IMU;
-	frame_pair.target = TANGO_COORDINATE_FRAME_CAMERA_DEPTH;
 	ret = TangoService_getPoseAtTime(0.0, frame_pair, &pose_data);
 	if (ret != TANGO_SUCCESS)
 	{
 		LOGE("NativeRTABMap: Failed to get transform between the color camera frame and device frames");
 		return false;
 	}
-	imuTDepthCamera_ = rtabmap::Transform(
+	deviceTColorCamera_ = rtabmap::Transform(
 			pose_data.translation[0],
 			pose_data.translation[1],
 			pose_data.translation[2],
@@ -293,8 +297,6 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 			pose_data.orientation[2],
 			pose_data.orientation[3]);
 
-	deviceTDepth_ = imuTDevice_.inverse() * imuTDepthCamera_;
-
 	// camera intrinsic
 	TangoCameraIntrinsics color_camera_intrinsics;
 	ret = TangoService_getCameraIntrinsics(TANGO_CAMERA_COLOR, &color_camera_intrinsics);
@@ -311,11 +313,13 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 			this->getLocalTransform());
 	model_.setImageSize(cv::Size(color_camera_intrinsics.width, color_camera_intrinsics.height));
 
-	// optical rotation
+	// device to camera optical rotation in rtabmap frame
-	model_.setLocalTransform(Transform(
+	model_.setLocalTransform(tango_device_T_rtabmap_device.inverse()*deviceTColorCamera_);
-			0.0f, 0.0f, 1.0f, 0.0f,
+
-			-1.0f, 0.0f, 0.0f, 0.0f,
+	LOGI("deviceTColorCameraTango  =%s", deviceTColorCamera_.prettyPrint().c_str());
-			0.0f, -1.0f, 0.0f, 0.0f));
+	LOGI("deviceTColorCameraRtabmap=%s", (tango_device_T_rtabmap_device.inverse()*deviceTColorCamera_).prettyPrint().c_str());
+
+	cameraStartedTime_.restart();
 
 	return true;
 }
@@ -335,7 +339,7 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
 {
 	if(this->isRunning())
 	{
-		UASSERT(cloud.type() == CV_32FC3);
+		UASSERT(cloud.type() == CV_32FC4);
 		boost::mutex::scoped_lock  lock(dataMutex_);
 
 		bool notify = cloud_.empty();
@@ -375,11 +379,16 @@ void CameraTango::rgbReceived(const cv::Mat & tangoImage, int type, double times
 	}
 }
 
+static rtabmap::Transform opticalRotationTango(
+								1.0f,  0.0f,  0.0f, 0.0f,
+							    0.0f, -1.0f,  0.0f, 0.0f,
+								0.0f,  0.0f, -1.0f, 0.0f);
 void CameraTango::poseReceived(const Transform & pose)
 {
 	if(!pose.isNull() && pose.getNormSquared() < 100000)
 	{
-		this->post(new PoseEvent(pose));
+		// send pose of the camera (without optical rotation), not the device
+		this->post(new PoseEvent(pose*deviceTColorCamera_*opticalRotationTango));
 	}
 }
 
@@ -398,73 +407,56 @@ std::string CameraTango::getSerial() const
 	return "Tango";
 }
 
-rtabmap::Transform CameraTango::tangoPoseToTransform(const TangoPoseData * tangoPose, bool inOpenGLFrame) const
+rtabmap::Transform CameraTango::tangoPoseToTransform(const TangoPoseData * tangoPose) const
 {
 	UASSERT(tangoPose);
 	rtabmap::Transform pose;
-	if(!deviceTDepth_.isNull())
-	{
-		pose = rtabmap::Transform(
-				tangoPose->translation[0],
-				tangoPose->translation[1],
-				tangoPose->translation[2],
-				tangoPose->orientation[0],
-				tangoPose->orientation[1],
-				tangoPose->orientation[2],
-				tangoPose->orientation[3]);
 
-		// transform in OpenGL + extrinsics
+	pose = rtabmap::Transform(
-		// opengl_world_T_opengl_camera =
+			tangoPose->translation[0],
-		//      opengl_world_T_start_service *
+			tangoPose->translation[1],
-		//      start_service_T_device *
+			tangoPose->translation[2],
-		//      device_T_imu *
+			tangoPose->orientation[0],
-		//      imu_T_depth_camera *
+			tangoPose->orientation[1],
-		//      depth_camera_T_opengl_camera;
+			tangoPose->orientation[2],
-		if(inOpenGLFrame)
+			tangoPose->orientation[3]);
-		{
-			pose = opengl_world_T_tango_world * pose * deviceTDepth_ * depth_camera_T_opengl_camera;
-		}
-	}
 
 	return pose;
 }
 
-rtabmap::Transform CameraTango::getPoseAtTimestamp(double timestamp, bool inOpenGLFrame)
+rtabmap::Transform CameraTango::getPoseAtTimestamp(double timestamp)
 {
 	rtabmap::Transform pose;
-	if(!deviceTDepth_.isNull())
+
+	TangoPoseData pose_start_service_T_device;
+	TangoCoordinateFramePair frame_pair;
+	frame_pair.base = TANGO_COORDINATE_FRAME_START_OF_SERVICE;
+	frame_pair.target = TANGO_COORDINATE_FRAME_DEVICE;
+	TangoErrorType status = TangoService_getPoseAtTime(timestamp, frame_pair, &pose_start_service_T_device);
+	if (status != TANGO_SUCCESS)
 	{
-		TangoPoseData pose_start_service_T_device;
+		LOGE(
-		TangoCoordinateFramePair frame_pair;
+			"PoseData: Failed to get transform between the Start of service and "
-		frame_pair.base = TANGO_COORDINATE_FRAME_START_OF_SERVICE;
+			"device frames at timestamp %lf",
-		frame_pair.target = TANGO_COORDINATE_FRAME_DEVICE;
+			timestamp);
-		TangoErrorType status = TangoService_getPoseAtTime(timestamp, frame_pair, &pose_start_service_T_device);
-		if (status != TANGO_SUCCESS)
-		{
-			LOGE(
-				"PoseData: Failed to get transform between the Start of service and "
-				"device frames at timestamp %lf",
-				timestamp);
-		}
-		if (pose_start_service_T_device.status_code != TANGO_POSE_VALID)
-		{
-			LOGW(
-				"PoseData: Failed to get transform between the Start of service and "
-				"device frames at timestamp %lf",
-				timestamp);
-		}
-		else
-		{
-
-			pose = tangoPoseToTransform(&pose_start_service_T_device, inOpenGLFrame);
-		}
-
-
 	}
+	if (pose_start_service_T_device.status_code != TANGO_POSE_VALID)
+	{
+		LOGW(
+			"PoseData: Failed to get transform between the Start of service and "
+			"device frames at timestamp %lf",
+			timestamp);
+	}
+	else
+	{
+
+		pose = tangoPoseToTransform(&pose_start_service_T_device);
+	}
+
 	return pose;
 }
 
-SensorData CameraTango::captureImage()
+SensorData CameraTango::captureImage(CameraInfo * info)
 {
 	LOGI("Capturing image...");
 
@@ -544,35 +536,54 @@ SensorData CameraTango::captureImage()
 		// Querying the depth image's frame transformation based on the depth image's
 		// timestamp.
 		cv::Mat depth;
-		Transform poseDepth = getPoseAtTimestamp(cloudStamp, false);
-		Transform poseColor = getPoseAtTimestamp(rgbStamp, false);
 
-		if(poseColor.getNormSquared() > 100000)
+		// Calculate the relative pose from color camera frame at timestamp
+		// color_timestamp t1 and depth
+		// camera frame at depth_timestamp t0.
+		Transform colorToDepth;
+		TangoPoseData pose_color_image_t1_T_depth_image_t0;
+		if (TangoSupport_calculateRelativePose(
+				rgbStamp, TANGO_COORDINATE_FRAME_CAMERA_COLOR, cloudStamp,
+			  TANGO_COORDINATE_FRAME_CAMERA_DEPTH,
+			  &pose_color_image_t1_T_depth_image_t0) == TANGO_SUCCESS)
 		{
-			LOGE("Very large odometry color pose detected (%s)! Ignoring this frame!", poseColor.prettyPrint().c_str());
+			colorToDepth = tangoPoseToTransform(&pose_color_image_t1_T_depth_image_t0);
-			poseColor.setNull();
 		}
-		if(poseDepth.getNormSquared() > 100000)
+		else
 		{
-			LOGE("Very large odometry depth pose detected (%s)! Ignoring this frame!", poseDepth.prettyPrint().c_str());
+			LOGE(
-			poseDepth.setNull();
+				"SynchronizationApplication: Could not find a valid relative pose at "
+				"time for color and "
+				" depth cameras.");
 		}
 
-		if(!poseDepth.isNull() && !poseColor.isNull())
+		if(colorToDepth.getNormSquared() > 100000)
+		{
+			LOGE("Very large color to depth error detected (%s)! Ignoring this frame!", colorToDepth.prettyPrint().c_str());
+			colorToDepth.setNull();
+		}
+		cv::Mat scan;
+		if(!colorToDepth.isNull())
 		{
 			// The Color Camera frame at timestamp t0 with respect to Depth
 			// Camera frame at timestamp t1.
-			Transform colorToDepth = deviceTDepth_.inverse() * poseColor.inverse() * poseDepth * deviceTDepth_;
 			LOGI("colorToDepth=%s", colorToDepth.prettyPrint().c_str());
 
 			int pixelsSet = 0;
 			depth = cv::Mat::zeros(model_.imageHeight()/8, model_.imageWidth()/8, CV_16UC1); // mm
 			CameraModel depthModel = model_.scaled(1.0f/8.0f);
+			std::vector<cv::Point3f> scanData(cloud.total());
+			int oi=0;
 			for(unsigned int i=0; i<cloud.total(); ++i)
 			{
-				cv::Vec3f & p = cloud.at<cv::Vec3f>(i);
+				float * p = cloud.ptr<float>(0,i);
 				cv::Point3f pt = util3d::transformPoint(cv::Point3f(p[0], p[1], p[2]), colorToDepth);
 
+				if(pt.z > 0.0f && i%scanDownsampling == 0)
+				{
+					scanData.at(oi++) = pt;
+				}
+
 				int pixel_x, pixel_y;
 				// get the coordinate on image plane.
 				pixel_x = static_cast<int>((depthModel.fx()) * (pt.x / pt.z) + depthModel.cx());
@@ -591,23 +602,35 @@ SensorData CameraTango::captureImage()
 					}
 				}
 			}
+
+			if(oi)
+			{
+				scan = cv::Mat(1, oi, CV_32FC3, scanData.data()).clone();
+			}
 			LOGI("pixels depth set= %d", pixelsSet);
 		}
 		else
 		{
-			LOGE("Poses are null?!? color=%d (stamp=%f) depth=%d (stamp=%f)", poseColor.isNull()?0:1, rgbStamp, poseDepth.isNull()?0:1, cloudStamp);
+			LOGE("color to depth pose is null?!? (rgb stamp=%f) (depth stamp=%f)", rgbStamp, cloudStamp);
 		}
 
 		if(!rgb.empty() && !depth.empty())
 		{
 			depth = rtabmap::util2d::fillDepthHoles(depth, holeSize, maxDepthError);
 
-			Transform poseColorOpenGL = getPoseAtTimestamp(rgbStamp, true);
+			Transform poseDevice = getPoseAtTimestamp(rgbStamp);
+
+			LOGD("Local    = %s", model.localTransform().prettyPrint().c_str());
+			LOGD("tango    = %s", poseDevice.prettyPrint().c_str());
+			LOGD("opengl(t)= %s", (opengl_world_T_tango_world * poseDevice).prettyPrint().c_str());
 
 			//Rotate in RTAB-Map's coordinate
-			Transform odom = rtabmap_world_T_opengl_world * poseColorOpenGL * depth_camera_T_opengl_camera * model.localTransform().inverse();
+			Transform odom = rtabmap_world_T_tango_world * poseDevice * tango_device_T_rtabmap_device;
 
-			data = SensorData(rgb, depth, model, this->getNextSeqID(), rgbStamp);
+			LOGD("rtabmap  = %s", odom.prettyPrint().c_str());
+			LOGD("opengl(r)= %s", (opengl_world_T_rtabmap_world * odom * rtabmap_device_T_opengl_device).prettyPrint().c_str());
+
+			data = SensorData(scan, LaserScanInfo(cloud.total()/scanDownsampling, 0, model.localTransform()), rgb, depth, model, this->getNextSeqID(), rgbStamp);
 			data.setGroundTruth(odom);
 		}
 		else
@@ -621,7 +644,11 @@ SensorData CameraTango::captureImage()
 
 void CameraTango::mainLoopBegin()
 {
-	uSleep(2000); // just to make sure that the camera is started
+	double t = cameraStartedTime_.elapsed();
+	if(t < 5.0)
+	{
+		uSleep((5.0-t)*1000); // just to make sure that the camera is started
+	}
 }
 
 void CameraTango::mainLoop()

app/android/jni/CameraTango.h

@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UEventsSender.h>
 #include <rtabmap/utilite/UThread.h>
 #include <rtabmap/utilite/UEvent.h>
+#include <rtabmap/utilite/UTimer.h>
 #include <boost/thread/mutex.hpp>
 
 class TangoPoseData;
@@ -76,7 +77,8 @@ public:
 	void close(); // close Tango connection
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
-	rtabmap::Transform tangoPoseToTransform(const TangoPoseData * tangoPose, bool inOpenGLFrame) const;
+	const CameraModel & getCameraModel() const {return model_;}
+	rtabmap::Transform tangoPoseToTransform(const TangoPoseData * tangoPose) const;
 	void setDecimation(int value) {decimation_ = value;}
 	void setAutoExposure(bool enabled) {autoExposure_ = enabled;}
 
@@ -86,10 +88,10 @@ public:
 	void tangoEventReceived(int type, const char * key, const char * value);
 
 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);
 
 private:
-	rtabmap::Transform getPoseAtTimestamp(double timestamp, bool inOpenGLFrame);
+	rtabmap::Transform getPoseAtTimestamp(double timestamp);
 
 	virtual void mainLoopBegin();
 	virtual void mainLoop();
@@ -97,6 +99,7 @@ private:
 private:
 	void * tango_config_;
 	bool firstFrame_;
+	UTimer cameraStartedTime_;
 	int decimation_;
 	bool autoExposure_;
 	cv::Mat cloud_;
@@ -106,10 +109,8 @@ private:
 	double tangoColorStamp_;
 	boost::mutex dataMutex_;
 	USemaphore dataReady_;
-	rtabmap::Transform imuTDevice_;
-	rtabmap::Transform imuTDepthCamera_;
-	rtabmap::Transform deviceTDepth_;
 	CameraModel model_;
+	Transform deviceTColorCamera_;
 };
 
 } /* namespace rtabmap */

app/android/jni/RTABMapApp.h

@@ -52,7 +52,7 @@ class RTABMapApp : public UEventsHandler {
 
   void onCreate(JNIEnv* env, jobject caller_activity);
 
-  void openDatabase(const std::string & databasePath);
+  void openDatabase(const std::string & databasePath = "");
 
   bool onTangoServiceConnected(JNIEnv* env, jobject iBinder);
 
@@ -112,21 +112,26 @@ class RTABMapApp : public UEventsHandler {
   void setPausedMapping(bool paused);
   void setMapCloudShown(bool shown);
   void setOdomCloudShown(bool shown);
-  void setMeshRendering(bool enabled);
+  void setMeshRendering(bool enabled, bool withTexture);
   void setLocalizationMode(bool enabled);
   void setTrajectoryMode(bool enabled);
   void setGraphOptimization(bool enabled);
   void setNodesFiltering(bool enabled);
+  void setDriftCorrection(bool enabled);
   void setGraphVisible(bool visible);
+  void setGridVisible(bool visible);
   void setAutoExposure(bool enabled);
   void setFullResolution(bool enabled);
+  void setAppendMode(bool enabled);
+  void setDataRecorderMode(bool enabled);
   void setMaxCloudDepth(float value);
+  void setMeshDecimation(int value);
   void setMeshAngleTolerance(float value);
   void setMeshTriangleSize(int value);
   int setMappingParameter(const std::string & key, const std::string & value);
 
   void resetMapping();
-  void save();
+  void save(const std::string & databasePath);
   bool exportMesh(const std::string & filePath);
   int postProcessing(int approach);
 
@@ -135,6 +140,7 @@ class RTABMapApp : public UEventsHandler {
 
  private:
   rtabmap::ParametersMap getRtabmapParameters();
+  bool smoothMesh(int id, Mesh & mesh);
 
  private:
   rtabmap::CameraTango * camera_;
@@ -145,22 +151,30 @@ class RTABMapApp : public UEventsHandler {
   bool odomCloudShown_;
   bool graphOptimization_;
   bool nodesFiltering_;
+  bool driftCorrection_;
   bool localizationMode_;
   bool trajectoryMode_;
   bool autoExposure_;
   bool fullResolution_;
+  bool appendMode_;
   float maxCloudDepth_;
+  int meshDecimation_;
   int meshTrianglePix_;
   float meshAngleToleranceDeg_;
 
   rtabmap::ParametersMap mappingParameters_;
 
-
+  bool paused_;
+  bool dataRecorderMode_;
   bool clearSceneOnNextRender_;
+  bool filterPolygonsOnNextRender_;
+  int gainCompensationOnNextRender_;
+  bool bilateralFilteringOnNextRender_;
+  bool cameraJustInitialized_;
   int totalPoints_;
   int totalPolygons_;
   int lastDrawnCloudsCount_;
-  float renderingFPS_;
+  float renderingTime_;
 
   // main_scene_ includes all drawable object for visualizing Tango device's
   // movement and point cloud.
@@ -174,14 +188,7 @@ class RTABMapApp : public UEventsHandler {
 	boost::mutex meshesMutex_;
 	boost::mutex odomMutex_;
 	boost::mutex poseMutex_;
-
+	boost::mutex renderingMutex_;
-	struct Mesh
-	{
-		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
-		std::vector<pcl::Vertices> polygons;
-		rtabmap::Transform pose;
-		cv::Mat texture;
-	};
 
 	std::map<int, Mesh> createdMeshes_;
 	std::map<int, rtabmap::Transform> rawPoses_;

app/android/jni/jni_interface.cpp

@@ -55,6 +55,13 @@ Java_com_introlab_rtabmap_RTABMapLib_onCreate(
 	return app.onCreate(env, activity);
 }
 
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_openEmptyDatabase(
+    JNIEnv* env, jobject)
+{
+	return app.openDatabase();
+}
+
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_openDatabase(
     JNIEnv* env, jobject, jstring databasePath)
@@ -133,9 +140,9 @@ Java_com_introlab_rtabmap_RTABMapLib_setOdomCloudShown(
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setMeshRendering(
-		JNIEnv*, jobject, bool enabled)
+		JNIEnv*, jobject, bool enabled, bool withTexture)
 {
-	return app.setMeshRendering(enabled);
+	return app.setMeshRendering(enabled, withTexture);
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setLocalizationMode(
@@ -162,12 +169,24 @@ Java_com_introlab_rtabmap_RTABMapLib_setNodesFiltering(
 	return app.setNodesFiltering(enabled);
 }
 JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setDriftCorrection(
+		JNIEnv*, jobject, bool enabled)
+{
+	return app.setDriftCorrection(enabled);
+}
+JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setGraphVisible(
 		JNIEnv*, jobject, bool visible)
 {
 	return app.setGraphVisible(visible);
 }
 JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setGridVisible(
+		JNIEnv*, jobject, bool visible)
+{
+	return app.setGridVisible(visible);
+}
+JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setAutoExposure(
 		JNIEnv*, jobject, bool enabled)
 {
@@ -180,12 +199,30 @@ Java_com_introlab_rtabmap_RTABMapLib_setFullResolution(
 	return app.setFullResolution(enabled);
 }
 JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setAppendMode(
+		JNIEnv*, jobject, bool enabled)
+{
+	return app.setAppendMode(enabled);
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setDataRecorderMode(
+		JNIEnv*, jobject, bool enabled)
+{
+	return app.setDataRecorderMode(enabled);
+}
+JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setMaxCloudDepth(
 		JNIEnv*, jobject, float value)
 {
 	return app.setMaxCloudDepth(value);
 }
 JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setMeshDecimation(
+		JNIEnv*, jobject, int value)
+{
+	return app.setMeshDecimation(value);
+}
+JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setMeshAngleTolerance(
 		JNIEnv*, jobject, float value)
 {
@@ -216,9 +253,11 @@ Java_com_introlab_rtabmap_RTABMapLib_resetMapping(
 
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_save(
-		JNIEnv* env, jobject)
+		JNIEnv* env, jobject, jstring databasePath)
 {
-	return app.save();
+	std::string databasePathC;
+	GetJStringContent(env,databasePath,databasePathC);
+	return app.save(databasePathC);
 }
 
 JNIEXPORT bool JNICALL

app/android/jni/point_cloud_drawable.cpp

@@ -39,120 +39,35 @@ PointCloudDrawable::PointCloudDrawable(
 		GLuint cloudShaderProgram,
 		GLuint textureShaderProgram,
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		const std::vector<pcl::Vertices> & polygons,
+		const pcl::IndicesPtr & indices,
-		const cv::Mat & image) :
+		float gain) :
 		vertex_buffers_(0),
 		textures_(0),
 		nPoints_(0),
 		pose_(1.0f),
 		visible_(true),
 		cloud_shader_program_(cloudShaderProgram),
-		texture_shader_program_(textureShaderProgram)
+		texture_shader_program_(textureShaderProgram),
+		gain_(1.0f)
 {
-	UASSERT(!cloud->empty());
+	updateCloud(cloud, indices, gain);
+}
 
-	glGenBuffers(1, &vertex_buffers_);
+PointCloudDrawable::PointCloudDrawable(
-	if(!vertex_buffers_)
+		GLuint cloudShaderProgram,
-	{
+		GLuint textureShaderProgram,
-		LOGE("OpenGL: could not generate vertex buffers\n");
+		const Mesh & mesh,
-		return;
+		const cv::Mat & texture) :
-	}
+		vertex_buffers_(0),
-
+		textures_(0),
-	if(!cloud->is_dense && !image.empty())
+		nPoints_(0),
-	{
+		pose_(1.0f),
-		LOGI("cloud=%dx%d image=%dx%d\n", (int)cloud->width, (int)cloud->height, image.cols, image.rows);
+		visible_(true),
-		UASSERT(polygons.size() && !cloud->is_dense && !image.empty() && image.type() == CV_8UC3);
+		cloud_shader_program_(cloudShaderProgram),
-		glGenTextures(1, &textures_);
+		texture_shader_program_(textureShaderProgram),
-		if(!textures_)
+		gain_(1.0f)
-		{
+{
-			vertex_buffers_ = 0;
+	updateMesh(mesh, texture);
-			LOGE("OpenGL: could not generate vertex buffers\n");
-			return;
-		}
-	}
-
-	LOGI("Creating cloud buffer %d", vertex_buffers_);
-	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;
-			vertices[i*4+1] = cloud->at(i).y;
-			vertices[i*4+2] = cloud->at(i).z;
-			vertices[i*4+3] = cloud->at(i).rgb;
-		}
-	}
-
-	glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
-	glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * (int)vertices.size(), (const void *)vertices.data(), GL_STATIC_DRAW);
-	glBindBuffer(GL_ARRAY_BUFFER, 0);
-
-	GLint error = glGetError();
-	if(error != GL_NO_ERROR)
-	{
-		LOGE("OpenGL: Could not allocate point cloud (0x%x)\n", error);
-		vertex_buffers_ = 0;
-		return;
-	}
-
-	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(polygons.size())
-	{
-		int polygonSize = polygons[0].vertices.size();
-		UASSERT(polygonSize == 3);
-		polygons_.resize(polygons.size() * polygonSize);
-		int oi = 0;
-		for(unsigned int i=0; i<polygons.size(); ++i)
-		{
-			UASSERT((int)polygons[i].vertices.size() == polygonSize);
-			for(int j=0; j<polygonSize; ++j)
-			{
-				polygons_[oi++] = (unsigned short)polygons[i].vertices[j];
-			}
-		}
-	}
 }
 
 PointCloudDrawable::~PointCloudDrawable()
@@ -173,6 +88,202 @@ PointCloudDrawable::~PointCloudDrawable()
 	}
 }
 
+void PointCloudDrawable::updatePolygons(const std::vector<pcl::Vertices> & polygons)
+{
+	polygons_.clear();
+	if(polygons.size() && organizedToDenseIndices_.size())
+	{
+		int polygonSize = polygons[0].vertices.size();
+		UASSERT(polygonSize == 3);
+		polygons_.resize(polygons.size() * polygonSize);
+		int oi = 0;
+		for(unsigned int i=0; i<polygons.size(); ++i)
+		{
+			UASSERT((int)polygons[i].vertices.size() == polygonSize);
+			for(int j=0; j<polygonSize; ++j)
+			{
+				polygons_[oi++] = organizedToDenseIndices_.at((unsigned short)polygons[i].vertices[j]);
+			}
+		}
+	}
+}
+
+void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, float gain)
+{
+	UASSERT(cloud.get() && !cloud->empty() && indices.get() && !indices->empty());
+	nPoints_ = 0;
+	polygons_.clear();
+	gain_ = gain;
+
+	if (vertex_buffers_)
+	{
+		glDeleteBuffers(1, &vertex_buffers_);
+		tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
+		vertex_buffers_ = 0;
+	}
+
+	if (textures_)
+	{
+		glDeleteTextures(1, &textures_);
+		tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
+		textures_ = 0;
+	}
+
+	glGenBuffers(1, &vertex_buffers_);
+	if(!vertex_buffers_)
+	{
+		LOGE("OpenGL: could not generate vertex buffers\n");
+		return;
+	}
+
+	LOGI("Creating cloud buffer %d", vertex_buffers_);
+	std::vector<float> vertices(indices->size()*4);
+	for(unsigned int i=0; i<indices->size(); ++i)
+	{
+		vertices[i*4] = cloud->at(indices->at(i)).x;
+		vertices[i*4+1] = cloud->at(indices->at(i)).y;
+		vertices[i*4+2] = cloud->at(indices->at(i)).z;
+		vertices[i*4+3] = cloud->at(indices->at(i)).rgb;
+	}
+
+	glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
+	glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * (int)vertices.size(), (const void *)vertices.data(), GL_STATIC_DRAW);
+	glBindBuffer(GL_ARRAY_BUFFER, 0);
+
+	GLint error = glGetError();
+	if(error != GL_NO_ERROR)
+	{
+		LOGE("OpenGL: Could not allocate point cloud (0x%x)\n", error);
+		vertex_buffers_ = 0;
+		return;
+	}
+
+	nPoints_ = indices->size();
+}
+
+void PointCloudDrawable::updateMesh(const Mesh & mesh, const cv::Mat & texture)
+{
+	UASSERT(mesh.cloud.get() && !mesh.cloud->empty() && mesh.indices.get() && !mesh.indices->empty());
+	nPoints_ = 0;
+
+	if (vertex_buffers_)
+	{
+		glDeleteBuffers(1, &vertex_buffers_);
+		tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
+		vertex_buffers_ = 0;
+	}
+
+	gain_ = mesh.gain;
+
+	bool textureUpdate = false;
+	if(!texture.empty() && texture.type() == CV_8UC3)
+	{
+		if (textures_)
+		{
+			glDeleteTextures(1, &textures_);
+			tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
+			textures_ = 0;
+		}
+		textureUpdate = true;
+	}
+
+	glGenBuffers(1, &vertex_buffers_);
+	if(!vertex_buffers_)
+	{
+		LOGE("OpenGL: could not generate vertex buffers\n");
+		return;
+	}
+
+	if(textureUpdate)
+	{
+		UASSERT(!mesh.cloud->is_dense);
+		glGenTextures(1, &textures_);
+		if(!textures_)
+		{
+			vertex_buffers_ = 0;
+			LOGE("OpenGL: could not generate texture buffers\n");
+			return;
+		}
+	}
+
+	LOGI("Creating cloud buffer %d", vertex_buffers_);
+	std::vector<float> vertices;
+	organizedToDenseIndices_ = std::vector<int>(mesh.cloud->width*mesh.cloud->height, -1);
+	if(textures_)
+	{
+		vertices = std::vector<float>(mesh.indices->size()*6);
+		for(unsigned int i=0; i<mesh.indices->size(); ++i)
+		{
+			vertices[i*6] = mesh.cloud->at(mesh.indices->at(i)).x;
+			vertices[i*6+1] = mesh.cloud->at(mesh.indices->at(i)).y;
+			vertices[i*6+2] = mesh.cloud->at(mesh.indices->at(i)).z;
+
+			// rgb
+			vertices[i*6+3] = mesh.cloud->at(mesh.indices->at(i)).rgb;
+
+			// texture uv
+			int index = mesh.indices->at(i);
+			vertices[i*6+4] = float(index % mesh.cloud->width)/float(mesh.cloud->width); //u
+			vertices[i*6+5] = float(index / mesh.cloud->width)/float(mesh.cloud->height);  //v
+
+			organizedToDenseIndices_[mesh.indices->at(i)] = i;
+		}
+	}
+	else
+	{
+		vertices = std::vector<float>(mesh.indices->size()*4);
+		for(unsigned int i=0; i<mesh.indices->size(); ++i)
+		{
+			vertices[i*4] = mesh.cloud->at(mesh.indices->at(i)).x;
+			vertices[i*4+1] = mesh.cloud->at(mesh.indices->at(i)).y;
+			vertices[i*4+2] = mesh.cloud->at(mesh.indices->at(i)).z;
+			vertices[i*4+3] = mesh.cloud->at(mesh.indices->at(i)).rgb;
+			organizedToDenseIndices_[mesh.indices->at(i)] = i;
+		}
+	}
+
+	glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
+	glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * (int)vertices.size(), (const void *)vertices.data(), GL_STATIC_DRAW);
+	glBindBuffer(GL_ARRAY_BUFFER, 0);
+
+	GLint error = glGetError();
+	if(error != GL_NO_ERROR)
+	{
+		LOGE("OpenGL: Could not allocate point cloud (0x%x)\n", error);
+		vertex_buffers_ = 0;
+		return;
+	}
+
+	if(textures_ && textureUpdate)
+	{
+		// gen texture from image
+		glBindTexture(GL_TEXTURE_2D, textures_);
+		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
+		cv::Mat rgbImage;
+		cv::cvtColor(texture, 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_ = mesh.indices->size();
+
+	if(polygons_.size() != mesh.polygons.size())
+	{
+		updatePolygons(mesh.polygons);
+	}
+}
+
 void PointCloudDrawable::setPose(const rtabmap::Transform & pose)
 {
 	UASSERT(!pose.isNull());
@@ -180,11 +291,11 @@ void PointCloudDrawable::setPose(const rtabmap::Transform & pose)
 	pose_ = glmFromTransform(pose);
 }
 
-void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix, const glm::mat4 & viewMatrix, bool meshRendering, float pointSize) {
+void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix, const glm::mat4 & viewMatrix, bool meshRendering, float pointSize, bool textureRendering) {
 
 	if(vertex_buffers_ && nPoints_ && visible_)
 	{
-		if(meshRendering && textures_)
+		if(meshRendering && textureRendering && textures_)
 		{
 			glUseProgram(texture_shader_program_);
 
@@ -200,6 +311,9 @@ void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix, const glm::m
 			GLuint texture_handle = glGetUniformLocation(texture_shader_program_, "u_Texture");
 			glUniform1i(texture_handle, 0);
 
+			GLuint gain_handle = glGetUniformLocation(texture_shader_program_, "u_gain");
+			glUniform1f(gain_handle, gain_);
+
 			GLint attribute_vertex = glGetAttribLocation(texture_shader_program_, "vertex");
 			GLint attribute_texture = glGetAttribLocation(texture_shader_program_, "a_TexCoordinate");
 
@@ -222,6 +336,9 @@ void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix, const glm::m
 			GLuint point_size_handle_ = glGetUniformLocation(cloud_shader_program_, "point_size");
 			glUniform1f(point_size_handle_, pointSize);
 
+			GLuint gain_handle = glGetUniformLocation(cloud_shader_program_, "u_gain");
+			glUniform1f(gain_handle, gain_);
+
 			GLint attribute_vertex = glGetAttribLocation(cloud_shader_program_, "vertex");
 			GLint attribute_color = glGetAttribLocation(cloud_shader_program_, "color");
 

app/android/jni/point_cloud_drawable.h

@@ -45,14 +45,23 @@ class PointCloudDrawable {
 		  GLuint cloudShaderProgram,
 		  GLuint textureShaderProgram,
   		  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-  		  const std::vector<pcl::Vertices> & polygons = std::vector<pcl::Vertices>(),
+		  const pcl::IndicesPtr & indices,
-		  const cv::Mat & image = cv::Mat());
+		  float gain);
+  PointCloudDrawable(
+  		  GLuint cloudShaderProgram,
+  		  GLuint textureShaderProgram,
+    	  const Mesh & mesh,
+		  const cv::Mat & texture);
   virtual ~PointCloudDrawable();
 
+  void updatePolygons(const std::vector<pcl::Vertices> & polygons);
+  void updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, float gain);
+  void updateMesh(const Mesh & mesh, const cv::Mat & texture);
   void setPose(const rtabmap::Transform & pose);
   void setVisible(bool visible) {visible_=visible;}
   rtabmap::Transform getPose() const {return glmToTransform(pose_);}
   bool isVisible() const {return visible_;}
+  bool hasTexture() const {return textures_ != 0;}
 
   // Update current point cloud data.
   //
@@ -60,7 +69,7 @@ class PointCloudDrawable {
   // @param view_mat: view matrix from current render camera.
   // @param model_mat: model matrix for this point cloud frame.
   // @param vertices: all vertices in this point cloud frame.
-  void Render(const glm::mat4 & projectionMatrix, const glm::mat4 & viewMatrix, bool meshRendering = true, float pointSize = 3.0f);
+  void Render(const glm::mat4 & projectionMatrix, const glm::mat4 & viewMatrix, bool meshRendering = true, float pointSize = 3.0f, bool textureRendering = false);
 
  private:
   // Vertex buffer of the point cloud geometry.
@@ -70,9 +79,12 @@ class PointCloudDrawable {
   int nPoints_;
   glm::mat4 pose_;
   bool visible_;
+  std::vector<int> organizedToDenseIndices_;
 
   GLuint cloud_shader_program_;
   GLuint texture_shader_program_;
+
+  float gain_;
 };
 
 #endif  // TANGO_POINT_CLOUD_POINT_CLOUD_DRAWABLE_H_

app/android/jni/scene.cpp

@@ -55,9 +55,10 @@ const std::string kPointCloudVertexShader =
 const std::string kPointCloudFragmentShader =
     "precision mediump float;\n"
     "precision mediump int;\n"
+	"uniform float u_gain;\n"
     "varying vec3 v_color;\n"
     "void main() {\n"
-    "  gl_FragColor = vec4(v_color.z, v_color.y, v_color.x, 1.0);\n"
+    "  gl_FragColor = vec4(v_color.z*u_gain, v_color.y*u_gain, v_color.x*u_gain, 1.0);\n"
     "}\n";
 
 const std::string kTextureMeshVertexShader =
@@ -75,9 +76,13 @@ const std::string kTextureMeshFragmentShader =
     "precision mediump float;\n"
     "precision mediump int;\n"
 	"uniform sampler2D u_Texture;\n"
+	"uniform float u_gain;\n"
     "varying vec2 v_TexCoordinate;\n"
     "void main() {\n"
     "  gl_FragColor = texture2D(u_Texture, v_TexCoordinate);\n"
+    "  gl_FragColor.x *= u_gain;\n"
+	"  gl_FragColor.y *= u_gain;\n"
+	"  gl_FragColor.z *= u_gain;\n"
     "}\n";
 
 const std::string kGraphVertexShader =
@@ -109,6 +114,7 @@ Scene::Scene() :
 		trace_(0),
 		graph_(0),
 		graphVisible_(true),
+		gridVisible_(true),
 		traceVisible_(true),
 		currentPose_(0),
 		cloud_shader_program_(0),
@@ -116,6 +122,7 @@ Scene::Scene() :
 		graph_shader_program_(0),
 		mapRendering_(true),
 		meshRendering_(true),
+		meshRenderingTexture_(true),
 		pointSize_(3.0f) {}
 
 Scene::~Scene() {DeleteResources();}
@@ -250,7 +257,7 @@ int Scene::Render() {
 	  }
 	  else
 	  {
-		// In third person or top down more, we follow the camera movement.
+		// In third person or top down mode, we follow the camera movement.
 		gesture_camera_->SetAnchorPosition(position, rotation);
 
 		frustum_->SetPosition(position);
@@ -275,9 +282,11 @@ int Scene::Render() {
 	  }
   }
 
-
+  	if(gridVisible_)
-	grid_->Render(gesture_camera_->GetProjectionMatrix(),
+  	{
-				  gesture_camera_->GetViewMatrix());
+  		grid_->Render(gesture_camera_->GetProjectionMatrix(),
+  					  gesture_camera_->GetViewMatrix());
+  	}
 
 	bool frustumCulling = true;
 	int cloudDrawn=0;
@@ -327,7 +336,7 @@ int Scene::Render() {
 			for(unsigned int i=0; i<indices->size(); ++i)
 			{
 				++cloudDrawn;
-				pointClouds_.find(ids[indices->at(i)])->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_);
+				pointClouds_.find(ids[indices->at(i)])->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_, meshRenderingTexture_);
 			}
 		}
 	}
@@ -338,7 +347,7 @@ int Scene::Render() {
 			if((mapRendering_ || iter->first < 0) && iter->second->isVisible())
 			{
 				++cloudDrawn;
-				iter->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_);
+				iter->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_, meshRenderingTexture_);
 			}
 		}
 	}
@@ -400,6 +409,11 @@ void Scene::setGraphVisible(bool visible)
 	graphVisible_ = visible;
 }
 
+void Scene::setGridVisible(bool visible)
+{
+	gridVisible_ = visible;
+}
+
 void Scene::setTraceVisible(bool visible)
 {
 	traceVisible_ = visible;
@@ -409,11 +423,10 @@ void Scene::setTraceVisible(bool visible)
 void Scene::addCloud(
 		int id,
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		const std::vector<pcl::Vertices> & polygons,
+		const pcl::IndicesPtr & indices,
-		const rtabmap::Transform & pose,
+		const rtabmap::Transform & pose)
-		const cv::Mat & image)
 {
-	LOGI("addOrUpdateCloud cloud %d", id);
+	LOGI("add cloud %d", id);
 	std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
 	if(iter != pointClouds_.end())
 	{
@@ -427,8 +440,33 @@ void Scene::addCloud(
 			cloud_shader_program_,
 			texture_mesh_shader_program_,
 			cloud,
-			polygons,
+			indices,
-			image);
+			1.0f);
+	drawable->setPose(pose);
+	pointClouds_.insert(std::make_pair(id, drawable));
+}
+
+void Scene::addMesh(
+		int id,
+		const Mesh & mesh,
+		const cv::Mat & texture,
+		const rtabmap::Transform & pose)
+{
+	LOGI("add mesh %d", id);
+	std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
+	if(iter != pointClouds_.end())
+	{
+		delete iter->second;
+		pointClouds_.erase(iter);
+	}
+
+	//create
+	UASSERT(cloud_shader_program_ != 0 && texture_mesh_shader_program_!=0);
+	PointCloudDrawable * drawable = new PointCloudDrawable(
+			cloud_shader_program_,
+			texture_mesh_shader_program_,
+			mesh,
+			texture);
 	drawable->setPose(pose);
 	pointClouds_.insert(std::make_pair(id, drawable));
 }
@@ -458,7 +496,30 @@ bool Scene::hasCloud(int id) const
 	return pointClouds_.find(id) != pointClouds_.end();
 }
 
+bool Scene::hasTexture(int id) const
+{
+	return pointClouds_.find(id) != pointClouds_.end() && pointClouds_.at(id)->hasTexture();
+}
+
 std::set<int> Scene::getAddedClouds() const
 {
 	return uKeysSet(pointClouds_);
 }
+
+void Scene::updateCloudPolygons(int id, const std::vector<pcl::Vertices> & polygons)
+{
+	std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
+	if(iter != pointClouds_.end())
+	{
+		iter->second->updatePolygons(polygons);
+	}
+}
+
+void Scene::updateMesh(int id, const Mesh & mesh, const cv::Mat & texture)
+{
+	std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
+	if(iter != pointClouds_.end())
+	{
+		iter->second->updateMesh(mesh, texture);
+	}
+}

app/android/jni/scene.h

@@ -94,24 +94,35 @@ class Scene {
   		const std::multimap<int, rtabmap::Link> & links);
 
   void setGraphVisible(bool visible);
+  void setGridVisible(bool visible);
   void setTraceVisible(bool visible);
 
   void addCloud(
   		  int id,
   		  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-  		  const std::vector<pcl::Vertices> & polygons,
+		  const pcl::IndicesPtr & indices,
-  		  const rtabmap::Transform & pose,
+  		  const rtabmap::Transform & pose);
-		  const cv::Mat & image = cv::Mat());
+  void addMesh(
+  		int id,
+  		const Mesh & mesh,
+		const cv::Mat & texture,
+  		const rtabmap::Transform & pose);
 
   void setCloudPose(int id, const rtabmap::Transform & pose);
   void setCloudVisible(int id, bool visible);
   bool hasCloud(int id) const;
+  bool hasTexture(int id) const;
   std::set<int> getAddedClouds() const;
+  void updateCloudPolygons(int id, const std::vector<pcl::Vertices> & polygons);
+  void updateMesh(int id, const Mesh & mesh, const cv::Mat & texture);
 
   void setMapRendering(bool enabled) {mapRendering_ = enabled;}
-  void setMeshRendering(bool enabled) {meshRendering_ = enabled;}
+  void setMeshRendering(bool enabled, bool withTexture) {meshRendering_ = enabled; meshRenderingTexture_ = withTexture;}
   void setPointSize(float size) {pointSize_ = size;}
 
+  bool isMeshRendering() const {return meshRendering_;}
+  bool isMeshTexturing() const {return meshRendering_ && meshRenderingTexture_;}
+
  private:
   // Camera object that allows user to use touch input to interact with.
   tango_gl::GestureCamera* gesture_camera_;
@@ -129,6 +140,7 @@ class Scene {
   tango_gl::Trace* trace_;
   GraphDrawable * graph_;
   bool graphVisible_;
+  bool gridVisible_;
   bool traceVisible_;
 
   std::map<int, PointCloudDrawable*> pointClouds_;
@@ -142,6 +154,7 @@ class Scene {
 
   bool mapRendering_;
   bool meshRendering_;
+  bool meshRenderingTexture_;
   float pointSize_;
 };
 

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

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

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

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

app/android/jni/util.h

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

app/android/res/menu/optionmenu.xml

@@ -8,15 +8,22 @@
     <group android:id="@+id/group_actions">
        <item android:id="@+id/post_processing" android:title="Post-Processing...">
           <menu>
-	         <group android:id="@+id/group_post_processing">
+             <item android:id="@+id/post_processing_standard" android:title="Standard Optimization" />
-	            <item android:id="@+id/detect_more_loop_closures" android:title="Detect More Loop Closures" />
+             <item android:id="@+id/post_processing_advanced" android:title="Advanced..." >
-	            <item android:id="@+id/global_graph_optimization" android:title="Global Graph Optimization" />
+	            <menu>
-	            <item android:id="@+id/sba" android:title="Bundle Adjustement" />
+	              <item android:id="@+id/global_graph_optimization" android:title="Global Graph Optimization" />
-	         </group>   
+	              <item android:id="@+id/detect_more_loop_closures" android:title="Detect More Loop Closures" />
+	              <item android:id="@+id/icp_refining" android:title="ICP Refining" />
+	              <item android:id="@+id/gain_compensation_fast" android:title="Adjust Colors (Fast)" />
+	              <item android:id="@+id/gain_compensation_full" android:title="Adjust Colors (Full)" />
+	              <item android:id="@+id/bilateral_filtering" android:title="Mesh Smoothing" />
+	              <item android:id="@+id/sba" android:title="Bundle Adjustement" />
+	              <item android:id="@+id/polygons_filtering" android:title="Noise Filtering" />
+	            </menu>  
+	         </item>
           </menu>
        </item>
        <item android:id="@+id/open" android:title="Open"/>
-       <item android:id="@+id/save" android:title="Save"/>
        <item android:id="@+id/export" android:title="Export...">
           <menu>
 	         <group android:id="@+id/group_export">
@@ -25,20 +32,31 @@
 	         </group>   
           </menu>
        </item>
+       <item android:id="@+id/save" android:title="Save"/>
        <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/menu_rendering" android:checkable="false" android:title="Mesh Rendering..." >
+           			<menu>
+           			   <group android:checkableBehavior="single">
+	           			   <item android:id="@+id/point_cloud" android:title="Point Cloud" />
+				           <item android:id="@+id/mesh" android:title="Mesh" />
+				           <item android:id="@+id/texture_mesh" android:checked="true" android:title="Texture Mesh" />
+			           </group>
+			           <item android:id="@+id/mesh_decimation" android:checkable="false" android:title="Mesh Decimation..." />
+			           <item android:id="@+id/mesh_angle_tolerance" android:checkable="false" android:title="Mesh Angle Tolerance..." />
+            		   <item android:id="@+id/mesh_triangle_size" android:checkable="false" android:title="Mesh Triangle Size..." />
+            		   <item android:id="@+id/max_depth" android:checkable="false" android:title="Max Depth..." />
+			       </menu>
+            </item>
             <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/grid_visible" android:checked="true" android:title="Grid Visible" />
-            <item android:id="@+id/max_depth" android:checkable="false" android:title="Cloud/Mesh Max Depth..." />
+            <item android:id="@+id/graph_optimization" android:checked="true" android:title="Optimized Graph" />
-            <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>
@@ -47,9 +65,12 @@
 	       <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/append" android:checked="true" android:title="Append Mode" />
 	           <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/drift_correction" android:checked="false" android:title="Drift Correction" />
+	           <item android:id="@+id/auto_exposure" android:checked="true" android:title="Auto Exposure" />
+	           <item android:id="@+id/resolution" android:checked="false" android:title="HD Mode" />
+	           <item android:id="@+id/data_recorder" android:checked="false" android:title="Data Recorder 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..." />

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

@@ -2,6 +2,9 @@ package com.introlab.rtabmap;
 
 import java.io.File;
 import java.io.FilenameFilter;
+import java.text.SimpleDateFormat;
+import java.util.Arrays;
+import java.util.Date;
 
 import android.app.Activity;
 import android.app.AlertDialog;
@@ -49,10 +52,10 @@ public class RTABMapActivity extends Activity implements OnClickListener {
   private static final String TAG = RTABMapActivity.class.getSimpleName();
 
   // The minimum Tango Core version required from this application.
-  private static final int  MIN_TANGO_CORE_VERSION = 6804;
+  private static final int  MIN_TANGO_CORE_VERSION = 9377;
 
   // The package name of Tang Core, used for checking minimum Tango Core version.
-  private static final String TANGO_PACKAGE_NAME = "com.projecttango.tango";
+  private static final String TANGO_PACKAGE_NAME = "com.google.tango";
   
   public static final String EXTRA_KEY_PERMISSIONTYPE = "PERMISSIONTYPE";
   public static final String EXTRA_VALUE_ADF = "ADF_LOAD_SAVE_PERMISSION";
@@ -66,7 +69,9 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 
   // Screen size for normalizing the touch input for orbiting the render camera.
   private Point mScreenSize = new Point();
-
+  private boolean mPauseFirstTime = true;
+  private boolean mOnPause = false;
+  
   private MenuItem mItemPause;
   private MenuItem mItemSave;
   private MenuItem mItemOpen;
@@ -74,21 +79,26 @@ public class RTABMapActivity extends Activity implements OnClickListener {
   private MenuItem mItemExport;
   private MenuItem mItemLocalizationMode;
   private MenuItem mItemTrajectoryMode;
+  private MenuItem mItemAppendMode;
+  private MenuItem mItemRenderingPointCloud;
+  private MenuItem mItemRenderingMesh;
+  private MenuItem mItemRenderingTextureMesh;
+  private MenuItem mItemDataRecorderMode;
+  
   
   private String mOpenedDatabasePath = "";
-  private String mTempDatabasePath = "";
-  private String mNewDatabasePath = "";
   private String mWorkingDirectory = "";
   
   private int mMaxDepthIndex = 5;
-  private int mMeshAngleToleranceIndex = 1;
+  private int mMeshDecimationIndex = 0;
+  private int mMeshAngleToleranceIndex = 2;
   private int mMeshTriangleSizeIndex = 0;
   
   private int mParamUpdateRateHzIndex = 1;
   private int mParamTimeThrMsIndex = 4;
-  private int mParamMaxFeaturesIndex = 4;
+  private int mParamMaxFeaturesIndex = 2;
   private int mParamLoopThrMsIndex = 1;
-  private int mParamOptimizeErrorIndex = 3;
+  private int mParamOptimizeErrorIndex = 4;
   
   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"};
@@ -99,6 +109,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
   private LinearLayout mLayoutDebug;
   
   private int mTotalLoopClosures = 0;
+  private boolean mMapIsEmpty = false;
   
   private Toast mToast = null;
   
@@ -124,7 +135,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
   protected void onCreate(Bundle savedInstanceState) {
     super.onCreate(savedInstanceState);
     setTitle(R.string.menu_name);
-
+    
     // Query screen size, the screen size is used for computing the normalized
     // touch point.
     Display display = getWindowManager().getDefaultDisplay();
@@ -156,6 +167,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
     mLayoutDebug.setVisibility(LinearLayout.GONE);
     
     mProgressDialog = new ProgressDialog(this);
+    mProgressDialog.setCanceledOnTouchOutside(false);
     mRenderer.setProgressDialog(mProgressDialog);
 
     // Check if the Tango Core is out dated.
@@ -166,8 +178,6 @@ public class RTABMapActivity extends Activity implements OnClickListener {
     }   
     
     mOpenedDatabasePath = "";
-    mTempDatabasePath = "";
-    mNewDatabasePath = "";
     mWorkingDirectory = "";
     mTotalLoopClosures = 0;
     
@@ -177,13 +187,6 @@ public class RTABMapActivity extends Activity implements OnClickListener {
     	mWorkingDirectory = extStore.getAbsolutePath() + "/" + getString(R.string.app_name) + "/";
     	extStore = new File(mWorkingDirectory);
     	extStore.mkdirs();
-    	mTempDatabasePath = mWorkingDirectory + "rtabmap.tmp.db";
-    	extStore = new File(mTempDatabasePath);
-    	
-    	if(extStore.exists())
-    	{
-    		extStore.delete();
-    	}   	
     }
     else
     {
@@ -194,7 +197,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
     }
     
     RTABMapLib.onCreate(this);
-    RTABMapLib.openDatabase(mTempDatabasePath);
+    RTABMapLib.openEmptyDatabase();
   }
   
   @Override
@@ -213,6 +216,21 @@ public class RTABMapActivity extends Activity implements OnClickListener {
   @Override
   protected void onResume() {
     super.onResume();
+
+    mProgressDialog.setTitle("");
+	mProgressDialog.setMessage(String.format("Hold Tight! Initializing Tango Service..."));
+	mProgressDialog.show();
+    
+	if(mOnPause)
+	{
+		mToast.makeText(this, "Mapping is paused!", mToast.LENGTH_LONG).show();
+	}
+	else
+	{
+		mToast.makeText(this, "Tip: If the camera is still drifting just after the mapping has started, do \"Reset\".", mToast.LENGTH_LONG).show();
+	}
+	
+	mOnPause = false;
     
     TangoInitializationHelper.bindTangoService(this, mTangoServiceConnection);
     
@@ -221,16 +239,6 @@ public class RTABMapActivity extends Activity implements OnClickListener {
     if (Tango.hasPermission(this, Tango.PERMISSIONTYPE_MOTION_TRACKING)) {
 
     	mGLView.onResume();
-        
-        mTotalLoopClosures = 0;
-        if(mItemOpen != null)
-        {
-    	    mItemOpen.setEnabled(false);
-    	    mItemPause.setChecked(false);
-    	    mItemSave.setEnabled(false);
-    		mItemExport.setEnabled(false);
-    		mItemPostProcessing.setEnabled(false);
-        }
 
     } else {
     	Log.i(TAG, String.format("Asking for motion tracking permission"));
@@ -243,14 +251,20 @@ public class RTABMapActivity extends Activity implements OnClickListener {
   @Override
   protected void onPause() {
     super.onPause();
+    
+    // This deletes OpenGL context!
     mGLView.onPause();
     
-    // Delete all the non-OpenGl resources.
+    mOnPause = true;
+    
     RTABMapLib.onPause();
-    mOpenedDatabasePath = "";
-    RTABMapLib.openDatabase(mTempDatabasePath);
     
     unbindService(mTangoServiceConnection);
+    
+    if(!mItemPause.isChecked())
+    {
+    	onOptionsItemSelected(mItemPause);
+    }
   }
 
   @Override
@@ -316,10 +330,16 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	  mItemExport = menu.findItem(R.id.export);
 	  mItemLocalizationMode = menu.findItem(R.id.localization_mode);
 	  mItemTrajectoryMode = menu.findItem(R.id.trajectory_mode);
+	  mItemAppendMode = menu.findItem(R.id.append);
+	  mItemRenderingPointCloud = menu.findItem(R.id.point_cloud);
+	  mItemRenderingMesh = menu.findItem(R.id.mesh);
+	  mItemRenderingTextureMesh = menu.findItem(R.id.texture_mesh);
+	  mItemDataRecorderMode = menu.findItem(R.id.data_recorder);
 	  mItemSave.setEnabled(false);
 	  mItemExport.setEnabled(false);
 	  mItemOpen.setEnabled(false);
 	  mItemPostProcessing.setEnabled(false);
+	  mItemDataRecorderMode.setEnabled(false);
 	  
 	  return true;
   }
@@ -342,7 +362,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
   {
 	  if(mItemPause!=null)
 	  {
-		  ((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.status)).setText(mItemPause.isChecked()?"Paused":mItemLocalizationMode.isChecked()?String.format("Localization (%s Hz)", mUpdateRateValues[mParamUpdateRateHzIndex]):mItemDataRecorderMode.isChecked()?String.format("Recording (%s Hz)", mUpdateRateValues[mParamUpdateRateHzIndex]):String.format("Mapping (%s Hz)", mUpdateRateValues[mParamUpdateRateHzIndex]));
 	  }
 	  
 	  ((TextView)findViewById(R.id.points)).setText(String.valueOf(points));
@@ -407,108 +427,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	  Log.i(TAG, String.format("rtabmapInitEventsUI() status=%d msg=%s", status, msg));
 	  
 	  ((TextView)findViewById(R.id.status)).setText(
-			  status == 1 && msg.isEmpty()?mItemPause!=null&&mItemPause.isChecked()?"Paused":mItemLocalizationMode!=null&&mItemLocalizationMode.isChecked()?"Localization":"Mapping":msg);
+			  status == 1 && msg.isEmpty()?(mItemPause!=null&&mItemPause.isChecked()?"Paused":mItemLocalizationMode!=null&&mItemLocalizationMode.isChecked()?"Localization":mItemDataRecorderMode!=null&&mItemDataRecorderMode.isChecked()?"Recording":"Mapping"):msg);
-	  
-	  /*0=kInitializing,
-		1=kInitialized,
-		2=kClosing,
-		3=kClosed,
-		4=kInfo,
-		5=kError*/
-	  
-		if(status == 3)
-		{
-			msg = "";
-			if(!mNewDatabasePath.isEmpty())
-			{
-				boolean removed = true;
-				File outputFile = new File(mNewDatabasePath);
-				if(outputFile.exists())
-				{
-					removed = outputFile.delete();
-				}
-				if(removed)
-				{
-					File tempFile = new File(mTempDatabasePath);
-					if(tempFile.renameTo(outputFile))
-					{
-						msg = String.format("Database saved to \"%s\".", mNewDatabasePath);
-						
-						Intent intent = new Intent(this, RTABMapActivity.class);
-						// use System.currentTimeMillis() to have a unique ID for the pending intent
-						PendingIntent pIntent = PendingIntent.getActivity(this, (int) System.currentTimeMillis(), intent, 0);
-
-						// build notification
-						// the addAction re-use the same intent to keep the example short
-						Notification n  = new Notification.Builder(this)
-						        .setContentTitle(getString(R.string.app_name))
-						        .setContentText(mNewDatabasePath + " saved!")
-						        .setSmallIcon(R.drawable.ic_launcher)
-						        .setContentIntent(pIntent)
-						        .setAutoCancel(true).build();
-						    
-						  
-						NotificationManager notificationManager = 
-						  (NotificationManager) getSystemService(NOTIFICATION_SERVICE);
-
-						notificationManager.notify(0, n); 
-					}
-					else
-					{
-						msg = String.format("Failed to rename temporary database from \"%s\" to \"%s\".",
-								mTempDatabasePath, mNewDatabasePath);
-					}
-				}
-				else
-				{
-					msg = String.format("Failed to overwrite the database \"%s\". The temporary database is still correctly saved at \"%s\".",
-							mNewDatabasePath, mTempDatabasePath);
-				}
-			}
-			else if(!mOpenedDatabasePath.isEmpty())
-			{
-				msg = String.format("Database \"%s\" updated.", mOpenedDatabasePath);
-			}
-
-			if(!msg.isEmpty())
-			{
-				mToast.makeText(this, msg, mToast.LENGTH_LONG).show();
-			}
-			
-			mOpenedDatabasePath = "";
-			mNewDatabasePath = "";
-			
-			//restart a new scan by default
-			RTABMapLib.openDatabase(mTempDatabasePath);
-			
-			((TextView)findViewById(R.id.points)).setText(String.valueOf(0));
-			((TextView)findViewById(R.id.polygons)).setText(String.valueOf(0));
-			((TextView)findViewById(R.id.nodes)).setText(String.valueOf(0));
-			((TextView)findViewById(R.id.words)).setText(String.valueOf(0));
-			((TextView)findViewById(R.id.memory)).setText(String.valueOf(Debug.getNativeHeapAllocatedSize()/(1024*1024)));
-			((TextView)findViewById(R.id.db_size)).setText(String.valueOf(0));
-			((TextView)findViewById(R.id.inliers)).setText(String.valueOf(0));
-			((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));
-			
-			if(mItemSave!=null)
-			{
-				if(mItemPause.isChecked())
-				{
-					mItemPause.setChecked(false);
-					mItemOpen.setEnabled(false);
-					mItemPostProcessing.setEnabled(false);
-					mItemSave.setEnabled(false);
-					mItemExport.setEnabled(false);
-					RTABMapLib.setPausedMapping(false); // resume mapping
-				}
-			}
-			mProgressDialog.dismiss();
-		}
   }
   
   //called from jni
@@ -623,6 +542,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 
 	        };
 	        fileList = path.list(filter);
+	        Arrays.sort(fileList);
 	    }
 	    else {
 	    	fileList = new String[0];
@@ -637,21 +557,62 @@ public class RTABMapActivity extends Activity implements OnClickListener {
       {
     	  item.setChecked(!item.isChecked());
     	  mItemSave.setEnabled(item.isChecked());
-		  mItemExport.setEnabled(item.isChecked());
+		  mItemExport.setEnabled(item.isChecked() && !mItemDataRecorderMode.isChecked());
-    	  mItemOpen.setEnabled(item.isChecked());
+    	  mItemOpen.setEnabled(item.isChecked() && !mItemDataRecorderMode.isChecked());
-    	  mItemPostProcessing.setEnabled(item.isChecked());
+    	  mItemPostProcessing.setEnabled(item.isChecked() && !mItemDataRecorderMode.isChecked());
-    	 // mItemSave.setEnabled(item.isChecked() && !mWorkingDirectory.isEmpty());
+    	  mItemDataRecorderMode.setEnabled(item.isChecked());
+    	 
     	  if(item.isChecked())
     	  {
     		  RTABMapLib.setPausedMapping(true);
     		  ((TextView)findViewById(R.id.status)).setText("Paused");
+    		  if(mPauseFirstTime && !mOnPause)
+    		  {
+    			  mPauseFirstTime = false;
+    			  mToast.makeText(getActivity(), String.format("Tip: Try \"Post-Processing...\" to optimize even more the map!"), mToast.LENGTH_LONG).show();
+    		  }
+    		  mMapIsEmpty = false;
     	  }
     	  else
     	  {
     		  RTABMapLib.setPausedMapping(false);
-    		  ((TextView)findViewById(R.id.status)).setText(mItemLocalizationMode.isChecked()?"Localization":"Mapping");
+    		  ((TextView)findViewById(R.id.status)).setText(mItemLocalizationMode.isChecked()?"Localization":mItemDataRecorderMode.isChecked()?"Recording":"Mapping");
+    		  if(mItemDataRecorderMode.isChecked())
+    		  {
+    			  mToast.makeText(getActivity(), String.format("Data Recorder Mode: no map is created, only raw data is recorded."), mToast.LENGTH_LONG).show();
+    		  }
+    		  else if(!mMapIsEmpty)
+    		  {
+	    		  mToast.makeText(getActivity(), String.format("On resume, a new map is created. Tip: Try relocalizing in the previous area."), mToast.LENGTH_LONG).show();
+    		  }
     	  }
       } 
+      else if (itemId == R.id.post_processing_standard)
+      {
+			mProgressDialog.setTitle("Post-Processing");
+			mProgressDialog.setMessage(String.format("Please wait while optimizing..."));
+			mProgressDialog.show();
+			
+			Thread workingThread = new Thread(new Runnable() {
+				    public void run() {
+				    	final int loopDetected = RTABMapLib.postProcessing(-1);
+				    	runOnUiThread(new Runnable() {
+				    		public void run() {
+								if(loopDetected >= 0)
+								{
+									mTotalLoopClosures+=loopDetected;
+									mProgressDialog.setMessage(String.format("Optimization done! Increasing visual appeal..."));
+								}
+								else if(loopDetected < 0)
+								{
+									mToast.makeText(getActivity(), String.format("Optimization failed!"), mToast.LENGTH_SHORT).show();
+								}
+					    	}
+				    	});
+				    } 
+			});
+			workingThread.start();
+      }
       else if (itemId == R.id.detect_more_loop_closures)
       {
 			mProgressDialog.setTitle("Post-Processing");
@@ -679,6 +640,32 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			});
 			workingThread.start();
       }
+      else if (itemId == R.id.icp_refining)
+      {
+			mProgressDialog.setTitle("Post-Processing");
+			mProgressDialog.setMessage(String.format("Please wait while refining links..."));
+			mProgressDialog.show();
+			
+			Thread workingThread = new Thread(new Runnable() {
+				    public void run() {
+				    	final int linksRefined = RTABMapLib.postProcessing(3);
+				    	runOnUiThread(new Runnable() {
+				    		public void run() {
+								mProgressDialog.dismiss();
+								if(linksRefined >= 0)
+								{
+									mToast.makeText(getActivity(), String.format("Refining done! %d link(s) refined.", linksRefined), mToast.LENGTH_SHORT).show();
+								}
+								else if(linksRefined < 0)
+								{
+									mToast.makeText(getActivity(), String.format("Refining failed!"), mToast.LENGTH_SHORT).show();
+								}
+				    		}
+				    	});
+				    } 
+			});
+			workingThread.start();
+      }
       else if (itemId == R.id.global_graph_optimization)
       {
 			mProgressDialog.setTitle("Post-Processing");
@@ -705,6 +692,34 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			});
 			workingThread.start();
       }
+      else if (itemId == R.id.polygons_filtering)
+      {		
+    	  mProgressDialog.setTitle("Post-Processing");
+		  mProgressDialog.setMessage(String.format("Noise filtering..."));
+		  mProgressDialog.show();
+    	  RTABMapLib.postProcessing(4);
+      }
+      else if (itemId == R.id.gain_compensation_fast)
+      {		
+    	  mProgressDialog.setTitle("Post-Processing");
+		  mProgressDialog.setMessage(String.format("Fast gain compensation..."));
+		  mProgressDialog.show();
+    	  RTABMapLib.postProcessing(5);
+      }
+      else if (itemId == R.id.gain_compensation_full)
+      {		
+    	  mProgressDialog.setTitle("Post-Processing");
+		  mProgressDialog.setMessage(String.format("Full gain compensation..."));
+		  mProgressDialog.show();
+    	  RTABMapLib.postProcessing(6);
+      }
+      else if (itemId == R.id.bilateral_filtering)
+      {		
+    	  mProgressDialog.setTitle("Post-Processing");
+		  mProgressDialog.setMessage(String.format("Mesh smoothing..."));
+		  mProgressDialog.show();
+    	  RTABMapLib.postProcessing(7);
+      }
       else if (itemId == R.id.sba)
       {
 			mProgressDialog.setTitle("Post-Processing");
@@ -743,10 +758,12 @@ public class RTABMapActivity extends Activity implements OnClickListener {
     		  mLayoutDebug.setVisibility(LinearLayout.VISIBLE);
     	  }
       }
-      else if(itemId == R.id.mesh_rendering)
+      else if(itemId == R.id.mesh || itemId == R.id.texture_mesh || itemId == R.id.point_cloud)
       {
-    	  item.setChecked(!item.isChecked());
+    	  item.setChecked(true);
-    	  RTABMapLib.setMeshRendering(item.isChecked());
+    	  RTABMapLib.setMeshRendering(
+    			  mItemRenderingMesh.isChecked() || mItemRenderingTextureMesh.isChecked(), 
+    			  mItemRenderingTextureMesh.isChecked());
       }
       else if(itemId == R.id.map_shown)
       {
@@ -778,11 +795,25 @@ public class RTABMapActivity extends Activity implements OnClickListener {
     	  item.setChecked(!item.isChecked());
     	  RTABMapLib.setNodesFiltering(item.isChecked());
       }
+      else if(itemId == R.id.drift_correction)
+      {
+    	  item.setChecked(!item.isChecked());
+    	  RTABMapLib.setDriftCorrection(item.isChecked());
+    	  if(item.isChecked())
+    	  {
+    		  mToast.makeText(getActivity(), String.format("Tip: With drift correction is enabled, move slowly to get better results."), mToast.LENGTH_LONG).show();
+    	  }
+      }
       else if(itemId == R.id.graph_visible)
       {
     	  item.setChecked(!item.isChecked());
     	  RTABMapLib.setGraphVisible(item.isChecked());
       }
+      else if(itemId == R.id.grid_visible)
+      {
+    	  item.setChecked(!item.isChecked());
+    	  RTABMapLib.setGridVisible(item.isChecked());
+      }
       else if(itemId == R.id.auto_exposure)
       {
     	  item.setChecked(!item.isChecked());
@@ -797,6 +828,11 @@ public class RTABMapActivity extends Activity implements OnClickListener {
     	  item.setChecked(!item.isChecked());
     	  RTABMapLib.setFullResolution(item.isChecked());
       }
+      else if(itemId == R.id.append)
+      {
+    	  item.setChecked(!item.isChecked());
+    	  RTABMapLib.setAppendMode(item.isChecked());
+      }
       else if(itemId == R.id.max_depth)
       {
     	  // get double
@@ -816,6 +852,25 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		  });
 		  builder.show();
       }
+      else if(itemId == R.id.mesh_decimation)
+      {
+    	  // get double
+		  AlertDialog.Builder builder = new AlertDialog.Builder(this);
+		  builder.setTitle("Mesh Decimation");
+		  final String[] values = {"Disabled", "Medium", "High"};
+		  builder.setSingleChoiceItems(values, mMeshDecimationIndex, new DialogInterface.OnClickListener() {
+		    @Override
+		    public void onClick(DialogInterface dialog, int which) {
+		        dialog.dismiss();
+		        if(which >=0 && which <= 2)
+		        {
+		        	mMeshDecimationIndex = which;
+			        RTABMapLib.setMeshDecimation(which);
+		        }
+		    }
+		  });
+		  builder.show();
+      }
       else if(itemId == R.id.mesh_angle_tolerance)
       {
     	  // get double
@@ -866,7 +921,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		        if(which >=0 && which < mUpdateRateValues.length)
 		        {
 		        	mParamUpdateRateHzIndex = which;
-			        if(RTABMapLib.setMappingParameter("Rtabmap/DetectionRate", mUpdateRateValues[which]) != 0)
+			        if(RTABMapLib.setMappingParameter("Rtabmap/DetectionRate", which == mUpdateRateValues.length-1?"0":mUpdateRateValues[which]) != 0)
 			        {
 			        	mToast.makeText(getActivity(), "Failed to set parameter \"Rtabmap/DetectionRate\"!", mToast.LENGTH_LONG).show();
 			        }
@@ -961,83 +1016,165 @@ public class RTABMapActivity extends Activity implements OnClickListener {
       }
       else if (itemId == R.id.save)
       {
-    	  if(mOpenedDatabasePath.isEmpty())
+		  AlertDialog.Builder builder = new AlertDialog.Builder(this);
-    	  {
+		  builder.setTitle("RTAB-Map Database Name (*.db):");
-			  AlertDialog.Builder builder = new AlertDialog.Builder(this);
+		  final EditText input = new EditText(this);
-			  builder.setTitle("RTAB-Map Database Name (*.db):");
+		  input.setInputType(InputType.TYPE_CLASS_TEXT); 
-			  final EditText input = new EditText(this);
+		  if(mOpenedDatabasePath.isEmpty())
-			  input.setInputType(InputType.TYPE_CLASS_TEXT);        
+		  {
-			  builder.setView(input);
+			  String timeStamp = new SimpleDateFormat("yyMMdd-hhmmss").format(new Date());
-			  builder.setPositiveButton("OK", new DialogInterface.OnClickListener() {
+			  input.setText(timeStamp);
-				  @Override
+		  }
-				  public void onClick(DialogInterface dialog, int which)
+		  else
+		  {
+			  File f = new File(mOpenedDatabasePath);
+			  String name = f.getName();
+			  input.setText(name.substring(0,name.lastIndexOf(".")));
+		  }
+		  input.setSelectAllOnFocus(true);
+		  input.selectAll();
+		  builder.setView(input);
+		  builder.setPositiveButton("OK", new DialogInterface.OnClickListener() {
+			  @Override
+			  public void onClick(DialogInterface dialog, int which)
+			  {
+				  final String fileName = input.getText().toString();  
+				  dialog.dismiss();
+				  if(!fileName.isEmpty())
 				  {
-					  final String fileName = input.getText().toString();  
+					  File newFile = new File(mWorkingDirectory + fileName + ".db");
-					  dialog.dismiss();
+					  if(newFile.exists())
-					  if(!fileName.isEmpty())
 					  {
-						  File newFile = new File(mWorkingDirectory + fileName + ".db");
+						  new AlertDialog.Builder(getActivity())
-						  if(newFile.exists())
+			                .setTitle("File Already Exists")
-						  {
+			                .setMessage("Do you want to overwrite the existing file?")
-							  new AlertDialog.Builder(getActivity())
+			                .setPositiveButton("Yes", new DialogInterface.OnClickListener() {
-				                .setTitle("File Already Exists")
+			                    public void onClick(DialogInterface dialog, int which) {
-				                .setMessage("Do you want to overwrite the existing file?")
+			                    	
-				                .setPositiveButton("Yes", new DialogInterface.OnClickListener() {
+			                    	final String newDatabasePath = mWorkingDirectory + fileName + ".db";
-				                    public void onClick(DialogInterface dialog, int which) {
+			                    	mProgressDialog.setTitle("Saving");
-				                    	mNewDatabasePath = mWorkingDirectory + fileName + ".db";
+			                    	if(mOpenedDatabasePath.equals(newDatabasePath))
-				                    	
+			                    	{
-				                    	mProgressDialog.setTitle("Saving");
+			                    		mProgressDialog.setMessage(String.format("Please wait while updating \"%s\"...", newDatabasePath));
-				                  	    mProgressDialog.setMessage(String.format("Please wait while saving \"%s\"...", mNewDatabasePath));
+			                    	}
-				                  	    mProgressDialog.show();
+			                    	else
-				                    	
+			                    	{
-										RTABMapLib.save(); // send save event
+			                    		mProgressDialog.setMessage(String.format("Please wait while saving \"%s\"...", newDatabasePath));
-										//disable gui actions
+			                    	}
-										mItemSave.setEnabled(false);
+			                  	    mProgressDialog.show();
-										mItemOpen.setEnabled(false);
+			                    	
-										mItemPostProcessing.setEnabled(false);
+			                  	  Thread saveThread = new Thread(new Runnable() {
-										mItemExport.setEnabled(false);
+			                		    public void run() {
-				                    }
+			                		    	RTABMapLib.save(newDatabasePath); // save
-				                })
+			                		    	runOnUiThread(new Runnable() {
-				                .setNegativeButton("No", new DialogInterface.OnClickListener() {
+			                		    		public void run() {
-				                    public void onClick(DialogInterface dialog, int which) {
+				                		    		if(mOpenedDatabasePath.equals(newDatabasePath))
-				                        dialog.dismiss();
+				        							{
-				                    }
+				        								mToast.makeText(getActivity(), String.format("Database \"%s\" updated.", newDatabasePath), mToast.LENGTH_LONG).show();
-				                })
+				        							}
-				                .show();
+				        							else
-						  }
+				        							{
-						  else
+				        								mToast.makeText(getActivity(), String.format("Database saved to \"%s\".", newDatabasePath), mToast.LENGTH_LONG).show();
-						  {
+				        							
-							  mNewDatabasePath = mWorkingDirectory + fileName + ".db";
+				        								Intent intent = new Intent(getActivity(), RTABMapActivity.class);
-							  
+				        								// use System.currentTimeMillis() to have a unique ID for the pending intent
-							  mProgressDialog.setTitle("Saving");
+				        								PendingIntent pIntent = PendingIntent.getActivity(getActivity(), (int) System.currentTimeMillis(), intent, 0);
-				        	  mProgressDialog.setMessage(String.format("Please wait while saving \"%s\"...", mNewDatabasePath));
+
-				        	  mProgressDialog.show();
+				        								// build notification
-							  
+				        								// the addAction re-use the same intent to keep the example short
-							  RTABMapLib.save(); // send save event
+				        								Notification n  = new Notification.Builder(getActivity())
-							  //disable gui actions
+				        								        .setContentTitle(getString(R.string.app_name))
-							  mItemSave.setEnabled(false);
+				        								        .setContentText(newDatabasePath + " saved!")
-							  mItemOpen.setEnabled(false);
+				        								        .setSmallIcon(R.drawable.ic_launcher)
-							  mItemPostProcessing.setEnabled(false);
+				        								        .setContentIntent(pIntent)
-							  mItemExport.setEnabled(false);
+				        								        .setAutoCancel(true).build();
-						  }
+				        								    
+				        								  
+				        								NotificationManager notificationManager = 
+				        								  (NotificationManager) getSystemService(NOTIFICATION_SERVICE);
+
+				        								notificationManager.notify(0, n); 
+				        							}
+				                		    		if(!mItemDataRecorderMode.isChecked())
+				                		    		{
+				                		    			mOpenedDatabasePath = newDatabasePath;
+				                		    		}
+					                		    	mProgressDialog.dismiss();
+			                		    		}
+			                		    	});
+			                		    } 
+			                		});
+					        	    saveThread.start();
+								}
+			                })
+			                .setNegativeButton("No", new DialogInterface.OnClickListener() {
+			                    public void onClick(DialogInterface dialog, int which) {
+			                        dialog.dismiss();
+			                    }
+			                })
+			                .show();
+					  }
+					  else
+					  {
+						    final String newDatabasePath = mWorkingDirectory + fileName + ".db";
+	                        mProgressDialog.setTitle("Saving");
+	                        if(mOpenedDatabasePath.equals(newDatabasePath))
+	                        {
+	                        	mProgressDialog.setMessage(String.format("Please wait while updating \"%s\"...", mOpenedDatabasePath));
+	                        }
+	                        else
+	                        {
+	                   	  		mProgressDialog.setMessage(String.format("Please wait while saving \"%s\"...", newDatabasePath));
+	                        }
+			        	    mProgressDialog.show();
+			        	    
+			        	    Thread saveThread = new Thread(new Runnable() {
+	                		    public void run() {
+	                		    	RTABMapLib.save(newDatabasePath); // save
+	                		    	runOnUiThread(new Runnable() {
+	                		    		public void run() {
+		                		    		if(mOpenedDatabasePath.equals(newDatabasePath))
+		        							{
+		        								mToast.makeText(getActivity(), String.format("Database \"%s\" updated.", newDatabasePath), mToast.LENGTH_LONG).show();
+		        							}
+		        							else
+		        							{
+		        								mToast.makeText(getActivity(), String.format("Database saved to \"%s\".", newDatabasePath), mToast.LENGTH_LONG).show();
+		        							
+		        								Intent intent = new Intent(getActivity(), RTABMapActivity.class);
+		        								// use System.currentTimeMillis() to have a unique ID for the pending intent
+		        								PendingIntent pIntent = PendingIntent.getActivity(getActivity(), (int) System.currentTimeMillis(), intent, 0);
+
+		        								// build notification
+		        								// the addAction re-use the same intent to keep the example short
+		        								Notification n  = new Notification.Builder(getActivity())
+		        								        .setContentTitle(getString(R.string.app_name))
+		        								        .setContentText(newDatabasePath + " saved!")
+		        								        .setSmallIcon(R.drawable.ic_launcher)
+		        								        .setContentIntent(pIntent)
+		        								        .setAutoCancel(true).build();
+		        								    
+		        								  
+		        								NotificationManager notificationManager = 
+		        								  (NotificationManager) getSystemService(NOTIFICATION_SERVICE);
+
+		        								notificationManager.notify(0, n); 
+		        							}
+		                		    		if(!mItemDataRecorderMode.isChecked())
+		                		    		{
+		                		    			mOpenedDatabasePath = newDatabasePath;
+		                		    		}
+			                		    	mProgressDialog.dismiss();
+	                		    		}
+	                		    	});
+	                		    } 
+	                		});
+			        	    saveThread.start();
 					  }
 				  }
-			  });
+			  }
-			  builder.show();
+		  });
-    	  }
+		  builder.show();
-    	  else
-    	  {
-    		  mProgressDialog.setTitle("Saving");
-        	  mProgressDialog.setMessage(String.format("Please wait while updating \"%s\"...", mOpenedDatabasePath));
-        	  mProgressDialog.show();
-        	  
-    		  RTABMapLib.save(); // send save event
-    		  //disable gui actions
-			  mItemSave.setEnabled(false);
-			  mItemOpen.setEnabled(false);
-			  mItemPostProcessing.setEnabled(false);
-			  mItemExport.setEnabled(false);
-    	  }
       }
       else if(itemId == R.id.reset)
       {
@@ -1062,8 +1199,62 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		  else
 		  {
 			  mOpenedDatabasePath = "";
-        	  RTABMapLib.openDatabase(mTempDatabasePath);
+        	  RTABMapLib.openEmptyDatabase();
 		  }
+		  mMapIsEmpty = true;
+      }
+      else if(itemId == R.id.data_recorder)
+      {
+		  final boolean dataRecorderOldState = item.isChecked();
+		  new AlertDialog.Builder(getActivity())
+          .setTitle("Data Recorder Mode")
+          .setMessage("Changing from/to data recorder mode will close the current session. Do you want to continue?")
+          .setPositiveButton("Yes", new DialogInterface.OnClickListener() {
+              public void onClick(DialogInterface dialog, int which) {           	  
+            	  // reset
+            	  ((TextView)findViewById(R.id.points)).setText(String.valueOf(0));
+            	  ((TextView)findViewById(R.id.polygons)).setText(String.valueOf(0));
+            	  ((TextView)findViewById(R.id.nodes)).setText(String.valueOf(0));
+            	  ((TextView)findViewById(R.id.words)).setText(String.valueOf(0));
+            	  ((TextView)findViewById(R.id.memory)).setText(String.valueOf(Debug.getNativeHeapAllocatedSize()/(1024*1024)));
+            	  ((TextView)findViewById(R.id.db_size)).setText(String.valueOf(0));
+            	  ((TextView)findViewById(R.id.inliers)).setText(String.valueOf(0));
+            	  ((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));
+            	   
+        		  mItemDataRecorderMode.setChecked(!dataRecorderOldState);
+            	  RTABMapLib.setDataRecorderMode(mItemDataRecorderMode.isChecked());
+            	  
+            	  mOpenedDatabasePath = "";
+        		  RTABMapLib.openEmptyDatabase();
+        		  
+        		  mItemOpen.setEnabled(!mItemDataRecorderMode.isChecked() && mItemPause.isChecked());
+    			  mItemPostProcessing.setEnabled(!mItemDataRecorderMode.isChecked() && mItemPause.isChecked());
+    			  mItemExport.setEnabled(!mItemDataRecorderMode.isChecked() && mItemPause.isChecked());
+    			  
+    			  mItemLocalizationMode.setEnabled(!mItemDataRecorderMode.isChecked());
+    			  mItemAppendMode.setEnabled(!mItemDataRecorderMode.isChecked());   			   
+            	  
+            	  if(mItemDataRecorderMode.isChecked())
+            	  {
+            		  mToast.makeText(getActivity(), String.format("Data recorder mode activated! Tip: You can increase data update rate in Parameters menu under Mapping options."), mToast.LENGTH_LONG).show();
+            	  }
+            	  else
+            	  {
+            		  mToast.makeText(getActivity(), String.format("Data recorder mode deactivated!"), mToast.LENGTH_LONG).show();
+            	  }
+              }
+          })
+          .setNegativeButton("No", new DialogInterface.OnClickListener() {
+              public void onClick(DialogInterface dialog, int which) {
+                  dialog.dismiss();
+              }
+          })
+          .show();
       }
       else if(itemId == R.id.export_obj || itemId == R.id.export_ply)
       {
@@ -1077,6 +1268,19 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		  final EditText input = new EditText(this);
 		  input.setInputType(InputType.TYPE_CLASS_TEXT);        
 		  builder.setView(input);
+		  if(mOpenedDatabasePath.isEmpty())
+		  {
+			  String timeStamp = new SimpleDateFormat("yyMMdd-hhmmss").format(new Date());
+			  input.setText(timeStamp);
+		  }
+		  else
+		  {
+			  File f = new File(mOpenedDatabasePath);
+			  String name = f.getName();
+			  input.setText(name.substring(0,name.lastIndexOf(".")));
+		  }
+		  input.setSelectAllOnFocus(true);
+		  input.selectAll();
 		  builder.setPositiveButton("OK", new DialogInterface.OnClickListener() {
 			  @Override
 			  public void onClick(DialogInterface dialog, int which)
@@ -1130,6 +1334,24 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 					                		    		{
 					                		    			mToast.makeText(getActivity(), String.format("Mesh \"%s\" successfully exported!", path), mToast.LENGTH_LONG).show();
 					                		    		}
+					                		    		Intent intent = new Intent(getActivity(), RTABMapActivity.class);
+				        								// use System.currentTimeMillis() to have a unique ID for the pending intent
+				        								PendingIntent pIntent = PendingIntent.getActivity(getActivity(), (int) System.currentTimeMillis(), intent, 0);
+
+				        								// build notification
+				        								// the addAction re-use the same intent to keep the example short
+				        								Notification n  = new Notification.Builder(getActivity())
+				        								        .setContentTitle(getString(R.string.app_name))
+				        								        .setContentText(path + " exported!")
+				        								        .setSmallIcon(R.drawable.ic_launcher)
+				        								        .setContentIntent(pIntent)
+				        								        .setAutoCancel(true).build();
+				        								    
+				        								  
+				        								NotificationManager notificationManager = 
+				        								  (NotificationManager) getSystemService(NOTIFICATION_SERVICE);
+
+				        								notificationManager.notify(0, n); 
 							                    	}
 							                    	else
 							                    	{
@@ -1173,6 +1395,25 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			                		    		{
 			                		    			mToast.makeText(getActivity(), String.format("Mesh \"%s\" successfully exported!", path), mToast.LENGTH_LONG).show();
 			                		    		}
+			                		    		
+			                		    		Intent intent = new Intent(getActivity(), RTABMapActivity.class);
+		        								// use System.currentTimeMillis() to have a unique ID for the pending intent
+		        								PendingIntent pIntent = PendingIntent.getActivity(getActivity(), (int) System.currentTimeMillis(), intent, 0);
+
+		        								// build notification
+		        								// the addAction re-use the same intent to keep the example short
+		        								Notification n  = new Notification.Builder(getActivity())
+		        								        .setContentTitle(getString(R.string.app_name))
+		        								        .setContentText(path + " exported!")
+		        								        .setSmallIcon(R.drawable.ic_launcher)
+		        								        .setContentIntent(pIntent)
+		        								        .setAutoCancel(true).build();
+		        								    
+		        								  
+		        								NotificationManager notificationManager = 
+		        								  (NotificationManager) getSystemService(NOTIFICATION_SERVICE);
+
+		        								notificationManager.notify(0, n);
 					                    	}
 					                    	else
 					                    	{
@@ -1196,9 +1437,16 @@ public class RTABMapActivity extends Activity implements OnClickListener {
     	  final String[] files = loadFileList(mWorkingDirectory);
     	  if(files.length > 0)
     	  {
+    		  String[] filesWithSize = new String[files.length];
+    		  for(int i = 0; i<filesWithSize.length; ++i)
+  	          {
+  	        	  File filePath = new File(mWorkingDirectory+files[i]);
+  	        	  long mb = filePath.length()/(1024*1024);
+  	        	  filesWithSize[i] = files[i] + " ("+mb+" MB)";
+  	          }
     		  AlertDialog.Builder builder = new AlertDialog.Builder(this);
     		  builder.setTitle("Choose your file");
-              builder.setItems(files, new DialogInterface.OnClickListener() {
+              builder.setItems(filesWithSize, new DialogInterface.OnClickListener() {
                   public void onClick(DialogInterface dialog, int which) {
                 	  mOpenedDatabasePath = mWorkingDirectory + files[which];
                 	  
@@ -1211,12 +1459,6 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 
                 	  RTABMapLib.openDatabase(mOpenedDatabasePath);
                 	  RTABMapLib.setCamera(1);
-                	  
-                	  File extStore = new File(mTempDatabasePath);
-                  	  if(extStore.exists())
-                  	  {
-                          extStore.delete();
-                  	  }  
                   }
               });
               builder.show();

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

@@ -25,6 +25,7 @@ public class RTABMapLib
     // The activity object is used for checking if the API version is outdated.
     public static native void onCreate(RTABMapActivity activity);
     
+    public static native void openEmptyDatabase();
     public static native void openDatabase(String databasePath);
     
     /*
@@ -58,21 +59,26 @@ public class RTABMapLib
     public static native void setPausedMapping(boolean paused);
     public static native void setMapCloudShown(boolean shown);
     public static native void setOdomCloudShown(boolean shown);
-    public static native void setMeshRendering(boolean enabled);
+    public static native void setMeshRendering(boolean enabled, boolean withTexture);
     public static native void setLocalizationMode(boolean enabled);
     public static native void setTrajectoryMode(boolean enabled);
     public static native void setGraphOptimization(boolean enabled);
     public static native void setNodesFiltering(boolean enabled);
+    public static native void setDriftCorrection(boolean enabled);
     public static native void setGraphVisible(boolean visible);
+    public static native void setGridVisible(boolean visible);
     public static native void setAutoExposure(boolean enabled);
     public static native void setFullResolution(boolean enabled);
+    public static native void setAppendMode(boolean enabled);
+    public static native void setDataRecorderMode(boolean enabled);
     public static native void setMaxCloudDepth(float value);
+    public static native void setMeshDecimation(int 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 void save(String outputDatabasePath);
     public static native boolean exportMesh(String filePath);
     public static native int postProcessing(int approach);
     

app/src/ObjDeletionHandler.h

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

app/src/main.cpp

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -47,6 +47,8 @@ int main(int argc, char* argv[])
 
 	/* Create tasks */
 	QApplication * app = new QApplication(argc, argv);
+
+	ParametersMap parameters = Parameters::parseArguments(argc, argv, true);
 	MainWindow * mainWindow = new MainWindow();
     app->installEventFilter(mainWindow); // to catch FileOpen events.
     
@@ -82,7 +84,11 @@ int main(int argc, char* argv[])
     
     if(!database.empty())
     {
-    	QMetaObject::invokeMethod(mainWindow, "openDatabase", Qt::QueuedConnection, Q_ARG(QString, QString(database.c_str())));
+    	mainWindow->openDatabase(database.c_str());
+    }
+    if(parameters.size())
+    {
+    	mainWindow->updateParameters(parameters);
     }
 
 	// Now wait for application to finish

cmake_modules/FindDC1394.cmake

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

cmake_modules/FindG2O.cmake

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

cmake_modules/FindRealSense.cmake

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

cmake_modules/FindTango.cmake

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

corelib/include/rtabmap/core/BayesFilter.h

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

corelib/include/rtabmap/core/Camera.h

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

corelib/include/rtabmap/core/CameraEvent.h

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

corelib/include/rtabmap/core/CameraInfo.h

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

corelib/include/rtabmap/core/CameraModel.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -76,7 +76,8 @@ public:
 
 	void initRectificationMap();
 
-	bool isValidForProjection() const {return fx()>0.0 && fy()>0.0;}
+	bool isValidForProjection() const {return fx()>0.0 && fy()>0.0 && cx()>0.0 && cy()>0.0;}
+	bool isValidForReprojection() const {return fx()>0.0 && fy()>0.0 && cx()>0.0 && cy()>0.0 && imageWidth()>0 && imageHeight()>0;}
 	bool isValidForRectification() const
 	{
 		return imageSize_.width>0 &&
@@ -106,7 +107,7 @@ public:
 	void setLocalTransform(const Transform & transform) {localTransform_ = transform;}
 	const Transform & localTransform() const {return localTransform_;}
 
-	void setImageSize(const cv::Size & size) {imageSize_ = size;}
+	void setImageSize(const cv::Size & size);
 	const cv::Size & imageSize() const {return imageSize_;}
 	int imageWidth() const {return imageSize_.width;}
 	int imageHeight() const {return imageSize_.height;}
@@ -115,6 +116,7 @@ public:
 	bool save(const std::string & directory) const;
 
 	CameraModel scaled(double scale) const;
+	CameraModel roi(const cv::Rect & roi) const;
 
 	double horizontalFOV() const; // in degrees
 	double verticalFOV() const;   // in degrees
@@ -123,6 +125,13 @@ public:
 	cv::Mat rectifyImage(const cv::Mat & raw, int interpolation = cv::INTER_LINEAR) const;
 	cv::Mat rectifyDepth(const cv::Mat & raw) const;
 
+	// Project 2D pixel to 3D (in /camera_link frame)
+	void project(float u, float v, float depth, float & x, float & y, float & z) const;
+	// Reproject 3D point (in /camera_link frame) to pixel
+	void reproject(float x, float y, float z, float & u, float & v) const;
+	void reproject(float x, float y, float z, int & u, int & v) const;
+	bool inFrame(int u, int v) const;
+
 private:
 	std::string name_;
 	cv::Size imageSize_;

corelib/include/rtabmap/core/CameraRGB.h

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

corelib/include/rtabmap/core/CameraRGBD.h

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

corelib/include/rtabmap/core/CameraStereo.h

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

corelib/include/rtabmap/core/CameraThread.h

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

corelib/include/rtabmap/core/Compression.h

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

corelib/include/rtabmap/core/DBDriver.h

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

corelib/include/rtabmap/core/DBReader.h

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

corelib/include/rtabmap/core/EpipolarGeometry.h

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

corelib/include/rtabmap/core/Features2d.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -133,6 +133,7 @@ public:
 
 	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints);
 	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints);
+	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & keypoints3D, cv::Mat & descriptors, int maxKeypoints);
 
 	static cv::Rect computeRoi(const cv::Mat & image, const std::string & roiRatios);
 	static cv::Rect computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios);

corelib/include/rtabmap/core/FlannIndex.h

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

corelib/include/rtabmap/core/GainCompensator.h

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

corelib/include/rtabmap/core/Graph.h

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

corelib/include/rtabmap/core/Image.h

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

corelib/include/rtabmap/core/LaserScanInfo.h

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

corelib/include/rtabmap/core/Link.h

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

corelib/include/rtabmap/core/Memory.h

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

corelib/include/rtabmap/core/OccupancyGrid.h

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

corelib/include/rtabmap/core/OctoMap.h

@@ -0,0 +1,104 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef SRC_OCTOMAP_H_
+#define SRC_OCTOMAP_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <octomap/ColorOcTree.h>
+#include <octomap/OcTreeKey.h>
+
+#include <pcl/pcl_base.h>
+#include <pcl/point_types.h>
+
+#include <rtabmap/core/Transform.h>
+
+#include <map>
+#include <string>
+
+namespace rtabmap {
+
+class OcTreeNodeInfo
+{
+public:
+	OcTreeNodeInfo(int nodeRefId, const octomap::OcTreeKey & key, bool isObstacle) :
+		nodeRefId_(nodeRefId),
+		key_(key),
+		isObstacle_(isObstacle) {}
+	int nodeRefId_;
+	octomap::OcTreeKey key_;
+	bool isObstacle_;
+};
+
+class RTABMAP_EXP OctoMap {
+public:
+	OctoMap(float voxelSize = 0.1f, float occupancyThr = 0.5f);
+
+	const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
+	void addToCache(int nodeId,
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr & ground,
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstacles,
+			const pcl::PointXYZ & viewPoint);
+	void addToCache(int nodeId,
+			const cv::Mat & ground,
+			const cv::Mat & obstacles,
+			const cv::Point3f & viewPoint);
+	void update(const std::map<int, Transform> & poses);
+
+	const octomap::ColorOcTree * octree() const {return octree_;}
+
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr createCloud(
+			unsigned int treeDepth = 0,
+			std::vector<int> * obstacleIndices = 0,
+			std::vector<int> * emptyIndices = 0) const;
+
+	cv::Mat createProjectionMap(
+			float & xMin,
+			float & yMin,
+			float & gridCellSize,
+			float minGridSize);
+
+	bool writeBinary(const std::string & path);
+
+	virtual ~OctoMap();
+	void clear();
+
+private:
+	std::map<int, std::pair<cv::Mat, cv::Mat> > cache_;
+	std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> > cacheClouds_;
+	std::map<int, cv::Point3f> cacheViewPoints_;
+	octomap::ColorOcTree * octree_;
+	std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo> occupiedCells_;
+	std::map<int, Transform> addedNodes_;
+	octomap::KeyRay keyRay_;
+	bool hasColor_;
+};
+
+} /* namespace rtabmap */
+
+#endif /* SRC_OCTOMAP_H_ */

corelib/include/rtabmap/core/Odometry.h

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

corelib/include/rtabmap/core/OdometryEvent.h

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

corelib/include/rtabmap/core/OdometryF2F.h

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

corelib/include/rtabmap/core/OdometryF2M.h

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

corelib/include/rtabmap/core/OdometryInfo.h

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

corelib/include/rtabmap/core/OdometryMono.h

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

corelib/include/rtabmap/core/OdometryThread.h

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

corelib/include/rtabmap/core/Optimizer.h

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

corelib/include/rtabmap/core/OptimizerCVSBA.h

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

corelib/include/rtabmap/core/OptimizerG2O.h

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

corelib/include/rtabmap/core/OptimizerGTSAM.h

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

corelib/include/rtabmap/core/OptimizerTORO.h

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

corelib/include/rtabmap/core/ParamEvent.h

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

corelib/include/rtabmap/core/Parameters.h

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

corelib/include/rtabmap/core/ParticleFilter.h

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

corelib/include/rtabmap/core/Registration.h

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

corelib/include/rtabmap/core/RegistrationIcp.h

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

corelib/include/rtabmap/core/RegistrationInfo.h

@@ -1,9 +1,29 @@
 /*
- * RegistrationInfo.h
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
- *
+All rights reserved.
- *  Created on: Jan 5, 2016
+
- *      Author: mathieu
+Redistribution and use in source and binary forms, with or without
- */
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
 
 #ifndef REGISTRATIONINFO_H_
 #define REGISTRATIONINFO_H_
@@ -15,14 +35,18 @@ class RegistrationInfo
 {
 public:
 	RegistrationInfo() :
-		variance(0),
+		varianceLin(0),
+		varianceAng(0),
 		inliers(0),
 		matches(0),
-		icpInliersRatio(0)
+		icpInliersRatio(0),
+		icpTranslation(0.0f),
+		icpRotation(0.0f)
 	{
 	}
 
-	float variance;
+	float varianceLin;
+	float varianceAng;
 	std::string rejectedMsg;
 
 	// RegistrationVis
@@ -33,6 +57,8 @@ public:
 
 	// RegistrationIcp
 	float icpInliersRatio;
+	float icpTranslation;
+	float icpRotation;
 };
 
 }

corelib/include/rtabmap/core/RegistrationVis.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -81,8 +81,10 @@ private:
 	int _flowMaxLevel;
 	float _nndr;
 	int _guessWinSize;
+	int _bundleAdjustment;
 
 	ParametersMap _featureParameters;
+	ParametersMap _bundleParameters;
 };
 
 }

corelib/include/rtabmap/core/Rtabmap.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -61,15 +61,23 @@ public:
 	bool process(const cv::Mat & image, int id=0); // for convenience, an id is automatically generated if id=0
 	bool process(
 			const SensorData & data,
-			const Transform & odomPose,
+			Transform odomPose,
 			const cv::Mat & covariance = cv::Mat::eye(6,6,CV_64FC1)); // for convenience
 
 	void init(const ParametersMap & parameters, const std::string & databasePath = "");
 	void init(const std::string & configFile = "", const std::string & databasePath = "");
 
-	void close(bool databaseSaved = true);
+	/**
+	 * Close rtabmap. This will delete rtabmap object if set.
+	 * @param databaseSaved true=database saved, false=database discarded.
+	 * @param databasePath output database file name, ignored if
+	 *                     Db/Sqlite3InMemory=false (opened database is
+	 *                     then overwritten).
+	 */
+	void close(bool databaseSaved = true, const std::string & ouputDatabasePath = "");
 
 	const std::string & getWorkingDir() const {return _wDir;}
+	bool isRGBDMode() const { return _rgbdSlamMode; }
 	int getLoopClosureId() const {return _loopClosureHypothesis.first;}
 	float getLoopClosureValue() const {return _loopClosureHypothesis.second;}
 	int getHighestHypothesisId() const {return _highestHypothesis.first;}
@@ -100,6 +108,13 @@ public:
 
 	int triggerNewMap();
 	bool labelLocation(int id, const std::string & label);
+	/**
+	 * Set user data. Detect automatically if raw or compressed. If raw, the data is
+	 * compressed too. A matrix of type CV_8UC1 with 1 row is considered as compressed.
+	 * If you have one dimension unsigned 8 bits raw data, make sure to transpose it
+	 * (to have multiple rows instead of multiple columns) in order to be detected as
+	 * not compressed.
+	 */
 	bool setUserData(int id, const cv::Mat & data);
 	void generateDOTGraph(const std::string & path, int id=0, int margin=5);
 	void exportPoses(
@@ -127,6 +142,7 @@ public:
 			bool global,
 			std::map<int, Signature> * signatures = 0);
 	int detectMoreLoopClosures(float clusterRadius = 0.5f, float clusterAngle = M_PI/6.0f, int iterations = 1);
+	int refineLinks();
 
 	int getPathStatus() const {return _pathStatus;} // -1=failed 0=idle/executing 1=success
 	void clearPath(int status); // -1=failed 0=idle/executing 1=success
@@ -141,7 +157,7 @@ public:
 	const Transform & getPathTransformToGoal() const {return _pathTransformToGoal;}
 
 	std::map<int, Transform> getForwardWMPoses(int fromId, int maxNearestNeighbors, float radius, int maxDiffID) const;
-	std::list<std::map<int, Transform> > getPaths(std::map<int, Transform> poses) const;
+	std::map<int, std::map<int, Transform> > getPaths(std::map<int, Transform> poses, const Transform & target, int maxGraphDepth = 0) const;
 	void adjustLikelihood(std::map<int, float> & likelihood) const;
 	std::pair<int, float> selectHypothesis(const std::map<int, float> & posterior,
 											const std::map<int, float> & likelihood) const;
@@ -194,6 +210,8 @@ private:
 	float _localRadius;
 	float _localImmunizationRatio;
 	int _proximityMaxGraphDepth;
+	int _proximityMaxPaths;
+	int _proximityMaxNeighbors;
 	float _proximityFilteringRadius;
 	bool _proximityRawPosesUsed;
 	float _proximityAngle;
@@ -234,6 +252,7 @@ private:
 	std::map<int, Transform> _optimizedPoses;
 	std::multimap<int, Link> _constraints;
 	Transform _mapCorrection;
+	Transform _mapCorrectionBackup; // used in localization mode when odom is lost
 	Transform _lastLocalizationPose; // Corrected odometry pose. In mapping mode, this corresponds to last pose return by getLocalOptimizedPoses().
 	int _lastLocalizationNodeId; // for localization mode
 
@@ -245,6 +264,7 @@ private:
 	unsigned int _pathGoalIndex;
 	Transform _pathTransformToGoal;
 	int _pathStuckCount;
+	float _pathStuckDistance;
 
 };
 

corelib/include/rtabmap/core/RtabmapEvent.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -61,7 +61,7 @@ public:
 	enum Cmd {
 			kCmdInit,             // params: [string] database path + ParametersMap
 			kCmdResetMemory,
-			kCmdClose,            // params: [bool] database saved (default true)
+			kCmdClose,            // params: [bool] database saved (default true), [string] output database path (empty=use same database to save, only work when Db/Sqlite3InMemory=true)
 			kCmdDumpMemory,
 			kCmdDumpPrediction,
 			kCmdGenerateDOTGraph, // params: [bool] global, [string] path, if global=false: [int] id, [int] margin

corelib/include/rtabmap/core/RtabmapExp.h

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

corelib/include/rtabmap/core/RtabmapThread.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -82,8 +82,14 @@ public:
 	void setDataBufferSize(unsigned int bufferSize);
 	void createIntermediateNodes(bool enabled);
 
-	// this will delete rtabmap object if set
+	/**
-	void close(bool databaseSaved);
+	 * Close rtabmap. This will delete rtabmap object if set.
+	 * @param databaseSaved true=database saved, false=database discarded.
+	 * @param databasePath output database file name, ignored if
+	 *                     Db/Sqlite3InMemory=false (opened database is
+	 *                     then overwritten).
+	 */
+	void close(bool databaseSaved, const std::string & databasePath = "");
 
 protected:
 	virtual void handleEvent(UEvent * anEvent);

corelib/include/rtabmap/core/SensorData.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/CameraModel.h>
 #include <rtabmap/core/StereoCameraModel.h>
 #include <rtabmap/core/Transform.h>
+#include <rtabmap/core/LaserScanInfo.h>
 #include <opencv2/core/core.hpp>
 #include <opencv2/features2d/features2d.hpp>
 
@@ -75,8 +76,7 @@ public:
 	// RGB-D constructor + laser scan
 	SensorData(
 			const cv::Mat & laserScan,
-			int laserScanMaxPts,
+			const LaserScanInfo & laserScanInfo,
-			float laserScanMaxRange,
 			const cv::Mat & rgb,
 			const cv::Mat & depth,
 			const CameraModel & cameraModel,
@@ -96,8 +96,7 @@ public:
 	// Multi-cameras RGB-D constructor + laser scan
 	SensorData(
 			const cv::Mat & laserScan,
-			int laserScanMaxPts,
+			const LaserScanInfo & laserScanInfo,
-			float laserScanMaxRange,
 			const cv::Mat & rgb,
 			const cv::Mat & depth,
 			const std::vector<CameraModel> & cameraModels,
@@ -117,8 +116,7 @@ public:
 	// Stereo constructor + laser scan
 	SensorData(
 			const cv::Mat & laserScan,
-			int laserScanMaxPts,
+			const LaserScanInfo & laserScanInfo,
-			float laserScanMaxRange,
 			const cv::Mat & left,
 			const cv::Mat & right,
 			const StereoCameraModel & cameraModel,
@@ -131,7 +129,6 @@ public:
 	bool isValid() const {
 		return !(_id == 0 &&
 			_stamp == 0.0 &&
-			_laserScanMaxPts == 0 &&
 			_imageRaw.empty() &&
 			_imageCompressed.empty() &&
 			_depthOrRightRaw.empty() &&
@@ -150,8 +147,7 @@ public:
 	void setId(int id) {_id = id;}
 	double stamp() const {return _stamp;}
 	void setStamp(double stamp) {_stamp = stamp;}
-	int laserScanMaxPts() const {return _laserScanMaxPts;}
+	const LaserScanInfo & laserScanInfo() const {return _laserScanInfo;}
-	float laserScanMaxRange() const {return _laserScanMaxRange;}
 
 	const cv::Mat & imageCompressed() const {return _imageCompressed;}
 	const cv::Mat & depthOrRightCompressed() const {return _depthOrRightCompressed;}
@@ -162,7 +158,7 @@ public:
 	const cv::Mat & laserScanRaw() const {return _laserScanRaw;}
 	void setImageRaw(const cv::Mat & imageRaw) {_imageRaw = imageRaw;}
 	void setDepthOrRightRaw(const cv::Mat & depthOrImageRaw) {_depthOrRightRaw =depthOrImageRaw;}
-	void setLaserScanRaw(const cv::Mat & laserScanRaw, int maxPts, float maxRange) {_laserScanRaw =laserScanRaw;_laserScanMaxPts = maxPts;_laserScanMaxRange=maxRange;}
+	void setLaserScanRaw(const cv::Mat & laserScanRaw, const LaserScanInfo & info) {_laserScanRaw =laserScanRaw;_laserScanInfo = info;}
 	void setCameraModel(const CameraModel & model) {_cameraModels.clear(); _cameraModels.push_back(model);}
 	void setCameraModels(const std::vector<CameraModel> & models) {_cameraModels = models;}
 	void setStereoCameraModel(const StereoCameraModel & stereoCameraModel) {_stereoCameraModel = stereoCameraModel;}
@@ -172,33 +168,64 @@ public:
 	cv::Mat rightRaw() const {return _depthOrRightRaw.type()==CV_8UC1?_depthOrRightRaw:cv::Mat();}
 
 	void uncompressData();
-	void uncompressData(cv::Mat * imageRaw, cv::Mat * depthOrRightRaw, cv::Mat * laserScanRaw = 0, cv::Mat * userDataRaw = 0);
+	void uncompressData(
-	void uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthOrRightRaw, cv::Mat * laserScanRaw = 0, cv::Mat * userDataRaw = 0) const;
+			cv::Mat * imageRaw,
+			cv::Mat * depthOrRightRaw,
+			cv::Mat * laserScanRaw = 0,
+			cv::Mat * userDataRaw = 0,
+			cv::Mat * groundCellsRaw = 0,
+			cv::Mat * obstacleCellsRaw = 0);
+	void uncompressDataConst(
+			cv::Mat * imageRaw,
+			cv::Mat * depthOrRightRaw,
+			cv::Mat * laserScanRaw = 0,
+			cv::Mat * userDataRaw = 0,
+			cv::Mat * groundCellsRaw = 0,
+			cv::Mat * obstacleCellsRaw = 0) const;
 
 	const std::vector<CameraModel> & cameraModels() const {return _cameraModels;}
 	const StereoCameraModel & stereoCameraModel() const {return _stereoCameraModel;}
 
 	void setUserDataRaw(const cv::Mat & userDataRaw); // only set raw
-	void setUserData(const cv::Mat & userData); // detect automatically if raw or compressed. If raw, the data is compressed too.
+	/**
+	 * Set user data. Detect automatically if raw or compressed. If raw, the data is
+	 * compressed too. A matrix of type CV_8UC1 with 1 row is considered as compressed.
+	 * If you have one dimension unsigned 8 bits raw data, make sure to transpose it
+	 * (to have multiple rows instead of multiple columns) in order to be detected as
+	 * not compressed.
+	 */
+	void setUserData(const cv::Mat & userData);
 	const cv::Mat & userDataRaw() const {return _userDataRaw;}
 	const cv::Mat & userDataCompressed() const {return _userDataCompressed;}
 
-	void setFeatures(const std::vector<cv::KeyPoint> & keypoints, const cv::Mat & descriptors)
+	// detect automatically if raw or compressed. If raw, the data will be compressed.
-	{
+	void setOccupancyGrid(
-		_keypoints = keypoints;
+			const cv::Mat & ground,
-		_descriptors = descriptors;
+			const cv::Mat & obstacles,
-	}
+			float cellSize,
+			const cv::Point3f & viewPoint);
+	// remove raw occupancy grids
+	void clearOccupancyGridRaw() {_groundCellsRaw = cv::Mat(); _obstacleCellsRaw = cv::Mat();}
+	const cv::Mat & gridGroundCellsRaw() const {return _groundCellsRaw;}
+	const cv::Mat & gridGroundCellsCompressed() const {return _groundCellsCompressed;}
+	const cv::Mat & gridObstacleCellsRaw() const {return _obstacleCellsRaw;}
+	const cv::Mat & gridObstacleCellsCompressed() const {return _obstacleCellsCompressed;}
+	float gridCellSize() const {return _cellSize;}
+	const cv::Point3f & gridViewPoint() const {return _viewPoint;}
+
+	void setFeatures(const std::vector<cv::KeyPoint> & keypoints, const std::vector<cv::Point3f> & keypoints3D, const cv::Mat & descriptors);
 	const std::vector<cv::KeyPoint> & keypoints() const {return _keypoints;}
+	const std::vector<cv::Point3f> & keypoints3D() const {return _keypoints3D;}
 	const cv::Mat & descriptors() const {return _descriptors;}
 
 	void setGroundTruth(const Transform & pose) {groundTruth_ = pose;}
 	const Transform & groundTruth() const {return groundTruth_;}
 
+	long getMemoryUsed() const; // Return memory usage in Bytes
+
 private:
 	int _id;
 	double _stamp;
-	int _laserScanMaxPts;
-	float _laserScanMaxRange;
 
 	cv::Mat _imageCompressed;          // compressed image
 	cv::Mat _depthOrRightCompressed;   // compressed image
@@ -211,12 +238,23 @@ private:
 	std::vector<CameraModel> _cameraModels;
 	StereoCameraModel _stereoCameraModel;
 
+	LaserScanInfo _laserScanInfo;
+
 	// user data
 	cv::Mat _userDataCompressed;      // compressed data
 	cv::Mat _userDataRaw;
 
+	// occupancy grid
+	cv::Mat _groundCellsCompressed;
+	cv::Mat _obstacleCellsCompressed;
+	cv::Mat _groundCellsRaw;
+	cv::Mat _obstacleCellsRaw;
+	float _cellSize;
+	cv::Point3f _viewPoint;
+
 	// features
 	std::vector<cv::KeyPoint> _keypoints;
+	std::vector<cv::Point3f> _keypoints3D;
 	cv::Mat _descriptors;
 
 	Transform groundTruth_;

corelib/include/rtabmap/core/Signature.h

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

corelib/include/rtabmap/core/Statistics.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -69,6 +69,8 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Proximity, Time_detections,);
 	RTABMAP_STATS(Proximity, Space_last_detection_id,);
 	RTABMAP_STATS(Proximity, Space_paths,);
+	RTABMAP_STATS(Proximity, Space_visual_paths_checked,);
+	RTABMAP_STATS(Proximity, Space_scan_paths_checked,);
 	RTABMAP_STATS(Proximity, Space_detections_added_visually,);
 	RTABMAP_STATS(Proximity, Space_detections_added_icp_only,);
 
@@ -92,6 +94,8 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Memory, Rehearsal_merged,);
 	RTABMAP_STATS(Memory, Local_graph_size,);
 	RTABMAP_STATS(Memory, Small_movement,);
+	RTABMAP_STATS(Memory, Odometry_variance_ang,);
+	RTABMAP_STATS(Memory, Odometry_variance_lin,);
 	RTABMAP_STATS(Memory, Distance_travelled, m);
 
 	RTABMAP_STATS(Timing, Memory_update, ms);
@@ -124,14 +128,19 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(TimingMem, Joining_dictionary_update, ms);
 	RTABMAP_STATS(TimingMem, Add_new_words, ms);
 	RTABMAP_STATS(TimingMem, Compressing_data, ms);
+	RTABMAP_STATS(TimingMem, Post_decimation, ms);
+	RTABMAP_STATS(TimingMem, Scan_downsampling, ms);
+	RTABMAP_STATS(TimingMem, Scan_normals, ms);
+	RTABMAP_STATS(TimingMem, Occupancy_grid, ms);
 
 	RTABMAP_STATS(Keypoint, Dictionary_size, words);
 	RTABMAP_STATS(Keypoint, Indexed_words, words);
 	RTABMAP_STATS(Keypoint, Index_memory_usage, KB);
-	RTABMAP_STATS(Keypoint, Response_threshold,);
 
 public:
 	static const std::map<std::string, float> & defaultData();
+	static std::string serializeData(const std::map<std::string, float> & data);
+	static std::map<std::string, float> deserializeData(const std::string & data);
 
 public:
 	Statistics();
@@ -145,6 +154,7 @@ public:
 	void setRefImageId(int refImageId) {_refImageId = refImageId;}
 	void setLoopClosureId(int loopClosureId) {_loopClosureId = loopClosureId;}
 	void setProximityDetectionId(int id) {_proximiyDetectionId = id;}
+	void setStamp(double stamp) {_stamp = stamp;}
 
 	void setSignatures(const std::map<int, Signature> & signatures) {_signatures = signatures;}
 
@@ -165,6 +175,7 @@ public:
 	int refImageId() const {return _refImageId;}
 	int loopClosureId() const {return _loopClosureId;}
 	int proximityDetectionId() const {return _proximiyDetectionId;}
+	double stamp() const {return _stamp;}
 
 	const std::map<int, Signature> & getSignatures() const {return _signatures;}
 
@@ -188,6 +199,7 @@ private:
 	int _refImageId;
 	int _loopClosureId;
 	int _proximiyDetectionId;
+	double _stamp;
 
 	std::map<int, Signature> _signatures;
 

corelib/include/rtabmap/core/Stereo.h

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

corelib/include/rtabmap/core/StereoCameraModel.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -35,7 +35,7 @@ namespace rtabmap {
 class RTABMAP_EXP StereoCameraModel
 {
 public:
-	StereoCameraModel() {}
+	StereoCameraModel() : leftSuffix_("left"), rightSuffix_("right") {}
 	StereoCameraModel(
 			const std::string & name,
 			const cv::Size & imageSize1,
@@ -87,7 +87,7 @@ public:
 
 	void initRectificationMap() {left_.initRectificationMap(); right_.initRectificationMap();}
 
-	void setName(const std::string & name);
+	void setName(const std::string & name, const std::string & leftSuffix = "left", const std::string & rightSuffix = "right");
 	const std::string & name() const {return name_;}
 
 	// backward compatibility
@@ -95,8 +95,9 @@ public:
 
 	bool load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform = true);
 	bool save(const std::string & directory, bool ignoreStereoTransform = true) const;
+	bool saveStereoTransform(const std::string & directory) const;
 
-	double baseline() const {return right_.fx()!=0.0?-right_.Tx()/right_.fx():0.0;}
+	double baseline() const {return right_.fx()!=0.0 && left_.fx() != 0.0 ? left_.Tx() / left_.fx() - right_.Tx()/right_.fx():0.0;}
 
 	float computeDepth(float disparity) const;
 	float computeDisparity(float depth) const; // m
@@ -108,6 +109,7 @@ public:
 	const cv::Mat & F() const {return F_;} //extrinsic fundamental matrix
 
 	void scale(double scale);
+	void roi(const cv::Rect & roi);
 
 	void setLocalTransform(const Transform & transform) {left_.setLocalTransform(transform);}
 	const Transform & localTransform() const {return left_.localTransform();}
@@ -116,7 +118,12 @@ public:
 	const CameraModel & left() const {return left_;}
 	const CameraModel & right() const {return right_;}
 
+	const std::string & getLeftSuffix() const {return leftSuffix_;}
+	const std::string & getRightSuffix() const {return rightSuffix_;}
+
 private:
+	std::string leftSuffix_;
+	std::string rightSuffix_;
 	CameraModel left_;
 	CameraModel right_;
 	std::string name_;

corelib/include/rtabmap/core/StereoDense.h

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

corelib/include/rtabmap/core/Transform.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -106,6 +106,7 @@ public:
 	void getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const;
 	void getEulerAngles(float & roll, float & pitch, float & yaw) const;
 	void getTranslation(float & x, float & y, float & z) const;
+	float getAngle(float x=1.0f, float y=0.0f, float z=0.0f) const;
 	float getNorm() const;
 	float getNormSquared() const;
 	float getDistance(const Transform & t) const;

corelib/include/rtabmap/core/UserDataEvent.h

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

corelib/include/rtabmap/core/VWDictionary.h

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

corelib/include/rtabmap/core/VisualWord.h

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

corelib/include/rtabmap/core/clams/discrete_depth_distortion_model.h

@@ -0,0 +1,130 @@
+/*
+Copyright (c) 2013, Alex Teichman and Stephen Miller (Stanford University)
+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 <organization> 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 <COPYRIGHT HOLDER> 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.
+
+RTAB-Map integration: Mathieu Labbe
+*/
+
+#ifndef DISCRETE_DEPTH_DISTORTION_MODEL_H
+#define DISCRETE_DEPTH_DISTORTION_MODEL_H
+
+#include <assert.h>
+#include <vector>
+#include <Eigen/Core>
+#include <opencv2/opencv.hpp>
+#include <rtabmap/utilite/UMutex.h>
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+namespace clams
+{
+  class RTABMAP_EXP DiscreteFrustum
+  {
+  public:
+    DiscreteFrustum(int smoothing = 1, double bin_depth = 1.0);
+    //! z value, not distance to origin.
+    //! thread-safe.
+    void addExample(double ground_truth, double measurement);
+    int index(double z) const;
+    void undistort(double* z) const;
+    void interpolatedUndistort(double* z) const;
+    void serialize(std::ostream& out, bool ascii) const;
+    void deserialize(std::istream& in, bool ascii);
+  
+  protected:
+    double max_dist_;
+    int num_bins_;
+    double bin_depth_;
+    Eigen::VectorXf counts_;
+    Eigen::VectorXf total_numerators_;
+    Eigen::VectorXf total_denominators_;
+    Eigen::VectorXf multipliers_;
+
+    friend class DiscreteDepthDistortionModel;
+  };
+
+  class RTABMAP_EXP DiscreteDepthDistortionModel
+  {
+  public:
+    DiscreteDepthDistortionModel() :
+    	width_(0),
+		height_(0),
+		bin_width_(0),
+		bin_height_(0),
+		bin_depth_(0),
+		num_bins_x_(0),
+		num_bins_y_(0),
+		training_samples_(0)
+    {}
+    virtual ~DiscreteDepthDistortionModel();
+    DiscreteDepthDistortionModel(int width, int height, int bin_width = 8, int bin_height = 6, double bin_depth = 2.0, int smoothing = 1);
+    DiscreteDepthDistortionModel(const DiscreteDepthDistortionModel& other);
+    DiscreteDepthDistortionModel& operator=(const DiscreteDepthDistortionModel& other);
+    void undistort(cv::Mat & depth) const;
+    //! Returns the number of training examples it used from this pair.
+    //! Thread-safe.
+    size_t accumulate(const cv::Mat& ground_truth, const cv::Mat& measurement);
+    void addExample(int v, int u, double ground_truth, double measurement);
+    void save(const std::string& path) const;
+	void load(const std::string& path);
+	void serialize(std::ostream& out, bool ascii) const;
+	void deserialize(std::istream& in, bool ascii);
+    cv::Mat visualize(const std::string& path = "") const;
+
+    int getWidth() const {return width_;}
+    int getHeight() const {return height_;}
+    size_t getTrainingSamples() const {return training_samples_;}
+    bool isValid() const
+    {
+    	return !frustums_.empty();
+    }
+
+  protected:
+    //! Image width.
+    int width_;
+    //! Image height.
+    int height_;
+    //! Width of each bin in pixels.
+    int bin_width_;
+    //! Height of each bin in pixels.
+    int bin_height_;
+    //! Depth of each bin in meters.
+    double bin_depth_;
+    int num_bins_x_;
+    int num_bins_y_;
+    //! frustums_[y][x]
+    std::vector< std::vector<DiscreteFrustum*> > frustums_;
+
+    size_t training_samples_;
+
+    void deleteFrustums();
+    DiscreteFrustum& frustum(int y, int x);
+    const DiscreteFrustum& frustum(int y, int x) const;
+
+    UMutex mutex_;
+  };
+
+}  // namespace clams
+
+#endif // DISCRETE_DEPTH_DISTORTION_MODEL_H

corelib/include/rtabmap/core/clams/frame_projector.h

@@ -0,0 +1,96 @@
+/*
+Copyright (c) 2013, Alex Teichman and Stephen Miller (Stanford University)
+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 <organization> 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 <COPYRIGHT HOLDER> 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.
+
+RTAB-Map integration: Mathieu Labbe
+*/
+
+#ifndef FRAME_PROJECTOR_H
+#define FRAME_PROJECTOR_H
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <opencv2/core/core.hpp>
+#include <rtabmap/core/CameraModel.h>
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#define MAX_MULT 1.3
+#define MIN_MULT 0.7
+
+namespace clams
+{
+
+  //! "Projective" point comes from the OpenNI terminology, and refers to (u, v, z), i.e.
+  //! pixel id and depth value.  Here I've added color, too, so that this represents everything
+  //! that is known about a pixel in an RBGD camera.
+  class ProjectivePoint
+  {
+  public:
+	  ProjectivePoint() :
+		  u_(0),
+		  v_(0),
+		  z_(0.0f) {}
+
+    int u_;
+    int v_;
+    float z_; // in meters
+  };
+
+  //! This is essentially a pinhole camera model for an RGBD sensor, with
+  //! some extra functions added on for use during calibration.
+  class RTABMAP_EXP FrameProjector
+  {
+  public:
+    // For storing z values in meters.  This is not Euclidean distance.
+    typedef std::vector< std::vector< std::vector<double> > > RangeIndex;
+
+    FrameProjector(const rtabmap::CameraModel & model);
+
+    RangeIndex cloudToRangeIndex(const pcl::PointCloud<pcl::PointXYZ>::Ptr & pcd) const;
+    //! transform is applied to the map, then projected into a depth index.
+    //! The best depth estimate from the map corresponding to the measurement depth frame
+    //! will be returned.
+    cv::Mat estimateMapDepth(
+    		const pcl::PointCloud<pcl::PointXYZ>::Ptr & map,
+    		const rtabmap::Transform & transform,
+			const cv::Mat & measurement,
+			double coneRadius = 0.02,
+			double coneStdevThresh = 0.03) const;
+                          
+    pcl::PointXYZ project(const ProjectivePoint& ppt) const;
+    ProjectivePoint reproject(const pcl::PointXYZ& pt) const;
+
+  protected:
+    bool coneFit(const cv::Size& imageSize, const RangeIndex& rindex,
+                 int uc, int vc, double radius, double measurement_depth,
+                 double* mean, double* stdev) const;
+
+  private:
+    rtabmap::CameraModel model_;
+  };
+
+}  // namespace clams
+
+#endif // FRAME_PROJECTOR_H

corelib/include/rtabmap/core/clams/slam_calibrator.h

@@ -0,0 +1,52 @@
+/*
+Copyright (c) 2013, Alex Teichman and Stephen Miller (Stanford University)
+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 <organization> 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 <COPYRIGHT HOLDER> 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.
+
+RTAB-Map integration: Mathieu Labbe
+*/
+
+#ifndef SLAM_CALIBRATOR_H
+#define SLAM_CALIBRATOR_H
+
+#include <rtabmap/core/clams/discrete_depth_distortion_model.h>
+#include <rtabmap/core/SensorData.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+namespace clams
+{
+  
+DiscreteDepthDistortionModel RTABMAP_EXP calibrate(
+		const std::map<int, rtabmap::SensorData> & sequence,
+		const std::map<int, rtabmap::Transform> & trajectory,
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & map,
+		double coneRadius = 0.02,
+		double coneStdevThresh = 0.03);
+
+}
+
+#endif // SLAM_CALIBRATOR_H

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

@@ -0,0 +1,163 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef CORELIB_INCLUDE_RTABMAP_CORE_IMPL_OCCUPANCYGRID_HPP_
+#define CORELIB_INCLUDE_RTABMAP_CORE_IMPL_OCCUPANCYGRID_HPP_
+
+#include <rtabmap/core/util3d_mapping.h>
+#include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/utilite/ULogger.h>
+
+namespace rtabmap {
+
+template<typename PointT>
+typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
+		const typename pcl::PointCloud<PointT>::Ptr & cloudIn,
+		const pcl::IndicesPtr & indicesIn,
+		const Transform & pose,
+		const cv::Point3f & viewPoint,
+		pcl::IndicesPtr & groundIndices,
+		pcl::IndicesPtr & obstaclesIndices,
+		pcl::IndicesPtr * flatObstacles) const
+{
+	typename pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
+
+	// voxelize to grid cell size
+	cloud = util3d::voxelize(cloudIn, indicesIn, cellSize_);
+	pcl::IndicesPtr indices(new std::vector<int>);
+	indices->resize(cloud->size());
+	for(unsigned int i=0; i<indices->size(); ++i)
+	{
+		indices->at(i) = i;
+	}
+
+	// add pose rotation without yaw
+	float roll, pitch, yaw;
+	pose.getEulerAngles(roll, pitch, yaw);
+	UDEBUG("node.getPose()=%s projMapFrame_=%d", pose.prettyPrint().c_str(), projMapFrame_?1:0);
+	cloud = util3d::transformPointCloud(cloud, Transform(0,0, projMapFrame_?pose.z():0, roll, pitch, 0));
+
+	// filter footprint
+	if(footprintLength_ > 0.0f || footprintWidth_ > 0.0f || footprintHeight_ > 0.0f)
+	{
+		indices = util3d::cropBox(
+				cloud,
+				indices,
+				Eigen::Vector4f(
+						footprintLength_>0.0f?-footprintLength_/2.0f:std::numeric_limits<int>::min(),
+						footprintWidth_>0.0f&&footprintLength_>0.0f?-footprintWidth_/2.0f:std::numeric_limits<int>::min(),
+						0,
+						1),
+				Eigen::Vector4f(
+						footprintLength_>0.0f?footprintLength_/2.0f:std::numeric_limits<int>::max(),
+						footprintWidth_>0.0f&&footprintLength_>0.0f?footprintWidth_/2.0f:std::numeric_limits<int>::max(),
+						footprintHeight_>0.0f&&footprintLength_>0.0f&&footprintWidth_>0.0f?footprintHeight_:std::numeric_limits<int>::max(),
+						1),
+				Transform::getIdentity(),
+				true);
+	}
+
+	// filter ground/obstacles zone
+	if(minGroundHeight_ != 0.0f || maxObstacleHeight_ > 0.0f)
+	{
+		indices = util3d::passThrough(cloud, indices, "z",
+				minGroundHeight_!=0.0f?minGroundHeight_:std::numeric_limits<int>::min(),
+				maxObstacleHeight_>0.0f?maxObstacleHeight_:std::numeric_limits<int>::max());
+		UDEBUG("indices after max obstacles height filtering = %d", (int)indices->size());
+	}
+
+	if(indices->size())
+	{
+		if(normalsSegmentation_)
+		{
+			UDEBUG("normalKSearch=%d", normalKSearch_);
+			UDEBUG("maxGroundAngle=%f", maxGroundAngle_);
+			UDEBUG("Cluster radius=%f", clusterRadius_);
+			UDEBUG("flatObstaclesDetected=%d", flatObstaclesDetected_?1:0);
+			UDEBUG("maxGroundHeight=%f", maxGroundHeight_?1:0);
+			util3d::segmentObstaclesFromGround<PointT>(
+					cloud,
+					indices,
+					groundIndices,
+					obstaclesIndices,
+					normalKSearch_,
+					maxGroundAngle_,
+					clusterRadius_,
+					minClusterSize_,
+					flatObstaclesDetected_,
+					maxGroundHeight_,
+					flatObstacles,
+					Eigen::Vector4f(viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0), 1));
+			UDEBUG("viewPoint=%f,%f,%f", viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0));
+			//UWARN("Saving ground.pcd and obstacles.pcd");
+			//pcl::io::savePCDFile("ground.pcd", *cloud, *groundIndices);
+			//pcl::io::savePCDFile("obstacles.pcd", *cloud, *obstaclesIndices);
+		}
+		else
+		{
+			UDEBUG("");
+			// passthrough filter
+			groundIndices = rtabmap::util3d::passThrough(cloud, indices, "z", minGroundHeight_<0.0f?minGroundHeight_:std::numeric_limits<int>::min(), maxGroundHeight_);
+			obstaclesIndices = rtabmap::util3d::extractIndices(cloud, groundIndices, true);
+		}
+
+		UDEBUG("groundIndices=%d obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
+
+		// Do radius filtering after voxel filtering ( a lot faster)
+		if(noiseFilteringRadius_ > 0.0 && noiseFilteringMinNeighbors_ > 0)
+		{
+			UDEBUG("");
+			if(groundIndices->size())
+			{
+				groundIndices = rtabmap::util3d::radiusFiltering(cloud, groundIndices, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
+			}
+			if(obstaclesIndices->size())
+			{
+				obstaclesIndices = rtabmap::util3d::radiusFiltering(cloud, obstaclesIndices, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
+			}
+			if(flatObstacles && (*flatObstacles)->size())
+			{
+				*flatObstacles = rtabmap::util3d::radiusFiltering(cloud, *flatObstacles, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
+			}
+
+			if(groundIndices->empty() && obstaclesIndices->empty())
+			{
+				UWARN("Cloud (with %d points) is empty after noise "
+						"filtering. Occupancy grid cannot be "
+						"created.",
+						(int)cloud->size());
+
+			}
+		}
+	}
+	return cloud;
+}
+
+}
+
+
+#endif /* CORELIB_INCLUDE_RTABMAP_CORE_IMPL_OCCUPANCYGRID_HPP_ */

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

@@ -1,9 +1,29 @@
 /*
- * util3d_mapping.hpp
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
- *
+All rights reserved.
- *  Created on: 2015-05-13
+
- *      Author: mathieu
+Redistribution and use in source and binary forms, with or without
- */
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
 
 #ifndef UTIL3D_MAPPING_HPP_
 #define UTIL3D_MAPPING_HPP_
@@ -17,6 +37,19 @@
 namespace rtabmap{
 namespace util3d{
 
+template<typename PointT>
+typename pcl::PointCloud<PointT>::Ptr projectCloudOnXYPlane(
+		const typename pcl::PointCloud<PointT> & cloud)
+{
+	typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
+	*output = cloud;
+	for(unsigned int i=0; i<output->size(); ++i)
+	{
+		output->at(i).z = 0;
+	}
+	return output;
+}
+
 template<typename PointT>
 void segmentObstaclesFromGround(
 		const typename pcl::PointCloud<PointT>::Ptr & cloud,
@@ -29,7 +62,8 @@ void segmentObstaclesFromGround(
 		int minClusterSize,
 		bool segmentFlatObstacles,
 		float maxGroundHeight,
-		pcl::IndicesPtr * flatObstacles)
+		pcl::IndicesPtr * flatObstacles,
+		const Eigen::Vector4f & viewPoint)
 {
 	ground.reset(new std::vector<int>);
 	obstacles.reset(new std::vector<int>);
@@ -47,9 +81,11 @@ void segmentObstaclesFromGround(
 				groundNormalAngle,
 				Eigen::Vector4f(0,0,1,0),
 				normalKSearch,
-				Eigen::Vector4f(0,0,100,0));
+				viewPoint);
 
-		if(segmentFlatObstacles)
+		UDEBUG("cloud=%d, indices=%d flatSurfaces=%d", (int)cloud->size(), (int)indices->size(), (int)flatSurfaces->size());
+
+		if(segmentFlatObstacles && flatSurfaces->size())
 		{
 			int biggestFlatSurfaceIndex;
 			std::vector<pcl::IndicesPtr> clusteredFlatSurfaces = extractClusters(
@@ -59,7 +95,7 @@ void segmentObstaclesFromGround(
 					minClusterSize,
 					std::numeric_limits<int>::max(),
 					&biggestFlatSurfaceIndex);
-
+			UDEBUG("clusteredFlatSurfaces=%d", (int)clusteredFlatSurfaces.size());
 
 			// cluster all surfaces for which the centroid is in the Z-range of the bigger surface
 			if(clusteredFlatSurfaces.size())
@@ -74,7 +110,7 @@ void segmentObstaclesFromGround(
 					{
 						if((int)i!=biggestFlatSurfaceIndex)
 						{
-							Eigen::Vector4f centroid;
+							Eigen::Vector4f centroid(0,0,0,1);
 							pcl::compute3DCentroid(*cloud, *clusteredFlatSurfaces.at(i), centroid);
 							if(centroid[2] >= min[2]-0.01 &&
 							  (centroid[2] <= max[2]+0.01 || (maxGroundHeight>0 && centroid[2] <= maxGroundHeight+0.01))) // epsilon
@@ -104,10 +140,18 @@ void segmentObstaclesFromGround(
 			ground = flatSurfaces;
 		}
 
+		UDEBUG("ground=%d", (int)ground->size());
+
 		if(ground->size() != cloud->size())
 		{
 			// Remove ground
-			pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, ground, true);
+			pcl::IndicesPtr notObstacles = ground;
+			if(indices->size())
+			{
+				notObstacles = util3d::extractIndices(cloud, indices, true);
+				notObstacles = util3d::concatenate(notObstacles, ground);
+			}
+			pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, notObstacles, true);
 
 			// If ground height is set, remove obstacles under it
 			if(maxGroundHeight > 0.0f)
@@ -142,7 +186,8 @@ void segmentObstaclesFromGround(
 		int minClusterSize,
 		bool segmentFlatObstacles,
 		float maxGroundHeight,
-		pcl::IndicesPtr * flatObstacles)
+		pcl::IndicesPtr * flatObstacles,
+		const Eigen::Vector4f & viewPoint)
 {
 	pcl::IndicesPtr indices(new std::vector<int>);
 	segmentObstaclesFromGround<PointT>(
@@ -156,7 +201,81 @@ void segmentObstaclesFromGround(
 			minClusterSize,
 			segmentFlatObstacles,
 			maxGroundHeight,
-			flatObstacles);
+			flatObstacles,
+			viewPoint);
+}
+
+template<typename PointT>
+void occupancy2DFromGroundObstacles(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		const pcl::IndicesPtr & groundIndices,
+		const pcl::IndicesPtr & obstaclesIndices,
+		cv::Mat & ground,
+		cv::Mat & obstacles,
+		float cellSize)
+{
+	typename pcl::PointCloud<PointT>::Ptr groundCloud(new pcl::PointCloud<PointT>);
+	typename pcl::PointCloud<PointT>::Ptr obstaclesCloud(new pcl::PointCloud<PointT>);
+
+	if(groundIndices->size())
+	{
+		pcl::copyPointCloud(*cloud, *groundIndices, *groundCloud);
+	}
+
+	if(obstaclesIndices->size())
+	{
+		pcl::copyPointCloud(*cloud, *obstaclesIndices, *obstaclesCloud);
+	}
+
+	occupancy2DFromGroundObstacles<PointT>(
+			groundCloud,
+			obstaclesCloud,
+			ground,
+			obstacles,
+			cellSize);
+}
+
+template<typename PointT>
+void occupancy2DFromGroundObstacles(
+		const typename pcl::PointCloud<PointT>::Ptr & groundCloud,
+		const typename pcl::PointCloud<PointT>::Ptr & obstaclesCloud,
+		cv::Mat & ground,
+		cv::Mat & obstacles,
+		float cellSize)
+{
+	ground = cv::Mat();
+	if(groundCloud->size())
+	{
+		//project on XY plane
+		typename pcl::PointCloud<PointT>::Ptr groundCloudProjected;
+		groundCloudProjected = util3d::projectCloudOnXYPlane(*groundCloud);
+		//voxelize to grid cell size
+		groundCloudProjected = util3d::voxelize(groundCloudProjected, cellSize);
+
+		ground = cv::Mat(1, (int)groundCloudProjected->size(), CV_32FC2);
+		for(unsigned int i=0;i<groundCloudProjected->size(); ++i)
+		{
+			ground.at<cv::Vec2f>(i)[0] = groundCloudProjected->at(i).x;
+			ground.at<cv::Vec2f>(i)[1] = groundCloudProjected->at(i).y;
+		}
+	}
+
+	obstacles = cv::Mat();
+	if(obstaclesCloud->size())
+	{
+		//project on XY plane
+		typename pcl::PointCloud<PointT>::Ptr obstaclesCloudProjected;
+		obstaclesCloudProjected = util3d::projectCloudOnXYPlane(*obstaclesCloud);
+		//voxelize to grid cell size
+		obstaclesCloudProjected = util3d::voxelize(obstaclesCloudProjected, cellSize);
+
+		obstacles = cv::Mat(1, (int)obstaclesCloudProjected->size(), CV_32FC2);
+		for(unsigned int i=0;i<obstaclesCloudProjected->size(); ++i)
+		{
+			obstacles.at<cv::Vec2f>(i)[0] = obstaclesCloudProjected->at(i).x;
+			obstacles.at<cv::Vec2f>(i)[1] = obstaclesCloudProjected->at(i).y;
+		}
+	}
 }
 
 template<typename PointT>
@@ -189,48 +308,13 @@ void occupancy2DFromCloud3D(
 			segmentFlatObstacles,
 			maxGroundHeight);
 
-	pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
+	occupancy2DFromGroundObstacles<PointT>(
-	pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
+			cloud,
-
+			groundIndices,
-	if(groundIndices->size())
+			obstaclesIndices,
-	{
+			ground,
-		pcl::copyPointCloud(*cloud, *groundIndices, *groundCloud);
+			obstacles,
-		//project on XY plane
+			cellSize);
-		util3d::projectCloudOnXYPlane(groundCloud);
-		//voxelize to grid cell size
-		groundCloud = util3d::voxelize(groundCloud, cellSize);
-	}
-
-	if(obstaclesIndices->size())
-	{
-		pcl::copyPointCloud(*cloud, *obstaclesIndices, *obstaclesCloud);
-		//project on XY plane
-		util3d::projectCloudOnXYPlane(obstaclesCloud);
-		//voxelize to grid cell size
-		obstaclesCloud = util3d::voxelize(obstaclesCloud, cellSize);
-	}
-
-	ground = cv::Mat();
-	if(groundCloud->size())
-	{
-		ground = cv::Mat((int)groundCloud->size(), 1, CV_32FC2);
-		for(unsigned int i=0;i<groundCloud->size(); ++i)
-		{
-			ground.at<cv::Vec2f>(i)[0] = groundCloud->at(i).x;
-			ground.at<cv::Vec2f>(i)[1] = groundCloud->at(i).y;
-		}
-	}
-
-	obstacles = cv::Mat();
-	if(obstaclesCloud->size())
-	{
-		obstacles = cv::Mat((int)obstaclesCloud->size(), 1, CV_32FC2);
-		for(unsigned int i=0;i<obstaclesCloud->size(); ++i)
-		{
-			obstacles.at<cv::Vec2f>(i)[0] = obstaclesCloud->at(i).x;
-			obstacles.at<cv::Vec2f>(i)[1] = obstaclesCloud->at(i).y;
-		}
-	}
 }
 
 template<typename PointT>

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

@@ -0,0 +1,50 @@
+/*
+ * util3d_surface.hpp
+ *
+ *  Created on: Sep 3, 2016
+ *      Author: mathieu
+ */
+
+#ifndef CORELIB_INCLUDE_RTABMAP_CORE_IMPL_UTIL3D_SURFACE_HPP_
+#define CORELIB_INCLUDE_RTABMAP_CORE_IMPL_UTIL3D_SURFACE_HPP_
+
+namespace rtabmap {
+
+namespace util3d {
+
+template<typename pointT>
+std::vector<pcl::Vertices> normalizePolygonsSide(
+		const typename pcl::PointCloud<pointT> & cloud,
+		const std::vector<pcl::Vertices> & polygons,
+		const pcl::PointXYZ & viewPoint)
+{
+	std::vector<pcl::Vertices> output(polygons.size());
+	for(unsigned int i=0; i<polygons.size(); ++i)
+	{
+		pcl::Vertices polygon = polygons[i];
+		Eigen::Vector3f v1 = cloud.at(polygon.vertices[1]).getVector3fMap() - cloud.at(polygon.vertices[0]).getVector3fMap();
+		Eigen::Vector3f v2 = cloud.at(polygon.vertices[2]).getVector3fMap() - cloud.at(polygon.vertices[0]).getVector3fMap();
+		Eigen::Vector3f n = (v1.cross(v2)).normalized();
+
+		Eigen::Vector3f p = Eigen::Vector3f(viewPoint.x, viewPoint.y, viewPoint.z) - cloud.at(polygon.vertices[1]).getVector3fMap();
+
+		float result = n.dot(p);
+		if(result < 0)
+		{
+			//reverse vertices order
+			int tmp = polygon.vertices[0];
+			polygon.vertices[0] = polygon.vertices[2];
+			polygon.vertices[2] = tmp;
+		}
+
+		output[i] = polygon;
+	}
+	return output;
+}
+
+}
+
+}
+
+
+#endif /* CORELIB_INCLUDE_RTABMAP_CORE_IMPL_UTIL3D_SURFACE_HPP_ */

corelib/include/rtabmap/core/util2d.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -109,6 +109,11 @@ float RTABMAP_EXP getDepth(
 		float maxZError = 0.02f,
 		bool estWithNeighborsIfNull = false);
 
+cv::Rect RTABMAP_EXP computeRoi(const cv::Mat & image, const std::string & roiRatios);
+cv::Rect RTABMAP_EXP computeRoi(const cv::Size & imageSize, const std::string & roiRatios);
+cv::Rect RTABMAP_EXP computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios);
+cv::Rect RTABMAP_EXP computeRoi(const cv::Size & imageSize, const std::vector<float> & roiRatios);
+
 cv::Mat RTABMAP_EXP decimate(const cv::Mat & image, int d);
 cv::Mat RTABMAP_EXP interpolate(const cv::Mat & image, int factor, float depthErrorRatio = 0.02f);
 
@@ -116,6 +121,7 @@ cv::Mat RTABMAP_EXP interpolate(const cv::Mat & image, int factor, float depthEr
 cv::Mat RTABMAP_EXP registerDepth(
 		const cv::Mat & depth,
 		const cv::Mat & depthK,
+		const cv::Size & colorSize,
 		const cv::Mat & colorK,
 		const rtabmap::Transform & transform);
 
@@ -130,6 +136,12 @@ void RTABMAP_EXP fillRegisteredDepthHoles(
 		bool horizontal,
 		bool fillDoubleHoles = false);
 
+cv::Mat RTABMAP_EXP fastBilateralFiltering(
+		const cv::Mat & depth,
+		float sigmaS = 15.0f,
+		float sigmaR = 0.05f,
+		bool earlyDivision = false);
+
 } // namespace util3d
 } // namespace rtabmap
 

corelib/include/rtabmap/core/util3d.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -74,16 +74,23 @@ pcl::PointXYZ RTABMAP_EXP projectDepthTo3D(
 		bool smoothing,
 		float maxZError = 0.02f);
 
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDepth(
+RTABMAP_DEPRECATED (pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDepth(
 		const cv::Mat & imageDepth,
 		float cx, float cy,
 		float fx, float fy,
 		int decimation = 1,
 		float maxDepth = 0.0f,
 		float minDepth = 0.0f,
+		std::vector<int> * validIndices = 0), "Use cloudFromDepth with CameraModel interface.");
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDepth(
+		const cv::Mat & imageDepth,
+		const CameraModel & model,
+		int decimation = 1,
+		float maxDepth = 0.0f,
+		float minDepth = 0.0f,
 		std::vector<int> * validIndices = 0);
 
-pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDepthRGB(
+RTABMAP_DEPRECATED (pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDepthRGB(
 		const cv::Mat & imageRgb,
 		const cv::Mat & imageDepth,
 		float cx, float cy,
@@ -91,6 +98,14 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDepthRGB(
 		int decimation = 1,
 		float maxDepth = 0.0f,
 		float minDepth = 0.0f,
+		std::vector<int> * validIndices = 0), "Use cloudFromDepthRGB with CameraModel interface.");
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDepthRGB(
+		const cv::Mat & imageRgb,
+		const cv::Mat & imageDepth,
+		const CameraModel & model,
+		int decimation = 1,
+		float maxDepth = 0.0f,
+		float minDepth = 0.0f,
 		std::vector<int> * validIndices = 0);
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDisparity(
@@ -126,7 +141,8 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 		float maxDepth = 0.0f,
 		float minDepth = 0.0f,
 		std::vector<int> * validIndices = 0,
-		const ParametersMap & parameters = ParametersMap());
+		const ParametersMap & stereoParameters = ParametersMap(),
+		const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo
 
 /**
  * Create an RGB cloud from the images contained in SensorData. If there is only one camera,
@@ -139,6 +155,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
  * @param maxDepth, maximum depth of the projected points (farther points are set to null in case of an organized cloud).
  * @param minDepth, minimum depth of the projected points (closer points are set to null in case of an organized cloud).
  * @param validIndices, the indices of valid points in the cloud
+ * @param roiRatios, [left, right, top, bottom] region of interest (in ratios) of the image projected.
  * @return a RGB cloud.
  */
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
@@ -147,7 +164,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 		float maxDepth = 0.0f,
 		float minDepth = 0.0f,
 		std::vector<int> * validIndices = 0,
-		const ParametersMap & parameters = ParametersMap());
+		const ParametersMap & stereoParameters = ParametersMap(),
+		const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo
 
 pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImage(
 					const cv::Mat & depthImage,
@@ -158,17 +176,35 @@ pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImage(
 					float maxDepth = 0,
 					float minDepth = 0,
 					const Transform & localTransform = Transform::getIdentity());
+pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImages(
+		const cv::Mat & depthImages,
+		const std::vector<CameraModel> & cameraModels,
+		float maxDepth,
+		float minDepth);
 
 // return CV_32FC3
 cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform());
 // return CV_32FC6
 cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform = Transform());
+cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform());
+// return CV_32FC4
+cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform = Transform());
 // return CV_32FC2
 cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform());
 // For laserScan of type CV_32FC2, z is set to null.
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan, const Transform & transform = Transform());
-// For laserScan of type CV_32FC2 or CV_32FC3, normals are set to null.
+// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC4, normals are set to null.
 pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP laserScanToPointCloudNormal(const cv::Mat & laserScan, const Transform & transform = Transform());
+// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC6, rgb is set to default r,g,b parameters.
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP laserScanToPointCloudRGB(const cv::Mat & laserScan, const Transform & transform = Transform(), unsigned char r = 255, unsigned char g = 255, unsigned char b = 255);
+
+// For laserScan of type CV_32FC2, z is set to null.
+pcl::PointXYZ RTABMAP_EXP laserScanToPoint(const cv::Mat & laserScan, int index);
+// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC4, normals are set to null.
+pcl::PointNormal RTABMAP_EXP laserScanToPointNormal(const cv::Mat & laserScan, int index);
+// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC6, rgb is set to default r,g,b parameters.
+pcl::PointXYZRGB RTABMAP_EXP laserScanToPointRGB(const cv::Mat & laserScan, int index, unsigned char r = 255, unsigned char g = 255, unsigned char b = 255);
+
 
 cv::Point3f RTABMAP_EXP projectDisparityTo3D(
 		const cv::Point2f & pt,

corelib/include/rtabmap/core/util3d_correspondences.h

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

corelib/include/rtabmap/core/util3d_features.h

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

corelib/include/rtabmap/core/util3d_filtering.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <pcl/pcl_base.h>
+#include <pcl/ModelCoefficients.h>
 
 namespace rtabmap
 {
@@ -135,6 +136,33 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP passThrough(
 		float max,
 		bool negative = false);
 
+pcl::IndicesPtr RTABMAP_EXP cropBox(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const Eigen::Vector4f & min,
+		const Eigen::Vector4f & max,
+		const Transform & transform = Transform::getIdentity(),
+		bool negative = false);
+pcl::IndicesPtr RTABMAP_EXP cropBox(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const Eigen::Vector4f & min,
+		const Eigen::Vector4f & max,
+		const Transform & transform = Transform::getIdentity(),
+		bool negative = false);
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cropBox(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const Eigen::Vector4f & min,
+		const Eigen::Vector4f & max,
+		const Transform & transform = Transform::getIdentity(),
+		bool negative = false);
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cropBox(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const Eigen::Vector4f & min,
+		const Eigen::Vector4f & max,
+		const Transform & transform = Transform::getIdentity(),
+		bool negative = false);
+
 //Note: This assumes a coordinate system where X is forward, * Y is up, and Z is right.
 pcl::IndicesPtr RTABMAP_EXP frustumFiltering(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
@@ -383,7 +411,6 @@ pcl::IndicesPtr RTABMAP_EXP normalFiltering(
  * @param viewpoint from which viewpoint the normals should be estimated (see pcl::NormalEstimation).
  * @return the indices of the points which respect the normal constraint.
  */
-
 pcl::IndicesPtr RTABMAP_EXP normalFiltering(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
@@ -485,6 +512,18 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP extractIndices(
 		bool negative,
 		bool keepOrganized);
 
+pcl::IndicesPtr extractPlane(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		float distanceThreshold,
+		int maxIterations = 100,
+		pcl::ModelCoefficients * coefficientsOut = 0);
+pcl::IndicesPtr extractPlane(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float distanceThreshold,
+		int maxIterations = 100,
+		pcl::ModelCoefficients * coefficientsOut = 0);
+
 } // namespace util3d
 } // namespace rtabmap
 

corelib/include/rtabmap/core/util3d_mapping.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -58,7 +58,8 @@ cv::Mat RTABMAP_EXP create2DMapFromOccupancyLocalMaps(
 		float & xMin,
 		float & yMin,
 		float minMapSize = 0.0f,
-		bool erode = false);
+		bool erode = false,
+		float footprintRadius = 0.0f);
 
 cv::Mat RTABMAP_EXP create2DMap(const std::map<int, Transform> & poses,
 		const std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans,
@@ -76,8 +77,9 @@ void RTABMAP_EXP rayTrace(const cv::Point2i & start,
 
 cv::Mat RTABMAP_EXP convertMap2Image8U(const cv::Mat & map8S);
 
-void RTABMAP_EXP projectCloudOnXYPlane(
+template<typename PointT>
-		pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud);
+typename pcl::PointCloud<PointT>::Ptr projectCloudOnXYPlane(
+		const typename pcl::PointCloud<PointT> & cloud);
 
 // templated methods
 template<typename PointT>
@@ -92,7 +94,8 @@ void segmentObstaclesFromGround(
 		int minClusterSize,
 		bool segmentFlatObstacles = false,
 		float maxGroundHeight = 0.0f,
-		pcl::IndicesPtr * flatObstacles = 0);
+		pcl::IndicesPtr * flatObstacles = 0,
+		const Eigen::Vector4f & viewPoint = Eigen::Vector4f(0,0,100,0));
 template<typename PointT>
 void segmentObstaclesFromGround(
 		const typename pcl::PointCloud<PointT>::Ptr & cloud,
@@ -104,7 +107,25 @@ void segmentObstaclesFromGround(
 		int minClusterSize,
 		bool segmentFlatObstacles = false,
 		float maxGroundHeight = 0.0f,
-		pcl::IndicesPtr * flatObstacles = 0);
+		pcl::IndicesPtr * flatObstacles = 0,
+		const Eigen::Vector4f & viewPoint = Eigen::Vector4f(0,0,100,0));
+
+template<typename PointT>
+void occupancy2DFromGroundObstacles(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		const pcl::IndicesPtr & groundIndices,
+		const pcl::IndicesPtr & obstaclesIndices,
+		cv::Mat & ground,
+		cv::Mat & obstacles,
+		float cellSize);
+
+template<typename PointT>
+void occupancy2DFromGroundObstacles(
+		const typename pcl::PointCloud<PointT>::Ptr & groundCloud,
+		const typename pcl::PointCloud<PointT>::Ptr & obstaclesCloud,
+		cv::Mat & ground,
+		cv::Mat & obstacles,
+		float cellSize);
 
 template<typename PointT>
 void occupancy2DFromCloud3D(

corelib/include/rtabmap/core/util3d_motion_estimation.h

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

corelib/include/rtabmap/core/util3d_registration.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -92,18 +92,6 @@ Transform RTABMAP_EXP icpPointToPlane(
 		float epsilon = 0.0f,
 		bool icp2D = false);
 
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP getICPReadyCloud(
-		const cv::Mat & depth,
-		float fx,
-		float fy,
-		float cx,
-		float cy,
-		int decimation,
-		double maxDepth,
-		float voxel,
-		int samples,
-		const Transform & transform = Transform::getIdentity());
-
 } // namespace util3d
 } // namespace rtabmap
 

corelib/include/rtabmap/core/util3d_surface.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Transform.h>
 #include <rtabmap/core/CameraModel.h>
 #include <set>
+#include <list>
 
 namespace rtabmap
 {
@@ -60,6 +61,16 @@ void RTABMAP_EXP createPolygonIndexes(
 		std::vector<std::set<int> > & neighborPolygons,
 		std::vector<std::set<int> > & vertexPolygons);
 
+std::list<std::list<int> > RTABMAP_EXP clusterPolygons(
+		const std::vector<std::set<int> > & neighborPolygons,
+		int minClusterSize = 0);
+
+std::vector<pcl::Vertices> RTABMAP_EXP organizedFastMesh(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		double angleTolerance,
+		bool quad,
+		int trianglePixelSize,
+		const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0));
 std::vector<pcl::Vertices> RTABMAP_EXP organizedFastMesh(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		double angleTolerance = M_PI/16,
@@ -85,12 +96,17 @@ void RTABMAP_EXP appendMesh(
 		const std::vector<pcl::Vertices> & polygonsB);
 
 // return map from new to old polygon indices
-std::map<int, int> RTABMAP_EXP filterNotUsedVerticesFromMesh(
+std::vector<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(
+std::vector<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<int> RTABMAP_EXP filterNaNPointsFromMesh(
 		const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
 		const std::vector<pcl::Vertices> & polygons,
 		pcl::PointCloud<pcl::PointXYZRGB> & outputCloud,
@@ -121,32 +137,39 @@ pcl::TextureMesh::Ptr RTABMAP_EXP createTextureMesh(
 		const std::map<int, Transform> & poses,
 		const std::map<int, CameraModel> & cameraModels,
 		const std::map<int, cv::Mat> & images,
-		const std::string & tmpDirectory = ".");
+		const std::string & tmpDirectory = ".",
+		int kNormalSearch = 20); // if mesh doesn't have normals, compute them with k neighbors
 
-pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP computeNormals(
+pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
-		int normalKSearch = 20);
+		int normalKSearch = 20,
-pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP computeNormals(
+		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
+pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		int normalKSearch = 20);
+		int normalKSearch = 20,
-pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP computeNormals(
+		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
+pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
-		int normalKSearch = 20);
+		int normalKSearch = 20,
-pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP computeNormals(
+		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
+pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
-		int normalKSearch = 20);
+		int normalKSearch = 20,
+		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
 
-pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeFastOrganizedNormals(
+pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		float maxDepthChangeFactor = 0.02f,
-		float normalSmoothingSize = 10.0f);
+		float normalSmoothingSize = 10.0f,
-pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeFastOrganizedNormals(
+		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
+pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		float maxDepthChangeFactor = 0.02f,
-		float normalSmoothingSize = 10.0f);
+		float normalSmoothingSize = 10.0f,
+		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
 
 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP mls(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
@@ -187,33 +210,11 @@ template<typename pointT>
 std::vector<pcl::Vertices> normalizePolygonsSide(
 		const pcl::PointCloud<pointT> & cloud,
 		const std::vector<pcl::Vertices> & polygons,
-		const pcl::PointXYZ & viewPoint = pcl::PointXYZ(0,0,0))
+		const pcl::PointXYZ & viewPoint = pcl::PointXYZ(0,0,0));
-{
-	std::vector<pcl::Vertices> output(polygons.size());
-	for(unsigned int i=0; i<polygons.size(); ++i)
-	{
-		pcl::Vertices polygon = polygons[i];
-		Eigen::Vector3f v1 = cloud.at(polygon.vertices[1]).getVector3fMap() - cloud.at(polygon.vertices[0]).getVector3fMap();
-		Eigen::Vector3f v2 = cloud.at(polygon.vertices[2]).getVector3fMap() - cloud.at(polygon.vertices[0]).getVector3fMap();
-		Eigen::Vector3f n = (v1.cross(v2)).normalized();
-
-		Eigen::Vector3f p = Eigen::Vector3f(viewPoint.x, viewPoint.y, viewPoint.z) - cloud.at(polygon.vertices[1]).getVector3fMap();
-
-		float result = n.dot(p);
-		if(result < 0)
-		{
-			//reverse vertices order
-			int tmp = polygon.vertices[0];
-			polygon.vertices[0] = polygon.vertices[2];
-			polygon.vertices[2] = tmp;
-		}
-
-		output[i] = polygon;
-	}
-	return output;
-}
 
 } // namespace util3d
 } // namespace rtabmap
 
+#include "rtabmap/core/impl/util3d_surface.hpp"
+
 #endif /* UTIL3D_SURFACE_H_ */

corelib/include/rtabmap/core/util3d_transforms.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
+#include <pcl/pcl_base.h>
 #include <rtabmap/core/Transform.h>
 
 namespace rtabmap
@@ -40,6 +41,13 @@ namespace rtabmap
 namespace util3d
 {
 
+cv::Mat RTABMAP_EXP transformLaserScan(
+		const cv::Mat & laserScan,
+		const Transform & transform);
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP transformPointCloud(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const Transform & transform);
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP transformPointCloud(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const Transform & transform);
@@ -53,6 +61,27 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP transformPointCloud(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const Transform & transform);
 
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP transformPointCloud(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const Transform & transform);
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP transformPointCloud(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const Transform & transform);
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP transformPointCloud(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const Transform & transform);
+pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP transformPointCloud(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const Transform & transform);
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP transformPointCloud(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const Transform & transform);
+
 cv::Point3f RTABMAP_EXP transformPoint(
 		const cv::Point3f & pt,
 		const Transform & transform);