Keyframe

• Keyframe을 생성하는 기준

  1. 이전 keyframe 기준 로봇이 일정 거리만큼 움직임

     → 하지만 그 거리가 “적당히” 멀어야 함

     → Keyframe 사이의 거리가 너무 멀어지게 되면 pose 추정을 하는 것이 오히려 부정확해짐

  2. 이전 keyframe 기준 어느 정도 시간이 흘렀을 경우, 다음 keyframe을 생성

Rotation from 2D to 3D

2D, 3D 공간 상에서 Rotation을 표현하는 방법

• rotation term에 대한 optimization을 할 때, SO(3)를 Angle-axis Representation으로 parametrization를 하는 이유
  1. SO(3)의 3x3 rotation matrix를 표현하려면 6개의 parameter가 필요한 반면, Angle-axis Representation으로 rotation을 표현하면 3개의 parameter만으로 3D rotation을 표현 가능
     • 만약 quaternion으로 parametrization을 하면, quaternion의 크기가 1이어야 한다는 constraint 때문에 optimization을 할 때 상당히 까다로움
  2. 주로 optimization을 할 때는 iterative하게 푸는데, 만약 rotation의 변화량의 크기가 작다면 (i.e. 위의 표 상에서 $\rm{v}$의 크기가 작다면), $\operatorname{Exp}(\mathbf{v}) \simeq \mathbf{I}{3}+[\mathbf{v}]{\times}$로 approximation이 쉽게 가능
  3. Rotation의 불확실성(uncertainty)을 표현하는 것이 용이
     • 기존의 Euclidean geometry 상에서 사용하던 linear equation이나 non-linear equation에 uncertainty를 표현할 때 Gaussian noise를 더해주는 행위와 같은 의미로서 사용이 가능

Uncertainty Description in SO(3)

• 3차원 상에서의 Rotation에 대한 uncertainty($\tilde{\mathrm{R}}$)는

  • $\mathrm{R}$: noise-free인 rotation matrix
  • $\operatorname{Exp}(\epsilon)$: uncertainty vector $\epsilon \in \mathbb{R}^{3}$를 exponential map을 통해 투영한 rotation matrix

  의 곱으로 아래와 같이 표현이 가능 ($\rm{SO(3)}\times\rm{SO(3)}=\rm{SO(3)}$):

  $$ \tilde{\mathrm{R}}=\mathrm{R} \operatorname{Exp}(\epsilon), \quad \epsilon \sim \mathcal{N}(0, \Sigma) $$

  • 즉, 위의 식은 3차원 공간 상의 회전을 표현하는 manifold에 대한 uncertainty를 3x1 크기를 지니고 zero-mean인 Gaussian distribution으로 표현할 수 있음

• Graph SLAM 관점에서의 uncertainty

  • Factor graph SLAM의 최종 목표: 아래의 optimization 수식을 푸는 것

    • $\mathbf{e}_{i j}$: $i$번째 keyframe과 $j$번째 keyframe 사이의 어떤 error term (residual)
    • $\Omega_{i j}$: residual에 해당하는 uncertainty (= information matrix)

    $$ \mathbf{x}^{*}=\operatorname{argmin} \sum_{\mathbf{x}} \mathbf{e}{i j}^{T} \Omega{i j} \mathbf{e}_{i j} $$

  • “$\epsilon$이 크다”의 의미

    • rotation에 대한 measurements가 불확실하다는 의미
    • 해당 measurements에 해당하는 error 크기의 중경도를 따질 때 덜 중요하다고 여김 (measurements가 불확실하니까)
    • 즉, 지니고 있는 information의 중요한 정도가 낮다고 판단

• manifold에서 IMU preintegration의 목적

  • keyframe $i$와 $j$의 사이의 수십~수백 여개의 IMU measurements를 $i$와 $j$간의 하나의 $\epsilon$ term로 간략화

<aside> 💡 3D 공간 상의 rotation의 uncertainty를 exponential map을 활용하여 표현함으로써, 기존의 factor graph SLAM framework를 그대로 사용해서 optimization하는 것이 가능하다.

</aside>

IMU Model and Motion Integration