Open TakShimoda opened 1 year ago
Hi, Looks like Kimera-VIO is not working properly.
To answer your two points:
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.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?
Hello,
Thank you for your quick response.
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.
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:
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.
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.
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.
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: $Ei = 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}(Ei) |$ With respect to full transformation: $RPE{i,j} = |E{i,j} - I{4 \times 4} |_F$
$ATEi = |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.
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
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
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
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:
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
kimera_vio_jackal.launch
tokimera_vio_jackal_MH.launch
in the vio window, andmit_rosbag.launch
toMH_rosbag.launch
in the data window which I'll explain the changes in 2. and 3.<arg name="params_folder" value="$(find kimera_vio)/params/Euroc" />
<include file="$(find kimera_vio_ros)/launch/kimera_vio_ros_euroc.launch" pass_all_args="true"/>
kimera_vios_ros.launch
./$(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
)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 theExternalOdometryParams.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:
use_external_odom
parameter. It's set to false in thekimera_vio_ros.launch
andkimera_vio_ros_euroc.launch
files, but is set to true inkimera_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.Any help would be appreciated. Thank you.