diff --git a/py_workspace/aruco/pose_math.py b/py_workspace/aruco/pose_math.py index a1408f7..1da5dad 100644 --- a/py_workspace/aruco/pose_math.py +++ b/py_workspace/aruco/pose_math.py @@ -32,9 +32,13 @@ def compose_transforms(T1, T2): return T1 @ T2 -def compute_reprojection_error(obj_pts, img_pts, rvec, tvec, K): - projected_pts, _ = cv2.projectPoints(obj_pts, rvec, tvec, K, None) +def compute_reprojection_error(obj_pts, img_pts, rvec, tvec, K, dist=None): + projected_pts, _ = cv2.projectPoints(obj_pts, rvec, tvec, K, dist) projected_pts = projected_pts.squeeze() img_pts = img_pts.squeeze() + if projected_pts.ndim == 1: + projected_pts = projected_pts.reshape(1, -1) + if img_pts.ndim == 1: + img_pts = img_pts.reshape(1, -1) error = np.linalg.norm(img_pts - projected_pts, axis=1) return float(np.mean(error))