Non-linear Optimization

SLAM 추정은 결국 다음 비용 함수를 최소화하는 문제로 귀결됩니다.

F(\mathbf{x}) = \frac{1}{2}\sum_i \|\mathbf{e}_i(\mathbf{x})\|^2_{\Sigma_i^{-1}}

상태 $\mathbf{x}$ (자세, 랜드마크, 바이어스)에 대해, 각 $\mathbf{e}_i$ 는 잔차입니다 — 재투영 오차, 오도메트리 오차, IMU 오차일 수 있으며, 역공분산으로 가중됩니다. 이 잔차들은 상태에 대해 비선형입니다: 카메라 투영은 깊이로 나누고, 회전은 곡면 형태의 매니폴드 위에 존재합니다. 따라서 닫힌 형태의 해가 없으며, 반복적으로 풀어야 합니다.

반복적 하강 템플릿

실용적인 모든 솔버는 하나의 루프를 공유합니다: 초기 추정값 $\mathbf{x}_0$ 에서 시작하여, 비용을 감소시키는 업데이트 $\Delta\mathbf{x}$ 를 반복적으로 찾아 적용하고, 업데이트나 그래디언트가 미미해지면 멈춥니다. 방법들은 $\Delta\mathbf{x}$ 를 선택하는 방식에서 차이가 납니다.

경사 하강(Gradient descent): $\Delta\mathbf{x} = -\alpha\, \nabla F$ . 1차 도함수만 필요하지만 지그재그로 움직이며 선형적으로만 천천히 수렴합니다 — SLAM에서 직접 사용되는 경우는 드뭅니다.
뉴턴 방법(Newton’s method): 실제 헤시안 $H$ 를 이용해 $H\,\Delta\mathbf{x} = -\nabla F$ 를 풉니다. 2차적으로 수렴하지만, 잔차의 2차 도함수는 계산 비용이 크고 최적해에서 먼 경우 $H$ 가 양의 정부호가 아닐 수 있습니다.
가우스-뉴턴: 최소제곱 구조를 활용합니다. 각 잔차를 선형화하여 $\mathbf{e}(\mathbf{x} + \Delta\mathbf{x}) \approx \mathbf{e}(\mathbf{x}) + J\Delta\mathbf{x}$ 로 근사하고, 정규 방정식 $(J^T J)\,\Delta\mathbf{x} = -J^T\mathbf{e}$ 를 풉니다. 행렬 $J^T J$ 는 1차 도함수만으로 헤시안을 근사하는데 — 이것이 대규모 SLAM을 다룰 수 있게 만드는 핵심 트릭입니다.
레벤버그-마쿼트: 적응형 댐핑을 더한 가우스-뉴턴, $(J^T J + \lambda I)\,\Delta\mathbf{x} = -J^T\mathbf{e}$ 이며, 선형화를 신뢰할 수 없을 때 경사 하강 쪽으로 섞여 들어갑니다. 사실상의 기본 선택입니다.
Dogleg / 신뢰 영역(trust-region): 영역 반경을 명시적으로 유지하면서 그 안에서 가우스-뉴턴 스텝과 경사 하강 스텝을 결합합니다. 개념적으로는 LM과 유사하지만 번들 조정에서 더 빠른 경우가 많습니다.

매니폴드 위에서의 최적화

카메라 자세는 벡터가 아니라 $\mathrm{SE}(3)$ 의 원소입니다 — 회전 행렬에 업데이트를 단순히 더하면 직교성이 깨집니다. 표준적인 해법은 리 대수 상의 국소 섭동(local perturbation) $\boldsymbol{\xi} \in \mathbb{R}^6$ 을 최적화하고, 지수 사상을 통해 이를 적용하는 것입니다.

T \leftarrow T \cdot \exp(\hat{\boldsymbol{\xi}})

솔버는 항상 작은 벡터 $\boldsymbol{\xi}$ 만 보게 되고, 리트랙션이 상태를 매니폴드 위에 유지시켜 줍니다. 모든 SLAM 라이브러리가 이를 구현하고 있습니다 (Ceres의 local parameterization, g2o의 vertex oplus, GTSAM의 retraction).

SLAM 문제를 풀 수 있게 만드는 요소들

희소성. 각 잔차는 매우 적은 수의 변수만 다룹니다 (자세 하나 + 랜드마크 하나, 또는 자세 두 개), 따라서 $J^T J$ 는 극도로 희소합니다. 희소 Cholesky 분해와 슈어 보완은 그렇지 않으면 불가능했을 문제(수만 개의 변수)를 실시간으로 풀 수 있는 문제로 바꿔줍니다.
좋은 초기화. 이러한 방법들은 비볼록 비용의 국소 최소값만 찾습니다. SLAM이 이를 견딜 수 있는 이유는 항상 그럴듯한 초기 추정값이 있기 때문입니다 — 이전 프레임의 자세, 운동 모델, RANSAC의 최소 솔버 결과 등. 콜드 스타트 문제(재지역화, 지도 병합)가 어려운 이유는 정확히 이 초기 추정값이 없기 때문입니다.
강건성. 제곱 비용은 가우시안 잡음을 가정하지만, 실제 매칭기는 이상치를 만들어냅니다. 강건 커널(M-estimator)은 큰 오차가 지배적이 되지 않도록 비용 형태를 바꿔주지만, 그 대가로 비볼록성이 커집니다.

솔버 출력 읽는 법

반복은 실용적인 기준에 따라 멈춥니다: 반복당 비용 감소량이 허용 오차 이하로 떨어지거나, 업데이트 노름 $\|\Delta\mathbf{x}\|$ 가 미미해지거나, 그래디언트 노름이 0에 가까워지거나, 반복/시간 예산이 소진되는 경우입니다 (실시간 시스템은 종종 키프레임당 번들 조정 반복 횟수를 몇 번으로 제한합니다). 풀이가 잘못되었을 때 증상은 원인과 매핑됩니다: 비용이 폭증하는 것은 대체로 초기 추정값이 나쁘거나 야코비안에 버그가 있다는 뜻이고, 작은 스텝들이 많은데도 비용이 높은 상태로 정체되는 것은 나쁜 국소 최소값으로 수렴했거나 모델링되지 않은 이상치가 있음을 시사하며, 정규 방정식 행렬이 랭크 결핍인 것은 관측 불가능한 방향(게이지 자유도, 제약 없는 랜드마크)이 존재하여 사전 정보나 고정이 필요함을 가리킵니다.

SLAM에서의 의미

현대 SLAM 시스템의 백엔드 — 번들 조정, 포즈 그래프 최적화, VIO 슬라이딩 윈도우, 직접 광도 정렬 — 는 모두 매니폴드 위에서 가우스-뉴턴/LM 반복으로 풀리는 희소 비선형 최소제곱의 한 사례입니다. 확률론적 관점(MLE & MAP)은 무엇을 최소화할지를 말해주고, 비선형 최적화는 어떻게 최소화할지를 말해줍니다. 여기에 익숙해지면 어디서든 도움이 됩니다: 논문의 “다음 에너지를 최소화한다”는 절을 읽는 것, 발산을 진단하는 것(초기화가 나쁜가? 파라미터화가 잘못됐나? 이상치인가?), Ceres/g2o/GTSAM을 효과적으로 사용하는 것 모두가 이 내용에 기반합니다.

실습

Ceres-solver 실습