kitti rosbag rtabmap_ros

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

lixz123007
27 ●5 ●7 ●11

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

matlabbe

6409 ●11 ●96 ●80

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

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

matlabbe

6409 ●11 ●96 ●80

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

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 ( 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 ( 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 ( 2019-07-30 21:48:55 -0600 )edit

add a comment

1 Answer

Comments

Question Tools

Stats

Related questions

kitti rosbag rtabmap_ros edit

1 Answer

Comments

Question Tools

Stats

Related questions

kitti rosbag rtabmap_ros