Patrick-Calibrator

gistfile1.py

import numpy as np
from primesense import openni2
import numpy.ctypeslib
import numpy
import cv2

# Checkerboard
# https://github.com/obviousjim/DepthKit/blob/master/chessboard_a3.pdf?raw=true
#
# After printing, don't forget to adjust "size of the squares in mm"
# and adjust the (10, 7) in findChessboardCorners to the number of corners.

openni2.initialize("/home/niklas/tmp/openni/OpenNI2/Bin/x64-Release")

dev = openni2.Device.open_any()
# print dev.get_sensor_info()

color_stream = dev.create_color_stream()

video_mode = openni2.VideoMode()
video_mode.fps = 30
video_mode.resolutionX = 640
video_mode.resolutionY = 480
video_mode.pixelFormat = openni2.PIXEL_FORMAT_RGB888

color_stream.set_video_mode(video_mode)

color_stream.set_mirroring_enabled(False)

color_stream.start()


def kinect_get_rgb():
    """ Gets an RGB frame from the Kinect."""

    frame = color_stream.read_frame()
    frame_data = frame.get_buffer_as_uint8()

    # print frame_data

    # a = numpy.array(frame_data, dtype=numpy.uint8, copy=False).reshape((480,640,3))
    a = numpy.frombuffer(frame_data, dtype=numpy.uint8).reshape((480,640,3))

    rgb = cv2.cvtColor(a, cv2.COLOR_RGB2BGR)
    return rgb

umap1 = None
umap2 = None

# 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 ])) # size of the squares in mm
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

undistorting = False
finding = False

while True:
    bgr = kinect_get_rgb()

    # 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)

    if undistorting and 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)
      cameraMatrix = np.array(
        [ [ 535.002029, 0, 313.0 ],
          [ 0, 535.002086, 240.0 ],
          [ 0, 0, 1]
        ],
        dtype = np.float32)
      err, intrinsic, dist, rvecs, tvecs = cv2.calibrateCamera(
        obj_points,
        img_points,
        img_size,
        cameraMatrix = cameraMatrix,
        flags = cv2.CALIB_FIX_PRINCIPAL_POINT | cv2.CALIB_USE_INTRINSIC_GUESS
        )
      print "err", err
      print intrinsic
      print dist

    if key == ord('u') and intrinsic != None:
      undistorting = not undistorting
      # 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