본문 바로가기

Fundamental

[SLAM] 에러와 자코비안 정리(Errors and Jacobians) 정리 Part 2

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

 

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

 

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

 

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