[Paper Review] LVI-SAM 요약 및 설명

LiDAR-Visual-inertial SLAM 논문 중 하나인 LVI-SAM을 읽고 요약해보자.

논문은 아래 링크를 통해 확인할 수 있다.
코드도 공개되어 있다.

• LVI-SAM : Tightly-coupled Lidar-Visual-Inertial Odometry via Smoothing and Mapping
  (ICRA 2021, Arxiv)
• Github : TixiaoShan/LVI-SAM

LVI-SAM의 주요 Contribution은 다음과 같다.

• LiDAR-Camera-IMU를 tightly coupled 방식의 sensor fusion을 사용하여
  state estimation 및 Mapping을 진행
• VIS(Visual-inertial-System)의 state가 LIS(LiDAR-inertial-System)의 초기화에 도움을 주도록 설계
• LiDAR point cloud를 사용하여 3D visual feature point의 depth information을 추출
• VIS(Visual-inertial System)이 실패하더라도 LIS(LiDAR-inertial System)이 도움을 주고, 그 역도 마찬가지

참고로 LVI-SAM의 1저자인 Tixiao Shan은 LIO-SAM (IROS 2020)과 LeGO-LOAM (IROS 2018)이라는 유명한 LiDAR SLAM을 만드신 분이다. LVI-SAM은 LIO-SAM이라는 LiDAR-inertal SLAM에 VINS-MONO (TRO 2018)을 결합한 것이라 생각하면 이해하기 쉽다. 따라서 두 논문을 모두 이해하고 있다면 LVI-SAM을 비교적 수월하게 읽을 수 있다!

VINS-MONO에 대한 요약 포스팅은 [Paper Review] VINS-mono 요약 및 설명에서 확인할 수 있다.

1. Introduction

Introduction에서는 Visual-inertial System과 LiDAR-inertial System에 대한 소개, 그리고 LVIO(LiDAR-Visual-inertial odometry)를 사용했을 때의 장점에 대해서 설명해주고 있다. 각각 센서마다 장,단점을 가지고 있고 센서별로 잘되는 환경이 각각 다른데, 3개의 센서를 융합하여 센서간의 단점을 보완하고 여러 환경에서도 강인한 SLAM 알고리즘을 만들 수 있다는 것이 LVIO의 가장 큰 장점이다.

수 많은 VIS 알고리즘 중에서 VINS-MONO framework를 LVI-SAM에 채택한 이유는 Monocular Visual-inertial System에서 가장 SOTA의 정확도를 보이고 있기 때문이라 설명한다. 그리고 LIS 알고리즘 중에서 LIO-SAM을 선택한 이유는 아까도 언급했지만, LVI-SAM 저자 본인의 작품(?)이기 때문이다. (논문에 그렇게 나와있지 않지만 그런 생각이 든다.)

LVI-SAM에서 주장하는 Contribution을 다시한번 정리해보면 다음과 같다.

1. Graph based tightly-coupled LVIO(LiDAR-visual-inertial odometry) framework를 제안
2. Failure detection을 통해 sub-system들을 각각 통제할 수 있도록 알고리즘을 제안
3. 여러 환경, robot platform에서 실험하여 Robustness를 입증함.

2. Overview

LVI-SAM은 크게 두 가지 sub-system으로 이루어져 있는데, 하나는 VIS(Visual-inertial System), 그리고 또 다른 하나는 LIS(LiDAR-inertial System)이다. VIS는 대부분 VINS-MONO에서 코드를 활용했으며, Original VINS-MONO와 달라진 점은, 3D visual feature points의 depth를 LiDAR point cloud를 이용하여 구한다는 것이다. LIS는 LIO-SAM에서 코드를 활용했다.

