search

MISTLab / Swarm-SLAM

Sparse Decentralized Collaborative Simultaneous Localization and Mapping Framework for Multi-Robot Systems
https://lajoiepy.github.io/cslam_documentation/html/index.html
MIT License
391 stars 40 forks source link

How does the estimated pose correspond to the GT pose #14

Closed TwiceMao closed 1 year ago

TwiceMao commented 1 year ago

I want to evaluate the effect of Swarm-SLAM by calculating the effect of Swarm-SLAM estimated poses. But I have a problem now, I can't find the correspondence between the estimated pose generated by Swarm-SLAM and the Ground Truth. Or, is there any other way to directly calculate the ATE of the trajectory?

log_folder in Swarm-SLAM There are optimized global pose graph.g2o and initial_global_pose_graph.g2o in ./results. Below is a screenshot of one of the files. The first column should be the ID of the current frame in g2o. image

And I want to confirm, is the timestamp used by Swarm-SLAM the time when the ros2 bag was recorded? Or is it when the code is running?

lajoiepy commented 1 year ago

The first column are the GTSAM keys corresponding to each pose computed with this class: https://github.com/devbharat/gtsam/blob/5f15d264ed639cb0add335e2b089086141127fff/gtsam/inference/LabeledSymbol.cpp#L62 As defined here the second and third params correspond to the robot ID and pose ID respectively.

To obtain the ground truth, you need to feed the gps ground truth by enabling the logging: https://github.com/lajoiepy/cslam_experiments/blob/9f72df6549478e55643e5c1ae566675d14faee13/config/graco_lidar.yaml#L37 and publishing it on the specified topic.

Then, I suggest using https://github.com/MichaelGrupp/evo to compute the error metrics of your choice.