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

@@ -21,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 8)
+SET(RTABMAP_PATCH_VERSION 13)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -53,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")  
@@ -125,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)
@@ -138,6 +148,7 @@ 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)
 
@@ -176,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)
@@ -185,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)
@@ -195,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)
 
@@ -213,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)
 
@@ -285,6 +314,13 @@ IF(WITH_ZED)
     ENDIF(ZED_FOUND)
 ENDIF(WITH_ZED)
 
+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)
@@ -292,7 +328,7 @@ IF(WITH_OCTOMAP)
     ENDIF(OCTOMAP_FOUND)
 ENDIF(WITH_OCTOMAP)
 
-IF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND)
+IF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND)
    #Newest versions require std11
    IF(NOT MSVC)
      include(CheckCXXCompilerFlag)
@@ -306,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)
@@ -396,11 +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))
@@ -687,6 +734,14 @@ 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)

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,7 @@ 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);
-	app->poseReceived(app->tangoPoseToTransform(pose, true));
+	if(pose->status_code == TANGO_POSE_VALID)
+	{
+		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.
-	frame_pair.base = TANGO_COORDINATE_FRAME_IMU;
-	frame_pair.target = TANGO_COORDINATE_FRAME_DEVICE;
-	ret = TangoService_getPoseAtTime(0.0, frame_pair, &pose_data);
-	if (ret != TANGO_SUCCESS)
-	{
-		LOGE("NativeRTABMap: Failed to get transform between the IMU frame and device frames");
-		return false;
-	}
-	imuTDevice_ = rtabmap::Transform(
-			pose_data.translation[0],
-			pose_data.translation[1],
-			pose_data.translation[2],
-			pose_data.orientation[0],
-			pose_data.orientation[1],
-			pose_data.orientation[2],
-			pose_data.orientation[3]);
-
-	// Get color camera with respect to imu transformation matrix.
-	frame_pair.base = TANGO_COORDINATE_FRAME_IMU;
-	frame_pair.target = TANGO_COORDINATE_FRAME_CAMERA_DEPTH;
+	// Get color camera with respect to device transformation matrix.
+	frame_pair.base = 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 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
-	model_.setLocalTransform(Transform(
-			0.0f, 0.0f, 1.0f, 0.0f,
-			-1.0f, 0.0f, 0.0f, 0.0f,
-			0.0f, -1.0f, 0.0f, 0.0f));
+	// device to camera optical rotation in rtabmap frame
+	model_.setLocalTransform(tango_device_T_rtabmap_device.inverse()*deviceTColorCamera_);
+
+	LOGI("deviceTColorCameraTango  =%s", deviceTColorCamera_.prettyPrint().c_str());
+	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,69 +407,52 @@ 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
-		// opengl_world_T_opengl_camera =
-		//      opengl_world_T_start_service *
-		//      start_service_T_device *
-		//      device_T_imu *
-		//      imu_T_depth_camera *
-		//      depth_camera_T_opengl_camera;
-		if(inOpenGLFrame)
-		{
-			pose = opengl_world_T_tango_world * pose * deviceTDepth_ * depth_camera_T_opengl_camera;
-		}
-	}
+	pose = rtabmap::Transform(
+			tangoPose->translation[0],
+			tangoPose->translation[1],
+			tangoPose->translation[2],
+			tangoPose->orientation[0],
+			tangoPose->orientation[1],
+			tangoPose->orientation[2],
+			tangoPose->orientation[3]);
 
 	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;
-		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)
-		{
-			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);
-		}
-
-
+		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);
+	}
+
 	return pose;
 }
 
@@ -544,35 +536,54 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 		// 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());
-			poseColor.setNull();
+			colorToDepth = tangoPoseToTransform(&pose_color_image_t1_T_depth_image_t0);
 		}
-		if(poseDepth.getNormSquared() > 100000)
+		else
 		{
-			LOGE("Very large odometry depth pose detected (%s)! Ignoring this frame!", poseDepth.prettyPrint().c_str());
-			poseDepth.setNull();
+			LOGE(
+				"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(CameraInfo * info)
 					}
 				}
 			}
+
+			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(CameraInfo * info)
 
 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;}
 
@@ -89,7 +91,7 @@ protected:
 	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_;
-
-	struct Mesh
-	{
-		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
-		std::vector<pcl::Vertices> polygons;
-		rtabmap::Transform pose;
-		cv::Mat texture;
-	};
+	boost::mutex renderingMutex_;
 
 	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 cv::Mat & image) :
+		const pcl::IndicesPtr & indices,
+		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_);
-	if(!vertex_buffers_)
-	{
-		LOGE("OpenGL: could not generate vertex buffers\n");
-		return;
-	}
-
-	if(!cloud->is_dense && !image.empty())
-	{
-		LOGI("cloud=%dx%d image=%dx%d\n", (int)cloud->width, (int)cloud->height, image.cols, image.rows);
-		UASSERT(polygons.size() && !cloud->is_dense && !image.empty() && image.type() == CV_8UC3);
-		glGenTextures(1, &textures_);
-		if(!textures_)
-		{
-			vertex_buffers_ = 0;
-			LOGE("OpenGL: could not generate vertex buffers\n");
-			return;
-		}
-	}
-
-	LOGI("Creating cloud buffer %d", vertex_buffers_);
-	std::vector<float> vertices;
-	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(
+		GLuint cloudShaderProgram,
+		GLuint textureShaderProgram,
+		const Mesh & mesh,
+		const cv::Mat & texture) :
+		vertex_buffers_(0),
+		textures_(0),
+		nPoints_(0),
+		pose_(1.0f),
+		visible_(true),
+		cloud_shader_program_(cloudShaderProgram),
+		texture_shader_program_(textureShaderProgram),
+		gain_(1.0f)
+{
+	updateMesh(mesh, texture);
 }
 
 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 cv::Mat & image = cv::Mat());
+		  const pcl::IndicesPtr & indices,
+		  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() {
 	  }
   }
 
-
-	grid_->Render(gesture_camera_->GetProjectionMatrix(),
-				  gesture_camera_->GetViewMatrix());
+  	if(gridVisible_)
+  	{
+  		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 rtabmap::Transform & pose,
-		const cv::Mat & image)
+		const pcl::IndicesPtr & indices,
+		const rtabmap::Transform & pose)
 {
-	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,
-			image);
+			indices,
+			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 rtabmap::Transform & pose,
-		  const cv::Mat & image = cv::Mat());
+		  const pcl::IndicesPtr & indices,
+  		  const rtabmap::Transform & pose);
+  void addMesh(
+  		int id,
+  		const Mesh & mesh,
+		const 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_y = touch0_start_position_.x - x0;
+        float rotation_x = (touch0_start_position_.y - y0) * kRotationSpeed;
+        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 =
-            start_touch_dist_ - std::sqrt(abs_x * abs_x + abs_y * abs_y);
-        cam_cur_dist_ =
-            tango_gl::util::Clamp(cam_start_dist_ + dist * kZoomSpeed,
+        float dist = start_touch_dist_ - std::sqrt(abs_x * abs_x + abs_y * abs_y);
+
+        cam_cur_dist_ = tango_gl::util::Clamp(cam_start_dist_ + dist * kZoomSpeed,
                                   kCamViewMinDist, kCamViewMaxDist);
+
+        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,
-		0.0f, 0.0f, 1.0f, 0.0f,
+		1.0f,  0.0f,  0.0f, 0.0f,
+		0.0f,  0.0f,  1.0f, 0.0f,
 		0.0f, -1.0f,  0.0f, 0.0f);
 
-static const rtabmap::Transform depth_camera_T_opengl_camera(
-		1.0f, 0.0f, 0.0f, 0.0f,
-		0.0f, -1.0f, 0.0f, 0.0f,
-		0.0f, 0.0f, -1.0f, 0.0f);
+static const rtabmap::Transform rtabmap_world_T_tango_world(
+		 0.0f, 1.0f, 0.0f, 0.0f,
+	    -1.0f, 0.0f, 0.0f, 0.0f,
+		 0.0f, 0.0f, 1.0f, 0.0f);
+
+static const rtabmap::Transform tango_device_T_rtabmap_device(
+		 0.0f, -1.0f, 0.0f, 0.0f,
+	     0.0f,  0.0f, 1.0f, 0.0f,
+		-1.0f,  0.0f, 0.0f, 0.0f);
 
 static const rtabmap::Transform opengl_world_T_rtabmap_world(
-		0.0f, -1.0f,  0.0f, 0.0f,
-		0.0f, 0.0f, 1.0f, 0.0f,
-		-1.0f, 0.0f,  0.0f, 0.0f);
+		 0.0f, -1.0f, 0.0f, 0.0f,
+		 0.0f,  0.0f, 1.0f, 0.0f,
+		-1.0f,  0.0f, 0.0f, 0.0f);
 
-static const rtabmap::Transform rtabmap_world_T_opengl_world(
+static const rtabmap::Transform rtabmap_device_T_opengl_device(
 		 0.0f, 0.0f, -1.0f, 0.0f,
 		-1.0f, 0.0f,  0.0f, 0.0f,
 		 0.0f, 1.0f,  0.0f, 0.0f);
@@ -129,4 +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/detect_more_loop_closures" android:title="Detect More Loop Closures" />
-	            <item android:id="@+id/global_graph_optimization" android:title="Global Graph Optimization" />
-	            <item android:id="@+id/sba" android:title="Bundle Adjustement" />
-	         </group>   
+             <item android:id="@+id/post_processing_standard" android:title="Standard Optimization" />
+             <item android:id="@+id/post_processing_advanced" android:title="Advanced..." >
+	            <menu>
+	              <item android:id="@+id/global_graph_optimization" android:title="Global Graph Optimization" />
+	              <item android:id="@+id/detect_more_loop_closures" android:title="Detect More Loop Closures" />
+	              <item android:id="@+id/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/max_depth" android:checkable="false" android:title="Cloud/Mesh Max Depth..." />
-            <item android:id="@+id/mesh_angle_tolerance" android:checkable="false" android:title="Mesh Angle Tolerance..." />
-            <item android:id="@+id/mesh_triangle_size" android:checkable="false" android:title="Mesh Triangle Size..." />
+            <item android:id="@+id/grid_visible" android:checked="true" android:title="Grid Visible" />
+            <item android:id="@+id/graph_optimization" android:checked="true" android:title="Optimized Graph" />
         </group>   
         </menu>
 	    </item>
@@ -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);
-	  
-	  /*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();
-		}
+			  status == 1 && msg.isEmpty()?(mItemPause!=null&&mItemPause.isChecked()?"Paused":mItemLocalizationMode!=null&&mItemLocalizationMode.isChecked()?"Localization":mItemDataRecorderMode!=null&&mItemDataRecorderMode.isChecked()?"Recording":"Mapping"):msg);
   }
   
   //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());
-    	  mItemOpen.setEnabled(item.isChecked());
-    	  mItemPostProcessing.setEnabled(item.isChecked());
-    	 // mItemSave.setEnabled(item.isChecked() && !mWorkingDirectory.isEmpty());
+		  mItemExport.setEnabled(item.isChecked() && !mItemDataRecorderMode.isChecked());
+    	  mItemOpen.setEnabled(item.isChecked() && !mItemDataRecorderMode.isChecked());
+    	  mItemPostProcessing.setEnabled(item.isChecked() && !mItemDataRecorderMode.isChecked());
+    	  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());
-    	  RTABMapLib.setMeshRendering(item.isChecked());
+    	  item.setChecked(true);
+    	  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):");
-			  final EditText input = new EditText(this);
-			  input.setInputType(InputType.TYPE_CLASS_TEXT);        
-			  builder.setView(input);
-			  builder.setPositiveButton("OK", new DialogInterface.OnClickListener() {
-				  @Override
-				  public void onClick(DialogInterface dialog, int which)
+		  AlertDialog.Builder builder = new AlertDialog.Builder(this);
+		  builder.setTitle("RTAB-Map Database Name (*.db):");
+		  final EditText input = new EditText(this);
+		  input.setInputType(InputType.TYPE_CLASS_TEXT); 
+		  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.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();  
-					  dialog.dismiss();
-					  if(!fileName.isEmpty())
+					  File newFile = new File(mWorkingDirectory + fileName + ".db");
+					  if(newFile.exists())
 					  {
-						  File newFile = new File(mWorkingDirectory + fileName + ".db");
-						  if(newFile.exists())
-						  {
-							  new AlertDialog.Builder(getActivity())
-				                .setTitle("File Already Exists")
-				                .setMessage("Do you want to overwrite the existing file?")
-				                .setPositiveButton("Yes", new DialogInterface.OnClickListener() {
-				                    public void onClick(DialogInterface dialog, int which) {
-				                    	mNewDatabasePath = mWorkingDirectory + fileName + ".db";
-				                    	
-				                    	mProgressDialog.setTitle("Saving");
-				                  	    mProgressDialog.setMessage(String.format("Please wait while saving \"%s\"...", mNewDatabasePath));
-				                  	    mProgressDialog.show();
-				                    	
-										RTABMapLib.save(); // send save event
-										//disable gui actions
-										mItemSave.setEnabled(false);
-										mItemOpen.setEnabled(false);
-										mItemPostProcessing.setEnabled(false);
-										mItemExport.setEnabled(false);
-				                    }
-				                })
-				                .setNegativeButton("No", new DialogInterface.OnClickListener() {
-				                    public void onClick(DialogInterface dialog, int which) {
-				                        dialog.dismiss();
-				                    }
-				                })
-				                .show();
-						  }
-						  else
-						  {
-							  mNewDatabasePath = mWorkingDirectory + fileName + ".db";
-							  
-							  mProgressDialog.setTitle("Saving");
-				        	  mProgressDialog.setMessage(String.format("Please wait while saving \"%s\"...", mNewDatabasePath));
-				        	  mProgressDialog.show();
-							  
-							  RTABMapLib.save(); // send save event
-							  //disable gui actions
-							  mItemSave.setEnabled(false);
-							  mItemOpen.setEnabled(false);
-							  mItemPostProcessing.setEnabled(false);
-							  mItemExport.setEnabled(false);
-						  }
+						  new AlertDialog.Builder(getActivity())
+			                .setTitle("File Already Exists")
+			                .setMessage("Do you want to overwrite the existing file?")
+			                .setPositiveButton("Yes", new DialogInterface.OnClickListener() {
+			                    public void onClick(DialogInterface dialog, int which) {
+			                    	
+			                    	final String newDatabasePath = mWorkingDirectory + fileName + ".db";
+			                    	mProgressDialog.setTitle("Saving");
+			                    	if(mOpenedDatabasePath.equals(newDatabasePath))
+			                    	{
+			                    		mProgressDialog.setMessage(String.format("Please wait while updating \"%s\"...", newDatabasePath));
+			                    	}
+			                    	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();
+								}
+			                })
+			                .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();
-    	  }
-    	  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);
-    	  }
+			  }
+		  });
+		  builder.show();
       }
       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/main.cpp

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

cmake_modules/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
-# macro.
+# libraries; this is adapted from the rtabmap config
 
 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)
-      SET(${MYLIBRARY}_DEBUG ${MYLIBRARY})
-    ENDIF(MYLIBRARY)
-  ENDIF( NOT ${MYLIBRARY}_DEBUG)
+  IF(${MYLIBRARY}_DEBUG AND ${MYLIBRARY})
+    SET(${MYLIBRARY}
+      debug ${${MYLIBRARY}_DEBUG}
+      optimized ${${MYLIBRARY}}
+    )
+  ELSEIF(${MYLIBRARY}_DEBUG)
+    SET(${MYLIBRARY} ${${MYLIBRARY}_DEBUG})
+  ENDIF()  
   
 ENDMACRO(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME)
 

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})
-ENDIF (Tango_INCLUDE_DIR AND Tango_LIBRARY)
+   SET(Tango_LIBRARIES ${Tango_LIBRARY} ${Tango_support_LIBRARY})
+ENDIF (Tango_INCLUDE_DIR AND Tango_LIBRARY AND Tango_support_LIBRARY)
 
 IF (Tango_FOUND)
    # show which Tango was found only if not quiet
@@ -24,7 +25,7 @@ IF (Tango_FOUND)
 ELSE (Tango_FOUND)
    # fatal error if Tango is required but not found
    IF (Tango_FIND_REQUIRED)
-      MESSAGE(FATAL_ERROR "Could not find Tango")
+      MESSAGE(FATAL_ERROR "Could not find Tango (client and/or support libraries)")
    ENDIF (Tango_FIND_REQUIRED)
 ENDIF (Tango_FOUND)
 

corelib/include/rtabmap/core/CameraInfo.h

@@ -45,6 +45,9 @@ public:
 		timeMirroring(0.0f),
 		timeImageDecimation(0.0f),
 		timeScanFromDepth(0.0f),
+		timeUndistortDepth(0.0f),
+		timeBilateralFiltering(0.0f),
+		timeTotal(0.0f),
 		odomCovariance(cv::Mat::eye(6,6,CV_64FC1))
 	{
 	}
@@ -58,6 +61,9 @@ public:
 	float timeMirroring;
 	float timeImageDecimation;
 	float timeScanFromDepth;
+	float timeUndistortDepth;
+	float timeBilateralFiltering;
+	float timeTotal;
 	Transform odomPose;
 	cv::Mat odomCovariance;
 };

corelib/include/rtabmap/core/CameraModel.h

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

@@ -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;
-		syncImageRateWithStamps_ = syncImageRateWithStamps;
+		_timestampsPath=filePath;
+		_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;
 	}
 
@@ -120,6 +127,7 @@ public:
 
 protected:
 	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_;
-	bool syncImageRateWithStamps_;
+	std::string _timestampsPath;
+	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;
 

corelib/include/rtabmap/core/CameraRGBD.h

@@ -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;
 
@@ -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(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();
@@ -207,9 +222,13 @@ protected:
 	virtual SensorData captureImage(CameraInfo * info = 0);
 
 private:
+#ifdef RTABMAP_FREENECT
 	int deviceId_;
+	Type type_;
 	freenect_context * ctx_;
 	FreenectDevice * freenectDevice_;
+	StereoCameraModel stereoModel_;
+#endif
 };
 
 /////////////////////////
@@ -252,6 +271,7 @@ protected:
 	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
 };
 
 

corelib/include/rtabmap/core/CameraStereo.h

@@ -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
@@ -73,8 +74,10 @@ protected:
 	virtual SensorData captureImage(CameraInfo * info = 0);
 
 private:
+#ifdef RTABMAP_DC1394
 	DC1394Device *device_;
 	StereoCameraModel stereoModel_;
+#endif
 };
 
 /////////////////////////
@@ -98,8 +101,10 @@ protected:
 	virtual SensorData captureImage(CameraInfo * info = 0);
 
 private:
+#ifdef RTABMAP_FLYCAPTURE2
 	FlyCapture2::Camera * camera_;
 	void * triclopsCtx_; // TriclopsContext
+#endif
 };
 
 /////////////////////////
@@ -120,7 +125,8 @@ public:
 			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
@@ -128,18 +134,20 @@ public:
 			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 { return computeOdometry_; }
+	virtual bool odomProvided() const;
 
 protected:
 	virtual SensorData captureImage(CameraInfo * info = 0);
 
 private:
+#ifdef RTABMAP_ZED
 	sl::zed::Camera * zed_;
 	StereoCameraModel stereoModel_;
 	CameraVideo::Source src_;
@@ -147,10 +155,12 @@ private:
 	std::string svoFilePath_;
 	int resolution_;
 	int quality_;
+	bool selfCalibration_;
 	int sensingMode_;
 	int confidenceThr_;
 	bool computeOdometry_;
 	bool lost_;
+#endif
 };
 
 /////////////////////////
@@ -202,7 +212,13 @@ 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());
@@ -222,7 +238,9 @@ protected:
 
 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

@@ -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/DBDriver.h

@@ -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 ParametersMap & parameters) 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;
@@ -108,6 +116,7 @@ public:
 	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;
 
@@ -118,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 getNodeData(int signatureId, SensorData & data) const;
+	void loadNodeData(std::list<Signature *> & signatures, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
+	void getNodeData(int signatureId, SensorData & data, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
 	bool getCalibration(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
 	bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose) const;
 	void loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
@@ -137,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;
@@ -151,6 +160,7 @@ private:
 	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;
 
@@ -164,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;
@@ -171,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/Features2d.h

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

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

@@ -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(int fromId, int toId, const Transform & transform, const cv::Mat & covariance);
+	void updateLink(const Link & link, bool updateInDatabase = false);
 	void removeAllVirtualLinks();
 	void removeVirtualLinks(int signatureId);
 	std::map<int, int> getNeighborsId(
@@ -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(int fromId, int toId, Transform guess, RegistrationInfo * info = 0);
+	Transform computeTransform(Signature & fromS, Signature & toS, Transform guess, RegistrationInfo * info = 0, bool useKnownCorrespondencesIfPossible = false) const;
+	Transform computeTransform(int fromId, int toId, Transform guess, RegistrationInfo * info = 0, bool useKnownCorrespondencesIfPossible = false);
 	Transform 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

@@ -57,12 +57,17 @@ public:
 
 class RTABMAP_EXP OctoMap {
 public:
-	OctoMap(float voxelSize = 0.1f);
+	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);
+			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_;}
@@ -84,11 +89,14 @@ public:
 	void clear();
 
 private:
-	std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> > cache_;
+	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 */

corelib/include/rtabmap/core/Odometry.h

@@ -44,7 +44,7 @@ class RTABMAP_EXP Odometry
 public:
 	enum Type {
 		kTypeUndef = -1,
-		kTypeLocalMap = 0,
+		kTypeF2M = 0,
 		kTypeF2F = 1
 	};
 
@@ -57,6 +57,7 @@ public:
 	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;}

corelib/include/rtabmap/core/OdometryF2F.h

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

@@ -32,11 +32,13 @@ 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/Link.h>
 
 namespace rtabmap {
 
 class Signature;
 class Registration;
+class Optimizer;
 
 class RTABMAP_EXP OdometryF2M : public Odometry
 {
@@ -48,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);
 
@@ -55,16 +59,25 @@ private:
 	//Parameters
 	int maximumMapSize_;
 	float keyFrameThr_;
+	int visKeyFrameThr_;
 	int maxNewFeatures_;
 	float scanKeyFrameThr_;
 	int scanMaximumMapSize_;
 	float scanSubtractRadius_;
-	std::string fixedMapPath_;
+	int bundleAdjustment_;
+	int bundleMaxFrames_;
 
 	Registration * regPipeline_;
 	Signature * map_;
 	Signature * lastFrame_;
 	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

@@ -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/OdometryThread.h

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

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

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

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

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

@@ -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/Parameters.h

@@ -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>
 
@@ -176,7 +177,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Rtabmap, MemoryThr, 		             int, 0, 	 "Maximum signatures in the Working Memory (ms) (0 means infinity).");
 	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.");
@@ -206,10 +207,11 @@ 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, 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, 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. 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");
@@ -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, "");
@@ -307,7 +309,7 @@ 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,    "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,    "Linear velocity (m/sec) used to compute path weights.");
@@ -317,36 +319,44 @@ class RTABMAP_EXP Parameters
 	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,    "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,      "Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies). Not compatible with \"RGBD/OptimizeMaxError\" if enabled.");
+	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(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,       "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,8 +374,9 @@ 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
@@ -373,7 +384,8 @@ class RTABMAP_EXP Parameters
 	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, 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.");
@@ -389,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, RefineIterations,         int, 5,        "[Vis/EstimationType = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.");
-	RTABMAP_PARAM(Vis, PnPReprojError, 	         float, 2,      "[Vis/EstimationType = 1] PnP reprojection error.");
-	RTABMAP_PARAM(Vis, PnPFlags,                 int, 1,        "[Vis/EstimationType = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P");
-	RTABMAP_PARAM(Vis, PnPRefineIterations,      int, 1,        "[Vis/EstimationType = 1] Refine iterations.");
-	RTABMAP_PARAM(Vis, EpipolarGeometryVar,      float, 0.02,   "[Vis/EstimationType = 2] Epipolar geometry maximum variance to accept the transformation.");
+	RTABMAP_PARAM(Vis, 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,        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,      uFormat("[%s = 1] PnP reprojection error.", kVisEstimationType().c_str()));
+	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,        uFormat("[%s = 1] Refine iterations.", kVisEstimationType().c_str()));
+	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
@@ -416,18 +428,19 @@ class RTABMAP_EXP Parameters
 	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, CorNNDR,                  float, 0.8,    "[Vis/CorrespondenceType=0] NNDR: nearest neighbor distance ratio. Used for features matching approach.");
-	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, CorFlowWinSize,           int, 16,       "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.");
-	RTABMAP_PARAM(Vis, CorFlowIterations,        int, 30,       "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.");
-	RTABMAP_PARAM(Vis, CorFlowEps,               float, 0.01,   "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.");
-	RTABMAP_PARAM(Vis, CorFlowMaxLevel,          int, 3,        "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow 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.6,    uFormat("[%s=0] NNDR: nearest neighbor distance ratio. Used for features matching approach.", kVisCorType().c_str()));
+	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,       uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
+	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,   uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
+	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.");
@@ -444,12 +457,12 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Stereo, MinDisparity,          int, 1,        "Minimum disparity.");
 	RTABMAP_PARAM(Stereo, MaxDisparity,          int, 128,      "Maximum disparity.");
 	RTABMAP_PARAM(Stereo, OpticalFlow,           bool, true,    "Use optical flow to find stereo correspondences, otherwise a simple block matching approach is used.");
-	RTABMAP_PARAM(Stereo, SSD,                   bool, true,    "[Stereo/OpticalFlow = false] Use Sum of Squared Differences (SSD) window, otherwise Sum of Absolute Differences (SAD) window is used.");
-	RTABMAP_PARAM(Stereo, Eps,                   double, 0.01,  "[Stereo/OpticalFlow = true] Epsilon stop criterion.");
+	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,  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");
@@ -457,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();
 
@@ -481,16 +526,16 @@ public:
 	 */
 	static std::string getDescription(const std::string & paramKey);
 
-	static void parse(const ParametersMap & parameters, const std::string & key, bool & value);
-	static void parse(const ParametersMap & parameters, const std::string & key, int & value);
-	static void parse(const ParametersMap & parameters, const std::string & key, unsigned int & value);
-	static void parse(const ParametersMap & parameters, const std::string & key, float & value);
-	static void 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, bool & value);
+	static bool parse(const ParametersMap & parameters, const std::string & key, int & value);
+	static bool parse(const ParametersMap & parameters, const std::string & key, unsigned int & value);
+	static bool parse(const ParametersMap & parameters, const std::string & key, float & value);
+	static bool parse(const ParametersMap & parameters, const std::string & key, double & 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();
@@ -499,7 +544,7 @@ public:
 	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/RegistrationInfo.h

@@ -35,7 +35,8 @@ class RegistrationInfo
 {
 public:
 	RegistrationInfo() :
-		variance(0),
+		varianceLin(0),
+		varianceAng(0),
 		inliers(0),
 		matches(0),
 		icpInliersRatio(0),
@@ -44,7 +45,8 @@ public:
 	{
 	}
 
-	float variance;
+	float varianceLin;
+	float varianceAng;
 	std::string rejectedMsg;
 
 	// RegistrationVis

corelib/include/rtabmap/core/RegistrationVis.h

@@ -81,8 +81,10 @@ private:
 	int _flowMaxLevel;
 	float _nndr;
 	int _guessWinSize;
+	int _bundleAdjustment;
 
 	ParametersMap _featureParameters;
+	ParametersMap _bundleParameters;
 };
 
 }

corelib/include/rtabmap/core/Rtabmap.h

@@ -61,13 +61,20 @@ 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; }
@@ -101,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(
@@ -128,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
@@ -142,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;
@@ -195,6 +210,8 @@ private:
 	float _localRadius;
 	float _localImmunizationRatio;
 	int _proximityMaxGraphDepth;
+	int _proximityMaxPaths;
+	int _proximityMaxNeighbors;
 	float _proximityFilteringRadius;
 	bool _proximityRawPosesUsed;
 	float _proximityAngle;
@@ -235,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
 

corelib/include/rtabmap/core/RtabmapEvent.h

@@ -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/RtabmapThread.h

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

@@ -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,
-			float laserScanMaxRange,
+			const LaserScanInfo & laserScanInfo,
 			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,
-			float laserScanMaxRange,
+			const LaserScanInfo & laserScanInfo,
 			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,
-			float laserScanMaxRange,
+			const LaserScanInfo & laserScanInfo,
 			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;}
-	float laserScanMaxRange() const {return _laserScanMaxRange;}
+	const LaserScanInfo & laserScanInfo() const {return _laserScanInfo;}
 
 	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,23 +168,54 @@ 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 uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthOrRightRaw, cv::Mat * laserScanRaw = 0, cv::Mat * userDataRaw = 0) const;
+	void uncompressData(
+			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)
-	{
-		_keypoints = keypoints;
-		_descriptors = descriptors;
-	}
+	// detect automatically if raw or compressed. If raw, the data will be compressed.
+	void setOccupancyGrid(
+			const cv::Mat & ground,
+			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;}
@@ -199,8 +226,6 @@ public:
 private:
 	int _id;
 	double _stamp;
-	int _laserScanMaxPts;
-	float _laserScanMaxRange;
 
 	cv::Mat _imageCompressed;          // compressed image
 	cv::Mat _depthOrRightCompressed;   // compressed image
@@ -213,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/Statistics.h

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

corelib/include/rtabmap/core/StereoCameraModel.h

@@ -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/Transform.h

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

@@ -83,7 +83,9 @@ void segmentObstaclesFromGround(
 				normalKSearch,
 				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(
@@ -93,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())
@@ -108,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
@@ -138,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)
@@ -242,7 +252,7 @@ void occupancy2DFromGroundObstacles(
 		//voxelize to grid cell size
 		groundCloudProjected = util3d::voxelize(groundCloudProjected, cellSize);
 
-		ground = cv::Mat((int)groundCloudProjected->size(), 1, CV_32FC2);
+		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;
@@ -259,7 +269,7 @@ void occupancy2DFromGroundObstacles(
 		//voxelize to grid cell size
 		obstaclesCloudProjected = util3d::voxelize(obstaclesCloudProjected, cellSize);
 
-		obstacles = cv::Mat((int)obstaclesCloudProjected->size(), 1, CV_32FC2);
+		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;

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

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

@@ -141,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,
@@ -154,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(
@@ -162,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,
@@ -173,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_filtering.h

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

corelib/include/rtabmap/core/util3d_mapping.h

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

corelib/include/rtabmap/core/util3d_surface.h

@@ -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,
@@ -194,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))
-{
-	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;
-}
+		const pcl::PointXYZ & viewPoint = pcl::PointXYZ(0,0,0));
 
 } // namespace util3d
 } // namespace rtabmap
 
+#include "rtabmap/core/impl/util3d_surface.hpp"
+
 #endif /* UTIL3D_SURFACE_H_ */

corelib/include/rtabmap/core/util3d_transforms.h

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

corelib/src/CMakeLists.txt

@@ -64,11 +64,22 @@ SET(SRC_FILES
 	Stereo.cpp
 	StereoDense.cpp
 	StereoCameraModel.cpp
+	
+	OccupancyGrid.cpp
+	
+	GainCompensator.cpp
 		
 	rtflann/ext/lz4.c
 	rtflann/ext/lz4hc.c
+	FlannIndex.cpp
 	
 	sqlite3/sqlite3.c	
+	
+	#clams stuff
+	clams/discrete_depth_distortion_model_helpers.cpp
+	clams/discrete_depth_distortion_model.cpp
+	clams/frame_projector.cpp
+	clams/slam_calibrator.cpp
 )
 
 IF(OpenCV_VERSION_MAJOR EQUAL 2)
@@ -131,6 +142,17 @@ IF(freenect2_FOUND)
 	)
 ENDIF(freenect2_FOUND)
 
