Pose Graph Optimization
- 다른 이름: PGO, pose-graph SLAM, pose graph optimisation
- 카테고리: State estimation & navigation › Localisation
- 태그: slam, state-estimation, optimization, localization, mapping, factor-graphs
포즈들 사이의 오도메트리 제약과 루프 클로저 제약이 최대한 일치하도록 포즈 그래프를 조정해 로봇의 궤적을 정밀화하는 SLAM 백엔드 기법.
정의
포즈 그래프 최적화(pose graph optimization)는 그래프 위의 최소제곱 문제를 풀어 로봇의 궤적을 추정하는 과정이다. 그래프의 각 노드는 로봇 포즈(한 시점의 위치와 방향)이고, 각 간선은 두 포즈 사이에서 측정된 상대 변환과 그 불확실성으로, 불확실성은 보통 정보 행렬로 표현된다. 간선은 연속된 포즈 사이의 오도메트리와, 로봇이 이전에 방문한 장소를 알아보는 루프 클로저에서 나온다.
모든 측정에는 노이즈가 있으므로 간선들은 결코 완벽하게 일치하지 않는다. 루프를 따라 오도메트리 간선을 이어 붙여도 로봇은 루프 클로저 간선이 말하는 위치로 정확히 돌아오지 않는다. 포즈 그래프 최적화는 모든 간선에 걸친 가중 제곱 오차의 총합을 최소화하는 포즈 집합을 찾으며, 각 측정이 얼마나 확실한지에 비례해 보정을 궤적 전체에 분산시킨다.
회전은 덧셈으로 합성되지 않으므로 이 문제는 비선형이며, 따라서 Gauss-Newton이나 Levenberg-Marquardt 같은 방법으로 매 단계 현재 추정값 주변에서 선형화하며 반복적으로 푼다. 각 포즈는 다른 몇몇 포즈와만 연결되므로 결과 선형 시스템은 희소하며, 희소 솔버 덕분에 수천 개의 포즈를 가진 그래프도 다룰 수 있다. 이 단계에는 g2o, GTSAM, Ceres Solver 같은 오픈소스 라이브러리가 흔히 쓰인다.
포즈 그래프 최적화는 대부분의 그래프 기반 SLAM 시스템의 백엔드이다. 랜드마크를 변수로 유지하지 않는다는 점에서 번들 조정과 다르다. 포즈만 최적화하므로 더 빠르지만 일부 정보를 버린다. 알려진 약점은 잘못된 루프 클로저에 민감하다는 것이며, 그래서 강건 비용 함수와 이상치 제거가 표준적으로 추가된다.
중요한 이유
오도메트리와 시각 오도메트리는 시간이 지남에 따라 한없이 드리프트하며, 포즈 그래프 최적화는 루프 클로저가 발견되었을 때 누적된 드리프트를 제거하는 표준 방법이다. 국소적으로는 정확하지만 전역적으로는 일관되지 않은 측정들을 건물이나 현장 전체에 걸쳐 일관된 궤적과 지도로 바꾼다.
주요 응용
- LiDAR 및 카메라 기반 지도 작성을 위한 그래프 기반 SLAM 백엔드
- 휠, 시각, 시각-관성 오도메트리에서 누적된 드리프트 제거
- 서로 다른 시간에 기록된 지도를 병합하는 다중 세션 지도 작성
- 여러 로봇의 궤적을 하나의 좌표계로 합치는 다중 로봇 지도 작성
실제 사례
일반적인 LiDAR 또는 시각 SLAM 스택에서 프론트엔드는 로봇이 정해진 거리나 각도만큼 움직일 때마다 새 노드를 추가하고 오도메트리 간선으로 이전 노드와 연결한다. 장소 인식 모듈이 루프 클로저를 제안하면, 간선으로 추가되기 전에 (예를 들어 ICP나 특징 매칭으로) 기하학적으로 검증된다. 그런 다음 백엔드가 g2o나 GTSAM 같은 최적화기를 실행하는데, 루프 클로저마다 배치로 실행하거나 (iSAM2처럼) 점진적으로 실행하며, 보정된 포즈는 지도를 다시 만드는 데 쓰인다. 튜닝 노력의 상당 부분은 각 간선에 할당하는 공분산과 Huber나 Cauchy 같은 강건 커널에 들어가는데, 신뢰도가 높은 잘못된 루프 클로저 하나가 지도 전체를 휘게 할 수 있기 때문이다.
흔한 오해
- 포즈 그래프 최적화가 지도를 만든다 — 포즈만 보정하며, 지도는 그 후 보정된 포즈에 센서 데이터를 배치해 다시 만든다
- 루프 클로저는 많을수록 항상 도움이 된다 — 잘못된 루프 클로저 하나가 궤적 전체를 왜곡할 수 있으므로, 클로저는 검증하고 강건 비용으로 가중치를 낮춰야 한다
- 번들 조정과 같은 것이다 — 번들 조정은 랜드마크 위치도 최적화하지만, 포즈 그래프는 포즈와 상대 제약만 유지한다
알아야 할 사람
- SLAM 및 위치 추정 엔지니어
- 모바일 로봇 및 자율 주행 차량 개발자
- 시각 오도메트리와 지도 작성을 다루는 컴퓨터 비전 엔지니어
- 확률론적 로보틱스와 상태 추정을 배우는 학생
관련 기술
- g2o
- GTSAM
- Ceres Solver
- 팩터 그래프(Factor Graphs)
- 번들 조정(Bundle Adjustment)
- Levenberg-Marquardt 알고리즘(Levenberg-Marquardt Algorithm)
관련 용어
관련 프로젝트
- LingBot-Map: Streaming 3D reconstruction from one camera: pose and depth for every frame, live
- oomwoo: Open-source DIY robot vacuum: 3D-printed, ROS2 + 2D LiDAR, fully local.
- Cartographer: Google's real-time 2D and 3D SLAM system for multi-platform robotics
- OpenDroneMap: Open-source photogrammetry toolkit that turns drone images into maps and 3D models
- Nav2: ROS 2 navigation stack with path planning and obstacle avoidance for mobile robots
- FAST-LIVO2: Fast, direct LiDAR-inertial-visual odometry from HKU-MARS, published in IEEE T-RO
동영상
- Understanding SLAM Using Pose Graph Optimization | Autonomous Navigation, Part 3: MATLAB