[3D CV] PnP

Perspective-n-Point(PnP)

 

켈리브레이션과 유사한데, $K$는 주어져있다고 가정하고 $R$, $\mathbf{t}$를 찾는 알고리즘
(카메라가 어디서 촬영된건지를 알아내는 알고리즘)

위에서 사진과 실제 현실과의 Alignment를 비교해서 어디서 촬영됬는지 알아냄
PnP : $K$가 주어졌을 때 $n$개의 point를 가지고 camera pose를 알아내는 문제

 

PnP는 위처럼 3D와 2D 사이의 correspondence가 주어졌을 때 $R$, $\mathbf{t}$를 구할 수 있음

 

Decomposition With The Know $K$

이전의 DLT를 다시 상기해보자

$$\underset{\text{Known}}{\mathbf{x}_i} = \underset{\text{Unknown}}{\begin{bmatrix} P_{11} & P_{12} & P_{13} & P_{14} \\ P_{21} & P_{22} & P_{23} & P_{24} \\ P_{31} & P_{32} & P_{33} & P_{34} \end{bmatrix}} \underset{\text{Known}}{\mathbf{X}_i}$$

 

$$x_i = \frac{P_{11}X_i + P_{12}Y_i + P_{13}Z_i + P_{14}}{P_{31}X_i + P_{32}Y_i + P_{33}Z_i + P_{34}}, \quad y_i = \frac{P_{21}X_i + P_{22}Y_i + P_{23}Z_i + P_{24}}{P_{31}X_i + P_{32}Y_i + P_{33}Z_i + P_{34}}$$

 

$$\begin{bmatrix} X_i & Y_i & Z_i & 1 &  &  &  &  & -x_iX_i & -x_iY_i & -x_iZ_i & -x_i \\  &  &  &  & X_i & Y_i & Z_i & 1 & -y_iX_i & -y_iY_i & -y_iZ_i & -y_i \end{bmatrix} \begin{bmatrix}  P_{11} \\ P_{12} \\ P_{13} \\ P_{14} \\ P_{21} \\ P_{22} \\ P_{23} \\ P_{24} \\ P_{31} \\ P_{32} \\ P_{33} \\ P_{34} \end{bmatrix} 
= \mathbf{0}_{2 \times 1}$$

 

correspondence 1개($\mathbf{x}$, $\mathbf{X}$ pair)에 의해 방정식 1개 생기고 row 2개씩 생김

$P$는 12 - 1(up-to scale) = 11 DoF 이므로

DLT를 풀려면 최소 6개의 point pair 필요