+IF(RealSense_FOUND)
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${RealSense_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${RealSense_LIBRARIES}
+	)
+ENDIF(RealSense_FOUND)
+
 IF(DC1394_FOUND)
 	SET(INCLUDE_DIRS
 		${INCLUDE_DIRS}

corelib/src/CameraModel.cpp

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UDirectory.h>
 #include <rtabmap/utilite/UFile.h>
 #include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UMath.h>
 #include <opencv2/imgproc/imgproc.hpp>
 
 namespace rtabmap {
@@ -75,9 +76,19 @@ CameraModel::CameraModel(
 {
 	UASSERT_MSG(fx > 0.0, uFormat("fx=%f", fx).c_str());
 	UASSERT_MSG(fy > 0.0, uFormat("fy=%f", fy).c_str());
-	UASSERT_MSG(cx >= 0.0, uFormat("cx=%f", cx).c_str());
-	UASSERT_MSG(cy >= 0.0, uFormat("cy=%f", cy).c_str());
+	UASSERT_MSG(cx >= 0.0 && imageSize.width>=0, uFormat("cx=%f imageSize.width=%d", cx, imageSize.width).c_str());
+	UASSERT_MSG(cy >= 0.0 && imageSize.height>=0, uFormat("cy=%f imageSize.height=%d", cy, imageSize.height).c_str());
 	UASSERT(!localTransform.isNull());
+
+	if(cx==0.0 && imageSize.width > 0)
+	{
+		cx = double(imageSize.width)/2.0-0.5;
+	}
+	if(cy==0.0 && imageSize.height > 0)
+	{
+		cy = double(imageSize.height)/2.0-0.5;
+	}
+
 	if(Tx != 0.0)
 	{
 		P_ = cv::Mat::eye(3, 4, CV_64FC1),
@@ -110,9 +121,19 @@ CameraModel::CameraModel(
 {
 	UASSERT_MSG(fx > 0.0, uFormat("fx=%f", fx).c_str());
 	UASSERT_MSG(fy > 0.0, uFormat("fy=%f", fy).c_str());
-	UASSERT_MSG(cx >= 0.0, uFormat("cx=%f", cx).c_str());
-	UASSERT_MSG(cy >= 0.0, uFormat("cy=%f", cy).c_str());
+	UASSERT_MSG(cx >= 0.0 && imageSize.width>=0, uFormat("cx=%f imageSize.width=%d", cx, imageSize.width).c_str());
+	UASSERT_MSG(cy >= 0.0 && imageSize.height>=0, uFormat("cy=%f imageSize.height=%d", cy, imageSize.height).c_str());
 	UASSERT(!localTransform.isNull());
+
+	if(cx==0.0 && imageSize.width > 0)
+	{
+		cx = double(imageSize.width)/2.0-0.5;
+	}
+	if(cy==0.0 && imageSize.height > 0)
+	{
+		cy = double(imageSize.height)/2.0-0.5;
+	}
+
 	if(Tx != 0.0)
 	{
 		P_ = cv::Mat::eye(3, 4, CV_64FC1),
@@ -140,6 +161,32 @@ void CameraModel::initRectificationMap()
 	cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
 }
 
+void CameraModel::setImageSize(const cv::Size & size)
+{
+	UASSERT((size.height > 0 && size.width > 0) || (size.height == 0 && size.width == 0));
+	imageSize_ = size;
+	double ncx = cx();
+	double ncy = cy();
+	if(ncx==0.0 && imageSize_.width > 0)
+	{
+		ncx = double(imageSize_.width)/2.0-0.5;
+	}
+	if(ncy==0.0 && imageSize_.height > 0)
+	{
+		ncy = double(imageSize_.height)/2.0-0.5;
+	}
+	if(!P_.empty())
+	{
+		P_.at<double>(0,2) = ncx;
+		P_.at<double>(1,2) = ncy;
+	}
+	if(!K_.empty())
+	{
+		K_.at<double>(0,2) = ncx;
+		K_.at<double>(1,2) = ncy;
+	}
+}
+
 bool CameraModel::load(const std::string & directory, const std::string & cameraName)
 {
 	K_ = cv::Mat();
@@ -383,6 +430,36 @@ CameraModel CameraModel::scaled(double scale) const
 	return scaledModel;
 }
 
+CameraModel CameraModel::roi(const cv::Rect & roi) const
+{
+	CameraModel roiModel = *this;
+	if(this->isValidForProjection())
+	{
+		// has only effect on cx and cy
+		cv::Mat K;
+		if(!K_.empty())
+		{
+			K = K_.clone();
+			K.at<double>(0,2) -= roi.x;
+			K.at<double>(1,2) -= roi.y;
+		}
+
+		cv::Mat P;
+		if(!P_.empty())
+		{
+			P = P_.clone();
+			P.at<double>(0,2) -= roi.x;
+			P.at<double>(1,2) -= roi.y;
+		}
+		roiModel = CameraModel(name_, roi.size(), K, D_, R_, P, localTransform_);
+	}
+	else
+	{
+		UWARN("Trying to extract roi from a camera model not valid! Ignoring roi...");
+	}
+	return roiModel;
+}
+
 double CameraModel::horizontalFOV() const
 {
 	if(imageWidth() > 0 && fx() > 0.0)
@@ -455,8 +532,8 @@ cv::Mat CameraModel::rectifyDepth(const cv::Mat & raw) const
 
 							//http://stackoverflow.com/questions/13299409/how-to-get-the-image-pixel-at-real-locations-in-opencv
 							rectified.at<unsigned short>(y,x) =
-									(raw.at<unsigned short>(yL, xL) * (1.f - a) + raw.at<unsigned short>(yL, xH) * a) * (1.f - c) +
-									(raw.at<unsigned short>(yH, xL) * (1.f - a) + raw.at<unsigned short>(yH, xH) * a) * c;
+									(pLT * (1.f - a) + pRT * a) * (1.f - c) +
+									(pLB * (1.f - a) + pRB * a) * c;
 						}
 					}
 				}
@@ -471,4 +548,40 @@ cv::Mat CameraModel::rectifyDepth(const cv::Mat & raw) const
 	}
 }
 
+// resulting 3D point is in /camera_link frame
+void CameraModel::project(float u, float v, float depth, float & x, float & y, float & z) const
+{
+	if(depth > 0.0f)
+	{
+		// Fill in XYZ
+		x = (u - cx()) * depth / fx();
+		y = (v - cy()) * depth / fy();
+		z = depth;
+	}
+	else
+	{
+		x = y = z = std::numeric_limits<float>::quiet_NaN();
+	}
+}
+// 3D point is in /camera_link frame
+void CameraModel::reproject(float x, float y, float z, float & u, float & v) const
+{
+	UASSERT(z!=0.0f);
+	float invZ = 1.0f/z;
+	u = (fx()*x)*invZ + cx();
+	v = (fy()*y)*invZ + cy();
+}
+void CameraModel::reproject(float x, float y, float z, int & u, int & v) const
+{
+	UASSERT(z!=0.0f);
+	float invZ = 1.0f/z;
+	u = (fx()*x)*invZ + cx();
+	v = (fy()*y)*invZ + cy();
+}
+
+bool CameraModel::inFrame(int u, int v) const
+{
+	return uIsInBounds(u, 0, imageWidth()) && uIsInBounds(v, 0, imageHeight());
+}
+
 } /* namespace rtabmap */

corelib/src/CameraRGB.cpp

@@ -65,6 +65,7 @@ CameraImages::CameraImages() :
 		_dir(0),
 		_countScan(0),
 		_scanDir(0),
+		_scanLocalTransform(Transform::getIdentity()),
 		_scanMaxPts(0),
 		_scanDownsampleStep(1),
 		_scanVoxelSize(0.0f),
@@ -73,7 +74,8 @@ CameraImages::CameraImages() :
 		_depthFromScanFillHoles(1),
 		_depthFromScanFillHolesFromBorder(false),
 		_filenamesAreTimestamps(false),
-		syncImageRateWithStamps_(true),
+		_syncImageRateWithStamps(true),
+		_odometryFormat(0),
 		_groundTruthFormat(0),
 		_captureDelay(0.0)
 	{}
@@ -92,6 +94,7 @@ CameraImages::CameraImages(const std::string & path,
 	_dir(0),
 	_countScan(0),
 	_scanDir(0),
+	_scanLocalTransform(Transform::getIdentity()),
 	_scanMaxPts(0),
 	_scanDownsampleStep(1),
 	_scanVoxelSize(0.0f),
@@ -100,7 +103,8 @@ CameraImages::CameraImages(const std::string & path,
 	_depthFromScanFillHoles(1),
 	_depthFromScanFillHolesFromBorder(false),
 	_filenamesAreTimestamps(false),
-	syncImageRateWithStamps_(true),
+	_syncImageRateWithStamps(true),
+	_odometryFormat(0),
 	_groundTruthFormat(0),
 	_captureDelay(0.0)
 {
@@ -224,7 +228,8 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 	}
 
 	bool success = _dir->isValid();
-	stamps_.clear();
+	_stamps.clear();
+	odometry_.clear();
 	groundTruth_.clear();
 	if(success)
 	{
@@ -244,7 +249,7 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 					double stamp = uStr2Double(sec + "." + decimals);
 					if(stamp > 0.0)
 					{
-						stamps_.push_back(stamp);
+						_stamps.push_back(stamp);
 					}
 					else
 					{
@@ -252,19 +257,19 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 					}
 				}
 			}
-			if(stamps_.size() != this->imagesCount())
+			if(_stamps.size() != this->imagesCount())
 			{
 				UERROR("The stamps count is not the same as the images (%d vs %d)! "
 					   "Converting filenames to timestamps is activated.",
-						(int)stamps_.size(), this->imagesCount());
-				stamps_.clear();
+						(int)_stamps.size(), this->imagesCount());
+				_stamps.clear();
 				success = false;
 			}
 		}
-		else if(timestampsPath_.size())
+		else if(_timestampsPath.size())
 		{
 			std::ifstream file;
-			file.open(timestampsPath_.c_str(), std::ifstream::in);
+			file.open(_timestampsPath.c_str(), std::ifstream::in);
 			while(file.good())
 			{
 				std::string str;
@@ -288,120 +293,29 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 					}
 					stampStr = stampStr+'.'+millisecStr;
 				}
-				stamps_.push_back(uStr2Double(stampStr));
+				_stamps.push_back(uStr2Double(stampStr));
 			}
 
 			file.close();
 
-			if(stamps_.size() != this->imagesCount())
+			if(_stamps.size() != this->imagesCount())
 			{
 				UERROR("The stamps count (%d) is not the same as the images (%d)! Please remove "
 						"the timestamps file path if you don't want to use them (current file path=%s).",
-						(int)stamps_.size(), this->imagesCount(), timestampsPath_.c_str());
-				stamps_.clear();
+						(int)_stamps.size(), this->imagesCount(), _timestampsPath.c_str());
+				_stamps.clear();
 				success = false;
 			}
 		}
 
-		if(groundTruthPath_.size())
+		if(success && _odometryPath.size())
 		{
-			std::map<int, Transform> poses;
-			std::map<int, double> stamps;
-			if(!graph::importPoses(groundTruthPath_, _groundTruthFormat, poses, 0, &stamps))
-			{
-				UERROR("Cannot read ground truth file \"%s\".", groundTruthPath_.c_str());
-				success = false;
-			}
-			else if((_groundTruthFormat != 1 && _groundTruthFormat != 5 && _groundTruthFormat != 6 && _groundTruthFormat != 7) && poses.size() != this->imagesCount())
-			{
-				UERROR("The ground truth count is not the same as the images (%d vs %d)! Please remove "
-						"the ground truth file path if you don't want to use it (current file path=%s).",
-						(int)poses.size(), this->imagesCount(), groundTruthPath_.c_str());
-				success = false;
-			}
-			else if((_groundTruthFormat == 1 || _groundTruthFormat == 5 || _groundTruthFormat == 6 || _groundTruthFormat == 7) && stamps_.size() == 0)
-			{
-				UERROR("When using RGBD-SLAM, GPS, MALAGA and ST LUCIA formats for ground truth, images must have timestamps!");
-				success = false;
-			}
-			else if(_groundTruthFormat == 1 || _groundTruthFormat == 5 || _groundTruthFormat == 6 || _groundTruthFormat == 7)
-			{
-				UDEBUG("");
-				//Match ground truth values with images
-				groundTruth_.clear();
-				std::map<double, int> stampsToIds;
-				for(std::map<int, double>::iterator iter=stamps.begin(); iter!=stamps.end(); ++iter)
-				{
-					stampsToIds.insert(std::make_pair(iter->second, iter->first));
-				}
-				std::vector<double> values = uValues(stamps);
+			success = readPoses(odometry_, _stamps, _odometryPath, _odometryFormat);
+		}
 
-				int validPoses = 0;
-				for(std::list<double>::iterator ster=stamps_.begin(); ster!=stamps_.end(); ++ster)
-				{
-					Transform pose; // null transform
-					std::map<double, int>::iterator endIter = stampsToIds.lower_bound(*ster);
-					bool warned = false;
-					if(endIter != stampsToIds.end())
-					{
-						if(endIter->first == *ster)
-						{
-							pose = poses.at(endIter->second);
-						}
-						else if(endIter != stampsToIds.begin())
-						{
-							//interpolate
-							std::map<double, int>::iterator beginIter = endIter;
-							--beginIter;
-							double stampBeg = beginIter->first;
-							double stampEnd = endIter->first;
-							UASSERT(stampEnd > stampBeg && *ster>stampBeg && *ster < stampEnd);
-							if(stampEnd - stampBeg > 10.0)
-							{
-								warned = true;
-								UDEBUG("Cannot interpolate ground truth pose for stamp %f between %f and %f (>10 sec)", 
-									*ster,
-									stampBeg,
-									stampEnd);
-							}
-							else
-							{
-								float t = (*ster - stampBeg) / (stampEnd-stampBeg);
-								Transform & ta = poses.at(beginIter->second);
-								Transform & tb = poses.at(endIter->second);
-								if(!ta.isNull() && !tb.isNull())
-								{
-									++validPoses;
-									pose = ta.interpolate(t, tb);
-								}
-							}
-						}
-					}
-					if(pose.isNull() && !warned)
-					{
-						UDEBUG("Ground truth pose not found for stamp %f", *ster);
-					}
-					groundTruth_.push_back(pose);
-				}
-				if(validPoses != (int)stamps_.size())
-				{
-					UWARN("%d valid ground truth poses of %d stamps", validPoses, (int)stamps_.size());
-				}
-			}
-			else
-			{
-				UDEBUG("");
-				groundTruth_ = uValuesList(poses);
-				if(stamps_.size() == 0 && stamps.size() == poses.size())
-				{
-					stamps_ = uValuesList(stamps);
-				}
-				else if(_groundTruthFormat==8 && stamps_.size() == 0 && stamps.size()>0 && stamps.size() != poses.size())
-				{
-					UERROR("With Karlsruhe ground truth format, timestamps (%d) and poses (%d) should match!", (int)stamps.size(), (int)poses.size());
-				}
-			}
-			UASSERT_MSG(groundTruth_.size() == stamps_.size(), uFormat("%d vs %d", (int)groundTruth_.size(), (int)stamps_.size()).c_str());
+		if(success && _groundTruthPath.size())
+		{
+			success = readPoses(groundTruth_, _stamps, _groundTruthPath, _groundTruthFormat);
 		}
 	}
 
@@ -410,6 +324,110 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 	return success;
 }
 
+bool CameraImages::readPoses(std::list<Transform> & outputPoses, std::list<double> & inOutStamps, const std::string & filePath, int format) const
+{
+	outputPoses.clear();
+	std::map<int, Transform> poses;
+	std::map<int, double> stamps;
+	if(!graph::importPoses(filePath, format, poses, 0, &stamps))
+	{
+		UERROR("Cannot read pose file \"%s\".", filePath.c_str());
+		return false;
+	}
+	else if((format != 1 && format != 5 && format != 6 && format != 7) && poses.size() != this->imagesCount())
+	{
+		UERROR("The pose count is not the same as the images (%d vs %d)! Please remove "
+				"the pose file path if you don't want to use it (current file path=%s).",
+				(int)poses.size(), this->imagesCount(), filePath.c_str());
+		return false;
+	}
+	else if((format == 1 || format == 5 || format == 6 || format == 7) && inOutStamps.size() == 0)
+	{
+		UERROR("When using RGBD-SLAM, GPS, MALAGA and ST LUCIA formats, images must have timestamps!");
+		return false;
+	}
+	else if(format == 1 || format == 5 || format == 6 || format == 7)
+	{
+		UDEBUG("");
+		//Match ground truth values with images
+		outputPoses.clear();
+		std::map<double, int> stampsToIds;
+		for(std::map<int, double>::iterator iter=stamps.begin(); iter!=stamps.end(); ++iter)
+		{
+			stampsToIds.insert(std::make_pair(iter->second, iter->first));
+		}
+		std::vector<double> values = uValues(stamps);
+
+		int validPoses = 0;
+		for(std::list<double>::iterator ster=inOutStamps.begin(); ster!=inOutStamps.end(); ++ster)
+		{
+			Transform pose; // null transform
+			std::map<double, int>::iterator endIter = stampsToIds.lower_bound(*ster);
+			bool warned = false;
+			if(endIter != stampsToIds.end())
+			{
+				if(endIter->first == *ster)
+				{
+					pose = poses.at(endIter->second);
+				}
+				else if(endIter != stampsToIds.begin())
+				{
+					//interpolate
+					std::map<double, int>::iterator beginIter = endIter;
+					--beginIter;
+					double stampBeg = beginIter->first;
+					double stampEnd = endIter->first;
+					UASSERT(stampEnd > stampBeg && *ster>stampBeg && *ster < stampEnd);
+					if(stampEnd - stampBeg > 10.0)
+					{
+						warned = true;
+						UDEBUG("Cannot interpolate pose for stamp %f between %f and %f (>10 sec)",
+							*ster,
+							stampBeg,
+							stampEnd);
+					}
+					else
+					{
+						float t = (*ster - stampBeg) / (stampEnd-stampBeg);
+						Transform & ta = poses.at(beginIter->second);
+						Transform & tb = poses.at(endIter->second);
+						if(!ta.isNull() && !tb.isNull())
+						{
+							++validPoses;
+							pose = ta.interpolate(t, tb);
+						}
+					}
+				}
+			}
+			if(pose.isNull() && !warned)
+			{
+				UDEBUG("Pose not found for stamp %f", *ster);
+			}
+			outputPoses.push_back(pose);
+		}
+		if(validPoses != (int)inOutStamps.size())
+		{
+			UWARN("%d valid poses of %d stamps", validPoses, (int)inOutStamps.size());
+		}
+	}
+	else
+	{
+		UDEBUG("");
+		outputPoses = uValuesList(poses);
+		if(inOutStamps.size() == 0 && stamps.size() == poses.size())
+		{
+			inOutStamps = uValuesList(stamps);
+		}
+		else if(format==8 && inOutStamps.size() == 0 && stamps.size()>0 && stamps.size() != poses.size())
+		{
+			UERROR("With Karlsruhe format, timestamps (%d) and poses (%d) should match!", (int)stamps.size(), (int)poses.size());
+			return false;
+		}
+	}
+	UASSERT_MSG(outputPoses.size() == inOutStamps.size(), uFormat("%d vs %d", (int)outputPoses.size(), (int)inOutStamps.size()).c_str());
+	return true;
+}
+
 bool CameraImages::isCalibrated() const
 {
 	return _model.isValidForProjection();
@@ -440,7 +458,7 @@ std::vector<std::string> CameraImages::filenames() const
 
 SensorData CameraImages::captureImage(CameraInfo * info)
 {
-	if(syncImageRateWithStamps_ && _captureDelay>0.0)
+	if(_syncImageRateWithStamps && _captureDelay>0.0)
 	{
 		int sleepTime = (1000*_captureDelay - 1000.0f*_captureTimer.getElapsedTime());
 		if(sleepTime > 2)
@@ -474,6 +492,7 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 	cv::Mat img;
 	cv::Mat scan;
 	double stamp = UTimer::now();
+	Transform odometryPose;
 	Transform groundTruthPose;
 	cv::Mat depthFromScan;
 	UDEBUG("");
@@ -539,13 +558,18 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 			}
 		}
 
-		if(stamps_.size())
+		if(_stamps.size())
 		{
-			stamp = stamps_.front();
-			stamps_.pop_front();
-			if(stamps_.size())
+			stamp = _stamps.front();
+			_stamps.pop_front();
+			if(_stamps.size())
 			{
-				_captureDelay = stamps_.front() - stamp;
+				_captureDelay = _stamps.front() - stamp;
+			}
+			if(odometry_.size())
+			{
+				odometryPose = odometry_.front();
+				odometry_.pop_front();
 			}
 			if(groundTruth_.size())
 			{
@@ -666,11 +690,11 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 				pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK);
 				pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
 				pcl::concatenateFields(*cloud, *normals, *cloudNormals);
-				scan = util3d::laserScanFromPointCloud(*cloudNormals);
+				scan = util3d::laserScanFromPointCloud(*cloudNormals, _scanLocalTransform.inverse());
 			}
 			else
 			{
-				scan = util3d::laserScanFromPointCloud(*cloud);
+				scan = util3d::laserScanFromPointCloud(*cloud, _scanLocalTransform.inverse());
 			}
 		}
 	}
@@ -684,8 +708,15 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 		_model.setImageSize(img.size());
 	}
 
-	SensorData data(scan, scan.empty()?0:_scanMaxPts, 0, _isDepth?cv::Mat():img, _isDepth?img:depthFromScan, _model, this->getNextSeqID(), stamp);
+	SensorData data(scan, LaserScanInfo(scan.empty()?0:_scanMaxPts, 0, _scanLocalTransform), _isDepth?cv::Mat():img, _isDepth?img:depthFromScan, _model, this->getNextSeqID(), stamp);
 	data.setGroundTruth(groundTruthPose);
+
+	if(info && !odometryPose.isNull())
+	{
+		info->odomPose = odometryPose;
+		info->odomCovariance = cv::Mat::eye(6,6,CV_64FC1); // Note that with TORO and g2o file formats, we could get the covariance
+	}
+
 	return data;
 }
 

corelib/src/CameraRGBD.cpp

@@ -64,6 +64,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <libfreenect2/config.h>
 #endif
 
+#ifdef RTABMAP_REALSENSE
+#include <librealsense/rs.hpp>
+#endif
+
 #ifdef RTABMAP_OPENNI2
 #include <OniVersion.h>
 #include <OpenNI.h>
