희소 비선형 최소제곱으로서의 MAP 추정

이것은 현대 SLAM의 핵심적인 이론적 동일성입니다: 가우시안 잡음 하에서 팩터 그래프에 대한 최대 사후 확률 (MAP) 추정은 정확히 희소 비선형 최소제곱 문제와 같습니다. 백엔드가 하는 모든 일은 이 하나의 유도로부터 나옵니다.

베이즈에서 최소제곱까지

베이즈 법칙에서 시작합니다. 우리는 측정값이 주어졌을 때 가장 확률이 높은 상태를 찾고자 합니다:

x∗=arg⁡max⁡x p(x∣z)∝p(z∣x) p(x)\mathbf{x}^* = \arg\max_{\mathbf{x}}\, p(\mathbf{x} \mid \mathbf{z}) \propto p(\mathbf{z} \mid \mathbf{x})\, p(\mathbf{x})

운동 모델 xt=f(xt−1,ut)+wt\mathbf{x}_{t} = f(\mathbf{x}_{t-1}, \mathbf{u}_t) + \mathbf{w}_t, wt∼N(0,Qt)\mathbf{w}_t \sim \mathcal{N}(\mathbf{0}, Q_t)와 관측 모델 zt=h(xt,m)+vt\mathbf{z}_t = h(\mathbf{x}_t, \mathbf{m}) + \mathbf{v}_t, vt∼N(0,Rt)\mathbf{v}_t \sim \mathcal{N}(\mathbf{0}, R_t)가 있을 때, 가우시안 곱의 음의 로그를 취하면 확률 최대화가 제곱, 공분산 가중 잔차의 합 최소화로 바뀝니다:

x∗=arg⁡min⁡x[∑t∥h(xt)−zt∥Rt−12+∑t∥f(xt−1,ut)−xt∥Qt−12]\mathbf{x}^* = \arg\min_{\mathbf{x}} \left[ \sum_t \|h(\mathbf{x}_t) - \mathbf{z}_t\|^2_{R_t^{-1}} + \sum_t \|f(\mathbf{x}_{t-1}, \mathbf{u}_t) - \mathbf{x}_t\|^2_{Q_t^{-1}} \right]

팩터 그래프의 각 팩터가 하나의 항을 기여합니다; 시각 SLAM에서는 관측 항이 재투영 오차이며, 문제는 번들 조정으로 특수화됩니다. 유용한 기법적 동일성 하나: 공분산 가중은 잔차에 흡수시킬 수 있습니다. ∥r∥Σ−12=∥Σ−1/2r∥2\|\mathbf{r}\|^2_{\Sigma^{-1}} = \|\Sigma^{-1/2}\mathbf{r}\|^2 (“화이트닝(whitening)“)이므로, 모든 가중 문제는 화이트닝된 잔차에 대한 평범한 최소제곱 문제가 됩니다.

풀이: 가우스-뉴턴과 레벤버그-마쿼트

이 문제는 (투영, 회전으로 인해) 비선형이며 반복적으로 풀립니다. 가우스-뉴턴은 현재 추정값 xk\mathbf{x}_k 주변에서 누적 잔차 e\mathbf{e}를 선형화합니다:

e(xk+Δx)≈e(xk)+JkΔx\mathbf{e}(\mathbf{x}_k + \Delta\mathbf{x}) \approx \mathbf{e}(\mathbf{x}_k) + J_k \Delta\mathbf{x}

비용에 대입하고 Δx\Delta\mathbf{x}에 대해 최소화하면 정규 방정식이 나옵니다:

(JkTJk) Δx=−JkTe(xk)(J_k^T J_k)\, \Delta\mathbf{x} = -J_k^T \mathbf{e}(\mathbf{x}_k)

행렬 H=JTJH = J^T J는 헤시안을 근사합니다 (2차 항 무시). 가우스-뉴턴은 해 근처에서 빠르게 수렴하지만, 초기 추정값이 나쁘면 발산할 수 있습니다. 레벤버그-마쿼트는 감쇠를 통해 이를 해결합니다:

(JkTJk+λI) Δx=−JkTe(xk)(J_k^T J_k + \lambda I)\, \Delta\mathbf{x} = -J_k^T \mathbf{e}(\mathbf{x}_k)

