강건 포즈 그래프 최적화

표준 포즈 그래프 최적화에는 위험한 성질이 하나 있다: 모든 에지를 신뢰한다는 것이다. 오도메트리 에지는 대체로 신뢰할 만하지만, 루프 클로저 에지는 장소 인식에서 나오는데, 이는 때때로 거짓말을 한다 — 지각적 앨리어싱(perceptual aliasing, 똑같이 생긴 두 개의 복도)이 거짓 루프 클로저를 만들어낸다. 하나의 잘못된 루프 에지를 최소제곱법에 입력하면, 최적화기는 여러분의 맵을 스스로 접어버릴 것이다. 왜냐하면 이차 비용에서는 하나의 나쁜 제약이 나머지 모든 것을 지배할 수 있기 때문이다. 강건 포즈 그래프 최적화는 백엔드가 이상치 제약 조건 속에서도 살아남게 하는 기법들의 집합이다.

로드맵에 명명된 세 가지 고전적 접근법:

• Switchable Constraints (Sunderhauf & Protzel, 2012). 각 루프 클로저 에지 $(i,j)$는 그 에지의 정보 행렬을 스케일링하는 추가 최적화 변수, 즉 스위치 $s_{ij} \in [0, 1]$을 갖는다. 최적화기는 나머지 그래프와 불일치하는 에지를 “꺼버릴” 수 있으며, 무분별하게 에지를 비활성화하는 것을 억제하는 벌점 항을 지불한다:

  $$\min_{\{T_i\}, \{s_{ij}\}} \sum_{\text{odom}} \|\mathbf{e}_{ij}\|^2_{\Sigma^{-1}} + \sum_{\text{loops}} s_{ij}^2\, \|\mathbf{e}_{ij}\|^2_{\Sigma^{-1}} + \lambda (1 - s_{ij})^2$$

• Dynamic Covariance Scaling (DCS) (Agarwal et al., 2013). 최적의 스위치 값이 현재 잔차로부터 닫힌 형태로 계산될 수 있음을 관찰하여, 추가 변수를 없앤다. 각 루프 에지의 공분산은 즉석에서 스케일링된다 — 잔차가 작으면 완전히 신뢰하고, 잔차가 크면 공분산을 부풀린다. DCS는 강건한 M-추정기 커널처럼 동작하며 switchable constraints보다 빠르게 수렴한다.

• Pairwise Consistency Maximization (PCM) (Mangelson et al., 2018). 최적화기 내부에서 에지를 부드럽게 만드는 대신, PCM은 최적화 이전에 루프 클로저를 필터링한다: 두 루프 클로저가 함축하는 상대 운동이(그 사이의 오도메트리와 합성했을 때) 각자의 공분산 범위 내에서 일치하면 두 루프 클로저는 쌍별로 일관적(pairwise consistent)이다. 노드가 후보이고 에지가 쌍별 기하학적 일관성을 나타내는 일관성 그래프를 구성하고 그 최대 클리크(maximum clique)를 찾으면, 상호 일관된 가장 큰 루프 클로저 부분집합을 얻는다; 나머지는 거부된다. PCM은 멀티로봇 SLAM(DOOR-SLAM, Kimera-Multi)에서 표준적인 관문(gatekeeper)이 되었는데, 이 영역에서는 로봇 간 루프 클로저가 특히 신뢰하기 어렵다.

이러한 아이디어들은 더 넓은 강건 추정 도구상자와 연결된다: M-추정기 커널(Huber, Cauchy)은 중간 정도의 이상치를 낮게 가중하고, Graduated Non-Convexity (GNC)는 초기화 없이도 강하게 비볼록한 강건 비용을 푼다. 실제로는 시스템들이 방어선을 여러 겹 쌓는다: 프론트엔드에서의 기하학적 검증, PCM 방식의 일관성 검사, 그리고 최적화기 내부의 최후 방어선으로서의 강건 커널 또는 DCS.

M-추정기 관점

모든 부드러운(soft) 방법들은 하나의 동작을 변형한 것이다: 이차 비용을 강건 커널 $\rho$로 대체하는 것,

$$\min_{\{T_i\}} \sum_{(i,j)} \rho\left(\|\mathbf{e}_{ij}\|_{\Sigma_{ij}^{-1}}\right),$$

