Get angle between camera Z-axis and ArUco marker Z-axis

Question

I am interested in calculating the angle between the vector that defines the Z-axis of a camera with respect to the Z-axis of an ArUco marker.

For this purpose, I first detect the ArUco markers within an image:

corners, ids, rvec, tvec = aruco_utils.get_aruco_marker_info(cv2.cvtColor(img, cv2.COLOR_BGR2GRAY), marker_length, camera_matrix, dist_coeffs)

I create the transformation matrix to convert coordinates between the camera coordinate system and the marker coordinate system:

M        = np.zeros((4,4))
M[:3,:3] = cv2.Rodrigues(rvec)[0]
M[:3,3]  = tvec[0].T
M[3,3]   = 1

marker_to_camera_matrix = np.matrix(M)
camera_to_marker_matrix = np.linalg.inv(marker_to_camera_matrix)

I convert the Z camera axis to the marker coordinate system:

coord_z = np.array([0,0,1, 1])
coord_z = np.dot(camera_to_marker_matrix, coord_z)

And I finally calculate the angle between the translated vector and the Z-axis of the marker:

camera_z_proj = np.array(coord_z)[0][:3]
camera_z_proj = camera_z_proj / np.linalg.norm(camera_z_proj)
marker_z_proj = np.array([0,0,1])

dot_product = np.dot(camera_z_proj, marker_z_proj)

# Calculate the angle in radians
angle_radians = np.arccos(dot_product)

# Convert the angle from radians to degrees
angle_degrees = np.degrees(angle_radians)

However, this solution does not work as I calculate this angle for different markers that have different orientations, and the final result is a pretty similar angle.

In every case, even if I download a synthetic ArUco marker (not a real photo), this angle is always around 70º.

Any clue on what I am doing wrong?

I am expecting to find the angle between the Z-axis of the camera corodinate system and the Z-axis of the ArUco coordinate system.

Answer 1

Your issue is that you deal with points when you should be dealing with vectors. Review your code to find where that happens. Hint: anywhere you deal with 4-dimensional vectors.

---

The angle between both Z-axes would need to be calculated by creating a vector valued (0,0,1) in marker space, transforming it from the marker frame into camera space (at least applying the rotation; translation doesn't matter to vectors), and then calculating its angle against a vector (0,0,1) with a little bit of trig/linear algebra (dot product).

The rotation can be applied by first calculating a rotation matrix from the rvec using Rodrigues(), then just multiplying that matrix and the vector.

That can be simplified by just taking the third column of the rotation matrix. That is the result of the multiplication.

You can then calculate the angle using either a dot product or a cross-product. Those two vectors are of unit length already. That'll make the math simpler.

Answer 2

The main issue I had came from the camera calibration. It was done using the common chessboard, which mainly led to an incorrect estimation of the Z-axes of the ArUco markers. After calibrating the camera with a ChArUco board, this estimation improved.

Finally, I calculate the angle between a marker axis (using its rotation vector rvec) w.r.t. the analogous camera marker (image_plane_vector) as:

R = cv2.Rodrigues(rvec)[0]
pt = np.dot(R, image_plane_vector)
angle_radians, angle_degrees = angle_between_two_vectors(pt, image_plane_vector)

The angle is calculated as:

def angle_between_two_vectors(v1, v2):
    v1, v2 = np.array(v1), np.array(v2)
    if len(v1.shape) > 1: v1 = v1[0]
    if len(v2.shape) > 1: v2 = v2[0]
    v1, v2 = v1[:3], v2[:3]
    assert v1.shape == (3,) and v2.shape == (3,)
    v1 = v1 / np.linalg.norm(v1)  # Normalize the normal vector
    v2 = v2 / np.linalg.norm(v2)  # Normalize the normal vector
    dot_product = np.dot(v1, v2)
    angle_radians = np.arccos(dot_product)
    angle_degrees = np.degrees(angle_radians)
    return angle_radians, angle_degrees

Here are some results of these calculations with this code after the new camera calibration. Text descriptors include between brackets [ang_x, and_y, ang_z] the angles (in degrees) of each axes (X,Y,Z) from the marker w.r.t. their analogous ones from the camera. Final value corresponds to the Z-angle normalized to the range [0,90]º.

Image 1

Image 2

Image 3