카메라의 위치 및 3D 자세정보

2차원 영상 내에서 3차원 거리정보를 얻기 위해서는 

카메라 내부 파라미터 뿐만 아니라 카메라의 위치 및 3D 자세정보가 필요하다. 

 

지난 글에서 카메라 내부 파라미터를 구했으니 

https://dohyeon.tistory.com/23

[영상처리] 카메라 캘리브레이션 (Camera Calibration)

이번 글에서 카메라의 위치 및 3D 자세정보를 구해보겠다.

---

카메라로 촬영한 영상을 보고 이 영상을 획득할 당시의 카메라의 위치 및 3D 자세정보(팬,틸트)를 추출하기 위해서는

먼저 다음과 같은 전제조건이 필요하다.

 

a. 해당 카메라의 내부 파라미터 및 왜곡계수 : fx, fy, cx, cy, k1, k2, p1, p2

b. 물체에 대한 최소 4개 이상의 3D 월드좌표+ 이에 대응되는 2D 영상좌표

 

a. 는 이전 글에서 구했으니 넘어가고,

b. 를 각각 구해줘야 하는데 말로만 하면 이해가 가지 않으니 그림으로 예를 들겠다. 

 

1. 물체의 3D 월드좌표 + 이에 대응되는 2D 영상좌표 구하기

A4 용지에 10cm x 10cm 크기의 정사각형을 그리고 프린트하였다. 

위 그림과 같이 각각의 사각형 꼭지점들에 대한 3D 월드좌표를 임의로 정의해준 뒤, 해당점에 매칭되는 사각형 꼭지점들의 2D 영상좌표쌍을 구해준다. 구해준 네쌍의 점의 좌표를 solvePnP 함수에 인자로 넣어주면 카메라의 위치 및 자세정보가 나온다.

 

위 그림에서는

사각형의 3D 월드좌표 = (0,0,0), (10,0,0), (0,10,0), (10,10,0)

사각형의 2D 영상좌표 = ?

 

사각형의 2D 영상좌표는 어떻게 구할 수 있을까? 

openCV 마우스클릭 이벤트를 이용하면 구할 수 있다.

import cv2
import numpy as np  

# 마우스 이벤트 콜백함수 정의
def mouse_callback(event, x, y, flags, param): 
    print("마우스 이벤트 발생, x:", x ," y:", y) # 마우스 위치 출력

img = cv2.imread("이미지 절대경로")

cv2.namedWindow('image')  #마우스 이벤트 영역 윈도우 생성
cv2.setMouseCallback('image', mouse_callback)

while(True):

    cv2.imshow('image', img)

    k = cv.waitKey(1) & 0xFF
    if k == 27:    # ESC 키 눌러졌을 경우 종료
        print("ESC 키 눌러짐")
        break
cv2.destroyAllWindows()

위 코드를 이용하면

사각형의 2D 영상좌표 = (236,420), (423,428), (244,239), (428,246)

를 구할 수 있다. 

 

결과적으로

사각형의 2D 영상좌표 = (236,420), (423,428), (244,239), (428,246)

사각형의 3D 월드좌표 = (0,0,0), (10,0,0), (0,10,0), (10,10,0)

를 구하였다. 

 

 

따라서 사각형 네 꼭지점의

2차원 영상좌표와 3차원 월드좌표를 코드로 나타내면 아래와 같다.

#2차원 영상좌표
points_2D = np.array([
                        (966, 543),  #좌 하단 
                        (1155, 554),  #우 하단
                        (975, 363),  #좌 상단
                        (1159, 369),  #우 상단
                      ], dtype="double")
                      
#3차원 월드좌표
points_3D = np.array([
                      (0.0, 0.0, 0.0),       #좌 하단
                      (10, 0.0, 0.0),        #우 하단
                      (0.0, 10, 0.0),        #좌 상단
                      (10, 10, 0.0)          #우 상단
                     ], dtype="double")

 

solvePnP 함수의 인자를 살펴보면 

2. cameraMatrix 인자 정의하기

cameraMatrix (카메라 내부 파라미터) 는 행렬 형태로 나타내줘야 하기 때문에 

numpy 라이브러리를 이용하면 다음과 같다.

# camera 내부 파라미터 
cameraMatrix = np.array([[1065.352, 0, 960.127], [0, 1064.480, 569.483], [0, 0, 1]])

3. distCoeffs 인자 정의하기

distCoeffs 인자는 왜곡을 무시하고 진행할것이기 때문에 Null 값을 넣어주면 된다.

#distcoeffs는 카메라의 왜곡을 무시하기 때문에 null값 전달
dist_coeffs = np.zeros((4,1))

 

4. <최종 코드>

import cv2
import numpy as np

img = cv2.imread("C:/Users/MSDL-DESK-02/Desktop/img.png")
size = img.shape

#2차원 영상좌표
points_2D = np.array([
                        (966, 543),  #좌 하단 
                        (1155, 554),  #우 하단
                        (975, 363),  #좌 상단
                        (1159, 369),  #우 상단
                      ], dtype="double")
                      
#3차원 월드좌표
points_3D = np.array([
                      (0.0, 0.0, 0.0),       #좌 하단
                      (10, 0.0, 0.0),        #우 하단
                      (0.0, 10, 0.0),        #좌 상단
                      (10, 10, 0.0)          #우 상단
                     ], dtype="double")


# camera 내부 파라미터 
cameraMatrix = np.array([[1065.352, 0, 960.127], [0, 1064.480, 569.483], [0, 0, 1]])

#distcoeffs는 카메라의 왜곡을 무시하기 때문에 null값 전달
dist_coeffs = np.zeros((4,1))

#solvePnp 함수적용
retval, rvec, tvec = cv2.solvePnP(points_3D, points_2D, cameraMatrix, dist_coeffs, rvec=None, tvec=None, useExtrinsicGuess=None, flags=None)

R = cv2.Rodrigues(rvec)
t= tvec

print(R)
print("\n")
print(t

solvePnp 함수는 기본적으로 3D 월드좌표를 3D 카메라 좌표로 변환시키는 변환정보 (retval, rvec, tvec)를 반환하며 이로부터 회전변환 R과 평행이동 T를 아래와 같이 얻을 수 있다. 

 

rvec = 회전 벡터

tvec = 평행이동 벡터

 

R = cv2.Rodrigues(rvec)

T = tvec

 

openCV의 solvePnP 함수가 반환하는 rvec은 회전변환에 대한 Rodrigues 표현이기 때문에

실제 회전변환 행렬 R을 구하기 위해서는 openCV의 Rodrigues() 함수를 취해주어야 한다.

 

결과적으로 

R = (array([[ 0.99833387,  0.05028667, -0.02829721],
       [ 0.0462584 , -0.99063762, -0.12844172],
       [-0.03449118,  0.12691873, -0.99131327]])

 

t = [[ 0.29921233]
 [-1.35458807]
 [56.65101544]]