RealSense driver: added odometry option (ZR300)

CMakeLists.txt

@@ -319,6 +319,9 @@ IF(WITH_REALSENSE)
     IF(RealSense_FOUND)
        MESSAGE(STATUS "Found RealSense: ${RealSense_INCLUDE_DIRS}")
     ENDIF(RealSense_FOUND)
+    IF(RealSenseSlam_FOUND)
+       MESSAGE(STATUS "Found RealSenseSlam: ${RealSense_INCLUDE_DIRS}")
+    ENDIF(RealSenseSlam_FOUND)
 ENDIF(WITH_REALSENSE)
 
 IF(WITH_OCTOMAP)
@@ -435,6 +438,9 @@ ENDIF()
 IF(NOT RealSense_FOUND)
    SET(REALSENSE "//")
 ELSE()
+   IF(NOT RealSenseSlam_FOUND)
+      SET(REALSENSESLAM "//")
+   ENDIF(NOT RealSenseSlam_FOUND)
    SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${RealSense_LIBRARIES}) 
 ENDIF()
 IF(NOT OCTOMAP_FOUND)
@@ -736,6 +742,11 @@ ENDIF()
 
 IF(RealSense_FOUND)
 MESSAGE(STATUS "  With RealSense            = YES (License: Apache-2)")
+IF(RealSenseSlam_FOUND)
+MESSAGE(STATUS "  With RealSenseSlam        = YES")
+ELSE()
+MESSAGE(STATUS "  With RealSenseSlam        = NO (librealsense_slam not found)")
+ENDIF()
 ELSEIF(NOT WITH_REALSENSE)
 MESSAGE(STATUS "  With RealSense            = NO (WITH_REALSENSE=OFF)")
 ELSE()

Version.h.in

@@ -51,6 +51,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @FLYCAPTURE2@#define RTABMAP_FLYCAPTURE2
 @ZED@#define RTABMAP_ZED
 @REALSENSE@#define RTABMAP_REALSENSE
+@REALSENSESLAM@#define RTABMAP_REALSENSE_SLAM
 @OCTOMAP@#define RTABMAP_OCTOMAP
 
 #endif /* VERSION_H_ */

cmake_modules/FindRealSense.cmake

@@ -4,6 +4,7 @@
 #
 # It sets the following variables:
 #  RealSense_FOUND       - Set to false, or undefined, if RealSense isn't found.
+#  RealSenseSlam_FOUND    - Set to false, or undefined, if RealSense slam module isn't found.
 #  RealSense_INCLUDE_DIRS - The RealSense include directory.
 #  RealSense_LIBRARIES     - The RealSense library to link against.
 
@@ -19,9 +20,35 @@ IF (RealSense_INCLUDE_DIRS AND RealSense_LIBRARY)
    SET(RealSense_FOUND TRUE)
 ENDIF (RealSense_INCLUDE_DIRS AND RealSense_LIBRARY)
 
