RealSense driver: added odometry option (ZR300)

2026-09-02 01:20:25 +08:00 · 2017-04-21 21:24:23 -04:00
parent 2e515f5fc7
commit bc2da10713
8 changed files with 509 additions and 30 deletions
--- a/corelib/src/CameraRGBD.cpp
+++ b/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
-		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);
+		cv::Mat rgb;
+		cv::Mat depthIn;
+
+		{
+			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