Added support to use local keypoints and descriptors from RGBDImage. OdometryROS: added new output topic odom_rgbd_image with features extracted.

include/rtabmap_ros/CommonDataSubscriber.h

@@ -103,9 +103,9 @@ protected:
 				const sensor_msgs::PointCloud2& scan3dMsg,
 				const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
 				const std::vector<rtabmap_ros::GlobalDescriptor> & globalDescriptorMsgs = std::vector<rtabmap_ros::GlobalDescriptor>(),
-				const std::vector<std::vector<rtabmap_ros::KeyPoint> > & localKeyPoints = std::vector<std::vector<rtabmap_ros::KeyPoint> >(),
+				const std::vector<rtabmap_ros::KeyPoint> & localKeyPoints = std::vector<rtabmap_ros::KeyPoint>(),
-				const std::vector<std::vector<rtabmap_ros::Point3f> > & localPoints3d = std::vector<std::vector<rtabmap_ros::Point3f> >(),
+				const std::vector<rtabmap_ros::Point3f> & localPoints3d = std::vector<rtabmap_ros::Point3f>(),
-				const std::vector<cv::Mat> & localDescriptors = std::vector<cv::Mat>()) = 0;
+				const cv::Mat & localDescriptors = cv::Mat()) = 0;
 	virtual void commonLaserScanCallback(
 				const nav_msgs::OdometryConstPtr & odomMsg,
 				const rtabmap_ros::UserDataConstPtr & userDataMsg,

include/rtabmap_ros/CoreWrapper.h

@@ -137,9 +137,9 @@ private:
 				const sensor_msgs::PointCloud2& scan3dMsg,
 				const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
 				const std::vector<rtabmap_ros::GlobalDescriptor> & globalDescriptorMsgs = std::vector<rtabmap_ros::GlobalDescriptor>(),
-				const std::vector<std::vector<rtabmap_ros::KeyPoint> > & localKeyPoints = std::vector<std::vector<rtabmap_ros::KeyPoint> >(),
+				const std::vector<rtabmap_ros::KeyPoint> & localKeyPoints = std::vector<rtabmap_ros::KeyPoint>(),
-				const std::vector<std::vector<rtabmap_ros::Point3f> > & localPoints3d = std::vector<std::vector<rtabmap_ros::Point3f> >(),
+				const std::vector<rtabmap_ros::Point3f> & localPoints3d = std::vector<rtabmap_ros::Point3f>(),
-				const std::vector<cv::Mat> & localDescriptors = std::vector<cv::Mat>());
+				const cv::Mat & localDescriptors = cv::Mat());
 	virtual void commonLaserScanCallback(
 				const nav_msgs::OdometryConstPtr & odomMsg,
 				const rtabmap_ros::UserDataConstPtr & userDataMsg,

include/rtabmap_ros/GuiWrapper.h

@@ -91,9 +91,9 @@ private:
 			const sensor_msgs::PointCloud2& scan3dMsg,
 			const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
 			const std::vector<rtabmap_ros::GlobalDescriptor> & globalDescriptorMsgs = std::vector<rtabmap_ros::GlobalDescriptor>(),
-			const std::vector<std::vector<rtabmap_ros::KeyPoint> > & localKeyPoints = std::vector<std::vector<rtabmap_ros::KeyPoint> >(),
+			const std::vector<rtabmap_ros::KeyPoint> & localKeyPoints = std::vector<rtabmap_ros::KeyPoint>(),
-			const std::vector<std::vector<rtabmap_ros::Point3f> > & localPoints3d = std::vector<std::vector<rtabmap_ros::Point3f> >(),
+			const std::vector<rtabmap_ros::Point3f> & localPoints3d = std::vector<rtabmap_ros::Point3f>(),
-			const std::vector<cv::Mat> & localDescriptors = std::vector<cv::Mat>());
+			const cv::Mat & localDescriptors = cv::Mat());
 	virtual void commonLaserScanCallback(
 			const nav_msgs::OdometryConstPtr & odomMsg,
 			const rtabmap_ros::UserDataConstPtr & userDataMsg,

include/rtabmap_ros/MsgConversion.h

@@ -74,6 +74,7 @@ rtabmap::Transform transformFromPoseMsg(const geometry_msgs::Pose & msg, bool ig
 
 void toCvCopy(const rtabmap_ros::RGBDImage & image, cv_bridge::CvImagePtr & rgb, cv_bridge::CvImagePtr & depth);
 void toCvShare(const rtabmap_ros::RGBDImageConstPtr & image, cv_bridge::CvImageConstPtr & rgb, cv_bridge::CvImageConstPtr & depth);
+void rgbdImageToROS(const rtabmap::SensorData & data, rtabmap_ros::RGBDImage & msg, const std::string & sensorFrameId);
 rtabmap::SensorData rgbdImageFromROS(const rtabmap_ros::RGBDImageConstPtr & image);
 
 // copy data
@@ -90,6 +91,7 @@ cv::KeyPoint keypointFromROS(const rtabmap_ros::KeyPoint & msg);
 void keypointToROS(const cv::KeyPoint & kpt, rtabmap_ros::KeyPoint & msg);
 
 std::vector<cv::KeyPoint> keypointsFromROS(const std::vector<rtabmap_ros::KeyPoint> & msg);
+void keypointsFromROS(const std::vector<rtabmap_ros::KeyPoint> & msg, std::vector<cv::KeyPoint> & kpts, int xShift=0);
 void keypointsToROS(const std::vector<cv::KeyPoint> & kpts, std::vector<rtabmap_ros::KeyPoint> & msg);
 
 rtabmap::GlobalDescriptor globalDescriptorFromROS(const rtabmap_ros::GlobalDescriptor & msg);
@@ -112,8 +114,9 @@ void points2fToROS(const std::vector<cv::Point2f> & kpts, std::vector<rtabmap_ro
 cv::Point3f point3fFromROS(const rtabmap_ros::Point3f & msg);
 void point3fToROS(const cv::Point3f & pt, rtabmap_ros::Point3f & msg);
 
-std::vector<cv::Point3f> points3fFromROS(const std::vector<rtabmap_ros::Point3f> & msg);
+std::vector<cv::Point3f> points3fFromROS(const std::vector<rtabmap_ros::Point3f> & msg, const rtabmap::Transform & transform = rtabmap::Transform());
-void points3fToROS(const std::vector<cv::Point3f> & pts, std::vector<rtabmap_ros::Point3f> & msg);
+void points3fFromROS(const std::vector<rtabmap_ros::Point3f> & msg, std::vector<cv::Point3f> & points3, const rtabmap::Transform & transform = rtabmap::Transform());
+void points3fToROS(const std::vector<cv::Point3f> & pts, std::vector<rtabmap_ros::Point3f> & msg, const rtabmap::Transform & transform = rtabmap::Transform());
 
 rtabmap::CameraModel cameraModelFromROS(
 		const sensor_msgs::CameraInfo & camInfo,
@@ -213,7 +216,13 @@ bool convertRGBDMsgs(
 		cv::Mat & depth,
 		std::vector<rtabmap::CameraModel> & cameraModels,
 		tf::TransformListener & listener,
-		double waitForTransform);
+		double waitForTransform,
+		const std::vector<std::vector<rtabmap_ros::KeyPoint> > & localKeyPointsMsgs = std::vector<std::vector<rtabmap_ros::KeyPoint> >(),
+		const std::vector<std::vector<rtabmap_ros::Point3f> > & localPoints3dMsgs = std::vector<std::vector<rtabmap_ros::Point3f> >(),
+		const std::vector<cv::Mat> & localDescriptorsMsgs = std::vector<cv::Mat>(),
+		std::vector<cv::KeyPoint> * localKeyPoints = 0,
+		std::vector<cv::Point3f> * localPoints3d = 0,
+		cv::Mat * localDescriptors = 0);
 
 bool convertStereoMsg(
 		const cv_bridge::CvImageConstPtr& leftImageMsg,

include/rtabmap_ros/OdometryROS.h

@@ -57,7 +57,7 @@ public:
 	OdometryROS(bool stereoParams, bool visParams, bool icpParams);
 	virtual ~OdometryROS();
 
-	void processData(const rtabmap::SensorData & data, const ros::Time & stamp);
+	void processData(const rtabmap::SensorData & data, const ros::Time & stamp, const std::string & sensorFrameId);
 
 	bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 	bool resetToPose(rtabmap_ros::ResetPose::Request&, rtabmap_ros::ResetPose::Response&);
@@ -115,6 +115,7 @@ private:
 	ros::Publisher odomLocalMap_;
 	ros::Publisher odomLocalScanMap_;
 	ros::Publisher odomLastFrame_;
+	ros::Publisher odomRgbdImagePub_;
 	ros::ServiceServer resetSrv_;
 	ros::ServiceServer resetToPoseSrv_;
 	ros::ServiceServer pauseSrv_;
@@ -142,7 +143,7 @@ private:
 	bool waitIMUToinit_;
 	bool imuProcessed_;
 	std::map<double, rtabmap::IMU> imus_;
-	std::pair<rtabmap::SensorData, ros::Time> bufferedData_;
+	std::pair<rtabmap::SensorData, std::pair<ros::Time, std::string> > bufferedData_;
 };
 
 }

launch/tests/test_use_odom_features.launch

@@ -0,0 +1,53 @@
+
+<launch>
+
+  <!-- Kinect: -->
+  <include file="$(find freenect_launch)/launch/freenect.launch">
+    <arg name="depth_registration" value="true" />
+  </include>
+  
+  <arg name="frame_id"                default="camera_link"/> 
+  <arg name="rtabmap_args"            default="--delete_db_on_start"/>   <!-- delete_db_on_start, udebug -->  
+  <arg name="odom_args"               default="$(arg rtabmap_args)"/>
+   
+  <!-- RGB-D related topics -->
+  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
+  <arg name="depth_topic"             default="/camera/depth_registered/image_raw" />
+  <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
+ 
+  <group ns="camera"> 
+  
+    <!-- Use RGBD synchronization -->
+    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync camera_nodelet_manager">
+      <remap from="rgb/image"         to="$(arg rgb_topic)"/>
+      <remap from="depth/image"       to="$(arg depth_topic)"/>
+      <remap from="rgb/camera_info"   to="$(arg camera_info_topic)"/>
+    </node>
+  
+    <!-- RGB-D Odometry -->
+      <node pkg="nodelet" type="nodelet" name="rgbd_odometry" args="load rtabmap_ros/rgbd_odometry camera_nodelet_manager $(arg odom_args)">
+        <param name="frame_id"        type="string" value="$(arg frame_id)"/>
+        <param name="subscribe_depth" type="bool"   value="false"/>
+        <param name="subscribe_rgbd"  type="bool"   value="true"/>
+        <param name="keep_color"      type="bool"   value="true"/>
+      </node>
+  
+    <!-- RTAB-Map -->
+    <node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" args="$(arg rtabmap_args)" output="screen">
+      <param name="frame_id"        type="string" value="$(arg frame_id)"/>
+      <param name="subscribe_rgbd"  type="bool"   value="true"/>
+      <param name="approx_sync"     type="bool"   value="false"/>
+      <remap from="rgbd_image" to="odom_rgbd_image"/>
+    </node>
+  
+    <!-- Visualisation -->
+    <node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" output="screen">
+      <param name="frame_id" type="string" value="$(arg frame_id)"/>
+      <param name="subscribe_rgbd"  type="bool"   value="true"/>
+      <param name="subscribe_odom_info"  type="bool"   value="true"/>
+      <param name="approx_sync"     type="bool"   value="false"/>
+    </node>
+    
+  </group>
+
+</launch>

src/CommonDataSubscriber.cpp

@@ -1021,9 +1021,9 @@ void CommonDataSubscriber::commonSingleDepthCallback(
 				scan3dMsg,
 				odomInfoMsg,
 				globalDescriptorMsgs,
-				localKeyPointsMsgs,
+				localKeyPoints,
-				localPoints3dMsgs,
+				localPoints3d,
-				localDescriptorsMsgs);
+				localDescriptors);
 	}
 }
 

