Realsense로 camera calibration하기(2)

아래 코드는 camera calibration을 하기위한 코드입니다.

 

#!/usr/bin/env python3
#!/usr/bin/env python2

import numpy as np
import cv2
import glob
import argparse


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


def calibrate(dirpath, prefix, image_format, square_size, width=9, height=6):
    """ Apply camera calibration operation for images in the given directory path. """
    # prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(8,6,0)
    objp = np.zeros((height*width, 3), np.float32)
    objp[:, :2] = np.mgrid[0:width, 0:height].T.reshape(-1, 2)

    objp = objp * square_size

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

    if dirpath[-1:] == '/':
        dirpath = dirpath[:-1]

    images = glob.glob(dirpath+'/' + prefix + '*.' + image_format)

    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, (width, height), None)

        # If found, add object points, image points (after refining them)
        if ret:
            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, (width, height), corners2, ret)

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

    return [ret, mtx, dist, rvecs, tvecs]

def save_coefficients(mtx, dist, path):
    """ Save the camera matrix and the distortion coefficients to given path/file. """
    cv_file = cv2.FileStorage(path, cv2.FILE_STORAGE_WRITE)
    cv_file.write("K", mtx)
    cv_file.write("D", dist)
    # note you *release* you don't close() a FileStorage object
    cv_file.release()

def load_coefficients(path):
    """ Loads camera matrix and distortion coefficients. """
    # FILE_STORAGE_READ
    cv_file = cv2.FileStorage(path, cv2.FILE_STORAGE_READ)

    # note we also have to specify the type to retrieve other wise we only get a
    # FileNode object back instead of a matrix
    camera_matrix = cv_file.getNode("K").mat()
    dist_matrix = cv_file.getNode("D").mat()

    cv_file.release()
    return [camera_matrix, dist_matrix]

if __name__ == '__main__':
    parser = argparse.ArgumentParser(description='Camera calibration')
    parser.add_argument('--image_dir', type=str, required=True, help='image directory path')  # 캘리브레이션 할 이미지의 위치
    parser.add_argument('--image_format', type=str, required=True,  help='image format, png/jpg') # 이미지의 저장형식
    parser.add_argument('--prefix', type=str, required=True, help='image prefix')  # 이미지의 이름
    parser.add_argument('--square_size', type=float, required=False, help='chessboard square size')  # 체스판의 사이즈(required가 false이면 default값이 들가거나 help라고 뜨면 써주면 됩니다. 
    parser.add_argument('--width', type=int, required=False, help='chessboard width size, default is 9') # 체스판의 너비
    parser.add_argument('--height', type=int, required=False, help='chessboard height size, default is 6') #체스판의 높이
    parser.add_argument('--save_file', type=str, required=True, help='YML file to save calibration matrices') #캘리브레이션 결과를 저장할 파일

    args = parser.parse_args()
    ret, mtx, dist, rvecs, tvecs = calibrate(args.image_dir, args.prefix, args.image_format, args.square_size, args.width, args.height)
    save_coefficients(mtx, dist, args.save_file)
    print("Calibration is finished. RMS: ", ret)

* 위 코드에서 오류가 뜨면 한글주석을 제거하고 실행해 보세요

 

위코드를 실행시키면

 

yehjin@yehjin-900X3L:~$ rosrun marker camera_calibration.py 
usage: camera_calibration.py [-h] --image_dir IMAGE_DIR --image_format
                             IMAGE_FORMAT --prefix PREFIX
                             [--square_size SQUARE_SIZE] [--width WIDTH]
                             [--height HEIGHT] --save_file SAVE_FILE
camera_calibration.py: error: the following arguments are required: --image_dir, --image_format, --prefix, --save_file

 

이렇게 뜹니다. 이것은 입력해줘야하는 input값들이 없어서 그런건데 젤 마지막줄이 해결방법을 제시해 주는 부분입니다.

이렇게 쓰면 되는거죠

 

yehjin@yehjin-900X3L:~$ rosrun marker camera_calibration.py --image_dir ~/catkin_ws/src/marker/src/image --image_format png --prefix left* --square_size 25 --width 8 --height 6 --save_file ~/catkin_ws/src/marker/src/image/ost3.yaml

 

오류가 뜬다면 left* 에서 *을 제거하고 다시해보세요

 

calibration roswiki를 이용하여 카메라를 이용해서 calibration한 결과

 

image_width: 640
image_height: 480
camera_name: narrow_stereo
camera_matrix:
  rows: 3
  cols: 3
  data: [ 586.9111 ,    0.     ,  309.6689 ,
            0.     ,  592.78916,  233.67894,
            0.     ,    0.     ,    1.     ]
camera_model: plumb_bob
distortion_coefficients:
  rows: 1
  cols: 5
  data: [0.090829, -0.204671, -0.001057, -0.000050, 0.000000]
rectification_matrix:
  rows: 3
  cols: 3
  data: [ 1.,  0.,  0.,
          0.,  1.,  0.,
          0.,  0.,  1.]
projection_matrix:
  rows: 3
  cols: 4
  data: [ 591.24005,    0.     ,  308.97831,    0.     ,
            0.     ,  597.47736,  232.8488 ,    0.     ,
            0.     ,    0.     ,    1.     ,    0.     ]

 

이미 저장된 이미지를 이용해서 위 code 를 사용하여 calibration한 결과

 

%YAML:1.0
---
K: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [ 5.7494769297367748e+02, 0., 3.0445999169574372e+02, 0.,
       5.8099846172481693e+02, 2.3035481242963945e+02, 0., 0., 1. ]
D: !!opencv-matrix
   rows: 1
   cols: 5
   dt: d
   data: [ 2.4236958514376992e-02, 8.2918441234635779e-02,
       -1.8400808936841081e-03, -1.8722329579970692e-03,
       -3.7845474348278413e-01 ]

 

각 행렬에서 e부분을 없애기 위해 다시 계산해보면

 

%YAML:1.0
---
K: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [ 574.947693,       0. ,   304.4599917,
                0.,    580.9984617, 230.3548124, 
                0.,          0.,         1. ]
D: !!opencv-matrix
   rows: 1
   cols: 5
   dt: d
   data: [ 0.02423695851, 0.08291844123, -0.00184, -000.1872232957, -0.00378454 ]

 

패키지를 이용하여 한 것과 파이썬 코드를 이용하여 calibration한 결과, 값이 조금 다르네요...사진이 흐릿해서 그런가..