Ask Your Question
0

kitti rosbag rtabmap_ros

asked 2019-07-23 11:15:41 -0600

lixz123007 gravatar image

updated 2020-04-26 22:56:13 -0600

matlabbe gravatar image

i download kitti kitti_2011_09_26_drive_0001_synced and use kitti2bag python make it become kitti_2011_09_26_drive_0001_synced.bag In ORBslam2 ,it works well. So i try to us rtabmap_ros my roslanuch:

< launch>
< node pkg="rosbag" type="play" name="stereo_camera" output="screen" args="--loop --clock /home/harold/rosbag/kitti_2011_09_26_drive_0001_synced.bag">
< remap from="/kitti/camera_color_left/image_raw" to="/stereo_camera/left/image_raw"/>
< remap from="/kitti/camera_color_right/image_raw" to="/stereo_camera/right/image_raw"/>
< remap from="/kitti/camera_color_left/camera_info" to="/stereo_camera/left/camera_info"/>
< remap from="/kitti/camera_color_right/camera_info" to="/stereo_camera/right/camera_info"/>
< remap from="/tf" to="/stereo_camera/tf"/>
< remap from="/tf_static" to="/stereo_camera/tf_static"/>
< /node>
< !-- Rotate the camera frame. -->
  < arg name="pi/2" value="1.5707963267948966" />
  < arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
  < node pkg="tf" type="static_transform_publisher" name="camera_base_link" output="screen"
         args="$(arg optical_rotate) base_link stereo_camera 100"  />  
    < group ns="/stereo_camera" > 
    < node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc"/>
  < /group>
< /launch>

(some space becuase it will disappear) i run rtab_map:

roslaunch rtabmap_ros stereo_mapping.launch stereo_namespace:="/stereo_camera" rtabmap_args:="--delete_db_on_start" approximate_sync:=true

then i get:

[ WARN] [1563896739.041306891]: /rtabmap/stereo_odometry: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. Parameter "approx_sync" is false, which means that input topics should have all the exact timestamp for the callback to be called.
/rtabmap/stereo_odometry subscribed to (exact sync):
   /stereo_camera/left/image_rect_color,
   /stereo_camera/right/image_rect,
   /stereo_camera/left/camera_info,
   /stereo_camera/right/camera_info

[ WARN] [1563896735.981168926]: /rtabmap/rtabmapviz: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). Parameter "approx_sync" is false, which means that input topics should have all the exact timestamp for the callback to be called.
/rtabmap/rtabmapviz subscribed to (exact sync):
   /odom,
   /stereo_camera/left/image_rect_color,
   /stereo_camera/right/image_rect,
   /stereo_camera/left/camera_info,
   /stereo_camera/right/camera_info,
   /rtabmap/odom_info

[ WARN] [1563896736.761637361]: /rtabmap/rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). Parameter "approx_sync" is false, which means that input topics should have all the exact timestamp for the callback to be called.
/rtabmap/rtabmap subscribed to (exact sync):
   /odom,
   /stereo_camera/left/image_rect_color,
   /stereo_camera/right/image_rect,
   /stereo_camera/left/camera_info,
   /stereo_camera/right/camera_info,
   /rtabmap/odom_info

*i have every topic it mentioned But /odom and /odom_info have no new message *

rqt_graph

edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
2

answered 2019-07-23 14:48:46 -0600

matlabbe gravatar image

updated 2020-04-26 22:54:16 -0600

The problem is that the "sync" dataset doesn't have exactly the same stamps for left and right images. You may copy the stamps from one timestamps.txt to all others so that the image/camera_info contained in the generated bag have the same stamps (if so, use approx_sync:=false and approx_rgbd_sync:=false in examples below). Otherwise, we should set approx_sync to true. I recommend to use rtabmap.launch directly. Example for stereo:

$ roslaunch rtabmap_ros rtabmap.launch \
   stereo:="true" \
   left_image_topic:=/kitti/camera_color_left/image_raw \
   right_image_topic:=/kitti/camera_color_right/image_raw \
   left_camera_info_topic:=/kitti/camera_color_left/camera_info \
   right_camera_info_topic:=/kitti/camera_color_right/camera_info \
   rtabmap_args:="--delete_db_on_start --Grid/CellSize 0.3 --Grid/ClusterRadius 1" \
   approx_sync:=true \
   use_sim_time:=true\
   frame_id:=base_link

