Tutorial code for camera pose estimation doesnt work

asked 2015-06-29 05:09:10 -0600

I copied some code for camera calibration and pose estimation but the axes drawn point in the wrong directions.

I used these photos for camera calibration: photos

import numpy as np
import cv2
import glob
import sys
import urllib 

def draw(img, corners, imgpts):
    corner = tuple(corners[0].ravel())
    img = cv2.line(img, corner, tuple(imgpts[0].ravel()), (255,0,0), 5)
    img = cv2.line(img, corner, tuple(imgpts[1].ravel()), (0,255,0), 5)
    img = cv2.line(img, corner, tuple(imgpts[2].ravel()), (0,0,255), 5)
    return img

criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
objp = np.zeros((6*9,3), np.float32)
objp[:,:2] = np.mgrid[0:9,0:6].T.reshape(-1,2)

objpoints = [] 
imgpoints = [] 
images = glob.glob('*.jpg')

print images
for fname in images:
   img = cv2.imread(fname)
   gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
   ret, corners = cv2.findChessboardCorners(gray, (9,6),None)
   if ret == True:
      objpoints.append(objp)
      cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
      imgpoints.append(corners)

ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1],None,None)
objp = np.zeros((6*9,3), np.float32)
objp[:,:2] = np.mgrid[0:9,0:6].T.reshape(-1,2)
axis = np.float32([[3,0,0], [0,3,0], [0,0,-3]]).reshape(-1,3)
stream = urllib.urlopen('http://10.0.0.5:8080/videofeed')
byts = ''

while True:
    byts += stream.read(1024)
    a = byts.find('\xff\xd8')
    b = byts.find('\xff\xd9')
    if a != -1 and b != -1:
        jpg = byts[a:b+2]
        btes = byts[b+2:]
        img = cv2.imdecode(np.fromstring(jpg, dtype=np.uint8),cv2.IMREAD_COLOR)
        gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
        ret, corners = cv2.findChessboardCorners(gray, (9,6),None)
        if ret == True:
            corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
            _, rvecs, tvecs, inliers = cv2.solvePnPRansac(objp, corners2, mtx, dist)
            imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist)
            img = draw(img,corners2,imgpts)
            cv2.imshow('img',img)        
        byts = btes
        if cv2.waitKey(2000) ==27:
            exit(0)
edit retag flag offensive close merge delete