There is a large delay on trajectory visualization while using Kimera-VIO-ROS with D435i

[AaronLPS]

I was following the Hardware Setup. Trajectory visualization with RVIZ didn't update in real time. There was a large time lag ( >30 sec and may become worse) between the movement of the D435i and trajectory visualization.

6. Visualize trajectory with RVIZ using rviz -d $(rospack find kimera_vio_ros)/rviz/kimera_vio_euroc.rviz, where kimera_vio_euroc.rviz can be repaced with your rviz setup file.

Time lag is also observed while using rostopic echo /kimera_vio_ros/odometry instead of Rviz.

The attached is the printed statistics during the test.

[msadowski]

@AaronLPS I've just run into exactly the same issue. Did you happen to find a solution for it?

[ToniRV]

What is the command you are running Kimera with?

[msadowski]

I've run a slightly modified version of kimera_vio_ros_realsense_IR.launch. The only change I did were connected to the topic names.

[ToniRV]

Ok, are you running it in:

• online mode: in another terminal you are running the rosbag play rosbag_path.bag --clock or
• offline mode: is Kimera parsing the rosbag?
  And also, are you running Kimera in parallel or sequential mode (see params/dataset/PipelineParams.yaml).

[msadowski]

I was hoping to run it directly with the D435i connected to my computer, so I've made sure to set online to true. The prallel_run in Kimera-VIO/params/RealSenseIR/PipelineParams.yaml is set to 1.

Here is my full luanch file (the modified copy of kimera_vio_ros_realsense_IR.launch) in case it helps:

<launch>
  <arg name="dataset_name" value="RealSenseIR"/>
  <arg name="online" default="True" />

  <arg name="log_output" default="false"/>
  <arg name="log_output_path"
       default="$(find kimera_vio_ros)/output_logs/"
       if="$(arg log_output)"/>
  <arg name="log_gt_data" default="false"/>
  <arg name="gt_topic" default=""/>
  <!-- Set use_sim_time to true if you use rosbag with clock argument -->
  <arg name="should_use_sim_time" default="false"/>

  <!-- Only used when parsing a rosbag -->
  <arg name="rosbag_path" default="/home/yunchang/data/kitti_2011_09_26_drive_0005_unsynced.bag"
       unless="$(arg online)"/>"/>

  <!-- Frame IDs. These DO NOT match frame id's on the video streams, as the
	RealSense and Kimera publish conflicting Tf's -->
  <arg name="world_frame_id"     value="world"/>
  <arg name="base_link_frame_id" value="base_link"/>
  <arg name="map_frame_id"       value="map"/>
  <arg name="left_cam_frame_id"  value="left_cam"/>
  <arg name="right_cam_frame_id" value="right_cam"/>

  <!-- Subscribed Topics -->
  <arg name="left_cam_topic"  value="/rs_d435i/infra1/image_rect_raw"/>
  <arg name="left_cam_info_topic"  value="/rs_d435i/infra1/camera_info"/>
  <arg name="right_cam_topic" value="/rs_d435i/infra2/image_rect_raw"/>
  <arg name="right_cam_info_topic"  value="/rs_d435i/infra2/camera_info"/>
  <arg name="imu_topic"       value="/rs_d435i/imu"/>

  <!-- Launch actual pipeline -->
  <include file="$(find kimera_vio_ros)/launch/kimera_vio_ros.launch"
           pass_all_args="true"/>

 <!-- Launch static TF node from base_link to velo_link -->
 <node pkg="tf" type="static_transform_publisher" name="velo_link_broadcaster"
   args="0 0 0 0 0 0 1 base_link velo_link 100"/>

</launch>

I've also checked the delay of the kimera odometry topic and got the following results:

rostopic delay /kimera_vio_ros/odometry
subscribed to [/kimera_vio_ros/odometry]
average delay: 16.646
        min: 16.548s max: 16.738s std dev: 0.08026s window: 5
average delay: 16.758
        min: 16.548s max: 16.922s std dev: 0.13167s window: 9
average delay: 16.860
        min: 16.548s max: 17.140s std dev: 0.18486s window: 13
average delay: 16.962
        min: 16.548s max: 17.322s std dev: 0.23921s window: 17
average delay: 17.065
        min: 16.548s max: 17.554s std dev: 0.29853s window: 21
average delay: 17.145
        min: 16.548s max: 17.780s std dev: 0.34748s window: 24
average delay: 17.259
        min: 16.548s max: 18.000s std dev: 0.42224s window: 28
average delay: 17.371
        min: 16.548s max: 18.184s std dev: 0.49004s window: 32
average delay: 17.486
        min: 16.548s max: 18.483s std dev: 0.56255s window: 36
average delay: 17.516
        min: 16.548s max: 18.553s std dev: 0.58172s window: 37

[AaronLPS]

When running it with real hardware, you may try to lower the input image frequency from the realsense ros node. The default value is 30Hz. You can try to set to 15hz. The computation is too heavy to maintain real-time tracking. You may also try to change other parameters to control the computation.