def estimate_pose(K, pts_2d, pts_3d): if len(pts_2d) < 3: return [(np.full((3, 3), np.nan), np.full(3, np.nan))] return pnp(pts_2d, pts_3d, K)
def estimate_pose(pts_2d, pts_3d, K): return pnp(pts_2d, pts_3d, K)
np.random.seed(0) np.random.seed(42) # instantiate a couple of points centered around the origin pts = 0.6 * (np.random.random((6, 3)) - 0.5) # Made up projective matrix K = np.array([[160, 0, 320], [0, 120, 240], [0, 0, 1]]) # A pose R_gt = np.array([ [-0.48048015, 0.1391384, -0.86589799], [-0.0333282, -0.98951829, -0.14050899], [-0.8763721, -0.03865296, 0.48008113], ]) t_gt = np.array([-0.10266772, 0.25450789, 1.70391109]) # Project points to 2D pts_2d = (pts @ R_gt.T + t_gt) @ K.T pts_2d = (pts_2d / pts_2d[:, -1, None])[:, :-1] # Compute pose candidates. the problem is not minimal so only one # will be provided poses = pnp(pts_2d=pts_2d, pts_3d=pts, K=K) R, t = poses[0] print("Nr of possible poses:", len(poses)) print("R (ground truth):", R_gt, "R (estimate):", R, sep="\n") print("t (ground truth):", t_gt) print("t (estimate):", t)