# Camera Calibration
###### tags: `視覺組`
> [name=楊育陞([e-mail])]

reference:
https://docs.opencv.org/4.x/dc/dbb/tutorial_py_calibration.html
repo:
https://github.com/NCTU-AUV/orca_camera
https://github.com/NCTU-AUV/zed_calib

鏡頭在水下拍攝時，會經過水、機殼及鏡頭本身多次折射，和實際影像產生偏差，因此使用OpenCV內建函數校正相機。

## 產生校正用Pattern
可以使用現成的Pattern:
https://github.com/opencv/opencv/blob/4.x/doc/pattern.png

或者參考[Create calibration pattern
](https://docs.opencv.org/4.x/da/d0d/tutorial_camera_calibration_pattern.html)使用[gen_pattern.py](https://github.com/opencv/opencv/blob/4.x/doc/pattern_tools/gen_pattern.py) 建立自訂Pattern：

  python gen_pattern.py -o chessboard.svg --rows 9 --columns 6 --type checkerboard --square_size 20

產生Pattern後列印並護貝，以便於水下工作。

## 取得校正前影像
需要至少十張影像來校正相機，直接使用鏡頭拍攝即可，如果是ZED相機，需要左右鏡頭分別校正，可以使用以下程式分別取得左右鏡頭影像：
https://github.com/NCTU-AUV/zed_calib/blob/master/image_capture.py

import pyzed.sl as sl
import cv2

def main():
    path_right = "images/right/"
    path_left = "images/left/"

    # Create a Camera object
    zed = sl.Camera()

    # Create a InitParameters object and set configuration parameters
    init_params = sl.InitParameters()
    init_params.camera_resolution = sl.RESOLUTION.HD1080  # Use HD1080 video mode
    init_params.camera_fps = 30  # Set fps at 30

    # Open the camera
    err = zed.open(init_params)
    if err != sl.ERROR_CODE.SUCCESS:
        exit(1)

    image = sl.Mat()

    runtime_parameters = sl.RuntimeParameters()
    i = 0
    key = ""
    while key != 113: # for 'q' key
        # Grab an image, a RuntimeParameters object must be given to grab()
        if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS:
            # A new image is available if grab() returns SUCCESS
            cv2.namedWindow("ZED", cv2.WINDOW_NORMAL)      
            zed.retrieve_image(image, sl.VIEW.SIDE_BY_SIDE),sl.Resolution(640,480)
            cv2.imshow("ZED", image.get_data())
            key = cv2.waitKey(5)
            if key == ord('s'):
                zed.retrieve_image(image, sl.VIEW.LEFT)
                cv2.imwrite(path_left + str(i) + ".tif", image.get_data())
                zed.retrieve_image(image, sl.VIEW.RIGHT)
                cv2.imwrite(path_right + str(i) + ".tif", image.get_data())
                i = i+1
    # Close the camera
    cv2.destroyAllWindows()
    zed.close()

if __name__ == "__main__":
    main()

## 取得校正參數

使用以下程式取得校正參數，要調整的自訂參數如下：
1. h, w: Pattern(棋盤)內部的格子點數，注意並非棋盤長寬
2. chessboard_square_mm: 棋盤的格子邊長(單位為mm)
3. path_left/right: 校正用影像路徑

https://github.com/NCTU-AUV/zed_calib/blob/master/calibrate.py

import numpy as np
import cv2 as cv
import glob

path_right = "images/right/*.tif"
path_left = "images/left/*.tif"
path_calib = "parameters/"

def calib(LR, img_path):
  print("start calibrate " + LR)
  h, w = 8, 5 #size of chessboard (h*w)
  chessboard_squares_mm = 20 #size of chessboard square in mm

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

  # prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(w,5,0)
  objp = np.zeros((w*h,3), np.float32)
  objp[:,:2] = np.mgrid[0:h,0:w].T.reshape(-1,2)
  objp = objp*chessboard_squares_mm 

  # 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(img_path)
  num = 0
  found = 0
  for fname in images:
      img = cv.imread(fname)
      gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
      # Find the chess board corners
      # ret, corners = cv.findChessboardCorners(gray, (h,w), None)
      ret, corners = cv.findChessboardCorners(gray, (h,w), cv.CALIB_CB_ADAPTIVE_THRESH + cv.CALIB_CB_NORMALIZE_IMAGE + cv.CALIB_CB_FAST_CHECK)
      # ret, corners = cv.findChessboardCornersSB(gray, (h,w), cv.CALIB_CB_EXHAUSTIVE+cv.CALIB_CB_ACCURACY)
      # If found, add object points, image points (after refining them)
      if ret == True:
          print(str(num) + ' found')
          objpoints.append(objp)
          corners2 = cv.cornerSubPix(gray,corners, (7,7), (-1,-1), criteria)
          imgpoints.append(corners2)
          # Draw and display the corners
          cv.drawChessboardCorners(img, (h,w), corners2, ret)
          #cv.imshow('img', img)
          #cv.waitKey(5)
          found += 1
      else:
          print(str(num) + ' not found')
      num = num+1
  cv.destroyAllWindows()
  print('================')
  print('found ' + str(found) + ' patterns in ' + str(num) + ' pic')

  if(found > 10):
      print('generate calib param')
      ret, mtx, dist, rvecs, tvecs = cv.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)
      np.savez(path_calib+'calib_param_' + LR + '.npz', mtx=mtx, dist=dist, rvecs=rvecs, tvecs=tvecs)
      mean_error = 0
      for i in range(len(objpoints)):
          imgpoints2, _ = cv.projectPoints(objpoints[i], rvecs[i], tvecs[i], mtx, dist)
          error = cv.norm(imgpoints[i], imgpoints2, cv.NORM_L2)/len(imgpoints2)
          mean_error += error
      print( "total error: {}".format(mean_error/len(objpoints)) )
      # test(h, w)
  else:
      print('not enough found pattern')


