Moment-Matching Probabilistic Data Association for Optimization-Based SLAM

기술 요약: 최적화 기반 SLAM을 위한 모멘트 매칭 확률적 데이터 결합

문제 정의
최적화 기반 동시적 위치 추정 및 지도 작성(SLAM)은 팩터 그래프(factor graph) 상의 비선형 최소제곱 문제를 해결함으로써 루프 폐쇄(loop closure)를 통해 누적된 항법 오차를 효과적으로 줄입니다. 그러나 **데이터 결합(Data Association, DA)**에는 근본적인 과제가 남아 있습니다: 즉, 센서 측정값이 이전에 관측된 랜드마크와 일치하는지 결정하는 것입니다. 전통적인 최적화 기반 SLAM은 일반적으로 전역 최근접 이웃(global nearest-neighbor) 할당을 사용하는 "하드(hard)" DA에 의존합니다. 이는 표준 비선형 최소제곱 솔버와는 호환되지만, 허위 양성(False Positives, FPs)(클러터) 및 허위 음성(False Negatives, FNs)(미검출)이 존재하는 환경에서는 신뢰하기 어렵습니다. 하드 결합은 과도하게 확신하는 불확실성 정량화, 트랙 손실, 그리고 지도 일관성 저하를 초래할 수 있습니다. 또한 잘못된 하드 결합은 이차 비용 함수(quadratic cost function)를 지배하여 최적화 도구가 잘못된 궤적 및 지도 추정치로 수렴하게 만듭니다.

기존의 강건한 기술들은 종종 프론트엔드 이상치 제거(예: RANSAC)나 백엔드 강건 손실 함수(예: 반복 재가중 최소제곱법)를 사용합니다. 이러한 방식은 이상치의 영향을 완화할 수는 있지만, 비볼록성(non-convexity)과 초기화 민감도를 유발할 수 있습니다. 반면, **확률적 데이터 결합(Probabilistic Data Association, PDA)**은 모든 후보 측정값에 가중치를 부여함으로써 DA 불확실성을 고려하는 베이지안 프레임워크를 제공합니다. 그러나 표준 PDA 업데이트는 개별 Likelihood의 가중 합으로 구성된 Likelihood 함수를 생성하며, 이는 현대 SLAM(예: iSAM2)에서 사용되는 최소제곱 최적화 프레임워크와 직접적으로 호환되지 않습니다.

방법론
본 논문은 PDA 업데이트 단계를 동등한 **가상 선형-가우시안 측정(virtual linear-Gaussian measurement)**으로 변환함으로써 PDA를 최적화 기반 SLAM에 통합하는 새로운 접근 방식을 제안합니다. 방법론은 다음과 같이 세 가지 주요 단계로 진행됩니다:

1. 확률적 할당 및 모멘트 매칭(Moment Matching):
   단일 측정값을 선택하는 대신, 이 방법은 여러 측정값을 확률적으로 할당합니다. 이 방법은 모멘트 매칭을 통해 랜드마크 분포의 평균과 공분산을 계산합니다. 이는 PDA 사후 확률을 가우시안 혼합 모델(각 후보 측정값에 대한 성분과 미검출에 대한 성분으로 구성됨)로 취급하고, 이를 동일한 1차 및 2차 모멘트를 가진 단일 가우시안 분포로 축소하는 과정을 포함합니다. 이 단계는 효과적으로 결합 불확실성을 정량화합니다.

2. 가상 측정 생성:
   PDA 업데이트를 최소제곱 솔버와 호환되도록 만들기 위해, 저자들은 선형이며 가우시안인 가상 측정 모델($y_{k,j} = H_{k,j}[x_k^T, \ell_j^T]^T + v_{k,j}$)을 구축합니다. 이 가상 측정의 파라미터(측정 행렬 $H$ 및 측정 벡터 $y$)는 모멘트 매칭 PDA 단계에서 얻은 결과와 정확히 일치하는 사후 평균 및 공분산을 갖도록 유도됩니다.

   • 수학적으로, 이는 정보 이득(Information Gain, $J = \Sigma^{-1} - \Sigma^{-1}_{pred}$)과 정보 벡터 증분(Information Vector Increment, $c = \Sigma^{-1}\mu - \Sigma^{-1}_{pred}\mu_{pred}$)을 계산함으로써 달성됩니다.
   • 양의 준정부호(positive semi-definite) 정보 이득 행렬 $J$에 대한 고유값 분해를 사용하여, 저자들은 칼만 업데이트 방정식을 정보 형태(information form)로 만족시키는 $H$와 $y$를 도출합니다.

