【opencv-python】カメラキャリブレーション(箱の描画)

1. 記事の目的
opencv-pythonのチュートリアル(http://labs.eecs.tottori-u.ac.jp/sd/Member/oyamada/OpenCV/html/py_tutorials/py_calib3d/py_pose/py_pose.html#pose-estimation)で、キャリブレーションのパラメーターがnumpy形式で保存してから読み込むと言う手順で実施されており、実行しにくいと感じたのでキャリブレーションと箱の描画を一挙に行うコードを提供することにした。

2. 動作確認済み環境
MacOS Catalina 10.15.7
Python 3.8.5
opencv-python 4.4.0.46
numpy 1.19.1

3. 実装コード

import numpy as np
import cv2

def draw(img, corners, imgpts):
    imgpts = np.int32(imgpts).reshape(-1,2)

    img = cv2.drawContours(img, [imgpts[:4]],-1,(0,255,0),-3)

    for i,j in zip(range(4),range(4,8)):
        img = cv2.line(img, tuple(imgpts[i]), tuple(imgpts[j]),(255),3)

    img = cv2.drawContours(img, [imgpts[4:]],-1,(0,0,255),3)

    return img

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

    objp = np.zeros((6*7,3), np.float32)
    objp[:,:2] = np.mgrid[0:7,0:6].T.reshape(-1,2)

    axis = np.float32([[0,0,0],[0,3,0],[3,3,0],[3,0,0],[0,0,-3],[0,3,-3],[3,3,-3],[3,0,-3]])

    objpoints = []
    imgpoints = []

    img = cv2.imread('./left02.jpg')
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

    ret, corners = cv2.findChessboardCorners(gray, (7,6), None)

    if ret == True:
        objpoints.append(objp)
        corner2 = cv2.cornerSubPix(gray, corners, (11,11), (-1,-1), criteria)
        imgpoints.append(corner2)

    ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)

    h, w = img.shape[:2]
    newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w,h), 1, (w,h))

    dst = cv2.undistort(img, mtx, dist, None, newcameramtx)

    x,y,w,h = roi
    dst = dst[y:y+h, x:x+w]
    cv2.imwrite('../images/calibresult.png', dst)
    corners2 = cv2.cornerSubPix(gray, corners, (11,11),(-1,-1),criteria)

    imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist)

    img = draw(img, corners, imgpts)
    cv2.imwrite('./box_drow.png', img)

if __name__ == "__main__":
    main()

4. 実行方法
上記コードを記載したpythonファイルと同じ場所に"left02.jpg"と言う名前をつけた次の画像を保存して、実行する。