本文用到的软件包有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官网