[논문 리뷰] LOAM: LiDAR Odometry and Mapping in Real-time

저자 : Ji Zhang, Sanjiv Singh

발행연도 : 2014

Abstract

LOAM은 6자유도로 움직이는 2축 라이다의 range 측정을 이용한 Odometry와 mapping을 실시간으로 계산하는 SLAM 알고리즘이다. range 측정 값은 수신되는 시간이 다르고 motion estimation 에러가 Point Cloud 결과에 오정합(misregistration)을 야기한다. 기존에는 offline에서 loop closure를 활용해 drift를 최소화한 3D 맵을 만드는 데 초점이 맞추어져 있다. 그러나 LOAM에서는 이 문제를 자체적인 Odometry 알고리즘과 Mapping 알고리즘 2개로 나누어 병렬로 처리하여 low-drift, low-computational complexity 특징을 지닌다. Odometry 알고리즘은 높은 주파수로 돌아가지만 낮은 신뢰도를 가지고, Mapping 알고리즘은 Point Cloud의 정밀한 매칭과 정합을 위해 낮은 주파수로 돌아간다. 이러한 두 알고리즘의 결합이 실시간으로 Mapping 할 수 있게 만든다. 

 

Introduction

라이다 센서가 정지한 채로 360도 회전하며 mapping 하는 일은 상대적으로 쉽다. 그러나 라이다 센서가 움직이면서 mapping 하는 경우, 라이다의 위치와 방향을 계속해서 추정하면서 mapping 해야 한다. 이 문제를 해결하기 위해서 기존에 제시된 방법들은 GPS나 INS 센서 같은 라이다와 독립적인 센서로 라이다의 움직임을 추정하거나 wheel encoder를 사용해 로봇의 odometry를 추정했다. 하지만 이러한 방법들은 매 루프마다 로봇의 움직임을 누적시키면서 odometry를 추정하기 때문에 drift 문제가 발생했다. 이 문제를 해결하기 위해서 LOAM을 개발하였고 drift를 최소화하는 것을 목표로 loop closure를 사용하지 않았다.

 

System Overview

LiDAR Hardware

Hokuyo UTM-30LX 센서를 사용했다.

 

Software System Overview

전체적인 흐름은 다음과 같다. LiDAR 센서 데이터가 들어오면 Point Cloud Registraion을 통해 LiDAR Odometry 과정으로 넘어간다. Odometry의 결과인 Pose를 이용해 LiDAR Mapping을 수행하고 Mapping의 결과와 Odometry 의 pose를 합쳐 final pose를 얻는다.

 

LiDAR Odometry

LiDAR Odometry은 세 가지 알고리즘으로 구성된다.

• Feature Point Extraction
• Finding Feature Point Correspondence
• Motion Estimation

Feature Point Extraction

라이다 포인트 클라우드에서부터 feature point를 추출할 것이다. 이 논문에서는 edge와 planar 두 종류를 구분할 것이다. 

• $i$ : $P_{k}$ 안의 한 점
• $S$ : 같은 laser scan 데이터에서 $i$와 연속적으로 위치된 다른 점들의 집합

두 포인트 간 간격은 0.25 degree이고 local surface의 smoothness를 평가하기 위한 지표로 아래와 같이 정의했다.

 

이 $c$ 값을 기준으로 스캔된 데이터의 포인트들을 정렬하고 feature point는 c의 최댓값인 edge point와 c의 최솟값인 planar point로 구분된다. 환경 내 feature point를 고르게 분류하려면, 스캔된 데이터들을 4개의 sub region으로 나누어준다. 각각의 sub region에서 최대 2개의 edge point, 4개의 planar point를 추출한다. threshold 값을 넘으면 edge나 planar로 구분될 수 있다.

 

 

feature point들을 선택할 때 주의해야 할 점이 주변 점이 선택되었거나 레이저 빔과 거의 평행한 평면의 점은 피해야 한다. 또한 가려진 영역의 경계에 있는 점들을 피해야 한다. 왜냐하면 신뢰성이 떨어지기 때문이다. 예를 들어서 아래 그림을 보자. 점 A는 연결된 표면이 다른 물체에 의해서 차단되기 때문에 edge point로 분류된다. 그러나 라이다 센서가 다른 시점으로 이동하면 차단되었던 영역이 이제 관찰 가능한 상태로 바뀐다. 그래서 앞서 언급했던 포인트들이 선택되는 것을 막기 위해서 Point Set $S$를 다시 설정한다. $S$가 레이저 빔과 평행한 surface patch를 형성하지 않고 레이저 빔 방향의 간격에 의해 $i$가 분리되고 동시에 라이다에 더 가까운 점이 $S$에 없는 경우에 점 $i$가 선택될 수 있다. 

 

요약하면, feature point들은 maximum $c$에서 시작하는 edge point와 minimum $c$에서 시작하는 planar point 둘 중 하나로 선택된다. 만약 한 점이 선택되면 선택된 edge point와 planar point들의 수는 sub region의 maximum을 초과할 수 없다. 선택된 점 주변의 점이 선택될 수 없고 레이저 빔과 평행인 surface patch일 수 없고 차단된 영역의 경계일 수 없다.

