ROS中利用opencv3完成四个视频流拼接成一副图像显示，并添加track条进行参数调节
ROS开发过程中，经常会调试摄像头。我使用的双目在调试过程中可能需要同时查看其左边摄像头的图像，右边摄像头的图像，RGB图像和depth图像。而利用RVIZ或者image_view同时查看这几幅图像并不方便。这几幅图像不在同一个窗口显示，同时查看起来不太容易。又用于涉及到自动曝光等动态参数的调试，调用rqt_reconfigure来调试也比较麻烦。
因此我通过opencv来同时显示这四帧图像，之前搜了很多用opencv同时显示几幅图像的，讲得都不是很明白，因此写一篇博客来总结一下。本文是用opencv3来实现显示的，opencv2与3的差别较大，可能会出错。
废话不多说直接上代码：
#include //ros标准库头文件
#include //C++标准输入输出库
#include
/*
cv_bridge中包含CvBridge库
*/
#include
/*
ROS图象类型的编码函数
*/
#include
/*
image_transport 头文件用来在ROS系统中的话题上发布和订阅图象消息
*/
#include
//OpenCV2标准头文件
#include
#include
#include
#include
using namespace cv;
static const std::string OPENCV_WINDOW = "color/image";//该变量是定义显示的窗口名称
static const std::string imagetopic = "/camera/color/image";//ROS系统中订阅topic，通过topic来获取图像数据，一共有4个topic
static const std::string ir1topic = "/camera/lc/image";
static const std::string depthtopic = "/camera/depth/image";
static const std::string ir2topic = "/camera/rc/image";
int auto_exposure_state=0;//定义自动曝光是否开启的变量
int colorwidth,colorheight;//定义显示图像的宽和高
cv::Mat A,B,C,D,ptr_color,ptr_depth,ptr_ir1,ptr_ir2,sumMat,test1;//定义一些重要的全局cv::Mat矩阵
//sumMat是最终拼接的总的图像，ABCD分别是sumMat图像中的4个区域
class ImageConverter //这是ROS系统中关于图像显示的常规的设置，定义类
{
ros::NodeHandle nh_;//ROS句柄
image_transport::ImageTransport it_;
image_transport::Subscriber image_sub1_;
image_transport::Subscriber image_sub2_;
image_transport::Subscriber image_sub3_;
image_transport::Subscriber image_sub4_;
public:
ImageConverter()
: it_(nh_)//构造函数
{
// Subscrive to input video feed and publish output video feed
image_sub1_ = it_.subscribe(imagetopic, 1,&ImageConverter::imageCb, this);//订阅TOPIC，回调函数为imageCb
image_sub2_ = it_.subscribe(depthtopic, 1,&ImageConverter::depthCb, this);//每个订阅的topic都有回调函数，接收到数据则进入回调函数处理
image_sub3_ = it_.subscribe(ir1topic, 1,&ImageConverter::ir1Cb, this);
image_sub4_ = it_.subscribe(ir2topic, 1,&ImageConverter::ir2Cb, this);
cv::namedWindow(OPENCV_WINDOW);//创建显示窗口，名称为之前定义的变量
cv::createTrackbar("Auto_exposure",OPENCV_WINDOW,&auto_exposure_state,1,track_change);//在刚创的窗口创建滑动条，用于调节自动曝光
}
~ImageConverter()//析构函数
{
cv::destroyWindow(OPENCV_WINDOW);//关掉显示窗口
}
static void track_change(int position,void* userdata){ //这是滑动条滑动后的回调函数，滑动后在此进行处理
if(position==0){
system("rosrun dynamic_reconfigure dynparam set /camera/driver r200_lr_auto_exposure_enabled 0");
}
else{
system("rosrun dynamic_reconfigure dynparam set /camera/driver r200_lr_auto_exposure_enabled 1");
}
}
void imageCb(const sensor_msgs::ImageConstPtr& msg) //Cbtopic的回调函数，接收到RGB摄像头数据后进入此回调函数
{
cv_bridge::CvImagePtr cv_ptr;// 声明一个CvImage指针的实例
try
{
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);//将ROS消息中的图象信息提取，生成新cv类型的图象，复制给CvImage指针
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());//异常处理
return;
}
A=(cv::Mat)cv_ptr->image;//将topic得到的数据赋给A矩阵
cvtColor(A,A,CV_BGR2GRAY);//A矩阵转化成单通道灰度图（由于四个MAT拼成一个大MAT，必须保持格式一致，否则MAT大小对不齐会出现数组越界）
mergeImage(A,1);//将A拼到图像上
}
void depthCb(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImagePtr cv_ptr;// 声明一个CvImage指针的实例
try
{
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_16UC1);//将ROS消息中的图象信息提取，生成新cv类型的图象，复制给CvImage指针
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());//异常处理
return;
}
cv_ptr->image.convertTo(B,CV_8UC1,255.0 / 4500);
mergeImage(B,2);
}
void ir1Cb(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImagePtr cv_ptr;// 声明一个CvImage指针的实例
try
{
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_8UC1);//将ROS消息中的图象信息提取，生成新cv类型的图象，复制给CvImage指针
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());//异常处理
return;
}
C=(cv::Mat)cv_ptr->image;
mergeImage(C,3);
}
void ir2Cb(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImagePtr cv_ptr;// 声明一个CvImage指针的实例
try
{
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_8UC1);//将ROS消息中的图象信息提取，生成新cv类型的图象，复制给CvImage指针
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());//异常处理
return;
}
D=(cv::Mat)cv_ptr->image;
mergeImage(D,4);
}
/*
这是图象处理的主要函数，一般会把图像处理的主要程序写在这个函数中。这里的例子只是一个彩色图象到灰度图象的转化
*/
void mergeImage(cv::Mat img,int classy)
{
switch (classy) {
case 1:
img.convertTo(ptr_color,CV_8UC1);//将A矩阵单通道格式付给ptr_color
break;
case 2:
img.convertTo(ptr_depth,CV_8UC1);
break;
case 3:
img.convertTo(ptr_ir1,CV_8UC1);
break;
case 4:
img.convertTo(ptr_ir2,CV_8UC1);
break;
default:
break;
}
image_process(sumMat);//显示sumMat
}
void image_process(cv::Mat img)
{
cv::imshow(OPENCV_WINDOW, img);
cv::waitKey(10);
}
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "realcam_dnew_node");
ros::param::get("/camera/driver/r200_lr_auto_exposure_enabled",auto_exposure_state);
ros::param::get("/camera/driver/color_width",colorwidth);//得到一副图像的宽
ros::param::get("/camera/driver/color_height",colorheight);//得到一副图像的高
sumMat=cv::Mat::zeros(colorheight*2,colorwidth*2,CV_8UC1);//初始化sumMat，2倍的宽，2倍的高，单通道灰度图像
ptr_color = sumMat(cv::Rect(0,0,colorwidth,colorheight));//ptr_color即为sumMat的左上角部分
ptr_depth = sumMat(cv::Rect(colorwidth,0,colorwidth,colorheight));//ptr_depth为sumMat的右上角
ptr_ir1=sumMat(cv::Rect(0,colorheight,colorwidth,colorheight));
ptr_ir2=sumMat(cv::Rect(colorwidth,colorheight,colorwidth,colorheight));
ImageConverter ic;
ros::spin();
return 0;
}