slam_eval

Tools for offline evaluation of a SLAM system (GLIM) against Qualisys motion-capture ground truth, on ROS2 bags (.db3 / .mcap). It contains two independent pipelines meant to be used in sequence:

1. slam_eval_pipeline.py — trajectory accuracy (ATE/RPE) via evo.
2. fido_eval.py — evaluation of the FIDO dynamic object rejection module: checks whether lidar points belonging to the moving person are correctly classified as "dynamic" by the SLAM.

pip install rosbags evo

Versions used in this project: rosbags==0.11.2, evo==1.36.4, Python 3.12.

Both scripts read mocap_msgs/RigidBodies / mocap4r2_msgs/RigidBody messages (defined in msg/), registering them at runtime in the rosbags typestore, so the ROS2 mocap packages don't need to be installed — only the bags are needed.

Data folders (bags/, filter_bags/, results*/, fido_results/) are in .gitignore because they contain large files generated locally.

Extracts the SLAM and Qualisys trajectories in TUM format, aligns them (Umeyama), and computes:

• ATE (Absolute Trajectory Error) via evo_ape
• RPE (Relative Pose Error) via evo_rpe

The Umeyama alignment computed by evo_ape (saved in ape_result.zip) is a prerequisite for fido_eval.py.

python3 slam_eval_pipeline.py \
    --slam_bag ./filter_bags/1912_none/1912_none_0.mcap \
    --slam_topic /glim_ros/odom_corrected \
    --gt_bag /path/to/qualisys_bag/qualisys_0.db3 \
    --gt_topic /rigid_bodies \
    --rigid_body_name JO \
    --slam_ros_version jazzy \
    --gt_ros_version humble \
    --t_max_diff 0.05 \
    --output_dir ./results

If --t_offset is 0.0 and the initial timestamps of SLAM and GT differ by more than 0.5s, the script prints a warning with the suggested offset.

For each run, a subfolder <output_dir>/<slam_bag_name>[_t<N>s][_e<N>s]/ is created with:

• gt.txt, slam.txt — trajectories in TUM format
• ape_result.zip, ape_plot_map.png — ATE results (also contains alignment_transformation_sim3.npy, used by fido_eval.py)
• rpe_result.zip, rpe_plot_map.png — RPE results (unless --skip_rpe)

For each synchronized lidar frame:

1. Reads the person's head position from Qualisys.
2. Transforms it into the lidar frame using the GLIM odometry and the Umeyama alignment computed in step 1 (or, if --robot_body_name is given, using the robot's per-frame pose instead of the static alignment — more robust to drift).
3. Builds a vertical cylinder (head → feet) around the person in the lidar frame.
4. Classifies the points from the "dynamic" and "static" topics against the cylinder → TP/FP/FN/TN.
5. Computes precision, recall, F1 per frame and aggregated over the whole sequence.

Requires the output of slam_eval_pipeline.py (ape_result.zip) as input, from which it reads the Umeyama alignment matrix.

python3 fido_eval.py \
    --slam_bag ./filter_bags/1912_none/1912_none_0.mcap \
    --gt_bag /path/to/qualisys_bag/qualisys_0.db3 \
    --output_bag ./filter_bags/1912_none/1912_none_0.mcap \
    --evo_result ./results/1912_none_0/ape_result.zip \
    --rigid_body_name JO \
    --slam_ros_version jazzy \
    --gt_ros_version humble \
    --output_ros_version jazzy \
    --person_height 1.75 \
    --cylinder_radius 0.4 \
    --output_csv ./fido_results/fido_eval_1912.csv

--output_bag can be the same as --slam_bag if the dynamic/static topics, the odometry, and the ground truth are all in the same bag.

• CSV (--output_csv) with one row per frame: t, TP, FP, FN, TN, precision, recall, f1, n_dyn, n_sta, head_x/y/z_lidar.
• Aggregated summary printed to screen (total TP/FP/FN/TN, global precision/recall/F1, and per-frame mean±σ).
• (optional) --rviz_bag: .mcap bag with the cylinder marker and the dynamic/static pointclouds remapped to /fido_eval/points_dynamic and /fido_eval/points_static, useful for visually validating the alignment in RViz2.

1. Record a SLAM (GLIM) bag and a Qualisys bag for the same experiment (see [[project_experiment_setup]] for the setup: one person walking around the room for the whole duration of the bag).
2. Run slam_eval_pipeline.py to get ATE/RPE and the Umeyama alignment matrix (ape_result.zip).
3. Run fido_eval.py, passing the ape_result.zip from step 2, to evaluate the dynamic/static classification on the person.
4. (Optional) Generate the --rviz_bag and inspect it in RViz2 to visually verify that the cylinder correctly follows the person.

• ROS2 topics used in this project: Qualisys → /rigid_bodies (RigidBodies), SLAM → /glim_ros/odom_corrected (Odometry).
• If the SLAM and Qualisys bags come from different ROS2 versions (e.g. Jazzy vs Humble), set --slam_ros_version/--gt_ros_version (and --output_ros_version for fido_eval.py) correctly: CDR typestores differ between versions.
• If the script reports many frames skipped due to failed synchronization (skipped_odom/skipped_gt), increase --t_max_diff or check that the bags were recorded in the same time window (see the time-offset warning).