calculate the relative rotation based on the pose matrices in visual SLAM

7 Views Asked by At

In visual SLAM, given the poses of two consecutive cameras, how to calculate the relative rotation of the latter camera with respect to the former one based on the pose matrices and obtain its Euler angles.

Assuming R1 and R2 are the rotation matrices of the two cameras

R_rel = R2 @ np.linalg.inv(R1)

Convert to Euler angles

euler_angles = rotation_matrix_to_euler_angles(R_rel)

def rotation_matrix_to_euler_angles(R):
    
    yaw = np.arctan2(R[1, 0], R[0, 0])

    pitch = np.arctan2(-R[2, 0], np.sqrt(R[2, 1]**2 + R[2, 2]**2))

    roll = np.arctan2(R[2, 1], R[2, 2])

    yaw = np.degrees(yaw)
    pitch = np.degrees(pitch)
    roll = np.degrees(roll)
    return yaw, pitch, roll

But it is always inconsistent with the actual rotation, trouble please how to calculate it? By the way, I'm using images taken with the drone camera facing the ground.

Please help me find the cause of the error.

0

There are 0 best solutions below