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.

[yunzc]

Hi,
Looks like Kimera-VIO is not working properly.

To answer your two points:

1. use_external_odom should definitely be off because this flag tells Kimera-VIO to fuse in an external odometry source (like wheel odometry, kinematic odometry, etc.) but I don't think we have any odometry that we can fuse for EuROC.
2. That line should not be needed.

Things to try:
Let's take a step back and try with just a single robot. Are you able to directly use kimera_vio_ros_euroc.launch and get it running with euroc? How does the output look?

[TakShimoda]

Hello,

Thank you for your quick response.

1. That makes sense, so setting it to false should be correct.
2. This line was commented out anyways so it definitely wasn't the problem or going to solve any issues.

I did try launching the kimera_vio_ros_euroc.launch file on MH01 with the 4 steps explained here.

What happened was that the system was running, but due to an error of there being no /world topic on rviz, I had to switch to the /odom topic to visualize, where the mesh was visible, but not really building and seemed to flicker a lot. In the terminal, there were occasional error messages Delaunay Triangle out of image.

I'm currently home and won't be back at my school computer where I do experiments until Monday, so I could try running a full run on single robot VIO, evaluate the trajectory, and provide more details when I get back.

Thank you.

[TakShimoda]

Hello,

I ran the same example for kimera_vio_ros_euroc.launch and some more details about what happens when I run it are:

• rviz: the sidebar only showed errors relating to "no frame /world". If I change to odom, all the errors go away. However, it was hard to see any frames outside of odom even when I zoomed out. Here is an example where I can see odom, but it looks the base frame is very far away:
  

  • The mesh is also only partially visible like below, where it also flickers a lot:
    

• nodes: the output of rosnodes list is:

/kimera_vio_ros/kimera_vio_ros_node
/kimera_vio_ros/posegraph_viewer
/play_1695650794559388577
/rosout
/rviz_1695650754859263882

• Terminal output: in the terminal where I run the vio launch file, I get output like the following which repeats:

