Running Kimera-Multi on EuRoC MH: issues with results

[TakShimoda]

Hello, I'm raising a new issue, as directed by @yunzc from this thread. Thank you for helping me to get it running on campus_outdoor_1014. I'm now trying to run it on EuRoC Machine Hall dataset with 5 robots.

I worked from 1014-example.yaml and took a look at kimera_vios_ros_euroc.launch as suggested by yunzc. Here's what I changed:

1. I edited the yaml to look like the following:

session_name: kimera-distributed-mh-example

environment:
  ROBOT0: "acl_jackal"
  ROBOT1: "acl_jackal2"
  ROBOT2: "sparkal1"
  ROBOT3: "sparkal2"
  ROBOT4: "hathor"
  #ROBOT5: "thoth"
  ROSBAG0: "$DATA_PATH/MH_01_easy.bag"
  ROSBAG1: "$DATA_PATH/MH_02_easy.bag"
  ROSBAG2: "$DATA_PATH/MH_03_medium.bag"
  ROSBAG3: "$DATA_PATH/MH_04_difficult.bag"
  ROSBAG4: "$DATA_PATH/MH_05_difficult.bag"
  #ROSBAG5: "$DATA_PATH/10_14_thoth.bag"
  RATE: "1.0"
  NUM_ROBOTS: "5"
  BOW_SKIP_NUM: "3"

options:
  default-command: /bin/bash

windows:
- window_name: frontend
  layout: tiled  
  shell_command_before:
    - source $CATKIN_WS/devel/setup.bash
    - mkdir -p $LOG_DIR/$ROBOT0/distributed
    - mkdir -p $LOG_DIR/$ROBOT1/distributed
    - mkdir -p $LOG_DIR/$ROBOT2/distributed
    - mkdir -p $LOG_DIR/$ROBOT3/distributed
    - mkdir -p $LOG_DIR/$ROBOT4/distributed
    #- mkdir -p $LOG_DIR/$ROBOT5/distributed
    - sleep 5;

  panes:
    - roslaunch kimera_distributed kimera_distributed.launch robot_id:=0 robot_name:=$ROBOT0 num_robots:=$NUM_ROBOTS dataset_name:=Jackal log_output_path:=$LOG_DIR/$ROBOT0/distributed
    - roslaunch kimera_distributed kimera_distributed.launch robot_id:=1 robot_name:=$ROBOT1 num_robots:=$NUM_ROBOTS dataset_name:=Jackal log_output_path:=$LOG_DIR/$ROBOT1/distributed
    - roslaunch kimera_distributed kimera_distributed.launch robot_id:=2 robot_name:=$ROBOT2 num_robots:=$NUM_ROBOTS dataset_name:=Jackal log_output_path:=$LOG_DIR/$ROBOT2/distributed
    - roslaunch kimera_distributed kimera_distributed.launch robot_id:=3 robot_name:=$ROBOT3 num_robots:=$NUM_ROBOTS dataset_name:=Jackal log_output_path:=$LOG_DIR/$ROBOT3/distributed
    - roslaunch kimera_distributed kimera_distributed.launch robot_id:=4 robot_name:=$ROBOT4 num_robots:=$NUM_ROBOTS dataset_name:=Jackal log_output_path:=$LOG_DIR/$ROBOT4/distributed
    #- roslaunch kimera_distributed kimera_distributed.launch robot_id:=5 robot_name:=$ROBOT5 num_robots:=$NUM_ROBOTS dataset_name:=Jackal log_output_path:=$LOG_DIR/$ROBOT5/distributed

