그래프 기반 SLAM 튜토리얼 - 4.C 매니폴드에서 최소 제곱

C. 매니폴드에서 최소 제곱

 비 유클리디안 공간을 수치적으로 다루는 흔한 방법은 매니폴드에서 최적화를 수행하는 것입니다. 매니폴드는 전역 스케일에서 반드시 유클리디안이 아니나 지역적으로는 유클리디안이 나타나는 수학적 공간을 말합니다. 여기서 소개하는 매니폴드 기반 방법은 태일러와 크래이그만이 소개한 SO(3)에서 함수의 최소화 방법과 비슷합니다.

 

 SLAM 문제에서 각 파라미터 블록 $x_i$는 전이 벡터 $t_i$와 회전 요소 $\alpha_{i}$로 구성되어 있고, 전이 벡터 $t_i$는 유클리디안 공간에서 나타나나, 회전 요소 $\alpha_i$는 비유클리디안 2차원이나 3차원 회전 그룹 SO(2)나 SO(3)에서 나타납니다. 이러한 특이성을 피하기 위해서 이러한 공간들은 회전 행렬이나 쿠터니언 같은 모수화를 벗어난 방법들을 사용하기도 합니다.

 

 식 (16)에 이런 모수화를 벗어난 표현법들을 적용하면, 이로인해 제약관계가 망가질 수 있습니다. 모수화를 벗어나는건 추가적인 자유도를 만들고, 오차를 만들어버립니다. 이 문제를 극복하기 위해서 회전에 대해 (3차원에서 오일러각과같은) 최소한 표현법을 사용할 수도 있겠습니다. 하지만 이러는것은 특이성에 영향을 주게 됩니다. 2차원에서 특이성은 각을 정규화하여 쉽게 복원할수 있으나 3차원에서 이과정은 직관적이지 않게 됩니다.

 

 그래서 다른 방법은 다루어야 하는 공간을 매니폴드로 보고, 유클리디안 공간상의 지역적인 변화 $\Delta$x를, 매니폴드에서의 변화화 사상시키는 연산자를 정의하면 됩니다. 수학적으로 자세한 내용은 해르츠버그의 연구 자료를 참고하면 되겠습니다. 이 연산자를 사용하여 새로운 오차 함수를 정의할수 있게 됩니다.

 여기서 $\breve{x}$는 쿼터니언 인스턴스로 모수화를 벗어난 공간상에 나타납니다. 여기서 $\Delta \tilde{x}$는 원점 위치 $\breve{x}$에서의 작은 증분으로, 최소 표현상에서 사용됩니다.

 

 3차원 SLAM 예제에서는 단위 쿼터니언의 백터를 사용하는것이 회전 모수화에서 좋은 선택이 됩니다. 자세히 설명하자면, 증분  $\Delta \tilde{x}$를 평행이동 $\Delta \tilde{t}$와 3차원 회전을 나타내는 단위 쿼터니언의 백터 파트인  $\tilde{q}^{T}$로 6차원 벡터  $\Delta \tilde{x}^{T}$ = ($\Delta \tilde{t}^{T}$ $\tilde{q}^{T}$)와 같이 정의할수 있겠습니다.

 

 역으로 $\breve{x}^{T}$ = ($\breve{t}^T$ $\breve{q}$^{T})에서 쿼터니언 $\breve{q}$를 회전 부분을 나타내기 위해 사용할수 있겠습니다. 그래서 그 연산자는 $\Delta \breve{q}$를 완전 쿼터니언 $\Delta q$로 변환하고,  $\Delta x^{T}$ = ($\Delta t^{T}$ $q^{T}$)를 $\breve{x}$로 변환하는데 사용할 수 있겠습니다.

 

 쿼터니언을 이용하여 오차 최소화 시에는, 이러한 연산들이 다음의 연산자로 정리할 수 있고, 자코비안 $\breve{J}_{ij}$는 다음과 같이 나타낼수 있겠습니다.

 이전의 방정식에서 e는 $\Delta \breve{x}_i$와 $\Deta \breve{x}_j$에만 의존하므로 다음과 같이 전개할 수 있겠습니다.

 

 편미분에 대한 규칙을 사용하고, $\Delta \breve{x} = 0$에서의 자코비안을 을 사용하면, 영이 아닌 블록들은 다음과 같이 됩니다.

 이 표기법을 확장하여 식 (9)에 식 (21)을 대입하고, 이를 통해 다음의 선형 시스템을 구할 수 있습니다.

 증분 $\Delta \breve{x*}$는 초기 추측 $\breve x$ 주위의 지역 유클리디안에서 계산되기 때문에 이들은 그 연산자를 이용해 모수를 벗어난 공간상에서 재사상되어야 합니다. 식 (16)의 갱신 규칙에 따라 다음과 같이 됩니다.

 매니폴드에서의 최소화 문제 공식화에서는 우선 식 (26)으로 초기 추측 주위의 지역 유클리디안 근사 증분 집합을 계산하고, 다음에는 식 (27)로 전역 비 유클리디안 공간상에서 증분들을 누적해야 합니다.

 

 매니폴드 표현법에서 계싼되는 선형 시스템은 유클리디안 공간에서 계사되는 선형시스템과 같은 구조를 가지며, 비 매니폴드 버전으로부터 그래프 최소화 하는 매니폴드 버전을 쉽게 얻을수 있으며, 대응하는 파라미터 블록에 대해  그 연산자와 자코비안 $M_t$만 정의하면 됩니다. 알고리즘 2는 SLAM에 적용하기 위한 가우스 뉴턴의 매니폴드 버전을 보여주고 있습니다. 

 

 매니폴드 문제에서의 헤시안 $\breve{H}$는 더이상 궤적의 정보 행렬로 사용되지 않습니다. 알고리즘 2에서 궤적의 정보 행렬을 얻기 위해서 원래의 자세 x공간에서 H를 계산하면 되겠습니다.

 

알고리즘 2. 알고리즘 1의 매니폴드 버전.

 이 알고리즘은 같은 계산 복잡도를 가지며, 특히 3차원의 경우에 비 매니폴드 버전보다 더 강인합니다.