use cv2.solvePnP from homography result to get camera pose

Question

I use the following code to georeference images

with input

grid    = "for example a utm grid"
img_raw = cv2.imread(filename)
mtx, dist = "intrinsic camera matrix and 
             distortion coefficient from calibration matrix"
src_pts = "camera location of gcp on undistorted image"
dst_pts = "world location of gcp in the grid coordinate"

I correct camera distortion and apply homography

img = cv2.undistort(img_raw, mtx, dist, None, None)

H, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC,5.0)
img_geo = cv2.warpPerspective(img,(grid.shape[0],grid.shape[1]),\
                              flags=cv2.INTER_NEAREST,borderValue=0)

then I want to get the location of the camera. I try to use the rotation and translation matrix calculated in cv2.solvePnP as shown here for example. If I am right, I need camera and world coordinates for at least a set of 4 coplanar points.

flag, rvec, tvec = cv2.solvePnP(world, cam, mtx, dist)

Again if I am right, in solvePnP, the camera coordinates need to be from the raw image frame and not the undistorted frame as in src_pts.

So my question is, how could I get the pixels location of src_pts in the raw image frame? Or is there any other way to get rvec and tvec?

Answer 1

Maybe the function projectPoints is what you need. Here the link: http://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html#projectpoints

Answer 2

This is the solution I found

grid    = "for example a utm grid"
img_raw = cv2.imread(filename)
mtx, dist = "intrinsic camera matrix and 
         distortion coefficient from calibration matrix"
src_pts = "camera location of gcp on raw image"
dst_pts = "world location of gcp in the grid coordinate"

note src_pts are now points in the original distorted image

src_pts_undistorted = cv2.undistortPoints(src_pts, K, D, P=K)
img = cv2.undistort(img_raw, mtx, dist, None, None)

H, mask = cv2.findHomography(src_pts_undistorted, dst_pts, cv2.RANSAC,5.0)
img_geo = cv2.warpPerspective(img,(grid.shape[0],grid.shape[1]),\
                          flags=cv2.INTER_NEAREST,borderValue=0)

then I can get the pose from solvePnP

flag, rvec, tvec = cv2.solvePnP(dst_pts, src_pts, mtx, dist)