[SLAM] Direct Sparse Odometry (DSO) 논문 및 코드 리뷰 (1)

본 포스트에서는 direct method 기반의 VO 알고리즘으로 유명한 DSO 논문을 리뷰한다. DSO 코드를 분석하면서 논문에서는 생략된 디테일한 부분들이 굉장히 많다는 것을 알게되었고 이미 잘 정리된 다른 분들의 자료를 참고하여 수식 유도부터 코드 리뷰까지 포함하는 정리본을 작성하게 되었다.

 

1. Initialization

1.1 Calibration

direct method는 밝기오차를 최적화하여 카메라의 포즈를 추정하기 때문에 이미지의 밝기 차이에 민감하다. SLAM에서 주로 사용하는 머신비전 카메라는 일반적으로 이미지의 밝기 변화에 따라 노출시간을 자동으로 조절하므로(auto exposure) 노출시간에 따른 밝기 변화가 심한 경우 direct method는 정상적으로 작동하지 않을 수 있다. 아래는 [13] 논문의 예시이다.

Photometric Calibration (위) before (아래) after

1.1.1 Photometric Calibration

따라서 DSO 논문에서는 VO 알고리즘에서 일반적으로 수행하는 geometric calibration과 더불어 photometric calibration이라는 방법을 수행하였다. photometric calibration은 카메라의 노출시간(exposure time), 비네팅(vignetting), 카메라 반응함수 등을 고려하여 direct method가 지속적으로 밝기오차를 최적화할 수 있도록 한다. 자세한 공식은 다음과 같다.

$$\begin{array}{c}{I}_i(\mathbf{x})=G(t_{i}V(\mathbf{x})B_i(\mathbf{x}))\\ I_i^{\prime }(\mathbf{x}):=t_i{B}_i(\mathbf{x})=\frac{G^{-1}(I_i(\mathbf{x}))}{V(\mathbf{x})}\end{array}$$

- $G(\cdot )$ : 카메라 노출시간에 따른 반응함수 (response function)

- $t_i$ : 노출시간 (exposure time)

- $V(\cdot )$ : 비네팅 (vignetting)

- $B(\cdot )$ : 복사 조도 (irradiance)

- $I(\cdot )$ : 관측된 픽셀 밝기

- $I^{{}^{\prime }}(\cdot )$ : 캘리브레이션 된 픽셀 밝기

 

$B(\cdot )$ 에 대한 자세한 설명은 링크1, 링크2를 참고하면 된다. 우선 같은 장소에 대해 다른 노출시간을 적용하면서 카메라의 노출시간에 따른 반응함수 $G(\cdot )$와 복사 조도 $B(\cdot )$을 추정한다. 해당 방법은 DSO Photometric Calibration paper 를 참조하여 작성하였다. 

Response Calibration. Where U := G^-1

다음으로 $V(\cdot )$을 추정한다. 벽에 AR marker를 설치하여 카메라의 포즈를 추정하면서 이미지 중앙을 기점으로 주변의 어두워지는 정도를 함수로 예측한다.

DSO는 위와 같은 photometric calibration 과정을 거쳐서 노출시간 변화에 상대적으로 invarant한 이미지 영상 $I^{\prime }(\cdot )$을 사용하였다.  

DSO's Photometric Calibration

위 그림에서 중간의 그래프를 보면 이미지 시퀀스에 따른 노출시간의 변화를 의미한다. 값을 보면 동일한 데이터 내에서도 0.018ms ~ 10.5ms까지  최대 500배 이상의 노출시간 변화가 발생하는 것을 알 수 있다. DSO는 이러한 카메라 노출시간 변화에 따른 밝기 변화를 에러함수로 모델링하지 않고 캘리브레이션함으로써 direct method의 포즈 추정 성능을 대폭 상승시켰다. 

 

1.2 Error Function Formulation

다음과 같이 3차원 공간 상의 한 점 ${\mathbf{X}}_1$과 2차원 이미지 평면 상의 한 점 ${\mathbf{p}}_1$은 다음과 같은 관계를 가진다. 

$${\mathbf{p}}_1=\pi ({\mathbf{X}}_{\mathbf{1}})=\mathbf{K}{\rho }_1{\mathbf{X}}_1$$

$${\mathbf{X}}_1={\pi }^{-1}({\mathbf{p}}_{\mathbf{1}})={\mathbf{K}}^{-1}/{\rho }_1{\mathbf{p}}_1$$

 

- $\{{\mathbf{C}}_i\}$ : 카메라 좌표계

- ${\mathbf{X}}_1$ : 3차원 공간 상의 한 점

- ${\mathbf{p}}_1$ : 2차원 이미지 평면 상의 한 점

- $\mathbf{K}$ : 카메라 내부 파라미터 (3x3 행렬)

- ${\rho }_1=1/Z_1$ : 깊이 값의 역수 (inverse depth)

- $\mathbf{K}=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right],{\mathbf{K}}^{-1}=\left[\begin{array}{ccc}{f}_x^{-1}& 0& -f_x^{-1}{c}_x\\ 0& f_y^{-1}& -f_y^{-1}{c}_y\\ 0& 0& 1\end{array}\right]$

 

1.2.1 inverse depth parameterization

DSO에서는 3차원 점 ${\mathbf{X}}_1$를 표현할 때 3개의 파라미터 $[X_1,Y_1,Z_1]$을 사용하는 것이 아닌 1개의 파라미터 ($Z_1$의 역수 ${\rho }_1$)만 사용한다. 이를 통해 이미지 평면 상의 픽셀 ${\mathbf{p}}_1$의 위치만 알고 있으면 오직 inverse depth ${\rho }_1$를 사용하여 3차원 점을 완벽하게 표현할 수 있다. 이는 최적화를 수행할 때 1개의 파라미터만 추정하면 되므로 계산 상의 이점을 지닌다. 

 

실제 inverse depth 관련 논문[14]에서는 inverse depth를 사용하면 기존 3차원 공간의 3개 파라미터 대신 6개의 파라미터를 사용한다.

$$[X,Y,Z]\to [x,y,z,\rho ,\theta ,\varphi ]$$

하지만, DSO 논문에서는 $\rho$만 차용하여 사용한 것으로 추정된다. 이 때, inverse depth의 성질에 따라 $\rho$ 값은 레퍼런스 프레임의 $\rho$ 값으로만 제한된다. inverse depth를 통해 얻는 이점에 대한 자세한 내용은 해당 블로그를 참조하면 된다.

 

 

---

 

 

다음으로 카메라가 $\exp ({\xi }_{21}^{\wedge })\in \text{SE}(3)$만큼 움직여서 $\{C_1\}\to \{C_2\}$으로 이동한 경우를 생각해보자. 

- ${\mathbf{T}}_i$ : 카메라의 3차원 포즈 (4x4 행렬)

- ${\xi }_{21}\in {\mathbb{R}}^6$ : C1, C2 카메라 간 상대 포즈 변화량 (twist) (6차원 벡터)

- ${\xi }_{21}^{\wedge }\in \text{se}(3)$ : hat 연산자가 적용된 skew symmetric matrix of twist (lie algebra) (4x4 행렬)

- $\exp ({\xi }_{21}^{\wedge })\in \text{SE}(3)$ : C1, C2 카메라 간 상대 포즈 (lie group) (4x4 행렬)

 

${\mathbf{X}}_1$을 두 번째 카메라 위에 프로젝션한 점 ${\mathbf{p}}_2$는 다음과 같은 공식으로 표현할 수 있다. 
$${\mathbf{p}}_2=\pi (\exp ({\xi }_{21}^{\wedge }){\mathbf{X}}_1)=\pi (\exp ({\xi }_{21}^{\wedge }){\pi }^{-1}({\mathbf{p}}_1))$$

 

위 식을 inverse depth ${\rho }_1$, 회전행렬 $\mathbf{R}$과 이동량 $\mathbf{t}$으로 나타내면 논문의 Eq (5) 공식이 된다.

$${\mathbf{p}}_2=\pi (\mathbf{R}{\pi }^{-1}({\mathbf{p}}_1,{\rho }_1)+\mathbf{t})$$

- $\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0& 1\end{array}\right]:={\mathbf{T}}_2{\mathbf{T}}_1^{-1}$

 