@@ -209,9 +213,9 @@ SensorData CameraOpenni::captureImage(CameraInfo * info)
 #ifdef HAVE_OPENNI
 	if(interface_ && interface_->isRunning())
 	{
-		if(!dataReady_.acquire(1, 2000))
+		if(!dataReady_.acquire(1, 5000))
 		{
-			UWARN("Not received new frames since 2 seconds, end of stream reached!");
+			UWARN("Not received new frames since 5 seconds, end of stream reached!");
 		}
 		else
 		{
@@ -375,22 +379,21 @@ bool CameraOpenNI2::exposureGainAvailable()
 
 CameraOpenNI2::CameraOpenNI2(
 		const std::string & deviceId,
+		Type type,
 		float imageRate,
 		const rtabmap::Transform & localTransform) :
-	Camera(imageRate, localTransform),
+	Camera(imageRate, localTransform)
 #ifdef RTABMAP_OPENNI2
+    ,
+	_type(type),
 	_device(new openni::Device()),
 	_color(new openni::VideoStream()),
 	_depth(new openni::VideoStream()),
-#else
-	_device(0),
-	_color(0),
-	_depth(0),
-#endif
 	_depthFx(0.0f),
 	_depthFy(0.0f),
 	_deviceId(deviceId),
 	_openNI2StampsAndIDsUsed(false)
+#endif
 {
 }
 
@@ -482,26 +485,93 @@ bool CameraOpenNI2::setMirroring(bool enabled)
 	return false;
 }
 
+void CameraOpenNI2::setOpenNI2StampsAndIDsUsed(bool used)
+{
+#ifdef RTABMAP_OPENNI2
+	_openNI2StampsAndIDsUsed = used;
+#endif
+}
+
 bool CameraOpenNI2::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
 #ifdef RTABMAP_OPENNI2
 	openni::OpenNI::initialize();
 
-	if(_device->open(_deviceId.empty()?openni::ANY_DEVICE:_deviceId.c_str()) != openni::STATUS_OK)
+	openni::Array<openni::DeviceInfo> devices;
+	openni::OpenNI::enumerateDevices(&devices);
+	for(int i=0; i<devices.getSize(); ++i)
+	{
+		UINFO("Device %d: Name=%s URI=%s Vendor=%s",
+				i,
+				devices[i].getName(),
+				devices[i].getUri(),
+				devices[i].getVendor());
+	}
+	if(_deviceId.empty() && devices.getSize() == 0)
+	{
+		UERROR("CameraOpenNI2: No device detected!");
+		return false;
+	}
+
+	openni::Status error = _device->open(_deviceId.empty()?openni::ANY_DEVICE:_deviceId.c_str());
+	if(error != openni::STATUS_OK)
 	{
 		if(!_deviceId.empty())
 		{
-			UERROR("CameraOpenNI2: Cannot open device \"%s\".", _deviceId.c_str());
+			UERROR("CameraOpenNI2: Cannot open device \"%s\" (error=%d).", _deviceId.c_str(), error);
 		}
 		else
 		{
-			UERROR("CameraOpenNI2: Cannot open device.");
+#ifdef _WIN32
+			UERROR("CameraOpenNI2: Cannot open device \"%s\" (error=%d).", devices[0].getName(), error);
+#else
+			UERROR("CameraOpenNI2: Cannot open device \"%s\" (error=%d). Verify if \"%s\" is in udev rules: \"/lib/udev/rules.d/40-libopenni2-0.rules\". If not, add it and reboot.", devices[0].getName(), error, devices[0].getUri());
+#endif
 		}
+
 		_device->close();
 		openni::OpenNI::shutdown();
 		return false;
 	}
 
+	// look for calibration files
+	_stereoModel = StereoCameraModel();
+	bool hardwareRegistration = true;
+	if(!calibrationFolder.empty())
+	{
+		// we need the serial
+		std::string calibrationName = _device->getDeviceInfo().getName();
+		if(!cameraName.empty())
+		{
+			calibrationName = cameraName;
+		}
+		_stereoModel.setName(calibrationName, "depth", "rgb");
+		hardwareRegistration = !_stereoModel.load(calibrationFolder, calibrationName, false);
+
+		if(_type != kTypeColorDepth)
+		{
+			hardwareRegistration = false;
+		}
+
+
+		if((_type != kTypeColorDepth && !_stereoModel.left().isValidForRectification()) ||
+		   (_type == kTypeColorDepth && !_stereoModel.right().isValidForRectification()))
+		{
+			UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, default calibration used.",
+					calibrationName.c_str(), calibrationFolder.c_str());
+		}
+		else if(_type == kTypeColorDepth && _stereoModel.right().isValidForRectification() && hardwareRegistration)
+		{
+			UWARN("Missing extrinsic calibration file for camera \"%s\" in \"%s\" folder, default registration is used even if rgb is rectified!",
+					calibrationName.c_str(), calibrationFolder.c_str());
+		}
+		else if(_type == kTypeColorDepth && _stereoModel.right().isValidForRectification() && !hardwareRegistration)
+		{
+			UINFO("Custom calibration files for \"%s\" were found in \"%s\" folder. To use "
+				  "factory calibration, remove the corresponding files from that directory.", calibrationName.c_str(), calibrationFolder.c_str());
+		}
+	}
+
 	if(UFile::getExtension(_deviceId).compare("oni")==0)
 	{
 		if(_device->getPlaybackControl() &&
@@ -513,7 +583,8 @@ bool CameraOpenNI2::init(const std::string & calibrationFolder, const std::strin
 			return false;
 		}
 	}
-	else if(!_device->isImageRegistrationModeSupported(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR))
+	else if(_type==kTypeColorDepth && hardwareRegistration &&
+			!_device->isImageRegistrationModeSupported(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR))
 	{
 		UERROR("CameraOpenNI2: Device doesn't support depth/color registration.");
 		_device->close();
@@ -522,9 +593,9 @@ bool CameraOpenNI2::init(const std::string & calibrationFolder, const std::strin
 	}
 
 	if(_device->getSensorInfo(openni::SENSOR_DEPTH) == NULL ||
-	  _device->getSensorInfo(openni::SENSOR_COLOR) == NULL)
+	  _device->getSensorInfo(_type==kTypeColorDepth?openni::SENSOR_COLOR:openni::SENSOR_IR) == NULL)
 	{
-		UERROR("CameraOpenNI2: Cannot get sensor info for depth and color.");
+		UERROR("CameraOpenNI2: Cannot get sensor info for depth and %s.", _type==kTypeColorDepth?"color":"ir");
 		_device->close();
 		openni::OpenNI::shutdown();
 		return false;
@@ -538,16 +609,17 @@ bool CameraOpenNI2::init(const std::string & calibrationFolder, const std::strin
 		return false;
 	}
 
-	if(_color->create(*_device, openni::SENSOR_COLOR) != openni::STATUS_OK)
+	if(_color->create(*_device, _type==kTypeColorDepth?openni::SENSOR_COLOR:openni::SENSOR_IR) != openni::STATUS_OK)
 	{
-		UERROR("CameraOpenNI2: Cannot create color stream.");
+		UERROR("CameraOpenNI2: Cannot create %s stream.", _type==kTypeColorDepth?"color":"ir");
 		_depth->destroy();
 		_device->close();
 		openni::OpenNI::shutdown();
 		return false;
 	}
 
-	if(_device->setImageRegistrationMode(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR ) != openni::STATUS_OK)
+	if(_type==kTypeColorDepth && hardwareRegistration &&
+	   _device->setImageRegistrationMode(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR ) != openni::STATUS_OK)
 	{
 		UERROR("CameraOpenNI2: Failed to set depth/color registration.");
 	}
@@ -574,7 +646,8 @@ bool CameraOpenNI2::init(const std::string & calibrationFolder, const std::strin
 	const openni::Array<openni::VideoMode>& colorVideoModes = _color->getSensorInfo().getSupportedVideoModes();
 	for(int i=0; i<colorVideoModes.getSize(); ++i)
 	{
-		UINFO("CameraOpenNI2: Color video mode %d: fps=%d, pixel=%d, w=%d, h=%d",
+		UINFO("CameraOpenNI2: %s video mode %d: fps=%d, pixel=%d, w=%d, h=%d",
+				_type==kTypeColorDepth?"color":"ir",
 				i,
 				colorVideoModes[i].getFps(),
 				colorVideoModes[i].getPixelFormat(),
@@ -601,6 +674,40 @@ bool CameraOpenNI2::init(const std::string & calibrationFolder, const std::strin
 			_depth->getVideoMode().getResolutionY(),
 			_depth->getHorizontalFieldOfView(),
 			_depth->getVerticalFieldOfView());
+	UINFO("CameraOpenNI2: Using %s video mode: fps=%d, pixel=%d, w=%d, h=%d, H-FOV=%f rad, V-FOV=%f rad",
+			_type==kTypeColorDepth?"color":"ir",
+			_color->getVideoMode().getFps(),
+			_color->getVideoMode().getPixelFormat(),
+			_color->getVideoMode().getResolutionX(),
+			_color->getVideoMode().getResolutionY(),
+			_color->getHorizontalFieldOfView(),
+			_color->getVerticalFieldOfView());
+
+	if(_depth->getVideoMode().getResolutionX() != 640 ||
+		_depth->getVideoMode().getResolutionY() != 480 ||
+		_depth->getVideoMode().getPixelFormat() != openni::PIXEL_FORMAT_DEPTH_1_MM)
+	{
+		UERROR("Could not set depth format to 640x480 pixel=%d(mm)!",
+				openni::PIXEL_FORMAT_DEPTH_1_MM);
+		_depth->destroy();
+		_color->destroy();
+		_device->close();
+		openni::OpenNI::shutdown();
+		return false;
+	}
+	if(_color->getVideoMode().getResolutionX() != 640 ||
+		_color->getVideoMode().getResolutionY() != 480 ||
+		_color->getVideoMode().getPixelFormat() != openni::PIXEL_FORMAT_RGB888)
+	{
+		UERROR("Could not set %s format to 640x480 pixel=%d!",
+				_type==kTypeColorDepth?"color":"ir",
+				openni::PIXEL_FORMAT_RGB888);
+		_depth->destroy();
+		_color->destroy();
+		_device->close();
+		openni::OpenNI::shutdown();
+		return false;
+	}
 
 	if(_color->getCameraSettings())
 	{
@@ -612,8 +719,7 @@ bool CameraOpenNI2::init(const std::string & calibrationFolder, const std::strin
 #endif
 	}
 
-	bool registered = true;
-	if(registered)
+	if(_type==kTypeColorDepth && hardwareRegistration)
 	{
 		_depthFx = float(_color->getVideoMode().getResolutionX()/2) / std::tan(_color->getHorizontalFieldOfView()/2.0f);
 		_depthFy = float(_color->getVideoMode().getResolutionY()/2) / std::tan(_color->getVerticalFieldOfView()/2.0f);
@@ -625,16 +731,13 @@ bool CameraOpenNI2::init(const std::string & calibrationFolder, const std::strin
 	}
 	UINFO("depth fx=%f fy=%f", _depthFx, _depthFy);
 
-	UINFO("CameraOpenNI2: Using color video mode: fps=%d, pixel=%d, w=%d, h=%d, H-FOV=%f rad, V-FOV=%f rad",
-			_color->getVideoMode().getFps(),
-			_color->getVideoMode().getPixelFormat(),
-			_color->getVideoMode().getResolutionX(),
-			_color->getVideoMode().getResolutionY(),
-			_color->getHorizontalFieldOfView(),
-			_color->getVerticalFieldOfView());
+	if(_type == kTypeIR)
+	{
+		UWARN("With type IR-only, depth stream will not be started");
+	}
 
-	if(_depth->start() != openni::STATUS_OK ||
-	   _color->start() != openni::STATUS_OK)
+	if((_type != kTypeIR && _depth->start() != openni::STATUS_OK) ||
+		_color->start() != openni::STATUS_OK)
 	{
 		UERROR("CameraOpenNI2: Cannot start depth and/or color streams.");
 		_depth->stop();
@@ -646,7 +749,7 @@ bool CameraOpenNI2::init(const std::string & calibrationFolder, const std::strin
 		return false;
 	}
 
-	uSleep(1000); // just to make sure the sensor is correctly initialized
+	uSleep(3000); // just to make sure the sensor is correctly initialized and exposure is set
 
 	return true;
 #else
@@ -680,35 +783,49 @@ SensorData CameraOpenNI2::captureImage(CameraInfo * info)
 		_depth->isValid() &&
 		_color->isValid() &&
 		_device->getSensorInfo(openni::SENSOR_DEPTH) != NULL &&
-		_device->getSensorInfo(openni::SENSOR_COLOR) != NULL)
+		_device->getSensorInfo(_type==kTypeColorDepth?openni::SENSOR_COLOR:openni::SENSOR_IR) != NULL)
 	{
 		openni::VideoStream* depthStream[] = {_depth};
 		openni::VideoStream* colorStream[] = {_color};
-		if(openni::OpenNI::waitForAnyStream(depthStream, 1, &readyStream, 2000) != openni::STATUS_OK ||
-		   openni::OpenNI::waitForAnyStream(colorStream, 1, &readyStream, 2000) != openni::STATUS_OK)
+		if((_type != kTypeIR && openni::OpenNI::waitForAnyStream(depthStream, 1, &readyStream, 5000) != openni::STATUS_OK) ||
+		   openni::OpenNI::waitForAnyStream(colorStream, 1, &readyStream, 5000) != openni::STATUS_OK)
 		{
-			UWARN("No frames received since the last 2 seconds, end of stream is reached!");
+			UWARN("No frames received since the last 5 seconds, end of stream is reached!");
 		}
 		else
 		{
 			openni::VideoFrameRef depthFrame, colorFrame;
-			_depth->readFrame(&depthFrame);
+			if(_type != kTypeIR)
+			{
+				_depth->readFrame(&depthFrame);
+			}
 			_color->readFrame(&colorFrame);
 			cv::Mat depth, rgb;
-			if(depthFrame.isValid() && colorFrame.isValid())
+			if((_type == kTypeIR || depthFrame.isValid()) && colorFrame.isValid())
 			{
-				int h=depthFrame.getHeight();
-				int w=depthFrame.getWidth();
-				depth = cv::Mat(h, w, CV_16U, (void*)depthFrame.getData()).clone();
-
+				int h,w;
+				if(_type != kTypeIR)
+				{
+					h=depthFrame.getHeight();
+					w=depthFrame.getWidth();
+					depth = cv::Mat(h, w, CV_16U, (void*)depthFrame.getData()).clone();
+				}
 				h=colorFrame.getHeight();
 				w=colorFrame.getWidth();
 				cv::Mat tmp(h, w, CV_8UC3, (void *)colorFrame.getData());
-				cv::cvtColor(tmp, rgb, CV_RGB2BGR);
+				if(_type==kTypeColorDepth)
+				{
+					cv::cvtColor(tmp, rgb, CV_RGB2BGR);
+				}
+				else // IR
+				{
+					rgb = tmp.clone();
+				}
 			}
 			UASSERT(_depthFx != 0.0f && _depthFy != 0.0f);
-			if(!rgb.empty() && !depth.empty())
+			if(!rgb.empty() && (_type == kTypeIR || !depth.empty()))
 			{
+				// default calibration
 				CameraModel model(
 						_depthFx, //fx
 						_depthFy, //fy
@@ -717,6 +834,35 @@ SensorData CameraOpenNI2::captureImage(CameraInfo * info)
 						this->getLocalTransform(),
 						0,
 						rgb.size());
+
+				if(_type==kTypeColorDepth)
+				{
+					if(_stereoModel.right().isValidForRectification())
+					{
+						rgb = _stereoModel.right().rectifyImage(rgb);
+						model = _stereoModel.right();
+
+						if(_stereoModel.left().isValidForRectification() && !_stereoModel.stereoTransform().isNull())
+						{
+							depth = _stereoModel.left().rectifyImage(depth, 0);
+							depth = util2d::registerDepth(depth, _stereoModel.left().K(), rgb.size(), _stereoModel.right().K(), _stereoModel.stereoTransform());
+						}
+					}
+				}
+				else // IR
+				{
+					if(_stereoModel.left().isValidForRectification())
+					{
+						rgb = _stereoModel.left().rectifyImage(rgb);
+						if(_type!=kTypeIR)
+						{
+							depth = _stereoModel.left().rectifyImage(depth, 0);
+						}
+						model = _stereoModel.left();
+					}
+				}
+				model.setLocalTransform(this->getLocalTransform());
+
 				if(_openNI2StampsAndIDsUsed)
 				{
 					data = SensorData(rgb, depth, model, depthFrame.getFrameIndex(), double(depthFrame.getTimestamp()) / 1000000.0);
@@ -744,8 +890,10 @@ SensorData CameraOpenNI2::captureImage(CameraInfo * info)
 //
 class FreenectDevice : public UThread {
   public:
-	FreenectDevice(freenect_context * ctx, int index) :
+	FreenectDevice(freenect_context * ctx, int index, bool color = true, bool registered = true) :
 		index_(index),
+		color_(color),
+		registered_(registered),
 		ctx_(ctx),
 		device_(0),
 		depthFocal_(0.0f)
@@ -794,22 +942,37 @@ class FreenectDevice : public UThread {
 			UERROR("Could not get serial for index %d", index_);
 		}
 
+		UINFO("color=%d registered=%d", color_?1:0, registered_?1:0);
+
 		freenect_set_user(device_, this);
-		freenect_set_video_mode(device_, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB));
-		freenect_set_depth_mode(device_, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_REGISTERED));
-		depthBuffer_ = cv::Mat(cv::Size(640,480),CV_16UC1);
-		rgbBuffer_ = cv::Mat(cv::Size(640,480), CV_8UC3);
+		freenect_frame_mode videoMode = freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, color_?FREENECT_VIDEO_RGB:FREENECT_VIDEO_IR_8BIT);
+		freenect_frame_mode depthMode = freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, color_ && registered_?FREENECT_DEPTH_REGISTERED:FREENECT_DEPTH_MM);
+		if(!videoMode.is_valid)
+		{
+			UERROR("Freenect: video mode selected not valid!");
+			return false;
+		}
+		if(!depthMode.is_valid)
+		{
+			UERROR("Freenect: depth mode selected not valid!");
+			return false;
+		}
+		UASSERT(videoMode.data_bits_per_pixel == 8 || videoMode.data_bits_per_pixel == 24);
+		UASSERT(depthMode.data_bits_per_pixel == 16);
+		freenect_set_video_mode(device_, videoMode);
+		freenect_set_depth_mode(device_, depthMode);
+		rgbIrBuffer_ = cv::Mat(cv::Size(videoMode.width,videoMode.height), color_?CV_8UC3:CV_8UC1);
+		depthBuffer_ = cv::Mat(cv::Size(depthMode.width,depthMode.height), CV_16UC1);
 		freenect_set_depth_buffer(device_, depthBuffer_.data);
-		freenect_set_video_buffer(device_, rgbBuffer_.data);
+		freenect_set_video_buffer(device_, rgbIrBuffer_.data);
 		freenect_set_depth_callback(device_, freenect_depth_callback);
 		freenect_set_video_callback(device_, freenect_video_callback);
 
-		bool registered = true;
 		float rgb_focal_length_sxga = 1050.0f;
 		float width_sxga = 1280.0f;
 		float width = freenect_get_current_depth_mode(device_).width;
 		float scale = width / width_sxga;
-		if(registered)
+		if(color_ && registered_)
 		{
 			depthFocal_ =  rgb_focal_length_sxga * scale;
 		}
@@ -832,16 +995,16 @@ class FreenectDevice : public UThread {
 	{
 		if(this->isRunning())
 		{
-			if(!dataReady_.acquire(1, 2000))
+			if(!dataReady_.acquire(1, 5000))
 			{
-				UERROR("Not received any frames since 2 seconds, try to restart the camera again.");
+				UERROR("Not received any frames since 5 seconds, try to restart the camera again.");
 			}
 			else
 			{
 				UScopeMutex s(dataMutex_);
-				rgb = rgbLastFrame_;
+				rgb = rgbIrLastFrame_;
 				depth = depthLastFrame_;
-				rgbLastFrame_ = cv::Mat();
+				rgbIrLastFrame_ = cv::Mat();
 				depthLastFrame_= cv::Mat();
 			}
 		}
@@ -851,10 +1014,18 @@ private:
 	// Do not call directly even in child
 	void VideoCallback(void* rgb)
 	{
-		UASSERT(rgbBuffer_.data == rgb);
+		UASSERT(rgbIrBuffer_.data == rgb);
 		UScopeMutex s(dataMutex_);
-		bool notify = rgbLastFrame_.empty();
-		cv::cvtColor(rgbBuffer_, rgbLastFrame_, CV_RGB2BGR);
+		bool notify = rgbIrLastFrame_.empty();
+
+		if(color_)
+		{
+			cv::cvtColor(rgbIrBuffer_, rgbIrLastFrame_, CV_RGB2BGR);
+		}
+		else // IrDepth
+		{
+			rgbIrLastFrame_ = rgbIrBuffer_.clone();
+		}
 		if(!depthLastFrame_.empty() && notify)
 		{
 			dataReady_.release();
@@ -868,7 +1039,7 @@ private:
 		UScopeMutex s(dataMutex_);
 		bool notify = depthLastFrame_.empty();
 		depthLastFrame_ = depthBuffer_.clone();
-		if(!rgbLastFrame_.empty() && notify)
+		if(!rgbIrLastFrame_.empty() && notify)
 		{
 			dataReady_.release();
 		}
@@ -927,14 +1098,16 @@ private:
 
   private:
 	int index_;
+	bool color_;
+	bool registered_;
 	std::string serial_;
 	freenect_context * ctx_;
 	freenect_device * device_;
 	cv::Mat depthBuffer_;
-	cv::Mat rgbBuffer_;
+	cv::Mat rgbIrBuffer_;
 	UMutex dataMutex_;
 	cv::Mat depthLastFrame_;
-	cv::Mat rgbLastFrame_;
+	cv::Mat rgbIrLastFrame_;
 	float depthFocal_;
 	USemaphore dataReady_;
 };
@@ -952,11 +1125,15 @@ bool CameraFreenect::available()
 #endif
 }
 
-CameraFreenect::CameraFreenect(int deviceId, float imageRate, const Transform & localTransform) :
-		Camera(imageRate, localTransform),
+CameraFreenect::CameraFreenect(int deviceId, Type type, float imageRate, const Transform & localTransform) :
+		Camera(imageRate, localTransform)
+#ifdef RTABMAP_FREENECT
+        ,
 		deviceId_(deviceId),
+		type_(type),
 		ctx_(0),
 		freenectDevice_(0)
+#endif
 {
 #ifdef RTABMAP_FREENECT
 	if(freenect_init(&ctx_, NULL) < 0) UERROR("Cannot initialize freenect library");
@@ -993,7 +1170,50 @@ bool CameraFreenect::init(const std::string & calibrationFolder, const std::stri
 
 	if(ctx_ && freenect_num_devices(ctx_) > 0)
 	{
-		freenectDevice_ = new FreenectDevice(ctx_, deviceId_);
+		// look for calibration files
+		bool hardwareRegistration = true;
+		stereoModel_ = StereoCameraModel();
+		if(!calibrationFolder.empty())
+		{
+			// we need the serial, HACK: init a temp device to get it
+			FreenectDevice dev(ctx_, deviceId_);
+			if(!dev.init())
+			{
+				UERROR("CameraFreenect: Init failed!");
+			}
+			std::string calibrationName = dev.getSerial();
+			if(!cameraName.empty())
+			{
+				calibrationName = cameraName;
+			}
+			stereoModel_.setName(calibrationName, "depth", "rgb");
+			hardwareRegistration = !stereoModel_.load(calibrationFolder, calibrationName, false);
+
+			if(type_ == kTypeIRDepth)
+			{
+				hardwareRegistration = false;
+			}
+
+
+			if((type_ == kTypeIRDepth && !stereoModel_.left().isValidForRectification()) ||
+			   (type_ == kTypeColorDepth && !stereoModel_.right().isValidForRectification()))
+			{
+				UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, default calibration used.",
+						calibrationName.c_str(), calibrationFolder.c_str());
+			}
+			else if(type_ == kTypeColorDepth && stereoModel_.right().isValidForRectification() && hardwareRegistration)
+			{
+				UWARN("Missing extrinsic calibration file for camera \"%s\" in \"%s\" folder, default registration is used even if rgb is rectified!",
+						calibrationName.c_str(), calibrationFolder.c_str());
+			}
+			else if(type_ == kTypeColorDepth && stereoModel_.right().isValidForRectification() && !hardwareRegistration)
+			{
+				UINFO("Custom calibration files for \"%s\" were found in \"%s\" folder. To use "
+					  "factory calibration, remove the corresponding files from that directory.", calibrationName.c_str(), calibrationFolder.c_str());
+			}
+		}
+
+		freenectDevice_ = new FreenectDevice(ctx_, deviceId_, type_==kTypeColorDepth, hardwareRegistration);
 		if(freenectDevice_->init())
 		{
 			freenectDevice_->start();
@@ -1046,18 +1266,43 @@ SensorData CameraFreenect::captureImage(CameraInfo * info)
 			if(!rgb.empty() && !depth.empty())
 			{
 				UASSERT(freenectDevice_->getDepthFocal() != 0.0f);
-				if(!rgb.empty() && !depth.empty())
+
+				// default calibration
+				CameraModel model(
+						freenectDevice_->getDepthFocal(), //fx
+						freenectDevice_->getDepthFocal(), //fy
+						float(rgb.cols/2) - 0.5f,  //cx
+						float(rgb.rows/2) - 0.5f,  //cy
+						this->getLocalTransform(),
+						0,
+						rgb.size());
+
+				if(type_==kTypeIRDepth)
 				{
-					CameraModel model(
-							freenectDevice_->getDepthFocal(), //fx
-							freenectDevice_->getDepthFocal(), //fy
-							float(rgb.cols/2) - 0.5f,  //cx
-							float(rgb.rows/2) - 0.5f,  //cy
-							this->getLocalTransform(),
-							0,
-							rgb.size());
-					data = SensorData(rgb, depth, model, this->getNextSeqID(), UTimer::now());
+					if(stereoModel_.left().isValidForRectification())
+					{
+						rgb = stereoModel_.left().rectifyImage(rgb);
+						depth = stereoModel_.left().rectifyImage(depth, 0);
+						model = stereoModel_.left();
+					}
 				}
+				else
+				{
+					if(stereoModel_.right().isValidForRectification())
+					{
+						rgb = stereoModel_.right().rectifyImage(rgb);
+						model = stereoModel_.right();
+
+						if(stereoModel_.left().isValidForRectification() && !stereoModel_.stereoTransform().isNull())
+						{
+							depth = stereoModel_.left().rectifyImage(depth, 0);
+							depth = util2d::registerDepth(depth, stereoModel_.left().K(), rgb.size(), stereoModel_.right().K(), stereoModel_.stereoTransform());
+						}
+					}
+				}
+				model.setLocalTransform(this->getLocalTransform());
+
+				data = SensorData(rgb, depth, model, this->getNextSeqID(), UTimer::now());
 			}
 		}
 		else
@@ -1095,7 +1340,9 @@ CameraFreenect2::CameraFreenect2(
 	bool bilateralFiltering,
 	bool edgeAwareFiltering,
 	bool noiseFiltering) :
-		Camera(imageRate, localTransform),
+		Camera(imageRate, localTransform)
+#ifdef RTABMAP_FREENECT2
+        ,
 		deviceId_(deviceId),
 		type_(type),
 		freenect2_(0),
@@ -1107,6 +1354,7 @@ CameraFreenect2::CameraFreenect2(
 		bilateralFiltering_(bilateralFiltering),
 		edgeAwareFiltering_(edgeAwareFiltering),
 		noiseFiltering_(noiseFiltering)
+#endif
 {
 #ifdef RTABMAP_FREENECT2
 	UASSERT(minKinect2Depth_ < maxKinect2Depth_ && minKinect2Depth_>0 && maxKinect2Depth_>0 && maxKinect2Depth_<=65.535f);
@@ -1175,6 +1423,9 @@ bool CameraFreenect2::init(const std::string & calibrationFolder, const std::str
 	}
 
 	libfreenect2::PacketPipeline * pipeline;
+#ifdef LIBFREENECT2_WITH_CUDA_SUPPORT
+	pipeline = new libfreenect2::CudaPacketPipeline();
+#else
 #ifdef LIBFREENECT2_WITH_OPENGL_SUPPORT
 	pipeline = new libfreenect2::OpenGLPacketPipeline();
 #else
@@ -1183,6 +1434,7 @@ bool CameraFreenect2::init(const std::string & calibrationFolder, const std::str
 #else
 	pipeline = new libfreenect2::CpuPacketPipeline();
 #endif
+#endif
 #endif
 
 	if(deviceId_ <= 0)
@@ -1226,6 +1478,7 @@ bool CameraFreenect2::init(const std::string & calibrationFolder, const std::str
 		reg_ = new libfreenect2::Registration(depthParams, colorParams);
 
 		// look for calibration files
+		stereoModel_ = StereoCameraModel();
 		if(!calibrationFolder.empty())
 		{
 			std::string calibrationName = dev_->getSerialNumber();
@@ -1233,13 +1486,20 @@ bool CameraFreenect2::init(const std::string & calibrationFolder, const std::str
 			{
 				calibrationName = cameraName;
 			}
+			stereoModel_.setName(calibrationName, "depth", "rgb");
 			if(!stereoModel_.load(calibrationFolder, calibrationName, false))
 			{
-				UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, default calibration used.",
+				UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, default calibration "
+					"is used. Note that from version 0.11.10, calibration suffixes for Freenect2 driver have "
+					"changed from \"_left\"->\"_depth\" and \"_right\"->\"_rgb\". You can safely rename "
+					"the calibration files to avoid recalibrating.",
 						calibrationName.c_str(), calibrationFolder.c_str());
 			}
 			else
 			{
+				UINFO("Custom calibration files for \"%s\" were found in \"%s\" folder. To use "
+					  "factory calibration, remove the corresponding files from that directory.", calibrationName.c_str(), calibrationFolder.c_str());
+
 				if(type_==kTypeColor2DepthSD)
 				{
 					UWARN("Freenect2: When using custom calibration file, type "
@@ -1402,7 +1662,15 @@ SensorData CameraFreenect2::captureImage(CameraInfo * info)
 				{
 					cv::Mat rgbMatC4((int)rgbFrame->height, (int)rgbFrame->width, CV_8UC4, rgbFrame->data);
 					cv::Mat rgbMat; // rtabmap uses 3 channels RGB
-					cv::cvtColor(rgbMatC4, rgbMat, CV_BGRA2BGR);
+					#ifdef LIBFREENECT2_WITH_TEGRAJPEG_SUPPORT 
+
+					 cv::cvtColor(rgbMatC4, rgbMat, CV_RGBA2BGR); 
+
+					#else 
+
+					cv::cvtColor(rgbMatC4, rgbMat, CV_BGRA2BGR); 
+
+					#endif 
 					cv::flip(rgbMat, rgb, 1);
 
 					//rectify color
@@ -1419,7 +1687,9 @@ SensorData CameraFreenect2::captureImage(CameraInfo * info)
 						//rectify depth
 						cv::Mat((int)depthFrame->height, (int)depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
 						cv::flip(depth, depth, 1);
-						depth = stereoModel_.left().rectifyDepth(depth);
+
+						//depth = stereoModel_.left().rectifyImage(depth, 0); // ~0.5/4 ms but is more noisy
+						depth = stereoModel_.left().rectifyDepth(depth);      // ~16/25 ms
 
 						bool registered = true;
 						if(registered)
@@ -1427,6 +1697,7 @@ SensorData CameraFreenect2::captureImage(CameraInfo * info)
 							depth = util2d::registerDepth(
 									depth,
 									stereoModel_.left().P().colRange(0,3).rowRange(0,3), //scaled depth K
+									depth.size(),
 									stereoModel_.right().P().colRange(0,3).rowRange(0,3), //scaled color K
 									stereoModel_.stereoTransform());
 							util2d::fillRegisteredDepthHoles(depth, true, false);
@@ -1451,7 +1722,15 @@ SensorData CameraFreenect2::captureImage(CameraInfo * info)
 					{
 						cv::Mat rgbMatC4((int)rgbFrame->height, (int)rgbFrame->width, CV_8UC4, rgbFrame->data);
 						cv::Mat rgbMat; // rtabmap uses 3 channels RGB
-						cv::cvtColor(rgbMatC4, rgbMat, CV_BGRA2BGR);
+						#ifdef LIBFREENECT2_WITH_TEGRAJPEG_SUPPORT 
+
+						 cv::cvtColor(rgbMatC4, rgbMat, CV_RGB2BGR); 
+
+						#else 
+
+						cv::cvtColor(rgbMatC4, rgbMat, CV_BGRA2BGR); 
+
+						#endif 
 						cv::flip(rgbMat, rgb, 1);
 
 						cv::Mat((int)irFrame->height, (int)irFrame->width, CV_32FC1, irFrame->data).convertTo(depth, CV_16U, 1);
@@ -1560,7 +1839,15 @@ SensorData CameraFreenect2::captureImage(CameraInfo * info)
 							}
 	
 							// rtabmap uses 3 channels RGB
-							cv::cvtColor(rgbMatBGRA, rgb, CV_BGRA2BGR);
+							#ifdef LIBFREENECT2_WITH_TEGRAJPEG_SUPPORT 
+
+							 cv::cvtColor(rgbMatBGRA, rgb, CV_RGBA2BGR); 
+
+							#else 
+
+							cv::cvtColor(rgbMatBGRA, rgb, CV_BGRA2BGR); 
+
+							#endif 
 							cv::flip(rgb, rgb, 1);
 						}
 						else //register depth to color (OLD WAY)
@@ -1574,7 +1861,15 @@ SensorData CameraFreenect2::captureImage(CameraInfo * info)
 								rgbMatBGRA = tmp;
 							}
 							// rtabmap uses 3 channels RGB
-							cv::cvtColor(rgbMatBGRA, rgb, CV_BGRA2BGR);
+							#ifdef LIBFREENECT2_WITH_TEGRAJPEG_SUPPORT 
+
+							 cv::cvtColor(rgbMatBGRA, rgb, CV_RGBA2BGR); 
+
+							#else 
+
+							cv::cvtColor(rgbMatBGRA, rgb, CV_BGRA2BGR); 
+
+							#endif 
 							cv::flip(rgb, rgb, 1);	
 
 							cv::Mat depthFrameMat = cv::Mat((int)depthFrame->height, (int)depthFrame->width, CV_32FC1, depthFrame->data);
@@ -1667,6 +1962,221 @@ SensorData CameraFreenect2::captureImage(CameraInfo * info)
 	return data;
 }
 
+/////////////////////////
+// CameraRealSense
+/////////////////////////
+bool CameraRealSense::available()
+{
+#ifdef RTABMAP_REALSENSE
+	return true;
+#else
+	return false;
+#endif
+}
+
+CameraRealSense::CameraRealSense(int device, int presetRGB, int presetDepth, float imageRate, const rtabmap::Transform & localTransform) :
+	Camera(imageRate, localTransform)
+#ifdef RTABMAP_REALSENSE
+    ,
+	ctx_(0),
+	dev_(0),
+	deviceId_(device),
+	presetRGB_(presetRGB),
+	presetDepth_(presetDepth)
+#endif
+{
+	UDEBUG("");
+}
+
+CameraRealSense::~CameraRealSense()
+{
+#ifdef RTABMAP_REALSENSE
+	if (ctx_)
+	{
+		delete ctx_;
+	}
+#endif
+}
+
+bool CameraRealSense::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+	UDEBUG("");
+#ifdef RTABMAP_REALSENSE
+	if (ctx_ == 0)
+	{
+		ctx_ = new rs::context();
+	}
+
+	UDEBUG("");
+	if (ctx_->get_device_count() == 0)
+	{
+		UERROR("No RealSense device detected!");
+		return false;
+	}
+
+	UDEBUG("");
+	dev_ = ctx_->get_device(deviceId_);
+	if (dev_ == 0)
+	{
+		UERROR("Cannot connect to device %d", deviceId_);
+		return false;
+	}
+	UINFO("Using device %d, an %s", deviceId_, dev_->get_name());
+	UINFO("    Serial number: %s", dev_->get_serial());
+	UINFO("    Firmware version: %s", dev_->get_firmware_version());
+	UINFO("    Preset RGB: %d", presetRGB_);
+	UINFO("    Preset Depth: %d", presetDepth_);
+
+	// Configure depth and color to run with the device's preferred settings
+	UINFO("Enabling streams...");
+	// R200: 
+	//    0=640x480   vs 480x360
+	//    1=1920x1080 vs 640x480
+	//    2=640x480   vs 320x240
+	dev_->enable_stream(rs::stream::depth, (rs::preset)presetDepth_);
+	dev_->enable_stream(rs::stream::color, (rs::preset)presetRGB_);
+
+	rs::intrinsics depth_intrin = dev_->get_stream_intrinsics(rs::stream::depth);
+	rs::intrinsics color_intrin = dev_->get_stream_intrinsics(rs::stream::color);
+	UINFO("    RGB:   %dx%d", color_intrin.width, color_intrin.height);
+	UINFO("    Depth: %dx%d", depth_intrin.width, depth_intrin.height);
+	dev_->start();
+
+	dev_->wait_for_frames();
+	uSleep(1000); // ignore the first frames
+	UINFO("Enabling streams...done!");
+	
+	return true;
+
+#else
+	UERROR("CameraRealSense: RTAB-Map is not built with RealSense support!");
+	return false;
+#endif
+}
+
+bool CameraRealSense::isCalibrated() const
+{
+	return true;
+}
+
+std::string CameraRealSense::getSerial() const
+{
+#ifdef RTABMAP_REALSENSE
+	if (dev_)
+	{
+		return dev_->get_serial();
+	}
+	else
+	{
+		UERROR("Cannot get a serial before initialization. Call init() before.");
+	}
+#endif
+	return "NA";
+}
+
+SensorData CameraRealSense::captureImage(CameraInfo * info)
+{
+	SensorData data;
+#ifdef RTABMAP_REALSENSE
+	if (dev_)
+	{
+		dev_->wait_for_frames();
+
+		// Retrieve our images
+		const uint16_t * depth_image = (const uint16_t *)dev_->get_frame_data(rs::stream::depth);
+		const uint8_t * color_image = (const uint8_t *)dev_->get_frame_data(rs::stream::color);
+
+		// Retrieve camera parameters for mapping between depth and color
+		rs::intrinsics depth_intrin = dev_->get_stream_intrinsics(rs::stream::depth);
+		rs::extrinsics depth_to_color = dev_->get_extrinsics(rs::stream::depth, rs::stream::color);
+		rs::intrinsics color_intrin = dev_->get_stream_intrinsics(rs::stream::color);
+		float scale = dev_->get_depth_scale();
+
+		// factory registration...
+		cv::Mat rgb = cv::Mat(cv::Size(color_intrin.width, color_intrin.height), CV_8UC3, (void*)color_image);
+		cv::Mat bgr;
+		cv::cvtColor(rgb, bgr, CV_RGB2BGR);
+
+		CameraModel model(
+			color_intrin.fx, //fx
+			color_intrin.fy, //fy
+			color_intrin.ppx,  //cx
+			color_intrin.ppy,  //cy
+			this->getLocalTransform(),
+			0,
+			bgr.size());
+
+		cv::Mat depth;
+		if (color_intrin.width % depth_intrin.width == 0 && color_intrin.height % depth_intrin.height == 0 &&
+			depth_intrin.width < color_intrin.width &&
+			depth_intrin.height < color_intrin.height)
+		{
+			//we can keep the depth image size as is
+			depth = cv::Mat::zeros(cv::Size(depth_intrin.width, depth_intrin.height), CV_16UC1);
+			float scaleX = float(depth_intrin.width) / float(color_intrin.width);
+			float scaleY = float(depth_intrin.height) / float(color_intrin.height);
+			color_intrin.fx *= scaleX;
+			color_intrin.fy *= scaleY;
+			color_intrin.ppx *= scaleX;
+			color_intrin.ppy *= scaleY;
+			color_intrin.height = depth_intrin.height;
+			color_intrin.width = depth_intrin.width;
+		}
+		else
+		{
+			//depth to color
+			depth = cv::Mat::zeros(bgr.size(), CV_16UC1);
+		}
+		for (int dy = 0; dy < depth_intrin.height; ++dy)
+		{
+			for (int dx = 0; dx < depth_intrin.width; ++dx)
+			{
+				// Retrieve the 16-bit depth value and map it into a depth in meters
+				uint16_t depth_value = depth_image[dy * depth_intrin.width + dx];
+				float depth_in_meters = depth_value * scale;
+
+				// Skip over pixels with a depth value of zero, which is used to indicate no data
+				if (depth_value == 0 || depth_in_meters>10.0f) continue;
+
+				// Map from pixel coordinates in the depth image to pixel coordinates in the color image
+				rs::float2 depth_pixel = { (float)dx, (float)dy };
+				rs::float3 depth_point = depth_intrin.deproject(depth_pixel, depth_in_meters);
+				rs::float3 color_point = depth_to_color.transform(depth_point);
+				rs::float2 color_pixel = color_intrin.project(color_point);
+
+				int pdx = color_pixel.x;
+				int pdy = color_pixel.y;
+				if (uIsInBounds(pdx, 0, depth.cols) && uIsInBounds(pdy, 0, depth.rows))
+				{
+					depth.at<unsigned short>(pdy, pdx) = (unsigned short)(depth_in_meters*1000.0f); // convert to mm
+				}
+			}
+		}
+
+		if (color_intrin.width > depth_intrin.width)
+		{
+			// Fill holes
+			UTimer time;
+			util2d::fillRegisteredDepthHoles(depth, true, true, color_intrin.width > depth_intrin.width * 2);
+			util2d::fillRegisteredDepthHoles(depth, true, true, color_intrin.width > depth_intrin.width * 2);//second pass
+			UDEBUG("Filling depth holes: %fs", time.ticks());
+		}
+		
+		if (!bgr.empty() && !depth.empty())
+		{
+			data = SensorData(bgr, depth, model, this->getNextSeqID(), UTimer::now());
+		}
+	}
+	else
+	{
+		ULOGGER_WARN("The camera must be initialized before requesting an image.");
+	}
+#else
+	UERROR("CameraRealSense: RTAB-Map is not built with RealSense support!");
+#endif
+	return data;
+}
+
 //
 // CameraRGBDImages
 //

corelib/src/CameraStereo.cpp

@@ -347,8 +347,11 @@ bool CameraStereoDC1394::available()
 }
 
 CameraStereoDC1394::CameraStereoDC1394(float imageRate, const Transform & localTransform) :
-		Camera(imageRate, localTransform),
+		Camera(imageRate, localTransform)
+#ifdef RTABMAP_DC1394
+        ,
 		device_(0)
+#endif
 {
 #ifdef RTABMAP_DC1394
 	device_ = new DC1394Device();
@@ -401,7 +404,11 @@ bool CameraStereoDC1394::init(const std::string & calibrationFolder, const std::
 
 bool CameraStereoDC1394::isCalibrated() const
 {
+#ifdef RTABMAP_DC1394
 	return stereoModel_.isValidForProjection();
+#else
+	return false;
+#endif
 }
 
 std::string CameraStereoDC1394::getSerial() const
@@ -460,9 +467,12 @@ SensorData CameraStereoDC1394::captureImage(CameraInfo * info)
 // CameraTriclops
 //
 CameraStereoFlyCapture2::CameraStereoFlyCapture2(float imageRate, const Transform & localTransform) :
-		Camera(imageRate, localTransform),
+		Camera(imageRate, localTransform)
+#ifdef RTABMAP_FLYCAPTURE2
+        ,
 		camera_(0),
 		triclopsCtx_(0)
+#endif
 {
 #ifdef RTABMAP_FLYCAPTURE2
 	camera_ = new FlyCapture2::Camera();
@@ -755,19 +765,25 @@ CameraStereoZed::CameraStereoZed(
 		int confidenceThr,
 		bool computeOdometry,
 		float imageRate,
-		const Transform & localTransform) :
-	Camera(imageRate, localTransform),
+		const Transform & localTransform,
+		bool selfCalibration) :
+	Camera(imageRate, localTransform)
+#ifdef RTABMAP_ZED
+    ,
 	zed_(0),
 	src_(CameraVideo::kUsbDevice),
 	usbDevice_(deviceId),
 	svoFilePath_(""),
 	resolution_(resolution),
 	quality_(quality),
+	selfCalibration_(selfCalibration),
 	sensingMode_(sensingMode),
 	confidenceThr_(confidenceThr),
 	computeOdometry_(computeOdometry),
 	lost_(true)
+#endif
 {
+	UDEBUG("");
 #ifdef RTABMAP_ZED
 	UASSERT(resolution_ >= sl::zed::HD2K && resolution_ <sl::zed::LAST_RESOLUTION);
 	UASSERT(quality_ >= sl::zed::NONE && quality_ <sl::zed::LAST_MODE);
@@ -783,19 +799,25 @@ CameraStereoZed::CameraStereoZed(
 		int confidenceThr,
 		bool computeOdometry,
 		float imageRate,
-		const Transform & localTransform) :
-	Camera(imageRate, localTransform),
+		const Transform & localTransform,
+		bool selfCalibration) :
+	Camera(imageRate, localTransform)
+#ifdef RTABMAP_ZED
+    ,
 	zed_(0),
 	src_(CameraVideo::kVideoFile),
 	usbDevice_(0),
 	svoFilePath_(filePath),
 	resolution_(2),
 	quality_(quality),
+	selfCalibration_(selfCalibration),
 	sensingMode_(sensingMode),
 	confidenceThr_(confidenceThr),
 	computeOdometry_(computeOdometry),
 	lost_(true)
+#endif
 {
+	UDEBUG("");
 #ifdef RTABMAP_ZED
 	UASSERT(resolution_ >= sl::zed::HD2K && resolution_ <sl::zed::LAST_RESOLUTION);
 	UASSERT(quality_ >= sl::zed::NONE && quality_ <sl::zed::LAST_MODE);
@@ -816,6 +838,7 @@ CameraStereoZed::~CameraStereoZed()
 
 bool CameraStereoZed::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
+	UDEBUG("");
 #ifdef RTABMAP_ZED
 	if(zed_)
 	{
@@ -826,32 +849,29 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
 	lost_ = true;
 	if(src_ == CameraVideo::kVideoFile)
 	{
+		UINFO("svo file = %s", svoFilePath_.c_str());
 		zed_ = new sl::zed::Camera(svoFilePath_); // Use in SVO playback mode
 	}
 	else
 	{
-		if(zed_->isZEDconnected())
-		{
-			zed_ = new sl::zed::Camera((sl::zed::ZEDResolution_mode)resolution_, getImageRate(), usbDevice_); // Use in Live Mode
-		}
-		else
-		{
-			UERROR("ZED camera initialization failed: ZED is not connected!");
-			return false;
-		}
+		UINFO("Resolution=%d imagerate=%f device=%d", resolution_, getImageRate(), usbDevice_);
+		zed_ = new sl::zed::Camera((sl::zed::ZEDResolution_mode)resolution_, getImageRate(), usbDevice_); // Use in Live Mode
 	}
 
-	//init WITH self-calibration
 	sl::zed::InitParams parameters(
 		(sl::zed::MODE)quality_, //MODE
-		sl::zed::METER,  //UNIT
-		sl::zed::IMAGE,  //COORDINATE_SYSTEM
+		(sl::zed::UNIT)sl::zed::METER,  //UNIT
+		(sl::zed::COORDINATE_SYSTEM)sl::zed::IMAGE,  //COORDINATE_SYSTEM
 		false,  // verbose
-		-1,     //device
+		-1,     //device (GPU)
 		-1.,    //minDist
-		false,  //disableSelfCalib
+		!selfCalibration_,  //disableSelfCalib: false = self calibrated
 		false); //vflip
+
+	UINFO("Init ZED: Mode=%d Unit=%d CoordinateSystem=%d Verbose=false device=-1 minDist=-1 self-calibration=%s vflip=false",
+			quality_, sl::zed::METER, sl::zed::IMAGE, selfCalibration_?"true":"false");
 	sl::zed::ERRCODE err = zed_->init(parameters);
+	UDEBUG("");
 
 	// Quit if an error occurred
 	if (err != sl::zed::SUCCESS)
@@ -892,7 +912,11 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
 
 bool CameraStereoZed::isCalibrated() const
 {
+#ifdef RTABMAP_ZED
 	return stereoModel_.isValidForProjection();
+#else
+	return false;
+#endif
 }
 
 std::string CameraStereoZed::getSerial() const
@@ -906,6 +930,15 @@ std::string CameraStereoZed::getSerial() const
 	return "";
 }
 
+bool CameraStereoZed::odomProvided() const
+{
+#ifdef RTABMAP_ZED
+	return computeOdometry_;
+#else
+	return false;
+#endif
+}
+
 SensorData CameraStereoZed::captureImage(CameraInfo * info)
 {
 	SensorData data;
@@ -923,7 +956,8 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
 		if(!res)
 		{
 			// get left image
-			cv::Mat rgbaLeft = slMat2cvMat(zed_->retrieveImage(static_cast<sl::zed::SIDE> (sl::zed::STEREO_LEFT)));
+			cv::Mat rgbaLeft = sl::zed::slMat2cvMat(zed_->retrieveImage(static_cast<sl::zed::SIDE> (sl::zed::LEFT)));
+
 			cv::Mat left;
 			cv::cvtColor(rgbaLeft, left, cv::COLOR_BGRA2BGR);
 
@@ -938,7 +972,7 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
 			else
 			{
 				// get right image
-				cv::Mat rgbaRight = slMat2cvMat(zed_->retrieveImage(static_cast<sl::zed::SIDE> (sl::zed::STEREO_RIGHT)));
+				cv::Mat rgbaRight = sl::zed::slMat2cvMat(zed_->retrieveImage(static_cast<sl::zed::SIDE> (sl::zed::RIGHT)));
 				cv::Mat right;
 				cv::cvtColor(rgbaRight, right, cv::COLOR_BGRA2GRAY);
 			
@@ -1163,7 +1197,7 @@ SensorData CameraStereoImages::captureImage(CameraInfo * info)
 				stereoModel_.setImageSize(leftImage.size());
 			}
 
-			data = SensorData(left.laserScanRaw(), left.laserScanMaxPts(), 0, leftImage, rightImage, stereoModel_, left.id()/(camera2_?1:2), left.stamp());
+			data = SensorData(left.laserScanRaw(), left.laserScanInfo(), leftImage, rightImage, stereoModel_, left.id()/(camera2_?1:2), left.stamp());
 			data.setGroundTruth(left.groundTruth());
 		}
 	}
@@ -1191,13 +1225,27 @@ CameraStereoVideo::CameraStereoVideo(
 {
 }
 
+CameraStereoVideo::CameraStereoVideo(
+		const std::string & pathLeft,
+		const std::string & pathRight,
+		bool rectifyImages,
+		float imageRate,
+		const Transform & localTransform) :
+		Camera(imageRate, localTransform),
+		path_(pathLeft),
+		path2_(pathRight),
+		rectifyImages_(rectifyImages),
+		src_(CameraVideo::kVideoFile),
+		usbDevice_(0)
+{
+}
+
 CameraStereoVideo::CameraStereoVideo(
 	int device,
 	bool rectifyImages,
 	float imageRate,
 	const Transform & localTransform) :
 	Camera(imageRate, localTransform),
-	path_(""),
 	rectifyImages_(rectifyImages),
 	src_(CameraVideo::kUsbDevice),
 	usbDevice_(device)
@@ -1207,6 +1255,7 @@ CameraStereoVideo::CameraStereoVideo(
 CameraStereoVideo::~CameraStereoVideo()
 {
 	capture_.release();
+	capture2_.release();
 }
 
 bool CameraStereoVideo::init(const std::string & calibrationFolder, const std::string & cameraName)
@@ -1216,6 +1265,10 @@ bool CameraStereoVideo::init(const std::string & calibrationFolder, const std::s
 	{
 		capture_.release();
 	}
+	if(capture2_.isOpened())
+	{
+		capture2_.release();
+	}
 
 	if (src_ == CameraVideo::kUsbDevice)
 	{
@@ -1224,55 +1277,63 @@ bool CameraStereoVideo::init(const std::string & calibrationFolder, const std::s
 	}
 	else if (src_ == CameraVideo::kVideoFile)
 	{
-		ULOGGER_DEBUG("CameraStereoVideo: filename=\"%s\"", path_.c_str());
-		capture_.open(path_.c_str());
+		if(path2_.empty())
+		{
+			ULOGGER_DEBUG("CameraStereoVideo: filename=\"%s\"", path_.c_str());
+			capture_.open(path_.c_str());
+		}
+		else
+		{
+			ULOGGER_DEBUG("CameraStereoVideo: filenames=\"%s\" and \"%s\"", path_.c_str(), path2_.c_str());
+			capture_.open(path_.c_str());
+			capture2_.open(path2_.c_str());
+		}
 	}
 	else
 	{
 		ULOGGER_ERROR("CameraStereoVideo: Unknown source...");
 	}
 
-	if(!capture_.isOpened())
+	if(!capture_.isOpened() || (!path2_.empty() && !capture2_.isOpened()))
 	{
 		ULOGGER_ERROR("CameraStereoVideo: Failed to create a capture object!");
 		capture_.release();
+		capture2_.release();
 		return false;
 	}
-	else
+
+	if (cameraName_.empty())
 	{
-		if (cameraName_.empty())
+		unsigned int guid = (unsigned int)capture_.get(CV_CAP_PROP_GUID);
+		if (guid != 0 && guid != 0xffffffff)
 		{
-			unsigned int guid = (unsigned int)capture_.get(CV_CAP_PROP_GUID);
-			if (guid != 0 && guid != 0xffffffff)
-			{
-				cameraName_ = uFormat("%08x", guid);
-			}
+			cameraName_ = uFormat("%08x", guid);
 		}
+	}
 
-		// look for calibration files
-		if(!calibrationFolder.empty() && !cameraName_.empty())
+	// look for calibration files
+	if(!calibrationFolder.empty() && !cameraName_.empty())
+	{
+		if(!stereoModel_.load(calibrationFolder, cameraName_))
 		{
-			if(!stereoModel_.load(calibrationFolder, cameraName_))
-			{
-				UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
-					cameraName_.c_str(), calibrationFolder.c_str());
-			}
-			else
-			{
-				UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f",
-						stereoModel_.left().fx(),
-						stereoModel_.left().cx(),
-						stereoModel_.left().cy(),
-						stereoModel_.baseline());
-			}
+			UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
+				cameraName_.c_str(), calibrationFolder.c_str());
 		}
+		else
+		{
+			UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f",
+					stereoModel_.left().fx(),
+					stereoModel_.left().cx(),
+					stereoModel_.left().cy(),
+					stereoModel_.baseline());
+		}
+	}
 
-		stereoModel_.setLocalTransform(this->getLocalTransform());
-		if(rectifyImages_ && !stereoModel_.isValidForRectification())
-		{
-			UERROR("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid.");
-			return false;
-		}
+	stereoModel_.setLocalTransform(this->getLocalTransform());
+	if(rectifyImages_ && !stereoModel_.isValidForRectification())
+	{
+		UERROR("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid.");
+		return false;
 	}
 	return true;
 }
@@ -1292,43 +1353,61 @@ SensorData CameraStereoVideo::captureImage(CameraInfo * info)
 	SensorData data;
 
 	cv::Mat img;
-	if(capture_.isOpened())
+	if(capture_.isOpened() && (path2_.empty() || capture2_.isOpened()))
 	{
-		if(capture_.read(img))
+		cv::Mat leftImage;
+		cv::Mat rightImage;
+		if(path2_.empty())
 		{
-			// Rectification
-			cv::Mat leftImage(img, cv::Rect( 0, 0, img.size().width/2, img.size().height ));
-			cv::Mat rightImage(img, cv::Rect( img.size().width/2, 0, img.size().width/2, img.size().height ));
-			bool rightCvt = false;
-			if(rightImage.type() != CV_8UC1)
+			if(!capture_.read(img))
 			{
-				cv::Mat tmp;
-				cv::cvtColor(rightImage, tmp, CV_BGR2GRAY);
-				rightImage = tmp;
-				rightCvt = true;
+				return data;
 			}
-
-			if(rectifyImages_ && stereoModel_.left().isValidForRectification() && stereoModel_.right().isValidForRectification())
-			{
-				leftImage = stereoModel_.left().rectifyImage(leftImage);
-				rightImage = stereoModel_.right().rectifyImage(rightImage);
-			}
-			else
-			{
-				leftImage = leftImage.clone();
-				if(!rightCvt)
-				{
-					rightImage = rightImage.clone();
-				}
-			}
-
-			if(stereoModel_.left().imageHeight() == 0 || stereoModel_.left().imageWidth() == 0)
-			{
-				stereoModel_.setImageSize(leftImage.size());
-			}
-
-			data = SensorData(leftImage, rightImage, stereoModel_, this->getNextSeqID(), UTimer::now());
+			// Side by side stream
+			leftImage = cv::Mat(img, cv::Rect( 0, 0, img.size().width/2, img.size().height ));
+			rightImage = cv::Mat(img, cv::Rect( img.size().width/2, 0, img.size().width/2, img.size().height ));
 		}
+		else if(!capture_.read(leftImage) || !capture2_.read(rightImage))
+		{
+			return data;
+		}
+		else if(leftImage.cols != rightImage.cols || leftImage.rows != rightImage.rows)
+		{
+			UERROR("Left and right streams don't have image of the same size: left=%dx%d right=%dx%d",
+					leftImage.cols, leftImage.rows, rightImage.cols, rightImage.rows);
+			return data;
+		}
+
+		// Rectification
+		bool rightCvt = false;
+		if(rightImage.type() != CV_8UC1)
+		{
+			cv::Mat tmp;
+			cv::cvtColor(rightImage, tmp, CV_BGR2GRAY);
+			rightImage = tmp;
+			rightCvt = true;
+		}
+
+		if(rectifyImages_ && stereoModel_.left().isValidForRectification() && stereoModel_.right().isValidForRectification())
+		{
+			leftImage = stereoModel_.left().rectifyImage(leftImage);
+			rightImage = stereoModel_.right().rectifyImage(rightImage);
+		}
+		else
+		{
+			leftImage = leftImage.clone();
+			if(!rightCvt)
+			{
+				rightImage = rightImage.clone();
+			}
+		}
+
+		if(stereoModel_.left().imageHeight() == 0 || stereoModel_.left().imageWidth() == 0)
+		{
+			stereoModel_.setImageSize(leftImage.size());
+		}
+
+		data = SensorData(leftImage, rightImage, stereoModel_, this->getNextSeqID(), UTimer::now());
 	}
 	else
 	{

corelib/src/CameraThread.cpp

@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util3d_surface.h"
 #include "rtabmap/core/util3d_filtering.h"
 #include "rtabmap/core/StereoDense.h"
+#include "rtabmap/core/clams/discrete_depth_distortion_model.h"
 
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/ULogger.h>
@@ -56,7 +57,11 @@ CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
 		_scanMinDepth(0.0f),
 		_scanVoxelSize(0.0f),
 		_scanNormalsK(0),
-		_stereoDense(new StereoBM(parameters))
+		_stereoDense(new StereoBM(parameters)),
+		_distortionModel(0),
+		_bilateralFiltering(false),
+		_bilateralSigmaS(10),
+		_bilateralSigmaR(0.1)
 {
 	UASSERT(_camera != 0);
 }
@@ -69,6 +74,10 @@ CameraThread::~CameraThread()
 	{
 		delete _camera;
 	}
+	if(_distortionModel)
+	{
+		delete _distortionModel;
+	}
 	delete _stereoDense;
 }
 
@@ -80,8 +89,42 @@ void CameraThread::setImageRate(float imageRate)
 	}
 }
 
+void CameraThread::setDistortionModel(const std::string & path)
+{
+	if(_distortionModel)
+	{
+		delete _distortionModel;
+		_distortionModel = 0;
+	}
+	if(!path.empty())
+	{
+		_distortionModel = new clams::DiscreteDepthDistortionModel();
+		_distortionModel->load(path);
+		if(!_distortionModel->isValid())
+		{
+			UERROR("Loaded distortion model \"%s\" is not valid!", path.c_str());
+			delete _distortionModel;
+			_distortionModel = 0;
+		}
+	}
+}
+
+void CameraThread::enableBilateralFiltering(float sigmaS, float sigmaR)
+{
+	UASSERT(sigmaS > 0.0f && sigmaR > 0.0f);
+	_bilateralFiltering = true;
+	_bilateralSigmaS = sigmaS;
+	_bilateralSigmaR = sigmaR;
+}
+
+void CameraThread::mainLoopBegin()
+{
+	ULogger::registerCurrentThread("Camera");
+}
+
 void CameraThread::mainLoop()
 {
+	UTimer totalTime;
 	UDEBUG("");
 	CameraInfo info;
 	SensorData data = _camera->takeImage(&info);
@@ -92,6 +135,33 @@ void CameraThread::mainLoop()
 		{
 			data.setDepthOrRightRaw(cv::Mat());
 		}
+
+		if(_distortionModel && !data.depthRaw().empty())
+		{
+			UTimer timer;
+			if(_distortionModel->getWidth() == data.depthRaw().cols &&
+			   _distortionModel->getHeight() == data.depthRaw().rows	)
+			{
+				cv::Mat depth = data.depthRaw().clone();// make sure we are not modifying data in cached signatures.
+				_distortionModel->undistort(depth);
+				data.setDepthOrRightRaw(depth);
+			}
+			else
+			{
+				UERROR("Distortion model size is %dx%d but dpeth image is %dx%d!",
+						_distortionModel->getWidth(), _distortionModel->getHeight(),
+						data.depthRaw().cols, data.depthRaw().rows);
+			}
+			info.timeUndistortDepth = timer.ticks();
+		}
+
+		if(_bilateralFiltering && !data.depthRaw().empty())
+		{
+			UTimer timer;
+			data.setDepthOrRightRaw(util2d::fastBilateralFiltering(data.depthRaw(), _bilateralSigmaS, _bilateralSigmaR));
+			info.timeBilateralFiltering = timer.ticks();
+		}
+
 		if(_imageDecimation>1 && !data.imageRaw().empty())
 		{
 			UDEBUG("");
@@ -158,7 +228,15 @@ void CameraThread::mainLoop()
 					_stereoDense->computeDisparity(data.imageRaw(), data.rightRaw()),
 					data.stereoCameraModel().left().fx(),
 					data.stereoCameraModel().baseline());
-			data.setCameraModel(data.stereoCameraModel().left());
+			// set Tx for stereo bundle adjustment (when used)
+			CameraModel model = CameraModel(
+					data.stereoCameraModel().left().fx(),
+					data.stereoCameraModel().left().fy(),
+					data.stereoCameraModel().left().cx(),
+					data.stereoCameraModel().left().cy(),
+					data.stereoCameraModel().localTransform(),
+					-data.stereoCameraModel().baseline()*data.stereoCameraModel().left().fx());
+			data.setCameraModel(model);
 			data.setDepthOrRightRaw(depth);
 			data.setStereoCameraModel(StereoCameraModel());
 			info.timeDisparity = timer.ticks();
@@ -175,9 +253,15 @@ void CameraThread::mainLoop()
 				UASSERT(_scanDecimation >= 1);
 				UTimer timer;
 				pcl::IndicesPtr validIndices(new std::vector<int>);
-				pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(data, _scanDecimation, _scanMaxDepth, _scanMinDepth, validIndices.get());
+				pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(
+						data,
+						_scanDecimation,
+						_scanMaxDepth,
+						_scanMinDepth,
+						validIndices.get());
 				float maxPoints = (data.depthRaw().rows/_scanDecimation)*(data.depthRaw().cols/_scanDecimation);
 				cv::Mat scan;
+				const Transform & baseToScan = data.cameraModels()[0].localTransform();
 				if(validIndices->size())
 				{
 					if(_scanVoxelSize>0.0f)
@@ -197,32 +281,19 @@ void CameraThread::mainLoop()
 					{
 						if(_scanNormalsK>0)
 						{
-							// view point
-							Eigen::Vector3f viewPoint(0.0f,0.0f,0.0f);
-							if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull())
-							{
-								viewPoint[0] = data.cameraModels()[0].localTransform().x();
-								viewPoint[1] = data.cameraModels()[0].localTransform().y();
-								viewPoint[2] = data.cameraModels()[0].localTransform().z();
-							}
-							else if(!data.stereoCameraModel().localTransform().isNull())
-							{
-								viewPoint[0] = data.stereoCameraModel().localTransform().x();
-								viewPoint[1] = data.stereoCameraModel().localTransform().y();
-								viewPoint[2] = data.stereoCameraModel().localTransform().z();
-							}
+							Eigen::Vector3f viewPoint(baseToScan.x(), baseToScan.y(), baseToScan.z());
 							pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, viewPoint);
 							pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
 							pcl::concatenateFields(*cloud, *normals, *cloudNormals);
-							scan = util3d::laserScanFromPointCloud(*cloudNormals);
+							scan = util3d::laserScanFromPointCloud(*cloudNormals, baseToScan.inverse());
 						}
 						else
 						{
-							scan = util3d::laserScanFromPointCloud(*cloud);
+							scan = util3d::laserScanFromPointCloud(*cloud, baseToScan.inverse());
 						}
 					}
 				}
-				data.setLaserScanRaw(scan, (int)maxPoints, _scanMaxDepth);
+				data.setLaserScanRaw(scan, LaserScanInfo((int)maxPoints, _scanMaxDepth, baseToScan));
 				info.timeScanFromDepth = timer.ticks();
 				UDEBUG("Computing scan from depth = %f s", info.timeScanFromDepth);
 			}
@@ -235,6 +306,7 @@ void CameraThread::mainLoop()
 		}
 
 		info.cameraName = _camera->getSerial();
+		info.timeTotal = totalTime.ticks();
 		this->post(new CameraEvent(data, info));
 	}
 	else if(!this->isKilled())

corelib/src/DBDriver.cpp

@@ -61,7 +61,7 @@ void DBDriver::parseParameters(const ParametersMap & parameters)
 {
 }
 
-void DBDriver::closeConnection(bool save)
+void DBDriver::closeConnection(bool save, const std::string & outputUrl)
 {
 	UDEBUG("isRunning=%d", this->isRunning());
 	this->join(true);
@@ -78,7 +78,7 @@ void DBDriver::closeConnection(bool save)
 		_trashesMutex.unlock();
 	}
 	_dbSafeAccessMutex.lock();
-	this->disconnectDatabaseQuery(save);
+	this->disconnectDatabaseQuery(save, outputUrl);
 	_dbSafeAccessMutex.unlock();
 	UDEBUG("");
 }
@@ -197,6 +197,15 @@ ParametersMap DBDriver::getLastParameters() const
 	return parameters;
 }
 
+std::map<std::string, float> DBDriver::getStatistics(int nodeId, double & stamp) const
+{
+	std::map<std::string, float> statistics;
+	_dbSafeAccessMutex.lock();
+	statistics = getStatisticsQuery(nodeId, stamp);
+	_dbSafeAccessMutex.unlock();
+	return statistics;
+}
+
 std::string DBDriver::getDatabaseVersion() const
 {
 	std::string version = "0.0.0";
@@ -405,6 +414,25 @@ void DBDriver::updateLink(const Link & link)
 	this->updateLinkQuery(link);
 	_dbSafeAccessMutex.unlock();
 }
+void DBDriver::updateOccupancyGrid(
+		int nodeId,
+		const cv::Mat & ground,
+		const cv::Mat & obstacles,
+		float cellSize,
+		const cv::Point3f & viewpoint)
+{
+	_dbSafeAccessMutex.lock();
+	//just to make sure the occupancy grids are compressed for convenience
+	SensorData data;
+	data.setOccupancyGrid(ground, obstacles, cellSize, viewpoint);
+	this->updateOccupancyGridQuery(
+			nodeId,
+			data.gridGroundCellsCompressed(),
+			data.gridObstacleCellsCompressed(),
+			cellSize,
+			viewpoint);
+	_dbSafeAccessMutex.unlock();
+}
 
 void DBDriver::load(VWDictionary * dictionary) const
 {
@@ -514,7 +542,7 @@ void DBDriver::loadWords(const std::set<int> & wordIds, std::list<VisualWord *>
 	}
 }
 
-void DBDriver::loadNodeData(std::list<Signature *> & signatures) const
+void DBDriver::loadNodeData(std::list<Signature *> & signatures, bool images, bool scan, bool userData, bool occupancyGrid) const
 {
 	// Don't look in the trash, we assume that if we want to load
 	// data of a signature, it is not in thrash! Print an error if so.
@@ -530,13 +558,14 @@ void DBDriver::loadNodeData(std::list<Signature *> & signatures) const
 	_trashesMutex.unlock();
 
 	_dbSafeAccessMutex.lock();
-	this->loadNodeDataQuery(signatures);
+	this->loadNodeDataQuery(signatures, images, scan, userData, occupancyGrid);
 	_dbSafeAccessMutex.unlock();
 }
 
 void DBDriver::getNodeData(
 		int signatureId,
-		SensorData & data) const
+		SensorData & data,
+		bool images, bool scan, bool userData, bool occupancyGrid) const
 {
 	bool found = false;
 	// look in the trash
@@ -544,7 +573,11 @@ void DBDriver::getNodeData(
 	if(uContains(_trashSignatures, signatureId))
 	{
 		const Signature * s = _trashSignatures.at(signatureId);
-		if(!s->sensorData().imageCompressed().empty() || !s->isSaved())
+		if(!s->sensorData().imageCompressed().empty() ||
+			!s->sensorData().laserScanCompressed().empty() ||
+			!s->sensorData().userDataCompressed().empty() ||
+			s->sensorData().gridCellSize() != 0.0f ||
+			!s->isSaved())
 		{
 			data = (SensorData)s->sensorData();
 			found = true;
@@ -558,7 +591,7 @@ void DBDriver::getNodeData(
 		std::list<Signature *> signatures;
 		Signature tmp(signatureId);
 		signatures.push_back(&tmp);
-		loadNodeDataQuery(signatures);
+		loadNodeDataQuery(signatures, images, scan, userData, occupancyGrid);
 		data = signatures.front()->sensorData();
 		_dbSafeAccessMutex.unlock();
 	}
@@ -569,6 +602,7 @@ bool DBDriver::getCalibration(
 		std::vector<CameraModel> & models,
 		StereoCameraModel & stereoModel) const
 {
+	UDEBUG("");
 	bool found = false;
 	// look in the trash
 	_trashesMutex.lock();
@@ -750,6 +784,15 @@ void DBDriver::getLastNodeId(int & id) const
 
 	_dbSafeAccessMutex.lock();
 	this->getLastIdQuery("Node", id);
+	int statisticsId = 0;
+	if(uStrNumCmp(this->getDatabaseVersion(), "0.11.11") >= 0)
+	{
+		this->getLastIdQuery("Statistics", statisticsId);
+		if(statisticsId > id)
+		{
+			id = statisticsId;
+		}
+	}
 	_dbSafeAccessMutex.unlock();
 }
 
@@ -842,7 +885,7 @@ void DBDriver::getAllLabels(std::map<int, std::string> & labels) const
 	_dbSafeAccessMutex.unlock();
 }
 
-void DBDriver::addStatisticsAfterRun(
+void DBDriver::addInfoAfterRun(
 		int stMemSize,
 		int lastSignAdded,
 		int processMemUsed,
@@ -857,13 +900,26 @@ void DBDriver::addStatisticsAfterRun(
 		if(uStrNumCmp(this->getDatabaseVersion(), "0.11.8") >= 0)
 		{
 			std::string param = Parameters::serialize(parameters);
-			query << "INSERT INTO Statistics(STM_size,last_sign_added,process_mem_used,database_mem_used,dictionary_size,parameters) values("
-				  << stMemSize << ","
-				  << lastSignAdded << ","
-				  << processMemUsed << ","
-				  << databaseMemUsed << ","
-				  << dictionarySize << ","
-				  "\"" << param.c_str() << "\");";
+			if(uStrNumCmp(this->getDatabaseVersion(), "0.11.11") >= 0)
+			{
+				query << "INSERT INTO Info(STM_size,last_sign_added,process_mem_used,database_mem_used,dictionary_size,parameters) values("
+					  << stMemSize << ","
+					  << lastSignAdded << ","
+					  << processMemUsed << ","
+					  << databaseMemUsed << ","
+					  << dictionarySize << ","
+					  "\"" << param.c_str() << "\");";
+			}
+			else
+			{
+				query << "INSERT INTO Statistics(STM_size,last_sign_added,process_mem_used,database_mem_used,dictionary_size,parameters) values("
+					  << stMemSize << ","
+					  << lastSignAdded << ","
+					  << processMemUsed << ","
+					  << databaseMemUsed << ","
+					  << dictionarySize << ","
+					  "\"" << param.c_str() << "\");";
+			}
 		}
 		else
 		{
@@ -879,6 +935,13 @@ void DBDriver::addStatisticsAfterRun(
 	}
 }
 
+void DBDriver::addStatistics(const Statistics & statistics) const
+{
+	_dbSafeAccessMutex.lock();
+	addStatisticsQuery(statistics);
+	_dbSafeAccessMutex.unlock();
+}
+
 void DBDriver::generateGraph(
 		const std::string & fileName,
 		const std::set<int> & idsInput,

corelib/src/DBDriverSqlite3.h

@@ -49,7 +49,7 @@ public:
 
 private:
 	virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false);
-	virtual void disconnectDatabaseQuery(bool save = true);
+	virtual void disconnectDatabaseQuery(bool save = true, const std::string & outputUrl = "");
 	virtual bool isConnectedQuery() const;
 	virtual long getMemoryUsedQuery() const; // In bytes
 	virtual bool getDatabaseVersionQuery(std::string & version) const;
@@ -63,6 +63,7 @@ private:
 	virtual int getTotalNodesSizeQuery() const;
 	virtual int getTotalDictionarySizeQuery() const;
 	virtual ParametersMap getLastParametersQuery() const;
+	virtual std::map<std::string, float> getStatisticsQuery(int nodeId, double & stamp) const;
 
 	virtual void executeNoResultQuery(const std::string & sql) const;
 
@@ -76,6 +77,15 @@ private:
 	virtual void addLinkQuery(const Link & link) const;
 	virtual void updateLinkQuery(const Link & link) const;
 
+	virtual void updateOccupancyGridQuery(
+			int nodeId,
+			const cv::Mat & ground,
+			const cv::Mat & obstacles,
+			float cellSize,
+			const cv::Point3f & viewpoint) const;
+
+	virtual void addStatisticsQuery(const Statistics & statistics) const;
+
 	// Load objects
 	virtual void loadQuery(VWDictionary * dictionary) const;
 	virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const;
@@ -83,7 +93,7 @@ private:
 	virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const;
 	virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
 
-	virtual void loadNodeDataQuery(std::list<Signature *> & signatures) const;
+	virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const;
 	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
 	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose) const;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const;
@@ -102,6 +112,7 @@ private:
 	std::string queryStepLink() const;
 	std::string queryStepWordsChanged() const;
 	std::string queryStepKeypoint() const;
+	std::string queryStepOccupancyGridUpdate() const;
 	void stepNode(sqlite3_stmt * ppStmt, const Signature * s) const;
 	void stepImage(
 			sqlite3_stmt * ppStmt,
@@ -112,6 +123,12 @@ private:
 	void stepLink(sqlite3_stmt * ppStmt, const Link & link) const;
 	void stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
 	void stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp, const cv::Point3f & pt, const cv::Mat & descriptor) const;
+	void stepOccupancyGrid(sqlite3_stmt * ppStmt,
+			int nodeId,
+			const cv::Mat & ground,
+			const cv::Mat & obstacles,
+			float cellSize,
+			const cv::Point3f & viewpoint) const;
 
 private:
 	void loadLinksQuery(std::list<Signature *> & signatures) const;

corelib/src/DBReader.cpp

@@ -162,9 +162,24 @@ bool DBReader::init(
 		StereoCameraModel stereoModel;
 		if(_dbDriver->getCalibration(*_ids.begin(), models, stereoModel))
 		{
-			if(models.size() && models.at(0).isValidForProjection())
+			if(models.size())
 			{
-				_calibrated = true;
+				if(models.at(0).isValidForProjection())
+				{
+					_calibrated = true;
+				}
+				else if(models.at(0).fx() && models.at(0).fy() && models.at(0).imageWidth() == 0)
+				{
+					// backward compatibility for databases not saving cx,cy and imageSize
+					SensorData data;
+					_dbDriver->getNodeData(*_ids.begin(), data, true, false, false, false);
+					cv::Mat rgb;
+					data.uncompressData(&rgb, 0); // this will update camera models if old format
+					if(data.cameraModels().size() && data.cameraModels().at(0).isValidForProjection())
+					{
+						_calibrated = true;
+					}
+				}
 			}
 			else if(stereoModel.isValidForProjection())
 			{
@@ -172,6 +187,10 @@ bool DBReader::init(
 			}
 		}
 	}
+	else
+	{
+		_calibrated = true; // database is empty, make sure calibration warning is not shown.
+	}
 
 	_timer.start();
 

corelib/src/Features2d.cpp

@@ -209,8 +209,15 @@ void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKey
 }
 
 void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints)
+{
+	std::vector<cv::Point3f> keypoints3D;
+	limitKeypoints(keypoints, keypoints3D, descriptors, maxKeypoints);
+}
+
+void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & keypoints3D, cv::Mat & descriptors, int maxKeypoints)
 {
 	UASSERT_MSG((int)keypoints.size() == descriptors.rows || descriptors.rows == 0, uFormat("keypoints=%d descriptors=%d", (int)keypoints.size(), descriptors.rows).c_str());
+	UASSERT_MSG(keypoints.size() == keypoints3D.size() || keypoints3D.size() == 0, uFormat("keypoints=%d keypoints3D=%d", (int)keypoints.size(), (int)keypoints3D.size()).c_str());
 	if(maxKeypoints > 0 && (int)keypoints.size() > maxKeypoints)
 	{
 		UTimer timer;
@@ -229,6 +236,7 @@ void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat &
 		int removed = (int)hessianMap.size()-maxKeypoints;
 		std::multimap<float, int>::reverse_iterator iter = hessianMap.rbegin();
 		std::vector<cv::KeyPoint> kptsTmp(maxKeypoints);
+		std::vector<cv::Point3f> kpts3DTmp(maxKeypoints);
 		cv::Mat descriptorsTmp;
 		if(descriptors.rows)
 		{
@@ -237,6 +245,10 @@ void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat &
 		for(unsigned int k=0; k < kptsTmp.size() && iter!=hessianMap.rend(); ++k, ++iter)
 		{
 			kptsTmp[k] = keypoints[iter->second];
+			if(keypoints3D.size())
+			{
+				kpts3DTmp[k] = keypoints3D[iter->second];
+			}
 			if(descriptors.rows)
 			{
 				if(descriptors.type() == CV_32FC1)
@@ -252,6 +264,7 @@ void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat &
 		ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, (int)kptsTmp.size(), kptsTmp.size()?kptsTmp.back().response:0.0f);
 		ULOGGER_DEBUG("removing words time = %f s", timer.ticks());
 		keypoints = kptsTmp;
+		keypoints3D = kpts3DTmp;
 		if(descriptors.rows)
 		{
 			descriptors = descriptorsTmp;
@@ -261,78 +274,12 @@ void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat &
 
 cv::Rect Feature2D::computeRoi(const cv::Mat & image, const std::string & roiRatios)
 {
-	std::list<std::string> strValues = uSplit(roiRatios, ' ');
-	if(strValues.size() != 4)
-	{
-		UERROR("The number of values must be 4 (roi=\"%s\")", roiRatios.c_str());
-	}
-	else
-	{
-		std::vector<float> values(4);
-		unsigned int i=0;
-		for(std::list<std::string>::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter)
-		{
-			values[i] = uStr2Float(*iter);
-			++i;
-		}
-
-		if(values[0] >= 0 && values[0] < 1 && values[0] < 1.0f-values[1] &&
-			values[1] >= 0 && values[1] < 1 && values[1] < 1.0f-values[0] &&
-			values[2] >= 0 && values[2] < 1 && values[2] < 1.0f-values[3] &&
-			values[3] >= 0 && values[3] < 1 && values[3] < 1.0f-values[2])
-		{
-			return computeRoi(image, values);
-		}
-		else
-		{
-			UERROR("The roi ratios are not valid (roi=\"%s\")", roiRatios.c_str());
-		}
-	}
-	return cv::Rect();
+	return util2d::computeRoi(image, roiRatios);
 }
 
 cv::Rect Feature2D::computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios)
 {
-	if(!image.empty() && roiRatios.size() == 4)
-	{
-		float width = image.cols;
-		float height = image.rows;
-		cv::Rect roi(0, 0, width, height);
-		UDEBUG("roi ratios = %f, %f, %f, %f", roiRatios[0],roiRatios[1],roiRatios[2],roiRatios[3]);
-		UDEBUG("roi = %d, %d, %d, %d", roi.x, roi.y, roi.width, roi.height);
-
-		//left roi
-		if(roiRatios[0] > 0 && roiRatios[0] < 1 - roiRatios[1])
-		{
-			roi.x = width * roiRatios[0];
-		}
-
-		//right roi
-		if(roiRatios[1] > 0 && roiRatios[1] < 1 - roiRatios[0])
-		{
-			roi.width -= width * roiRatios[1] + width * roiRatios[0];
-		}
-
-		//top roi
-		if(roiRatios[2] > 0 && roiRatios[2] < 1 - roiRatios[3])
-		{
-			roi.y = height * roiRatios[2];
-		}
-
-		//bottom roi
-		if(roiRatios[3] > 0 && roiRatios[3] < 1 - roiRatios[2])
-		{
-			roi.height -= height * roiRatios[3] + height * roiRatios[2];
-		}
-		UDEBUG("roi = %d, %d, %d, %d", roi.x, roi.y, roi.width, roi.height);
-
-		return roi;
-	}
-	else
-	{
-		UERROR("Image is null or _roiRatios(=%d) != 4", roiRatios.size());
-		return cv::Rect();
-	}
+	return util2d::computeRoi(image, roiRatios);
 }
 
 /////////////////////

corelib/src/FlannIndex.cpp

@@ -0,0 +1,519 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/FlannIndex.h>
+#include <rtabmap/utilite/ULogger.h>
+
+#include "rtflann/flann.hpp"
+
+namespace rtabmap {
+
+FlannIndex::FlannIndex():
+		index_(0),
+		nextIndex_(0),
+		featuresType_(0),
+		featuresDim_(0),
+		isLSH_(false),
+		useDistanceL1_(false)
+{
+}
+FlannIndex::~FlannIndex()
+{
+	this->release();
+}
+
+void FlannIndex::release()
+{
+	if(index_)
+	{
+		if(featuresType_ == CV_8UC1)
+		{
+			delete (rtflann::Index<rtflann::Hamming<unsigned char> >*)index_;
+		}
+		else
+		{
+			if(useDistanceL1_)
+			{
+				delete (rtflann::Index<rtflann::L1<float> >*)index_;
+			}
+			else if(featuresDim_ <= 3)
+			{
+				delete (rtflann::Index<rtflann::L2_Simple<float> >*)index_;
+			}
+			else
+			{
+				delete (rtflann::Index<rtflann::L2<float> >*)index_;
+			}
+		}
+		index_ = 0;
+	}
+	nextIndex_ = 0;
+	isLSH_ = false;
+	addedDescriptors_.clear();
+	removedIndexes_.clear();
+}
+
+unsigned int FlannIndex::indexedFeatures() const
+{
+	if(!index_)
+	{
+		return 0;
+	}
+	if(featuresType_ == CV_8UC1)
+	{
+		return ((const rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->size();
+	}
+	else
+	{
+		if(useDistanceL1_)
+		{
+			return ((const rtflann::Index<rtflann::L1<float> >*)index_)->size();
+		}
+		else if(featuresDim_ <= 3)
+		{
+			return ((const rtflann::Index<rtflann::L2_Simple<float> >*)index_)->size();
+		}
+		else
+		{
+			return ((const rtflann::Index<rtflann::L2<float> >*)index_)->size();
+		}
+	}
+}
+
+// return KB
+unsigned int FlannIndex::memoryUsed() const
+{
+	if(!index_)
+	{
+		return 0;
+	}
+	if(featuresType_ == CV_8UC1)
+	{
+		return ((const rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->usedMemory()/1000;
+	}
+	else
+	{
+		if(useDistanceL1_)
+		{
+			return ((const rtflann::Index<rtflann::L1<float> >*)index_)->usedMemory()/1000;
+		}
+		else if(featuresDim_ <= 3)
+		{
+			return ((const rtflann::Index<rtflann::L2_Simple<float> >*)index_)->usedMemory()/1000;
+		}
+		else
+		{
+			return ((const rtflann::Index<rtflann::L2<float> >*)index_)->usedMemory()/1000;
+		}
+	}
+}
+
+void FlannIndex::buildLinearIndex(
+		const cv::Mat & features,
+		bool useDistanceL1)
+{
+	this->release();
+	UASSERT(index_ == 0);
+	UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1);
+	featuresType_ = features.type();
+	featuresDim_ = features.cols;
+	useDistanceL1_ = useDistanceL1;
+
+	rtflann::LinearIndexParams params;
+
+	if(featuresType_ == CV_8UC1)
+	{
+		rtflann::Matrix<unsigned char> dataset(features.data, features.rows, features.cols);
+		index_ = new rtflann::Index<rtflann::Hamming<unsigned char> >(dataset, params);
+		((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->buildIndex();
+	}
+	else
+	{
+		rtflann::Matrix<float> dataset((float*)features.data, features.rows, features.cols);
+		if(useDistanceL1_)
+		{
+			index_ = new rtflann::Index<rtflann::L1<float> >(dataset, params);
+			((rtflann::Index<rtflann::L1<float> >*)index_)->buildIndex();
+		}
+		else if(featuresDim_ <=3)
+		{
+			index_ = new rtflann::Index<rtflann::L2_Simple<float> >(dataset, params);
+			((rtflann::Index<rtflann::L2_Simple<float> >*)index_)->buildIndex();
+		}
+		else
+		{
+			index_ = new rtflann::Index<rtflann::L2<float> >(dataset, params);
+			((rtflann::Index<rtflann::L2<float> >*)index_)->buildIndex();
+		}
+	}
+
+	// incremental FLANN
+	addedDescriptors_.insert(std::make_pair(nextIndex_, features));
+
+	nextIndex_ = features.rows;
+}
+
+void FlannIndex::buildKDTreeIndex(
+		const cv::Mat & features,
+		int trees,
+		bool useDistanceL1)
+{
+	this->release();
+	UASSERT(index_ == 0);
+	UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1);
+	featuresType_ = features.type();
+	featuresDim_ = features.cols;
+	useDistanceL1_ = useDistanceL1;
+
+	rtflann::KDTreeIndexParams params(trees);
+
+	if(featuresType_ == CV_8UC1)
+	{
+		rtflann::Matrix<unsigned char> dataset(features.data, features.rows, features.cols);
+		index_ = new rtflann::Index<rtflann::Hamming<unsigned char> >(dataset, params);
+		((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->buildIndex();
+	}
+	else
+	{
+		rtflann::Matrix<float> dataset((float*)features.data, features.rows, features.cols);
+		if(useDistanceL1_)
+		{
+			index_ = new rtflann::Index<rtflann::L1<float> >(dataset, params);
+			((rtflann::Index<rtflann::L1<float> >*)index_)->buildIndex();
+		}
+		else if(featuresDim_ <=3)
+		{
+			index_ = new rtflann::Index<rtflann::L2_Simple<float> >(dataset, params);
+			((rtflann::Index<rtflann::L2_Simple<float> >*)index_)->buildIndex();
+		}
+		else
+		{
+			index_ = new rtflann::Index<rtflann::L2<float> >(dataset, params);
+			((rtflann::Index<rtflann::L2<float> >*)index_)->buildIndex();
+		}
+	}
+
+	// incremental FLANN
+	addedDescriptors_.insert(std::make_pair(nextIndex_, features));
+
+	nextIndex_ = features.rows;
+}
+
+void FlannIndex::buildKDTreeSingleIndex(
+		const cv::Mat & features,
+		int leafMaxSize,
+		bool reorder,
+		bool useDistanceL1)
+{
+	this->release();
+	UASSERT(index_ == 0);
+	UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1);
+	featuresType_ = features.type();
+	featuresDim_ = features.cols;
+	useDistanceL1_ = useDistanceL1;
+
+	rtflann::KDTreeSingleIndexParams params(leafMaxSize, reorder);
+
+	if(featuresType_ == CV_8UC1)
+	{
+		rtflann::Matrix<unsigned char> dataset(features.data, features.rows, features.cols);
+		index_ = new rtflann::Index<rtflann::Hamming<unsigned char> >(dataset, params);
+		((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->buildIndex();
+	}
+	else
+	{
+		rtflann::Matrix<float> dataset((float*)features.data, features.rows, features.cols);
+		if(useDistanceL1_)
+		{
+			index_ = new rtflann::Index<rtflann::L1<float> >(dataset, params);
+			((rtflann::Index<rtflann::L1<float> >*)index_)->buildIndex();
+		}
+		else if(featuresDim_ <=3)
+		{
+			index_ = new rtflann::Index<rtflann::L2_Simple<float> >(dataset, params);
+			((rtflann::Index<rtflann::L2_Simple<float> >*)index_)->buildIndex();
+		}
+		else
+		{
+			index_ = new rtflann::Index<rtflann::L2<float> >(dataset, params);
+			((rtflann::Index<rtflann::L2<float> >*)index_)->buildIndex();
+		}
+	}
+
+	// incremental FLANN
+	addedDescriptors_.insert(std::make_pair(nextIndex_, features));
+
+	nextIndex_ = features.rows;
+}
+
+void FlannIndex::buildLSHIndex(
+		const cv::Mat & features,
+		unsigned int table_number,
+		unsigned int key_size,
+		unsigned int multi_probe_level)
+{
+	this->release();
+	UASSERT(index_ == 0);
+	UASSERT(features.type() == CV_8UC1);
+	featuresType_ = features.type();
+	featuresDim_ = features.cols;
+	useDistanceL1_ = true;
+
+	rtflann::Matrix<unsigned char> dataset(features.data, features.rows, features.cols);
+	index_ = new rtflann::Index<rtflann::Hamming<unsigned char> >(dataset, rtflann::LshIndexParams(12, 20, 2));
+	((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->buildIndex();
+
+	// incremental FLANN
+	addedDescriptors_.insert(std::make_pair(nextIndex_, features));
+
+	nextIndex_ = features.rows;
+}
+
+bool FlannIndex::isBuilt()
+{
+	return index_!=0;
+}
+
+unsigned int FlannIndex::addPoints(const cv::Mat & features)
+{
+	if(!index_)
+	{
+		UERROR("Flann index not yet created!");
+		return 0;
+	}
+	UASSERT(features.type() == featuresType_);
+	UASSERT(features.cols == featuresDim_);
+	bool indexRebuilt = false;
+	size_t removedPts = 0;
+	if(featuresType_ == CV_8UC1)
+	{
+		rtflann::Matrix<unsigned char> points(features.data, features.rows, features.cols);
+		rtflann::Index<rtflann::Hamming<unsigned char> > * index = (rtflann::Index<rtflann::Hamming<unsigned char> >*)index_;
+		removedPts = index->removedCount();
+		index->addPoints(points, 0);
+		// Rebuild index if it doubles in size
+		if(index->sizeAtBuild() * 2 < index->size()+index->removedCount())
+		{
+			index->buildIndex();
+		}
+		// if no more removed points, the index has been rebuilt
+		indexRebuilt = index->removedCount() == 0 && removedPts>0;
+	}
+	else
+	{
+		rtflann::Matrix<float> points((float*)features.data, features.rows, features.cols);
+		if(useDistanceL1_)
+		{
+			rtflann::Index<rtflann::L1<float> > * index = (rtflann::Index<rtflann::L1<float> >*)index_;
+			removedPts = index->removedCount();
+			index->addPoints(points, 0);
+			// Rebuild index if it doubles in size
+			if(index->sizeAtBuild() * 2 < index->size()+index->removedCount())
+			{
+				index->buildIndex();
+			}
+			// if no more removed points, the index has been rebuilt
+			indexRebuilt = index->removedCount() == 0 && removedPts>0;
+		}
+		else if(featuresDim_ <= 3)
+		{
+			rtflann::Index<rtflann::L2_Simple<float> > * index = (rtflann::Index<rtflann::L2_Simple<float> >*)index_;
+			removedPts = index->removedCount();
+			index->addPoints(points, 0);
+			// Rebuild index if it doubles in size
+			if(index->sizeAtBuild() * 2 < index->size()+index->removedCount())
+			{
+				index->buildIndex();
+			}
+			// if no more removed points, the index has been rebuilt
+			indexRebuilt = index->removedCount() == 0 && removedPts>0;
+		}
+		else
+		{
+			rtflann::Index<rtflann::L2<float> > * index = (rtflann::Index<rtflann::L2<float> >*)index_;
+			removedPts = index->removedCount();
+			index->addPoints(points, 0);
+			// Rebuild index if it doubles in size
+			if(index->sizeAtBuild() * 2 < index->size()+index->removedCount())
+			{
+				index->buildIndex();
+			}
+			// if no more removed points, the index has been rebuilt
+			indexRebuilt = index->removedCount() == 0 && removedPts>0;
+		}
+	}
+
+	if(indexRebuilt)
+	{
+		UASSERT(removedPts == removedIndexes_.size());
+		// clean not used features
+		for(std::list<int>::iterator iter=removedIndexes_.begin(); iter!=removedIndexes_.end(); ++iter)
+		{
+			addedDescriptors_.erase(*iter);
+		}
+		removedIndexes_.clear();
+	}
+
+	addedDescriptors_.insert(std::make_pair(nextIndex_, features));
+
+	int r = nextIndex_;
+	nextIndex_ += features.rows;
+	return r;
+}
+
+void FlannIndex::removePoint(unsigned int index)
+{
+	if(!index_)
+	{
+		UERROR("Flann index not yet created!");
+		return;
+	}
+
+	// If a Segmentation fault occurs in removePoint(), verify that you have this fix in your installed "flann/algorithms/nn_index.h":
+	// 707 - if (ids_[id]==id) {
+	// 707 + if (id < ids_.size() && ids_[id]==id) {
+	// ref: https://github.com/mariusmuja/flann/commit/23051820b2314f07cf40ba633a4067782a982ff3#diff-33762b7383f957c2df17301639af5151
+
+	if(featuresType_ == CV_8UC1)
+	{
+		((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->removePoint(index);
+	}
+	else if(useDistanceL1_)
+	{
+		((rtflann::Index<rtflann::L1<float> >*)index_)->removePoint(index);
+	}
+	else if(featuresDim_ <= 3)
+	{
+		((rtflann::Index<rtflann::L2_Simple<float> >*)index_)->removePoint(index);
+	}
+	else
+	{
+		((rtflann::Index<rtflann::L2<float> >*)index_)->removePoint(index);
+	}
+
+	removedIndexes_.push_back(index);
+}
+
+void FlannIndex::knnSearch(
+		const cv::Mat & query,
+		cv::Mat & indices,
+		cv::Mat & dists,
+		int knn,
+		int checks,
+		float eps,
+		bool sorted) const
+{
+	if(!index_)
+	{
+		UERROR("Flann index not yet created!");
+		return;
+	}
+	indices.create(query.rows, knn, CV_32S);
+	dists.create(query.rows, knn, featuresType_ == CV_8UC1?CV_32S:CV_32F);
+
+	rtflann::Matrix<int> indicesF((int*)indices.data, indices.rows, indices.cols);
+
+	rtflann::SearchParams params = rtflann::SearchParams(checks, eps, sorted);
+
+	if(featuresType_ == CV_8UC1)
+	{
+		rtflann::Matrix<unsigned int> distsF((unsigned int*)dists.data, dists.rows, dists.cols);
+		rtflann::Matrix<unsigned char> queryF(query.data, query.rows, query.cols);
+		((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params);
+	}
+	else
+	{
+		rtflann::Matrix<float> distsF((float*)dists.data, dists.rows, dists.cols);
+		rtflann::Matrix<float> queryF((float*)query.data, query.rows, query.cols);
+		if(useDistanceL1_)
+		{
+			((rtflann::Index<rtflann::L1<float> >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params);
+		}
+		else if(featuresDim_ <= 3)
+		{
+			((rtflann::Index<rtflann::L2_Simple<float> >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params);
+		}
+		else
+		{
+			((rtflann::Index<rtflann::L2<float> >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params);
+		}
+	}
+}
+
+void FlannIndex::radiusSearch(
+		const cv::Mat & query,
+		std::vector<std::vector<size_t> > & indices,
+		std::vector<std::vector<float> > & dists,
+		float radius,
+		int maxNeighbors,
+		int checks,
+		float eps,
+		bool sorted) const
+{
+	if(!index_)
+	{
+		UERROR("Flann index not yet created!");
+		return;
+	}
+
+	rtflann::SearchParams params = rtflann::SearchParams(checks, eps, sorted);
+	params.max_neighbors = maxNeighbors<=0?-1:maxNeighbors; // -1 is all in radius
+
+	if(featuresType_ == CV_8UC1)
+	{
+		std::vector<std::vector<unsigned int> > distsF;
+		rtflann::Matrix<unsigned char> queryF(query.data, query.rows, query.cols);
+		((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->radiusSearch(queryF, indices, distsF, radius*radius, params);
+		dists.resize(distsF.size());
+		for(unsigned int i=0; i<dists.size(); ++i)
+		{
+			dists[i].resize(distsF[i].size());
+			for(unsigned int j=0; j<distsF[i].size(); ++j)
+			{
+				dists[i][j] = (float)distsF[i][j];
+			}
+		}
+	}
+	else
+	{
+		rtflann::Matrix<float> queryF((float*)query.data, query.rows, query.cols);
+		if(useDistanceL1_)
+		{
+			((rtflann::Index<rtflann::L1<float> >*)index_)->radiusSearch(queryF, indices, dists, radius*radius, params);
+		}
+		else if(featuresDim_ <= 3)
+		{
+			((rtflann::Index<rtflann::L2_Simple<float> >*)index_)->radiusSearch(queryF, indices, dists, radius*radius, params);
+		}
+		else
+		{
+			((rtflann::Index<rtflann::L2<float> >*)index_)->radiusSearch(queryF, indices, dists, radius*radius, params);
+		}
+	}
+}
+
+} /* namespace rtabmap */

corelib/src/GainCompensator.cpp

@@ -0,0 +1,423 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/core/GainCompensator.h"
+
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/core/util3d_transforms.h>
+#include <pcl/search/kdtree.h>
+#include <pcl/common/common.h>
+#include <pcl/common/transforms.h>
+#include <pcl/correspondence.h>
+
+namespace rtabmap {
+
+double sqr(uchar v)
+{
+	return double(v)*double(v);
+}
+
+GainCompensator::GainCompensator(double maxCorrespondenceDistance, double minOverlap, double alpha, double beta) :
+		maxCorrespondenceDistance_(maxCorrespondenceDistance),
+		minOverlap_(minOverlap),
+		alpha_(alpha),
+		beta_(beta)
+{
+}
+
+GainCompensator::~GainCompensator() {
+}
+
+void GainCompensator::feed(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloudA,
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloudB,
+		const Transform & transformB)
+{
+	std::multimap<int, Link> links;
+	links.insert(std::make_pair(0, Link(0,1,Link::kUserClosure, transformB)));
+	std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
+	clouds.insert(std::make_pair(0, cloudA));
+	clouds.insert(std::make_pair(1, cloudB));
+	feed(clouds, links);
+}
+
+void GainCompensator::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)
+{
+	std::multimap<int, Link> links;
+	links.insert(std::make_pair(0, Link(0,1,Link::kUserClosure, transformB)));
+	std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
+	clouds.insert(std::make_pair(0, cloudA));
+	clouds.insert(std::make_pair(1, cloudB));
+	std::map<int, pcl::IndicesPtr> indices;
+	indices.insert(std::make_pair(0, indicesA));
+	indices.insert(std::make_pair(1, indicesB));
+	feed(clouds, indices, links);
+}
+
+void GainCompensator::feed(
+		const std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds,
+		const std::multimap<int, Link> & links)
+{
+	std::map<int, pcl::IndicesPtr> indices;
+	feed(clouds, indices, links);
+}
+
+// @see https://studiofreya.com/3d-math-and-physics/simple-aabb-vs-aabb-collision-detection/
+struct AABB
+{
+    AABB() : c(), r() {}
+
+    AABB(const Eigen::Vector3f & center, const Eigen::Vector3f & halfwidths)
+        : c(center)
+        , r(halfwidths)
+    {}
+
+    Eigen::Vector3f c;        // center point
+    Eigen::Vector3f r;        // halfwidths
+};
+
+bool testAABBAABB(const AABB &a, const AABB &b)
+{
+    if ( fabs(a.c[0] - b.c[0]) > (a.r[0] + b.r[0]) ) return false;
+    if ( fabs(a.c[1] - b.c[1]) > (a.r[1] + b.r[1]) ) return false;
+    if ( fabs(a.c[2] - b.c[2]) > (a.r[2] + b.r[2]) ) return false;
+
+    // We have an overlap
+    return true;
+};
+
+/**
+ * @see https://github.com/opencv/opencv/blob/master/modules/stitching/src/exposure_compensate.cpp
+ */
+template<typename PointT>
+void feedImpl(
+		const std::map<int, typename pcl::PointCloud<PointT>::Ptr> & clouds,
+		const std::map<int, pcl::IndicesPtr> & indices,
+		const std::multimap<int, Link> & links,
+		float maxCorrespondenceDistance,
+		double minOverlap,
+		double alpha,
+		double beta,
+		cv::Mat_<double> & gains,
+		std::map<int, int> & idToIndex)
+{
+	UDEBUG("Exposure compensation...");
+
+	UASSERT(maxCorrespondenceDistance > 0.0f);
+	UASSERT(indices.size() == 0 || clouds.size() == indices.size());
+
+	const int num_images = static_cast<int>(clouds.size());
+	cv::Mat_<int> N(num_images, num_images); N.setTo(0);
+	cv::Mat_<double> I(num_images, num_images); I.setTo(0);
+
+	// make id to index map
+	idToIndex.clear();
+	std::vector<int> indexToId(clouds.size());
+	int oi=0;
+	std::map<int, std::pair<pcl::PointXYZ, pcl::PointXYZ> > boundingBoxes;
+	for(typename std::map<int, typename pcl::PointCloud<PointT>::Ptr>::const_iterator iter=clouds.begin(); iter!=clouds.end(); ++iter)
+	{
+		idToIndex.insert(std::make_pair(iter->first, oi));
+		indexToId[oi] = iter->first;
+		UASSERT(indices.empty() || uContains(indices, iter->first));
+		Eigen::Vector4f minPt(0,0,0,0);
+		Eigen::Vector4f maxPt(0,0,0,0);
+		if(indices.empty() || indices.at(iter->first)->empty())
+		{
+			N(oi,oi) = iter->second->size();
+			pcl::getMinMax3D(*iter->second, minPt, maxPt);
+		}
+		else
+		{
+			N(oi,oi) = indices.at(iter->first)->size();
+			pcl::getMinMax3D(*iter->second, *indices.at(iter->first), minPt, maxPt);
+		}
+		minPt[0] -= maxCorrespondenceDistance;
+		minPt[1] -= maxCorrespondenceDistance;
+		minPt[2] -= maxCorrespondenceDistance;
+		maxPt[0] += maxCorrespondenceDistance;
+		maxPt[1] += maxCorrespondenceDistance;
+		maxPt[2] += maxCorrespondenceDistance;
+		boundingBoxes.insert(std::make_pair(iter->first, std::make_pair(pcl::PointXYZ(minPt[0], minPt[1], minPt[2]), pcl::PointXYZ(maxPt[0], maxPt[1], maxPt[2]))));
+		++oi;
+	}
+
+	typename pcl::search::KdTree<PointT> kdtree;
+	int lastKdTreeId = 0;
+	for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
+	{
+		if(uContains(idToIndex, iter->second.from()) && uContains(idToIndex, iter->second.to()))
+		{
+			const typename pcl::PointCloud<PointT>::Ptr & cloudFrom = clouds.at(iter->second.from());
+			const typename pcl::PointCloud<PointT>::Ptr & cloudTo = clouds.at(iter->second.to());
+			if(cloudFrom->size() && cloudTo->size())
+			{
+				//Are bounding boxes intersect?
+				std::pair<pcl::PointXYZ, pcl::PointXYZ> bbMinMaxFrom = boundingBoxes.at(iter->second.from());
+				std::pair<pcl::PointXYZ, pcl::PointXYZ> bbMinMaxTo = boundingBoxes.at(iter->second.to());
+				Eigen::Affine3f t = Transform::getIdentity().toEigen3f();
+				if(!iter->second.transform().isIdentity() && !iter->second.transform().isNull())
+				{
+					t = iter->second.transform().toEigen3f();
+					bbMinMaxTo.first = pcl::transformPoint(bbMinMaxTo.first, t);
+					bbMinMaxTo.second = pcl::transformPoint(bbMinMaxTo.second, t);
+				}
+				AABB bbFrom(Eigen::Vector3f((bbMinMaxFrom.second.x + bbMinMaxFrom.first.x)/2.0f, (bbMinMaxFrom.second.y + bbMinMaxFrom.first.y)/2.0f, (bbMinMaxFrom.second.z + bbMinMaxFrom.first.z)/2.0f),
+						 Eigen::Vector3f((bbMinMaxFrom.second.x - bbMinMaxFrom.first.x)/2.0f, (bbMinMaxFrom.second.y - bbMinMaxFrom.first.y)/2.0f, (bbMinMaxFrom.second.z - bbMinMaxFrom.first.z)/2.0f));
+				AABB bbTo(Eigen::Vector3f((bbMinMaxTo.second.x + bbMinMaxTo.first.x)/2.0f, (bbMinMaxTo.second.y + bbMinMaxTo.first.y)/2.0f, (bbMinMaxTo.second.z + bbMinMaxTo.first.z)/2.0f),
+						 Eigen::Vector3f((bbMinMaxTo.second.x - bbMinMaxTo.first.x)/2.0f, (bbMinMaxTo.second.y - bbMinMaxTo.first.y)/2.0f, (bbMinMaxTo.second.z - bbMinMaxTo.first.z)/2.0f));
+				//UDEBUG("%d = %f,%f,%f %f,%f,%f", iter->second.from(), bbMinMaxFrom.first[0], bbMinMaxFrom.first[1], bbMinMaxFrom.first[2], bbMinMaxFrom.second[0], bbMinMaxFrom.second[1], bbMinMaxFrom.second[2]);
+				//UDEBUG("%d = %f,%f,%f %f,%f,%f", iter->second.to(), bbMinMaxTo.first[0], bbMinMaxTo.first[1], bbMinMaxTo.first[2], bbMinMaxTo.second[0], bbMinMaxTo.second[1], bbMinMaxTo.second[2]);
+				if(testAABBAABB(bbFrom, bbTo))
+				{
+					if(lastKdTreeId <= 0 || lastKdTreeId!=iter->second.from())
+					{
+						//reconstruct kdtree
+						if(indices.size() && indices.at(iter->second.from())->size())
+						{
+							kdtree.setInputCloud(cloudFrom, indices.at(iter->second.from()));
+						}
+						else
+						{
+							kdtree.setInputCloud(cloudFrom);
+						}
+					}
+
+					pcl::Correspondences correspondences;
+					pcl::IndicesPtr indicesTo(new std::vector<int>);
+					std::set<int> addedFrom;
+					if(indices.size() && indices.at(iter->second.to())->size())
+					{
+						const pcl::IndicesPtr & indicesTo = indices.at(iter->second.to());
+						correspondences.resize(indicesTo->size());
+						int oi=0;
+						for(unsigned int i=0; i<indicesTo->size(); ++i)
+						{
+							std::vector<int> k_indices;
+							std::vector<float> k_sqr_distances;
+							if(kdtree.radiusSearch(pcl::transformPoint(cloudTo->at(indicesTo->at(i)), t), maxCorrespondenceDistance, k_indices, k_sqr_distances, 1))
+							{
+								if(addedFrom.find(k_indices[0]) == addedFrom.end())
+								{
+									correspondences[oi].index_match = k_indices[0];
+									correspondences[oi].index_query = indicesTo->at(i);
+									correspondences[oi].distance = k_sqr_distances[0];
+									addedFrom.insert(k_indices[0]);
+									++oi;
+								}
+							}
+						}
+						correspondences.resize(oi);
+					}
+					else
+					{
+						correspondences.resize(cloudTo->size());
+						int oi=0;
+						for(unsigned int i=0; i<cloudTo->size(); ++i)
+						{
+							std::vector<int> k_indices;
+							std::vector<float> k_sqr_distances;
+							if(kdtree.radiusSearch(pcl::transformPoint(cloudTo->at(i), t), maxCorrespondenceDistance, k_indices, k_sqr_distances, 1))
+							{
+								if(addedFrom.find(k_indices[0]) == addedFrom.end())
+								{
+									correspondences[oi].index_match = k_indices[0];
+									correspondences[oi].index_query = i;
+									correspondences[oi].distance = k_sqr_distances[0];
+									addedFrom.insert(k_indices[0]);
+									++oi;
+								}
+							}
+						}
+						correspondences.resize(oi);
+					}
+
+					UDEBUG("%d->%d: correspondences = %d", iter->second.from(), iter->second.to(), (int)correspondences.size());
+					if((minOverlap <= 0.0 && correspondences.size()) ||
+							(double(correspondences.size()) / double(clouds.at(iter->second.from())->size()) >= minOverlap &&
+							 double(correspondences.size()) / double(clouds.at(iter->second.to())->size()) >= minOverlap))
+					{
+						int i = idToIndex.at(iter->second.from());
+						int j = idToIndex.at(iter->second.to());
+
+						double Isum1 = 0, Isum2 = 0;
+						for (unsigned int c = 0; c < correspondences.size(); ++c)
+						{
+							const PointT & pt1 = cloudFrom->at(correspondences.at(c).index_match);
+							const PointT & pt2 = cloudTo->at(correspondences.at(c).index_query);
+
+							Isum1 += std::sqrt(static_cast<double>(sqr(pt1.r) + sqr(pt1.g) + sqr(pt1.b)));
+							Isum2 += std::sqrt(static_cast<double>(sqr(pt2.r) + sqr(pt2.g) + sqr(pt2.b)));
+						}
+						N(i, j) = N(j, i) = correspondences.size();
+						I(i, j) = Isum1 / N(i, j);
+						I(j, i) = Isum2 / N(i, j);
+					}
+				}
+			}
+		}
+	}
+
+	cv::Mat_<double> A(num_images, num_images); A.setTo(0);
+	cv::Mat_<double> b(num_images, 1); b.setTo(0);
+	for (int i = 0; i < num_images; ++i)
+	{
+		for (int j = 0; j < num_images; ++j)
+		{
+			b(i, 0) += beta * N(i, j);
+			A(i, i) += beta * N(i, j);
+			if (j == i) continue;
+			A(i, i) += 2 * alpha * I(i, j) * I(i, j) * N(i, j);
+			A(i, j) -= 2 * alpha * I(i, j) * I(j, i) * N(i, j);
+		}
+	}
+
+	gains = cv::Mat_<double>();
+	cv::solve(A, b, gains);
+	if(ULogger::kDebug)
+	{
+		for(int i=0; i<gains.rows; ++i)
+		{
+			UDEBUG("Gain index=%d (id=%d) = %f", i, indexToId[i], gains.row(i)[0]);
+		}
+	}
+}
+
+void GainCompensator::feed(
+		const std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds,
+		const std::map<int, pcl::IndicesPtr> & indices,
+		const std::multimap<int, Link> & links)
+{
+	feedImpl<pcl::PointXYZRGB>(clouds, indices, links, maxCorrespondenceDistance_, minOverlap_, alpha_, beta_, gains_, idToIndex_);
+}
+void GainCompensator::feed(
+		const std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> & clouds,
+		const std::map<int, pcl::IndicesPtr> & indices,
+		const std::multimap<int, Link> & links)
+{
+	feedImpl<pcl::PointXYZRGBNormal>(clouds, indices, links, maxCorrespondenceDistance_, minOverlap_, alpha_, beta_, gains_, idToIndex_);
+}
+void GainCompensator::feed(
+		const std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::IndicesPtr> > & cloudsIndices,
+		const std::multimap<int, Link> & links)
+{
+	std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> clouds;
+	std::map<int, pcl::IndicesPtr> indices;
+	for(std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::IndicesPtr> >::const_iterator iter=cloudsIndices.begin(); iter!=cloudsIndices.end(); ++iter)
+	{
+		clouds.insert(std::make_pair(iter->first, iter->second.first));
+		indices.insert(std::make_pair(iter->first, iter->second.second));
+	}
+	feedImpl<pcl::PointXYZRGBNormal>(clouds, indices, links, maxCorrespondenceDistance_, minOverlap_, alpha_, beta_, gains_, idToIndex_);
+}
+
+template<typename PointT>
+void applyImpl(
+		int index,
+		typename pcl::PointCloud<PointT>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const cv::Mat_<double> & gains)
+{
+	double gain = gains(index, 0);
+	UDEBUG("index=%d gain=%f", index, gain);
+	if(indices->size())
+	{
+		for(unsigned int i=0; i<indices->size(); ++i)
+		{
+			PointT & pt = cloud->at(indices->at(i));
+			pt.r = uchar(std::max(0.0, std::min(255.0, double(pt.r) * gain)));
+			pt.g = uchar(std::max(0.0, std::min(255.0, double(pt.g) * gain)));
+			pt.b = uchar(std::max(0.0, std::min(255.0, double(pt.b) * gain)));
+		}
+	}
+	else
+	{
+		for(unsigned int i=0; i<cloud->size(); ++i)
+		{
+			PointT & pt = cloud->at(i);
+			pt.r = uchar(std::max(0.0, std::min(255.0, double(pt.r) * gain)));
+			pt.g = uchar(std::max(0.0, std::min(255.0, double(pt.g) * gain)));
+			pt.b = uchar(std::max(0.0, std::min(255.0, double(pt.b) * gain)));
+		}
+	}
+}
+
+void GainCompensator::apply(
+		int id,
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud)
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	apply(id, cloud, indices);
+}
+void GainCompensator::apply(
+		int id,
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices)
+{
+	UASSERT_MSG(uContains(idToIndex_, id), uFormat("id=%d idToIndex_.size()=%d", id, (int)idToIndex_.size()).c_str());
+	applyImpl<pcl::PointXYZRGB>(idToIndex_.at(id), cloud, indices, gains_);
+}
+void GainCompensator::apply(
+		int id,
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices)
+{
+	UASSERT_MSG(uContains(idToIndex_, id), uFormat("id=%d idToIndex_.size()=%d", id, (int)idToIndex_.size()).c_str());
+	applyImpl<pcl::PointXYZRGBNormal>(idToIndex_.at(id), cloud, indices, gains_);
+}
+
+void GainCompensator::apply(
+		int id,
+		cv::Mat & image)
+{
+	UASSERT_MSG(uContains(idToIndex_, id), uFormat("id=%d idToIndex_.size()=%d", id, (int)idToIndex_.size()).c_str());
+	cv::multiply(image, gains_(idToIndex_.at(id), 0), image);
+}
+
+double GainCompensator::getGain(int id) const
+{
+	UASSERT_MSG(uContains(idToIndex_, id), uFormat("id=%d idToIndex_.size()=%d", id, (int)idToIndex_.size()).c_str());
+	return gains_(idToIndex_.at(id), 0);
+}
+
+int GainCompensator::getIndex(int id) const
+{
+	if(uContains(idToIndex_, id))
+	{
+		return idToIndex_.at(id);
+	}
+	return -1;
+}
+
+} /* namespace rtabmap */

corelib/src/Graph.cpp

@@ -73,12 +73,7 @@ bool exportPoses(
 		{
 			tmpPath+=".g2o";
 		}
-#ifdef WITH_G2O
 		return OptimizerG2O::saveGraph(tmpPath, poses, constraints, g2oRobust);
-#else
-		UERROR("Cannot export in g2o format because RTAB-Map is not built with g2o support!");
-		return false;
-#endif
 	}
 	else
 	{
@@ -1479,20 +1474,16 @@ int findNearestNode(
 			ids[oi++] = iter->first;
 		}
 
-		std::map<int, float> foundNodes;
-		if(cloud->size())
+		pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
+		kdTree->setInputCloud(cloud);
+		std::vector<int> ind;
+		std::vector<float> dist;
+		pcl::PointXYZ pt(targetPose.x(), targetPose.y(), targetPose.z());
+		kdTree->nearestKSearch(pt, 1, ind, dist);
+		if(ind.size() && dist.size() && ind[0] >= 0)
 		{
-			pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
-			kdTree->setInputCloud(cloud);
-			std::vector<int> ind;
-			std::vector<float> dist;
-			pcl::PointXYZ pt(targetPose.x(), targetPose.y(), targetPose.z());
-			kdTree->nearestKSearch(pt, 1, ind, dist);
-			if(ind.size() && dist.size() && ind[0] >= 0)
-			{
-				UDEBUG("Nearest node = %d: %f", ids[ind[0]], dist[0]);
-				id = ids[ind[0]];
-			}
+			UDEBUG("Nearest node = %d: %f", ids[ind[0]], dist[0]);
+			id = ids[ind[0]];
 		}
 	}
 	return id;

corelib/src/Link.cpp

@@ -120,38 +120,6 @@ void Link::setVariance(double rotVariance, double transVariance) {
 	infMatrix_.at<double>(5,5) = 1.0/rotVariance;
 }
 
-void Link::setUserDataRaw(const cv::Mat & userDataRaw)
-{
-	if(!_userDataRaw.empty())
-	{
-		UWARN("Writing new user data over existing user data. This may result in data loss.");
-	}
-	_userDataRaw = userDataRaw;
-}
-
-void Link::setUserData(const cv::Mat & userData)
-{
-	if(!userData.empty() && (!_userDataCompressed.empty() || !_userDataRaw.empty()))
-	{
-		UWARN("Writing new user data over existing user data. This may result in data loss.");
-	}
-	_userDataRaw = cv::Mat();
-	_userDataCompressed = cv::Mat();
-
-	if(!userData.empty())
-	{
-		if(userData.type() == CV_8UC1) // Bytes
-		{
-			_userDataCompressed = userData; // assume compressed
-		}
-		else
-		{
-			_userDataRaw = userData;
-			_userDataCompressed = compressData2(userData);
-		}
-	}
-}
-
 void Link::uncompressUserData()
 {
 	cv::Mat dataRaw = uncompressUserDataConst();

corelib/src/OccupancyGrid.cpp

@@ -0,0 +1,931 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/OccupancyGrid.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UTimer.h>
+
+#include <pcl/io/pcd_io.h>
+
+namespace rtabmap {
+
+OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) :
+	parameters_(parameters),
+	cloudDecimation_(Parameters::defaultGridDepthDecimation()),
+	cloudMaxDepth_(Parameters::defaultGridDepthMax()),
+	cloudMinDepth_(Parameters::defaultGridDepthMin()),
+	//roiRatios_(Parameters::defaultGridDepthRoiRatios()), // initialized in parseParameters()
+	footprintLength_(Parameters::defaultGridFootprintLength()),
+	footprintWidth_(Parameters::defaultGridFootprintWidth()),
+	footprintHeight_(Parameters::defaultGridFootprintHeight()),
+	scanDecimation_(Parameters::defaultGridScanDecimation()),
+	cellSize_(Parameters::defaultGridCellSize()),
+	occupancyFromCloud_(Parameters::defaultGridFromDepth()),
+	projMapFrame_(Parameters::defaultGridMapFrameProjection()),
+	maxObstacleHeight_(Parameters::defaultGridMaxObstacleHeight()),
+	normalKSearch_(Parameters::defaultGridNormalK()),
+	maxGroundAngle_(Parameters::defaultGridMaxGroundAngle()*M_PI/180.0f),
+	clusterRadius_(Parameters::defaultGridClusterRadius()),
+	minClusterSize_(Parameters::defaultGridMinClusterSize()),
+	flatObstaclesDetected_(Parameters::defaultGridFlatObstacleDetected()),
+	minGroundHeight_(Parameters::defaultGridMinGroundHeight()),
+	maxGroundHeight_(Parameters::defaultGridMaxGroundHeight()),
+	normalsSegmentation_(Parameters::defaultGridNormalsSegmentation()),
+	grid3D_(Parameters::defaultGrid3D()),
+	groundIsObstacle_(Parameters::defaultGrid3DGroundIsObstacle()),
+	noiseFilteringRadius_(Parameters::defaultGridNoiseFilteringRadius()),
+	noiseFilteringMinNeighbors_(Parameters::defaultGridNoiseFilteringMinNeighbors()),
+	scan2dUnknownSpaceFilled_(Parameters::defaultGridScan2dUnknownSpaceFilled()),
+	scan2dMaxUnknownSpaceFilledRange_(Parameters::defaultGridScan2dMaxFilledRange()),
+	xMin_(0.0f),
+	yMin_(0.0f)
+{
+	this->parseParameters(parameters);
+}
+
+void OccupancyGrid::parseParameters(const ParametersMap & parameters)
+{
+	Parameters::parse(parameters, Parameters::kGridFromDepth(), occupancyFromCloud_);
+	Parameters::parse(parameters, Parameters::kGridDepthDecimation(), cloudDecimation_);
+	if(cloudDecimation_ == 0)
+	{
+		cloudDecimation_ = 1;
+	}
+	Parameters::parse(parameters, Parameters::kGridDepthMin(), cloudMinDepth_);
+	Parameters::parse(parameters, Parameters::kGridDepthMax(), cloudMaxDepth_);
+	Parameters::parse(parameters, Parameters::kGridFootprintLength(), footprintLength_);
+	Parameters::parse(parameters, Parameters::kGridFootprintWidth(), footprintWidth_);
+	Parameters::parse(parameters, Parameters::kGridFootprintHeight(), footprintHeight_);
+	Parameters::parse(parameters, Parameters::kGridScanDecimation(), scanDecimation_);
+	float cellSize = cellSize_;
+	if(Parameters::parse(parameters, Parameters::kGridCellSize(), cellSize))
+	{
+		this->setCellSize(cellSize);
+	}
+	Parameters::parse(parameters, Parameters::kGridMapFrameProjection(), projMapFrame_);
+	Parameters::parse(parameters, Parameters::kGridMaxObstacleHeight(), maxObstacleHeight_);
+	Parameters::parse(parameters, Parameters::kGridMinGroundHeight(), minGroundHeight_);
+	Parameters::parse(parameters, Parameters::kGridMaxGroundHeight(), maxGroundHeight_);
+	if(maxGroundHeight_ > 0 &&
+		maxObstacleHeight_ > 0 &&
+		maxObstacleHeight_ < maxGroundHeight_)
+	{
+		UWARN("\"%s\" should be lower than \"%s\", setting \"%s\" to 0 (disabled).",
+				Parameters::kGridMaxGroundHeight().c_str(),
+				Parameters::kGridMaxObstacleHeight().c_str(),
+				Parameters::kGridMaxObstacleHeight().c_str());
+		maxObstacleHeight_ = 0;
+	}
+	if(maxGroundHeight_ > 0 &&
+			minGroundHeight_ > 0 &&
+		maxGroundHeight_ < minGroundHeight_)
+	{
+		UWARN("\"%s\" should be lower than \"%s\", setting \"%s\" to 0 (disabled).",
+				Parameters::kGridMinGroundHeight().c_str(),
+				Parameters::kGridMaxGroundHeight().c_str(),
+				Parameters::kGridMinGroundHeight().c_str());
+		minGroundHeight_ = 0;
+	}
+	Parameters::parse(parameters, Parameters::kGridNormalK(), normalKSearch_);
+	if(Parameters::parse(parameters, Parameters::kGridMaxGroundAngle(), maxGroundAngle_))
+	{
+		maxGroundAngle_ *= M_PI/180.0f;
+	}
+	Parameters::parse(parameters, Parameters::kGridClusterRadius(), clusterRadius_);
+	UASSERT_MSG(clusterRadius_ > 0.0f, uFormat("Param name is \"%s\"", Parameters::kGridClusterRadius().c_str()).c_str());
+	Parameters::parse(parameters, Parameters::kGridMinClusterSize(), minClusterSize_);
+	Parameters::parse(parameters, Parameters::kGridFlatObstacleDetected(), flatObstaclesDetected_);
+	Parameters::parse(parameters, Parameters::kGridNormalsSegmentation(), normalsSegmentation_);
+	Parameters::parse(parameters, Parameters::kGrid3D(), grid3D_);
+	Parameters::parse(parameters, Parameters::kGrid3DGroundIsObstacle(), groundIsObstacle_);
+	Parameters::parse(parameters, Parameters::kGridNoiseFilteringRadius(), noiseFilteringRadius_);
+	Parameters::parse(parameters, Parameters::kGridNoiseFilteringMinNeighbors(), noiseFilteringMinNeighbors_);
+	Parameters::parse(parameters, Parameters::kGridScan2dUnknownSpaceFilled(), scan2dUnknownSpaceFilled_);
+	Parameters::parse(parameters, Parameters::kGridScan2dMaxFilledRange(), scan2dMaxUnknownSpaceFilledRange_);
+
+	// convert ROI from string to vector
+	ParametersMap::const_iterator iter;
+	if((iter=parameters.find(Parameters::kGridDepthRoiRatios())) != parameters.end())
+	{
+		std::list<std::string> strValues = uSplit(iter->second, ' ');
+		if(strValues.size() != 4)
+		{
+			ULOGGER_ERROR("The number of values must be 4 (%s=\"%s\")", iter->first.c_str(), iter->second.c_str());
+		}
+		else
+		{
+			std::vector<float> tmpValues(4);
+			unsigned int i=0;
+			for(std::list<std::string>::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter)
+			{
+				tmpValues[i] = uStr2Float(*jter);
+				++i;
+			}
+
+			if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
+				tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
+				tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
+				tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
+			{
+				roiRatios_ = tmpValues;
+			}
+			else
+			{
+				ULOGGER_ERROR("The roi ratios are not valid (%s=\"%s\")", iter->first.c_str(), iter->second.c_str());
+			}
+		}
+	}
+
+	if(maxGroundHeight_ <= 0.0f && !normalsSegmentation_)
+	{
+		UWARN("\"%s\" should be greater than 0 if not using normals "
+				"segmentation approach. Setting it to cell size (%f).",
+				Parameters::kGridMaxGroundHeight().c_str(), cellSize_);
+		maxGroundHeight_ = cellSize_;
+	}
+}
+
+void OccupancyGrid::setCellSize(float cellSize)
+{
+	UASSERT_MSG(cellSize > 0.0f, uFormat("Param name is \"%s\"", Parameters::kGridCellSize().c_str()).c_str());
+	if(cellSize_ != cellSize)
+	{
+		if(!map_.empty())
+		{
+			UWARN("Grid cell size has changed, the map is cleared!");
+		}
+		this->clear();
+		cellSize_ = cellSize;
+	}
+}
+
+void OccupancyGrid::createLocalMap(
+		const Signature & node,
+		cv::Mat & ground,
+		cv::Mat & obstacles,
+		cv::Point3f & viewPoint) const
+{
+	UDEBUG("scan channels=%d, occupancyFromCloud_=%d normalsSegmentation_=%d grid3D_=%d",
+			node.sensorData().laserScanRaw().empty()?0:node.sensorData().laserScanRaw().channels(), occupancyFromCloud_?1:0, normalsSegmentation_?1:0, grid3D_?1:0);
+
+	if(node.sensorData().laserScanRaw().channels() == 2 && !occupancyFromCloud_)
+	{
+		UDEBUG("2D laser scan");
+		//2D
+		util3d::occupancy2DFromLaserScan(
+				util3d::transformLaserScan(node.sensorData().laserScanRaw(), node.sensorData().laserScanInfo().localTransform()),
+				ground,
+				obstacles,
+				cellSize_,
+				scan2dUnknownSpaceFilled_,
+				node.sensorData().laserScanInfo().maxRange()>scan2dMaxUnknownSpaceFilledRange_?scan2dMaxUnknownSpaceFilledRange_:node.sensorData().laserScanInfo().maxRange());
+	}
+	else
+	{
+		// 3D
+		pcl::IndicesPtr indices(new std::vector<int>);
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
+		if(!occupancyFromCloud_)
+		{
+			UDEBUG("3D laser scan");
+			const Transform & t = node.sensorData().laserScanInfo().localTransform();
+			cv::Mat scan = util3d::downsample(node.sensorData().laserScanRaw(), scanDecimation_);
+			cloud = util3d::laserScanToPointCloudRGB(
+					scan,
+					t);
+
+			// update viewpoint
+			viewPoint = cv::Point3f(t.x(), t.y(), t.z());
+		}
+		else
+		{
+			UDEBUG("Depth image : decimation=%d max=%f min=%f",
+					cloudDecimation_,
+					cloudMaxDepth_,
+					cloudMinDepth_);
+			cloud = util3d::cloudRGBFromSensorData(
+					node.sensorData(),
+					cloudDecimation_,
+					cloudMaxDepth_,
+					cloudMinDepth_,
+					indices.get(),
+					parameters_,
+					roiRatios_);
+
+			// update viewpoint
+			if(node.sensorData().cameraModels().size())
+			{
+				// average of all local transforms
+				float sum = 0;
+				for(unsigned int i=0; i<node.sensorData().cameraModels().size(); ++i)
+				{
+					const Transform & t = node.sensorData().cameraModels()[i].localTransform();
+					if(!t.isNull())
+					{
+						viewPoint.x += t.x();
+						viewPoint.y += t.y();
+						viewPoint.z += t.z();
+						sum += 1.0f;
+					}
+				}
+				if(sum > 0.0f)
+				{
+					viewPoint.x /= sum;
+					viewPoint.y /= sum;
+					viewPoint.z /= sum;
+				}
+			}
+			else
+			{
+				const Transform & t = node.sensorData().stereoCameraModel().localTransform();
+				viewPoint = cv::Point3f(t.x(), t.y(), t.z());
+			}
+		}
+
+		if(cloud->size())
+		{
+			pcl::IndicesPtr groundIndices(new std::vector<int>);
+			pcl::IndicesPtr obstaclesIndices(new std::vector<int>);
+			cloud = this->segmentCloud<pcl::PointXYZRGB>(
+					cloud,
+					indices,
+					node.getPose(),
+					viewPoint,
+					groundIndices,
+					obstaclesIndices);
+
+			if(!groundIndices->empty() || !obstaclesIndices->empty())
+			{
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+
+				if(groundIndices->size())
+				{
+					pcl::copyPointCloud(*cloud, *groundIndices, *groundCloud);
+				}
+
+				if(obstaclesIndices->size())
+				{
+					pcl::copyPointCloud(*cloud, *obstaclesIndices, *obstaclesCloud);
+				}
+
+				if(grid3D_)
+				{
+					UDEBUG("");
+					if(groundIsObstacle_)
+					{
+						*obstaclesCloud += *groundCloud;
+						groundCloud->clear();
+					}
+
+					// transform back in base frame
+					float roll, pitch, yaw;
+					node.getPose().getEulerAngles(roll, pitch, yaw);
+					Transform tinv = Transform(0,0, projMapFrame_?node.getPose().z():0, roll, pitch, 0).inverse();
+					ground = util3d::laserScanFromPointCloud(*groundCloud, tinv);
+					obstacles = util3d::laserScanFromPointCloud(*obstaclesCloud, tinv);
+				}
+				else
+				{
+					UDEBUG("groundCloud=%d, obstaclesCloud=%d", (int)groundCloud->size(), (int)obstaclesCloud->size());
+					// projection on the xy plane
+					util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGB>(
+							groundCloud,
+							obstaclesCloud,
+							ground,
+							obstacles,
+							cellSize_);
+				}
+			}
+		}
+	}
+	UDEBUG("ground=%d obstacles=%d channels=%d", ground.cols, obstacles.cols, ground.cols?ground.channels():obstacles.channels());
+}
+
+void OccupancyGrid::clear()
+{
+	cache_.clear();
+	map_ = cv::Mat();
+	mapInfo_ = cv::Mat();
+	cellCount_.clear();
+	xMin_ = 0.0f;
+	yMin_ = 0.0f;
+	addedNodes_.clear();
+}
+
+void OccupancyGrid::addToCache(
+		int nodeId,
+		const cv::Mat & ground,
+		const cv::Mat & obstacles)
+{
+	UDEBUG("nodeId=%d", nodeId);
+	cache_.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
+}
+
+void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMapSize, float footprintRadius)
+{
+	UTimer timer;
+	UDEBUG("Update (poses=%d addedNodes_=%d)", (int)posesIn.size(), (int)addedNodes_.size());
+
+	float margin = cellSize_*10.0f+footprintRadius;
+
+	float minX=-minMapSize/2.0f;
+	float minY=-minMapSize/2.0f;
+	float maxX=minMapSize/2.0f;
+	float maxY=minMapSize/2.0f;
+	bool undefinedSize = minMapSize == 0.0f;
+	std::map<int, cv::Mat> emptyLocalMaps;
+	std::map<int, cv::Mat> occupiedLocalMaps;
+
+	// First, check of the graph has changed. If so, re-create the octree by moving all occupied nodes.
+	bool graphChanged = false;
+	std::map<int, Transform> transforms;
+	for(std::map<int, Transform>::iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter)
+	{
+		std::map<int, Transform>::const_iterator jter = posesIn.find(iter->first);
+		if(jter != posesIn.end())
+		{
+			UASSERT(!iter->second.isNull() && !jter->second.isNull());
+			Transform t = Transform::getIdentity();
+			if(iter->second.getDistanceSquared(jter->second) > 0.0001)
+			{
+				t = jter->second * iter->second.inverse();
+				graphChanged = true;
+			}
+			transforms.insert(std::make_pair(jter->first, t));
+
+			float x = jter->second.x();
+			float y  =jter->second.y();
+			if(undefinedSize)
+			{
+				minX = maxX = x;
+				minY = maxY = y;
+				undefinedSize = false;
+			}
+			else
+			{
+				if(minX > x)
+					minX = x;
+				else if(maxX < x)
+					maxX = x;
+
+				if(minY > y)
+					minY = y;
+				else if(maxY < y)
+					maxY = y;
+			}
+		}
+		else
+		{
+			UDEBUG("Updated pose for node %d is not found, some points may not be copied if graph has changed.", iter->first);
+		}
+	}
+
+	if(graphChanged && !map_.empty())
+	{
+		UINFO("Graph changed!");
+
+		// 1) recreate all local maps
+		UASSERT(map_.cols == mapInfo_.cols &&
+				map_.rows == mapInfo_.rows);
+		std::map<int, std::pair<int, int> > tmpIndices;
+		for(std::map<int, std::pair<int, int> >::iterator iter=cellCount_.begin(); iter!=cellCount_.end(); ++iter)
+		{
+			if(iter->second.first)
+			{
+				emptyLocalMaps.insert(std::make_pair( iter->first, cv::Mat(1, iter->second.first, CV_32FC2)));
+			}
+			if(iter->second.second)
+			{
+				occupiedLocalMaps.insert(std::make_pair( iter->first, cv::Mat(1, iter->second.second, CV_32FC2)));
+			}
+			tmpIndices.insert(std::make_pair(iter->first, std::make_pair(0,0)));
+		}
+		for(int y=1; y<map_.rows-1; ++y)
+		{
+			for(int x=1; x<map_.cols-1; ++x)
+			{
+				float * info = mapInfo_.ptr<float>(y,x);
+				int nodeId = (int)info[0];
+				if(nodeId > 0 && map_.at<char>(y,x) >= 0)
+				{
+					std::map<int, Transform>::iterator tter = transforms.find(nodeId);
+					if(tter != transforms.end() && !uContains(cache_, nodeId))
+					{
+						cv::Point3f pt(info[1], info[2], 0.0f);
+						pt = util3d::transformPoint(pt, tter->second);
+
+						if(minX > pt.x)
+							minX = pt.x;
+						else if(maxX < pt.x)
+							maxX = pt.x;
+
+						if(minY > pt.y)
+							minY = pt.y;
+						else if(maxY < pt.y)
+							maxY = pt.y;
+
+						std::map<int, std::pair<int, int> >::iterator jter = tmpIndices.find(nodeId);
+						if(map_.at<char>(y, x) == 0)
+						{
+							// ground
+							std::map<int, cv::Mat>::iterator iter = emptyLocalMaps.find(nodeId);
+							UASSERT(iter != emptyLocalMaps.end());
+							UASSERT(jter->second.first < iter->second.cols);
+							float * ptf = iter->second.ptr<float>(0,jter->second.first++);
+							ptf[0] = pt.x;
+							ptf[1] = pt.y;
+						}
+						else
+						{
+							// obstacle
+							std::map<int, cv::Mat>::iterator iter = occupiedLocalMaps.find(nodeId);
+							UASSERT(iter != occupiedLocalMaps.end());
+							UASSERT(iter!=occupiedLocalMaps.end());
+							UASSERT(jter->second.second < iter->second.cols);
+							float * ptf = iter->second.ptr<float>(0,jter->second.second++);
+							ptf[0] = pt.x;
+							ptf[1] = pt.y;
+						}
+					}
+				}
+			}
+		}
+
+		UDEBUG("min (%f,%f) max(%f,%f)", minX, minY, maxX, maxY);
+		addedNodes_.clear();
+		map_ = cv::Mat();
+		mapInfo_ = cv::Mat();
+		cellCount_.clear();
+		xMin_ = 0.0f;
+		yMin_ = 0.0f;
+	}
+	else if(!map_.empty())
+	{
+		// update
+		minX=xMin_+margin;
+		minY=yMin_+margin;
+		maxX=xMin_+float(map_.cols)*cellSize_ - margin;
+		maxY=yMin_+float(map_.rows)*cellSize_ - margin;
+		undefinedSize = false;
+	}
+
+	std::list<std::pair<int, Transform> > poses;
+	// place negative poses at the end
+	for(std::map<int, Transform>::const_reverse_iterator iter = posesIn.rbegin(); iter!=posesIn.rend(); ++iter)
+	{
+		if(iter->first>0)
+		{
+			poses.push_front(*iter);
+		}
+		else
+		{
+			poses.push_back(*iter);
+		}
+	}
+	for(std::list<std::pair<int, Transform> >::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+	{
+		UASSERT(!iter->second.isNull());
+
+		float x = iter->second.x();
+		float y  =iter->second.y();
+		if(undefinedSize)
+		{
+			minX = maxX = x;
+			minY = maxY = y;
+			undefinedSize = false;
+		}
+		else
+		{
+			if(minX > x)
+				minX = x;
+			else if(maxX < x)
+				maxX = x;
+
+			if(minY > y)
+				minY = y;
+			else if(maxY < y)
+				maxY = y;
+		}
+	}
+
+	if(!cache_.empty())
+	{
+		for(std::list<std::pair<int, Transform> >::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+		{
+			if(uContains(cache_, iter->first))
+			{
+				const std::pair<cv::Mat, cv::Mat> & pair = cache_.at(iter->first);
+
+				//ground
+				if(pair.first.cols)
+				{
+					if(pair.first.rows > 1 && pair.first.cols == 1)
+					{
+						UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.first.rows, pair.first.cols);
+					}
+					cv::Mat ground(1, pair.first.cols, CV_32FC2);
+					for(int i=0; i<ground.cols; ++i)
+					{
+						const float * vi = pair.first.ptr<float>(0,i);
+						float * vo = ground.ptr<float>(0,i);
+						cv::Point3f vt;
+						if(pair.first.channels() > 2)
+						{
+							vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
+						}
+						else
+						{
+							vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
+						}
+						vo[0] = vt.x;
+						vo[1] = vt.y;
+						if(minX > vo[0])
+							minX = vo[0];
+						else if(maxX < vo[0])
+							maxX = vo[0];
+
+						if(minY > vo[1])
+							minY = vo[1];
+						else if(maxY < vo[1])
+							maxY = vo[1];
+					}
+					uInsert(emptyLocalMaps, std::make_pair(iter->first, ground));
+				}
+
+				//obstacles
+				if(pair.second.cols)
+				{
+					if(pair.second.rows > 1 && pair.second.cols == 1)
+					{
+						UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.second.rows, pair.second.cols);
+					}
+					cv::Mat obstacles(1, pair.second.cols, CV_32FC2);
+					for(int i=0; i<obstacles.cols; ++i)
+					{
+						const float * vi = pair.second.ptr<float>(0,i);
+						float * vo = obstacles.ptr<float>(0,i);
+						cv::Point3f vt;
+						if(pair.second.channels() > 2)
+						{
+							vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
+						}
+						else
+						{
+							vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
+						}
+						vo[0] = vt.x;
+						vo[1] = vt.y;
+						if(minX > vo[0])
+							minX = vo[0];
+						else if(maxX < vo[0])
+							maxX = vo[0];
+
+						if(minY > vo[1])
+							minY = vo[1];
+						else if(maxY < vo[1])
+							maxY = vo[1];
+					}
+					uInsert(occupiedLocalMaps, std::make_pair(iter->first, obstacles));
+				}
+			}
+		}
+	}
+
+	cv::Mat map;
+	cv::Mat mapInfo;
+	if(minX != maxX && minY != maxY)
+	{
+		//Get map size
+		float xMin = minX-margin;
+		float yMin = minY-margin;
+		float xMax = maxX+margin;
+		float yMax = maxY+margin;
+		if(fabs((yMax - yMin) / cellSize_) > 99999 ||
+		   fabs((xMax - xMin) / cellSize_) > 99999)
+		{
+			UERROR("Large map size!! map min=(%f, %f) max=(%f,%f). "
+					"There's maybe an error with the poses provided! The map will not be created!",
+					xMin, yMin, xMax, yMax);
+		}
+		else
+		{
+			UDEBUG("map min=(%f, %f) odlMin(%f,%f) max=(%f,%f)", xMin, yMin, xMin_, yMin_, xMax, yMax);
+			cv::Size newMapSize((xMax - xMin) / cellSize_ + 0.5f, (yMax - yMin) / cellSize_ + 0.5f);
+			if(map_.empty())
+			{
+				UDEBUG("Map empty!");
+				map = cv::Mat::ones(newMapSize, CV_8S)*-1;
+				mapInfo = cv::Mat::zeros(newMapSize, CV_32FC3);
+			}
+			else
+			{
+				if(xMin == xMin_ && yMin == yMin_ &&
+						newMapSize.width == map_.cols &&
+						newMapSize.height == map_.rows)
+				{
+					// same map size and origin, don't do anything
+					UDEBUG("Map same size!");
+					map = map_;
+					mapInfo = mapInfo_;
+				}
+				else
+				{
+					UDEBUG("Copy map");
+					// copy the old map in the new map
+					// make sure the translation is cellSize
+					int deltaX = 0;
+					if(xMin < xMin_)
+					{
+						deltaX = (xMin_ - xMin) / cellSize_ + 1.0f;
+						xMin = xMin_-float(deltaX)*cellSize_;
+					}
+					int deltaY = 0;
+					if(yMin < yMin_)
+					{
+						deltaY = (yMin_ - yMin) / cellSize_ + 1.0f;
+						yMin = yMin_-float(deltaY)*cellSize_;
+					}
+					UDEBUG("deltaX=%d, deltaY=%d", deltaX, deltaY);
+					newMapSize.width = (xMax - xMin) / cellSize_ + 0.5f;
+					newMapSize.height = (yMax - yMin) / cellSize_ + 0.5f;
+					map = cv::Mat::ones(newMapSize, CV_8S)*-1;
+					mapInfo = cv::Mat::zeros(newMapSize, mapInfo_.type());
+					map_.copyTo(map(cv::Rect(deltaX, deltaY, map_.cols, map_.rows)));
+					mapInfo_.copyTo(mapInfo(cv::Rect(deltaX, deltaY, map_.cols, map_.rows)));
+				}
+			}
+			UASSERT(map.cols == mapInfo.cols && map.rows == mapInfo.rows);
+			UDEBUG("map %d %d", map.cols, map.rows);
+			if(poses.size())
+			{
+				UDEBUG("first pose= %d last pose=%d", poses.begin()->first, poses.rbegin()->first);
+			}
+			for(std::list<std::pair<int, Transform> >::const_iterator kter = poses.begin(); kter!=poses.end(); ++kter)
+			{
+				if(kter->first > 0)
+				{
+					uInsert(addedNodes_, *kter);
+				}
+				std::map<int, cv::Mat >::iterator iter = emptyLocalMaps.find(kter->first);
+				std::map<int, cv::Mat >::iterator jter = occupiedLocalMaps.find(kter->first);
+				std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(kter->first);
+				if(cter == cellCount_.end() && kter->first > 0)
+				{
+					cter = cellCount_.insert(std::make_pair(kter->first, std::pair<int,int>(0,0))).first;
+				}
+				if(iter!=emptyLocalMaps.end())
+				{
+					for(int i=0; i<iter->second.cols; ++i)
+					{
+						float * ptf = iter->second.ptr<float>(0,i);
+						cv::Point2i pt((ptf[0]-xMin)/cellSize_ + 0.5f, (ptf[1]-yMin)/cellSize_ + 0.5f);
+						UASSERT_MSG(pt.y < map.rows && pt.x < map.cols,
+								uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d",
+										kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, iter->second.channels(), mapInfo.channels()-1).c_str());
+						char & value = map.at<char>(pt.y, pt.x);
+						if(value != -2)
+						{
+							float * info = mapInfo.ptr<float>(pt.y, pt.x);
+							int nodeId = (int)info[0];
+							if(value != -1)
+							{
+								if(kter->first > 0 && (kter->first <  nodeId || nodeId < 0))
+								{
+									// cannot rewrite on cells referred by more recent nodes
+									continue;
+								}
+								if(nodeId > 0)
+								{
+									std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
+									UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
+									if(value == 0)
+									{
+										eter->second.first -= 1;
+									}
+									else if(value == 100)
+									{
+										eter->second.second -= 1;
+									}
+									if(kter->first < 0)
+									{
+										eter->second.first += 1;
+									}
+								}
+							}
+							if(kter->first > 0)
+							{
+								info[0] = (float)kter->first;
+								info[1] = ptf[0];
+								info[2] = ptf[1];
+								cter->second.first+=1;
+							}
+							value = 0; // free space
+						}
+					}
+				}
+
+				if(footprintRadius >= cellSize_*1.5f)
+				{
+					// place free space under the footprint of the robot
+					cv::Point2i ptBegin((kter->second.x()-footprintRadius-xMin)/cellSize_ + 0.5f, (kter->second.y()-footprintRadius-yMin)/cellSize_ + 0.5f);
+					cv::Point2i ptEnd((kter->second.x()+footprintRadius-xMin)/cellSize_ + 0.5f, (kter->second.y()+footprintRadius-yMin)/cellSize_ + 0.5f);
+					if(ptBegin.x < 0)
+						ptBegin.x = 0;
+					if(ptEnd.x >= map.cols)
+						ptEnd.x = map.cols-1;
+
+					if(ptBegin.y < 0)
+						ptBegin.y = 0;
+					if(ptEnd.y >= map.rows)
+						ptEnd.y = map.rows-1;
+					for(int i=ptBegin.x; i<ptEnd.x; ++i)
+					{
+						for(int j=ptBegin.y; j<ptEnd.y; ++j)
+						{
+							UASSERT(j < map.rows && i < map.cols);
+							char & value = map.at<char>(j, i);
+							float * info = mapInfo.ptr<float>(j, i);
+							int nodeId = (int)info[0];
+							if(value != -1)
+							{
+								if(kter->first > 0 && (kter->first <  nodeId || nodeId < 0))
+								{
+									// cannot rewrite on cells referred by more recent nodes
+									continue;
+								}
+								if(nodeId>0)
+								{
+									std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
+									UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
+									if(value == 0)
+									{
+										eter->second.first -= 1;
+									}
+									else if(value == 100)
+									{
+										eter->second.second -= 1;
+									}
+									if(kter->first < 0)
+									{
+										eter->second.first += 1;
+									}
+								}
+							}
+							if(kter->first > 0)
+							{
+								info[0] = (float)kter->first;
+								info[1] = float(i) * cellSize_ + xMin_ + 0.5f;
+								info[2] = float(j) * cellSize_ + yMin_ + 0.5f;
+								cter->second.first+=1;
+							}
+							value = -2; // free space (footprint)
+						}
+					}
+				}
+
+				if(jter!=occupiedLocalMaps.end())
+				{
+					for(int i=0; i<jter->second.cols; ++i)
+					{
+						float * ptf = jter->second.ptr<float>(0,i);
+						cv::Point2i pt((ptf[0]-xMin)/cellSize_ + 0.5f, (ptf[1]-yMin)/cellSize_ + 0.5f);
+						UASSERT_MSG(pt.y < map.rows && pt.x < map.cols,
+									uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d",
+											kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, jter->second.channels(), mapInfo.channels()-1).c_str());
+						char & value = map.at<char>(pt.y, pt.x);
+						if(value != -2)
+						{
+							float * info = mapInfo.ptr<float>(pt.y, pt.x);
+							int nodeId = (int)info[0];
+							if(value != -1)
+							{
+								if(kter->first > 0 && (kter->first <  nodeId || nodeId < 0))
+								{
+									// cannot rewrite on cells referred by more recent nodes
+									continue;
+								}
+								if(nodeId>0)
+								{
+									std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
+									UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
+									if(value == 0)
+									{
+										eter->second.first -= 1;
+									}
+									else if(value == 100)
+									{
+										eter->second.second -= 1;
+									}
+									if(kter->first < 0)
+									{
+										eter->second.second += 1;
+									}
+								}
+							}
+							if(kter->first > 0)
+							{
+								info[0] = (float)kter->first;
+								info[1] = ptf[0];
+								info[2] = ptf[1];
+								cter->second.second+=1;
+							}
+							value = 100; // obstacles
+						}
+					}
+				}
+			}
+
+			// fill holes and put footprint values to empty (0)
+			//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+			//cloud->resize(map.rows*map.cols);
+			//int oi=0;
+			for(int i=1; i<map.rows-1; ++i)
+			{
+				for(int j=1; j<map.cols-1; ++j)
+				{
+					char & value = map.at<char>(i, j);
+					if(value == -2)
+					{
+						value = 0;
+					}
+
+					char sum =  (map.at<char>(i+1, j) != -1?1:0) +
+								(map.at<char>(i-1, j) != -1?1:0) +
+								(map.at<char>(i, j+1) != -1?1:0) +
+								(map.at<char>(i, j-1) != -1?1:0);
+					if(value == -1 && sum >=3)
+					{
+						value = 0;
+					}
+
+					//float * info = mapInfo.ptr<float>(i,j);
+					//if(info[0] > 0)
+					//{
+					//	cloud->at(oi).x = info[1];
+					//	cloud->at(oi).y = info[2];
+					//	oi++;
+					//}
+				}
+			}
+			//if(graphChanged)
+			//{
+			//	cloud->resize(oi);
+			//	pcl::io::savePCDFileBinary("mapInfo.pcd", *cloud);
+			//	UWARN("Saved mapInfo.pcd");
+			//}
+
+			map_ = map;
+			mapInfo_ = mapInfo;
+			xMin_ = xMin;
+			yMin_ = yMin;
+
+			// clean cellCount_
+			for(std::map<int, std::pair<int, int> >::iterator iter= cellCount_.begin(); iter!=cellCount_.end();)
+			{
+				UASSERT(iter->second.first >= 0 && iter->second.second >= 0);
+				if(iter->second.first == 0 && iter->second.second == 0)
+				{
+					cellCount_.erase(iter++);
+				}
+				else
+				{
+					++iter;
+				}
+			}
+		}
+	}
+
+	cache_.clear();
+
+	UDEBUG("Occupancy Grid update time = %f s", timer.ticks());
+}
+
+}

corelib/src/OctoMap.cpp

@@ -31,12 +31,15 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/util3d_transforms.h>
 #include <rtabmap/core/util3d_filtering.h>
 #include <rtabmap/core/util3d_mapping.h>
+#include <pcl/common/transforms.h>
 
 namespace rtabmap {
 
-OctoMap::OctoMap(float voxelSize) :
-		octree_(new octomap::ColorOcTree(voxelSize))
+OctoMap::OctoMap(float voxelSize, float occupancyThr) :
+		octree_(new octomap::ColorOcTree(voxelSize)),
+		hasColor_(false)
 {
+	octree_->setOccupancyThres(occupancyThr);
 	UASSERT(voxelSize>0.0f);
 }
 
@@ -51,16 +54,32 @@ void OctoMap::clear()
 	octree_->clear();
 	occupiedCells_.clear();
 	cache_.clear();
+	cacheClouds_.clear();
+	cacheViewPoints_.clear();
 	addedNodes_.clear();
 	keyRay_ = octomap::KeyRay();
+	hasColor_ = false;
 }
 
 void OctoMap::addToCache(int nodeId,
 		pcl::PointCloud<pcl::PointXYZRGB>::Ptr & ground,
-		pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstacles)
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstacles,
+		const pcl::PointXYZ & viewPoint)
 {
+	UDEBUG("nodeId=%d", nodeId);
+	cacheClouds_.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
+	cacheViewPoints_.insert(std::make_pair(nodeId, cv::Point3f(viewPoint.x, viewPoint.y, viewPoint.z)));
+}
+void OctoMap::addToCache(int nodeId,
+		const cv::Mat & ground,
+		const cv::Mat & obstacles,
+		const cv::Point3f & viewPoint)
+{
+	UASSERT(ground.empty() || ground.type() == CV_32FC3 || ground.type() == CV_32FC(4) || ground.type() == CV_32FC(6));
+	UASSERT(obstacles.empty() || obstacles.type() == CV_32FC3 || obstacles.type() == CV_32FC(4) || obstacles.type() == CV_32FC(6));
 	UDEBUG("nodeId=%d", nodeId);
 	cache_.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
+	cacheViewPoints_.insert(std::make_pair(nodeId, viewPoint));
 }
 
 void OctoMap::update(const std::map<int, Transform> & poses)
@@ -174,12 +193,19 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 		for(std::list<std::pair<int, Transform> >::const_iterator iter=orderedPoses.begin(); iter!=orderedPoses.end(); ++iter)
 		{
 			std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> >::iterator cloudIter;
-			cloudIter = cache_.find(iter->first);
-			if(cloudIter != cache_.end())
+			std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator occupancyIter;
+			std::map<int, cv::Point3f>::iterator viewPointIter;
+			cloudIter = cacheClouds_.find(iter->first);
+			occupancyIter = cache_.find(iter->first);
+			viewPointIter = cacheViewPoints_.find(iter->first);
+			if(occupancyIter != cache_.end() || cloudIter != cacheClouds_.end())
 			{
 				UDEBUG("Adding %d to octomap (resolution=%f)", iter->first, octree_->getResolution());
 
+				UASSERT(viewPointIter != cacheViewPoints_.end());
 				octomap::point3d sensorOrigin(iter->second.x(), iter->second.y(), iter->second.z());
+				sensorOrigin += octomap::point3d(viewPointIter->second.x, viewPointIter->second.y, viewPointIter->second.z);
+
 				octomap::OcTreeKey tmpKey;
 				if (!octree_->coordToKeyChecked(sensorOrigin, tmpKey)
 						|| !octree_->coordToKeyChecked(sensorOrigin, tmpKey))
@@ -190,10 +216,21 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 				// instead of direct scan insertion, compute update to filter ground:
 				octomap::KeySet free_cells, occupied_cells, ground_cells;
 				// insert ground points only as free:
-				UDEBUG("%d: compute free cells (from %d ground points)", iter->first, (int)cloudIter->second.first->size());
-				for (unsigned int i=0; i<cloudIter->second.first->size(); ++i)
+				unsigned int maxGroundPts = occupancyIter != cache_.end()?occupancyIter->second.first.cols:cloudIter->second.first->size();
+				UDEBUG("%d: compute free cells (from %d ground points)", iter->first, (int)maxGroundPts);
+				Eigen::Affine3f t = iter->second.toEigen3f();
+				for (unsigned int i=0; i<maxGroundPts; ++i)
 				{
-					pcl::PointXYZRGB pt = util3d::transformPoint(cloudIter->second.first->at(i), iter->second);
+					pcl::PointXYZRGB pt;
+					if(occupancyIter != cache_.end())
+					{
+						pt = util3d::laserScanToPointRGB(occupancyIter->second.first, i);
+						pt = pcl::transformPoint(pt, t);
+					}
+					else
+					{
+						pt = pcl::transformPoint(cloudIter->second.first->at(i), t);
+					}
 
 					octomap::point3d point(pt.x, pt.y, pt.z);
 
@@ -211,6 +248,10 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 						octomap::ColorOcTreeNode * n = octree_->updateNode(key, false);
 						if(n)
 						{
+							if(!hasColor_ && (pt.r !=0 || pt.g != 0 || pt.b != 0))
+							{
+								hasColor_ = true;
+							}
 							octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
 							if(iter->first > 0)
 							{
@@ -226,10 +267,20 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 				UDEBUG("%d: free cells = %d", iter->first, (int)free_cells.size());
 
 				// all other points: free on ray, occupied on endpoint:
-				UDEBUG("%d: compute occupied cells (from %d obstacle points)", iter->first, (int) cloudIter->second.second->size());
-				for (unsigned int i=0; i<cloudIter->second.second->size(); ++i)
+				unsigned int maxObstaclePts = occupancyIter != cache_.end()?occupancyIter->second.second.cols:cloudIter->second.second->size();
+				UDEBUG("%d: compute occupied cells (from %d obstacle points)", iter->first, (int)maxObstaclePts);
+				for (unsigned int i=0; i<maxObstaclePts; ++i)
 				{
-					pcl::PointXYZRGB pt = util3d::transformPoint(cloudIter->second.second->at(i), iter->second);
+					pcl::PointXYZRGB pt;
+					if(occupancyIter != cache_.end())
+					{
+						pt = util3d::laserScanToPointRGB(occupancyIter->second.second, i);
+						pt = pcl::transformPoint(pt, t);
+					}
+					else
+					{
+						pt = pcl::transformPoint(cloudIter->second.second->at(i), t);
+					}
 
 					octomap::point3d point(pt.x, pt.y, pt.z);
 
@@ -247,6 +298,10 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 						octomap::ColorOcTreeNode * n = octree_->updateNode(key, true);
 						if(n)
 						{
+							if(!hasColor_ && (pt.r !=0 || pt.g != 0 || pt.b != 0))
+							{
+								hasColor_ = true;
+							}
 							octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
 							if(iter->first > 0)
 							{
@@ -298,6 +353,8 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 		}
 	}
 	cache_.clear();
+	cacheClouds_.clear();
+	cacheViewPoints_.clear();
 }
 
 void HSVtoRGB( float *r, float *g, float *b, float h, float s, float v )
@@ -382,10 +439,10 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
 	int gi=0;
 	for (octomap::ColorOcTree::iterator it = octree_->begin(treeDepth); it != octree_->end(); ++it)
 	{
-		if(octree_->isNodeOccupied(*it))
+		if(octree_->isNodeOccupied(*it) && (obstacleIndices || emptyIndices == 0))
 		{
 			octomap::point3d pt = octree_->keyToCoord(it.getKey());
-			if(octree_->getTreeDepth() == it.getDepth())
+			if(octree_->getTreeDepth() == it.getDepth() && hasColor_)
 			{
 				(*cloud)[oi]  = pcl::PointXYZRGB(it->getColor().r, it->getColor().g, it->getColor().b);
 			}
@@ -408,7 +465,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
 			}
 			++oi;
 		}
-		else
+		else if(emptyIndices || obstacleIndices == 0)
 		{
 			octomap::point3d pt = octree_->keyToCoord(it.getKey());
 			(*cloud)[oi]  = pcl::PointXYZRGB(it->getColor().r, it->getColor().g, it->getColor().b);
@@ -475,14 +532,14 @@ cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCel
 		ground = util3d::voxelize(ground, gridCellSize);
 	}
 
-	cv::Mat obstaclesMat = cv::Mat((int)obstacles->size(), 1, CV_32FC2);
+	cv::Mat obstaclesMat = cv::Mat(1, (int)obstacles->size(), CV_32FC2);
 	for(unsigned int i=0;i<obstacles->size(); ++i)
 	{
 		obstaclesMat.at<cv::Vec2f>(i)[0] = obstacles->at(i).x;
 		obstaclesMat.at<cv::Vec2f>(i)[1] = obstacles->at(i).y;
 	}
 
-	cv::Mat groundMat = cv::Mat((int)ground->size(), 1, CV_32FC2);
+	cv::Mat groundMat = cv::Mat(1, (int)ground->size(), CV_32FC2);
 	for(unsigned int i=0;i<ground->size(); ++i)
 	{
 		groundMat.at<cv::Vec2f>(i)[0] = ground->at(i).x;

corelib/src/Odometry.cpp

@@ -61,7 +61,7 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet
 		break;
 	default:
 		odometry = new OdometryF2M(parameters);
-		type = Odometry::kTypeLocalMap;
+		type = Odometry::kTypeF2M;
 		break;
 	}
 	return odometry;
@@ -108,7 +108,10 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kOdomKalmanMeasurementNoise(), _kalmanMeasurementNoise);
 	Parameters::parse(parameters, Parameters::kOdomImageDecimation(), _imageDecimation);
 	Parameters::parse(parameters, Parameters::kOdomAlignWithGround(), _alignWithGround);
-	UASSERT(_imageDecimation>=1);
+	if(_imageDecimation == 0)
+	{
+		_imageDecimation = 1;
+	}
 
 	if(_filteringStrategy == 2)
 	{
@@ -200,69 +203,67 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
 
 Transform Odometry::process(SensorData & data, const Transform & guessIn, OdometryInfo * info)
 {
-	UASSERT(!data.imageRaw().empty());
-
 	// Ground alignment
 	if(_pose.isIdentity() && _alignWithGround)
 	{
-		UTimer alignTimer;
-		pcl::IndicesPtr indices(new std::vector<int>);
-		pcl::IndicesPtr ground, obstacles;
-		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(data, 1, 0, 0, indices.get());
-		cloud = util3d::voxelize(cloud, indices, 0.01);
-		bool success = false;
-		if(cloud->size())
+		if(data.depthOrRightRaw().empty())
 		{
-			util3d::segmentObstaclesFromGround<pcl::PointXYZ>(cloud, ground, obstacles, 20, M_PI/4.0f, 0.02, 200, true);
-			if(ground->size())
+			UWARN("\"%s\" is true but the input has no depth information, ignoring alignment with ground...", Parameters::kOdomAlignWithGround().c_str());
+		}
+		else
+		{
+			UTimer alignTimer;
+			pcl::IndicesPtr indices(new std::vector<int>);
+			pcl::IndicesPtr ground, obstacles;
+			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(data, 1, 0, 0, indices.get());
+			cloud = util3d::voxelize(cloud, indices, 0.01);
+			bool success = false;
+			if(cloud->size())
 			{
-				pcl::ModelCoefficients coefficients;
-				util3d::extractPlane(cloud, ground, 0.02, 100, &coefficients);
-				if(coefficients.values.at(3) >= 0)
+				util3d::segmentObstaclesFromGround<pcl::PointXYZ>(cloud, ground, obstacles, 20, M_PI/4.0f, 0.02, 200, true);
+				if(ground->size())
 				{
-					UWARN("Ground detected! coefficients=(%f, %f, %f, %f) time=%fs",
-							coefficients.values.at(0),
-							coefficients.values.at(1),
-							coefficients.values.at(2),
-							coefficients.values.at(3),
-							alignTimer.ticks());
+					pcl::ModelCoefficients coefficients;
+					util3d::extractPlane(cloud, ground, 0.02, 100, &coefficients);
+					if(coefficients.values.at(3) >= 0)
+					{
+						UWARN("Ground detected! coefficients=(%f, %f, %f, %f) time=%fs",
+								coefficients.values.at(0),
+								coefficients.values.at(1),
+								coefficients.values.at(2),
+								coefficients.values.at(3),
+								alignTimer.ticks());
+					}
+					else
+					{
+						UWARN("Ceiling detected! coefficients=(%f, %f, %f, %f) time=%fs",
+								coefficients.values.at(0),
+								coefficients.values.at(1),
+								coefficients.values.at(2),
+								coefficients.values.at(3),
+								alignTimer.ticks());
+					}
+					Eigen::Vector3f n(coefficients.values.at(0), coefficients.values.at(1), coefficients.values.at(2));
+					Eigen::Vector3f z(0,0,1);
+					//get rotation from z to n;
+					Eigen::Matrix3f R;
+					R = Eigen::Quaternionf().setFromTwoVectors(n,z);
+					Transform rotation(
+							R(0,0), R(0,1), R(0,2), 0,
+							R(1,0), R(1,1), R(1,2), 0,
+							R(2,0), R(2,1), R(2,2), coefficients.values.at(3));
+					_pose *= rotation;
+					success = true;
 				}
-				else
-				{
-					UWARN("Ceiling detected! coefficients=(%f, %f, %f, %f) time=%fs",
-							coefficients.values.at(0),
-							coefficients.values.at(1),
-							coefficients.values.at(2),
-							coefficients.values.at(3),
-							alignTimer.ticks());
-				}
-				Eigen::Vector3f n(coefficients.values.at(0), coefficients.values.at(1), coefficients.values.at(2));
-				Eigen::Vector3f z(0,0,1);
-				//get rotation from z to n;
-				Eigen::Matrix3f R;
-				R = Eigen::Quaternionf().setFromTwoVectors(n,z);
-				Transform rotation(
-						R(0,0), R(0,1), R(0,2), 0,
-						R(1,0), R(1,1), R(1,2), 0,
-						R(2,0), R(2,1), R(2,2), coefficients.values.at(3));
-				_pose *= rotation;
-				success = true;
+			}
+			if(!success)
+			{
+				UERROR("Odometry failed to detect the ground. You have this "
+						"error because parameter \"%s\" is true. "
+						"Make sure the camera is seeing the ground (e.g., tilt ~30 "
+						"degrees toward the ground).", Parameters::kOdomAlignWithGround().c_str());
 			}
 		}
-		if(!success)
-		{
-			UERROR("Odometry failed to detect the ground. You have this "
-					"error because parameter \"Odom/AlignWithGround\" is true. "
-					"Make sure the camera is seeing the ground (e.g., tilt ~30 "
-					"degrees toward the ground).");
-		}
-	}
-
-	if(!data.stereoCameraModel().isValidForProjection() &&
-	   (data.cameraModels().size() == 0 || !data.cameraModels()[0].isValidForProjection()))
-	{
-		UERROR("Rectified images required! Calibrate your camera.");
-		return Transform();
 	}
 
 	double dt = previousStamp_>0.0f?data.stamp() - previousStamp_:0.0;
@@ -331,7 +332,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			kpts[i].size *= _imageDecimation;
 			kpts[i].octave += log2value;
 		}
-		data.setFeatures(kpts, decimatedData.descriptors());
+		data.setFeatures(kpts, decimatedData.keypoints3D(), decimatedData.descriptors());
 
 		if(info)
 		{
@@ -495,8 +496,14 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			}
 		}
 
+		if(data.stamp() == 0)
+		{
+			UWARN("Null stamp detected");
+		}
+
 		previousStamp_ = data.stamp();
 		previousVelocityTransform_.setNull();
+
 		if(dt)
 		{
 			previousVelocityTransform_ = Transform(vx, vy, vz, vroll, vpitch, vyaw);
@@ -508,6 +515,11 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			info->distanceTravelled = distanceTravelled_;
 		}
 
+		info->varianceLin *= t.getNorm();
+		info->varianceAng *= t.getAngle();
+		info->varianceLin = info->varianceLin>0.0f?info->varianceLin:0.0001f; // epsilon if exact transform
+		info->varianceAng = info->varianceAng>0.0f?info->varianceAng:0.0001f; // epsilon if exact transform
+
 		return _pose *= t; // update
 	}
 	else if(_resetCurrentCount > 0)

corelib/src/OdometryF2F.cpp

@@ -39,12 +39,15 @@ namespace rtabmap {
 OdometryF2F::OdometryF2F(const ParametersMap & parameters) :
 	Odometry(parameters),
 	keyFrameThr_(Parameters::defaultOdomKeyFrameThr()),
+	visKeyFrameThr_(Parameters::defaultOdomVisKeyFrameThr()),
 	scanKeyFrameThr_(Parameters::defaultOdomScanKeyFrameThr())
 {
 	registrationPipeline_ = Registration::create(parameters);
 	Parameters::parse(parameters, Parameters::kOdomKeyFrameThr(), keyFrameThr_);
+	Parameters::parse(parameters, Parameters::kOdomVisKeyFrameThr(), visKeyFrameThr_);
 	Parameters::parse(parameters, Parameters::kOdomScanKeyFrameThr(), scanKeyFrameThr_);
 	UASSERT(keyFrameThr_>=0.0f && keyFrameThr_<=1.0f);
+	UASSERT(visKeyFrameThr_>=0);
 	UASSERT(scanKeyFrameThr_>=0.0f && scanKeyFrameThr_<=1.0f);
 }
 
@@ -96,9 +99,34 @@ Transform OdometryF2F::computeTransform(
 		output = registrationPipeline_->computeTransformationMod(
 				tmpRefFrame,
 				newFrame,
-				!guess.isNull()?motionSinceLastKeyFrame*guess:Transform(),
+				// special case for ICP-only odom, set guess to identity if we just started
+				!guess.isNull()?motionSinceLastKeyFrame*guess:!registrationPipeline_->isImageRequired()&&this->getPose().isIdentity()?Transform::getIdentity():Transform(),
 				&regInfo);
 
+		if(output.isNull() && !guess.isNull() && registrationPipeline_->isImageRequired())
+		{
+			tmpRefFrame = refFrame_;
+			// reset matches, but keep already extracted features in newFrame.sensorData()
+			newFrame.setWords(std::multimap<int, cv::KeyPoint>());
+			newFrame.setWords3(std::multimap<int, cv::Point3f>());
+			newFrame.setWordsDescriptors(std::multimap<int, cv::Mat>());
+			UWARN("Failed to find a transformation with the provided guess (%s), trying again without a guess.", guess.prettyPrint().c_str());
+			output = registrationPipeline_->computeTransformationMod(
+				tmpRefFrame,
+				newFrame,
+				Transform(), // null guess
+				&regInfo);
+
+			if(output.isNull())
+			{
+				UWARN("Trial with no guess still fail.");
+			}
+			else
+			{
+				UWARN("Trial with no guess succeeded.");
+			}
+		}
+
 		if(info && this->isInfoDataFilled())
 		{
 			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
@@ -128,7 +156,11 @@ Transform OdometryF2F::computeTransform(
 			{
 				info->localMap.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, t)));
 			}
+			info->localMapSize = tmpRefFrame.getWords3().size();
 			info->words = newFrame.getWords();
+
+			info->localScanMapSize = tmpRefFrame.sensorData().laserScanRaw().cols;
+			info->localScanMap = util3d::transformLaserScan(tmpRefFrame.sensorData().laserScanRaw(), t*tmpRefFrame.sensorData().laserScanInfo().localTransform());
 		}
 	}
 	else
@@ -136,7 +168,8 @@ Transform OdometryF2F::computeTransform(
 		//return Identity
 		output = Transform::getIdentity();
 		// a very high variance tells that the new pose is not linked with the previous one
-		regInfo.variance = 9999;
+		regInfo.varianceLin = 9999;
+		regInfo.varianceAng = 9999;
 	}
 
 	if(!output.isNull())
@@ -144,8 +177,12 @@ Transform OdometryF2F::computeTransform(
 		output = motionSinceLastKeyFrame.inverse() * output;
 
 		// new key-frame?
-		if( (registrationPipeline_->isImageRequired() && (keyFrameThr_ == 0 || float(regInfo.inliers) <= keyFrameThr_*float(refFrame_.sensorData().keypoints().size()))) ||
-			(registrationPipeline_->isScanRequired() && (scanKeyFrameThr_ == 0 || regInfo.icpInliersRatio <= scanKeyFrameThr_)))
+		if( (registrationPipeline_->isImageRequired() &&
+				(keyFrameThr_ == 0.0f ||
+				 visKeyFrameThr_ == 0 ||
+				 float(regInfo.inliers) <= keyFrameThr_*float(refFrame_.sensorData().keypoints().size()) ||
+				 regInfo.inliers <= visKeyFrameThr_)) ||
+			(registrationPipeline_->isScanRequired() && (scanKeyFrameThr_ == 0.0f || regInfo.icpInliersRatio <= scanKeyFrameThr_)))
 		{
 			UDEBUG("Update key frame");
 			int features = newFrame.getWordsDescriptors().size();
@@ -163,7 +200,7 @@ Transform OdometryF2F::computeTransform(
 			if((features >= registrationPipeline_->getMinVisualCorrespondences()) &&
 			   (registrationPipeline_->getMinGeometryCorrespondencesRatio()==0.0f ||
 					   (newFrame.sensorData().laserScanRaw().cols &&
-					   (newFrame.sensorData().laserScanMaxPts() == 0 || float(newFrame.sensorData().laserScanRaw().cols)/float(newFrame.sensorData().laserScanMaxPts())>=registrationPipeline_->getMinGeometryCorrespondencesRatio()))))
+					   (newFrame.sensorData().laserScanInfo().maxPoints() == 0 || float(newFrame.sensorData().laserScanRaw().cols)/float(newFrame.sensorData().laserScanInfo().maxPoints())>=registrationPipeline_->getMinGeometryCorrespondencesRatio()))))
 			{
 				refFrame_ = newFrame;
 
@@ -191,9 +228,9 @@ Transform OdometryF2F::computeTransform(
 				{
 					UWARN("Too low scan points (%d), keeping last key frame...", newFrame.sensorData().laserScanRaw().cols);
 				}
-				else if(registrationPipeline_->getMinGeometryCorrespondencesRatio()>0.0f && newFrame.sensorData().laserScanMaxPts() != 0 && float(newFrame.sensorData().laserScanRaw().cols)/float(newFrame.sensorData().laserScanMaxPts())<registrationPipeline_->getMinGeometryCorrespondencesRatio())
+				else if(registrationPipeline_->getMinGeometryCorrespondencesRatio()>0.0f && newFrame.sensorData().laserScanInfo().maxPoints() != 0 && float(newFrame.sensorData().laserScanRaw().cols)/float(newFrame.sensorData().laserScanInfo().maxPoints())<registrationPipeline_->getMinGeometryCorrespondencesRatio())
 				{
-					UWARN("Too low scan points ratio (%d < %d), keeping last key frame...", float(newFrame.sensorData().laserScanRaw().cols)/float(newFrame.sensorData().laserScanMaxPts()), registrationPipeline_->getMinGeometryCorrespondencesRatio());
+					UWARN("Too low scan points ratio (%d < %d), keeping last key frame...", float(newFrame.sensorData().laserScanRaw().cols)/float(newFrame.sensorData().laserScanInfo().maxPoints()), registrationPipeline_->getMinGeometryCorrespondencesRatio());
 				}
 			}
 		}
@@ -203,12 +240,13 @@ Transform OdometryF2F::computeTransform(
 		UWARN("Registration failed: \"%s\"", regInfo.rejectedMsg.c_str());
 	}
 
-	data.setFeatures(newFrame.sensorData().keypoints(), newFrame.sensorData().descriptors());
+	data.setFeatures(newFrame.sensorData().keypoints(), newFrame.sensorData().keypoints3D(), newFrame.sensorData().descriptors());
 
 	if(info)
 	{
 		info->type = 1;
-		info->variance = regInfo.variance;
+		info->varianceLin = regInfo.varianceLin;
+		info->varianceAng = regInfo.varianceAng;
 		info->inliers = regInfo.inliers;
 		info->icpInliersRatio = regInfo.icpInliersRatio;
 		info->matches = regInfo.matches;

corelib/src/OdometryMono.cpp

@@ -976,7 +976,8 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 		if(info)
 		{
 			// a very high variance tells that the new pose is not linked with the previous one
-			info->variance = 9999;
+			info->varianceLin = 9999;
+			info->varianceAng = 9999;
 		}
 
 			// generate kpts

corelib/src/OdometryThread.cpp

@@ -74,6 +74,11 @@ void OdometryThread::handleEvent(UEvent * event)
 	}
 }
 
+void OdometryThread::mainLoopBegin()
+{
+	ULogger::registerCurrentThread("Odometry");
+}
+
 void OdometryThread::mainLoopKill()
 {
 	_dataAdded.release();
@@ -96,14 +101,16 @@ void OdometryThread::mainLoop()
 		OdometryInfo info;
 		Transform pose = _odometry->process(data, &info);
 		// a null pose notify that odometry could not be computed
-		double variance = info.variance>0?info.variance:1;
-		this->post(new OdometryEvent(data, pose, variance, variance, info));
+		double varianceLin = info.varianceLin>0?info.varianceLin:1;
+		double varianceAng = info.varianceAng>0?info.varianceAng:1;
+		UDEBUG("Odom pose = %s", pose.prettyPrint().c_str());
+		this->post(new OdometryEvent(data, pose, varianceAng, varianceLin, info));
 	}
 }
 
 void OdometryThread::addData(const SensorData & data)
 {
-	if(dynamic_cast<OdometryMono*>(_odometry) == 0 && dynamic_cast<OdometryF2M*>(_odometry) == 0)
+	if(dynamic_cast<OdometryMono*>(_odometry) == 0)
 	{
 		if(data.imageRaw().empty() || data.depthOrRightRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValidForProjection()))
 		{
@@ -113,7 +120,7 @@ void OdometryThread::addData(const SensorData & data)
 	}
 	else
 	{
-		// Mono and BOW can accept RGB only
+		// Mono can accept RGB only
 		if(data.imageRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValidForProjection()))
 		{
 			ULOGGER_ERROR("Missing some information (image empty or missing calibration)!?");

corelib/src/Optimizer.cpp

@@ -154,56 +154,6 @@ Optimizer * Optimizer::create(Optimizer::Type type, const ParametersMap & parame
 	return optimizer;
 }
 
-Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored, double epsilon, bool robust) :
-		iterations_(iterations),
-		slam2d_(slam2d),
-		covarianceIgnored_(covarianceIgnored),
-		epsilon_(epsilon),
-		robust_(robust)
-{
-}
-
-Optimizer::Optimizer(const ParametersMap & parameters) :
-		iterations_(Parameters::defaultOptimizerIterations()),
-		slam2d_(Parameters::defaultOptimizerSlam2D()),
-		covarianceIgnored_(Parameters::defaultOptimizerVarianceIgnored()),
-		epsilon_(Parameters::defaultOptimizerEpsilon()),
-		robust_(Parameters::defaultOptimizerRobust())
-{
-	parseParameters(parameters);
-}
-
-void Optimizer::parseParameters(const ParametersMap & parameters)
-{
-	Parameters::parse(parameters, Parameters::kOptimizerIterations(), iterations_);
-	Parameters::parse(parameters, Parameters::kOptimizerVarianceIgnored(), covarianceIgnored_);
-	Parameters::parse(parameters, Parameters::kOptimizerSlam2D(), slam2d_);
-	Parameters::parse(parameters, Parameters::kOptimizerEpsilon(), epsilon_);
-	Parameters::parse(parameters, Parameters::kOptimizerRobust(), robust_);
-}
-
-std::map<int, Transform> Optimizer::optimize(
-		int rootId,
-		const std::map<int, Transform> & poses,
-		const std::multimap<int, Link> & constraints,
-		std::list<std::map<int, Transform> > * intermediateGraphes,
-		double * finalError,
-		int * iterationsDone)
-{
-	UERROR("Optimizer %d doesn't implement optimize() method.", (int)this->type());
-	return std::map<int, Transform>();
-}
-
-std::map<int, Transform> Optimizer::optimizeBA(
-		int rootId,
-		const std::map<int, Transform> & poses,
-		const std::multimap<int, Link> & links,
-		const std::map<int, Signature> & signatures)
-{
-	UERROR("Optimizer %d doesn't implement optimizeBA() method.", (int)this->type());
-	return std::map<int, Transform>();
-}
-
 void Optimizer::getConnectedGraph(
 		int fromId,
 		const std::map<int, Transform> & posesIn,
@@ -271,14 +221,149 @@ void Optimizer::getConnectedGraph(
 	}
 }
 
+Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored, double epsilon, bool robust) :
+		iterations_(iterations),
+		slam2d_(slam2d),
+		covarianceIgnored_(covarianceIgnored),
+		epsilon_(epsilon),
+		robust_(robust)
+{
+}
+
+Optimizer::Optimizer(const ParametersMap & parameters) :
+		iterations_(Parameters::defaultOptimizerIterations()),
+		slam2d_(Parameters::defaultRegForce3DoF()),
+		covarianceIgnored_(Parameters::defaultOptimizerVarianceIgnored()),
+		epsilon_(Parameters::defaultOptimizerEpsilon()),
+		robust_(Parameters::defaultOptimizerRobust())
+{
+	parseParameters(parameters);
+}
+
+void Optimizer::parseParameters(const ParametersMap & parameters)
+{
+	Parameters::parse(parameters, Parameters::kOptimizerIterations(), iterations_);
+	Parameters::parse(parameters, Parameters::kOptimizerVarianceIgnored(), covarianceIgnored_);
+	Parameters::parse(parameters, Parameters::kRegForce3DoF(), slam2d_);
+	Parameters::parse(parameters, Parameters::kOptimizerEpsilon(), epsilon_);
+	Parameters::parse(parameters, Parameters::kOptimizerRobust(), robust_);
+}
+
+std::map<int, Transform> Optimizer::optimize(
+		int rootId,
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & constraints,
+		std::list<std::map<int, Transform> > * intermediateGraphes,
+		double * finalError,
+		int * iterationsDone)
+{
+	UERROR("Optimizer %d doesn't implement optimize() method.", (int)this->type());
+	return std::map<int, Transform>();
+}
+
+std::map<int, Transform> Optimizer::optimizeBA(
+		int rootId,
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & links,
+		const std::map<int, CameraModel> & models,
+		std::map<int, cv::Point3f> & points3DMap,
+		const std::map<int, std::map<int, cv::Point3f> > & wordReferences,
+		std::set<int> * outliers)
+{
+	UERROR("Optimizer %d doesn't implement optimizeBA() method.", (int)this->type());
+	return std::map<int, Transform>();
+}
+
+std::map<int, Transform> Optimizer::optimizeBA(
+		int rootId,
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & links,
+		const std::map<int, Signature> & signatures)
+{
+	UDEBUG("");
+	std::map<int, CameraModel> models;
+	for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+	{
+		// Get camera model
+		CameraModel model;
+		if(uContains(signatures, iter->first))
+		{
+			if(signatures.at(iter->first).sensorData().cameraModels().size() == 1 && signatures.at(iter->first).sensorData().cameraModels().at(0).isValidForProjection())
+			{
+				model = signatures.at(iter->first).sensorData().cameraModels()[0];
+			}
+			else if(signatures.at(iter->first).sensorData().stereoCameraModel().isValidForProjection())
+			{
+				model = signatures.at(iter->first).sensorData().stereoCameraModel().left();
+
+				// Set Tx = -baseline*fx for stereo BA
+				model = CameraModel(
+						model.fx(),
+						model.fy(),
+						model.cx(),
+						model.cy(),
+						model.localTransform(),
+						-signatures.at(iter->first).sensorData().stereoCameraModel().baseline()*model.fx());
+			}
+			else
+			{
+				UERROR("Missing calibration for node %d", iter->first);
+				return std::map<int, Transform>();
+			}
+		}
+		else
+		{
+			UERROR("Did not find node %d in cache", iter->first);
+			return std::map<int, Transform>();
+		}
+
+		UASSERT(model.isValidForProjection());
+
+		models.insert(std::make_pair(iter->first, model));
+	}
+
+	// compute correspondences
+	std::map<int, cv::Point3f> points3DMap;
+	std::map<int, std::map<int, cv::Point3f> > wordReferences;
+	this->computeBACorrespondences(poses, links, signatures, points3DMap, wordReferences);
+
+	return optimizeBA(rootId, poses, links, models, points3DMap, wordReferences);
+}
+
+Transform Optimizer::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)
+{
+	std::map<int, Transform> poses;
+	poses.insert(std::make_pair(link.from(), Transform::getIdentity()));
+	poses.insert(std::make_pair(link.to(), link.transform()));
+	std::multimap<int, Link> links;
+	links.insert(std::make_pair(link.from(), link));
+	std::map<int, CameraModel> models;
+	models.insert(std::make_pair(link.from(), model));
+	models.insert(std::make_pair(link.to(), model));
+	poses = optimizeBA(link.from(), poses, links, models, points3DMap, wordReferences, outliers);
+	if(poses.size() == 2)
+	{
+		return poses.rbegin()->second;
+	}
+	else
+	{
+		return link.transform();
+	}
+}
 
 void Optimizer::computeBACorrespondences(
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & links,
 		const std::map<int, Signature> & signatures,
 		std::map<int, cv::Point3f> & points3DMap,
-		std::map<int, std::map<int, cv::Point2f> > & wordReferences) // <ID words, IDs frames + keypoint>
+		std::map<int, std::map<int, cv::Point3f> > & wordReferences) // <ID words, IDs frames + keypoint/depth>
 {
+	UDEBUG("");
 	int wordCount = 0;
 	int edgeWithWordsAdded = 0;
 	for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
@@ -293,49 +378,75 @@ void Optimizer::computeBACorrespondences(
 		   uContains(poses, link.from()))
 		{
 			Signature sFrom = signatures.at(link.from());
-			Signature sTo = signatures.at(link.to());
-
-			if(sFrom.getWords().size() &&
-				sTo.getWords().size() &&
-				sFrom.getWords3().size())
+			if(sFrom.getWeight() >= 0) // ignore intermediate links
 			{
-				ParametersMap regParam;
-				regParam.insert(ParametersPair(Parameters::kVisEstimationType(), "1"));
-				regParam.insert(ParametersPair(Parameters::kVisPnPReprojError(), "5"));
-				regParam.insert(ParametersPair(Parameters::kVisMinInliers(), "5"));
-				regParam.insert(ParametersPair(Parameters::kVisCorNNDR(), "0.6"));
-				RegistrationVis reg(regParam);
-
-				//sFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
-				//sTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
-
-				RegistrationInfo info;
-				Transform t = reg.computeTransformationMod(sFrom, sTo, Transform(), &info);
-				//Transform t = reg.computeTransformationMod(sFrom, sTo, iter->second.transform(), &info);
-				UDEBUG("%d->%d, inliers=%d",sFrom.id(), sTo.id(), (int)info.inliersIDs.size());
-
-				if(!t.isNull())
+				Signature sTo = signatures.at(link.to());
+				if(sTo.getWeight() < 0)
 				{
-					Transform pose = poses.at(sFrom.id());
-					for(unsigned int i=0; i<info.inliersIDs.size(); ++i)
+					for(std::multimap<int, Link>::const_iterator jter=links.find(sTo.id());
+						sTo.getWeight() < 0 && jter!=links.end() &&  uContains(signatures, jter->second.to());
+						++jter)
 					{
-						cv::Point3f p = sFrom.getWords3().lower_bound(info.inliersIDs[i])->second;
-						if(p.x > 0.0f) // make sure the point is valid
-						{
-							int wordId = ++wordCount;
-
-							p = util3d::transformPoint(p, pose);
-							points3DMap.insert(std::make_pair(wordId, p));
-							wordReferences.insert(std::make_pair(wordId, std::map<int, cv::Point2f>()));
-							wordReferences.at(wordId).insert(std::make_pair(sFrom.id(), sFrom.getWords().lower_bound(info.inliersIDs[i])->second.pt));
-							wordReferences.at(wordId).insert(std::make_pair(sTo.id(), sTo.getWords().lower_bound(info.inliersIDs[i])->second.pt));
-						}
+						sTo = signatures.at(jter->second.to());
 					}
-					++edgeWithWordsAdded;
 				}
-				else
+
+				if(sFrom.getWords().size() &&
+					sTo.getWords().size() &&
+					sFrom.getWords3().size())
 				{
-					UWARN("Not enough inliers (%d) between %d and %d", info.inliersIDs.size(), sFrom.id(), sTo.id());
+					ParametersMap regParam;
+					regParam.insert(ParametersPair(Parameters::kVisEstimationType(), "1"));
+					regParam.insert(ParametersPair(Parameters::kVisPnPReprojError(), "5"));
+					regParam.insert(ParametersPair(Parameters::kVisMinInliers(), "5"));
+					regParam.insert(ParametersPair(Parameters::kVisCorNNDR(), "0.6"));
+					RegistrationVis reg(regParam);
+
+					//sFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
+					//sTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
+
+					RegistrationInfo info;
+					Transform t = reg.computeTransformationMod(sFrom, sTo, Transform(), &info);
+					//Transform t = reg.computeTransformationMod(sFrom, sTo, iter->second.transform(), &info);
+					UDEBUG("%d->%d, inliers=%d",sFrom.id(), sTo.id(), (int)info.inliersIDs.size());
+
+					if(!t.isNull())
+					{
+						Transform pose = poses.at(sFrom.id());
+						UASSERT(!pose.isNull());
+						for(unsigned int i=0; i<info.inliersIDs.size(); ++i)
+						{
+							cv::Point3f p = sFrom.getWords3().lower_bound(info.inliersIDs[i])->second;
+							if(p.x > 0.0f) // make sure the point is valid
+							{
+								int wordId = ++wordCount;
+
+								wordReferences.insert(std::make_pair(wordId, std::map<int, cv::Point3f>()));
+
+								cv::Point2f pt = sFrom.getWords().lower_bound(info.inliersIDs[i])->second.pt;
+								wordReferences.at(wordId).insert(std::make_pair(sFrom.id(), cv::Point3f(pt.x, pt.y, p.x)));
+
+
+								pt = sTo.getWords().lower_bound(info.inliersIDs[i])->second.pt;
+								float depth = 0.0f;
+								std::multimap<int, cv::Point3f>::const_iterator iterTo = sTo.getWords3().lower_bound(info.inliersIDs[i]);
+								if( iterTo!=sTo.getWords3().end() &&
+									iterTo->second.x > 0)
+								{
+									depth = iterTo->second.x;
+								}
+								wordReferences.at(wordId).insert(std::make_pair(sTo.id(), cv::Point3f(pt.x, pt.y, depth)));
+
+								p = util3d::transformPoint(p, pose);
+								points3DMap.insert(std::make_pair(wordId, p));
+							}
+						}
+						++edgeWithWordsAdded;
+					}
+					else
+					{
+						UWARN("Not enough inliers (%d) between %d and %d", info.inliersIDs.size(), sFrom.id(), sTo.id());
+					}
 				}
 			}
 		}

corelib/src/OptimizerCVSBA.cpp

@@ -57,7 +57,10 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
 		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/Disparity>)
+		std::set<int> * outliers)
 {
 #ifdef RTABMAP_CVSBA
 	// run sba optimization
@@ -73,100 +76,72 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
 	params.verbose=ULogger::level() <= ULogger::kInfo;
 	sba.setParams(params);
 
-	std::map<int, Transform> frames = poses;
-
-	std::vector<cv::Mat> cameraMatrix(frames.size()); //nframes
-	std::vector<cv::Mat> R(frames.size()); //nframes
-	std::vector<cv::Mat> T(frames.size()); //nframes
-	std::vector<cv::Mat> distCoeffs(frames.size()); //nframes
+	std::vector<cv::Mat> cameraMatrix(poses.size()); //nframes
+	std::vector<cv::Mat> R(poses.size()); //nframes
+	std::vector<cv::Mat> T(poses.size()); //nframes
+	std::vector<cv::Mat> distCoeffs(poses.size()); //nframes
 	std::map<int, int> frameIdToIndex;
-	std::map<int, CameraModel> models;
 	int oi=0;
-	for(std::map<int, Transform>::iterator iter=frames.begin(); iter!=frames.end(); )
+	for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 	{
-		CameraModel model;
-		if(uContains(signatures, iter->first))
+		// Get camera model
+		std::map<int, CameraModel>::const_iterator iterModel = models.find(iter->first);
+		UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection());
+
+		frameIdToIndex.insert(std::make_pair(iter->first, oi));
+
+		cameraMatrix[oi] = iterModel->second.K();
+		if(iterModel->second.D().cols != 5)
 		{
-			if(signatures.at(iter->first).sensorData().cameraModels().size() == 1 && signatures.at(iter->first).sensorData().cameraModels().at(0).isValidForProjection())
-			{
-				model = signatures.at(iter->first).sensorData().cameraModels()[0];
-			}
-			else if(signatures.at(iter->first).sensorData().stereoCameraModel().isValidForProjection())
-			{
-				model = signatures.at(iter->first).sensorData().stereoCameraModel().left();
-			}
-			else
-			{
-				UERROR("Missing calibration for node %d", iter->first);
-			}
+			distCoeffs[oi] = cv::Mat::zeros(1, 5, CV_64FC1);
+			UWARN("Camera model %d: Distortion coefficients are not 5, setting all them to 0 (assuming no distortion)", iter->first);
 		}
 		else
 		{
-			UERROR("Did not find node %d in cache", iter->first);
+			distCoeffs[oi] = iterModel->second.D();
 		}
 
-		if(model.isValidForProjection())
-		{
-			frameIdToIndex.insert(std::make_pair(iter->first, oi));
+		Transform t = (iter->second * iterModel->second.localTransform()).inverse();
 
-			cameraMatrix[oi] = model.K();
-			if(model.D().cols != 5)
-			{
-				distCoeffs[oi] = cv::Mat::zeros(1, 5, CV_64FC1);
-				UWARN("Camera model %d: Distortion coefficients are not 5, setting all them to 0 (assuming no distortion)", iter->first);
-			}
-			else
-			{
-				distCoeffs[oi] = model.D();
-			}
+		R[oi] = (cv::Mat_<double>(3,3) <<
+				(double)t.r11(), (double)t.r12(), (double)t.r13(),
+				(double)t.r21(), (double)t.r22(), (double)t.r23(),
+				(double)t.r31(), (double)t.r32(), (double)t.r33());
+		T[oi] = (cv::Mat_<double>(1,3) << (double)t.x(), (double)t.y(), (double)t.z());
+		++oi;
 
-			Transform t = (iter->second * model.localTransform()).inverse();
-
-			R[oi] = (cv::Mat_<double>(3,3) <<
-					(double)t.r11(), (double)t.r12(), (double)t.r13(),
-					(double)t.r21(), (double)t.r22(), (double)t.r23(),
-					(double)t.r31(), (double)t.r32(), (double)t.r33());
-			T[oi] = (cv::Mat_<double>(1,3) << (double)t.x(), (double)t.y(), (double)t.z());
-			++oi;
-
-			models.insert(std::make_pair(iter->first, model));
-
-			UDEBUG("Pose %d = %s", iter->first, t.prettyPrint().c_str());
-
-			++iter;
-		}
-		else
-		{
-			frames.erase(iter++);
-		}
+		UDEBUG("Pose %d = %s", iter->first, t.prettyPrint().c_str());
 	}
 	cameraMatrix.resize(oi);
 	R.resize(oi);
 	T.resize(oi);
 	distCoeffs.resize(oi);
 
-	std::map<int, cv::Point3f> points3DMap;
-	std::map<int, std::map<int, cv::Point2f> > wordReferences; // <ID words, IDs frames + keypoint>
-	computeBACorrespondences(frames, links, signatures, points3DMap, wordReferences);
-
-	UDEBUG("points=%d frames=%d", (int)wordReferences.size(), (int)frames.size());
-	std::vector<cv::Point3f> points(wordReferences.size()); //npoints
-	std::vector<std::vector<cv::Point2f> >  imagePoints(frames.size()); //nframes -> npoints
-	std::vector<std::vector<int> > visibility(frames.size()); //nframes -> npoints
-	for(unsigned int i=0; i<frames.size(); ++i)
+	UDEBUG("points=%d frames=%d", (int)points3DMap.size(), (int)poses.size());
+	std::vector<cv::Point3f> points(points3DMap.size()); //npoints
+	std::vector<std::vector<cv::Point2f> >  imagePoints(poses.size()); //nframes -> npoints
+	std::vector<std::vector<int> > visibility(poses.size()); //nframes -> npoints
+	for(unsigned int i=0; i<poses.size(); ++i)
 	{
 		imagePoints[i].resize(wordReferences.size(), cv::Point2f(std::numeric_limits<float>::quiet_NaN(), std::numeric_limits<float>::quiet_NaN()));
 		visibility[i].resize(wordReferences.size(), 0);
 	}
 	int i=0;
-	for(std::map<int, std::map<int, cv::Point2f> >::iterator iter = wordReferences.begin(); iter!=wordReferences.end(); ++iter)
+	for(std::map<int, cv::Point3f>::const_iterator kter = points3DMap.begin(); kter!=points3DMap.end(); ++kter)
 	{
-		points[i] = points3DMap.at(iter->first);
+		points[i] = kter->second;
 
-		for(std::map<int, cv::Point2f>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
+		std::map<int, std::map<int, cv::Point3f> >::const_iterator iter = wordReferences.find(kter->first);
+		if(iter != wordReferences.end())
 		{
-			imagePoints[frameIdToIndex.at(jter->first)][i] = jter->second;
-			visibility[frameIdToIndex.at(jter->first)][i] = 1;
+			for(std::map<int, cv::Point3f>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
+			{
+				if(frameIdToIndex.find(jter->first) != frameIdToIndex.end())
+				{
+					imagePoints[frameIdToIndex.at(jter->first)][i] = cv::Point2f(jter->second.x, jter->second.y);
+					visibility[frameIdToIndex.at(jter->first)][i] = 1;
+				}
+			}
 		}
 		++i;
 	}
@@ -184,7 +159,8 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
 
 	//update poses
 	i=0;
-	for(std::map<int, Transform>::iterator iter=frames.begin(); iter!=frames.end(); ++iter)
+	std::map<int, Transform> newPoses = poses;
+	for(std::map<int, Transform>::iterator iter=newPoses.begin(); iter!=newPoses.end(); ++iter)
 	{
 		Transform t(R[i].at<double>(0,0), R[i].at<double>(0,1), R[i].at<double>(0,2), T[i].at<double>(0),
 					R[i].at<double>(1,0), R[i].at<double>(1,1), R[i].at<double>(1,2), T[i].at<double>(1),
@@ -192,12 +168,28 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
 
 		UDEBUG("New pose %d = %s", iter->first, t.prettyPrint().c_str());
 
-		iter->second = (models.at(iter->first).localTransform() * t).inverse();
+		if(this->isSlam2d())
+		{
+			t = (models.at(iter->first).localTransform() * t).inverse();
+			t = iter->second.inverse() * t;
+			iter->second *= t.to3DoF();
+		}
+		else
+		{
+			iter->second = (models.at(iter->first).localTransform() * t).inverse();
+		}
 
 		++i;
 	}
 
-	return frames;
+	//update 3D points
+	i=0;
+	for(std::map<int, cv::Point3f>::iterator kter = points3DMap.begin(); kter!=points3DMap.end(); ++kter)
+	{
+		kter->second = points[i++];
+	}
+
+	return newPoses;
 
 #else
 	UERROR("RTAB-Map is not built with cvsba!");

corelib/src/OptimizerG2O.cpp

@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/OptimizerG2O.h>
 #include <rtabmap/core/util3d_transforms.h>
 #include <rtabmap/core/util3d_motion_estimation.h>
+#include <rtabmap/core/util3d.h>
 
 #ifdef RTABMAP_G2O
 #include "g2o/config.h"
@@ -54,12 +55,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifdef G2O_HAVE_CHOLMOD
 #include "g2o/solvers/cholmod/linear_solver_cholmod.h"
 #endif
+#include "g2o/solvers/eigen/linear_solver_eigen.h"
 #include "g2o/types/slam3d/vertex_se3.h"
 #include "g2o/types/slam3d/edge_se3.h"
 #include "g2o/types/slam2d/vertex_se2.h"
 #include "g2o/types/slam2d/edge_se2.h"
 
 typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> > SlamBlockSolver;
+typedef g2o::LinearSolverEigen<SlamBlockSolver::PoseMatrixType> SlamLinearEigenSolver;
 typedef g2o::LinearSolverPCG<SlamBlockSolver::PoseMatrixType> SlamLinearPCGSolver;
 #ifdef G2O_HAVE_CSPARSE
 typedef g2o::LinearSolverCSparse<SlamBlockSolver::PoseMatrixType> SlamLinearCSparseSolver;
@@ -113,7 +116,10 @@ void OptimizerG2O::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kg2oSolver(), solver_);
 	Parameters::parse(parameters, Parameters::kg2oOptimizer(), optimizer_);
 	Parameters::parse(parameters, Parameters::kg2oPixelVariance(), pixelVariance_);
+	Parameters::parse(parameters, Parameters::kg2oRobustKernelDelta(), robustKernelDelta_);
+	Parameters::parse(parameters, Parameters::kg2oBaseline(), baseline_);
 	UASSERT(pixelVariance_ > 0.0);
+	UASSERT(baseline_ >= 0.0);
 
 #ifndef G2O_HAVE_CHOLMOD
 	if(solver_ == 2)
@@ -162,7 +168,14 @@ std::map<int, Transform> OptimizerG2O::optimize(
 
 		SlamBlockSolver * blockSolver = 0;
 
-		if(solver_ == 2)
+		if(solver_ == 3)
+		{
+			//eigen
+			SlamLinearEigenSolver * linearSolver = new SlamLinearEigenSolver();
+			linearSolver->setBlockOrdering(false);
+			blockSolver = new SlamBlockSolver(linearSolver);
+		}
+		else if(solver_ == 2)
 		{
 #ifdef G2O_HAVE_CHOLMOD
 			//chmold
@@ -235,7 +248,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
 		int vertigoVertexId = poses.rbegin()->first+1;
 		for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
 		{
-			int id1 = iter->first;
+			int id1 = iter->second.from();
 			int id2 = iter->second.to();
 
 			UASSERT(!iter->second.transform().isNull());
@@ -523,10 +536,6 @@ std::map<int, Transform> OptimizerG2O::optimize(
 				}
 			}
 		}
-		optimizer.clear();
-		g2o::Factory::destroy();
-		g2o::OptimizationAlgorithmFactory::destroy();
-		g2o::HyperGraphActionLibrary::destroy();
 	}
 	else if(poses.size() == 1 || iterations() <= 0)
 	{
@@ -547,21 +556,28 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 		int rootId,
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & links,
-		const std::map<int, Signature> & signatures)
+		const std::map<int, CameraModel> & models,
+		std::map<int, cv::Point3f> & points3DMap,
+		const std::map<int, std::map<int, cv::Point3f> > & wordReferences,
+		std::set<int> * outliers)
 {
 	std::map<int, Transform> optimizedPoses;
 #ifdef RTABMAP_G2O
 	UDEBUG("Optimizing graph...");
 
 	optimizedPoses.clear();
-	if(links.size()>=1 && poses.size()>=2 && iterations() > 0)
+	if(poses.size()>=2 && iterations() > 0 && models.size() == poses.size())
 	{
 		g2o::SparseOptimizer optimizer;
 		optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
 		g2o::BlockSolver_6_3::LinearSolverType * linearSolver = 0;
-		bool robustKernel = true;
 
-		if(solver_ == 2)
+		if(solver_ == 3)
+		{
+			//eigen
+			linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
+		}
+		else if(solver_ == 2)
 		{
 #ifdef G2O_HAVE_CHOLMOD
 			//chmold
@@ -593,81 +609,57 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 			optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(solver_ptr));
 		}
 
-		std::map<int, Transform> frames = poses;
-
 		UDEBUG("fill poses to g2o...");
-		std::map<int, CameraModel> models;
-		for(std::map<int, Transform>::iterator iter=frames.begin(); iter!=frames.end(); )
+		for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); )
 		{
 			// Get camera model
-			CameraModel model;
-			if(uContains(signatures, iter->first))
-			{
-				if(signatures.at(iter->first).sensorData().cameraModels().size() == 1 && signatures.at(iter->first).sensorData().cameraModels().at(0).isValidForProjection())
-				{
-					model = signatures.at(iter->first).sensorData().cameraModels()[0];
-				}
-				else if(signatures.at(iter->first).sensorData().stereoCameraModel().isValidForProjection())
-				{
-					model = signatures.at(iter->first).sensorData().stereoCameraModel().left();
-				}
-				else
-				{
-					UERROR("Missing calibration for node %d", iter->first);
-					return optimizedPoses;
-				}
-			}
-			else
-			{
-				UERROR("Did not find node %d in cache", iter->first);
-			}
+			std::map<int, CameraModel>::const_iterator iterModel = models.find(iter->first);
+			UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection());
 
-			if(model.isValidForProjection())
-			{
-				models.insert(std::make_pair(iter->first, model));
-				Transform camPose = iter->second * model.localTransform();
-				//iter->second = (iter->second * model.localTransform()).inverse();
-				UDEBUG("%d t=%s", iter->first, camPose.prettyPrint().c_str());
+			Transform camPose = iter->second * iterModel->second.localTransform();
 
-				// Add node's pose
-				UASSERT(!camPose.isNull());
-				g2o::VertexCam * vCam = new g2o::VertexCam();
+			// Add node's pose
+			UASSERT(!camPose.isNull());
+			g2o::VertexCam * vCam = new g2o::VertexCam();
 
-				Eigen::Affine3d a = camPose.toEigen3d();
-				g2o::SBACam cam(Eigen::Quaterniond(a.rotation()), a.translation());
-				cam.setKcam(model.fx(), model.fy(), model.cx(), model.cy(), 0);
-				vCam->setEstimate(cam);
-				if(iter->first == rootId)
-				{
-					vCam->setFixed(true);
-				}
-				vCam->setId(iter->first);
-				std::cout << cam << std::endl;
-				UASSERT_MSG(optimizer.addVertex(vCam), uFormat("cannot insert vertex %d!?", iter->first).c_str());
+			Eigen::Affine3d a = camPose.toEigen3d();
+			g2o::SBACam cam(Eigen::Quaterniond(a.rotation()), a.translation());
+			cam.setKcam(
+					iterModel->second.fx(),
+					iterModel->second.fy(),
+					iterModel->second.cx(),
+					iterModel->second.cy(),
+					iterModel->second.Tx()<0.0?-iterModel->second.Tx()/iterModel->second.fx():baseline_); // baseline in meters
+			vCam->setEstimate(cam);
+			vCam->setId(iter->first);
 
-				++iter;
-			}
-			else
-			{
-				frames.erase(iter++);
-			}
+			// negative root means that all other poses should be fixed instead of the root
+			vCam->setFixed((rootId >= 0 && iter->first == rootId) || (rootId < 0 && iter->first != -rootId));
+
+			UDEBUG("cam %d (fixed=%d) fx=%f fy=%f cx=%f cy=%f Tx=%f baseline=%f t=%s",
+					iter->first,
+					vCam->fixed()?1:0,
+					iterModel->second.fx(),
+					iterModel->second.fy(),
+					iterModel->second.cx(),
+					iterModel->second.cy(),
+					iterModel->second.Tx(),
+					iterModel->second.Tx()<0.0?-iterModel->second.Tx()/iterModel->second.fx():baseline_,
+					camPose.prettyPrint().c_str());
+
+			UASSERT_MSG(optimizer.addVertex(vCam), uFormat("cannot insert vertex %d!?", iter->first).c_str());
+
+			++iter;
 		}
 
-		UDEBUG("fill edges to g2o and associate each 3D point to all frames observing it...");
+		UDEBUG("fill edges to g2o...");
 		for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
 		{
-			Link link = iter->second;
-			if(link.to() < link.from())
-			{
-				link = link.inverse();
-			}
-			if(uContains(signatures, link.from()) &&
-			   uContains(signatures, link.to()) &&
-			   uContains(frames, link.from()) &&
-			   uContains(frames, link.to()))
+			if(uContains(poses, iter->second.from()) &&
+			   uContains(poses, iter->second.to()))
 			{
 				// add edge
-				int id1 = iter->first;
+				int id1 = iter->second.from();
 				int id2 = iter->second.to();
 
 				UASSERT(!iter->second.transform().isNull());
@@ -680,17 +672,15 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 
 				// between cameras, not base_link
 				Transform camLink = models.at(id1).localTransform().inverse()*iter->second.transform()*models.at(id2).localTransform();
-				//Transform t = iter->second.transform();
-				UDEBUG("added edge %d=%s -> %d=%s",
+				UDEBUG("added edge %d->%d (in cam frame=%s)",
 						id1,
-						iter->second.transform().prettyPrint().c_str(),
 						id2,
 						camLink.prettyPrint().c_str());
 				Eigen::Affine3d a = camLink.toEigen3d();
 
 				g2o::EdgeSBACam * e = new g2o::EdgeSBACam();
-				g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
-				g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2);
+				g2o::VertexCam* v1 = (g2o::VertexCam*)optimizer.vertex(id1);
+				g2o::VertexCam* v2 = (g2o::VertexCam*)optimizer.vertex(id2);
 				UASSERT(v1 != 0);
 				UASSERT(v2 != 0);
 				e->setVertex(0, v1);
@@ -707,47 +697,78 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 			}
 		}
 
-		std::map<int, cv::Point3f> points3DMap;
-		std::map<int, std::map<int, cv::Point2f> > wordReferences; // <ID words, IDs frames + keypoint>
-		this->computeBACorrespondences(frames, links, signatures, points3DMap, wordReferences);
-
 		UDEBUG("fill 3D points to g2o...");
-		int stepVertexId = frames.rbegin()->first+1;
-		for(std::map<int, std::map<int, cv::Point2f> >::iterator iter = wordReferences.begin(); iter!=wordReferences.end(); ++iter)
+		const int stepVertexId = poses.rbegin()->first+1;
+		std::list<g2o::OptimizableGraph::Edge*> edges;
+		for(std::map<int, std::map<int, cv::Point3f> >::const_iterator iter = wordReferences.begin(); iter!=wordReferences.end(); ++iter)
 		{
-			const cv::Point3f & pt3d = points3DMap.at(iter->first);
-			g2o::VertexSBAPointXYZ* vpt3d = new g2o::VertexSBAPointXYZ();
-
-			vpt3d->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
-			vpt3d->setId(stepVertexId + iter->first);
-			vpt3d->setMarginalized(true);
-			optimizer.addVertex(vpt3d);
-
-			// set observations
-			for(std::map<int, cv::Point2f>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
+			if(points3DMap.find(iter->first) != points3DMap.end())
 			{
-				int camId = jter->first;
+				const cv::Point3f & pt3d = points3DMap.at(iter->first);
+				g2o::VertexSBAPointXYZ* vpt3d = new g2o::VertexSBAPointXYZ();
 
-				const cv::Point2f & pt = jter->second;
+				vpt3d->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
+				vpt3d->setId(stepVertexId + iter->first);
+				vpt3d->setMarginalized(true);
+				optimizer.addVertex(vpt3d);
 
-				Eigen::Matrix<double,2,1> obs;
-				obs << pt.x, pt.y;
+				//UDEBUG("Added 3D point %d (%f,%f,%f)", vpt3d->id()-stepVertexId, pt3d.x, pt3d.y, pt3d.z);
 
-				UDEBUG("Added observation pt=%d to cam=%d (%f,%f)", vpt3d->id(), camId, pt.x, pt.y);
-
-				g2o::EdgeProjectP2MC* e = new g2o::EdgeProjectP2MC();
-
-				e->setVertex(0, vpt3d);
-				e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(camId)));
-				e->setMeasurement(obs);
-				e->setInformation(Eigen::Matrix2d::Identity() / pixelVariance_);
-
-				if(robustKernel)
+				// set observations
+				for(std::map<int, cv::Point3f>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
 				{
-					e->setRobustKernel(new g2o::RobustKernelHuber);
-				}
+					int camId = jter->first;
+					if(poses.find(camId) != poses.end() && optimizer.vertex(camId) != 0)
+					{
+						const cv::Point3f & pt = jter->second;
+						double depth = pt.z;
 
-				optimizer.addEdge(e);
+						//UDEBUG("Added observation pt=%d to cam=%d (%f,%f) d=%f", vpt3d->id()-stepVertexId, camId, pt.x, pt.y, depth);
+
+						g2o::OptimizableGraph::Edge * e;
+						g2o::VertexCam* vcam = dynamic_cast<g2o::VertexCam*>(optimizer.vertex(camId));
+						double variance = pixelVariance_;
+						if(uIsFinite(depth) && depth > 0.0 && vcam->estimate().baseline > 0.0)
+						{
+							// stereo edge
+							g2o::EdgeProjectP2SC* es = new g2o::EdgeProjectP2SC();
+							float disparity = vcam->estimate().baseline * vcam->estimate().Kcam(0,0) / depth;
+							Eigen::Vector3d obs( pt.x, pt.y, pt.x-disparity);
+							es->setMeasurement(obs);
+							//variance *= log(exp(1)+disparity);
+							es->setInformation(Eigen::Matrix3d::Identity() / variance);
+							e = es;
+						}
+						else
+						{
+							if(vcam->estimate().baseline > 0.0)
+							{
+								UWARN("Stereo camera model detected but current "
+										"observation (pt=%d to cam=%d) has null depth (%f m), adding "
+										"mono observation instead.",
+										vpt3d->id()-stepVertexId, camId, depth);
+							}
+							// mono edge
+							g2o::EdgeProjectP2MC* em = new g2o::EdgeProjectP2MC();
+							Eigen::Vector2d obs( pt.x, pt.y);
+							em->setMeasurement(obs);
+							em->setInformation(Eigen::Matrix2d::Identity() / variance);
+							e = em;
+						}
+						e->setVertex(0, vpt3d);
+						e->setVertex(1, vcam);
+
+						if(robustKernelDelta_ > 0.0)
+						{
+							g2o::RobustKernelHuber* kernel = new g2o::RobustKernelHuber;
+							kernel->setDelta(robustKernelDelta_);
+							e->setRobustKernel(kernel);
+						}
+
+						optimizer.addEdge(e);
+						edges.push_back(e);
+					}
+				}
 			}
 		}
 
@@ -756,56 +777,65 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 
 		UASSERT(optimizer.verifyInformationMatrices());
 
-		UINFO("g2o optimizing begin (max iterations=%d, epsilon=%f robustKernel=%d)", iterations(), this->epsilon(), robustKernel?1:0);
+		UINFO("g2o optimizing begin (max iterations=%d, epsilon=%f robustKernel=%f)", iterations(), this->epsilon(), robustKernelDelta_);
 
 		int it = 0;
 		UTimer timer;
-		double lastError = 0.0;
-		if(this->epsilon() > 0.0)
+		int outliersCount = 0;
+		int outliersCountFar = 0;
+
+		for(int i=0; i<(robustKernelDelta_>0.0?2:1); ++i)
 		{
-			for(int i=0; i<iterations(); ++i)
+			it += optimizer.optimize(i==0&&robustKernelDelta_>0.0?3:iterations());
+
+			// early stop condition
+			optimizer.computeActiveErrors();
+			double chi2 = optimizer.activeRobustChi2();
+			if(uIsNan(chi2))
 			{
-				it += optimizer.optimize(1);
+				UERROR("Optimization generated NANs, aborting optimization! Try another g2o's optimizer (current=%d).", optimizer_);
+				return optimizedPoses;
+			}
+			UDEBUG("iteration %d: %d nodes, %d edges, chi2: %f", i, (int)optimizer.vertices().size(), (int)optimizer.edges().size(), chi2);
 
-				// early stop condition
-				optimizer.computeActiveErrors();
-				double chi2 = optimizer.activeRobustChi2();
-				UDEBUG("iteration %d: %d nodes, %d edges, chi2: %f", i, (int)optimizer.vertices().size(), (int)optimizer.edges().size(), chi2);
+			if(i>0 && (optimizer.activeRobustChi2() > 1000000000000.0 || !uIsFinite(optimizer.activeRobustChi2())))
+			{
+				UWARN("g2o: Large optimization error detected (%f), aborting optimization!");
+				return optimizedPoses;
+			}
 
-				if(i>0 && (optimizer.activeRobustChi2() > 1000000000000.0 || !uIsFinite(optimizer.activeRobustChi2())))
+			if(robustKernelDelta_>0.0)
+			{
+				for(std::list<g2o::OptimizableGraph::Edge*>::iterator iter=edges.begin(); iter!=edges.end();++iter)
 				{
-					UWARN("g2o: Large optimization error detected (%f), aborting optimization!");
-					return optimizedPoses;
-				}
-
-				double errorDelta = lastError - chi2;
-				if(i>0 && errorDelta < this->epsilon())
-				{
-					if(errorDelta < 0)
+					if((*iter)->level() == 0 && (*iter)->chi2() > (*iter)->robustKernel()->delta())
 					{
-						UDEBUG("Negative improvement?! Ignore and continue optimizing... (%f < %f)", errorDelta, this->epsilon());
-					}
-					else
-					{
-						UINFO("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
-						break;
+						(*iter)->setLevel(1);
+						++outliersCount;
+						double d = ((g2o::EdgeProjectP2SC*)(*iter))->measurement()[0]-((g2o::EdgeProjectP2SC*)(*iter))->measurement()[2];
+						UDEBUG("Ignoring edge (%d<->%d) d=%f var=%f kernel=%f chi2=%f", (*iter)->vertex(0)->id()-stepVertexId, (*iter)->vertex(1)->id(), d, 1.0/((g2o::EdgeProjectP2SC*)(*iter))->information()(0,0), (*iter)->robustKernel()->delta(), (*iter)->chi2());
+
+						const cv::Point3f & pt3d = points3DMap.at((*iter)->vertex(0)->id()-stepVertexId);
+						((g2o::VertexSBAPointXYZ*)(*iter)->vertex(0))->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
+
+						if(outliers)
+						{
+							outliers->insert((*iter)->vertex(0)->id()-stepVertexId);
+						}
+						if(d < 5.0)
+						{
+							outliersCountFar++;
+						}
 					}
+					//(*iter)->setRobustKernel(0);
 				}
-				else if(i==0 && chi2 < this->epsilon())
-				{
-					UINFO("Stop optimizing, error is already under epsilon (%f < %f)", chi2, this->epsilon());
-					break;
-				}
-				lastError = chi2;
+				if(i==0)
+					optimizer.initializeOptimization(0);
+				UDEBUG("outliers=%d outliersCountFar=%d", outliersCount, outliersCountFar);
 			}
 		}
-		else
-		{
-			it = optimizer.optimize(iterations());
-			optimizer.computeActiveErrors();
-			UDEBUG("%d nodes, %d edges, chi2: %f", (int)optimizer.vertices().size(), (int)optimizer.edges().size(), optimizer.activeRobustChi2());
-		}
-		UINFO("g2o optimizing end (%d iterations done, error=%f, time = %f s)", it, optimizer.activeRobustChi2(), timer.ticks());
+
+		UINFO("g2o optimizing end (%d iterations done, error=%f, outliers=%d/%d (delta=%f) time = %f s)", it, optimizer.activeRobustChi2(), outliersCount, (int)edges.size(), robustKernelDelta_, timer.ticks());
 
 		if(optimizer.activeRobustChi2() > 1000000000000.0)
 		{
@@ -813,23 +843,62 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 			return optimizedPoses;
 		}
 
+		// update poses
 		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 		{
 			const g2o::VertexCam* v = (const g2o::VertexCam*)optimizer.vertex(iter->first);
 			if(v)
 			{
 				Transform t = Transform::fromEigen3d(v->estimate());
-				UDEBUG("%d t=%s", iter->first, t.prettyPrint().c_str());
+
 				// remove model local transform
 				t *= models.at(iter->first).localTransform().inverse();
-				optimizedPoses.insert(std::pair<int, Transform>(iter->first, t));
-				UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
+				UDEBUG("%d from=%s to=%s", iter->first, iter->second.prettyPrint().c_str(), t.prettyPrint().c_str());
+				if(t.isNull())
+				{
+					UERROR("Optimized pose %d is null!?!?", iter->first);
+					optimizedPoses.clear();
+					return optimizedPoses;
+				}
+
+				// FIXME: is there a way that we can add the 2D constraint directly in SBA?
+				if(this->isSlam2d())
+				{
+					// get transform between old and new pose
+					t = iter->second.inverse() * t;
+					optimizedPoses.insert(std::pair<int, Transform>(iter->first, iter->second * t.to3DoF()));
+				}
+				else
+				{
+					optimizedPoses.insert(std::pair<int, Transform>(iter->first, t));
+				}
 			}
 			else
 			{
-				UERROR("Vertex %d not found!?", iter->first);
+				UERROR("Vertex (pose) %d not found!?", iter->first);
 			}
 		}
+
+		//update points3D
+
+		for(std::map<int, cv::Point3f>::iterator iter = points3DMap.begin(); iter!=points3DMap.end(); ++iter)
+		{
+			const g2o::VertexSBAPointXYZ* v = (const g2o::VertexSBAPointXYZ*)optimizer.vertex(stepVertexId + iter->first);
+			if(v)
+			{
+				cv::Point3f p(v->estimate()[0], v->estimate()[1], v->estimate()[2]);
+				//UDEBUG("%d from=%f,%f,%f to=%f,%f,%f", iter->first, iter->second.x, iter->second.y, iter->second.z, p.x, p.y, p.z);
+				iter->second = p;
+			}
+			else
+			{
+				iter->second.x = iter->second.y = iter->second.z = std::numeric_limits<float>::quiet_NaN();
+			}
+		}
+	}
+	else if(poses.size() > 1 && poses.size() != models.size())
+	{
+		UERROR("This method should be called with size of poses = size camera models!");
 	}
 	else if(poses.size() == 1 || iterations() <= 0)
 	{
@@ -897,7 +966,7 @@ bool OptimizerG2O::saveGraph(
 			Eigen::Quaternionf q = iter->second.transform().getQuaternionf();
 			fprintf(file, "%s %d %d%s %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
 					prefix.c_str(),
-					iter->first,
+					iter->second.from(),
 					iter->second.to(),
 					suffix.c_str(),
 					iter->second.transform().x(),

corelib/src/OptimizerGTSAM.cpp

@@ -126,7 +126,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 		int switchCounter = poses.rbegin()->first+1;
 		for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
 		{
-			int id1 = iter->first;
+			int id1 = iter->second.from();
 			int id2 = iter->second.to();
 
 			UASSERT(!iter->second.transform().isNull());