2D 영상에서 물체까지 3D 거리 구하기

2022-02-18

2D 영상 내에서 특정 물체까지의 3D 거리정보를 얻는 방법은 여러가지가 있었는데 

나는 그중 3D 변환을 이용한 방법을 사용하였다. 

---

<이론>

3D 변환을 이용한 방법은 가장 일반적인 방법이며, 영상 기하학에서 좌표계에 대한 이해만 있으면 

어렵지 않게 적용할 수 있다.

이 방법을 사용하기 위해서는 카메라의 내부 파라미터(fx, fy, cx, cy)와 카메라 3D 자세정보(R, t)가 미리 구해져 있어야만 사용 가능하다. 

 

카메라의 내부 파라미터(fx, fy, cx, cy) 및 3D 자세정보(R,t)는 앞선 글에서 구하였다.

 

카메라 내부 파라미터(fx, fy, cx, cy) -  <참조: https://dohyeon.tistory.com/23 > 

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

카메라 3D 자세정보(R, t) - <참조: https://dohyeon.tistory.com/25?category=1056956 >

[영상처리] 카메라의 위치 및 3D 자세정보

 

구해진 내부 파라미터와 3D 자세정보를 이용하여 2D 영상 내 특정 픽셀의 3D 거리정보를 구하는 방법을 알아보자.

 

아래 설명의 이해를 돕기위한 좌표계 그림

 

어떤 3차원 공간상의 한 점 P에 대한 카메라좌표를 Pc, 월드좌표를 Pw라 하면 Pc와 Pw 사이의 변환은 다음 수식에 의해 진행된다.

 

식 (1)

입력 영상의 픽셀좌표를 p(x,y), 대응되는 지면좌표를 P(X, Y)라 하자.

 

먼저 카메라의 내부파라미터에 대한 영향을 없애기 위해 픽셀좌표를 정규좌표로 변환한다 (영상에서 모든 기하학적 해석은 정규좌표를 통해 이루어진다).

식 (2)

이 때, 구한 정규좌표를 3차원 카메라좌표로 해석하면 (u, v, 1)이 된다. 정규이미지평면은 카메라 원점에서 초점거리가 1인 평면이므로 정규이미지 평면 상의 점 (u, v)의 카메라좌표계 좌표는 (u, v, 1)이 된다. 이 점을 Pc = (u, v, 1)라 하자.

 

이제 카메라 원점과 Pc를 연결한 직선이 지면과 만나는 점을 구하면 최종 목표를 이루는 것이다.

지면과의 교점을 구하기 위해서는 카메라좌표계가 아닌 월드좌표계에서 계산을 수행해야하는데, 

카메라 원점의 카메라좌표를 Cc, 월드좌표를 Cw라 하고 점 Pc의 월드좌표를 Pw라 하자. 

<카메라 원점의 카메라좌표는 항상 (0,0,0)임에 주의, 즉 Cc=(0,0,0)>

 

대응되는 월드좌표는 식 (1)를 이용하여 다음과 같이 계산된다. 

 

3D 좌표계 변환

이제 월드 좌표계 상에서 Cw와 Pw를 잇는 직선이 지면과 만나는 점을 구해보자. 벡터의 개념을 이용하면 Cw와 Pw를 잇는 직선상의 임의의 점 P = Cw + k(Pw - Cw) 로 표현할 수 있다. (K는 임의의 상수)

 

지면과 만나는 점 P

 그런데, 월드좌표계 상에서 지면은 Z =0 이므로 Cw + k(Pw - Cw)의 Z좌표가 0이 되도록 k의 값을 구하면 

월드 좌표계에서 지면과 만나는 점 P를 구할 수 있다!!! 

<여기서 구한 지면좌표는 월드좌표계를 어떻게 설정하느냐에 따라 달라지는 값임에 주의>

 

---

<적용>

나의 경우 ZED2i camera를 이용해 테스트하였다.(2개의 렌즈를 가진 streo camera지만 한쪽 렌즈에서 촬영한 2D 이미지 정보만 이용하였다.)

 

앞서 구한 카메라 파라미터 및 3D 자세정보

Fx = 1065.352

Fy = 1064.480

Cx = 960.127

Cy = 569.483

R =

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

입력영상의 픽셀좌표 x ,y =(966,544) 

픽셀좌료의 정규좌표 u = (x - Cx)/Fx = 0.005512731942

                            v = (y - Cy)/Fy = -0.02393938825

 

정규이미지 평면상의 점의 카메라좌표 Pc = (0.005512731942, -0.02393938825, 1)^T

카메라 원점의 카메라좌표 Cc = (0,0,0)^T

점 Pc 의 월드좌표 Pw = R^T(Pc - t)

= [[ 1.68781261]
   [-8.39611602]
   [55.00499017]]

아래 코드 참조

import numpy as np 

Rt = np.array([[ 0.99833387,  0.05028667, -0.02829721],
       [ 0.0462584 , -0.99063762, -0.12844172],
       [-0.03449118,  0.12691873, -0.99131327]]).T



Pc = np.array([[0.005512731942, -0.02393938825, 1]]).T
t = np.array([[0.29921233, -1.35458807, 56.65101544]]).T

a = Rt
b = (Pc - t)

# a행렬 b행렬 행렬곱
print(np.dot(a,b))

 

 

Cw = R^T(Cc - t) = -(R^T)t =

[[ 1.71790764] 
 [-8.54702723] 
 [55.99338462]]

 

아래코드 참조

import numpy as np 

Rt = np.array([[ 0.99833387,  0.05028667, -0.02829721],
       [ 0.0462584 , -0.99063762, -0.12844172],
       [-0.03449118,  0.12691873, -0.99131327]]).T



Pc = np.array([[0.005512731942, -0.02393938825, 1]]).T
t = np.array([[0.29921233, -1.35458807, 56.65101544]]).T

a = Rt
b = (Pc - t)

#print(np.dot(a,b))

c = (-t)

print(np.dot(a,c))

 

마지막으로 내가 최종적으로 구하려는 지면과의 교점

위 공식에서 임의의 상수 k는 점 P의 z좌표가 0이라는 사실을 이용하여 구할 수 있다. 

 

(Cw의 z 좌표) + k(Pw의 z좌표- Cw의 z 좌표) = 0 

을 이용하면 결과적으로 

k = 56.65084883 을 얻을 수 있다. 

 

따라서 

점 P = Cw + k(Pw-Cw) 이므로 

 

최종코드 

import numpy as np 

Rt = np.array([[ 0.99833387,  0.05028667, -0.02829721],
       [ 0.0462584 , -0.99063762, -0.12844172],
       [-0.03449118,  0.12691873, -0.99131327]]).T



Pc = np.array([[0.005512731942, -0.02393938825, 1]]).T
t = np.array([[0.29921233, -1.35458807, 56.65101544]]).T

a = Rt
b = (Pc - t)
c = (-t)
Pw = np.dot(a,b)
Cw = np.dot(a,c)
k = 56.65084883
# print(Pw)
# print(Cw)

P = Cw + k*(Pw-Cw)
print(P)

위 코드로 지면과의 교점 P의 3차원 좌표를 구할 수 있다.