src/CoreWrapper.cpp

@@ -1135,13 +1135,16 @@ void CoreWrapper::commonDepthCallbackImpl(
 		const sensor_msgs::PointCloud2& scan3dMsg,
 		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
 		const std::vector<rtabmap_ros::GlobalDescriptor> & globalDescriptorMsgs,
-		const std::vector<std::vector<rtabmap_ros::KeyPoint> > & localKeyPoints,
+		const std::vector<std::vector<rtabmap_ros::KeyPoint> > & localKeyPointsMsgs,
-		const std::vector<std::vector<rtabmap_ros::Point3f> > & localPoints3d,
+		const std::vector<std::vector<rtabmap_ros::Point3f> > & localPoints3dMsgs,
-		const std::vector<cv::Mat> & localDescriptors)
+		const std::vector<cv::Mat> & localDescriptorsMsgs)
 {
 	cv::Mat rgb;
 	cv::Mat depth;
 	std::vector<rtabmap::CameraModel> cameraModels;
+	std::vector<cv::KeyPoint> keypoints;
+	std::vector<cv::Point3f> points;
+	cv::Mat descriptors;
 	if(!rtabmap_ros::convertRGBDMsgs(
 			imageMsgs,
 			depthMsgs,
@@ -1153,7 +1156,13 @@ void CoreWrapper::commonDepthCallbackImpl(
 			depth,
 			cameraModels,
 			tfListener_,
-			waitForTransform_?waitForTransformDuration_:0.0))
+			waitForTransform_?waitForTransformDuration_:0.0,
+			localKeyPointsMsgs,
+			localPoints3dMsgs,
+			localDescriptorsMsgs,
+			&keypoints,
+			&points,
+			&descriptors))
 	{
 		NODELET_ERROR("Could not convert rgb/depth msgs! Aborting rtabmap update...");
 		return;
@@ -1259,6 +1268,13 @@ void CoreWrapper::commonDepthCallbackImpl(
 		data.setGlobalDescriptors(rtabmap_ros::globalDescriptorsFromROS(globalDescriptorMsgs));
 	}
 
+	if(!keypoints.empty())
+	{
+		UASSERT(points.empty() || points.size() == keypoints.size());
+		UASSERT(descriptors.empty() || descriptors.rows == (int)keypoints.size());
+		data.setFeatures(keypoints, points, descriptors);
+	}
+
 	process(lastPoseStamp_,
 			data,
 			lastPose_,
@@ -1279,9 +1295,9 @@ void CoreWrapper::commonStereoCallback(
 		const sensor_msgs::PointCloud2& scan3dMsg,
 		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
 		const std::vector<rtabmap_ros::GlobalDescriptor> & globalDescriptorMsgs,
-		const std::vector<std::vector<rtabmap_ros::KeyPoint> > & localKeyPoints,
+		const std::vector<rtabmap_ros::KeyPoint> & localKeyPointsMsg,
-		const std::vector<std::vector<rtabmap_ros::Point3f> > & localPoints3d,
+		const std::vector<rtabmap_ros::Point3f> & localPoints3dMsg,
-		const std::vector<cv::Mat> & localDescriptors)
+		const cv::Mat & localDescriptorsMsg)
 {
 	std::string odomFrameId = odomFrameId_;
 	if(odomMsg.get())
@@ -1390,12 +1406,27 @@ void CoreWrapper::commonStereoCallback(
 		depthImages[0] = imgDepth;
 		cameraInfos[0] = leftCamInfoMsg;
 
+		std::vector<std::vector<rtabmap_ros::KeyPoint> > localKeyPointsMsgs;
+		std::vector<std::vector<rtabmap_ros::Point3f> > localPoints3dMsgs;
+		std::vector<cv::Mat> localDescriptorsMsgs;
+		if(!localKeyPointsMsg.empty())
+		{
+			localKeyPointsMsgs.push_back(localKeyPointsMsg);
+		}
+		if(!localPoints3dMsg.empty())
+		{
+			localPoints3dMsgs.push_back(localPoints3dMsg);
+		}
+		if(!localDescriptorsMsg.empty())
+		{
+			localDescriptorsMsgs.push_back(localDescriptorsMsg);
+		}
 		commonDepthCallbackImpl(odomFrameId,
 				rtabmap_ros::UserDataConstPtr(),
 				rgbImages, depthImages, cameraInfos,
 				scan2dMsg, scan3dMsg,
 				odomInfoMsg,
-				globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+				globalDescriptorMsgs, localKeyPointsMsgs, localPoints3dMsgs, localDescriptorsMsgs);
 		return;
 	}
 
@@ -1472,6 +1503,24 @@ void CoreWrapper::commonStereoCallback(
 		data.setGlobalDescriptors(rtabmap_ros::globalDescriptorsFromROS(globalDescriptorMsgs));
 	}
 
+	std::vector<cv::KeyPoint> keypoints;
+	std::vector<cv::Point3f> points;
+	if(!localKeyPointsMsg.empty())
+	{
+		keypoints = rtabmap_ros::keypointsFromROS(localKeyPointsMsg);
+	}
+	if(!localPoints3dMsg.empty())
+	{
+		// Points should be in base frame
+		points = rtabmap_ros::points3fFromROS(localPoints3dMsg, stereoModel.localTransform().inverse());
+	}
+	if(!keypoints.empty())
+	{
+		UASSERT(points.empty() || points.size() == keypoints.size());
+		UASSERT(localDescriptorsMsg.empty() || localDescriptorsMsg.rows == (int)keypoints.size());
+		data.setFeatures(keypoints, points, localDescriptorsMsg);
+	}
+
 	process(lastPoseStamp_,
 			data,
 			lastPose_,

src/GuiWrapper.cpp

@@ -607,9 +607,9 @@ void GuiWrapper::commonStereoCallback(
 		const sensor_msgs::PointCloud2& scan3dMsg,
 		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
 		const std::vector<rtabmap_ros::GlobalDescriptor> & globalDescriptorMsgs,
-		const std::vector<std::vector<rtabmap_ros::KeyPoint> > & localKeyPoints,
+		const std::vector<rtabmap_ros::KeyPoint> & localKeyPoints,
-		const std::vector<std::vector<rtabmap_ros::Point3f> > & localPoints3d,
+		const std::vector<rtabmap_ros::Point3f> & localPoints3d,
-		const std::vector<cv::Mat> & localDescriptors)
+		const cv::Mat & localDescriptors)
 {
 	std_msgs::Header odomHeader;
 	if(odomMsg.get())

src/MsgConversion.cpp

@@ -202,6 +202,82 @@ void toCvShare(const rtabmap_ros::RGBDImageConstPtr & image, cv_bridge::CvImageC
 	}
 }
 
+void rgbdImageToROS(const rtabmap::SensorData & data, rtabmap_ros::RGBDImage & msg, const std::string & sensorFrameId)
+{
+	std_msgs::Header header;
+	header.frame_id = sensorFrameId;
+	header.stamp = ros::Time(data.stamp());
+	rtabmap::Transform localTransform;
+	if(data.cameraModels().size()>1)
+	{
+		UERROR("Cannot convert multi-camera data to rgbd image");
+		return;
+	}
+	if(data.cameraModels().size() == 1)
+	{
+		//rgb+depth
+		rtabmap_ros::cameraModelToROS(data.cameraModels().front(), msg.rgb_camera_info);
+		msg.rgb_camera_info.header = header;
+		localTransform = data.cameraModels().front().localTransform();
+	}
+	else
+	{
+		//stereo
+		rtabmap_ros::cameraModelToROS(data.stereoCameraModel().left(), msg.rgb_camera_info);
+		rtabmap_ros::cameraModelToROS(data.stereoCameraModel().right(), msg.depth_camera_info);
+		msg.rgb_camera_info.header = header;
+		msg.depth_camera_info.header = header;
+		localTransform = data.stereoCameraModel().localTransform();
+	}
+
+	if(!data.imageRaw().empty())
+	{
+		cv_bridge::CvImage cvImg;
+		cvImg.header = header;
+		cvImg.image = data.imageRaw();
+		UASSERT(data.imageRaw().type()==CV_8UC1 || data.imageRaw().type()==CV_8UC3);
+		cvImg.encoding = data.imageRaw().type()==CV_8UC1?sensor_msgs::image_encodings::MONO8:sensor_msgs::image_encodings::BGR8;
+		cvImg.toImageMsg(msg.rgb);
+	}
+	else if(!data.imageCompressed().empty())
+	{
+		ROS_ERROR("Conversion of compressed SensorData to RGBDImage is not implemented...");
+	}
+
+	if(!data.depthOrRightRaw().empty())
+	{
+		cv_bridge::CvImage cvDepth;
+		cvDepth.header = header;
+		cvDepth.image = data.depthOrRightRaw();
+		UASSERT(data.depthOrRightRaw().type()==CV_8UC1 || data.depthOrRightRaw().type()==CV_16UC1 || data.depthOrRightRaw().type()==CV_32FC1);
+		cvDepth.encoding = data.depthOrRightRaw().type()==CV_8UC1?sensor_msgs::image_encodings::MONO8:data.depthOrRightRaw().type()==CV_16UC1?sensor_msgs::image_encodings::TYPE_16UC1:sensor_msgs::image_encodings::TYPE_32FC1;
+		cvDepth.toImageMsg(msg.depth);
+	}
+	else if(!data.depthOrRightCompressed().empty())
+	{
+		ROS_ERROR("Conversion of compressed SensorData to RGBDImage is not implemented...");
+	}
+
+	//convert features
+	if(!data.keypoints().empty())
+	{
+		rtabmap_ros::keypointsToROS(data.keypoints(), msg.key_points);
+	}
+	if(!data.keypoints3D().empty())
+	{
+		rtabmap_ros::points3fToROS(data.keypoints3D(), msg.points, localTransform.inverse());
+	}
+	if(!data.descriptors().empty())
+	{
+		msg.descriptors = rtabmap::compressData(data.descriptors());
+	}
+	if(!data.globalDescriptors().empty())
+	{
+		rtabmap_ros::globalDescriptorToROS(data.globalDescriptors().front(), msg.global_descriptor);
+		msg.global_descriptor.header = header;
+	}
+}
+
 rtabmap::SensorData rgbdImageFromROS(const rtabmap_ros::RGBDImageConstPtr & image)
 {
 	rtabmap::SensorData data;
@@ -499,6 +575,17 @@ std::vector<cv::KeyPoint> keypointsFromROS(const std::vector<rtabmap_ros::KeyPoi
 	return v;
 }
 
+void keypointsFromROS(const std::vector<rtabmap_ros::KeyPoint> & msg, std::vector<cv::KeyPoint> & kpts, int xShift)
+{
+	size_t outCurrentIndex = kpts.size();
+	kpts.resize(kpts.size()+msg.size());
+	for(unsigned int i=0; i<msg.size(); ++i)
+	{
+		kpts[outCurrentIndex+i] = keypointFromROS(msg[i]);
+		kpts[outCurrentIndex+i].pt.x += xShift;
+	}
+}
+
 void keypointsToROS(const std::vector<cv::KeyPoint> & kpts, std::vector<rtabmap_ros::KeyPoint> & msg)
 {
 	msg.resize(kpts.size());
@@ -629,22 +716,51 @@ void point3fToROS(const cv::Point3f & pt, rtabmap_ros::Point3f & msg)
 	msg.z = pt.z;
 }
 
-std::vector<cv::Point3f> points3fFromROS(const std::vector<rtabmap_ros::Point3f> & msg)
+std::vector<cv::Point3f> points3fFromROS(const std::vector<rtabmap_ros::Point3f> & msg, const rtabmap::Transform & transform)
 {
+	bool transformPoints = !transform.isNull() && !transform.isIdentity();
 	std::vector<cv::Point3f> v(msg.size());
 	for(unsigned int i=0; i<msg.size(); ++i)
 	{
 		v[i] = point3fFromROS(msg[i]);
+		if(transformPoints)
+		{
+			v[i] = rtabmap::util3d::transformPoint(v[i], transform);
+		}
 	}
 	return v;
 }
 
-void points3fToROS(const std::vector<cv::Point3f> & pts, std::vector<rtabmap_ros::Point3f> & msg)
+void points3fFromROS(const std::vector<rtabmap_ros::Point3f> & msg, std::vector<cv::Point3f> & points3, const rtabmap::Transform & transform)
 {
-	msg.resize(pts.size());
+	size_t currentIndex = points3.size();
+	points3.resize(points3.size()+msg.size());
+	bool transformPoint = !transform.isNull() && !transform.isIdentity();
 	for(unsigned int i=0; i<msg.size(); ++i)
 	{
-		point3fToROS(pts[i], msg[i]);
+		points3[currentIndex+i] = point3fFromROS(msg[i]);
+		if(transformPoint)
+		{
+			points3[currentIndex+i] = rtabmap::util3d::transformPoint(points3[currentIndex+i], transform);
+		}
+	}
+}
+
+void points3fToROS(const std::vector<cv::Point3f> & pts, std::vector<rtabmap_ros::Point3f> & msg, const rtabmap::Transform & transform)
+{
+	msg.resize(pts.size());
+	bool transformPoints = !transform.isNull() && !transform.isIdentity();
+	for(unsigned int i=0; i<msg.size(); ++i)
+	{
+		if(transformPoints)
+		{
+			cv::Point3f pt = rtabmap::util3d::transformPoint(pts[i], transform);
+			point3fToROS(pt, msg[i]);
+		}
+		else
+		{
+			point3fToROS(pts[i], msg[i]);
+		}
 	}
 }
 
@@ -1577,7 +1693,13 @@ bool convertRGBDMsgs(
 		cv::Mat & depth,
 		std::vector<rtabmap::CameraModel> & cameraModels,
 		tf::TransformListener & listener,
-		double waitForTransform)
+		double waitForTransform,
+		const std::vector<std::vector<rtabmap_ros::KeyPoint> > & localKeyPointsMsgs,
+		const std::vector<std::vector<rtabmap_ros::Point3f> > & localPoints3dMsgs,
+		const std::vector<cv::Mat> & localDescriptorsMsgs,
+		std::vector<cv::KeyPoint> * localKeyPoints,
+		std::vector<cv::Point3f> * localPoints3d,
+		cv::Mat * localDescriptors)
 {
 	UASSERT(imageMsgs.size()>0 &&
 			(imageMsgs.size() == depthMsgs.size() || depthMsgs.empty()) &&
@@ -1728,6 +1850,20 @@ bool convertRGBDMsgs(
 		}
 
 		cameraModels.push_back(rtabmap_ros::cameraModelFromROS(cameraInfoMsgs[i], localTransform));
+
+		if(localKeyPoints && localKeyPointsMsgs.size() == imageMsgs.size())
+		{
+			rtabmap_ros::keypointsFromROS(localKeyPointsMsgs[i], *localKeyPoints, imageWidth*i);
+		}
+		if(localPoints3d && localPoints3dMsgs.size() == imageMsgs.size())
+		{
+			// Points should be in base frame
+			rtabmap_ros::points3fFromROS(localPoints3dMsgs[i], *localPoints3d, localTransform);
+		}
+		if(localDescriptors && localDescriptorsMsgs.size() == imageMsgs.size())
+		{
+			localDescriptors->push_back(localDescriptorsMsgs[i]);
+		}
 	}
 	return true;
 }
@@ -1753,13 +1889,13 @@ bool convertStereoMsg(
 		leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
 		leftImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
 		leftImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 || 
-	        leftImageMsg->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
+		leftImageMsg->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
 		leftImageMsg->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0) ||
 		!(rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
 		rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
 		rightImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
 		rightImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 || 
-	        rightImageMsg->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
+		rightImageMsg->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
 		rightImageMsg->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0))
 	{
 		ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8,bgra8,rgba8");

src/OdometryROS.cpp

@@ -118,6 +118,7 @@ void OdometryROS::onInit()
 	odomLocalMap_ = nh.advertise<sensor_msgs::PointCloud2>("odom_local_map", 1);
 	odomLocalScanMap_ = nh.advertise<sensor_msgs::PointCloud2>("odom_local_scan_map", 1);
 	odomLastFrame_ = nh.advertise<sensor_msgs::PointCloud2>("odom_last_frame", 1);
+	odomRgbdImagePub_ = nh.advertise<rtabmap_ros::RGBDImage>("odom_rgbd_image", 1);
 
 	Transform initialPose = Transform::getIdentity();
 	std::string initialPoseStr;
@@ -502,7 +503,7 @@ void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg)
 			{
 				SensorData data = bufferedData_.first;
 				bufferedData_.first = SensorData();
-				processData(data, bufferedData_.second);
+				processData(data, bufferedData_.second.first, bufferedData_.second.second);
 			}
 			
 			if(imus_.size() > 1000)
@@ -513,7 +514,7 @@ void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg)
 	}
 }
 