+#SLAM
+find_path(RealSenseSlam_INCLUDE_DIRS NAMES librealsense/slam/slam.h PATHS $ENV{RealSense_ROOT_DIR}/include)
+if(CMAKE_CL_64)
+find_library(RealSenseSlam_LIBRARY NAMES realsense_slam PATHS $ENV{RealSense_ROOT_DIR}/lib  $ENV{RealSense_ROOT_DIR}/bin $ENV{RealSense_ROOT_DIR}/bin/x64)
+find_library(RealSenseImage_LIBRARY NAMES realsense_image PATHS $ENV{RealSense_ROOT_DIR}/lib  $ENV{RealSense_ROOT_DIR}/bin $ENV{RealSense_ROOT_DIR}/bin/x64)
+find_library(RealSenseSP_Core_LIBRARY NAMES SP_Core PATHS $ENV{RealSense_ROOT_DIR}/lib  $ENV{RealSense_ROOT_DIR}/bin $ENV{RealSense_ROOT_DIR}/bin/x64)
+find_library(RealSenseTracker_LIBRARY NAMES tracker PATHS $ENV{RealSense_ROOT_DIR}/lib  $ENV{RealSense_ROOT_DIR}/bin $ENV{RealSense_ROOT_DIR}/bin/x64)
+else()
+find_library(RealSenseSlam_LIBRARY NAMES realsense_slam PATHS $ENV{RealSense_ROOT_DIR}/lib  $ENV{RealSense_ROOT_DIR}/bin $ENV{RealSense_ROOT_DIR}/bin/Win32)
+find_library(RealSenseImage_LIBRARY NAMES realsense_image PATHS $ENV{RealSense_ROOT_DIR}/lib  $ENV{RealSense_ROOT_DIR}/bin $ENV{RealSense_ROOT_DIR}/bin/Win32)
+find_library(RealSenseSP_Core_LIBRARY NAMES SP_Core PATHS $ENV{RealSense_ROOT_DIR}/lib  $ENV{RealSense_ROOT_DIR}/bin $ENV{RealSense_ROOT_DIR}/bin/Win32)
+find_library(RealSenseTracker_LIBRARY NAMES tracker PATHS $ENV{RealSense_ROOT_DIR}/lib  $ENV{RealSense_ROOT_DIR}/bin $ENV{RealSense_ROOT_DIR}/bin/Win32)
+endif()
+
+IF (RealSenseSlam_INCLUDE_DIRS AND RealSenseSlam_LIBRARY AND RealSenseImage_LIBRARY AND RealSenseSP_Core_LIBRARY AND RealSenseTracker_LIBRARY)
+   SET(RealSenseSlam_FOUND TRUE)
+ENDIF (RealSenseSlam_INCLUDE_DIRS AND RealSenseSlam_LIBRARY AND RealSenseImage_LIBRARY AND RealSenseSP_Core_LIBRARY AND RealSenseTracker_LIBRARY)
+
 IF (RealSense_FOUND)
    # show which RealSense was found only if not quiet
    SET(RealSense_LIBRARIES ${RealSense_LIBRARY})
+   IF (RealSenseSlam_FOUND)
+      SET(RealSense_LIBRARIES 
+            ${RealSense_LIBRARIES}
+            ${RealSenseSlam_LIBRARY}
+            ${RealSenseImage_LIBRARY}
+            ${RealSenseSP_Core_LIBRARY}
+            ${RealSenseTracker_LIBRARY})
+   ENDIF(RealSenseSlam_FOUND)
    IF (NOT RealSense_FIND_QUIETLY)
       MESSAGE(STATUS "Found RealSense: ${RealSense_LIBRARIES}")
    ENDIF (NOT RealSense_FIND_QUIETLY)

corelib/include/rtabmap/core/CameraRGBD.h

@@ -69,6 +69,9 @@ namespace rs
 {
 	class context;
 	class device;
+	namespace slam {
+	class slam;
+	}
 }
 
 typedef struct _freenect_context freenect_context;
@@ -290,6 +293,7 @@ private:
 /////////////////////////
 // CameraRealSense
 /////////////////////////
+class slam_event_handler;
 class RTABMAP_EXP CameraRealSense :
 	public Camera
 {
@@ -302,6 +306,7 @@ public:
 		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
+		bool computeOdometry = false,
 		float imageRate = 0,
 		const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraRealSense();
@@ -309,6 +314,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;
 
 protected:
 	virtual SensorData captureImage(CameraInfo * info = 0);
@@ -320,6 +326,16 @@ private:
 	int deviceId_;
 	int presetRGB_;
 	int presetDepth_;
+	bool computeOdometry_;
+
+	int motionSeq_[2];
+	rs::slam::slam * slam_;
+	UMutex slamLock_;
+
+	std::map<double, std::pair<cv::Mat, cv::Mat> > bufferedFrames_;
+	std::pair<cv::Mat, cv::Mat> lastSyncFrames_;
+	UMutex dataMutex_;
+	USemaphore dataReady_;
 #endif
 };
 

corelib/src/CameraRGBD.cpp

@@ -66,6 +66,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #ifdef RTABMAP_REALSENSE
 #include <librealsense/rs.hpp>
+#include <rs_core.h>
+#include <rs_utils.h>
+#ifdef RTABMAP_REALSENSE_SLAM
+#include <librealsense/slam/slam.h>
+#endif
 #endif
 
 #ifdef RTABMAP_OPENNI2
@@ -1974,7 +1979,13 @@ bool CameraRealSense::available()
 #endif
 }
 
