Square root SAM 톺아보기
추후 유튜브 영상으로 제작 예정 …
Summary
(Modern) (smoothing 기반) SLAM 은 (nonlinear) least-square (optimization) problem 이다.
SLAM (back-end) 은 Ax = b 를 푸는 것이다.
SLAM back-end은 A가 sparse 할 때, Ax = b 를 더 효율적이고 효과적으로 푸는 방법에 대한 것이다. -> Squared root SAM 에서 Ax=b대신 Rx=d 를 푸는 내용
SLAM back-end은 A가 sparse 하고, x의 semantic (예: pose인가 landmark인가?) 과 connectivity 에 대한 knowledge (예: 이 x는 다른 어떤 x들과 실제 물리적 그래프에서 연결되어 있는가?) 가 존재 할 때, Ax = b 를 더 효율적이고 효과적으로 푸는 방법에 대한 것이다. -> Squared root SAM 에서의 variable (column) ordering 이나 isam2 에서 bayes tree formulation 에 관한 내용
여기서는 안 다루겠지만 SLAM front-end는 …
- (Raw, noisy, massive) sensor data를 잘 요약하기 -> visual sensor를 예로 들면, feature 추출하기
- 새로운 (가볍거나, 더 나은) measurement model 을 제안하기 -> visual sensor를 예로 들면, projection model 을 이해하고 reprojection error 를 정의하기
- Robust data association (i.e., metric learning, nearest neighbor, matching 등) -> visual sensor를 예로 들면, 두 프레임 사이의 feature 를 outlier 적게 잘 매칭하기
Nature of SLAM
Likelihood of the sensor measurement: p(Z|X)
Mean 이 Z_i
Gaussian Variable X
아무튼 이 p (posterior)를 최대화 하는 것은
exponential 안의 것을 최소화 하는 것이므로
따라서 SLAM 의 탄생은 probabilistic 했지만
SLAM 문제를 푸는 Engine 은 LS (least square) problem 이 되었다.
SLAM is solving Ax=b
Measurement model: h_i(X)
Linearized model: H_i*X
Sensor Measurement: Z_i
Error: H_i(X) — Z_i
Error 들을 쌓은 것: HX — Z
||HX — Z||_2 (error 의 2 norm을 최소화)
A’Ax = A’b 를 풀면 가장 나이브 한데
이 식을 보고 normal equation 이라고 부른다.
이 때 슬램문제에서 A가 Jacobian (i.e., linearized) 되었던 것이기 때문에 A’A 를 헤시안이라고도 부른다.
Λ 로 간단히 쓰기도 하며 인포메이션 매트릭스라고도 자주 불린다.
(이 때 factor graph 책 및 isam2 에서 강조되고 있는 graphical model 과 LS problem 사이의 관계를 잘 알아야 한다, 이런 이야기들이 매우 흥미로운 부분이 있는데 나중에 더 자세히 다룬다)
SLAM is <efficiently> solving <sparse> Ax=b
그럼 기존에 linear alg 사람들이, Ax=b 를 어떻게 빠르고 잘 푸는지 연구해둔 머시너리를 갖다 쓰면 된다.
여기 설명이 좋다. http://www.cs.cornell.edu/courses/cs4220/2014sp/CVLBook/chap7.pdf 이 책은 코넬대학교 2014 lecture CS4220 Numerical Analysis: Linear and Nonlinear Problems 에서 소개된 바 있다.
Squared root SAM의 핵심은
Ax=b 대신에 upper triangular form 인 Ry=d 를 풀어도 argmin 은 똑같으니, 더 풀기쉬운 (+작은 dimension의) Rx=d 를 풀자는 것
http://www.cs.cornell.edu/courses/cs4220/2014sp/CVLBook/chap7.pdf 의 248쪽 에서도 그 필요성이 잘 이해되고 있다. Rx=d 형태일 경우 solution 을 구하는 작업이, 마지막 variable 의 경우 그냥 주어지고, 이를 back-substitution 하면 되기 때문에 매우 쉽다.
(여기서 소개하는 게 맞는지는 모르겠는데, 일단 이야기하자면: Λ은 물리적 의미는 SLAM system의 MRF에 해당하고, R의 물리적 의미는 bayes network 에 해당한다<그래서 upper triangle (방향성존재) 이다!> — 이게 그냥 그런갑다 할 수 있겠지만 Kaess는 상당히 강조하고 있으며 이후에 isam2 의 개발에도 영감을 주게 된다.)
근데 그러면 다시 문제는, 이 R이란 놈을 구해야 하는데
이 걸 더 잘 구하고, 더 빠르게 구하는 것이 SLAM back-end 연구에서 core 이다.
크게 Cholesky factorization 와 QR factorization 두 가지 방법이 유명하다.
Cholesky 는 A’A = Λ = R’R 의 과정을 거쳐 R을 얻어낸다. 즉, information matrix 를 계산 후, 이를 다시 분해하는 기법이다.
반면에 QR decomposition 은 Λ 계산할 필요 없이 A를 바로 = Q[R;0] 으로 분해한다. Cholesky 보다 더 numerically 안정적이고 정확하다고 알려져있다.
A 가 m by d 일 때 (즉 measurement 가 m개 있고, d 개의 variable 에 대한 최적해를 얻고싶은 상황. 보통 m >> d),
두 방법 모두 O(m*n²) 이지만 QR factorization 이 상수배 (2배) 만큼 느리다.
한편 sparse matrix 에 대해 더 효과적 효율적 으로 factorization 하는 (==R을 구하는 것이) library 들도 여럿 공개되어 있다.
근데 information matrix 를 분해할 때, column order 를 바꾸어도, information matrix 의 sparsity 는 변함이 없으며, 최종 solution도 바뀌지 않는다.
하지만 어떤 column order 에서는 분해된 R이 dense 할수도 있고, 어떤 column order 에서 분해된 R은 더 sparse 할수도 있다.
이 때 원래 Λ의 (i, j) 는 비어있었지만, R에서는 그 (i, j)에 값이 생겨버릴 경우 이를 fill-in 이라고 부른다. 따라서 우리의 목표는 이 fill-in 을 줄이는 것 (==sparser 한 R을 만드는 것) 이 된다.
더 sparse 한 R을 얻는 것이 좋기 때문에, 여기서 Λ 의 column order 를 다시 잘 재배열 해주는 것이 우리의 관심사가 된다. — 이 때 column 들은 SLAM 에서 variable 에 해당하기 때문에, 이 과정을 variable (re)ordering 이라고도 부른다.
근데 최적 order 를 찾는 것은 NP-complete 하기 때문에, 실제로는 domain knowledge 가 요구된다.
가장 나이브하게는: 제일 간단한 형태의 SLAM에서 (i.e., pose 와 landmark 로 system이 구성), landmark 를 먼저 column order에 위치 (한쪽으로 다 몰고) 하고, 그 다음에 pose variable 들을 놓아두는 방법이 있다. — 이 경우 최종 결과로 pose 들 사이에 fill-in 들이 생기게 된다. 즉, pose들이 densely connected 되게 됨. Pose 개수가 상대적으로 적고 landmark 개수가 많을 경우에는 (예: SfM) 그럭저럭 괜찮겠지만, 우리는 더 나은 방법이 필요하다.
위의 나이브한 휴리스틱은 Schur-complement trick 이라고도 불린다. Variable 들을 두 그룹으로 쪼개고 몰아넣는 과정에서 Schur complement of a matrix 가 이용되기 때문이다.
이를 개선하기 위해 node 의 degree 기반으로 하는 방법이 Approximate Minimum Degree (AMD) method 이며 naive 한 방식보다 더 sparse 한 R을 얻을 수 있다.
Agarwal and Olson 이 12 IROS Variable reordering strategies for SLAM 에서 다양한 ordering 방법들을 비교하기도 했다.
여기까지가 Squared root SAM (2006 IJRR) 내용이다.
SLAM is <iteratively> solving <sparse> Ax=b
그런데 SLAM문제를 푸는 로봇이라는 상황은: 센서데이터들이 하나씩 순서대로 들어온다는 특성이 있다.
이 때 센서데이터가 하나 더 들어온다는 것은 measurement term 이 하나 더 생긴다는 것이며, 수학적으로는 Ax=b 에서 a single row 가 추가된다는 것을 의미한다.
그러면 A가 달라지게 되고 이 달라진 (사실 달라진 건 아니고 겨우 한 행이 추가된) A’ 를 이용해서 information 을 다시 계산하고, 다시 이를 분해하고, 하는 과정을 거쳐야만 하는가?
그것은 (batch) 너무 비싼 작업이다. 우리는 이미 직전 스텝 (t-1) 에서 구해놓은 R이 있다.
즉, R(t) 를 이용해서 R(t+1) 를 더 빠르고 쉽게 구할 수 있지 않을까? 하는 것이 incremental SAM (iSAM version 1) 논문의 내용이다.
이는 다음 강의에서 다룬다 …
출처
- Factor graph book (2017)
- Squared root SAM (2006 IJRR)
- Introduction to Scientific Computing (2e) — 특히 Squared root SAM관련인 QR factorization 에 관한 chap7 — http://www.cs.cornell.edu/courses/cs4220/2014sp/CVLBook/chap7.pdf
