Solvepnp(camera extrinsic parameter, 카메라 외부파라미터)

import cv2
import numpy as np
import sys
import pyzed.sl as sl
CHECKERBOARD = (6, 8)
  
# stop the iteration when specified
# accuracy, epsilon, is reached or
# specified number of iterations are completed.
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
  
  
# Vector for 3D points
threedpoints = []
  
# Vector for 2D points
twodpoints = []
  
#  3D points real world coordinates
objectp3d = np.zeros((1, CHECKERBOARD[0] * CHECKERBOARD[1], 3), np.float32)
objectp3d[0, :, :2] = np.mgrid[0:CHECKERBOARD[0], 0:CHECKERBOARD[1]].T.reshape(-1, 2)
prev_img_shape = None

zed = sl.Camera()
input_type = sl.InputType()
if len(sys.argv) >= 2 :
    input_type.set_from_svo_file(sys.argv[1])
init = sl.InitParameters(input_t=input_type)
init.camera_resolution = sl.RESOLUTION.HD1080
init.coordinate_units = sl.UNIT.MILLIMETER
err = zed.open(init)
if err != sl.ERROR_CODE.SUCCESS :
    print(repr(err))
    zed.close()
    exit(1)
runtime = sl.RuntimeParameters()
runtime.sensing_mode = sl.SENSING_MODE.STANDARD
image_size = zed.get_camera_information().camera_resolution
image_size.width = image_size.width /2
image_size.height = image_size.height /2
image_zed = sl.Mat(image_size.width, image_size.height, sl.MAT_TYPE.U8_C4)
key = ' '


while key != 113 :
    err = zed.grab(runtime)
    if err == sl.ERROR_CODE.SUCCESS :
        zed.retrieve_image(image_zed, sl.VIEW.LEFT, sl.MEM.CPU, image_size)
        image_ocv = image_zed.get_data()
        grayColor = cv2.cvtColor(image_ocv, cv2.COLOR_BGR2GRAY)
        ret, corners = cv2.findChessboardCorners(grayColor, CHECKERBOARD, cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_FAST_CHECK + cv2.CALIB_CB_NORMALIZE_IMAGE)
        if ret == True:
            threedpoints.append(objectp3d)
            corners2 = cv2.cornerSubPix(grayColor, corners, (11, 11), (-1, -1), criteria)
            twodpoints.append(corners2)
            image_ocv = cv2.drawChessboardCorners(image_ocv, 
                                        CHECKERBOARD, 
                                        corners2, ret)
        cv2.imshow("Image", image_ocv)
        cv2.imwrite('sssss.png', image_ocv)
        if cv2.waitKey(1) & 0xFF == ord('q'):
            cv2.destroyAllWindows()
            key = cv2.waitKey(10)
            print("start calculation")
            h, w = image_ocv.shape[:2]
            ret, matrix, distortion, r_vecs, t_vecs = cv2.calibrateCamera(threedpoints, twodpoints, grayColor.shape[::-1], None, None)
            print(" Camera matrix:")
            print(matrix)
            print("\n Distortion coefficient:")
            print(distortion)
            print("\n Rotation Vectors:")
            print(r_vecs)
            print("\n Translation Vectors:")
            print(t_vecs)

위의 코드는 ZED 카메라로 checkerboard를 이용하여 캘리브레이션을 진행하는 코드이다.

위의 코드로 먼저 카메라의 내부 파라미터들을 구하였다.

Camera matrix:
[[1.28190679e+03 0.00000000e+00 6.47066165e+02]
 [0.00000000e+00 1.25095833e+03 4.65778996e+02]
 [0.00000000e+00 0.00000000e+00 1.00000000e+00]]

 Distortion coefficient:
[[ 4.14364325e-02 -1.34730717e+00 -3.10126373e-02  4.30711180e-03
   6.95942873e+00]]

Distortion coefficient 왜곡 보정 계수

k1, k2, p1, p2, k3가 나왔다.

그리고 ZED로 얼굴 사진을 찍어 저장을 하였다. cv2.imwrite 함수 이용

import cv2

def onMouse(event, x, y, flags, param) :
    if event == cv2.EVENT_LBUTTONDOWN :
        print('왼쪽 마우스 클릭 했을 때 좌표 : ', x, y)

img = cv2.imread('/home/jaewoong/opencv_calibration/sssss.png')
cv2.imshow('image', img)
cv2.setMouseCallback('image', onMouse)
cv2.waitKey()

그리고 사진의 좌표를 마우스로 클릭을 하여 알 수 있게 하였다.

