iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree

(PDF) iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree

iSAM2는 square SAM, iSAM 이후에 나온 후속 논문으로 GTSAM(Georgia Tech Smoothing and Mapping) 라이브러리에서 localization 알고리즘으로 사용되고있는 컨셉입니다. ISAM2에서는 Bayes tree라는 데이터 구조를 제안합니다. Square SAM에서도 matrix factorization이 variable elimination 과정과 동일하다고 언급하는데 이러한 사실을 기반으로 

 

Chordal Graph

Figure 1. A cycle with two chords. Removing one green edge would result in a non chordal graph.

"In the mathematical area of graph threory, a chordal graph is one in which all cycles of four or more vertices have a chord, which is an edge that is not part of the cycle but connects two vertices of the cycle." - WIKIPEDIA

일반적으로 Bayes net에서 margianlization과 optimization이 쉽지 않다고 알려져있는데 elimination/factorization 을 통해 나오는 Bayes net은 chordal이며(?), 이런 chordal bayes net은 tree구조로 변환될수가 있다고 합니다.

---

Clique Tree

Figure 2. The 11 light blue triangles form maximal cliques. The two dark blue 4-cliques are both maximum and maximal, and the clique number of the graph is 4.

"In the mathematical area of graph  theory, a clique  is a subset of vertices of an undirected graph such that every two distinct vertices in the clique are adjecent." - WIKIPEDIA

iSAM2에서 제안하는 data structure는 clique tree와 유사하지만 bayes tree는 directed 그래프이며 QR factorization의 결과에 좀더 fit 하다고 합니다.

 

---

 

Problem Statement

자주 봤던 여러 과정들은 생략하고 위 수식부터 시작하겠습니다. 결국 linear system \(A\Delta - b\) 는 choelsky factorization 또는 QR matrix decomposition으로 풀게됩니다. iSAM을 잠시 복기해보자면 batch factorization을 수행하는 대신 Givens rotation을 이용해 직접 \(R\)을 업데이트 해줍니다. 하지만 새로운 variable들이 추가되면서 variable ordering 이 optimal에서 멀어지고 fill-in역시 발생하게 됩니다. 따라서 주기적으로 variable reordering이 수행되는데 이는 batch factorization을 동반합니다. 이때 relinearization도 끼워서 연산하는데 heuristic periodic batch step linearization은 optimal solution이 아니게 됩니다. 

 

The Bayes Tree

Figure 3. Chordal Bayes net (a) result from factor graph and Bayes tree (b) describing the clique structure in the chordal Bayes net.

Bayes tree는 node가 chordal Bayes net의 clique \(C_{k}\)를 표현하는 directed tree입니다.

paper를 보면서 이해하려고 했지만 이부분은 글로만 봐서는 전혀 이해가 가지 않았습니다...

RI Seminar: Michael Kaess : Robust and Efficient Real-time Mapping for Autonomous Robots - YouTube

RI seminar : Michael Kaess 

위 영상에서 Bayes tree를 Chordal Bayes net으로 부터 어떻게 만들어내는지 잘 설명해줍니다. Factor graph에서 variable elimination을 통해 bayes net을 결과로 만들었는데 Bayes tree는 elimination의 역순으로 clique를 찾아갑니다. Variable elimination은 Figure 3. (c)의 column 순서대로 진행되기 때문에 clique searching은 \(x_{3}\) 부터 \(l_{1}\) 방향으로 진행됩니다.

1. 처음 \(x_{3}\) 단일노드로는 clique 형성이 되지 않기 때문에 다음 \(x_{2}\) 노드로 진행하면 clique이 형성되고 root가 되게 됩니다.
2. \(x_{1}\)으로 진행하면 \(x_{2}\) 와는 연결되어있지만 \(x_{3}\)와 연결되어있지 않기 때문에 새로운 clique이 되고 root의 child로 들어가게 됩니다.
3. \(l_{2}\)로 진행하면 \(x_{3}\) 와 clique이 만들어집니다.
4. \(l_{1}\)은 \(x_{1}\), \(x_{2}\)와 둘다 연결이 되어있기 때문에 2번 스텝에서 만들어진 clique의 멤버로 들어갑니다.

Incremental Inference

Figure 4. Evolution of the Bayes tree

Measurement가 새로 추가되면 Bayes Tree \(\mathscr{T}\)는 업데이트 되어야합니다. 예를들어 \(f^{'}(x_{j}, x_{j^{'}})\) factor가 추가된다면 \(x_{j}\), \(x_{j^{'}}\)를 포함하는 clique 사이에 있는 path, 그리고 root 만 업데이트되게 됩니다. 위 그림에서 보면 loop closing이 없는 일반적인 상황에서는 업데이트 되는 노드들이 상당이 적은것을 볼 수 있습니다.

 

Figure 5. Updating a Bayes tree with a new factor, based of Figure 3.

Bayes Tree 각각의 node들은 Bayes net의 clique를 의미합니다. 따라서 Figure 5. 처럼 새로운 factor가 추가된다고 가정하면 \(x_{j}\)에 해당하는 \(x_{1}\), \(x_{j^{'}}\)에 해당하는 \(x_{3}\)를 포함하는 path 인 빨간색 점선으로 표시된 부분만 업데이트 되게 됩니다. 해당 부분에 대한 factor graph에 대해 다시 variable elimination, reverse order clique searching 과정을 거쳐 새로운 bayes tree \(\mathscr{T^{'}}\)을 만들게됩니다.

Bayes tree update의 알고리즘은 위와 같습니다.

 

Fluid Relinearization

Figure 6. (a) Applying COLAND to the affected variables in each step. (b) Forcing the most recently accessed variables to the end of the ordering using constraint COLAMD.

사실 incremental SLAM은 이전 논문 iSAM에서도 Givens Rotation 을 이용해 가능했습니다. iSAM이나 iSAM2 나 결국 update 되는 node를 graphical model이든 matrix 의 형태이든 backsubstitution을 통해 해가 구해지기 incremental update의 performance에 극적인 영향을 주진 않습니다.

iSAM2에서 언급하는 문제는 iSAM의 Relinearlization 의 시점입니다. Incremental SLAM을 수행하면서 새로운 measurement가 추가될때 fill-in이 발생하게 되는데 이는 performance에 큰 영향을 주게됩니다. 이를 피하기위해 iSAM에서는 periodic variable reordering을 수행하게 되는데 이때 Relinearization을 끼워서 수행합니다. Relinearization 수행 시점이 variable reordering 주기에 종속되기 때문에 linearization point가 optimal이 아니게 됩니다. 

하지만 iSAM2에서 Bayes Tree Structure가 도입되면서 Variable Reordering 이 매 incremental update 마다 수행되도록 수정되었습니다. Bayes tree에서 새로운 measurement가 추가되면서 영향을 받는 clique node 개수가 대체로 적기 때문에 cost가 크지 않기때문입니다. 물론 전체 노드의 일부만 reordering 하는것은 optimal ordering에 멀어지지만 COLAMD와 비교해 경쟁력이 있다고 합니다.

 

매 update state마다 variable reordering이 가능해지면서 relinearization이 reordering period에 독립적으로 수행되게 됩니다. 매번 update가 되면서 relinearization validity를 유지하고 필요할때만 relinearization을 수행한다고 합니다. 사실 이부분은 정확히 이해가 되지 않는데 편차가 현재 linearization point에 대해 편차가 일정 threshold를 넘어가면 해당되는 variable들만 모아서 재선형화를 한다는 것 같은데... 이부분은 좀더 공부가 필요한 것 같습니다.