-void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
+void OdometryROS::processData(const SensorData & data, const ros::Time & stamp, const std::string & sensorFrameId)
 {
 	if((waitIMUToinit_ && !imuProcessed_) && odometry_->framesProcessed() == 0 && odometry_->getPose().isIdentity() && imus_.empty())
 	{
@@ -536,7 +537,8 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 						bufferedData_.first.stamp(), data.stamp(), imus_.empty()?0:imus_.rbegin()->first);
 			}
 			bufferedData_.first = data;
-			bufferedData_.second = stamp;
+			bufferedData_.second.first = stamp;
+			bufferedData_.second.second = sensorFrameId;
 			return;
 		}
 		// process all imu data up to current image stamp (or just after so that underlying odom approach can do interpolation of imu at image stamp)
@@ -900,6 +902,22 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 		odomInfoPub_.publish(infoMsg);
 	}
 
+	if(!data.imageRaw().empty() && odomRgbdImagePub_.getNumSubscribers())
+	{
+		if(!sensorFrameId.empty())
+		{
+			rtabmap_ros::RGBDImage msg;
+			rtabmap_ros::rgbdImageToROS(dataCpy, msg, sensorFrameId);
+			msg.header.stamp = stamp; // use corresponding time stamp to image
+			msg.header.frame_id = sensorFrameId;
+			odomRgbdImagePub_.publish(msg);
+		}
+		else
+		{
+			ROS_WARN("Sensor frame not set, cannot convert SensorData to RGBDImage");
+		}
+	}
+
 	if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
 	{
 		if(visParams_)

src/nodelets/icp_odometry.cpp

@@ -442,7 +442,7 @@ private:
 				0,
 				rtabmap_ros::timestampFromROS(scanMsg->header.stamp));
 