왼쪽 마우스 클릭 했을 때 좌표 :  520 295
왼쪽 마우스 클릭 했을 때 좌표 :  543 406
왼쪽 마우스 클릭 했을 때 좌표 :  506 245
왼쪽 마우스 클릭 했을 때 좌표 :  614 243
왼쪽 마우스 클릭 했을 때 좌표 :  526 348
왼쪽 마우스 클릭 했을 때 좌표 :  574 352

import cv2
import numpy as np
img = cv2.imread('/home/jaewoong/opencv_calibration/sssss.png')
# img = cv2.imread('/home/jaewoong/opencv_calibration/hihi.png')
size = img.shape

# image_points = np.array([
#                         (520,295),  # Nose tip
#                         (543,406),
#                         (506,245),  # Left eye corner
#                         # (562,218),  # Right eye corner
#                         (614,243),
#                         (526,348),  # Left mouth 
#                         (574,352)   # Right mouth 
#                       ], dtype="double")
# model_points = np.array([
#                       (0.0, 0.0, 0.0),
#                       (0.0, -300.0, -65.0),
#                       (-150.0, 170.0, -135.0),#Left eye corner
#                       # (3.5, 0.0, 0.0), #Right eye corner 
#                       (225.0, 170.0, -135.0),
#                       (-100.0, -150.0, -125.0),#Left mouth 
#                       (150.0, -150.0, -125.0) #Right mouth 
#                      ])


image_points = np.array([
                        (560,284),  # Nose tip
                        (548,410),
                        (469,231),  # Left eye corner
                        # (562,218),  # Right eye corner
                        (602,232),
                        (514,345),  # Left mouth 
                        (573,342)   # Right mouth 
                      ], dtype="double")
model_points = np.array([
                      (0.0, 0.0, 0.0),
                      (0.0, -330.0, -50.0),
                      (-225.0, 170.0, -135.0),#Left eye corner
                      (225.0, 170.0, -135.0),
                      (-150.0, -150.0, -125.0),#Left mouth 
                      (150.0, -150.0, -125.0) #Right mouth 
                     ])

focal_length = size[1]
center = (size[1]/2, size[0]/2)
camera_matrix = np.array([[612.37592572,   0.0,        475.56871451],
 [  0.0,         538.66554302, 264.90837774],
 [  0.0,          0.0,           1.0,        ]])
print ("Camera Matrix :\n {0}".format(camera_matrix))
# dist_coeffs = np.zeros((4,1))
dist_coeffs = np.array([[ -0.01684839, 0.73584156, -0.01990622,  -0.00853983 ]])
(success, rotation_vector, translation_vector) = cv2.solvePnP(model_points, image_points, camera_matrix, dist_coeffs, flags=0)
print ("Rotation Vector:\n {0}".format(rotation_vector))
print ("Translation Vector:\n {0}".format(translation_vector))
(nose_end_point2D, jacobian) = cv2.projectPoints(np.array([(0.0, 0.0, 500.0)]), rotation_vector, translation_vector, camera_matrix, dist_coeffs)
 
input_rotation = np.array([[-0.71354762],[-0.75935055],[-1.40898726]])
rod = cv2.Rodrigues(input_rotation,rotation_vector,jacobian=0)
print(rod)
for p in image_points:
    cv2.circle(img, (int(p[0]), int(p[1])), 3, (0,0,255), -1)
 
p1 = ( int(image_points[0][0]), int(image_points[0][1]))
p2 = ( int(nose_end_point2D[0][0][0]), int(nose_end_point2D[0][0][1]))
 
cv2.line(img, p1, p2, (255,0,0), 2)
 
# Display image
cv2.imshow("Output", img)
cv2.waitKey(0)

image_point에 아까 찍었던 2D 좌표를 입력하고 model_point에는 임의의 원점을 두어 좌표를 입력하였다.

그리고 아까 구했던 내부파라미터와 dist_coeffs 값을 solvePnP 함수에 입력하여 카메라의 외부 파라미터 값을 추출하였다.

Rotation Vector: [[-2.83401413] [ 0.06103462] [ 1.35665158]] Translation Vector: [[ 98.02675224] [ 113.84249106] [1601.04734024]]

여기서 Rotation Vector를 3x3 형태의 rotation matrix를 구해야 하는데 아직 해결방법을 찾지 못했다.

cv2.Rodrigues함수를 이용하여 구할 수 있다는데 아직 더 공부를 해봐야한다.

solvePnP는 사용자가 제공하는 3차원 점에 관련된 카메라의 계산된 포즈를 리턴한다.