12.17 포즈 그래프와 그래프 기반 SLAM

12.17 포즈 그래프와 그래프 기반 SLAM

1. 그래프 기반 SLAM의 개요

그래프 기반 SLAM(graph-based SLAM)은 로봇의 자세 궤적과 환경 지도를 그래프 구조로 표현하고, 그래프 최적화를 통해 두 가지를 동시에 추정하는 SLAM 접근법이다. 1997년 프랭크 룬코(Frank Lu)와 에반젤로스 밀리오스(Evangelos Milios)의 선구적 연구 이후 발전해 왔으며, 현재 SLAM의 가장 주류적인 접근법 중 하나이다. 포즈 그래프(pose graph)는 노드가 로봇의 자세이고 엣지가 자세 사이의 측정인 그래프이며, 그래프 기반 SLAM의 중심 표현이다. 이는 비주얼 SLAM, 라이다 SLAM, 비주얼-관성 SLAM 등 다양한 SLAM 시스템에서 활용된다.

2. SLAM 문제의 정식화

2.1 변수

SLAM 문제의 변수는 두 가지이다.

  • 로봇의 자세 궤적: \mathbf{x}_{1:T} = \{\mathbf{x}_1, \mathbf{x}_2, \ldots, \mathbf{x}_T\}
  • 환경 지도: \mathbf{m}

여기서 \mathbf{x}_t는 시간 t에서의 로봇 자세이며, SE(2) 또는 SE(3)의 원소이다.

2.2 측정값

측정값은 두 종류이다.

  • 자기 운동 측정: \mathbf{u}_{1:T-1} (예: 휠 인코더, IMU)
  • 외부 측정: \mathbf{z}_{1:T} (예: 라이다, 카메라)

2.3 목적

SLAM의 목적은 측정값으로부터 자세 궤적과 지도를 추정하는 것이다.

P(\mathbf{x}_{1:T}, \mathbf{m} | \mathbf{u}_{1:T-1}, \mathbf{z}_{1:T})

3. 포즈 그래프

3.1 정의

포즈 그래프는 다음으로 구성된 그래프이다.

  • 노드: 로봇의 자세 \mathbf{x}_i
  • 엣지: 두 자세 사이의 상대 측정 \mathbf{z}_{ij}

엣지는 두 종류가 있다.

  • 오도메트리 엣지: 연속된 자세 사이의 자기 운동 측정에서 유래
  • 회로 폐쇄 엣지: 시간상 멀리 떨어진 자세 사이의 측정 (이전 위치 재방문)

3.2 그래프의 시각적 표현

포즈 그래프는 다음과 같이 시각화된다.

  • 노드는 로봇의 자세(궤적의 키프레임)
  • 엣지는 자세 사이의 측정 제약
  • 회로 폐쇄는 그래프에 사이클을 형성

3.3 그래프의 희소성

포즈 그래프는 일반적으로 매우 희소하다. 각 노드는 인접한 노드와 일부 회로 폐쇄 엣지로만 연결된다. 이러한 희소성이 효율적인 최적화를 가능하게 한다.

4. 비용 함수

4.1 최대 우도 추정

SLAM은 측정값이 가장 잘 설명되는 자세 궤적을 찾는 최대 우도 추정 문제이다. 측정 오차가 가우시안이라고 가정하면 비용 함수는 잔차의 제곱합이다.

J(\mathbf{x}_{1:T}) = \sum_{(i, j) \in E}\mathbf{r}_{ij}^T(\mathbf{x}_i, \mathbf{x}_j)\Omega_{ij}\mathbf{r}_{ij}(\mathbf{x}_i, \mathbf{x}_j)

여기서

  • \mathbf{r}_{ij}: 엣지 (i, j)의 잔차
  • \Omega_{ij}: 측정의 정보 행렬 (공분산의 역)

4.2 잔차의 정의

잔차는 측정된 상대 자세와 예측된 상대 자세의 차이이다.