-		this->processData(data, scanMsg->header.stamp);
+		this->processData(data, scanMsg->header.stamp, "");
 	}
 
 	void callbackCloud(const sensor_msgs::PointCloud2ConstPtr& pointCloudMsg)
@@ -721,7 +721,7 @@ private:
 				0,
 				rtabmap_ros::timestampFromROS(cloudMsg.header.stamp));
 
-		this->processData(data, cloudMsg.header.stamp);
+		this->processData(data, cloudMsg.header.stamp, cloudMsg.header.frame_id);
 	}
 
 protected:

src/nodelets/rgbd_odometry.cpp

@@ -69,7 +69,8 @@ public:
 		exactSync3_(0),
 		approxSync4_(0),
 		exactSync4_(0),
-		queueSize_(5)
+		queueSize_(5),
+		keepColor_(false)
 	{
 	}
 
@@ -136,11 +137,13 @@ private:
 		{
 			NODELET_FATAL("Only 4 cameras maximum supported yet.");
 		}
+		pnh.param("keep_color", keepColor_, keepColor_);
 
 		NODELET_INFO("RGBDOdometry: approx_sync    = %s", approxSync?"true":"false");
 		NODELET_INFO("RGBDOdometry: queue_size     = %d", queueSize_);
 		NODELET_INFO("RGBDOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
 		NODELET_INFO("RGBDOdometry: rgbd_cameras   = %d", rgbdCameras);
+		NODELET_INFO("RGBDOdometry: keep_color     = %s", keepColor_?"true":"false");
 
 		std::string subscribedTopicsMsg;
 		if(subscribeRGBD)
@@ -391,7 +394,14 @@ private:
 			if(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
 			   rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
 			{
-				ptrImage = cv_bridge::cvtColor(rgbImages[i], "mono8");
+				if(keepColor_ && rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0)
+				{
+					ptrImage = cv_bridge::cvtColor(rgbImages[i], "bgr8");
+				}
+				else
+				{
+					ptrImage = cv_bridge::cvtColor(rgbImages[i], "mono8");
+				}
 			}
 
 			cv_bridge::CvImageConstPtr ptrDepth = depthImages[i];
@@ -437,7 +447,7 @@ private:
 				0,
 				rtabmap_ros::timestampFromROS(higherStamp));
 
-		this->processData(data, higherStamp);
+		this->processData(data, higherStamp, rgbImages.size()==1?rgbImages[0]->header.frame_id:"");
 	}
 
 	void callback(
@@ -647,6 +657,7 @@ private:
 	typedef message_filters::sync_policies::ExactTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyExactSync4Policy;
 	message_filters::Synchronizer<MyExactSync4Policy> * exactSync4_;
 	int queueSize_;
+	bool keepColor_;
 };
 
 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::RGBDOdometry, nodelet::Nodelet);

