Incremental smoothing

SLAM은 성장하는 추정 문제입니다: 새로운 키프레임이 추가될 때마다 팩터 그래프에 변수와 팩터가 늘어납니다. 매 단계마다 전체 비선형 최소제곱 문제를 처음부터 다시 푸는 것(배치 최적화)은 궤적이 길어질수록 점점 더 비싸지지만, 각각의 새로운 측정값은 대개 그래프의 작고 최근의 일부에만 영향을 미칩니다. **증분 스무딩(incremental smoothing)**은 이를 활용합니다: 모든 것을 다시 계산하는 대신, 이전 해를 새로운 정보만으로 업데이트합니다.

선형대수적인 관점

선형화된 최소제곱 시스템을 푸는 것은 측정 야코비안($J = QR$을 구한 뒤 $R\,\Delta\mathbf{x} = \mathbf{d}$를 후진 대입)이나 정보 행렬($H = J^TJ$를 콜레스키 분해)을 인수분해하는 것을 의미합니다. 새로운 측정값은 $J$에 새로운 행을 추가합니다. iSAM(증분 스무딩 및 매핑, Incremental Smoothing and Mapping, Kaess et al.)의 핵심 관찰은: 기존의 삼각 인수 $R$은 그 새로운 행들에 대해 기븐스 회전(Givens rotations)의 시퀀스로 — 즉 저랭크(low-rank) 수정으로 — 처음부터 다시 만들 필요 없이 업데이트될 수 있다는 것입니다. 업데이트는 새로운 측정값의 변수와 연결된 항목들만 채워 넣기 때문에, $R$의 대부분은 손대지 않은 채로 남습니다. iSAM은 여전히 주기적인 배치 단계를 필요로 했는데, 선형화 지점과 (fill-in을 좌우하는) 변수 순서 모두가 추정치가 진화하면서 낡아지기 때문입니다.

iSAM2와 베이즈 트리

iSAM2는 **베이즈 트리(Bayes tree)**를 도입하여 주기적 배치라는 버팀목을 제거했습니다. 베이즈 트리는 팩터 그래프에서의 변수 제거(variable elimination)로부터 파생된 트리 구조의 데이터 구조입니다: 제거를 완전히 수행하면 클리크(clique)들이 방향성 트리로 조직화되는 베이즈망이 얻어집니다. 베이즈 트리는 정확히 어떤 변수들의 해가 어떤 다른 변수들에 의존하는지를 인코딩합니다 — 각 클리크의 해는 루트 방향으로 그 조상들에만 의존합니다.

새로운 팩터가 도착하면:

• 영향을 받은 변수들에서 루트까지의 경로 위에 있는 클리크들만 다시 제거(re-eliminate)됩니다 — 나머지, 보통 트리의 대부분은 손대지 않습니다.
• 유동적 재선형화(fluid relinearization): 변수는 마지막 선형화 지점 이후 추정치가 임계값 이상 이동했을 때만 다시 선형화되므로, 비선형성은 그것이 중요한 곳에서만(그리고 오직 그곳에서만) 처리됩니다.
• 부분 상태 업데이트: 해의 업데이트는 수정된 클리크로부터 아래로 전파되며, 변화가 임계값 이하로 떨어지는 곳에서 멈추므로, 실제로 움직인 추정치만 다시 계산됩니다.

그 결과는 탐색형(exploration-style) 궤적에 대해 거의 상수 시간의 업데이트이며, 선형화 임계값까지는 정확함을 유지합니다 — 이는 정보를 버리는 고정 지연(fixed-lag) 근사가 아니라, 모든 변수에 대한 전체 스무딩 문제가 그대로 유지되는 것입니다. 큰 루프 클로저는 여전히 트리의 큰 부분을 건드립니다(이는 올바른 동작입니다 — 실제로 모든 것에 영향을 미치기 때문입니다), 이는 프로파일링에서 간간이 나타나는 지연 스파이크로 드러납니다.

백엔드 스타일 선택하기

iSAM2는 GTSAM에 구현되어 있으며, 실시간 속도로 전체 이력 스무딩을 원할 때 선택하는 백엔드입니다: 예를 들어 Kimera-VIO는 구조 없는(structureless) “스마트” 프로젝션 팩터로 증분 시각-관성 추정을 수행하기 위해 GTSAM의 iSAM2를 실행하며, LIO-SAM도 자신의 LiDAR-관성 팩터 그래프를 같은 솔버에 전달합니다. 주요 대안 백엔드 스타일 — 마지널화를 이용한 고정 지연 슬라이딩 윈도우 — 는 오래된 변수를 버림으로써 비용을 제한하는 반면, 증분 스무딩은 그것들을 유지한 채로 영리하게 업데이트함으로써 비용을 제한합니다.

실무 노트

• 재선형화 임계값은 속도와 정확성을 맞바꾸는 값입니다; 너무 느슨한 임계값은 고곡률(빠른 회전) 구간에서 조용히 정확도를 떨어뜨립니다.
• 구조 없는/”스마트” 팩터(팩터 내부에서 랜드마크를 해석적으로 제거하는 방식)는 수명이 짧은 랜드마크가 많은 비전 문제에서 베이즈 트리를 작게 유지해줍니다.
• 플랫폼이 루프 클로저의 지연 스파이크를 감당할 수 없다면, 흔한 아키텍처는 빠른 고정 지연 오도메트리 프론트엔드와 그 뒤에서 돌아가는 iSAM2(또는 포즈 그래프) 스무더의 조합입니다.

SLAM에서의 의미

증분 스무딩은 “스무딩이 필터링보다 낫다”는 통찰을 실시간 속도로 실용화하는 것입니다: 매 프레임마다 배치 비용을 지불하지 않고도 전체 궤적에 대한 완전한 비선형 최적화의 정확도를 얻습니다. “GTSAM/iSAM2 기반”이라고 설명된 시스템을 볼 때, 이제 그 계산적 스토리를 알 수 있습니다: 베이즈 트리 업데이트, 유동적 재선형화, 그리고 정확한 전체 이력 MAP 추론입니다. 이는 루프 클로저를 원활하게 통합해야 하는 VIO 및 LiDAR-관성 시스템(예: LIO-SAM, Kimera)의 표준 백엔드입니다.

관련 문서

• Factor graph
• MAP inference as sparse nonlinear least squares
• Marginalization
• Kimera-VIO
• LIO-SAM