Interactive note · LiDAR-inertial odometry

Two sweeps,
one transform.

LOAM 계열의 LIO는 결국 한 문장입니다. 이번 sweep을 지난 sweep 위로 가장 잘 포개는 6-DoF 변환을 찾는다. 그 "가장 잘"이 무엇인지, 왜 점을 통째로 쓰지 않고 curvature로 edge와 plane을 골라내는지, 그리고 LIO-SAM에서 IMU가 정확히 어디에 끼어드는지를 실제로 돌려보면서 봅니다.

LOAM · LeGO-LOAM · LIO-SAM약 12분모든 계산은 브라우저에서

한 번의 정합을 다섯 단계로

오른쪽은 16채널 LiDAR가 방 안을 실제로 레이캐스팅해서 만든 데이터입니다. 점 하나하나가 진짜 벽, 기둥과의 교점이에요. 스크롤을 내리면 랩이 한 단계씩 넘어가고, 각 단계의 숫자는 전부 실제로 계산된 값입니다.

Step 1 · Sweep

sweep은 스냅샷이 아니다

센서는 한 바퀴 도는 0.1초 동안 계속 움직입니다. 그래서 azimuth 인덱스는 각도이면서 동시에 타임스탬프예요. 각 열이 서로 다른 pose에서 찍힙니다.

회전 진행0%

점 / sweep4,096

IMU deskew

스위치를 끄면 모든 점을 sweep 끝 pose에서 잰 것처럼 쌓습니다. 붉은 점이 그렇게 어긋난 결과이고, 옅은 회색이 제대로 보정된 위치예요. 이 벌어짐이 motion distortion이고, LIO-SAM은 IMU 적분으로 각 열의 pose를 알아내 이걸 펴고 시작합니다.

Step 2 · Two sweeps

두 점군, 아직 모르는 T

두 점군은 월드 좌표계가 아니라 각자의 센서 좌표계에 찍혀 있어서, 그냥 겹치면 어긋납니다. 회색이 sweep k, 주황이 sweep k+1이에요. 점이 벽에서 떨어져 보이는 건 오류가 아니라 아직 T를 모른다는 뜻이고, 이 어긋남이 곧 상대 pose입니다.

전진 거리— 회전각— 정답 T 적용해서 겹쳐보기

정답 T를 적용하면 주황 점군이 회색 위에 정확히 포개지고 방 구조도 돌아옵니다. Step 5에서 푸는 게 바로 이 T예요. 전진 거리를 키우면 겹치는 영역이 줄어 정합이 어려워집니다.

Step 3 · Curvature

edge와 plane만 남긴다

같은 scan line 위 좌우 5개씩을 모아 곡률 c를 계산하고, 큰 쪽은 edge, 작은 쪽은 plane으로 분류합니다. 4,096개 중 기하학적으로 의미 있는 소수만 남기는 거예요.

edge—

plane—

edge 임계값— plane 임계값—

임계값만으로 뽑으면 코너 많은 구석에 feature가 몰립니다. 그래서 LOAM은 각 ring을 6개 sector로 쪼개 sector마다 개수를 제한해요. 정의는 아래 02절에서 자세히 봅니다.

Step 4 · Correspondence

점이 아니라 직선과 평면에

현재 점 i는 map의 한 점이 아니라 직선(edge) 또는 평면(planar)에 대응됩니다. 스캔 패턴이 매번 달라서 같은 물리적 점을 두 번 맞히는 일은 없기 때문이에요.

Step 5 · Solve

모든 거리를 동시에 줄이는 T

대응을 찾고, 선형화하고, 조금 움직이고, 다시 대응을 찾습니다. 한 step이 damped Gauss-Newton 한 번이에요.

iteration0

residual RMS—

대응 쌍—

translation 오차—

rotation 오차—

λLM

추정 T—참값 T—

IMU pre-integration을 초기값으로

스위치를 끄면 초기값이 단위행렬, 즉 "안 움직였다"가 됩니다. 같은 데이터인데도 엉뚱한 local minimum으로 굴러떨어지는데, 무서운 건 그때 residual이 오히려 더 낮다는 점이에요. 소수의 점만 억지로 맞춘 해라서요. 유일한 단서는 대응 쌍이 3분의 1로 줄었다는 것뿐입니다. LIO-SAM이 IMU를 쓰는 첫 번째 이유가 이겁니다.

