[논문 요약] Robust Real-time LiDAR-inertial Initialization
2022 IROS 논문.
요즘 라이다-IMU Calibaration 하고 있어서 중심 내용만 짧게 정리한다.
SLAM 돌리는데 calibration이나 다른 초기값들이 너무 중요한듯.. 이 값들에 의해 알고리즘 결과가 천차만별.
https://arxiv.org/abs/2202.11006
Robust Real-time LiDAR-inertial Initialization
For most LiDAR-inertial odometry, accurate initial states, including temporal offset and extrinsic transformation between LiDAR and 6-axis IMUs, play a significant role and are often considered as prerequisites. However, such information may not be always
arxiv.org
Abstract
대부분의 LiDAR-inertial odometry(LIO)는 정확한 초기 상태(temporal offset, extrinsic transformation between LiDAR and 6-axis IMU)를 갖는 것이 중요하다.
이 논문에서는 Li-Init 이라는 실시간 LIO 초기화 과정을 제안한다. 이는 temporal offset and extrinsic parameter between LiDARs and IMUs을 calibration한다.
Introduction
LiDAR-IMU를 쓰는 이유는 short-term egomotion estimations without any external references 이기 때문이다.
센서 퓨전의 성능은 정확한 초기 상태에 매우 강력하게 의존된다.
이 논문은 cross-correlation and a unified temporal-spatial optimization으로 temporal offset을 찾아 calibration을 수행한다.
Related Works
기존의 LiDAR-IMU 시스템의 initialization methods는 동기화를 위해 외부 센서나 어떤 초기 상태를 무시하는 과정이 필요하다. [1], [20] 방식은 초기화를 위해 일정 시간 동안 가만히 있어야 한다.
우리의 목표는 어떤 초기 추정을 하지 않고 라이다와 IMU간의 extrinsic을 calibartion하는 것이다.
Methodology
제안하는 lidar odometry는 FAST-LIO2를 수정하여 constant(both angular and linear) velocity (CV) 모델을 라이다의 모션을 예측하고 포인트 왜곡을 보상하는데 적용했다.
CV모델과 실제 센서 모션의 차이를 최소화하기 위해, lidar odometry rate을 입력 프레임을 여러 개의 sub-frame으로 나누면서 증가시켰다.
코드에서는 'cut_frame_num' 변수로 설정.
알고리즘 돌려보면 시각적으로 알 수 있는데, 이 값에 따라 스캔 영역이 나눠지는 것을 볼 수 있다.
예를 들어, cut_frame_num=3 이라면 360도 스캔 영역이 120도씩 3부분으로 나눠진다.
사실 정확한 표현으로는 duration을 나눈거지만, 결과적으로 보기에는 영역이 분할된 것 처럼 보인다.
이를 통해 더 자주 오도메트리를 추정할 수 있는듯.
초기화 과정: IMU time offset 보정 ➡ Lidar와 align ➡ time offset 최적화 ➡ calibration(extrinsic parameter)
여기서부터는 수식이 많이 등장하는데, 천천히 읽어보면 다 이해 된다.
이전 논문에서 발표된 방식들을 사용하는 것이 대부분이라 이 논문에서는 '아 이 방식을 사용하였고, 그 수식이 이거구나' 정도를 이해하면 되고, 세부적으로 알고싶다면 해당 논문을 찾아서 읽는 것을 추천한다.
LiDAR Odometry
라이다 오도메트리와 매핑은 constant velocity(CV) model 기반.
이는 angular and linear velocity가 상수라고 가정한다.
* SO(3): 3차원 공간에 대한 회전
하지만 현실에서는 센서 모션은 constant velocity가 아닐 것이다.
이러한 모델 에러를 줄이기 위해 sub-frame을 쓰는 것이다.
1) Error State Iterated Kalman Filter
상태 추정을 위해 Error State Iterated Kalman Filter (ESIKF) 사용. 상태 추정과 공분한 공식은 다음과 같다.
칼만 필터에 대해 언제 한 번 정리 해야겠다..
칼만 필터에서 상태 추정식과 공분산을 같이 사용하는 이유는, 상태 추정 결과를 오차 공분산으로 판단하기 때문이다.
오차 공분산이 작을수록 상태 추정 분포가 평균값 근처에 몰려있다는 뜻이므로 추정 결과를 신뢰할 만 한 것이고,
그 반대의 경우 오차 공분산이 크면 상태 추정 결과를 신뢰하기 어렵다는 뜻이다.
2) Motion Compensation
라이다와 IMU는 동기화가 되어있지 않다.
우리는 motion distortion을 보완하기 위해서 $t_k$ 부터 $t_{k+1}$ 사이의 어떤 시점에서 샘플링된 라이다 포인트들을 $t_{k+1}$ 시점에 받은 라이다 스캔에 투영한다.
CV 모델을 사용하기 때문에, angular and linear velocity 값을 이용해서 relative transformation $T$를 유추할 수 있다.
그 다음 이 변환행렬을 이용해 포인트들을 scan-end 라이다 프레임에 투영할 수 있다.
여기서 왼쪽 항이 distortion-compensated scan이다.
이를 이용하면 미지의 상태 $T$의 추정값을 point-to-plane distance redisual로 표현할 수 있다. 추정값 $x_{k+1}$는 ESIKF을 이용하여 추정. 수렴할 때 까지 추정하면 얻는 $\bar{x}_{k+1}$ 추정값은 IMU의 측정을 위해 사용. 반복적인 추정 과정에 대한 더 자세한 내용은 FAST_LIO2 논문에 있음.
LiDAR-inertial Initialization
이제 라이다 IMU 초기화를 하는데, 여기엔 bias와 noise들이 많다.
1) Data Preprocess
보통 높은 주파수를 띄는 노이즈를 줄이기 위해서 non-causal zero phase low-pass filter [24] 사용.
보통 IMU 주파수는 라이다보다 높기 때문에 두 시퀀스는 같은 사이즈가 아니다.
이를 고치기 위해서 IMU를 라이다 timestamp에 맞춰 선형적으로 downsampling 한다.
그 과정을 그림으로 나타내면 다음과 같다.
2) Temporal Initialization by Cross-Correlation
대부분 현실에서는 알 수 없는 이유로 위 과정을 거쳤음에도 불구하고 라이다와 IMU사이엔 offset은 항상 존재한다.
그래서 zero-centered cross-correlation을 이용해서 그 라이다와 IMU 사이의 각속도를 최대한 동일하게 최적화하는 time offset $d^*$를 찾는다.
3) Unified Extrinsic Rotation and Temporal Calibration
Cross-correlation 방식은 노이즈와 small-scale gyroscope bias에 강인하다.
그러나, temporal offset이 $\Delta{t}$보다 작으면 동작할 수 없다.
라이다와 IMU 사이 time offset을 다음과 같이 정의. 여기서 $\delta{t}$는 any residual offset.
그림을 보며 수식을 보면 이해하기 쉽다. $k^{\prime} = k + d^*$
라이다와 IMU 사이는 다음 수식으로 나타낼 수 있다. 오른쪽 그래프를 보면 y축 값이 같은 것을 볼 수 있다.
IMU 측정값을 다음과 같이 가정할 수 있는데, 이는 IMU의 각 가속도가 $\delta{t}$보다 작은 상수인 경우이다.
그럼 (15) 식에 의해 (16)식은 (17)처럼 대체될 수 있고,
이 제약 안에서 unified temporal-spatial opimization problem은 다음과 같이 나타낼 수 있다.
Ceres Solver^2 알고리즘으로 반복되며 최적화 된다.
4) Extrinsic Translation and Gravity Initialization
지금까지 extrinsic rotation, gyroscope bias, temporal offset을 구했고 이제 그 값들을 이용해서 calibration을 수행한다.
우선, time offset이 번저 calibrated 된다.
다음, 라이다와 IMU 사이의 가속도 제약을 찾을 수 있다.
두 프레임 $A, B$의 extrinsic는 다음의 관계를 갖고 있다. ([26] 논문 참고)
LiDAR as $A$ and IMU as $B$.
마지막으로, extrinsic translation, accelerometer bias, and gravity vector은 다음 최적화 문제로 추정된다.
5) Data Accumulation Assessment
제안된 방식은 충분한 motion이 있어야 얻을 수 있다.
따라서, 시스템은 충분한 자극이 있는지 스스로 확인할 수 있어야 한다.
이상적으로는 모든 Jacobian matrix의 rank를 평가해야하지만, 우리는 extrinsic rotation and extrinsic translation에서만 해도 충분하다는 것을 알아냈다. 이는 다음처럼 나타낸다.
이 이론에 입각하여, 사용자가 얼마나 장치를 움직여야 충분한 자극을 얻을 수 있는지 알아낼 수 있다.
EXPERIMENTS
데이터셋은 자체 제작
Temporal Initialization Evaluation
확인을 위해 고의적으로 IMU timestamp를 shift.
성능을 다른 알고리즘과 비교하진 않았네.
동기화되지 않은 LIO 시스템에서 GT time offset을 얻기는 매우 어렵다.
그래서 성능을 평가하기 위해서 tightly-coupled LIO와 time offset을 비교했다. (정성적으로)
Spatial Initialization Evaluation
1) Multiple LiDAR Types Test
Fig 1. 에 있는 플랫폼의 GT:
The ground-true relative pose is (0, 0, 0) degrees in Euler angle for rotation and (0.25, 0, 0) meters for translation, the manufacturing accuracy level of CAD can achieve 0.01 degrees and millimeters.
- In all experiments, we set initial extrinsic as ( IRL, IpL) = (I3×3, 03×1), even though the ground-truth is far from the initial values (e.g., for Mid360, IMU is at I1, the actual extrinsic is about (0, −2, 178) degrees for rotation and (0.12, 0, 0.11) meters for translation).
상대 IMU 포즈 에러의 평균과 표준편차 계산. 결과는 아래에.
non-repetitive scanning인 Livox 계열 라이다에서의 성능이 더 좋다.
라이다 오도메트리를 위해 라이다 입력 프레임울 여러 개의 subframes로 나누었는데, non-repetitive scanning 라이다에서 프레임 splitting은 FOV(Field of View) of subframes를 변형시키지 않기 때문이다.
이런 세팅에서는 한 자리에 멈춰있어 포인트가 덴스해지면 혜택을 본다.
반대로, mechanical spinning LiDARs like Hesai PandarXT 세팅에서는 프레임 splitting이 FOV of subframe과 라이다 오도메트리 성능을 감소시키기 때문에 데이터가 모이기 전에 빠르게 움직여야 한다.
2) Accuracy and Robustness Comparison
extrinsic initialization의 정확성을 증명하기 위해 SOTA LIO calibration 방식, LI-Calib and Target-Free과 비교.
LI-Calib and Target-Free은 time calibration이 없어서 이 부분에선 훨신 이득.
정성적으로 비교. Li-Calib는 Surfel map으로 시각화를 하는데 (b)처럼 실패 케이스가 있고, 우리 모델은 라이다 포인트 클라우드 맵으로 잘 보임(c, d). Target-Free는 시각화 툴이 없어서 평가 불가.
Time Consumption Evaluation
실시간성이다.
Gravity and Bias Initialization Evaluation
FAST-LIO2에 우리의 초기화 과정 추가. Refinement 전에 이미 꽤 정확하다.
CONCLUSION
This paper proposes a fast, robust, temporal and spatial initialization method for LiDAR-inertial system.
An accurate and efficient coarse-to-fine temporal calibration method is proposed for unsynchronized LiDAR and IMU which is independent of any hardware synchronization setup.
