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

#Counter to track which set image we are currently on
#Needed as some images need to be rotated such as to maintain consistency
a = 0
for fname in images:
    img = cv2.imread(fname)
    #Need to flip image as primary camera mirrors images over y axis
    img = cv2.flip(img,1)
    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)
        #Need to rotate corner points for some images by 90 degrees counterclockwise
        if (a == 0 or a == 3 or a == 4 or a == 6 or a ==7):
            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 = 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
    a = a + 1
    #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)
    img = cv2.flip(img,1)
    cv2.aruco.drawAxis(img,mtx,dist,rvecs[i],tvecs[i],50.8)
    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)



#Only using last 3 images for rvecs, tvecs as rest were just for camera matrix, distortion
rvecs = rvecs[-3:]
tvecs = tvecs[-3:]

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

#Variable for all outputs for averaging later
outputs = []

#Averaging outputs of all rvec, tvec transforms
for i in range(0, len(rvecs2)):
    #Applyihg first transform with (0,0,0) for top left corner
    rvec_three = rvecs3[i]
    tvec_three = tvecs3[i]
    rot_three,_ = cv2.Rodrigues(rvec_three)
    #Inverting transform
    rot_three = rot_three.T
    tvec_three = (-1*rot_three).dot(tvec_three)
    corner1 = np.zeros((1,3),np.float32)
    first_transform_output = rot_three.dot(corner1.T) + tvec_three

    #Applying second transform
    rvec_secondary = rvecs2[i]
    tvec_secondary = tvecs2[i]
    rot_secondary,_ = cv2.Rodrigues(rvec_secondary)
    second_transform_output = rot_secondary.dot(first_transform_output) + tvec_secondary

    #Applying Third transform
    rot_primary,_ = cv2.Rodrigues(rvec_primary)
    #Inverting transform
    rot_primary = rot_primary.T
    tvec_primary = (-1*rot_primary).dot(tvec_primary)
    third_transform_output = rot_primary.dot(second_transform_output) + tvec_primary

    #Appending output to list
    outputs.append(third_transform_output)

    #Printing output for specific rvec, tvec pair -- should be close to (-165,5,0)
    print third_transform_output

#Averaging final output:
final_output = 0
for output in outputs:
    final_output = final_output + output
final_output = final_output/len(outputs)

print "Final output \n"
print final_output


cv2.destroyAllWindows()