import pylab as pl
import cv2 as cv

pattern_size = (9, 6)
square_size = 25.0
pattern_world = pl.zeros( (pl.prod(pattern_size), 3), pl.float32 )
pattern_world[:,:2] = pl.indices(pattern_size).T.reshape(-1, 2)
pattern_world *= square_size

camera_matrix= pl.float64([[ 611.47746477,   0.        , 432.3113038 ],
                           [   0.        , 613.15654266, 208.46520386],
                           [   0.        ,   0.        ,   1.        ]])

dist_coef= pl.float64([-2.57493677e-01, 1.44375144e-01, -1.70059153e-04 ,  4.34604104e-03 , 3.24227079e-02])

cv.namedWindow('video', 1)

R =pl.float64([[0,1,0],[1,0,0],[0,0,1]])
t =pl.float64([-200, 100, 1000])

cap = cv.VideoCapture(0)
ret, img = cap.read()
w,h = img.shape[:2]

found, corners_img = cv.findChessboardCorners(img, pattern_size)
cv.drawChessboardCorners(img, pattern_size, corners_img, found)
if found:
  ret, rvec, tvec= cv.solvePnP(pattern_world, corners_img, camera_matrix, dist_coef) 
  R, ret = cv.Rodrigues(rvec)
  t = tvec
pattern_img, jacob = cv.projectPoints(pattern_world, R, t, camera_matrix, dist_coef)

while True:
  ret, img = cap.read()
  found, corners_img = cv.findChessboardCorners(img, pattern_size)
  cv.drawChessboardCorners(img, pattern_size, corners_img, found)
  if found:
    homograf, mask = cv.findHomography(corners_img, pattern_img)
    img = cv.warpPerspective( img, homograf, (h,w)) 
    cv.imshow('video', img)
  key = cv.waitKey(1)
  if key == 1048603:
    break

cv.destroyAllWindows()



