这篇文章我们一起看一下如何从RGBD摄像头得到你想要的rgb图像和depth图像：

一、通过rqt来测试

这部分请参考：Exbot xi开发札记（一）Exbot xi测试

二、通过编写node代码控制

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/objdetect/objdetect.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <sstream>

using namespace std;
using namespace cv;

void rgbCallback(const sensor_msgs::Image::ConstPtr& msg);
void depthCallback(const sensor_msgs::Image::ConstPtr& msg);

void rgbCallback(const sensor_msgs::Image::ConstPtr& msg)
{
	//convert sensor_msgs/Image to Mat
	cv_bridge::CvImagePtr cvImgPtr;
	Mat rgbImg;
	try
	{
		cvImgPtr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
		rgbImg = cvImgPtr->image;
	}
	catch(cv_bridge::Exception& e)
	{
		ROS_ERROR("cv_bridge exception: %s", e.what());
	}
	
	int rows = rgbImg.rows;
	int cols = rgbImg.cols;

	try
	{
		imshow("Get RGBD Image", rgbImg);
		waitKey(1);
	}
	catch (Exception& e)
	{
		ROS_ERROR("GetRGBDImage rgbCallback exception: %s", e.what());
	}
}

void depthCallback(const sensor_msgs::Image::ConstPtr& msg)
{
	Mat depthImg;
	sensor_msgs::Image img = *msg;
	uint16_t* data = (uint16_t*)&img.data[0];
	for(int i=0; i<img.height; i++)
	{
	  for(int j=0; j<img.width; j++)
	  {
	    uint16_t depth = data[img.width * i + j];
            ROS_INFO("%d, %d处的深度值为：%d", i, j, depth);
	  }
	}
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "GetImageFromRGBD");
	ros::NodeHandle node;
	ros::Subscriber depthRawSub = node.subscribe("/camera/depth/image_raw", 1, depthCallback);
	ROS_INFO("begin getting camera depth image");
	ros::Subscriber RgbRawSub = node.subscribe("/camera/rgb/image_raw", 1, rgbCallback);
	ROS_INFO("begin getting camera rgb image");

	ros::spin();

	return 0;
}

下面对代码进行简单解读：

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/objdetect/objdetect.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <sstream>

using namespace std;
using namespace cv;

包含了ros的基本头文件。
sensor_msgs/Image.h和sensor_msgs/image_encodings.h里面是传感器相关的信息，包括图像相关的一些topics。
后面带cv的都是用来将得到的图像转换为OpenCV识别的格式。

ros::init(argc, argv, "GetImageFromRGBD");//初始化ROS
ros::NodeHandle node;
ros::Subscriber depthRawSub = node.subscribe("/camera/depth/image_raw", 1, depthCallback);//用来获得rgb图像
ROS_INFO("begin getting camera depth image");
ros::Subscriber RgbRawSub = node.subscribe("/camera/rgb/image_raw", 1, rgbCallback);//用来获得depth图像
ROS_INFO("begin getting camera rgb image");

ros::spin();//用来不断地循环跑主函数中的代码

void rgbCallback(const sensor_msgs::Image::ConstPtr& msg)
{
	//convert sensor_msgs/Image to Mat
	cv_bridge::CvImagePtr cvImgPtr;
	Mat rgbImg;
	try
	{
		cvImgPtr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
		rgbImg = cvImgPtr->image;
	}
	catch(cv_bridge::Exception& e)
	{
		ROS_ERROR("cv_bridge exception: %s", e.what());
	}
	
	int rows = rgbImg.rows;
	int cols = rgbImg.cols;

	try
	{
		imshow("Get RGBD Image", rgbImg);
		waitKey(1);
	}
	catch (Exception& e)
	{
		ROS_ERROR("GetRGBDImage rgbCallback exception: %s", e.what());
	}
}

这段代码用来将得到的rgb图像转换成OpenCV格式的图像。
转换后，就可以到我们熟悉的OpenCV领域了，可以利用OpenCV中的大量图像处理和机器学习代码了。

void depthCallback(const sensor_msgs::Image::ConstPtr& msg)
{
	Mat depthImg;
	sensor_msgs::Image img = *msg;
	uint16_t* data = (uint16_t*)&img.data[0];
	for(int i=0; i<img.height; i++)
	{
	  for(int j=0; j<img.width; j++)
	  {
	    uint16_t depth = data[img.width * i + j];
            ROS_INFO("%d, %d处的深度值为：%d", i, j, depth);
	  }
	}
}

这段代码从sensor_msgs::Image中得到深度图像，其中，img.data数组中存的就是深度信息。

好了，从这篇文章你应该知道如何从RGBD摄像头获得rgb图像和depth图像了。
另外，将rgb图像转换到OpenCV格式参照上面的代码应该也没问题了。

Written by Exbot_玄影游侠，

转载请注明出处。