LIO-SAM

Paper URL: https://arxiv.org/pdf/2007.00258.pdf

Authors: Tixiao Shan, Brendan Englot, Drew Meyers, Wei Wang, Carlo Ratti, and Daniela Rus

 

Abstract

• Smoothing and mapping 방식의 tightly-coupled LiDAR inertial odometry 방식을 제안함
• IMU pre-integration 을 통해 얻어지는 estimated motion 은 point clouds 의 de-skews 와 LiDAR odometry optimization 을 위한 초기값으로 사용됨
• 최적화된 LiDAR odometry solution 은 IMU 의 bias 추정에 사용됨
• Real-time performance 를 위해 old LiDAR scans 을 marginalize 시키고 scan-matching 또한 local scale 에서 수행함

Introduction

• LOAM 의 경우 몇 가지 limitation 들이 존재함
  • Map 데이터를 global voxel map 형태로 저장함으로써 loop closure detection 수행에 문제가 됨
  • Pose correction 을 위해서는 추가적인 센서 (GPS 등) 이 필요함
  • Scan-matching 기반의 loop detection 을 사용하기 때문에 large-scale 데이터에서 drift 가 발생함
• LIO-SAM 에서는 아래와 같은 방법들을 제안하여 위에서 제시된 LOAM 의 문제점을 해결하고자 함
  • Raw IMU 측정값을 이용하여 LiDAR scan frame 간의 point cloud de-skew 를 위한 non-linear motion model 을 가정함
  • 위 motion model 을 이용하여 LiDAR odometry optimization 을 위한 초기값을 설정함
  • LiDAR odometry optimization solution 을 이용하여 factor graph 에서 IMU 의 bias 를 추정함
  • Robot trajectory estimation 을 위한 global factor graph 를 도입함
  • Global map 에서의 scan matching (LOAM 방식) 이 아니라 old LiDAR scan 을 marginalize 하여 pose optimization 을 수행함 (keyframes 의 sliding window 방식을 이용한 local scale 에서의 scan-matching)

Related Work

• Loosely-coupled method
  • LOAM 에서 제안된 IMU 를 이용한 de-skew 방식은 scan-matching 을 위한 motion prior (초기값) 을 제공하지만 optimization process 에는 참여하지 않기 때문에 loosely-coupled 방식이라고 볼 수 있음
  • LeGO-LOAM (A lightweight and ground-optimized lidar odometry and mapping) 은 ground vehicle mapping task 를 위해 제안된 알고리즘이며, LOAM 과 같이 loosely-coupled 방식으로 IMU 를 이용함
• Tightly-coupled method
  • IN2LAMA 에서는 pre-integrated IMU 측정값이 point clouds 의 de-skewing 에 사용됨
  • R-LINS 에서는 error-state Kalman filter 를 이용하여 tightly-coupled 방식으로 recursive 하게 robot 의 state estimate 를 correct 함
  • LIOM 에서는 LiDAR 와 IMU 를 이용하여 얻어진 factor 들을 jointly optimize 하여 LOAM 보다 나은 성능을 보였으나, 모든 sensor measurement 를 처리하는것으로 디자인 되어 있기 때문에 real-time 으로 수행이 어려움

 

 

LiDAR Intertial Odometry via Smoothing and Mapping

• Notition
  • World frame / Body frame : $\textbf{W}$ / $\textbf{B}$
  • Robot state $\textbf{x} = [\textbf{R}^T, \ \textbf{p}^T, \ \textbf{v}^T, \ \textbf{b}^T]^T$
    • $\textbf{R} \in SO(3)$ (rotation matrix)
    • $\textbf{p} \in \mathbb{R}^3$ (position vector)
    • $\textbf{v}$ (speed)
    • $\textbf{b}$ (IMU bias)
    • $\textbf{T}_{\textbf{W}\textbf{B}} = [\textbf{R}|\textbf{p}] \in SE(3)$ (Transformation from body frame to world frame)
• Factor type
  • IMU pre-integration factor
  • LiDAR odometry factor
  • GPS factor (optional)
  • Loop closure factor
• IMU pre-integrated measurement (by Luca’s 2016 T-RO paper)
  • $\Delta \textbf{v}_{ij} = \textbf{R}_i^T (\textbf{v}_j - \textbf{v}i - \textbf{g}\Delta t{ij})$
  • $\Delta \textbf{p}_{ij} = \textbf{R}_i^T(\textbf{p}_j - \textbf{p}i - \textbf{v}i \Delta t{ij} - \frac{1}{2} \textbf{g} \Delta t^2{ij})$
  • $\Delta \textbf{R}_{ij} = \textbf{R}_i^T \textbf{R}_j$
• LiDAR odometry factor
  • 새로운 LiDAR scan 을 roughness 를 기준으로 plane 과 edge point cloud 로 구분함
    • $\textbf{F}_i^e$ (edge features from scan at time i)
    • $\textbf{F}_i^p$ (planar features from scan at time i)
    • $\mathbb{F}_i = \{ \textbf{F}_i^e, \textbf{F}_i^p \}$
  • Robot pose 의 변화가 일정 수준 이상일 때 새로운 LiDAR keyframe 을 추가하는 방식으로 계산 효율성을 추구함 (keyframes 간의 frame 들은 Factor graph 에 반영되지 않음)
  • 논문에서는 translation 이 1m 이상이고 rotation 각도가 10도를 넘어가면 새로운 keyframe 을 추가한다고 설명함
  • 최근 n 개의 keyframe 들을 가지고 odometry 를 추정함 (sliding window 방식). 이 때 n 개의 keyframe 들을 sub-keyframes 라고 정의함
    • Sub-keyframe 들의 point clouds 를 world frame 에서 merge 하여 voxel map 만듦
    • 이 voxel map 또한 edge feature 와 planar feature 로 구분
    • 새로 얻어진 scan 을 이 voxel map 에 scan-match 하여 odometry 를 추정함
    • Initial guess 는 IMU pre-integration 을 통해 얻어진 값을 사용함
  • Point 간의 residual 은 LOAM 과 동일
• Loop closure detection
  • Euclidean coordinated 에 기반한 loop closure detection (relative transform)

 

참고자료

Shan, T., Englot, B., Meyers, D., Wang, W., Ratti, C., & Rus, D. (2020). LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping. 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 5135-5142.