\mathbf{r}_{ij} = \log(\mathbf{T}_{ij}^{-1}\mathbf{T}_i^{-1}\mathbf{T}_j)^\vee

여기서

  • \mathbf{T}_i, \mathbf{T}_j: 자세 i와 j의 강체 변환
  • \mathbf{T}_{ij}: 측정된 상대 변환

잔차는 트위스트(리 대수의 원소)로 표현된다.

5. 그래프 최적화

5.1 비선형 최소제곱

비용 함수의 최소화는 비선형 최소제곱(nonlinear least squares) 문제이다. 가우스-뉴턴법이나 레벤버그-마쿼트법이 사용된다.

5.2 가우스-뉴턴법

가우스-뉴턴법의 한 단계는 다음과 같다.

solve (J^T Ω J) Δx = -J^T Ω r
update x = x + Δx

여기서 \mathbf{J}는 자코비안, \Omega는 정보 행렬이다.

5.3 야코비안

야코비안은 잔차의 자세에 대한 미분이다. 리 군 위의 미분이 사용된다.

5.4 정보 행렬의 희소성

포즈 그래프의 정보 행렬은 매우 희소하다. 각 노드는 적은 수의 다른 노드와만 연결된다. 이를 활용한 효율적인 솔버가 사용된다.

5.5 희소 선형 솔버

희소 선형 시스템의 풀이에 다음의 알고리즘이 사용된다.

  • 콜레스키 분해: 가장 일반적
  • QR 분해: 수치적으로 안정적
  • 켤레 기울기: 큰 시스템에 적합

6. 회로 폐쇄

6.1 회로 폐쇄의 중요성

회로 폐쇄(loop closure)는 로봇이 이전에 방문한 위치로 돌아왔을 때 이를 인식하는 것이다. 회로 폐쇄가 없으면 자세 추정의 누적 오차가 무한히 커진다. 회로 폐쇄는 누적 오차를 보정한다.

6.2 회로 폐쇄 검출

회로 폐쇄 검출은 다양한 방법으로 이루어진다.

6.2.1 시각적 방법

  • Bag of Words (BoW): 이미지의 특징점을 단어로 표현하고 비교
  • NetVLAD: 신경망 기반 위치 인식
  • DBoW2/3: BoW의 효율적 구현

6.2.2 라이다 방법

  • Scan Context: 라이다 스캔의 글로벌 디스크립터
  • OverlapNet: 신경망 기반

6.2.3 위치 기반

  • 추정된 위치가 가까우면 회로 폐쇄 후보로 간주

6.3 회로 폐쇄의 검증

검출된 회로 폐쇄는 잘못된 매칭일 수 있으므로 검증이 필요하다.

  • 기하학적 검증: 측정의 일관성 확인
  • RANSAC: 강건한 정합

6.4 잘못된 회로 폐쇄의 영향

잘못된 회로 폐쇄는 SLAM 결과를 크게 손상시킨다. 강건한 검출과 검증이 필수이다.

7. 그래프 최적화 알고리즘

7.1 가우스-뉴턴법

가우스-뉴턴법은 비선형 최소제곱 문제의 표준 알고리즘이다. 빠르게 수렴하지만 초기 추정이 좋아야 한다.

7.2 레벤버그-마쿼트법

레벤버그-마쿼트법은 가우스-뉴턴법에 댐핑을 추가한다. 더 강건하다.

7.3 다양한 솔버

7.3.1 g2o

g2o(General Graph Optimization)는 그래프 최적화를 위한 C++ 라이브러리이다. 라스무스 쿠멜레(Rainer Kümmerle) 등이 개발하였다.

7.3.2 GTSAM

GTSAM(Georgia Tech Smoothing and Mapping)은 SLAM과 인자 그래프(factor graph)를 위한 C++ 라이브러리이다. 프랭크 델라르트(Frank Dellaert) 그룹이 개발하였다.