λ→0\lambda \to 0이면 가우스-뉴턴처럼 동작합니다(최솟값 근처에서 빠름); λ→∞\lambda \to \infty이면 작은 보폭의 최급 하강이 됩니다(먼 곳에서 강건함). λ\lambda는 매 반복마다 조정됩니다 — 보폭이 비용을 줄이면 감소하고, 그렇지 않으면 증가합니다. LM은 Ceres와 대부분의 SLAM 백엔드에서 표준 알고리즘입니다.

리 군 노트의 다양체 기법을 포함한 전체 반복 루프는 다음과 같습니다:

  1. 현재 추정값에서 모든 잔차를 선형화합니다 (탄젠트 공간 섭동 ξ\boldsymbol{\xi}에 대한 야코비안).
  2. Δx\Delta\mathbf{x}에 대한 희소 (감쇠) 정규 방정식을 풉니다.
  3. 다양체 위에서 업데이트합니다: 각 자세에 대해 T←T⋅exp⁡(ξ^)T \leftarrow T \cdot \exp(\hat{\boldsymbol{\xi}}), 유클리드 변수에는 단순 덧셈.
  4. 비용 또는 업데이트 노름이 수렴할 때까지 반복합니다.

강건 커널. 실제 대응점 집합에는 이상치가 있으며, 하나의 나쁜 제곱 잔차가 합을 지배할 수 있습니다. 실용적인 시스템은 각 항을 강건 손실 ρ\rho로 감쌉니다 (Huber: 작은 잔차에는 이차, 임계값을 넘으면 선형; Cauchy: 로그 증가로 큰 이상치를 강하게 하향 가중). 그런 다음 최적화는 반복 재가중 최소제곱으로 풀립니다 — 각 잔차는 가중값 wi=ρ′(ri)/riw_i = \rho'(r_i)/r_i를 받습니다 — 이는 정확히 동일한 정규 방정식 기법에 들어맞습니다.

희소성: 이야기의 나머지 절반

각 팩터는 몇 개의 변수만 관여합니다 (하나의 관측은 하나의 자세와 하나의 랜드마크만 포함), 따라서 JJ와 HH는 압도적으로 희소하고 블록 구조를 가집니다. 이를 활용하는 것이 대규모 SLAM을 다룰 수 있게 만드는 핵심입니다 — 희소 콜레스키/QR 분해를 통해서, 그리고 번들 조정에서 모든 랜드마크를 먼저 소거하는 슈어 보완을 통해서 ((6m+3n)(6m+3n)차원 시스템을 6m6m개의 카메라 변수로 줄이며, 랜드마크 수가 자세 수를 압도적으로 초과할 때 큰 효과를 냅니다).

변수 소거와 베이즈 트리. 희소 시스템을 푸는 것은 그래프 이론적으로 볼 수 있습니다: 팩터 그래프에서 변수를 하나씩 소거하며, 각 소거는 조건부 밀도와 나머지 변수에 대한 새로운 유도 팩터를 생성합니다. 소거 순서는 얼마나 많은 필인(fill-in, 밀집화)이 생기는지를 결정합니다 — 좋은 순서 (예: COLAMD)는 분해를 희소하게 유지합니다. 소거를 완료까지 진행하면 베이즈 네트가 생성되며, 그 클리크들은 베이즈 트리로 구성됩니다: 각 변수의 해가 오직 그 조상에만 의존하는 클리크들의 방향 트리입니다. 베이즈 트리는 단순한 구현 세부 사항이 아닙니다 — 새로운 측정값이 해의 어느 부분에 영향을 미치는지를 드러내는데, 이는 정확히 iSAM2가 증분 업데이트에 활용하는 구조이며, 마지널라이제이션(변수를 영구적으로 소거하는 것)을 동일한 대수적 연산으로 설명해 줍니다.

이 기반 위에 두 계열의 풀이 스케줄이 세워집니다:

SLAM에서의 의미

이 형식화는 이 분야가 필터링에서 최적화(“스무딩”)로 옮겨 간 이유입니다: 필요할 때마다 재선형화하고, 임의의 팩터 유형을 균일하게 처리하며, 희소성을 통해 확장됩니다. 모든 백엔드 라이브러리 (Ceres, g2o, GTSAM)는 정확히 이 파이프라인의 구현이며, 여러분이 읽게 될 모든 백엔드 논문 — ORB-SLAM의 BA에서 VINS-Mono의 슬라이딩 윈도우, iSAM2에 이르기까지 — 은 “어떤 변수, 어떤 팩터, 어떤 소거/풀이 스케줄”이라는 질문에 대한 특정한 답입니다.

관련 문서