Posts

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-SAMReal ray-cast sweepsGauss-Newton in your browser

①

sweep이 쌓이고, 다음 sweep이 쌓인다

LiDAR 한 바퀴는 한 순간의 스냅샷이 아니다. 그 사실이 모든 것의 출발점이다.

아래 랩은 16채널 LiDAR가 방 안을 실제로 레이캐스팅해서 만든 데이터로 돌아갑니다. 점 하나하나가 진짜 벽·기둥과의 교점이에요. 다섯 단계를 순서대로 눌러보세요
— 각 단계에서 계산되는 값은 전부 실제 값이고, 화면은 그걸 그리기만 합니다.

Ray-casting the first sweep…

drag to orbit scroll to zoom

VLP-16 · 16 rings × 256 az · ray-cast

• ② “정답 T 적용”을 켜면 주황 점군이 참값 T만큼 옮겨져 회색 위에 정확히 포개집니다
  — 방 구조도 함께 돌아와요. ⑤에서 푸는 게 바로 이 T입니다. 전진 거리를 키우면 겹치는 영역이 줄어 정합이 어려워지니, ⑤에서 얼마나 나빠지는지도 확인해 보세요.
• ⑤ IMU 초기값 체크를 풀면 초기값이 단위행렬(=안 움직였다고 가정)이 됩니다. 같은 데이터인데도 엉뚱한 local minimum으로 굴러떨어져요. 무서운 건 그때 residual이 오히려 더 낮다는 점입니다
  — 소수의 점만 억지로 맞춘 해라서요. 유일한 단서는 pairs 수가 3분의 1로 줄었다는 것뿐입니다. LIO-SAM이 IMU를 쓰는 첫 번째 이유가 이겁니다.

②

무엇을 비교하는가 — curvature로 고른 두 종류의 점

4096개를 전부 쓰지 않는다. 기하학적으로 의미 있는 소수만 남긴다.

두 sweep을 비교한다고 했지만, 실제로 비교하는 건 점 전체가 아닙니다.
LOAM은 같은 scan line 위에서의 국소 곡률 하나로 각 점이 모서리인지 평면인지를 판정해요. 정의는 이렇게 생겼습니다.

1. 임의의 점 P_i와 같은 scan line 위에서 가장 가까운 점을 양옆으로 5개씩, 총 10개 뽑는다

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

c = 1|S| · ‖P_i‖ · ‖Σj∈S, j≠i( P_i − P_j )‖

이웃들과의 차이 벡터를 합친 뒤 크기를 잽니다. 평면 위라면 좌우 차이가 서로 상쇄돼 합이 0에 가까워지고, 모서리에서는 한쪽으로 쏠려 커집니다. ‖Pi‖로 나누는 건 멀리 있는 점일수록 이웃 간격이 벌어지는 걸 보정하기 위해서예요.

3. 곡률이 임계값보다 높으면 edge, 낮으면 plane

여기까지가 feature extraction입니다. 위 랩의 ③에서 임계값을 움직이면 이 판정이 점 색으로 그대로 반영돼요.

③

Feature correspondence — 점이 아니라 직선과 평면에 붙인다

같은 물리적 점을 두 번 맞힐 수 없다면, 맞힐 수 있는 건 그 점이 놓인 구조뿐이다.

LiDAR는 매 sweep마다 조금씩 다른 각도로 쏘기 때문에, 이전 sweep에서 맞힌 바로 그 점을 다시 맞히는 일은 사실상 없습니다. 그래서 LOAM은 대응을 점↔점이 아니라 점↔직선, 점↔평면으로 정의합니다.

Edge correspondence d_ℰ

current edge point global edge point

d_ℰ = | ij × il || jl |

j는 i와 가장 가까운 global edge point, l은 다른 scan line에서 가장 가까운 점입니다. 두 점이 직선을 정의하고, 분자의 외적 크기는 그 직선과 i가 만드는 평행사변형의 넓이
— 밑변 |jl|로 나누면 높이, 즉 점-직선 거리가 됩니다.

Planar correspondence d_ℋ

current planar point global planar point

d_ℋ = | ij · ( jm × jl ) || jm × jl |