Finding Feature Point Correspondence

feature을 추출했다면, 정합(registration)을 위해서 서로 다른 sweep 간에 상관관계(correspondence)를 생성해야 한다. sweeping 과정을 시간 순서로 표현하면 아래와 같다.

• $t_{k}$: k번째 sweep의 시작점
• $P_{k}$: k번째 sweep 동안 수집된 3D Point Cloud Data
• $\bar{P}_{k}$: k+1번째 sweep의 시작점에서 reprojection된 k번째 Point Cloud Data
• $P_{k+1}$: k+1번째 sweep동안 수집된 3D Point Cloud Data

 

$\bar{P}_{k}$와 $P_{k+1}$는 현재 사용 가능하다. $P_{k+1}$를 통해서 찾은 edge point Set과 planar point Set을 각각 $\varepsilon_{k+1}$, $H_{k+1}$이라고 하자.  $P_{k+1}$은 [$\varepsilon_{k+1}$, $H_{k+1}$]로 구성된다. 결국은 $\bar{P}_{k}$와 [$\varepsilon_{k+1}$, $H_{k+1}$]의 상관관계를 구하는 문제가 된다.

 

k+1번째 sweep 동안 수집된 3D Point Cloud Data인 $P_{k+1}$를 다시 생각해보자. 이것은 $t_{k+1}$ 시점에서 아무것도 없는 공집합이다. 시간이 지남에 따라서 sweep한 포인트들이 누적된다. 즉 $P_{k+1}$는 k+1번째 sweep이 끝나는 시점인 $t_{k+2}$에 존재하는 데이터이다. 그래서 $\bar{P}_{k}$와 $P_{k+1}$의 상관관계를 구하기 위해서는 $t_{k+2}$ 시점에서 $t_{k+1}$ 시점으로 reprojection이 필요하다. $\tilde{H}_{k+1}$, $\tilde{\varepsilon}_{k+1}$을 각각 $\bar{P}_{k}$에서 찾은 closest neighbor point라고 하자. 즉, $\bar{P}_{k}$와 [$\tilde{H}_{k+1}$, $\tilde{\varepsilon}_{k+1}$]는 closest neighbor point를 통해 상관 관계를 구할 수 있다.

 

 

Edge Point

edge point의 상관관계인 edge line을 찾는 과정은 다음과 같다.

1. $i\in \tilde{\varepsilon}_{k+1}$를 만족하는 하나의 점 $i$를 선택한다.
2. $i$와 가장 가까운 이웃 점인 $j$($j\in \bar{P}_{k}$)를 선택한다.
3. $j$와 연속적으로 위치한 다른 laser scan 상의 점 $l$($l\in \bar{P}_{k}$)을 구한다.
4. 이렇게 구한 ($j$, $l$)이 edge point라는 것을 증명하기 위해서 local surface의 smoothness $c$를 계산한다.
5. ($j$, $l$)이 edge feature인 경우, 아래 공식을 사용해 $\tilde{\varepsilon}_{k+1}$와 $\bar{P}_{k}$ 간 거리를 구할 수 있다.

이때 $X_{(k, i)}^{L}$는 라이다 좌표계 {$L$}에서 $i\in \tilde{\varepsilon}_{k+1}$를 만족하는 점 $i$의 좌표를 말한다.

 

Planar Point

planar point로 마찬가지다.

1. $i\in \tilde{H}_{k+1}$을 만족하는 하나의 점 $i$를 선택한다.
2. $i$와 가장 가까운 이웃점인 $j$($j\in \bar{P}_{k}$)를 선택한다.
3. $j$와 연속적으로 위치한 같은 laser scan 상의 점 $l$을 구한다.
4. $j$와 연속적으로 위치한 다른 laser scan 상의 점 $m$을 구한다.
5. 이렇게 구한 ($j$, $l$, $m$)이 planar point라는 것을 증명하기 위해서 local surface의 smoothness $c$를 계산한다.
6. ($j$, $l$, $m$)이 planar feature인 경우, 아래 공식을 사용해 $\tilde{H}_{k+1}$ 와 $\bar{P}_{k}$ 간 거리를 구할 수 있다.

 

Motion Estimation

이제 위에서 구한 correspondence를 기반으로 두 frame 사이의 motion estimation을 수행해야 한다. 라이다 센서 데이터는 모든 point들이 동시에 찍혀 나오는 게 아니라, 일정한 속도로 돌아가면서 한 프레임을 만들기 때문에 linear interpolation(선형 보간) 해주어야 한다. $t_{k+1}$ 시점에 sweeping을 시작한다고 가정하면, [$t_{k+1}$, $t$] 시간 사이 라이다 Pose Transform은  $T_{(k+1)}^{L}$로 정의할 수 있다.

 

$T_{(k+1)}^L = [t_x, t_y, t_z, \theta_x,\theta_y,\theta_z]^{T}$

 