src/nodelets/rgbdicp_odometry.cpp

@@ -73,6 +73,7 @@ public:
 		approxCloudSync_(0),
 		exactCloudSync_(0),
 		queueSize_(5),
+		keepColor_(false),
 		scanCloudMaxPoints_(0),
 		scanVoxelSize_(0.0),
 		scanNormalK_(0),
@@ -122,6 +123,7 @@ private:
 			pnh.param("scan_cloud_normal_k", scanNormalK_, scanNormalK_);
 		}
 		pnh.param("scan_normal_radius", scanNormalRadius_, scanNormalRadius_);
+		pnh.param("keep_color", keepColor_, keepColor_);
 
 		NODELET_INFO("RGBDIcpOdometry: approx_sync           = %s", approxSync?"true":"false");
 		NODELET_INFO("RGBDIcpOdometry: queue_size            = %d", queueSize_);
@@ -130,6 +132,7 @@ private:
 		NODELET_INFO("RGBDIcpOdometry: scan_voxel_size       = %f", scanVoxelSize_);
 		NODELET_INFO("RGBDIcpOdometry: scan_normal_k         = %d", scanNormalK_);
 		NODELET_INFO("RGBDIcpOdometry: scan_normal_radius    = %f", scanNormalRadius_);
+		NODELET_INFO("RGBDIcpOdometry: keep_color            = %s", keepColor_?"true":"false");
 
 		ros::NodeHandle rgb_nh(nh, "rgb");
 		ros::NodeHandle depth_nh(nh, "depth");