direct method는 밝기 오차 (photometric error)를 에러함수로 사용하므로 픽셀의 밝기 값의 차이를 구해야 한다. 이 때, DSO는 카메라의 exposure time을 고려하여 밝기 오차를 조정하는 파라미터를 사용한다. 이를 photometric calibration parameters (a,b)라고 한다. 또는 affine brightness transfer parameters (a,b)라고도 부른다.

$${\mathbf{I}}_2({\mathbf{p}}_2)=\exp (a){\mathbf{I}}_1({\mathbf{p}}_1)+b$$

 

- $a=\frac{t_j\exp (a_j)}{t_i\exp (a_i)}$

- $b=b_i-b_j$

- ${\mathbf{I}}_i({\mathbf{p}}_j)$ : i번째 이미지에서 j번째 점의 밝기 (grayscale) (0~255)

- $t_i$ : i번째 이미지가 촬영된 순간의 exposure time ([ms] 단위)

- $a,b$ : 두 이미지 간 exposure time을 고려하여 이미지 밝기를 조절하는 파라미터

 

이 때, 두 점의 픽셀 밝기 차이를 에러(error, residual)로 설정한다.

$$\mathbf{r}({\mathbf{p}}_1)={\mathbf{I}}_2({\mathbf{p}}_2)-\exp (a){\mathbf{I}}_1({\mathbf{p}}_1)-b$$

DSO는 한 점에서 밝기 차이만을 고려하지 않고 한 점 주위의 총 8개의 점들 patch 밝기 차이를 고려한다. 위 이미지와 같이 1개의 점이 생략된 이유는 성능에 크게 영향을 주지 않으면서 4번의 float 연산을 한 번에 계산할 수 있는 SSE2 레지스터를 사용하여 속도를 가속하기 위함이다.

 

patch를 고려하고 weighting 함수 + huber 함수까지 적용한 에러함수 ${\mathbf{E}}_p$는 다음과 같다. 이 때, patch는 weighted SSD (sum of squared distance)를 사용하여 계산하였다. 

$${\mathbf{E}}_p=\sum _{{\mathbf{p}}_1\in {\mathcal{N}}_{\mathbf{p}}}{w}_p{\Vert {\mathbf{I}}_2({\mathbf{p}}_2)-\exp (a){\mathbf{I}}_1({\mathbf{p}}_1)-b\Vert }_{\gamma }$$

- ${\mathcal{N}}_{\mathbf{p}}$ : 중앙 점과 주위의 8개 점들 patch

 

위 공식을 풀어서 다시 표현하면 논문의 Eq (4) 공식이 된다.

$${\mathbf{E}}_p=\sum _{{\mathbf{p}}_1\in {\mathcal{N}}_{\mathbf{p}}}{w}_p{\Vert ({\mathbf{I}}_2({\mathbf{p}}_2)-b_2)-\frac{t_2{\exp }^{a_2}}{t_1{\exp }^{a_1}}({\mathbf{I}}_1({\mathbf{p}}_1)-b_1)\Vert }_{\gamma }=\sum _{{\mathbf{p}}_1\in {\mathcal{N}}_{\mathbf{p}}}{w}_p\text{H}(\mathbf{r}({\mathbf{p}}_1))$$

 

임의의 weighting 함수 $w_p$는 다음과 같이 정의되고 이미지 gradient가 큰 포인트의 경우 down weighting 된다. (논문의 Eq (7) 공식)

$$w_p=\frac{c^2}{c^2+{\Vert \mathrm{\nabla }\mathbf{I}(\mathbf{p})\Vert }_2^2}$$

 

huber function $\text{H}(\mathbf{r})$는 다음과 같이 정의되고 DSO의 경우 $\sigma =9$의 상수값을 가진다.

