포즈 그래프 최적화

**포즈 그래프 최적화(Pose graph optimization, PGO)**는 SLAM 백엔드의 축소된 형태로, 변수가 오직 로봇/카메라 자세 $\{T_i\} \subset SE(3)$뿐이며 3D 맵 점은 포함되지 않는다. 이 그래프는 다음으로 구성된다:

• 노드: 자세 $T_i$ (보통 키프레임 자세).
• 에지: 오도메트리(연속된 자세 사이)와 루프 클로저(동일한 장소로 인식된, 시간적으로 연속되지 않은 자세 사이)로부터 얻어지는, 불확실성 $\Sigma_{ij}$를 갖는 상대 자세 제약 조건 $T_{ij}$.

각 에지는 “노드 $i$로부터, 노드 $j$는 상대 자세 $T_{ij}$에 위치해야 한다”고 말한다. 최적화는 모든 제약 조건을 동시에 가장 잘 만족시키는 자세 구성을 찾는다:

$$\min_{\{T_i\}} \sum_{(i,j) \in \mathcal{E}} \left\| \log\!\left(T_{ij}^{-1}\, T_i^{-1} T_j\right) \right\|^2_{\Sigma_{ij}^{-1}}$$

잔차는 $SE(3)$ 로그 사상으로 계산되므로 오차는 6자유도 접공간(tangent space)에 존재한다. 이 문제는 번들 조정에서와 정확히 동일하게 매니폴드 위에서 가우스-뉴턴 또는 레벤버그-마쿼트로 풀리지만, 변수의 수는 훨씬 적다.

시각 SLAM 시스템에서의 일반적인 사용 패턴은 다음과 같다:

1. 프론트엔드가 카메라를 추적하며 오도메트리 에지를 구축하고, 드리프트가 누적된다.
2. 장소 인식이 루프 클로저를 감지하고, 기하학적 검증을 통해 시간적으로 멀리 떨어진 두 키프레임 사이의 상대 자세 에지가 생성된다.
3. PGO가 누적된 드리프트를 전체 궤적에 재분배하여 루프를 “닫아” 맞춘다.
4. 선택적으로, 이후 완전한 번들 조정이 자세와 점을 정제한다.

(다수의) 랜드마크 변수를 제외하기 때문에, PGO는 완전한 번들 조정보다 훨씬 빠르며, 루프 클로저 이후 전역 궤적을 보정하는 표준 도구다 — 이는 ORB-SLAM의 essential graph 최적화와 대부분의 LiDAR SLAM 백엔드(예: GTSAM이나 g2o를 통한)가 수행하는 것이다. 단안 SLAM에서는 스케일 드리프트도 보정할 수 있도록 그래프가 $SE(3)$ 대신 $\mathrm{Sim}(3)$ 위에서 최적화되는 경우가 많다.

한 가지 주의할 점: PGO는 자신의 에지를 신뢰한다. 하나의 잘못된 루프 클로저 에지가 전체 맵을 스스로 접어버릴 수 있으며, 이것이 강건한 포즈 그래프 최적화 기법이 필요한 이유다.

최적화는 어떻게 실행되는가

PGO는 매니폴드 위에서의 비선형 최소제곱법이다. 회전은 단순한 덧셈으로 갱신될 수 없으므로, 각 가우스-뉴턴/LM 반복은 접공간에서 작동한다:

1. 모든 에지에 대해 잔차 $\mathbf{e}_{ij} = \log\!\left(T_{ij}^{-1} T_i^{-1} T_j\right) \in \mathbb{R}^6$과, 두 자세의 작은 섭동 $\boldsymbol{\delta}_i, \boldsymbol{\delta}_j$에 대한 야코비안을 계산한다.
2. 정규 방정식 $H \boldsymbol{\delta} = -\mathbf{b}$를 구성하고 푼다. 여기서 $H = \sum J_{ij}^T \Sigma_{ij}^{-1} J_{ij}$는 희소하다: 각 에지는 오직 두 개의 자세에만 관여하므로, $H$의 희소성 패턴은 곧 그래프의 인접 구조 그 자체다.
3. 각 자세를 리트랙션(retraction) $T_i \leftarrow T_i \cdot \mathrm{Exp}(\boldsymbol{\delta}_i)$로 갱신하고 수렴할 때까지 반복한다.

