[SLAM] g2o - Alternative Parameterizations 논문 섹션 리뷰

본 포스트는 공부 목적으로 작성하였습니다.

혹시 보시는 도중 잘못된 부분이나 개선할 부분이 있다면 댓글이나 메일주시면 확인 후 수정하도록 하겠습니다.

해당 포스트는 앞서 pdf 파일로 정리했던 자료를 포스팅한 글입니다. 

자세한 내용은 다음 링크를 통해 확인해주세요.

https://www.facebook.com/groups/slamkr/permalink/894277280931917/

g2o_least_squares_on_a_manifold.pdf

0.62MB

 

 

 

본 포스트에서는 g2o 논문의 여러 섹션 중 "B. Alternative Parameterizations" 부분을 리뷰한다. 해당 부분은 A Tutorial on Graph-Based SLAM 논문에서 "C. Least Squares on a Manifold" 부분을 봐야 정확한 이해가 가능해서 해당 부분을 코드와 같이 리뷰한다. 

 

least squares 최적화를 수행할 때 회전과 관련된 표현법은 주로 over-parameterized (e.g. Rotation Matrix, Quaternion) 방법을 사용한다. 하지만 해당 이를 사용하여 최적화를 수행하는 경우 각각 회전 표현 방법들이 고유의 manifold를 유지하기 위한 제약조건을 가지고 있는데 최적화 도중 이러한 manifold 제약조건들이 위배되어 에러가 증가하는 경우가 발생한다. 이를 사용하지 않고 Euler Angle(3 params)로 최적화를 수행하게 되면 최적화 도중 singularity에 쉽게 빠지는 경향을 보이게 된다.

 

대표적으로 quaternion이 3차원 공간 상 회전을 표현하기 위한 manifold 제약조건은 'unit quaternion'을 유지하는 것이다. 즉, 수 많은 quaternion들 중 unit quaternion만이 3차원 회전을 표현할 때 사용된다.

$$\mathbf{q} = \left [ q_{x} \ q_{y} \  q_{z} \ q_{w} \right ]\text{    , where    } | \mathbf{q} | = 1$$

 

하지만 over-parameterized 표현법을 사용하는 경우 least squares로 증분량을 업데이트하면서 종종 unit quaternion 제약조건이 위배되는 경우가 발생한다. 따라서 g2o 논문에서는 위 문제를 해결하기 위해 quaternion의 벡터 부분만 최적화에 사용하였다. 이렇게 최적화 증분량은 minimal quaternion으로 표기하고 업데이트는 $\boxplus$ 연산자를 통해 full quaternion과 합해지는 방법을 사용함으로써 over-parameterized의 제약없이 최적화를 수행하였다. 
$$\Delta \mathbf{x} \mapsto \mathbf{x} \boxplus \Delta \mathbf{x}$$

Least Squares on a Manifold

- $\breve{\mathbf{x}} $ : 알고 있는 로봇의 포즈 (over-parameterized)

- $\Delta \mathbf{x}$ : $\breve{\mathbf{x}} $ 주변에서 로봇 포즈의 작은 변화량 (over-parameterized)

- $\Delta \tilde{\mathbf{x}}$ : $\breve{\mathbf{x}} $ 주변에서 로봇 포즈의 작은 변화량 (minial representation)

 

$\boxplus$ 연산자에 대해 자세하게 설명하면 다음과 같다. 

 

$\breve{\mathbf{x}} $의 회전이 full quaternion (over-parameterized)로 표현된 경우 $\breve{\mathbf{x}} $와 $\Delta \tilde{\mathbf{x}}$는 아래와 같이 표현할 수 있다. 

$$\breve{\mathbf{x}}^{T} = (\breve{\mathbf{t}}^{T} \ \breve{\mathbf{q}}^{T})$$

$$\Delta \tilde{\mathbf{x}}^{T} = (\Delta \tilde{\mathbf{t}}^{T} \ \tilde{\mathbf{q}}^{T})$$

 

이 때, translation 부분은

$$\breve{\mathbf{t}}^{T} = \left [ \breve{x} \ \breve{y} \ \breve{z} \right ]$$

$$\Delta \breve{\mathbf{t}}^{T} = \left [ \Delta \breve{x} \ \Delta\breve{y} \ \Delta\breve{z} \right ]$$