이는 큰 잔차에 대해 비용의 영향력(그 도함수)이 더 이상 커지지 않거나 오히려 줄어들도록 선택된다. 이차 함수 아래에서는 잔차 $10\sigma$인 에지가 $1\sigma$인 에지보다 100배 더 강하게 끌어당기지만, Huber 아래에서는 선형적으로만 더 강하게 끌어당기며, Cauchy나 Geman-McClure 아래에서는 잔차가 커질수록 그 끌어당김이 오히려 약해진다. 구현 관점에서 이는 **반복 재가중 최소제곱법(iteratively reweighted least squares)**이 된다: 각 가우스-뉴턴 반복은 모든 에지의 정보 행렬에 현재 잔차로부터 계산된 가중치 $w(\|\mathbf{e}\|)$를 곱하는데, 이것이 정확히 DCS가 특정한 닫힌 형태의 가중치로 수행하는 것이며, switchable constraints가 명시적인 스위치 변수로 달성하는 것이다. 트레이드오프는 다음과 같다: 재하강(redescending) 커널(Cauchy 이상)은 심한 이상치를 완전히 거부할 수 있지만, 비용을 비볼록하게 만들기 때문에 초기화가 나쁘면 좋은 에지를 잠가버릴(lock out) 수 있다 — 이것이 GNC가 볼록 대리 함수(convex surrogate)에서 시작해 점진적으로 이를 날카롭게 만들어 해결하는 문제다.

프론트엔드 측의 보완적인 하드 검사는 **카이제곱 게이팅(chi-square gating)**이다: 에지가 받아들여지기 전에, 현재 추정치 아래에서 그 잔차가 통계적으로 타당한지 검사한다, $\mathbf{e}^T \Sigma^{-1} \mathbf{e} < \chi^2_{d,\,0.95}$ ($SE(3)$ 에지의 경우 6자유도). 게이팅은 명백한 이상치를 저렴하게 잡아내지만, 이는 현재 추정치를 신뢰한다 — 심한 드리프트 아래에서는 올바른 루프 클로저조차 타당하지 않아 보일 수 있으며, 이것이 정확히 PCM의 쌍별 일관성 논리가 더 잘 처리하는 경우다.

흔한 함정

• 잘못된 에지를 강건화하는 것 — 오도메트리 에지가 이상치인 경우는 드물다; 이를 공격적인 커널로 감싸면 최적화기가 실제 드리프트를 “설명해 없애버려” 궤적을 분리된 섬들로 쪼갤 수 있다. 루프 클로저를 강건화하고, 오도메트리는 (거의) 이차로 유지하라.
• 강건성이 검증을 대체한다고 가정하는 것 — 커널과 스위치는 일부 비율의 이상치까지만 견뎌낸다; 40%의 거짓 루프를 내놓는 프론트엔드는 여전히 맵을 손상시킬 것이다. 방어선을 여러 겹으로 쌓아라.
• 눈먼 튜닝 — switchable constraints의 $\lambda$, DCS의 $\Phi$ 파라미터, 커널 너비는 모두 “잔차가 얼마나 커야 이 에지를 의심할 것인가”를 인코딩한다; 이들은 실제 에지 공분산에 상대적으로 설정되어야 하는데, 그 공분산 자체도 흔히 추측에 불과하다.
• 재하강 커널 + 나쁜 초기화 — 드리프트가 크면, 올바른 루프 클로저조차 처음에 거대한 잔차로 시작해 즉시 꺼져버린다; GNC 방식의 연속화(continuation)나 PCM 사전 필터링이 이 실패를 막아준다.
• 지각적 앨리어싱은 구조화된 환경에서 일관된 거짓말을 만들어낸다 — 똑같이 생긴 복도 구간에서 나온 여러 개의 거짓 매칭이 서로 일치할 수 있다; 일관성 최대화는 기준을 높이지만 증명은 아니다.

SLAM에서의 의미

하나의 거짓 루프 클로저가 한 시간에 걸쳐 만든 맵을 파괴할 수 있으므로, 배치된 시스템에서 백엔드의 강건성은 선택 사항이 아니다. 이 주제는 또한 멀티로봇 SLAM으로 들어가는 진입점이기도 한데, 이 환경에서는 로봇들이 다른 로봇이 만든 제약 조건을 신뢰할지 여부를 결정해야 하며, 이는 PCM이 설계된 바로 그 상황이다.

실습

• Kimera-RPGO 실습

관련 문서

• Pose graph optimization
• GNC
• DOOR-SLAM
• Inter-robot loop closure
• Visual Place Recognition (VPR)