iSAM: incremental Smoothing and Mapping

[PDF] iSAM: Incremental Smoothing and Mapping | Semantic Scholar

iSAM은 square root SAM 이후의 후속 논문입니다. Square Root SAM은 factor graph의 variable elimination 과정이 sparse factorization과 매우 유사하고 이를통해 효율적인 back-end 연산을 제안했습니다. 하지만 square root SAM는 연산이 batch로 이루어져 새로운 measurement가 들어오게되면 전체 information matrix를 update를 해 불필요한 연산이 수행되었습니다. iSAM은 새로운 measurement가 들어오게되면 이전에 계산한 값 재사용하고 새로운 값에 의해 영향을 받는 entry만 계산하는 방식으로 square root information matrix를 업데이트 합니다.

 

SAM : A Smoothing Approach To SLAM

Bayesian belief network representation of th e SLAM problem.

The joint probability of all variables and measurements

잠시 복기하자면 SLAM의 back-end는 위 수식과 같은 joint probability를 최대화하는 과정입니다. 여기서 가정하는것은 correspondence(어느 위치에서 어떤 landmark를 관측했는지) $(i_{k}, j_{k})$가 주어진다는점과 Gausian measurement model을 사용한다는 점입니다.

위 두 수식은 각각 motion model과 measurement model을 의미합니다. 따라서 joint probability를 최대화하는 optimal $X^{*}, L^{*}$는 구하기 위해 풀어서 써보면 아래의 수식처럼 정리됩니다.

수식이 복잡해보일 수 있지만 정규분포의 확률밀도함수를 생각하시면 직관적으로 받아드릴 수 있습니다. 수식에서 볼 수 있듯이 문제가 least square ​형태로 바뀌었습니다. Motion model과 measurement model이 선형이 아닐때 linear approximation을 통해 문제를 해결하게됩니다. Linearization을 통해 수식을 정리하면 다음과 같습니다.

 

QR Factorization

위에서 구한 least square 문제를 풀때 주로 Cholesky factorization과 QR factorization(decomposition)입니다.  QR factorization은 matrix $A \in \mathcal{R}^{m \times n}$가 주어졌을때 orthogonal matrix \(Q \in  \mathcal{R}^{m \times m}\)와 upper triangular$R \in \mathcal{R}^{n \times n}$을 찾는것입니다. SLAM에서는 $m > n$이기 때문에 위와 같은 형태로 구해지게 됩니다.

Matrix $Q$가 orthogonal matrix이기 때문에 위와 같이 정리되고 결국 $R\theta^{*} = d$ 라는 linear system을 얻게됩니다.

 

ISAM : Incremental Smoothing And Mapping

Incremental smoothing and mapping은 새로운 measurement가 들어왔을 때 불필요한 계산을 하지 않는것이 목적입니다. 

Matrix Factorization by Givens Rotations

Givens rotation matrix 행렬의 특정위치에 값을 0으로 만드는데 사용됩니다. Givens rotation이 사용되는 이유는 새로운 measurement가 들어왓을때 matrix A를 update하고 다시 전 과정을 거치는것보다 current $R$ 과 $d$ 에 새로운 row $w^{T}$ 와 $\gamma$를 추가해 givens rotation을 통해 직접 update를 하는것이 효율적이기 때문입니다.

일반적으로 Back-substitution은 computational complexity가 $O(n^{2})$ 지만 위와같이 band-diagonal matrix 형태를 가질경우 $O(n)$으로 제한할 수 있습니다.

 

Loops and Non-Linear Functions

제가 SLAM에서 loop을 이해하기로는 이전에 이미 방문한 곳을 재방문하면서 uncertainty를 줄일 수 있는 상황입니다. 하지만 back-end 연산에서만 본다면 complexity를 늘린다는 단점이 있습니다. 

 

 

위 그림처럼 loop이 생기게 되면 행렬 $R$에선 localy dense 한 영역이 만들어지고, back-substitution 의 complexity가 올라가게됩니다. 이런 fill-in을 피하기 위해 variable reordering technique을 이용합니다. Square SAM에서도 잠깐 언급되었지만 fill-in을 최소화하는 optimal order searching은 NP problem이기 때문에 heuristic 방식을 이용합니다. 

 

A column approximate minimum degree ordering algorithm | ACM Transactions on Mathematical Software

A column approximate minimum degree ordering algorithm | ACM Transactions on Mathematical Software

 

Experimental Results

 

 

paper 에 Data Association에 대한 내용도 상당 부분을 차지하고있지만 모든 내용을 다 보기가 쉽지 않아서 여기서 끊습니다 ㅠㅠ.