In my other tutorial, I showed you how to calibrate you stereo camera. After Calibration, we can get disparity map and RGBD PCL point cloud from our stereo camera cool huh 🙂
1)Save the following text under “stereo_usb_cam_stream_publisher.launch”
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 |
<launch> <arg name="video_device_right" default="/dev/video1" /> <arg name="video_device_left" default="/dev/video2" /> <arg name="image_width" default="640" /> <arg name="image_height" default="480" /> <arg name="right_image_namespace" default="stereo" /> <arg name="left_image_namespace" default="stereo" /> <arg name="right_image_node" default="right" /> <arg name="left_image_node" default="left" /> <arg name="image_topic_name_right" default="image_raw" /> <arg name="camera_info_topic_name_right" default="camera_info" /> <arg name="image_topic_name_left" default="image_raw" /> <arg name="camera_info_topic_name_left" default="camera_info" /> <arg name="left_camera_name" default="left" /> <arg name="right_camera_name" default="right" /> <arg name="left_camera_frame_id" default="left_camera" /> <arg name="right_camera_frame_id" default="right_camera" /> <arg name="framerate" default="30" /> <arg name="camera_info" default="false" /> <arg name="camera_info_path" default="stereo_camera_info" /> <node ns="$(arg right_image_namespace)" name="$(arg right_image_node)" pkg="usb_cam" type="usb_cam_node" output="screen" > <param name="video_device" value="$(arg video_device_right)" /> <param name="image_width" value="$(arg image_width)" /> <param name="image_height" value="$(arg image_height)"/> <param name="pixel_format" value="yuyv" /> <param name="io_method" value="mmap"/> <remap from="/usb_cam/image_raw" to="$(arg image_topic_name_right)" /> <param name="framerate" value="$(arg framerate)"/> <!-- if camera_info is available, we will use it--> <param name="camera_frame_id" value="$(arg right_camera_frame_id)" if="$(arg camera_info)" /> <param name="camera_info_url" value="file://${ROS_HOME}/$(arg camera_info_path)/right.yaml" if="$(arg camera_info)"/> <param name="camera_name" value="narrow_stereo/$(arg right_camera_name)" if="$(arg camera_info)"/> <remap from="/usb_cam/camera_info" to="$(arg camera_info_topic_name_right)" if="$(arg camera_info)"/> </node> <node ns="$(arg left_image_namespace)" name="$(arg left_image_node)" pkg="usb_cam" type="usb_cam_node" output="screen" > <param name="video_device" value="$(arg video_device_left)" /> <param name="image_width" value="$(arg image_width)" /> <param name="image_height" value="$(arg image_height)"/> <param name="pixel_format" value="yuyv" /> <param name="io_method" value="mmap"/> <param name="framerate" value="$(arg framerate)"/> <remap from="/usb_cam/image_raw" to="$(arg image_topic_name_left)"/> <!-- if camera_info is available, we will use it--> <param name="camera_frame_id" value="$(arg left_camera_frame_id)" if="$(arg camera_info)"/> <param name="camera_name" value="narrow_stereo/$(arg left_camera_name)" if="$(arg camera_info)"/> <param name="camera_info_url" value="file://${ROS_HOME}/$(arg camera_info_path)/left.yaml" if="$(arg camera_info)"/> <remap from="/usb_cam/camera_info" to="$(arg camera_info_topic_name_left)" if="$(arg camera_info)"/> </node> </launch> |
2) Then run the following node to publish both cameras and camera info (calibration matrix)
|
1 |
roslaunch stereo_usb_cam_stream_publisher.launch video_device_right:=/dev/video1 video_device_left:=/dev/video2 image_width:=640 image_height:=480 left_camera_name:=left right_camera_name:=right camera_info:=true camera_info_path:=stereo_camera_info |
3) Run the following to rectify image and compute the disparity map:
|
1 |
ROS_NAMESPACE=stereo rosrun stereo_image_proc stereo_image_proc _approximate_sync:=true |
Super important: If you have USB cam with some delays you should add the following “_approximate_sync:=true”
4) Let’s view everything:
|
1 |
rosrun image_view stereo_view stereo:=/stereo image:=image_rect _approximate_sync:=True _queue_size:=10 |
Super important: If you have USB cam with some delays you should add the following “_approximate_sync:=True _queue_size:=10”
5) Running rqt graph should give you the following:
6) Run the to configure the matching algorithm parameter:
|
1 |
rosrun rqt_reconfigure rqt_reconfigure |
7) PCL pointcloud in RVIZ