$$\text{H}(\mathbf{r})=\{\begin{array}{cc}{\mathbf{r}}^2/2,& \left|\mathbf{r}\right|<\sigma \\ \sigma (\left|\mathbf{r}\right|-\sigma /2)& \left|\mathbf{r}\right|\ge \sigma \end{array}$$

 

위 식을 컴팩트하게 표현하기 위해 $w_h=\{\begin{array}{cc}1& \left|\mathbf{r}\right|<\sigma \\ \sigma /\left|\mathbf{r}\right|& \left|\mathbf{r}\right|\ge \sigma \end{array}$를 사용하면 아래와 같이 나타낼 수 있다. 

 

$$\text{H}(\mathbf{r})=w_h{\mathbf{r}}^2(1-w_h/2)$$

 

이를 두 카메라 이미지 상의 모든 점들에 대해 에러를 계산하면 아래와 같다. (논문의 Eq (8) 공식)

$${\mathbf{E}}_{photo}:=\sum _{i\in \mathcal{F}}\sum _{\mathbf{p}\in {\mathcal{P}}_i}\sum _{j\in obs(\mathbf{p})}{\mathbf{E}}_p$$

- $\mathcal{F}$ : 모든 프레임

- ${\mathcal{P}}_i$ : i번째 프레임의 모든 포인트

- $obs(\mathbf{p})$ : 관측된 모든 포인트

1.3 Gauss-Newton Optimization

huber function까지 적용한 에러함수 $\text{H}(\mathbf{r})$은 다음과 같다. 

$$\text{H}(\mathbf{r})=w_h{\mathbf{r}}^2(1-w_h/2)$$

이 때, DSO에서 $\text{H}(\mathbf{r})$의 $-w_h/2$ 부분은 상대적으로 크기가 작으므로 생략하였다. 

$$\text{H}(\mathbf{r})=w_h{\mathbf{r}}^2(1-w_h/2)\simeq w_h{\mathbf{r}}^2$$

 

위 함수를 least square method로 최적화하기 위해서 $\text{H}(\mathbf{r})$가 squared form이라고 가정하면 $f(\mathbf{x}{)}^{T}f(\mathbf{x})=\text{H}(\mathbf{r})$를 만족하는 $f(\mathbf{x})$를 생각할 수 있고 이 때, $f(\mathbf{x})$가 최소이면 $f(\mathbf{x})$의 제곱인 $\text{H}(\mathbf{r})$ 역시 최소가 된다. 

 

$$f(\mathbf{x}{)}^{T}f(\mathbf{x})=\text{H}(\mathbf{r})$$

$$f(\mathbf{x})=\sqrt{w_h}\mathbf{r}=\sqrt{w_h}({\mathbf{I}}_2({\mathbf{p}}_2)-\exp (a){\mathbf{I}}_1({\mathbf{p}}_1)-b)$$

 

따라서 해당 문제를 $f(\mathbf{x})$ 최소화 문제로 변경할 수 있고 이 때 state variable $\mathbf{x}$는 다음과 같다. 

$$\mathbf{x}=[{\rho }_1^{(1)},\cdots ,{\rho }_1^{(N)},\delta {\xi }^T,a,b{]}_{(N+8)\times 1}^T$$

 

- ${\rho }_1^{(i)}$ : 1번 이미지에서 i번째 inverse depth 값 (N개)

- $\delta \xi$ : 두 카메라 간 상대 포즈 변화량 (twist) (6차원 벡터) (6개)

- $a,b$ : 두 이미지 간 exposure time을 고려하여 이미지 밝기를 조절하는 파라미터 (2개)

 

$f(\mathbf{x})$는 비선형 함수이므로 non-linear least square method를 사용하여 최소화해야 한다. 이를 위해 Gauss-Newton(GN) method가 사용된다. GN은 반복적으로(iterative)  $f(\mathbf{x}+\mathrm{\Delta }\mathbf{x})$ 값이 감소하는 $\mathrm{\Delta }\mathbf{x}$를 구하여 비선형 함수를 최적화하는 방법 중 하나이다.

1.3.1. Gauss-Newton method

자세한 과정은 다음과 같다.

1. $f(\mathbf{x}+\mathrm{\Delta }\mathbf{x})$를 1차 테일러 전개하면 다음과 같이 근사할 수 있다. 

$$f(\mathbf{x}+\mathrm{\Delta }\mathbf{x})\simeq f(\mathbf{x})+\mathbf{J}\mathrm{\Delta }\mathbf{x}$$

2. $\frac{1}{2}{f}^2(\mathbf{x}+\mathrm{\Delta }\mathbf{x})$의 최소값을 구한다고 가정하면 해당 함수의 1차 미분이 0이 되는 $\mathrm{\Delta }\mathbf{x}$를 구하면 그 점이 최소가 된다. 

$$f(\mathbf{x}+\mathrm{\Delta }\mathbf{x}{)}^{T}f(\mathbf{x}+\mathrm{\Delta }\mathbf{x})\simeq (f(\mathbf{x})+\mathbf{J}\mathrm{\Delta }\mathbf{x}{)}^T(f(\mathbf{x})+\mathbf{J}\mathrm{\Delta }\mathbf{x})$$

$$=f(\mathbf{x}{)}^{T}f(\mathbf{x})+2f(\mathbf{x}{)}^T\mathbf{J}\mathrm{\Delta }\mathbf{x}+\mathrm{\Delta }{\mathbf{x}}^T{\mathbf{J}}^T\mathbf{J}\mathrm{\Delta }\mathbf{x}$$

$$=\mathbf{c}+2\mathbf{b}\mathrm{\Delta }\mathbf{x}+\mathrm{\Delta }{\mathbf{x}}^T\mathbf{H}\mathrm{\Delta }\mathbf{x}$$

3. $\frac{1}{2}{f}^2(\mathbf{x}+\mathrm{\Delta }\mathbf{x})$의 최소값은 다음과 같이 구할 수 있다. 

$$\frac{\partial \frac{1}{2}{f}^2(\mathbf{x}+\mathrm{\Delta }\mathbf{x})}{\partial \mathrm{\Delta }\mathbf{x}}=f(\mathbf{x}+\mathrm{\Delta }\mathbf{x})\frac{\partial f(\mathbf{x}+\mathrm{\Delta }\mathbf{x})}{\partial \mathrm{\Delta }\mathbf{x}}\simeq (f(\mathbf{x})+\mathbf{J}\mathrm{\Delta }\mathbf{x})\mathbf{J}=0$$

4. 위 식을 정리하면 다음과 같은 식이 나오고 

$${\mathbf{J}}^T\mathbf{J}\mathrm{\Delta }\mathbf{x}=-{\mathbf{J}}^{T}f(\mathbf{x})$$

5. 이를 컴팩트하게 표현하기 위해 치환하면 다음과 같다. 

 $$\mathbf{H}\mathrm{\Delta }\mathbf{x}=-\mathbf{b}$$

- $\mathbf{H}=\sum {\mathbf{J}}^T\mathbf{J}$

- $\mathbf{b}=\sum {\mathbf{J}}^{T}f(\mathbf{x})$

6. 위 식을 계산하면 최적의 $\mathrm{\Delta }\mathbf{x}$를 계산할 수 있고 이를 기존의 state variable에 업데이트 해준다.

$$\mathrm{\Delta }{\mathbf{x}}^{\ast }=-{\mathbf{H}}^{-1}\mathbf{b}$$

$$\therefore \mathbf{x}\leftarrow \mathbf{x}+\mathrm{\Delta }{\mathbf{x}}^{\ast }$$

1.4 Jacobian Derivation

앞서 언급한 GN 방법을 사용하여 최적화를 하기 위해서는 Jacobian $\mathbf{J}$ 계산이 선행되어야 한다.

$$\because f(\mathbf{x}+\mathrm{\Delta }\mathbf{x})\simeq f(\mathbf{x})+\mathbf{J}\mathrm{\Delta }\mathbf{x}$$

 

$f(\mathbf{x})$의 state variable $\mathbf{x}$를 다음과 같이 정의했으므로 총 N+8개의 변수에 대한 Jacobian을 계산해야 한다. (idepth(N), relative pose(6), photometric param(2))

$$\mathbf{x}=[{\rho }_1^{(1)},\cdots ,{\rho }_1^{(N)},\delta {\xi }^T,a,b{]}_{(N+8)\times 1}^T$$

 

1.4.1 Derivation of photometric parameters

photometric param a,b에 대한 Jacobian은 다음과 같이 계산할 수 있다.

$$\frac{\partial f(\mathbf{x})}{\partial a}=-\sqrt{w_h}\exp (a){\mathbf{I}}_1({\mathbf{p}}_1)$$

$$\frac{\partial f(\mathbf{x})}{\partial b}=-\sqrt{w_h}$$

 

- $f(\mathbf{x})=\sqrt{w_h}\mathbf{r}=\sqrt{w_h}({\mathbf{I}}_2({\mathbf{p}}_2)-\exp (a){\mathbf{I}}_1({\mathbf{p}}_1)-b)$

 

1.4.2 Derivation of relative pose

에러함수 $f(\mathbf{x})$가 카메라 포즈 변화량 $\delta \xi$에 따라서 어떻게 변하는 지 계산하기 위해 $\frac{\partial f(\mathbf{x})}{\partial \delta \xi }$를 구해야 한다. 다음과 같이 Jacobian 연쇄법칙을 사용하여 $\frac{\partial f(\mathbf{x})}{\partial \delta \xi }$를 유도할 수 있다. 이를 위해 개별 Jacobian들을 모두 구해야 한다.

 

$$\frac{\partial f(\mathbf{x})}{\partial \delta \xi }=\frac{\partial f(\mathbf{x})}{\partial {\mathbf{p}}_2}\frac{\partial {\mathbf{p}}_2}{\partial \delta \xi }=\frac{\partial f(\mathbf{x})}{\partial {\mathbf{p}}_2}\frac{\partial {\mathbf{p}}_2}{\partial {\mathbf{X}}_2}\frac{\partial {\mathbf{X}}_2}{\partial \delta \xi }$$

 

$\mathrm{\nabla }{\mathbf{I}}_x,\mathrm{\nabla }{\mathbf{I}}_y$를 ${\mathbf{p}}_2$에서 각각 x,y 방향의 이미지 gradient라고 하면 $\frac{\partial f(\mathbf{x})}{\partial {\mathbf{p}}_2}$는 다음과 같이 구할 수 있다. 

$$\frac{\partial f(\mathbf{x})}{\partial {\mathbf{p}}_2}=\sqrt{w_h}\frac{\partial {\mathbf{I}}_{\mathbf{2}}({\mathbf{p}}_{\mathbf{2}})}{\partial {\mathbf{p}}_2}=\sqrt{w_h}[\mathrm{\nabla }{\mathbf{I}}_x\mathrm{\nabla }{\mathbf{I}}_y]$$

 

다음으로 $\frac{\partial {\mathbf{p}}_2}{\partial \delta \xi }=\frac{\partial {\mathbf{p}}_2}{\partial {\mathbf{X}}_2}\frac{\partial {\mathbf{X}}_2}{\partial \delta \xi }$를 구해보면, 우선 $\frac{\partial {\mathbf{p}}_2}{\partial {\mathbf{X}}_2}$는 2차원 이미지 평면 상의 점 ${\mathbf{p}}_2$와 3차원 공간 상의 점 ${\mathbf{X}}_2$ 사이의 관계를 의미하므로 두 점 사이에는 다음과 같은 공식이 성립한다.

$$\left[\begin{array}{c}{u}_2\\ v_2\\ 1\end{array}\right]={\rho }_2\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{X}_2\\ Y_2\\ Z_2\end{array}\right]$$

 

- ${\rho }_2=\frac{1}{Z_2},{\mathbf{X}}_2=[X_2{Y}_2{Z}_2{]}^T$

 

위 식을 통해 $u_2,v_2,X_2,Y_2,Z_2$ 관계식을 도출할 수 있고 이를 토대로 $\frac{\partial {\mathbf{p}}_2}{\partial {\mathbf{X}}_2}$를 구해보면 아래와 같다. 

$$\frac{\partial {\mathbf{p}}_2}{\partial {\mathbf{X}}_2}=\left[\begin{array}{ccc}\frac{\partial u_2}{\partial X_2}& \frac{\partial u_2}{\partial Y_2}& \frac{\partial u_2}{\partial Z_2}\\ \frac{\partial v_2}{\partial X_2}& \frac{\partial v_2}{\partial Y_2}& \frac{\partial v_2}{\partial Z_2}\end{array}\right]=\left[\begin{array}{ccc}{\rho }_2{f}_x& 0& -{\rho }_2^2{f}_x{X}_2\\ 0& {\rho }_2{f}_y& -{\rho }_2^2{f}_y{Y}_2\end{array}\right]$$

 

다음으로 $\frac{\partial {\mathbf{p}}_2}{\partial \delta \xi }=\frac{\partial {\mathbf{p}}_2}{\partial {\mathbf{X}}_2}\frac{\partial {\mathbf{X}}_2}{\partial \delta \xi }$에서 $\frac{\partial {\mathbf{X}}_2}{\partial \delta \xi }$를 구하기 전 $\delta \xi$에 대해 알아보면 $\delta \xi \in {\mathbb{R}}^6$는 3차원 카메라의 포즈 변화량을 의미하고 twist라고 부른다. $\delta {\xi }^{\wedge }\in \text{se}(3)$는 twist에 hat operator를 적용하여 skew symmetric matrix로 변환한 값으로써 lie algebra of SE(3)라고 부른다. 위 값에 exponential mapping을 적용하면 4x4 행렬의 3차원 포즈 $\mathrm{\Delta }\mathbf{T}\in \text{SE}(3)$로 변환할 수 있다. 이를 lie group of SE(3)라고 부른다. 자세하게 표현하면 다음과 같다. 

$$\delta \xi =\left[\begin{array}{c}\delta \mathbf{v}\\ \delta \mathbf{w}\end{array}\right]=\left[\begin{array}{c}\delta v_x\\ \delta v_y\\ \delta v_z\\ \delta w_x\\ \delta w_y\\ \delta w_z\end{array}\right]\in {\mathbb{R}}^6$$

$$\delta {\xi }^{\wedge }=\left[\begin{array}{cc}\delta {\mathbf{w}}^{\wedge }& \delta \mathbf{v}\\ {\mathbf{0}}^T& 0\end{array}\right]\in se(3)\in {\mathbb{R}}^{4\times 4}$$

$$\mathrm{\Delta }\mathbf{T}=\exp (\delta {\xi }^{\wedge })=\left[\begin{array}{cc}\mathrm{\Delta }\mathbf{R}& \mathrm{\Delta }\mathbf{t}\\ \mathbf{0}& 1\end{array}\right]\in \text{SE}(3)\in {\mathbb{R}}^{4\times 4}$$

 

이를 바탕으로 $\frac{\partial {\mathbf{X}}_2}{\partial \delta \xi }$를 유도해보면 다음과 같다. 

$$\begin{array}{rl}\frac{\partial {\mathbf{X}}_2}{\partial \delta \xi }& =\underset{\delta \xi \to 0}{lim}\frac{\exp (\delta {\xi }^{\wedge }){\mathbf{X}}_2-{\mathbf{X}}_2}{\delta \xi }\\ & \simeq \underset{\delta \xi \to 0}{lim}\frac{(\mathbf{I}+\delta {\xi }^{\wedge }){\mathbf{X}}_2-{\mathbf{X}}_2}{\delta \xi }\\ & =\underset{\delta \xi \to 0}{lim}\frac{\delta {\xi }^{\wedge }{\mathbf{X}}_2}{\delta \xi }\\ & =\underset{\delta \xi \to 0}{lim}\frac{\left[\begin{array}{cc}\delta {\mathbf{w}}^{\wedge }& \delta \mathbf{v}\\ {\mathbf{0}}^T& 0\end{array}\right]\left[\begin{array}{c}{\mathbf{X}}_2\\ 1\end{array}\right]}{\delta \xi }\\ & =\left[\begin{array}{cc}\mathbf{I}& -{\mathbf{X}}_2^{\wedge }\\ {\mathbf{0}}^T& {\mathbf{0}}^T\end{array}\right]\end{array}$$

$$\therefore \frac{\partial {\mathbf{X}}_2}{\partial \delta \xi }=[\mathbf{I}-{\mathbf{X}}_2^{\wedge }]=\left[\begin{array}{cccccc}1& 0& 0& 0& Z_2& Y_2\\ 0& 1& 0& -Z_2& 0& X_2\\ 0& 0& 1& Y_2& -X_2& 0\end{array}\right]\text{in non-homogeneous form}$$

지금까지 유도한 공식들을 결합하여 하나의 Jacobian of relative pose로 표현하면 다음과 같다. 

$$\frac{\partial f(\mathbf{x})}{\partial \delta \xi }=\frac{\partial f(\mathbf{x})}{\partial {\mathbf{p}}_2}\frac{\partial {\mathbf{p}}_2}{\partial \delta \xi }=\frac{\partial f(\mathbf{x})}{\partial {\mathbf{p}}_2}\frac{\partial {\mathbf{p}}_2}{\partial {\mathbf{X}}_2}\frac{\partial {\mathbf{X}}_2}{\partial \delta \xi }$$

$$\frac{\partial f(\mathbf{x})}{\partial {\mathbf{p}}_2}=\sqrt{w_h}\frac{\partial {\mathbf{I}}_{\mathbf{2}}({\mathbf{p}}_{\mathbf{2}})}{\partial {\mathbf{p}}_2}=\sqrt{w_h}[\mathrm{\nabla }{\mathbf{I}}_x\mathrm{\nabla }{\mathbf{I}}_y]$$

$$\begin{array}{rl}\frac{\partial {\mathbf{p}}_2}{\partial \delta \xi }& =\left[\begin{array}{cccccc}{\rho }_2{f}_x& 0& -{\rho }_2^2{f}_x{X}_2& -{\rho }_2^2{f}_x{X}_2{Y}_2& f_x+{\rho }_2^2{f}_x{X}_2^2& -{\rho }_2{f}_x{Y}_2\\ 0& {\rho }_2{f}_y& -{\rho }_2^2{f}_y{Y}_2& -f_y-{\rho }_2^2{f}_y{Y}_2^2& {\rho }_2^2{f}_y{X}_2{Y}_2& {\rho }_2{f}_y{X}_2\end{array}\right]\\ \\ & =\left[\begin{array}{cccccc}{\rho }_2{f}_x& 0& -{\rho }_2^2{f}_x{u}_2^{{}^{\prime }}& -f_x{u}_2^{{}^{\prime }}{v}_2^{{}^{\prime }}& f_x+f_x{u}_2^{{}^{\prime }2}& -f_x{v}_2^{{}^{\prime }}\\ 0& {\rho }_2{f}_y& -{\rho }_2^2{f}_y{v}_2^{{}^{\prime }}& -f_y-f_y{v}_2^{{}^{\prime }2}& f_y{u}_2^{{}^{\prime }}{v}_2^{{}^{\prime }}& f_y{u}_2^{{}^{\prime }}\end{array}\right]\end{array}$$

- $u^{{}^{\prime }}=\frac{X_2}{Z_2},v^{{}^{\prime }}=\frac{Y_2}{Z_2}\text{in the normalized coordinate.}$ 

$$\therefore \frac{\partial f(\mathbf{x})}{\partial \delta \xi }=\sqrt{w_h}{\left[\begin{array}{c}\mathrm{\nabla }{\mathbf{I}}_x{\rho }_2{f}_x\\ \mathrm{\nabla }{\mathbf{I}}_y{\rho }_2{f}_y\\ -{\rho }_2(\mathrm{\nabla }{\mathbf{I}}_x{f}_x{u}_2^{{}^{\prime }}+\mathrm{\nabla }{\mathbf{I}}_y{f}_y{v}_2^{{}^{\prime }})\\ -\mathrm{\nabla }{\mathbf{I}}_x{f}_x{u}_2^{{}^{\prime }}{v}_2^{{}^{\prime }}-\mathrm{\nabla }{\mathbf{I}}_y{f}_y(1+v_2^{{}^{\prime }2})\\ \mathrm{\nabla }{\mathbf{I}}_x{f}_x(1+u_2^{{}^{\prime }2})+\mathrm{\nabla }{\mathbf{I}}_y{f}_y{u}_2^{{}^{\prime }}{v}_2^{{}^{\prime }}\\ -\mathrm{\nabla }{\mathbf{I}}_x{f}_x{v}_2^{{}^{\prime }}+\mathrm{\nabla }{\mathbf{I}}_y{f}_y{u}_2^{{}^{\prime }}\end{array}\right]}^T$$

 

1.4.3 Derivation of inverse depth

마지막으로 Jacobian of inverse depth $\frac{\partial f(\mathbf{x})}{\partial {\rho }_1}$를 유도해야 한다. $\frac{\partial f(\mathbf{x})}{\partial {\rho }_1}$는 다음과 같이 연쇄적인 Jacobian의 곱으로 표현할 수 있다. 

$$\frac{\partial f(\mathbf{x})}{\partial {\rho }_1}=\frac{\partial f(\mathbf{x})}{\partial {\mathbf{p}}_2}\frac{\partial {\mathbf{p}}_2}{\partial {\rho }_1}=\frac{\partial f(\mathbf{x})}{\partial {\mathbf{p}}_2}\frac{\partial {\mathbf{p}}_2}{\partial {\mathbf{X}}_2}\frac{\partial {\mathbf{X}}_2}{\partial {\rho }_1}$$

 

이 때, $\frac{\partial f(\mathbf{x})}{\partial {\mathbf{p}}_2}\frac{\partial {\mathbf{p}}_2}{\partial {\mathbf{X}}_2}$는 이전에 미리 구했으므로 $\frac{\partial {\mathbf{X}}_2}{\partial {\rho }_1}$만 추가적으로 구하면 된다. 

$$\frac{\partial f(\mathbf{x})}{\partial {\mathbf{p}}_2}=\sqrt{w_h}\frac{\partial {\mathbf{I}}_{\mathbf{2}}({\mathbf{p}}_{\mathbf{2}})}{\partial {\mathbf{p}}_2}=\sqrt{w_h}[\mathrm{\nabla }{\mathbf{I}}_x\mathrm{\nabla }{\mathbf{I}}_y]$$

$$\frac{\partial {\mathbf{p}}_2}{\partial {\mathbf{X}}_2}=\left[\begin{array}{ccc}\frac{\partial u_2}{\partial X_2}& \frac{\partial u_2}{\partial Y_2}& \frac{\partial u_2}{\partial Z_2}\\ \frac{\partial v_2}{\partial X_2}& \frac{\partial v_2}{\partial Y_2}& \frac{\partial v_2}{\partial Z_2}\end{array}\right]=\left[\begin{array}{ccc}{\rho }_2{f}_x& 0& -{\rho }_2^2{f}_x{X}_2\\ 0& {\rho }_2{f}_y& -{\rho }_2^2{f}_y{Y}_2\end{array}\right]$$

$$\frac{\partial {\mathbf{X}}_2}{\partial {\rho }_1}=?$$

 

$\frac{\partial {\mathbf{p}}_2}{\partial {\rho }_1}=\frac{\partial {\mathbf{p}}_2}{\partial {\mathbf{X}}_2}\frac{\partial {\mathbf{X}}_2}{\partial {\rho }_1}$에서 $\frac{\partial {\mathbf{X}}_2}{\partial {\rho }_1}$는 3차원 공간 상의 점 ${\mathbf{X}}_2$와 inverse depth ${\rho }_1$ 사이의 관계를 나타내므로 이를 공식으로 표현하면 다음과 같다. 
$${\mathbf{X}}_2={\mathbf{R}}_{21}(\frac{{\mathbf{K}}^{-1}{\mathbf{X}}_1}{{\rho }_1})+{\mathbf{t}}_{21}$$

 

위 관계식을 사용하여 $\frac{\partial {\mathbf{X}}_2}{\partial {\rho }_1}$를 구해보면 다음과 같다.

$$\begin{array}{rl}\frac{\partial {\mathbf{X}}_2}{\partial {\rho }_1}=-{\mathbf{R}}_{21}(\frac{{\mathbf{K}}^{-1}{\mathbf{X}}_1}{{\rho }_1^2})=-\frac{({\mathbf{X}}_2-{\mathbf{t}}_{21})}{{\rho }_1}& =-{\rho }_1^{-1}[X_2-t_x{Y}_2-t_y{Z}_2-t_z{]}^T\end{array}$$

 

$\frac{\partial {\mathbf{p}}_2}{\partial {\rho }_1}$는 다음과 같이 구할 수 있다.

$$\begin{array}{rl}\frac{\partial {\mathbf{p}}_2}{\partial {\rho }_1}=& =-{\rho }_1^{-1}{\rho }_2\left[\begin{array}{c}{f}_x(u_2^{{}^{\prime }}{t}_z-t_x)\\ f_y(v_2^{{}^{\prime }}{t}_z-t_y)\end{array}\right]\end{array}$$

 

 

따라서 $\frac{\partial f(\mathbf{x})}{\partial {\rho }_1}=\frac{\partial f(\mathbf{x})}{\partial {\mathbf{p}}_2}\frac{\partial {\mathbf{p}}_2}{\partial {\rho }_1}=\frac{\partial f(\mathbf{x})}{\partial {\mathbf{p}}_2}\frac{\partial {\mathbf{p}}_2}{\partial {\mathbf{X}}_2}\frac{\partial {\mathbf{X}}_2}{\partial {\rho }_1}$에서 $\frac{\partial f(\mathbf{x})}{\partial {\rho }_1}$는 다음과 같이 구할 수 있다. 

$$\therefore \frac{\partial f(\mathbf{x})}{\partial {\rho }_1}=\sqrt{w_h}{\rho }_1^{-1}{\rho }_2(\mathrm{\nabla }{\mathbf{I}}_x{f}_x(t_x-u_2^{{}^{\prime }}{t}_z)+\mathrm{\nabla }{\mathbf{I}}_y{f}_y(t_y-v_2^{{}^{\prime }}{t}_z))$$

 

정리해보면 Initialization에서 사용하는 Jacobian들은 아래와 같다. 

idepth(N)

$$\frac{\partial f(\mathbf{x})}{\partial {\rho }_1}=\sqrt{w_h}{\rho }_1^{-1}{\rho }_2(\mathrm{\nabla }{\mathbf{I}}_x{f}_x(t_x-u_2^{{}^{\prime }}{t}_z)+\mathrm{\nabla }{\mathbf{I}}_y{f}_y(t_y-v_2^{{}^{\prime }}{t}_z))$$

pose(6)

$$\frac{\partial f(\mathbf{x})}{\partial \delta \xi }=\sqrt{w_h}{\left[\begin{array}{c}\mathrm{\nabla }{\mathbf{I}}_x{\rho }_2{f}_x\\ \mathrm{\nabla }{\mathbf{I}}_y{\rho }_2{f}_y\\ -{\rho }_2(\mathrm{\nabla }{\mathbf{I}}_x{f}_x{u}_2^{{}^{\prime }}+\mathrm{\nabla }{\mathbf{I}}_y{f}_y{v}_2^{{}^{\prime }})\\ -\mathrm{\nabla }{\mathbf{I}}_x{f}_x{u}_2^{{}^{\prime }}{v}_2^{{}^{\prime }}-\mathrm{\nabla }{\mathbf{I}}_y{f}_y(1+v_2^{{}^{\prime }2})\\ \mathrm{\nabla }{\mathbf{I}}_x{f}_x(1+u_2^{{}^{\prime }2})+\mathrm{\nabla }{\mathbf{I}}_y{f}_y{u}_2^{{}^{\prime }}{v}_2^{{}^{\prime }}\\ -\mathrm{\nabla }{\mathbf{I}}_x{f}_x{v}_2^{{}^{\prime }}+\mathrm{\nabla }{\mathbf{I}}_y{f}_y{u}_2^{{}^{\prime }}\end{array}\right]}^T$$

photometric(2)

$$\frac{\partial f(\mathbf{x})}{\partial a}=-\sqrt{w_h}\exp (a){\mathbf{I}}_1({\mathbf{p}}_1)$$

$$\frac{\partial f(\mathbf{x})}{\partial b}=-\sqrt{w_h}$$

 

모든 Jacobian을 유도했으면 아래와 같이 합쳐준다.

$$\mathbf{J}=[{\mathbf{J}}_{\rho }{\mathbf{J}}_{\mathbf{y}}{]}_{1\times (N+8)}^T$$

 

- $\mathbf{y}=[\delta {\xi }^{T}ab]$

- ${\mathbf{J}}_{\rho }=[\frac{\partial f(\mathbf{x})}{\partial {\rho }_1^{(1)}}\cdots \frac{\partial f(\mathbf{x})}{\partial {\rho }_1^{(N)}}{]}_{1\times N}$ for ${\mathbf{x}}_{\rho }$

- ${\mathbf{J}}_{\mathbf{y}}=[\frac{\partial f(\mathbf{x})}{\partial \delta \xi }\frac{\partial f(\mathbf{x})}{\partial a}\frac{\partial f(\mathbf{x})}{\partial b}{]}_{1\times 8}$  for ${\mathbf{x}}_{\mathbf{y}}$

 

최종적으로 위 Jacobian $\mathbf{J}$를 사용하여 state variable $\mathbf{x}=[{\rho }_1^{(1)},\cdots ,{\rho }_1^{(N)},\delta {\xi }^T,a,b{]}_{(N+8)\times 1}^T=[{\mathbf{x}}_{\rho }{\mathbf{x}}_{\mathbf{y}}{]}^T$를 업데이트 할 수 있다. 

 

1.5 Solving the incremental equation (Schur Complement)

Jacobian까지 모두 유도를 마쳤으면 GN 방법을 사용하여 아래와 같은 최적화를 반복해서 풀어야 한다. 

하지만 실제 위 최적화 공식을 반복적으로 풀면 매우 느린 속도로 인해 정상적으로 pose tracking을 할 수 없다. 이는 ${\mathbf{H}}^{-1}$를 계산할 때 매우 큰 $\mathbf{H}$ 행렬의 특성 상 역함수를 구하는데 매우 많은 시간이 소모되기 때문이다. 해당 공식을 조금 더 자세히 표현해보면 다음과 같다. 

$$\mathbf{H}\mathrm{\Delta }\mathbf{x}=\mathbf{b}\to \left[\begin{array}{cc}{\mathbf{H}}_{\rho \rho }& {\mathbf{H}}_{\rho \mathbf{y}}\\ {\mathbf{H}}_{\mathbf{y}\rho }& {\mathbf{H}}_{\mathbf{y}\mathbf{y}}\end{array}\right]\left[\begin{array}{c}\mathrm{\Delta }{\mathbf{x}}_{\rho }\\ \mathrm{\Delta }{\mathbf{x}}_{\mathbf{y}}\end{array}\right]=\left[\begin{array}{c}{\mathbf{b}}_{\rho }\\ {\mathbf{b}}_{\mathbf{y}}\end{array}\right]$$

 

- $\mathbf{y}=[\delta {\xi }^{T}ab]$

- ${\mathbf{H}}_{\rho \rho }=\sum {\mathbf{J}}_{\rho }^T{\mathbf{J}}_{\rho }$

- ${\mathbf{H}}_{\rho \mathbf{y}}=\sum {\mathbf{J}}_{\rho }^T{\mathbf{J}}_{\mathbf{y}}$

- ${\mathbf{H}}_{\mathbf{y}\rho }=\sum {\mathbf{J}}_{\mathbf{y}}^T{\mathbf{J}}_{\rho }$

- ${\mathbf{H}}_{\mathbf{y}\mathbf{y}}=\sum {\mathbf{J}}_{\mathbf{y}}^T{\mathbf{J}}_{\mathbf{y}}$

- ${\mathbf{b}}_{\rho }=-\sum {\mathbf{J}}_{\rho }^{T}f(\mathbf{x})$

- ${\mathbf{b}}_{\mathbf{y}}=-\sum {\mathbf{J}}_{\mathbf{y}}^{T}f(\mathbf{x})$ : (-)부호를 b 안으로 삽입함

 

Hessian Matrix $\mathbf{H}$를 전개하면 내부 구조는 다음과 같다.

일반적인 경우 3차원 점의 개수가 카메라 pose보다 훨씬 많으므로 다음과 같은 거대한 행렬이 생성된다. 따라서 역행렬을 계산할 때 매우 큰 연산시간이 소요되므로 이를 빠르게 계산할 수 있는 다른 방법이 필요하다. 

이 때, 일반적으로 Schur Complement를 사용하여 computational cost를 낮추어 빠르게 계산한다.

$$\left[\begin{array}{cc}{\mathbf{H}}_{\rho \rho }& {\mathbf{H}}_{\rho \mathbf{y}}\\ {\mathbf{H}}_{\mathbf{y}\rho }& {\mathbf{H}}_{\mathbf{y}\mathbf{y}}\end{array}\right]\left[\begin{array}{c}\mathrm{\Delta }{\mathbf{x}}_{\rho }\\ \mathrm{\Delta }{\mathbf{x}}_{\mathbf{y}}\end{array}\right]=\left[\begin{array}{c}{\mathbf{b}}_{\rho }\\ {\mathbf{b}}_{\mathbf{y}}\end{array}\right]$$

 

Schur Complement를 위해 양변에 아래와 같은 특수한 형태의 행렬을 곱해준다.

$$\left[\begin{array}{cc}\mathbf{I}& \mathbf{0}\\ -{\mathbf{H}}_{\mathbf{y}\rho }{\mathbf{H}}_{\rho \rho }^{-1}& \mathbf{I}\end{array}\right]\left[\begin{array}{cc}{\mathbf{H}}_{\rho \rho }& {\mathbf{H}}_{\rho \mathbf{y}}\\ {\mathbf{H}}_{\mathbf{y}\rho }& {\mathbf{H}}_{\mathbf{y}\mathbf{y}}\end{array}\right]\left[\begin{array}{c}\mathrm{\Delta }{\mathbf{x}}_{\rho }\\ \mathrm{\Delta }{\mathbf{x}}_{\mathbf{y}}\end{array}\right]=\left[\begin{array}{cc}\mathbf{I}& \mathbf{0}\\ -{\mathbf{H}}_{\mathbf{y}\rho }{\mathbf{H}}_{\rho \rho }^{-1}& \mathbf{I}\end{array}\right]\left[\begin{array}{c}{\mathbf{b}}_{\rho }\\ {\mathbf{b}}_{\mathbf{y}}\end{array}\right]$$

$$\left[\begin{array}{cc}{\mathbf{H}}_{\rho \rho }& {\mathbf{H}}_{\rho \mathbf{y}}\\ \mathbf{0}& -{\mathbf{H}}_{\mathbf{y}\rho }{\mathbf{H}}_{\rho \rho }^{-1}{\mathbf{H}}_{\rho \mathbf{y}}+{\mathbf{H}}_{\mathbf{y}\mathbf{y}}\end{array}\right]\left[\begin{array}{c}\mathrm{\Delta }{\mathbf{x}}_{\rho }\\ \mathrm{\Delta }{\mathbf{x}}_{\mathbf{y}}\end{array}\right]=\left[\begin{array}{c}{\mathbf{b}}_{\rho }\\ -{\mathbf{H}}_{\mathbf{y}\rho }{\mathbf{H}}_{\rho \rho }^{-1}{\mathbf{b}}_{\rho }+{\mathbf{b}}_{\mathbf{y}}\end{array}\right]$$

 

위 식을 전개해서 $\mathrm{\Delta }{\mathbf{x}}_{\mathbf{y}}$와 $\mathrm{\Delta }{\mathbf{x}}_{\rho }$를 순차적으로 계산할 수 있다. 위 식을 전개하면  $\mathrm{\Delta }{\mathbf{x}}_{\mathbf{y}}$만 존재하는 항이 유도된다.

 

1.5.1 Forward Substitution

$$\mathrm{\Delta }{\mathbf{x}}_{\mathbf{y}}=({\mathbf{H}}_{\mathbf{y}\mathbf{y}}-{\mathbf{H}}_{sc}{)}^{-1}({\mathbf{b}}_{\mathbf{y}}-{\mathbf{b}}_{sc})$$

- ${\mathbf{H}}_{sc}={\mathbf{H}}_{\mathbf{y}\rho }{\mathbf{H}}_{\rho \rho }^{-1}{\mathbf{H}}_{\rho \mathbf{y}}$

- ${\mathbf{b}}_{sc}={\mathbf{H}}_{\mathbf{y}\rho }{\mathbf{H}}_{\rho \rho }^{-1}{\mathbf{b}}_{\rho }$

 

위 식을 통해 $\mathrm{\Delta }{\mathbf{x}}_{\mathbf{y}}$를 먼저 계산한 후 이를 토대로 $\mathrm{\Delta }{\mathbf{x}}_{\rho }$를 계산한다. 이는 ${\mathbf{H}}^{-1}$을 구하는 것보다 더욱 빠르게 최적화 공식을 풀 수 있다.

 

1.5.2 Backward Substitution

$$\mathrm{\Delta }{\mathbf{x}}_{\rho }={\mathbf{H}}_{\rho \rho }^{-1}({\mathbf{b}}_{\rho }-{\mathbf{H}}_{\rho \mathbf{y}}\mathrm{\Delta }{\mathbf{x}}_{\mathbf{y}})$$

 

inverse depth와 관련있는 항인 ${\mathbf{H}}_{\rho \rho }$는 NxN 크기의 대각행렬(diagonal matrix)이므로 쉽게 역행렬을 구할 수 있다. 따라서 ${\mathbf{H}}_{sc},{\mathbf{b}}_{sc}$ 또한 다음과 같이 비교적 간단하게 계산할 수 있다. 

$${\mathbf{H}}_{sc}={\mathbf{H}}_{\mathbf{y}\rho }{\mathbf{H}}_{\rho \rho }^{-1}{\mathbf{H}}_{\rho \mathbf{y}}=\sum \frac{1}{{\mathbf{J}}_{\rho }{\mathbf{J}}_{\rho }^T}({\mathbf{J}}_{\rho }^T{\mathbf{J}}_{\mathbf{y}}{)}^T{\mathbf{J}}_{\rho }^T{\mathbf{J}}_{\mathbf{y}}$$

$${\mathbf{b}}_{sc}={\mathbf{H}}_{\mathbf{y}\rho }{\mathbf{H}}_{\rho \rho }^{-1}{\mathbf{b}}_{\rho }=-\sum \frac{1}{{\mathbf{J}}_{\rho }{\mathbf{J}}_{\rho }^T}({\mathbf{J}}_{\rho }^T{\mathbf{J}}_{\mathbf{y}}{)}^T{\mathbf{J}}_{\rho }^{T}f(\mathbf{x})$$

 

최종적으로 damping 계수 $\lambda$ 기반의 Levenberg-Marquardt (LM) 방법을 사용하여 Optimization을 수행한다. 

$$(\mathbf{H}+\lambda \mathbf{I})\mathrm{\Delta }\mathbf{x}=\mathbf{b}$$

 

2. Frames

2.1. Pose Tracking

다음으로 pose tracking을 수행하면서 키프레임을 생성하는 알고리즘은 다음과 같다. 새로운 이미지가 들어오면 해당 이미지에 해당하는 3차원 포즈를 아직 모르기 때문에 이를 예측하기 위해 DSO는 가장 최근 두 프레임 $\{C_{i-1,2}\}$과 직전의 키프레임 $\{C_{kf}\}$의 정보를 사용한다.

 

두 프레임(${\mathbf{T}}_{i-1,i-2}$)과 한 개의 키프레임(${\mathbf{T}}_{i-1,kf}$)을 참조하여 이들의 상대 포즈를 구한 후 정지, 직진, 후진, 회전 등 다양한 케이스에 대한 밝기 오차를 구한다. 이 때, 가장 높은 이미지 피라미트(가장 작은 크기 이미지)부터 가장 낮은 이미지 피라미드(원본 크기)까지 순차적으로 밝기 오차를 구하는 coarse-to-fine 방법을 통해 현재 프레임의 3차원 포즈를 구한다. 

각 포즈 후보마다 순차적으로 밝기 오차(photometric error)를 계산하고 이를 최적화하여 최적의 포즈를 구한다. 그리고 최적의 포즈에서 다시 한 번 밝기 오차를 구한 후(resNew) 이전 오차(resOld)와 비교하여 충분히 오차가 감소하였는지 판단한다. 충분히 오차가 감소하였다면 다른 포즈 후보를 고려하지 않고 루프를 탈출한다. (coarseTracker::trackNewestCoarse() 함수 참조)

 

여러 포즈 후보들 중 하나의 포즈를 선택한 후 밝기 오차는 다음과 같이 구할 수 있다.

$$\mathbf{r}={\mathbf{I}}_2({\mathbf{p}}_2)-\exp (a)({\mathbf{I}}_1({\mathbf{p}}_1)-b_0)-b$$

- ${\mathbf{p}}_2=\pi (\exp ({\xi }_{21}^{\wedge }){\mathbf{X}}_1)=\pi (\exp ({\xi }_{21}^{\wedge }){\pi }^{-1}({\mathbf{p}}_1))$

 

이 때, 한 점의 밝기 오차만 계산하는 것이 아닌 주변 8개 점들의 밝기 오차를 모두 계산한다. 다음으로 해당 비선형 함수를 최적화하기 위해 Jacobian을 계산한다.

photometric(2)

$$\frac{\partial \mathbf{r}}{\partial a}=\exp (a){\mathbf{I}}_1(b_0-{\mathbf{p}}_1)$$

$$\frac{\partial \mathbf{r}}{\partial b}=-1$$

pose(6)

$$\frac{\partial \mathbf{r}}{\partial \delta \xi }={\left[\begin{array}{c}\mathrm{\nabla }{\mathbf{I}}_x{\rho }_2{f}_x\\ \mathrm{\nabla }{\mathbf{I}}_y{\rho }_2{f}_y\\ -{\rho }_2(\mathrm{\nabla }{\mathbf{I}}_x{f}_x{u}_2^{{}^{\prime }}+\mathrm{\nabla }{\mathbf{I}}_y{f}_y{v}_2^{{}^{\prime }})\\ -\mathrm{\nabla }{\mathbf{I}}_x{f}_x{u}_2^{{}^{\prime }}{v}_2^{{}^{\prime }}-\mathrm{\nabla }{\mathbf{I}}_y{f}_y(1+v_2^{{}^{\prime }2})\\ \mathrm{\nabla }{\mathbf{I}}_x{f}_x(1+u_2^{{}^{\prime }2})+\mathrm{\nabla }{\mathbf{I}}_y{f}_y{u}_2^{{}^{\prime }}{v}_2^{{}^{\prime }}\\ -\mathrm{\nabla }{\mathbf{I}}_x{f}_x{v}_2^{{}^{\prime }}+\mathrm{\nabla }{\mathbf{I}}_y{f}_y{u}_2^{{}^{\prime }}\end{array}\right]}^T$$

 

해당 Jacobian을 사용하여 $\mathbf{H},\mathbf{b}$를 각각 계산한다.

$$\mathbf{H}=\sum {\mathbf{J}}^T\mathbf{J}$$

$$\mathbf{b}=\sum {\mathbf{J}}^T\mathbf{r}$$

- $\mathbf{J}=[\frac{\partial f(\mathbf{x})}{\partial \delta \xi }\frac{\partial f(\mathbf{x})}{\partial a}\frac{\partial f(\mathbf{x})}{\partial b}{]}_{8\times 1}$

 

$\mathbf{H},\mathbf{b}$를 각각 계산한 다음 Levenberg-Marquardt (LM) 방법을 통해 반복적으로 에러를 감소시킨다. 초기 $\lambda =0.01$로 설정한다.

$$(\mathbf{H}+\lambda \mathbf{I})\mathrm{\Delta }\mathbf{x}=\mathbf{b}$$

$$\mathbf{x}\leftarrow \mathbf{x}+\mathrm{\Delta }\mathbf{x}$$

- $\mathrm{\Delta }\mathbf{x}=[\delta {\xi }^T,\delta a,\delta b{]}_{1\times 8}^T$

 

이 때, ${\mathbf{x}}_{1:6}\in \text{SE}(3)$은 카메라의 포즈를 의미하므로 증분량 $\mathrm{\Delta }{\mathbf{x}}_{1:6}^{\wedge }\in \text{se}(3)$은 실제로 다음과 같이 업데이트 된다.
$${\mathbf{x}}_{1:6}\leftarrow \exp (\mathrm{\Delta }{\mathbf{x}}_{1:6}^{\wedge }){\mathbf{x}}_{1:6}$$

 

${\mathbf{x}}_{7:8}$은 photometric parameters a,b를 의미하므로 다음과 같이 업데이트 된다.

$${\mathbf{x}}_{7:8}\leftarrow {\mathbf{x}}_{7:8}+\mathrm{\Delta }{\mathbf{x}}_{7:8}$$

 

포즈가 업데이트되면 다시 한 번 에러를 계산한 후 에러가 충분히 감소하였는지 판단한다. 에러가 충분히 감소하였다면 나머지 포즈 후보들을 고려하지 않고 루프를 탈출한다. 그리고 lambda 값을 절반으로 줄인다. 만약 에러가 줄어들지 않는다면 lambda 값을 4배로 늘린 후 다시 수행한다.

$$\frac{f(\mathbf{x}{)}_{\text{new}}}{\text{\# of points}}<\frac{f(\mathbf{x}{)}_{\text{old}}}{\text{\# of points}}$$

$$\text{YES: }\lambda \leftarrow \lambda /2$$

$$\text{NO: }\lambda \leftarrow \lambda \cdot 4$$

 

2.2. Keyframe Decision

매 프레임마다 pose tracking이 끝나면 해당 프레임을 키프레임으로 결정할 지 판단한다. 키프레임으로 결정하는 기준은 다음과 같다. 

 

1. 해당 프레임이 가장 첫 프레임인가?

2. (순수 회전을 제외한) 카메라 움직임으로 인한 optical flow의 변화량 + photometric param a,b의 변화량이 일정 기준 이상인가?

3. residual 값이 이전 키프레임과 비교했을 때 급격하게 변했는가?

 

위 세가지 기준 중 하나라도 만족하게 되면 해당 프레임은 키프레임으로 간주된다.

 

3. Non-keyframes

3.1. Inverse Depth Update

키프레임으로 간주되지 않는 프레임의 경우, 해당 프레임은 현재 sliding window에 있는 키프레임들의 immature point idepth 업데이트에 사용된다.

immature point idepth update

업데이트의 자세한 과정은 다음과 같다.

1. 우선, 이전 키프레임에 존재하는 idepth를 알고 있는 immature point들을 상대포즈 $[\mathbf{R}|\mathbf{t}]$를 기반으로 현재 프레임에 프로젝션한다.

 

2. 이 때, ${\mathbf{p}}_1$을 한 번만 프로젝션하지 않고 epipolar line을 찾기 위해 idepth 값을 변경해서 총 2번 프로젝션한다.

$${\mathbf{p}}_{2\mathbf{r}}=\mathbf{K}\mathbf{R}{\mathbf{K}}^{-1}{\mathbf{p}}_1$$

$${\mathbf{p}}_{2,min}={\mathbf{p}}_{2\mathbf{r}}+\mathbf{K}\mathbf{t}{\rho }_{1,min}$$

$${\mathbf{p}}_{2,max}={\mathbf{p}}_{2\mathbf{r}}+\mathbf{K}\mathbf{t}{\rho }_{1,max}$$

 

- ${\rho }_{1,max}$가 기존에 설정되어 있지 않다면 임의의 값인 0.01을 사용한다.

 

3. $\frac{{\mathbf{p}}_{2,max}-{\mathbf{p}}_{2,min}}{\Vert {\mathbf{p}}_{2,max}-{\mathbf{p}}_{2,min}\Vert }$ 값을 계산해서 epipolar line의 방향을 구한다. epipolar line을 수식으로 표현하면 다음과 같다.

$$\mathbf{L}:=\{{\mathbf{l}}_0+\lambda [l_x{l}_y{]}^T\}$$

이 때, ${\mathbf{l}}_0={\mathbf{p}}_{2,min}$는 원점을 의미하고 $[l_x{l}_y{]}^T=\frac{{\mathbf{p}}_{2,max}-{\mathbf{p}}_{2,min}}{\Vert {\mathbf{p}}_{2,max}-{\mathbf{p}}_{2,min}\Vert }$은 방향을 의미한다. $\lambda$는 discrete search step size를 의미한다.

4. 다음으로 maximum discrete search 범위를 정한 후 $\mathbf{L}$ 위에서 가장 밝기 오차가 작은 픽셀을 선정한다. 이 때도 8-point patch 기반으로 밝기 오차를 계산한다.

 

5. 밝기 오차가 가장 작은 점을 선정했으면 patch 내에서 밝기 오차가 두 번째로 작은 점을 선정한 후 두 오차의 비율을 통해 해당 점의 퀄리티를 계산한다.

 

6. 해당 ${\mathbf{p}}_{2,est}$ 점은 discrete search를 통해 찾은 값이므로 보다 정교한 위치를 찾기 위해 GN 최적화를 수행하여 최적의 위치 ${\mathbf{p}}_2^{\ast }$를 구한다. 

7. 최적화 과정을 통해 최적의 위치 ${\mathbf{p}}_2^{\ast }$를 찾았으면 다음으로 해당 픽셀의 idepth 값을 업데이트해준다. idepth 값을 업데이트하기 위해 해당 점을 수식으로 전개하면 다음과 같다. 

$${\mathbf{p}}_{2\mathbf{r}}=\mathbf{K}\mathbf{R}{\mathbf{K}}^{-1}{\mathbf{p}}_1$$

$${\mathbf{p}}_2={\mathbf{p}}_{2\mathbf{r}}+\mathbf{K}\mathbf{t}{\rho }_1$$

- ${\mathbf{p}}_{2\mathbf{r}}=\mathbf{K}\mathbf{R}{\mathbf{K}}^{-1}[u_1{v}_{1}1{]}^T=[m_1{m}_2{m}_3{]}^T$

- $\mathbf{Kt}=[n_1{n}_2{n}_3{]}^T$

를 전개하면

$${\mathbf{p}}_2=\left[\begin{array}{c}{u}_2\\ v_2\\ 1\end{array}\right]$$

$$u_2=\frac{m_1+n_1{\rho }_1}{m_3+n_3{\rho }_1},v_2=\frac{m_2+n_2{\rho }_1}{m_3+n_3{\rho }_1}$$

 

이 된다. 이를 idepth를 기준으로 다시 정리하면 다음과 같은 2개의 식이 유도된다.

$${\rho }_1=\frac{m_3{u}_2-m_1}{n_1-n_3{u}_2},{\rho }_1=\frac{m_3{v}_2-m_2}{n_2-n_3{v}_2}$$

 

8. 다음과 같이 유도된 식에서 최적의 위치 ${\mathbf{p}}_2^{\ast }$를 넣고 다시 전개하면 다음과 같다.

x gradient가 큰 경우 $\mathrm{\Delta }u>\mathrm{\Delta }v$

$${\rho }_{1,min}=\frac{m_3(u_2^{\ast }-\alpha \mathrm{\Delta }u)-m_1}{n_1-n_3(u_2^{\ast }-\alpha \mathrm{\Delta }u)}$$

$${\rho }_{1,max}=\frac{m_3(u_2^{\ast }+\alpha \mathrm{\Delta }u)-m_1}{n_1-n_3(u_2^{\ast }+\alpha \mathrm{\Delta }u)}$$

 

y gradient가 큰 경우 $\mathrm{\Delta }u<\mathrm{\Delta }v$

$${\rho }_{1,min}=\frac{m_3(v_2^{\ast }-\alpha \mathrm{\Delta }v)-m_2}{n_2-n_3(v_2^{\ast }-\alpha \mathrm{\Delta }v)}$$

$${\rho }_{1,max}=\frac{m_3(v_2^{\ast }+\alpha \mathrm{\Delta }v)-m_2}{n_2-n_3(v_2^{\ast }+\alpha \mathrm{\Delta }v)}$$

 

여기서 $\alpha$ 값은 gradient 방향과 epipolar line의 방향이 수직일수록 1에서부터 값이 증가하는 가중치를 의미한다. 해당 $\alpha$ 값이 너무 커지면 두 방향이 거의 수직에 가깝다고 판단하여 idepth 업데이트를 중단한다. 이와 같은 1~8. 과정을 통해 이전 키프레임에서 immature point ${\mathbf{p}}_1$의 idepth 값을 업데이트할 수 있다.

 

References

1. [EN] DSO paper

2. [KR] SLAMKR study 

3. [CH] DSO code reading

4. [CH] jingeTU's SLAM blog

5. [CH] DSO tracking and optimization

6. [CH] DSO initialization

7. [CH] Detailed in DSO

8. [CH] DSO photometric calibration

9. [CH] DSO code with comments

10.[CH] DSO Null Space

11. [CH] DSO (5) Calculation and Derivation of Null Space

12. [KR] goodgodgd's Visual Odometry and vSLAM

13. [EN] DSO Photometric Calibration paper

14. [EN] Inverse Depth paper