# Off-line camera calibration
# Author: Leow Wee Kheng
# Department of Computer Science, National University of Singapore
# Last update: 20 Sep 2012
# 
# Default file name convention:
# Folder containing calibration pattern files: mycam
# Calibration pattern images: calib-nn.jpg, nn starts from 00 in consecutive order
# Detected corner images: corner-nn.jpg, nn starts from 00 in consecutive order
# Camera intrinsic parameter file: camera-int.txt

import sys
sys.path.append("C:\OpenCV2.1\Python2.6\Lib\site-packages")
import cv

dir = "mycam/"
inimgname = "calib-"
outimgname = "corner-"
fileext = ".jpg"
nfile = 15
pts_arow = 9
pts_acol = 6
# grid_width = 1 # canonical
# grid_height = 1 # canonical
grid_width = 2.55 # cm
grid_height = 2.53 # cm
camint = dir + "camera-int.txt"

patternSize = (pts_arow, pts_acol)

# Initialization
image = list(range(nfile))
corners = list(range(nfile))
criteria = (cv.CV_TERMCRIT_ITER | cv.CV_TERMCRIT_EPS, 30, 0.01)

# Read calibration images
print "reading calibration images..."
nboard = 0
for i in range(nfile):
    filename = dir + inimgname + format(i, "02d") + fileext
    print filename,
    image[i] = cv.LoadImage(filename, cv.CV_LOAD_IMAGE_GRAYSCALE)
   
    # Find chessboard corners
    retcode, cor = cv.FindChessboardCorners(image[i], patternSize, cv.CV_CALIB_CB_ADAPTIVE_THRESH | cv.CV_CALIB_CB_FILTER_QUADS)
    if retcode == 1:
        print retcode #, nboard
        corners[nboard] = cv.FindCornerSubPix(image[i], cor, (3,3), (0,0), criteria)
        colorimg = cv.CreateMat(image[i].height, image[i].width, cv.CV_8UC3)
        cv.CvtColor(image[i], colorimg, cv.CV_GRAY2RGB)
        cv.DrawChessboardCorners(colorimg, patternSize, corners[nboard], retcode)
   
        filename = dir + outimgname + format(i, "02d") + fileext
        cv.SaveImage(filename, colorimg)
        nboard += 1
print "number of images =", nboard

# Transfer data points for calibration
ncor = pts_arow * pts_acol
npts = nboard * ncor
object_points = cv.CreateMat(npts, 3, cv.CV_32FC1)
image_points = cv.CreateMat(npts, 2, cv.CV_32FC1)
point_counts = cv.CreateMat(nboard, 1, cv.CV_32SC1)

p = 0
for i in range(nboard):
    # print i
    cv.SetReal2D(point_counts, i, 0, ncor)
    for c in range(ncor):
        cv.SetReal2D(object_points, p, 0, (c / pts_arow) * grid_height)
        cv.SetReal2D(object_points, p, 1, (c % pts_arow) * grid_width)
        cv.SetReal2D(object_points, p, 2, 0.0)
        cv.SetReal2D(image_points, p, 0, corners[i][c][0])
        cv.SetReal2D(image_points, p, 1, corners[i][c][1])
        p += 1
        #print c/pts_arow,c%pts_arow,".",

# Calibrate
camera_matrix = cv.CreateMat(3, 3, cv.CV_32FC1)
dist_coeffs = cv.CreateMat(5, 1, cv.CV_32FC1)
rvecs = cv.CreateMat(nboard, 3, cv.CV_32FC1)
tvecs = cv.CreateMat(nboard, 3, cv.CV_32FC1)

print "calibrating..."
cv.CalibrateCamera2(object_points, image_points, point_counts, (colorimg.width, colorimg.height), camera_matrix, dist_coeffs, rvecs, tvecs, 0)

# Print results
file = open(camint, "w")
print "camera matrix"
for i in range(3):
    for j in range(3):
        print camera_matrix[i, j],
        file.write(str(camera_matrix[i, j]))
        if j < 2:
		    file.write(", ")
        else:
		    file.write("\n")
    print

print "distortion coefficients"
for i in range(5):
    print dist_coeffs[i, 0],
    file.write(str(dist_coeffs[i, 0]))
    if i < 4:
	    file.write(", ")
file.close()