Python Opencv SolvePnP gives invalid translation vector

Question

I am trying to calibrate and find the location and rotation of one virtual camera in Blender 3d using homography. I use Blender to double check my results before moving into the real world, where it will be more difficult.

I took ten pictures of the chessboard in different places and turns in the field of view of my stationary camera. In OpenCV Python, I used cv2.calibrateCamera to find the internal matrix from the detected corners of the chessboard in ten images, and then used it in cv2.solvePnP to find the external parameters (translation and rotation).

However, although the estimated parameters were close to the actual, something suspicious is happening. My initial translation estimate was (-0.11205481,-0.0490256,8.13892491) . The actual location was (0,0,8.07105) . Pretty close, right?

But when I moved the camera a bit, rotated it and displayed the images again, the proposed translation went even further. Estimated: (-0.15933154,0.13367286,9.34058867) . In fact: (-1.7918,-1.51073,9.76597) . The value of Z is close, but X and Y are not.

I am completely baffled. If anyone can help me figure this out, I would really appreciate it. Here is the code (based on the Python2 calibration example that ships with OpenCV):

 #imports left out USAGE = ''' USAGE: calib.py [--save <filename>] [--debug <output path>] [--square_size] [<image mask>] ''' args, img_mask = getopt.getopt(sys.argv[1:], '', ['save=', 'debug=', 'square_size=']) args = dict(args) try: img_mask = img_mask[0] except: img_mask = '../cpp/0*.png' img_names = glob(img_mask) debug_dir = args.get('--debug') square_size = float(args.get('--square_size', 1.0)) pattern_size = (5, 8) pattern_points = np.zeros( (np.prod(pattern_size), 3), np.float32 ) pattern_points[:,:2] = np.indices(pattern_size).T.reshape(-1, 2) pattern_points *= square_size obj_points = [] img_points = [] h, w = 0, 0 count = 0 for fn in img_names: print 'processing %s...' % fn, img = cv2.imread(fn, 0) h, w = img.shape[:2] found, corners = cv2.findChessboardCorners(img, pattern_size) if found: if count == 0: #corners first is a list of the image points for just the first image. #This is the image I know the object points for and use in solvePnP corners_first = [] for val in corners: corners_first.append(val[0]) np_corners_first = np.asarray(corners_first,np.float64) count+=1 term = ( cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 30, 0.1 ) cv2.cornerSubPix(img, corners, (5, 5), (-1, -1), term) if debug_dir: vis = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR) cv2.drawChessboardCorners(vis, pattern_size, corners, found) path, name, ext = splitfn(fn) cv2.imwrite('%s/%s_chess.bmp' % (debug_dir, name), vis) if not found: print 'chessboard not found' continue img_points.append(corners.reshape(-1, 2)) obj_points.append(pattern_points) print 'ok' rms, camera_matrix, dist_coefs, rvecs, tvecs = cv2.calibrateCamera(obj_points, img_points, (w, h)) print "RMS:", rms print "camera matrix:\n", camera_matrix print "distortion coefficients: ", dist_coefs.ravel() cv2.destroyAllWindows() np_xyz = np.array(xyz,np.float64).T #xyz list is from file. Not shown here for brevity camera_matrix2 = np.asarray(camera_matrix,np.float64) np_dist_coefs = np.asarray(dist_coefs[:,:],np.float64) found,rvecs_new,tvecs_new = cv2.solvePnP(np_xyz, np_corners_first,camera_matrix2,np_dist_coefs) np_rodrigues = np.asarray(rvecs_new[:,:],np.float64) print np_rodrigues.shape rot_matrix = cv2.Rodrigues(np_rodrigues)[0] def rot_matrix_to_euler(R): y_rot = asin(R[2][0]) x_rot = acos(R[2][2]/cos(y_rot)) z_rot = acos(R[0][0]/cos(y_rot)) y_rot_angle = y_rot *(180/pi) x_rot_angle = x_rot *(180/pi) z_rot_angle = z_rot *(180/pi) return x_rot_angle,y_rot_angle,z_rot_angle print "Euler_rotation = ",rot_matrix_to_euler(rot_matrix) print "Translation_Matrix = ", tvecs_new 

Answer 1

I think you can think of tvecs_new as a camera position. It is a little embarrassing that this is not so! In fact, this is a position of world origin in the cells. I believe that in order to get a camera pose in an object / world, you need to do the following:

 -np.matrix(rotation_matrix).T * np.matrix(tvecs_new) 

And you can get Euler angles using cv2.decomposeProjectionMatrix(P)[-1] where P is the outer matrix [r|t] 3 by 4.

I found this to be a pretty good article on built-in functions and extrinsics ...