1.概念
ROS以自己的sensor_msgs / Image消息格式传递图像,但许多用户希望将图像与OpenCV结合使用。
CvBridge是一个ROS库,提供ROS和OpenCV之间的接口。
CvBridge可以在cv_bridge包在vision_opencv stack找到。
在本教程中,您将学习如何编写使用CvBridge将ROS图像转换为OpenCV cv :: Mat格式的节点。
您还将学习如何将OpenCV图像转换为ROS格式,并以通过ROS发布。
1.1 从C-Turtle或更早版本编写的代码迁移
关于OpenCV有一个主要的改变是ROS Diamondback,其向后兼容性已经维持了一段时间,但在较新的发行版(hydro)中被删除,比如sensor_msgs / CvBridge。查看the design decision.。此外this QA是有帮助的。
2. 将ROS图像消息转换为OpenCV图像
CvBridge定义了一个包含OpenCV图像的CvImage类型,its encoding and a ROS header。CvImage也包含sensor_msgs / Image的信息,因此我们可以将表示转换为另一个。CvImage类格式:
namespace cv_bridge {
class CvImage
{
public:
std_msgs::Header header;
std::string encoding;
cv::Mat image;
};
typedef boost::shared_ptr<CvImage> CvImagePtr;
typedef boost::shared_ptr<CvImage const> CvImageConstPtr;
}
将ROS sensor_msgs / Image消息转换为CvImage时,CvBridge会识别两个不同的用例:
- 我们想要就地修改数据。我们必须复制ROS消息数据。
- 我们不会修改数据。我们可以安全地共享ROS消息所拥有的数据,而不是复制。
CvBridge提供以下用于转换为CvImage的函数:
// Case 1: Always copy, returning a mutable CvImage
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr& source,
const std::string& encoding = std::string());
CvImagePtr toCvCopy(const sensor_msgs::Image& source,
const std::string& encoding = std::string());
// Case 2: Share if possible, returning a const CvImage
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr& source,
const std::string& encoding = std::string());
CvImageConstPtr toCvShare(const sensor_msgs::Image& source,
const boost::shared_ptr<void const>& tracked_object,
const std::string& encoding = std::string());
输入是图像消息指针,以及可选的编码参数。编码 refers to目标CvImage。
即使源和目标编码匹配,toCvCopy也会从ROS消息创建图像数据的副本。不过,您可以自由修改返回的CvImage。
如果源和目标编码匹配,toCvShare将在ROS消息数据上指向返回的CV::Mat,避免复制。只要您还拥有返回的CvImage的副本,就不会释放ROS消息数据。
如果编码不匹配,它将分配一个新缓冲区并执行转换。您不能修改返回的CvImage,因为它可能与ROS图像消息共享数据,而ROS图像消息又可能与其他回调共享。
注意:如果您指向包含要转换的sensor_msgs / Image的其他消息类型(例如stereo_msgs / DisparityImage),则toCvShare的第二个重载函数会更方便。
如果没有给出编码(或者更确切地说是空字符串),则目标图像编码将与图像消息编码相同。在这种情况下,toCvShare保证不复制图像数据。图像编码可以是以下OpenCV图像编码中的任何一种:
- 8UC[1-4]
- 8SC[1-4]
- 16UC[1-4]
- 16SC[1-4]
- 32SC[1-4]
- 32FC[1-4]
- 64FC[1-4]
对于流行的图像编码,CvBridge可根据需要选择进行颜色或像素深度转换。要使用此功能,请将编码指定为以下字符串之一:
-
mono8:CV_8UC1,灰度图像
-
mono16:CV_16UC1,16位灰度图像
-
bgr8:CV_8UC3,彩色图像,蓝绿红色顺序
-
rgb8:CV_8UC3,带有红绿蓝颜色顺序的彩色图像
-
bgra8:CV_8UC4,带有alpha通道的BGR彩色图像
-
rgba8:CV_8UC4,带有alpha通道的RGB彩色图像
请注意,mono8和bgr8是大多数OpenCV函数所期望的两种图像编码。
最后,CvBridge会将Bayer pattern encodings识别为具有OpenCV类型8UC1(8位无符号,一个通道)。它不会与Bayer pattern进行转换; 在典型的ROS系统中,这是由image_proc完成的。CvBridge认可以下Bayer pattern:
-
bayer_rggb8
-
bayer_bggr8
-
bayer_gbrg8
-
bayer_grbg8
3. 将OpenCV图像转换为ROS图像消息
要将CvImage转换为ROS图像消息,请使用toImageMsg()成员函数之一:
class CvImage
{
sensor_msgs::ImagePtr toImageMsg() const;
// Overload mainly intended for aggregate messages that contain
// a sensor_msgs::Image as a member.
void toImageMsg(sensor_msgs::Image& ros_image) const;
};
如果CvImage是您自己分配的,don't forget to fill in the header and encoding fields。
有关自己分配一个的示例,请参阅发布图像教程。
4. 示例ROS节点
这是一个侦听ROS图像消息主题的节点,将图像转换为cv :: Mat,在其上绘制一个圆圈并使用OpenCV显示图像。然后通过ROS重新发布图像。
在package.xml和CMakeLists.xml中(或使用catkin_create_pkg时),添加以下依赖项:
sensor_msgs
cv_bridge
roscpp
std_msgs
image_transport
在/ src文件夹中创建image_converter.cpp文件并添加以下内容:
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
static const std::string OPENCV_WINDOW = "Image window";
class ImageConverter
{
ros::NodeHandle nh_;
image_transport::ImageTransport it_;
image_transport::Subscriber image_sub_;
image_transport::Publisher image_pub_;
public:
ImageConverter()
: it_(nh_)
{
// Subscrive to input video feed and publish output video feed
image_sub_ = it_.subscribe("/camera/image_raw", 1,
&ImageConverter::imageCb, this);
image_pub_ = it_.advertise("/image_converter/output_video", 1);
cv::namedWindow(OPENCV_WINDOW);
}
~ImageConverter()
{
cv::destroyWindow(OPENCV_WINDOW);
}
void imageCb(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
// Draw an example circle on the video stream
if (cv_ptr->image.rows > 60 && cv_ptr->image.cols > 60)
cv::circle(cv_ptr->image, cv::Point(50, 50), 10, CV_RGB(255,0,0));
// Update GUI Window
cv::imshow(OPENCV_WINDOW, cv_ptr->image);
cv::waitKey(3);
// Output modified video stream
image_pub_.publish(cv_ptr->toImageMsg());
}
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "image_converter");
ImageConverter ic;
ros::spin();
return 0;
}
我们分解上面的节点:
#include <image_transport/image_transport.h>
使用image_transport在ROS中发布和订阅图像,并允许您订阅压缩图像流。
请记住在package.xml中包含image_transport。
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
包括CvBridge的头文件,以及与图像编码相关的有用常量和函数。请记住在package.xml中包含cv_bridge。
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
包括OpenCV图像处理和GUI模块的头文件。请记住在package.xml中包含opencv2。
ros::NodeHandle nh_;
image_transport::ImageTransport it_;
image_transport::Subscriber image_sub_;
image_transport::Publisher image_pub_;
public:
ImageConverter()
: it_(nh_)
{
// Subscrive to input video feed and publish output video feed
image_sub_ = it_.subscribe("/camera/image_raw", 1,
&ImageConverter::imageCb, this);
image_pub_ = it_.advertise("/image_converter/output_video", 1);
订阅图像主题“in”并使用image_transport发布图像主题“out” 。
cv::namedWindow(OPENCV_WINDOW);
}
~ImageConverter()
{
cv::destroyWindow(OPENCV_WINDOW);
}
OpenCV HighGUI调用在启动/关闭时创建/销毁显示窗口。
void imageCb(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
在我们的订阅者回调中,我们首先将ROS图像消息转换为适合使用OpenCV 的CvImage。由于我们要绘制图像,我们需要一个可变的副本,因此我们使用toCvCopy()。sensor_msgs :: image_encodings :: BGR8只是“bgr8”的常量,但不太容易出现错别字。
请注意,OpenCV期望彩色图像使用BGR通道顺序。
您应始终将调用包装到toCvCopy() / toCvShared(),以捕获转换错误,因为这些函数不会检查数据的有效性。
// Draw an example circle on the video stream
if (cv_ptr->image.rows > 60 && cv_ptr->image.cols > 60)
cv::circle(cv_ptr->image, cv::Point(50, 50), 10, CV_RGB(255,0,0));
// Update GUI Window
在图像上绘制一个红色圆圈,然后在显示窗口中显示它。
cv::waitKey(3);
将CvImage转换为ROS图像消息并将其发布到“out”主题上。
要运行节点,您需要一个图像流。运行相机或播放包文件以生成图像流。
现在,您可以运行此节点,将“in” 重新映射到实际的图像流主题。
如果您已成功将图像转换为OpenCV格式,您将看到一个名为“图像窗口”的HighGui窗口,并显示您的图像+圆圈。
您可以使用rostopic或使用image_view查看图像,查看您的节点是否通过ROS正确发布图像。
5. 共享图像数据的示例
在上面的完整示例中,我们明确复制了图像数据,但共享(如果可能)同样容易:
namespace enc = sensor_msgs::image_encodings;
void imageCb(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImageConstPtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvShare(msg, enc::BGR8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
// Process cv_ptr->image using OpenCV
}
如果传入消息具有“bgr8”编码,则cv_ptr将对其数据进行引用而不进行复制。如果有不同,但可以进行转换编码,比如“MONO8”,CvBridge将分配一个新的缓冲区cv_ptr并执行转换。如果没有异常处理,这只会是一行代码,但是具有格式错误(或不支持)编码的传入消息会导致节点崩溃。例如,如果传入图像来自Bayer pattern摄像机的image_raw主题,则CvBridge将抛出异常,因为它(故意)不支持Bayer-to-color到颜色转换。
一个稍微复杂的例子:
namespace enc = sensor_msgs::image_encodings;
void imageCb(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImageConstPtr cv_ptr;
try
{
if (enc::isColor(msg->encoding))
cv_ptr = cv_bridge::toCvShare(msg, enc::BGR8);
else
cv_ptr = cv_bridge::toCvShare(msg, enc::MONO8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
// Process cv_ptr->image using OpenCV
}
在这种情况下,我们想要使用颜色那么就用,否则回到单色。如果传入的图像是“bgr8”或“mono8”,我们避免复制数据。