[Review] FAST-LIO2: Fast Direct LiDAR-inertial Odometry

논문에서 설명하는 수식은 제외하고 해당 논문의 개요만 간단하게 기록하였습니다.

아직 배우는 단계라 완벽하게 이해하지 못한 부분 및 틀리게 이해한 부분이 있을 수 있습니다. 해당 부분에 대해 댓글로 알려주세요.

 

LiDAR와 IMU를 사용하여 Odemetry를 구하는 LIO 계열의 SLAM이다.

https://github.com/hku-mars/FAST_LIO

GitHub - hku-mars/FAST_LIO: A computationally efficient and robust LiDAR-inertial odometry (LIO) package

Abstract

Camera를 이용한 Visual SLAM은 LiDAR보다 더 정확한 위치를 추정할 수 있지만 sparse feature map만 유지할 수 있으며 조도 변화에 민감하며 motion blur가 있다.

LiDAR를 사용한 SLAM은 조도 변화에 영향이 없으며 카메라 보다 더 정확한 Depth를 얻을 수 있다.

 

FAST-LIO2에서는 FAST-LIO보다 속도 면에서 크게 향상시켰다.

1. Feature Extraction을 사용하지 않고 LiDAR raw data를 그대로 사용
2. Point들을 관리하는 Data 구조를 ikd-tree를 사용하여 관리

위 두가지 내용말고도 IMU와 결합하였기 때문에 LiDAR만 사용한 SLAM보다 Drift(오차)를 최소화 할 수 있고 오차를 최소화 하는 과정에서 IKF;Iterated Kalman Filter를 사용하였기 때문에 연산 속도를 더 줄일 수 있었다고 설명하지만, FAST-LIO와의 큰 차이점은 앞서 말한 LiDAR Direct Registration과 ikd-tree인 것 같다.

 

LiDAR Data는 너무 무겁기 때문에 Data를 경량화 방법으로 평면이나 Edge 등의 특징을 찾아 유의미한 point들만 추출하여 사용하였지만 FAST-LIO2에서는 Direct로 point들을 사용한다.

 

System Overview

FAST-LIO2

 

대부분의 SLAM과 같이 FAST-LIO2도 위치추정(State Estimation;빨간박스)과 지도작성(Mapping;파란박스) 과정으로 나뉜다.

 

1. State Estimation(위치추정)

위치를 추정하는 단계는 Forward Propagation과 Backward Propagation을 통해 pose를 구한 뒤

예측한 pose값과 실제 pose값의 차이를 계산하여 최소화하는 Residual Computation 과정을 거쳐 state를 update하게 된다.

 

Forward Propagation

IMU를 사용하여 pose를 구하는 과정이다.

각속도를 적분하여 orientation(자세)를 구하고, 가속도를 적분하여 velocity(속도)도 알게 되며 velocity(속도)를 한 번 더 적분하여 pose(위치)값을 얻을 수 있다고 한다.

이 과정에서 얻은 pose값만 사용한다면 Drift(오차)가 심각하기 때문에 LiDAR도 결합한다.

 

Backward Propagation

IMU에서 얻은 pose값을 사용하여 LiDAR를 보정하는 단계라고 이해했다.

LiDAR에서 data를 취득할 때 LiDAR의 내부에서 센서가 한바퀴 돌면서 point들을 찍으면서 1개의 scan을 만들어낸다.

하지만 로봇이 지도를 작성할 때 움직이기 때문에 LiDAR에서 1개의 scan을 생성할 때 point들의 pose가 변하게 된다.

따라서 1개의 scan에서 각 point들의 틀어진 pose값을 보정해 주기 위해 앞서 진행했던 Forward Propagation의 IMU data를 사용하는 것이다.

IMU에서 가속도와 각속도를 통해 위치값을 알 수 있었으므로 해당 값들을 사용해 point들의 pose값을 보정해 주면 한 번의 LiDAR scan data에 들어있는 point들이 동일한 위치에서 받은 point들로 구성된다고 이해했다.

 

Residual Computation

Forward/Backward Propagation을 통해 추정한 pose들을 바로 사용하지 않고

추정한 pose값과 실제 pose값이 최소가 될 수 있도록 residual(잔차;오차)을 계산한다.

이 과정에서 IKF;Iterated Kalman Filter를 사용하는데, 정확히는 IESKF;Iterated Error State Kalman Filter를 이 논문에서 IKF라고 말한다. (관련 링크)

IESKF는 실제값과 예측값의 오차를 최소화하는 과정과는 달리 실제값과 예측값의 오차값에 대한 오차값을 최소화하는 과정이라고 이해했다.

이 과정을 거쳐 신뢰도 있는 pose값들을 얻은 point들이 update되는 것이다.

 

2. Mapping(지도작성)

update된 point들이 생기면 해당 point들로 Global 지도를 생성한다.

 

ikd-Tree

state estimation 단계에서 update된 point들을 ikd-tree에 이미 있는 point인지 확인한다.

이미 있는 point라면 그 point는 지도에 확정되는 point이며 없는 point라면 추가할지, 삭제할지 판단한다.

추가 및 삭제과정에서 기존에 사용된 kd-tree와는 다른 점이 있다.

기존의 kd-tree에서 point를 추가 및 삭제를 진행하려면 tree에 있는 모든 point들을 확인하지만, ikd-tree에서는 추가 및 삭제하려는 point에 가까운 point가 속해 있는 tree 부분만 확인하게 된다.

따라서 kd-tree를 사용하는 FAST-LIO 보다 ikd-tree를 사용하는 FAST-LIO2가 훨씬 빨라지게 된 결정적인 이유이다.

ikd-tree를 통해 point들을 관리하면서 새로 들어온 point 중에서 오래된 point라면 삭제하고, 새로 생긴 point라면 추가하게 된다.

(tree에서 특정 방향으로만 깊어지는 것을 방지하기 위해 Re-Balancing도 진행)

 

 

이 과정을 거쳐 Map이 생성되고 Mapping과 State Estimation이 계속 실행되면서 SLAM을 진행한다.