目的:参考ros的stereo_iamge_proc,采用两个一样的usb相机,来搭建双目相机。
实验步骤记录:
1. 根据usb相机的ros驱动,ros_usb_cam,修改一下launch文件,同时启动两个usb相机。
<launch>
<node name="left" pkg="usb_cam" type="usb_cam_node" output="screen" >
<param name="video_device" value="/dev/video1" />
<param name="image_width" value="640" />
<param name="image_height" value="480" />
<param name="pixel_format" value="yuyv" />
<param name="camera_frame_id" value="left" />
<param name="camera_name" value="left" />
<param name="io_method" value="mmap"/>
<param name="framerate" value="30" />
<!-- file://${ROS_HOME}/camera_info/${NAME}.yaml -->
<param name="camera_info_url" value="file:///home/nan/dev/cam_ws/src/usb_cam/config/left.yaml"/>
<remap from="/left/camera_info" to="/stereo/left/camera_info"/>
<remap from="/left/image_raw" to="/stereo/left/image_raw"/>
</node>
<node name="right" pkg="usb_cam" type="usb_cam_node" output="screen" >
<param name="video_device" value="/dev/video2" />
<param name="image_width" value="640" />
<param name="image_height" value="480" />
<param name="pixel_format" value="yuyv" />
<param name="camera_frame_id" value="right" />
<param name="camera_name" value="right" />
<param name="io_method" value="mmap"/>
<param name="framerate" value="30" />
<param name="camera_info_url" value="file:///home/nan/dev/cam_ws/src/usb_cam/config/right.yaml"/>
<remap from="/right/camera_info" to="/stereo/right/camera_info"/>
<remap from="/right/image_raw" to="/stereo/right/image_raw"/>
</node>
<!-- <node name="image_view" pkg="image_view" type="image_view" respawn="false" output="screen">
<remap from="image" to="/usb_cam/image_raw"/>
<param name="autosize" value="true" />
</node> -->
</launch>
这一步需要注意的有:
①需要提前标定好左右两个单目相机,可以直接采用ros标定单目相机,然后记得存储相机的参数文件
②给camera_info_url赋值,这里需要注意的是file:后有三道斜杠
③为了方便后续运行stereo_image_proc,这里将话题进行了映射,增加了一个ROS_NAMESPACE,也就是stereo
2. 运行stereo_image_proc
这一步首先
$ ROS_NAMESPACE=stereo rosrun stereo_image_proc stereo_image_proc
然后就可以输出矫正后的rect_img了
可以在rviz中进行订阅查看,也可以通过命令
rosrun image_view stereo_view image:=image_rect
由于这里的namespace就是stereo,上述命令省略了替换namespace的命令 :
$ rosrun image_view stereo_view stereo:=narrow_stereo_textured image:=image_rect
不出意外的话能够见到不错的结果,参考链接wiki之选择合适的参数
然而博主这里报错了:
[ WARN] [1568596225.043242571]: 'stereo' has not been remapped! Example command-line usage:
$ rosrun image_view stereo_view stereo:=narrow_stereo image:=image_color
[ INFO] [1568596225.240910723]: Subscribing to:
* /stereo/left/image_rect
* /stereo/right/image_rect
* /stereo/disparity
[ WARN] [1568596240.295962861]: [stereo_view] Low number of synchronized left/right/disparity triplets received.
Left images received: 437 (topic '/stereo/left/image_rect')
Right images received: 437 (topic '/stereo/right/image_rect')
Disparity images received: 0 (topic '/stereo/disparity')
Synchronized triplets: 0
Possible issues:
* stereo_image_proc is not running.
Does `rosnode info /stereo_view_1568596225043150738` show any connections?
* The cameras are not synchronized.
Try restarting stereo_view with parameter _approximate_sync:=True
* The network is too slow. One or more images are dropped from each triplet.
Try restarting stereo_view, increasing parameter 'queue_size' (currently 5)
对应的仅启动usb驱动时的rostopiclist结果:
/rosout
/rosout_agg
/stereo/left/camera_info
/stereo/left/image_raw
/stereo/left/image_raw/compressed
/stereo/left/image_raw/compressed/parameter_descriptions
/stereo/left/image_raw/compressed/parameter_updates
/stereo/left/image_raw/compressedDepth
/stereo/left/image_raw/compressedDepth/parameter_descriptions
/stereo/left/image_raw/compressedDepth/parameter_updates
/stereo/left/image_raw/theora
/stereo/left/image_raw/theora/parameter_descriptions
/stereo/left/image_raw/theora/parameter_updates
/stereo/right/camera_info
/stereo/right/image_raw
/stereo/right/image_raw/compressed
/stereo/right/image_raw/compressed/parameter_descriptions
/stereo/right/image_raw/compressed/parameter_updates
/stereo/right/image_raw/compressedDepth
/stereo/right/image_raw/compressedDepth/parameter_descriptions
/stereo/right/image_raw/compressedDepth/parameter_updates
/stereo/right/image_raw/theora
/stereo/right/image_raw/theora/parameter_descriptions
/stereo/right/image_raw/theora/parameter_updates
在启动了stereo_image_proc后的结果:
/rosout
/rosout_agg
/stereo/disparity
/stereo/left/camera_info
/stereo/left/image_color
/stereo/left/image_color/compressed
/stereo/left/image_color/compressed/parameter_descriptions
/stereo/left/image_color/compressed/parameter_updates
/stereo/left/image_color/compressedDepth
/stereo/left/image_color/compressedDepth/parameter_descriptions
/stereo/left/image_color/compressedDepth/parameter_updates
/stereo/left/image_color/theora
/stereo/left/image_color/theora/parameter_descriptions
/stereo/left/image_color/theora/parameter_updates
/stereo/left/image_mono
/stereo/left/image_mono/compressed
/stereo/left/image_mono/compressed/parameter_descriptions
/stereo/left/image_mono/compressed/parameter_updates
/stereo/left/image_mono/compressedDepth
/stereo/left/image_mono/compressedDepth/parameter_descriptions
/stereo/left/image_mono/compressedDepth/parameter_updates
/stereo/left/image_mono/theora
/stereo/left/image_mono/theora/parameter_descriptions
/stereo/left/image_mono/theora/parameter_updates
/stereo/left/image_raw
/stereo/left/image_raw/compressed
/stereo/left/image_raw/compressed/parameter_descriptions
/stereo/left/image_raw/compressed/parameter_updates
/stereo/left/image_raw/compressedDepth
/stereo/left/image_raw/compressedDepth/parameter_descriptions
/stereo/left/image_raw/compressedDepth/parameter_updates
/stereo/left/image_raw/theora
/stereo/left/image_raw/theora/parameter_descriptions
/stereo/left/image_raw/theora/parameter_updates
/stereo/left/image_rect
/stereo/left/image_rect/compressed
/stereo/left/image_rect/compressed/parameter_descriptions
/stereo/left/image_rect/compressed/parameter_updates
/stereo/left/image_rect/compressedDepth
/stereo/left/image_rect/compressedDepth/parameter_descriptions
/stereo/left/image_rect/compressedDepth/parameter_updates
/stereo/left/image_rect/theora
/stereo/left/image_rect/theora/parameter_descriptions
/stereo/left/image_rect/theora/parameter_updates
/stereo/left/image_rect_color
/stereo/left/image_rect_color/compressed
/stereo/left/image_rect_color/compressed/parameter_descriptions
/stereo/left/image_rect_color/compressed/parameter_updates
/stereo/left/image_rect_color/compressedDepth
/stereo/left/image_rect_color/compressedDepth/parameter_descriptions
/stereo/left/image_rect_color/compressedDepth/parameter_updates
/stereo/left/image_rect_color/theora
/stereo/left/image_rect_color/theora/parameter_descriptions
/stereo/left/image_rect_color/theora/parameter_updates
/stereo/points2
/stereo/right/camera_info
/stereo/right/image_color
/stereo/right/image_color/compressed
/stereo/right/image_color/compressed/parameter_descriptions
/stereo/right/image_color/compressed/parameter_updates
/stereo/right/image_color/compressedDepth
/stereo/right/image_color/compressedDepth/parameter_descriptions
/stereo/right/image_color/compressedDepth/parameter_updates
/stereo/right/image_color/theora
/stereo/right/image_color/theora/parameter_descriptions
/stereo/right/image_color/theora/parameter_updates
/stereo/right/image_mono
/stereo/right/image_mono/compressed
/stereo/right/image_mono/compressed/parameter_descriptions
/stereo/right/image_mono/compressed/parameter_updates
/stereo/right/image_mono/compressedDepth
/stereo/right/image_mono/compressedDepth/parameter_descriptions
/stereo/right/image_mono/compressedDepth/parameter_updates
/stereo/right/image_mono/theora
/stereo/right/image_mono/theora/parameter_descriptions
/stereo/right/image_mono/theora/parameter_updates
/stereo/right/image_raw
/stereo/right/image_raw/compressed
/stereo/right/image_raw/compressed/parameter_descriptions
/stereo/right/image_raw/compressed/parameter_updates
/stereo/right/image_raw/compressedDepth
/stereo/right/image_raw/compressedDepth/parameter_descriptions
/stereo/right/image_raw/compressedDepth/parameter_updates
/stereo/right/image_raw/theora
/stereo/right/image_raw/theora/parameter_descriptions
/stereo/right/image_raw/theora/parameter_updates
/stereo/right/image_rect
/stereo/right/image_rect/compressed
/stereo/right/image_rect/compressed/parameter_descriptions
/stereo/right/image_rect/compressed/parameter_updates
/stereo/right/image_rect/compressedDepth
/stereo/right/image_rect/compressedDepth/parameter_descriptions
/stereo/right/image_rect/compressedDepth/parameter_updates
/stereo/right/image_rect/theora
/stereo/right/image_rect/theora/parameter_descriptions
/stereo/right/image_rect/theora/parameter_updates
/stereo/right/image_rect_color
/stereo/right/image_rect_color/compressed
/stereo/right/image_rect_color/compressed/parameter_descriptions
/stereo/right/image_rect_color/compressed/parameter_updates
/stereo/right/image_rect_color/compressedDepth
/stereo/right/image_rect_color/compressedDepth/parameter_descriptions
/stereo/right/image_rect_color/compressedDepth/parameter_updates
/stereo/right/image_rect_color/theora
/stereo/right/image_rect_color/theora/parameter_descriptions
/stereo/right/image_rect_color/theora/parameter_updates
/stereo/stereo_image_proc/parameter_descriptions
/stereo/stereo_image_proc/parameter_updates
/stereo/stereo_image_proc_debayer_left/parameter_descriptions
/stereo/stereo_image_proc_debayer_left/parameter_updates
/stereo/stereo_image_proc_debayer_right/parameter_descriptions
/stereo/stereo_image_proc_debayer_right/parameter_updates
/stereo/stereo_image_proc_rectify_color_left/parameter_descriptions
/stereo/stereo_image_proc_rectify_color_left/parameter_updates
/stereo/stereo_image_proc_rectify_color_right/parameter_descriptions
/stereo/stereo_image_proc_rectify_color_right/parameter_updates
/stereo/stereo_image_proc_rectify_mono_left/parameter_descriptions
/stereo/stereo_image_proc_rectify_mono_left/parameter_updates
/stereo/stereo_image_proc_rectify_mono_right/parameter_descriptions
/stereo/stereo_image_proc_rectify_mono_right/parameter_updates
并且通过rviz可以观察到运行的image_rect,只是没有视差图。
接下来解决视差图的问题。
通过查阅相关资料,在该issue中找到了解决办法。其原因可能是使用了remap的缘故,在这里给usb相机的驱动前面加上namespace就可以了,去除最后的remap:
<launch>
<group ns="stereo">
<node name="left" pkg="usb_cam" type="usb_cam_node" output="screen" >
<param name="video_device" value="/dev/video1" />
<param name="image_width" value="640" />
<param name="image_height" value="480" />
<param name="pixel_format" value="yuyv" />
<param name="camera_frame_id" value="left" />
<param name="camera_name" value="left" />
<param name="io_method" value="mmap"/>
<param name="framerate" value="30" />
<!-- file://${ROS_HOME}/camera_info/${NAME}.yaml -->
<param name="camera_info_url" value="file:///home/nan/dev/cam_ws/src/usb_cam/config/left.yaml"/>
<!-- <remap from="/left/camera_info" to="/mystereo/left/camera_info"/>
<remap from="/left/image_raw" to="/mystereo/left/image_raw"/>
-->
</node>
<node name="right" pkg="usb_cam" type="usb_cam_node" output="screen" >
<param name="video_device" value="/dev/video2" />
<param name="image_width" value="640" />
<param name="image_height" value="480" />
<param name="pixel_format" value="yuyv" />
<param name="camera_frame_id" value="right" />
<param name="camera_name" value="right" />
<param name="io_method" value="mmap"/>
<param name="framerate" value="30" />
<param name="camera_info_url" value="file:///home/nan/dev/cam_ws/src/usb_cam/config/right.yaml"/>
<!-- <remap from="/right/camera_info" to="/mystereo/right/camera_info"/>
<remap from="/right/image_raw" to="/mystereo/right/image_raw"/>
-->
</node>
<node name="stereo_image_proc" pkg="stereo_image_proc" type="stereo_image_proc" output="screen">
<param name="queue_size" value="2000" />
<param name="approximate_sync" value="True" />
</node>
<!-- <node name="image_view" pkg="image_view" type="image_view" respawn="false" output="screen">
<remap from="image" to="/usb_cam/image_raw"/>
<param name="autosize" value="true" />
</node> -->
</group>
<node name="stereo_cam" pkg="image_view" type="stereo_view" output="screen" >
<param name="queue_size" value="2000" />
<remap from="/image" to="/image_rect" />
<param name="approximate_sync" value="True" />
</node>
</launch>
然后就可以正常显示了!
再次更新:
目前通过拍摄同一个秒表,可以发现如下情况:
每获取11帧图像 ,其中会有2帧图像的时间差异明显(大于10ms),显示为相差40ms左右,即相差了一帧图像。如何从软件算法上消除这种影响,是值得后续研究的内容。要不就直接换个可以硬件触发的设备吧...T_T...