From 6634d92483f3165b191faec35324a9d1e4ba845f Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@f169173b-cf89-36c8-b27e-44dbe73f0c83>
Date: Tue, 19 Aug 2014 20:13:43 +0000
Subject: [PATCH] ros-pkg: added new depth output 16U (mm) to
 disparity_to_depth nodelet

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1658 f169173b-cf89-36c8-b27e-44dbe73f0c83
---
 src/nodelets/disparity_to_depth.cpp | 58 +++++++++++++++++++++++------
 1 file changed, 47 insertions(+), 11 deletions(-)

diff --git a/src/nodelets/disparity_to_depth.cpp b/src/nodelets/disparity_to_depth.cpp
index c062a096..8abb16b7 100644
--- a/src/nodelets/disparity_to_depth.cpp
+++ b/src/nodelets/disparity_to_depth.cpp
@@ -54,7 +54,8 @@ private:
 		ros::NodeHandle & pnh = getPrivateNodeHandle();
 
 		image_transport::ImageTransport it(nh);
-		pub_ = it.advertise("depth", 1);
+		pub32f_ = it.advertise("depth", 1);
+		pub16u_ = it.advertise("depth_raw", 1);
 		sub_ = nh.subscribe("disparity", 1, &DisparityToDepth::callback, this);
 	}
 
@@ -66,12 +67,24 @@ private:
 			return;
 		}
 
-		if(pub_.getNumSubscribers())
+		bool publish32f = pub32f_.getNumSubscribers();
+		bool publish16u = pub16u_.getNumSubscribers();
+
+		if(publish32f || publish16u)
 		{
 			// sensor_msgs::image_encodings::TYPE_32FC1
 			cv::Mat disparity(disparityMsg->image.height, disparityMsg->image.width, CV_32FC1, const_cast<uchar*>(disparityMsg->image.data.data()));
 
-			cv::Mat depth = cv::Mat::zeros(disparity.rows, disparity.cols, CV_32F);
+			cv::Mat depth32f;
+			cv::Mat depth16u;
+			if(publish32f)
+			{
+				depth32f = cv::Mat::zeros(disparity.rows, disparity.cols, CV_32F);
+			}
+			if(publish16u)
+			{
+				depth16u = cv::Mat::zeros(disparity.rows, disparity.cols, CV_16U);
+			}
 			for (int i = 0; i < disparity.rows; i++)
 			{
 				for (int j = 0; j < disparity.cols; j++)
@@ -80,23 +93,46 @@ private:
 					if (disparity_value > disparityMsg->min_disparity && disparity_value < disparityMsg->max_disparity)
 					{
 						// baseline * focal / disparity
-						depth.at<float>(i,j) = disparityMsg->T * disparityMsg->f / disparity_value;
+						float depth = disparityMsg->T * disparityMsg->f / disparity_value;
+						if(publish32f)
+						{
+							depth32f.at<float>(i,j) = depth;
+						}
+						if(publish16u)
+						{
+							depth16u.at<unsigned short>(i,j) = (unsigned short)(depth*1000.0f);
+						}
 					}
 				}
 			}
 
-			// convert to ROS sensor_msg::Image
-			cv_bridge::CvImage cvDepth(disparityMsg->header, sensor_msgs::image_encodings::TYPE_32FC1, depth);
-			sensor_msgs::Image depthMsg;
-			cvDepth.toImageMsg(depthMsg);
+			if(publish32f)
+			{
+				// convert to ROS sensor_msg::Image
+				cv_bridge::CvImage cvDepth(disparityMsg->header, sensor_msgs::image_encodings::TYPE_32FC1, depth32f);
+				sensor_msgs::Image depthMsg;
+				cvDepth.toImageMsg(depthMsg);
 
-			//publish the message
-			pub_.publish(depthMsg);
+				//publish the message
+				pub32f_.publish(depthMsg);
+			}
+
+			if(publish16u)
+			{
+				// convert to ROS sensor_msg::Image
+				cv_bridge::CvImage cvDepth(disparityMsg->header, sensor_msgs::image_encodings::TYPE_16UC1, depth16u);
+				sensor_msgs::Image depthMsg;
+				cvDepth.toImageMsg(depthMsg);
+
+				//publish the message
+				pub16u_.publish(depthMsg);
+			}
 		}
 }
 
 private:
-	image_transport::Publisher pub_;
+	image_transport::Publisher pub32f_;
+	image_transport::Publisher pub16u_;
 	ros::Subscriber sub_;
 };