@@ -277,7 +280,10 @@ private:
 			if(image->data.size() && depth->data.size() && cameraInfo->K[4] != 0)
 			{
 				rtabmap::CameraModel rtabmapModel = rtabmap_ros::cameraModelFromROS(*cameraInfo, localTransform);
-				cv_bridge::CvImagePtr ptrImage = cv_bridge::toCvCopy(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8");
+				cv_bridge::CvImagePtr ptrImage = cv_bridge::toCvCopy(image,
+						image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0 ||
+						image->encoding.compare(sensor_msgs::image_encodings::MONO8)==0?"":
+								keepColor_ && image->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0?"bgr8":"mono8");
 				cv_bridge::CvImagePtr ptrDepth = cv_bridge::toCvCopy(depth);
 
 				cv::Mat scan;
@@ -412,7 +418,7 @@ private:
 						0,
 						rtabmap_ros::timestampFromROS(stamp));
 
-				this->processData(data, stamp);
+				this->processData(data, stamp, image->header.frame_id);
 			}
 		}
 	}
@@ -462,6 +468,7 @@ private:
 	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2> MyExactCloudSyncPolicy;
 	message_filters::Synchronizer<MyExactCloudSyncPolicy> * exactCloudSync_;
 	int queueSize_;
+	bool keepColor_;
 	int scanCloudMaxPoints_;
 	double scanVoxelSize_;
 	int scanNormalK_;