(참고: https://dohlab.tistory.com/114)

 

어쨌든 $Ax=0$꼴로 만들어서 SVD를 풀어 $P$를 찾을 수 있음

그다음

$$P = K [R |\mathbf{t} ] = K [ R | - R \mathbf{C} ] = [K R | -KR \mathbf{C}]$$

$$ [K R | -KR \mathbf{C}]$$

이 식을 보면 $P$의 왼쪽 3x3 matrix가 $KR$, 오른쪽 3x1 matrix $-KR \mathbf{C}$가 되는데

왼쪽 3x3 matrix에서 $K$는 upper triangular matrix, $R$은 orthogonal matrix이므로
특이한 형태이기 때문에 3x3 matrix가 $K$랑 $R$로 분해가 가능 → QR decomposition

 

QR decomposition에서 $R$은 upper triangular matrix($K$), $Q$는 orthogonal matrix($R$)

보면은 순서가 반대이기 때문에 RQ decomposition을 해야함

 

어쨌든 여기까지 $P$를 구한후 $K$, $R$, $\mathbf{t}$를 Decompose해서 구했었음

그런데 이제부터는 $K$를 알고 있다고 가정하고 PnP를 배우기 때문에 다른 방법을 사용할 수 있음

3D-2D correspondence 알고있고, $P$는 DLT로 구하고 $K$도 알고 있음
여기서는 RQ Decomposition으로 $R$, $\mathbf{t}$를 안구하고 다르게 구함

$$K[R|\mathbf{t}] \equiv [\mathbf{P_1 \ P_2 \ P_3 \ P_4 }]$$

$$\lambda [R | \mathbf{t}] = K^{-1} [\mathbf{P_1 \ P_2 \ P_3 \ P_4 }]$$

$\mathbf{P}_4$랑 $\mathbf{t}$를 빼서 3x3 행렬 형태로 만들어서 식을 계산하면 다음과 같음

$$\lambda R = K^{-1} [\mathbf{P_1 \ P_2 \ P_3 }] = UDV^T$$

SVD 했을 때 rotation matrix의 lambda 배 형태가 나오려면

$D$가 단위행렬에 $\lambda$배 한 형태가 되어야 함

즉, $d_{11}=d_{22}=d_{33}$가 되야하고, $d_{11}$은 $\lambda$가 되야함

$$D = \begin{bmatrix} d_{11} & & \\ & d_{22} & \\ & & d_{33} \end{bmatrix} \approx d_{11} \begin{bmatrix} 1 & & \\ & 1 & \\ & & 1 \end{bmatrix} $$

$$\lambda \approx d_{11}$$

$\approx$라고 표현한 이유는 error가 있어서 $d_{11}, d_{22}, d_{33}$이 완전히 같지는 않음

어쨌든 이걸 그대로 대입해서 단위행렬 $I$ 없애고, $\lambda$ 약분해버리면

$$R = UV^T$$

$\mathbf{t}$는 $K$랑 $\mathbf{P}_4$가지고 구하면 됨

$$\mathbf{t} = \frac{1}{d_{11}} K^{-1} \mathbf{P}_4$$

$\lambda = d_{11}$인걸 적용하면 위와 같이 나옴

이런식으로 $K$를 알고있으면 RQ Decomposition 말고 SVD로 $R$, $\mathbf{t}$를 알 수 있음
근데 문제는 $P$를 먼저 구하고 이 방법을 사용했는데

$K$를 알고 있다는 큰 이점을 $P$를 구할 때는 쓰질 않음

 

$P$는 11 DoF

correspondence 6개 필요

 

근데 $K$를 알고 있으면 $K$의 DoF만큼 뺄 수 있음 ($K$는 5 DoF)

11 - 5 = 6 DoF 이므로

우리는 6 DoF만 찾으면 됨

$R$이 3 DoF, $\mathbf{t}$가 3 DoF

3개의 correspondence로도 충분히 $P$ 찾을 수 있음

 

따라서, PnP는 P3P부터 풀 수 있음

당연히 더 많은 P4P도 가능하고 일반화한 PnP도 가능

 

P4P(Planar Cases)

 

4개의 correspondence가 주어졌을 때 푸는 특별한 case인데

4개가 전부 같은 plane 위에 있음 → Homography 구할 수 있음

비슷한 case를 Zhang's method에서 했었음

$$\begin{bmatrix} x \\ y \\ 1 \end{bmatrix} \equiv K [R \mid \mathbf{t}] \begin{bmatrix} X \\ Y \\ 0 \\ 1 \end{bmatrix} \rightarrow \lambda \begin{bmatrix} x \\ y \\ 1 \end{bmatrix} = K [\mathbf{r}_1 \ \mathbf{r}_2 \ \mathbf{r}_3 \ \mathbf{t}] \begin{bmatrix} X \\ Y \\ 0 \\ 1 \end{bmatrix}$$

$$\lambda \begin{bmatrix} x \\ y \\ 1 \end{bmatrix} = K [\mathbf{r}_1 \ \mathbf{r}_2 \ \mathbf{t}] \begin{bmatrix} X \\ Y \\ 1 \end{bmatrix}$$

(참고: https://dohlab.tistory.com/116)

여기서 가운데 $K[\mathbf{r}_1 \ \mathbf{r}_2 \ \mathbf{t}]$라는 Homography를 구했었는데

Zhang's method 때와는 다르게 $K$를 이미 알고 있음

 

$$\mathbf{r}_1 = \frac{K^{-1} \mathbf{h}_1}{\| K^{-1} \mathbf{h}_1 \|}, \quad \mathbf{r}_2 = \frac{K^{-1} \mathbf{h}_2}{\| K^{-1} \mathbf{h}_2 \|}$$

$$\mathbf{r}_1 \times \mathbf{r}_2 = \mathbf{r}_3$$

$$\mathbf{t} = \frac{K^{-1} \mathbf{h}_3}{\| K^{-1} \mathbf{h}_1 \|}$$

따라서, Zhang's method의 마지막 결론 부분만 과져와서 $R$, $\mathbf{t}$ 식에 바로 $K$ 대입해서 구하면됨

 

P3P(General Cases)

 

P3P에서 3개의 Corresponding Points 수가 풀수 있는 최소인 이유는?
$K$를 알고 있다는 건 어떤 점($\mathbf{x}_i$)으로부터 역행렬을 곱해($K^{-1}$) back-projection 해서 ray로 만들 수 있다는 것
점이 고정되어있으니 3D ray도 고정되어있고 ray 사이의 각도($\theta_i$)도 고정되어 있음

만일, 3개가아니라 2개의 correspondence라면 camera center를 이동시켜도 똑같은 correspondence가 나올 수 있음

따라서, 최소 3개는 있어야 3개의 correspondence를 가지도록하는 camera center가 정확하게 고정됨

 

$$\cos\theta = \frac{\mathbf{d}_1^T \mathbf{d}_2}{\sqrt{\mathbf{d}_1^T \mathbf{d}_1} \sqrt{\mathbf{d}_2^T \mathbf{d}_2}} = \frac{(K^{-1} \mathbf{x}_1)^T (K^{-1} \mathbf{x}_2)}{\sqrt{(K^{-1} \mathbf{x}_1)^T (K^{-1} \mathbf{x}_1)} \sqrt{(K^{-1} \mathbf{x}_2)^T (K^{-1} \mathbf{x}_2)}}$$

($\mathbf{d} = K^{-1} \mathbf{x}$)

$$= \frac{\mathbf{x}_1^T (K^{-T} K^{-1}) \mathbf{x}_2}{\sqrt{\mathbf{x}_1^T (K^{-T} K^{-1}) \mathbf{x}_1} \sqrt{\mathbf{x}_2^T (K^{-T} K^{-1}) \mathbf{x}_2}}$$

($\omega = K^{-T}K^{-1}$)

이전에 배웠던 방법으로 2개의 ray 사이의 각도를 구할 수 있는데

 

2개의 ray 사이의 각도 $\alpha$를 구한 후

잘 보면 $s_2$, $s_3$, $a$가 만든 삼각형이 보임

이 삼각형에 제2 코사인 법칙을 적용

$$a^2 = s_2^2 + s_3^2 - 2 s_2 s_3 \cos\alpha$$

$a$, $\cos\alpha$는 알고 있고 $s_2$, $s_3$를 모르니 구해야함

 

마찬가지로 $\beta$ 각도를 구하고

$s_1$, $s_3$, $b$가 만드는 삼각형을 찾을 수 있고

제2 코사인 법칙 적용

$$b^2 = s_1^2 + s_3^2 - 2 s_1 s_3 \cos\beta$$

 

똑같이 1개의 삼각형을 더 찾을 수 있음

$$a^2 = s_2^2 + s_3^2 - 2 s_2 s_3 \cos\alpha$$

$$b^2 = s_1^2 + s_3^2 - 2 s_1 s_3 \cos\beta$$

$$c^2 = s_1^2 + s_2^2 - 2 s_1 s_2 \cos\gamma$$

따라서, $s_1$, $s_2$, $s_3$ 미지수 3개인데 식이 3개이므로 풀 수 있음

조금 복잡하긴한데 치환해서 한 변수에 대한 4차 방정식이 나오고 쭉 풀면 됨

$$ u = \frac{s_2}{s_1} \quad v = \frac{s_3}{s_1}$$

$$A_4 v^4 + A_3 v^3 + A_2 v^2 + A_1 v^1 + A_0 = 0 $$

자세한건 생략하고 풀 수 있다고 생각하고 생략

문제는 풀어보면 4차 방정식의 solution이 4개가 나오는데

4개의 solution 중에 맞는 solution 1개를 골라야함

출처: https://www.ipb.uni-bonn.de/html/teaching/photo12-2021/2021-pho1-23-p3p.pptx.pdf

 

4개의 solution이 나오는 이유는 위에 처럼 가능한 여러 case가 있어서 그럼
3개의 ray 중 1개의 3D point를 이동시켜도 solution을 만족하기 때문에
이게 최소의 correspondence를 쓰는 P3P의 문제

 

EPnP

 

EPnP는 n개의 correspondence가 있을 때 푸는 PnP 중에 하나

가정으로 n개의 3D-2D point 쌍을 알고 있을 때

1개의 plane에 존재하지 않는 4개의 point를 뽑음 → 이 point들을 Control points라고 부름

 

이렇게 뽑으면 나머지 point들은 이 4개의 control points의 linear combination으로 표현이 가능해짐

왜냐하면 4개의 point면 vector를 3개 만들 수 있는데

이 vector 3개가 새로운 축이 되고 이 벡터들의 linear combination으로 나머지 point 다 표현 가능

$$P_{i}^w = \sum_{j=1}^4 \alpha_{ij} C_j^w$$

($w$ : world coordinate라는 의미)

이런식으로 4개의 control points의 weighted sum으로 나머지 3D point들을 전부 표현가능

 

중요한건 linear combination의 weight $\alpha_{ij}$
world coordinate에서 camera coordinate로 바꿔도 이 weight는 바뀌지 않음

$$P_{i}^c = \sum_{j=1}^4 \alpha_{ij} C_j^c$$

($c$ : camera coordinate라는 의미)
우리는 control points의 $C_j^w$인 woorld coordinate만 알고,

$R$, $\mathbf{t}$를 모르기 때문에 camera coordinate인 $C_j^c$는 모름

하지만 $C_j^c$의 linear combination인 $P_{i}^c$의 projection된 2D point들은 알고 있음

그래서 이걸 projection 시키면

$$w_i \underbrace{\begin{bmatrix} u_i \\ v_i \\ 1 \end{bmatrix}}_{\text{2D projection}} = \underbrace{\begin{bmatrix} f_u & 1 & u_c \\ 0 & f_v & v_c \\ 0 & 0 & 1 \end{bmatrix}}_{\text{Intrinsic matrix}} P_i^c$$

$$w_i \begin{bmatrix} u_i \\ v_i \\ 1 \end{bmatrix} = \begin{bmatrix} f_u & 1 & u_c \\ 0 & f_v & v_c \\ 0 & 0 & 1 \end{bmatrix} \sum_{j=1}^4 \alpha_{ij} \begin{bmatrix} x_j^c \\ y_j^c \\ z_j^c \end{bmatrix}$$

$w_i$는 등호($=$)를 만족하기 위해 넣은 scale factor

각 point마다 위의 식이 1개씩 생김, 위 식은 $i$번째 point 식

일단, 위의 식의 왼쪽에 2D projection에서 3번째 row가 1임, 이걸 이용해서 last row만 풀면 이런식이 나옴

$$w_i = \sum_{j=1}^4 \alpha_{ij} z_j^c$$

이거를 다시 식에 넣고 풀면 아래처럼 나옴

$$ \sum_{j=1}^4 \alpha_{ij} f_u x_j^c + \alpha_{ij} (u_c - u_i) z_j^c = 0$$

$$ \sum_{j=1}^4 \alpha_{ij} f_v y_j^c + \alpha_{ij} (v_c - v_i) z_j^c = 0$$

이제 2개의 equation이 나왔으니 DLT를 쓰면됨

$$\begin{bmatrix} 2 \times 3 \text{ matrix} \end{bmatrix} \underbrace{\begin{bmatrix} c_1^c \\ c_2^c \\ c_3^c \\ c_4^c \end{bmatrix}}_{12 \times 1} = 0$$

($c_1^c = \begin{bmatrix} x_1^c \\ y_1^c \\ z_1^c \end{bmatrix}$)

우리가 알고 있는 $\alpha_{ij}$, $f_u$, $f_v$, $u_c$, $u_i$, $v_c$, $v_i$에 대해서는 2x12 matrix에 넣으면 됨

 

1개의 control point당 위처럼 equation이 나오고
$x_1, y_1, z_1$을 묶은게 $c_1$이고 각각의 control point 4개 좌표가 $c_1, c_2, c_3, c_4$

 

만약, n개의 control point면 (2 x 12) matrix가 (2n x 12) matrix가 됨

 

어쨋든 이런식으로 풀면 control point 4개의 camera coordinate를 알게 되면

$$P_{i}^c = \sum_{j=1}^4 \alpha_{ij} C_j^c$$

모든 point들의 camera coordinate 상 좌표를 다 알게되고

 

$P_i^w$와 $P_i^c$ 사이의 관계를 통해 $R$, $\mathbf{t}$를 Rigid Transform 문제랑 같음

(참고: https://dohlab.tistory.com/113)

 

어쨌든 그 문제를 풀면 다음과 같은 식을 풀면 됨

$$(R, t) = \underset{ R \in SO(d), \, \mathbf{t} \in \mathbb{R}^d }{\text{argmin}} \sum_{i=1}^n w_i \left\| (R P_i^w + \mathbf{t}) - P_i^c \right\|^2$$

 

여기까지는 이론이고 실제로는 OpenCV에 다있으니 가져다 쓰면 됨

(참고: https://docs.opencv.org/4.x/d5/d1f/calib3d_solvePnP.html)

 

실제로 써보면 2D-3D correspondence에서 outlier, noise가 있음

이걸 제거해야됨

→ RANSAC 사용

→ iteration이 적어야함

먼저 3개의 correspondence 무작위로 뽑아서 P3P를 함 → $K$, $R$, $\mathbf{t}$ 구하고
inlier를 count ($K$, $R$, $\mathbf{t}$ 가지고 projection 하면 됨)
그다음 reprojection error 계산
이거를 iteration마다 반복
그리고 반복하다보면 inlier 여러개 찾게 되고 찾은 inlier n개를 가지고 EPnP 수행

EPnP 또한 reprojection error를 고려하지 않음 → reprojection error가 적은게 중요
따라서, EPnP로 초기화를 하고

$$R^*, \mathbf{t}^* = \underset{R, \mathbf{t}}{\text{argmin}} \sum_i d(K [R | \mathbf{t}] \mathbf{X}_i, \mathbf{x}_i)$$

위의 Reprojection error 식을 통해 $R$, $\mathbf{t}$를 계속 optimization하면 됨

PnP.pptx

0.06MB