• $t_x$, $t_y$, $t_z$ : 라이다 좌표계 {$L$}에서 병진운동
• $\theta_x$, $\theta_y$, $\theta_z$ : 라이다 좌표계 {$L$}에서 회전운동

이때 Transform하는 점의 index를 $i$라고 하면 점 $i$의 Transform은 아래와 같다.

선형 보간을 통해 [$\varepsilon_{k+1}$, $H_{k+1}$] -> [$\tilde{H}_{k+1}$, $\tilde{\varepsilon}_{k+1}$]변환을 수식으로 나타내면 다음과 같다.

• $X_{(k+1, i)}^{L}$: [$\varepsilon_{k+1}$, $H_{k+1}$] 집합에 속한 임의의 한 점 $i$의 좌표
• $\tilde{X}_{(k+1, i)}^{L}$: [$\tilde{\varepsilon}_{k+1}$, $\tilde{H}_{k+1}$] 집합에 속한 임의의 한 점 $i$ 좌표
• $T_{(k+1, i)}^{L}(1:3)$: $[t_x, t_y, t_z]^T$로 $T_{(k+1, i)}^{L}(1:3)$에서 1~3번 항목만 사용한다.

이 식에서 $R$은 Rodrigues 공식을 통해 표현한 회전행렬이다.

• $\theta = \left\| T_{(k+1, i)}^L(4:6)\right\|$: 회전 크기
• $\omega =  $T_{(k+1, i)}^L(4:6)/\left\| T_{(k+1, i)}^L(4:6)\right\|$: 회전 방향의 단위 벡터
• $\hat{\omega}$: $\omega$의 skew symmetric matrix

 

이 문제를 최적화 문제로 풀기 위해서 상관관계의 거리가 가까워진다는 것은 registration, 즉 motion estimation이 되었다는 의미이므로 아래와 같이 edge correspondence와 planar correspondence를 cost로 삼는다.

 

 

이 두 공식을 벡터화하여 표현하면 다음과 같다.

 

정리하면, d가 최소가 되는 $T_{k+1}^L$를 비선형 최적화 방식으로 구한다. $J= \partial f/\partial T_{k+1}^L$를 구한 다음 Levenberg-Marquardt 방식으로 최적화를 수행한다. 이때 error 값은 bisquare weight 함수를 통해 outlier에 강건하도록 설정된다.

 

그래서 지금까지의 과정을 요약하면 다음과 같다.

• $t_{k+1}$초에서 $\bar{P}_{k}$, $P_{k+1}$, $T_{k+1}^L$ 값을 입력으로 받는다.
• Sweeping을 시작할 때 $T_{k+1}^L$를 초기화 한다.
• Feature point를 추출한다.
• 이전 sweep에서 featur point에 상응하는 점들을 찾고 distance를 구한다.
• 거리 오차를 최적화하는 최적의 $T_{k+1}^L$를 찾기 위해서 비선형 최적화를 수행한다.
• sweep이 끝나는 순간 $P_{k+1}$에서 $\bar{P}_{k+1}$로 reprojection하고 값을 반환한다.

 

LiDAR Mapping

LiDAR Mapping 과정은 Odometry보다 더 낮은 빈도로 실행되고 sweep이 끝난 순간 한 번만 수행된다. k+1번째 sweep이 끝나면 Odometry는 왜곡되지 않은 point cloud인 $\bar{P}_{k+1}$과 동시에 sweep 동안의 라이다 움직임을 포함한 pose transform인 $T_{k+1}^L$을 생성한다. mapping 알고리즘은 World 좌표계 {$W$}와 $\bar{P}_{k+1}$를 매칭하는 것이 핵심이다. 

 

• $Q_{k}$: k번째 sweep에서 맵을 구성하는 Point Cloud
• $T_k^W$: k번째 sweep에서 World 좌표계 {$W$}를 기준으로 본 LiDAR의 Pose Transform
• $\bar{Q}_{k}$: $\bar{P}_k$에서 World 좌표계 {$W$}로 reprojection된 Point Cloud

Odometry에서 한 것처럼 feature extraction, finding correspondence, motion estimation을 통해서 $\bar{Q}_{k+1}$을 $Q_k$에 registration한다. mapping에서 $\bar{Q}_{k+1}$에 대한 feature extraction을 이미 odometry에서 수행했기 때문에 그대로 사용한다. Odometry는 10Hz, Mapping은 1Hz이므로 10배의 feature를 사용하게 된다. 

 

correspondence 생성은 $\bar{Q}_{k+1}$의 각 feature point마다 반경 10m 이내의 Point Cloud만을 사용한다. 그리고 $\bar{Q}_k$와 $Q_{k-1}$ 사이의 correspondence를 찾고 최적의 $T_k^W$를 찾기 위해 최적화를 수행해야 한다. 

• $S'$: $Q_{k-1}$에서 임의의 점들의 집합
• $M$: $S'$의 공분산 행렬
• $V$, $E$: M의 eigenvalue와 eigenvector

이렇게 정의하고서는 eigen decomposition을 수행하여 최적화한다. correspondence를 찾은 후에는 Odometry 방식과 같은 공식을 사용해 transformation을 구한다.