目的:參考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>
然後就可以正常顯示了!