[OpenCV] 07-2. Pose Estimation

< 7-2. Pose Estimation >

이번 장에서는

• 이미지에서 3D 효과를 생성하기 위해 calib3d 모듈을 이용하는 법을 배워볼 것이다.

Basics

매우 작은 섹션이 될 것이다. camera calibration를 하는 동안에는. camera matrix, 왜곡 계수등을 찾았었다. 패턴 이미지가 주어졌을 때, 위의 정보를 가지고 pose를 계산하고나, 물체가 공간에 어떻게 놓여있는지(회전,옮김)를 알 수 있다. 평면 물체에 대해서, Z축을 0이라고 할 수 있고, 이제 패턴 이미지를 보기 위해 카메라가 어디에 위치해야하는지가 문제가 된다. 그래서, 만약 물체가 공간에 어떻게 놓여있는지를 안다면, 물체에 2D 도형을 그려서 3D 효과를 시뮬레이션할 수 있다. 백문이 불여일견! 한 번 해보자!

우리의 문제는, 체스판의 첫 모서리에 3D 좌표(X,Y,Z 축)을 그리고 싶다는 것이다. X 축은 파랑색, Y 축은 초록, Z 축은 빨간색으로 표시하자. Z축은 체스판 평면에 대해 수직으로 나타나야한다.

먼저, 저번에 저장해놨던 camera matrix와 왜곡계수를 불러오자.

import cv2
import numpy as np
import glob

# 저장해뒀던 .npz 파일 불러오기
with np.load('calib.npz') as X:
    mtx, dist, _ , _ = [X[i] for i in ('mtx','dist','rvecs','tvecs')]

draw() 함수를 만들자. cv2.findChessboardCorner() 를 사용해서 체스판 모서리를 찾고, axis point 로 3D 축을 그리자!

def draw(img,corners,imgpts):
    corner = tuple(corner[0].ravel())
    img = cv2.line(img, corner, tuple(imgpts[0].ravel()), (255,0,0),5)
    img = cv2.line(img, corner, tuple(imgpts[1].ravel()), (0,255,0),5)    
    img = cv2.line(img, corner, tuple(imgpts[2].ravel()), (0,0,255),5)    
    return img

이전 경우처럼, termination criteria(종료 기준점), Object points(체스보드에서 모서리의 3D점)과 축 포인트(axis point)을 생성한다. 축 포인트는 3D 공간에서 축을 그리기 위한 점이다. 길이 3으로 그려볼 것이다. 그래서 우리의 X 축은 (0,0,0)에서 (3,0,0) 까지 그려진다. Z 축은, (0,0,0)에서 (0,0,-3)까지 그려진다. 음수는 카메라를 향해 그려진다는 것을 의미한다.

criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER,300,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([[3,0,0],[0,3,0],[0,0,-3]]).reshape(-1,3)

이제 각 이미지를 불러와보자! 7x6 격자에서 찾아보자. 만약 찾았다면, 하위코너 픽셀들(cv2.cornerSubPix)로 수정한다. 그리고 rotation과 translation을 계산하기 위해 cv2.solvePnPRansac() 함수를 사용한다. 축 포인트를 이미지 평면으로 투영시키기 위해 변환 행렬을 사용한다. 간단하게 보면, 이미지 평면에서 3D 공간의 (3,0,0),(0,3,0),(0,0,3)와 각각 대응하는 점들을 찾는 것이다. 점과 점을 잇는다고 생각하면 될 것 같다.(2D ~ 3D) 그리고 나서, 아까 만든 draw() 함수를 이용해서 첫 코너에서 각 점까지 선을 그리면 끝이다!

for name in glob.glob('./images/Calib/*.jpg'):
    img = cv2.imread(name)
    gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
    ret, corners = cv2.findChessboardCorners(gray,(7,6),None)
    # 패턴 보유시 ret = True

    if ret == True:
        corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)

        # rotation과 translation 벡터 찾기
        _,rvecs, tvecs, inliers = cv2.solvePnPRansac(objp,corners2,mtx,dist)

        # 3D 포인트를 이미지 평면에 투영시키자
        imgpts, jac = cv2.projectPoints(axis,rvecs,tvecs,mtx,dist)

        # 그리기
        img = draw(img,corners2,imgpts)
        cv2.imshow("Res",img)
        k = cv2.waitKey(0) & 0xff
        if k == "s":
            cv2.imwrite(name[:6]+".png",img)
cv2.destroyAllWindows()

Render a cube

큐브를 그리고 싶다면 draw() 함수와 axis 포인트를 조금 수정하면 된다.

def draw_cube(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,0,0),3)

    # 위의 층은 빨간색으로
    img = cv2.drawContours(img,[imgpts[4:]],-1,(0,0,255),3)

    return img

축 포인트를 3D 공간에서 큐브의 8개 코너들 넣어주면 된다.

이걸 보고 생각하면 좌표 찾기가 쉽다. 그림에서는 X : 파랑, Y : 빨강, Z : 초록 으로 생각하면 된다. 근데 여기서는 큐브가 카메라를 향해야하기 때문에 Z값들을 다 음수(-)로 처리하면 된다. 그러면 아래의 좌표가 도출된다.

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]])

그리고 똑같이 그려주면 된다

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]])

for name in glob.glob('./images/Calib/*.jpg'):
    img = cv2.imread(name)
    gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
    ret, corners = cv2.findChessboardCorners(gray,(7,6),None)
    # 패턴 보유시 ret = True

    if ret == True:
        corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)

        # rotation과 translation 벡터 찾기
        _,rvecs, tvecs, inliers = cv2.solvePnPRansac(objp,corners2,mtx,dist)

        # 3D 포인트를 이미지 평면에 투영시키자
        imgpts, jac = cv2.projectPoints(axis,rvecs,tvecs,mtx,dist)

        # 그리기
        img = draw_cube(img,corners2,imgpts)
        cv2.imshow("Res",img)
        k = cv2.waitKey(0) & 0xff
        if k == "s":
            cv2.imwrite(name[:6]+"_cube.png",img)
cv2.destroyAllWindows()