7.3.3 Ceres Solver

Ceres Solver는 Google의 비선형 최소제곱 라이브러리이다. 일반적인 최적화에 사용될 수 있다.

7.3.4 iSAM2

iSAM2(incremental Smoothing and Mapping 2)는 점진적 SLAM 최적화 알고리즘이다.

8. 인자 그래프

8.1 정의

인자 그래프(factor graph)는 그래프 기반 SLAM의 일반화이다. 노드는 변수(자세, 랜드마크, 칼리브레이션 매개변수 등)이고, 인자(factor)는 변수들 사이의 제약이다.

8.2 인자의 종류

인자는 다양한 종류가 있다.

  • 사전 인자: 변수의 사전 분포
  • 오도메트리 인자: 연속된 자세 사이의 운동 측정
  • 랜드마크 인자: 랜드마크 측정
  • 회로 폐쇄 인자: 회로 폐쇄 측정

8.3 일반화

인자 그래프는 포즈 그래프뿐만 아니라 변수가 포함된 일반 SLAM을 다룰 수 있다. 베이즈 네트워크의 일반화로 볼 수 있다.

9. 비주얼 SLAM

9.1 번들 조정

번들 조정(bundle adjustment)은 비주얼 SLAM의 핵심 최적화 문제이다. 카메라 자세와 3차원 점을 동시에 최적화한다.

9.2 재투영 오차

번들 조정의 잔차는 재투영 오차이다.

\mathbf{r}_{ij} = \mathbf{u}_{ij} - \pi(\mathbf{T}_i, \mathbf{p}_j)

여기서 \mathbf{u}_{ij}는 측정된 픽셀, \pi는 투영 함수이다.

9.3 자세 그래프 vs 번들 조정

  • 자세 그래프: 자세만 최적화 (경량)
  • 번들 조정: 자세와 3차원 점 최적화 (정확)

비주얼 SLAM은 일반적으로 두 접근법을 결합한다. 백엔드에서 번들 조정을, 회로 폐쇄 후 자세 그래프 최적화를 수행한다.

10. 라이다 SLAM

10.1 점 군 정합

라이다 SLAM에서 두 자세 사이의 측정은 점 군 정합(point cloud matching)으로 얻어진다. ICP(Iterative Closest Point)나 NDT(Normal Distributions Transform)가 사용된다.

10.2 자세 그래프

라이다 SLAM의 자세 그래프는 키스캔(key scan)의 자세를 노드로, 정합 결과를 엣지로 한다.

10.3 대표적 시스템

  • Cartographer: Google의 라이다 SLAM
  • LIO-SAM: 라이다-관성 SLAM
  • LOAM: 라이다 오도메트리와 매핑

11. 강건성

11.1 이상치 처리

회로 폐쇄에 잘못된 매칭이 포함될 수 있다. 강건 최적화 기법이 사용된다.

11.2 M-추정

M-추정은 큰 잔차의 영향을 줄이는 손실 함수를 사용한다. Huber, Cauchy 등이 일반적이다.

11.3 동적 공분산 스케일링

DCS(Dynamic Covariance Scaling)는 잔차에 따라 공분산을 동적으로 조정한다.

11.4 스위치 가능 제약

스위치 가능 제약(switchable constraints)은 의심스러운 회로 폐쇄를 비활성화할 수 있게 한다.

11.5 강건성 라이브러리

GTSAM, g2o 등의 라이브러리는 강건 최적화를 지원한다.

12. 점진적 최적화

12.1 동기

전체 그래프를 매번 최적화하는 것은 비용이 크다. 점진적 최적화는 새로운 측정이 도착할 때 부분적으로만 갱신한다.

12.2 iSAM과 iSAM2

iSAM(incremental SAM)과 그 후속인 iSAM2는 점진적으로 그래프 최적화를 수행한다. 베이즈 트리 자료 구조를 사용한다.

