Plot ORB-SLAM agent position against ground truth position

[frasermcghan]

❓ Questions and Help

I am trying to compare the position of an orbslam agent with the ground truth position.

I can get the history of the orbslam agent's position with:

agent.position_history

this returns a list of (4x4) arrays which look like homogeneous transformation matrices corresponding to the 6D pose estimated by orbslam.

I can also access the (ground truth?) position and rotation of the agent at each time step with:

sim.get_agent_state().position
sim.get_agent_state().rotation

which return an (x, y, z) array and a quaternion respectively.

How can I compare these different formats of position?

I think I need to align the two coordinate systems (orbslam and habitat) but I don't know how.

Any help would be greatly appreciated.

Thanks

[Skylion007]

The Agent Position is going to be in the meshes coordinate space, you probably need to transform it first in to the robot's local space (ex: the point the robot spawns in is (0,0,0)). Then if you know the starting rotation / position of the ORB slam's position, that should let you align them.

[frasermcghan]

Thanks for your help! I have made some progress since.

I have the following code that should transform the orbslam poses into the robot's coordinate system (I think?).

def get_gt_pose(gt_position, gt_rotation):
    '''
    function to create homogeneous pose from ground truth position (x, y, z) and 
    rotation (x, y, z, w)
    '''
    rot_matrix = quaternion.as_rotation_matrix(gt_rotation)
    tf_matrix = np.hstack(
        (rot_matrix, gt_position.reshape(3, 1))
    )
    homo_tf_matrix = np.vstack(
        (tf_matrix, np.array([0, 0, 0, 1]))
    )
    return homo_tf_matrix

# get initial ground truth pose of robot
starting_gt_pose = get_gt_pose(positions_gt[0], rotations_gt[0])

tf_poses = []
# loop through orbslam poses and transform using initial ground truth pose of robot
for orb_pose in pose6d_history:
    tf_orb_pose = np.matmul(starting_gt_pose, orb_pose)
    tf_poses.append(tf_orb_pose)

# extract x, y, z coordinates from transformed poses
xs_tf = [i[0, -1] for i in tf_poses]
ys_tf = [i[1, -1] for i in tf_poses]
zs_tf = [i[2, -1] for i in tf_poses]

# ground truth x, y, z coordinates
xs_gt = [i[0] for i in positions_gt]
ys_gt = [i[1] for i in positions_gt]
zs_gt = [i[2] for i in positions_gt]

# plot trajectories
plt.plot(xs_gt, zs_gt, 'g', label="GT", linewidth=3)
plt.plot(xs_tf[1:], zs_tf[1:], 'b--', label="ORB-SLAM2", linewidth=1)
plt.legend()
plt.show()

I get the following trajectory plot:

I have run this code on different scenes and the trajectories are always mirrored - I think there is some disagreement between the axes convention of the two coordinate systems that I need to address before transforming.

Any idea what I'm doing wrong?

Thanks :+1:

[erikwijmans]

My guess would be a difference in coordinate conventions. Roboticists often use x-right, y-down, z-forward, while habitat is using the graphics convention of x-right, y-up, z-backward. I tried to see if I could find what orb-slam uses, but couldn't.

Since you are just doing x, z plotting, can you try negating the z part in orb_pose? (There is more that would need to be done to correct the rotation that I forget off the top of my head).

[mathfac]

@frasermcghan was the issue resolved? Thank you!

[rpartsey]

Closing the issue because it has not had recent activity and looks like resolved.

[bharadwajsirigadi]

My guess would be a difference in coordinate conventions. Roboticists often use x-right, y-down, z-forward, while habitat is using the graphics convention of x-right, y-up, z-backward. I tried to see if I could find what orb-slam uses, but couldn't.

Since you are just doing x, z plotting, can you try negating the z part in orb_pose? (There is more that would need to be done to correct the rotation that I forget off the top of my head).

Hello, Could you please try to let me know, how to rotate the poses generated from ORB_SLAM2 to compare it with ground truth?