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認為有兩種不同的用例：

1. 我們想修改資料，需要拷貝一份資料。
2. 我們不想修改資料，但需要使用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”編碼，將不會進行拷貝。