双目使用stereo_image_proc

效果图

官方

http://wiki.ros.org/stereo_image_proc



简说

订阅
四个topic
/stereo/left/image_raw
/stereo/left/camera_info
/stereo/right/image_raw
/stereo/right/camera_info

发布
处理过的图像topic
left/image_mono
left/image_rect
left/image_color
left/image_rect_color

right/image_mono
right/image_rect
right/image_color
right/image_rect_color

3D 处理的topic
disparity (stereo_msgs/DisparityImage)Floating point disparity image with metadata.
points2 (sensor_msgs/PointCloud2)Stereo point cloud with RGB color.
points (sensor_msgs/PointCloud)


订阅主题来源于左右摄像头驱动节点,如果是摄像头传来的数据是一个合成的画面,则需要预处理
将其分割,一分为二.分割方法, 这里有示例.
模拟两个摄像头,并发布相应的camera_info.
标定完成就可以使用stereo_image_proc了.
stereo_image_proc会自动根据camera校正结果发布修正后的图像和3D的topic.


总结注意事项

1.命名以及名称空间
调用标定文件内部
camera_name: stereo/left

c++:

boost::shared_ptr<camera_info_manager::CameraInfoManager> cinfo_left;
//...
nha.param("video_device_left", video_device_name_left, std::string("/left_video"));
    nha.param("camera_name_left", camera_name_left, std::string("head_camera_left"));
    nha.param("camera_info_url_left", camera_info_url_left, std::string(""));
    cinfo_left.reset(new camera_info_manager::CameraInfoManager(nh_fakecam_left, camera_name_left, camera_info_url_left));
//....

launch:
camera_name_left参数

<param name="camera_name_left" value="stereo/left"/>
<!-- .... -->

<node ns="stereo" name="stereo_image_proc"  pkg="stereo_image_proc" type="stereo_image_proc" respawn="false"
       output="screen">
</node>

ns="stereo" 注意查询topic名称,它影响话题名的映射.

2.图像同步
图像同步是以时间戳为准的
发布左右帧图像时应设置为同一时间.

sensor_msgs::ImagePtr msg ;
ros::Time nt=ros::Time::now();
//....

msg->header.stamp=nt;
sensor_msgs::CameraInfoPtr ci_left(new sensor_msgs::CameraInfo(cinfo_left->getCameraInfo()));
ci_left->header.stamp =nt;
image_pub_left.publish(*msg, *ci_left);
//...
msg->header.stamp=nt;
sensor_msgs::CameraInfoPtr ci_right(new sensor_msgs::CameraInfo(cinfo_right->getCameraInfo()));
ci_right->header.stamp =nt;
image_pub_right.publish(*msg, *ci_right);

3.指定消息的frame_id
摄像头在机器人框架的位置,frame_id是框架名称.frame容易与画面帧混淆
必须发布摄像头与基座之间关系的tf

tf示例 :我们把摄像头定位在stereo_optical上
launch:

<?xml version="1.0" encoding="UTF-8"?>

<launch>
  <arg name="camera" default="stereo" />

  <arg name="baseline/2" value="0.06"/>
  <arg name="optical_translation" value="0 -$(arg baseline/2) 0"/>

  <arg name="pi/2" value="1.5707963267948966"/>
  <arg name="optical_rotation" value="-$(arg pi/2) 0 -$(arg pi/2)"/>

  <node pkg="tf" type="static_transform_publisher" name="$(arg camera)_link"
        args="0 0 0.1 0 0 0 /base_link /$(arg camera) 100"/>  
  <node pkg="tf" type="static_transform_publisher" name="$(arg camera)_optical_link"
        args="$(arg optical_translation) $(arg optical_rotation) /$(arg camera) /$(arg camera)_optical 100"/>
</launch>

c++:

msg->header.frame_id="stereo_optical";

ci_right->header.frame_id = "stereo_optical";

ci_left->header.frame_id = "stereo_optical"