python - How do I estimate positions of two cameras in OpenCV?

4
votes

I have two sets of corresponding points from two images. I have estimated the Essential matrix which encodes the transformation between the cameras:

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

I've then extracted the rotation and translation components:

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

But how do I actually get the cameras matrices of the two cameras so I can use cv2.triangulatePoints to generate a little point cloud?

1
pythonopencvstructure-from-motion
What exactly have you tried? – barny
@barny The only thing I can think of it that you can't get the positions of the camera so you have to assume that one camera is a 3x4 zero matrix and the other is R | t – nickponline
The title of the question and accepted answer do not match. Probably the title should be changed to how to estimate camera pose from two views. Estimating camera position is at least one step more where one camera is assumed to be at origin and relative position of other is estimated. – abggcv

1 Answers

10
votes

Here is what I did:

Input: 

pts_l - set of n 2d points in left image. nx2 numpy float array
pts_r - set of n 2d points in right image. nx2 numpy float array

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

Code:

# 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

Output:

point_3d - nx3 numpy array