introlab / rtabmap_ros

RTAB-Map's ROS package.
http://wiki.ros.org/rtabmap_ros
BSD 3-Clause "New" or "Revised" License
974 stars 557 forks source link

unable to run rtab map using ZED stereo camera #113

Closed umangty closed 7 years ago

umangty commented 8 years ago

i am trying to run rtab map using ZED stereo camera as in kinnet mode . I am following same process as given in the tutorial . The rqt_graph has been attached . Everything seems fine. I am running this over a bag file. I used use sim time true for simulating . I can see no output on my machine just blank GUI.

The launch file used was

  1. roslaunch rtabmap_ros rgbd_mapping.launch rtabmap_args:="--delete_db_on_start" depth_registered_topic:=/camera/depth/image_rect_color
  2. rosrun tf static_transform_publisher 0 0 0 -1.5707963267948966 0 -1.5707963267948966 camera_link zed_initial_frame 100

The rqt_graph seems like rosgraph The bag file i used is attached below https://drive.google.com/file/d/0B4fpmUWF5h-1eVVscFRMNnl1eVE/view?usp=sharing Please help me solve this issue

matlabbe commented 8 years ago

Hi,

The frame rate of the images (1.6 Hz) is quite low comparing to camera_info(16 Hz):

$ rosbag info 2016-09-01-14-13-54.bag 
path:        2016-09-01-14-13-54.bag
version:     2.0
duration:    23.9s
start:       Sep 01 2016 04:43:54.60 (1472719434.60)
end:         Sep 01 2016 04:44:18.49 (1472719458.49)
size:        265.8 MB
messages:    847
compression: none [89/89 chunks]
types:       sensor_msgs/CameraInfo [c9a58c1b0b154e0e6da7578cb991d214]
             sensor_msgs/Image      [060021388200f6f0f447d0fcd9c64743]
             tf2_msgs/TFMessage     [94810edda583a504dfda3829e70d7eec]
topics:      /camera/depth/camera_info        379 msgs    : sensor_msgs/CameraInfo
             /camera/depth/image_rect_color    38 msgs    : sensor_msgs/Image     
             /camera/left/camera_info         378 msgs    : sensor_msgs/CameraInfo
             /camera/left/image_rect_color     50 msgs    : sensor_msgs/Image     
             /tf_static                         2 msgs    : tf2_msgs/TFMessage     (2 connections)

We should increase the queue_size (e.g., 20 instead of 10) to be able to synchronize the input topics. Note that 1.6 Hz for odometry it is too low, unless the camera is moving very slowly, otherwise it gets easily lost.

To run the bag (note that I am using rtabmap.launch, which has more options like queue_size):

$ roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="--delete_db_on_start" depth_topic:=/camera/depth/image_rect_color rgb_topic:=/camera/left/image_rect_color camera_info_topic:=/camera/left/camera_info queue_size:=20
$ rosparam set use_sim_time true
$ rosrun tf static_transform_publisher 0 0 0 -1.5707963267948966 0 -1.5707963267948966 camera_link zed_optical_frame 100
$ rosbag play --clock 2016-09-01-14-13-54.bag

screenshot from 2016-09-01 09 35 55

cheers

willdzeng commented 8 years ago

Hi @umangty, I recommend using ZED on USB3.0 thus its frame rate is much higher, also try to reduce the resolution to achieve higher frame rate.