같은 표현 방법을 사용하므로 $\breve{\mathbf{t}} \leftarrow \breve{\mathbf{t}} +  \Delta \breve{\mathbf{t}}^{T}$와 같이 간단하게 덧셈으로 연산이 가능하다.

 

하지만 rotation 부분은 

$$\breve{\mathbf{q}}^{T} = \left [ q_{x} \ q_{y} \  q_{z} \ q_{w} \right ]$$

$$\tilde{\mathbf{q}}^{T} = \left [ \Delta q_{x} \ \Delta q_{y} \ \Delta q_{z} \right ]$$

같이 서로 차원이 달라서 연산을 수행할 수 없다. 

 

따라서 $\boxplus$는 일반적으로 연산이 불가능한 두 개의 서로 다른 회전 표현 방법을 연산이 가능하도록 해주는 연산자이다.

$$\breve{\mathbf{q}}^{T} \leftarrow \breve{\mathbf{q}}^{T} \boxplus \tilde{\mathbf{q}}^{T}$$

- full quaternion  $\boxplus$  vector part of the unit quaternion

 

수식을 간략하게 표현하기 위해 

\[ \begin{cases} 
      \breve{\mathbf{t}} \leftarrow \breve{\mathbf{t}} +  \Delta \breve{\mathbf{t}}^{T} \\
      \breve{\mathbf{q}}^{T} \leftarrow \breve{\mathbf{q}}^{T} \boxplus \tilde{\mathbf{q}}^{T} 
   \end{cases}
\]

를 일반적으로 한 번에 $\mathbf{x}^{*} \leftarrow \breve{\mathbf{x}} \boxplus \Delta \tilde{\mathbf{x}}^{*}$로 표기한다. 이는 A Tutorial on Graph-based SLAM 논문의 표기법을 사용하였다.

 

그리고 3D SLAM의 경우 해당 $\boxplus$ 연산자는 motion composition operator $\oplus$로 정의된다. 

$$\breve{\mathbf{x}} \boxplus \Delta \tilde{\mathbf{x}}^{*} \doteq \breve{\mathbf{x}} \oplus \Delta \tilde{\mathbf{x}}^{*}$$

 

회전을 quaternion으로 표현할 때 motion composition operator $\oplus$의 자세한 연산 과정은 다음과 같다. 

$$\breve{\mathbf{x}}_{i} \oplus \Delta \tilde{\mathbf{x}}^{*}_{j} = \binom{\breve{\mathbf{q}}_{i}(\tilde{\mathbf{t}}_{j})}{\breve{\mathbf{q}}_{i} \cdot \tilde{\mathbf{q}}_{j}}$$

 

최종적으로 회전을 quaternion으로 표현할 때 $\boxplus$ 연산자는 다음과 같이 계산된다. 

3D SLAM의 경우 i번째 노드의 포즈인 $\mathbf{x}_{i}^{T}$, i 노드와 j 노드 사이의 관측값인 $\mathbf{z}_{ij}^{T}$이 quatertnion 표기법일 때 정의는 아래와 같다. 

$$\mathbf{x}_{i}^{T} = (\mathbf{t}_{i}^{T}, \ \mathbf{q}_{i}^{T})$$

$$\mathbf{z}_{ij}^{T} = (\mathbf{t}_{ij}^{T}, \ \mathbf{q}_{ij}^{T})$$

- $\mathbf{q} = (q_{x},q_{y},q_{z},q_{w})^{T}$, $\left \| \mathbf{q} \right \| = 1$

 

이 때, manifold 최적화 방법이 적용된 에러함수는 다음과 같이 정의할 수 있다. 

이는 일반적으로 정의된 에러함수 $\mathbf{e}_{ij}(\mathbf{x}) = \mathbf{z}_{ij} - \hat{\mathbf{z}}_{ij}$에 Quaternion 표기법 + Manifold 최적화 방법을 적용한 버전으로 볼 수 있다. 따라서 최종적으로 모든 노드에 대한 에러함수는 $\mathbf{F}(\mathbf{x}) = \sum_{i,j} \mathbf{e}_{ij}(\mathbf{x})^{T} \Omega_{ij} \mathbf{e}_{ij}(\mathbf{x})$와 같이 정의되고 다음의 최적화 공식을 통해 최적의 포즈를 찾는 문제가 된다. 

