A-A+
ExBot XI开发札记(三)RGBD摄像机测试
这篇文章我们一起看一下如何从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_玄影游侠,
转载请注明出处。
楼主你好 我要如何在exbotxi_example里面include opencv 的header? 因为一直报错