ROS回顾学习(9): 单通道深度图像转化为彩色图发布

研究背景：将深度图像转化为有颜色的深度图，在网上好像没有找到相关的资料，那就暂时把我自己的方法记录下来吧。

1. 主函数如下

订阅深度图像节点，发布染色后的深度图像，注意image_transport的使用即可。

	int main(int argc, char** argv)
	{
		ros::init(argc, argv, "ColouringDepthImage");
		ros::NodeHandle nh;
		image_transport::ImageTransport it(nh);
		
		image_transport::Subscriber sub1 = it.subscribe("/camera/depth/image_raw", 1, depth_Callback);
		pubImage = it.advertise("/depth/image", 1);
		
		cv::namedWindow("coloured_depth", CV_WINDOW_NORMAL);
		ros::spin();    
		return 0;
	}

2. 回调函数

在回调中，如果你不知道的接收图像话题的数据格式，可以使用depth_msg->encoding.c_str()进行查看，然后明确你要转换的数据的类型是否合理。

	void depth_Callback(const sensor_msgs::ImageConstPtr& depth_msg)
	{
		cv_bridge::CvImagePtr depth_ptr;
		try
		{
			depth_ptr = cv_bridge::toCvCopy(depth_msg, sensor_msgs::image_encodings::TYPE_16UC1); 
		}
		catch (cv_bridge::Exception& e)
		{
			ROS_ERROR("Could not convert from '%s' to 'mono16'.", depth_msg->encoding.c_str());  //
		}
		depth_pic = depth_ptr->image; 
		
	// 将深度图像转换为视差图，然后利用染色函数ColorMap对图像进行染色
		convertToDisparity(depth_pic, disparity, numDisparities);
		disparity.convertTo(disparityColor, CV_8U, 255. / numDisparities);
		cv::applyColorMap(disparityColor, disparityColor, 9);  	// 可选择多种不同染色方式
		disparityColor.setTo(cv::Scalar::all(0), disparity < 0);
    
    // 声明一个图像消息，将转换后的图像编码成BGR8格式，否则ROS发布话题会显示不出数据	
		cv_bridge::CvImage out_msg;
		out_msg.encoding = sensor_msgs::image_encodings::BGR8;
		cv::imshow("coloured_depth", disparityColor);
		cv::waitKey(30);
		out_msg.image = disparityColor;
	
		pubImage.publish(out_msg.toImageMsg());
	}

3. 深度图像转视差图像

	void convertToDisparity(const cv::Mat& src, cv::Mat& dst, int maxd)
	{
		CV_Assert(src.depth() == CV_16U);
		cv::Mat tmp;
		if (src.channels() == 3)
			cv::cvtColor(src, tmp, cv::COLOR_BGR2GRAY);
		else
			tmp = src;
		const float denom = 1.f / 256;
		maxd = 256 * maxd + 1;

		dst.create(src.size(), CV_32F);
		for (int y = 0; y < dst.rows; y++)
		{
			const ushort* srcptr = tmp.ptr<ushort>(y);
			float* dstptr = dst.ptr<float>(y);
			for (int x = 0; x < dst.cols; x++)
			{
				const ushort p = srcptr[x];
				dstptr[x] = p > 0 && p < maxd ? denom * (p - 1) : -1.f;
			}
		}
	}

4. 转换结果对比如下

---------------------------------------------------------------------------------------------------------------------------------------------------------------

-----------------------------------------------------------------------------------------------------------------------------------------------------------