src/nodelets/stereo_odometry.cpp

@@ -63,7 +63,8 @@ public:
 		rtabmap_ros::OdometryROS(true, true, false),
 		approxSync_(0),
 		exactSync_(0),
-		queueSize_(5)
+		queueSize_(5),
+		keepColor_(false)
 	{
 	}
 
@@ -90,10 +91,12 @@ private:
 		pnh.param("approx_sync", approxSync, approxSync);
 		pnh.param("queue_size", queueSize_, queueSize_);
 		pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD);
+		pnh.param("keep_color", keepColor_, keepColor_);
 
 		NODELET_INFO("StereoOdometry: approx_sync = %s", approxSync?"true":"false");
 		NODELET_INFO("StereoOdometry: queue_size = %d", queueSize_);
 		NODELET_INFO("StereoOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
+		NODELET_INFO("StereoOdometry: keep_color = %s", keepColor_?"true":"false");
 
 		std::string subscribedTopicsMsg;
 		if(subscribeRGBD)
@@ -261,8 +264,10 @@ private:
 						shown = true;
 					}
 				}
-
+				cv_bridge::CvImagePtr ptrImageLeft = cv_bridge::toCvCopy(imageRectLeft,
-				cv_bridge::CvImagePtr ptrImageLeft = cv_bridge::toCvCopy(imageRectLeft, "mono8");
+						imageRectLeft->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0 ||
+						imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8)==0?"":
+							keepColor_ && imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0?"bgr8":"mono8");
 				cv_bridge::CvImagePtr ptrImageRight = cv_bridge::toCvCopy(imageRectRight, "mono8");
 
 				UTimer stepTimer;
