Question about the pose visualization

[Archer-204]

Hello,

Thanks a lot. You did excellent work! I encountered issues when trying to visualize the 6D Pose estimated.

In detail, I try to modify the visloc.py to draw the 3d boxes. As follows:

def draw_pose_on_image(image, pose, intrinsics, dist_coeffs, color, label):
    s = 0.05  
    object_points = np.array([
        [-s, -s, -s],
        [s, -s, -s],
        [s, s, -s],
        [-s, s, -s],
        [-s, -s, s],
        [s, -s, s],
        [s, s, s],
        [-s, s, s]
    ], dtype=np.float32)

    R = pose[:3, :3]
    t = pose[:3, 3]

    R_inv = R.T
    t_inv = -R_inv @ t
    rvec, _ = cv2.Rodrigues(R_inv)
    tvec = t_inv.reshape(3, 1)

    intrinsics = np.array(intrinsics, dtype=np.float64)
    rvec = np.array(rvec, dtype=np.float64)
    tvec = np.array(tvec, dtype=np.float64)
    object_points = np.array(object_points, dtype=np.float32)
    dist_coeffs = np.array(dist_coeffs, dtype=np.float64)

    image_points, _ = cv2.projectPoints(
        object_points, rvec, tvec, intrinsics, distCoeffs=dist_coeffs
    )
    image_points = np.int32(image_points).reshape(-1, 2)

    edges = [
        [0, 1], [1, 2], [2, 3], [3, 0],
        [4, 5], [5, 6], [6, 7], [7, 4],
        [0, 4], [1, 5], [2, 6], [3, 7]
    ]

    for edge in edges:
        pt1 = tuple(image_points[edge[0]])
        pt2 = tuple(image_points[edge[1]])
        cv2.line(image, pt1, pt2, color, 2)

    cv2.putText(image, label, tuple(image_points[0]), cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 2)

Then in the main function I call it as:

# Main script
# ...
if not success:
    abs_transl_error = float('inf')
    abs_angular_error = float('inf')
else:
    abs_transl_error, abs_angular_error = get_pose_error(pr_querycam_to_world, query_view['cam_to_world'])
    image = np.array(query_view['rgb'])
    intrinsics = query_view['intrinsics']
    dist_coeffs = query_view.get('distortion', None)
    
    if dist_coeffs is None:
        dist_coeffs = np.zeros((5,), dtype=np.float64) 
    else:
        dist_coeffs = np.array(dist_coeffs, dtype=np.float64)
        if dist_coeffs.shape[0] < 5:
            dist_coeffs = np.pad(dist_coeffs, (0, 5 - dist_coeffs.shape[0]), 'constant')

    draw_pose_on_image(image, query_view['cam_to_world'], intrinsics, dist_coeffs, color=(0, 255, 0), label='Ground Truth')  
    draw_pose_on_image(image, pr_querycam_to_world, intrinsics, dist_coeffs, color=(0, 0, 255), label='Predicted')

But I got the boxes with the wrong position:

(The green and red boxes are not in the correct position)
I have no idea where I am wrong. Could you please assist me with it?