Camera Pose Estimation With OpenCV and Python

edit

I'm trying to use OpenCV with python to track camera pose in a video stream. I have a sample of code that determines the pose between two images as a test environment.

The overall flow here is this:

Read in the images and convert to gray/resize. Extract features with cv2 goodfeaturestotrack from both images. Use cv2 calcOpticalFlowPyrLK to find matching points. Convert the p1 points (starting image) to (x,y,z) with z for all points set as 0. Resolve cv2 PnPRansac to get the rotation and translation vectors. Convert angles from radians to degrees.

>  def function(mtx,dist):
> 
>     
>     #feature dictionary
>     feature_params = dict( maxCorners = 100,
>                        qualityLevel = 0.3,
>                        minDistance = 7,
>                        blockSize = 7 )
>     lk_params = dict( winSize  = (15,15),
>                   maxLevel = 2,
>                   criteria = (cv2.TERM_CRITERIA_EPS |
> cv2.TERM_CRITERIA_COUNT, 10, 0.03))
>     
>     
>     #image 1
>     image_1=cv2.imread("/Users/johnmcconnell/Desktop/Test_images/Test_image.jpg")
>     image_1=cv2.resize(image_1,(640,480))
>     gray_1=cv2.cvtColor(image_1,cv2.COLOR_BGR2GRAY)
>     p1=cv2.goodFeaturesToTrack(gray, mask = None, **feature_params)
>     
>     #image read in
>     image_2=cv2.imread("/Users/johnmcconnell/Desktop/Test_images/Test_image.jpg")
>     image_2=cv2.resize(image_2,(640,480))
>     gray_2 = cv2.cvtColor(image_2,cv2.COLOR_BGR2GRAY)
>     p2, st, err = cv2.calcOpticalFlowPyrLK(gray_1,
> gray_2, p1, None, **lk_params)
>     
>     #convert the old points to 3d
>     zeros=np.zeros([len(p1),1],dtype=np.float)
>     old_3d=np.dstack((p1, zeros))
>     
>     #get rotational and translation vector
>     blank,rvecs, tvecs, inliers = cv2.solvePnPRansac(old_3d, p2, mtx,
> dist)
>     
>     rad=180/math.pi
>     
>     roll=rvecs[0]*rad
>     pitch=rvecs[1]*rad
>     yaw=rvecs[2]*rad
>     
>     print(roll)
>     print(pitch)
>     print(yaw)
>     print(tvecs)
>      function(mtx,dist)

Given the fact that I am using exactly the same image to run this sample calculation I was expecting rotation and translation vectors to be very close to zero. However they are quite high, take a look at the sample output below. Additionally with different images with a known translation the vectors are very wrong.

The question at hand is my method sound? Have I approached this problem right? Have I matched the points correctly? Is this level of noise normal or is there something I can do about it?