如何估算OpenCV中两台摄像机的位置？

Question

我有两组图像对应的点。我们估计编码的相机间的转换本质矩阵：

E, mask = cv2.findEssentialMat(points1, points2, 1.0) 

，我已经提取的旋转和平移组件：

points, R, t, mask = cv2.recoverPose(E, points1, points2) 

但实际上，我怎么拿到相机的矩阵两台摄像机，所以我可以用cv2.triangulatePoints来生成一个小点云？

Answer 1

这里是我做过什么：

输入：

pts_l - 2d points in left image. nx3 numpy float array 
pts_r - 2d points in left image. nx3 numpy float array 

K_l - Right Camera matrix. 3x3 numpy float array 
K_r - Right Camera matrix. 3x3 numpy float array 

代码：

# Normalize for Esential Matrix calaculation 
pts_l_norm = cv2.undistortPoints(np.expand_dims(pts_l, axis=1), cameraMatrix=K_l, distCoeffs=None) 
pts_r_norm = cv2.undistortPoints(np.expand_dims(pts_r, axis=1), cameraMatrix=K_r, distCoeffs=None) 

E, mask = cv2.findEssentialMat(pts_l_norm, pts_r_norm, focal=1.0, pp=(0., 0.), method=cv2.RANSAC, prob=0.999, threshold=3.0) 
points, R, t, mask = cv2.recoverPose(E, pts_l_norm, pts_r_norm) 

M_r = np.hstack((R, t)) 
M_l = np.hstack((np.eye(3, 3), np.zeros((3, 1)))) 

P_l = np.dot(K_l, M_l) 
P_r = np.dot(K_r, M_r) 
point_4d_hom = cv2.triangulatePoints(P_l, P_r, np.expand_dims(pts_l, axis=1), np.expand_dims(pts_r, axis=1)) 
point_4d = point_4d_hom/np.tile(point_4d_hom[-1, :], (4, 1)) 
point_3d = point_4d[:3, :].T 

输出：

point_3d - nx3 numpy array