- window_name: dpgo
  layout: tiled
  shell_command_before:
    - source $CATKIN_WS/devel/setup.bash
    - sleep 5;

  panes:
    - roslaunch kimera_distributed dpgo.launch robot_id:=0 robot_name:=$ROBOT0 num_robots:=$NUM_ROBOTS dataset_name:=Euroc multi_master:=false log_output_path:=$LOG_DIR/$ROBOT0/distributed
    - roslaunch kimera_distributed dpgo.launch robot_id:=1 robot_name:=$ROBOT1 num_robots:=$NUM_ROBOTS dataset_name:=Euroc multi_master:=false log_output_path:=$LOG_DIR/$ROBOT1/distributed
    - roslaunch kimera_distributed dpgo.launch robot_id:=2 robot_name:=$ROBOT2 num_robots:=$NUM_ROBOTS dataset_name:=Euroc multi_master:=false log_output_path:=$LOG_DIR/$ROBOT2/distributed
    - roslaunch kimera_distributed dpgo.launch robot_id:=3 robot_name:=$ROBOT3 num_robots:=$NUM_ROBOTS dataset_name:=Euroc multi_master:=false log_output_path:=$LOG_DIR/$ROBOT3/distributed
    - roslaunch kimera_distributed dpgo.launch robot_id:=4 robot_name:=$ROBOT4 num_robots:=$NUM_ROBOTS dataset_name:=Euroc multi_master:=false log_output_path:=$LOG_DIR/$ROBOT4/distributed
    #- roslaunch kimera_distributed dpgo.launch robot_id:=5 robot_name:=$ROBOT5 num_robots:=$NUM_ROBOTS dataset_name:=Euroc multi_master:=false log_output_path:=$LOG_DIR/$ROBOT5/distributed

- window_name: vio
  layout: tiled  
  shell_command_before:
    - source $CATKIN_WS/devel/setup.bash
    - sleep 5;

  panes:
    - roslaunch kimera_multi kimera_vio_jackal_MH.launch robot_name:=$ROBOT0 robot_id:=0 use_d455:=false multirobot:=true lcd_no_optimize:=true use_external_odom:=false replay:=true should_use_sim_time:=true left_cam_topic:=/$ROBOT0/cam0/image_raw right_cam_topic:=/$ROBOT0/cam1/image_raw imu_topic:=/$ROBOT0/imu0
    - roslaunch kimera_multi kimera_vio_jackal_MH.launch robot_name:=$ROBOT1 robot_id:=1 use_d455:=false multirobot:=true lcd_no_optimize:=true use_external_odom:=false replay:=true should_use_sim_time:=true left_cam_topic:=/$ROBOT1/cam0/image_raw right_cam_topic:=/$ROBOT1/cam1/image_raw imu_topic:=/$ROBOT1/imu0
    - roslaunch kimera_multi kimera_vio_jackal_MH.launch robot_name:=$ROBOT2 robot_id:=2 use_d455:=false multirobot:=true lcd_no_optimize:=true use_external_odom:=false replay:=true should_use_sim_time:=true left_cam_topic:=/$ROBOT2/cam0/image_raw right_cam_topic:=/$ROBOT2/cam1/image_raw imu_topic:=/$ROBOT2/imu0
    - roslaunch kimera_multi kimera_vio_jackal_MH.launch robot_name:=$ROBOT3 robot_id:=3 use_d455:=false multirobot:=true lcd_no_optimize:=true use_external_odom:=false replay:=true should_use_sim_time:=true left_cam_topic:=/$ROBOT3/cam0/image_raw right_cam_topic:=/$ROBOT3/cam1/image_raw imu_topic:=/$ROBOT3/imu0
    - roslaunch kimera_multi kimera_vio_jackal_MH.launch robot_name:=$ROBOT4 robot_id:=4 use_d455:=false multirobot:=true lcd_no_optimize:=true use_external_odom:=false replay:=true should_use_sim_time:=true left_cam_topic:=/$ROBOT4/cam0/image_raw right_cam_topic:=/$ROBOT4/cam1/image_raw imu_topic:=/$ROBOT4/imu0
    #- roslaunch kimera_multi kimera_vio_jackal.launch robot_name:=$ROBOT5 robot_id:=5 use_d455:=true multirobot:=true lcd_no_optimize:=true use_external_odom:=true replay:=true should_use_sim_time:=true
    - 