두 가지 구조적 세부 사항이 중요하다. 첫째, 게이지 자유도(gauge freedom): 비용은 모든 자세를 동시에 강체로 이동시켜도 변하지 않으므로, 게이지를 고정하지 않으면(첫 번째 자세를 고정하거나 이에 사전 팩터를 추가하지 않으면) $H$는 특이(singular) 행렬이 된다. 둘째, 정보 행렬 $\Sigma_{ij}^{-1}$은 보정이 어떻게 분배되는지를 결정하는 조절 장치다: 신뢰도가 높은 오도메트리 에지는 조금만 굽혀지고, 불확실한 에지가 루프 오차 대부분을 흡수한다. 모든 에지를 단위 정보로 설정하는 것도 “동작”은 하지만 드리프트를 비현실적으로 분배하게 된다.

직관적으로 이해하기

로봇이 큰 정사각형 경로를 주행하며 각 코너마다 오도메트리에 약간의 방향 오차가 누적되어, 추정된 최종 자세가 시작점에서 눈에 띄게 벗어나 있다고 상상해 보자. 이후 장소 인식이 마지막 뷰를 첫 뷰와 매칭하여, “이 두 자세가 일치한다”고 말하는 하나의 루프 에지를 추가한다. PGO 이전에는 이 에지가 큰 잔차를 가지며 오도메트리 에지는 잔차가 없다. 최적화기는 오차가 공유되는 구성을 찾아낸다: 모든 자세가 (각 에지의 공분산에 의해 가중되어) 조금씩 회전하고 이동하며, 각 오도메트리 에지는 작은 잔차를 갖게 되고, 루프 에지의 잔차는 같은 통계적 스케일로 줄어든다 — 궤적이 눈에 보이게 닫힌 정사각형으로 “맞춰진다”. 아무것도 다시 측정되지 않았다; 동일한 측정값이 단지 일관되게 재해석되었을 뿐이다. 이 오차 재분배의 그림이 모든 그래프 기반 SLAM 백엔드에 대한 올바른 심적 모델이다.

흔한 함정

• 게이지 고정을 잊는 것 — 특이 행렬 $H$는 솔버 실패나, 반복 사이에 전체 맵이 자유롭게 떠다니는 형태로 나타난다.
• 검증되지 않은 루프 에지를 입력하는 것 — 에지가 그래프에 들어가기 전에는 항상 기하학적으로 검증해야 한다(인라이어 개수, 일관성 검사 등); 하나의 이상치 에지가 치명적일 수 있다(강건 PGO 참고).
• 큰 루프에서의 부실한 초기화 — 가우스-뉴턴은 지역적으로만 수렴한다; 방향이 90° 드리프트된 궤적은 강건 커널, 더 나은 초기 추정(예: 회전 평균화(rotation averaging)/스패닝 트리 초기화), 또는 나쁜 지역 최솟값을 피하기 위한 댐핑을 가진 LM이 필요할 수 있다.
• 2D에서의 각도 처리 — $\theta$ 잔차는 $[-\pi, \pi]$로 래핑해야 한다; 언래핑되지 않은 각도는 “폭발하는” 2D 포즈 그래프의 전형적인 원인이다.
• $\mathrm{Sim}(3)$이 필요한 곳에 $SE(3)$을 사용하는 것 — 단안 루프 클로저는 스케일 드리프트도 보정해야 한다; $SE(3)$ 위에서 최적화하면 루프에 스케일 이음매(seam)가 남는다.

SLAM에서의 의미

PGO는 루프 클로저의 핵심 도구다 — 드리프트하는 오도메트리 궤적을 전역적으로 일관된 맵으로 바꾸는 단계다. 이는 그래프 기반 SLAM의 가장 단순한 형태이므로, 완전한 번들 조정과 팩터 그래프에 도전하기 전에 매니폴드 위 비선형 최소제곱, 희소성, 정보 행렬을 처음 이해하기에 가장 좋은 지점이기도 하다.

실습

• Kimera-RPGO 실습

관련 문서

• Factor graph
• Robust pose-graph optimization
• Lie groups
• MAP inference as sparse nonlinear least squares