논문과 코드를 비교해보면, 논문에 있던 것과 조금 다르게 구현을 했다는 것을 알 수 있다. 논문에서는 IMU preintegration constraints, Visual odometry constraints, LiDAR odometry constraints, 그리고 Loop closure constraints를 동시에 Graph Optimization을 수행하는 것처럼 설명이 나와있는데, 실제 코드를 들여다보면 LiDAR odometry와 IMU만을 사용하여 Graph Optimization을 수행한다.

이는 Github issue 상에서도 열띤 토론을 했고, 여러 사람들이 논문과 코드가 다르다고 느끼는 부분이였다.

처음 구현은 모든 constraints를 넣고 최적화를 시도했지만 실제로 VIS(Visual-inertial System)이 LIS(LiDAR-inertial System)에 별로 도움을 주지 못하여 제거를 한 것이 아닌가하는 생각이 든다.

3. Visual-inertial System

기존의 VINS-MONO와 차이가 나는 부분에 대해서만 논문에서 언급을 하고 있다.

기존의 VIO의 초기화가 어렵다는 것을 설명하고 있는데, 그 이유는 monocular camera 특성상 scale을 추정하는데 IMU에서 충분한 움직임(acceleration excitation)이 있어야 하기 때문이다. 따라서 scale을 추정하는 것을 LiDAR point cloud의 depth association을 통해서 해결했다는 것이 LVI-SAM의 contribution이다.

VIS의 state의 초기화를 LIS의 state 및 bias를 통해서 진행하므로써 조금 더 빠르고 정확한 초기화가 가능하다.

3D visual feature points를 LiDAR point cloud와 association하기 위한 방법론을 제시를 한다. 우선 Visual feature points와 LiDAR point cloud를 모두 원점이 카메라 중심인 단위 구(a unit sphere)에 투영을 시킨다. 그리고 polar coordinate를 기준으로 2차원 K-D tree를 이용하여 3D visual feature points와 가까운 3개의 LiDAR point cloud 점을 뽑게 된다.

LiDAR depth points의 경우, LiDAR frame을 쌓아서 사용을 하기 때문에 서로 다른 object에 대해 depth ambiguity가 발생한다. 이러한 경우 가까운 depth points에 대해 association을 진행하는 과정을 거친다.

아래 그림과 같이 Camera pose가 $t_i$에서 $t_j$로 옮겨지는 경우가 있는데, 이 때 회색으로 된 point가 $t_j$에서만 관찰되는 점이다. 초록색 점이 회색 점선으로 되어 있는 point들에 대해 어떤 LiDAR point를 선택해야 하는지 depth ambiguity가 발생하는데, 이 때 가까운 거리에 있는 depth point를 선택하는 전략이다.

Depth association 및 Depth validation과 관련된 코드는 visual_odometry/visual_feature/feature_tracker.h 파일에서 sensor_msgs::ChannelFloat32 get_depth() 함수 부분을 참고하면 된다.

VIS system은 빠른 움직임이나, tracking할 feature points가 많이 없을 때 그리고 IMU에서 bias값이 클 때 좋지 않은 결과를 초래한다. VIS system의 안 좋은 결과를 전체 system에 영향을 받지 않기 하기 위해 Fail detection이라는 Mode를 통해 VIS system의 상태를 판단한다. 만약 Fail detection을 통해, VIS systme이 실패를 했다고 판단했을 경우, VIS를 re-initializes 수행한다.

VIS의 initialization과 관련된 코드는 visual_odometry/visual_estimator/estimator.cpp 파일에서 bool Estimator::initialStructure() 함수 부분을 참고하면 된다.

Visual SLAM system에서 Loop clousre detection을 하는데 자주 사용되는 DBoW2를 활용한다. 이 때 Loop clousre의 후보군으로 선택된 것들에 timestamp 정보가 추후 LiDAR-inertial System에서 사용되고 LIS(LiDAR-inertial System)의 Loop closing에 사용된다.

코드를 추가적으로 분석하다가 알게 된 사실은, VIS system에서 제공하는 Loop closure detection 정보가 LIS에서 크게 중요하게 작용하지 않는다는 것을 알게 됐다.

