Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
Patrick-Calibrator
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
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.