Bundle Adjustment

Triangulation

- 2 view image를 이용해 correspondences , [R|t]를 구하여 3d landmark position을 estimate했다.

+ N view를 적용해보자

- frame별 [R|t]가 있으며 frame간 correspondence가 있다

- 당연히 noise가 있어 깔끔하게 1개의 좌표로 나오지 않는다.

 

reprojection error

- 3d 좌표를 2d image에 project했을때 생기는오차(각 센서의 error가 반영)

+ motion, observation을 반영하여 최적화 필요

-> Nonlinear optimization을 사용한다.

 

Bundle Adjustment를 언제 사용할까

1. Loop closure

2. sliding-window optimization

 

Gauss-Newton Method

$z_{i} = f_{i}(x) = \lambda K [R|t] \begin{bmatrix} x_{i} \\ y_{i} \\ z_{i} \\ 1 \end{bmatrix}$

K = fx, fy, cx, cy, s (mono는 안바뀌겠지만 multi camera는 바뀜)

R = rx ry, rz

t = tx, ty, tz

 

state $x_{l} = \begin{bmatrix} x_{i} \\ y_{i} \\ z_{i} \end{bmatrix}$

state $x_{cam} = \begin{bmatrix} t_{x} & t_{y} & t_{z} & R_{x} & R_{y} & R_{z} & \lambda & f_{x} & f_{u} & c_{x} & c_{y} & s \end{bmatrix}^{T}$

state vector $x = \begin{bmatrix} x_{cam} \\ x_{l_{1}} \\ x_{l_{2}} \\ ... \\ x_{l_{n}} \end{bmatrix}$

 

reprojection error를 최소화 하기위해 gaussian noise assumption을 가진다고 가정한다.

gaussian noise는 least squares problem을 Maximum Likelihood estimation problem로 만들어 풀기 쉽게 만들고 어느정도 성능을 보장한다.

 

state vector 의 least square를 e(x), 여기에 gaussian noise를 추가한 것을 E(x)라고 하자

$E(x) = \sum_{i} e_{i}(x)^{T}\Omega e_{i}(x)$

$x^{*} = argmin_{x}E(x)$

$\triangle x = x^{*} - x$

$x^{*} = argmin_{\triangle x}E(x+\triangle x}$

$argmin_{\triangle x} \sum_{i} e_{i}(x + \triangle x)^{T}\Omega e_{i}(x + \triangle x)$

$e_{i}(x+\triangle x}$는 non-linear임으로 테일러 급수로 근사화 하면

$e_{i}(x) = e_{i}(x_{0}) + \frac{de_{i}(x)/}{dx}|x_{0}x_{0} \triangle x$

            = $e_{i} + J_{i}\triangle x$이다.

$x^{*}$에 대입하면 2차방정식으로 정리된다.

$x^{*}$의 미분 = 0이면

$\trinangle x = -H^{-1}b$이다.

$\triangle x$를 step을 반복해서 수행하면 점점 최적화된다.

H가 엄청 커서 계산량이 많다.

-> node는 대부분 거리가 멀기 때문에 H, b는 sparse matrix다

 

solution1

$b=\sum_{ij} b_{ij}$ = dense한 nx1 matrix

$H=\sum_{ij} H_{ij}$ = diagonal, symetric한 nxn matrix(identity는 아님)

 

solution2

schur complement

$H\triangle x=g -> \begin{bmatrix} B & E \\ E^{T} & C \end{bmatrix} \begin{bmatrix}\triangle x_{c} \\ \triangle x_{p} \end{bmatrix} = \begin{bmatrix} v \\ w\end{bmatrix}$

$\begin{bmatrix} H_{s} & H_{sc} \\ H_{sc}^{T} & H_{c}\end{bmatrix} \begin{bmatrix} \delta _s \\ \delta _c \end{bmatrix} = \begin{bmatrix} \varepsilon _s \\ \varepsilon _c \end{bmatrix}$

 

least-squares는 outlier에 취약함으로 rejection이 필요하다.

 

open-sourced libraries

- google ceres-solver: https://code.google.com/p\ceres-solver\

- G2o: https://openslam.org/g2o.html 

- GTSAM: https://collab.cc.gatech.edu/borg/gtsam/