자세하게 설명하자면 VIS에서 /vins/loop/match_frame이라는 topic을 LIS의 lidar_odometry/mapOptmization.cpp에서 받게 되면 콜백 함수가 실행되도록 알고리즘이 구성되어 있는데, 콜백 함수에서는 void performLoopClosure()를 수행할 수 있다.

하지만 이미 lidar_odometry/mapOptmization.cpp의 main 함수에서 loopDetectionthread를 실행시키면서 void performLoopClosure()가 VIS에 상관없이 수행될 수 있도록 알고리즘이 구성되어 있다.

일부의 코드를 공개 안한건지, VIS에서 제공하는 Loop detection의 성능을 못 믿는건지는 조금 더 고민을 해봐야 할 것 같다.

4. LiDAR-inertial System

LVI-SAM에서 사용하는 LIS의 경우 저자의 이전 논문인 LIO-SAM을 사용했다.

위 그림과 같이 구성되어 있다고 논문에서는 이야기하고 있지만 직접 코드를 들여다보면 조금 다른 방법으로 구현했다는 것을 알 수 있다.

실제로는 Visual-inertial odometry constraints와 IMU preintegration constraints가 사용이 되지 않고 있는데, 이 또한 생각보다 좋지 않은 영향을 끼쳐서 빼지 않은게 아닌가 생각이 든다.

LiDAR keyframe의 경우 LIO-SAM에서 사용한 keyframe을 결정하는 기준을 따라 비슷하게 결정한다. LIO-SAM에서 keyframe을 결정하는 방법은 임형태님 블로그 - IMU Preintegration (Easy) - 2. Preliminaries (1) Keyframe를 참고하면 쉽게 이해할 수 있다.

VIS와 마찬가지로 LIS도 LIO-SAM과 다른 부분만 서술하고 있다. LIS에서 사용하는 state vector의 초기값(Initial guess)은 scan matching을 할 때, 굉장히 중요한 영향을 미치게 된다. 초기값(Initial guess)에 해당하는 parameter는 초기화(Initialization) 과정 전과 후로 바뀐다고 이야기하고 있다.

LIS initialization 전에는 Robot의 상태가 멈춰있는 상태로 가정한다. 두 개의 LiDAR keyframes에 Rotation 및 Translation을 계산하고 그 값을 활용하여 LIS initialization 과정을 진행한다. Initialization이 완료가 되면 bias 추정이 완료된 IMU measurement 및 VIS에 도움을 받아 초기값(Initial guess)을 추정한다.

LIS initialization의 구현은 LVI-SAM 코드 중에서 lidar_odometry/mapOptmization.cpp에 void updateInitialGuess()부분을 참고하자.

논문과 코드 상에 차이가 발생하는 부분이 또 다시 등장하는데 바로 LIS의 Failure detection 부분이다. LiDAR scan matching을 할 때도 실패할 수 있는 환경이 있다고 하고, 이를 fail deteciton 모듈을 통해 찾아낼 수 있다고 논문에서는 설명하고 있지만, 막상 코드를 들여다보면 해당 부분을 구현한 부분을 찾을 수가 없었다.

이 또한 Github issue 상에서 언급되었지만 자세한 답변은 들을 수 없었다.

5. Conclusion

LVI-SAM은 LiDAR-Camera-IMU를 tightly coupled 방식의 sensor fusion을 사용하여 state estimation 및 Mapping을 진행하고 소스 코드를 공개했다는 점에서 굉장히 대단한 논문임에는 틀림이 없다. 실험도 굉장히 다양하게 하여 제안하는 SLAM 알고리즘의 우수성을 입증하였다. 하지만 소스 구현 부분과 논문 상에 차이점이 존재하고 이에 대한 명확한 해답을 들을 수가 없다는 점이 조금 아쉬웠다.