12.3 응용

실시간 SLAM 시스템에서 점진적 최적화가 필수이다.

13. 그래프 최적화의 야코비안

13.1 자코비안 계산

자코비안 계산은 그래프 최적화의 핵심이다. 리 군 위의 미분이 사용된다.

13.2 자동 미분

자동 미분(automatic differentiation)은 자코비안의 자동 계산을 가능하게 한다. Ceres Solver와 GTSAM이 자동 미분을 지원한다.

13.3 분석 야코비안 vs 자동 미분

  • 분석 야코비안: 효율적이지만 구현이 복잡
  • 자동 미분: 단순하고 정확하지만 약간 느림

14. 마진화

14.1 마진화의 필요성

큰 SLAM 시스템에서 모든 변수를 유지할 수 없다. 오래된 변수를 마진화(marginalize)하여 그래프의 크기를 제어한다.

14.2 슈어 보충

마진화는 슈어 보충(Schur complement)을 사용하여 효율적으로 수행된다.

14.3 응용

비주얼-관성 SLAM의 슬라이딩 윈도우 최적화에서 마진화가 사용된다.

15. 응용

15.1 모바일 로봇

모바일 로봇이 환경을 매핑하면서 자기 위치를 추정한다. 그래프 SLAM이 표준이다.

15.2 자율 주행

자율 주행 차량의 HD 지도 구축과 위치 추정에 그래프 SLAM이 사용된다.

15.3 무인 항공기

드론과 무인 항공기의 SLAM에 그래프 기반 방법이 사용된다.

15.4 증강 현실

증강 현실 기기의 시점 추적과 환경 매핑에 그래프 SLAM이 활용된다.

15.5 매핑 서비스

Google Maps, Apple Maps 등의 매핑 서비스가 SLAM 기술을 사용한다.

16. SLAM 시스템

16.1 ORB-SLAM

ORB-SLAM은 ORB 특징점 기반 비주얼 SLAM이며, 자세 그래프와 번들 조정을 결합한다.

16.2 LSD-SLAM

LSD-SLAM은 직접 방식 비주얼 SLAM이며, 픽셀 강도를 직접 사용한다.

16.3 VINS-Mono / VINS-Fusion

VINS는 비주얼-관성 SLAM 시스템이며, 슬라이딩 윈도우 최적화를 사용한다.

16.4 Cartographer

Google의 Cartographer는 라이다 SLAM 시스템이며, 그래프 최적화를 사용한다.

16.5 LIO-SAM

LIO-SAM은 라이다-관성 SLAM 시스템이며, 미리 적분과 회로 폐쇄를 결합한다.

16.6 Kimera

Kimera는 의미 정보를 통합한 SLAM 시스템이다.

17. 응용 예시: 청소 로봇

청소 로봇이 새로운 집을 매핑할 때 그래프 SLAM이 사용된다. 라이다나 카메라 데이터로 자세 그래프가 구축되고, 회로 폐쇄로 누적 오차가 보정된다.

18. 응용 예시: 자율 주행

자율 주행 차량이 도시 도로의 HD 지도를 구축할 때 그래프 SLAM이 사용된다. 다양한 센서(라이다, 카메라, GPS, IMU)의 측정이 인자 그래프에 통합된다.

19. 응용 예시: 드론

드론이 GPS가 없는 실내 환경에서 비행할 때 비주얼-관성 SLAM이 사용된다. 카메라와 IMU의 측정이 그래프 최적화를 통해 결합된다.

20. 응용 예시: 증강 현실

스마트폰의 증강 현실 앱은 사용자의 시점을 추적한다. SLAM 기술이 핵심이며, 그래프 기반 방법이 사용된다.

21. 응용 예시: 화성 탐사

화성 탐사 로버는 GPS가 없으므로 SLAM이 위치 추정의 유일한 방법이다. 시각적 SLAM과 그래프 최적화가 활용된다.