@@ -275,7 +280,7 @@ private:
 						0,
 						rtabmap_ros::timestampFromROS(stamp));
 
-				this->processData(data, stamp);
+				this->processData(data, stamp, imageRectLeft->header.frame_id);
 			}
 			else
 			{
@@ -392,7 +397,19 @@ private:
 					}
 				}
 
-				cv_bridge::CvImagePtr ptrImageLeft = cv_bridge::cvtColor(imageRectLeft, "mono8");
+				cv_bridge::CvImageConstPtr ptrImageLeft = imageRectLeft;
+				if(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
+				   imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
+				{
+					if(keepColor_ && imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0)
+					{
+						ptrImageLeft = cv_bridge::cvtColor(imageRectLeft, "bgr8");
+					}
+					else
+					{
+						ptrImageLeft = cv_bridge::cvtColor(imageRectLeft, "mono8");
+					}
+				}
 				cv_bridge::CvImagePtr ptrImageRight = cv_bridge::cvtColor(imageRectRight, "mono8");
 
 				UTimer stepTimer;
@@ -405,7 +422,7 @@ private:
 						0,
 						rtabmap_ros::timestampFromROS(stamp));
 
-				this->processData(data, stamp);
+				this->processData(data, stamp, image->header.frame_id);
 			}
 			else
 			{
@@ -443,6 +460,7 @@ private:
 	message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
 	ros::Subscriber rgbdSub_;
 	int queueSize_;
+	bool keepColor_;
 };
 
 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::StereoOdometry, nodelet::Nodelet);