ROS视觉和图像-投影tf坐标系到图像

    本文用到的软件包有OpenCV，tf，rosbag，image_transport和image_geometry。image_geometry在ROS的sensor_msgs/CameraInfo与OpenCV之间建立图像转换桥梁，cv_bridge在ROS的sensor_msgs/Image与OpenCV之间建立图像转换桥梁。

0 准备工作

首先从ROS官网下载样本数据plug_on_base.bag(~750MB)。链接：http://wiki.ros.org/image_geometry/Tutorials/ProjectTfFrameToImage?action=AttachFile&do=view&target=plug_on_base.bag

1 源码解读

 

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv/cv.h>
#include <cv_bridge/cv_bridge.h>
#include <image_geometry/pinhole_camera_model.h>
#include <tf/transform_listener.h>
#include <boost/foreach.hpp>

namespace enc = sensor_msgs::image_encodings;

class FrameDrawer
{
  /*ROS通信相关*/
  ros::NodeHandle nh_;
  image_transport::ImageTransport it_;
  image_transport::CameraSubscriber sub_;
  image_transport::Publisher pub_;
  tf::TransformListener tf_listener_;
  cv_bridge::CvImagePtr cv_ptr;
  /*Bridge：ROS sensor_msgs::Image和sensor_msgs::CameraInfor到OpenCV之间转换*/
  sensor_msgs::CvBridge bridge_;
  image_geometry::PinholeCameraModel cam_model_;
  /*使用Frame_ids list来绘图，用CvFont类对象渲染文本*/
  std::vector<std::string> frame_ids_;
  CvFont font_;

public:
  /*构造函数，用NodeHandle初始化ImageTransport，tf_listener_自动连接到/tf主题*/
  FrameDrawer(const std::vector<std::string>& frame_ids) : it_(nh_), frame_ids_(frame_ids)
  {
    std::string image_topic = nh_.resolveName("image");
    /*CameraSubscriber的sub_同时订阅主题Image+CameraInfo*/
    sub_ = it_.subscribeCamera(image_topic, 1, &FrameDrawer::imageCb, this);
    /*设置广播添加注释的主题，初始化字体对象*/
    pub_ = it_.advertise("image_out", 1);
    cvInitFont(&font_, CV_FONT_HERSHEY_SIMPLEX, 0.5, 0.5);
  }

  /*图像回调函数，当处理来自已经标定过相机的时候，需要同时传入Image和CameraInfo校准参数信息*/
  void imageCb(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& info_msg)
  {
    cv::Mat image;
    try {
      /*转换ROS sensor_msgs::Image成OpenCV Mat image*/
      cv_ptr = cv_bridge::toCvCopy(image_msg, enc::BGR8);
      image = cv_ptr->image;
    }
    catch (cv_bridge::Exception& ex) {
      ROS_ERROR("[draw_frames] Failed to convert image");
      return;
    }

    /*使用CameraInfo信息，填充PinholeCameraModel的cam_model_，cam_model_是成员变量非本地变量*/
    cam_model_.fromCameraInfo(info_msg);

   /*对于每个请求的坐标系ID查询tf坐标系在图像中的位置，图像cam_model_.tfFrame()不包含CameraInfo头*/
    BOOST_FOREACH(const std::string& frame_id, frame_ids_) {
      tf::StampedTransform transform;
      try {
        ros::Time acquisition_time = info_msg->header.stamp;
        /*假定相机帧率是30fps，那么等1帧不超过1/30秒*/
        ros::Duration timeout(1.0/30);
        tf_listener_.waitForTransform(cam_model_.tfFrame(), frame_id, acquisition_time, timeout);
        tf_listener_.lookupTransform(cam_model_.tfFrame(), frame_id, acquisition_time, transform);
      }
      catch (tf::TransformException& ex) {
        ROS_WARN("[draw_frames] TF exception:\n%s", ex.what());
        return;
      }
     
      /*从图像中提取需要的坐标系，然后保存到cv::Point3d*/
      tf::Point pt = transform.getOrigin();
      cv::Point3d pt_cv(pt.x(), pt.y(), pt.z());
      /*使用image_geometry::PinholeCameraModel模型投影3D世界坐标系到2D相机坐标系，image_geometry也包含去畸变方法，
       但是ROS中通常用位于camera driver和vision processing节点之间的image_proc或者stereo_image_proc来处理畸变*/
      cv::Point2d uv;
      cam_model_.project3dToPixel(pt_cv, uv);

      
      static const int RADIUS = 3;
      /*图像上坐标系位置画出红色圆圈，并且打印坐标系ID*/
      cv::circle(image, uv, RADIUS, CV_RGB(255,0,0), -1);
      CvSize text_size;
      int baseline;
      cvGetTextSize(frame_id.c_str(), &font_, &text_size, &baseline);
      CvPoint origin = cvPoint(uv.x - text_size.width / 2, uv.y - RADIUS - baseline - 3);
      cv::putText(image, frame_id.c_str(), origin, cv::FONT_HERSHEY_SIMPLEX, 12, CV_RGB(255,0,0));
    }
    /*再次发布添加注释的图像信息*/
    pub_.publish(cv_ptr->toImageMsg());
  }

};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "draw_frames");
  std::vector<std::string> frame_ids(argv + 1, argv + argc);
  FrameDrawer drawer(frame_ids);
  ros::spin();
}

2 启动文件draw.lauch

<launch>
  <node name="draw_frames" pkg="learning_image_geometry" type="draw_frames" args="/base_link /base_laser_link /r_gripper_tool_frame" output="screen">
    <remap from="image" to="camera/image" />
  </node>

  <node name="input_viewer" pkg="image_view" type="image_view">
    <remap from="image" to="camera/image" />
    <param name="autosize" value="True" />
  </node>

  <node name="output_viewer" pkg="image_view" type="image_view">
    <remap from="image" to="image_out" />
    <param name="autosize" value="True" />
  </node>
</launch>

注意：image_view也可以用命令行执行remap订阅图像， rosrun image_view image_view image:=/camera/image

3 运行程序

1）roscore

2）roslauch draw.lauch

3)rosbag play plug_on_base.bag --clock

注：以上代码来源自ROS官网