OpenCV solvePnP and projectPoints roundtrip test

opencv_pnp_roundtrip.py

import cv2
import numpy as np

# Define 3D object points
object_points = np.array([
    [0, 0, 0],
    [1, 0, 0],
    [1, 1, 0],
    [0, 1, 0]
], dtype=np.float32)

# Define corresponding 2D image points
image_points = np.array([
    [320, 240],
    [400, 240],
    [400, 320],
    [320, 320]
], dtype=np.float32)

# Define camera intrinsic parameters
camera_matrix = np.array([
    [800, 0, 320],
    [0, 800, 240],
    [0, 0, 1]
], dtype=np.float32)

# Define distortion coefficients
dist_coeffs = np.zeros(5)

# Solve PnP to find the rotation and translation vectors
success, rvec, tvec = cv2.solvePnP(object_points, image_points, camera_matrix, dist_coeffs)

# Project the 3D points back to the image plane using the found pose
projected_points, _ = cv2.projectPoints(object_points, rvec, tvec, camera_matrix, dist_coeffs)

print("Original Image Points:\n", image_points)
print("Projected Image Points:\n", projected_points)