첫 sweep을 레이캐스팅하는 중…

드래그해서 회전 · Ctrl+스크롤 또는 핀치로 확대 · VLP-16, 16 ring × 256 azimuth

곡률로 고르는
두 종류의 점

두 sweep을 비교한다고 했지만, 실제로 비교하는 건 점 전체가 아닙니다. LOAM은 같은 scan line 위에서의 국소 곡률 하나로 각 점이 모서리인지 평면인지를 판정해요. 먼저 임의의 점 $P_i$와 같은 scan line 위에서 가장 가까운 점을 양옆으로 5개씩, 총 10개 뽑습니다.

그 이웃 집합 $S$에 대해 곡률을 계산합니다.

$$c = \frac{1}{|S| \cdot \lVert P_i \rVert}\, \Big\lVert \sum_{j \in S,\, j \neq i} \big(P_i - P_j\big) \Big\rVert$$

이웃과의 차이 벡터를 합친 뒤 크기를 잽니다. 평면 위라면 좌우 차이가 서로 상쇄돼 합이 0에 가까워지고, 모서리에서는 한쪽으로 쏠려 커져요. $\lVert P_i \rVert$로 나누는 건 멀리 있는 점일수록 이웃 간격이 벌어지는 걸 보정하기 위해서입니다. 곡률이 임계값보다 높으면 edge, 낮으면 plane이고, 위 랩의 Step 3에서 임계값을 움직이면 이 판정이 점 색으로 그대로 반영됩니다.

점이 아니라
직선과 평면에

LiDAR는 sweep마다 조금씩 다른 각도로 쏘기 때문에, 이전 sweep에서 맞힌 바로 그 점을 다시 맞히는 일은 사실상 없습니다. 같은 물리적 점을 두 번 맞힐 수 없다면, 맞힐 수 있는 건 그 점이 놓인 구조뿐이에요. 그래서 LOAM은 대응을 점↔직선, 점↔평면으로 정의합니다.

ℰ점 ↔ 직선

Edge 대응

$$d_{\mathcal E} = \frac{\big|\,\overrightarrow{ij} \times \overrightarrow{il}\,\big|}{\big|\,\overrightarrow{jl}\,\big|}$$

$j$는 $i$와 가장 가까운 map의 edge 점, $l$은 다른 scan line에서 가장 가까운 점입니다. 분자의 외적 크기는 직선 $jl$과 $i$가 만드는 평행사변형의 넓이라서, 밑변 $|\overrightarrow{jl}|$로 나누면 높이, 즉 점과 직선 사이 거리가 돼요.

ℋ점 ↔ 평면

Planar 대응

$$d_{\mathcal H} = \frac{\big|\,\overrightarrow{ij} \cdot \big(\overrightarrow{jm} \times \overrightarrow{jl}\big)\,\big|}{\big|\,\overrightarrow{jm} \times \overrightarrow{jl}\,\big|}$$

$l$은 같은 scan line, $m$은 다른 scan line에서 골라 세 점이 한 직선에 놓이지 않게 합니다. 보라색 화살표가 $j$에서 세운 평면의 법선이고, $\overrightarrow{ij}$를 그 방향으로 사영한 길이가 점과 평면 사이 거리예요.

랩의 Step 4에서 실제로 뽑힌 $i, j, l, m$과 그 직선, 평면, 수선을 3D로 돌려볼 수 있습니다.

relative pose는
어떻게 나오나

앞의 두 거리를 한 덩어리로 묶으면 그게 곧 목적함수입니다. $T$는 sweep $k+1$의 점을 sweep $k$의 좌표계로 옮기는 6-DoF 변환이고, 우리가 구하려는 상대 pose 그 자체예요.

$$T^* = \arg\min_{T} \; \sum_{i \in \mathcal E} d_{\mathcal E}(T p_i)^2 \;+\; \sum_{i \in \mathcal H} d_{\mathcal H}(T p_i)^2$$

비선형이다

$T$가 회전을 포함하므로 거리들은 $T$에 대해 비선형입니다. 현재 추정값 주변에서 선형화하고 $(J^\top J + \lambda I)\,\delta = -J^\top r$을 풀어요.

대응은 매 반복 갱신된다