$$\mathbf{x}^{*} =  \arg\min_{\mathbf{x}}\mathbf{F}(\mathbf{x}) = \arg\min_{\mathbf{x}}\sum_{i,j} \mathbf{e}_{ij}(\mathbf{x})^{T} \Omega_{ij} \mathbf{e}_{ij}(\mathbf{x})$$

 

Rotation Matrix Notation

위 경우는 \[ \begin{cases}
    \mathbf{z}_{ij} \\
    \mathbf{x}_{i}  \\
    \mathbf{x}_{j}
 \end{cases}
\]가 quaternion 표기 방법을 따르는 경우를 의미한다. 만약 회전에 대해 rotation matrix 표현 방법을 사용하는 경우 \[ 
\begin{cases}
    \mathbf{Z}_{ij} \\
    \mathbf{X}_{i}  \\
    \mathbf{X}_{j}
\end{cases}
\] 는 각각 [4x4] 변환행렬이 되며 

$$\mathbf{e}_{ij} (\mathbf{x}) = \text{t2v}( \mathbf{Z}_{ij}^{-1}(\mathbf{X}^{-1}_{i} \mathbf{X}_{j}))$$

- $\text{t2v}(\cdot)$ : 변환행렬 to 벡터 함수 (= lograithmic mapping)

- $ \mathbf{X}^{-1}_{i} \mathbf{X}_{j}$ : 두 노드 i,j의 상대 포즈 (예측값)

- $ \mathbf{Z}_{ij}^{-1}(\mathbf{X}^{-1}_{i} \mathbf{X}_{j})$ : 관측값과 예측값의 상대 포즈

가 된다. 이는 Freiburg 대학의 Robot Mapping 강의의 표기법을 사용하였다.

 

---

기존 최적화 공식을 전개하면 다음과 같다. 

$$\mathbf{F}(\breve{\mathbf{x}} + \Delta \mathbf{x}) \simeq \sum_{i,j} c_{ij} + 2b_{ij}\Delta \mathbf{x} + \Delta \mathbf{x}^{T} \mathbf{H}_{ij} \Delta \mathbf{x}$$

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

 

이를 manifold 상에서 최적화 공식으로 변환하면 다음과 같다. 

$$\mathbf{F}(\breve{\mathbf{x}} + \Delta \mathbf{x}) \rightarrow \mathbf{F}(\breve{\mathbf{x}} + \Delta \tilde{\mathbf{x}}) = \mathbf{c} + 2\tilde{\mathbf{b}}^{T}\Delta \mathbf{x} + \Delta \mathbf{x}^{T} \tilde{\mathbf{H}} \Delta \mathbf{x}$$

 

따라서 $\tilde{\mathbf{H}}, \ \tilde{\mathbf{b}}$를 새로 정의하기 위해 우선 manifold 상의 jacobian $\tilde{\mathbf{J}}_{ij}$를 정의해야 한다. $\boxplus$ 연산자를 사용한 jacobian은 다음과 같이 정의할 수 있다.

$$\tilde{\mathbf{J}}_{k} = \frac{\partial \mathbf{e}_{k} (\breve{\mathbf{x}}  \boxplus \Delta \tilde{\mathbf{x}}) }{\partial \Delta \breve{\mathbf{x}}} \bigg\rvert_{\Delta \tilde{\mathbf{x}}=0}$$

 

최종적으로 manifold 상에서 $\Delta \tilde{\mathbf{x}}^{*}$는 다음과 같이 구할 수 있다.

$$\tilde{\mathbf{H}} \Delta \tilde{\mathbf{x}}^{*} = - \tilde{\mathbf{b}}$$

$$\mathbf{x}^{*} = \breve{\mathbf{x}} \boxplus \Delta \tilde{\mathbf{x}}^{*}$$

 

Code Review

g2o 코드 중 "Least Squares on a Manifold:" 관련된 코드 부분만 리뷰하였다. 추가적으로 g2o 라이브러리를 사용한  HDL Graph SLAM 알고리즘의 코드를 일부 리뷰하였다.

 

 

References

- Kümmerle, Rainer, et al. "g 2 o: A general framework for graph optimization." 2011 IEEE International Conference on Robotics and Automation. IEEE, 2011.

- Grisetti, Giorgio, et al. "A tutorial on graph-based SLAM." IEEE Intelligent Transportation Systems Magazine 2.4 (2010): 31-43.