gistfile1.py

import cv2
import freenect
import numpy as np
import webcam

def kinect_get_rgb():
    """ Gets an RGB frame from the Kinect."""
    return freenect.sync_get_video(0)[0]

# generate known checkerboard coordinates
checker_points = []
for y in xrange(7):
  for x in xrange(10):
    checker_points.append(np.array([ x * 32.5, y * 32.5, 0.0 ]))
checker_points = np.array(checker_points)

# lists of corresponding object and image point arrays
obj_points = []
img_points = []

img_size = (640, 480)
#pix_size = 0.0093
#img_size = (1280, 1024)
#pix_size = 0.0093 / 2.0

# resolution-independent intrinsics
intrinsic_res = np.array([[ 526.62164376 * 0.0093, 0, 305.4784918 / 640.0 ],
 [   0, 527.42819265 * 0.0093, 259.90938123 / 480.0 ],
 [   0, 0, 1        ]])

'''
intrinsic = np.copy(intrinsic_res)
intrinsic[0, 0] /= pix_size
intrinsic[0, 2] *= img_size[0]
intrinsic[1, 1] /= pix_size
intrinsic[1, 2] *= img_size[1]
print intrinsic
'''

dist = np.array([[  2.47313172e-01, -8.21363756e-01, -4.91555629e-03, -2.31305774e-04, 9.41245766e-01]])
umap1 = None

#cap = cv2.VideoCapture(1)
#webcam.init_logitech(cap)

finding = False

while True:
    rgb = kinect_get_rgb()
    #_, bgr = cap.read()
    #bgr_reg = cv2.cvtColor(rgb, cv2.COLOR_RGB2BGR)

    #bgr = cv2.resize(bgr_reg, img_size, interpolation = cv2.INTER_NEAREST)

    # find chessboard corners given board size
    found = False

    if finding:
      found, corners = cv2.findChessboardCorners(bgr, (10, 7), flags = cv2.CALIB_CB_ADAPTIVE_THRESH | cv2.CALIB_CB_NORMALIZE_IMAGE | cv2.CALIB_CB_FAST_CHECK)

    if found:
      cv2.drawChessboardCorners(bgr, (10, 7), corners, True)

      #perr, rvec, tvec = cv2.solvePnP(checker_points, corners, intrinsic, dist)
      #rmat = cv2.Rodrigues(rvec)
      #print rmat
      #print tvec

    if umap1 != None:
      undist = cv2.remap(bgr, umap1, umap2, cv2.INTER_LINEAR)
      cv2.imshow('undist', undist)

    cv2.imshow('rgb', bgr)
    key = cv2.waitKey(1)
    if key == ord('p') and found:
      # points need to be float32s or opencv complains
      obj_points.append(checker_points.astype(np.float32))
      img_points.append(np.reshape(corners, (70, 2)).astype(np.float32))
      print len(img_points)
    if key == ord('c') and len(img_points) > 1:
      print "calibrating"
      err, intrinsic, dist, rvecs, tvecs = cv2.calibrateCamera(obj_points, img_points, img_size)
      print intrinsic
      print dist

    if key == ord('u') and intrinsic != None:
      # 16SC2 == 2 channel 16-bit fixed point for efficiency
      umap1, umap2 = cv2.initUndistortRectifyMap(intrinsic, dist, None, intrinsic, img_size, cv2.CV_16SC2)

    if key == ord('s'):
      finding = not finding