$T$가 바뀌면 가장 가까운 $j, l, m$도 바뀝니다. 한 번의 최소화가 아니라 조금 움직이고 다시 대응을 찾는 ICP 루프이고, 야코비안을 뽑는 동안만 대응을 고정해요.

iii

초기값이 전부다

목적함수가 볼록하지 않아서, Step 5에서 IMU 초기값을 끄면 회전을 거의 찾지 못한 채 주저앉습니다. 그때 residual은 오히려 더 낮게 나와요.

LIO-SAM:
IMU는 세 군데서 일한다

지금까지가 LOAM의 scan matching이었습니다. LIO-SAM은 여기에 IMU를 붙이는데, 흔한 오해와 달리 "결과를 필터링"하는 게 아니라 파이프라인 전체에 스며들어 세 지점에서 각각 다른 일을 합니다.

Deskew

sweep이 도는 0.1초 동안의 pose를 IMU로 적분해, 각 점을 sweep 끝 시점으로 끌어다 보정합니다. 랩 Step 1의 스위치가 이것이에요.

Initial guess

pre-integration으로 상대 pose를 미리 계산해 scan matching의 시작점으로 넣습니다. Step 5에서 켜고 끄는 그것이에요.

Factor graph

LiDAR odometry와 IMU pre-integration을 같은 그래프의 factor로 넣고 함께 풉니다. 그 해로 IMU bias까지 갱신해요.

앞에서 구한 $T$를 계속 이어 붙이면 그게 odometry입니다. 오차가 누적돼 주황 궤적이 참값에서 벌어지죠. 로봇은 참값을 모르니 지금 추정값 위에서 예전 노드를 찾습니다. 현재 위치를 따라다니는 파란 점선 원이 탐색 범위이고, 그 안에 들어온 예전 노드가 loop 후보가 돼요.

붉은 점선은 드리프트 때문에 가까워 보였을 뿐 실제로는 다른 장소인 후보입니다. 반경 검색은 이걸 걸러내지 못하고, 뒤따르는 scan matching이 기각해요. 반경을 키우면 후보도 늘지만 오탐도 같이 늘어납니다.

되돌아온 게 확인되면 두 노드 사이에 loop factor가 걸리고 그래프 전체를 다시 풉니다. 마지막 pose만 고치는 게 아니라 궤적 전체가 한꺼번에 휘어지는 게 핵심이에요. 그래서 pose "graph"입니다.

노드 $x$는 pose(+속도+bias), 사각형은 factor입니다. 위쪽 아치가 IMU pre-integration이에요. 두 sweep 사이 수백 개의 IMU 샘플을 하나의 상대 운동 제약으로 미리 적분해 둔 것이라 pose가 갱신돼도 다시 적분할 필요가 없습니다. 가운데 직선이 scan matching 결과 $T^*$이고, 아래 아치가 누적 드리프트를 한 번에 끌어당기는 loop closure입니다.

Geometry first,
points second.

두 sweep은 같은 물리적 점을 두 번 맞히지 않습니다. 둘이 공유하는 건 같은 벽, 같은 모서리 같은 구조예요. 곡률은 점이 둘 중 어디에 속하는지를 가장 싸게 판정하는 방법이고, 그걸 알고 나면 residual은 저절로 정해집니다. 모서리 점은 직선까지의 거리를, 벽 위의 점은 평면까지의 거리를 갚으면 돼요.

—

Degeneracy

특징 없는 긴 복도는 축 방향을 거의 제약하지 못합니다. 정규방정식의 rank가 부족해지고 추정이 미끄러져요. IMU factor가 제 몫을 하는 곳이 바로 여기입니다.

—

Dynamic objects

위의 모든 residual은 두 sweep 사이에 세상이 멈춰 있었다고 가정합니다. 지나가는 사람은 해를 엉뚱한 방향으로 끌어당기는 feature 덩어리예요.

—

Drift

프레임 간 정합은 결국 dead reckoning입니다. 오차가 쌓이고, 맵 수준의 제약이나 loop closure만이 그걸 없앨 수 있어요.

실제 LiDAR 데이터로 직접 돌려보기sparolab / SOLiD-A-LOAM

Zhang & Singh, LOAM: Lidar Odometry and Mapping in Real-time (RSS 2014) · Shan et al., LIO-SAM (IROS 2020)