Graph-based SLAM 입문 + Solver 프레임워크 소개 (Ceres-solver, g2o, GTSAM, SE-Sync)
Optimisation 기반 SLAM
SLAM 기술의 코어는 non-linear한 특성의 센서 데이터를 기반으로 map과 pose에 대한 최적의 state를 추정하는 것이다.
이전에는 Kalman filter나 particle filter 기반의 알고리즘들을 사용하여 observation model과 motion model이 확률/수치적으로 동의할 수 있는 최적의 state를 찾았다. 하지만 최근에는 batch optimisation 기술을 이용하여 최적의 state를 찾아내는 방식을 사용하는데, 이는 컴퓨터의 발전으로 인해 무거운 batch optimisation 게산을 실시간으로 수행할 수 있게 되었기 때문이다. Batch optimisation 방식은 filter 방식에 비교했을 때 절대적인 계산량은 더 많지만, 동일한 계산량 (i.e. CPU 사이클)에서 더 높은 정확도를 보여주기 때문에 컴퓨팅 파워가 받쳐주는 플랫폼에서는 optimisation 기법을 선호하는 경우가 많다.
이번 글에서는 이러한 optimisation 기반 SLAM solver에 대해 이야기한다.
Optimisation 기반 SLAM 방법론 중 가장 유명한 방법은 Graph-based SLAM이며, 보통 Frontend와 Backend 두가지 단계를 거친다.
Frontend는 센서 값들로부터 현재 로봇의 이동치와 주변 환경에 대한 정보를 계산하는데, 이 때 이동치는 motion model을, 주변 환경 정보는 observation model을 사용해서 계산한다. Graph-based SLAM에서는 로봇의 위치와 주위 랜드마크의 위치를 node로 표현하는데, 우리가 계산한 이동치는 로봇의 위치를 표현하는 node끼리를 이어주는 motion constraint, 그리고 주변 환경의 위치 정보는 로봇-랜드마크 간의 상대적인 관계를 표현하는 node를 이어주는 observation constraint로 표현된다. Frontend는 이렇게 새로운 node를 만들고, 또 node 끼리의 constraint도 이어주기 때문에 graph construction이라고 불리기도 한다.
Backend는 frontend에서 생성된 constraint들을 기반으로 최적의 (i.e. optimal) pose & map을 계산한다. Graph solving에 가장 보편적인 방법은 least squares로써, 선형대수에서 이야기하는 Ax=b 형태의 문제에서 A가 graph matrix, b가 센서 정보일 때, x가 무엇이 나와야하는지 찾는 것이라고 보면 된다. 이러한 Graph matrix는 엄청나게 거대하기 때문에 단번에 푸는 것은 불가능하고 (특히나 SLAM에서 요구하는 실시간 계산 조건에서는!), 그렇기 때문에 iterative하게 풀어야한다. 우리가 사용하는 센서들은 non-linear하기 때문에 iterative solving에도 매번 linearization 과정이 들어가야한다.
Backend를 좀 더 자세히 이해하고 싶다면 다음 글을 참조하면 좋다. 1.’Bundle adjustment란?‘, 2. ‘비선형 최적화 Non-linear optimisation - Gradient descent, Newton-Raphson, Gauss-Newton, Levenberg-Marquardt‘
Backend의 과정을 정리해보자면 다음과 같은 단계가 있다.
- Constraint 기반으로 least squares optimisation problem 생성
- Linearization
- Iterative solving
이 모든 과정을 쉽게 계산하기 위해 우리는 Solver 라이브러리들을 사용한다.
Solver 라이브러리 소개
이러한 optimisation 계산을 빠르게 구현할 수 있도록 여러 solver 프레임워크들이 개발되었다. 가장 유명한 것들로는 다음과 같은 라이브러리들이 있다.
- Freiburg 대학의 g2o
- Google의 ceres-solver
- Georgia Tech의 GTSAM
- MIT의 SE-Sync
g2o는 graph 형태의 nonlinear 함수들을 최적화 할 수 있는 오픈소스 프레임워크이다.
2011년 릴리즈 당시 저자들은 SOTA 알고리즘들의 성능에 준한다고 이야기했고, 현재도 지속적으로 업데이트되면서 다양한 모던 프로세서에 대해 호환성도 가지고 있다.
프레임워크에서 Pose graph optimisation을 푸는 방법으로는 Gauss-Newton, Levenberg-Marquardt, Doglog 방식을 지원하는데, 이는 왠만한 SLAM 문제는 다 풀 수 있다고 볼 수 있다. 사용하기 굉장히 쉬운 편이기 때문에 Visual-SLAM 연구/개발에서 많이 사용하며, ORB-SLAM과 SVO에서도 사용되었다.
특정 모듈들이 LGPL3+와 GPL3+ 라이센스를 가지고 있지만, 특정 linear solver 및 뷰어에 한정되어있다. 연구 쪽에서는 오픈소스하는 경우가 많기 때문에 GPL-v3 라이센스는 문제가 되지 않아 연구와 궁합이 잘 맞고, 상용쪽에서는 최적의 성능을 위해 어차피 뷰어를 잘 사용하지 않기 때문에 상용에도 궁합이 잘 맞는 편이다.
Ceres-solver는 대형 nonlinear least squares 최적화 문제를 풀기위해 만든 오픈소스 라이브러리이다. Bundle adjustment 기법을 사용하는 SLAM과 SfM (i.e. Structure from Motion) 분야에서 많이 사용한다.
사용하기 쉬운 편이며 뛰어난 성능을 가졌고 portable하다는 장점을 가지고 있다. 하지만 무엇보다 가장 큰 장점은 최적화에 필요한 objective function을 유저가 직접 만들 수 있다는 점 (i.e. custom function)인데, 이는 새로운 아이디어/방법론을 연구할 때 엄청난 이점이 된다. 여기에 linearization을 쉽게 할 수 있는 auto-diff 기능까지 있으니 기능과 편의성을 동시에 잡았다고 볼 수 있다.
지원하는 solver는 trust region solver (i.e. Levenberg-marquardt, dogleg)와 line search solver가 있다.
센서퓨전을 이용해서 tightly-fused 알고리즘들은 왠만하면 Ceres를 이용하며, Camera + IMU를 사용하는 OKVIS와 VINS-Mono가 Ceres 를 사용한다.
GTSAM은 로보틱스 환경에서 센서퓨전 최적화 문제를 풀기 위해 만든 오픈소스 라이브러리이다.
GTSAM은 factor graph를 이용해서 최적화 문제를 모델링하는데, 다른 라이브러리들에 비해 내부적으로 matrix sparsity를 잘 이용해서 계산 효율이 가장 좋다고 볼 수 있다.
다른 라이브러리들이 지원하는 Gauss-Newton, Levenberg-Marquardt, dogleg에 더불어 conjugate gradient optimizer와 iSAM solver (i.e. incremental smoothing and mapping) 기능을 가지고 있다. 최근 graph-based SLAM 형태로 VIO를 구현할 때는 IMU preintegration 기술의 사용이 필수적인데, 이러한 기능을 쉽게 사용 가능한 함수 API로 구현해두었다.
현재 구현 단계에서도 연구/개발 양쪽 다 쉽게 사용할 수 있지만, 최근 OpenSAM이라는 이름으로 좀 더 industry에서 사용하기 좋은 형태를 만들고 있다는 소식이 있다.
SE-Sync는 special Euclidean group (i.e. 3D pose) 최적화 문제에서 global minima를 보장하는 certifiably correct algorithm을 이용한 최적화 라이브러리이다.
2D/3D geometry fitting, graph-based SLAM, camera motion estimation 등에서 사용할 수 있다.
가장 최신의 프레임워크이라 아직 유저층이 두텁지는 않다.
다른 프레임워크들에 비해 가장 진보된 알고리즘을 가지고 있다. 예를 들어, large scale optimisation을 풀 때는 많은 constraint들로 인해 non-convex한 manifold가 구축되는데 (i.e. local minima가 많이 생기는데), 이 문제를 좀 더 풀기 쉬운 convex 형태의 문제로 근사시켜 global minima를 계산하는 semidefinite relaxation 기법 등이 있다. (Trust region 방법으로 truncated-Newton Riemannian method를 사용한다고 하는데… 뭔지 잘 모르겠다 이름은 멋있는데!)
어떤것을 써야하는가?
Juric 2021 논문에서 발표한 내용은 다음과 같다.
- 가장 빠른걸 원한다면 SE-Sync
- SLAM 프론트엔드에서 노이즈를 잘 잡아낸다면 GTSAM도 상관 없음
- 모든 성능면에서 평균적인건 Ceres-solver
- g2o는 쉬운 문제에서 잘 됨