OpenCV Python calibration code

[code lang='python']

-- coding: utf-8 --

import os import numpy as np import glob

import cv2

def generate_points_3d(rows, cols, square_size): """ prepare object points as ( 0, 0, 0), (1square_size, 0, 0), (2square_size, 0, 0), ... *1 img = cv2.line(img, corner, tuple(imgpts[0].ravel()), (255, 0, 0), 5) img = cv2.line(img, corner, tuple(imgpts[1].ravel()), (0, 255, 0), 5) img = cv2.line(img, corner, tuple(imgpts[2].ravel()), (0, 0, 255), 5) return img

def DetectCorners(img, cols, rows, criteria): flag, points_2d = cv2.findChessboardCorners(img, (cols, rows), None) if flag is True: points_2d = cv2.cornerSubPix(img, points_2d, (11, 11), (-1, -1), criteria)

return flag, points_2d

def SaveCorners(img, points, rows, cols, filename): imgDraw = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR) imgDraw = cv2.drawChessboardCorners(imgDraw, (rows, cols), points, True) cv2.imwrite(filename, imgDraw)

def InitializeIntrinsic(points_3d, points_2d, image_size): K = cv2.initCameraMatrix2D(points_3d, points_2d, image_size) return K

def InitializeDistortion(): delta = np.zeros(5) return delta

def InitializeExtrinsic(points_3d, points_2d, K, delta): rvecs = ts = for points_2d in points_2d: inliers, rvec, t = cv2.solvePnP(points_3d, points_2d, K, delta) rvecs.append(rvec) ts.append(t) return rvecs, ts

def LoadPattern(filename): fs = cv2.FileStorage(filename, cv2.FILE_STORAGE_READ)

# calibration pattern
board_width = int(fs.getNode('board_width').real())
board_height = int(fs.getNode('board_height').real())
square_size = int(fs.getNode('square_size').real())

fs.release()

return board_width, board_height, square_size

def SaveResults(filename, name_images, flags_detection, image_width, image_height, board_width, board_height, square_size, points_3d, points_2d, K, delta, rvecs, ts, rmse): fs = cv2.FileStorage(filename, cv2.FILE_STORAGE_WRITE)

# calibration pattern
fs.write('pattern', "{")
fs.write('board_width', board_width)
fs.write('board_height', board_height)
fs.write('square_size', square_size)
fs.write('corners_3d', points_3d[0])
fs.write('pattern', "}")

# information about images
fs.write('images', "{")
fs.write('num_images', len(rvecs))
fs.write('image_width', image_width)
fs.write('image_height', image_height)
poseid = 0
for n, flag_detection in enumerate(flags_detection):
    if flag_detection:
        fs.write('file_name', name_images[n])
        fs.write('corners', points_2d[n].reshape(-1, 2))
        fs.write('rvec', rvecs[poseid])
        fs.write('t', ts[poseid])
        poseid += 1
fs.write('images', "}")

# results
fs.write('calibration_matrix', K)
fs.write('distortion_parameters', delta)
fs.write('RMSE', rmse)

fs.release()

def LoadResults(filename): print(os.path.exists(filename)) fs = cv2.FileStorage(filename, cv2.FILE_STORAGE_READ) points_3d = points_2d = rvecs = ts =

# calibration pattern
_points_3d = fs.getNode('pattern').getNode('corners_3d').mat()

# image
num_images = fs.getNode('images').getNode('num_images').real()
image_width = fs.getNode('images').getNode('image_width').real()
image_height = fs.getNode('images').getNode('image_height').real()

for n in range(num_images):
    points_3d.append(_points_3d)
    points_2d.append(fs.getNode('images').getNode('corners')[n].mat())

# result
K = fs.getNode('calibration_matrix').mat()
delta = fs.getNode('distortion_parameters').mat()
rmse = fs.getNode('RMSE').real()
fs.release()

return name_images, (image_width, image_height), points_3d, points_2d, K, delta, rvecs, ts, rmse

def Undistortion(name_images, K, delta): # Lens undistortion for name_image in name_images: img = cv2.imread(name_image) # load as a gray-scale image img_undistort = cv2.undistort(img, K, delta) basename = os.path.splitext(os.path.basename(name_image))[0] cv2.imwrite(os.path.join(name_logdir, basename + '_undistort.jpg'), img_undistort)

def DrawPose(name_images, flags_detection, points_axis, points_2d, K, delta, rvecs, ts): # draw estimated pose poseid = 0 for n, flag_detection in enumerate(flags_detection): if flag_detection: # project 3D points to image plane points_axis_2d, _ = cv2.projectPoints(points_axis, rvecs[poseid], ts[poseid], K, delta)

        # draw result
        img = cv2.imread(name_images[n], 0)
        imgDraw = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
        imgDraw = draw(imgDraw, points_2d[poseid], points_axis_2d)
        basename = os.path.splitext(os.path.basename(name_images[n]))[0]
        cv2.imwrite(os.path.join(name_logdir, basename + '_pose.jpg'),
                    imgDraw)
        poseid += 1

def calibration(name_dir, name_logdir=None, flagStepByStep=True):

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

board_width, board_height, square_size = LoadPattern('./pattern.xml')
_points_3d = generate_points_3d(board_width, board_height, square_size)
points_axis = np.float32([[3, 0, 0],
                          [0, 3, 0],
                          [0, 0, -3]]).reshape(-1, 3)

# calibration
points_3d = []
points_2d = []

name_images = glob.glob(name_dir + '*.jpg')
image_size = None
flags_detection = []
for name_image in name_images:
    img = cv2.imread(name_image, 0)
    if image_size is None:
        image_size = (img.shape[1], img.shape[0])

    flag_detection, _points_2d = DetectCorners(img,
                                               board_height, board_width,
                                               calibration_criteria)
    flags_detection.append(flag_detection)
    if flag_detection is True:
        points_3d.append(_points_3d)
        points_2d.append(_points_2d)
        basename = os.path.splitext(os.path.basename(name_image))[0]
        savename = os.path.join(name_logdir, basename + '_corners.jpg')
        SaveCorners(img, _points_2d, board_width, board_height, savename)

if flagStepByStep:

    K = InitializeIntrinsic(points_3d, points_2d, image_size)
    delta = InitializeDistortion()
    rvecs, ts = InitializeExtrinsic(points_3d[0], points_2d, K, delta)

    rmse, K, delta, rvecs, ts = cv2.calibrateCamera(points_3d,
                                                    points_2d,
                                                    img.shape[::-1],
                                                    K,
                                                    delta,
                                                    rvecs,
                                                    ts,
                                                    flags=cv2.CALIB_USE_INTRINSIC_GUESS)
else:
    rmse, K, delta, rvecs, ts = cv2.calibrateCamera(points_3d,
                                                    points_2d,
                                                    img.shape[::-1],
                                                    None, None)

rmse /= len(points_2d)

SaveResults('./log/calibration.xml',
            name_images, flags_detection,
            image_size[0], image_size[1],
            board_width, board_height,
            square_size,
            points_3d, points_2d,
            K, delta, rvecs, ts, rmse)

print('Calibration using %d images under %s' % (len(name_images), name_dir))
print('    Used corners detected on %d images' % len(points_2d))
print('    Reprojection error: %f' % rmse)
print(K)
print(delta.transpose())

Undistortion(name_images, K, delta)
DrawPose(name_images, flags_detection,
         points_axis, points_2d,
         K, delta, rvecs, ts)

if name == 'main': name_dir = './' name_logdir = './log' flagStepByStep = True calibration(name_dir, name_logdir, True) calibration(name_dir, name_logdir, False)

name_images, image_size, points_3d, points_2d, K, delta, rvecs, ts, rmse = LoadResults(os.path.abspath('./log/calibration.xml'))

 [/code]

*1:rows-1)square_size, 0, 0), ... ( 0square_size, (cols-1)square_size, 0) ... ((rows-2)square_size, (cols-1)square_size, 0) ((rows-1)square_size, (cols-1)*square_size, 0) """ number_of_points = rows * cols points = np.zeros((number_of_points, 3), np.float32) points[:, :2] = square_size * np.mgrid[0:cols, 0:rows].T.reshape(-1, 2) return points

def draw(img, corners, imgpts): corner = tuple(corners[0].ravel(