-CameraRealSense::CameraRealSense(int device, int presetRGB, int presetDepth, float imageRate, const rtabmap::Transform & localTransform) :
+CameraRealSense::CameraRealSense(
+		int device,
+		int presetRGB,
+		int presetDepth,
+		bool computeOdometry,
+		float imageRate,
+		const rtabmap::Transform & localTransform) :
 	Camera(imageRate, localTransform)
 #ifdef RTABMAP_REALSENSE
     ,
@@ -1982,7 +1993,9 @@ CameraRealSense::CameraRealSense(int device, int presetRGB, int presetDepth, flo
 	dev_(0),
 	deviceId_(device),
 	presetRGB_(presetRGB),
-	presetDepth_(presetDepth)
+	presetDepth_(presetDepth),
+	computeOdometry_(computeOdometry),
+	slam_(0)
 #endif
 {
 	UDEBUG("");
@@ -1990,18 +2003,89 @@ CameraRealSense::CameraRealSense(int device, int presetRGB, int presetDepth, flo
 
 CameraRealSense::~CameraRealSense()
 {
+	UDEBUG("");
 #ifdef RTABMAP_REALSENSE
+	UDEBUG("");
+	if(dev_)
+	{
+		if(slam_!=0)
+		{
+			dev_->stop(rs::source::all_sources);
+		}
+		else
+		{
+			dev_->stop();
+		}
+		dev_ = 0;
+	}
+	UDEBUG("");
 	if (ctx_)
 	{
 		delete ctx_;
 	}
+#ifdef RTABMAP_REALSENSE_SLAM
+	UDEBUG("");
+	if(slam_)
+	{
+		UScopeMutex lock(slamLock_);
+		slam_->flush_resources();
+		delete slam_;
+		slam_ = 0;
+	}
+#endif
 #endif
 }
 
+#ifdef RTABMAP_REALSENSE_SLAM
+bool setStreamConfigIntrin(
+		rs::core::stream_type stream,
+		std::map< rs::core::stream_type, rs::core::intrinsics > intrinsics,
+		rs::core::video_module_interface::supported_module_config & supported_config,
+		rs::core::video_module_interface::actual_module_config & actual_config)
+{
+  auto & supported_stream_config = supported_config[stream];
+  if (!supported_stream_config.is_enabled || supported_stream_config.size.width != intrinsics[stream].width || supported_stream_config.size.height != intrinsics[stream].height)
+  {
+	  UERROR("size of stream is not supported by slam");
+	  			UERROR("  supported: stream %d, width: %d height: %d", (uint32_t) stream, supported_stream_config.size.width, supported_stream_config.size.height);
+	  			UERROR("  received: stream %d, width: %d height: %d", (uint32_t) stream, intrinsics[stream].width, intrinsics[stream].height);
+
+    return false;
+  }
+  rs::core::video_module_interface::actual_image_stream_config &actual_stream_config = actual_config[stream];
+  actual_config[stream].size.width = intrinsics[stream].width;
+  actual_config[stream].size.height = intrinsics[stream].height;
+  actual_stream_config.frame_rate = supported_stream_config.frame_rate;
+  actual_stream_config.intrinsics = intrinsics[stream];
+  actual_stream_config.is_enabled = true;
+  return true;
+}
+#endif
+
 bool CameraRealSense::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
 	UDEBUG("");
 #ifdef RTABMAP_REALSENSE
+
+	if(dev_)
+	{
+		dev_->stop(rs::source::all_sources);
+		dev_ = 0;
+	}
+	bufferedFrames_.clear();
+
+#ifdef RTABMAP_REALSENSE_SLAM
+	motionSeq_[0] = motionSeq_[1] = 0;
+	if(slam_)
+	{
+		UScopeMutex lock(slamLock_);
+		UDEBUG("Flush slam");
+		slam_->flush_resources();
+		delete slam_;
+		slam_ = 0;
+	}
+#endif
+
 	if (ctx_ == 0)
 	{
 		ctx_ = new rs::context();
@@ -2021,12 +2105,23 @@ bool CameraRealSense::init(const std::string & calibrationFolder, const std::str
 		UERROR("Cannot connect to device %d", deviceId_);
 		return false;
 	}
-	UINFO("Using device %d, an %s", deviceId_, dev_->get_name());
+	std::string name = dev_->get_name();
+	UINFO("Using device %d, an %s", deviceId_, name.c_str());
 	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_);
 
+#ifdef RTABMAP_REALSENSE_SLAM
+	bool computeOdometry = false;
+	if (name.find("ZR300") != std::string::npos && computeOdometry_)
+	{
+		// Only enable ZR300 functionality if fisheye stream is enabled.
+		// Accel/Gyro automatically enabled when fisheye requested
+		computeOdometry = true;
+	}
+#endif
+
 	// Configure depth and color to run with the device's preferred settings
 	UINFO("Enabling streams...");
 	// R200: 
@@ -2040,12 +2135,258 @@ bool CameraRealSense::init(const std::string & calibrationFolder, const std::str
 	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();
+	UDEBUG("Setup frame callback");
+	// Define lambda callback for receiving stream data
+	std::function<void(rs::frame)> frameCallback = [this](rs::frame frame)
+	{
+#ifdef RTABMAP_REALSENSE_SLAM
+		if(slam_ != 0)
+		{
+			const auto timestampDomain = frame.get_frame_timestamp_domain();
+			if (rs::timestamp_domain::microcontroller != timestampDomain)
+			{
+				UERROR("error: Junk time stamp in stream: %d\twith frame counter: %d",
+						(int)(frame.get_stream_type()), frame.get_frame_number());
+				return ;
+			}
+		}
+#endif
+
+		int width = frame.get_width();
+		int height = frame.get_height();
+		rs::core::correlated_sample_set sample_set = {};
+
+		rs::core::image_info info =
+		{
+		  width,
+		  height,
+		  rs::utils::convert_pixel_format(frame.get_format()),
+		  frame.get_stride()
+		};
+		cv::Mat image;
+		if(frame.get_format() == rs::format::raw8)
+		{
+			image = cv::Mat(height, width, CV_8UC1, (unsigned char*)frame.get_data());
+		}
+		else if(frame.get_format() == rs::format::z16)
+		{
+			image = cv::Mat(height, width, CV_16UC1, (unsigned char*)frame.get_data());
+			if(bufferedFrames_.find(frame.get_timestamp()) != bufferedFrames_.end())
+			{
+				bufferedFrames_.find(frame.get_timestamp())->second.second = image.clone();
+				UScopeMutex lock(dataMutex_);
+				bool notify = lastSyncFrames_.first.empty();
+				lastSyncFrames_ = bufferedFrames_.find(frame.get_timestamp())->second;
+				if(notify)
+				{
+					dataReady_.release();
+				}
+				bufferedFrames_.erase(frame.get_timestamp());
+			}
+			else
+			{
+				bufferedFrames_.insert(std::make_pair(frame.get_timestamp(), std::make_pair(cv::Mat(), image.clone())));
+			}
+			if(bufferedFrames_.size()>5)
+			{
+				UWARN("Frames cannot be synchronized!");
+				bufferedFrames_.clear();
+			}
+		}
+		else if(frame.get_format() == rs::format::rgb8)
+		{
+			image = cv::Mat(height, width, CV_8UC3, (unsigned char*)frame.get_data());
+			if(bufferedFrames_.find(frame.get_timestamp()) != bufferedFrames_.end())
+			{
+				bufferedFrames_.find(frame.get_timestamp())->second.first = image.clone();
+				UScopeMutex lock(dataMutex_);
+				bool notify = lastSyncFrames_.first.empty();
+				lastSyncFrames_ = bufferedFrames_.find(frame.get_timestamp())->second;
+				if(notify)
+				{
+					dataReady_.release();
+				}
+				bufferedFrames_.erase(frame.get_timestamp());
+			}
+			else
+			{
+				bufferedFrames_.insert(std::make_pair(frame.get_timestamp(), std::make_pair(image.clone(), cv::Mat())));
+			}
+			if(bufferedFrames_.size()>5)
+			{
+				UWARN("Frames cannot be synchronized!");
+				bufferedFrames_.clear();
+			}
+			return;
+		}
+		else
+		{
+			return;
+		}
+#ifdef RTABMAP_REALSENSE_SLAM
+		if(slam_ != 0)
+		{
+			rs::core::stream_type stream = rs::utils::convert_stream_type(frame.get_stream_type());
+			sample_set[stream] = rs::core::image_interface::create_instance_from_raw_data(
+								   & info,
+								   image.data,
+								   stream,
+								   rs::core::image_interface::flag::any,
+								   frame.get_timestamp(),
+								   (uint64_t)frame.get_frame_number(),
+								   rs::core::timestamp_domain::microcontroller);
+
+			UScopeMutex lock(slamLock_);
+			if (slam_->process_sample_set(sample_set) < rs::core::status_no_error)
+			{
+				UERROR("error: failed to process sample");
+			}
+			sample_set[stream]->release();
+		}
+#endif
+	};
+
+	// Setup stream callback for stream
+	if(computeOdometry)
+	{
+		dev_->set_frame_callback(rs::stream::fisheye, frameCallback);
+	}
+	dev_->set_frame_callback(rs::stream::depth, frameCallback);
+	dev_->set_frame_callback(rs::stream::color, frameCallback);
+
+
+#ifdef RTABMAP_REALSENSE_SLAM
+	if (computeOdometry)
+	{
+		dev_->enable_stream(rs::stream::fisheye, 640, 480, rs::format::raw8, 30);
+		rs::intrinsics fisheye_intrin = dev_->get_stream_intrinsics(rs::stream::fisheye);
+		UINFO("    Fish: %dx%d", fisheye_intrin.width, fisheye_intrin.height);
+
+		// Needed to align image timestamps to common clock-domain with the motion events
+		dev_->set_option(rs::option::fisheye_strobe, 1);
+		// This option causes the fisheye image to be aquired in-sync with the depth image.
+		dev_->set_option(rs::option::fisheye_external_trigger, 1);
+		dev_->set_option(rs::option::fisheye_color_auto_exposure, 1);
+
+		UDEBUG("Setup motion callback");
+		//define callback to the motion events and set it.
+		std::function<void(rs::motion_data)> motion_callback;
+		motion_callback = [this](rs::motion_data entry)
+		{
+			if ((entry.timestamp_data.source_id != RS_EVENT_IMU_GYRO) &&
+					(entry.timestamp_data.source_id != RS_EVENT_IMU_ACCEL))
+				return;
+
+			rs_event_source motionType = entry.timestamp_data.source_id;
+
+			rs::core::correlated_sample_set sample_set = {};
+			if (motionType == RS_EVENT_IMU_ACCEL)
+			{
+				sample_set[rs::core::motion_type::accel].timestamp = entry.timestamp_data.timestamp;
+				sample_set[rs::core::motion_type::accel].data[0] = (float)entry.axes[0];
+				sample_set[rs::core::motion_type::accel].data[1] = (float)entry.axes[1];
+				sample_set[rs::core::motion_type::accel].data[2] = (float)entry.axes[2];
+				sample_set[rs::core::motion_type::accel].type = rs::core::motion_type::accel;
+				++motionSeq_[0];
+				sample_set[rs::core::motion_type::accel].frame_number = motionSeq_[0];
+			}
+			else if (motionType == RS_EVENT_IMU_GYRO)
+			{
+				sample_set[rs::core::motion_type::gyro].timestamp = entry.timestamp_data.timestamp;
+				sample_set[rs::core::motion_type::gyro].data[0] = (float)entry.axes[0];
+				sample_set[rs::core::motion_type::gyro].data[1] = (float)entry.axes[1];
+				sample_set[rs::core::motion_type::gyro].data[2] = (float)entry.axes[2];
+				sample_set[rs::core::motion_type::gyro].type = rs::core::motion_type::gyro;
+				++motionSeq_[1];
+				sample_set[rs::core::motion_type::gyro].frame_number = motionSeq_[1];
+			}
+
+			UScopeMutex lock(slamLock_);
+			if (slam_->process_sample_set(sample_set) < rs::core::status_no_error)
+			{
+				UERROR("error: failed to process sample");
+			}
+		};
+
+		std::function<void(rs::timestamp_data)> timestamp_callback;
+		timestamp_callback = [](rs::timestamp_data entry) {};
+
+		dev_->enable_motion_tracking(motion_callback, timestamp_callback);
+		UINFO("  enabled accel and gyro stream");
+
+		rs::motion_intrinsics imuIntrinsics;
+		rs::extrinsics fisheye2ImuExtrinsics;
+		rs::extrinsics fisheye2DepthExtrinsics;
+		try
+		{
+			imuIntrinsics = dev_->get_motion_intrinsics();
+			fisheye2ImuExtrinsics = dev_->get_motion_extrinsics_from(rs::stream::fisheye);
+			fisheye2DepthExtrinsics = dev_->get_extrinsics(rs::stream::depth, rs::stream::fisheye);
+		}
+		catch (const rs::error & e) {
+			UERROR("Exception: %s (try to unplug/plug the camera)", e.what());
+			return false;
+		}
+
+		UDEBUG("Setup SLAM");
+		UScopeMutex lock(slamLock_);
+		slam_ = new rs::slam::slam();
+		slam_->set_auto_occupancy_map_building(false);
+		slam_->force_relocalization_pose(false);
+
+		rs::core::video_module_interface::supported_module_config supported_config = {};
+		if (slam_->query_supported_module_config(0, supported_config) < rs::core::status_no_error)
+		{
+			UERROR("Failed to query the first supported module configuration");
+			return false;
+		}
+
+		rs::core::video_module_interface::actual_module_config actual_config = {};
+
+		// Set camera intrinsics
+		std::map< rs::core::stream_type, rs::core::intrinsics > intrinsics;
+		intrinsics[rs::core::stream_type::fisheye] = rs::utils::convert_intrinsics(fisheye_intrin);
+		intrinsics[rs::core::stream_type::depth] = rs::utils::convert_intrinsics(depth_intrin);
+
+		if(!setStreamConfigIntrin(rs::core::stream_type::fisheye, intrinsics, supported_config, actual_config))
+		{
+			return false;
+		}
+		if(!setStreamConfigIntrin(rs::core::stream_type::depth, intrinsics, supported_config, actual_config))
+		{
+			return false;
+		}
+
+		// Set IMU intrinsics
+		actual_config[rs::core::motion_type::accel].is_enabled = true;
+		actual_config[rs::core::motion_type::gyro].is_enabled = true;
+		actual_config[rs::core::motion_type::gyro].intrinsics = rs::utils::convert_motion_device_intrinsics(imuIntrinsics.gyro);
+		actual_config[rs::core::motion_type::accel].intrinsics = rs::utils::convert_motion_device_intrinsics(imuIntrinsics.acc);
+
+		// Set extrinsics
+		actual_config[rs::core::stream_type::fisheye].extrinsics_motion = rs::utils::convert_extrinsics(fisheye2ImuExtrinsics);
+		actual_config[rs::core::stream_type::fisheye].extrinsics = rs::utils::convert_extrinsics(fisheye2DepthExtrinsics);
+
+		UDEBUG("Set SLAM config");
+		// Set actual config
+		if (slam_->set_module_config(actual_config) < rs::core::status_no_error)
+		{
+			UERROR("error : failed to set the enabled module configuration");
+			return false;
+		}
+
+		dev_->start(rs::source::all_sources);
+	}
+	else
+#endif
+	{
+		dev_->start();
+	}
+
 	uSleep(1000); // ignore the first frames
 	UINFO("Enabling streams...done!");
-	
+
 	return true;
 
 #else
@@ -2074,17 +2415,52 @@ std::string CameraRealSense::getSerial() const
 	return "NA";
 }
 
+bool CameraRealSense::odomProvided() const
+{
+#ifdef RTABMAP_REALSENSE_SLAM
+	return slam_!=0;
+#else
+	return false;
+#endif
+}
+
+#ifdef RTABMAP_REALSENSE_SLAM
+Transform rsPoseToTransform(const rs::slam::PoseMatrix4f & pose)
+{
+	return Transform(
+			pose.m_data[0], pose.m_data[1], pose.m_data[2], pose.m_data[3],
+			pose.m_data[4], pose.m_data[5], pose.m_data[6], pose.m_data[7],
+			pose.m_data[8], pose.m_data[9], pose.m_data[10], pose.m_data[11]);
+}
+#endif
+
 SensorData CameraRealSense::captureImage(CameraInfo * info)
 {
 	SensorData data;
 #ifdef RTABMAP_REALSENSE
 	if (dev_)
 	{
-		dev_->wait_for_frames();
+		if(!dataReady_.acquire(1, 5000))
+		{
+			UWARN("Not received new frames since 5 seconds, end of stream reached!");
+			return data;
+		}
 
-		// Retrieve our images
+		cv::Mat rgb;
-		const uint16_t * depth_image = (const uint16_t *)dev_->get_frame_data(rs::stream::depth);
+		cv::Mat depthIn;
-		const uint8_t * color_image = (const uint8_t *)dev_->get_frame_data(rs::stream::color);
+
+		{
+			UScopeMutex lock(dataMutex_);
+			rgb = lastSyncFrames_.first;
+			depthIn = lastSyncFrames_.second;
+			lastSyncFrames_.first = cv::Mat();
+			lastSyncFrames_.second = cv::Mat();
+		}
+
+		if(rgb.empty() || depthIn.empty())
+		{
+			return data;
+		}
 
 		// Retrieve camera parameters for mapping between depth and color
 		rs::intrinsics depth_intrin = dev_->get_stream_intrinsics(rs::stream::depth);
@@ -2093,7 +2469,6 @@ SensorData CameraRealSense::captureImage(CameraInfo * info)
 		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);
 
@@ -2132,7 +2507,7 @@ SensorData CameraRealSense::captureImage(CameraInfo * info)
 			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];
+				uint16_t depth_value = depthIn.at<unsigned short>(dy,dx);
 				float depth_in_meters = depth_value * scale;
 
 				// Skip over pixels with a depth value of zero, which is used to indicate no data
@@ -2165,6 +2540,22 @@ SensorData CameraRealSense::captureImage(CameraInfo * info)
 		if (!bgr.empty() && !depth.empty())
 		{
 			data = SensorData(bgr, depth, model, this->getNextSeqID(), UTimer::now());
+#ifdef RTABMAP_REALSENSE_SLAM
+			if(info && slam_)
+			{
+				UScopeMutex lock(slamLock_);
+				rs::slam::PoseMatrix4f pose;
+				if(slam_->get_camera_pose(pose) == rs::core::status_no_error)
+				{
+					Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
+					info->odomPose = opticalRotation * rsPoseToTransform(pose) * opticalRotation.inverse();
+				}
+				else
+				{
+					UERROR("Failed getting odometry pose");
+				}
+			}
+#endif
 		}
 	}
 	else

examples/RGBDMapping/main.cpp

@@ -138,7 +138,7 @@ int main(int argc, char * argv[])
 			UERROR("Not built with RealSense support...");
 			exit(-1);
 		}
-		camera = new CameraRealSense(0, 0, 0, 0, opticalRotation);
+		camera = new CameraRealSense(0, 0, 0, false, 0, opticalRotation);
 	}
 	else
 	{

guilib/src/PreferencesDialog.cpp

@@ -154,6 +154,11 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->groupBox_octomap->setEnabled(false);
 #endif
 
+#ifndef RTABMAP_REALSENSE_SLAM
+	_ui->checkbox_realsenseOdom->setChecked(false);
+	_ui->checkbox_realsenseOdom->setEnabled(false);
+#endif
+
 #ifndef RTABMAP_NONFREE
 		_ui->comboBox_detector_strategy->setItemData(0, 0, Qt::UserRole - 1);
 		_ui->comboBox_detector_strategy->setItemData(1, 0, Qt::UserRole - 1);
@@ -488,6 +493,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->checkBox_freenect2NoiseFiltering, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->comboBox_realsensePresetRGB, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->comboBox_realsensePresetDepth, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->checkbox_realsenseOdom, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 
 	connect(_ui->toolButton_cameraImages_timestamps, SIGNAL(clicked()), this, SLOT(selectSourceImagesStamps()));
 	connect(_ui->lineEdit_cameraImages_timestamps, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
@@ -1400,6 +1406,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->checkBox_freenect2NoiseFiltering->setChecked(true);
 		_ui->comboBox_realsensePresetRGB->setCurrentIndex(0);
 		_ui->comboBox_realsensePresetDepth->setCurrentIndex(2);
+		_ui->checkbox_realsenseOdom->setChecked(false);
 		_ui->lineEdit_openniOniPath->clear();
 		_ui->lineEdit_openni2OniPath->clear();
 		_ui->lineEdit_cameraRGBDImages_path_rgb->setText("");
@@ -1767,6 +1774,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	settings.beginGroup("RealSense");
 	_ui->comboBox_realsensePresetRGB->setCurrentIndex(settings.value("presetRGB", _ui->comboBox_realsensePresetRGB->currentIndex()).toInt());
 	_ui->comboBox_realsensePresetDepth->setCurrentIndex(settings.value("presetDepth", _ui->comboBox_realsensePresetDepth->currentIndex()).toInt());
+	_ui->checkbox_realsenseOdom->setChecked(settings.value("odom", _ui->checkbox_realsenseOdom->isChecked()).toBool());
 	settings.endGroup(); // RealSense
 
 	settings.beginGroup("RGBDImages");
@@ -2152,6 +2160,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.beginGroup("RealSense");
 	settings.setValue("presetRGB",           _ui->comboBox_realsensePresetRGB->currentIndex());
 	settings.setValue("presetDepth",         _ui->comboBox_realsensePresetDepth->currentIndex());
+	settings.setValue("odom",                _ui->checkbox_realsenseOdom->isChecked());
 	settings.endGroup(); // RealSense
 
 	settings.beginGroup("RGBDImages");
@@ -4518,6 +4527,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 				this->getSourceDevice().isEmpty() ? 0 : atoi(this->getSourceDevice().toStdString().c_str()),
 				_ui->comboBox_realsensePresetRGB->currentIndex(),
 				_ui->comboBox_realsensePresetDepth->currentIndex(),
+				_ui->checkbox_realsenseOdom->isChecked(),
 				this->getGeneralInputRate(),
 				this->getSourceLocalTransform());
 		}

