A L I D A
7. IMU measurement error
IMU measurement 에러를 구하기 위해서는 우선 IMU preintegration 기법과 error-state 모델링에 대해 알아야 한다. 전반적인 IMU measurement 에러 기반 최적화 과정을 표현한 그림은 위와 같다. [1]-[6] 순서대로 보면 된다. 보다 자세한 내용은 [SLAM] Formula Derivation and Analysis of the VINS-mono 내용 정리를 참조하면 된다.
NOMENCLATURE of IMU measurement error
- $\alpha_{b_{k+1}}^{b_{k}} \in \mathbb{R}^{3 \times 1}$: $t \in [b_{k}, b_{k+1}]$ 시간 동안 누적된 위치의 관측값
- $\hat{\alpha}_{b_{k+1}}^{b_{k}}\in \mathbb{R}^{3 \times 1}$: $t \in [b_{k}, b_{k+1}]$ 시간 동안 누적된 위치의 예측값
- $\beta_{b_{k+1}}^{b_{k}}\in \mathbb{R}^{3 \times 1}$: $t \in [b_{k}, b_{k+1}]$ 시간 동안 누적된 속도의 관측값
- $\hat{\beta}_{b_{k+1}}^{b_{k}}\in \mathbb{R}^{3 \times 1}$: $t \in [b_{k}, b_{k+1}]$ 시간 동안 누적된 속도의 예측값
- $\gamma_{b_{k+1}}^{b_{k}}\in \mathbb{R}^{3 \times 1}$: $t \in [b_{k}, b_{k+1}]$ 시간 동안 누적된 방향(orientation)의 관측값
- $\hat{\gamma}_{b_{k+1}}^{b_{k}}\in \mathbb{R}^{3 \times 1}$: $t \in [b_{k}, b_{k+1}]$ 시간 동안 누적된 방향(orientation)의 예측값
- $\mathcal{X} = [\mathbf{x}_{0}, \mathbf{x}_{1}, \cdots, \mathbf{x}_{n}, \mathbf{x}^{b}_{c}, \lambda_{0}, \lambda_{1}, \cdots, \lambda_{m}]$: 모든 상태 변수
- $\mathbf{x}_{k} = [\mathbf{p}^{w}_{b_{k}}, \mathbf{v}^{w}_{b_{k}}, \mathbf{q}^{w}_{b_{k}}, \mathbf{b}_{a}, \mathbf{b}_{g}]$: 특정 $k$ 시점에서 IMU 모델의 상태 변수
- $\mathbf{x}^{b}_{c} = [\mathbf{p}^{b}_{c}, \mathbf{q}^{b}_{c}]$: 카메라와 IMU의 외부 파라미터(extrinsic parameter)
- $\mathcal{X}_{k}$: 특정 두 시점 $[b_{k}, b_{k+1}]$의 상태변수. 이는 즉 $\mathcal{X}_{k} = (\mathbf{x}_{k}, \mathbf{x}_{k+1})$과 같다.
- $\lambda$: 특징점의 inverse depth
- $\otimes$: 쿼터니언 곱셈 연산자. (e.g., $\mathbf{q} = \mathbf{q}_{1} \otimes \mathbf{q}_{2}$)
- $\mathcal{B}$: 모든 IMU $b_{k}$ 값들의 집합
- $\ominus$: 벡터와 쿼터니언을 한 번에 뺄셈 연산하는 연산자
- $\mathbf{P}_{\mathcal{B}}$: 모든 IMU $b_{k}$ 값들의 공분산
- $\boldsymbol{\Omega}_{\mathcal{B}}$: 공분산 $\mathbf{P}_{\mathcal{B}}$의 역행렬. Information 행렬을 의미한다.
- $\mathbf{e}_{\mathcal{B},k} = \mathbf{e}_{\mathcal{B}}(\mathcal{X}_{k})$
IMU 또한 이전 섹션에서 설명한 에러들과 동일하게 관측값 - 예측값을 에러로 정의하며 이를 IMU measurement 에러 $\mathbf{e}_{\mathcal{B}}$라고 한다. 자세히 설명하면, IMU measurement 에러 $\mathbf{e}_{\mathcal{B}}$는 $t \in [b_{k}, b_{k+1}]$ 시간 동안 들어오는 IMU 데이터를 누적한 preintegration과 bias $[\alpha, \beta, \gamma, \mathbf{b}_{a}, \mathbf{b}_{g}]$의 관측값$(\mathbf{z}_{b_{k+1}}^{b_{k}})$과 예측값$(\hat{\mathbf{z}}_{b_{k+1}}^{b_{k}})$의 차이를 의미한다.
\begin{equation}
\boxed{ \begin{aligned}
\mathbf{e}_{\mathcal{B}}(\mathcal{X}_{k}) & = \mathbf{z}_{b_{k+1}}^{b_{k}} \ominus \hat{\mathbf{z}}_{b_{k+1}}^{b_{k}} = \begin{bmatrix}
\alpha_{b_{k+1}}^{b_{k}} - \hat{\alpha}_{b_{k+1}}^{b_{k}} \\ \beta_{b_{k+1}}^{b_{k}} -\hat{\beta}_{b_{k+1}}^{b_{k}}\\ \gamma_{b_{k+1}}^{b_{k}} \otimes \hat{\gamma}_{b_{k+1}}^{b_{k}} \\ \mathbf{b}_{a_{k}} - \hat{\mathbf{b}}_{a} \\ \mathbf{b}_{g} - \hat{\mathbf{b}}_{g}
\end{bmatrix}
\end{aligned} } \label{eq:imu3}
\end{equation}
관측값과 예측값에 대해 자세히 알아보자. 우선, 관측값은 두 시점 $b_{k}, b_{k+1}$의 위치 $\mathbf{p}$와 속도 $\mathbf{v}$ 그리고 방향 $\mathbf{q}$ 값을 사용하여 구할 수 있다. 관측값을 구하기 위해 $[b_{k}, b_{k+1}]$ 구간의 IMU kinematics 공식을 보면 다음과 같다.
\begin{equation}
\begin{aligned}
& \mathbf{R}^{b_{k}}_{w} \mathbf{p}^{w}_{b_{k+1}} = \mathbf{R}^{b_{k}}_{w} (\mathbf{p}^{w}_{b_{k}} + \mathbf{v}^{w}_{b_{k}}\Delta t - \frac{1}{2}\mathbf{g}^{w} \Delta t_{k}^{2}) + \alpha^{b_{k}}_{b_{k+1}} \\
& \mathbf{R}^{b_{k}}_{w}\mathbf{v}^{w}_{b_{k+1}} = \mathbf{R}^{b_{k}}_{w} (\mathbf{v}^{w}_{b_{k}} - \mathbf{g}^{w} \Delta t_{k}) + \beta^{b_k}_{b_{k+1}} \\
& \mathbf{q}^{b_{k}}_{w} \otimes \mathbf{q}^{w}_{b_{k+1}} = \gamma^{b_{k}}_{b_{k+1}}
\end{aligned}
\end{equation}
따라서 관측값은 다음과 같이 구할 수 있다.
\begin{equation}
\boxed{ \begin{aligned}
\mathbf{z}_{b_{k+1}}^{b_{k}}
= \begin{bmatrix}
\alpha^{b_{k}}_{b_{k+1}} \\
\beta^{b_{k}}_{b_{k+1}} \\
\gamma^{b_{k}}_{b_{k+1}} \\
\mathbf{b}_{a_{k+1}} - \mathbf{b}_{a_{k}} \\
\mathbf{b}_{g_{k+1}} - \mathbf{b}_{g_{k}} \\
\end{bmatrix}
= \begin{bmatrix}
\mathbf{R}^{b_{k}}_{w}(\mathbf{p}^{w}_{b_{k+1}} - \mathbf{p}^{w}_{b_{k}} - \mathbf{v}^{w}_{b_{k}}\Delta t_{k} + \frac{1}{2} \mathbf{g}^{w}\Delta t_{k}^{2}) \\
\mathbf{R}^{b_{k}}_{w} ( \mathbf{v}^{w}_{b_{k+1}} - \mathbf{v}^{w}_{b_{k}} + \mathbf{g}^{w}\Delta t_{k}) \\
(\mathbf{q}^{w}_{b_{k}})^{-1} \otimes \mathbf{q}^{w}_{b_{k+1}} \\
\mathbf{b}_{a_{k+1}} - \mathbf{b}_{a_{k}} \\
\mathbf{b}_{g_{k+1}} - \mathbf{b}_{g_{k}} \\
\end{bmatrix}
\end{aligned} }
\end{equation}
다음으로 예측값은 $t \in [b_{k}, b_{k+1}]$ 시간동안 누적한 preintegration 값을 통해 구할 수 있다. 예측값을 구하기 위해 preintegration 식을 자세히 보면 다음과 같다.
\begin{equation}
\begin{aligned}
& \hat{\alpha}^{b_{k}}_{b_{k+1}} = \iint_{t \in [k,k+1]} \mathbf{R}^{b_{k}}_{t} (\hat{\mathbf{a}}_{t} - \mathbf{b}_{at} - \mathbf{n}_{a}) dt^{2} \\
& \hat{\beta}^{b_{k}}_{b_{k+1}} = \int_{t \in [k,k+1]} \mathbf{R}^{b_{k}}_{t} (\hat{\mathbf{a}}_{t} - \mathbf{b}_{at} -\mathbf{n}_{a}) dt \\
& \hat{\gamma}^{b_{k}}_{b_{k+1}} = \int_{t \in [k,k+1]} \frac{1}{2} \Omega_{R}(\hat{\boldsymbol{\omega}}_{t} - \mathbf{b}_{gt} - \mathbf{n}_{g})\gamma^{b_{k}}_{t} dt
\end{aligned}
\end{equation}
위 식은 연속 신호에서 사용 가능한 공식이다. 하지만 실제 IMU 신호는 이산 신호(discrete signal)로 들어오므로 미분 방정식(differential equation)을 차분 방정식(difference equation)으로 표현해야 한다. 해당 과정에서 다양한 수치적분 알고리즘이 사용되는데, 수치적분에는 zero-order hold(euler), first-order hold(mid-point), higher order (RK4) 등이 존재한다. 이 중 VINS-mono에서 사용한 mid-point method를 사용해 차분 방정식을 표현하면 다음과 같다.
\begin{equation}
\begin{aligned}
\hat{\alpha}^{b_{k}}_{t+1} & = \hat{\alpha}^{b_{k}}_{t} + \frac{1}{2}(\hat{\beta}^{b_{k}}_{t} + \hat{\beta}^{b_{k}}_{t+1})\delta t \\
& = \hat{\alpha}^{b_{k}}_{t} + \hat{\beta}^{b_{k}}_{t} \delta t + \frac{1}{4}[\mathbf{R}\{\hat{\gamma}^{b_{k}}_{t}\}(\hat{\mathbf{a}}_{t} - \mathbf{b}_{at}) + \mathbf{R}\{\hat{\gamma}^{b_{k}}_{t+1}\}(\hat{\mathbf{a}}_{t+1} - \mathbf{b}_{at})]\delta t^{2} \\
\hat{\beta}^{b_{k}}_{t+1} & = \hat{\beta}^{b_{k}}_{t} + \frac{1}{2}[\mathbf{R}\{\hat{\gamma}^{b_{k}}_{t}\}(\hat{\mathbf{a}}_{t} - \mathbf{b}_{at}) + \mathbf{R}\{\hat{\gamma}^{b_{k}}_{t+1}\}(\hat{\mathbf{a}}_{t+1} - \mathbf{b}_{at})]\delta t \\
\hat{\gamma}^{b_{k}}_{t+1} & = \hat{\gamma}^{b_{k}}_{t} \otimes \hat{\gamma}^{b_{k}}_{t, t+1} = \hat{\gamma}^{b_{k}}_{t} \otimes \begin{bmatrix} 1 \\ 1/4(\hat{\boldsymbol{\omega}}_{t} + \hat{\boldsymbol{\omega}}_{t+1} - 2 \mathbf{b}_{gt}) \delta t \end{bmatrix} \\
\end{aligned} \label{eq:imu1}
\end{equation}
따라서 예측값은 (\ref{eq:imu1}) 식을 $t \in [b_{k}, b_{k+1}]$ 시간 동안 누적한 값으로 구할 수 있다. Bias 값은 예측값을 구할 수 없기 때문에 0으로 설정한다.
\begin{equation}
\boxed{ \begin{aligned}
\hat{\mathbf{z}}_{b_{k+1}}^{b_{k}}
= \begin{bmatrix} \hat{\alpha}^{b_{k}}_{b_{k+1}} \\ \hat{\beta}^{b_{k}}_{b_{k+1}} \\ \hat{\gamma}^{b_{k}}_{b_{k+1}} \\ \mathbf{0} \\ \mathbf{0} \end{bmatrix}
\end{aligned} }
\end{equation}
지금까지 구한 값을 토대로 (\ref{eq:imu3})를 풀어쓰면 IMU measurement 에러는 다음과 같이 나타낼 수 있다.
\begin{equation}
\boxed{ \begin{aligned}
\mathbf{e}_{\mathcal{B}}(\mathcal{X}_{k}) = \mathbf{z}_{b_{k+1}}^{b_{k}} \ominus \hat{\mathbf{z}}_{b_{k+1}}^{b_{k}} = \begin{bmatrix}
\mathbf{R}^{b_{k}}_{w}(\mathbf{p}^{w}_{b_{k+1}} - \mathbf{p}^{w}_{b_{k}} - \mathbf{v}^{w}_{b_{k}}\Delta t_{k} + \frac{1}{2} \mathbf{g}^{w}\Delta t_{k}^{2}) - \hat{\alpha}^{b_{k}}_{b_{k+1}} \\
\mathbf{R}^{b_{k}}_{w} ( \mathbf{v}^{w}_{b_{k+1}} - \mathbf{v}^{w}_{b_{k}} + \mathbf{g}^{w}\Delta t_{k}) - \hat{\beta}^{b_{k}}_{b_{k+1}} \\
\Big( \hat{\gamma}^{b_{k}}_{b_{k+1}} \Big)^{-1} \otimes (\mathbf{q}^{w}_{b_{k}})^{-1} \otimes \mathbf{q}^{w}_{b_{k+1}} \\
\mathbf{b}_{a_{k+1}} - \mathbf{b}_{a_{k}} \\
\mathbf{b}_{g_{k+1}} - \mathbf{b}_{g_{k}} \\
\end{bmatrix}
\end{aligned} }
\end{equation}
7.1. Error function formulation
모든 preintegration, bias들에 대한 에러 함수는 다음과 같이 정의된다.
\begin{equation}
\begin{aligned}
\mathbf{E}_{\mathcal{B}}(\mathcal{X}) & = \sum_{k \in \mathcal{B}} \left\| \mathbf{e}_{\mathcal{B},k} \right\|_{\mathbf{P}_{\mathcal{B}}}^{2} \\
\end{aligned}
\end{equation}
\begin{equation}
\begin{aligned}
\mathcal{X}^{*} &= \arg\min_{\mathcal{X}^{*}} \mathbf{E}_{\mathcal{B}}(\mathcal{X}) \\
& = \arg\min_{\mathcal{X}^{*}} \sum_{k \in \mathcal{B}} \left\| \mathbf{e}_{\mathcal{B},k} \right\|_{\mathbf{P}_{\mathcal{B}}}^{2} \\
& = \arg\min_{\mathcal{X}^{*}} \sum_{k \in \mathcal{B}} \mathbf{e}_{_{\mathcal{B}},k}^{\intercal}\boldsymbol{\Omega}_{\mathcal{B}}\mathbf{e}_{_{\mathcal{B}},k} \\
& = \arg\min_{\mathcal{X}^{*}} \sum_{k \in \mathcal{B}} (\mathbf{z}_{b_{k+1}}^{b_{k}} \ominus \hat{\mathbf{z}}_{b_{k+1}}^{b_{k}})^{\intercal} \boldsymbol{\Omega}_{\mathcal{B}} (\mathbf{z}_{b_{k+1}}^{b_{k}} \ominus \hat{\mathbf{z}}_{b_{k+1}}^{b_{k}})
\end{aligned}
\end{equation}
위 식에서 $\mathbf{e}_{\mathcal{B},k} = \mathbf{e}_{\mathcal{B}}(\mathcal{X}_{k})$를 의미한다.
실제 VINS-mono에서는 IMU measurement 에러 뿐만 아니라 visual residual $\mathbf{r}_{\mathcal{C}}$, marginalization prior residual $\mathbf{r}_{p}$ 값도 동시에 최적화하여 tightly-coupled VIO를 수행한다. VINS-mono에서는 IMU measurement 에러를 residual $\mathbf{r}_{\mathcal{B}}(\hat{\mathbf{z}}_{b_{k+1}}^{b_{k}}, \mathcal{X})$로 표현하였다.
\begin{equation}
\begin{aligned}
\min_{\mathcal{X}} \bigg\{ \left\| \mathbf{r}_{p} - \mathbf{J}_{p}\mathcal{X} \right\|_{\mathbf{P}_{M}} + \sum_{k \in \mathcal{B}} \left\| \mathbf{r}_{\mathcal{B}} \Big( \hat{\mathbf{z}}^{b_{k}}_{b_{k+1}}, \mathcal{X} \Big) \right\|_{\mathbf{P}_{B}} + \sum_{(l,j) \in \mathcal{C}} \left\| \mathbf{r}_{\mathcal{C}} \Big( \hat{\mathbf{z}}^{c_{j}}_{l}, \mathcal{X} \Big) \right\|_{\mathbf{P}^{c_{j}}_{l}} \bigg\}
\end{aligned}
\end{equation}
본 섹션에서는 이 중 IMU measurement 에러 $\mathbf{r}_{\mathcal{B}}(\hat{\mathbf{z}}_{b_{k+1}}^{b_{k}}, \mathcal{X})$에 대한 내용만 설명한다.
$\mathbf{E}_{\mathcal{B}}(\mathcal{X}^{*})$를 만족하는 $\left\|\mathbf{e}_{\mathcal{B}}(\mathcal{X}_{k}^{*})\right\|_{\mathbf{P}_{\mathcal{B}}}^{2}$를 non-linear least squares를 통해 반복적으로 계산할 수 있다. 작은 증분량 $\Delta \mathcal{X}$를 반복적으로 $\mathcal{X}$에 업데이트함으로써 최적의 상태를 찾는다.
\begin{equation}
\begin{aligned}
\arg\min_{\mathcal{X}^{*}} \mathbf{E}_{\mathcal{B}}(\mathcal{X} + \Delta \mathcal{X}) & = \arg\min_{\mathcal{X}^{*}} \sum_{k \in \mathcal{B}} \left\|\mathbf{e}_{\mathcal{B}}(\mathcal{X}_{k} +\Delta \mathcal{X}_{k})\right\|^{2}
\end{aligned}
\end{equation}
엄밀하게 말하면 상태 증분량 $\Delta \mathcal{X}$은 쿼터니언을 포함하므로 $\oplus$ 연산자를 통해 기존 상태 $\mathcal{X}$에 더해지는게 맞지만 표현의 편의를 위해 $+$ 연산자를 사용하였다.
\begin{equation}
\begin{aligned}
\mathbf{e}_{\mathcal{B}}( \mathcal{X}_{k} \oplus \Delta \mathcal{X}_{k})
\quad \rightarrow \quad\mathbf{e}_{\mathcal{B}}(\mathcal{X}_{k} + \Delta \mathcal{X}_{k})
\end{aligned}
\end{equation}
위 식은 테일러 1차 근사를 통해 다음과 같이 표현이 가능하다.
\begin{equation}
\begin{aligned}
\mathbf{e}_{\mathcal{B}}(\mathcal{X}_{k} + \Delta \mathcal{X}_{k}) & \approx \mathbf{e}_{\mathcal{B}}(\mathcal{X}) + \mathbf{J}\Delta \mathcal{X}_{k} \\
& = \mathbf{e}_{\mathcal{B}}(\mathcal{X}_{k}) + \begin{bmatrix}
\frac{\partial \mathbf{e}_{\mathcal{B}}}{\partial [\mathbf{p}^{w}_{b_{k}}, \mathbf{q}^{w}_{b_{k}}]} & \frac{\partial \mathbf{e}_{\mathcal{B}}}{\partial [\mathbf{v}^{w}_{b_{k}}, \mathbf{b}_{ak}, \mathbf{b}_{gk}]} & \frac{\partial \mathbf{e}_{\mathcal{B}}}{\partial [\mathbf{p}^{w}_{b_{k+1}}, \mathbf{q}^{w}_{b_{k+1}}]} & \frac{\partial \mathbf{e}_{\mathcal{B}}}{\partial [\mathbf{v}^{w}_{b_{k+1}}, \mathbf{b}_{ak+1}, \mathbf{b}_{ak+1}]}
\end{bmatrix} \begin{bmatrix}
\Delta \mathbf{p}^{w}_{k} \\ \Delta \mathbf{q}^{w}_{k} \\ \Delta \mathbf{v}^{w}_{k} \\ \Delta \mathbf{b}_{ak} \\ \Delta \mathbf{b}_{gk} \\ \Delta \mathbf{p}^{w}_{k+1} \\ \Delta \mathbf{q}^{w}_{k+1} \\ \Delta \mathbf{v}^{w}_{k+1} \\ \Delta \mathbf{b}_{ak+1} \\ \Delta \mathbf{b}_{gk+1}
\end{bmatrix} \\
& = \mathbf{e}_{\mathcal{B}}(\mathcal{X}_{k})
+ \frac{\partial \mathbf{e}_{\mathcal{B}}}{\partial [\mathbf{p}^{w}_{b_{k}}, \mathbf{q}^{w}_{b_{k}}]} (\Delta \mathbf{p}^{w}_{k}, \Delta \mathbf{q}^{w}_{k})
+ \frac{\partial \mathbf{e}_{\mathcal{B}}}{\partial [\mathbf{v}^{w}_{b_{k}}, \mathbf{b}_{ak}, \mathbf{b}_{gk}]} (\Delta \mathbf{v}^{w}_{k} , \Delta \mathbf{b}_{ak} ,\Delta \mathbf{b}_{gk})
\\
& + \frac{\partial \mathbf{e}_{\mathcal{B}}}{\partial [\mathbf{p}^{w}_{b_{k+1}}, \mathbf{q}^{w}_{b_{k+1}}]} (\Delta \mathbf{p}^{w}_{k+1} , \Delta \mathbf{q}^{w}_{k+1})
+ \frac{\partial \mathbf{e}_{\mathcal{B}}}{\partial [\mathbf{v}^{w}_{b_{k+1}}, \mathbf{b}_{ak+1}, \mathbf{b}_{ak+1}]} (\Delta \mathbf{v}^{w}_{k+1} , \Delta \mathbf{b}_{ak+1} , \Delta \mathbf{b}_{gk+1})
\end{aligned}
\end{equation}
$b_{k}$ 시점의 $[\mathbf{p}^{w}_{b_{k}},\mathbf{v}^{w}_{b_{k}},\mathbf{q}^{w}_{b_{k}}, \mathbf{b}_{a_{k}}, \mathbf{b}_{g_{k}}]$와 $b_{k+1}$ 시점의 $[\mathbf{p}^{w}_{b_{k+1}},\mathbf{v}^{w}_{b_{k+1}},\mathbf{q}^{w}_{b_{k+1}}, \mathbf{b}_{a_{k+1}}, \mathbf{b}_{g_{k+1}}]$ 모두 에러 값에 관여하기 때문에 총 10개 변수에 대한 자코비안을 모두 계산해야 한다. VINS-mono에서는 다음과 같이 4개의 그룹으로 묶어서 상태 변수를 표현하였다.
\begin{equation}
\begin{aligned}
& [\mathbf{p}^{w}_{b_{k}}, \mathbf{q}^{w}_{b_{k}}] \quad &&\cdots \text{ for } \mathbf{J}[0] \\
& [\mathbf{v}^{w}_{b_{k}}, \mathbf{b}_{ak}, \mathbf{b}_{gk}] \quad &&\cdots \text{ for } \mathbf{J}[1] \\
& [\mathbf{p}^{w}_{b_{k+1}}, \mathbf{q}^{w}_{b_{k+1}}] \quad &&\cdots \text{ for } \mathbf{J}[2] \\
& [\mathbf{v}^{w}_{b_{k+1}}, \mathbf{b}_{ak+1}, \mathbf{b}_{ak+1}] \quad &&\cdots \text{ for } \mathbf{J}[3] \\
\end{aligned}
\end{equation}
Tightly-coupled VIO에서 최적화하는 상태 변수 $\mathcal{X}$은 inverse depth $\lambda$와 외부 파라미터(extrinsic parameter) $\mathbf{x}^{b}_{c}$, time difference $td$의 상태를 포함하지만 IMU measurement 에러에서는 위와 같이 두 시점 $[b_{k}, b_{k+1}]$에서 포즈와 속도, bias 값들만 업데이트함에 유의한다.
에러 함수는 다음과 같이 근사할 수 있다.
\begin{equation}
\begin{aligned}
\arg\min_{\mathcal{X}^{*}} \mathbf{E}_{\mathcal{B}}(\mathcal{X} + \Delta \mathcal{X}) & \approx \arg\min_{\mathcal{X}^{*}} \sum_{k \in \mathcal{B}} \left\|\mathbf{e}_{\mathcal{B}}(\mathcal{X}_{k}) + \mathbf{J}\Delta \mathcal{X}_{k} \right\|_{\mathbf{P}_{\mathcal{B}}}^{2} \\
\end{aligned}
\end{equation}
이를 미분하여 최적의 증분량 $\Delta \mathcal{X}^{*}$ 값을 구하면 다음과 같다. 자세한 유도 과정은 본 섹션에서는 생략한다. 유도 과정에 대해 자세히 알고 싶으면 이전 섹션 을 참조하면 된다.
\begin{equation}
\begin{aligned}
& \mathbf{J}^{\intercal}\mathbf{J} \Delta \mathcal{X}^{*} = -\mathbf{J}^{\intercal}\mathbf{e} \\
& \mathbf{H}\Delta \mathcal{X}^{*} = - \mathbf{b} \\
\end{aligned} \label{eq:imu2}
\end{equation}
위 식은 선형시스템 $\mathbf{Ax} = \mathbf{b}$ 형태이므로 schur complement, cholesky decomposition과 같은 다양한 선형대수학 테크닉을 사용하여 $\Delta \mathcal{X}^{*}$를 구할 수 있다. 이렇게 구한 최적의 증분량을 현재 상태에 더한다. 이 때, 기존 상태 $\mathbf{x}$의 오른쪽에 곱하느냐 왼쪽에 곱하느냐에 따라서 각각 로컬 좌표계에서 본 포즈를 업데이트할 것 인지(오른쪽) 전역 좌표계에서 본 포즈를 업데이트할 것 인지(왼쪽) 달라지게 된다. IMU measurement 에러는 두 노드 $b_{k}, b_{k+1}$과 관련되어 있으므로 로컬 좌표계에서 업데이트하는 오른쪽 곱셈이 적용된다.
\begin{equation}
\begin{aligned}
\mathcal{X} \leftarrow \mathcal{X} \oplus \Delta \mathcal{X}^{*} \\
\end{aligned}
\end{equation}
$\mathcal{X}$ 중에서 IMU measurement 에러에 의해 업데이트되는 $\mathcal{X}_{k}$는 $[\mathbf{p}^{w}_{b_{k}},\mathbf{v}^{w}_{b_{k}},\mathbf{q}^{w}_{b_{k}}, \mathbf{b}_{a_{k}}, \mathbf{b}_{g_{k}}, \mathbf{p}^{w}_{b_{k+1}},\mathbf{v}^{w}_{b_{k+1}},\mathbf{q}^{w}_{b_{k+1}}, \mathbf{b}_{a_{k+1}}, \mathbf{b}_{g_{k+1}}]$로 구성되어 있으므로 다음과 같이 풀어 쓸 수 있다.
\begin{equation}
\begin{aligned}
& \mathbf{p}^{w}_{b_{k}} \leftarrow \mathbf{p}^{w}_{b_{k}} \oplus \Delta \mathbf{p}^{w*}_{b_{k}} \\
& \mathbf{q}^{w}_{b_{k}} \leftarrow \mathbf{q}^{w}_{b_{k}} \oplus \Delta \mathbf{q}^{w*}_{b_{k}} \\
& \mathbf{v}^{w}_{b_{k}} \leftarrow \mathbf{v}^{w}_{b_{k}} \oplus \Delta \mathbf{v}^{w*}_{b_{k}} \\
& \mathbf{b}_{ak} \leftarrow \mathbf{b}_{ak} \oplus \Delta \mathbf{b}_{ak}^{*} \\
& \mathbf{b}_{gk} \leftarrow \mathbf{b}_{gk} \oplus \Delta \mathbf{b}_{gk}^{*} \\
& \mathbf{p}^{w}_{b_{k+1}} \leftarrow \mathbf{p}^{w}_{b_{k+1}} \oplus \Delta \mathbf{p}^{w*}_{b_{k+1}} \\
& \mathbf{q}^{w}_{b_{k+1}} \leftarrow \mathbf{q}^{w}_{b_{k+1}} \oplus \Delta \mathbf{q}^{w*}_{b_{k+1}} \\
& \mathbf{v}^{w}_{b_{k+1}} \leftarrow \mathbf{v}^{w}_{b_{k+1}} \oplus \Delta \mathbf{v}^{w*}_{b_{k+1}} \\
& \mathbf{b}_{ak+1} \leftarrow \mathbf{b}_{ak+1} \oplus \Delta \mathbf{b}_{ak+1}^{*} \\
& \mathbf{b}_{gk+1} \leftarrow \mathbf{b}_{gk+1} \oplus \Delta \mathbf{b}_{gk+1}^{*} \\
\end{aligned}
\end{equation}
오른쪽 곱셈 $\oplus$ 연산의 정의는 다음과 같다.
\begin{equation}
\begin{aligned}
& \mathbf{p}^{w}_{b_{k}} \leftarrow \mathbf{p}^{w}_{b_{k}} + \Delta \mathbf{p}^{w*}_{b_{k}} \\
& \mathbf{q}^{w}_{b_{k}} \leftarrow \mathbf{q}^{w}_{b_{k}} \otimes \Delta \mathbf{q}^{w*}_{b_{k}} \quad \cdots \text{ locally updated (right mult)} \\
& \mathbf{v}^{w}_{b_{k}} \leftarrow \mathbf{v}^{w}_{b_{k}} + \Delta \mathbf{v}^{w*}_{b_{k}} \\
& \mathbf{b}_{ak} \leftarrow \mathbf{b}_{ak} + \Delta \mathbf{b}_{ak}^{*} \\
& \mathbf{b}_{gk} \leftarrow \mathbf{b}_{gk} + \Delta \mathbf{b}_{gk}^{*} \\
& \mathbf{p}^{w}_{b_{k+1}} \leftarrow \mathbf{p}^{w}_{b_{k+1}} + \Delta \mathbf{p}^{w*}_{b_{k+1}} \\
& \mathbf{q}^{w}_{b_{k+1}} \leftarrow \mathbf{q}^{w}_{b_{k+1}} \otimes \Delta \mathbf{q}^{w*}_{b_{k+1}} \quad \cdots \text{ locally updated (right mult)} \\
& \mathbf{v}^{w}_{b_{k+1}} \leftarrow \mathbf{v}^{w}_{b_{k+1}} + \Delta \mathbf{v}^{w*}_{b_{k+1}} \\
& \mathbf{b}_{ak+1} \leftarrow \mathbf{b}_{ak+1} + \Delta \mathbf{b}_{ak+1}^{*} \\
& \mathbf{b}_{gk+1} \leftarrow \mathbf{b}_{gk+1} + \Delta \mathbf{b}_{gk+1}^{*} \\
\end{aligned}
\end{equation}
7.2. Jacobian of IMU measurement error
(\ref{eq:imu2})를 수행하기 위해서는 IMU measurement 에러에 대한 자코비안 $\mathbf{J}$를 구해야 한다. 이를 풀어쓰면 다음과 같이 나타낼 수 있다.
\begin{equation}
\begin{aligned}
\mathbf{J} & = \begin{bmatrix} \mathbf{J}[0] & \mathbf{J}[1] & \mathbf{J}[2] & \mathbf{J}[3] \end{bmatrix} \\
& = \begin{bmatrix}
\frac{\partial \mathbf{e}_{\mathcal{B}}}{\partial [\mathbf{p}^{w}_{b_{k}}, \mathbf{q}^{w}_{b_{k}}]} & \frac{\partial \mathbf{e}_{\mathcal{B}}}{\partial [\mathbf{v}^{w}_{b_{k}}, \mathbf{b}_{ak}, \mathbf{b}_{gk}]} & \frac{\partial \mathbf{e}_{\mathcal{B}}}{\partial [\mathbf{p}^{w}_{b_{k+1}}, \mathbf{q}^{w}_{b_{k+1}}]} & \frac{\partial \mathbf{e}_{\mathcal{B}}}{\partial [\mathbf{v}^{w}_{b_{k+1}}, \mathbf{b}_{ak+1}, \mathbf{b}_{ak+1}]}
\end{bmatrix} \\
& = \frac{\partial }{\partial [\mathbf{p}^{w}_{b_{k}}, \mathbf{q}^{w}_{b_{k}}], [\mathbf{v}^{w}_{b_{k}}, \mathbf{b}_{ak}, \mathbf{b}_{gk}], [\mathbf{p}^{w}_{b_{k+1}}, \mathbf{q}^{w}_{b_{k+1}}], [\mathbf{v}^{w}_{b_{k+1}}, \mathbf{b}_{ak+1}, \mathbf{b}_{ak+1}]}
\begin{bmatrix}
\mathbf{R}^{b_{k}}_{w}(\mathbf{p}^{w}_{b_{k+1}} - \mathbf{p}^{w}_{b_{k}} - \mathbf{v}^{w}_{b_{k}}\Delta t_{k} + \frac{1}{2} \mathbf{g}^{w}\Delta t_{k}^{2}) - \hat{\alpha}^{b_{k}}_{b_{k+1}} \\
\mathbf{R}^{b_{k}}_{w} ( \mathbf{v}^{w}_{b_{k+1}} - \mathbf{v}^{w}_{b_{k}} + \mathbf{g}^{w}\Delta t_{k}) - \hat{\beta}^{b_{k}}_{b_{k+1}} \\
\Big( \hat{\gamma}^{b_{k}}_{b_{k+1}} \Big)^{-1} \otimes (\mathbf{q}^{w}_{b_{k}})^{-1} \otimes \mathbf{q}^{w}_{b_{k+1}} \\
\mathbf{b}_{a_{k+1}} - \mathbf{b}_{a_{k}} \\
\mathbf{b}_{g_{k+1}} - \mathbf{b}_{g_{k}} \\
\end{bmatrix} \\
& = \begin{bmatrix} \mathbb{R}^{15 \times 7} && \mathbb{R}^{15 \times 9} && \mathbb{R}^{15 \times 7} && \mathbb{R}^{15 \times 9} \end{bmatrix} = \mathbb{R}^{15 \times 32}
\end{aligned}
\end{equation}
7.2.1. Lie theory-based SO(3) optimization
위 자코비안을 구할 때, 위치 $\mathbf{p}$, 속도 $\mathbf{v}$, bias $\mathbf{b}_{a}, \mathbf{b}_{g}$에 관련된 항은 각각 3차원 벡터이므로 최적화 업데이트를 수행할 때 별도의 제약조건이 존재하지 않는다. 반면에, 쿼터니언 $\mathbf{q}$는 파라미터의 개수가 4개이고 이는 3차원 회전을 표현하는 최소 자유도인 3 자유도보다 많으므로 다양한 제약조건이 존재한다. 이를 over-parameterized 되었다고 한다. over-parameterized 표현법의 단점은 다음과 같다.
- 중복되는 파라미터를 계산해야 하기 때문에 최적화 수행 시 연산량이 증가한다.
- 추가적인 자유도로 인해 수치적인 불안정성(numerical instability) 문제가 야기될 수 있다.
- 파라미터가 업데이트될 때마다 항상 제약조건을 만족하는 지 체크해줘야 한다.
lie theory를 사용하면 제약조건으로부터 자유롭게 최적화를 수행할 수 있다. 따라서 쿼터니언 $\mathbf{q}$을 사용하는 대신 lie algebra so(3) $[\boldsymbol{\theta}]_{\times}$을 사용하여 제약조건으로부터 자유롭게 파라미터를 업데이트할 수 있게 된다. 이 때, $\boldsymbol{\theta} \in \mathbb{R}^{3}$는 각속도 벡터를 의미한다. SO(3)-based 최적화에 대한 디테일한 내용은 reproejction 에러 섹션과 동일하므로 이에 대한 자세한 설명은 생략한다.
각속도 벡터 $\boldsymbol{\theta}$를 사용하면 기존의 쿼터니언 $\mathbf{q}$의 자코비안은 다음과 같이 변경된다.
\begin{equation}
\begin{aligned}
& \frac{\partial \mathbf{e}_{\mathcal{B}}}{\partial \mathbf{q}^{w}_{b_{k}}} \quad \rightarrow \quad \frac{\partial \mathbf{e}_{\mathcal{B}}}{\partial \begin{bmatrix} 1 & \frac{1}{2}\boldsymbol{\theta}^{w}_{b_{k}} \end{bmatrix}} \\
& \frac{\partial \mathbf{e}_{\mathcal{B}}}{\partial \mathbf{q}^{w}_{b_{k+1}}} \quad \rightarrow \quad \frac{\partial \mathbf{e}_{\mathcal{B}}}{\partial \begin{bmatrix} 1 & \frac{1}{2}\boldsymbol{\theta}^{w}_{b_{k+1}} \end{bmatrix}}
\end{aligned}
\end{equation}
임의의 angle-axis 벡터 $\boldsymbol{\theta} = \theta \mathbf{u}$가 주어졌을 때, 이에 대한 exponential map은 오일러 공식의 확장 버전으로 표현할 수 있다.
\begin{equation}
\mathbf{q} \triangleq \text{Exp}(\boldsymbol{\theta}) = \text{Exp}(\theta \mathbf{u}) = e^{\theta \mathbf{u}/2} = \cos\frac{\theta}{2} + \mathbf{u}\sin\frac{\theta}{2} = \begin{bmatrix}
\cos(\theta/2) \\ \mathbf{u} \sin(\theta/2)
\end{bmatrix}
\end{equation}
충분히 작은 $\boldsymbol{\theta}$값에 대해 $\cos\frac{\theta}{2} \approx 1$과 $\sin\frac{\theta}{2} \approx \frac{\theta}{2}$이 만족하므로 충분히 작은 쿼터니언에 대한 다음 식이 성립한다.
\begin{equation}
\mathbf{q} \approx \begin{bmatrix}
1 \\ \frac{1}{2} \boldsymbol{\theta}
\end{bmatrix}
\end{equation}
이에 대한 자세한 내용은 Quaternion kinematics for the error-state Kalman filter 내용 정리 포스트의 4.4 챕터를 참고하면 된다.
일반적으로 최적화에 사용하는 에러는 크기가 작으므로 $\gamma$에 대한 에러 $\Big( \hat{\gamma}^{b_{k}}_{b_{k+1}} \Big)^{-1} \otimes (\mathbf{q}^{w}_{b_{k}})^{-1} \otimes \mathbf{q}^{w}_{b_{k+1}}$ 또한 크기가 작다고 가정한다. 따라서 실제 쿼터니언의 $\mathbf{q} = [w,x,y,z]$ 중에서 허수 부분 $[x,y,z] = \frac{1}{2}\boldsymbol{\theta}$만 최적화에 사용한다. 이를 통해 $\gamma$ 부분은 다음과 같이 변형된다.
\begin{equation}
\begin{aligned}
\gamma \quad & \rightarrow \quad 2[\gamma]_{xyz} = 2[x,y,z] = \boldsymbol{\theta} \\
\Big( \hat{\gamma}^{b_{k}}_{b_{k+1}} \Big)^{-1} \otimes (\mathbf{q}^{w}_{b_{k}})^{-1} \otimes \mathbf{q}^{w}_{b_{k+1}} \quad & \rightarrow \quad 2 \bigg[ \Big( \hat{\gamma}^{b_{k}}_{b_{k+1}} \Big)^{-1} \otimes (\mathbf{q}^{w}_{b_{k}})^{-1} \otimes \mathbf{q}^{w}_{b_{k+1}} \bigg]_{xyz}
\end{aligned}
\end{equation}
최종적인 SO(3) 버전 IMU measurement 에러 $\mathbf{e}_{\mathcal{B}}$는 다음과 같다.
\begin{equation}
\boxed{\begin{aligned}
\mathbf{e}_{\mathcal{B}}(\mathcal{X}_{k}) = \begin{bmatrix}
\mathbf{R}^{b_{k}}_{w}(\mathbf{p}^{w}_{b_{k+1}} - \mathbf{p}^{w}_{b_{k}} - \mathbf{v}^{w}_{b_{k}}\Delta t_{k} + \frac{1}{2} \mathbf{g}^{w}\Delta t_{k}^{2}) - \hat{\alpha}^{b_{k}}_{b_{k+1}} \\
2 \bigg[ \Big( \hat{\gamma}^{b_{k}}_{b_{k+1}} \Big)^{-1} \otimes (\mathbf{q}^{w}_{b_{k}})^{-1} \otimes \mathbf{q}^{w}_{b_{k+1}} \bigg]_{xyz}\\
\mathbf{R}^{b_{k}}_{w} ( \mathbf{v}^{w}_{b_{k+1}} - \mathbf{v}^{w}_{b_{k}} + \mathbf{g}^{w}\Delta t_{k}) - \hat{\beta}^{b_{k}}_{b_{k+1}} \\
\mathbf{b}_{a_{k+1}} - \mathbf{b}_{a_{k}} \\
\mathbf{b}_{g_{k+1}} - \mathbf{b}_{g_{k}} \\
\end{bmatrix}
\end{aligned} }
\end{equation}
$[\mathbf{p}, \mathbf{q}], [\mathbf{v}, \mathbf{b}_{a}, \mathbf{b}_{g}]$에 대한 자코비안을 편하게 계산하기 위해 기존 상태 변수의 두번째 줄 $\beta$와 세번째 줄 $\gamma$의 순서를 서로 변경하였다.
최종적으로 SO(3) 버전 IMU measurement 에러의 자코비안은 다음과 같이 구할 수 있다. 자세한 유도 과정은 Formula Derivation and Analysis of the VINS-Mono 논문의 Appendix 섹션을 참고하면 된다.
\begin{equation}
\boxed{ \begin{aligned}
\mathbf{J}[0]_{15\times 6} = \frac{\partial \mathbf{e}_{\mathcal{B}}}{\partial [\mathbf{p}^{w}_{b_{k}}, \mathbf{q}^{w}_{b_{k}}]} = \begin{bmatrix}
-\mathbf{R}^{b_{k}}_{w} & [\mathbf{R}^{b_{k}}_{w}(\mathbf{p}^{w}_{b_{k+1}} - \mathbf{p}^{w}_{b_{k}} - \mathbf{v}^{w}_{b_{k}}\Delta t_{k} + \frac{1}{2} \mathbf{g}^{w}\Delta t_{k}^{2})]_{\times} \\
0 & [\gamma^{b_{k}}_{b_{k+1}}]_{R}[(\mathbf{q}^{w}_{b_{k+1}})^{-1} \otimes \mathbf{q}^{b_{k}}_{w}]_{L, 3\times3} \\
0 & [\mathbf{R}^{b_{k}}_{w} (\mathbf{p}^{w}_{b_{k+1}} - \mathbf{p}^{w}_{b_{k}} + \mathbf{g}^{w} \Delta t_{k})]_{\times} \\
0&0\\
0&0
\end{bmatrix}
\end{aligned} }
\end{equation}
\begin{equation}
\boxed{ \begin{aligned}
\mathbf{J}[1]_{15\times9} = \frac{\partial \mathbf{e}_{\mathcal{B}} }{\partial [\mathbf{v}^{w}_{b_{k}}, \mathbf{b}_{a_{k}}, \mathbf{b}_{g_{k}}]} = \begin{bmatrix}
-\mathbf{R}^{b_{k}}_{w} \Delta t_{k} & -\mathbf{J}^{\alpha}_{b_{a}} & -\mathbf{J}^{\alpha}_{b_{g}} \\
0&0& -[(\hat{\gamma}^{b_{k}}_{b_{k+1}})^{-1} \otimes (\mathbf{q}^{w}_{b_{k}})^{-1} \otimes \mathbf{q}^{w}_{b_{k+1}}]_{R,3\times3} \mathbf{J}^{\gamma}_{b_{g}} \\
-\mathbf{R}^{b_{k}}_{w} & -\mathbf{J}^{\beta}_{b_{a}} & -\mathbf{J}^{\beta}_{b_{g}} \\
0 & - \mathbf{I} & 0 \\
0 & 0 &-\mathbf{I}
\end{bmatrix}
\end{aligned} }
\end{equation}
\begin{equation}
\boxed{ \begin{aligned}
\mathbf{J}[2]_{15\times 6} = \frac{\partial \mathbf{e}_{\mathcal{B}} }{\partial [\mathbf{p}^{w}_{b_{k+1}}, \mathbf{q}^{w}_{b_{k+1}}]} = \begin{bmatrix}
\mathbf{R}^{b_{k}}_{w} & 0 \\
0 & [(\hat{\gamma}^{b_{k}}_{b_{k+1}})^{-1} \otimes (\mathbf{q}^{w}_{b_{k}})^{-1} \otimes \mathbf{q}^{w}_{b_{k+1}}]_{L} \\
0&0\\
0&0\\
0&0
\end{bmatrix}
\end{aligned} }
\end{equation}
\begin{equation}
\boxed{ \begin{aligned}
\mathbf{J}[3]_{15\times9} = \frac{\partial \mathbf{e}_{\mathcal{B}} }{\partial [\mathbf{v}^{w}_{b_{k+1}}, \mathbf{b}_{a_{k+1}}, \mathbf{b}_{g_{k+1}}]} = \begin{bmatrix}
0&0&0\\
0&0&0\\
\mathbf{R}^{b_{k}}_{w} &0&0 \\
0&\mathbf{I}&0 \\
0&0&\mathbf{I}
\end{bmatrix}
\end{aligned} }
\end{equation}
NOTICE: 기존의 $\mathbf{J}[0], \mathbf{J}[2] \in \mathbb{R}^{15 \times 7}$이었으나 쿼터니언을 SO(3) 기반으로 업데이트하면서 $[xyz]$ 부분만 사용하므로 $w$ 부분은 항상 0이된다. $w$ 부분을 생략하여 표기하면 $\mathbf{J}[0], \mathbf{J}[2] \in \mathbb{R}^{15 \times 6}$이 된다.
NOTICE: 위 식을 보면 자코비안 안에 또다른 자코비안 $\mathbf{J}^{\alpha}_{b_{a}}, \mathbf{J}^{\alpha}_{b_{g}}, \mathbf{J}^{\beta}_{b_{a}}, \mathbf{J}^{\beta}_{b_{g}} \mathbf{J}^{\gamma}_{b_{g}}$이 사용된 것을 알 수 있다. 이는 IMU의 에러 상태 방정식(error-state equation)에서 파생된 자코비안 $\mathbf{J}^{b_{k}}_{b_{k+1}}$의 부분 자코비안을 의미한다.
IMU의 이산(discrete) 신호에 대한 에러 상태 방정식은 다음과 같다. (Mid-point 근사 방법 사용)
\begin{equation}
\begin{aligned}
\begin{bmatrix}
\delta \alpha_{k+1} \\
\delta \boldsymbol{\theta}_{k+1} \\
\delta \beta_{k+1} \\
\delta \mathbf{b}_{a_{k+1}} \\
\delta \mathbf{b}_{g_{k+1}} \\
\end{bmatrix}
= \begin{bmatrix}
\mathbf{I} & \mathbf{F}_{01} & \delta t \mathbf{I} & \mathbf{F}_{03} & \mathbf{F}_{04} \\
& \mathbf{F}_{11} & & & -\delta t \mathbf{I} \\
& \mathbf{F}_{21} & \mathbf{I} & \mathbf{F}_{23} & \mathbf{F}_{24} \\
&&&\mathbf{I}& \\
&&&&\mathbf{I}
\end{bmatrix}
\begin{bmatrix}
\delta \alpha_{k} \\
\delta \boldsymbol{\theta}_{k} \\
\delta \beta_{k} \\
\delta \mathbf{b}_{a_{k}} \\
\delta \mathbf{b}_{g_{k}} \\
\end{bmatrix} +
\begin{bmatrix}
\mathbf{G}_{00} & \mathbf{G}_{01} & \mathbf{G}_{02} & \mathbf{G}_{03} & & \\
& -\delta t /2 \mathbf{I} & & -\delta t /2 \mathbf{I} & & \\
-\frac{\mathbf{R}_{k}\delta t}{2} & \mathbf{G}_{21} & -\frac{\mathbf{R}_{k+1}\delta t}{2} & \mathbf{G}_{23} & & \\
&&&&\delta t \mathbf{I} & \\
&&&& & \delta t \mathbf{I}
\end{bmatrix}
\begin{bmatrix}
\mathbf{n}_{a_{k}} \\
\mathbf{n}_{g_{k}} \\
\mathbf{n}_{a_{k+1}} \\
\mathbf{n}_{g_{k+1}} \\
\mathbf{n}_{b_{a}} \\
\mathbf{n}_{b_{g}} \\
\end{bmatrix}
\end{aligned}
\end{equation}
이 때 상태 변수에 대한 자코비안 $\mathbf{J}^{b_{k}}_{t}$은 다음과 같이 업데이트 된다.
\begin{equation}
\begin{aligned}
\mathbf{J}^{b_{k}}_{t+\delta t} = (\mathbf{I} + \mathbf{F}_{t}\delta t)\mathbf{J}^{b_{k}}_{t}, \ \ t \in [k,k+1]
\end{aligned}
\end{equation}
보다 자세한 내용은 [SLAM] Formula Derivation and Analysis of the VINS-mono 내용 정리 포스트의 2.3, 2.4 섹션을 참고하면 된다.
7.3. Code implementations
- VINS-mono 코드: integration_base.h#L180
- SO(3) 버전 IMU measurement 에러 $\mathbf{e}_{\mathcal{B}}$가 구현되어 있다.
- VINS-mono 코드: imu_factor.h#L86
- $\mathbf{J}[0], \mathbf{J}[1], \mathbf{J}[2], \mathbf{J}[3]$가 구현되어 있다.
- 자코비안과 에러 함수에 공분산의 제곱근 역함수 $\sqrt{(\mathbf{P}^{b_{k}}_{b_{k+1}})^{-1}} = \sqrt{\boldsymbol{\Omega}_{\mathcal{B}}}$이 information 행렬의 형태로 곱해진다.
- $\mathbf{e}_{_{\mathcal{B}},k} \quad \rightarrow \quad \sqrt{\boldsymbol{\Omega}_{\mathcal{B}}}^{\intercal}\mathbf{e}_{_{\mathcal{B}},k}$: 실제 코드에서는 오른쪽 에러항이 최적화된다.
- 이는 실제 코드 구현에서 에러 함수 $\mathbf{E}_{_{\mathcal{B}}}(\mathcal{X}) = \mathbf{e}_{\mathcal{B},k}^{\intercal}\boldsymbol{\Omega}_{\mathcal{B}}\mathbf{e}_{_{\mathcal{B}},k}$의 제곱근 $\sqrt{\boldsymbol{\Omega}_{\mathcal{B}}}^{\intercal}\mathbf{e}_{_{\mathcal{B}},k}$을 에러로 설정했기 떄문이다.
- VINS-mono 코드: integration_base.h#L90
- 에러 상태 방정식을 Mid-point 방법으로 근사한 $\mathbf{F}, \mathbf{G}$ 상태천이행렬이 구현되어 있다.
- IMU 상태 변수의 자코비안 업데이트 공식 $\mathbf{J}^{b_{k}}_{t+\delta t} = (\mathbf{I} + \mathbf{F}_{t}\delta t)\mathbf{J}^{b_{k}}_{t}$이 구현되어 있다.
- IMU 상태 변수의 공분산 업데이트 공식 $\mathbf{P}^{b_{k}}_{t+\delta t} = (\mathbf{I} + \mathbf{F}_{t}\delta t)\mathbf{P}^{b_{k}}_{t}(\mathbf{I}+\mathbf{F}_{t}\delta t)^{\intercal} + (\mathbf{G}_{t}\delta t) \mathbf{Q} (\mathbf{G}_{t}\delta t)^{\intercal}$이 구현되어 있다.
8. Other jacobians
8.1. Jacobian of unit quaternion
NOMENCLATURE of jacobian of unit quaternion
- $\mathbf{X} = [X,Y,Z,1]^{\intercal} = [\tilde{\mathbf{X}}, 1]^{\intercal} \in \mathbb{P}^{3}$
- $\tilde{\mathbf{X}} = [X,Y,Z]^{\intercal} \in \mathbb{P}^{2}$
- $\mathbf{q} = [w,x,y,z]^{\intercal} = [w, \mathbf{v}]^{\intercal}$
- hamilton 표기법으로 표현한 쿼터니언. 이에 대한 자세한 내용은 해당 포스트를 참조하면 된다.
앞서 reprojection 에러 섹션에서 설명했던 자코비안은 다음과 같다.
\begin{equation}
\begin{aligned}
\mathbf{J}_{c}& = \frac{\partial \hat{\mathbf{p}}}{\partial \tilde{\mathbf{p}}}
\frac{\partial \tilde{\mathbf{p}}}{\partial \mathbf{X}'}
\frac{\partial \mathbf{X}'}{\partial [\mathbf{R}, \mathbf{t}]} \\
\end{aligned}
\end{equation}
이 중, $\frac{\partial \mathbf{X}'}{\partial \mathbf{R}}$은 회전을 회전행렬 $\mathbf{R}$로 표현하였을 때 사용할 수 있는 자코비안이다. 해당 섹션에서는 회전을 단위 쿼터니언 $\mathbf{q}$로 표현하였을 때 사용할 수 있는 자코비안 $\frac{\partial \mathbf{X}'}{\partial \mathbf{q}}$에 대해 설명한다.
3차원 공간 상의 점 $\mathbf{X}$이 주어졌을 때 임의의 단위 쿼터니언 $\mathbf{q}$을 통해 회전한 점 $\mathbf{X}'$는 다음과 같이 나타낼 수 있다.
\begin{equation}
\begin{aligned}
\tilde{\mathbf{X}'} & = \mathbf{q} \otimes \tilde{\mathbf{X}} \otimes \mathbf{q}^{*} \end{aligned}
\end{equation}
이를 다시 풀어서 전개하면 다음과 같다.
\begin{equation}
\begin{aligned}
\tilde{\mathbf{X}'} & = \mathbf{q} \otimes \tilde{\mathbf{X}} \otimes \mathbf{q}^{*} \\
& = (w+\mathbf{v}) \otimes \tilde{\mathbf{X}} \otimes (w-\mathbf{v}) \\
& = w^{2}\tilde{\mathbf{X}} + w(\mathbf{v}\otimes \tilde{\mathbf{X}} - \tilde{\mathbf{X}}\otimes \mathbf{v}) - \mathbf{v}\otimes \tilde{\mathbf{X}} \otimes \mathbf{v} \\
& = w^{2}\tilde{\mathbf{X}}+2w(\mathbf{v}\times \tilde{\mathbf{X}})- [(-\mathbf{v}^{\intercal}\tilde{\mathbf{X}} + \mathbf{v}\times \tilde{\mathbf{X}}) \otimes \mathbf{v}] \\
& = w^{2}\tilde{\mathbf{X}} + 2w(\mathbf{v}\times \tilde{\mathbf{X}}) -[(-\mathbf{v}^{\intercal}\tilde{\mathbf{X}})\mathbf{v} + (\mathbf{v}\times \tilde{\mathbf{X}})\otimes \mathbf{v}] \\
& = w^{2}\tilde{\mathbf{X}} + 2w(\mathbf{v} \times \tilde{\mathbf{X}}) - [(-\mathbf{v}^{\intercal}\tilde{\mathbf{X}})\mathbf{v} - \cancel{ (\mathbf{v}\times \tilde{\mathbf{X}})^{\intercal}\mathbf{v} } +(\mathbf{v} \times \tilde{\mathbf{X}})\times \mathbf{v}] \\
& = w^{2}\tilde{\mathbf{X}} + 2w(\mathbf{v} \times \tilde{\mathbf{X}}) - [(-\mathbf{v}^{\intercal}\tilde{\mathbf{X}})\mathbf{v} + (\mathbf{v}^{\intercal}\mathbf{v})\tilde{\mathbf{X}} - (\mathbf{v}^{\intercal}\tilde{\mathbf{X}})\mathbf{v}] \\
& = w^{2}\tilde{\mathbf{X}} + 2w(\mathbf{v}\times \tilde{\mathbf{X}}) + 2(\mathbf{v}^{\intercal}\tilde{\mathbf{X}})\mathbf{v} - (\mathbf{v}^{\intercal}\mathbf{v})\tilde{\mathbf{X}}
\end{aligned}
\end{equation}
이를 사용하여 쿼터니언에 대한 자코비안 $\frac{\partial \tilde{\mathbf{X}'}}{\partial \mathbf{q}}$을 구할 수 있다. 스칼라 파트 $\frac{\partial \tilde{\mathbf{X}'}}{\partial w}$와 벡터 파트 $\frac{\partial \tilde{\mathbf{X}'}}{\partial \mathbf{v}}$로 나누어 구하면 다음과 같다.
\begin{equation}
\begin{aligned}
& \frac{\partial \tilde{\mathbf{X}'}}{\partial w} && =2(w \tilde{\mathbf{X}} + \mathbf{v} \times \tilde{\mathbf{X}}) \\
& \frac{\partial \tilde{\mathbf{X}'}}{\partial \mathbf{v}} && = -2w[\tilde{\mathbf{X}}]_{\times} + 2(\mathbf{v}^{\intercal}\tilde{\mathbf{X}}\mathbf{I} + \mathbf{v}\tilde{\mathbf{X}}^{\intercal}) - 2 \tilde{\mathbf{X}}\mathbf{v}^{\intercal} \\
& && = 2(\mathbf{v}^{\intercal}\tilde{\mathbf{X}}\mathbf{I} + \mathbf{v}\tilde{\mathbf{X}}^{\intercal} - \tilde{\mathbf{X}}\mathbf{v}^{\intercal} - w[\tilde{\mathbf{X}}]_{\times})
\end{aligned}
\end{equation}
이 때, 쿼터니언 곱셈 중앙에 들어가는 $\tilde{\mathbf{X}}$는 실제로는 3차원 벡터가 들어가지 않고 스칼라 값이 0인 순수 쿼터니언(pure quaternion) $[0, X, Y, Z]^{\intercal}$ 형태로 변형되어 들어간다. 따라서 위 식에서 스칼라에 대한 자코비안 $\frac{\partial \tilde{\mathbf{X}'}}{\partial w}$는 실제 최적화 수행 시 사용되지 않기 때문에 별도로 구하지 않고 벡터에 대한 자코비안 $\frac{\partial \tilde{\mathbf{X}'}}{\partial \mathbf{v}}$만 구한다.
\begin{equation}
\begin{aligned}
\tilde{\mathbf{X}'} = \mathbf{q} \otimes \tilde{\mathbf{X}} \otimes \mathbf{q}^{*} \quad \rightarrow \quad \begin{bmatrix} 0 \\ \tilde{\mathbf{X}'} \end{bmatrix} & = \mathbf{q} \otimes \begin{bmatrix} 0 \\ \tilde{\mathbf{X}} \end{bmatrix} \otimes \mathbf{q}^{*} \quad \cdots \text{ strict notation} \\
\text{Then, } \frac{\partial \tilde{\mathbf{X}'}}{\partial w} &\text{ is going to be useless}
\end{aligned}
\end{equation}
또한, 충분히 작은 회전행렬을 $\mathbf{R} \approx \mathbf{I} + [\mathbf{w}]_{\times}$로 근사했던 방법과 동일하게 쿼터니언 $\mathbf{q}$이 충분히 작다고 가정하면 이는 identity로 근사할 수 있다($\mathbf{q} \approx \mathbf{q}_{1} = [1, 0, 0, 0]^{\intercal}$).
\begin{equation}
\begin{aligned}
& \left. \frac{\partial \tilde{\mathbf{X}'}}{\partial \mathbf{v}}\right\vert_{\mathbf{q} \approx \mathbf{q}_{1}}
&& = 2(\mathbf{v}^{\intercal}\tilde{\mathbf{X}}\mathbf{I} + \mathbf{v}\tilde{\mathbf{X}}^{\intercal} - \tilde{\mathbf{X}}\mathbf{v}^{\intercal} - w[\tilde{\mathbf{X}}]_{\times}) \\ & && = -2[\tilde{\mathbf{X}}]_{\times}
\end{aligned}
\end{equation}
따라서 최종적인 쿼터니언에 대한 자코비안 $\frac{\partial \tilde{\mathbf{X}'}}{\partial \mathbf{q}}$은 다음과 같다.
\begin{equation}
\boxed {\begin{aligned}
\frac{\partial \tilde{\mathbf{X}'}}{\partial \mathbf{q}} = -2[\tilde{\mathbf{X}}]_{\times} = -2 \begin{bmatrix} 0 & -Z & Y \\ Z & 0 & -X \\ -Y & X & 0 \end{bmatrix} \in \mathbb{R}^{3\times3}
\end{aligned} }
\end{equation}
8.1.1. Code Implementations
- ProSLAM 코드: trajectory_analyzer.cpp#L253
- jinyongjeong님의 블로그 포스트를 참고하였다.
8.2. Jacobian of camera intrinsics
NOMENCLATURE of jacobian of camera intrinsics
- $\pi^{-1}(\cdot) = Z\mathbf{K}^{-1}(\cdot)$: 이미지 상의 점을 3차원 공간 상에 back projection하는 함수
- $\pi(\cdot) = \pi_{k}(\pi_{h}(\cdot)) = \mathbf{K}(\frac{1}{Z} \cdot)$: 3차원 공간 상의 점을 이미지 평면 상에 프로젝션하는 함수
- $\mathbf{K} = \begin{bmatrix} f_{x} & 0 & c_{x} \\ 0 & f_{y} & c_{y} \\ 0 & 0 & 1 \end{bmatrix}$: 카메라 내부(intrinsic) 파라미터
- $\mathbf{K}^{-1} = \begin{bmatrix} f_x^{-1} & 0 & -f_x^{-1}c_{x} \\ 0 & f_{y}^{-1} & -f_{y}^{-1}c_{y} \\ 0&0&1 \end{bmatrix}$
- $\tilde{\mathbf{K}} = \begin{bmatrix} f_{x} & 0 & c_{x} \\ 0 & f_{y} & c_{y} \end{bmatrix}$: $\mathbb{P}^{2} \rightarrow \mathbb{R}^{2}$로 프로젝션하기 위해 내부 파라미터의 마지막 행을 생략했다.
- $\mathbf{X} = [\tilde{\mathbf{X}}, 1]^{\intercal}$
SLAM을 수행하기 위해 카메라 캘리브레이션을 수행하면 내부 파라미터(intrinsic matrix) $\mathbf{c} = [f_x, f_y, c_x, c_y]$와 렌즈 왜곡 파라미터 $\mathbf{d} = [k_1, k_2, p_1, p_2]$를 구할 수 있다. 하지만 캘리브레이션 값이 정확히 실제 센서의 파라미터와 일치하지는 않으므로 이를 최적화를 통해 fine tuning할 수 있다. 본 섹션에서는 이 중 $\mathbf{c}$에 대한 자코비안 $\mathbf{J}_{\mathbf{c}}$을 유도하는 과정에 대해 설명한다. 이 때, 초점 거리(focal length)는 $f_{x} \neq f_{y}$라고 가정한다.
예를 들어, (\ref{eq:photo3}) photometric 에러 대한 $\mathbf{J}_{\mathbf{c}}$를 구한다고 가정해보자. 이는 다음과 같이 나타낼 수 있다.
\begin{equation}
\begin{aligned}
\mathbf{J}_{\mathbf{c}} & = \frac{\partial \mathbf{e}}{\partial \mathbf{c}} \\ & = \frac{\partial \mathbf{I}}{\partial \mathbf{p}_{2}} \frac{\partial \mathbf{p}_{2}}{\partial \tilde{\mathbf{p}}_{2}} \frac{\partial \tilde{\mathbf{p}}_{2}}{\partial \mathbf{X}'} \frac{\partial \mathbf{X}'}{\partial \mathbf{c}} \\
& = \mathbb{R}^{1\times2} \cdot \mathbb{R}^{2\times3} \cdot \mathbb{R}^{3\times4} \cdot \mathbb{R}^{4\times4} = \mathbb{R}^{1\times4}
\end{aligned}
\end{equation}
이 때, 맨 앞의 $\frac{\partial \mathbf{I}}{\partial \mathbf{p}_{2}}$ 항은 photometric 에러를 구하기 위해 구해야 하는 자코비안이고 나머지 세 자코비안은 reprojection, photometric 에러 항과 관계없이 항상 구해야하는 항이다. 따라서 $\frac{\partial \mathbf{p}_{2}}{\partial\tilde{\mathbf{p}}_{2}}\frac{\partial \tilde{\mathbf{p}}_{2}}{\partial \mathbf{X}'}\frac{\partial \mathbf{X}'}{\partial \mathbf{c}}$ 를 구하면 일반적으로 SLAM에서 사용하는 에러 항에 대해 모두 적용이 가능하다.
두 카메라 $\{C_{1}\}, \{C_{2}\}$의 이미지 평면 상의 점 $\mathbf{p}_{1}, \mathbf{p}_{2}$의 관계는 다음과 같이 풀어 쓸 수 있다.
\begin{equation}
\begin{aligned}
\mathbf{p}_{1} & =\begin{bmatrix} u_{1} & v_{1} \end{bmatrix}^{\intercal} \\ \mathbf{p}_{2} & =\begin{bmatrix} u_{2} & v_{2} \end{bmatrix}^{\intercal}
\end{aligned}
\end{equation}
\begin{equation}
\begin{aligned}
\mathbf{p}_{2} & = \pi(\mathbf{X}') \\
& = \pi(\mathbf{RX} + \mathbf{t}) \\
& = \pi(\mathbf{R}\pi^{-1}(\mathbf{p}_{1}) + \mathbf{t}) \quad \cdots \text{ apply back-proj}\\
& = \pi(\mathbf{R}(Z\mathbf{K}^{-1}\mathbf{p}_{1}) + \mathbf{t}) \\
& = \pi_{k}(\pi_{h}(\mathbf{R}(Z\mathbf{K}^{-1}\mathbf{p}_{1}) + \mathbf{t})) \\
& = \pi_{k}(\frac{Z}{Z'}\mathbf{R}\mathbf{K}^{-1}\mathbf{p}_{1} + \frac{1}{Z'}\mathbf{t}) \quad \cdots \text{ apply } \pi_{h}(\cdot) \\
&= \frac{Z}{Z'} \tilde{\mathbf{K}}\mathbf{R}\mathbf{K}^{-1}\mathbf{p}_{1} + \frac{1}{Z'}\tilde{\mathbf{K}}\mathbf{t} \quad \cdots \text{ apply } \pi_{k}(\cdot)
\end{aligned}
\end{equation}
$\mathbf{p}_{1}$에 back projection $\rightarrow$ 변환행렬 적용 $\rightarrow$ 프로젝션이 연쇄적으로 발생하여 $\mathbf{p}_{2}$가 되기 때문에 위와 같은 복잡한 형태의 공식이 얻어진다. 위 식에서 보다시피 $\frac{\partial \mathbf{p}_{2}}{\partial\tilde{\mathbf{p}}_{2}}\frac{\partial \tilde{\mathbf{p}}_{2}}{\partial \mathbf{X}'}\frac{\partial \mathbf{X}'}{\partial \mathbf{c}}$는 $\mathbf{p}_{2}$부터 $\mathbf{c}$ 파라미터를 포함한다. 따라서 세 자코비안을 묶어서 $\frac{\partial \mathbf{p}_{2}}{\partial \mathbf{c}}$를 한 번에 계산해야 한다.
\begin{equation}
\begin{aligned}
\frac{\partial \mathbf{p}_{2}}{\partial \mathbf{c}}
&= \frac{\partial} {\partial \mathbf{c}} \begin{bmatrix} u_{2} \\ v_{2} \end{bmatrix} \\ &= \frac{\partial} {\partial [f_x, f_y, c_x, c_y]} \begin{bmatrix} f_{x}\tilde{u}_{2} + c_{x} \\ f_{y}\tilde{v}_{2} + c_{y} \end{bmatrix} \\
& = \begin{bmatrix}
\frac{\partial u_{2}}{\partial f_{x}} & \frac{\partial u_{2}}{\partial f_{y}}
& \frac{\partial u_{2}}{\partial c_{x}}
& \frac{\partial u_{2}}{\partial c_{y}}
\\
\frac{\partial v_{2}}{\partial f_{x}} & \frac{\partial v_{2}}{\partial f_{y}}
& \frac{\partial v_{2}}{\partial c_{x}}
& \frac{\partial v_{2}}{\partial c_{y}}
\end{bmatrix} \\
&=
\begin{bmatrix}
\tilde{u}_{2} + f_{x}\frac{\partial \tilde{u}_{2}}{\partial f_{x}}
& f_{x}\frac{\partial \tilde{u}_{2}}{\partial f_{y}}
& f_{x}\frac{\partial \tilde{u}_{2}}{\partial c_{x}} + 1
& f_{x}\frac{\partial \tilde{u}_{2}}{\partial c_{y}}
\\
f_{y}\frac{\partial \tilde{v}_{2}}{\partial f_{x}}
& \tilde{v}_{2} + f_{y}\frac{\partial \tilde{v}_{2}}{\partial f_{y}}
& f_{y}\frac{\partial \tilde{v}_{2}}{\partial c_{x}}
& f_{y}\frac{\partial \tilde{v}_{2}}{\partial c_{y}}+1
\end{bmatrix} \in \mathbb{R}^{2\times4}
\end{aligned} \label{eq:intrinsic2}
\end{equation}
다음으로 위 식의 원소를 계산해야 한다.
\begin{equation} \begin{aligned}
\begin{pmatrix} \frac{\partial \tilde{u}_{2}}{\partial f_{x}} & \frac{\partial \tilde{u}_{2}}{\partial f_{y}} & \frac{\partial \tilde{u}_{2}}{\partial c_{x}} & \frac{\partial \tilde{u}_{2}}{\partial c_{y}} \\ \frac{\partial \tilde{v}_{2}}{\partial f_{x}} & \frac{\partial \tilde{v}_{2}}{\partial f_{y}} & \frac{\partial \tilde{v}_{2}}{\partial c_{x}} & \frac{\partial \tilde{v}_{2}}{\partial c_{y}} \end{pmatrix}
\end{aligned} \label{eq:intrinsic1} \end{equation}
이를 구하기 위해 우선 $\tilde{\mathbf{p}}_{2} = [\tilde{u}_{2}, \tilde{v}_{2}, 1]^{\intercal}$을 구하면 다음과 같다.
\begin{equation}
\begin{aligned}
\tilde{\mathbf{p}}_{2} & = [\tilde{u}_{2}, \tilde{v}_{2}, 1]^{\intercal} \\ & = \frac{1}{Z'}\tilde{\mathbf{X}}' \\
&=
\frac{1}{Z'}(\mathbf{R}\tilde{\mathbf{X}} + \mathbf{t}) \\
&=
\frac{Z}{Z'}\mathbf{R}\mathbf{K}^{-1}\mathbf{p}_{1} + \frac{1}{Z'}\mathbf{t} \\
&=
\frac{Z}{Z'}
\begin{bmatrix}
& & \\
& \mathbf{R} & \\
& &
\end{bmatrix}
\begin{bmatrix}
f_{x}^{-1}& & -f_{x}^{-1}c_{x}\\
& f_{y}^{-1} & -f_{y}^{-1}c_{y} \\
& & 1
\end{bmatrix}
\begin{bmatrix}
u_{1} \\ v_{1} \\ 1
\end{bmatrix}
+ \frac{1}{Z'}
\begin{bmatrix}
t_{x} \\ t_{y} \\ t_{z}
\end{bmatrix} \\
&=
\frac{Z}{Z'}
\begin{bmatrix}
& & \\
& \mathbf{R} & \\
& &
\end{bmatrix}
\begin{bmatrix}
f_{x}^{-1}(u_{1}-c_{x}) \\
f_{y}^{-1}(v_{1}-c_{y}) \\
1
\end{bmatrix}
+ \frac{1}{Z'}
\begin{bmatrix}
t_{x} \\ t_{y} \\ t_{z}
\end{bmatrix} \\
&=
\frac{Z}{Z'}
\begin{bmatrix}
r_{11}f_{x}^{-1}(u_{1}-c_{x}) + r_{12}f_{y}^{-1}(v_{1}-c_{y}) + r_{13} \\
r_{21}f_{x}^{-1}(u_{1}-c_{x}) + r_{22}f_{y}^{-1}(v_{1}-c_{y}) + r_{23} \\
r_{31}f_{x}^{-1}(u_{1}-c_{x}) + r_{32}f_{y}^{-1}(v_{1}-c_{y}) + r_{33}
\end{bmatrix}
+ \frac{1}{Z'}
\begin{bmatrix}
t_{x} \\ t_{y} \\ t_{z}
\end{bmatrix} \\
\end{aligned}
\end{equation}
위 식을 정리하면 다음과 같다.
\begin{equation} \begin{aligned} \begin{bmatrix} \tilde{u}_{2} \\ \tilde{v} _{2}\\1 \end{bmatrix} &= \begin{bmatrix} \frac {r_{11}f_{x}^{-1}(u_{1}-c_{x}) + r_{12}f_{y}^{-1}(v_{1}-c_{y}) + r_{13} + \frac{1}{Z}t_{x}} {r_{31}f_{x}^{-1}(u_{1}-c_{x}) + r_{32}f_{y}^{-1}(v_{1}-c_{y}) + r_{33} + \frac{1}{Z}t_{z}} \\ \frac {r_{21}f_{x}^{-1}(u_{1}-c_{x}) + r_{22}f_{y}^{-1}(v_{1}-c_{y}) + r_{23} + \frac{1}{Z}t_{y}} {r_{31}f_{x}^{-1}(u_{1}-c_{x}) + r_{32}f_{y}^{-1}(v_{1}-c_{y}) + r_{33} + \frac{1}{Z}t_{z}} \\ 1 \end{bmatrix} \end{aligned} \end{equation}
이를 바탕으로 (\ref{eq:intrinsic1})을 구해보면 다음과 같다.
\begin{equation} \begin{aligned}
\frac{\partial \tilde{u}_{2}}{\partial f_{x}} & = \frac{Z}{Z'}(r_{31}\tilde{u}_{2} - r_{11})f_{x}^{-2}(u_{1}-c_{x}) \\
\frac{\partial \tilde{u}_{2}}{\partial f_{y}} & = \frac{Z}{Z'}(r_{32}\tilde{u}_{2} - r_{12})f_{y}^{-2}(v_{1}-c_{y}) \\
\frac{\partial \tilde{u}_{2}}{\partial c_{x}} & = \frac{Z}{Z'}(r_{31}\tilde{u}_{2} - r_{11})f_{x}^{-1} \\
\frac{\partial \tilde{u}_{2}}{\partial c_{y}} & = \frac{Z}{Z'}(r_{32}\tilde{u}_{2} - r_{12})f_{y}^{-1} \\
\frac{\partial \tilde{v}_{2}}{\partial f_{x}} & = \frac{Z}{Z'}(r_{31}\tilde{v}_{2} - r_{21})f_{x}^{-2}(u_{1}-c_{x}) \\
\frac{\partial \tilde{v}_{2}}{\partial f_{y}} & = \frac{Z}{Z'}(r_{32}\tilde{v}_{2} - r_{22})f_{y}^{-2}(v_{1}-c_{y}) \\
\frac{\partial \tilde{v}_{2}}{\partial c_{x}} & = \frac{Z}{Z'}(r_{31}\tilde{v}_{2} - r_{21})f_{x}^{-1} \\
\frac{\partial \tilde{u}_{2}}{\partial c_{y}} & = \frac{Z}{Z'}(r_{32}\tilde{v}_{2} - r_{22})f_{y}^{-1}
\end{aligned} \end{equation}
최종적으로 (\ref{eq:intrinsic2})는 다음과 같다.
\begin{equation} \boxed{
\begin{aligned}
\frac{\partial \mathbf{p}_{2}}{\partial \mathbf{c}}
&=
\begin{bmatrix}
\frac{\partial u_{2}}{\partial f_{x}} & \frac{\partial u_{2}}{\partial f_{y}}
& \frac{\partial u_{2}}{\partial c_{x}}
& \frac{\partial u_{2}}{\partial c_{y}}
\\
\frac{\partial v_{2}}{\partial f_{x}} & \frac{\partial v_{2}}{\partial f_{y}}
& \frac{\partial v_{2}}{\partial c_{x}}
& \frac{\partial v_{2}}{\partial c_{y}}
\end{bmatrix} \\
&=
\begin{bmatrix}
\tilde{u}_{2} + f_{x}\frac{\partial \tilde{u}_{2}}{\partial f_{x}}
& f_{x}\frac{\partial \tilde{u}_{2}}{\partial f_{y}}
& f_{x}\frac{\partial \tilde{u}_{2}}{\partial c_{x}} + 1
& f_{x}\frac{\partial \tilde{u}_{2}}{\partial c_{y}}
\\
f_{y}\frac{\partial \tilde{v}_{2}}{\partial f_{x}}
& \tilde{v}_{2} + f_{y}\frac{\partial \tilde{v}_{2}}{\partial f_{y}}
& f_{y}\frac{\partial \tilde{v}_{2}}{\partial c_{x}}
& f_{y}\frac{\partial \tilde{v}_{2}}{\partial c_{y}}+1
\end{bmatrix} \\
& = \begin{bmatrix}
\tilde{u}_{2} + \frac{Z}{Z'}f_{x}^{-1}(r_{31}\tilde{u}_{2} - r_{11})(u_{1}-c_{x})&
\frac{Z}{Z'}f_{x}f_{y}^{-2}(r_{32}\tilde{u}_{2} - r_{12})(v_{1}-c_{y})&
\frac{Z}{Z'}(r_{31}\tilde{u}_{2} - r_{11}) + 1&
\frac{Z}{Z'}f_{x}f_{y}^{-1}(r_{32}\tilde{u}_{2} - r_{12})
\\
\frac{Z}{Z'}f_{x}^{-2}f_{y}(r_{31}\tilde{v}_{2} - r_{21})(u_{1}-c_{x})&
\tilde{v}_{2} + \frac{Z}{Z'}f_{y}^{-1}(r_{32}\tilde{v}_{2} - r_{22})(v_{1}-c_{y})&
\frac{Z}{Z'}f_{x}^{-1}f_{y}(r_{31}\tilde{v}_{2} - r_{21})&
\frac{Z}{Z'}(r_{32}\tilde{u}_{2} - r_{12}) + 1
\end{bmatrix} \in \mathbb{R}^{2\times4}
\end{aligned} }
\end{equation}
8.2.1. Code Implementations
- DSO 코드: Residuals.cpp#L123
- 코드에 대한 자세한 설명은 [SLAM] Direct Sparse Odometry (DSO) 논문 및 코드 리뷰 (2)를 참조하면 된다.
8.3. Jacobian of inverse depth
NOMENCLATURE of jacobian of inverse depth
- $\mathbf{X} = [X,Y,Z,1]^{\intercal} = [\tilde{\mathbf{X}}, 1]^{\intercal} \in \mathbb{P}^{3}$
- $\tilde{\mathbf{X}} = [X,Y,Z]^{\intercal} \in \mathbb{P}^{2}$
- $\rho = \frac{1}{Z}, \rho^{-1} = Z$
8.3.1. Inverse depth parameterization
SLAM에서 inverse depth parameterization란 3차원 점 $\mathbf{X}$를 표현할 때 3개의 파라미터 $[X,Y,Z,1]$을 사용하는 것이 아닌 1개의 파라미터 ($Z$의 역수 $\rho$)만 사용하여 표현하는 방법을 말한다. 이를 통해 이미지 평면 상의 픽셀 $\mathbf{p} = [u,v]$의 위치만 알고 있으면 오직 inverse depth $\rho$를 사용하여 3차원 점 $\mathbf{X}$를 완벽하게 표현할 수 있다. 이는 최적화를 수행할 때 1개의 파라미터만 추정하면 되므로 계산 상의 이점을 지닌다.
8.3.2. Jacobian of inverse depth
inverse depth의 자코비안을 $\mathbf{J}_{\rho}$라고 했을 때 photometric 에러에 대한 $\mathbf{J}_{\rho}$를 구한다고 가정해보자. 이는 다음과 같이 나타낼 수 있다.
\begin{equation}
\begin{aligned}
\mathbf{J}_{\rho} & = \frac{\partial \mathbf{e}}{\partial \rho} \\ & = \frac{\partial \mathbf{I}}{\partial \mathbf{p}_{2}} \frac{\partial \mathbf{p}_{2}}{\partial \tilde{\mathbf{p}}_{2}} \frac{\partial \tilde{\mathbf{p}}_{2}}{\partial \mathbf{X}'} \frac{\partial \mathbf{X}'}{\partial \rho} \\
& = \mathbb{R}^{1\times2} \cdot \mathbb{R}^{2\times3} \cdot \mathbb{R}^{3\times4} \cdot \mathbb{R}^{4\times1} = \mathbb{R}^{1\times1}
\end{aligned}
\end{equation}
이 때, 맨 앞의 $\frac{\partial \mathbf{I}}{\partial \mathbf{p}_{2}}$ 항은 photometric 에러를 구하기 위해 구해야 하는 자코비안이고 나머지 세 자코비안은 reprojection, photometric 에러 항과 관계없이 항상 구해야하는 항이다. 따라서 $\frac{\partial \mathbf{p}_{2}}{\partial\tilde{\mathbf{p}}_{2}}\frac{\partial \tilde{\mathbf{p}}_{2}}{\partial \mathbf{X}'}\frac{\partial \mathbf{X}'}{\partial \rho}$ 를 구하면 일반적으로 SLAM에서 사용하는 에러 항에 대해 모두 적용이 가능하다.
우선 $\frac{\partial \tilde{\mathbf{p}}_{2}}{\partial \mathbf{X}'}$를 inverse depth로 표현하면 아래와 같다. 이는 $\rho' = \frac{1}{Z'}$로 치환하여 표현한 것과 같다.
\begin{equation}
\boxed{ \begin{aligned}
\frac{\partial \tilde{\mathbf{p}}_{2}}{\partial \mathbf{X}'} & =
\frac{\partial [\tilde{u}_{2}, \tilde{v}_{2}, 1]}{\partial \mathbf{X}'} \\
& = \begin{bmatrix} \rho' & 0 & -\rho'^{2}X' & 0\\
0 & \rho' & -\rho'^{2}Y' &0 \\ 0 & 0 & 0 & 0 \end{bmatrix} \in \mathbb{R}^{3\times 4}
\end{aligned} }
\end{equation}
다음으로 $\frac{\partial \mathbf{X}'}{\partial \rho}$를 구해야 한다. 우선 $\mathbf{X}'$는 다음과 같이 풀어서 쓸 수 있다.
\begin{equation}
\begin{aligned}
\mathbf{X}' = \begin{bmatrix} \tilde{\mathbf{X}'} \\ 1 \end{bmatrix} & = \begin{bmatrix}\mathbf{R}\tilde{\mathbf{X}} + \mathbf{t} \\ 1 \end{bmatrix} \\ & = \begin{bmatrix} \mathbf{R}\big(Z\mathbf{K}^{-1}\tilde{\mathbf{X}} \big) + \mathbf{t} \\ 1 \end{bmatrix} \\ &= \begin{bmatrix} \mathbf{R}\big( \frac{\mathbf{K}^{-1}\tilde{\mathbf{X}}}{\rho} \big) + \mathbf{t} \\ 1 \end{bmatrix}
\end{aligned} \label{eq:invd1}
\end{equation}
위 식을 참고하여 $\frac{\partial \mathbf{X}'}{\partial \rho}$를 구하면 다음과 같다.
\begin{equation}
\boxed{ \begin{aligned}
\frac{\partial \mathbf{X}'}{\partial \rho} &= \begin{bmatrix}-\mathbf{R} \big( \frac{\mathbf{K}^{-1}\tilde{\mathbf{X}}}{\rho^{2}} \big) \\ 0 \end{bmatrix} \\
&= \begin{bmatrix} -\frac{\tilde{\mathbf{X}}' - \mathbf{t}}{\rho} \\ 0 \end{bmatrix} \\
& = -\rho^{-1}\begin{bmatrix} X' - t_{x} \\ Y' - t_{y} \\ Z' - t_{z} \\ 0 \end{bmatrix} \in \mathbb{R}^{4\times 1}
\end{aligned} }
\end{equation}
위 식에서 두 번째 줄은 (\ref{eq:invd1})을 변형하여 구할 수 있다. 위 두 자코비안을 사용하여 최종적으로 $\frac{\partial \mathbf{p}_{2}}{\partial \rho}$를 구하면 다음과 같다.
\begin{equation}
\boxed{ \begin{aligned}
\frac{\partial \mathbf{p}_{2}}{\partial \rho} & = \frac{\partial \mathbf{p}_{2}}{\partial\tilde{\mathbf{p}}_{2}}\frac{\partial \tilde{\mathbf{p}}_{2}}{\partial \mathbf{X}'}\frac{\partial \mathbf{X}'}{\partial \rho} \\
&= \begin{bmatrix} f_x & 0 & c_{x} \\ 0 & f_y & c_{y} \end{bmatrix} \begin{bmatrix} \rho' & 0 & -\rho'^{2}X' & 0\\
0 & \rho' & -\rho'^{2}Y' &0 \\ 0 & 0 & 0 & 0 \end{bmatrix} \cdot -\rho^{-1}\begin{bmatrix} X' - t_{x} \\ Y' - t_{y} \\ Z' - t_{z} \\ 0 \end{bmatrix} \\
&= -\rho^{-1}\rho' \begin{bmatrix} f_x(\tilde{u}_{2}t_{z} - t_{x}) \\ f_y(\tilde{v}_{2}t_{z} - t_{y}) \end{bmatrix} \in \mathbb{R}^{2 \times 1}
\end{aligned} }
\end{equation}
- $\tilde{u}_{2} = \frac{X'}{Z'} = \rho' X'$
- $\tilde{v}_{2} = \frac{Y'}{Z'} = \rho' Y'$
8.3.3. Code Implementations
- DSO 코드: CoarseInitializer.cpp#L424
- 코드에 대한 자세한 설명은 [SLAM] Direct Sparse Odometry (DSO) 논문 및 코드 리뷰 (2)를 참조하면 된다.
References
[1] [Blog] [SLAM] Bundle Adjustment 개념 리뷰: Reprojection error
[2] [Blog] [SLAM] Optical Flow와 Direct Method 개념 및 코드 리뷰: Photometric error
[3] [Blog] [SLAM] Pose Graph Optimization 개념 설명 및 예제 코드 분석: Relative pose error
[4] [Blog] Plücker Coordinate 개념 정리: Line projection error
[5] [Blog] [SLAM] Formula Derivation and Analysis of the VINS-mono 내용 정리: IMU measurement error
'Fundamental' 카테고리의 다른 글
| [SLAM] Hand-eye Calibration 개념 정리 (+ Trajectory Evaluation) (1) | 2023.11.10 |
|---|---|
| 3D 강체 변환(Rigid Body Transformation) 개념 정리 (6) | 2023.01.24 |
| [SLAM] 에러와 자코비안 정리(Errors and Jacobians) 정리 Part 1 (15) | 2023.01.08 |
| Quaternion kinematics for the error-state Kalman filter 내용 정리 Part 3 (6) | 2022.10.10 |
| [SLAM] 파티클 필터(Particle Filter) 개념 정리 (0) | 2022.10.07 |
