Compare the poses using EndoSLAM dataset

[ddmanh99]

Hi, I'm currently running your code using the EndoSLAM dataset (LowCam/ColonIV/Tumorfree_Trajectory_1).
I'm trying to compare the estimated camera trajectory from BodySLAM with the ground truth.
From the code, i obtained the global pose (as shown in Fig. 1) and saved it to a .txt file. The system outputs a 3x4 transformation matrix, so i extracted the position as x = array[3], y = array[7], z = array[11], which i plotted in Fig. 2.
Then, i extracted the ground truth position (trans_x, trans_y, trans_z) from 'low_high_pose_colon_test_1_low_images.xlsx' and plotted it in Fig. 3.
However, the trajectories in Fig. 2 and Fig. 3 dont seem to align.
Could you please clarify whether I'm interpreting the pose matrices correctly or explain how the comparison between estimated and ground truth poses was performed in your paper?

[GuidoManni]

Hi! Thank you for your detailed question and for showing your work with the figures.

You're on the right track, but there are a few important points to address:

Transformation Matrix Interpretation

First, your extraction of the position from the 3x4 transformation matrix looks correct. For a standard transformation matrix in the form:

[R11 R12 R13 tx]
[R21 R22 R23 ty]
[R31 R32 R33 tz]

The translation components are indeed tx = array[3], ty = array[7], tz = array[11].

The Main Issues: Scale Ambiguity AND Coordinate Frame Misalignment

Looking at your plots, I can see two distinct problems:

1. Different Directions/Orientations

The trajectories are pointing in completely different directions. This is NOT due to scale ambiguity alone—this is a coordinate frame misalignment issue. BodySLAM initializes its coordinate frame based on the first few frames, which is completely independent of the ground truth coordinate system (defined by the robot/phantom). This is normal and expected for any SLAM system.

2. Scale Ambiguity

Due to the monocular nature of the endoscopic camera, the estimated trajectory is only accurate up to an unknown scale factor. This means even if the trajectories were oriented correctly, the estimated trajectory might be larger or smaller than the ground truth.

How to Properly Compare Trajectories

You cannot directly compare the raw poses. You need to align the trajectories using a Similarity transformation (Sim(3)), which accounts for:

• Translation (3-DoF) - aligns the origins
• Rotation (3-DoF) - aligns the orientations (this will fix your direction mismatch)
• Scale (1-DoF) - corrects the monocular scale ambiguity

This is standard procedure in SLAM evaluation—every paper that compares monocular SLAM to ground truth performs this alignment.

Recommended Approach: Use the evo Library

I strongly recommend using the evo Python library, which handles all these alignments automatically.

Installation:

pip install evo

Resources:

• Alignment and transformation guide: https://github.com/MichaelGrupp/evo/wiki/evo_traj#alignment-and-transformation
• How alignment is performed (technical details): How does scale alignment work? MichaelGrupp/evo#77

[ddmanh99]

Thank you for your recommendation. I am currently using 'evo' to align the trajectories.
I would like to ask how you selected the timestamps.

In my implementation, I extract the timestamps as illustrated in Fig. 1, and I use the ‘Pose_Index’ from the 'EndoSLAM' dataset as the timestamp for the ground truth (Fig. 2).
Then, I normalize both timestamps to start from zero and run 'evo_ape' and 'evo_traj', with the results shown in Fig. 3.