chessboard.py

#Forces code to use OPENCV install in virtual environment
import sys
sys.path.remove('/usr/local/lib/python2.7/site-packages')

import numpy as np
import cv2
import glob

####################################
#FIRST CHESSBOARD (PRIMARY CAMERA TO CHESSBOARD IN SPACE)
####################################

criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((7*7,3), np.float32)
objp[:,:2] = np.mgrid[0:7,0:7].T.reshape(-1,2)
#Multiply by 50.8 because 50.8 mm is length of real chessboard
objp = 50.8 * objp


# Arrays to store object points and image points from all the images.
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.

images = glob.glob('*.jpeg')

for fname in images:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
    # Find the chess board corners
    ret, corners = cv2.findChessboardCorners(gray, (7,7),None)

    # If found, add object points, image points (after refining them)
    if ret == True:
        print "found"
        objpoints.append(objp)
        #Rotating corner points because webcam flips chessboard
        corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
        corners2 = np.flipud(corners2)
        corners3 = np.copy(corners2)
        multiple = 0
        for i in range(0,49):
            if (i%7 == 0) and (i!=0):
                multiple = multiple + 1
            corners3[i] = corners2[(6-(i-(7*multiple))) + 7*multiple]
        corners4 = np.copy(corners3)
        k = 0
        for i in range(0,7):
            for j in range(0,49,7):
                corners4[k] = corners3[i+j]
                k = k + 1

        corners5 = np.copy(corners4)
        multiple = 0
        for i in range(0,49):
            if (i%7 == 0) and (i!=0):
                multiple = multiple + 1
            corners5[i] = corners4[(6-(i-(7*multiple))) + 7*multiple]

        corners2 = corners5
        imgpoints.append(corners2)

        # Draw and display the corners
        img = cv2.drawChessboardCorners(img, (7,7), corners2,ret)
        cv2.imshow('img',img)
        cv2.waitKey()
    else:
        print "not found"
        print fname
    #Displays grayscale image
    # cv2.imshow('img',gray)
    # cv2.waitKey()

#Getting camera matrix, distortion coefficients, and rvecs, tvecs
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1],None,None)

#Displaying the axes
i = 0
for fname in images:
    img = cv2.imread(fname)
    cv2.aruco.drawAxis(img,mtx,dist,rvecs[i],tvecs[i],20.64*5)
    cv2.imshow('img',img)
    cv2.waitKey()
    i = i +1

####################################
#SECOND CHESSBOARD (SECONDARY CAMERA TO CHESSBOARD IN SPACE)
####################################

objp = np.zeros((7*7,3), np.float32)
objp[:,:2] = np.mgrid[0:7,0:7].T.reshape(-1,2)
#Multiply by 50.8 because 50.8 mm is length of real chessboard
objp = 50.8 * objp

# Arrays to store object points and image points from all the images.
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.

secondary_images = glob.glob("/Users/XXX/code/facregtests/screen_loc/secondary_camera/*.jpeg")
for fname in secondary_images:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
    # Find the chess board corners
    ret, corners = cv2.findChessboardCorners(gray, (7,7),None)

    # If found, add object points, image points (after refining them)
    if ret == True:
        print "found"
        objpoints.append(objp)
        corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
        imgpoints.append(corners2)

        # Draw and display the corners
        img = cv2.drawChessboardCorners(img, (7,7), corners2,ret)
        # cv2.imshow('img',img)
        # cv2.waitKey()
    else:
        print "not found"
        print fname
    #Displays grayscale image
    # cv2.imshow('img',gray)
    # cv2.waitKey()

#Getting camera matrix, distortion coefficients, and rvecs, tvecs
ret2, mtx2, dist2, rvecs2, tvecs2 = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1],None,None)


####################################
#THIRD CHESSBOARD (SECONDARY CAMERA TO CHESSBOARD ON SCREEN)
####################################
objp = np.zeros((7*7,3), np.float32)
objp[:,:2] = np.mgrid[0:7,0:7].T.reshape(-1,2)
#Multiply by 20.64 because 20.64 mm is length of real chessboard
objp = 20.64 * objp

# Arrays to store object points and image points from all the images.
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.
three_images = glob.glob("/Users/XXX/code/facregtests/screen_loc/secondary_camera/cropped/*.jpeg")
for fname in three_images:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
    # Find the chess board corners
    ret, corners = cv2.findChessboardCorners(gray, (7,7),None)
    # If found, add object points, image points (after refining them)
    if ret == True:
        print "found"
        objpoints.append(objp)
        corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
        #Rotating corners to be consistent with other chessboard and cameras
        corners2 = np.flipud(corners2)
        corners3 = np.copy(corners2)
        multiple = 0
        for i in range(0,49):
            if (i%7 == 0) and (i!=0):
                multiple = multiple + 1
            corners3[i] = corners2[(6-(i-(7*multiple))) + 7*multiple]
        corners4 = np.copy(corners3)
        k = 0
        for i in range(0,7):
            for j in range(0,49,7):
                corners4[k] = corners3[i+j]
                k = k + 1
        corners2 = corners4
        imgpoints.append(corners2)
        # Draw and display the corners
        img = cv2.drawChessboardCorners(img, (7,7), corners2,ret)
        # cv2.imshow('img',img)
        # cv2.waitKey()
    else:
        print "not found"
        print fname
    #Displays grayscale image
    # cv2.imshow('img',gray)
    # cv2.waitKey()

#Getting camera matrix, distortion coefficients, and rvecs, tvecs
ret3, mtx3, dist3, rvecs3, tvecs3 = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1],None,None)



#Averaging the rvecs and tvecs for the primary camera
rvec_primary = rvecs[0]
tvec_primary = tvecs[0]
i = 0
for rvec in rvecs:
    if not i == 0:
        rvec_primary  = rvec_primary + rvec
    else:
        i = 1
j = 0
for tvec in tvecs:
    if not j == 0:
        tvec_primary = tvec_primary + tvec
    else:
        j = 1

rvec_primary = rvec_primary/len(rvecs)
tvec_primary = tvec_primary/len(tvecs)



#Applyihg first transform with (0,0,0) for top left corner
rot_primary,_ = cv2.Rodrigues(rvec_primary)
corner1 = np.zeros((1,3),np.float32)
first_transform_output = rot_primary.dot(corner1.T) + tvec_primary

#Applying second transform -- for now just testing with first rvec, tvec pair
#TODO: use all rvec, tvec pairs and average end results
rvec_secondary = rvecs2[0]
tvec_secondary = tvecs2[0]
rot_secondary,_ = cv2.Rodrigues(rvec_secondary)
#Inverting transform
rot_secondary = rot_secondary.T
tvec_secondary = (-1*rot_secondary).dot(tvec_secondary)
second_transform_output = rot_secondary.dot(first_transform_output) + tvec_secondary

#Applying Third transform -- for now just testing with first rvec, tvec pair
#TODO: use all rvec, tvec pairs and average end results
rvec_three = rvecs3[0]
tvec_three = tvecs3[0]
rot_three,_ = cv2.Rodrigues(rvec_three)
third_transform_output = rot_three.dot(second_transform_output) + tvec_three

#Printing output -- should be close to (-165,5,0)
print third_transform_output



cv2.destroyAllWindows()