1. FAST-LIO
2. Related Work
A. LiDAR Odometry and Mappint
B. Loosely-Coupled LiDAR-Inertial Odometry
C. Tightly-Coupled LiDAR-Inertial Odometry
3. Methodology
A. System Overview
B. System Description
C. State Estimation
D. Map Update
E. Initialization
4. Experiment Results
A. Computational Complexity Experiments
B. UAB Flight Experiments
C. Indoor Experiments
D. Outdoor Experiments
1. FAST-LIO
Tightly-coupled iterated EKF 사용하여 LiDAR feature points를 IMU data와 융합
→ degeneration이 발생하는 fast-motion, noisy & cluttered 환경에서도 robust
→ motion distortion 보상을 위한 formal back-propagation process 제안
Kalman Gain을 계산하는 새로운 fomula 유도
→ 라이다 feature points가 많아서 생기는 computation load 줄임
→ 연산 복잡도가 measurement dimension 대신 state dimension에 depend
KD tree 이용
forward propagation, backward propagation으로 state를 추정하고 motion 보상
Experiments : Video
2. Related Works
A. LiDAR Odometry and Mapping
Besl은 ICP로 스캔 정합을 해서 LiDAR odometry의 기반을 생성한다. ICP는 dense 3D scan에서 좋은 성능을 보이지만, sparse point cloud에서는 정확한 포인트 매칭이 거의 불가능하다. 이 문제를 해결하기 위해 Segal은 point-to-plane 거리에 기반한 GICP를 제안했다. 그후 Zhang은 LOAM 프레임워크에서 point-to-edge 거리와 GICP를 결합했다. 이후 LOAM에 기반해 LeGO-LOAM, LOAM-Livox 등 많은 알고리즘이 개발되었다. 이러한 방법들은 큰 FoV를 가진 라이다와 정형화된(structured) 환경에서는 잘 동작하지만, featureless 환경이나 작은 FoV 라이다에서는 매우 취약하다.
B. Loosely-Coupled LiDAR-Inertial Odometry
featureless 환경에서의 LiDAR degeneration을 완화하기 위해 일반적으로 IMU 측정값을 사용한다. Loosely-coupled LiDAR-inertial odometry (LIO) 방법은 라이다와 IMU 측정을 개별적으로 처리하고 그 결과를 후에 합친다. 예를 들어 LOAM은 IMU를 사용할 수도 있고 사용하지 않을 수도 있는데, IMU 측정값을 함께 사용하는 LOAM은 IMU 측정값을 라이다 스캔 정합의 초기 추정으로 사용한다. Zhen은 IMU 측정과 라이다 측정의 가우시안 Particle Filter 아웃풋을 error-state EKF로 합친다. Balazadegan은 라이다 스캔 정합을 위한 6-DoF ego-motion을 추정하기 위해 IMU-gravity 모델를 추가했다. Zuo는 MSCKF(Multi-State Constraint Kalman Filter)를 이용해 IMU와 visual 측정의 스캔 정합 결과를 결합한다. loosely-coupled 방법의 일반적인 과정은 새로운 스캔의 정합으로 포즈 측정을 얻은 후 IMU 측정을 포즈 측정과 결합한다. 스캔 정합과 데이터 퓨전을 나눔으로써 연산 부하(computation load)를 줄인다. 하지만, 이는 시스템의 다른 states(속도 등)와 새로운 스캔의 포즈 간의 correlation을 무시한다. 게다가 featureless 환경에서 스캔 정합이 degenerate되면 IMU 와의 퓨전도 신뢰할 수 없게 된다.
C. Tightly-Coupled LiDAR-Inertial Odometry
라이다의 스캔 정합 결과를 IMU 데이터와 결합하는 loosely-coupled 방법과 다르게, tightly-coupled LIO 방법은 라이다의 raw feature points를 IMU 데이터와 결합한다. tightly-coupled LIO 방법은 크게 optimization-based와 filter-based 로 나눌 수 있다. Geneva는 IMU pre-integration constrains와 라이다 feature points의 plane constraints로 그래프 최적화를 한다. Ye는 그래프 최적화와 비슷하지만 edge, plane features에 기반한 LIOM 패키지를 제안했다. filter-based 방법은 Bry가 GPF(Gaussian particle filter)를 사용해 IMU와 planar 2D LiDAR를 융합했다. 이 방법은 Boston Dynamics Atlas에서 적용되었다. feature points 수와 시스템 dimension이 증가함에 따라 particle filter의 연산 복잡도가 가파르게 증가하기 때문에 칼만 필터와 EKF, UKF, iterated KF 같은 칼만 필터의 variants가 더 선호된다.
본 논문의 방법은 tightly-coupled approach이다. Lins와 비슷하게, linearization 에러를 완화하기 위해 iterated EKF를 사용한다. 칼만 필터와 그 variants는 시간 복잡도 $\mathcal{O}\left(m^2\right)$를 가지고, 이때 $m$은 measurement dimension이다. 따라서 많은 수의 LiDAR 측정을 다루기 위해 엄청난 계산 부하가 걸린다. 단순하게 다운샘플링을 하면 measurement의 수를 줄일 수 있지만 정보 손실을 감수해야 한다. Lins는 ground plane을 추출하고 피팅하여 measurement 수를 줄인다. 하지만 이는 ground plane이 항상 존재하지 않는 공중 애플리케이션에는 적용할 수 없다.
3. Methodology
A. System Overview
Table 1의 표기법을 이용한다. Fig. 2에 전체적인 시스템의 흐름이 나와있다.
1. 라이다 인풋이 feature extraction 모듈로 들어가 edge, planar feature를 얻는다.
2. 추출된 feature 와 IMU 측정값이 state estimation 모듈에 들어가고, state estimation은 10Hz - 50Hz로 수행된다.
3. 추정된 포즈를 이용해 feature points를 글로벌 프레임에 정합하고 이때까지 만들어진 feature points map에 합친다.
B. System Description
1) $\boxplus / \boxminus$ Operator
$\mathcal{M}$이 $n$차원의 manifold이라고 하자 (e.g., $\mathcal{M} = SO(3)$). manifold는 $\mathbb{R}^n$에 locally homeomorphic 하기 때문에, $\boxplus, \boxminus$ 연산자를 통해 $\mathcal{M}$ 위의 local nieghborhood에서 접평면 $\mathbb{R}^n$로 bijective mapping을 할 수 있다.
$$
\begin{aligned}
& \quad \boxplus: \mathcal{M} \times \mathbb{R}^n \rightarrow \mathcal{M} ; \quad \boxminus: \mathcal{M} \times \mathcal{M} \rightarrow \mathbb{R}^n \\
& \mathcal{M}=S O(3): \mathbf{R} \boxplus \mathbf{r}=\mathbf{R} \operatorname{Exp}(\mathbf{r}) ; \quad \mathbf{R}_1 \boxminus \mathbf{R}_2=\log \left(\mathbf{R}_2^{\mathrm{T}} \mathbf{R}_1\right) \\
& \mathcal{M}=\mathbb{R}^n: \quad \mathbf{a} \boxplus \mathbf{b}=\mathbf{a}+\mathbf{b} ; \quad \mathbf{a} \boxminus \mathbf{b}=\mathbf{a}-\mathbf{b}
\end{aligned}
$$
$\operatorname{Exp}(\mathbf{r})=\mathbf{I}+\frac{\mathbf{r}}{\|\mathbf{r}\|} \sin (\|\mathbf{r}\|)+\frac{\mathbf{r}^2}{\|\mathbf{r}\|^2}(1-\cos (\|\mathbf{r}\|))$은 exponential map이고 $\operatorname{Log} (\cdot)$은 inverse map이다. compound manifold $\mathcal{M} = SO(3)$은 아래와 같이 정의한다.
$$
\left[\begin{array}{c}
\mathbf{R} \\
\mathbf{a}
\end{array}\right] \boxplus\left[\begin{array}{l}
\mathbf{r} \\
\mathbf{b}
\end{array}\right]=\left[\begin{array}{l}
\mathbf{R} \boxplus \mathbf{r} \\
\mathbf{a}+\mathbf{b}
\end{array}\right] ;\left[\begin{array}{c}
\mathbf{R}_1 \\
\mathbf{a}
\end{array}\right] \boxminus\left[\begin{array}{c}
\mathbf{R}_2 \\
\mathbf{b}
\end{array}\right]=\left[\begin{array}{c}
\mathbf{R}_1 \boxminus \mathbf{R}_2 \\
\mathbf{a}-\mathbf{b}
\end{array}\right]
$$
위 정의에 따라 아래를 쉽게 확인할 수 있다.
$$
(\mathbf{x} \boxplus \mathbf{u}) \boxminus \mathbf{x}=\mathbf{u} ; \mathbf{x} \boxplus(\mathbf{y} \boxminus \mathbf{x})=\mathbf{y} ; \forall \mathbf{x}, \mathbf{y} \in \mathcal{M}, \forall \mathbf{u} \in \mathbb{R}^n
$$
2) Continuous Model
IMU가 라이다에 단단하게 붙어 있고 extrinsic ${ }^I \mathbf{T}_L=\left({ }^I \mathbf{R}_L,{ }^I \mathbf{p}_L\right)$을 알고 있다고 가정하자. IMU frame $I$를 레퍼런스의 바디 프레임으로 두고 kinematic model을 얻을 수 있다.
\begin{align}
\begin{gathered}
{ }^G \dot{\mathbf{p}}_I={ }^G \mathbf{v}_I,{ }^G \dot{\mathbf{v}}_I={ }^G \mathbf{R}_I\left(\mathbf{a}_m-\mathbf{b}_{\mathbf{a}}-\mathbf{n}_{\mathbf{a}}\right)+{ }^G \mathbf{g},{ }^G \dot{\mathbf{g}}=\mathbf{0} \\
{ }^G \dot{\mathbf{R}}_I={ }^G \mathbf{R}_I\left\lfloor\boldsymbol{\omega}_m-\mathbf{b}_{\boldsymbol{\omega}}-\mathbf{n}_{\boldsymbol{\omega}}\right\rfloor _\wedge, \dot{\mathbf{b}}_{\boldsymbol{\omega}}=\mathbf{n}_{\mathbf{b} \boldsymbol{\omega}}, \dot{\mathbf{b}}_{\mathbf{a}}=\mathbf{n}_{\mathbf{b a}}
\end{gathered}
\tag{1}\end{align}
- ${ }^G \mathbf{p}_I,{ }^G \mathbf{R}_I$ :글로벌 프레임에서 IMU의 위치와 자세(i.e., first IMU 프레임 $G$)
- ${ }^G \mathbf{g}$ : 글로벌 프레임의 unknown gravity vector
- $\mathbf{a}_m, \boldsymbol{\omega}_m$ : IMU 측정값
- $\mathbf{n}_{\mathbf{a}},\mathbf{n}_{\boldsymbol{\omega}}$ : IMU 측정값의 white noise
- $\mathbf{b}_{\mathbf{a}},\mathbf{b}_{\boldsymbol{\omega}}$ : 가우시안 노이즈 $\mathbf{n}_{\mathbf{b} \mathbf{a}}, \mathbf{n}_{\mathbf{b} \boldsymbol{\omega}}$와 함께 random walk process로 모델링한 IMU bias
- $\lfloor\mathbf{a}\rfloor_{\wedge}$ : 벡터 $\mathbf{a} \in \mathbb{R}^3$의 skew-symmetric 행렬 (외적(cross product) 연산을 의미)
3) Discrete Model
$\boxplus$ 연산자에 기반하여 (1)의 continuous model을 IMU 샘플링 주기인 $\Delta t$로 zero-order holder(지정한 샘플 주기 동안 입력을 유지)를 이용해 이산화 할 수 있다. 결과 discrete model은 (2)와 같다.
\begin{align}
\mathbf{x}_{i+1}=\mathbf{x}_i \boxplus\left(\Delta t \mathbf{f}\left(\mathbf{x}_i, \mathbf{u}_i, \mathbf{w}_i\right)\right)
\tag{2}\end{align}
$i$는 IMU 측정값의 인덱스이고 function $\mathbf{f}$, state $\mathbf{x}$, input $\mathbf{u}$, noise $\mathbf{w}$는 아래와 같이 정의한다. (3) $\mathbf{f}$ 의 각 행에는 해당하는 $\mathbf{x}$ 행의 미분값 (1)이 들어간다.
$$
\begin{aligned}
& \mathcal{M}=S O(3) \times \mathbb{R}^{15}, \operatorname{dim}(\mathcal{M})=18 \\
& \mathbf{x} \doteq\left[\begin{array}{llllll}
{ }^G \mathbf{R}_I^T & { }^G \mathbf{p}_I^T & { }^G \mathbf{v}_I^T & \mathbf{b}_{\boldsymbol{\omega}}^T & \mathbf{b}_{\mathbf{a}}^T & { }^G \mathbf{g}^T
\end{array}\right]^T \in \mathcal{M} \\
& \mathbf{u} \doteq\left[\begin{array}{ll}
\boldsymbol{\omega}_m^T & \mathbf{a}_m^T
\end{array}\right]^T, \mathbf{w} \doteq\left[\begin{array}{llll}
\mathbf{n}_{\boldsymbol{\omega}}^T & \mathbf{n}_{\mathbf{a}}^T & \mathbf{n}_{\mathbf{b} \boldsymbol{\omega}}^T & \mathbf{n}_{\mathbf{b a}}^T
\end{array}\right]^T \\
&
\end{aligned}
$$
\begin{align}
\mathbf{f}\left(\mathbf{x}_i, \mathbf{u}_i, \mathbf{w}_i\right)=\left[\begin{array}{c}
\boldsymbol{\omega}_{m_i}-\mathbf{b}_{\boldsymbol{\omega}_i}-\mathbf{n}_{\boldsymbol{\omega}_i} \\
{ }^G \mathbf{v}_{I_i} \\
{ }^G \mathbf{R}_{I_i}\left(\mathbf{a}_{m_i}-\mathbf{b}_{\mathbf{a}_i}-\mathbf{n}_{\mathbf{a}_i}\right)+{ }^G \mathbf{g}_i \\
\mathbf{n}_{\mathbf{b} \boldsymbol{\omega}_i} \\
\mathbf{n}_{\mathbf{b} \mathbf{a}_i} \\
\mathbf{0}_{3 \times 1}
\end{array}\right]
\tag{3}\end{align}
4) Preprocessing of LiDAR Measurement
라이다 측정값은 로컬 바디 프레임의 포인트 좌표이다. raw LiDAR points가 매우 높은 주기 (e.g., 200kHz)로 샘플링되기 때문에, 새로운 포인트가 수신될 때마다 처리하는 것은 불가능하다. 현실적인 접근법은 포인트들을 특정 시간마다 축적하여 한 번에 처리하는 것이다. FAST-LIO에서는 최소 축적 간격을 20ms로 설정하였고 따라서 전체 state estimation (i.e., odometry output)와 맵 업데이트는 최대 50Hz이다(Fig. 2(a)). 축적된 포인트 집합을 '스캔'이라고 하며, 프로세싱에 걸리는 시간을 $t_k$라고 한다(Fig. 2(b)). raw points에서 높은 local smoothness를 가진 planar points와 낮은 local smoothness를 가진 edge points를 추출한다. featrue points 수를 $m$이라고 가정하자. 각 feature point는 시간 $\rho_j \in\left(t_{k-1}, t_k\right]$에서 샘플링되고 ${ }^{L_j} \mathbf{p}_{f_j}$라고 표기한다. 이때 $L_j$는 시간 $\rho_j$에서의 라이다 로컬 프레임이다. 한 라이다 스캔 중에 여러 IMU 측정이 있고 각 측정은 시간 $\tau_i \in\left[t_{k-1}, t_k\right]$에서 (2)와 같이 state $\mathbf{x}_i$에 대해 샘플링된다. 마지막 라이다 feature point는 스캔의 끝이지만(i.e., $\rho_m=t_k$), IMU 측정값은 스캔의 시작 또는 끝과 일치하지 않을 수도 있다.
C. State Estimation
state 공식 (2)에서 state를 추정하기 위해서, iterated EKF를 사용한다. 또한 state 추정의 tangent space에서 추정 공분산을 특성화한다. $t_{k-1}$의 마지막 라이다 스캔의 최적의 상태 추정이 $\overline{\mathbf{x}}_{k-1}$이고 공분산 행렬은 $\overline{\mathbf{P}}_{k-1}$라고 가정한다. 그러면 $\overline{\mathbf{P}}_{k-1}$는 랜덤 에러 상태 벡터의 공분산을 아래와 같이 표현한다.
\begin{align}
\widetilde{\mathbf{x}}_{k-1} \doteq \mathbf{x}_{k-1} \boxminus \overline{\mathbf{x}}_{k-1}=\left[\begin{array}{llllll}
\delta \boldsymbol{\theta}^T & {}^G \widetilde{\mathbf{p}}_I^T & {}^G \widetilde{\mathbf{v}}_I^T & \widetilde{\mathbf{b}}_{\boldsymbol{\omega}}^T & \widetilde{\mathbf{b}}_{\mathbf{a}}^T & {}^G \widetilde{\mathbf{g}}^T
\end{array}\right]^T
\end{align}
$\delta \boldsymbol{\theta}=\log \left({ }^G \overline{\mathbf{R}}_I^{T} {}^G \mathbf{R}_I\right)$는 자세(attitude) 에러이고 나머지는 standard additive errors이다(i.e., quantity $\mathbf{x}$의 추정치 $\hat{\mathbf{x}}$의 에러는 $\widetilde{\mathbf{x}} = \mathbf{x} - \hat{\mathbf{x}} $이다.). 직관적으로, 자세 에러 $\delta \boldsymbol{\theta}$는 실제와 추정한 자세 사이의 편차를 표현한다. 이렇게 에러를 정의하면 자세 불확실성을 3X3 공분산 행렬 $\mathbb{E}\left\{\delta \boldsymbol{\theta} \delta \boldsymbol{\theta}^T\right\}$로 표현할 수 있다는 이점이 있다. 자세는 3-DoF이기 때문에 이는 minimal representation이다.
1) Forward Propagation
forward propagation은 IMU 인풋을 한번 수신할 때마다 수행한다(Fig. 2 참고). 구체적으로, 프로세스 노이즈 $\mathbf{W}_i$를 0으로 두고 (2)를 따라 state를 propagate 한다.
\begin{align}
\widehat{\mathbf{x}}_{i+1}=\widehat{\mathbf{x}}_i \boxplus\left(\Delta t \mathbf{f}\left(\widehat{\mathbf{x}}_i, \mathbf{u}_i, \mathbf{0}\right)\right) ; \widehat{\mathbf{x}}_0=\overline{\mathbf{x}}_{k-1}
\tag{4}\end{align}
공분산을 propagate하기 위해서 아래의 error state dynamic model을 사용한다.
\begin{align}
\widetilde{\mathbf{x}}_{i+1} & =\mathbf{x}_{i+1} \boxminus \widehat{\mathbf{x}}_{i+1} \\
& =\left(\mathbf{x}_i \boxplus \Delta t \mathbf{f}\left(\mathbf{x}_i, \mathbf{u}_i, \mathbf{w}_i\right)\right) \boxminus\left(\widehat{\mathbf{x}}_i \boxplus \Delta t \mathbf{f}\left(\widehat{\mathbf{x}}_i, \mathbf{u}_i, \mathbf{0}\right)\right) \\
& \stackrel{(23)}{\simeq} \mathbf{F}_{\widetilde{\mathbf{x}}} \widetilde{\mathbf{x}}_i+\mathbf{F}_{\mathbf{w}} \mathbf{w}_i .
\tag{5}\end{align}
행렬 $\mathbf{F}_{\widetilde{\mathbf{x}}}, \mathbf{F}_{\mathbf{w}}$은 appendix에 따라 계산한다.
$$
\mathbf{F}_{\widetilde{\mathbf{x}}}=\left[\begin{array}{cccccc}
\operatorname{Exp}\left(-\widehat{\boldsymbol{\omega}}_i \Delta t\right) & \mathbf{0} & \mathbf{0} & -\mathbf{A}\left(\widehat{\boldsymbol{\omega}}_i \Delta t\right)^T \Delta t & \mathbf{0} & \mathbf{0} \\
\mathbf{0} & \mathbf{I} \mathbf{I} \Delta t & \mathbf{0} & \mathbf{0} & \mathbf{0} \\
-{ }^G \widehat{\mathbf{R}}_{I_i}\left\lfloor\widehat{\mathbf{a}}_i\right\rfloor_{\wedge} \Delta t & \mathbf{0} & \mathbf{I} & \mathbf{0} & -{ }^G \widehat{\mathbf{R}}_{I_i} \Delta t & \mathbf{I} \Delta t \\
\mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{I} & \mathbf{0} & \mathbf{0} \\
\mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{I} & \mathbf{0} \\
\mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{I}
\end{array}\right] (7-1)
$$
$$
\mathbf{F}_{\mathbf{w}}=\left[\begin{array}{cccc}
-\mathbf{A}\left(\widehat{\boldsymbol{\omega}}_i \Delta t\right)^T \Delta t & \mathbf{0} & \mathbf{0} & \mathbf{0} \\
\mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\
\mathbf{0} & -{ }^G \widehat{\mathbf{R}}_{I_i} \Delta t & \mathbf{0} & \mathbf{0} \\
\mathbf{0} & \mathbf{0} & \mathbf{I} \Delta t & \mathbf{0} \\
\mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{I} \Delta t \\
\mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0}
\end{array}\right] (7-2)
$$
$\widehat{\boldsymbol{\omega}}_i=\boldsymbol{\omega}_{m_i}-\widehat{\mathbf{b}}_{\boldsymbol{\omega}_i}, \widehat{\mathbf{a}}_i=\mathbf{a}_{m_i}-\widehat{\mathbf{b}}_{\mathbf{a}_i}$이고 $\mathbf{A}(\mathbf{u})^{-1}$은 아래 정의를 따른다.
\begin{align}
\mathbf{A}(\mathbf{u})^{-1} & =\mathbf{I}-\frac{1}{2}\lfloor\mathbf{u}\rfloor_{\wedge}+(1-\boldsymbol{\alpha}(\|\mathbf{u}\|)) \frac{\lfloor\mathbf{u}\rfloor_{\wedge}^2}{\|\mathbf{u}\|^2} \\
\alpha(\mathrm{m}) & =\frac{\mathrm{m}}{2} \cot \left(\frac{\mathrm{m}}{2}\right)=\frac{\mathrm{m}}{2} \frac{\cos (\mathrm{m} / 2)}{\sin (\mathrm{m} / 2)}
\tag{6}\end{align}
white noise $\mathbf{w}$의 공분산을 $\mathbf{Q}$라고 하면 propagted covariance $\widehat{\mathbf{P}}_i$는 아래 식에 따라 iterative하게 계산될 수 있다.
\begin{align}
\widehat{\mathbf{P}}_{i+1}=\mathbf{F}_{\widetilde{\mathbf{x}}} \widehat{\mathbf{P}}_i \mathbf{F}_{\widetilde{\mathbf{x}}}^T+\mathbf{F}_{\mathbf{w}} \mathbf{Q} \mathbf{F}_{\mathbf{w}}^T ; \widehat{\mathbf{P}}_0=\overline{\mathbf{P}}_{k-1}
\tag{8}\end{align}
propagation은 $t_k$의 새로운 스캔의 종료 시간에 도달할 때까지 계속되고 propagated state와 공분산은 $\widehat{\mathbf{x}}_k, \widehat{\mathbf{P}}_k$로 표기한다. $\widehat{\mathbf{P}}_k$은 ground-truth state $\mathbf{x}$와 state propagation $\widehat{\mathbf{x}}_k$ 간의 에러(i.e., $\mathbf{x} \boxminus \widehat{\mathbf{x}}_k$)의 공분산이다.
2) Backward Propagation and Motion Compensation
포인트 누적 시간 interval이 시간 $t_k$에 도달했을 때, feature points의 새로운 스캔이 propagated state $\widehat{\mathbf{x}}_k$와 공분산 $\widehat{\mathbf{P}}_k$와 합쳐져 최적의 state update를 생성해야 한다. 그러나, 새로운 스캔이 시간 $t_k$임에도 feature points는 각각의 샘플링 시간 $\rho_j \leq t_k$에 측정되므로(Section 3-B-4), Fig. 2(b) 참고) 레퍼런스의 바디 프레임에서 일치하지 않는다. 시간 $\rho_j$와 시간 $t_k$ 간의 상대 모션(i.e., 모션 왜곡)을 보상하기 위해서, $\check{\mathbf{x}}_{j-1}=\check{\mathbf{x}}_j \boxplus\left(-\Delta t \mathbf{f}\left(\check{\mathbf{x}}_j, \mathbf{u}_j, \mathbf{0}\right)\right)$처럼 (2)를 뒤로 propagate한다. $\widehat{\mathbf{x}}_k$에서 zero pose와 rest states(e.g., 속도, bias)에서 시작한다. Backward propagation은 feature point의 주파수로 수행되며 일반적으로 IMU rate보다 매우 높다. 두 IMU 측정 사이에 샘플링 된 모든 feature points에 대해, left IMU 측정을 back propagation에서 인풋으로 사용한다. 나아가 $\mathbf{f}\left(\mathbf{x}_j, \mathbf{u}_j, \mathbf{0}\right)$의 last three block elements (gyro bias, accelerometer bias, gravity에 해당)을 0이라고 하면 back propagation을 (9)와 같이 정리할 수 있다. $\rho_{j-1} \in\left[\tau_{i-1}, \tau_i\right)$와 s.f.는 "starting form"을 뜻한다.
\begin{align}
\begin{array}{r}
{ }^{I_k} \check{\mathbf{p}}_{I_{j-1}}={ }^{I_k} \check{\mathbf{p}}_{I_j}-{ }^{I_k} \check{\mathbf{v}}_{I_j} \Delta t, \quad \text { s.f. }{ }^{I_k} \check{\mathbf{p}}_{I_m}=\mathbf{0} ; \\
{ }^{I_k} \check{\mathbf{v}}_{I_{j-1}}={ }^{I_k} \check{\mathbf{v}}_{I_j}-{ }^{I_k} \check{\mathbf{R}}_{I_j}\left(\mathbf{a}_{m_{i-1}}-\widehat{\mathbf{b}}_{\mathbf{a}_k}\right) \Delta t-{ }^{I_k} \widehat{\mathbf{g}}_k \Delta t, \\
\text { s.f. }{ }^{I_k} \check{\mathbf{v}}_{I_m}={ }^G \widehat{\mathbf{R}}_{I_k}^T \widehat{\mathbf{v}}_{I_k},{ }^{I_k} \widehat{\mathbf{g}}_k={ }^G \widehat{\mathbf{R}}_{I_k}^T \widehat{\mathbf{g}}_k ; \\
{ }^{I_k} \check{\mathbf{R}}_{I_{j-1}}={ }^{I_k} \check{\mathbf{R}}_{I_j} \operatorname{Exp}\left(\left(\widehat{\mathbf{b}}_{\boldsymbol{\omega}_k}-\boldsymbol{\omega}_{m_{i-1}}\right) \Delta t\right), \text { s.f. }{ }^{I_k} \mathbf{R}_{I_m}=\mathbf{I}
\end{array}
\tag{9}\end{align}
역전파는 시간 $\rho_j$와 스캔이 끝나는 시간 $t_k$ 간의 상대 포즈 ${}^{I_k} \check{\mathbf{T}}_{I_j}=\left({ }^{I_k} \check{\mathbf{R}}_{I_j},{ }^{I_k} \check{\mathbf{p}}_{I_j}\right)$를 생성한다. 이 상대 포즈로 로컬 측정값 ${ }^{L_j} \mathbf{p}_{f_j}$을 scan-end 측정값 ${ }^{L_k} \mathbf{p}_{f_j}$로 투영할 수 있다(Fig. 2참고).
\begin{align}
{ }^{L_k} \mathbf{p}_{f_j}={ }^I \mathbf{T}_L^{-1 I_k} \check{\mathbf{T}}_{I_j}{ }^I \mathbf{T}_L{ }^{L_j} \mathbf{p}_{f_j}
\tag{10}\end{align}
${ }^I \mathbf{T}_L$은 known extrinsic (Section. 3-B-2))이다. projected point ${ }^{L^k} \mathbf{p}_{f_j}$은 다음 section의 residual을 구성하는 데 사용된다.
3) Residual Computation
(10)의 모션 보상과 함께, 동일 시간 $t_k$에서 샘플링 된 feature points $\left\{{ }^{L_k} \mathbf{p}_{f_j}\right\}$의 스캔을 보고 residual을 구성하기 위해 이를 사용할 수 있다. iterated KF의 현재 iteration이 $\kappa$라고 하자. 해당하는 state estimation은 $\widehat{\mathbf{X}}_k^\kappa$이다. $\kappa=0$일 때 (4)의 propagation에서 예측되는 state는 $\widehat{\mathbf{X}}_k^\kappa=\widehat{\mathbf{X}}$이다. 따라서 feature points $\left\{{ }^{L_k} \mathbf{p}_{f_j}\right\}$는 (11)과 같이 글로벌 프레임으로 변환될 수 있다.
\begin{align}
{}^G \widehat{\mathbf{p}}_{f_j}^\kappa={ }^G \widehat{\mathbf{T}}_{I_k}^{\kappa I} \mathbf{T}_L{ }^{L_k} \mathbf{p}_{f_j} ; j=1, \ldots, m
\tag{11}\end{align}
각 라이다 feature point에 대해 가장 가까운 plane 또는 edge는 point가 실제로 속하는 feature라고 가정한다. 즉, residual은 feature points의 글로벌 프레임 좌표 ${}^G \widehat{\mathbf{p}}_{f_j}^\kappa$와 맵에서 가장 가까운 plane (or edge) 사이의 거리로 정의된다. $\mathbf{u}_j$는 point ${ }^G \mathbf{q}_j$가 속한 해당 plane (or edge)의 normal vector (or edge orientation) 이다. residual $\mathbf{z}_j^\kappa$은 (12)와 같이 계산한다. planar features에 대해서는 $\mathbf{G}_j=\mathbf{u}_j^T$이고 edge features에 대해서는 $\mathbf{G}_j=\left\lfloor\mathbf{u}_j\right\rfloor_{\wedge}$이다. $\mathbf{u}_j$의 연산과 맵에서 가까운 포인트 탐색은 가장 마지막 맵의 포인트의 KD-tree를 구성하여 수행한다. 또한, norm이 특정 threshold (e.g., 0.5m) 이하인 residual만 고려한다. residul이 threshold를 넘으면 outlier이거나 새롭게 관측된 포인트이다.
\begin{align}
\mathbf{z}_j^\kappa=\mathbf{G}_j\left({ }^G \widehat{\mathbf{p}}_{f_j}^\kappa-{ }^G \mathbf{q}_j\right)
\tag{12}\end{align}
4) Iterated State Update
(12)에서 계산한 residual $\mathbf{z}_j^\kappa$를 IMU 데이터로 전파된 state prediction $\widehat{\mathbf{x}}_k$, 공분산 $\widehat{\mathbf{P}}_k$와 융합하기 위해 residual $\mathbf{z}_j^\kappa$을 ground-truth state $\widehat{\mathbf{x}}_k$, 측정 노이즈에 연관시키는 측정 모델을 선형화 해야 한다. 측정 노이즈는 포인트 ${ }^{{ }^j} \mathbf{p}_{f_j}$를 관측할 때 LiDAR 거리 측정과 beam-directing 노이즈 ${ }^{{ }^j} \mathbf{n}_{f_j}$에서 발생한다. 포인트 측정값 ${ }^{{ }^j} \mathbf{p}_{f_j}$에서 노이즈를 제거하면 실제 포인트 위치를 얻을 수 있다.
\begin{align}
{ }^{L_j} \mathbf{p}_{f_j}^{\mathrm{gt}}={ }^{L_j} \mathbf{p}_{f_j}-{ }^{L_j} \mathbf{n}_{f_j}
\tag{13}\end{align}
(10)을 통해 프레임 $L_k$에 투영한 다음, ground-truth state $\mathbf{x}_k$(i.e., pose)로 글로벌 프레임에 투영하면, 실제 포인트 위치는 맵에서 plane (or edge)에 정확히 놓여야 한다. 즉, (13)을 (10)에 대입한 후 (11)에 대입하고 (12)에 대입하면 0이 된다.
$$
\mathbf{0}=\mathbf{h}_j\left(\mathbf{x}_k,{ }^{L_j} \mathbf{n}_{f_j}\right)=\mathbf{G}_j\left({ }^G \mathbf{T}_{I_k}^{I_k} \widehat{\mathbf{T}}_{I_j}^I \mathbf{T}_L\left({ }^{L_j} \mathbf{p}_{f_j}-{ }^{L_j} \mathbf{n}_{f_j}\right)-{ }^G \mathbf{q}_j\right)
$$
$\widehat{\mathbf{X}}_k^\kappa$에서 위 식을 첫 번째 항까지 근사하면 아래와 같다.
\begin{align}
\begin{aligned}
\mathbf{0} & =\mathbf{h}_j\left(\mathbf{x}_k,{ }^{L_j} \mathbf{n}_{f_j}\right) \simeq \mathbf{h}_j\left(\widehat{\mathbf{x}}_k^\kappa, \mathbf{0}\right)+\mathbf{H}_j^\kappa \widetilde{\mathbf{x}}_k^\kappa+\mathbf{v}_j \\
& =\mathbf{z}_j^\kappa+\mathbf{H}_j^\kappa \widetilde{\mathbf{x}}_k^\kappa+\mathbf{v}_j
\end{aligned}
\tag{14}\end{align}
$\widetilde{\mathbf{x}}_k^\kappa=\mathbf{x}_k \boxminus \widehat{\mathbf{x}}_k^\kappa$ (또는 $\mathbf{x}_k=\widehat{\mathbf{x}}_k^\kappa \boxplus \widetilde{\mathbf{x}}_k^\kappa$도 같은 뜻)일 때, $\mathbf{H}_j^\kappa$는 $\widetilde{\mathbf{x}}_k^\kappa$에 대한 $\mathbf{h}_j\left(\widehat{\mathbf{x}}_k^\kappa \boxplus \widetilde{\mathbf{x}}_k^\kappa,{ }^{L_j} \mathbf{n}_{f_j}\right)$의 자코비안 행렬이다. 0에서 평가되고 $\mathbf{v}_j \in \mathcal{N}\left(\mathbf{0}, \mathbf{R}_j\right)$는 raw measurement noise ${ }^{L_j} \mathbf{n}_{f_j}$에서 나온다.
Section 3-C-1)의 forward propagation에서 얻은 $\mathbf{x}_k$의 prior distribution은 (15)와 같다.
\begin{align}
\mathbf{x}_k \boxminus \widehat{\mathbf{x}}_k=\left(\widehat{\mathbf{x}}_k^\kappa \boxplus \widetilde{\mathbf{x}}_k^\kappa\right) \boxminus \widehat{\mathbf{x}}_k=\widehat{\mathbf{x}}_k^\kappa \boxminus \widehat{\mathbf{x}}_k+\mathbf{J}^\kappa \widetilde{\mathbf{x}}_k^\kappa
\tag{15}\end{align}
$\mathbf{J}^\kappa$는 $\widehat{\mathbf{x}}_k^\kappa$에 대한 $\left(\widehat{\mathbf{x}}_k^\kappa \boxplus \widetilde{\mathbf{x}}_k^\kappa\right) \boxminus \widehat{\mathbf{x}}_k$의 편미분이고 0에서 평가된다.
\begin{align}
\mathbf{J}^\kappa=\left[\begin{array}{cc}
\mathbf{A}\left({ }^G \widehat{\mathbf{R}}_{I_k}^\kappa \boxminus^G \widehat{\mathbf{R}}_{I_k}\right)^{-T} & \mathbf{0}_{3 \times 15} \\
\mathbf{0}_{15 \times 3} & \mathbf{I}_{15 \times 15}
\end{array}\right]
\tag{16}\end{align}
$\mathbf{A}(\cdot)^{-1}$는 (6)에서 정의되었다. EKF의 첫번째 iteration에서 $\widehat{\mathbf{x}}_k^\kappa=\widehat{\mathbf{x}}_k$이므로 $\mathbf{J}^\kappa=\mathbf{I}$이다.
(15)의 prior을 (14)의 posteriori distribution과 합치면 maximum a-posteriori estimate (MAP)가 (17)과 같이 생성된다. $\|\mathbf{x}\|_{\mathbf{M}}^2=\mathbf{x}^T \mathbf{M} \mathbf{x}$이다.
\begin{align}
\min _{\widetilde{\mathbf{x}}_k^\kappa}\left(\left\|\mathbf{x}_k \boxminus \widehat{\mathbf{x}}_k\right\|_{\widehat{\mathbf{P}}_k^{-1}}^2+\sum_{j=1}^m\left\|\mathbf{z}_j^\kappa+\mathbf{H}_j^\kappa \widetilde{\mathbf{x}}_k^\kappa\right\|_{\mathbf{R}_j^{-1}}^2\right)
\tag{17}\end{align}
(15)의 prior의 선형화를 (17)로 대체하고 결과 quadratic cost를 최적화 하면 (18)에서 계산 가능 가능한 표준 iterated KF를 유도할 수 있다. 표기법을 단순화 하기 위해,
$\mathbf{H}=\left[\mathbf{H}_1^{\kappa^T}, \ldots, \mathbf{H}_m^{\kappa^T}\right]^T, \mathbf{R}=\operatorname{diag}\left(\mathbf{R}_1, \cdots \mathbf{R}_m\right), \mathbf{P}=\left(\mathbf{J}^\kappa\right)^{-1} \widehat{\mathbf{P}}_k\left(\mathbf{J}^\kappa\right)^{-T} \text {, and } \mathbf{z}_k^\kappa=\left[\mathbf{z}_1^{\kappa^T}, \ldots, \mathbf{z}_m^{\kappa^T}\right]^T$이다.
\begin{align}
\begin{aligned}
\mathbf{K} & =\mathbf{P H}^T\left(\mathbf{H} \mathbf{P} \mathbf{H}^T+\mathbf{R}\right)^{-1} \\
\widehat{\mathbf{x}}_k^{\kappa+1} & =\widehat{\mathbf{x}}_k^\kappa \boxplus\left(-\mathbf{K} \mathbf{z}_k^\kappa-(\mathbf{I}-\mathbf{K H})\left(\mathbf{J}^\kappa\right)^{-1}\left(\widehat{\mathbf{x}}_k^\kappa \boxminus \widehat{\mathbf{x}}_k\right)\right)
\end{aligned}
\tag{18}\end{align}
업데이트 된 추정 $\widetilde{\mathbf{x}}_k^{\kappa+1}$로 Section. 3-C-3)의 residual을 계산하고 수렴할 때까지(i.e., $\left\|\widehat{\mathbf{x}}_k^{\kappa+1} \boxminus \widehat{\mathbf{x}}_k^\kappa\right\|<\epsilon$) 프로세스를 반복한다. 수렴 후에는 최적의 state 추정과 공분산은 (19)와 같다.
\begin{align}
\overline{\mathbf{x}}_k=\widehat{\mathbf{x}}_k^{\kappa+1}, \overline{\mathbf{P}}_k=(\mathbf{I}-\mathbf{K H}) \mathbf{P}
\tag{19}\end{align}
(18) 형태의 칼만 게인을 사용했을 때 일반적인 문제는 measurement dimension을 가진 $\mathbf{H P H}^T+\mathbf{R}$의 역행렬을 계산해야 한다는 것이다. 기존 연구에서는 measurement의 적은 수만 사용하기도 한다. 본 논문은 이 제한을 피해 간다. (17)에서 비용 함수가 state에 대한 함수이기 때문에, 솔루션은 state dimension에 따른 복잡도로 연산되어야 한다. (17)을 직접적으로 풀면 (18)과 같지만 새로운 형태의 칼만 게인을 (20)과 같이 얻을 수 있다.
\begin{align}
\mathbf{K}=\left(\mathbf{H}^T \mathbf{R}^{-1} \mathbf{H}+\mathbf{P}^{-1}\right)^{-1} \mathbf{H}^T \mathbf{R}^{-1}
\tag{20}\end{align}
Appendix B에서 matrix inverse lemma에 기반하여 두 형태의 칼만 게인이 사실 동일함을 증명한다. 라이다 측정값이 독립적이기 때문에 공분산 행렬 $\mathbf{R}$은 (block) diagonal 하고 따라서 새로운 공식은 두 행렬을 measurement가 아닌 state dimension에서 invert 해주기만 하면 된다. LIO에서 state dimension은 measurement dimension 보다 훨씬 낮기 때문에 연산이 매우 크게 줄어든다. (10 Hz scan rate 에서 한 스캔에 1000개 이상의 feature points가 있을 때 state dimension은 18이다.)
5) The Algorithm
state estimation은 Algorithm 1에 정리되어 있다.
D. Map Update
state update $\overline{\mathbf{x}}_k$ (따라서 ${ }^G \overline{\mathbf{T}}_{I_k}=\left({ }^G \overline{\mathbf{R}}_{I_k},{ }^G \overline{\mathbf{p}}_{I_k}\right)$)에서, (10)에서 바디 프레임 $L_k$에 투영된 각 feature point (${ }^{L_k} \mathbf{p}_{f_j}$)는 (21)을 통해 글로벌 프레임으로 변환된다.
\begin{align}
{ }^G \overline{\mathbf{p}}_{f_j}={ }^G \overline{\mathbf{T}}_{I_k}{ }^I \mathbf{T}_L{ }^{L_k} \mathbf{p}_{f_j} ; j=1, \ldots, m
\tag{21}\end{align}
최종적으로 이 feature points는 모든 이전 단계의 feature points를 포함한 기존 맵에 추가된다.
E. Initialization
시스템 state(e.g., gravity vector ${}^G \mathcal{g}$, bias, noise covariance)의 좋은 초기 추정을 얻어 상태 추정의 속도를 높이기 위해, 초기화를 해주어야 한다. FAST-LIO에서 초기화는 간단하다. 라이다를 몇 초 동안 static하게 유지하고(본 논문의 experiment에서는 모두 2초) 수집한 데이터를 IMU bias와 gravity vector를 초기화 하는 데 사용한다. Livox AVIA와 같이 라이다가 non-repetitive scanning을 지원하는 경우, static하게 유지하는 것은 라이다가 초기 고해상도 맵을 얻게 해 후속 navigation에 좋다.
4. Experiment Results
A. Computational Complexity Experiments
새로운 칼만 게인을 사용했을 떄의 연산적 효율성을 검증하기 위해 시스템의 칼만 게인 부분을 old fomula로 대체하여 연산 시간을 비교한다. 시스템 파이프라인과 feature points 수는 동일하다. 결과는 Table 2에 나와 있다. 새로운 칼만 게인이 확실이 running time이 짧다.
B. UAB Flight Experiments
FAST-LIO의 robustness와 연산적 효율성을 검증한다. Fig. 1과 같이 Livox Avia LiDAR를 탑재한 작은 quadrotor를 만들었다. FoV는 70◦이다. FAST-LIO는 실내 환경에서 실시간이고 안정된 odometry output과 mapping을 최대 50 Hz로 수행한다. 50 Hz 프레임 레이트 실내 환경에서의 비행 궤적과 매핑 결과는 Fig. 3에 나와 있다. 평균 효율적인 feature points 수와 러닝 타임은 각각 270, 6.7 ms 이다. 드리프트는 0.3% 보다 작다(32m 궤적에 대해 0.08m의 드리프트).
C. Indoor Experiments
FAST-LIO를 큰 회전 속도와 함께 challenging 실내 환경 테스트를 진행한다. large rotation을 만들기 위해 센서를 손에 들고 빠르게 흔든다. FIg. 4는 실험 중의 각속도와 가속도를 보여준다. 각속도는 종종 100 deg/s를 넘는다. LOAM, LOAM+IMU과 비교한 결과는 Fig. 5와 Table 3에 있다. FAST-LIO는 odometry를 더 빠르게 출력하고 더 안정적이다. LOAM+IMU 은 loosely-coupled 방법이기 때문에 비일관적인 매핑을 보인다.
매핑 결과를 더 평가하기 위해 같은 공간에서 더 느린 모션으로 두 번째 실험을 진행했다. 두 실험에서 움직임이 동일하지 않기 때문에 occlusion이 발생하는 곳에서 시각적인 차이가 발생한다. 나머지 매핑 결과는 매우 비슷하다.
D. Outdoor Experiments
실외 환경에서의 성능을 보인다. Fig. 6은 홍콩대학교의 본관의 매핑 결과이다(모든 raw points을 디스플레이). 센서를 손에 들고 약 140 m를 걸은 뒤 시작 지점으로 돌아왔다. drift는 0.05% 이하(140m 이상의 궤적에서 0.07m)이다. 이 실험에서 scan rate는 10Hz이고 한 스캔의 평균 프로세싱 시간은 25ms이다(1497 effective feature points).
또한 FAST-LIO와 LINS를 비교한다. 공정한 비교를 위해 LINS의 데이터셋을 사용했으며 Velodyne VLP-16, Xsens MTiG-710 IMU로 수집한 항구 지역 데이터이다. Fig. 7에서 보이는 것처럼 FAST-LIO가 더 좋은 매핑 정확도를 보였다. 프로세싱 시간은 둘 다 10Hz 일때, FAST-LIO는 평균 7.3ms 이고 LINS는 34.5ms이다. LINS 패키지의 EKF 공식은 높은 연산 복잡도 (Section 3-C-4) 참고)를 가지고 있기 때문에 한 스캔에서 feature point를 평균 147개로 다운 샘플링한다(FAST-LIO는 한 스캔에서 784개). 이는 LINS의 매핑 정확도를 저하시킨다. Fig. 7의 결과는 다운 샘플링 전의 모든 feature points를 보인다. 모든 실험은 탑재된 DJI Manifold2 컴퓨터에서 수행되었다.