guilib/src/ui/preferencesDialog.ui

@@ -63,7 +63,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-641</y>
+            <y>-416</y>
             <width>678</width>
             <height>2701</height>
            </rect>
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>20</number>
+                <number>5</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -2582,7 +2582,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                     <item>
                      <widget class="QStackedWidget" name="stackedWidget_src">
                       <property name="currentIndex">
-                       <number>1</number>
+                       <number>0</number>
                       </property>
                       <widget class="QWidget" name="page_41">
                        <layout class="QVBoxLayout" name="verticalLayout_64">
@@ -2692,7 +2692,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            <item>
                             <widget class="QStackedWidget" name="stackedWidget_rgbd">
                              <property name="currentIndex">
-                              <number>1</number>
+                              <number>6</number>
                              </property>
                              <widget class="QWidget" name="page_32">
                               <layout class="QVBoxLayout" name="verticalLayout_63">
@@ -3186,6 +3186,19 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                   <string>RealSense</string>
                                  </property>
                                  <layout class="QGridLayout" name="gridLayout_73" columnstretch="0,1">
+                                  <item row="3" column="0">
+                                   <spacer name="verticalSpacer_51">
+                                    <property name="orientation">
+                                     <enum>Qt::Vertical</enum>
+                                    </property>
+                                    <property name="sizeHint" stdset="0">
+                                     <size>
+                                      <width>20</width>
+                                      <height>0</height>
+                                     </size>
+                                    </property>
+                                   </spacer>
+                                  </item>
                                   <item row="0" column="1">
                                    <widget class="QLabel" name="label_317">
                                     <property name="text">
@@ -3199,19 +3212,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="2" column="0">
-                                   <spacer name="verticalSpacer_51">
-                                    <property name="orientation">
-                                     <enum>Qt::Vertical</enum>
-                                    </property>
-                                    <property name="sizeHint" stdset="0">
-                                     <size>
-                                      <width>20</width>
-                                      <height>0</height>
-                                     </size>
-                                    </property>
-                                   </spacer>
-                                  </item>
                                   <item row="0" column="0">
                                    <widget class="QComboBox" name="comboBox_realsensePresetRGB">
                                     <property name="sizeAdjustPolicy">
@@ -3269,6 +3269,29 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </item>
                                    </widget>
                                   </item>
+                                  <item row="2" column="1">
+                                   <widget class="QLabel" name="label_367">
+                                    <property name="text">
+                                     <string>Use visual inertial odometry for ZR300.</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="2" column="0">
+                                   <widget class="QCheckBox" name="checkbox_realsenseOdom">
+                                    <property name="text">
+                                     <string/>
+                                    </property>
+                                    <property name="checked">
+                                     <bool>false</bool>
+                                    </property>
+                                   </widget>
+                                  </item>
                                  </layout>
                                 </widget>
                                </item>