ROS圖像和OpenCV圖像相互轉換
描述: 本文將描述如何使用cv_bridge來將ROS圖像轉換爲OpenCV圖像,以及OpenCV轉爲ROS圖像。
1. 概念
在ROS中是以自己的sensor_msgs/Image格式對圖像進行處理的,但開發者可能會希望以OpenCV的格式對圖像進行處理。CvBridge就是一個提供ROS和OpenCV間接口的ROS庫,它可以在vision_opencv Stack的cv_bridge包中找到。
在本文中,你將學習如何編寫一個利用CvBridge將ROS圖像格式轉換爲OpenCV中的cv::Mat格式。還將學習如何將OpenCV圖像轉換爲ROS格式以在ROS中發佈。
1.1 從C-Tutle或更早的版本代碼遷移
關於OpenCV,在ROS Diamondback中有很大的api變化,雖然後向兼容維護了一陣,但從hydro開始有些已經被移除了,如sensor_msgs/CvBridge。關於遷移的問題見鏈接。
2. 轉換ROS圖像到OpenCV圖像
CvBridge中定義了一個包含OpenCV圖像及其編碼、ROS header的CvImage類型。CvImage中包含了sensor_msgs/Image中的所有信息,因此我們可以在這兩者間相互轉換。CvImage的class如下:
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;
}
當從sensor_msgs/Image消息轉換到CvImage時,CvBridge認爲有兩種不同的用例:
- 我們想修改數據,需要拷貝一份數據。
- 我們不想修改數據,但需要使用CvImage格式,所以可以通過const指針共享該數據。
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());
函數的輸入是一個圖像指針,以及一個可選的編碼參數用於規定目標CvImage的編碼。
toCvCopy創建一個圖像數據的拷貝,你可以對返回的CvImage進行修改。
toCvShare將會返回一個指向ROS消息的cv::Mat const指針防止修改,只要你有返回的CvImage指針的拷貝,ROS消息就不會被釋放。如果編碼不匹配,ROS將分配一個新的buffer並執行轉換,但你還是不能對其進行修改。
注:當你有一個包含sensor_msgs/Image的其他消息類型的指針時,使用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提供了可選的color或pixel depth的轉換,要想使用這個特性,需要指定以下編碼格式:
- mono8: CV_8UC1, grayscale image
- mono16: CV_16UC1, 16-bit grayscale image
- bgr8: CV_8UC3, color image with blue-green-red color order
- rgb8: CV_8UC3, color image with red-green-blue color order
- bgra8: CV_8UC4, BGR color image with an alpha channel
- rgba8: CV_8UC4, RGB color image with an alpha channel
其中mono8和bgr8是大多數OpenCV函數所期望的圖像編碼格式。
最後,CvBridge也可以識別OpenCV中8UC1類型的Bayer pattern編碼,CvBridge將不會對Bayer pattern進行轉換,一般是由image_proc進行轉換的。CvBridge可以識別一下以下集中Bayer編碼:
- 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是你自己創建的,不要忘了填充header和編碼字段。對於自己創建CvImage的例子,可以參考發佈圖像教程
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_)
{
// Subscribe 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>
在ROS中使用image_transport發佈訂閱圖像可以讓你訂閱壓縮後的圖像流,記得在package.xml中include image_transport。
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
include CvBridge的頭文件以及image encodings(包含了很多有用的常量和函數),記得在package.xml中include cv_bridge。
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
includeOpenCV的圖像處理和GUI模塊頭文件,記得在package.xml中include 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);
使用image_transport訂閱發佈圖像話題。
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圖像消息轉換爲了CvImage以在OpenCV中使用。因爲我們需要在圖像中畫圓,所以需要一個圖像的拷貝,應使用toCvCopy()。sensor_msgs::image_encodings::BGR8是”bgr8”字符串常量。
調用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::imshow(OPENCV_WINDOW, cv_ptr->image);
cv::waitKey(3);
在圖像中畫一個紅色的圓圈並進行顯示。
// Output modified video stream
image_pub_.publish(cv_ptr->toImageMsg());
將CvImage轉爲ROS圖像消息並進行發佈。
運行這個節點你需要一個圖像流,啓動一個攝像頭或者回放一個bag文件來生成圖像流。現在你可以將”in”topic重新映射remaaping到真實的圖像流話題。
如果你成功地轉換爲OpenCV圖像,你將在創建的窗口中看到添加圓圈之後的圖像。
你可以通過rostopic或image_view查看圖像來確認節點是否正確地發佈了圖像。
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將會是圖像數據的一個別名而非拷貝。如果輸入圖像不是”bgr8”編碼但是可轉換爲”bgr8”編碼(如”mono8”),CvBridge將會爲cv_ptr分配一個新的buffer並執行轉換。如果沒有異常捕獲語句的話一行代碼就能共享圖像了,但可能輸入圖像的編碼無法轉換爲目標編碼從而導致節點崩潰。例如,當輸入圖像是從一個Bayer pattern攝像機的image_raw話題接收的,CvBridge將會拋出異常,因爲不支持Bayer到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
}
在這個例子中,如果可以的話我們將使用color的編碼,不行的話就是用monochrome類型的編碼,如果輸入圖像是”bgr8”或”mono8”編碼,將不會進行拷貝。