- window_name: data
  focus: true  
  layout: tiled  
  shell_command_before:
    - source $CATKIN_WS/devel/setup.bash

  panes:
    - sleep 15; roslaunch kimera_distributed MH_rosbag.launch bagfile:=$ROSBAG0 input_ns:=$ROBOT0 output_ns:=$ROBOT0 rate:=$RATE
    - sleep 15; roslaunch kimera_distributed MH_rosbag.launch bagfile:=$ROSBAG1 input_ns:=$ROBOT1 output_ns:=$ROBOT1 rate:=$RATE
    - sleep 15; roslaunch kimera_distributed MH_rosbag.launch bagfile:=$ROSBAG2 input_ns:=$ROBOT2 output_ns:=$ROBOT2 rate:=$RATE
    - sleep 15; roslaunch kimera_distributed MH_rosbag.launch bagfile:=$ROSBAG3 input_ns:=$ROBOT3 output_ns:=$ROBOT3 rate:=$RATE
    - sleep 15; roslaunch kimera_distributed MH_rosbag.launch bagfile:=$ROSBAG4 input_ns:=$ROBOT4 output_ns:=$ROBOT4 rate:=$RATE
    #- sleep 15; roslaunch kimera_distributed mit_rosbag.launch bagfile:=$ROSBAG5 input_ns:=$ROBOT5 output_ns:=$ROBOT5 rate:=$RATE
    - sleep 15; rviz -d $(rospack find kimera_distributed)/rviz/single_machine.rviz
    - roscore
    - rosnode kill -a \

• The main edits I made in this file are:
  • Changing the number of robots to 5 and commenting out commands for the 6th robot
  • Paths to rosbags
  • In the vio window, I changed the following argument: use_external_odom:=false, and added the arguments for the topics for the EuRoC bag file: left_cam_topic:=/$ROBOT0/cam0/image_raw right_cam_topic:=/$ROBOT0/cam1/image_raw imu_topic:=/$ROBOT0/imu0
  • Changed the files kimera_vio_jackal.launch to kimera_vio_jackal_MH.launch in the vio window, and mit_rosbag.launch to MH_rosbag.launch in the data window which I'll explain the changes in 2. and 3.

2. Changes to kimera_vio_jackal.launch. The only changes I made are:

• line 35 to <arg name="params_folder" value="$(find kimera_vio)/params/Euroc" />
  • This is so I used the appropriate parameters from the EuRoC dataset.
• line 55 to <include file="$(find kimera_vio_ros)/launch/kimera_vio_ros_euroc.launch" pass_all_args="true"/>
  • This is so the appropriate parameters and values are first set from the euroc launch file (e.g. the accelerometer/gyro bias parameters) before launching kimera_vios_ros.launch.

3. Changes to mit_rosbag.launch. The changes are so the appropriate topics are published from the EuRoC bag files:

• changed lines 14, 16, and 18 to go from (/$(arg input_ns)/forward/imu, /$(arg input_ns)/forward/infra1/image_rect_raw/compressed, /$(arg input_ns)/forward/infra2/image_rect_raw/compressed) to (/imu0, /cam0/image_raw, /cam1/image_raw)
• changed lines 28, 30, and 32 to:

<remap from="/imu0"                    to="/$(arg output_ns)/imu0"/>
<remap from="/cam0/image_raw" to="/$(arg output_ns)/cam0/image_raw"/>
<remap from="/cam1/image_raw" to="/$(arg output_ns)/cam1/image_raw"/>

4. Added an ExternalOdometryParams.yaml file in kimera_vio/params/EuRoC folder. The contents of this folder are the exact same as from the D455 folder used for the 1014 example. The reason I did this is that the vio node crashed due to an error of not finding the ExternalOdometryParams.yaml file, even though I set use_external_odom:=false as explained in step 1.

RESULTS I get proper output for all the windows: frontend receives candidate loop closures and performed robust initialization of robots in the global frame, dpgo performed optimization of the poses, and the vio was properly running for each robot.

The output in rviz, however gives a poor trajectory:

when I evaluated one of the trajectories (e.g. robot 1: jackal) with the latest kimera_distributed_poses_xxx.csv file in the output log, it seems there are only a few trajectories given in a short span.

What I think are causing the issue:

1. The use_external_odom parameter. It's set to false in the kimera_vio_ros.launch and kimera_vio_ros_euroc.launch files, but is set to true in kimera_vio_jackal.launch. I had to set it to false, because if I set it to true, the frontend wouldn't detect any loop closure. I'm thinking this parameter is true if the system has an IMU external to the camera, but I'm not sure.
2. Line 141 of kimera_vio_ros.launch being commented out. Maybe I thought this was related to loading calibration parameters and also would be helpful with my own datasets, but I realize there is no calibration.yaml file under the params folder.

Any help would be appreciated. Thank you.