BALM 저자의 랩에서 2023년 발표한 논문입니다.
앞선 BALM 논문의 리뷰는 아래 포스트를 참고해주세요.
BALM 1 (2021)
BALM 2 (2022)
Content
1. Introduction
2. System Overview
3. Related Work
A. PGO (pose graph optimization)
B. PA (plane adjustment)
C. BA (bundle adjustment)
4. Methodology
A. Overview
B. Bottom-Up Hierarchical BA
C. Top-Down Pose Graph Optimization
5. Experiments
A. Accuracy Analysis
1) Initial Odometry With Loop Closure
2) Initial Odometry Without Loop Closure
B. Ablation Study
1) Pose Graph Optimization Vs. Direct Assign
2) Hierarchical BA Vs. Reduced BA
C. Computation Cost
1. Introduction
Lio-sam, MULLS, Fast-lio2와 같은 LiDAR SLAM에서, scan-to-map registration error가 축적되기 때문에 odometry drift가 생기고 시간이 지나 발산하기에 이른다. mapping quality를 높이기 위해 주로 PGO(pose graph optimization)를 사용한다. PGO는 두 라이다 프레임 간의 relative pose 에러를 최소화한다. 이때 Gaussian distribution을 가정하고 relative pose를 추정한다. PGO는 time-efficient 하지만 mapping consistency를 직접적으로 최적화 하지 않는다. 포인트 클라우드 맵 내의 divergence는 줄어들 수 있지만 제거되지는 않고, 인식조차 하지 못하는 경우도 있다. 이 현상은 잘못된 loop가 감지되거나 틀린 relative transformation 추정을 했을 때 더욱 뚜렷하게 나타난다.
BALM(“BALM: Bundle adjustment for LiDAR mapping"), Zhou(“Lidar SLAM with plane adjustment for indoor environment”)의 방법과 같은 LiDAR BA는 이러한 문제를 해결할 수 있다. 모든 point-to-plane distance를 최소화하여 mapping consistency를 직접적으로 최적화한다. 따라서 high mapping quality를 얻을 수 있다. BALM은 먼저 plane 파라미터를 analytically하게 풀어서 최종 최적화 문제가 라이다 포즈에만 연관되게 했고, Zhou는 Schur complement 트릭을 사용해 각 반복에서 plane 파라미터를 제거했다. 하지만 둘 모두 최적화 차원이 최소한 라이다 포인트 수 $N$ 차원이며, 연산 복잡도는 $O(N^3)$ 이다. 연산 시간이 세제곱으로 늘어나기 때문에 large-scale map에서 엄청나게 time-consuming 하여 적용이 힘들다.
본 논문은 이를 완화하기 위해 time efficiency를 유지하며 mapping consisency를 전역적으로 최적화하는 hierarchical LiDAR BA를 제안한다. 본 논문은 bottom-up hierarchical BA 와 top-down pose graph 최적화의 장점을 결합하였다.
bottom-up hierarchical BA의 장점
- planar surface의 consistency(point-to-plane distance)를 직접적으로 최적화 한다.
- large dimension의 cost function을 푸는 것을 피할 수 있다.
- 병렬 연산에 적절하고, 포함된 포즈의 수가 적어 연산 시간이 적게 든다.
- Hessian matrix의 사이즈가 original BA보다 매우 작아졌다.
top-down pose graph 최적화의 장점
- LiDAR 포즈를 부드럽고(smoothly) 효율적(efficiently)으로 업데이트 한다.
본 논문의 방법을 통해 globally optimize the LiDAR mapping consistency and odometry accuracy이 가능하다. PGO 등으로부터 얻은 good initial pose trajectory가 주어졌을 때 mapping quality를 개선하고 initial pose trajectory에 큰 drift가 있을 때 gap을 줄일 수 있다.
구현에 있어서, 인접한 두 키프레임 사이의 smoothness를 유지하기 위해 stride size를 window size보다 작게 설정해 키프레임끼리 겹치는 영역을 유지한다. 더불어 최적화 속도를 가속하기 위해, outlier point를 제거하는 필터를 적용하고 피라미드를 구성할 때 CPU 기반 병렬 처리를 구현했다.
공간적/시간적으로 large-scale인 spinning 라이다 데이터셋(KITTI, MulRan, Newer College)과 직접 얻은 solid-state 데이터셋에서 이 방법의 효과와 robustness를 검증한다. 적절한 셋업에서 시퀀스 시간의 약 12%로 globally consistent map을 생성할 수 있다.
2. System Overview
프레임 포즈들의 피라미드 구조를 구성하고(Fig. 1 참고) bottom-up hierarchical BA와 top-down PGO를 수행한다(Fig. 2 참고). bottom-up process에서는 bottom-up layer(local BA)에서 top layer frames(global BA) 방향으로 local window를 이용해 hierarchical BA을 수행한다. 각 레이어의 local BA 과정이 병렬 연산에 적절하고, 포함된 포즈의 수가 적어 local BA의 시간 복잡도가 상대적으로 낮아 연산 시간 관점에서 좋다. bottom-up process의 한 가지 문제점은 다른 local window에서도 co-visible한 feature를 무시하기 때문에 정확도가 낮아진다. 이 문제를 완화하기 위해 top-down process에서는 top에서 bottom layer로 포즈 그래프를 구성하고 PGO로 error를 분산한다. 두 process는 수렴할때까지 반복된다.
Fig. 1에서, bottom-up process에서 다음 레이어에서 사용될 키프레임을 생성하기 위해 동일 local window 안의 프레임들을 BA로 최적화 한다. 두 인접 local window의 첫 번째 프레임은 s=3 만큼 떨어져 있다. top-down process에서 동일 레이어의 인접 프레임들은 bottom-up BA에서 구한 factor들에 의해 연결되어 있다. 두개 이상의 local window들이 겹치는 프레임들(예를 들어 $T^1_3$와 $T^1_4$)은 두 개 이상의 factor가 연결되고 각각은 하나의 local window BA로부터 나왔다. 두 인접 레이어를 가로지르는 빨간 점선은 동일한 두 노드를 연결한다.($T^1_9$, $T^2_3$, $T^3_1$)은 동일하다.
3. Related Work
매핑 품질을 개선하기 위한 방법은 크게 PGO(pose graph optimization)와 plane(bundle) adjustment 기반 방법으로 나눌 수 있다.
A. PGO(pose graph optimization)
ICP나 ICP variant로 두 프레임 간의 relative transformation(pose constraint)를 추정한다. 이 relative pose error는 information matrix(주로 ' inverse of the corresponding Hessian'이나 단순히 'constant matrix' 사용)로 가중치가 곱해진다. 포즈 그래프는 relative pose error의 합이 최소화 됐을 때 최적화 된다. 연산 효율성에도 불구하고 PGO는 포인트 클라우드의 consistency를 직접적으로 최적화 하지 않는다는 문제가 있다. 잘못된 추정이나 relative pose constraint의 부정확한 모델링으로 인해 local minimum에 빠질 수 있다.
B. PA(plane adjustment)
PA는 point-to-plane distance의 합을 최소화 함으로써 포인트 클라우드의 consistency를 직접적으로 최적화한다. 각 plane feature는 center에서 viewpoint 까지의 거리, 추정된 plane normal vector 두 파라미터로 표현된다. factor graph optimization을 이용한 논문에서 라이다 포즈들과 geometric plane feature들을 동시에 최적화 하려고 했으나, 이 방법은 최적화 동안 모든 feature들의 파라미터를 유지하고 업데이트 해야 하고 map이 커질수록 feature 수는 급격하게 커지므로 풀어야 하는 cost function의 차원이 매우 거대해진다. Schur complement technique을 사용해서 최적화 변수를 라이다 포즈로만 줄여 계산량을 감소시킬 수는 있으나 feature파라미터들에 대한 정보를 잃을 수 있다. 따라서 현실에서 포즈 추정에 오류를 발생시킬 수 있다. (BALM은 feature 파라미터를 제거하는 과정에서 단순히 파라미터를 줄이는 것이 아니라 모든 점들의 특징(평균, 공분산)을 반영하기 때문에 정보를 어느 정도 유지하면서도 모든 점을 고려할 필요가 없어 시간 복잡도가 감소한다.)
C. BA(bundle adjustment)
최적화에 앞서 closed-form solution으로 feature 파라미터를 제거함으로써 PA를 개선한다. BALM에서는 포인트 클라우드를 multiple voxel로 분할하는데 이때 각 voxel은 plane feature를 포함한다. original point-to-plane 최소화 문제를 각 복셀의 points covariance의 고유값을 최소화 하는 문제로 변환했다. 이러한 방법은 시간 복잡도가 포인트 수의 제곱인 헤시안 행렬을 도출하기 위해 각 feature의 모든 포인트에 대해 반복해야 하므로 연산이 엄청나다. BALM 2.0에서는 이 문제를 완전히 해결했다. 동일 포즈로 관찰한 feature의 모든 포인트들은 포인트 클러스터로 집계되어 총 포인트 수에 대한 시간 복잡도의 의존성을 근본적으로 제거하고 계산 정확도화 효율성을 더욱 향상시킨다. 다른 논문에서는 co-visible plane feature 들의 position을 anchor로 고정시키고 related LiDAR pose들에 대해서만 최적화 하여 이 문제를 해결했다.
앞서 언급한 모든 PA, BA 방법들은 포즈 수가 $N$일 때 $6N$ 차원의 비선형 최적화 문제이다. 이 문제를 풀기 위해서는 Cholesky factorization을 사용해 일반적으로 $O(N^3)$이 걸리는 $6N$ 차원 선형 시스템을 풀어야 한다. 이 선형 시스템을 $O(N)$의 시간 복잡도로 풀 수 있는 PCG(pre-conditioned conjugate gradient) 알고리즘에도 불구하고, 특히 pre-conditioned matrix의 크기가 큰 경우 추가 반복이 필요하다. 게다가 map에서의 차이가 최대 복셀 사이즈보다 크거나 근접할 때 이러한 PA/BA 방법은 수렴 속도가 느릴 수 있다.
본 논문에서 제안하는 hierarchical BA는 BALM 2.0에 기반하여 발전시켰으며 BA와 PGO의 장점을 모두 취한다. point-to-plane distance를 직접적으로 최소화 하기 위해 BA를 사용하고 라이다 포즈들을 부드럽고 효율적으로 업데이트 하기 위해 PGO를 활용한다. hierarchical design을 사용함으로써 multiple BA 문제들을 훨씬 작은 헤시안 행렬 사이즈로 병렬적으로 풀 수 있다. 또한 initial pose trajectory의 quality에 따라 BA 파라미터를 bottom layer에서 top layer까지 유연하게 설정할 수 있다.
4. Methodology
A. Overview
Fig. 2에 system workflow가 묘사되어 있다. 수렴할 때까지 반복되는 bottom-up과 top-down 두 process로 구성된다. input은 '각 라이다 스캔의 raw 혹은 deskewed points'와, 'global frame에 있는 이들의 해당 pose의 initial estimation'이다. deskewed LiDAR points와 스캔 포즈들은 일반적인 LiDAR odometry로부터 추정되거나 SLAM 알고리즘에서 최적화 될 수 있다. Fig. 1에서 보이는 것처럼 local window는 같은 레이어에서 고정된 수의 라이다 프레임의 collection에 해당한다. 한 레이어 안의 local window에 있는 포인트들은 다음 레이어에서 하나의 키프레임으로 모아진다(aggregated). first layer(bottom layer)는 initial LiDAR 프레임 및 포즈의 collection 이다. 비슷하게 second layer는 first layer에서 local BA를 통해 재구성 된 라이다 키프레임과 포즈의 collection 이다. top layer라는 용어는 마지막에 남아 있는 LiDAR 키프레임들의 collection 이다. (Fig. 1에서 top layer는 third layer에 해당한다.) bottom layer에서 top layer로 라이다 키프레임을 계층적으로 생성하는 과정을 bottom-up process라고 한다. PGO를 이용해 bottom layer LiDAR poses를 업데이트 하는 과정을 top-down process 라고 한다.
bottom-up process에서, bottom layer에서 상단 layer로 키프레임을 구성하기 위해 BA를 각 local window(local BA) 내에서 라이다 프레임들에 대해 수행한다. 즉 first layer에서 second layer로, second layer에서 third 레이어로 수행된다. 이때 각 local window 내에서 최적화된 relative pose는 후에 top-down process에서 사용하기 위해 저장한다. 이 프로세스는 최적의 레이어 수를 만족할 때까지 계층적으로 수행되며, 이후 전체 top layer 키프레임들에 대해 BA를 수행한다(global BA). bottom-up BA process에서 얻은 relative poses는 top-down process에서 포즈 그래프 구성을 위한 constraints(factors)로 사용한다. 이후 first layer poses를 업데이트 하기 위해 포즈 그래프를 푼다(Fig. 4 참고).
B. Bottom-Up Hierarchical BA
- $\mathbb{F}^i_j$ : $i$-th layer의 $j$-th LiDAR frame
- $\mathbf{T}_j^i=\left(\mathbf{R}_j^i, \mathbf{t}_j^i\right)$, with $\mathbf{R}_j^i \in S O(3)$ and $\mathbf{t}_j^i \in \mathbb{R}^3$ : $i$-th layer의 $j$-th LiDAR frame의 state이다.
- $\mathbf{T}_{j, k}^i$ : $\mathbf{T}_{j}^i$와 $\mathbf{T}_{k}^i$ 사이의 relative pose. 즉, $\mathbf{T}_{j, k}^i=\left(\mathbf{T}_j^i\right)^{-1} \cdot \mathbf{T}_k^i$
- $w$ : local window size during botto-up construction
- $s$ : stride size during botto-up construction
- $N_i$ : $i$-th layer 에서의 총 라이다 프레임 수
즉, $\mathbb{F}^i_j$의 포인트는 LiDAR local frame으로 표현되고 $\mathbf{T}_{j}^i$는 global frame으로 표현된다.
bottom-up process에서는 주어진 initial pose trajectory를 이용해 각 local window에서 BA를 수행한다. $i$-th layer의 $j$-th local window는 $w$ frames $\left\{\mathbb{F}_{s \cdot j+k}^i \mid j=0, \ldots,\left\lfloor\frac{N_i-w}{s}\right\rfloor ; k=0, \ldots, w-1\right\}$와 window 내의 first frame과 다른 frame 간의 relative poses $\left\{\mathbf{T}_{s \cdot j, s \cdot j+k}^i\right\}$을 포함하고 있으며 local BA를 이용해 최적화 되고 이때 first frame의 포즈는 gauge freedom 을 해결하기 위해 고정시킨다. local BA는 Hessian matrix $\mathbf{H}$를 구성하고 최적의 relative poses $\left\{\mathbf{T}_{s \cdot j, s \cdot j+k}^i\right\}$에 대해 풀어서 local window의 모든 포인트를 포함하는 키프레임 $\mathbb{F}_j^{i+1}$을 $(i+1)$-th layer에서 사용하기 위해 구성한다(Fig. 1, (1a) 참고). 키프레임의 포즈 $\mathbf{T}_j^{i+1}$ 는 앞서 수행된 모든 local window에서 푼 최적의 relative poses를 곱하여 계산한다((1b) 참고). 이 local window BA로 도출된 $\mathbf{H}$도 기록해두었다가 후에 top-down pose graph 구성에서 information matrix로 사용된다.
$$
\begin{align}
& \mathbb{F}_j^{i+1} \triangleq \bigcup_{k=0}^{w-1}\left(\mathbf{T}_{s \cdot j, s \cdot j+k}^{i *} \cdot \mathbb{F}_{s \cdot j+k}^i\right) \tag{1a}\\
& \mathbf{T}_j^{i+1}=\mathbf{T}_{s \cdot j}^{i *}=\prod_{k=1}^j \mathbf{T}_{s \cdot k-s, s \cdot k}^{i *} \tag{1b}
\end{align}
$$
이 프로세스는 최적의 layer number $l$에 도달할 때까지 lower layer에서 upper layer로 반복적으로 수행된다. 새로운 키프레임들의 구성(local BA)은 local window 밖의 프레임들에게 dependent 하지 않으므로 동일 레이어에서 병렬 프로세싱을 위해 multiple local window를 동시에 사용하기 적절하다.
Remark : 다른 thread에서 발생하는 동일 프레임의 경우 PGO 이후 포즈 일관성(pose consistency)를 보장하기 위해 다른 스레드의 다른 local window로부터 multiple relative constraints (factors)를 프레임에 적용한다. (각각 하나의 factor에 기여) (Fig. 1 참고)
라이다 프레임 전체 수가 $N$개이고 (i.e., $N=N_i$) 매번 lower layer의 $w$ 프레임들을 하나의 프레임으로 모아 upper frame으로 보낸다. (stride size $s$) $n$이 병렬 프로세싱 가능한 스레드 수라고 하자. 포함된 포즈 수가 $M$이라고 하면 BA의 연산 시간은 $O(M^3)$이기 때문에 $l$-th layer pyramid의 전체 소요 시간 $O(T_l)$을 도출할 수 있다.
$l$-th layer pyramid의 전체 소요 시간은 각 레이어의 local BA 수행과 top layer에서의 global BA 수행을 포함한다. $l$-th layer pyrimid에서 $i$-th layer ($i < l$)의 local window 수는 $\frac{N}{s^i}$이고 각 local window는 $O(w^3)$의 시간을 소요한다. 병렬 스레드가 $n$개일 때 local BA의 전체 시간 복잡도는 각 레이어의 local BA의 합과 같다(각 $w^3$) . 따라서 $\left(\sum_{i=1}^{l-1} \frac{N}{s^i} \cdot \frac{1}{n}\right)$이고, $i$-th layer의 global BA는 $O((\frac{N}{s^{l-1}})^3)$의 시간 복잡도를 가진다. 요약하면 $O(T_l)$은 다음과 같이 표현된다.
\begin{align}
T_l= \begin{cases}N^3 & (l=1) \\
\frac{w^3}{n} \cdot \sum_{i=1}^{l-1} \frac{N}{s^i}+\left(\frac{N}{s^{l-1}}\right)^3 & (l>1)\end{cases}
\tag{2} \end{align}
$T_l$이 $l$에 대한 함수이므로 최적의 $l^*$은 $T_l$의 미분을 0으로 두고 구할 수 있다.
\begin{align}
l^*=\left\lfloor\frac{1}{2} \log _s\left(\frac{3 N^2\left(s^3-s\right) n}{w^3}\right)\right\rfloor
\tag{3} \end{align}
Fig. 3은 서로 다른 프레임 수 $N$에서 연산 시간 $T_l$과 레이어 수 $l$의 관계를 보여준다. Fig. 3에서 보이는 것처럼 레이어 수가 $l=1$(Original BA인 BALM 2.0)에서 $l^*$로 증가했을 때 총 연산 시간은 크게 감소한다. $l > l^*$ 일때 연산 시간은 크게 증가하지 않고 거의 일정하게 유지된다. $l^*$보다 큰 레이어 수는 거의 비슷하게 잘 동작한다.
C. Top-Down Pose Graph Optimization
top-down 포즈 그래프 최적화 프로세스는 bottom-up hierarchical BA 프로세스(동일 local window에서 co-visible한 feature만 고려하고 다른 local window 에서는 관측되더라도 무시)에서의 포즈 추정 에러를 최소화 하도록 최적화 한다. Fig. 1에서 보이는 것처럼 포즈 그래프는 피라미드 구조의 top-down 방식으로 구성된다. 피라미드의 각 레이어에서 factor는 인접한 두 프레임 간의 relative pose이다. 노드 $T_j^{i+1}$와 $T^i_{s \cdot j}$ 가 본질적으로 같기 때문에, $\mathcal{L}=\{1, \ldots, l\}$이 set of $l$ layer이고 $\mathcal{F}^i=\left\{0, \ldots, N_i-1\right\}$이 set of $N_i$ number라고 할때 $\mathbf{T}_j^i=\mathbf{T}_{s \cdot j}^{i-1}=\cdots=\mathbf{T}_{s^{i-1} \cdot j}^1, \forall i \in \mathcal{L}, j \in \mathcal{F}^i$ 이다. Fig. 1의 original pose graph는 Fig. 4로 감소되고 objective function은 다음과 같이 최소화 된다.
식 유도는 Supplementary를 참고하자.
\begin{align}
f(\mathbb{F}, \mathcal{T})=\sum_{i \in \mathcal{L}} \sum_{j \in \mathcal{F}^i} c\left(\mathbf{T}_{s^{i-1} \cdot j}^1, \mathbf{T}_{s^{i-1} \cdot(j+1)}^1\right)
\tag{4} \end{align}
$\mathbb{F}=\left\{\mathbb{F}_i^1 \mid i \in \mathcal{F}^1\right\}$ 은 모든 first layer 프레임들의 collection이고 $\mathcal{T}=\left\{\mathbf{T}_i^1 \mid i \in \mathcal{F}^1\right\}$는 first layer 포즈들의 collection이다.(4)의 cost function은 bottom-up BA process에서 계산한 Hessian matrix에 의해 weigh가 부여된다. 최종적으로, factor graph는 GTSAM을 이용해 LM method로 푼다.
5. Experiments
A. Accuracy Analysis
1) Initial Odometry With Loop Closure
SOTA LiDAR SLAM 알고리즘인 Fast-lio2, Lio-sam, MULLS를 input으로 넣고 hierarchical BA로 최적화 한다. initial pose가 이미 포즈 그래프 최적화된 경우에도 mapping quality(consistency)과 포즈 추정 정확도를 모두 향상시킬 수 있다. 모든 실험에 사용된 BA와 피라미드 파라미터들은 Table 1에 정리되어 있다. 각 시퀀스에서 실제 포즈 수 $N$에 기반해 (3)을 이용해 최적의 $l^*$을 계산한다. data length $N < 5 \times 10^3$ (KITTI, MulRan, Newer College)에서는 $l^*=3$이고 더 큰 시퀀스 $N \geq 5 \times 10^3$ (New College) $l^*=4$를 사용한다.
Input : MULLS with loop closure / Dataset : KITTI
먼저 loop closure를 사용한 MULLS를 포즈 추정 결과로 사용해 KITTI 데이터셋에 대해 테스트 했다. Table 2에 absolution rotation (degree) 와 translation (meter)의 RMSE가 요약되어 있다. 각 라이다 프레임은 하나의 유일한 ground truth를 가지고 있기 때문에 평가 지표로 ATE(absolute trajectory error)를 사용한다. 보이는 것과 같이 hierarchical BA는 이미 포즈 그래프 최적화 된 경우에도 특히 translation에서 포즈 추정 정확도를 높인다. 모든 시퀀스에서 가장 좋은 결과를 달성하지는 못하지만 다른 SOTA 방법들과 비교해 최적의 평균 ATE 결과 (0.7◦/1.4 m)를 보인다. 또한 MULLS는 포즈 그래프 최적화가 포인트 클라우드의 consistency를 직접적으로 최적화 하지 않기 떄문에 map에서의 차이가 완전히 제거되지 않는다. (Fig. 5) 이 발산은 hierarchical BA와 포즈 그래프 최적화로 반복적으로 풀 수 있고 최종적으로 포즈 추정에서 ATE를 낮출 수 있다.
Input : LIO-SAM with loop closure / Dataset : MulRan
그후 다른 spatially large-scale spinning LiDAR dataset인 MulRan에 대해 테스트 한다. 포함된 시퀀스 DCC, KAIST, RIVER-SIDE의 평균 length는 각각 4,9km, 6.1km, 6.8km이다. 대부분의 데이터가 도시 지역인 KITTI 데이터셋과 달리 MulRan 데이터셋은 더 challenging 한 육교, 강, 숲을 포함한다. LIO-SAM으로부터 포즈 그래프 최적화된 pose trajectory를 input으로 사용한다. ATE의 RMSE는 Table 3에 요약되어 있다. hierarchical BA는 challenging unstructured 숲에서도 여전히 포즈 추정 정확도를 향상시켰다. 그러나 이러한 scene들에 의해 LIO-SAM이 좋지 않은 relative pose와 covariance를 추정했고 따라서 loop closure가 부분적으로 실피해 고도에서 큰 차이가 일어났다. (Fig. 6 참고) hierarchical BA와 PGO 매커니즘에서는 이 차이가 완전히 제거되었으며 pose trajectory 정확도도 향상되었다.
Input : FAST-LIO2 without loop closure / Dataset : Newer College
마지막으로 가장 긴 시퀀스($N$=26557)인 New College 데이터셋의 long_experiment 시퀀스에 대해 테스트 한다. FAST-LIO2가 제공하는 pose trajctory를 initial input으로 사용한다. initial pose는 loop closure 없이 생성되었다. Table 4는 ATE를 보여준다. hierarchical BA는 다른 loop closure가 가능한 SOTA 방법보다 성능이 좋으며 이 large time-scale 시퀀스에서 최적의 정확도를 보인다.
2) Initial Odometry Without Loop Closure
여기서는 hierarchical BA가 initial pose trajectory에서 loop closure를 하지 않을 때도 잘 수렴함을 보인다.
Input : MULLS without loop closure / Dataset : KITTI
absolute rotation과 translation errors의 RMSE는 Table 5에 요약되어 있다. 보이는 것처럼 다른 SOTA 방법은 loop closure function이 없어더 안 좋은 ATE를 보이지만 본 논문의 방식은 신뢰 가능한 포즈 추정을 한다. 특히 Seq. 00과 Seq. 08에서 에러가 크게 감소했으며 (0.8◦/1.9 m)로 최적의 평균 ATE를 달성했다. 이는 local BA에서의 strict parameter (Table 1 참고)가 각 local window 내의 프레임이 발산하지 않도록 하며 top layer global BA에서의 loop parameter가 잠재적인 divergence를 식별해 올바른 relative pose constraint를 생성하기 때문이다(Fig. 7 참고). 잘못된 top layer factor가 포즈 그래프에 추가되어 global BA에서의 잘못된 feature correspondence matching이 발생해도 dense bottom layer factor들이 이 잘못된 factor들이 잘못된 방향으로 포즈를 끌고 가지 않게 해준다.
Input : FAST-LIO2 without loop closure / Dataset : Self-Collected Dataset
structured 와 unstructured scene이 모두 포함된 solid-state LiDAR로 직접 획득한 데이터셋에서 본 논문의 다양성을 검증한다. 첫번째 테스트는 여러 개의 불규칙한 모양의 파이프라인과 기계를 갖춘 structured indoor factory (Fig. 8 참고) scene의 사이즈는 $14 \times 16 \times 8m$ 이고 시퀀스의 길이는 7339 프레임이다. 피라미드 파라미터는 Table 1과 같이 세팅했는데 작은 규모의 실내 환경이기 때문에 initial voxel size를 더 작게 했다. ($V_{local}=V_{global}=1m$)
두번째 테스트는 주변에 수풀, 초원, 나무가 있는 unstructured outdoor park이다. 사이즈는 약 $95 \times 195 m$ 이고 시퀀스의 길이는 3407 프레임이다. 따라서 다른 파라미터는 Table 1과 같고 initial voxel size는 ($V_{local}=V_{global}=2m$)로 조정했다. 두 테스트 시퀀스에서 FAST-LIO2로 부터의 추정 포즈를 사용했다. ground truth 관측이 유효하지 않기 때문에 매핑 품질 평가를 위해 MME(mean map entropy)를 사용했다(Table 6). MME는 공분산 행렬의 determinant의 자연로그를 계산하기 때문에 MME가 작을수록 포인트 클라우드가 더 consist 하다. 본 논문의 방법은라이다 타입에 관계 없이 매핑 일관성을 개선하고 structured and unstructured scenes 모두에서 차이를 줄였다.
B. Ablation Study
1) Pose Graph Optimization Vs. Direct Assign
이 섹션에서는 본 논문의 top-down 설계가 non-trivial 함을 보인다. trivial method는 bottom layer 포즈들을 업데이트 하기 위해서 upper layer의 최적화된 포즈들을 lower layer 포즈들에 직접 할당한다. 예를 들어 local window 안의 lower layer의 first $s$ 포즈들을 업데이트 하기 위해 해당 local window의 upper layer 키프레임의 최적화된 포즈를 사용한다. 예시를 들면 Fig.1 에서 포즈 $T^2_0, T^2_1, T^2_2$는 $T^{3*}_0$으로 업데이트 되고 $T^2_3, T^2_4, T^2_5$ 는 $T^{3*}_1$ 로 각각 업데이트 된다. 우리는 이 trivial한 방법 "Direct Assign(직접 할당)"을 MULLS에서 loop closure 전후의 포즈를 input으로 하여 본 논문의 PGO 매커니즘과 함께 KITTI 데이터셋에 테스트 했다. absolute rotation and translation errors의 RMSE는 table 7에 요약되어 있다. 본 논문의 PGO는 포즈 추정 정확도와 매핑 일관성에서 직접 할당 방법을 능가한다(FIg. 10 참고). 위의 trivial 전략이 포즈 추정 정확도를 향상시킬 수는 있더라도 각 local window로부터의 상대 포즈 제약을 무시한다. 예를 들어 $T^2_3$는 두 로컬 윈도우에 관련되어 있지만, $T^2_2$ 로부터의 상대적인 제약은 고려하지 않고 오직 $T^{3*}_1$에 의해서만 업데이트 된다. 이는 $\mathbb{F}^2_2$와 $\mathbb{F}^2_3$ 그리고 더 나아가 $\mathbb{F}^1_8$와 $\mathbb{F}^1_9$ 사이의 mapping inconsistency를 초래한다. 본 논문의 PGO 접근법에서 first layer factor는 모든 인접 프레임 간의 consistency를 보장하고 second layer 이상의 factor는 gap이 올바른 방향으로 수렴하도록 보장한다.
2) Hierarchical BA Vs. Reduced BA
이 섹션에서는 본 논문의 bottom-up 설계가 non-trivial 함을 보인다. Hessian matrix solving process를 가속화 하기 위해서 tivial 한 방법은 오리지널 헤시안 행렬의 block diagonal elements (of size $s$ of the stride length)만 유지하고 이 reduced matrix를 푸는 것이다. 이렇게 되면 본 논문의 방식에서 하는 것처럼 다른 로컬 윈도우의 상대 포즈 제약은 고려하지 않게 된다. 우리는 오리지널 BA (BALM 2.0)에 이 reduced BA 적용한 것과 본 논문의 방식을 MulRan 데이터셋의 DCC 시퀀스에서 확인했다. absolute translation error의 RMSE와 전체 최적화 시간은 Table 8에 요약되어 있다. 본 논문의 방식은 original BA 방식과 비슷한 정확도를 내면서도 연산 시간을 크게 줄였다. 이는 original BA는 모든 포인트를 사용해 adaptive voxel map을 구성해야 해서(adaptive-voxel map) 포즈 수가 증가함에 따라 급증하기 때문이다. 본 논문의 방법은 BA를 로컬 윈도우에서 수행하므로 로컬 윈도우 안의 매우 소수의 포인트만을 이용해 adaptive voxel map을 구성한다. 또한 다른 로컬 윈도우들은 병렬 연산될 수 있다. 사실 reduced BA는 두 가지 이유로 인해 original BA보다 더 오래 걸린다. 첫째는 original BA와 비슷한 adaptive voxel map을 구성해야 한다. 둘째, reduced BA는 off0diagobal block element를 0으로 만들어 헤시안 추정을 부정확하게 하고 수렴 속도를 늦춘다. 우리의 실험에서 reduced BA는 maximum iteration number에 도달했을 때 수렴에 실패할 수도 있으나 ($max_iteration=10$) original BA는 몇 번의 step에서 수렴한다. (Fig. 11 참고) 게다가 우리는 local BA에서 사대적으로 strict parameter를 사용하므로 이러한 layer factors는 모든 인접 프레임 사이에서 이상현상이 발생하지 않도록 한다. 단순화된 original BA의 경우 strict parameter만 사용될 수 있다. 그렇지 않으면 false feature matching이 자주 발생한다. (voxel 내의 포인트들이 plane feature를 형성하지 않는다.)
C. Computation Cost
이 섹션에서는 본 논문의 접근법이 특히 large-scale dataset에서 연산적으로 효율적임을 보여준다. 우리는 사용된 레이어의 다양한 설정으로 New College, and Newer College 데이터셋에서 테스트 하였고 이 데이터셋의 데이터 길이는 $10^3$ 부터 $10^4$ 프레임까지 다양하다. adaptive-voxel map 구성과 BA 시간을 포함한 전체 연산 시간과 각각의 설정에서 모든 시퀀스에 대해 기록된 최대 RAM 메모리 소요는 Fig. 12에서 확인할 수 있다. original BA 방법의 거대한 시간과 RAM 사용량 때문에 마지막 두 시퀀스에 대해서는 테스트 하지 않았으나 충분히 경향을 볼 수 있다.
모든 테스트 scene 에서 더 많은 레이어를 사용할수록 피라미드는 더 적은 연산 시간을 소요했다. 포즈 수가 $N < 5 \times 10^3$ 일때 3-layer와 4-layer의 시간과 RAM 사용량은 비슷하다. 그리고 $N > 5 \times 10^3$일때 4-layer 피라미트가 최적이 된다. 이 모든 현상은 Fig. 3과 (3)에서 보여준 이론적 분석에 부합한다. 세 번째 테스트 scene ($N=2436$)은 더 복잡한 환경을 (따라서 더 많은 adaptive-voxel map 구성 시간)을 포함하고 있으므로 실제 총 시간 소요는 나중의 scene 보다 더 크다. 그럼에도 불구하고 최적의 레이어 설정을 선택함으로써 전체 데이터 시간의 약 12% 내로 수렴하며 동작 중에 훨씬 적은 RAM을 사용한다. 이는 실제 사용에 적합하다.
