LiDAR SLAM을 활용해야 하는 순간이 왔는데,
그동안 로봇팔만 다뤄왔던 터라 공부가 좀 필요했다.
그래서 기존에 있던 Point-LIO와 FAST-LIO2에 대해 알게 된 김에,
이 거창한 이름이 대체 뭐 하는 건지 궁금한 나머지
해당 논문과 기법에 대해 대략적으로 공부하며 정리한 내용이다.
논문은 FAST-LIO2, 홍콩대 MARS Lab에서 나왔고 코드는 hku-mars/FAST_LIO에 공개돼 있다.
또한, 전작인 FAST-LIO(RA-L 2021)의 내용이 전제로 깔려 있어서 그 부분도 간략하게 같이 봤다.
LIO가 뭘 하는 건지
LiDAR-Inertial Odometry. LiDAR와 IMU를 같이 써서 위치와 자세를 추정한다.
둘을 같이 쓰는 이유는 약점이 서로 반대라서 그렇다.
IMU는 빠르게 나오고 순간 회전에 강하지만 적분하면 드리프트가 쌓인다.
LiDAR는 절대적인 기하 정보를 주지만 느리고, 한 스캔 도는 동안 로봇이 움직이면 포인트가 뒤틀린다.
그래서 IMU로 스캔 왜곡을 펴고, 펴진 포인트를 지도에 맞춰서 IMU 드리프트를 잡는다.
FAST-LIO2의 세 가지
게인 계산을 뒤집었다.
전작 FAST-LIO에서 온 방식이다.
칼만 게인은 원래 측정값 개수 크기의 역행렬을 푼다. LiDAR 한 스캔이면 측정값이 수천 개다.
FAST-LIO는 수학적으로 동치인 다른 형태를 써서 역행렬 크기를 상태 차원으로 바꿨다.
FAST-LIO2 상태는 23차원이니, 수천 x 수천이 23 x 23이 된 셈이다.
덕분에 측정값을 아무리 많이 넣어도 연산량이 잘 안 터진다고 한다.
feature를 안 뽑는다.
LOAM 계열은 곡률을 계산해 edge / plane feature를 뽑고 그것만 정합한다.
FAST-LIO2는 raw point를 그대로 쓴다고 한다.
feature 추출이 손으로 튜닝한 모듈이라 센서가 바뀌면 깨지기 때문이다.
Livox 같은 solid-state LiDAR는 스캔 패턴이 비반복적이라 "같은 ring 위의 이웃 점"이라는 개념 자체가 없다.
임계값에 걸러지던 작은 구조물이 정합에 도움이 된다는 이유도 있다.
정합은 point-to-plane ICP다.
지도에서 최근접 5개 점으로 평면을 피팅하고 수직 거리를 잔차로 쓴다.
ikd-Tree.
raw point를 다 쓰면 병목이 최근접 이웃 탐색으로 옮겨간다.
일반 kd-tree는 정적 구조라 점을 넣으려면 트리를 다시 지어야 한다. ikd-Tree는 이걸 증분 구조로 바꿨다.
- 삭제는 표시만 하고 재구축 때 몰아서 처리
- 시야 밖 영역은 박스 단위로 통째로 삭제
- 트리 위에서 다운샘플링, voxel당 점 하나만 유지
- 기울어진 서브트리는 별도 스레드에서 재구축, 메인은 계속 쿼리 처리
논문은 이 구조로 대형 실외 100Hz, 초당 1000도 회전 상황의 실내 추정, ARM 프로세서 동작까지 보고한다.
전체 흐름은 이렇게 된다.
LiDAR ─┐
├→ 스캔 누적(10~100ms) → IMU 역전파로 왜곡 보정
IMU ───┘ ↓
ikd-Tree에서 최근접 5개 탐색
↓
평면 피팅 → 잔차 계산
↓
상태 갱신 → 수렴까지 반복
↓
점 삽입 + 시야 밖 영역 박스 삭제
↓
odom + map누적 구간이 10ms면 100Hz, 100ms면 10Hz로 동작한다.
실제로 쓰려면
upstream은 ROS1(catkin) 기반이라 ROS2에선 포팅본을 찾아야 한다.
Ericsii의 ros2 브랜치가 기준점이고,
Taeyoung96/FAST_LIO_ROS2, MIT-SPARK의 spark-fast-lio 등이 있다고 한다.
포팅본마다 지원하는 포인트 포맷과 메시지 타입이 다르다고 하니 어느 브랜치인지부터 봐야 할 듯하다.
Livox를 쓴다면 Livox-SDK2를 먼저 빌드하고 livox_ros_driver2를 설치한다.
주의할 점으로 언급되는 것들도 있다.
시간 동기화가 어긋나면 왜곡 보정이 반대로 작동한다. 실패 사례로 가장 많이 나온다.
회전 없이 직진만 하면 외부 파라미터가 관측 불가능해 수렴하지 않는다고 한다.
긴 복도나 터널처럼 기하 제약이 사라지는 곳은 LIO 계열 공통 약점이다.
루프 클로저가 없는 순수 오도메트리라, 필요하면 Scan Context를 붙인 FAST_LIO_SLAM 같은 구성을 봐야 한다.
마무리
내가 다루는 로봇에 FAST-LIO2가 쓰였다고 했는데,
처음엔 그냥 돌아가니까 넘어갔다.
근데 알아볼수록 내가 아는 SLAM의 개념과 조금씩 다른 점이 드러났다.(Loop Closure가 없는 등)
AI한테 물어보며 작업하면 수행은 가능하다.
다만 그게 어떤 원리인지 전혀 모르기에, 일단 내가 알아야 한다고 생각했다.
대략적으로 정보를 찾아보며 뭐가 뭔지 정도는 알 것도 같다.
한 번 읽고 다 이해했다고는 못 하겠다.
장기기억이 된 것도 아니고 그냥 딱 봤을 때 아 이런 거였지~ 싶을 정도인 것 같다.
어쨌든 급한 실무 환경에서도 결국 내가 쓰는 기술이 뭔지는 알고 써야 하니까,
나도 이걸 반복해서 읽으면서 계속 봐야겠다.
참고
- Xu, W., Cai, Y., He, D., Lin, J., Zhang, F., "FAST-LIO2: Fast Direct LiDAR-Inertial Odometry", IEEE T-RO, 2022
- Xu, W., Zhang, F., "FAST-LIO: A Fast, Robust LiDAR-Inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter", RA-L, 2021
- Cai, Y., Xu, W., Zhang, F., "ikd-Tree: An Incremental K-D Tree for Robotic Applications", 2021
- ALIDA, "[SLAM] FAST-LIO2 논문 리뷰 (+ IKF, ikd-tree)"