22. 라이브러리 지원

22.1 g2o

g2o는 그래프 최적화의 표준 라이브러리이다. 다양한 종류의 노드와 엣지를 지원한다.

#include <g2o/core/sparse_optimizer.h>
g2o::SparseOptimizer optimizer;
// 노드와 엣지 추가
optimizer.addVertex(vertex);
optimizer.addEdge(edge);
optimizer.initializeOptimization();
optimizer.optimize(iterations);

22.2 GTSAM

GTSAM은 인자 그래프를 사용한 SLAM 라이브러리이다.

#include <gtsam/nonlinear/NonlinearFactorGraph.h>
gtsam::NonlinearFactorGraph graph;
// 인자 추가
graph.add(factor);
gtsam::Values initial;
gtsam::LevenbergMarquardtOptimizer optimizer(graph, initial);
gtsam::Values result = optimizer.optimize();

22.3 Ceres Solver

Ceres Solver는 일반 비선형 최소제곱 라이브러리이며, SLAM에 사용될 수 있다.

23. 학습의 가치

포즈 그래프와 그래프 기반 SLAM을 깊이 이해하는 것은 다음과 같은 이점을 제공한다.

  • 현대 SLAM의 가장 주류적인 접근법을 익힌다.
  • 비선형 최적화의 응용을 학습한다.
  • 다양한 SLAM 시스템의 원리를 이해할 수 있다.
  • 실제 시스템을 구현하고 디버깅할 수 있다.
  • 학술 문헌과 산업 표준을 이해할 수 있다.

24. 학습 권장사항

24.1 직접 구현

작은 규모의 자세 그래프 SLAM을 직접 구현해 본다. 시뮬레이션 데이터로 시작한다.

24.2 라이브러리 활용

g2o, GTSAM 등의 라이브러리를 사용하여 실제 SLAM 시스템을 구축해 본다.

24.3 시각화

자세 그래프와 최적화 과정을 시각화하면 이해가 깊어진다.

24.4 기존 시스템 학습

ORB-SLAM, VINS-Mono 등의 기존 시스템의 코드를 학습한다.

25. 본 절의 의의

본 절은 포즈 그래프와 그래프 기반 SLAM을 다루었다. 그래프 기반 SLAM은 현대 SLAM의 가장 주류적인 접근법이며, 비주얼, 라이다, 비주얼-관성 등 다양한 SLAM 시스템에서 활용된다. 포즈 그래프의 이해는 로봇공학의 핵심 기술을 학습하는 데 필수적이다.

26. 참고 문헌

  • Lu, F., & Milios, E. (1997). “Globally consistent range scan alignment for environment mapping.” Autonomous Robots, 4(4), 333–349.
  • Grisetti, G., Kümmerle, R., Stachniss, C., & Burgard, W. (2010). “A tutorial on graph-based SLAM.” IEEE Intelligent Transportation Systems Magazine, 2(4), 31–43.
  • Kümmerle, R., Grisetti, G., Strasdat, H., Konolige, K., & Burgard, W. (2011). “g2o: A general framework for graph optimization.” IEEE International Conference on Robotics and Automation, 3607–3613.
  • Dellaert, F., & Kaess, M. (2017). “Factor graphs for robot perception.” Foundations and Trends in Robotics, 6(1-2), 1–139.
  • Cadena, C., Carlone, L., Carrillo, H., Latif, Y., Scaramuzza, D., Neira, J., Reid, I., & Leonard, J. J. (2016). “Past, present, and future of simultaneous localization and mapping: Toward the robust-perception age.” IEEE Transactions on Robotics, 32(6), 1309–1332.
  • Mur-Artal, R., Montiel, J. M. M., & Tardós, J. D. (2015). “ORB-SLAM: A versatile and accurate monocular SLAM system.” IEEE Transactions on Robotics, 31(5), 1147–1163.

version: 1.0