$ rosbag play --clock kitti_2011_09_26_drive_0001_synced.bag

image description

Velodyne: you can also record the velodyne clouds at the same time:

$ roslaunch rtabmap_ros rtabmap.launch \
   stereo:="true" \
   left_image_topic:=/kitti/camera_color_left/image_raw \
   right_image_topic:=/kitti/camera_color_right/image_raw \
   left_camera_info_topic:=/kitti/camera_color_left/camera_info \
   right_camera_info_topic:=/kitti/camera_color_right/camera_info \
   rtabmap_args:="--delete_db_on_start --Grid/CellSize 0.3 --Grid/ClusterRadius 1" \
   approx_sync:=true \
   use_sim_time:=true \
   frame_id:=base_link \
   rgbd_sync:=true \
   approx_rgbd_sync:=true \
   subscribe_scan_cloud:=true \
   scan_cloud_topic:=/kitti/velo/pointcloud

$ rosbag play --clock kitti_2011_09_26_drive_0001_synced.bag

image description

IMU, Ground truth and GPS First we need to modify TF /world -> /base_link in the bag to /world -> /base_link_gt. This can be done with a script like this (called filterTF.py):

import rosbag
from tf.msg import tfMessage
with rosbag.Bag('kitti_2011_09_26_drive_0001_synced_out.bag', 'w') as outbag:
    for topic, msg, t in rosbag.Bag('kitti_2011_09_26_drive_0001_synced.bag').read_messages():
        if topic == "/tf" and msg.transforms:
            newList = [];
            for m in msg.transforms:
                if m.header.frame_id == "world":
                    m.child_frame_id = "base_link_gt"
                newList.append(m)
            if len(newList)>0:
                msg.transforms = newList
                outbag.write(topic, msg, t)
        else:
            outbag.write(topic, msg, t)

Then:

$ python filterTF.py

$ roslaunch rtabmap_ros rtabmap.launch \
   stereo:="true" \
   left_image_topic:=/kitti/camera_color_left/image_raw \
   right_image_topic:=/kitti/camera_color_right/image_raw \
   left_camera_info_topic:=/kitti/camera_color_left/camera_info \
   right_camera_info_topic:=/kitti/camera_color_right/camera_info \
   rtabmap_args:="--delete_db_on_start --Grid/CellSize 0.3 --Grid/ClusterRadius 1 --Optimizer/GravitySigma 0.3" \
   approx_sync:=true \
   use_sim_time:=true \
   frame_id:=base_link \
   rgbd_sync:=false \
   approx_rgbd_sync:=false \
   subscribe_scan_cloud:=true \
   scan_cloud_topic:=/kitti/velo/pointcloud \
   imu_topic:=/kitti/oxts/imu \
   gps_topic:=/kitti/oxts/gps/fix \
   wait_imu_to_init:=true \
   ground_truth_frame_id:=world \
   ground_truth_base_frame_id:=base_link_gt

$ rosbag play --clock kitti_2011_09_26_drive_0001_synced_out.bag

cheers,
Mathieu

edit flag offensive delete link more

Comments

It works !!Thank you very very very very much!This question quzzled me 2 days. Another little question: kitti2rosbag made camera_info's the height and the width reversed. Any ideal to fix it ? OR it is unnecessary to fix?

lixz123007 gravatar image lixz123007  ( 2019-07-24 04:59:56 -0600 )edit

The generated bag I tested is ok, left color camera info: height: 375 width: 1242 width/height are saved in rtabmap, so it would be better if they are fixed. The related code in kitti2bag seems here. However, on my side it is ok, I used the docker image to generate the bag.

matlabbe gravatar image matlabbe  ( 2019-07-24 08:31:13 -0600 )edit

Very thanks! And i meet a new question!here: https://answers.ros.org/question/3297... Cloud you save me agin,please~ Thank you,again!

lixz123007 gravatar image lixz123007  ( 2019-07-30 21:48:55 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2019-07-23 11:15:41 -0600

Seen: 453 times

Last updated: Apr 26