calib("left", path_left)
calib("right", path_right)

如果有找到棋盤格點，會顯示以下圖片：

## 套用校正參數
接著便可套用前面取得的參數於校正將來拍攝的圖片：

img = cv.imread('left12.jpg') # img to be undistorted
h,  w = img.shape[:2]
newcameramtx, roi = cv.getOptimalNewCameraMatrix(mtx, dist, (w,h), 1, (w,h))

# undistort
dst = cv.undistort(img, mtx, dist, None, newcameramtx)
# crop the image
x, y, w, h = roi
dst = dst[y:y+h, x:x+w]
cv.imwrite('calibresult.png', dst)

## 儲存校正參數
只要鏡頭及環境沒有變化，校正參數就不須做更改，因此可以儲存參數以利將來使用
1. 儲存參數

np.savez('cali_parameter.npz', mtx=mtx, dist=dist, rvecs=rvecs, tvecs=tvecs)

2. 讀取參數：

cali_parameter_npz = np.load('cali_parameter.npz')
mtx = cali_parameter_npz['mtx']
dist = cali_parameter_npz['dist']
rvecs = cali_parameter_npz['rvecs']
tvecs = cali_parameter_npz['tvecs']

## 提高準確度
如果找不到棋盤格線，可以將`findChessboardCorners()`添加參數：

ret, corners = cv.findChessboardCorners(gray, (h,w), cv.CALIB_CB_ADAPTIVE_THRESH + cv.CALIB_CB_NORMALIZE_IMAGE + cv.CALIB_CB_FAST_CHECK)

或是使用`findChessboardCornersSB()`，使用這個function需要對應的pattern:

ret, corners = cv.findChessboardCornersSB(gray, (h,w), cv.CALIB_CB_EXHAUSTIVE+cv.CALIB_CB_ACCURACY)

## 測試校正結果
https://github.com/NCTU-AUV/zed_calib/blob/master/test_undistort.py