l은 같은 scan line, m은 다른 scan line에서 고릅니다
— 세 점이 한 직선에 놓이지 않아야 평면이 정의되기 때문이에요. 보라색 화살표가 jm × jl, 즉 j에서 세운 그 평면의 법선입니다. ij를 이 법선 방향으로 사영한 길이가 곧 점-평면 거리이고, 법선 크기로 나누는 건 단위벡터로 만들기 위해서예요.

랩 ④에서 실제로 뽑힌 i, j, l, m과 그 직선·평면, 그리고 내려긋는 수선을 3D로 돌려볼 수 있습니다.

④

그래서 relative pose는 어떻게 나오나

모든 거리를 동시에 0으로 밀어내는 변환 하나를 찾는 것.

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

T* = arg min_T Σi∈ℰd_ℰ(T·p_i)² + Σi∈ℋd_ℋ(T·p_i)²

01

비선형이다

• T가 회전을 포함하므로 거리들은 T에 대해 비선형
• 현재 추정값 주변에서 선형화
  → 정규방정식 (JᵀJ + λI)δ = −Jᵀr 을 풂
• 조금 움직이고 다시 대응을 찾는 걸 반복 — 랩 ⑤의 한 step

02

대응은 매 반복 갱신된다

• T가 바뀌면 가장 가까운 j·l·m도 바뀜
• 한 번의 최소화가 아니라 ICP 루프
• 야코비안을 뽑는 동안만 대응을 고정

03

초기값이 전부다

• 목적함수는 볼록하지 않음
• 랩 ⑤에서 IMU 초기값을 끄면 회전을 거의 찾지 못한 채 주저앉음
• 그때 residual은 오히려 더 낮게 나옴

⑤

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

LOAM에 IMU를 얹은 게 아니라, IMU가 파이프라인 전체에 스며든 구조.

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

A

Deskew

• sweep이 도는 0.1초 동안의 pose를 IMU로 적분
• 각 점을 sweep 끝 시점으로 끌어다 보정
• 랩 ①의 체크박스가 이것

B

Initial guess

• pre-integration으로 상대 pose를 미리 계산
• 그 값을 scan matching의 시작점으로 투입
• 랩 ⑤에서 켜고 끄는 그것

C

Factor graph

• LiDAR odometry와 IMU pre-integration을 같은 그래프의 factor로 투입
• 둘을 함께 풀어 하나의 해를 얻음
• 그 해로 IMU bias까지 갱신

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

LOOP CLOSURE—

• 붉은 점선은 드리프트 때문에 가까워 보였을 뿐 실제로는 다른 장소인 후보입니다. 반경 검색은 이런 걸 걸러내지 못해요
  — 뒤따르는 scan matching이 기각합니다. 반경을 키우면 후보도 늘지만 오탐도 같이 늘어납니다.
• 되돌아온 게 확인되면 그 두 노드 사이에 loop factor가 하나 걸리고, 그래프 전체를 다시 풉니다. 마지막 pose만 고치는 게 아니라 궤적 전체가 한꺼번에 휘어지는 게 핵심이에요
  — 그래서 pose "graph"입니다.

LIO-SAM factor graph

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

직접 돌려보고 싶다면 — 실제 LiDAR 데이터로 돌려볼 수 있는 sparolab / SOLiD-A-LOAM GitHub

THE IDEA / 01

Geometry first, points second.

Two sweeps never hit the same physical point. What they do share is structure
— the same wall, the same corner. Curvature is the cheapest possible test for which of the two a point belongs to, and once you know that, the residual writes itself: a corner point owes its distance to a line, a wall point to a plane.

d_ℰ, d_ℋ→Σd²→T*geometric residualone objectiverelative pose

01

Degeneracy

A long featureless corridor constrains almost nothing along its axis. The normal equations go rank-deficient and the estimate slides
— which is exactly where the IMU factor earns its place.

02

Dynamic objects

Every residual above assumes the world stood still between the two sweeps. A passing person is a set of features that pulls the solution the wrong way.

03

Drift

Frame-to-frame matching is dead reckoning. Errors compound; only a map-level constraint or a loop closure removes them.

Zhang & Singh, LOAM: Lidar Odometry and Mapping in Real-time (RSS 2014) · Shan et al., LIO-SAM (IROS 2020). 이 페이지의 모든 점군·곡률·대응·최적화는 브라우저에서 실제로 계산됩니다.