I20230925 10:21:31.503928  7705 KimeraVioRos.cpp:215] Statistics
-----------                                  #	Log Hz	{avg     +- std    }	[min,max]
Display [ms]                            	  673	7.65751	{0.0594354 +- 0.00000}	[0,31]
Mesher [ms]                             	  337	3.83446	{5.05638 +- 1.63534}	[1,12]
Stereo Data Provider [ms]               	    0	
VioBackend [ms]                         	  337	3.83482	{10.3323 +- 2.03194}	[0,17]
VioFrontend Frame Rate [ms]             	 1419	16.1814	{3.48485 +- 0.922174}	[2,7]
VioFrontend Keyframe Rate [ms]          	  336	3.83662	{42.7768 +- 3.35983}	[32,63]
VioFrontend [ms]                        	 1756	19.9582	{11.0524 +- 16.4569}	[2,63]
Visualizer [ms]                         	  337	3.83444	{0.0830861 +- 0.287623}	[0,1]
backend_input_queue Size [#]            	  337	3.83500	{1.00000 +- 0.00000}	[1,1]
data_provider_left_frame_queue Size [#] 	 1757	19.9704	{1.00000 +- 0.00000}	[1,1]
data_provider_right_frame_queue Size [#]	 1757	19.9703	{1.00000 +- 0.00000}	[1,1]
display_input_queue Size [#]            	  673	7.65688	{1.00000 +- 0.00000}	[1,1]
frontend_input_queue Size [#]           	 1756	19.9576	{1.00000 +- 0.00000}	[1,1]
mesher_backend Size [#]                 	  337	3.83481	{1.00000 +- 0.00000}	[1,1]
mesher_frontend Size [#]                	 1756	19.9581	{14.0900 +- 1.20286}	[1,101]
visualizer_backend_queue Size [#]       	  337	3.83481	{1.00000 +- 0.00000}	[1,1]
visualizer_frontend_queue Size [#]      	 1756	19.9582	{14.0951 +- 1.20286}	[1,101]
visualizer_mesher_queue Size [#]        	  337	3.83444	{1.00000 +- 0.00000}	[1,1]
W20230925 10:21:31.553601  7705 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W20230925 10:21:31.580574  7845 Mesher.cpp:1459] Missing landmark information to build 3D Mesh.
W20230925 10:21:31.603605  7705 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W20230925 10:21:31.603684  7705 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W20230925 10:21:31.653607  7705 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W20230925 10:21:31.653687  7705 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W20230925 10:21:31.703606  7705 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W20230925 10:21:31.703676  7705 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W20230925 10:21:31.753602  7705 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W20230925 10:21:31.796223  7845 Mesher.cpp:1459] Missing landmark information to build 3D Mesh.
W20230925 10:21:31.803619  7705 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W20230925 10:21:31.803689  7705 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W20230925 10:21:31.853601  7705 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W20230925 10:21:31.853678  7705 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W20230925 10:21:31.903800  7705 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W20230925 10:21:31.903872  7705 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W20230925 10:21:31.953796  7705 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W20230925 10:21:31.987296  7845 Mesher.cpp:1459] Missing landmark information to build 3D Mesh.
W20230925 10:21:32.003593  7705 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W20230925 10:21:32.003641  7705 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W20230925 10:21:32.053797  7705 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W20230925 10:21:32.053881  7705 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W20230925 10:21:32.103613  7705 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W20230925 10:21:32.103701  7705 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W20230925 10:21:32.153609  7705 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W20230925 10:21:32.188374  7845 Mesher.cpp:1459] Missing landmark information to build 3D Mesh.
W20230925 10:21:32.203598  7705 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W20230925 10:21:32.203676  7705 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W20230925 10:21:32.253796  7705 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W20230925 10:21:32.253880  7705 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W20230925 10:21:32.303835  7705 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W20230925 10:21:32.303905  7705 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W20230925 10:21:32.353605  7705 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W20230925 10:21:32.396420  7845 Mesher.cpp:1459] Missing landmark information to build 3D Mesh.
E20230925 10:21:32.396813  7845 Mesher.cpp:1811] Delaunay Triangle out of image (size: x: 0, y: 0, height: 480, width 752
 Triangle: x, y: 
156.75, 421.451
401.483, 447.787
0, 2256

I tried looking for trajectory output to compare it with groundtruth, but I couldn't find any.

Thanks.

[yunzc]

Hi @TakShimoda sorry I forgot to respond. Any updates?
Looks like it's mostly a problem with the setup for Euroc on Kimera-VIO. I can try on my side when I get the chance.

[TakShimoda]

Hello @yunzc, I haven't been working on it, but I ran a single robot VIO and I got the same issues I mentioned above (no output as well, so I can't assess trajectory)

I tried running Kimera-Multi again on MH and what I noticed is that either the loop closure or dpgo seems to get stuck on optimization. After a few minutes after all the rosbags finished playing, the trajectory looks something like this:

where what's shown is that 4 of the trajectories have approximately the correct scale and seemed to have aligned with each other with loop closures. The blue line is the trajectory from robot 0 where the trajectory failed.

Even among the 4 trajectories that seem to have approximately correct, you can see they don't have many keyframes. For example, for robot 1, there's only 118 poses compared to 29993 for ground truth. Here's what the trajectory and xyz for it looks like:

For additional info, dpgo shows that a lot of potential loop closures are not being accepted:

Considering robot 0's failed trajectory and robot 2-5 having few poses, I'm assuming the issue is still kimera-vio for individual robots.

I also noticed dpgo takes a long time, but I think this is due to the faulty individual trajectories and tmux (which I noticed was slow for other multi-robot algorithms as well) as I don't think this was an issue with D-GNC taking more RBCD iterations due to converging to approximate solutions as mentioned in the paper as MH isn't a perceptually aliased or challenging scene.

[TakShimoda]

I had another question which isn't related to the current issue but is related to the output of Kimera-Multi, so I'll ask here in this thread if you don't mind. I was wondering about RPY values not being aligned, which I also asked here. The answer I got was to multiply the rotation by the SO(3) I got from Umeyama's Alignment from the evo library.

But I thought this was already corrected when applying the SIM(3) or SE(3) trajectory alignment between estimate and ground-truth? This was the case for other SLAM algorithms where the RPY values were off initially, but corrected after applying the trajectory alignment with the evo library.

Here's an example using acl_jackal's trajectory from the 1208 dataset:
Unaligned:
evo_traj tum kimera_distributed_poses_1830_tum_new.tum --ref ~/Documents/Trial_Data/Kimera-Multi/ground_truth/1208/acl_jackal_gt_odom.tum -p --plot_mode=xy -v:

Aligned:

As you can see, even after applying the alignment which aligns the xy clearly, the roll angle is still off by about 90 degrees.

This has a significant effect when I calculate ATE/RPE with evo, which according to the library, first computes an error matrix as below:
RPE: $E_{i,j} = \delta_{est_{i,j}} \ominus \delta_{ref_{i,j}} = (P_{ref,i}^{-1}P_{ref,j})^{-1} (P_{est,i}^{-1}P_{est,j}) \in \mathrm{SE}(3)$
ATE: $E_i = P_{est,i} \ominus P_{ref,i} = P_{ref,i}^{-1} P_{est,i} \in \mathrm{SE}(3)$

Then, giving these error matrices, they have multiple ways to calculate ATE/RPE. I used 2 of these:
With respect to translation (default):
$RPE_{i,j} = | \mathrm{trans}(E_{i,j}) |$
$ATE_i = | \mathrm{trans}(E_i) |$
With respect to full transformation:
$RPE_{i,j} = |E_{i,j} - I_{4 \times 4} |_F$

$ATE_i = |E_{i} - I_{4 \times 4} |_F$

For example, here are the ATE statistics by calculating only wrt translation:

       max	0.942798
      mean	0.473431
    median	0.484972
       min	0.134751
      rmse	0.506712
       sse	89.094818
       std	0.180612

And here are the ATE statistics by calculating wrt full transformation:

       max	2.670293
      mean	2.547082
    median	2.536250
       min	2.491495
      rmse	2.547387
       sse	2251.746363
       std	0.039430

Where there's quite a large difference. In section 7B of the Kimera-Multi paper, with the results in Table I, are the ATE values calculated only wrt translation or wrt full transformation? When I compare the two methods with other algorithms, there's almost no difference since the RPY values are fairly close to ground-truth after applying trajectory alignment, but it doesn't happen with the Kimera-Multi output.

[yunzc]

I had another question which isn't related to the current issue but is related to the output of Kimera-Multi, so I'll ask here in this thread if you don't mind. I was wondering about RPY values not being aligned, which I also asked here. The answer I got was to multiply the rotation by the SO(3) I got from Umeyama's Alignment from the evo library.

But I thought this was already corrected when applying the SIM(3) or SE(3) trajectory alignment between estimate and ground-truth? This was the case for other SLAM algorithms where the RPY values were off initially, but corrected after applying the trajectory alignment with the evo library.

Here's an example using acl_jackal's trajectory from the 1208 dataset: Unaligned: evo_traj tum kimera_distributed_poses_1830_tum_new.tum --ref ~/Documents/Trial_Data/Kimera-Multi/ground_truth/1208/acl_jackal_gt_odom.tum -p --plot_mode=xy -v:

Aligned:

As you can see, even after applying the alignment which aligns the xy clearly, the roll angle is still off by about 90 degrees.

This has a significant effect when I calculate ATE/RPE with evo, which according to the library, first computes an error matrix as below: RPE: Ei,j=δesti,j⊖δrefi,j=(Pref,i−1Pref,j)−1(Pest,i−1Pest,j)∈SE(3) ATE: Ei=Pest,i⊖Pref,i=Pref,i−1Pest,i∈SE(3)

Then, giving these error matrices, they have multiple ways to calculate ATE/RPE. I used 2 of these: With respect to translation (default): RPEi,j=|trans(Ei,j)| ATEi=|trans(Ei)| With respect to full transformation: RPEi,j=|Ei,j−I4×4|F

ATEi=|Ei−I4×4|F

For example, here are the ATE statistics by calculating only wrt translation:

       max	0.942798
      mean	0.473431
    median	0.484972
       min	0.134751
      rmse	0.506712
       sse	89.094818
       std	0.180612

And here are the ATE statistics by calculating wrt full transformation:

       max	2.670293
      mean	2.547082
    median	2.536250
       min	2.491495
      rmse	2.547387
       sse	2251.746363
       std	0.039430

Where there's quite a large difference. In section 7B of the Kimera-Multi paper, with the results in Table I, are the ATE values calculated only wrt translation or wrt full transformation? When I compare the two methods with other algorithms, there's almost no difference since the RPY values are fairly close to ground-truth after applying trajectory alignment, but it doesn't happen with the Kimera-Multi output.

Hi!
Yes, you will have to account for the camera transform. The gt is built via lidar, and Kimera-Multi is using the camera. The transform is indeed about 90 degrees

[TakShimoda]

Hi @yunzc thanks, I did the SE(3) transform then rotated 90 degrees ccw about the Z-axis and it mostly aligned the angles correctly.

I still am encountering the issues with kimera-VIO on the MH dataset

[zhixun25]

I have the same problem. Is there any solution? Or can you provide a readme.md about how to run it on Euroc? @yunzc

The picture is the trajectory results I ran on MH_01-MH_05. I don't think it's running correctly.

Thanks a lot anyway