그래프 기반 SLAM 튜토리얼 - 4.B 선형화된 시스템의 구조에 대한 고려사항들

B. 선형화된 시스템의 구조에 대한 구조사항들

 식 (14)에 따르면 행렬 H와 벡터 b는 하나 하나가 제약조건들인 행렬과 벡터들의 합으로 얻을수 있습니다. 모든 제약조건들은 시스템에 추가가 되어지는데, 추가되어지는 시스템의 구조는 에러 함수의 자코비안에 의존하게 됩니다. 제약 조건의 에러함수가 두 노드의 값에만 의존하므로 식 (7)에서 자코비안은 다음과 같은 형태를 가지게 됩니다.

 

 여기에 $A_{ij}$와 $B_{ij}$는 각각 $x_i$와 $x_j$에 대한 에러함수의 미분으로, 식 (10)으로부터 다음의 블록 행렬 $H_{ij}$를 얻을수가 있게 됩니다.

 표기를 편하게 하기위해 0인 블록들은 생략하겠습니다.

 

 

알고리즘 1. 그래프의 제약조건으로부터 로봇의 자세 사후확률의 다변수 가우시안 근사의 평균 $x*$와 정보 행렬 H* 계산

 

 

 알고리즘 1은 로봇의 자세에 대한 사후확률의 평균과 정보행렬을 계산하는 반복 가우스-뉴턴 과정을 요약하였습니다. 이 시스템의 구조 대부분이 희소하기 때문에 시스템의 헤시안 H을 저장하기 위해서 메모리 효율적인 표현법을 사용하기를 추천합니다.

 

 헤시안의 구조는 그래프의 연결성으로부터 즉시 알수 있으므로, 반복의 시작시에 헤시안을 미리 만들고, 새로운 선형화가 수행될때마다 모든 에지에 대해 루프함으로서 갱신시키는것을 추천 합니다. 각 에지들은 블록 $H_{[ii]}$, $H_{[ij]}$, $H_{[ji]}$, $H_{[jj]}$과 계수 벡터의 블록 $b_{[i]}$, $b_{[j]}$에 영향을 줍니다. 추가적인 최적화는 H의 위 3가지 부분에서만 수행하면 되겠습니다.

 

 제약 조건의 오차 $e_{ij}$는 연결된 자세 $x_i$와 $x_j$의 상대적인 사에에만 의존한다는 점을 생각하면 자세 x의 특정 부분의 에러 F(x)는 모든 자세의 강체 변환에 대해 불변하게 됩니다. 이를 통해 식 (15)와 같은 결과가 나오게 됩니다.

 

 이 시스템을 수치적으로 풀기 위한 시용적인 방법으로는 증분치들 중 하나를 0으로 제한하면 됩니다. 그러므로 이 과정은 $k^{th}$ 대각 블록 H[kk]에 아이덴티티 행렬을 추가하여 수행할수 있겠습니다. 알고리즘 1에서 일반화 손실 없이, 첫 노드 $x_!$을 고칠수가 있습니다.

 

 자세 그래프의 특정 노드를 고치기위한 다른 방법으로 식 (15)의 선형 시스템의 $k^{th}$ 블록의 행과 열을 억제시키면 되겠습니다.