3. 최적화 통합:
   결과물인 가상 측정과 그 선형-가우시안 모델은 최적화 기반 SLAM 시스템(구체적으로 iSAM2)의 팩터 그래프에 직접 삽입됩니다. 이를 통해 최적화 솔버는 결합 불확실성을 표준 측정 업데이트로서 처리하며, 이는 무작위 결합 변수를 포함하는 서브그래프에 대해 **기댓값 전파(Expectation Propagation, EP)**를 수행하는 것과 같습니다.

주요 기여

• 호환성: 본 연구의 주요 기여는 PDA(소프트 결합 처리)와 최적화 기반 SLAM(하드 및 미분 가능한 비용 함수 필요) 사이의 간극을 메우는 방법론입니다.
• 가상 측정 공식화: PDA 업데이트의 정확한 평균과 공분산을 보존하는 가상 선형-가우시안 측정을 유도하여, 표준 희소 최소제곱 솔버를 사용할 수 있게 합니다.
• 강건성: 이 접근 방식은 최적화 기반 SLAM이 결합 불확실성을 직접 반영할 수 있게 하여, 노이즈가 많은 환경에서 발생하는 하드 결합의 문제점을 피할 수 있게 합니다.

수치 결과
저자들은 제안된 PDA + iSAM2 방식을 참조 DA + iSAM2(하드 전역 최근접 이웃 결합 사용) 및 이상적인 Batch SLAM (Oracle) 베이스라인(정답 결합을 사용하며 미검출 없음)과 비교하여 평가했습니다. 시뮬레이션은 10개의 랜드마크가 있는 환경에서 에이전트가 피겨 에이트(figure-eight) 경로를 주행하는 상황을 가정하였으며, 검출 확률($p_d$)과 평균 허위 양성률($\mu_{fp}$)을 변화시키며 수행되었습니다.

• 불확실성 하에서의 성능: 낮은 검출 확률과 높은 허위 양성률(예: $p_d=0.6, \mu_{fp}=0.20$)을 가진 시나리오에서, 하드 결합 방식(DA + iSAM2)은 심각한 성능 저하를 겪으며 11.961 m의 평균 절대 오차(MAE)를 기록했습니다.
• 제안된 방법: PDA + iSAM2 방식은 동일한 까다로운 조건에서 2.515 m의 MAE를 달와하며 우수한 강건성을 입증했습니다.
• Oracle과의 비교: 제안된 방법이 Oracle 베이스라인(MAE ~0.5 m)에는 미치지 못했지만, 조건이 악화됨에 따라 하드 결합 방식보다 이상적인 케이스에 훨씬 더 근접한 성능을 유지했습니다.
• 궤적 시각화: 시각적 결과(그림 2)는 PDA + iSAM2가 상당한 드리프트(drift)를 보이는 DA + iSAM2에 비해 정답 궤적에 훨씬 더 가까운 결과를 생성함을 확인시켜 주었습니다.

의의 및 주장
본 논문은 가상 선형-가우시안 측정으로 PDA 업데이트를 변환함으로써, 모든 최적화 기반 SLAM 방법 내에서 확률적 데이터 결합을 효과적으로 수행할 수 있다고 주장합니다. 저자들은 이 접근 방식이 전통적인 하드 결합이 실패하는 높은 FP 및 FN 시나리오에서 에이전트의 위치 추정 및 지도 작성 성능을 향상시킨다고 단언합니다. 이 방법은 복잡한 다중 가설 추적(multi-hypothesis tracking)이나 초기화에 민감한 비볼록 강건 최적화에 의존하지 않고도 결합 불확실성을 처리할 수 있는 계산 효율적인 방법을 제공합니다. 저자들은 이 방법이 강건성을 개선하지만, 더 크고 다양한 시뮬레이션에서 평가하고, 3D 운동 및 비선형 측정 모델로 확장하며, 실제 데이터셋에서 검증하기 위한 후속 연구가 필요하다고 언급했습니다.