Overall process

최적화를 수행해야 할 state vector

k번째 IMU state

3D feature points의 Inverse depth

x_k에는 world 좌표계 기준으로 body frame까지의 translation, velocity, rotation, 그리고 bias 값

camera와 body frame의 extrinsic parameter

$$ \begin{align} \mathcal {X} &= \left[ \mathbf {x}0,\,\mathbf {x}{1},\, \ldots \,\mathbf {x}_{n},\, \mathbf {x}^b_c,\, \lambda _0,\,\lambda {1},\, \ldots \,\lambda {m} \right] \\ \mathbf {x}k &= \left[ \mathbf {p}^w{b_k},\,\mathbf {v}^w{b_k},\,\mathbf {q}^w{b_k}, \,\mathbf {b}_a, \,\mathbf {b}g \right], k\in [0,n] \\ \mathbf {x}^b_c &= \left[ \mathbf {p}^b_c,\,\mathbf {q}^b{c} \right] \tag{13} \end{align} $$

최소 자승법을 통해서 X를 구하면 된다. (아래는 cost function에 대한 설명이다.)

$$ \begin{align} \min {\mathcal {X}} \left\lbrace \left\Vert \mathbf {r}p - \mathbf {H}p \mathcal {X} \right\Vert ^2 + \sum {k \in \mathcal {B}} \left\Vert \mathbf {r}{\mathcal {B}}(\hat{\mathbf {z}}^{b_k}{b{k+1}},\, \mathcal {X}) \right\Vert {\mathbf {P}^{b_k}{b{k+1}}}^2 \right. \\ \left. +\sum {(l,j) \in \mathcal {C}} \rho (\left\Vert \mathbf {r}{\mathcal {C}}(\hat{\mathbf {z}}^{c_j}_l, \, \mathcal {X}) \right\Vert _{\mathbf {P}^{c_j}_l}^2) \right\rbrace \tag{14} \end{align} $$

-1. marginalization 전략

VIO의 계산복잡도를 줄이기 위해 필요한 전략이다. Window에서 두 번째로 최근에 들어온 frame을 기준으로 그 frame이 Keyframe이면 살리고 가장 Old한 Keyframe이면 죽인다. 만약 두 번째로 최근에 들어온 frame이 Keyframe이 아니라면 이를 죽인다.

단, IMU와 관련된 inertrial measurement와 관련된 factor들을 제거하진 않는다.