calibration for richo theta 's movie

calib.py

"""
引数に与えられた動画ファイルからmeshroomに設定すべきキャリブレーションパラメータを算出
キャリブレーションボードは以下を印刷するなり、画面に表示するなりで。
http://opencv.jp/sample/pics/chesspattern_7x10.pdf
印刷サイズは気にしなくてよい。
"""
import cv2
# assert cv2.__version__[0] == '3', 'The fisheye module requires opencv version >= 3.0.0'
import numpy as np
import sys


#設定
CHECKERBOARD = (10,7)
CHECK_LEN=24 #意味はないけど現実とあってるほうがカッコイイ気がして
calibration_flags = cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC+cv2.fisheye.CALIB_FIX_SKEW#+cv2.fisheye.CALIB_CHECK_COND
subpix_criteria = (cv2.TERM_CRITERIA_EPS+cv2.TERM_CRITERIA_MAX_ITER, 30, 0.1)
FRAME_SKIP=10# キャリブレーションに使うフレーム（frame_skip毎に1枚つかう)
file=sys.argv[1]


#チェックボードの座標
objp = np.zeros((1, CHECKERBOARD[0]*CHECKERBOARD[1], 3), np.float32)
objp[0,:,:2] = np.mgrid[0:CHECKERBOARD[0], 0:CHECKERBOARD[1]].T.reshape(-1, 2)
objp*=CHECK_LEN



imgpoints = [[],[]] # 2d points in image plane.
gray_shape=None
def get_corners(img):
    global gray_shape
    gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
    ret, corners = cv2.findChessboardCorners(gray, CHECKERBOARD, cv2.CALIB_CB_ADAPTIVE_THRESH+cv2.CALIB_CB_FAST_CHECK+cv2.CALIB_CB_NORMALIZE_IMAGE)
    if ret == True:
        cv2.cornerSubPix(gray,corners,(3,3),(-1,-1),subpix_criteria)
        gray_shape=gray.shape
        return corners
    else:
      return None
    
count=0
cap_file = cv2.VideoCapture(file)
while True:
    ret,img=cap_file.read()
    if not ret: break
    count += 1
    if count %FRAME_SKIP != 0: continue
    simg=np.array_split(img, 2, axis=1)
    found=[True,True]
    c=get_corners(simg[0])
    if(c is not None):imgpoints[0].append(c)
    else: found[0]=False
    c=get_corners(simg[1])
    if(c is not None):imgpoints[1].append(c)
    else: found[1]=False
    print("Frame:{} Found: {}/{}".format(count,found[0],found[1]))
 
print('img end')

def do_cal(imgpoints):
    objpoints=(objp,)*len(imgpoints)
    N_OK = len(objpoints)
    K = np.zeros((3, 3))
    D = np.zeros((4, 1))
    rvecs = [np.zeros((1, 1, 3), dtype=np.float64) for i in range(N_OK)]
    tvecs = [np.zeros((1, 1, 3), dtype=np.float64) for i in range(N_OK)]
    rms, _, _, _, _ = \
        cv2.fisheye.calibrate(
            objpoints,
            imgpoints,
            gray_shape[::-1],
            K,
            D,
            rvecs,
            tvecs,
            calibration_flags,
            (cv2.TERM_CRITERIA_EPS+cv2.TERM_CRITERIA_MAX_ITER, 30, 1e-6)
        )
    print("Found " + str(N_OK) + " valid images for calibration")
    print("DIM=" + str(gray_shape[::-1]))
    print("K=np.array(" + str(K.tolist()) + ")")
    print("D=np.array(" + str(D.tolist()) + ")")
    print("==========MESHROOM PARAM(camera init)=========")
    print("Forcal:     ",np.mean(K[:2,:2])*2)
    print("PrincipalX: ",K[0,2])
    print("PrincipalY: ",K[1,2])
    print("Distortion:")
    print("    ",D[0,0])
    print("    ",D[1,0])
    print("    ",D[2,0])
    print("    ",D[3,0])
    print("==============================================")
    
print("Camera0:")
do_cal(imgpoints[0])
print("Camera1:")
do_cal(imgpoints[1])