센서 퓨전 심화 가이드

Rendering guide...

# Ch.1 — Why Sensor Fusion?

자율주행 차량, 드론, 서비스 로봇은 모두 같은 한 가지 질문에서 출발한다. 어떤 센서로 세상을 인지할 것인가. 어떤 단일 센서도 답이 되지 못한다는 사실에서 센서 퓨전(Sensor Fusion)이라는 분야가 시작했다. 이 가이드는 그 분야의 이론과 실전을 다루며, 첫 챕터는 왜 단일 센서가 부족한지, 어떤 결합 방식이 있는지, 고전 방법과 딥러닝이 어디에서 서로를 보완하는지 살핀다.

---

## 1.1 단일 센서의 한계

각 센서는 물리적 원리에 기반하여 환경의 특정 측면을 관측한다. 그리고 바로 그 물리적 원리 때문에 본질적인 한계를 갖는다.

### 카메라의 한계

카메라는 풍부한 시각 정보를 제공하지만 한계가 분명하다.

**조명 의존성.** 카메라는 피사체로부터 반사된 빛을 감지하는 수동(passive) 센서이다. 따라서 야간, 터널, 역광 등 조명 조건이 열악한 환경에서 성능이 급격히 저하된다. 자동 노출(auto-exposure)로 일부 완화할 수 있으나, 센서 자체의 다이내믹 레인지(dynamic range)를 넘어서는 장면에서는 포화(saturation) 또는 언더익스포저(underexposure)가 불가피하다.

**스케일 모호성.** 단안(monocular) 카메라는 3D 세계를 2D 이미지로 투영하면서 깊이 정보를 잃는다. 2m 거리의 1m 물체와 20m 거리의 10m 물체는 이미지에서 동일한 크기로 나타날 수 있다. 단안 비주얼 오도메트리(monocular visual odometry)가 절대 스케일을 복원할 수 없는 것도 이 스케일 모호성(scale ambiguity) 때문이다. 스테레오 카메라나 다른 센서와의 융합 없이는 미터 단위의 정확한 거리 추정이 원천적으로 불가능하다.

**텍스처리스(textureless) 환경.** 흰 벽, 긴 복도, 넓은 포장도로처럼 시각적 특징이 부족한 환경에서는 특징점(feature point) 추출과 추적이 실패한다. Direct 방식의 비주얼 오도메트리도 포토메트릭 그래디언트(photometric gradient)가 부족하면 동일한 문제에 직면한다.

**모션 블러.** 고속 이동이나 급격한 회전 시 이미지가 흐려지며, 특징점 추출과 매칭 성능이 크게 저하된다.

### LiDAR의 한계

LiDAR(Light Detection And Ranging)는 레이저 펄스를 발사하고 반사파의 비행시간(Time-of-Flight)을 측정하여 정밀한 3D 거리 정보를 제공하는 능동(active) 센서이다. 하지만 다음과 같은 한계를 갖는다.

**텍스처 정보 부재.** LiDAR는 기하학적 구조(geometry)는 정밀하게 캡처하지만, 색상이나 텍스처는 거의 제공하지 않는다 (반사 강도(intensity)는 일부 가능하나 카메라 이미지에 비할 바는 아니다). 그래서 구조가 유사한 장소—예컨대 같은 모양의 건물이 반복되는 거리—에서 장소 인식(place recognition)이 어렵다.

**날씨 및 환경 민감성.** 비, 안개, 눈, 먼지에서 레이저 빔이 산란돼 허위 반사점(ghost points)이 쏟아지거나 탐지 거리가 짧아진다. 검은색 물체와 고반사 표면(유리, 금속)에서도 측정이 불안정하다.

**저해상도 및 비용.** 기계식 스피닝(spinning) LiDAR의 수직 해상도는 16~32채널 수준이다. 고해상도 모델은 수천에서 수만 달러를 호가한다. 최근 솔리드 스테이트(solid-state) LiDAR가 가격을 낮추고 있지만, 좁아진 시야각(FoV)이 트레이드오프다.

### IMU의 한계

관성 측정 장치(Inertial Measurement Unit, IMU)는 가속도와 각속도를 측정하는 센서로, 고주파(100Hz–1kHz)의 자기수용적(proprioceptive) 데이터를 제공한다. 외부 환경에 의존하지 않는다는 것이 최대 장점이나, 치명적인 한계가 있다.

**드리프트(drift).** IMU 측정을 적분하여 속도와 위치를 계산하면, 센서 바이어스(bias)와 노이즈가 시간에 따라 적분되어 오차가 누적된다. 가속도의 이중 적분으로 위치를 구하면 오차는 $t^2$에 비례하여 발산한다. 항법 등급(navigation-grade)의 고급 IMU조차 수 분 내에 상당한 위치 오차를 보이며, 로보틱스에 흔히 사용되는 MEMS급 IMU는 수 초 만에 미터 단위의 오차가 발생한다.

$$\delta \mathbf{p}(t) \approx \frac{1}{2} \mathbf{b}_a \, t^2 + \frac{1}{\sqrt{3}} \sigma_a \, t^{3/2}$$

여기서 $\mathbf{b}_a$는 가속도계 바이어스, $\sigma_a$는 가속도 노이즈 밀도이다. 바이어스가 $0.01\,\text{m/s}^2$만 되어도 10초 후 위치 오차는 0.5m에 달한다.

**절대 기준 부재.** IMU는 상대적 변화만 측정하며, 절대 위치나 절대 방위(heading)에 대한 정보를 제공하지 않는다. 중력 방향으로부터 롤(roll)과 피치(pitch)를 추출할 수 있으나, 요(yaw)는 자력계(magnetometer) 없이는 관측 불가능하다(observability 문제).

### GNSS의 한계

위성 항법 시스템(Global Navigation Satellite System, GNSS)은 전지구적 절대 위치를 제공하지만, 다음과 같은 한계가 있다.

**차폐 환경.** 실내, 터널, 도심의 고층 빌딩 사이(urban canyon)에서는 위성 신호가 차단되거나 다중경로(multipath) 반사로 수십 미터의 오차가 생긴다.

**업데이트 주기.** 수신기 출력 주기는 1–10Hz로, 빠른 동적 운동을 추적하기 어렵다.

**정밀도 한계.** 표준 단독 측위의 정밀도는 수 미터 수준이다. RTK(Real-Time Kinematic)를 사용하면 센티미터 급으로 향상되지만, 기준국(base station)이 필요하고, 초기 수렴(convergence)에 시간이 걸린다.

### 센서 한계의 상보성

위에서 본 각 센서의 한계를 표로 모아 보면, 한 센서의 약점이 다른 센서의 강점에서 메워지는 패턴이 보인다.

| 특성 | 카메라 | LiDAR | IMU | GNSS |
|------|--------|-------|-----|------|
| 절대 위치 | ✗ | ✗ | ✗ | ✓ |
| 상대 이동 (단기) | ✓ | ✓ | ✓ | ✗ |
| 상대 이동 (장기) | △ (드리프트) | △ (드리프트) | ✗ (발산) | ✓ |
| 고주파 모션 캡처 | ✗ | ✗ | ✓ | ✗ |
| 조명 독립 | ✗ | ✓ | ✓ | ✓ |
| 날씨 강건성 | △ | ✗ | ✓ | ✓ |
| 3D 기하정보 | △ (깊이 모호) | ✓ | ✗ | ✗ |
| 텍스처/의미 정보 | ✓ | ✗ | ✗ | ✗ |
| 실내 동작 | ✓ | ✓ | ✓ | ✗ |
| 비용 | 저 | 고 | 중~저 | 중 |

어떤 단일 센서도 모든 상황을 감당하지 못한다. 센서 퓨전(Sensor Fusion)은 이 한계를 체계적으로 메우는 방법이다.

---

## 1.2 센서 퓨전의 분류

센서 퓨전은 여러 센서의 정보를 결합해 개별 센서로는 닿지 못하는 정확도와 강건성을 끌어내는 기술이다. 결합 방식에 따라 세 가지 범주로 나뉜다.

### Complementary Fusion (상보적 융합)

서로 다른 물리량을 측정하는 센서들이 각자의 부족한 부분을 보완하는 형태이다. 각 센서는 전체 상태(state)의 서로 다른 부분집합을 관측하며, 이들을 결합하면 단일 센서로는 관측할 수 없는 완전한 상태를 추정할 수 있다.

**대표 예시: 카메라 + IMU (Visual-Inertial Odometry, VIO)**

- 카메라는 6-DoF 포즈의 상대 변화를 제공하지만 스케일이 모호하고 고속 모션에서 실패한다.
- IMU는 고주파의 가속도/각속도를 제공하여 카메라 프레임 사이의 빠른 모션을 보간하고, 중력 방향으로부터 스케일을 복원한다.
- 두 센서는 서로 보완한다. 카메라가 제공하지 못하는 스케일·고주파 모션을 IMU가, IMU가 제공하지 못하는 드리프트 보정을 카메라가 맡는다.

**대표 예시: GNSS + IMU**

- GNSS는 저주파(1–10Hz)의 절대 위치를 제공한다.
- IMU는 고주파(수백 Hz)의 상대 이동을 제공한다.
- GNSS가 끊기는 터널 내부에서는 IMU가 단기 항법을 이어가고, GNSS가 복귀하면 누적된 IMU 드리프트를 교정한다.

이 유형의 핵심은 **관측 가능성(observability)**의 확장이다. 다른 센서의 관측이, 단일 센서로는 관측 불가능한(unobservable) 상태 변수를 관측 가능하게 만든다.

### Competitive Fusion (경쟁적 융합)

동일한 물리량을 측정하는 센서를 중복(redundancy)으로 배치하여 신뢰성과 정확도를 높이는 형태이다.

**대표 예시: 다중 카메라 시스템**

- 동일 방향을 바라보는 두 카메라가 각각 독립적으로 특징점을 추적한다.
- 한 카메라가 오염(렌즈 오염, 고장)되어도 나머지 카메라로 시스템이 계속 동작한다.
- 두 추정치를 결합하면 개별 추정치보다 분산이 줄어든다.

**통계적 기초.** 두 독립 관측 $z_1 \sim \mathcal{N}(\mu, \sigma_1^2)$, $z_2 \sim \mathcal{N}(\mu, \sigma_2^2)$를 최적으로 결합하면:

$$\hat{\mu} = \frac{\sigma_2^2 z_1 + \sigma_1^2 z_2}{\sigma_1^2 + \sigma_2^2}, \quad \sigma_{\text{fused}}^2 = \frac{\sigma_1^2 \sigma_2^2}{\sigma_1^2 + \sigma_2^2}$$

결합된 추정치의 분산은 항상 개별 추정치의 분산보다 작다: $\sigma_{\text{fused}}^2 < \min(\sigma_1^2, \sigma_2^2)$. 이것은 칼만 필터(Kalman Filter)의 갱신 단계와 정확히 동일한 원리이다.

**대표 예시: 다중 LiDAR 시스템**

자율주행 차량에서 4–6개의 LiDAR를 차량 주위에 배치하여 360° 시야를 확보하는 동시에, 겹치는 영역에서 중복 관측으로 신뢰성을 높이는 것이 일반적이다.

### Cooperative Fusion (협력적 융합)

각 센서의 원시(raw) 데이터를 결합하여 어느 단일 센서로도 불가능한 **새로운 형태의 정보**를 생성하는 형태이다.

**대표 예시: 스테레오 비전 (Stereo Vision)**

- 좌우 카메라의 이미지를 결합하여 시차(disparity)를 계산한다.
- 시차로부터 3D 깊이 정보를 복원한다.
- 단일 카메라로는 깊이를 알 수 없지만, 두 카메라의 협력으로 새로운 물리량(깊이)이 생성된다.

**대표 예시: 카메라 + LiDAR → 컬러 포인트 클라우드**

- LiDAR 포인트를 카메라 이미지에 투영하여 각 3D 점에 색상을 부여한다.
- 결과물인 컬러 포인트 클라우드(colored point cloud)는 LiDAR의 정밀 기하정보와 카메라의 풍부한 텍스처를 동시에 가지며, 이는 개별 센서로는 생성 불가능하다.
- [R3LIVE (Lin et al., 2022)](https://arxiv.org/abs/2109.07982) 같은 시스템이 이를 실시간으로 수행한다.

**대표 예시: Radar + Camera → 악천후 인지**

- 레이더의 도플러(Doppler) 측정과 카메라의 시각 정보를 결합하여, 안개나 비 속에서도 이동 물체의 속도와 클래스를 동시에 인식한다.

세 범주는 상호 배타적이지 않다. 실제 시스템은 종종 이 세 가지를 동시에 활용한다. 예를 들어, 자율주행 차량의 센서 퓨전 시스템은 카메라+LiDAR의 cooperative 융합(컬러 포인트 클라우드), 카메라+IMU의 complementary 융합(VIO), 다중 LiDAR의 competitive 융합(중복 커버리지)을 모두 포함한다.

---

## 1.3 결합 수준에 따른 분류: Loosely vs Tightly vs Ultra-tightly Coupled

센서 퓨전의 또 다른 중요한 분류 축은 **센서 데이터가 결합되는 수준**이다. 이 분류는 시스템의 정확도, 복잡도, 강건성에 직접적인 영향을 미친다.

### Loosely Coupled (느슨한 결합)

각 센서 서브시스템이 독립적으로 자신의 추정치를 산출하고, 이 결과(output)들을 상위 레벨에서 결합하는 방식이다.

**구조:**
```
센서A → [서브시스템A] → 추정치A ─┐
                                   ├─→ [Fusion] → 최종 추정
센서B → [서브시스템B] → 추정치B ─┘
```

**대표 예시: 독립 VO + 독립 LiDAR Odometry → EKF Fusion**

- Visual Odometry가 독립적으로 포즈를 출력한다.
- LiDAR Odometry가 독립적으로 포즈를 출력한다.
- 상위의 EKF가 두 포즈를 결합하여 최종 추정치를 생성한다.

**장점:**
- 모듈성: 각 서브시스템을 독립적으로 개발, 테스트, 교체할 수 있다.
- 단순성: 퓨전 레이어의 설계가 상대적으로 간단하다.
- 부분 실패 대응: 한 서브시스템이 실패해도 다른 서브시스템의 출력으로 계속 동작 가능하다.

**단점:**
- 정보 손실: 각 서브시스템이 내부적으로 관측 정보를 요약(compress)하여 출력하므로, 원시 관측의 세부 정보(예: 각 특징점의 개별 불확실성)가 손실된다.
- 상관관계 무시: 독립 서브시스템들이 공통 관측(예: 동일 IMU 데이터)을 사용할 경우, 두 추정치가 상관관계를 가지게 되지만 이를 무시하면 과신(overconfidence)이 발생한다. 이는 "이중 카운팅(double counting)" 문제로 알려져 있다.
- 최적성 상실: 정보가 요약되므로 전체 시스템이 정보 이론적으로 최적(optimal)이 되지 못한다.

### Tightly Coupled (긴밀한 결합)

모든 센서의 **원시 관측(raw measurement)**을 단일 추정 프레임워크(single estimator)에 직접 투입하는 방식이다.

**구조:**
```
센서A → raw 관측A ─┐
                     ├─→ [Single Estimator] → 최종 추정
센서B → raw 관측B ─┘
```

**대표 예시: [VINS-Mono (Qin et al., 2018)](https://arxiv.org/abs/1708.03852)**

- 카메라의 원시 특징점 관측과 IMU의 원시 가속도/각속도 측정을 하나의 비선형 최적화(sliding-window optimization)에 함께 넣는다.
- 최적화의 비용 함수는 재투영 오차(reprojection error)와 IMU 사전적분 잔차(preintegration residual)를 동시에 최소화한다.

$$\min_{\mathcal{X}} \left\{ \sum_{(i,j) \in \mathcal{B}} \| \mathbf{r}_{\text{IMU}}(\mathbf{x}_i, \mathbf{x}_j) \|^2_{\mathbf{P}_{ij}} + \sum_{(i,l) \in \mathcal{C}} \| \mathbf{r}_{\text{cam}}(\mathbf{x}_i, \mathbf{f}_l) \|^2_{\mathbf{\Sigma}_l} \right\}$$

여기서 $\mathbf{r}_{\text{IMU}}$는 IMU 사전적분 잔차, $\mathbf{r}_{\text{cam}}$은 시각 재투영 잔차, $\mathcal{X}$는 전체 상태(포즈, 속도, 바이어스, 랜드마크)이다.

**대표 예시: [FAST-LIO2 (Xu et al., 2022)](https://arxiv.org/abs/2107.06829)**

- LiDAR의 개별 포인트와 IMU의 원시 측정을 하나의 반복 에러스테이트 칼만 필터(Iterated Error-State Kalman Filter)에 직접 투입한다.

**대표 예시: [LIO-SAM (Shan et al., 2020)](https://arxiv.org/abs/2007.00258)**

- LiDAR 특징점, IMU 사전적분, GNSS 위치 관측을 하나의 팩터 그래프(factor graph)에서 통합 최적화한다.

**장점:**
- 정보 최대 활용: 원시 관측의 모든 정보를 활용하므로 정보 이론적으로 더 나은 추정이 가능하다.
- 교차 보정: 센서 간 상호 보정(cross-calibration)이 자연스럽게 이루어진다. 예를 들어, 카메라 관측이 IMU 바이어스 추정에 기여하고, IMU 데이터가 카메라 특징점 추적을 안정화한다.
- 열화 대응(graceful degradation): 한 센서의 관측 수가 줄어들어도(예: 특징점이 적은 환경) 나머지 센서의 관측이 추정을 지탱한다.

**단점:**
- 복잡도: 단일 추정기에 모든 센서의 관측 모델을 구현해야 하므로 시스템 설계와 디버깅이 복잡하다.
- 계산 비용: 상태 벡터가 커지고 관측 수가 많아져 계산량이 증가한다.
- 센서 의존성: 특정 센서를 제거하거나 교체하려면 전체 추정기를 수정해야 한다.

### Ultra-tightly Coupled (초긴밀 결합)

센서의 **신호 수준(signal level)**에서 융합이 이루어지는 방식이다. 이 용어는 주로 GNSS/INS 통합에서 사용된다.

**대표 예시: GNSS/INS Ultra-tight Integration**

- 일반적인 tightly coupled에서는 GNSS 수신기가 의사거리(pseudorange)를 출력하고 이를 항법 필터에 입력한다.
- Ultra-tight에서는 INS의 예측 속도를 GNSS 수신기 내부의 코드/반송파 추적 루프(tracking loop)에 피드백한다.
- 이를 통해 수신기의 추적 루프 대역폭을 줄여 노이즈에 대한 내성을 높이고, 심한 간섭이나 약신호 환경에서도 위성 추적을 유지할 수 있다.

**비전 분야에서의 유사 개념:**

비전-관성 시스템에서 ultra-tight coupling에 해당하는 것은 IMU 예측을 이용하여 카메라의 특징점 탐색 영역을 제한하거나, 이미지 왜곡(motion blur) 보정에 IMU 데이터를 직접 사용하는 것이다. VINS-Mono에서 IMU 예측으로 특징점 추적의 초기값을 설정하는 것이 이에 가깝다.

### 결합 수준 비교

| 특성 | Loosely Coupled | Tightly Coupled | Ultra-tightly Coupled |
|------|----------------|-----------------|----------------------|
| 융합 레벨 | 결과(output) | 관측(measurement) | 신호(signal) |
| 정보 활용도 | 낮음 | 높음 | 최고 |
| 구현 복잡도 | 낮음 | 중간~높음 | 매우 높음 |
| 모듈성 | 높음 | 낮음 | 매우 낮음 |
| 부분 실패 대응 | 용이 | 설계 필요 | 어려움 |
| 대표 시스템 | 독립 VO + LO → EKF | VINS-Mono, LIO-SAM, FAST-LIO2, ORB-SLAM3 | GNSS/INS deep integration |

현대 로보틱스에서는 **tightly coupled** 방식이 주류이다. Loosely coupled는 구현이 간단하지만 정보 손실로 인해 정확도가 떨어지고, ultra-tightly coupled는 특수한 하드웨어 접근이 필요하여 적용 범위가 제한된다. VINS-Mono, FAST-LIO2, LIO-SAM 등 현재 가장 널리 사용되는 오픈소스 시스템들은 모두 tightly coupled 아키텍처를 채택하고 있다. 최근에는 LiDAR-관성-비전 세 센서를 단일 프레임워크에서 tightly coupled로 융합하는 [FAST-LIVO2 (Zheng et al., 2024)](https://arxiv.org/abs/2408.14035)가 정확도와 실시간 성능 모두에서 기존 시스템을 크게 능가하는 결과를 보여주고 있다.

---

## 1.4 Classical vs Learning-based: 딥러닝이 바꾼 것과 바꾸지 못한 것

센서 퓨전 분야는 수십 년간 확률론적 추정 이론(Kalman Filter, Factor Graph)과 기하학적 방법(epipolar geometry, ICP)에 기반한 **고전적(classical)** 접근법이 지배해왔다. 2010년대 중반 이후 딥러닝이 컴퓨터 비전의 거의 모든 영역을 혁신하면서, 센서 퓨전에도 학습 기반(learning-based) 방법이 빠르게 침투하고 있다. 그러나 그 침투의 양상은 영역에 따라 크게 다르다.

### 딥러닝이 바꾼 것

**특징점 추출과 매칭.** 전통적으로는 SIFT, ORB 같은 수작업 설계(handcrafted) 특징 기술자를 썼다. [SuperPoint (DeTone et al., 2018)](https://arxiv.org/abs/1712.07629)는 자기 지도 학습(self-supervised learning)으로 키포인트 검출과 기술을 동시에 수행하며, 조명과 시점 변화에 대한 강건성을 크게 높였다. [SuperGlue (Sarlin et al., 2020)](https://arxiv.org/abs/1911.11763)는 그래프 뉴럴 네트워크(GNN)와 어텐션 메커니즘으로 특징점 매칭에 적용하여, 수작업 기술자 기반의 최근접 이웃 매칭보다 낮은 오매칭률을 기록했다. 가장 최근에는 [LoFTR (Sun et al., 2021)](https://arxiv.org/abs/2104.00680), [RoMa (Edstedt et al., 2024)](https://arxiv.org/abs/2305.15404) 같은 **detector-free** 방법이 키포인트 없이 직접 밀집 대응(dense correspondence)을 찾아, 텍스처가 부족한 환경에서도 매칭에 성공하고 있다.

이 영역에서 학습 기반 방법은 전통 방법을 명확히 능가한다. **패러다임 전환**이 진행 중이다.

**장소 인식(Place Recognition).** Bag of Words (DBoW2)에서 [NetVLAD (Arandjelović et al., 2016)](https://arxiv.org/abs/1511.07247)로의 전환은 성능 격차가 컸다. CNN 기반의 전역 기술자(global descriptor)는 조명, 계절 변화가 있는 환경에서 DBoW2보다 재인식률이 크게 높았다. 최근 [AnyLoc (Keetha et al., 2023)](https://arxiv.org/abs/2308.00688)은 DINOv2 같은 Foundation Model의 특징을 활용하여 별도의 학습 없이도 다양한 환경에서 범용적으로 동작하는 장소 인식을 보여주었다.

**단안 깊이 추정.** 단일 이미지로부터 깊이를 추정하는 것은 고전적 방법으로는 불가능한 작업이다(기하학적 단서가 부족). [Depth Anything (Yang et al., 2024)](https://arxiv.org/abs/2401.10891)은 대규모 데이터에서 학습하여 KITTI에서 metric 오차를 0.1m 이하로 끌어내렸다. 후속작인 [Depth Anything V2 (Yang et al., 2024)](https://arxiv.org/abs/2406.09414)는 합성 데이터 학습과 대규모 pseudo-labeling을 통해 정밀도를 더 끌어올렸고, [Metric3D v2 (Hu et al., 2024)](https://arxiv.org/abs/2404.15506)는 zero-shot으로 절대 스케일의 깊이 추정까지 가능해, LiDAR 없이도 메트릭 깊이 정보를 센서 퓨전에 가져올 수 있는 길을 열었다.

**맵 표현.** NeRF와 3D Gaussian Splatting은 장면을 신경망으로 표현하는 새로운 패러다임을 열었다. NeRF-SLAM, Gaussian Splatting SLAM 등은 전통적인 점 지도(point map)나 복셀 격자(voxel grid)를 넘어서는 포토리얼리스틱 맵 표현을 제공한다.

**이벤트 카메라(Event Camera).** 뉴로모픽 비전 센서로 불리는 이벤트 카메라는 각 픽셀이 밝기 변화를 비동기적으로 감지하여, 극도로 높은 시간 분해능(마이크로초 수준)과 넓은 다이나믹 레인지를 제공한다. 최근 [이벤트 카메라 서베이 (Huang et al., 2024)](https://arxiv.org/abs/2408.13627)가 정리하듯, 이벤트 기반 VIO와 SLAM 연구가 빠르게 발전하고 있으며, 기존 프레임 기반 카메라와의 퓨전을 통해 고속 모션과 저조도 환경에서 새로운 가능성을 열고 있다.

### 딥러닝이 바꾸지 못한 것

**상태 추정(State Estimation) 백엔드.** 칼만 필터와 팩터 그래프 최적화 같은 확률론적 추정 프레임워크는 딥러닝이 대체하지 못했다. 네 가지 측면에서 그렇다.

1. **불확실성의 엄밀한 전파**: 칼만 필터와 팩터 그래프는 관측의 불확실성을 수학적으로 엄밀하게 추적하고 전파한다. 딥러닝 모델이 유사한 수준의 calibrated uncertainty를 제공하기 어렵다.
2. **물리 법칙의 보장**: 상태 전이 모델(동역학, 기구학)에 물리 법칙을 직접 인코딩하여 물리적으로 불가능한 추정을 방지한다. 학습 기반 방법은 이런 하드 제약(hard constraint)을 보장하지 못한다.
3. **데이터 효율성**: 확률론적 프레임워크는 센서 노이즈 모델과 시스템 모델만 있으면 데이터 없이도 동작한다. 학습 기반 방법은 대규모 학습 데이터가 필요하다.
4. **일반화**: 학습 기반 오도메트리(예: DeepVO)는 학습 데이터와 다른 환경에서 성능이 크게 떨어진다. 기하학적 방법은 환경에 구애받지 않는다.

**LiDAR 오도메트리.** [LOAM (Zhang & Singh, 2014)](https://frc.ri.cmu.edu/~zhangji/publications/RSS_2014.pdf) 이후의 LiDAR 오도메트리는 여전히 전통 방법이 주류다. ICP, GICP, NDT 같은 점군 정합 방법은 수학적으로 잘 이해되어 있고, 실시간 성능을 내며, 새로운 환경에 곧장 적용된다. 학습 기반 LiDAR 오도메트리(DeepLO 계열)는 아직 전통 방법의 정확도와 일반화 성능에 미치지 못한다.

**캘리브레이션.** 카메라 내부 파라미터 캘리브레이션은 [Zhang (2000)](https://doi.org/10.1109/34.888718)의 체커보드 방법이 여전히 표준이다. 타겟리스(targetless) 캘리브레이션에서 학습 기반 방법이 연구되고 있지만, 정밀도 면에서 타겟 기반 방법을 넘어서지 못하고 있다.

### Hybrid 접근: 현재의 주류

현재 가장 성공적인 시스템들은 학습 기반 프런트엔드(frontend)와 고전적 백엔드(backend)를 결합하는 **하이브리드** 구조를 취한다.

```
[학습 기반 프런트엔드]           [고전적 백엔드]
 SuperPoint/SuperGlue          Factor Graph / EKF
 Mono Depth Estimation    →    Nonlinear Optimization
 Semantic Segmentation         Kalman Filtering
 Place Recognition              Pose Graph SLAM
```

- 프런트엔드에서 딥러닝이 원시 센서 데이터로부터 고수준 특징(feature point, depth, semantic label)을 추출한다.
- 백엔드에서 기하학적/확률론적 프레임워크가 이 특징들을 시간적으로 일관된 상태 추정으로 통합한다.

[DROID-SLAM (Teed & Deng, 2021)](https://arxiv.org/abs/2108.10869)은 이러한 하이브리드 접근의 좋은 예이다. 학습된 특징 추출과 대응 찾기를 사용하면서, 최종 포즈 추정은 미분 가능한(differentiable) 번들 조정(Bundle Adjustment)으로 수행한다.

### 기술 계보 요약

이 가이드는 각 주제에서 **전통 방법 → 딥러닝이 가능하게 한 것 → 아직 전통이 필요한 부분**의 흐름을 일관되게 보여줄 것이다. 아래 표는 이 가이드 전체를 관통하는 기술 계보를 요약한다.

| 영역 | Classical | Learning-based | 현재 주류 |
|------|-----------|---------------|----------|
| Feature matching | SIFT/ORB + BF/FLANN | SuperPoint+SuperGlue → LoFTR → RoMa | Hybrid |
| Visual odometry | Feature-based (ORB) / Direct (DSO) | DROID-SLAM, DPV-SLAM | 전통 우세, 학습 추격 |
| LiDAR odometry | ICP/LOAM | DeepLO 계열 | 전통 압도적 우세 |
| Place recognition | BoW/VLAD | NetVLAD → AnyLoc | 학습 우세 |
| Depth estimation | Stereo matching | Mono depth (Depth Anything) | 학습 우세 (mono) |
| Calibration | Target-based | Targetless + learning | 전통 우세 |
| Map representation | Occupancy/TSDF | NeRF / 3DGS | 공존 |
| State estimation backend | KF / Factor Graph | End-to-end 시도 | 전통 압도적 우세 |

표에서 눈에 띄는 패턴이 있다. **지각(perception)에 가까울수록 학습이 강하고, 추론(inference/estimation)에 가까울수록 전통이 강하다.** 센서 퓨전 시스템을 설계할 때 어디에 딥러닝을 투입하고 어디에 전통 방법을 유지할지, 이 패턴이 기준이 된다.

---

## 1.5 이 가이드의 범위와 구성

### robotics-practice와의 관계

이 가이드는 robotics-practice 가이드의 심화 편이다. robotics-practice가 Spatial AI 전반을 넓게 조망하는 입문서라면, 이 가이드는 **센서 퓨전, 로컬라이제이션, 리트리벌**에 집중한 심화 레퍼런스이다.

- robotics-practice에서 EKF/PF를 1–2페이지로 소개했다면, 이 가이드에서는 칼만 필터의 유도 과정부터 ESKF, IMU 사전적분, 팩터 그래프 최적화까지 챕터 단위로 깊이 있게 다룬다.
- robotics-practice에서 센서 소개를 개략적으로 했다면, 이 가이드에서는 각 센서의 **노이즈 모델과 관측 방정식**을 수식으로 유도한다.
- 겹치는 기초 내용은 robotics-practice 참조로 대체하고, 이 가이드에서는 추가적인 깊이만 다룬다.

### 가이드 구성

이 가이드는 다음과 같은 구성을 따른다:

1. **Ch.1 — Introduction** (이 챕터): 센서 퓨전의 동기와 분류
2. **Ch.2 — Sensor Modeling**: 각 센서의 관측 모델과 노이즈 특성 (수식 중심)
3. **Ch.3 — Calibration**: 다양한 센서 조합의 캘리브레이션 이론과 실전
4. **Ch.4 — State Estimation 이론**: 베이지안 필터링, 칼만 필터 계열, 파티클 필터, 팩터 그래프
5. **Ch.5 — Feature Matching & Correspondence**: SIFT에서 RoMa까지의 기술 계보
6. **Ch.6 — Visual Odometry & VIO**: VO/VIO 시스템의 내부 구조
7. **Ch.7 — LiDAR Odometry & LIO**: LiDAR 기반 오도메트리와 LiDAR-관성 퓨전
8. **Ch.8 — Multi-Sensor Fusion 아키텍처**: 다중 센서 통합 설계론
9. **Ch.9 — Place Recognition & Retrieval**: BoW에서 Foundation Model까지
10. **Ch.10 — Loop Closure & Global Optimization**: 루프 클로저와 전역 최적화
11. **Ch.11 — Spatial Representations**: 점 지도에서 Neural Map까지
12. **Ch.12 — 실전 시스템 & 벤치마크**: 실제 응용과 평가
13. **Ch.13 — Frontiers**: 최신 동향과 열린 문제

### 대상 독자

이 가이드의 대상 독자는 다음과 같은 배경을 가진 로보틱스 입문자이다:

- **선형대수**: 행렬 연산, 고유값 분해, SVD에 대한 이해
- **확률론**: 확률 분포, 조건부 확률, 베이즈 정리, 가우시안 분포에 대한 이해
- **기초 최적화**: 최소자승법, 그래디언트 디센트의 개념
- **Python**: numpy, scipy 기반 코드를 읽고 실행할 수 있는 수준

이 가이드의 각 챕터는 **직관 → 수식 → 코드/예제**의 흐름을 따른다. 먼저 개념의 직관적 이해를 제공하고, 수학적으로 엄밀하게 유도한 뒤, Python 코드로 구현하여 확인한다.

---

## 1.6 이 가이드의 관통 테마

이 가이드 전체를 읽으면서 독자가 반복적으로 만나게 될 핵심 질문들이 있다:

1. **"왜 이 전통 방법이 중요했는가?"** — 각 전통 방법이 해결한 근본적 문제와 그 해법의 우아함을 이해한다.
2. **"딥러닝이 뭘 바꿨는가?"** — 학습 기반 방법이 전통 방법의 어떤 한계를 극복했는지 구체적으로 본다.
3. **"아직 전통이 필요한 부분은 어디인가?"** — 딥러닝이 대체하지 못한 영역과 그 이유를 분석한다.
4. **"이론과 실전의 간극은 어디에 있는가?"** — 논문과 실제 시스템 사이의 차이, 실전에서 마주치는 엔지니어링 문제를 다룬다.

이 질문들을 염두에 두고 각 챕터를 읽으면, 개별 알고리즘의 이해를 넘어서 센서 퓨전이라는 분야의 전체 그림을 조망할 수 있을 것이다.

다음 챕터에서는 각 센서의 관측 모델을 수식으로 유도한다. 센서가 세상을 어떻게 "보는지"가 모든 퓨전 알고리즘의 출발점이다.

---

# Ch.2 — Sensor Modeling

Ch.1에서 센서 퓨전의 분류와 설계 원칙을 살펴보았다. 이제 본격적으로 각 센서가 세상을 어떻게 "보는지"를 수학적으로 정의한다. 센서의 관측 모델은 Ch.4에서 다룰 칼만 필터와 팩터 그래프의 관측 함수 $h(\mathbf{x})$에 직접 대입되므로, 이 챕터의 수식들은 이후 모든 알고리즘의 기초가 된다.

> robotics-practice Ch.2에서 센서를 소개 수준으로 다루었다면, 이 챕터에서는 **노이즈 모델과 수학적 관측 모델**에 집중한다. 퓨전 알고리즘은 측정값이 실제 물리량과 어떤 수학적 관계에 있고 오차가 어떻게 분포하는가를 정확히 모르면 설계할 수 없다.

---

## 2.1 카메라 관측 모델

카메라는 3D 세계의 점을 2D 이미지 평면에 투영하는 센서이다. 이 투영 과정을 수학적으로 모델링하는 것이 카메라 관측 모델의 핵심이다.

### 2.1.1 핀홀 카메라 모델 (Pinhole Camera Model)

핀홀 카메라 모델은 카메라의 가장 기본적인 수학적 모델이다. 광학 중심(optical center)을 통과하는 직선으로 3D 점이 이미지 평면에 투영된다고 가정한다.

카메라 좌표계에서 3D 점 $\mathbf{P}_c = [X_c, Y_c, Z_c]^\top$의 이미지 평면 위의 투영점 $\mathbf{p} = [u, v]^\top$은 다음과 같이 계산된다:

$$\begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \frac{1}{Z_c} \mathbf{K} \begin{bmatrix} X_c \\ Y_c \\ Z_c \end{bmatrix}$$

여기서 $\mathbf{K}$는 카메라 내부 파라미터 행렬(intrinsic matrix)이다:

$$\mathbf{K} = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix}$$

각 파라미터의 의미:
- $f_x, f_y$: 초점 거리(focal length). 물리적 초점 거리 $f$를 픽셀 크기 $(\Delta x, \Delta y)$로 나눈 값이다. $f_x = f / \Delta x$, $f_y = f / \Delta y$. 정사각형 픽셀($\Delta x = \Delta y$)에서는 $f_x = f_y$이지만, 직사각형 픽셀에서는 두 값이 다르다.
- $c_x, c_y$: 주점(principal point). 광축(optical axis)이 이미지 평면과 만나는 점의 픽셀 좌표. 이상적으로는 이미지 중심이지만, 제조 공차로 인해 수 픽셀 벗어날 수 있다.

동차 좌표(homogeneous coordinates)로 표현하면:

$$s \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \mathbf{K} [\mathbf{R} | \mathbf{t}] \begin{bmatrix} X_w \\ Y_w \\ Z_w \\ 1 \end{bmatrix}$$

여기서 $[\mathbf{R} | \mathbf{t}]$는 월드 좌표계에서 카메라 좌표계로의 외부 파라미터(extrinsic parameters)이고, $s = Z_c$는 깊이 스케일 팩터이다.

투영 함수를 $\pi(\cdot)$로 표기하면:

$$\mathbf{p} = \pi(\mathbf{P}_c) = \begin{bmatrix} f_x \frac{X_c}{Z_c} + c_x \\ f_y \frac{Y_c}{Z_c} + c_y \end{bmatrix}$$

이 비선형 투영 함수의 자코비안(Jacobian)은 상태 추정에서 핵심적으로 사용된다:

$$\frac{\partial \pi}{\partial \mathbf{P}_c} = \begin{bmatrix} \frac{f_x}{Z_c} & 0 & -\frac{f_x X_c}{Z_c^2} \\ 0 & \frac{f_y}{Z_c} & -\frac{f_y Y_c}{Z_c^2} \end{bmatrix}$$

이 $2 \times 3$ 자코비안은 EKF 기반 VIO에서 관측 모델의 선형화에 직접 사용되며, 비선형 최적화에서도 잔차의 자코비안을 구성하는 핵심 요소이다.

### 2.1.2 렌즈 왜곡 모델 (Lens Distortion Model)

실제 카메라의 렌즈는 핀홀 모델의 이상적 투영에서 벗어나는 왜곡(distortion)을 도입한다. 왜곡을 무시하면 재투영 오차(reprojection error)가 수 픽셀에서 수십 픽셀까지 증가하므로, 정밀한 센서 퓨전을 위해서는 반드시 보정해야 한다.

#### Radial-Tangential 왜곡 (Brown-Conrady 모델)

OpenCV의 기본 왜곡 모델이며, 대부분의 SLAM·VIO 시스템이 표준으로 채택한다.

정규화된 이미지 좌표 $\mathbf{p}_n = [x_n, y_n]^\top = [X_c/Z_c, \, Y_c/Z_c]^\top$에 대해:

$$r^2 = x_n^2 + y_n^2$$

**방사 왜곡(Radial Distortion):**

$$x_r = x_n (1 + k_1 r^2 + k_2 r^4 + k_3 r^6)$$
$$y_r = y_n (1 + k_1 r^2 + k_2 r^4 + k_3 r^6)$$

방사 왜곡은 렌즈의 곡률에서 비롯되며, 이미지 중심으로부터의 거리에 따라 증가한다. $k_1 < 0$이면 배럴 왜곡(barrel distortion, 중심부 확대), $k_1 > 0$이면 핀쿠션 왜곡(pincushion distortion, 가장자리 확대)이 발생한다.

**접선 왜곡(Tangential Distortion):**

$$x_t = 2p_1 x_n y_n + p_2 (r^2 + 2x_n^2)$$
$$y_t = p_1 (r^2 + 2y_n^2) + 2p_2 x_n y_n$$

접선 왜곡은 렌즈가 이미지 센서와 완벽히 평행하지 않을 때(decentering) 발생한다. 방사 왜곡보다 크기가 훨씬 작지만, 정밀 응용에서는 무시할 수 없다.

**왜곡된 좌표의 최종 계산:**

$$\begin{bmatrix} x_d \\ y_d \end{bmatrix} = \begin{bmatrix} x_r + x_t \\ y_r + y_t \end{bmatrix}$$

$$\begin{bmatrix} u \\ v \end{bmatrix} = \begin{bmatrix} f_x x_d + c_x \\ f_y y_d + c_y \end{bmatrix}$$

왜곡 파라미터 $[k_1, k_2, p_1, p_2, k_3]$는 [Zhang (2000)](https://doi.org/10.1109/34.888718)의 캘리브레이션 방법으로 추정되며, 이는 Ch.3에서 상세히 다룬다.

#### 어안 (Fisheye / Equidistant) 모델

시야각(FoV)이 넓은 어안 렌즈(180° 이상)에서는 radial-tangential 모델이 적합하지 않다. 이미지 가장자리에서 $r$이 매우 크므로 다항식 근사가 발산하기 때문이다.

[Kannala & Brandt (2006)](https://doi.org/10.1109/TPAMI.2006.153)의 generic camera model은 다음과 같이 정의된다:

입사각 $\theta$를 3D 점의 광축으로부터의 각도로 정의한다:

$$\theta = \arctan\left(\frac{\sqrt{X_c^2 + Y_c^2}}{Z_c}\right)$$

왜곡된 반경 $r_d$를 $\theta$의 홀수 다항식으로 모델링한다:

$$r_d = k_1 \theta + k_2 \theta^3 + k_3 \theta^5 + k_4 \theta^7 + k_5 \theta^9$$

순수 등거리(equidistant) 투영에서는 $r_d = f \cdot \theta$이며, 이는 $k_1 = f$, $k_2 = k_3 = \cdots = 0$에 해당한다.

투영 좌표:

$$\begin{bmatrix} u \\ v \end{bmatrix} = \begin{bmatrix} f_x \cdot r_d \cdot \frac{x_n}{\sqrt{x_n^2 + y_n^2}} + c_x \\ f_y \cdot r_d \cdot \frac{y_n}{\sqrt{x_n^2 + y_n^2}} + c_y \end{bmatrix}$$

어안 렌즈는 넓은 FoV로 인해 주변 환경 인식에 유리하며, [VINS-Mono](https://arxiv.org/abs/1708.03852), Basalt 등의 VIO 시스템에서 지원된다. 캘리브레이션에는 [Scaramuzza et al. (2006)](https://rpg.ifi.uzh.ch/docs/IROS06_scaramuzza.pdf)의 OCamCalib이나 Kalibr를 사용한다.

```python
import numpy as np

def project_pinhole(P_c, K, dist_coeffs=None):
    """핀홀 카메라 모델에 의한 3D→2D 투영.
    
    Args:
        P_c: (3,) 카메라 좌표계의 3D 점 [X, Y, Z]
        K: (3,3) 내부 파라미터 행렬
        dist_coeffs: (5,) 왜곡 계수 [k1, k2, p1, p2, k3] 또는 None
    
    Returns:
        (2,) 이미지 좌표 [u, v]
    """
    X, Y, Z = P_c
    # 정규화 좌표
    x_n = X / Z
    y_n = Y / Z
    
    if dist_coeffs is not None:
        k1, k2, p1, p2, k3 = dist_coeffs
        r2 = x_n**2 + y_n**2
        r4 = r2**2
        r6 = r2 * r4
        
        # 방사 왜곡
        radial = 1 + k1 * r2 + k2 * r4 + k3 * r6
        x_d = x_n * radial + 2 * p1 * x_n * y_n + p2 * (r2 + 2 * x_n**2)
        y_d = y_n * radial + p1 * (r2 + 2 * y_n**2) + 2 * p2 * x_n * y_n
    else:
        x_d = x_n
        y_d = y_n
    
    fx, fy = K[0, 0], K[1, 1]
    cx, cy = K[0, 2], K[1, 2]
    
    u = fx * x_d + cx
    v = fy * y_d + cy
    return np.array([u, v])

def project_fisheye(P_c, K, fisheye_coeffs):
    """어안 (equidistant) 카메라 모델에 의한 3D→2D 투영.
    
    OpenCV fisheye 모듈의 모델을 사용한다:
      theta_d = theta * (1 + k1*theta^2 + k2*theta^4 + k3*theta^6 + k4*theta^8)
    이는 위 본문의 Kannala-Brandt 5-파라미터 모델과 파라미터화가 다르다.
    
    Args:
        P_c: (3,) 카메라 좌표계의 3D 점 [X, Y, Z]
        K: (3,3) 내부 파라미터 행렬
        fisheye_coeffs: (4,) 왜곡 계수 [k1, k2, k3, k4] (OpenCV 컨벤션)
    
    Returns:
        (2,) 이미지 좌표 [u, v]
    """
    X, Y, Z = P_c
    r_xyz = np.sqrt(X**2 + Y**2)
    theta = np.arctan2(r_xyz, Z)
    
    k1, k2, k3, k4 = fisheye_coeffs
    theta2 = theta**2
    r_d = theta * (1 + k1 * theta2 + k2 * theta2**2 + k3 * theta2**3 + k4 * theta2**4)
    
    if r_xyz < 1e-10:
        return np.array([K[0, 2], K[1, 2]])
    
    u = K[0, 0] * r_d * (X / r_xyz) + K[0, 2]
    v = K[1, 1] * r_d * (Y / r_xyz) + K[1, 2]
    return np.array([u, v])
```

### 2.1.3 재투영 오차 (Reprojection Error)

재투영 오차(reprojection error)는 센서 퓨전에서 카메라 관측을 활용하는 거의 모든 알고리즘의 핵심 비용 함수이다.

재투영 오차는 카메라 포즈 $\mathbf{T}_{cw} = [\mathbf{R}|\mathbf{t}]$로 3D 랜드마크 $\mathbf{P}_w$를 이미지에 투영한 예측 좌표 $\hat{\mathbf{p}}$와 실제 관측된 특징점 좌표 $\mathbf{p}_{\text{obs}}$ 사이의 차이이다:

$$\mathbf{e}_{\text{reproj}} = \mathbf{p}_{\text{obs}} - \pi(\mathbf{T}_{cw} \cdot \mathbf{P}_w)$$

여기서 $\pi(\cdot)$는 위에서 정의한 투영 함수(왜곡 포함)이다.

**번들 조정(Bundle Adjustment)**에서는 모든 카메라 포즈와 모든 랜드마크에 대한 재투영 오차의 합을 최소화한다:

$$\min_{\{\mathbf{T}_i\}, \{\mathbf{P}_j\}} \sum_{i,j} \rho\left(\| \mathbf{p}_{ij} - \pi(\mathbf{T}_i \cdot \mathbf{P}_j) \|^2_{\mathbf{\Sigma}_{ij}}\right)$$

여기서:
- $\mathbf{p}_{ij}$: 카메라 $i$에서 관측된 랜드마크 $j$의 이미지 좌표
- $\mathbf{\Sigma}_{ij}$: 관측 노이즈 공분산 (등방성 모델에서 $\sigma^2 \mathbf{I}_2$, $\sigma \approx 1$ 픽셀)
- $\rho(\cdot)$: 로버스트 커널 (Huber, Cauchy 등) — 아웃라이어의 영향을 억제

재투영 오차의 분포는 $\sigma = 0.5 \sim 2$ 픽셀의 가우시안으로 모델링된다. 이 값은 특징점 검출기의 정밀도에 의존하며, 서브픽셀 정밀도의 코너 검출이 가능한 경우 $\sigma \approx 0.5$ 픽셀까지 줄어든다. 최근에는 [Depth Anything V2 (Yang et al., 2024)](https://arxiv.org/abs/2406.09414)와 [Metric3D v2 (Hu et al., 2024)](https://arxiv.org/abs/2404.15506) 같은 foundation model이 단일 이미지로부터 밀집 깊이를 추정하여, 카메라의 관측 모델을 2D 재투영 오차에서 3D 깊이 관측으로 확장하는 데 활용되고 있다.

### 2.1.4 롤링 셔터 모델 (Rolling Shutter Model)

대부분의 저가 카메라(스마트폰, 웹캠)는 CMOS 이미지 센서를 사용하며, **롤링 셔터(rolling shutter)** 방식으로 이미지를 취득한다. 글로벌 셔터(global shutter)가 모든 픽셀을 동시에 노출하는 것과 달리, 롤링 셔터는 행(row) 단위로 순차적으로 노출한다. 이미지의 상단과 하단은 서로 다른 시점에 캡처된다.

$k$번째 행의 노출 시각은:

$$t_k = t_0 + k \cdot t_r$$

여기서 $t_0$은 첫 번째 행의 노출 시각, $t_r$은 행 간 시간 간격(row readout time)이다. 전체 이미지 노출에 걸리는 시간은 $H \cdot t_r$ ($H$: 이미지 높이)이며, 이는 수 밀리초에서 수십 밀리초에 달한다.

카메라가 움직이는 동안 롤링 셔터로 이미지를 취득하면 다음과 같은 아티팩트가 발생한다:
- **기하학적 왜곡**: 수직선이 기울어지거나, 움직이는 물체가 젤리처럼 변형된다.
- **특징점 위치 오차**: 각 특징점이 서로 다른 카메라 포즈에서 촬영되므로, 글로벌 셔터를 가정한 투영 모델이 부정확해진다.

롤링 셔터를 고려한 투영 모델에서는, 각 특징점의 행 좌표 $v$에 해당하는 시각의 카메라 포즈를 사용해야 한다:

$$\mathbf{p}_i = \pi\left(\mathbf{T}(t_{v_i}) \cdot \mathbf{P}_i\right)$$

여기서 $\mathbf{T}(t_{v_i})$는 $i$번째 특징점의 행 $v_i$에 대응하는 시각의 카메라 포즈이다. 이 포즈는 IMU 측정을 이용한 보간(interpolation)으로 구한다:

$$\mathbf{T}(t_{v_i}) = \mathbf{T}(t_0) \cdot \text{Exp}\left(\frac{v_i}{H} \cdot \text{Log}(\mathbf{T}(t_0)^{-1} \mathbf{T}(t_0 + H \cdot t_r))\right)$$

여기서 $\text{Exp}$와 $\text{Log}$는 $SE(3)$ 리 군 위의 지수/로그 맵이다.

롤링 셔터 보정은 VIO 시스템([VINS-Mono](https://arxiv.org/abs/1708.03852), [ORB-SLAM3](https://arxiv.org/abs/2007.11898))에서 선택적으로 지원되며, 특히 스마트폰이나 드론 탑재 카메라처럼 고속 모션과 저가 센서의 조합에서 중요하다.

```python
import numpy as np
from scipy.spatial.transform import Rotation, Slerp

def rolling_shutter_project(P_w, T_start, T_end, K, H, v_row):
    """롤링 셔터 카메라 모델의 투영.
    
    이미지 첫 행(T_start)과 마지막 행(T_end)의 포즈를 보간하여
    해당 행의 포즈에서 3D 점을 투영한다.
    
    Args:
        P_w: (3,) 월드 좌표 3D 점
        T_start: (4,4) 첫 행 노출 시 카메라→월드 변환
        T_end: (4,4) 마지막 행 노출 시 카메라→월드 변환
        K: (3,3) 내부 파라미터 행렬
        H: 이미지 높이 (행 수)
        v_row: 투영할 특징점의 행 좌표
    
    Returns:
        (2,) 이미지 좌표 [u, v]
    """
    alpha = v_row / H  # 보간 비율 [0, 1]
    
    # 회전 보간 (SLERP)
    R_start = Rotation.from_matrix(T_start[:3, :3])
    R_end = Rotation.from_matrix(T_end[:3, :3])
    slerp = Slerp([0, 1], Rotation.concatenate([R_start, R_end]))
    R_interp = slerp(alpha).as_matrix()
    
    # 이동 보간 (선형)
    t_interp = (1 - alpha) * T_start[:3, 3] + alpha * T_end[:3, 3]
    
    # 월드→카메라 변환 후 투영
    P_c = R_interp.T @ (P_w - t_interp)
    
    u = K[0, 0] * P_c[0] / P_c[2] + K[0, 2]
    v = K[1, 1] * P_c[1] / P_c[2] + K[1, 2]
    return np.array([u, v])
```

---

## 2.2 LiDAR 관측 모델

LiDAR는 레이저 펄스를 발사하여 반사광의 비행시간(Time-of-Flight)이나 위상차로 거리를 계산하는 능동 센서다. 관측 모델과 오차 특성을 차례로 본다.

### 2.2.1 Range-Bearing 모델

LiDAR의 기본 관측은 각 레이저 빔에 대한 **거리(range)**와 **방위(bearing)** 쌍이다. 3D LiDAR에서 각 포인트의 관측은 구면 좌표 $(r, \alpha, \omega)$로 표현된다:

- $r$: 거리 (range)
- $\alpha$: 수평 각도 (azimuth)
- $\omega$: 수직 각도 (elevation)

관측 모델은 LiDAR 좌표계의 3D 점 $\mathbf{P}_L = [x, y, z]^\top$에 대해:

$$\begin{aligned}
r &= \sqrt{x^2 + y^2 + z^2} + n_r \\
\alpha &= \arctan2(y, x) + n_\alpha \\
\omega &= \arcsin\left(\frac{z}{\sqrt{x^2 + y^2 + z^2}}\right) + n_\omega
\end{aligned}$$

여기서 $n_r, n_\alpha, n_\omega$는 각각 거리, 수평각, 수직각의 관측 노이즈이다.

**노이즈 특성:**
- **거리 노이즈** $n_r$: $\sigma_r \approx 1\text{–}3\,\text{cm}$ (기계식 LiDAR), $\sigma_r \approx 2\text{–}5\,\text{cm}$ (솔리드 스테이트). 거리가 증가하면 빔 확산(beam divergence)과 수신 에너지 감소로 노이즈가 커진다.
- **각도 노이즈** $n_\alpha, n_\omega$: 인코더 정밀도에 의존. $\sigma_\alpha, \sigma_\omega \approx 0.01°\text{–}0.1°$. 작은 값이지만 원거리에서는 위치 오차로 증폭된다 — 50m 거리에서 $0.1°$의 각도 오차는 약 $8.7\,\text{cm}$의 횡방향 오차에 해당한다.

구면 좌표에서 직교 좌표로의 변환:

$$\mathbf{P}_L = \begin{bmatrix} r \cos\omega \cos\alpha \\ r \cos\omega \sin\alpha \\ r \sin\omega \end{bmatrix}$$

**빔 확산(Beam Divergence).** LiDAR의 레이저 빔은 완벽한 직선이 아니라 미세한 원뿔형으로 확산된다. 빔 확산각은 $0.1°\text{–}0.5°$이며, 이는 원거리에서 빔의 풋프린트(footprint)가 커져 하나의 반사점이 아닌 영역의 평균 거리를 측정하게 만든다. 물체의 경계에서 빔이 부분적으로 물체와 배경 모두에 걸치면 두 거리의 중간값이 측정되어 허위 점이 생성된다 — 이를 **혼합 픽셀(mixed pixel)** 효과라고 한다.

### 2.2.2 Motion Distortion (모션 왜곡)

기계식 스피닝(spinning) LiDAR는 센서가 360° 회전하면서 레이저를 발사한다. Velodyne VLP-16의 경우 1회전에 약 100ms가 소요된다. 이 100ms 동안 플랫폼(차량, 드론)이 이동하면, 한 스캔 내의 포인트들이 서로 다른 좌표계에서 측정된 것이 된다. 이것이 **모션 왜곡(motion distortion)** 또는 **ego-motion compensation** 문제이다.

이 문제는 카메라의 롤링 셔터와 구조가 같다. 스캔의 $i$번째 포인트가 시각 $t_i$에 측정되었다면, 이 포인트를 기준 시각 $t_0$의 좌표계로 변환해야 한다:

$$\mathbf{P}_L^{(t_0)} = \mathbf{T}(t_0)^{-1} \cdot \mathbf{T}(t_i) \cdot \mathbf{P}_L^{(t_i)}$$

여기서 $\mathbf{T}(t_i)$는 시각 $t_i$에서의 LiDAR 포즈이다.

**보정 방법:**

1. **IMU 기반 보간**: IMU의 고주파 측정으로 스캔 기간 동안의 포즈 변화를 보간한다. 가장 일반적인 방법이며, LIO-SAM, FAST-LIO2 등에서 사용된다.
2. **이전 프레임 오도메트리 기반**: 직전 프레임의 추정 속도로 등속(constant velocity) 모델을 적용한다.
3. **연속 시간(Continuous-time) 방법**: B-스플라인 등으로 궤적을 연속 함수로 모델링하고, 각 포인트의 시각에서의 포즈를 평가한다. [CT-ICP (Dellenbach et al., 2022)](https://arxiv.org/abs/2109.12979)가 대표적이다.

```python
import numpy as np
from scipy.spatial.transform import Rotation, Slerp

def undistort_scan(points, timestamps, T_start, T_end, t_start, t_end):
    """IMU 기반 모션 왜곡 보정.
    
    스캔 시작과 끝의 포즈를 보간하여 각 포인트를 시작 시각 좌표계로 변환.
    
    Args:
        points: (N, 3) LiDAR 포인트 클라우드
        timestamps: (N,) 각 포인트의 타임스탬프
        T_start: (4,4) 스캔 시작 시 LiDAR 포즈 (lidar→world)
        T_end: (4,4) 스캔 끝 시 LiDAR 포즈 (lidar→world)
        t_start, t_end: 스캔 시작/끝 시각
    
    Returns:
        (N, 3) 보정된 포인트 클라우드
    """
    R_start = Rotation.from_matrix(T_start[:3, :3])
    R_end = Rotation.from_matrix(T_end[:3, :3])
    slerp = Slerp([t_start, t_end], Rotation.concatenate([R_start, R_end]))
    
    corrected = np.zeros_like(points)
    for i in range(len(points)):
        alpha = (timestamps[i] - t_start) / (t_end - t_start)
        alpha = np.clip(alpha, 0, 1)
        
        # 해당 시각의 포즈 보간
        R_i = slerp(timestamps[i]).as_matrix()
        t_i = (1 - alpha) * T_start[:3, 3] + alpha * T_end[:3, 3]
        
        # 해당 시각 좌표계 → 시작 시각 좌표계
        p_world = R_i @ points[i] + t_i
        corrected[i] = T_start[:3, :3].T @ (p_world - T_start[:3, 3])
    
    return corrected
```

### 2.2.3 Spinning vs Solid-State LiDAR가 퓨전에 미치는 영향

**기계식 스피닝 LiDAR** (Velodyne, Ouster, Hesai)는 360° 수평 FoV를 제공하며, 한 스캔이 완전한 환형 포인트 클라우드를 구성한다. LOAM 계열 알고리즘은 이 특성을 전제로 만들어졌다 — edge/planar feature를 수평 스캔 라인에서 추출하고, 전방위 관측으로 6-DoF 포즈를 추정한다.

**솔리드 스테이트 LiDAR** (Livox Mid-40/70, Avia, HAP 등)는 기계적 회전부가 없으며, 제한된 FoV(예: Livox Mid-70은 약 70.4° 원형) 내에서 비반복(non-repetitive) 스캔 패턴을 사용한다. 시간이 지남에 따라 FoV 내의 커버리지가 점진적으로 증가하는 특성이 있다.

이 차이가 퓨전 알고리즘에 미치는 영향:

| 특성 | 스피닝 LiDAR | 솔리드 스테이트 LiDAR |
|------|-------------|-------------------|
| FoV | 360° 수평 | 제한적 (40°~120°) |
| 스캔 패턴 | 반복적 (수평 라인) | 비반복적 (로제트, 리사주 등) |
| 단일 프레임 밀도 | 균일 | 불균일 (시간에 따라 증가) |
| Feature 추출 | 스캔 라인 기반 가능 | 스캔 라인 구조 없음 |
| 적합한 알고리즘 | LOAM, LeGO-LOAM | FAST-LIO/LIO2 (점 단위 처리) |

FAST-LIO / [FAST-LIO2](https://arxiv.org/abs/2107.06829)가 솔리드 스테이트 LiDAR에서 특히 강력한 이유는, 스캔 라인 구조에 의존하지 않고 **개별 포인트를 순차적으로 처리**하는 iterated EKF 구조를 사용하기 때문이다. 반면 LOAM의 edge/planar feature 추출은 스캔 라인 구조를 전제하므로 솔리드 스테이트에 직접 적용하기 어렵다. 최근 [FAST-LIVO2 (Zheng et al., 2024)](https://arxiv.org/abs/2408.14035)는 이 구조를 확장하여 LiDAR-관성-비전 세 센서를 동일한 iterated EKF 내에서 순차적으로 융합하며, direct 방법으로 별도의 특징점 추출 없이 LiDAR 포인트와 이미지 모두를 처리한다.

---

## 2.3 IMU 모델

IMU(Inertial Measurement Unit)는 3축 가속도계(accelerometer)와 3축 자이로스코프(gyroscope)로 구성된다. 거의 모든 센서 퓨전 시스템에서 IMU는 척추 역할을 한다. 100Hz–1kHz의 고주파 관측이 카메라·LiDAR 프레임 사이를 보간하고, 초기화와 스케일 관측성을 떠받친다. 오차 모델을 상세히 본다.

### 2.3.1 자이로스코프 오차 모델

자이로스코프는 3축 각속도 $\boldsymbol{\omega}$를 측정한다. 실제 측정값 $\tilde{\boldsymbol{\omega}}$는 다음과 같이 모델링된다:

$$\tilde{\boldsymbol{\omega}} = \boldsymbol{\omega} + \mathbf{b}_g + \mathbf{n}_g$$

각 항의 의미:
- $\boldsymbol{\omega}$: 참 각속도 (IMU 바디 프레임에서)
- $\mathbf{b}_g$: **바이어스(bias)** — 시간에 따라 천천히 변하는 상수적 오프셋
- $\mathbf{n}_g$: **측정 노이즈** — 백색 가우시안 노이즈(Additive White Gaussian Noise, AWGN)

**바이어스의 동역학.** 바이어스의 느린 시간 변화는 **랜덤 워크(random walk)**로 모델링한다:

$$\dot{\mathbf{b}}_g = \mathbf{n}_{bg}$$

여기서 $\mathbf{n}_{bg} \sim \mathcal{N}(\mathbf{0}, \sigma_{bg}^2 \mathbf{I})$는 바이어스 변화의 구동 노이즈이다. 이산 시간에서:

$$\mathbf{b}_{g,k+1} = \mathbf{b}_{g,k} + \sigma_{bg} \sqrt{\Delta t} \cdot \mathbf{w}_k, \quad \mathbf{w}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{I})$$

MEMS급 자이로스코프의 전형적인 파라미터:
- 측정 노이즈 밀도: $\sigma_g \approx 0.004\,\text{rad/s}/\sqrt{\text{Hz}}$ (약 $0.2\,°/\text{s}/\sqrt{\text{Hz}}$)
- 바이어스 안정성(in-run bias stability): $\sigma_{bg} \approx 10\text{–}100\,°/\text{hr}$
- 바이어스 랜덤 워크: $\sigma_{bg} \approx 0.0002\,\text{rad/s}^2/\sqrt{\text{Hz}}$

### 2.3.2 가속도계 오차 모델

가속도계는 3축 비력(specific force) $\mathbf{a}$를 측정한다. 비력은 진가속도에서 중력을 뺀 값이다. 실제 측정값 $\tilde{\mathbf{a}}$는:

$$\tilde{\mathbf{a}} = \mathbf{R}_{bw}(\mathbf{a}_w - \mathbf{g}_w) + \mathbf{b}_a + \mathbf{n}_a$$

각 항의 의미:
- $\mathbf{a}_w$: 월드 프레임에서의 진가속도
- $\mathbf{g}_w = [0, 0, -g]^\top$: 중력 벡터 ($g \approx 9.81\,\text{m/s}^2$)
- $\mathbf{R}_{bw}$: 월드→바디 회전 행렬
- $\mathbf{b}_a$: 가속도계 바이어스
- $\mathbf{n}_a \sim \mathcal{N}(\mathbf{0}, \sigma_a^2 \mathbf{I})$: 측정 노이즈

**중력의 역할.** 가속도계가 중력을 "느끼는" 것은 IMU 기반 퓨전에서 매우 중요하다. 정지 상태에서도 가속도계는 $[0, 0, g]^\top$ (위 방향을 z로 놓은 경우)을 측정한다. 이 중력 관측으로부터 롤(roll)과 피치(pitch)를 추정할 수 있다. 그러나 요(yaw)는 중력 벡터에 대한 회전이므로 관측 불가능(unobservable)하다. 이 때문에 VIO/LIO 초기화에서 요 각도를 추정하려면 시각적 특징점의 이동 등 추가 관측이 필요하다.

**바이어스 동역학.** 자이로스코프와 동일하게 랜덤 워크로 모델링한다:

$$\dot{\mathbf{b}}_a = \mathbf{n}_{ba}, \quad \mathbf{n}_{ba} \sim \mathcal{N}(\mathbf{0}, \sigma_{ba}^2 \mathbf{I})$$

MEMS급 가속도계의 전형적인 파라미터:
- 측정 노이즈 밀도: $\sigma_a \approx 0.04\,\text{m/s}^2/\sqrt{\text{Hz}}$ (약 $4\,\text{mg}/\sqrt{\text{Hz}}$)
- 바이어스 안정성: $\sigma_{ba} \approx 0.01\text{–}0.1\,\text{mg}$
- 바이어스 랜덤 워크: $\sigma_{ba} \approx 0.001\,\text{m/s}^3/\sqrt{\text{Hz}}$

### 2.3.3 Allan Variance

Allan Variance는 IMU의 노이즈 특성을 분석하는 표준적인 방법이다. 정지 상태에서 장시간(수 시간) 데이터를 수집하여 다양한 평균 시간(cluster time) $\tau$에서의 분산을 계산한다.

Allan Variance $\sigma^2(\tau)$의 정의:

$$\sigma^2(\tau) = \frac{1}{2} \langle (\bar{y}_{k+1} - \bar{y}_k)^2 \rangle$$

여기서 $\bar{y}_k$는 $k$번째 구간(길이 $\tau$)의 평균 출력이다.

log-log 플롯에서 Allan Deviation $\sigma(\tau)$의 기울기로 노이즈 종류를 식별한다:

| 기울기 | 노이즈 종류 | 물리적 의미 |
|--------|-----------|-----------|
| $-1/2$ | 각도/속도 랜덤 워크 (ARW/VRW) | 백색 노이즈 $\mathbf{n}_g, \mathbf{n}_a$ |
| $0$ | 바이어스 불안정성 (Bias Instability) | 플리커 노이즈, 최소값이 바이어스 안정성 |
| $+1/2$ | 레이트 랜덤 워크 (RRW) | 바이어스 랜덤 워크 $\mathbf{n}_{bg}, \mathbf{n}_{ba}$ |

**데이터시트 읽는 법.** IMU 데이터시트에서 센서 퓨전에 필요한 핵심 파라미터를 추출하는 방법:

1. **각도 랜덤 워크(Angular Random Walk, ARW)**: 단위 $°/\sqrt{\text{hr}}$ 또는 $\text{rad/s}/\sqrt{\text{Hz}}$. Allan Deviation 플롯에서 $\tau = 1\,\text{s}$일 때의 값, 또는 기울기 $-1/2$ 구간에서 읽는다. 이것이 $\sigma_g$에 해당한다.
2. **속도 랜덤 워크(Velocity Random Walk, VRW)**: 단위 $\text{m/s}/\sqrt{\text{hr}}$ 또는 $\text{m/s}^2/\sqrt{\text{Hz}}$. 가속도계의 백색 노이즈 밀도. 이것이 $\sigma_a$에 해당한다.
3. **바이어스 안정성(In-run Bias Stability)**: Allan Deviation 플롯의 최솟값. 시스템이 도달할 수 있는 바이어스 추정의 이론적 하한이다.
4. **레이트 랜덤 워크(Rate Random Walk)**: 바이어스가 시간에 따라 변하는 속도. 이것이 $\sigma_{bg}, \sigma_{ba}$에 해당한다.

```python
import numpy as np

def compute_allan_variance(data, dt, max_cluster_size=None):
    """Allan Variance 계산.
    
    Args:
        data: (N,) 정지 상태에서 수집한 IMU 한 축의 데이터
        dt: 샘플링 주기 (초)
        max_cluster_size: 최대 클러스터 크기 (None이면 N//2)
    
    Returns:
        taus: 클러스터 시간 배열
        adevs: Allan Deviation 배열
    """
    N = len(data)
    if max_cluster_size is None:
        max_cluster_size = N // 2
    
    # 로그 스케일로 클러스터 크기 생성
    cluster_sizes = np.unique(np.logspace(
        0, np.log10(max_cluster_size), num=100
    ).astype(int))
    cluster_sizes = cluster_sizes[cluster_sizes > 0]
    
    taus = []
    adevs = []
    
    for m in cluster_sizes:
        tau = m * dt
        # 비겹침 평균 계산
        n_clusters = N // m
        if n_clusters < 2:
            break
        
        # 각 클러스터의 평균
        truncated = data[:n_clusters * m].reshape(n_clusters, m)
        cluster_means = truncated.mean(axis=1)
        
        # Allan Variance
        diff = np.diff(cluster_means)
        avar = 0.5 * np.mean(diff**2)
        
        taus.append(tau)
        adevs.append(np.sqrt(avar))
    
    return np.array(taus), np.array(adevs)

def extract_imu_params(taus, adevs):
    """Allan Deviation 플롯에서 IMU 노이즈 파라미터 추출.
    
    Args:
        taus: 클러스터 시간 배열
        adevs: Allan Deviation 배열
    
    Returns:
        dict: {
            'white_noise': tau=1에서의 값 (ARW 또는 VRW),
            'bias_instability': 최솟값,
            'bias_instability_tau': 최솟값에 해당하는 tau
        }
    """
    # 백색 노이즈: tau=1에서의 Allan Deviation (기울기 -1/2 구간)
    idx_1s = np.argmin(np.abs(taus - 1.0))
    white_noise = adevs[idx_1s]
    
    # 바이어스 안정성: Allan Deviation의 최솟값
    idx_min = np.argmin(adevs)
    bias_instability = adevs[idx_min]
    bias_instability_tau = taus[idx_min]
    
    return {
        'white_noise': white_noise,
        'bias_instability': bias_instability,
        'bias_instability_tau': bias_instability_tau
    }
```

### 2.3.4 Strapdown Navigation Equation

IMU 측정으로부터 포즈(위치, 속도, 자세)를 적분하는 방정식을 스트랩다운 관성 항법 방정식(Strapdown Navigation Equation)이라 한다. "스트랩다운(strapdown)"이란 센서가 플랫폼에 직접 고정(strapped down)되어 있어, 기계식 짐벌 없이 소프트웨어로 좌표 변환을 수행한다는 의미이다.

월드 프레임(또는 항법 프레임)에서의 상태 $[\mathbf{R}, \mathbf{v}, \mathbf{p}]$의 연속 시간 동역학:

$$\dot{\mathbf{R}} = \mathbf{R} \cdot [\tilde{\boldsymbol{\omega}} - \mathbf{b}_g - \mathbf{n}_g]_\times$$

$$\dot{\mathbf{v}} = \mathbf{R} (\tilde{\mathbf{a}} - \mathbf{b}_a - \mathbf{n}_a) + \mathbf{g}$$

$$\dot{\mathbf{p}} = \mathbf{v}$$

여기서:
- $\mathbf{R} \in SO(3)$: 바디→월드 회전 행렬
- $\mathbf{v} \in \mathbb{R}^3$: 월드 프레임에서의 속도
- $\mathbf{p} \in \mathbb{R}^3$: 월드 프레임에서의 위치
- $[\cdot]_\times$: 반대칭(skew-symmetric) 행렬 (벡터의 외적을 행렬 곱으로 표현)

$$[\boldsymbol{\omega}]_\times = \begin{bmatrix} 0 & -\omega_z & \omega_y \\ \omega_z & 0 & -\omega_x \\ -\omega_y & \omega_x & 0 \end{bmatrix}$$

**이산 시간 적분.** 시각 $t_k$에서 $t_{k+1} = t_k + \Delta t$로의 상태 전파:

$$\mathbf{R}_{k+1} = \mathbf{R}_k \cdot \text{Exp}\left((\tilde{\boldsymbol{\omega}}_k - \mathbf{b}_{g,k}) \Delta t\right)$$

$$\mathbf{v}_{k+1} = \mathbf{v}_k + \mathbf{g} \Delta t + \mathbf{R}_k (\tilde{\mathbf{a}}_k - \mathbf{b}_{a,k}) \Delta t$$

$$\mathbf{p}_{k+1} = \mathbf{p}_k + \mathbf{v}_k \Delta t + \frac{1}{2} \mathbf{g} \Delta t^2 + \frac{1}{2} \mathbf{R}_k (\tilde{\mathbf{a}}_k - \mathbf{b}_{a,k}) \Delta t^2$$

여기서 $\text{Exp}(\boldsymbol{\phi})$는 $SO(3)$ 위의 지수 맵으로, 회전 벡터 $\boldsymbol{\phi}$를 회전 행렬로 변환한다. Rodrigues' 공식으로 계산할 수 있다:

$$\text{Exp}(\boldsymbol{\phi}) = \mathbf{I} + \frac{\sin\|\boldsymbol{\phi}\|}{\|\boldsymbol{\phi}\|} [\boldsymbol{\phi}]_\times + \frac{1 - \cos\|\boldsymbol{\phi}\|}{\|\boldsymbol{\phi}\|^2} [\boldsymbol{\phi}]_\times^2$$

**중간점(midpoint) 적분.** 위의 오일러 적분은 1차 정밀도이다. 2차 정밀도를 위해 연속된 두 IMU 측정의 중간값을 사용하는 방법이 있다:

$$\bar{\boldsymbol{\omega}} = \frac{1}{2}(\tilde{\boldsymbol{\omega}}_k + \tilde{\boldsymbol{\omega}}_{k+1}) - \mathbf{b}_{g,k}$$

$$\mathbf{R}_{k+1} = \mathbf{R}_k \cdot \text{Exp}(\bar{\boldsymbol{\omega}} \Delta t)$$

$$\bar{\mathbf{a}} = \frac{1}{2}\left(\mathbf{R}_k(\tilde{\mathbf{a}}_k - \mathbf{b}_{a,k}) + \mathbf{R}_{k+1}(\tilde{\mathbf{a}}_{k+1} - \mathbf{b}_{a,k})\right)$$

$$\mathbf{v}_{k+1} = \mathbf{v}_k + (\bar{\mathbf{a}} + \mathbf{g}) \Delta t$$

$$\mathbf{p}_{k+1} = \mathbf{p}_k + \mathbf{v}_k \Delta t + \frac{1}{2}(\bar{\mathbf{a}} + \mathbf{g}) \Delta t^2$$

이 중간점 적분이 VINS-Mono, FAST-LIO2 등에서 사용되는 기본 적분 방식이다. 더 정밀한 4차 Runge-Kutta(RK4) 적분도 가능하지만, 일반적인 IMU 주파수(200–400Hz)에서는 중간점 적분으로 충분하다.

```python
import numpy as np
from scipy.spatial.transform import Rotation

def skew(v):
    """3D 벡터의 skew-symmetric 행렬."""
    return np.array([
        [0, -v[2], v[1]],
        [v[2], 0, -v[0]],
        [-v[1], v[0], 0]
    ])

def exp_so3(phi):
    """SO(3) 지수 맵: 회전 벡터 → 회전 행렬 (Rodrigues' formula)."""
    angle = np.linalg.norm(phi)
    if angle < 1e-10:
        return np.eye(3) + skew(phi)
    
    axis = phi / angle
    K = skew(axis)
    return np.eye(3) + np.sin(angle) * K + (1 - np.cos(angle)) * K @ K

def imu_strapdown(gyro_data, accel_data, dt, R0, v0, p0, bg, ba, gravity):
    """스트랩다운 관성 항법 (중간점 적분).
    
    Args:
        gyro_data: (N, 3) 자이로스코프 측정 [rad/s]
        accel_data: (N, 3) 가속도계 측정 [m/s^2]
        dt: 샘플링 주기 [s]
        R0: (3,3) 초기 회전 행렬 (바디→월드)
        v0: (3,) 초기 속도 [m/s] (월드 프레임)
        p0: (3,) 초기 위치 [m] (월드 프레임)
        bg: (3,) 자이로 바이어스
        ba: (3,) 가속도 바이어스
        gravity: (3,) 중력 벡터 (예: [0, 0, -9.81])
    
    Returns:
        Rs: (N+1, 3, 3) 회전 이력
        vs: (N+1, 3) 속도 이력
        ps: (N+1, 3) 위치 이력
    """
    N = len(gyro_data)
    Rs = np.zeros((N + 1, 3, 3))
    vs = np.zeros((N + 1, 3))
    ps = np.zeros((N + 1, 3))
    
    Rs[0] = R0
    vs[0] = v0
    ps[0] = p0
    
    for k in range(N - 1):
        # 중간점 자이로
        omega_k = gyro_data[k] - bg
        omega_k1 = gyro_data[k + 1] - bg
        omega_mid = 0.5 * (omega_k + omega_k1)
        
        # 회전 업데이트
        Rs[k + 1] = Rs[k] @ exp_so3(omega_mid * dt)
        
        # 중간점 가속도 (월드 프레임)
        a_k = Rs[k] @ (accel_data[k] - ba)
        a_k1 = Rs[k + 1] @ (accel_data[k + 1] - ba)
        a_mid = 0.5 * (a_k + a_k1)
        
        # 속도/위치 업데이트
        vs[k + 1] = vs[k] + (a_mid + gravity) * dt
        ps[k + 1] = ps[k] + vs[k] * dt + 0.5 * (a_mid + gravity) * dt**2
    
    # 마지막 스텝 (단순 오일러)
    k = N - 1
    omega_k = gyro_data[k] - bg
    Rs[k + 1] = Rs[k] @ exp_so3(omega_k * dt)
    a_k = Rs[k] @ (accel_data[k] - ba)
    vs[k + 1] = vs[k] + (a_k + gravity) * dt
    ps[k + 1] = ps[k] + vs[k] * dt + 0.5 * (a_k + gravity) * dt**2
    
    return Rs, vs, ps
```

**드리프트의 수치적 의미.** 위의 스트랩다운 적분을 바이어스 보정 없이 수행하면 얼마나 빠르게 발산하는지 간단히 계산해보자. 가속도계 바이어스가 $b_a = 0.01\,\text{m/s}^2$ (약 $1\,\text{mg}$, 전형적인 MEMS 수준)인 경우:

- 1초 후 위치 오차: $\frac{1}{2} \times 0.01 \times 1^2 = 0.005\,\text{m}$ (5mm)
- 10초 후: $\frac{1}{2} \times 0.01 \times 100 = 0.5\,\text{m}$
- 60초 후: $\frac{1}{2} \times 0.01 \times 3600 = 18\,\text{m}$

IMU 단독으로는 항법이 불가능한 이유가 바로 이 발산 속도이다. 바이어스는 센서 퓨전을 통해 지속적으로 추정하고 보정해야 한다. VIO/LIO 시스템에서 바이어스 $\mathbf{b}_g, \mathbf{b}_a$는 **상태 벡터의 일부로 포함**되어 다른 센서의 관측을 통해 지속적으로 업데이트된다. 한편, 최근 딥러닝 기반 관성 오도메트리 연구도 활발하다. [AirIO (Chen et al., 2025)](https://arxiv.org/abs/2501.15659)는 IMU 특징의 관측 가능성을 강화하여 드론 환경에서 기존 학습 기반 관성 오도메트리 대비 50% 이상의 정확도 향상을 보고하고 있다.

### 2.3.5 IMU 등급 분류

IMU는 성능에 따라 크게 세 등급으로 나뉘며, 센서 퓨전 시스템 설계 시 어떤 등급을 사용하느냐에 따라 필요한 외부 센서 보조의 빈도와 종류가 달라진다.

| 등급 | ARW | 바이어스 안정성 (자이로) | 가격대 | 단독 항법 시간 | 예시 |
|------|-----|----------------------|-------|-------------|------|
| 항법등급 (Navigation) | $< 0.002°/\sqrt{\text{hr}}$ | $< 0.01°/\text{hr}$ | $>$ \$10k | 수 시간 | HG1700, LN-200 |
| 전술등급 (Tactical) | $0.05\text{–}0.5°/\sqrt{\text{hr}}$ | $0.1\text{–}10°/\text{hr}$ | \$1k–10k | 수 분 | STIM300, ADIS16490 |
| MEMS | $0.1\text{–}1°/\sqrt{\text{hr}}$ | $1\text{–}100°/\text{hr}$ | $<$ \$100 | 수 초 | BMI088, ICM-42688 |

로보틱스와 자율주행에서는 대부분 MEMS급 IMU를 사용하므로, 카메라, LiDAR 등과의 긴밀한 퓨전이 필수적이다.

---

## 2.4 GNSS 모델

GNSS(Global Navigation Satellite System)는 GPS, GLONASS, Galileo, BeiDou 등 위성 항법 시스템의 총칭이다. 센서 퓨전에서 GNSS 관측을 활용하기 위한 수학적 모델을 본다.

### 2.4.1 의사거리 관측 모델 (Pseudorange)

GNSS 수신기는 각 위성으로부터의 **의사거리(pseudorange)** $\rho$를 측정한다. 의사거리는 위성까지의 실제 거리에 수신기 시계 오차 등의 오차 항이 더해진 것이다:

$$\rho^s = r^s + c \cdot \delta t_r - c \cdot \delta t^s + I^s + T^s + \epsilon_\rho$$

각 항의 의미:
- $r^s = \|\mathbf{p}_r - \mathbf{p}^s\|$: 수신기 위치 $\mathbf{p}_r$와 위성 $s$ 위치 $\mathbf{p}^s$ 사이의 기하학적 거리
- $c \cdot \delta t_r$: 수신기 시계 오차 (미지수, 위치와 함께 추정)
- $c \cdot \delta t^s$: 위성 시계 오차 (항법 메시지에서 보정 가능)
- $I^s$: 전리층(ionosphere) 지연 — 전리층의 자유 전자에 의한 신호 지연. L1/L2 이중 주파수 수신기로 제거 가능.
- $T^s$: 대류권(troposphere) 지연 — 대기의 수증기와 공기에 의한 지연. Saastamoinen 모델 등으로 보정.
- $\epsilon_\rho$: 잔여 노이즈 (열잡음, 다중경로 등). 일반적으로 $\sigma_\rho \approx 1\text{–}5\,\text{m}$ (단독 측위), $\sigma_\rho \approx 0.1\text{–}0.3\,\text{m}$ (이중 주파수 + 보정).

**측위(positioning) 원리.** 4개 이상의 위성으로부터 의사거리를 관측하면, 수신기 위치 $(x, y, z)$와 시계 오차 $\delta t_r$의 4개 미지수를 풀 수 있다. 비선형 방정식을 최소자승법 또는 칼만 필터로 풀며, 이것이 표준 단독 측위(Single Point Positioning, SPP)의 기본이다.

**다중경로(Multipath).** 도심 환경에서 위성 신호가 건물에 반사되어 직접 경로 외의 경로로 도달하면, 의사거리에 수 미터에서 수십 미터의 오차가 추가된다. 이는 모델링이 어렵고 환경에 따라 변하므로, 센서 퓨전에서 GNSS를 사용할 때 아웃라이어 처리가 특히 중요하다.

### 2.4.2 반송파 위상 관측 모델 (Carrier Phase)

반송파 위상(carrier phase) 관측은 의사거리보다 훨씬 정밀하다(밀리미터 수준). L1 반송파(약 1575.42MHz)의 파장은 약 19cm이며, 위상을 1%만 분해해도 약 2mm의 거리 정밀도가 가능하다.

$$\Phi^s = r^s + c \cdot \delta t_r - c \cdot \delta t^s + \lambda N^s - I^s + T^s + \epsilon_\Phi$$

여기서:
- $\lambda$: 반송파 파장
- $N^s$: **정수 모호성(integer ambiguity)** — 수신기와 위성 사이의 전체 파장 수. 미지의 정수값으로, 이를 정확히 결정하는 것이 RTK/PPP의 핵심이다.
- $\epsilon_\Phi \approx 1\text{–}5\,\text{mm}$: 반송파 위상 노이즈 (의사거리 노이즈의 약 1/100)

전리층 지연의 부호는 의사거리와 반대이다(군속도 vs 위상속도). 이중 주파수 관측으로 이 차이를 활용하면 전리층 지연을 제거할 수 있다.

### 2.4.3 RTK (Real-Time Kinematic)

RTK는 가까운 거리(수 km 이내)에 있는 기준국(base station)과의 **차분(differencing)** 관측을 통해 공통 오차(위성 시계, 전리층, 대류권)를 제거하고, 반송파 위상의 정수 모호성을 실시간으로 해결하여 **센티미터 급** 측위를 달성하는 기법이다.

**이중 차분(Double Difference).** 위성 $s$와 기준 위성 $r$에 대한 기준국-이동국 간 이중 차분:

$$\nabla\Delta\Phi_{br}^{sr} = \nabla\Delta r_{br}^{sr} + \lambda \nabla\Delta N_{br}^{sr} + \epsilon$$

이중 차분을 통해 수신기/위성 시계 오차가 제거되고, 전리층/대류권 오차가 (기준국이 가까울 때) 거의 제거된다. 남은 미지수는 기하학적 거리(위치에 의존)와 정수 모호성이며, LAMBDA 알고리즘 등으로 정수 모호성을 해결한다.

### 2.4.4 PPP (Precise Point Positioning)

PPP는 기준국 없이 단일 수신기로 센티미터 급 측위를 달성하는 기법이다. 정밀 궤도력(precise orbit)과 정밀 시계 보정(precise clock)을 외부 서비스에서 수신하여 위성 관련 오차를 제거하고, 전리층/대류권 오차를 상태 벡터에 포함하여 추정한다.

**RTK vs PPP:**

| 특성 | RTK | PPP |
|------|-----|-----|
| 기준국 필요 | 예 (수 km 이내) | 아니오 |
| 수렴 시간 | 수 초~수십 초 | 수십 분 (전통), 수 분 (PPP-AR) |
| 정밀도 (수렴 후) | $\sim 2\,\text{cm}$ | $\sim 5\,\text{cm}$ |
| 커버리지 | 기준국 근처 | 전지구 |

**센서 퓨전에서의 GNSS 활용.** GNSS는 절대 위치를 제공하므로, VIO/LIO와 결합하면 장기 드리프트를 완전히 제거할 수 있다. [LIO-SAM (Shan et al., 2020)](https://arxiv.org/abs/2007.00258)은 GNSS 팩터를 팩터 그래프에 직접 추가하는 대표적인 예이다. GNSS 관측을 퓨전에 포함시킬 때 주요 고려사항:

1. **좌표계 변환**: GNSS는 WGS84(위도, 경도, 타원체고)로 출력되며, 로보틱스 시스템은 ENU(East-North-Up) 또는 NED(North-East-Down) 로컬 프레임을 사용한다. 변환이 필요하다.
2. **공분산 활용**: GNSS 수신기가 출력하는 DOP(Dilution of Precision) 값이나 위치 공분산을 퓨전 시스템의 관측 공분산으로 활용한다.
3. **아웃라이어 처리**: 다중경로 환경에서 GNSS 측위 결과가 수십 미터 오차를 가질 수 있으므로, 로버스트 커널이나 $\chi^2$ 테스트로 이상 관측을 탐지/제거해야 한다.

```python
import numpy as np

def pseudorange_model(p_receiver, p_satellites, clock_bias):
    """의사거리 관측 모델 (단순화).
    
    Args:
        p_receiver: (3,) ECEF 좌표에서의 수신기 위치 [m]
        p_satellites: (N, 3) 각 위성의 ECEF 위치 [m]
        clock_bias: 수신기 시계 오차 [m] (c * dt_r)
    
    Returns:
        pseudoranges: (N,) 예측 의사거리 [m]
        H: (N, 4) 관측 자코비안 (선형화 기준점에서)
    """
    N = len(p_satellites)
    pseudoranges = np.zeros(N)
    H = np.zeros((N, 4))
    
    for i in range(N):
        diff = p_receiver - p_satellites[i]
        r = np.linalg.norm(diff)
        pseudoranges[i] = r + clock_bias
        
        # 자코비안: d(rho)/d(x,y,z,cb) 
        e = diff / r  # 단위 벡터 (수신기→위성 방향)
        H[i, :3] = e
        H[i, 3] = 1.0  # clock bias에 대한 편미분
    
    return pseudoranges, H

def geodetic_to_enu(lat, lon, alt, lat0, lon0, alt0):
    """WGS84 좌표를 로컬 ENU 좌표로 변환.
    
    Args:
        lat, lon: 변환할 점의 위도, 경도 [rad]
        alt: 타원체 고도 [m]
        lat0, lon0, alt0: 원점의 위도, 경도, 고도 [rad, rad, m]
    
    Returns:
        (3,) ENU 좌표 [m]
    """
    # 간이 변환 (WGS84 지구 반경 사용)
    a = 6378137.0  # WGS84 장반경
    e2 = 0.00669437999014  # 이심률 제곱
    
    sin_lat0 = np.sin(lat0)
    N0 = a / np.sqrt(1 - e2 * sin_lat0**2)
    
    dlat = lat - lat0
    dlon = lon - lon0
    dalt = alt - alt0
    
    east = (N0 + alt0) * np.cos(lat0) * dlon
    north = (N0 * (1 - e2) + alt0) * dlat
    up = dalt
    
    return np.array([east, north, up])
```

---

## 2.5 Radar 모델

레이더(Radar)는 전파(radio wave)를 사용하는 능동 센서로, 카메라와 LiDAR가 성능이 저하되는 **악천후(비, 안개, 눈, 먼지)** 환경에서도 안정적으로 동작한다. 도플러 효과를 이용하여 상대 **속도를 직접 측정**할 수 있다는 점도 다른 센서에는 없는 특성이다.

### 2.5.1 FMCW Radar 원리

자동차/로보틱스에 사용되는 레이더의 대부분은 **FMCW(Frequency Modulated Continuous Wave)** 방식이다. 연속적으로 주파수가 변하는 전파(chirp)를 발사하고, 반사파와의 주파수 차이(beat frequency)로부터 거리와 속도를 추출한다.

**Chirp 신호.** 송신 신호의 순시 주파수가 시간에 따라 선형적으로 증가한다:

$$f_{\text{TX}}(t) = f_0 + \frac{B}{T_c} t$$

여기서 $f_0$은 시작 주파수, $B$는 주파수 대역폭, $T_c$는 chirp 기간이다.

**Beat 주파수.** 거리 $R$에 있는 물체로부터의 반사파는 시간 지연 $\tau = 2R/c$ ($c$: 광속)을 가진다. 송신파와 수신파를 혼합(mixing)하면 beat 주파수가 발생한다:

$$f_b = \frac{2BR}{cT_c}$$

따라서 거리는:

$$R = \frac{f_b \cdot c \cdot T_c}{2B}$$

**거리 분해능.** 두 물체를 구별할 수 있는 최소 거리 차이는 대역폭이 결정한다:

$$\Delta R = \frac{c}{2B}$$

예를 들어, 77GHz 레이더에서 $B = 4\,\text{GHz}$이면 $\Delta R \approx 3.75\,\text{cm}$이다.

**도플러 측정.** 여러 chirp를 연속 발사하여 같은 물체로부터의 위상 변화를 관측하면 시선 방향(radial) 속도를 측정할 수 있다:

$$v_r = \frac{\lambda \cdot f_d}{2}$$

여기서 $f_d$는 도플러 주파수, $\lambda$는 반송파 파장이다. 77GHz 레이더의 $\lambda \approx 3.9\,\text{mm}$이므로, 매우 미세한 속도 변화도 감지 가능하다.

**Range-Doppler Map.** 여러 chirp의 2D FFT를 통해 거리-속도 2D 맵(Range-Doppler Map)을 생성한다. 이 맵의 각 피크가 하나의 반사체에 해당하며, 그 위치에서 거리와 시선 방향 속도를 동시에 읽을 수 있다.

**방위각 측정.** 다수의 수신 안테나(antenna array)로부터 수신 신호의 위상 차이를 이용하여 각도(angle of arrival)를 추정한다. 수평 배열은 수평 각도, 수직 배열은 수직 각도를 제공한다.

### 2.5.2 4D Imaging Radar 최신 동향

전통적인 자동차 레이더는 거리, 속도, 수평 각도의 3차원 정보를 제공하며, 수직 방향 분해능은 매우 낮았다. **4D 이미징 레이더(4D Imaging Radar)**는 거리, 속도(도플러), 수평 각도, **수직 각도**의 4차원 정보를 높은 분해능으로 제공하는 차세대 레이더이다.

4D 이미징 레이더의 핵심은 대규모 가상 안테나 어레이(virtual antenna array)를 MIMO(Multiple Input Multiple Output) 기술로 구현하는 것이다. 예를 들어, 12개 송신 안테나 × 16개 수신 안테나 = 192개 가상 안테나로, 수평/수직 모두에서 충분한 각도 분해능을 달성한다.

**4D Radar vs LiDAR:**

| 특성 | 4D Imaging Radar | LiDAR |
|------|-----------------|-------|
| 악천후 동작 | 우수 | 취약 |
| 포인트 밀도 | 중간 (수천 점/프레임) | 높음 (수십만 점/프레임) |
| 속도 측정 | 직접 측정 (도플러) | 불가 (두 프레임 차분 필요) |
| 각도 분해능 | $\sim 1°$ | $\sim 0.1°$ |
| 비용 | 저~중 | 중~고 |
| 정적 물체 감지 | 제한적 (도플러 = 0) | 우수 |

**센서 퓨전에서의 레이더 활용.** 레이더의 도플러 측정은 센서 퓨전에서 고유한 가치를 제공한다:

1. **자기 속도 추정(Ego-velocity Estimation)**: 정적 환경의 반사체들로부터 측정된 시선 방향 속도를 피팅하여 자체 3D 속도를 추정할 수 있다. 이는 IMU의 가속도 적분보다 직접적이고 드리프트가 없다.

$$v_r^{(i)} = -\mathbf{e}^{(i)\top} \mathbf{v}_{\text{ego}}$$

여기서 $\mathbf{e}^{(i)}$는 $i$번째 반사체 방향의 단위 벡터, $\mathbf{v}_{\text{ego}}$는 자기 속도. 여러 반사체로부터의 관측으로 $\mathbf{v}_{\text{ego}}$를 최소자승법으로 추정한다.

2. **동적 물체 탐지**: 정적 환경에 대한 예측 도플러와 실제 관측의 차이로 이동 물체를 식별한다.
3. **악천후 보완**: 비, 안개에서 카메라와 LiDAR가 실패할 때 레이더가 유일한 외향(exteroceptive) 센서로 기능한다.

```python
import numpy as np

def fmcw_range_from_beat(f_beat, bandwidth, chirp_time, c=3e8):
    """FMCW 레이더의 beat 주파수로부터 거리 계산.
    
    Args:
        f_beat: beat 주파수 [Hz]
        bandwidth: 주파수 대역폭 [Hz]
        chirp_time: chirp 기간 [s]
        c: 광속 [m/s]
    
    Returns:
        range_m: 거리 [m]
    """
    return f_beat * c * chirp_time / (2 * bandwidth)

def estimate_ego_velocity(bearings, doppler_velocities):
    """정적 반사체의 도플러 관측으로부터 자기 속도 추정.
    
    v_doppler_i = -e_i^T @ v_ego  (정적 환경 가정)
    
    Args:
        bearings: (N, 3) 각 반사체 방향 단위 벡터
        doppler_velocities: (N,) 각 반사체의 시선 방향 속도 [m/s]
    
    Returns:
        v_ego: (3,) 추정 자기 속도 [m/s]
    """
    # -E @ v_ego = v_doppler  →  v_ego = -(E^T E)^{-1} E^T v_doppler
    E = bearings  # (N, 3)
    v_ego = -np.linalg.lstsq(E, doppler_velocities, rcond=None)[0]
    return v_ego
```

레이더는 직접적인 속도 측정이라는 다른 센서가 쉽게 줄 수 없는 정보를 제공한다. Ch.8의 멀티 센서 퓨전 아키텍처에서 레이더가 점점 핵심 센서로 자리잡고 있는 것은 이 때문이다.

---

## 2.6 기타 센서

센서 퓨전 시스템에서는 카메라, LiDAR, IMU, GNSS, Radar 외에도 다양한 보조 센서가 활용된다. 이들은 단독으로는 충분한 항법을 제공하지 못하지만, 메인 센서의 한계를 보완하는 데 유용하다.

### 2.6.1 Wheel Odometry (차륜 오도메트리)

차륜 인코더(wheel encoder)는 바퀴의 회전수를 측정하여 이동 거리를 추정하는 가장 기본적인 자기수용(proprioceptive) 센서이다.

**관측 모델.** 차동 구동(differential drive) 로봇의 경우, 좌우 바퀴의 회전 각도 $\Delta \theta_L, \Delta \theta_R$로부터:

$$\Delta s = \frac{r(\Delta \theta_L + \Delta \theta_R)}{2}, \quad \Delta \psi = \frac{r(\Delta \theta_R - \Delta \theta_L)}{d}$$

여기서 $r$은 바퀴 반경, $d$는 좌우 바퀴 간 거리(트레드), $\Delta s$는 전진 거리, $\Delta \psi$는 요 각도 변화이다.

**Ackermann 조향 모델** (자동차):

$$\dot{x} = v \cos\psi, \quad \dot{y} = v \sin\psi, \quad \dot{\psi} = \frac{v \tan\delta}{L}$$

여기서 $v$는 후륜 속도, $\delta$는 조향 각도, $L$은 휠베이스이다.

**슬립(Slip) 모델.** 실제 환경에서는 바퀴가 미끄러진다. 특히 젖은 노면, 비포장, 급가속/급감속 시 슬립이 크다. 슬립 비율(slip ratio)은:

$$s = \frac{v_{\text{wheel}} - v_{\text{actual}}}{v_{\text{actual}}}$$

슬립이 큰 환경에서 차륜 오도메트리의 신뢰도는 급격히 떨어진다. 센서 퓨전에서는 이를 **적응적 관측 공분산(adaptive observation covariance)**으로 처리한다 — 슬립이 감지되면 차륜 오도메트리의 불확실성을 크게 설정하여 다른 센서의 관측이 주도하도록 한다.

**센서 퓨전에서의 역할.** 차륜 오도메트리는 VIO/LIO 시스템에서 추가적인 속도/위치 관측으로 활용된다. 특히 직선 주행 시 단기 정밀도가 높아, 시각 특징점이 부족한 환경(터널, 긴 직선 도로)에서 IMU 드리프트를 보완한다. VINS-Mono의 확장판에서 차륜 오도메트리 팩터를 추가한 연구들이 있다.

### 2.6.2 기압계 (Barometer)

기압계는 대기압을 측정하여 **고도**를 추정하는 센서이다.

**관측 모델.** 국제 표준 대기(ISA)에 따른 기압-고도 관계:

$$h = \frac{T_0}{L}\left(1 - \left(\frac{P}{P_0}\right)^{\frac{RL}{g_0}}\right)$$

여기서:
- $P$: 측정 기압 [Pa]
- $P_0 = 101325\,\text{Pa}$: 해수면 표준 기압
- $T_0 = 288.15\,\text{K}$: 해수면 표준 온도
- $L = 0.0065\,\text{K/m}$: 온도 감률(lapse rate)
- $R = 287.053\,\text{J/(kg·K)}$: 공기의 기체 상수
- $g_0 = 9.80665\,\text{m/s}^2$: 표준 중력 가속도

간략화된 근사식 (저고도):

$$\Delta h \approx -\frac{\Delta P}{\rho g} \approx -\frac{\Delta P}{12.0}\,[\text{m}], \quad (\Delta P\text{는 Pa 단위})$$

해수면 근처에서 약 $8.5\,\text{Pa}$의 기압 변화가 $1\,\text{m}$의 고도 변화에 해당한다.

**노이즈 특성:**
- 단기 정밀도: $\pm 0.1\text{–}0.5\,\text{m}$ (매우 우수)
- 장기 정밀도: $\pm 1\text{–}10\,\text{m}$ (기상 변화에 의한 기압 변동)

**센서 퓨전에서의 역할.** 기압계는 수직 방향(고도)의 관측을 제공한다. 이는 IMU의 수직 드리프트를 억제하고, 특히 드론의 호버링 시 수직 위치 유지에 유용하다. 단, 기상 변화에 의한 장기 드리프트가 있으므로 절대 고도보다는 상대 고도 변화에 활용하는 것이 적절하다. 실내에서는 에어컨이나 문의 개폐에 의한 기압 변화에 주의해야 한다.

### 2.6.3 자력계 (Magnetometer)

자력계는 3축 자기장 벡터를 측정한다. 지구 자기장으로부터 **절대 요(yaw) 방위**를 추출할 수 있다.

**관측 모델.** 자력계 측정값은:

$$\tilde{\mathbf{m}} = \mathbf{R}_{bw} \mathbf{m}_w + \mathbf{b}_{\text{hard}} + \mathbf{S}_{\text{soft}} \mathbf{R}_{bw} \mathbf{m}_w + \mathbf{n}_m$$

여기서:
- $\mathbf{m}_w$: 월드 프레임에서의 지구 자기장 벡터 (크기 약 $25\text{–}65\,\mu\text{T}$, 위치에 따라 다름)
- $\mathbf{b}_{\text{hard}}$: **하드아이언(hard-iron) 바이어스** — 근처의 영구 자석이나 금속에 의한 상수 자기장
- $\mathbf{S}_{\text{soft}}$: **소프트아이언(soft-iron) 왜곡** — 주변 강자성 물질에 의한 자기장의 방향 의존적 왜곡
- $\mathbf{n}_m$: 측정 노이즈

**요 각도 추출.** 롤과 피치가 알려져 있으면 (가속도계로부터), 자력계 측정에서 요 각도를 계산할 수 있다:

$$\psi = \arctan2(m_y \cos\phi - m_z \sin\phi, \, m_x \cos\theta + m_y \sin\theta \sin\phi + m_z \sin\theta \cos\phi)$$

여기서 $\phi$는 롤, $\theta$는 피치, $m_x, m_y, m_z$는 하드아이언 보정된 자력계 측정이다.

**한계와 주의사항:**
- 실내, 차량 내, 건물 근처에서 자기장 왜곡이 매우 크다. 철근 콘크리트 건물 근처에서는 수십 도의 방위 오차가 발생할 수 있다.
- 모터, 전선 등의 전류에 의한 전자기 간섭에 민감하다.
- 센서 퓨전에서 자력계는 보조적 역할에 그치며, 신뢰도가 높을 때만 사용하거나 적응적 가중치를 적용한다.

### 2.6.4 UWB (Ultra-Wideband)

UWB는 매우 넓은 대역폭(500MHz 이상)의 극초단 펄스를 사용하여 노드 간 **거리**를 정밀하게 측정하는 무선 기술이다.

**관측 모델 (TWR — Two-Way Ranging):**

$$d = \frac{c \cdot (t_{\text{round}} - t_{\text{reply}})}{2} + n_d$$

여기서:
- $t_{\text{round}}$: 요청 펄스 전송에서 응답 펄스 수신까지의 시간
- $t_{\text{reply}}$: 응답 노드의 처리 시간
- $n_d$: 거리 노이즈, 일반적으로 $\sigma_d \approx 5\text{–}30\,\text{cm}$ (LOS 환경)

**NLOS(Non-Line-of-Sight) 문제.** UWB는 직접 가시선(LOS)이 확보된 환경에서는 높은 정밀도를 보이지만, 벽이나 장애물을 통과하면(NLOS) 신호가 지연되어 실제보다 먼 거리를 보고한다. NLOS 탐지와 완화는 UWB 기반 측위의 핵심 과제이다.

**센서 퓨전에서의 역할.** UWB 앵커(anchor)를 환경에 미리 설치하면, 각 앵커까지의 거리 측정으로 삼변측량(trilateration)하여 절대 위치를 추정할 수 있다. 이는 실내에서 GNSS를 대체하는 역할을 하며, VIO와 결합하면 장기 드리프트를 보정할 수 있다.

**관측 방정식 (삼변측량):**

$$d_i = \|\mathbf{p} - \mathbf{a}_i\| + n_{d,i}, \quad i = 1, \ldots, N_a$$

여기서 $\mathbf{p}$는 미지의 태그 위치, $\mathbf{a}_i$는 $i$번째 앵커의 알려진 위치이다. 3개 이상의 앵커로 2D 위치를, 4개 이상으로 3D 위치를 추정할 수 있다.

```python
import numpy as np

def uwb_trilateration(anchor_positions, ranges):
    """UWB 삼변측량 (최소자승법).
    
    Args:
        anchor_positions: (N, 3) 앵커 위치 [m]
        ranges: (N,) 각 앵커까지의 측정 거리 [m]
    
    Returns:
        position: (3,) 추정 위치 [m]
    """
    N = len(ranges)
    # 선형화: 첫 앵커를 기준으로 차분
    # ||p - a_i||^2 - ||p - a_0||^2 = d_i^2 - d_0^2
    # 2(a_0 - a_i)^T p = d_i^2 - d_0^2 - ||a_i||^2 + ||a_0||^2
    
    A = np.zeros((N - 1, 3))
    b = np.zeros(N - 1)
    
    a0 = anchor_positions[0]
    d0 = ranges[0]
    
    for i in range(1, N):
        ai = anchor_positions[i]
        di = ranges[i]
        A[i - 1] = 2 * (a0 - ai)
        b[i - 1] = di**2 - d0**2 - np.dot(ai, ai) + np.dot(a0, a0)
    
    # 최소자승 해
    position = np.linalg.lstsq(A, b, rcond=None)[0]
    return position
```

---

## 2.7 센서 모델링 요약

이 챕터에서 다룬 각 센서의 관측 모델과 핵심 특성을 정리한다.

| 센서 | 관측량 | 관측 모델 | 주요 노이즈원 | 전형적 노이즈 수준 |
|------|--------|----------|-------------|------------------|
| 카메라 | 2D 이미지 좌표 | $\pi(\mathbf{T} \cdot \mathbf{P})$ (핀홀 + 왜곡) | 검출 노이즈, 왜곡 잔차 | $0.5\text{–}2$ 픽셀 |
| LiDAR | 3D 점 $(r, \alpha, \omega)$ | Range-bearing | 거리 노이즈, 빔 확산, 혼합 픽셀 | $1\text{–}5\,\text{cm}$ |
| IMU (자이로) | 각속도 $\boldsymbol{\omega}$ | $\tilde{\boldsymbol{\omega}} = \boldsymbol{\omega} + \mathbf{b}_g + \mathbf{n}_g$ | 바이어스, 랜덤 워크 | ARW $\sim 0.1\text{–}1°/\sqrt{\text{hr}}$ |
| IMU (가속도) | 비력 $\mathbf{a}$ | $\tilde{\mathbf{a}} = \mathbf{R}(\mathbf{a}-\mathbf{g}) + \mathbf{b}_a + \mathbf{n}_a$ | 바이어스, 랜덤 워크 | VRW $\sim 0.02\text{–}0.2\,\text{m/s}/\sqrt{\text{hr}}$ |
| GNSS | 의사거리 $\rho$ | $\rho = r + c\delta t_r + I + T + \epsilon$ | 다중경로, 전리층, 대류권 | $1\text{–}5\,\text{m}$ (SPP) |
| Radar | Range, Doppler, 방위 | FMCW beat frequency | 클러터, 다중경로 | $\Delta R \sim 4\,\text{cm}$, $v \sim 0.1\,\text{m/s}$ |
| Wheel Odom. | 회전수 | $\Delta s = r \Delta\theta$ | 슬립 | $1\text{–}5\%$ 이동 거리 |
| 기압계 | 기압 $P$ | $h = f(P)$ | 기상 변화 | $0.1\text{–}0.5\,\text{m}$ (단기) |
| 자력계 | 자기장 $\mathbf{m}$ | $\tilde{\mathbf{m}} = \mathbf{R}\mathbf{m}_w + \mathbf{b} + \mathbf{n}$ | 하드/소프트 아이언 | $1\text{–}5°$ (캘리브 후) |
| UWB | 거리 $d$ | $d = \|\mathbf{p} - \mathbf{a}\| + n$ | NLOS | $5\text{–}30\,\text{cm}$ (LOS) |

관측 모델은 센서 퓨전의 첫걸음일 뿐이다. 모델이 아무리 정확해도, 센서 간의 상대 위치와 시간 동기가 어긋나면 퓨전 성능이 무너진다. 다음 챕터는 그래서 **캘리브레이션(Calibration)**, 즉 센서 간의 기하·시간 관계를 결정하는 과정을 다룬다.

---

# Ch.3 — Calibration Deep Dive

Ch.2에서 각 센서의 관측 모델을 수학적으로 정의했다. 그런데 이 모델을 실제 센서 데이터에 적용하려면 한 가지 전제가 필요하다. 모델의 파라미터가 정확히 알려져 있어야 한다. 카메라의 초점 거리, LiDAR와 IMU 사이의 상대 위치, 센서 간 시간 오프셋. 이 값을 정밀하게 결정하는 과정이 캘리브레이션이다.

> 센서 퓨전의 정확도는 캘리브레이션의 정확도를 넘지 못한다. 이 챕터는 카메라 내부 파라미터부터 다중 센서 간 외부 파라미터, 시간 동기화까지 캘리브레이션의 모든 측면을 다룬다.

캘리브레이션(calibration)은 센서 퓨전 파이프라인에서 가장 먼저 해결해야 하는 문제다. 아무리 정교한 상태 추정 알고리즘을 사용하더라도, 센서의 내부 모델이 부정확하거나 센서 간 상대 위치·자세가 잘못되어 있으면 퓨전 결과는 발산한다. LiDAR-카메라 퓨전에서 외부 파라미터가 1도만 틀어져도, 50m 거리의 물체에서 약 87cm의 정합 오차가 난다. 이 챕터는 각 캘리브레이션 문제의 수학적 기초를 유도하고, 실전에서 바로 쓰는 코드와 도구를 함께 제공한다.

---

## 3.1 Camera Intrinsic Calibration

카메라 내부 캘리브레이션(intrinsic calibration)은 3D 세계의 점이 2D 이미지 위 어디에 투영되는지를 결정하는 파라미터를 추정하는 과정이다. 이 파라미터에는 초점 거리(focal length), 주점(principal point), 그리고 렌즈 왜곡 계수(distortion coefficients)가 포함된다.

### 3.1.1 핀홀 카메라 모델 복습

3D 세계 좌표 $\mathbf{P}_w = [X, Y, Z]^\top$이 카메라 좌표계에서 $\mathbf{P}_c = [X_c, Y_c, Z_c]^\top$로 변환된 후, 이미지 평면에 투영되는 과정은 다음과 같다:

$$
s \begin{bmatrix} u \\ v \\ 1 \end{bmatrix}
= \mathbf{K} \begin{bmatrix} X_c \\ Y_c \\ Z_c \end{bmatrix}
= \begin{bmatrix} f_x & \gamma & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} X_c \\ Y_c \\ Z_c \end{bmatrix}
$$

여기서:
- $f_x, f_y$: 픽셀 단위의 초점 거리 (focal length). $f_x = f / p_x$이며 $f$는 물리적 초점 거리(mm), $p_x$는 픽셀 크기(mm/pixel).
- $(c_x, c_y)$: 주점. 이미지 센서의 중심과 광축(optical axis)의 교점.
- $\gamma$: 비대칭 계수(skew coefficient). 현대 카메라에서는 거의 0.
- $s = Z_c$: 스케일 팩터 (깊이값).

$\mathbf{K}$를 **카메라 내부 행렬(intrinsic matrix)** 또는 **캘리브레이션 행렬**이라 부른다. 이 행렬은 5개의 자유도를 가진다($f_x, f_y, c_x, c_y, \gamma$). 실전에서는 $\gamma = 0$으로 두어 4개로 줄이는 경우가 많다.

### 3.1.2 렌즈 왜곡 모델

실제 렌즈는 핀홀 모델의 이상적인 직선 투영을 따르지 않는다. 왜곡은 크게 방사 왜곡(radial distortion)과 접선 왜곡(tangential distortion)으로 나뉜다.

**정규화된 좌표**(normalized coordinates)를 먼저 정의한다:

$$
x = X_c / Z_c, \quad y = Y_c / Z_c, \quad r^2 = x^2 + y^2
$$

**방사 왜곡** (radial distortion):
$$
x_{\text{radial}} = x(1 + k_1 r^2 + k_2 r^4 + k_3 r^6)
$$
$$
y_{\text{radial}} = y(1 + k_1 r^2 + k_2 r^4 + k_3 r^6)
$$

배럴(barrel) 왜곡은 $k_1 < 0$, 핀쿠션(pincushion) 왜곡은 $k_1 > 0$일 때 나타난다. 대부분의 렌즈는 $k_1, k_2$만으로 충분히 모델링된다.

**접선 왜곡** (tangential distortion):
$$
x_{\text{tangential}} = 2p_1 xy + p_2(r^2 + 2x^2)
$$
$$
y_{\text{tangential}} = p_1(r^2 + 2y^2) + 2p_2 xy
$$

렌즈 요소가 이미지 센서 평면과 완벽히 평행하지 않을 때 발생한다.

**왜곡이 적용된 최종 좌표**:
$$
x_d = x_{\text{radial}} + x_{\text{tangential}}, \quad y_d = y_{\text{radial}} + y_{\text{tangential}}
$$

$$
u = f_x \cdot x_d + c_x, \quad v = f_y \cdot y_d + c_y
$$

왜곡 파라미터 벡터는 $\mathbf{d} = [k_1, k_2, p_1, p_2, k_3]$로 표현하며, OpenCV의 `calibrateCamera()`가 이 순서를 따른다.

### 3.1.3 Zhang's Method: 호모그래피 기반 캘리브레이션

[Zhang (2000)](https://ieeexplore.ieee.org/document/888718)이 제안한 방법은 평면 패턴(체커보드)을 다양한 자세로 촬영한 이미지로부터 카메라 파라미터를 추정한다. 3D 캘리브레이션 장비 없이 프린터로 출력한 체커보드만 있으면 충분해서, OpenCV의 `calibrateCamera()`도 이 알고리즘을 그대로 구현하고 있다.

패턴이 $Z = 0$ 평면에 놓이면 3D-2D 투영이 호모그래피로 단순화된다는 것이 핵심 아이디어다.

#### Step 1: 호모그래피 추출

체커보드가 $Z = 0$ 평면에 있으므로, 세계 좌표 $\mathbf{M} = [X, Y, 0]^\top$과 이미지 좌표 $\mathbf{m} = [u, v]^\top$의 관계를 동차(homogeneous) 좌표로 쓰면:

$$
s \tilde{\mathbf{m}} = \mathbf{K} [\mathbf{r}_1 \quad \mathbf{r}_2 \quad \mathbf{r}_3 \quad \mathbf{t}] \begin{bmatrix} X \\ Y \\ 0 \\ 1 \end{bmatrix}
= \mathbf{K} [\mathbf{r}_1 \quad \mathbf{r}_2 \quad \mathbf{t}] \begin{bmatrix} X \\ Y \\ 1 \end{bmatrix}
$$

$Z = 0$이므로 $\mathbf{r}_3$ 열이 사라진다. 이로부터:

$$
s \tilde{\mathbf{m}} = \mathbf{H} \tilde{\mathbf{M}}, \quad \mathbf{H} = \mathbf{K} [\mathbf{r}_1 \quad \mathbf{r}_2 \quad \mathbf{t}]
$$

$\mathbf{H}$는 $3 \times 3$ 호모그래피 행렬이다. 각 이미지에서 최소 4개의 대응점으로 $\mathbf{H}$를 DLT(Direct Linear Transform)로 추정할 수 있다. 실전에서는 체커보드 코너가 수십 개이므로 과결정(over-determined) 시스템을 SVD로 풀고, RANSAC으로 아웃라이어를 제거한다.

**DLT를 이용한 호모그래피 추정**: 대응점 $(\mathbf{M}_j, \mathbf{m}_j)$에서 $\tilde{\mathbf{m}}_j \times \mathbf{H} \tilde{\mathbf{M}}_j = \mathbf{0}$으로부터 다음 선형 시스템을 얻는다:

$$
\begin{bmatrix}
\tilde{\mathbf{M}}_j^\top & \mathbf{0}^\top & -u_j \tilde{\mathbf{M}}_j^\top \\
\mathbf{0}^\top & \tilde{\mathbf{M}}_j^\top & -v_j \tilde{\mathbf{M}}_j^\top
\end{bmatrix}
\mathbf{h} = \mathbf{0}
$$

여기서 $\mathbf{h}$는 $\mathbf{H}$의 9개 원소를 벡터로 편 것이다. $n$개 대응점에서 $2n \times 9$ 행렬 $\mathbf{A}$를 구성하고, $\|\mathbf{A}\mathbf{h}\|$를 최소화하는 $\mathbf{h}$를 $\mathbf{A}$의 SVD에서 가장 작은 singular value에 대응하는 right singular vector로 구한다.

#### Step 2: 내부 파라미터의 제약 조건 도출

$\mathbf{H} = [\mathbf{h}_1 \quad \mathbf{h}_2 \quad \mathbf{h}_3]$로 열을 나누면:

$$
[\mathbf{h}_1 \quad \mathbf{h}_2 \quad \mathbf{h}_3] = \lambda \mathbf{K} [\mathbf{r}_1 \quad \mathbf{r}_2 \quad \mathbf{t}]
$$

여기서 $\lambda$는 임의의 스케일 팩터이다. 회전 행렬의 직교성에서 두 가지 제약 조건을 얻는다:

1. **직교 조건**: $\mathbf{r}_1^\top \mathbf{r}_2 = 0$
   $$\mathbf{h}_1^\top \mathbf{K}^{-\top} \mathbf{K}^{-1} \mathbf{h}_2 = 0$$

2. **등장 조건**: $\|\mathbf{r}_1\| = \|\mathbf{r}_2\|$
   $$\mathbf{h}_1^\top \mathbf{K}^{-\top} \mathbf{K}^{-1} \mathbf{h}_1 = \mathbf{h}_2^\top \mathbf{K}^{-\top} \mathbf{K}^{-1} \mathbf{h}_2$$

$\mathbf{B} = \mathbf{K}^{-\top} \mathbf{K}^{-1}$로 정의하자. $\mathbf{B}$는 대칭 양의 정치(positive definite) 행렬이므로 6개의 독립 원소를 가진다:

$$
\mathbf{B} = \begin{bmatrix} B_{11} & B_{12} & B_{13} \\ B_{12} & B_{22} & B_{23} \\ B_{13} & B_{23} & B_{33} \end{bmatrix}
$$

이를 벡터로 쓰면 $\mathbf{b} = [B_{11}, B_{12}, B_{22}, B_{13}, B_{23}, B_{33}]^\top$이다.

#### Step 3: 선형 시스템 구성

$\mathbf{h}_i^\top \mathbf{B} \mathbf{h}_j$를 $\mathbf{b}$에 대한 내적으로 표현할 수 있다:

$$
\mathbf{h}_i^\top \mathbf{B} \mathbf{h}_j = \mathbf{v}_{ij}^\top \mathbf{b}
$$

여기서:
$$
\mathbf{v}_{ij} = \begin{bmatrix} h_{1i}h_{1j} \\ h_{1i}h_{2j} + h_{2i}h_{1j} \\ h_{2i}h_{2j} \\ h_{3i}h_{1j} + h_{1i}h_{3j} \\ h_{3i}h_{2j} + h_{2i}h_{3j} \\ h_{3i}h_{3j} \end{bmatrix}
$$

각 이미지에서 두 가지 제약으로부터:

$$
\begin{bmatrix} \mathbf{v}_{12}^\top \\ (\mathbf{v}_{11} - \mathbf{v}_{22})^\top \end{bmatrix} \mathbf{b} = \mathbf{0}
$$

$n$장의 이미지가 있으면 $2n \times 6$ 시스템을 얻는다.

**최소 이미지 수**: $\gamma = 0$으로 두면 ($B_{12} = 0$ 제약 추가) 5개 미지수에 대해 최소 3장. 일반적인 5-파라미터 모델은 최소 3장이 필요하다. 실전에서는 15~25장을 촬영한다.

#### Step 4: K 복원

$\mathbf{b}$를 풀면 $\mathbf{B} = \mathbf{K}^{-\top} \mathbf{K}^{-1}$을 복원할 수 있고, Cholesky 분해 $\mathbf{B} = \mathbf{L}\mathbf{L}^\top$에서 $\mathbf{K}^{-1} = \mathbf{L}^\top$로 $\mathbf{K}$를 구한다. 구체적으로:

$$
v_0 = (B_{12}B_{13} - B_{11}B_{23}) / (B_{11}B_{22} - B_{12}^2)
$$
$$
\lambda = B_{33} - [B_{13}^2 + v_0(B_{12}B_{13} - B_{11}B_{23})] / B_{11}
$$
$$
f_x = \sqrt{\lambda / B_{11}}
$$
$$
f_y = \sqrt{\lambda B_{11} / (B_{11}B_{22} - B_{12}^2)}
$$
$$
\gamma = -B_{12} f_x^2 f_y / \lambda
$$
$$
c_x = \gamma v_0 / f_y - B_{13} f_x^2 / \lambda
$$
$$
c_y = v_0
$$

#### Step 5: 외부 파라미터 계산

$\mathbf{K}$를 알면 각 이미지 $i$에 대해:

$$
\mathbf{r}_1 = \lambda \mathbf{K}^{-1} \mathbf{h}_1, \quad \mathbf{r}_2 = \lambda \mathbf{K}^{-1} \mathbf{h}_2, \quad \mathbf{t} = \lambda \mathbf{K}^{-1} \mathbf{h}_3
$$

여기서 $\lambda = 1 / \|\mathbf{K}^{-1} \mathbf{h}_1\|$이다. $\mathbf{r}_3 = \mathbf{r}_1 \times \mathbf{r}_2$로 구한다. 추정된 $[\mathbf{r}_1, \mathbf{r}_2, \mathbf{r}_3]$는 노이즈로 인해 완벽한 회전 행렬이 아니므로, SVD를 이용하여 가장 가까운 회전 행렬로 투영한다: $\mathbf{R} = \mathbf{U}\mathbf{V}^\top$ (단, $\det(\mathbf{R}) = 1$).

#### Step 6: 비선형 최적화 (MLE)

선형 해는 좋은 초기값을 제공하지만, 왜곡 파라미터를 고려하지 않으며 노이즈에 최적이지 않다. 최종 단계에서 Levenberg-Marquardt (LM) 알고리즘으로 **재투영 오차(reprojection error)**를 최소화한다:

$$
\min_{\mathbf{K}, \mathbf{d}, \{\mathbf{R}_i, \mathbf{t}_i\}} \sum_{i=1}^{n}\sum_{j=1}^{m} \|\mathbf{m}_{ij} - \hat{\mathbf{m}}(\mathbf{K}, \mathbf{d}, \mathbf{R}_i, \mathbf{t}_i, \mathbf{M}_j)\|^2
$$

여기서 $n$은 이미지 수, $m$은 각 이미지의 코너 수, $\hat{\mathbf{m}}(\cdot)$은 왜곡을 포함한 전체 투영 함수이다.

이 최적화의 자코비안은 각 파라미터에 대한 투영 함수의 편미분으로 구성된다. OpenCV의 `calibrateCamera()`는 이 전체 파이프라인(DLT → 선형 K 추정 → LM 최적화)을 구현한다.

### 3.1.4 OpenCV를 이용한 캘리브레이션 코드

```python
import numpy as np
import cv2
import glob

# 체커보드 설정
CHECKERBOARD = (9, 6)  # 내부 코너 수 (columns, rows)
SQUARE_SIZE = 0.025    # 25mm 정사각형

# 3D 월드 좌표 생성 (Z=0 평면)
objp = np.zeros((CHECKERBOARD[0] * CHECKERBOARD[1], 3), np.float32)
objp[:, :2] = np.mgrid[0:CHECKERBOARD[0], 0:CHECKERBOARD[1]].T.reshape(-1, 2)
objp *= SQUARE_SIZE

obj_points = []  # 3D 점 (모든 이미지에서 동일)
img_points = []  # 2D 점 (이미지마다 다름)

images = sorted(glob.glob("calibration_images/*.jpg"))

for fname in images:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    
    # 코너 검출
    ret, corners = cv2.findChessboardCorners(
        gray, CHECKERBOARD,
        cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_FAST_CHECK + cv2.CALIB_CB_NORMALIZE_IMAGE
    )
    
    if ret:
        # 서브픽셀 정밀도로 코너 위치 정제
        criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
        corners_refined = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
        
        obj_points.append(objp)
        img_points.append(corners_refined)

# 캘리브레이션 수행 (Zhang's method)
ret, K, dist, rvecs, tvecs = cv2.calibrateCamera(
    obj_points, img_points, gray.shape[::-1], None, None
)

print(f"RMS reprojection error: {ret:.4f} pixels")
print(f"Camera matrix K:\n{K}")
print(f"Distortion coefficients: {dist.ravel()}")

# 재투영 오차 분석
errors = []
for i in range(len(obj_points)):
    img_points_proj, _ = cv2.projectPoints(obj_points[i], rvecs[i], tvecs[i], K, dist)
    error = cv2.norm(img_points[i], img_points_proj, cv2.NORM_L2) / len(img_points_proj)
    errors.append(error)
    
print(f"Per-image mean error: {np.mean(errors):.4f} pixels")
print(f"Max error image: {np.argmax(errors)} ({max(errors):.4f} px)")
```

### 3.1.5 실전 팁: 좋은 캘리브레이션을 위한 조건

캘리브레이션의 품질은 데이터 수집 과정에서 결정된다. Zhang(2000)과 OpenCV 공식 문서, 그리고 현장 경험이 공통으로 강조하는 조건들은 다음과 같다.

**포즈 다양성 (Pose Diversity)**: 가장 중요한 요소. 체커보드를 다양한 각도와 위치에서 촬영해야 한다. 구체적으로:
- 체커보드를 이미지의 상하좌우, 중앙 모든 영역에 배치 (주점 추정에 필수)
- 보드를 45도 이상 기울여서 촬영 (초점 거리 추정의 정밀도 향상)
- 보드를 카메라에 가깝게/멀리 배치하여 다양한 스케일 확보
- 최소 15~25장, 이상적으로는 50장 이상

**코너 정확도**: `cv2.cornerSubPix()`로 서브픽셀 정밀도를 반드시 확보한다. 코너 검출 실패 이미지는 과감히 제외한다.

**조명 조건**: 균일한 조명이 이상적이지만, 약간의 그림자는 코너 검출에 영향을 주지 않는다. 반사(glare)는 코너 검출을 방해하므로 무광(matte) 종이에 인쇄한 체커보드를 사용한다.

**재투영 오차 기준**: 
- $< 0.3$ pixels: 우수
- $0.3 - 0.5$ pixels: 양호
- $0.5 - 1.0$ pixels: 재수집 고려
- $> 1.0$ pixels: 문제 있음 (포즈 다양성 부족, 코너 검출 오류 등)

**이상치 이미지 감지**: per-image 재투영 오차를 계산하여 평균보다 2~3배 큰 이미지는 제거 후 재캘리브레이션한다. 위 코드의 `errors` 배열로 확인할 수 있다.

**경고 — 정사각형 크기의 정확성**: `SQUARE_SIZE`는 실제 인쇄된 체커보드의 정사각형 크기와 정확히 일치해야 한다. 프린터 스케일링으로 인해 실제 크기가 지정 크기와 다를 수 있으므로, 반드시 자로 측정한다. 이 값이 틀리면 `tvecs` (이동 벡터)가 잘못되지만, `K`와 왜곡 계수에는 영향을 주지 않는다 (왜곡 모델은 정규화 좌표에서 정의되므로).

### 3.1.6 Fisheye / Omnidirectional 캘리브레이션

FoV(Field of View)가 180도 이상인 어안 렌즈(fisheye lens)에서는 표준 radial-tangential 왜곡 모델이 작동하지 않는다. 왜곡이 너무 극심하여 다항식 근사가 수렴하지 않기 때문이다.

**등거리 투영 모델 (Equidistant Projection Model)**:

[Kannala & Brandt (2006)](https://ieeexplore.ieee.org/document/1642666)가 제안한 일반 카메라 모델은 입사각(incidence angle) $\theta$와 이미지 거리 $r$의 관계를 다항식으로 표현한다:

$$
r(\theta) = k_1 \theta + k_2 \theta^3 + k_3 \theta^5 + k_4 \theta^7 + k_5 \theta^9
$$

여기서 $\theta = \arctan\left(\sqrt{x^2 + y^2}\right)$는 입사각, $x, y$는 정규화 좌표이다. 이상적인 등거리 투영은 $r = f\theta$이며, 다항식의 추가 항이 실제 렌즈의 편차를 보정한다.

OpenCV는 `cv2.fisheye` 모듈에서 이 모델의 변형을 구현한다:

$$
\theta_d = \theta(1 + k_1\theta^2 + k_2\theta^4 + k_3\theta^6 + k_4\theta^8)
$$

```python
# Fisheye 캘리브레이션 (OpenCV fisheye 모듈)
import cv2
import numpy as np

calibration_flags = (
    cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC
    + cv2.fisheye.CALIB_CHECK_COND
    + cv2.fisheye.CALIB_FIX_SKEW
)

K_fisheye = np.zeros((3, 3))
D_fisheye = np.zeros((4, 1))  # k1, k2, k3, k4

ret, K_fisheye, D_fisheye, rvecs, tvecs = cv2.fisheye.calibrate(
    obj_points,
    img_points,
    gray.shape[::-1],
    K_fisheye,
    D_fisheye,
    None, None,
    calibration_flags,
    (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 1e-6)
)

print(f"Fisheye RMS error: {ret:.4f}")
print(f"K:\n{K_fisheye}")
print(f"D: {D_fisheye.ravel()}")
```

**Scaramuzza OCamCalib (Omnidirectional Camera Calibration)**:

[Scaramuzza et al. (2006)](https://rpg.ifi.uzh.ch/docs/IROS06_scaramuzza.pdf)의 OCamCalib은 catadioptric 시스템(거울 + 렌즈)과 초광각 어안 렌즈를 위한 통합 캘리브레이션 도구이다. 투영 함수를 다항식으로 직접 모델링하여 센서 타입에 독립적이다:

$$
\begin{bmatrix} u \\ v \end{bmatrix} = \begin{bmatrix} x_c \\ y_c \end{bmatrix} + \mathbf{A} \cdot \rho(\theta) \begin{bmatrix} \cos(\phi) \\ \sin(\phi) \end{bmatrix}
$$

여기서 $\mathbf{A}$는 affine 변환 행렬 (stretch와 non-square pixel 보정), $\rho(\theta)$는 입사각에 따른 이미지 반경이다.

### 3.1.7 Kalibr를 이용한 카메라 캘리브레이션

[Kalibr](https://github.com/ethz-asl/kalibr)는 ETH Zurich에서 개발한 캘리브레이션 도구로, 카메라-IMU 캘리브레이션(3.4절)에서 가장 광범위하게 사용되는 오픈소스 도구다. B-spline 궤적 모델과 batch 최적화 덕분에 카메라 intrinsic 캘리브레이션에서도 OpenCV의 단순 LM 최적화보다 정밀한 결과를 낸다.

**Kalibr의 카메라 모델 지원**:

| 모델 | 파라미터 수 | 적합한 렌즈 |
|------|-----------|-----------|
| `pinhole-radtan` | 4 + 4 | 일반 렌즈 (OpenCV와 동일) |
| `pinhole-equi` | 4 + 4 | 어안 렌즈 |
| `omni-radtan` | 5 + 4 | 초광각/catadioptric |
| `ds` (Double Sphere) | 6 | 광각 렌즈 (최신 모델) |
| `eucm` (Extended UCM) | 6 | 광각 렌즈 |

**AprilGrid vs Checkerboard**: Kalibr는 AprilGrid 타겟을 권장한다. AprilGrid는 각 태그에 고유 ID가 인코딩되어 있어 부분 가려짐(partial occlusion)에서도 코너 검출이 가능하고, 코너 순서를 자동으로 식별한다.

**Kalibr 실행 예시** (카메라 intrinsic만):
```bash
# AprilGrid 타겟 파일 생성
kalibr_create_target_pdf --type apriltag \
    --nx 6 --ny 6 --tsize 0.024 --tspace 0.3

# 카메라 캘리브레이션
kalibr_calibrate_cameras \
    --target april_6x6_24x24mm.yaml \
    --bag camera_calibration.bag \
    --models pinhole-radtan \
    --topics /cam0/image_raw
```

Kalibr의 내부 동작은 다음과 같다:
1. AprilGrid의 코너를 각 프레임에서 검출
2. 코너 대응으로부터 호모그래피 추정 (Zhang's method의 Step 1과 유사)
3. 카메라 모델의 전체 파라미터에 대해 batch 최적화 수행
4. 최적화 결과와 잔차(residuals) 분포를 시각화

Kalibr가 OpenCV 기본 캘리브레이션보다 유리한 점은:
- **B-spline 궤적 표현**: 연속 시간 모델로 모션 블러 효과를 자연스럽게 처리
- **다양한 카메라 모델**: DS, EUCM 등 최신 모델 지원
- **멀티카메라**: 여러 카메라의 상대 포즈를 동시에 추정 가능
- **IMU 연동**: 3.4절에서 다룰 camera-IMU 캘리브레이션과 자연스럽게 연결

---

## 3.2 Camera-Camera (Stereo) Extrinsic

스테레오 카메라 시스템에서 두 카메라 간의 상대 포즈(extrinsic)를 캘리브레이션하는 것은 깊이 추정의 전제 조건이다.

### 3.2.1 스테레오 캘리브레이션

두 카메라 $C_L$(왼쪽)과 $C_R$(오른쪽)이 있을 때, 같은 3D 점 $\mathbf{P}$에 대해:

$$
\mathbf{P}_{C_R} = \mathbf{R} \cdot \mathbf{P}_{C_L} + \mathbf{t}
$$

여기서 $(\mathbf{R}, \mathbf{t})$는 왼쪽 카메라 좌표계에서 오른쪽 카메라 좌표계로의 변환이다. 이를 추정하기 위해 두 카메라로 동일한 체커보드를 동시에 촬영한다.

OpenCV의 `cv2.stereoCalibrate()`는 두 카메라의 intrinsic을 고정하거나 동시에 정제하면서 $(\mathbf{R}, \mathbf{t})$를 추정한다:

```python
# 두 카메라의 intrinsic은 이미 캘리브레이션 되었다고 가정
# K_L, dist_L, K_R, dist_R: 각 카메라의 내부 파라미터

flags = cv2.CALIB_FIX_INTRINSIC  # intrinsic 고정

ret, K_L, dist_L, K_R, dist_R, R, t, E, F = cv2.stereoCalibrate(
    obj_points,
    img_points_left,
    img_points_right,
    K_L, dist_L,
    K_R, dist_R,
    gray.shape[::-1],
    flags=flags,
    criteria=(cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 1e-6)
)

print(f"Stereo RMS error: {ret:.4f} pixels")
print(f"Baseline: {np.linalg.norm(t):.4f} m")
print(f"R:\n{R}")
print(f"t: {t.ravel()}")
```

### 3.2.2 Stereo Rectification

캘리브레이션 후, 두 카메라의 이미지를 **평행 정렬(rectification)**하여 epipolar 라인이 수평이 되도록 변환한다. 정렬 후에는 대응점 탐색이 1D 문제(같은 행에서 탐색)로 축소되어 스테레오 매칭이 크게 효율화된다.

**Epipolar Geometry 복습**:

두 카메라의 투영 중심 $O_L, O_R$과 3D 점 $\mathbf{P}$가 이루는 평면을 epipolar plane이라 한다. 이 평면이 각 이미지 평면과 만나는 선이 epipolar line이다. 왼쪽 이미지의 점 $\mathbf{m}_L$에 대응하는 오른쪽 이미지의 점 $\mathbf{m}_R$은 반드시 대응하는 epipolar line 위에 존재한다.

이 관계를 수식으로 표현하면:

$$
\tilde{\mathbf{m}}_R^\top \mathbf{F} \tilde{\mathbf{m}}_L = 0
$$

여기서 $\mathbf{F}$는 **기본 행렬(Fundamental matrix)**이다. 캘리브레이션된 카메라에서는 **본질 행렬(Essential matrix)** $\mathbf{E}$를 사용한다:

$$
\hat{\mathbf{m}}_R^\top \mathbf{E} \hat{\mathbf{m}}_L = 0, \quad \mathbf{E} = [\mathbf{t}]_\times \mathbf{R}
$$

여기서 $\hat{\mathbf{m}} = \mathbf{K}^{-1}\tilde{\mathbf{m}}$는 정규화 좌표이고, $[\mathbf{t}]_\times$는 이동 벡터의 반대칭 행렬(skew-symmetric matrix)이다.

**Rectification 과정**:

Rectification은 두 카메라의 이미지 평면을 가상의 공통 평면으로 변환하는 호모그래피를 찾는 것이다. 이 공통 평면은 두 카메라의 기선(baseline) 벡터와 평행하도록 설정한다.

OpenCV의 `cv2.stereoRectify()`는 Bouguet의 알고리즘을 사용한다:

```python
# Rectification 매핑 계산
R_L, R_R, P_L, P_R, Q, roi_L, roi_R = cv2.stereoRectify(
    K_L, dist_L, K_R, dist_R,
    gray.shape[::-1],
    R, t,
    alpha=0  # 0: 유효 픽셀만 보존, 1: 모든 원본 픽셀 보존
)

# Undistort + Rectify 매핑 생성
map_Lx, map_Ly = cv2.initUndistortRectifyMap(
    K_L, dist_L, R_L, P_L, gray.shape[::-1], cv2.CV_32FC1
)
map_Rx, map_Ry = cv2.initUndistortRectifyMap(
    K_R, dist_R, R_R, P_R, gray.shape[::-1], cv2.CV_32FC1
)

# 이미지 정렬
img_L_rect = cv2.remap(img_L, map_Lx, map_Ly, cv2.INTER_LINEAR)
img_R_rect = cv2.remap(img_R, map_Rx, map_Ry, cv2.INTER_LINEAR)
```

**Epipolar 제약 기반 검증**: 정렬이 올바른지 확인하는 가장 직관적인 방법은 정렬된 좌우 이미지에 수평선을 그어보는 것이다. 대응점이 같은 수평선 위에 있으면 정렬이 정확한 것이다.

```python
# epipolar line 검증 시각화
def draw_epilines(img_L_rect, img_R_rect, num_lines=20):
    h, w = img_L_rect.shape[:2]
    canvas = np.hstack([img_L_rect, img_R_rect])
    for y in np.linspace(0, h-1, num_lines, dtype=int):
        color = tuple(np.random.randint(0, 255, 3).tolist())
        cv2.line(canvas, (0, y), (2*w, y), color, 1)
    return canvas
```

**디스패리티에서 깊이로**: 정렬된 스테레오 이미지에서 좌우 대응점의 수평 변위 $d = u_L - u_R$를 디스패리티(disparity)라 한다. 깊이는:

$$
Z = \frac{f \cdot B}{d}
$$

여기서 $f$는 초점 거리(픽셀 단위), $B = \|\mathbf{t}\|$는 기선 길이(baseline)이다. $Q$ 행렬을 이용하면 디스패리티 맵에서 직접 3D 점군을 계산할 수 있다: $\mathbf{P}_{3D} = Q \cdot [u, v, d, 1]^\top$.

---

## 3.3 Camera-LiDAR Extrinsic Calibration

카메라와 LiDAR 간의 외부 파라미터 $(\mathbf{R}, \mathbf{t})$를 추정하는 것은 멀티모달 센서 퓨전의 핵심 전제 조건이다. LiDAR 점군을 이미지에 투영하거나, 이미지 특징을 3D 공간에 배치하려면 이 변환이 정확해야 한다.

### 3.3.1 Target-based 캘리브레이션

가장 전통적인 접근법으로, 알려진 기하학적 타겟(체커보드, AprilTag 등)을 카메라와 LiDAR가 동시에 관측하여 대응점을 만든다.

**원리**: 체커보드의 코너는 카메라 이미지에서 2D 점으로, LiDAR 점군에서 3D 평면으로 관측된다. 체커보드 평면에 맞는 LiDAR 점들을 추출하고, 평면의 법선과 경계를 이용하여 3D-2D 대응을 구축한다.

**3D-2D 대응 기반 방법**:

1. 카메라 이미지에서 체커보드 코너 검출 → 2D 점 $\{\mathbf{m}_j\}$
2. LiDAR 점군에서 체커보드 평면을 RANSAC으로 피팅 → 평면 방정식 $\mathbf{n}^\top \mathbf{p} + d = 0$
3. 평면 위의 점들에서 체커보드 경계를 추출하여 코너의 3D 좌표 $\{\mathbf{P}_j\}$ 추정
4. 3D-2D 대응 $\{(\mathbf{P}_j, \mathbf{m}_j)\}$으로부터 PnP(Perspective-n-Point) 알고리즘으로 $(\mathbf{R}, \mathbf{t})$ 추정

**평면 제약 기반 방법**:

코너의 정확한 3D 위치를 추정하기 어려운 경우, 평면 제약만으로도 캘리브레이션이 가능하다. 카메라에서 검출한 코너를 역투영(back-project)하여 3D 광선을 만들고, 이 광선이 LiDAR에서 추정한 평면과 만나는 점을 대응점으로 사용한다.

$n$개의 체커보드 포즈에서 평면 제약:

$$
\mathbf{n}_i^\top (\mathbf{R} \mathbf{p}_{L,i} + \mathbf{t}) + d_i = 0, \quad \forall i = 1, \ldots, n
$$

여기서 $\mathbf{n}_i$는 카메라 좌표계에서의 평면 법선, $\mathbf{p}_{L,i}$는 LiDAR 좌표계에서의 평면 위 점이다.

```python
import numpy as np
import cv2

def calibrate_camera_lidar_target(
    img_corners_list,    # 각 이미지의 체커보드 2D 코너 [N_imgs x N_corners x 2]
    lidar_planes_list,   # 각 관측의 LiDAR 평면 파라미터 [N_imgs x (n, d)]
    K, dist,             # 카메라 intrinsic
    board_corners_3d     # 체커보드 좌표계에서의 3D 코너 [N_corners x 3]
):
    """Target-based Camera-LiDAR extrinsic calibration via PnP."""
    
    # 각 이미지에서 카메라->체커보드 변환 추정
    all_points_3d_lidar = []
    all_points_2d_camera = []
    
    for i, (corners_2d, (normal, d)) in enumerate(zip(img_corners_list, lidar_planes_list)):
        # 카메라에서 본 체커보드 pose (PnP)
        ret, rvec, tvec = cv2.solvePnP(
            board_corners_3d, corners_2d, K, dist
        )
        R_cam_board, _ = cv2.Rodrigues(rvec)
        
        # 체커보드 코너를 카메라 좌표계로 변환
        corners_cam = (R_cam_board @ board_corners_3d.T + tvec).T
        
        # LiDAR 평면 위의 점들 수집
        # (실제로는 LiDAR 점군에서 평면 inlier를 사용)
        all_points_2d_camera.append(corners_2d)
    
    # 최종 결과는 비선형 최적화로 정제
    return R_cam_lidar, t_cam_lidar
```

**실전 주의사항**:
- 체커보드의 크기가 충분히 커야 LiDAR 점이 충분히 들어온다. 최소 A2 이상, 이상적으로 A0 크기.
- 다양한 거리와 각도에서 10~20회 관측 필요.
- LiDAR 빔이 체커보드 평면에 충분한 수의 점을 찍어야 한다. 희소한 LiDAR(16채널)에서는 어렵다.

### 3.3.2 Targetless 캘리브레이션

실전에서는 캘리브레이션 타겟을 준비하고 설치하는 것이 번거롭거나 불가능한 경우가 많다. Targetless 캘리브레이션은 자연 장면의 정보만으로 센서 간 변환을 추정한다.

#### Mutual Information (MI) 기반 방법

**직관**: LiDAR 점군을 카메라 이미지에 투영했을 때, 두 데이터의 통계적 의존성(statistical dependency)이 최대가 되는 변환이 정확한 캘리브레이션이다.

**Mutual Information 정의**:

두 확률 변수 $X$(LiDAR 반사 강도 또는 깊이)와 $Y$(이미지 밝기)의 상호 정보량:

$$
\text{MI}(X; Y) = \sum_{x}\sum_{y} p(x, y) \log \frac{p(x, y)}{p(x)p(y)}
$$

또는 엔트로피로 표현하면:

$$
\text{MI}(X; Y) = H(X) + H(Y) - H(X, Y)
$$

$X$와 $Y$가 독립이면 $\text{MI} = 0$, 완벽히 의존적이면 $\text{MI} = H(X) = H(Y)$.

**Normalized Information Distance (NID)**:

[Koide et al. (2023)](https://arxiv.org/abs/2302.05094)은 MI를 정규화한 NID를 비용 함수로 사용한다:

$$
\text{NID}(X; Y) = 1 - \frac{\text{MI}(X; Y)}{H(X, Y)} = \frac{H(X, Y) - \text{MI}(X; Y)}{H(X, Y)}
$$

NID는 $[0, 1]$ 범위를 가지며, 값이 작을수록 두 데이터의 정합이 좋다.

**히스토그램 기반 MI 계산**:

실전에서 MI는 결합 히스토그램(joint histogram)으로 추정한다:

```python
import numpy as np

def compute_nid(lidar_intensity, image_intensity, bins=64):
    """
    NID (Normalized Information Distance) 계산.
    lidar_intensity: LiDAR 반사 강도 [N]
    image_intensity: 투영된 위치의 이미지 밝기 [N]
    """
    # 결합 히스토그램 (joint histogram) 계산
    hist_2d, _, _ = np.histogram2d(
        lidar_intensity, image_intensity, bins=bins,
        range=[[0, 255], [0, 255]]
    )
    
    # 확률로 정규화
    pxy = hist_2d / hist_2d.sum()
    px = pxy.sum(axis=1)  # marginal X
    py = pxy.sum(axis=0)  # marginal Y
    
    # 엔트로피 계산 (0 log 0 = 0 처리)
    eps = 1e-10
    H_xy = -np.sum(pxy[pxy > eps] * np.log(pxy[pxy > eps]))
    H_x = -np.sum(px[px > eps] * np.log(px[px > eps]))
    H_y = -np.sum(py[py > eps] * np.log(py[py > eps]))
    
    MI = H_x + H_y - H_xy
    NID = 1.0 - MI / (H_xy + eps)
    
    return NID, MI
```

MI 기반 캘리브레이션의 최적화는 기울기가 명시적이지 않으므로 Nelder-Mead 같은 기울기 없는(gradient-free) 최적화를 사용하거나, 수치 기울기를 사용한다. [Pandey et al. (2015)](https://arxiv.org/abs/1507.07595)이 이 접근법을 LiDAR-카메라 캘리브레이션에 처음 적용했다.

#### Edge Alignment 기반 방법

**직관**: LiDAR 점군에서 추출한 깊이 불연속(depth discontinuity) 에지와 이미지에서 추출한 에지가 정합되도록 변환을 최적화한다.

LiDAR 깊이 이미지에서 에지를 추출하고 $\mathbf{e}_L$, 카메라 이미지의 에지를 $\mathbf{e}_C$라 하면:

$$
\min_{\mathbf{R}, \mathbf{t}} \sum_i \text{dist}(\pi(\mathbf{R}\mathbf{p}_{L,i} + \mathbf{t}), \mathbf{e}_C)
$$

여기서 $\pi(\cdot)$는 카메라 투영 함수, $\text{dist}(\cdot, \mathbf{e}_C)$는 가장 가까운 에지까지의 거리(distance transform 사용)이다.

#### Learning-based 방법

RegNet, CalibNet 등의 딥러닝 기반 방법은 LiDAR 점군의 깊이 이미지와 카메라 이미지를 입력으로 받아 6-DoF 변환을 직접 회귀(regression)한다. 이 방법들은 초기값 없이 캘리브레이션을 수행할 수 있지만, Koide et al. (2023) 비교 실험에서 target-based 방법 대비 회전 오차가 2~3배 높았으며, 학습 데이터 도메인 밖에서 일반화 성능이 저하된다.

### 3.3.3 Koide et al. (2023) — 최신 Targetless 캘리브레이션

[Koide et al. (2023)](https://arxiv.org/abs/2302.05094)의 `direct_visual_lidar_calibration`은 현재 가장 실용적인 targetless LiDAR-카메라 캘리브레이션 도구이다. 핵심 파이프라인:

**1단계: LiDAR 포인트 클라우드 밀집화**

회전식 LiDAR(Ouster, Velodyne 등)의 단일 스캔은 희소하여 MI 정합에 불충분하다. CT-ICP(Continuous-Time ICP) 알고리즘으로 수 초간의 연속 스캔을 정밀하게 누적하여 밀집 점군을 생성한다. Solid-state LiDAR(Livox 등)는 비반복 스캔 패턴 덕분에 자연스럽게 밀집화된다.

**2단계: SuperGlue 기반 초기 추정**

밀집 점군을 가상 카메라 시점에서 렌더링하여 LiDAR 강도 이미지를 생성한다. 이 렌더링 이미지와 실제 카메라 이미지 사이에서 SuperGlue(학습 기반 매칭)로 2D-2D 대응점을 검출한다. 이를 2D-3D 대응으로 변환한 뒤 RANSAC + PnP로 초기 변환을 추정한다.

서로 다른 모달리티(LiDAR 강도 vs 카메라 RGB)의 이미지를 매칭하는 cross-modal correspondence 문제를 SuperGlue가 처리한다는 점이 이 단계의 핵심이다. 초기 추정 성공률은 80% 이상이다.

**3단계: NID 기반 정밀 정합**

초기 추정을 시작점으로 NID를 최소화하는 Nelder-Mead 최적화를 수행한다. 이 때 뷰 기반 은닉점 제거(hidden point removal)로 카메라에서 보이지 않는 LiDAR 점을 제거하여 정합 품질을 높인다.

**결과**: 평균 이동 오차 0.043m, 회전 오차 0.374도. 회전식/솔리드스테이트 LiDAR, 핀홀/어안/전방향 카메라 등 다양한 조합에서 동작한다.

### 3.3.4 실전 도구 비교

| 도구 | 방식 | 타겟 필요 | 정밀도 | 자동화 수준 |
|------|------|----------|--------|-----------|
| Autoware Calibration Toolkit | Target-based | O | 높음 | 반자동 |
| `direct_visual_lidar_calibration` (Koide) | Targetless (NID) | X | 높음 | 자동 |
| ACSC (Automatic Calibration) | Target-based + auto corner | O | 높음 | 자동 |
| LiveCalib | Online | X | 중간 | 완전 자동 |

최근 [MFCalib (2024)](https://arxiv.org/abs/2409.00992)은 깊이 연속/불연속 에지와 강도 불연속 에지를 동시에 활용하여 single-shot targetless 캘리브레이션의 정밀도를 크게 향상시켰다. LiDAR 빔의 물리적 측정 원리를 모델링하여 에지 팽창(edge inflation) 문제를 해결한 점이 특징이다.

Koide et al. (2023)은 초기 target-based 캘리브레이션 후 targetless NID 기반 방법으로 드리프트를 모니터링하는 이중 전략을 권장한다.

---

## 3.4 Camera-IMU Extrinsic + Temporal Calibration

카메라와 IMU 사이의 공간적 변위(extrinsic)뿐 아니라 시간적 오프셋(temporal offset)도 동시에 추정해야 한다. VINS-Mono, OpenVINS, Kalibr 등 대표적인 VIO(Visual-Inertial Odometry) 시스템은 모두 이 두 파라미터를 캘리브레이션 입력으로 요구한다.

### 3.4.1 왜 시간 오프셋이 중요한가

카메라와 IMU는 서로 다른 클록으로 데이터를 생성한다. 두 센서 간 시간 오프셋 $t_d$가 존재하면, 카메라 타임스탬프 $t_c$에 대응하는 IMU 데이터는 실제로 $t_c + t_d$ 시점의 것이다.

Furgale et al. (2013)의 실험에서 측정된 카메라-IMU 시간 오프셋은 수 밀리초에서 수십 밀리초 범위였다. 이 오프셋을 무시하면, 빠른 회전 시 재투영 오차가 크게 증가한다. 예를 들어, 시간 오프셋이 10ms이고 카메라가 100 deg/s로 회전 중이라면, 1도의 회전 오차가 발생한다.

### 3.4.2 Kalibr: Continuous-Time B-Spline 기반 캘리브레이션

[Furgale et al. (2013)](https://ieeexplore.ieee.org/document/6696514)이 제안한 Kalibr는 VINS-Mono, OpenVINS, MSCKF 등 주요 VIO 시스템의 캘리브레이션 도구로 채택되어 있다.

**핵심 아이디어**: 궤적(trajectory)을 이산적인 포즈의 시퀀스가 아닌, 연속 시간 B-spline으로 표현한다. 이렇게 하면 서로 다른 샘플링 레이트의 센서(카메라: 20~30Hz, IMU: 200~1000Hz)를 자연스럽게 처리할 수 있다.

**B-Spline 궤적 표현**:

3차(cubic) B-spline에서 시간 $t$에서의 포즈 $\mathbf{T}(t) \in SE(3)$는 제어점(control points) $\{\mathbf{T}_i\}$의 가중 조합으로 표현된다:

$$
\mathbf{T}(t) = \mathbf{T}_i \prod_{j=1}^{3} \text{Exp}(\mathbf{B}_j(u) \cdot \Omega_{i+j})
$$

여기서:
- $u = (t - t_i) / \Delta t$는 정규화된 시간 ($0 \leq u < 1$)
- $\mathbf{B}_j(u)$는 3차 B-spline 기저 함수의 계수
- $\Omega_{i+j} = \text{Log}(\mathbf{T}_{i+j-1}^{-1} \mathbf{T}_{i+j})$는 인접 제어점 간 상대 변환의 리 대수(Lie algebra) 표현
- $\text{Exp}, \text{Log}$는 $SE(3)$의 지수/로그 사상

B-spline의 핵심 장점:
1. **미분 가능**: 임의 시간에서 속도, 가속도를 해석적으로 계산 가능 → IMU 관측 모델과 직접 연결
2. **비동기 센서 처리**: 센서별 타임스탬프에 구속받지 않음
3. **국소성(locality)**: 각 기저 함수는 4개의 제어점에만 영향 → 희소(sparse) 최적화 가능

**관측 모델**:

카메라 관측: 시간 $t_c + t_d$ (시간 오프셋 보정)에서의 궤적 포즈로 3D 랜드마크를 투영:

$$
\mathbf{e}_{\text{cam},k} = \mathbf{m}_k - \pi\left(\mathbf{T}_{CB} \cdot \mathbf{T}(t_{c,k} + t_d) \cdot \mathbf{p}_w\right)
$$

여기서 $\mathbf{T}_{CB}$는 카메라-IMU 외부 파라미터 (추정 대상).

IMU 관측: 시간 $t_{\text{imu}}$에서의 궤적 미분으로 가속도와 각속도를 예측:

$$
\mathbf{e}_{\text{accel},k} = \mathbf{a}_k - \left[\mathbf{R}(t_k)^\top(\ddot{\mathbf{p}}(t_k) - \mathbf{g}) + \mathbf{b}_a\right]
$$
$$
\mathbf{e}_{\text{gyro},k} = \boldsymbol{\omega}_k - \left[\boldsymbol{\omega}(t_k) + \mathbf{b}_g\right]
$$

**최적화 문제**:

$$
\min_{\mathbf{T}_{CB}, t_d, \mathbf{b}_a, \mathbf{b}_g, \{\mathbf{T}_i\}} \sum_k \|\mathbf{e}_{\text{cam},k}\|^2_{\Sigma_c} + \sum_k \left(\|\mathbf{e}_{\text{accel},k}\|^2_{\Sigma_a} + \|\mathbf{e}_{\text{gyro},k}\|^2_{\Sigma_g}\right)
$$

이 최적화는 비선형 최소자승 문제로, Gauss-Newton 또는 LM 알고리즘으로 풀 수 있다. B-spline의 국소성 덕분에 자코비안이 희소하여 대규모 문제도 효율적으로 풀린다.

### 3.4.3 Kalibr 실행 가이드

```bash
# 1. AprilGrid 타겟 준비
kalibr_create_target_pdf --type apriltag \
    --nx 6 --ny 6 --tsize 0.024 --tspace 0.3

# 2. 데이터 수집 (ROS bag)
#    - 타겟을 카메라 시야에 놓고, 센서 리그를 다양하게 움직임
#    - 모든 축에서 회전 + 병진 운동을 포함
#    - 최소 60초, 이상적으로 2분 이상 수집

# 3. Camera-IMU 캘리브레이션 실행
kalibr_calibrate_imu_camera \
    --target april_6x6_24x24mm.yaml \
    --cam camchain.yaml \
    --imu imu.yaml \
    --bag calibration.bag \
    --bag-freq 20.0 \
    --timeoffset-padding 0.1
```

**IMU 설정 파일 (imu.yaml) 예시**:
```yaml
# imu.yaml
rostopic: /imu0
update_rate: 200.0  # Hz

# IMU 노이즈 파라미터 (Allan variance에서 측정, 3.4.4절 참조)
accelerometer_noise_density: 0.01    # m/s^2/sqrt(Hz)
accelerometer_random_walk: 0.0002   # m/s^3/sqrt(Hz)
gyroscope_noise_density: 0.005      # rad/s/sqrt(Hz)
gyroscope_random_walk: 4.0e-06      # rad/s^2/sqrt(Hz)
```

**데이터 수집 핵심 팁**:
1. **운동의 다양성**: 모든 6-DoF를 흥분(excite)해야 한다. 특히 각 축의 회전이 중요.
2. **운동 속도**: 너무 느리면 IMU bias 추정이 어렵고, 너무 빠르면 이미지가 흐려진다.
3. **타겟 가시성**: 전체 수집 시간의 80% 이상에서 타겟이 보여야 한다.
4. **시작과 끝**: 정지 상태에서 시작/끝나야 IMU bias 초기화가 용이하다.

**결과 해석**: Kalibr 출력에서 확인해야 할 항목:
- `T_cam_imu`: 카메라-IMU 외부 파라미터 (4x4 변환 행렬)
- `timeshift_cam_imu`: 시간 오프셋 $t_d$ (보통 수 ms)
- 재투영 오차 분포: 평균 0.2~0.5 px이 이상적
- 가속도계/자이로 잔차: 노이즈 모델과 일치해야 함

### 3.4.4 Allan Variance 측정 실습

IMU 노이즈 파라미터를 정확히 아는 것은 Kalibr 캘리브레이션뿐 아니라 모든 IMU 기반 시스템의 성능에 직접적으로 영향을 미친다. Allan variance는 시계열 데이터의 노이즈 특성을 클러스터 시간(cluster time)에 따라 분석하는 기법으로, 원래 원자 시계의 안정성을 측정하기 위해 개발되었다.

**Allan Variance 정의**:

시계열 데이터 $\{x_k\}$에서 클러스터 시간 $\tau = n \cdot \tau_0$ ($\tau_0$: 샘플링 주기)에 대한 Allan variance:

$$
\sigma^2(\tau) = \frac{1}{2(N-2n)} \sum_{k=1}^{N-2n} \left[\bar{x}_{k+n} - \bar{x}_k\right]^2
$$

여기서 $\bar{x}_k = \frac{1}{n}\sum_{j=0}^{n-1} x_{k+j}$는 클러스터 평균이다.

**로그-로그 플롯에서의 노이즈 식별**:

Allan deviation $\sigma(\tau)$를 $\tau$에 대해 로그-로그 스케일로 그리면, 각 노이즈 원인이 서로 다른 기울기의 직선으로 나타난다:

| 노이즈 유형 | 기울기 | $\sigma(\tau)$ |
|-----------|-------|---------------|
| Quantization noise | $-1$ | $\propto \tau^{-1}$ |
| **Angle/Velocity random walk** | $-1/2$ | $\propto \tau^{-1/2}$ |
| **Bias instability** | $0$ | 일정 (최솟값) |
| Rate random walk | $+1/2$ | $\propto \tau^{+1/2}$ |
| Rate ramp | $+1$ | $\propto \tau$ |

**실습 코드**:

```python
import numpy as np
import matplotlib.pyplot as plt

def compute_allan_variance(data, dt, max_clusters=100):
    """
    Allan variance 계산.
    data: 1D 시계열 (예: 자이로 x축 raw 데이터) [N]
    dt: 샘플링 주기 [s]
    max_clusters: 로그 스케일로 분배할 클러스터 수
    """
    N = len(data)
    max_n = N // 2
    
    # 로그 스케일로 cluster size 선택
    n_values = np.unique(
        np.logspace(0, np.log10(max_n), max_clusters).astype(int)
    )
    
    taus = []
    allan_vars = []
    
    for n in n_values:
        tau = n * dt
        
        # 클러스터 평균 계산
        n_clusters = N // n
        trimmed = data[:n_clusters * n]
        clusters = trimmed.reshape(n_clusters, n)
        cluster_means = clusters.mean(axis=1)
        
        # Allan variance
        diffs = np.diff(cluster_means)
        avar = 0.5 * np.mean(diffs**2)
        
        taus.append(tau)
        allan_vars.append(avar)
    
    return np.array(taus), np.array(allan_vars)

def extract_imu_noise_params(taus, allan_vars, dt):
    """
    Allan variance 플롯에서 IMU 노이즈 파라미터를 추출.
    """
    adev = np.sqrt(allan_vars)
    
    # 1. Random walk (기울기 -1/2): tau=1에서의 값
    # N (noise density) = sigma(tau=1) 
    idx_1s = np.argmin(np.abs(taus - 1.0))
    noise_density = adev[idx_1s]
    
    # 2. Bias instability: Allan deviation의 최솟값
    bias_instability = np.min(adev)
    tau_min = taus[np.argmin(adev)]
    
    # 3. Random walk (기울기 +1/2): 장기 기울기에서 추출
    # K (rate random walk) = sigma(tau=3) * sqrt(3) (근사)
    # 실전에서는 선형 회귀로 정밀 추출
    
    return {
        'noise_density': noise_density,
        'bias_instability': bias_instability,
        'tau_min': tau_min
    }

# 사용 예시
# 1. IMU를 정지 상태로 최소 2시간 이상 녹화
# 2. 자이로/가속도 각 축의 raw 데이터를 분석

# 예시: 200Hz IMU 데이터, 2시간 분량
dt = 1.0 / 200  # 5ms
# gyro_x = np.loadtxt("imu_static_gyro_x.txt")  # 실제 데이터

# 시뮬레이션 데이터로 시연
np.random.seed(42)
N = 200 * 3600 * 2  # 2시간
noise_density = 0.005  # rad/s/sqrt(Hz)
bias = 0.0001  # rad/s (천천히 드리프트)
gyro_x = noise_density * np.sqrt(1/dt) * np.random.randn(N)
gyro_x += bias * np.cumsum(np.random.randn(N)) * np.sqrt(dt)

taus, avars = compute_allan_variance(gyro_x, dt)
params = extract_imu_noise_params(taus, avars, dt)

print(f"Gyroscope noise density: {params['noise_density']:.6f} rad/s/sqrt(Hz)")
print(f"Bias instability: {params['bias_instability']:.6f} rad/s")
print(f"Min at tau = {params['tau_min']:.1f} s")

# Allan deviation 플롯
plt.figure(figsize=(10, 6))
plt.loglog(taus, np.sqrt(avars), 'b-', linewidth=1.5)
plt.xlabel('Cluster time τ (s)')
plt.ylabel('Allan deviation σ(τ)')
plt.title('Gyroscope Allan Deviation')
plt.grid(True, which='both', alpha=0.3)

# 기울기 참조선
tau_ref = np.array([0.01, 100])
plt.loglog(tau_ref, params['noise_density'] / np.sqrt(tau_ref), 
           'r--', label='Random walk (-1/2)')
plt.axhline(y=params['bias_instability'], color='g', linestyle='--', 
            label=f'Bias instability ({params["bias_instability"]:.1e})')
plt.legend()
plt.savefig('allan_deviation.png', dpi=150)
plt.show()
```

**Allan Variance 측정 실전 팁**:
- **정지 데이터 수집**: IMU를 진동이 없는 단단한 표면 위에 놓고, 최소 2시간 (이상적으로 6시간) 녹화한다. 짧은 데이터로는 bias instability의 최솟값을 정확히 식별할 수 없다.
- **데이터시트와의 비교**: 제조사 데이터시트의 noise density 값과 Allan variance에서 추출한 값을 비교하여 센서 상태를 검증한다.
- **Kalibr와의 연결**: Kalibr의 `imu.yaml`에 입력하는 `accelerometer_noise_density`와 `gyroscope_noise_density`가 정확히 Allan variance에서 $\tau = 1$초에서의 Allan deviation 값이다.
- **온도 안정화**: IMU는 온도에 민감하므로, 전원 투입 후 15~30분 동안 워밍업 후 녹화를 시작한다.

---

## 3.5 LiDAR-IMU Extrinsic Calibration

LiDAR와 IMU 간의 외부 파라미터 추정은 고전적인 **hand-eye calibration** 문제로 귀결된다.

### 3.5.1 Hand-Eye Calibration (AX = XB)

로봇 팔의 끝(hand)에 카메라가 달려 있고, 로봇의 기저(base)에서 본 팔 끝의 움직임과 카메라에서 본 타겟의 움직임 사이의 관계를 구하는 문제가 hand-eye calibration의 원형이다 ([Tsai & Lenz, 1989](https://ieeexplore.ieee.org/document/34770)).

**문제 정의**:

센서 $A$(예: LiDAR)와 센서 $B$(예: IMU)가 강체(rigid body)에 고정되어 있을 때, 두 시점 $i, j$에서 각 센서가 관측한 상대 운동이 $\mathbf{A}_{ij}$와 $\mathbf{B}_{ij}$라 하자:

$$
\mathbf{A}_{ij} = \mathbf{T}_A^{-1}(t_i) \cdot \mathbf{T}_A(t_j) \quad \text{(센서 A의 상대 운동)}
$$
$$
\mathbf{B}_{ij} = \mathbf{T}_B^{-1}(t_i) \cdot \mathbf{T}_B(t_j) \quad \text{(센서 B의 상대 운동)}
$$

두 센서 간 고정된 변환 $\mathbf{X} = \mathbf{T}_{AB}$는 다음을 만족한다:

$$
\mathbf{A}_{ij} \mathbf{X} = \mathbf{X} \mathbf{B}_{ij}
$$

이것이 $\mathbf{AX} = \mathbf{XB}$ 방정식이다. $\mathbf{X} \in SE(3)$를 풀어야 한다.

**회전과 이동의 분리**:

회전과 이동으로 분리하면:

$$
\mathbf{R}_A \mathbf{R}_X = \mathbf{R}_X \mathbf{R}_B \quad \text{(회전)}
$$
$$
\mathbf{R}_A \mathbf{t}_X + \mathbf{t}_A = \mathbf{R}_X \mathbf{t}_B + \mathbf{t}_X \quad \text{(이동)}
$$

회전 방정식은 $\mathbf{t}_X$에 독립이므로, 먼저 $\mathbf{R}_X$를 풀고, 그 다음 이동 방정식에서 $\mathbf{t}_X$를 풀 수 있다.

**[Tsai & Lenz (1989)](https://ieeexplore.ieee.org/document/34770)의 해법**:

회전 방정식을 축-각(axis-angle) 표현으로 변환한다. $\mathbf{R}_A$의 회전축이 $\hat{\mathbf{a}}$이고 회전각이 $\alpha$이면, modified Rodrigues 파라미터를 사용하여:

$$
\text{skew}(\hat{\mathbf{a}}_A + \hat{\mathbf{a}}_B) \cdot \mathbf{r}_X = \hat{\mathbf{a}}_A - \hat{\mathbf{a}}_B
$$

여기서 $\mathbf{r}_X$는 $\mathbf{R}_X$의 수정 로드리게스 벡터이다. 여러 운동 쌍에서 이 방정식을 쌓으면 선형 시스템 $\mathbf{C} \mathbf{r}_X = \mathbf{d}$를 얻고, 최소 2개의 (비평행 회전축을 가진) 운동 쌍이 있으면 풀 수 있다. 이동 벡터 $\mathbf{t}_X$도 유사한 선형 시스템으로 풀린다.

**더 많은 운동 쌍의 활용**: 실전에서는 수십에서 수백 개의 운동 쌍을 사용하여 overdetermined system을 최소자승으로 풀고, LM 최적화로 정제한다.

### 3.5.2 Motion-based 자동 캘리브레이션

Hand-eye calibration을 쓰려면 두 센서 각각의 모션 추정(odometry)이 이미 있어야 한다. LiDAR odometry와 IMU integration이 독립적으로 동작하므로, 별도의 캘리브레이션 타겟 없이 센서를 자유롭게 움직이면서 데이터를 수집할 수 있다.

```python
import numpy as np
from scipy.spatial.transform import Rotation

def hand_eye_calibration_tsai(A_rotations, A_translations, 
                                B_rotations, B_translations):
    """
    Tsai-Lenz hand-eye calibration (AX = XB).
    
    A_rotations: [N x 3 x 3] 센서 A의 상대 회전 행렬
    A_translations: [N x 3] 센서 A의 상대 이동
    B_rotations: [N x 3 x 3] 센서 B의 상대 회전 행렬
    B_translations: [N x 3] 센서 B의 상대 이동
    """
    N = len(A_rotations)
    
    # Step 1: 회전 RX 추정
    C = []
    d = []
    for i in range(N):
        # 축-각 표현으로 변환
        rA = Rotation.from_matrix(A_rotations[i]).as_rotvec()
        rB = Rotation.from_matrix(B_rotations[i]).as_rotvec()
        
        alpha = np.linalg.norm(rA)
        beta = np.linalg.norm(rB)
        
        if alpha < 1e-6 or beta < 1e-6:
            continue  # 작은 회전은 무시
        
        # Modified Rodrigues parameters
        a_prime = np.tan(alpha / 2) * rA / alpha
        b_prime = np.tan(beta / 2) * rB / beta
        
        # skew(a' + b') * rX = a' - b'
        skew_sum = np.array([
            [0, -(a_prime[2]+b_prime[2]), a_prime[1]+b_prime[1]],
            [a_prime[2]+b_prime[2], 0, -(a_prime[0]+b_prime[0])],
            [-(a_prime[1]+b_prime[1]), a_prime[0]+b_prime[0], 0]
        ])
        
        C.append(skew_sum)
        d.append(a_prime - b_prime)
    
    C = np.vstack(C)
    d = np.concatenate(d)
    
    # 최소자승 풀이
    rX, _, _, _ = np.linalg.lstsq(C, d, rcond=None)
    
    # Modified Rodrigues → 회전 행렬
    angle = 2 * np.arctan(np.linalg.norm(rX))
    if angle > 1e-6:
        axis = rX / np.linalg.norm(rX)
        R_X = Rotation.from_rotvec(angle * axis).as_matrix()
    else:
        R_X = np.eye(3)
    
    # Step 2: 이동 tX 추정
    C_t = []
    d_t = []
    for i in range(N):
        C_t.append(A_rotations[i] - np.eye(3))
        d_t.append(R_X @ B_translations[i] - A_translations[i])
    
    C_t = np.vstack(C_t)
    d_t = np.concatenate(d_t)
    
    t_X, _, _, _ = np.linalg.lstsq(C_t, d_t, rcond=None)
    
    return R_X, t_X
```

### 3.5.3 LI-Init (FAST-LIO 계열)

FAST-LIO2 같은 최신 LIO 시스템에는 LiDAR-IMU 외부 파라미터를 온라인으로 자동 초기화하는 기능이 있다. **LI-Init** 방식의 핵심 아이디어:

1. IMU 데이터만으로 자세(attitude)를 추정하고, LiDAR 매칭으로 포즈를 추정
2. 두 추정의 차이로부터 상대 변환을 반복적으로 정제
3. Error-State Iterated Kalman Filter (ESIKF)의 상태 벡터에 LiDAR-IMU extrinsic을 포함하여 온라인 추정

별도의 캘리브레이션 절차 없이, LIO 시스템 기동 직후 자동으로 외부 파라미터를 추정한다. 초기 수 초 동안 충분히 다양한 운동이 있으면 수렴한다.

**장점**: 별도 도구나 절차가 필요 없음. 현장에서 바로 사용 가능.
**단점**: 초기 운동이 불충분하면 수렴하지 않거나 부정확할 수 있음. Target-based 방법보다 정밀도가 낮을 수 있음.

**GRIL-Calib**: 지상 로봇처럼 운동이 평면에 제한되는 경우, 기존 방법은 일부 축의 관측 가능성이 떨어져 정밀도가 저하된다. [GRIL-Calib (Kim et al., 2024)](https://arxiv.org/abs/2312.14035)은 지면 평면 잔차(ground plane residual)를 LiDAR odometry에 활용하고, 지면 평면 운동(GPM) 제약을 최적화에 넣어 평면 운동만으로도 6-DoF 캘리브레이션 파라미터를 추정한다.

여기서는 단일 LiDAR와 단일 IMU 사이의 캘리브레이션을 다루었다. 자율주행 차량처럼 여러 대의 LiDAR를 쓰는 시스템에서는 LiDAR 간의 상대 포즈도 결정해야 한다.

---

## 3.6 LiDAR-LiDAR Extrinsic Calibration

다중 LiDAR 시스템(multi-LiDAR rig)에서 각 LiDAR 간의 상대 포즈를 추정하는 문제이다. 자율주행 차량에서 전방/측방/후방에 LiDAR를 배치하는 경우 필수적이다.

### 3.6.1 문제 설정

$n$개의 LiDAR $\{L_1, L_2, \ldots, L_n\}$가 차체에 고정되어 있을 때, 기준 LiDAR $L_1$에 대한 나머지 LiDAR의 상대 변환 $\{\mathbf{T}_{L_1 L_i}\}_{i=2}^{n}$를 추정한다.

### 3.6.2 Target-based 방법

대형 평면 타겟(패널)을 사용하여 여러 LiDAR가 동시에 관측할 수 있도록 한다. 각 LiDAR에서 타겟 평면을 RANSAC으로 피팅하고, 평면 파라미터의 제약 조건으로부터 상대 변환을 추정한다.

### 3.6.3 Targetless: ICP 기반

가장 자연스러운 targetless 방법은 중첩 영역(overlapping region)이 있는 두 LiDAR의 점군을 ICP로 정합하는 것이다.

**중첩 영역이 있는 경우**:

두 LiDAR의 FoV가 겹치는 영역이 있으면, 해당 영역의 점군에 대해 직접 ICP를 적용한다:

$$
\min_{\mathbf{R}, \mathbf{t}} \sum_{i} \|\mathbf{p}_{L_2,i} - (\mathbf{R} \mathbf{p}_{L_1,i'} + \mathbf{t})\|^2
$$

Point-to-plane ICP가 일반적으로 더 빠르게 수렴한다:

$$
\min_{\mathbf{R}, \mathbf{t}} \sum_{i} \left[(\mathbf{R} \mathbf{p}_{L_1,i'} + \mathbf{t} - \mathbf{p}_{L_2,i})^\top \mathbf{n}_{L_2,i}\right]^2
$$

여기서 $\mathbf{n}_{L_2,i}$는 target 점의 법선 벡터이다.

**중첩 영역이 없는 경우**:

FoV가 겹치지 않으면 직접 정합이 불가능하다. 이 경우:
1. Hand-eye calibration(3.5.1절): 각 LiDAR의 odometry로부터 상대 운동을 추출하여 AX=XB를 풀 수 있다.
2. SLAM 기반: 각 LiDAR가 독립적으로 SLAM을 수행하고, 공통 환경의 전역 맵에서 매칭하여 상대 변환을 추정한다.

### 3.6.4 Feature-based 방법

구조화된 환경(건물, 도로 등)에서는 평면, 기둥, 에지 같은 기하학적 특징을 추출하여 매칭에 사용한다. 이 방법은 점 단위 매칭보다 노이즈에 강건하다:

- 여러 LiDAR에서 동일한 대형 평면(벽, 바닥)을 추출
- 평면 파라미터 $(n_i, d_i)$를 각 LiDAR 좌표계에서 계산
- 대응 평면 쌍으로부터 상대 변환 추정

최소 3개의 비공선(non-coplanar) 평면 대응이 필요하다.

지금까지 다룬 캘리브레이션(3.1~3.6)은 카메라, LiDAR, IMU 사이의 관계였다. 야외 환경에서 GNSS를 활용하는 시스템에서는 GNSS 안테나와 IMU 사이의 공간적 관계도 정밀하게 알아야 한다.

---

## 3.7 GNSS-IMU Lever Arm & Boresight

GNSS 안테나와 IMU 사이의 공간적 관계를 **레버 암(lever arm)**이라 한다. 이는 GNSS/INS 통합 항법에서 핵심적인 캘리브레이션 파라미터이다.

### 3.7.1 Lever Arm Vector

GNSS 안테나의 위상 중심(phase center)과 IMU의 원점 사이의 3D 벡터 $\mathbf{l} = [l_x, l_y, l_z]^\top$를 IMU body frame에서 정의한다.

GNSS가 측정하는 위치는 안테나의 위상 중심이지만, 우리가 원하는 것은 IMU(또는 차량 기준점)의 위치이다:

$$
\mathbf{p}_{\text{IMU}} = \mathbf{p}_{\text{GNSS}} - \mathbf{R}_{\text{body}}^{\text{nav}} \cdot \mathbf{l}
$$

여기서 $\mathbf{R}_{\text{body}}^{\text{nav}}$는 body frame에서 navigation frame으로의 회전 행렬이다. 차량이 회전하면 GNSS 안테나 위치가 변하므로, lever arm을 보정하지 않으면 위치 오차가 발생한다. lever arm이 1m이고 차량이 10 deg 기울면, 약 17cm의 위치 오차가 생긴다.

### 3.7.2 Lever Arm 추정 방법

**방법 1: 물리적 측정**

가장 직관적인 방법은 줄자, 레이저 거리 측정기 등으로 직접 측정하는 것이다. 정밀도는 수 cm 수준으로, 자율주행 수준의 측위 정밀도(10cm 이하)를 요구하지 않는 로봇 응용에서는 이 수준으로 충분히 운용된다.

**방법 2: 필터 기반 온라인 추정**

EKF 상태 벡터에 lever arm $\mathbf{l}$을 포함하여 온라인으로 추정한다. GNSS 관측 모델에서 lever arm이 관측 가능(observable)하려면 충분한 회전 운동이 필요하다. 직선 주행만으로는 lever arm의 전방 성분($l_x$)을 추정하기 어렵다.

**방법 3: 사후 처리(post-processing)**

수집된 GNSS/IMU 데이터를 전후방(forward-backward) 스무더로 처리하면서 lever arm을 최적화한다. 정밀 측량 분야에서 주로 사용한다.

### 3.7.3 GNSS 안테나 위상 중심

GNSS 안테나의 위상 중심(Antenna Phase Center, APC)은 안테나의 물리적 중심과 일치하지 않으며, 위성의 고도각과 주파수에 따라 달라진다. 이 변동을 Phase Center Variation (PCV)이라 하며, mm~cm 수준이다.

정밀 측위(RTK/PPP)에서는 안테나 위상 중심 보정 데이터(ANTEX 파일)를 적용해야 한다. 로봇 수준의 응용에서는 보통 무시해도 되지만, 측량 수준의 정밀도가 필요하면 반드시 고려한다.

---

## 3.8 Online / Continuous Calibration

운용 중에 캘리브레이션 파라미터가 변할 수 있다. 온도 변화, 진동, 충격 등으로 센서 마운트가 미세하게 변형되면 초기 캘리브레이션이 점차 부정확해진다. 이를 보정하기 위해 **온라인 캘리브레이션(online calibration)**이 필요하다.

### 3.8.1 Self-Calibration during SLAM

SLAM 시스템의 상태 벡터에 캘리브레이션 파라미터를 포함하여 운용 중에 추정하는 방법이다.

**EKF 기반**: OpenVINS는 카메라 intrinsic, camera-IMU extrinsic, 시간 오프셋을 상태 벡터에 포함하여 온라인으로 추정한다.

상태 벡터 확장:
$$
\mathbf{x} = \begin{bmatrix} \mathbf{x}_{\text{nav}} \\ \mathbf{x}_{\text{calib}} \end{bmatrix}
= \begin{bmatrix} \mathbf{q}, \mathbf{p}, \mathbf{v}, \mathbf{b}_g, \mathbf{b}_a \\ \mathbf{q}_{CI}, \mathbf{p}_{CI}, t_d, f_x, f_y, c_x, c_y, k_1, \ldots \end{bmatrix}
$$

캘리브레이션 파라미터의 프로세스 모델은 일반적으로 random walk로 가정한다:

$$
\dot{\mathbf{x}}_{\text{calib}} = \mathbf{w}_{\text{calib}}, \quad \mathbf{w}_{\text{calib}} \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_{\text{calib}})
$$

$\mathbf{Q}_{\text{calib}}$의 크기는 캘리브레이션 파라미터가 얼마나 빨리 변할 수 있는지를 반영한다. 너무 크면 파라미터가 불안정하게 변동하고, 너무 작으면 실제 변화를 추적하지 못한다.

**Factor graph 기반**: LIO-SAM이나 VINS-Mono에서도 외부 파라미터를 최적화 변수로 포함할 수 있다. Between factor의 노이즈 모델로 캘리브레이션의 안정성을 제어한다.

### 3.8.2 Extrinsic Drift 보정

장기간 운용하는 시스템(자율주행 차량, 로봇)에서 외부 파라미터의 드리프트를 감지하고 보정하는 전략:

1. **재투영 오차 모니터링**: LiDAR 점군을 이미지에 투영했을 때 에지 정합 품질을 지속적으로 모니터링. 정합 품질이 떨어지면 캘리브레이션 재수행을 트리거.

2. **주기적 재캘리브레이션**: 운용 데이터를 이용하여 주기적으로 targetless 캘리브레이션을 재수행.

3. **온라인 미세 조정**: 현재 캘리브레이션을 초기값으로 하여 작은 범위 내에서 연속적으로 최적화.

최근 [CalibRefine (2025)](https://arxiv.org/abs/2502.17648)은 딥러닝 기반으로 raw LiDAR 점군과 카메라 이미지를 직접 입력받아 온라인 targetless 캘리브레이션을 수행하며, 반복적 후처리 정제(iterative post-refinement)로 정밀도를 높인 자동 프레임워크를 제안했다.

### 3.8.3 OpenCalib: 자율주행 통합 캘리브레이션 프레임워크

[OpenCalib (Yan et al., 2023)](https://arxiv.org/abs/2205.14087)은 자율주행 시스템에 필요한 다양한 캘리브레이션을 하나의 프레임워크로 통합한 오픈소스 도구이다.

**지원하는 캘리브레이션 유형**:

| 센서 쌍 | 방법 | 타겟 |
|---------|------|------|
| Camera intrinsic | Zhang's method | 체커보드/AprilTag |
| Camera-Camera | Stereo calibration | 공유 타겟 |
| Camera-LiDAR | PnP / Edge alignment | 있음/없음 |
| LiDAR-LiDAR | ICP / Feature matching | 없음 |
| Camera-Ground | Vanishing point | 없음 |
| 온라인 보정 | 모니터링 + 재캘리브레이션 | 없음 |

**OpenCalib의 설계 철학**:
- **모듈화**: 각 캘리브레이션 유형이 독립 모듈로 구현되어, 필요한 것만 사용 가능
- **통합 인터페이스**: ROS 기반 통합 인터페이스로 다양한 센서 입력을 처리
- **시각화**: 캘리브레이션 결과를 실시간으로 시각화하여 품질을 직관적으로 확인

자율주행 시스템에서의 캘리브레이션은 단일 센서 쌍이 아닌 **센서 체인(sensor chain)**의 일관성이 중요하다. 예를 들어, 차량에 6대의 카메라와 1대의 LiDAR가 있으면, 각 카메라-LiDAR 캘리브레이션이 독립적으로 수행되더라도 카메라 간 상대 포즈가 일관되어야 한다. OpenCalib은 이런 일관성 제약을 전역 최적화에 통합한다.

---

## 3.9 Temporal Calibration

센서 간 시간 동기화는 공간 캘리브레이션만큼 중요하다. 빠르게 움직이는 플랫폼에서 수 밀리초의 시간 오차도 센티미터 수준의 위치 오차로 이어진다.

### 3.9.1 Hardware Synchronization

하드웨어 동기화는 가장 정확하고 신뢰할 수 있는 방법이다.

**Trigger 기반 동기화**:

하나의 마스터 타이머가 모든 센서에 하드웨어 트리거 신호를 보내 동시에 데이터를 캡처하게 한다.

- **카메라 트리거**: 외부 트리거 입력에 연결된 GPIO 핀으로 노출 시작 시점을 제어
- **LiDAR 동기**: 일부 LiDAR(Ouster 등)는 PPS(Pulse Per Second) 입력을 지원하여 스캔 시작 시점을 동기화
- **IMU 동기**: IMU의 샘플링 클록을 외부 기준에 고정

**PPS (Pulse Per Second)**:

GNSS 수신기는 GPS 시간에 동기된 1Hz의 전기 펄스(PPS)를 출력한다. 이 펄스의 상승 에지(rising edge)가 정확히 GPS 초의 경계에 대응하므로, 모든 센서의 로컬 타임스탬프를 GPS 시간에 정렬하는 기준으로 사용할 수 있다.

```
                   PPS Signal (from GNSS)
                   ───┐     ┌───┐     ┌───┐     ┌───
                      │     │   │     │   │     │
                      └─────┘   └─────┘   └─────┘
                   t=0     t=1       t=2       t=3   (GPS seconds)
                   
   Camera capture  ──X──X──X──X──X──X──X──X──X──X──  (30 Hz)
   LiDAR scan      ──X─────X─────X─────X─────X─────  (10 Hz)
   IMU sample      ──XXXX──XXXX──XXXX──XXXX──XXXX──  (200 Hz)
```

PPS 동기화의 핵심은 각 센서의 로컬 타임스탬프와 PPS 펄스 사이의 오프셋을 측정하여, 모든 데이터를 공통 시간축(GPS time)에 정렬하는 것이다.

### 3.9.2 PTP (Precision Time Protocol)

IEEE 1588 PTP는 이더넷 네트워크를 통해 마이크로초 수준의 시간 동기화를 제공하는 프로토콜이다. NTP(Network Time Protocol)의 밀리초 수준 정확도보다 훨씬 정밀하다.

**PTP 동작 원리**:

1. **마스터-슬레이브 구조**: 네트워크에서 하나의 마스터 클록(Grandmaster)이 기준 시간을 제공
2. **Sync 메시지**: 마스터가 주기적으로 Sync 메시지를 브로드캐스트
3. **Follow-up**: 정확한 송신 타임스탬프를 별도 메시지로 전송
4. **Delay Request/Response**: 슬레이브가 네트워크 지연을 측정
5. **오프셋 계산**: $\text{offset} = \frac{(t_2 - t_1) - (t_4 - t_3)}{2}$

```
   Master (Grandmaster)              Slave (Sensor)
         |                                |
         |------- Sync (t1) ------------->|  (t2: 수신 시각)
         |                                |
         |------- Follow-up (t1) -------->|  (정확한 t1 전달)
         |                                |
         |<------ Delay_Req (t3) ---------|  (t3: 송신 시각)
         |                                |
         |------- Delay_Resp (t4) ------->|  (t4: 수신 시각 전달)
         |                                |
         
   offset = [(t2 - t1) - (t4 - t3)] / 2
   delay  = [(t2 - t1) + (t4 - t3)] / 2
```

최신 LiDAR(Ouster, Hesai 등)와 산업용 카메라(FLIR/Lucid 등)는 PTP 하드웨어 타임스탬핑을 지원하여, 소프트웨어 오버헤드 없이 마이크로초 수준의 동기화가 가능하다.

### 3.9.3 Software Synchronization

하드웨어 동기화가 불가능한 경우, 소프트웨어로 시간 정렬을 수행한다.

**Host Clock 기반**: 모든 센서 데이터가 호스트 컴퓨터에 도착하면 호스트의 시스템 시계로 타임스탬프를 찍는다. 단순하지만 USB/네트워크 지연의 불확실성(jitter)이 밀리초 수준으로 존재한다.

**ROS Time**: ROS(Robot Operating System)에서는 `ros::Time::now()`로 메시지 수신 시각을 기록한다. 드라이버에서 센서의 로컬 타임스탬프를 ROS 시간으로 변환하는 경우, 변환의 정확도가 전체 동기화 정밀도를 결정한다.

### 3.9.4 Time Offset 온라인 추정

[Li & Mourikis (2014)](https://arxiv.org/abs/1310.7863)는 시간 오프셋 $t_d$를 VIO의 상태 벡터에 포함하여 온라인으로 추정하는 방법을 제안했다. 핵심 아이디어:

**관측 모델에서의 시간 오프셋 반영**:

카메라 관측 시각 $t_c$에서의 실제 센서 포즈는 $t_c + t_d$이다. IMU preintegration에서 이를 반영:

$$
\mathbf{z}(t_c) = \pi(\mathbf{T}(t_c + t_d) \cdot \mathbf{p}_w)
$$

$t_d$가 작다고 가정하면, 1차 테일러 전개:

$$
\mathbf{T}(t_c + t_d) \approx \mathbf{T}(t_c) \cdot \text{Exp}(\boldsymbol{\xi} \cdot t_d)
$$

여기서 $\boldsymbol{\xi}$는 $t_c$에서의 body 속도 (angular + linear velocity)이다.

이 근사를 통해 $t_d$에 대한 자코비안을 해석적으로 계산할 수 있고, EKF 또는 최적화 프레임워크에서 다른 상태 변수와 함께 추정할 수 있다.

최근 [iKalibr (Chen et al., 2024)](https://arxiv.org/abs/2407.11420)는 이 아이디어를 다중 센서로 확장하여, LiDAR·카메라·IMU·radar 등 이종 센서 간의 시공간 파라미터를 B-spline 연속 시간 프레임워크에서 **한 번에** 추정하는 통합 캘리브레이션 도구를 제안했다 (IEEE T-RO 2025).

**관측 가능성(Observability) 조건**: 시간 오프셋이 관측 가능하려면 플랫폼이 충분한 가속 운동을 해야 한다. 등속 직선 운동에서는 시간 오프셋을 추정할 수 없다 (시간 이동이 공간 이동과 구별 불가).

Kalibr(3.4.2절), OpenVINS, VINS-Mono 등 현대의 VIO 시스템은 모두 이 방법의 변형을 구현하고 있다.

### 3.9.5 실전 동기화 전략 가이드

| 정밀도 요구 | 추천 방법 | 비용 |
|-----------|----------|------|
| $< 1\mu s$ | PPS + HW trigger | 높음 (전용 HW 필요) |
| $1\mu s - 100\mu s$ | PTP | 중간 (PTP 지원 장비) |
| $100\mu s - 1ms$ | NTP + 온라인 추정 | 낮음 |
| $> 1ms$ | Host clock + 온라인 추정 | 없음 |

**자율주행 수준**: PPS + PTP 조합이 표준. 모든 센서를 GPS 시간에 동기화.

**연구/프로토타이핑 수준**: 소프트웨어 동기화 + 온라인 시간 오프셋 추정으로 충분한 경우가 많다. Kalibr로 초기 오프셋을 추정하고, VIO 시스템에서 온라인으로 미세 조정한다.

하드웨어 동기화가 가능하면 반드시 쓴다. 소프트웨어 동기화는 하드웨어 동기화의 보완이지 대체가 아니다.

---

## 3.10 챕터 요약

이 챕터에서 다룬 캘리브레이션의 전체 체계를 정리한다.

| 캘리브레이션 유형 | 추정 파라미터 | 핵심 방법 | 최소 요구 조건 |
|----------------|-------------|----------|-------------|
| Camera intrinsic | $\mathbf{K}, \mathbf{d}$ | Zhang's method | 체커보드 15~25장 |
| Stereo extrinsic | $\mathbf{R}, \mathbf{t}$ | 공유 타겟 + stereoCalibrate | 동시 관측 15~25쌍 |
| Camera-LiDAR | $\mathbf{T}_{CL}$ | Target-based / NID | 타겟 10~20회 / 자연 장면 |
| Camera-IMU | $\mathbf{T}_{CI}, t_d$ | Kalibr (B-spline) | 60초 이상 다양한 운동 |
| LiDAR-IMU | $\mathbf{T}_{LI}$ | Hand-eye (AX=XB) / LI-Init | 다양한 운동 |
| LiDAR-LiDAR | $\mathbf{T}_{L_1 L_2}$ | ICP / Feature matching | 중첩 영역 또는 운동 데이터 |
| GNSS-IMU | $\mathbf{l}$ (lever arm) | 물리적 측정 / EKF 추정 | 회전 운동 |
| Temporal | $t_d$ | PPS/PTP + 온라인 추정 | 가속 운동 |

**캘리브레이션 순서 권장**:

1. 각 카메라의 intrinsic (독립적으로 수행)
2. 스테레오 extrinsic (있는 경우)
3. Camera-IMU extrinsic + temporal (Kalibr)
4. LiDAR-IMU extrinsic (hand-eye 또는 LI-Init)
5. Camera-LiDAR extrinsic (camera-IMU와 LiDAR-IMU의 체인으로 간접 계산하거나 직접 캘리브레이션)
6. GNSS lever arm
7. 온라인 캘리브레이션 설정 (운용 중 보정)

간접 계산의 예: Camera-LiDAR 변환은 $\mathbf{T}_{CL} = \mathbf{T}_{CI} \cdot \mathbf{T}_{IL}$로 구할 수 있다. 단, 오차가 누적되므로 직접 캘리브레이션으로 검증하는 것이 좋다.

**핵심 논문 정리**:

- [Zhang (2000)](https://ieeexplore.ieee.org/document/888718): 카메라 intrinsic — 호모그래피 기반의 유연한 캘리브레이션
- [Furgale et al. (2013)](https://ieeexplore.ieee.org/document/6696514): Camera-IMU — B-spline 연속 시간 궤적으로 시공간 동시 캘리브레이션
- [Tsai & Lenz (1989)](https://ieeexplore.ieee.org/document/34770): Hand-eye — AX=XB의 원조
- [Koide et al. (2023)](https://arxiv.org/abs/2302.05094): Camera-LiDAR targetless — NID + SuperGlue 기반 자동 캘리브레이션
- [Li & Mourikis (2014)](https://arxiv.org/abs/1310.7863): Temporal — 시간 오프셋 온라인 추정
- [OpenCalib (2023)](https://arxiv.org/abs/2205.14087): 자율주행 통합 캘리브레이션 프레임워크
- [GRIL-Calib (Kim et al., 2024)](https://arxiv.org/abs/2312.14035): 지상 로봇 환경에서 평면 운동 제약을 활용한 targetless IMU-LiDAR 캘리브레이션. 제한된 운동에서도 6-DoF 추정 가능.
- [MFCalib (2024)](https://arxiv.org/abs/2409.00992): 다중 특징 에지(깊이 연속/불연속, 강도 불연속)를 활용한 single-shot targetless LiDAR-카메라 캘리브레이션. LiDAR 빔 모델로 에지 팽창 문제를 해결.
- [iKalibr (Chen et al., 2024)](https://arxiv.org/abs/2407.11420): Temporal — B-spline 연속 시간 기반 다중 이종 센서(LiDAR·카메라·IMU·radar) 통합 시공간 캘리브레이션 (IEEE T-RO 2025).

센서 모델(Ch.2)과 캘리브레이션 파라미터(Ch.3)가 갖추어졌으면, 이제 센서 데이터로부터 로봇의 상태를 추정하는 알고리즘을 다룰 차례다. Ch.4에서는 칼만 필터에서 팩터 그래프까지, 센서 퓨전의 수학적 엔진인 **상태 추정 이론**을 체계적으로 유도한다.

---

# Ch.4 — State Estimation 이론

Ch.2에서 센서의 관측 모델을, Ch.3에서 센서 간 캘리브레이션을 다루었다. 이제 이 관측 데이터로부터 로봇의 상태(위치, 자세, 속도 등)를 추정하는 **알고리즘**을 다룰 차례다. 이 챕터에서 다루는 칼만 필터, 팩터 그래프, IMU 사전적분은 Ch.6-8에서 살펴볼 모든 odometry/fusion 시스템의 수학적 엔진이다.

> **목표**: 센서 퓨전의 수학적 기반인 상태 추정(state estimation) 이론을 체계적으로 다룬다. Bayesian filtering에서 출발하여 Kalman 필터 계열, Particle Filter를 거쳐, 현대 SLAM의 핵심인 factor graph 기반 최적화까지 이어지는 기술 계보를 따라간다. 각 방법의 유도 과정을 상세히 보여주고, 왜 현대 로봇 시스템이 filtering에서 optimization으로 이동했는지를 설명한다.

---

## 4.1 Bayesian Filtering Framework

### 4.1.1 상태 추정 문제의 정의

로봇의 상태 추정 문제는 본질적으로 **조건부 확률 추론** 문제다. 우리가 알고 싶은 것은 현재까지의 모든 관측 $\mathbf{z}_{1:k}$와 제어 입력 $\mathbf{u}_{1:k}$가 주어졌을 때 상태 $\mathbf{x}_k$의 사후 확률(posterior)이다:

$$p(\mathbf{x}_k \mid \mathbf{z}_{1:k}, \mathbf{u}_{1:k})$$

여기서:
- $\mathbf{x}_k \in \mathbb{R}^n$: 시각 $k$에서의 상태 벡터 (위치, 속도, 자세, 바이어스 등)
- $\mathbf{z}_k \in \mathbb{R}^m$: 시각 $k$에서의 관측 벡터 (카메라, LiDAR, IMU 등)
- $\mathbf{u}_k \in \mathbb{R}^l$: 시각 $k$에서의 제어 입력

이 문제를 풀기 위해 두 가지 모델을 정의한다:

**운동 모델(Motion Model, Process Model)**:
$$\mathbf{x}_k = f(\mathbf{x}_{k-1}, \mathbf{u}_k) + \mathbf{w}_k, \quad \mathbf{w}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_k)$$

$$p(\mathbf{x}_k \mid \mathbf{x}_{k-1}, \mathbf{u}_k)$$

이것은 제어 입력 $\mathbf{u}_k$가 주어졌을 때, 이전 상태에서 현재 상태로의 전이 확률을 나타낸다. 프로세스 노이즈 $\mathbf{w}_k$는 모델의 불확실성을 반영한다.

**관측 모델(Observation Model, Measurement Model)**:
$$\mathbf{z}_k = h(\mathbf{x}_k) + \mathbf{v}_k, \quad \mathbf{v}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{R}_k)$$

$$p(\mathbf{z}_k \mid \mathbf{x}_k)$$

현재 상태가 주어졌을 때, 관측값이 나올 likelihood다. 관측 노이즈 $\mathbf{v}_k$는 센서의 불확실성을 반영한다.

직관적으로 말하면, 로봇은 "내가 어디에 있는지" 정확히 알 수 없지만, "어떻게 움직였는지"(운동 모델)와 "무엇을 보는지"(관측 모델)를 결합하여 자신의 위치에 대한 **믿음(belief)**을 점진적으로 개선해 나간다.

### 4.1.2 Markov 가정과 Recursive Estimation

실용적인 필터링을 위해 **Markov 가정**을 도입한다:

1. **1차 Markov 프로세스**: 현재 상태 $\mathbf{x}_k$는 직전 상태 $\mathbf{x}_{k-1}$과 제어 $\mathbf{u}_k$에만 의존하며, 그 이전의 상태나 관측에는 조건부 독립이다:
$$p(\mathbf{x}_k \mid \mathbf{x}_{0:k-1}, \mathbf{u}_{1:k}, \mathbf{z}_{1:k-1}) = p(\mathbf{x}_k \mid \mathbf{x}_{k-1}, \mathbf{u}_k)$$

2. **조건부 관측 독립**: 관측 $\mathbf{z}_k$는 현재 상태 $\mathbf{x}_k$가 주어지면 다른 모든 것에 독립이다:
$$p(\mathbf{z}_k \mid \mathbf{x}_{0:k}, \mathbf{u}_{1:k}, \mathbf{z}_{1:k-1}) = p(\mathbf{z}_k \mid \mathbf{x}_k)$$

이 두 가정 덕분에, 전체 관측 이력 $\mathbf{z}_{1:k}$를 저장할 필요 없이 직전 추정치만으로 현재 추정치를 갱신할 수 있다. 이것이 **재귀적 추정(recursive estimation)**의 핵심이다.

### 4.1.3 Prediction-Update Cycle

Bayesian filter는 두 단계를 반복한다:

**예측 단계(Prediction Step)**: 운동 모델을 사용하여 이전 사후 확률로부터 현재 사전 확률(prior)을 예측한다.

$$\boxed{p(\mathbf{x}_k \mid \mathbf{z}_{1:k-1}, \mathbf{u}_{1:k}) = \int p(\mathbf{x}_k \mid \mathbf{x}_{k-1}, \mathbf{u}_k) \, p(\mathbf{x}_{k-1} \mid \mathbf{z}_{1:k-1}, \mathbf{u}_{1:k-1}) \, d\mathbf{x}_{k-1}}$$

이 적분이 바로 **Chapman-Kolmogorov 방정식**이다. 물리적 의미는 다음과 같다:
- $p(\mathbf{x}_{k-1} \mid \mathbf{z}_{1:k-1})$: 시각 $k-1$에서의 사후 확률 (이전 단계의 결과)
- $p(\mathbf{x}_k \mid \mathbf{x}_{k-1}, \mathbf{u}_k)$: 상태 전이 확률 (운동 모델)
- 이전 상태의 가능한 모든 값에 대해 전이 확률을 가중 평균하여 예측 분포를 구한다

**갱신 단계(Update Step)**: 새로운 관측 $\mathbf{z}_k$가 들어오면 Bayes 정리로 사후 확률을 갱신한다.

$$\boxed{p(\mathbf{x}_k \mid \mathbf{z}_{1:k}, \mathbf{u}_{1:k}) = \frac{p(\mathbf{z}_k \mid \mathbf{x}_k) \, p(\mathbf{x}_k \mid \mathbf{z}_{1:k-1}, \mathbf{u}_{1:k})}{p(\mathbf{z}_k \mid \mathbf{z}_{1:k-1})}}$$

여기서:
- 분자의 $p(\mathbf{z}_k \mid \mathbf{x}_k)$: likelihood (관측 모델)
- 분자의 $p(\mathbf{x}_k \mid \mathbf{z}_{1:k-1})$: 예측 단계에서 구한 prior
- 분모의 $p(\mathbf{z}_k \mid \mathbf{z}_{1:k-1}) = \int p(\mathbf{z}_k \mid \mathbf{x}_k) p(\mathbf{x}_k \mid \mathbf{z}_{1:k-1}) d\mathbf{x}_k$: 정규화 상수 (evidence)

직관적으로, 관측이 들어올 때마다 "이 관측은 내 예측과 얼마나 일치하는가?"를 평가하여 믿음을 수정하는 것이다.

### 4.1.4 왜 Closed-Form이 안 되는가

위의 prediction-update가 이론적으로는 완벽하지만, 실전에서는 Chapman-Kolmogorov 적분 $\int p(\mathbf{x}_k \mid \mathbf{x}_{k-1}) p(\mathbf{x}_{k-1} \mid \mathbf{z}_{1:k-1}) d\mathbf{x}_{k-1}$을 해석적으로 풀 수 없는 경우가 대부분이다. 그 이유는:

1. **비선형 운동/관측 모델**: $f(\cdot)$과 $h(\cdot)$가 비선형이면 가우시안 분포를 전파했을 때 가우시안이 유지되지 않는다. 사후 분포가 임의의 복잡한 형태를 가질 수 있다.

2. **고차원 상태 공간**: 상태 벡터의 차원이 커지면 적분의 계산량이 지수적으로 증가한다 (차원의 저주).

3. **다봉(multimodal) 분포**: 로봇의 위치가 여러 곳일 가능성이 있을 때 (예: 대칭적 환경에서의 global localization) 사후 분포가 다봉이 된다.

따라서 실전에서는 근사(approximation)가 필수적이며, 근사 방법에 따라 다양한 필터가 존재한다:

| 근사 방법 | 필터 | 분포 표현 | 적용 조건 |
|----------|------|---------|----------|
| 선형-가우시안 가정 | Kalman Filter | $\mathcal{N}(\hat{\mathbf{x}}, \mathbf{P})$ | 선형 시스템 |
| 1차 선형화 + 가우시안 | EKF, ESKF | $\mathcal{N}(\hat{\mathbf{x}}, \mathbf{P})$ | 약한 비선형 |
| Sigma point 변환 | UKF | $\mathcal{N}(\hat{\mathbf{x}}, \mathbf{P})$ | 중간 비선형 |
| 샘플 기반 | Particle Filter | $\{(\mathbf{x}^{(i)}, w^{(i)})\}_{i=1}^N$ | 강한 비선형, 다봉 |
| 반복 선형화 | IEKF | $\mathcal{N}(\hat{\mathbf{x}}, \mathbf{P})$ | 강한 비선형 |

이후 각 방법을 상세히 유도한다.

---

## 4.2 Kalman Filter 계열

### 4.2.1 Kalman Filter: 유도와 최적성

Kalman Filter(KF)는 **선형 시스템 + 가우시안 노이즈** 가정 하에서 Bayesian filter의 정확한(exact) 해다. Rudolf Kalman이 1960년 논문 ["A New Approach to Linear Filtering and Prediction Problems"](https://doi.org/10.1115/1.3662552)에서 제시했으며, 이후 Apollo 프로그램의 궤도 추정에 적용되면서 실용성이 입증되었다.

#### 선형 시스템 모델

상태 전이와 관측이 모두 선형인 시스템을 가정한다:

$$\mathbf{x}_k = \mathbf{F}_k \mathbf{x}_{k-1} + \mathbf{B}_k \mathbf{u}_k + \mathbf{w}_k, \quad \mathbf{w}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_k)$$
$$\mathbf{z}_k = \mathbf{H}_k \mathbf{x}_k + \mathbf{v}_k, \quad \mathbf{v}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{R}_k)$$

여기서:
- $\mathbf{F}_k \in \mathbb{R}^{n \times n}$: 상태 전이 행렬 (state transition matrix)
- $\mathbf{B}_k \in \mathbb{R}^{n \times l}$: 제어 입력 행렬
- $\mathbf{H}_k \in \mathbb{R}^{m \times n}$: 관측 행렬
- $\mathbf{Q}_k \in \mathbb{R}^{n \times n}$: 프로세스 노이즈 공분산 (양의 반정치, symmetric)
- $\mathbf{R}_k \in \mathbb{R}^{m \times m}$: 관측 노이즈 공분산 (양의 정치, symmetric)

핵심 성질: 가우시안 분포에 선형 변환을 적용하면 결과도 가우시안이다. 따라서 사후 분포가 항상 가우시안으로 유지되며, 평균과 공분산만으로 완전히 기술할 수 있다.

#### MMSE 유도 — 왜 Kalman Filter가 최적인가

Kalman Filter의 최적성을 Minimum Mean Square Error(MMSE) 관점에서 유도한다. 우리가 원하는 것은 평균 제곱 오차를 최소화하는 추정량이다:

$$\hat{\mathbf{x}}_k = \arg\min_{\hat{\mathbf{x}}} \mathbb{E}[\|\mathbf{x}_k - \hat{\mathbf{x}}\|^2 \mid \mathbf{z}_{1:k}]$$

MMSE 추정량은 조건부 기댓값 $\hat{\mathbf{x}}_k = \mathbb{E}[\mathbf{x}_k \mid \mathbf{z}_{1:k}]$이다. 선형-가우시안 시스템에서 이것이 정확히 Kalman Filter의 상태 갱신 방정식과 일치함을 보인다.

**Bayesian 관점의 유도**: 시각 $k-1$에서의 사후 분포가 가우시안이라고 가정한다:

$$p(\mathbf{x}_{k-1} \mid \mathbf{z}_{1:k-1}) = \mathcal{N}(\hat{\mathbf{x}}_{k-1|k-1}, \mathbf{P}_{k-1|k-1})$$

**Step 1: Prediction — Chapman-Kolmogorov 적분 수행**

선형 운동 모델에서:
$$\mathbf{x}_k = \mathbf{F}_k \mathbf{x}_{k-1} + \mathbf{B}_k \mathbf{u}_k + \mathbf{w}_k$$

$\mathbf{x}_{k-1}$이 가우시안이고 $\mathbf{w}_k$가 독립 가우시안이므로, $\mathbf{x}_k$도 가우시안이다. 평균과 공분산을 계산하면:

$$\hat{\mathbf{x}}_{k|k-1} = \mathbb{E}[\mathbf{x}_k \mid \mathbf{z}_{1:k-1}] = \mathbf{F}_k \hat{\mathbf{x}}_{k-1|k-1} + \mathbf{B}_k \mathbf{u}_k$$

공분산은 $\tilde{\mathbf{x}}_{k|k-1} = \mathbf{x}_k - \hat{\mathbf{x}}_{k|k-1} = \mathbf{F}_k \tilde{\mathbf{x}}_{k-1|k-1} + \mathbf{w}_k$이므로:

$$\mathbf{P}_{k|k-1} = \mathbb{E}[\tilde{\mathbf{x}}_{k|k-1} \tilde{\mathbf{x}}_{k|k-1}^\top] = \mathbf{F}_k \mathbf{P}_{k-1|k-1} \mathbf{F}_k^\top + \mathbf{Q}_k$$

예측된 분포: $p(\mathbf{x}_k \mid \mathbf{z}_{1:k-1}) = \mathcal{N}(\hat{\mathbf{x}}_{k|k-1}, \mathbf{P}_{k|k-1})$

**Step 2: Update — Bayes 정리 적용**

관측 모델 $\mathbf{z}_k = \mathbf{H}_k \mathbf{x}_k + \mathbf{v}_k$에서, 예측 상태와 관측의 결합 분포(joint distribution)를 구성한다:

$$\begin{bmatrix} \mathbf{x}_k \\ \mathbf{z}_k \end{bmatrix} \sim \mathcal{N}\left( \begin{bmatrix} \hat{\mathbf{x}}_{k|k-1} \\ \mathbf{H}_k \hat{\mathbf{x}}_{k|k-1} \end{bmatrix}, \begin{bmatrix} \mathbf{P}_{k|k-1} & \mathbf{P}_{k|k-1} \mathbf{H}_k^\top \\ \mathbf{H}_k \mathbf{P}_{k|k-1} & \mathbf{S}_k \end{bmatrix} \right)$$

여기서 $\mathbf{S}_k = \mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^\top + \mathbf{R}_k \in \mathbb{R}^{m \times m}$는 혁신 공분산(innovation covariance)이다.

가우시안 결합 분포의 조건부 분포 공식을 적용하면 (가우시안의 핵심 성질: 결합이 가우시안이면 조건부도 가우시안이고, 조건부 평균은 원래 평균 + 관측과의 상관에 비례하는 보정):

$$\hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + \mathbf{P}_{k|k-1} \mathbf{H}_k^\top \mathbf{S}_k^{-1} (\mathbf{z}_k - \mathbf{H}_k \hat{\mathbf{x}}_{k|k-1})$$

**칼만 이득(Kalman gain)** $\mathbf{K}_k \in \mathbb{R}^{n \times m}$을 정의하면:

$$\boxed{\mathbf{K}_k = \mathbf{P}_{k|k-1} \mathbf{H}_k^\top \mathbf{S}_k^{-1} = \mathbf{P}_{k|k-1} \mathbf{H}_k^\top (\mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^\top + \mathbf{R}_k)^{-1}}$$

칼만 이득의 직관적 의미:
- $\mathbf{R}_k \to \mathbf{0}$ (관측이 매우 정확): $\mathbf{K}_k \to \mathbf{H}_k^{-1}$ → 관측을 거의 그대로 믿는다
- $\mathbf{P}_{k|k-1} \to \mathbf{0}$ (예측이 매우 정확): $\mathbf{K}_k \to \mathbf{0}$ → 관측을 무시하고 예측을 믿는다
- 칼만 이득은 예측의 불확실성과 관측의 불확실성 사이의 **최적 가중치**를 자동으로 결정한다

**혁신(Innovation)**:
$$\tilde{\mathbf{y}}_k = \mathbf{z}_k - \mathbf{H}_k \hat{\mathbf{x}}_{k|k-1} \in \mathbb{R}^m$$

예측한 관측과 실제 관측의 차이. 이것이 0이면 예측이 완벽했다는 뜻이다.

**상태 갱신**:
$$\boxed{\hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_k \tilde{\mathbf{y}}_k}$$

**공분산 갱신**:
$$\boxed{\mathbf{P}_{k|k} = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k|k-1}}$$

이 공분산 갱신 공식은 Joseph form $\mathbf{P}_{k|k} = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k|k-1} (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k)^\top + \mathbf{K}_k \mathbf{R}_k \mathbf{K}_k^\top$으로 쓰면 수치적으로 더 안정적이다. Joseph form은 $\mathbf{K}_k$에 수치 오차가 있어도 $\mathbf{P}_{k|k}$의 대칭성과 양의 반정치성을 보장한다.

#### KF 최적성 정리

**정리**: 선형-가우시안 시스템에서 Kalman Filter는 다음 세 가지 의미에서 최적이다:
1. **MMSE (Minimum Mean Square Error) 추정량**: 평균 제곱 오차를 최소화
2. **MAP (Maximum A Posteriori) 추정량**: 가우시안 분포에서 평균 = 최빈값이므로 MAP과 MMSE가 일치
3. **BLUE (Best Linear Unbiased Estimator)**: 가우시안 가정 없이도, 선형 비편향 추정량 중에서 최소 분산

Kalman의 1960년 원논문은 Wiener 필터의 주파수 도메인 접근법을 상태공간(state-space) 시간 도메인 표현으로 대체하여, 시변(time-varying) 시스템과 다변량 시스템으로 자연스럽게 확장할 수 있게 한 혁신이었다.

#### Python 구현

```python
import numpy as np

class KalmanFilter:
    """선형 Kalman Filter 구현.
    
    상태 공간 모델:
        x_k = F @ x_{k-1} + B @ u_k + w_k,  w_k ~ N(0, Q)
        z_k = H @ x_k + v_k,                 v_k ~ N(0, R)
    """
    def __init__(self, F, H, Q, R, B=None):
        """
        Parameters
        ----------
        F : ndarray, shape (n, n) — 상태 전이 행렬
        H : ndarray, shape (m, n) — 관측 행렬
        Q : ndarray, shape (n, n) — 프로세스 노이즈 공분산
        R : ndarray, shape (m, m) — 관측 노이즈 공분산
        B : ndarray, shape (n, l) — 제어 입력 행렬 (optional)
        """
        self.F = F
        self.H = H
        self.Q = Q
        self.R = R
        self.B = B
        self.n = F.shape[0]
        
    def predict(self, x, P, u=None):
        """예측 단계.
        
        Parameters
        ----------
        x : ndarray, shape (n,) — 이전 상태 추정
        P : ndarray, shape (n, n) — 이전 공분산
        u : ndarray, shape (l,) — 제어 입력 (optional)
        
        Returns
        -------
        x_pred : ndarray, shape (n,) — 예측 상태
        P_pred : ndarray, shape (n, n) — 예측 공분산
        """
        x_pred = self.F @ x
        if self.B is not None and u is not None:
            x_pred += self.B @ u
        P_pred = self.F @ P @ self.F.T + self.Q
        return x_pred, P_pred
    
    def update(self, x_pred, P_pred, z):
        """갱신 단계.
        
        Parameters
        ----------
        x_pred : ndarray, shape (n,) — 예측 상태
        P_pred : ndarray, shape (n, n) — 예측 공분산
        z : ndarray, shape (m,) — 관측값
        
        Returns
        -------
        x_upd : ndarray, shape (n,) — 갱신 상태
        P_upd : ndarray, shape (n, n) — 갱신 공분산
        """
        # 혁신 (innovation)
        y = z - self.H @ x_pred                          # (m,)
        # 혁신 공분산
        S = self.H @ P_pred @ self.H.T + self.R           # (m, m)
        # 칼만 이득
        K = P_pred @ self.H.T @ np.linalg.inv(S)          # (n, m)
        # 상태 갱신
        x_upd = x_pred + K @ y                            # (n,)
        # 공분산 갱신 (Joseph form for numerical stability)
        I_KH = np.eye(self.n) - K @ self.H                # (n, n)
        P_upd = I_KH @ P_pred @ I_KH.T + K @ self.R @ K.T  # (n, n)
        return x_upd, P_upd

# 예제: 1D 등속 운동 모델에서의 위치 추정
# 상태: [position, velocity]^T
dt = 0.1  # 시간 간격
F = np.array([[1, dt],
              [0, 1]])      # (2, 2) 등속 운동 전이 행렬
H = np.array([[1, 0]])       # (1, 2) 위치만 관측
Q = np.array([[dt**3/3, dt**2/2],
              [dt**2/2, dt]]) * 0.1  # (2, 2) 프로세스 노이즈 (등가속도 모델)
R = np.array([[1.0]])         # (1, 1) 관측 노이즈 분산

kf = KalmanFilter(F, H, Q, R)

# 초기 상태
x = np.array([0.0, 1.0])  # 위치=0, 속도=1
P = np.eye(2) * 10.0       # 초기 불확실성 큼

# 시뮬레이션
np.random.seed(42)
true_positions = []
estimated_positions = []

for k in range(100):
    # Ground truth
    true_pos = 0.0 + 1.0 * k * dt  # 등속 운동
    true_positions.append(true_pos)
    
    # 예측
    x, P = kf.predict(x, P)
    
    # 노이즈가 섞인 관측
    z = np.array([true_pos + np.random.randn() * 1.0])
    
    # 갱신
    x, P = kf.update(x, P, z)
    estimated_positions.append(x[0])

print(f"최종 위치 추정: {x[0]:.3f}, 실제: {true_positions[-1]:.3f}")
print(f"최종 위치 불확실성 (1σ): {np.sqrt(P[0,0]):.3f}")
```

### 4.2.2 Extended Kalman Filter (EKF)

실제 로봇 시스템은 거의 항상 비선형이다. 카메라의 3D→2D 투영, IMU의 쿼터니언 기반 회전, LiDAR의 scan matching, GPS의 좌표 변환 모두 비선형 함수다. EKF는 Kalman Filter를 비선형 시스템으로 확장하는 가장 직접적인 방법이다.

#### 핵심 아이디어: 1차 테일러 전개 (선형화)

비선형 시스템 모델:
$$\mathbf{x}_k = f(\mathbf{x}_{k-1}, \mathbf{u}_k) + \mathbf{w}_k$$
$$\mathbf{z}_k = h(\mathbf{x}_k) + \mathbf{v}_k$$

이 시스템에서는 가우시안을 비선형 함수에 통과시키면 결과가 더 이상 가우시안이 아니다. EKF의 핵심 근사는 **현재 추정치 근방에서 함수를 1차 테일러 전개하여 선형화**하는 것이다.

운동 모델의 선형화 (추정치 $\hat{\mathbf{x}}_{k-1|k-1}$ 근방에서):

$$f(\mathbf{x}_{k-1}, \mathbf{u}_k) \approx f(\hat{\mathbf{x}}_{k-1|k-1}, \mathbf{u}_k) + \mathbf{F}_k (\mathbf{x}_{k-1} - \hat{\mathbf{x}}_{k-1|k-1})$$

$$\mathbf{F}_k = \left.\frac{\partial f}{\partial \mathbf{x}}\right|_{\hat{\mathbf{x}}_{k-1|k-1}, \mathbf{u}_k} \in \mathbb{R}^{n \times n}$$

관측 모델의 선형화 (예측 추정치 $\hat{\mathbf{x}}_{k|k-1}$ 근방에서):

$$h(\mathbf{x}_k) \approx h(\hat{\mathbf{x}}_{k|k-1}) + \mathbf{H}_k (\mathbf{x}_k - \hat{\mathbf{x}}_{k|k-1})$$

$$\mathbf{H}_k = \left.\frac{\partial h}{\partial \mathbf{x}}\right|_{\hat{\mathbf{x}}_{k|k-1}} \in \mathbb{R}^{m \times n}$$

#### EKF 알고리즘

**예측 단계**:
$$\hat{\mathbf{x}}_{k|k-1} = f(\hat{\mathbf{x}}_{k-1|k-1}, \mathbf{u}_k)$$
$$\mathbf{P}_{k|k-1} = \mathbf{F}_k \mathbf{P}_{k-1|k-1} \mathbf{F}_k^\top + \mathbf{Q}_k$$

주의: 상태 예측에는 비선형 함수 $f$를 그대로 사용하지만, 공분산 전파에는 자코비안 $\mathbf{F}_k$를 사용한다.

**갱신 단계**:
$$\tilde{\mathbf{y}}_k = \mathbf{z}_k - h(\hat{\mathbf{x}}_{k|k-1})$$
$$\mathbf{S}_k = \mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^\top + \mathbf{R}_k$$
$$\mathbf{K}_k = \mathbf{P}_{k|k-1} \mathbf{H}_k^\top \mathbf{S}_k^{-1}$$
$$\hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_k \tilde{\mathbf{y}}_k$$
$$\mathbf{P}_{k|k} = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k|k-1}$$

혁신 $\tilde{\mathbf{y}}_k$에서도 비선형 함수 $h$를 직접 사용한다는 점에 주의한다.

#### EKF의 한계

1. **선형화 오차**: 비선형성이 심할수록 1차 근사의 오차가 커진다. 이는 필터의 일관성(consistency)을 해칠 수 있다. 실제 오차가 필터가 추정한 불확실성보다 크게 벌어지기 때문이다.

2. **자코비안 계산 부담**: 모든 시간 단계에서 $\mathbf{F}_k$와 $\mathbf{H}_k$의 해석적 미분을 구해야 한다. 시스템이 복잡하면 자코비안 유도가 매우 번거롭고 오류가 발생하기 쉽다.

3. **단봉(unimodal) 가정**: 가우시안은 항상 단봉이므로, 다봉 사후 분포를 표현할 수 없다.

### 4.2.3 Error-State Kalman Filter (ESKF)

ESKF(Error-State Kalman Filter)는 현대 로봇 센서 퓨전 시스템에서 가장 널리 사용되는 필터 형태다. MSCKF, VINS-Mono, OpenVINS, FAST-LIO 등 거의 모든 주요 VIO/LIO 시스템이 ESKF를 채택하고 있다.

#### 왜 EKF 대신 ESKF를 쓰는가

3D 자세(orientation)를 포함하는 로봇 상태를 추정할 때 EKF를 직접 쓰면 여러 문제가 발생한다:

**문제 1: 회전의 비유클리드 특성**

3D 회전은 SO(3) 매니폴드 위에 존재하며, $\mathbb{R}^n$이 아니다. 쿼터니언 $\mathbf{q} \in \mathbb{H}$로 표현하면 $\|\mathbf{q}\| = 1$ 제약이 있고, 회전 행렬 $\mathbf{R} \in SO(3)$은 $\mathbf{R}^\top \mathbf{R} = \mathbf{I}$, $\det(\mathbf{R}) = 1$ 제약이 있다.

EKF의 상태 갱신 $\hat{\mathbf{x}} \leftarrow \hat{\mathbf{x}} + \mathbf{K} \tilde{\mathbf{y}}$에서 "+" 연산은 유클리드 공간의 덧셈이다. 쿼터니언에 벡터를 더하면 단위 쿼터니언이 아니게 된다. 갱신 후 재정규화하는 임시방편은 이론적으로 올바르지 않으며, 일관성 문제를 야기한다.

**문제 2: 오차 상태는 "거의 0"**

오차 상태(error state) $\delta\mathbf{x} = \mathbf{x} - \hat{\mathbf{x}}$는 정의상 0 근처에 머문다 (갱신 때마다 리셋되므로). 따라서 1차 선형화의 정확도가 매우 높다. 반면 원래 상태에 대한 선형화는 운동이 큰 경우 정확도가 떨어진다.

**문제 3: 느린 변화(slow-varying) 상태와 빠른 변화(fast-varying) 상태의 분리**

IMU 바이어스처럼 느리게 변하는 상태와 속도/자세처럼 빠르게 변하는 상태를 분리하여 처리하면, 각각에 적합한 업데이트 전략을 적용할 수 있다.

#### ESKF 구조

ESKF는 두 개의 상태를 동시에 관리한다:

1. **명목 상태(Nominal State)** $\hat{\mathbf{x}}$: 비선형 운동 모델을 따라 적분되며, 노이즈 항을 포함하지 않는다. 불확실성을 추적하지 않는다.

2. **오차 상태(Error State)** $\delta\mathbf{x}$: 명목 상태와 실제 상태의 차이. 칼만 필터로 추정한다. 오차 상태는 정의상 "작은 값"이므로 선형화 오차가 최소화된다.

실제 상태는 두 상태의 합성(composition)으로 복원된다:

$$\mathbf{x}_{\text{true}} = \hat{\mathbf{x}} \boxplus \delta\mathbf{x}$$

여기서 $\boxplus$는 매니폴드 위의 합성 연산이다. 유클리드 성분에서는 일반 덧셈이고, 회전 성분에서는:

$$\mathbf{R}_{\text{true}} = \hat{\mathbf{R}} \cdot \text{Exp}(\delta\boldsymbol{\theta})$$

또는 쿼터니언으로:

$$\mathbf{q}_{\text{true}} = \hat{\mathbf{q}} \otimes \begin{bmatrix} 1 \\ \frac{1}{2}\delta\boldsymbol{\theta} \end{bmatrix} \approx \hat{\mathbf{q}} \otimes \delta\mathbf{q}$$

여기서 $\delta\boldsymbol{\theta} \in \mathbb{R}^3$는 회전 오차의 각축(angle-axis) 표현이다.

#### IMU 기반 ESKF의 상태 벡터

전형적인 IMU-camera/LiDAR 퓨전 시스템에서의 상태 벡터:

**명목 상태** (16차원, 쿼터니언 사용시):
$$\hat{\mathbf{x}} = \begin{bmatrix} {}^W\hat{\mathbf{p}} \\ {}^W\hat{\mathbf{v}} \\ \hat{\mathbf{q}}_{WB} \\ \hat{\mathbf{b}}_a \\ \hat{\mathbf{b}}_g \end{bmatrix} \in \mathbb{R}^{3} \times \mathbb{R}^{3} \times \mathbb{S}^3 \times \mathbb{R}^{3} \times \mathbb{R}^{3}$$

**오차 상태** (15차원 — 회전의 최소 파라미터화):
$$\delta\mathbf{x} = \begin{bmatrix} \delta\mathbf{p} \\ \delta\mathbf{v} \\ \delta\boldsymbol{\theta} \\ \delta\mathbf{b}_a \\ \delta\mathbf{b}_g \end{bmatrix} \in \mathbb{R}^{15}$$

핵심: 쿼터니언(4차원)의 오차 표현이 3차원 벡터 $\delta\boldsymbol{\theta}$다. 단위 쿼터니언 제약 때문에 실제 자유도는 3이고, ESKF는 이 최소 파라미터화를 자연스럽게 사용한다. EKF에서 쿼터니언을 직접 상태에 넣으면 4차원 표현의 1개 자유도가 과잉이 되어 공분산 행렬이 singular해지는 문제가 있다.

#### ESKF 알고리즘

**1단계: 명목 상태 전파 (IMU mechanization)**

IMU 측정 $(\tilde{\boldsymbol{\omega}}_k, \tilde{\mathbf{a}}_k)$로부터 명목 상태를 적분한다. 바이어스를 빼고 노이즈 항은 무시한다:

$$\hat{\mathbf{q}}_{k+1} = \hat{\mathbf{q}}_k \otimes \mathbf{q}\{(\tilde{\boldsymbol{\omega}}_k - \hat{\mathbf{b}}_{g,k}) \Delta t\}$$
$$\hat{\mathbf{v}}_{k+1} = \hat{\mathbf{v}}_k + (\hat{\mathbf{R}}_k (\tilde{\mathbf{a}}_k - \hat{\mathbf{b}}_{a,k}) + \mathbf{g}) \Delta t$$
$$\hat{\mathbf{p}}_{k+1} = \hat{\mathbf{p}}_k + \hat{\mathbf{v}}_k \Delta t + \frac{1}{2}(\hat{\mathbf{R}}_k (\tilde{\mathbf{a}}_k - \hat{\mathbf{b}}_{a,k}) + \mathbf{g}) \Delta t^2$$
$$\hat{\mathbf{b}}_{a,k+1} = \hat{\mathbf{b}}_{a,k}$$
$$\hat{\mathbf{b}}_{g,k+1} = \hat{\mathbf{b}}_{g,k}$$

여기서 $\hat{\mathbf{R}}_k = \mathbf{R}(\hat{\mathbf{q}}_k) \in SO(3)$이고, $\mathbf{g} = [0, 0, -9.81]^\top \, \text{m/s}^2$는 중력 벡터이다.

**2단계: 오차 상태 전파 (prediction)**

오차 상태의 연속 시간 동역학을 유도한다. 실제 IMU 측정 $\tilde{\boldsymbol{\omega}} = \boldsymbol{\omega} + \mathbf{b}_g + \mathbf{n}_g$, $\tilde{\mathbf{a}} = \mathbf{R}^\top(\mathbf{a}_W - \mathbf{g}) + \mathbf{b}_a + \mathbf{n}_a$를 대입하고 명목 상태와의 차이를 취하면:

$$\delta\dot{\boldsymbol{\theta}} = -[\tilde{\boldsymbol{\omega}} - \hat{\mathbf{b}}_g]_\times \delta\boldsymbol{\theta} - \delta\mathbf{b}_g - \mathbf{n}_g$$
$$\delta\dot{\mathbf{v}} = -\hat{\mathbf{R}}[\tilde{\mathbf{a}} - \hat{\mathbf{b}}_a]_\times \delta\boldsymbol{\theta} - \hat{\mathbf{R}} \delta\mathbf{b}_a - \hat{\mathbf{R}} \mathbf{n}_a$$
$$\delta\dot{\mathbf{p}} = \delta\mathbf{v}$$
$$\delta\dot{\mathbf{b}}_a = \mathbf{n}_{ba}$$
$$\delta\dot{\mathbf{b}}_g = \mathbf{n}_{bg}$$

여기서 $[\mathbf{a}]_\times$는 벡터 $\mathbf{a}$의 반대칭 행렬(skew-symmetric matrix):

$$[\mathbf{a}]_\times = \begin{bmatrix} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{bmatrix} \in \mathbb{R}^{3 \times 3}$$

이를 행렬 형태로 쓰면:

$$\delta\dot{\mathbf{x}} = \mathbf{F}_c \delta\mathbf{x} + \mathbf{G}_c \mathbf{n}$$

$$\mathbf{F}_c = \begin{bmatrix}
\mathbf{0}_3 & \mathbf{I}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 \\
\mathbf{0}_3 & \mathbf{0}_3 & -\hat{\mathbf{R}}[\tilde{\mathbf{a}} - \hat{\mathbf{b}}_a]_\times & -\hat{\mathbf{R}} & \mathbf{0}_3 \\
\mathbf{0}_3 & \mathbf{0}_3 & -[\tilde{\boldsymbol{\omega}} - \hat{\mathbf{b}}_g]_\times & \mathbf{0}_3 & -\mathbf{I}_3 \\
\mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 \\
\mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3
\end{bmatrix} \in \mathbb{R}^{15 \times 15}$$

이산화 (1차 근사): $\mathbf{F}_d \approx \mathbf{I} + \mathbf{F}_c \Delta t$

$$\mathbf{P}_{k+1|k} = \mathbf{F}_d \mathbf{P}_{k|k} \mathbf{F}_d^\top + \mathbf{G}_d \mathbf{Q}_d \mathbf{G}_d^\top$$

**3단계: 관측 업데이트**

카메라/LiDAR 관측이 들어오면 표준 EKF 업데이트를 오차 상태에 대해 수행한다. 관측 모델 $\mathbf{z} = h(\mathbf{x}_{\text{true}})$를 오차 상태에 대해 선형화:

$$\mathbf{z} - h(\hat{\mathbf{x}}) \approx \mathbf{H} \delta\mathbf{x} + \mathbf{v}$$

$$\mathbf{H} = \frac{\partial h}{\partial \delta\mathbf{x}}\bigg|_{\hat{\mathbf{x}}} \in \mathbb{R}^{m \times 15}$$

이 자코비안은 체인 룰로 계산한다:

$$\mathbf{H} = \frac{\partial h}{\partial \mathbf{x}_{\text{true}}} \cdot \frac{\partial \mathbf{x}_{\text{true}}}{\partial \delta\mathbf{x}}\bigg|_{\delta\mathbf{x}=\mathbf{0}}$$

표준 칼만 갱신:
$$\mathbf{K} = \mathbf{P}_{k|k-1} \mathbf{H}^\top (\mathbf{H} \mathbf{P}_{k|k-1} \mathbf{H}^\top + \mathbf{R})^{-1}$$
$$\delta\hat{\mathbf{x}} = \mathbf{K}(\mathbf{z} - h(\hat{\mathbf{x}}))$$
$$\mathbf{P}_{k|k} = (\mathbf{I} - \mathbf{K}\mathbf{H})\mathbf{P}_{k|k-1}$$

**4단계: 오차 상태 주입 및 리셋**

오차 상태 추정치 $\delta\hat{\mathbf{x}}$를 명목 상태에 주입(inject)한다:

$$\hat{\mathbf{p}} \leftarrow \hat{\mathbf{p}} + \delta\hat{\mathbf{p}}$$
$$\hat{\mathbf{v}} \leftarrow \hat{\mathbf{v}} + \delta\hat{\mathbf{v}}$$
$$\hat{\mathbf{q}} \leftarrow \hat{\mathbf{q}} \otimes \mathbf{q}\{\delta\hat{\boldsymbol{\theta}}\}$$
$$\hat{\mathbf{b}}_a \leftarrow \hat{\mathbf{b}}_a + \delta\hat{\mathbf{b}}_a$$
$$\hat{\mathbf{b}}_g \leftarrow \hat{\mathbf{b}}_g + \delta\hat{\mathbf{b}}_g$$

주입 후, 오차 상태를 $\delta\hat{\mathbf{x}} \leftarrow \mathbf{0}$으로 리셋한다. 공분산도 리셋 자코비안으로 변환해야 한다:

$$\mathbf{P} \leftarrow \mathbf{G} \mathbf{P} \mathbf{G}^\top$$

여기서 $\mathbf{G} = \frac{\partial (\delta\mathbf{x} \boxminus \delta\hat{\mathbf{x}})}{\partial \delta\mathbf{x}}\big|_{\delta\hat{\mathbf{x}}}$이다. 실전에서 $\delta\hat{\boldsymbol{\theta}}$가 작으면 $\mathbf{G} \approx \mathbf{I}$로 근사하는 경우가 많다.

### 4.2.4 Unscented Kalman Filter (UKF)

UKF는 "분포를 근사하는 것이 비선형 함수를 근사하는 것보다 쉽다"는 통찰에 기반한다. EKF는 비선형 함수를 선형화하여 근사하지만, UKF는 비선형 함수는 그대로 두고 분포를 유한 개의 **sigma point**로 근사한다.

#### Unscented Transform

$n$차원 가우시안 확률 변수 $\mathbf{x} \sim \mathcal{N}(\hat{\mathbf{x}}, \mathbf{P})$에 대해, $2n+1$개의 sigma point와 가중치를 생성한다:

$$\boldsymbol{\chi}_0 = \hat{\mathbf{x}}, \quad w_0 = \frac{\lambda}{n + \lambda}$$
$$\boldsymbol{\chi}_i = \hat{\mathbf{x}} + \left(\sqrt{(n+\lambda)\mathbf{P}}\right)_i, \quad w_i = \frac{1}{2(n+\lambda)}, \quad i = 1, \ldots, n$$
$$\boldsymbol{\chi}_{n+i} = \hat{\mathbf{x}} - \left(\sqrt{(n+\lambda)\mathbf{P}}\right)_i, \quad w_{n+i} = \frac{1}{2(n+\lambda)}, \quad i = 1, \ldots, n$$

여기서:
- $\lambda = \alpha^2(n + \kappa) - n$는 스케일링 파라미터 ($\alpha$: sigma point 분산 조절, $\kappa$: 보조 파라미터, 보통 $\kappa = 0$ 또는 $3-n$)
- $\left(\sqrt{(n+\lambda)\mathbf{P}}\right)_i$는 행렬 $(n+\lambda)\mathbf{P}$의 Cholesky 분해의 $i$번째 열

각 sigma point를 비선형 함수에 통과시킨다:

$$\boldsymbol{\gamma}_i = f(\boldsymbol{\chi}_i)$$

변환된 sigma point들로부터 평균과 공분산을 복원한다:

$$\hat{\mathbf{y}} = \sum_{i=0}^{2n} w_i^{(m)} \boldsymbol{\gamma}_i$$
$$\mathbf{P}_y = \sum_{i=0}^{2n} w_i^{(c)} (\boldsymbol{\gamma}_i - \hat{\mathbf{y}})(\boldsymbol{\gamma}_i - \hat{\mathbf{y}})^\top$$

$w_i^{(m)}$과 $w_i^{(c)}$는 평균과 공분산에 각각 사용하는 가중치로, $w_0^{(c)} = w_0^{(m)} + (1 - \alpha^2 + \beta)$ ($\beta = 2$는 가우시안에 최적)이고, 나머지는 동일하다.

#### UKF 알고리즘

**예측 단계**:
1. 현재 상태 $(\hat{\mathbf{x}}_{k-1|k-1}, \mathbf{P}_{k-1|k-1})$에서 sigma point 생성
2. 각 sigma point를 운동 모델에 통과: $\boldsymbol{\chi}_{k|k-1}^{(i)} = f(\boldsymbol{\chi}_{k-1|k-1}^{(i)}, \mathbf{u}_k)$
3. 예측 평균과 공분산 계산: $\hat{\mathbf{x}}_{k|k-1} = \sum w_i^{(m)} \boldsymbol{\chi}_{k|k-1}^{(i)}$
4. $\mathbf{P}_{k|k-1} = \sum w_i^{(c)} (\boldsymbol{\chi}_{k|k-1}^{(i)} - \hat{\mathbf{x}}_{k|k-1})(\cdots)^\top + \mathbf{Q}_k$

**갱신 단계**:
1. 예측 상태에서 sigma point 재생성 (또는 예측 단계의 sigma point를 재사용)
2. 관측 모델에 통과: $\boldsymbol{\zeta}_k^{(i)} = h(\boldsymbol{\chi}_{k|k-1}^{(i)})$
3. 예측 관측 평균: $\hat{\mathbf{z}}_k = \sum w_i^{(m)} \boldsymbol{\zeta}_k^{(i)}$
4. 관측 공분산: $\mathbf{P}_{zz} = \sum w_i^{(c)} (\boldsymbol{\zeta}_k^{(i)} - \hat{\mathbf{z}}_k)(\cdots)^\top + \mathbf{R}_k$
5. 교차 공분산: $\mathbf{P}_{xz} = \sum w_i^{(c)} (\boldsymbol{\chi}_{k|k-1}^{(i)} - \hat{\mathbf{x}}_{k|k-1})(\boldsymbol{\zeta}_k^{(i)} - \hat{\mathbf{z}}_k)^\top$
6. 칼만 이득: $\mathbf{K}_k = \mathbf{P}_{xz} \mathbf{P}_{zz}^{-1}$
7. 갱신: $\hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_k (\mathbf{z}_k - \hat{\mathbf{z}}_k)$
8. $\mathbf{P}_{k|k} = \mathbf{P}_{k|k-1} - \mathbf{K}_k \mathbf{P}_{zz} \mathbf{K}_k^\top$

#### UKF의 장단점

**장점**:
- 자코비안 계산이 불필요하다. 복잡한 관측 모델(예: 카메라 투영 + 왜곡)에서 큰 이점.
- 2차 비선형성까지 정확히 포착한다 (EKF는 1차까지만).
- 구현이 EKF보다 간단할 수 있다 (자코비안 유도 대신 함수 호출만 필요).

**단점**:
- $2n+1$개 sigma point 각각을 비선형 함수에 통과시켜야 하므로, 상태 차원 $n$이 클 때 연산량이 증가한다.
- 매니폴드 위의 상태(SO(3) 등)를 다루려면 sigma point의 생성과 통계 계산을 매니폴드 연산으로 대체해야 하며, 이것이 깔끔하지 않다.
- 실전에서 ESKF가 UKF보다 선호되는 이유: ESKF는 이미 오차 상태(작은 값)에서 동작하므로 1차 선형화의 정확도가 충분하고, 매니폴드 처리가 자연스러우며, 계산량이 적다.

### 4.2.5 Iterated Extended Kalman Filter (IEKF)

IEKF는 EKF의 갱신 단계에서 선형화를 한 번만 하는 대신, 반복적으로 재선형화하여 비선형 관측 모델의 처리 정확도를 높이는 방법이다.

#### 동기: EKF 갱신의 선형화 오차

EKF에서 관측 모델의 자코비안 $\mathbf{H}_k$는 예측 추정치 $\hat{\mathbf{x}}_{k|k-1}$에서 계산된다. 그런데 갱신 후의 추정치 $\hat{\mathbf{x}}_{k|k}$가 $\hat{\mathbf{x}}_{k|k-1}$과 크게 다르면, 선형화 지점이 최적이 아니게 된다. IEKF는 갱신 후의 추정치에서 다시 선형화하고 갱신을 반복함으로써 이 문제를 완화한다.

#### IEKF 알고리즘

예측 단계는 EKF와 동일하다. 갱신 단계에서 반복을 수행한다:

초기화: $\hat{\mathbf{x}}^{(0)} = \hat{\mathbf{x}}_{k|k-1}$

$j = 0, 1, 2, \ldots$ 에 대해 수렴할 때까지:

$$\mathbf{H}^{(j)} = \left.\frac{\partial h}{\partial \mathbf{x}}\right|_{\hat{\mathbf{x}}^{(j)}}$$

$$\mathbf{K}^{(j)} = \mathbf{P}_{k|k-1} \mathbf{H}^{(j)\top} (\mathbf{H}^{(j)} \mathbf{P}_{k|k-1} \mathbf{H}^{(j)\top} + \mathbf{R}_k)^{-1}$$

$$\hat{\mathbf{x}}^{(j+1)} = \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}^{(j)} \left[\mathbf{z}_k - h(\hat{\mathbf{x}}^{(j)}) - \mathbf{H}^{(j)}(\hat{\mathbf{x}}_{k|k-1} - \hat{\mathbf{x}}^{(j)})\right]$$

수렴 후: $\hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}^{(j+1)}$, $\mathbf{P}_{k|k} = (\mathbf{I} - \mathbf{K}^{(j)} \mathbf{H}^{(j)}) \mathbf{P}_{k|k-1}$

IEKF는 사실상 관측 업데이트 단계에서 **Gauss-Newton 최적화**를 수행하는 것과 동치이다. 이 관점은 §4.5의 factor graph 기반 최적화와의 연결을 이해할 때 중요하다.

FAST-LIO2가 IEKF를 채택한 이유: LiDAR point-to-plane/point-to-edge 관측 모델은 비선형성이 상당하며, 수백~수천 개의 점을 한 번에 갱신해야 한다. IEKF의 반복 선형화는 이 상황에서 한 번의 EKF 업데이트보다 상당히 정확한 결과를 준다.

---

## 4.3 Particle Filter

### 4.3.1 Sequential Monte Carlo (SMC) 개요

Particle Filter(PF), 또는 Sequential Monte Carlo(SMC) 방법은 사후 분포를 가중치가 부여된 샘플(particle)들의 집합으로 표현한다. 가우시안 가정이 필요 없으므로, 다봉(multimodal) 분포나 강한 비선형 시스템을 다룰 수 있다.

사후 분포의 입자 근사:

$$p(\mathbf{x}_k \mid \mathbf{z}_{1:k}) \approx \sum_{i=1}^{N} w_k^{(i)} \delta(\mathbf{x}_k - \mathbf{x}_k^{(i)})$$

여기서:
- $N$: 입자 수
- $\mathbf{x}_k^{(i)}$: $i$번째 입자의 상태
- $w_k^{(i)}$: $i$번째 입자의 가중치 ($\sum_{i=1}^N w_k^{(i)} = 1$)
- $\delta(\cdot)$: 디랙 델타 함수

$N \to \infty$이면 입자 분포는 실제 사후 분포에 수렴한다. 실전에서는 유한 개의 입자로 근사하며, 입자 수는 문제의 복잡도와 상태 공간의 차원에 따라 결정한다.

### 4.3.2 Importance Sampling

사후 분포 $p(\mathbf{x}_k \mid \mathbf{z}_{1:k})$에서 직접 샘플링하는 것은 일반적으로 불가능하다. 대신 **제안 분포(proposal distribution)** $q(\mathbf{x}_k \mid \mathbf{x}_{k-1}, \mathbf{z}_k)$에서 샘플을 뽑고, 중요도 가중치(importance weight)로 보정한다.

재귀적 가중치 갱신을 유도한다. Bayes 정리에서:

$$p(\mathbf{x}_{0:k} \mid \mathbf{z}_{1:k}) = \frac{p(\mathbf{z}_k \mid \mathbf{x}_k) \, p(\mathbf{x}_k \mid \mathbf{x}_{k-1}) \, p(\mathbf{x}_{0:k-1} \mid \mathbf{z}_{1:k-1})}{p(\mathbf{z}_k \mid \mathbf{z}_{1:k-1})}$$

제안 분포로 나누어 중요도 비율을 구하면:

$$w_k^{(i)} \propto w_{k-1}^{(i)} \cdot \frac{p(\mathbf{z}_k \mid \mathbf{x}_k^{(i)}) \, p(\mathbf{x}_k^{(i)} \mid \mathbf{x}_{k-1}^{(i)})}{q(\mathbf{x}_k^{(i)} \mid \mathbf{x}_{k-1}^{(i)}, \mathbf{z}_k)}$$

**가장 간단한 제안 분포**: 전이 사전(transition prior)을 제안 분포로 사용, 즉 $q(\mathbf{x}_k \mid \mathbf{x}_{k-1}, \mathbf{z}_k) = p(\mathbf{x}_k \mid \mathbf{x}_{k-1})$. 이 경우 가중치가 간단해진다:

$$w_k^{(i)} \propto w_{k-1}^{(i)} \cdot p(\mathbf{z}_k \mid \mathbf{x}_k^{(i)})$$

각 입자의 가중치는 해당 입자 위치에서의 관측 likelihood에 비례한다. 직관적으로, 관측과 일치하는 입자는 높은 가중치를, 일치하지 않는 입자는 낮은 가중치를 받는다.

최적 제안 분포는 $q^*(\mathbf{x}_k \mid \mathbf{x}_{k-1}^{(i)}, \mathbf{z}_k) = p(\mathbf{x}_k \mid \mathbf{x}_{k-1}^{(i)}, \mathbf{z}_k)$이지만, 대부분의 경우 이를 구할 수 없다.

### 4.3.3 Resampling

제안 분포로 입자를 전파하다 보면 **가중치 퇴화(weight degeneracy)** 문제가 발생한다: 몇 개의 입자에 가중치가 집중되고 나머지 입자들은 무시해도 될 만큼 작은 가중치를 갖게 된다. 이러면 사실상 소수의 입자만 유효하므로 근사 품질이 급격히 저하된다.

유효 입자 수(effective sample size)로 퇴화를 진단한다:

$$N_{\text{eff}} = \frac{1}{\sum_{i=1}^N (w_k^{(i)})^2}$$

$N_{\text{eff}} < N_{\text{threshold}}$ (보통 $N/2$)이면 리샘플링을 수행한다.

**리샘플링**: 가중치가 높은 입자를 복제하고 낮은 입자를 제거하여 가중치를 균등하게 만드는 과정.

주요 리샘플링 전략:

**Multinomial Resampling**: 가중치를 확률로 사용하여 $N$번 독립 추출. 가장 직관적이지만 분산이 크다.

**Systematic Resampling**: 하나의 균등 난수 $U_0 \sim \text{Uniform}(0, 1/N)$을 생성하고, $U_i = U_0 + (i-1)/N$으로 CDF를 타서 리샘플링. 분산이 가장 작아 실전에서 가장 많이 사용된다.

**Stratified Resampling**: 각 층에서 독립 균등 난수를 사용. Systematic과 multinomial의 중간.

```python
import numpy as np

def systematic_resampling(weights, N):
    """체계적 리샘플링 (Systematic Resampling).
    
    Parameters
    ----------
    weights : ndarray, shape (N,) — 정규화된 가중치
    N : int — 리샘플링할 입자 수
    
    Returns
    -------
    indices : ndarray, shape (N,) — 선택된 입자의 인덱스
    """
    cumsum = np.cumsum(weights)
    u0 = np.random.uniform(0, 1.0 / N)
    u = u0 + np.arange(N) / N
    indices = np.searchsorted(cumsum, u)
    return indices

def bootstrap_particle_filter(f, h, Q, R, z_seq, N=1000, x0_sampler=None):
    """Bootstrap Particle Filter (SIR: Sampling Importance Resampling).
    
    제안 분포 = 전이 사전 (가장 기본적인 PF)
    
    Parameters
    ----------
    f : callable — 상태 전이 함수 f(x, noise) → x_next
    h : callable — 관측 함수 h(x) → z_predicted
    Q : ndarray — 프로세스 노이즈 공분산
    R : ndarray — 관측 노이즈 공분산
    z_seq : list of ndarray — 관측 시퀀스
    N : int — 입자 수
    x0_sampler : callable — 초기 입자 샘플러 (없으면 N(0,I))
    
    Returns
    -------
    x_est : list of ndarray — 각 시각의 가중 평균 추정치
    """
    n = Q.shape[0]
    m = R.shape[0]
    T = len(z_seq)
    
    # 초기화
    if x0_sampler:
        particles = np.array([x0_sampler() for _ in range(N)])  # (N, n)
    else:
        particles = np.random.randn(N, n)
    weights = np.ones(N) / N
    
    x_est = []
    L_Q = np.linalg.cholesky(Q)
    
    for k in range(T):
        # 1. 예측: 전이 모델을 통해 입자 전파
        noise = (L_Q @ np.random.randn(n, N)).T  # (N, n)
        particles = np.array([f(particles[i], noise[i]) for i in range(N)])
        
        # 2. 가중치 갱신: 관측 likelihood
        for i in range(N):
            z_pred = h(particles[i])
            innovation = z_seq[k] - z_pred
            # 가우시안 likelihood
            log_w = -0.5 * innovation @ np.linalg.solve(R, innovation)
            weights[i] *= np.exp(log_w)
        
        # 정규화
        weights /= np.sum(weights)
        
        # 가중 평균 추정치
        x_est.append(np.average(particles, weights=weights, axis=0))
        
        # 3. 리샘플링 (유효 입자 수가 임계값 미만이면)
        N_eff = 1.0 / np.sum(weights ** 2)
        if N_eff < N / 2:
            indices = systematic_resampling(weights, N)
            particles = particles[indices]
            weights = np.ones(N) / N
    
    return x_est
```

### 4.3.4 Rao-Blackwellized Particle Filter (RBPF)

RBPF는 상태 공간을 두 부분으로 분할하여 일부는 입자로, 나머지는 해석적(예: 칼만 필터)으로 추정하는 방법이다. 이렇게 하면 입자 필터가 담당하는 차원이 줄어들어 훨씬 적은 수의 입자로도 좋은 근사가 가능하다.

상태를 $\mathbf{x} = [\mathbf{x}_1, \mathbf{x}_2]$로 분할한다고 하자. $\mathbf{x}_1$이 주어지면 $\mathbf{x}_2$의 조건부 분포가 해석적으로 (예: 가우시안으로) 추적 가능하다면:

$$p(\mathbf{x}_1, \mathbf{x}_2 \mid \mathbf{z}_{1:k}) = p(\mathbf{x}_2 \mid \mathbf{x}_1, \mathbf{z}_{1:k}) \cdot p(\mathbf{x}_1 \mid \mathbf{z}_{1:k})$$

- $p(\mathbf{x}_1 \mid \mathbf{z}_{1:k})$: 입자 필터로 근사
- $p(\mathbf{x}_2 \mid \mathbf{x}_1, \mathbf{z}_{1:k})$: 각 입자에 부착된 칼만 필터로 추적

**Rao-Blackwell 정리**: 이 분할에 의한 추정량의 분산은 순수 입자 필터의 분산보다 항상 작거나 같다.

$$\text{Var}[\hat{\mathbf{x}}_{\text{RBPF}}] \leq \text{Var}[\hat{\mathbf{x}}_{\text{PF}}]$$

**FastSLAM과의 연결**: FastSLAM은 RBPF의 대표적 응용이다. 로봇 경로(trajectory)를 입자로, 랜드마크 위치를 각 입자에 부착된 개별 EKF로 추적한다.

- $\mathbf{x}_1 = \mathbf{x}_{0:k}^{\text{robot}}$ (로봇 경로) → 입자 필터
- $\mathbf{x}_2 = \{\mathbf{m}_1, \ldots, \mathbf{m}_M\}$ (랜드마크) → 각 입자마다 $M$개의 독립 2D EKF

로봇 경로가 주어지면 각 랜드마크의 관측들은 서로 독립이 되므로(조건부 독립), 하나의 거대한 EKF 대신 $M$개의 소형 EKF를 독립적으로 운영할 수 있다. 이것이 EKF-SLAM의 $O(M^2)$ 복잡도를 FastSLAM의 $O(M \log M)$으로 낮추는 핵심이다.

### 4.3.5 Particle Filter의 한계와 현재 위치

PF의 가장 큰 한계는 **차원의 저주(curse of dimensionality)**다. 상태 공간의 차원이 높아지면 의미 있는 근사를 위해 필요한 입자 수가 지수적으로 증가한다. 전형적인 VIO/LIO 시스템의 상태 벡터는 15차원 이상이므로, 순수 PF는 실용적이지 않다.

따라서 현대 로봇 시스템에서 PF의 역할은 제한적이다:

- **2D SLAM (RBPF 기반)**: GMapping 같은 2D 점유 격자 SLAM에서 여전히 사용. 로봇 자세(3-DoF)만 입자로, 맵은 각 입자에 부착된 격자로 관리.
- **Global Localization (MCL)**: 이미 만들어진 맵에서 로봇의 초기 위치를 모를 때 (kidnapped robot problem). 다봉 분포를 자연스럽게 표현할 수 있으므로 적합하다.
- **저차원 비선형 추정**: 상태 차원이 낮고 비선형성이 심한 특수 문제.

고차원 상태 추정은 Kalman 필터 계열 (특히 ESKF) 또는 factor graph 기반 최적화가 지배하고 있다.

---

## 4.4 Smoothing vs Filtering

### 4.4.1 Filtering과 Smoothing의 차이

**Filtering**: 현재까지의 관측을 사용하여 현재 상태를 추정한다.
$$p(\mathbf{x}_k \mid \mathbf{z}_{1:k})$$

**Smoothing**: 모든 관측 (미래 포함)을 사용하여 과거 상태를 추정한다.
$$p(\mathbf{x}_k \mid \mathbf{z}_{1:T}), \quad k < T$$

Smoother는 "미래의 관측"을 활용하므로, 같은 시각의 추정치가 filter보다 항상 같거나 더 정확하다. 단, 실시간 추정에는 filter가 필요하고, smoother는 후처리(batch) 또는 지연(fixed-lag) 형태로 사용된다.

### 4.4.2 Fixed-Lag Smoother

Fixed-lag smoother는 현재 시각 $k$에서 $L$단계 이전까지의 관측을 활용하여 시각 $k-L$의 상태를 추정한다:

$$p(\mathbf{x}_{k-L} \mid \mathbf{z}_{1:k})$$

이것은 filtering과 full smoothing의 절충이다. $L$만큼의 지연(latency)을 허용하면 더 나은 추정을 얻을 수 있다.

VINS-Mono, ORB-SLAM3 등의 슬라이딩 윈도우 최적화 시스템은 사실상 fixed-lag smoother다. 윈도우 내의 키프레임들을 동시에 최적화하므로, 단순 필터링보다 정확하다.

### 4.4.3 Full Smoothing (Batch Optimization)

전체 궤적의 모든 상태를 모든 관측을 사용하여 동시에 추정한다:

$$p(\mathbf{x}_{0:T} \mid \mathbf{z}_{1:T})$$

이것을 MAP(Maximum A Posteriori) 추정으로 풀면:

$$\mathbf{x}_{0:T}^* = \arg\max_{\mathbf{x}_{0:T}} p(\mathbf{x}_{0:T} \mid \mathbf{z}_{1:T})$$

가우시안 노이즈 가정 하에서 MAP는 비선형 최소자승(Nonlinear Least Squares, NLS) 문제가 된다. 이것이 factor graph 기반 최적화의 출발점이다.

### 4.4.4 왜 현대 SLAM은 Filtering에서 Optimization으로 갔는가

2000년대 초반까지 SLAM은 EKF-SLAM이 주류였다. 그러나 점차 graph-based optimization(= batch smoothing)으로 이동했다. 그 이유:

**1. 선형화 지점의 문제 (Linearization Point)**

EKF는 "한 번 선형화하면 끝"이다. 시각 $k$에서의 자코비안은 시각 $k$의 추정치에서 계산되고, 이후에 더 나은 추정치를 얻어도 과거의 자코비안을 수정하지 않는다. 반면 batch optimization은 전체 궤적에 대해 자코비안을 현재 추정치에서 반복적으로 재계산(relinearize)할 수 있다.

[Strasdat et al. (2012) "Visual SLAM: Why Filter?"](https://doi.org/10.1016/j.imavis.2012.02.009)가 이 논증을 체계적으로 제시했다: 같은 계산량이 주어지면, optimization에 더 많은 키프레임을 넣는 것이 filtering에 더 많은 관측을 넣는 것보다 정확도가 높다.

**2. 일관성(Consistency) 문제**

EKF-SLAM은 관측 가능성(observability) 조건을 위반하는 경향이 있다. 특히 SLAM에서 첫 번째 포즈가 고정되어야(unobservable) 하는데, EKF의 선형화가 이를 깨뜨려 4개의 unobservable direction이 observable해지는 inconsistency가 발생한다. 이 문제는 FEJ(First-Estimate Jacobian)로 완화할 수 있지만 완전히 해결되지는 않는다.

**3. 스케일러빌리티(Scalability)**

EKF-SLAM에서 $M$개 랜드마크의 공분산 행렬 크기는 $O((n + 3M)^2)$이고, 각 갱신의 계산량은 $O((n+3M)^2)$이다. 맵이 커지면 감당할 수 없다.

Graph-based SLAM에서는 정보 행렬(information matrix, Hessian)이 **희소(sparse)**하다. 각 변수(포즈, 랜드마크)는 직접 관측한 다른 변수들과만 연결되므로, 정보 행렬의 대부분이 0이다. 이 희소성을 활용하면 변수 수에 거의 선형인 시간에 최적화할 수 있다.

**4. 루프 클로저(Loop Closure) 처리의 자연스러움**

필터 기반 시스템에서 루프 클로저를 처리하려면 과거 상태의 공분산 정보를 유지해야 하며, 이것이 계산량을 크게 증가시킨다. Graph-based 시스템에서는 루프 클로저를 단순히 새로운 factor(constraint)로 추가하고 전체 그래프를 재최적화하면 된다.

| 관점 | Filtering (EKF) | Optimization (Graph) |
|------|----------------|---------------------|
| 선형화 | 한 번, 고정 | 반복 재선형화 가능 |
| 과거 상태 | 마지널라이즈되어 소실 | 전부 유지 |
| 루프 클로저 | 어렵고 비쌈 | factor 추가로 자연스럽게 |
| 정보 행렬 구조 | Dense | Sparse |
| 계산량 (M 랜드마크) | $O(M^2)$ per step | $O(M)$ (sparse 활용) |
| 일관성 | FEJ 등 추가 조치 필요 | 재선형화로 자연 완화 |

그러나 필터 기반이 완전히 사라진 것은 아니다. [MSCKF (Mourikis & Roumeliotis, 2007)](https://ieeexplore.ieee.org/document/4209642)와 OpenVINS는 EKF 기반이면서도 경쟁력 있는 성능을 보여주며, 특히 계산 자원이 극히 제한된 환경(마이크로 UAV 등)에서 여전히 유용하다. FAST-LIO2의 IEKF도 필터 기반이지만, ikd-tree와 결합하여 최적화 기반 시스템과 대등한 정확도를 달성한다.

---

## 4.5 Factor Graph & Optimization

### 4.5.1 Factor Graph 표현

Factor graph는 확률적 그래피컬 모델(probabilistic graphical model)의 한 종류로, 변수(variable)와 인자(factor)로 구성된 이분 그래프(bipartite graph)다.

$$p(\mathbf{X} \mid \mathbf{Z}) \propto \prod_{i} f_i(\mathbf{X}_i)$$

여기서:
- $\mathbf{X} = \{\mathbf{x}_0, \mathbf{x}_1, \ldots, \mathbf{x}_T, \mathbf{l}_1, \ldots, \mathbf{l}_M\}$: 변수 노드 (포즈, 랜드마크, 바이어스 등)
- $f_i(\mathbf{X}_i)$: $i$번째 factor. 변수의 부분집합 $\mathbf{X}_i$에 대한 "에너지 함수" 또는 "확률적 구속 조건"
- $\mathbf{Z}$: 모든 관측

각 factor는 특정 관측이나 사전 정보에 대응한다:

| Factor 유형 | 연결하는 변수 | 의미 |
|------------|-------------|------|
| Prior factor | $\mathbf{x}_0$ | 초기 상태 사전 분포 |
| Odometry factor | $\mathbf{x}_{k-1}, \mathbf{x}_k$ | 연속 포즈 간 상대 운동 |
| IMU preintegration factor | $\mathbf{x}_{i}, \mathbf{x}_{j}, \mathbf{v}_i, \mathbf{v}_j, \mathbf{b}_i$ | 키프레임 간 IMU 적분 |
| Vision factor | $\mathbf{x}_k, \mathbf{l}_m$ | 카메라에서 랜드마크 관측 |
| LiDAR factor | $\mathbf{x}_k$ | 포인트-투-플레인/엣지 정합 |
| GPS factor | $\mathbf{x}_k$ | 절대 위치 관측 |
| Loop closure factor | $\mathbf{x}_i, \mathbf{x}_j$ | 루프 클로저 상대 포즈 |

factor graph의 핵심 강점은 **모듈성(modularity)**이다. 새로운 센서를 추가하려면 해당 센서에 대응하는 factor를 정의하고 그래프에 추가하기만 하면 된다. 기존 factor들은 수정할 필요가 없다.

### 4.5.2 MAP Inference = Nonlinear Least Squares

가우시안 노이즈 모델을 가정하면, 각 factor는 다음 형태가 된다:

$$f_i(\mathbf{X}_i) \propto \exp\left(-\frac{1}{2} \|\mathbf{r}_i(\mathbf{X}_i)\|^2_{\boldsymbol{\Sigma}_i}\right)$$

여기서 $\mathbf{r}_i(\mathbf{X}_i)$는 잔차(residual)이고, $\|\mathbf{r}\|^2_{\boldsymbol{\Sigma}} = \mathbf{r}^\top \boldsymbol{\Sigma}^{-1} \mathbf{r}$은 Mahalanobis 거리의 제곱이다.

예를 들어, 관측 factor에서:
$$\mathbf{r}_i = \mathbf{z}_i - h_i(\mathbf{X}_i), \quad \boldsymbol{\Sigma}_i = \mathbf{R}_i \text{ (관측 노이즈 공분산)}$$

전체 사후 분포의 MAP 추정:

$$\mathbf{X}^* = \arg\max_\mathbf{X} p(\mathbf{X} \mid \mathbf{Z}) = \arg\max_\mathbf{X} \prod_i f_i(\mathbf{X}_i)$$

로그를 취하고 부호를 뒤집으면:

$$\boxed{\mathbf{X}^* = \arg\min_\mathbf{X} \sum_i \|\mathbf{r}_i(\mathbf{X}_i)\|^2_{\boldsymbol{\Sigma}_i}}$$

이것이 **NLS** 문제다. 확률적 추론 문제가 최적화 문제로 바뀐다. [Dellaert & Kaess (2017)의 "Factor Graphs for Robot Perception"](https://doi.org/10.1561/2300000043) 튜토리얼이 이 과정을 139페이지에 걸쳐 풀어낸다.

### 4.5.3 Gauss-Newton Method

NLS 문제를 풀기 위해 Gauss-Newton(GN) 방법을 사용한다. 잔차를 현재 추정치 $\mathbf{X}^{(0)}$ 근방에서 1차 테일러 전개한다:

$$\mathbf{r}_i(\mathbf{X}^{(0)} \boxplus \Delta\mathbf{X}) \approx \mathbf{r}_i(\mathbf{X}^{(0)}) + \mathbf{J}_i \Delta\mathbf{X}$$

여기서 $\mathbf{J}_i = \frac{\partial \mathbf{r}_i}{\partial \mathbf{X}}\big|_{\mathbf{X}^{(0)}}$는 잔차의 자코비안이고, $\boxplus$는 매니폴드 위의 증분 연산이다.

대입하면:

$$\sum_i \|\mathbf{r}_i + \mathbf{J}_i \Delta\mathbf{X}\|^2_{\boldsymbol{\Sigma}_i}$$

$\mathbf{r}_i' = \boldsymbol{\Sigma}_i^{-1/2} \mathbf{r}_i$, $\mathbf{J}_i' = \boldsymbol{\Sigma}_i^{-1/2} \mathbf{J}_i$로 화이트닝하면, 표준 최소자승 문제가 된다:

$$\sum_i \|\mathbf{r}_i' + \mathbf{J}_i' \Delta\mathbf{X}\|^2$$

$\Delta\mathbf{X}$에 대해 미분하여 0으로 놓으면 **정규 방정식(normal equation)**을 얻는다:

$$\underbrace{\left(\sum_i \mathbf{J}_i'^\top \mathbf{J}_i'\right)}_{\mathbf{H}} \Delta\mathbf{X} = -\underbrace{\sum_i \mathbf{J}_i'^\top \mathbf{r}_i'}_{\mathbf{b}}$$

$$\boxed{\mathbf{H} \Delta\mathbf{X} = -\mathbf{b}}$$

여기서 $\mathbf{H} = \mathbf{J}^\top \boldsymbol{\Sigma}^{-1} \mathbf{J} \in \mathbb{R}^{N \times N}$는 근사 Hessian (정보 행렬)이고, $\mathbf{b} = \mathbf{J}^\top \boldsymbol{\Sigma}^{-1} \mathbf{r}$은 gradient이다.

SLAM 문제에서 $\mathbf{H}$는 **희소**하다. 각 factor의 자코비안 $\mathbf{J}_i$는 해당 factor에 연결된 변수에 대한 열만 비영이고 나머지는 0이다. 따라서 $\mathbf{H}$의 비영 원소는 factor graph의 간선에 대응하며, 그래프가 희소하면 $\mathbf{H}$도 희소하다.

Gauss-Newton 반복:

$$\mathbf{X}^{(k+1)} = \mathbf{X}^{(k)} \boxplus \Delta\mathbf{X}^{(k)}$$

각 반복에서 정규 방정식을 풀어야 한다. 희소 선형 시스템의 풀이에는 **희소 Cholesky 분해** ($\mathbf{H} = \mathbf{L}\mathbf{L}^\top$, 전방/후방 대입)를 사용하며, 변수 순서(variable ordering)에 따라 $\mathbf{L}$의 fill-in이 달라지므로 COLAMD 등의 근사 최적 순서(approximate minimum degree ordering)를 사용한다.

### 4.5.4 Levenberg-Marquardt Method

Gauss-Newton은 순수한 근사 2차 방법이지만, 초기값이 나쁘거나 비선형성이 심하면 발산할 수 있다. **Levenberg-Marquardt(LM)** 방법은 GN과 경사 하강법(gradient descent)의 절충으로, 정규화 항을 추가한다:

$$(\mathbf{H} + \lambda \mathbf{I}) \Delta\mathbf{X} = -\mathbf{b}$$

- $\lambda$가 작으면 → GN에 가까움 (빠른 수렴, 2차 수렴)
- $\lambda$가 크면 → gradient descent에 가까움 (작은 스텝, 안전)

$\lambda$의 조절 전략: 갱신이 비용 함수를 감소시키면 $\lambda$를 줄이고 (GN 모드), 증가시키면 $\lambda$를 키운다 (보수적 모드).

### 4.5.5 매니폴드 위의 최적화 (Optimization on Manifolds)

3D 자세 $\mathbf{T} \in SE(3)$를 최적화할 때, $SE(3)$는 유클리드 공간이 아닌 매니폴드이므로 일반 덧셈을 쓸 수 없다. 표준 해법은 **retraction** (또는 **exponential map**)이다.

현재 추정치 $\mathbf{T}^{(k)}$ 근방에서 접선 공간(tangent space) $\boldsymbol{\xi} \in \mathbb{R}^6$의 증분을 정의하고:

$$\mathbf{T}^{(k+1)} = \mathbf{T}^{(k)} \cdot \text{Exp}(\boldsymbol{\xi})$$

또는:

$$\mathbf{T}^{(k+1)} = \text{Exp}(\boldsymbol{\xi}) \cdot \mathbf{T}^{(k)}$$

(왼쪽/오른쪽 증분의 선택은 convention에 따름)

여기서 $\text{Exp}: \mathbb{R}^6 \to SE(3)$는 Lie group의 exponential map이다. $\boldsymbol{\xi} = [\boldsymbol{\rho}^\top, \boldsymbol{\phi}^\top]^\top$에서 $\boldsymbol{\rho} \in \mathbb{R}^3$는 이동, $\boldsymbol{\phi} \in \mathbb{R}^3$는 회전(각축 표현)이다.

SO(3)에서의 exponential map (Rodrigues' formula):

$$\text{Exp}(\boldsymbol{\phi}) = \mathbf{I} + \frac{\sin\theta}{\theta}[\boldsymbol{\phi}]_\times + \frac{1 - \cos\theta}{\theta^2}[\boldsymbol{\phi}]_\times^2 \in \mathbb{R}^{3 \times 3}$$

여기서 $\theta = \|\boldsymbol{\phi}\|$이다.

반대로 $\text{Log}: SE(3) \to \mathbb{R}^6$은 logarithmic map이다.

Gauss-Newton/LM의 정규 방정식은 접선 공간에서의 증분 $\boldsymbol{\xi}$에 대해 풀고, exponential map으로 매니폴드 위의 상태를 갱신한다. 자코비안도 접선 공간에 대해 계산한다:

$$\mathbf{J}_i = \frac{\partial \mathbf{r}_i}{\partial \boldsymbol{\xi}}\bigg|_{\boldsymbol{\xi}=\mathbf{0}}$$

### 4.5.6 iSAM2: Incremental Smoothing

Batch 최적화를 매 키프레임마다 처음부터 다시 수행하면 시간이 비현실적으로 오래 걸린다. **iSAM2** ([Kaess et al., 2012](https://doi.org/10.1177/0278364911430419))는 Bayes tree 자료구조를 사용하여 incremental하게 최적화를 수행한다.

핵심 아이디어:

1. **Bayes Tree**: factor graph의 elimination 결과를 방향성 트리로 표현. 각 노드는 clique(변수의 부분집합)에 대한 조건부 확률 밀도를 저장.

2. **Incremental Update**: 새 factor가 추가되면, 영향을 받는 clique만 재계산. 트리의 대부분은 변경되지 않음.

3. **Fluid Relinearization**: 선형화 지점이 현재 추정치와 크게 달라진 변수만 선택적으로 재선형화. 주기적 batch 처리 불필요.

4. **변수 순서 재정렬(Variable Reordering)**: 새 변수/factor 추가 시 전체 재정렬 없이 영향받는 부분만 지역적으로 재정렬.

iSAM2는 GTSAM의 핵심 알고리즘으로, LIO-SAM·VINS-Mono 등 현대 SLAM 시스템 다수가 이를 백엔드로 쓴다.

> **최근 동향 — Continuous-Time Factor Graph**: 이산 키프레임 기반 factor graph를 **연속 시간(continuous-time)**으로 확장하는 연구가 활발하다. [Wong et al. (2024)](https://arxiv.org/abs/2402.06174)는 Gaussian Process motion prior를 사용하여 radar-inertial 및 LiDAR-inertial odometry를 연속 시간 factor graph로 통합하고, 비동기 센서 측정을 자연스럽게 처리할 수 있음을 보였다.

### 4.5.7 GTSAM / Ceres / g2o 비교

| 특성 | GTSAM | Ceres Solver | g2o |
|------|-------|-------------|-----|
| 개발 | Georgia Tech ([Dellaert](https://gtsam.org/)) | Google ([Ceres](http://ceres-solver.org/)) | [Kümmerle et al.](https://doi.org/10.1109/ICRA.2011.5979949) |
| 핵심 철학 | Factor graph + Bayes tree | 범용 NLS solver | Graph optimization |
| Incremental | iSAM2 (native) | 없음 (batch) | 없음 (batch) |
| 매니폴드 | 내장 (Rot2, Rot3, Pose2, Pose3, ...) | Local parameterization | 내장 |
| IMU Preintegration | 내장 (`PreintegratedImuMeasurements`) | 사용자 정의 | 사용자 정의 |
| 자동 미분 | 수치 미분 가능 | 자동 미분 (ceres::AutoDiffCostFunction) | 없음 |
| 언어 | C++ (Python 바인딩) | C++ | C++ |
| 대표 사용처 | LIO-SAM, VINS-Mono | Cartographer, ORB-SLAM3 BA | 많은 SLAM 시스템의 포즈 그래프 |
| 학습 곡선 | Factor 정의만 하면 됨 | Cost function 정의 | Vertex/Edge 정의 |

```python
# GTSAM을 이용한 간단한 Pose Graph Optimization 예제
import gtsam
import numpy as np

# 1. Factor graph 생성
graph = gtsam.NonlinearFactorGraph()

# 2. 초기 추정치
initial = gtsam.Values()

# 3. Prior factor on first pose
prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))  # (x, y, theta)
graph.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(0.0, 0.0, 0.0), prior_noise))
initial.insert(0, gtsam.Pose2(0.0, 0.0, 0.0))

# 4. Odometry factors (연속 포즈 간)
odom_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))

# 사각형 경로: 4개 포즈 (각 변에서 전진 2m + 좌회전 90도)
odometry = [
    gtsam.Pose2(2.0, 0.0, np.pi / 2),   # x0 → x1: 전진 2m + 좌회전 90도
    gtsam.Pose2(2.0, 0.0, np.pi / 2),   # x1 → x2: 전진 2m + 좌회전 90도
    gtsam.Pose2(2.0, 0.0, np.pi / 2),   # x2 → x3: 전진 2m + 좌회전 90도
    gtsam.Pose2(2.0, 0.0, np.pi / 2),   # x3 → x4: 전진 2m + 좌회전 90도
]

# Odometry factor 추가 및 초기값 설정
pose = gtsam.Pose2(0.0, 0.0, 0.0)
for i, odom in enumerate(odometry):
    graph.add(gtsam.BetweenFactorPose2(i, i + 1, odom, odom_noise))
    pose = pose.compose(odom)
    # 초기값에 노이즈를 약간 추가 (실제로는 odometry 누적값)
    initial.insert(i + 1, pose)

# 5. Loop closure factor: x4와 x0이 같은 위치 (사각형 경로가 닫힘)
loop_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 0.05]))
graph.add(gtsam.BetweenFactorPose2(4, 0, gtsam.Pose2(0.0, 0.0, 0.0), loop_noise))

# 6. iSAM2로 최적화
params = gtsam.ISAM2Params()
isam = gtsam.ISAM2(params)
isam.update(graph, initial)
result = isam.calculateEstimate()

# 결과 출력
for i in range(5):
    pose_i = result.atPose2(i)
    print(f"Pose {i}: x={pose_i.x():.3f}, y={pose_i.y():.3f}, theta={pose_i.theta():.3f}")
```

---

## 4.6 IMU Preintegration

### 4.6.1 왜 Preintegration이 필요한가

IMU는 보통 200~1000Hz로 가속도와 각속도를 측정하지만, 카메라/LiDAR 키프레임은 10~30Hz 간격이다. 두 키프레임 $i, j$ 사이에 수백 개의 IMU 측정이 존재한다.

**Naive 접근: 직접 적분**

두 키프레임 사이의 IMU 측정을 적분하여 상대 포즈를 구하면:

$$\mathbf{R}_j = \mathbf{R}_i \prod_{k=i}^{j-1} \text{Exp}((\tilde{\boldsymbol{\omega}}_k - \mathbf{b}_g) \Delta t)$$
$$\mathbf{v}_j = \mathbf{v}_i + \mathbf{g} \Delta t_{ij} + \sum_{k=i}^{j-1} \mathbf{R}_k (\tilde{\mathbf{a}}_k - \mathbf{b}_a) \Delta t$$
$$\mathbf{p}_j = \mathbf{p}_i + \mathbf{v}_i \Delta t_{ij} + \frac{1}{2}\mathbf{g}\Delta t_{ij}^2 + \sum_{k=i}^{j-1}\left[\mathbf{v}_k \Delta t + \frac{1}{2}\mathbf{R}_k(\tilde{\mathbf{a}}_k - \mathbf{b}_a)\Delta t^2\right]$$

문제: 이 적분은 키프레임 $i$의 상태 $(\mathbf{R}_i, \mathbf{v}_i, \mathbf{p}_i)$와 바이어스 $(\mathbf{b}_g, \mathbf{b}_a)$에 의존한다. 최적화 반복에서 $\mathbf{R}_i, \mathbf{v}_i, \mathbf{p}_i$의 추정치가 바뀌면, 모든 중간 상태를 재적분해야 한다. 바이어스 추정치가 바뀔 때도 마찬가지다. 이것은 수백 번의 exponential map 계산이 매 최적화 반복마다 필요함을 의미한다.

**Preintegration의 해법**: 키프레임 $i$의 body frame 기준의 상대 변화량을 정의하여, 이 값이 키프레임 $i$의 글로벌 포즈와 무관하게 만든다. 바이어스는 1차 자코비안 보정으로 재적분을 회피한다.

### 4.6.2 On-Manifold Preintegration 유도

[Forster et al. (2017)](https://doi.org/10.1109/TRO.2016.2597321)의 on-manifold preintegration을 단계별로 유도한다. 원논문은 [arXiv:1512.02363](https://arxiv.org/abs/1512.02363)에서도 확인할 수 있다.

#### Step 1: 상대 변화량 정의

글로벌 프레임에서의 적분 방정식을 키프레임 $i$의 body frame 기준으로 재배열한다. 글로벌 변수($\mathbf{R}_i, \mathbf{v}_i, \mathbf{p}_i$)를 좌변으로 이동:

$$\Delta\mathbf{R}_{ij} \triangleq \mathbf{R}_i^\top \mathbf{R}_j = \prod_{k=i}^{j-1} \text{Exp}((\tilde{\boldsymbol{\omega}}_k - \mathbf{b}_g^i)\Delta t) \in SO(3)$$

$$\Delta\mathbf{v}_{ij} \triangleq \mathbf{R}_i^\top (\mathbf{v}_j - \mathbf{v}_i - \mathbf{g}\Delta t_{ij}) = \sum_{k=i}^{j-1} \Delta\mathbf{R}_{ik}(\tilde{\mathbf{a}}_k - \mathbf{b}_a^i)\Delta t \in \mathbb{R}^3$$

$$\Delta\mathbf{p}_{ij} \triangleq \mathbf{R}_i^\top (\mathbf{p}_j - \mathbf{p}_i - \mathbf{v}_i \Delta t_{ij} - \frac{1}{2}\mathbf{g}\Delta t_{ij}^2) = \sum_{k=i}^{j-1}\left[\Delta\mathbf{v}_{ik}\Delta t + \frac{1}{2}\Delta\mathbf{R}_{ik}(\tilde{\mathbf{a}}_k - \mathbf{b}_a^i)\Delta t^2\right] \in \mathbb{R}^3$$

핵심 관찰: **우변은 IMU 측정값과 바이어스 추정치에만 의존하고, 키프레임 $i$의 글로벌 포즈 $(\mathbf{R}_i, \mathbf{v}_i, \mathbf{p}_i)$와 무관하다.** 따라서 키프레임 포즈가 최적화로 바뀌어도 우변을 재계산할 필요가 없다.

#### Step 2: 재귀적 계산 (On-Manifold)

Preintegrated 측정은 재귀적으로 누적 계산할 수 있다:

$$\Delta\mathbf{R}_{i,k+1} = \Delta\mathbf{R}_{ik} \cdot \text{Exp}((\tilde{\boldsymbol{\omega}}_k - \mathbf{b}_g)\Delta t) \in SO(3)$$
$$\Delta\mathbf{v}_{i,k+1} = \Delta\mathbf{v}_{ik} + \Delta\mathbf{R}_{ik}(\tilde{\mathbf{a}}_k - \mathbf{b}_a)\Delta t \in \mathbb{R}^3$$
$$\Delta\mathbf{p}_{i,k+1} = \Delta\mathbf{p}_{ik} + \Delta\mathbf{v}_{ik}\Delta t + \frac{1}{2}\Delta\mathbf{R}_{ik}(\tilde{\mathbf{a}}_k - \mathbf{b}_a)\Delta t^2 \in \mathbb{R}^3$$

초기값: $\Delta\mathbf{R}_{ii} = \mathbf{I}_{3\times 3}$, $\Delta\mathbf{v}_{ii} = \mathbf{0}$, $\Delta\mathbf{p}_{ii} = \mathbf{0}$.

"On-manifold"의 의미: 회전 $\Delta\mathbf{R}_{ij}$를 직접 $SO(3)$ 위에서 누적한다. 오일러 각이나 쿼터니언 정규화 같은 임시방편이 필요 없다.

#### Step 3: 바이어스 변화에 대한 1차 보정

최적화 과정에서 바이어스 추정치가 $\mathbf{b} \to \mathbf{b} + \delta\mathbf{b}$로 변하면, preintegrated 측정도 변한다. 그러나 전체 재적분을 피하기 위해 1차 테일러 전개로 보정한다:

**회전 보정**:
$$\Delta\hat{\mathbf{R}}_{ij}(\mathbf{b}_g + \delta\mathbf{b}_g) \approx \Delta\hat{\mathbf{R}}_{ij}(\mathbf{b}_g) \cdot \text{Exp}\left(\frac{\partial \Delta\bar{\mathbf{R}}_{ij}}{\partial \mathbf{b}_g} \delta\mathbf{b}_g\right)$$

**속도 보정**:
$$\Delta\hat{\mathbf{v}}_{ij}(\mathbf{b} + \delta\mathbf{b}) \approx \Delta\hat{\mathbf{v}}_{ij}(\mathbf{b}) + \frac{\partial \Delta\bar{\mathbf{v}}_{ij}}{\partial \mathbf{b}_g} \delta\mathbf{b}_g + \frac{\partial \Delta\bar{\mathbf{v}}_{ij}}{\partial \mathbf{b}_a} \delta\mathbf{b}_a$$

**위치 보정**:
$$\Delta\hat{\mathbf{p}}_{ij}(\mathbf{b} + \delta\mathbf{b}) \approx \Delta\hat{\mathbf{p}}_{ij}(\mathbf{b}) + \frac{\partial \Delta\bar{\mathbf{p}}_{ij}}{\partial \mathbf{b}_g} \delta\mathbf{b}_g + \frac{\partial \Delta\bar{\mathbf{p}}_{ij}}{\partial \mathbf{b}_a} \delta\mathbf{b}_a$$

자코비안 $\frac{\partial \Delta\bar{\mathbf{R}}_{ij}}{\partial \mathbf{b}_g}$ 등은 preintegration 과정에서 재귀적으로 누적 계산한다. 예를 들어 회전의 바이어스 자코비안:

$$\frac{\partial \Delta\bar{\mathbf{R}}_{i,k+1}}{\partial \mathbf{b}_g} = -\Delta\bar{\mathbf{R}}_{k,k+1}^\top \text{Jr}((\tilde{\boldsymbol{\omega}}_k - \mathbf{b}_g)\Delta t) \Delta t + \Delta\bar{\mathbf{R}}_{k,k+1}^\top \frac{\partial \Delta\bar{\mathbf{R}}_{ik}}{\partial \mathbf{b}_g}$$

여기서 $\text{Jr}(\boldsymbol{\phi})$는 SO(3)의 right Jacobian:

$$\text{Jr}(\boldsymbol{\phi}) = \mathbf{I} - \frac{1 - \cos\theta}{\theta^2}[\boldsymbol{\phi}]_\times + \frac{\theta - \sin\theta}{\theta^3}[\boldsymbol{\phi}]_\times^2, \quad \theta = \|\boldsymbol{\phi}\|$$

바이어스 변화가 작으면 ($\|\delta\mathbf{b}\|$가 작으면) 이 1차 보정은 충분히 정확하다. 바이어스가 크게 변하면 preintegration을 처음부터 다시 계산하지만, 실전에서는 거의 발생하지 않는다.

#### Step 4: 공분산 전파

IMU 측정 노이즈 $\mathbf{n}_g, \mathbf{n}_a \sim \mathcal{N}(\mathbf{0}, \boldsymbol{\Sigma})$가 preintegration을 통해 전파된다. 공분산도 재귀적으로 계산:

$$\boldsymbol{\Sigma}_{k+1} = \mathbf{A}_k \boldsymbol{\Sigma}_k \mathbf{A}_k^\top + \mathbf{B}_k \boldsymbol{\Sigma}_\eta \mathbf{B}_k^\top$$

여기서 $\mathbf{A}_k, \mathbf{B}_k$는 재귀적 전파의 자코비안이고, $\boldsymbol{\Sigma}_\eta$는 IMU 노이즈 공분산이다. 이 공분산 $\boldsymbol{\Sigma}_{ij}$는 factor graph에서 IMU factor의 정보 행렬 $\boldsymbol{\Sigma}_{ij}^{-1}$로 사용된다.

#### Step 5: Factor Graph에서의 IMU Factor

최종적으로 preintegrated 측정은 두 키프레임 $i, j$ 사이의 factor로 삽입된다. 잔차:

$$\mathbf{r}_{\Delta\mathbf{R}_{ij}} = \text{Log}\left(\Delta\hat{\mathbf{R}}_{ij}^\top \mathbf{R}_i^\top \mathbf{R}_j\right) \in \mathbb{R}^3$$

$$\mathbf{r}_{\Delta\mathbf{v}_{ij}} = \mathbf{R}_i^\top(\mathbf{v}_j - \mathbf{v}_i - \mathbf{g}\Delta t_{ij}) - \Delta\hat{\mathbf{v}}_{ij} \in \mathbb{R}^3$$

$$\mathbf{r}_{\Delta\mathbf{p}_{ij}} = \mathbf{R}_i^\top(\mathbf{p}_j - \mathbf{p}_i - \mathbf{v}_i\Delta t_{ij} - \frac{1}{2}\mathbf{g}\Delta t_{ij}^2) - \Delta\hat{\mathbf{p}}_{ij} \in \mathbb{R}^3$$

이 9차원 잔차가 IMU preintegration factor의 잔차이며, Mahalanobis 거리의 제곱 $\|\mathbf{r}\|^2_{\boldsymbol{\Sigma}_{ij}}$이 비용 함수에 추가된다.

### 4.6.3 코드로 보는 Preintegration

```python
import numpy as np
from scipy.spatial.transform import Rotation

def skew(v):
    """3D 벡터 → 반대칭 행렬 (skew-symmetric matrix).
    
    Parameters
    ----------
    v : ndarray, shape (3,)
    
    Returns
    -------
    S : ndarray, shape (3, 3)
    """
    return np.array([[0, -v[2], v[1]],
                     [v[2], 0, -v[0]],
                     [-v[1], v[0], 0]])

def exp_so3(phi):
    """so(3) → SO(3) exponential map (Rodrigues' formula).
    
    Parameters
    ----------
    phi : ndarray, shape (3,) — 각축(angle-axis) 벡터
    
    Returns
    -------
    R : ndarray, shape (3, 3) — 회전 행렬
    """
    theta = np.linalg.norm(phi)
    if theta < 1e-10:
        return np.eye(3) + skew(phi)
    axis = phi / theta
    K = skew(axis)
    return np.eye(3) + np.sin(theta) * K + (1 - np.cos(theta)) * K @ K

def log_so3(R):
    """SO(3) → so(3) logarithmic map.
    
    Parameters
    ----------
    R : ndarray, shape (3, 3) — 회전 행렬
    
    Returns
    -------
    phi : ndarray, shape (3,) — 각축(angle-axis) 벡터
    """
    cos_theta = (np.trace(R) - 1) / 2
    cos_theta = np.clip(cos_theta, -1, 1)
    theta = np.arccos(cos_theta)
    if theta < 1e-10:
        return np.array([R[2,1] - R[1,2], R[0,2] - R[2,0], R[1,0] - R[0,1]]) / 2
    return theta / (2 * np.sin(theta)) * np.array([R[2,1] - R[1,2],
                                                    R[0,2] - R[2,0],
                                                    R[1,0] - R[0,1]])

def right_jacobian_so3(phi):
    """SO(3)의 right Jacobian.
    
    Parameters
    ----------
    phi : ndarray, shape (3,)
    
    Returns
    -------
    Jr : ndarray, shape (3, 3)
    """
    theta = np.linalg.norm(phi)
    if theta < 1e-10:
        return np.eye(3) - 0.5 * skew(phi)
    K = skew(phi)
    return (np.eye(3) 
            - (1 - np.cos(theta)) / theta**2 * K 
            + (theta - np.sin(theta)) / theta**3 * K @ K)

class IMUPreintegration:
    """On-manifold IMU preintegration (Forster et al., 2017).
    
    두 키프레임 사이의 IMU 측정을 preintegrate하여 
    factor graph에서 IMU factor로 사용할 수 있는 형태로 만든다.
    """
    def __init__(self, bias_gyro, bias_acc, 
                 gyro_noise_density, acc_noise_density,
                 gyro_random_walk, acc_random_walk):
        """
        Parameters
        ----------
        bias_gyro : ndarray, shape (3,) — 자이로 바이어스 초기 추정치 [rad/s]
        bias_acc : ndarray, shape (3,) — 가속도 바이어스 초기 추정치 [m/s^2]
        gyro_noise_density : float — 자이로 노이즈 밀도 [rad/s/sqrt(Hz)]
        acc_noise_density : float — 가속도 노이즈 밀도 [m/s^2/sqrt(Hz)]
        gyro_random_walk : float — 자이로 바이어스 random walk [rad/s^2/sqrt(Hz)]
        acc_random_walk : float — 가속도 바이어스 random walk [m/s^3/sqrt(Hz)]
        """
        self.bg = bias_gyro.copy()
        self.ba = bias_acc.copy()
        
        # Preintegrated 측정 (초기값)
        self.delta_R = np.eye(3)     # SO(3) 상의 상대 회전
        self.delta_v = np.zeros(3)   # 상대 속도 변화
        self.delta_p = np.zeros(3)   # 상대 위치 변화
        self.dt_sum = 0.0            # 총 적분 시간
        
        # 공분산 (9x9: rotation, velocity, position)
        self.cov = np.zeros((9, 9))
        
        # 바이어스 자코비안 (바이어스 보정용)
        self.d_R_d_bg = np.zeros((3, 3))
        self.d_v_d_bg = np.zeros((3, 3))
        self.d_v_d_ba = np.zeros((3, 3))
        self.d_p_d_bg = np.zeros((3, 3))
        self.d_p_d_ba = np.zeros((3, 3))
        
        # 노이즈 파라미터
        self.sigma_g = gyro_noise_density
        self.sigma_a = acc_noise_density
        self.sigma_bg = gyro_random_walk
        self.sigma_ba = acc_random_walk
        
    def integrate(self, gyro_meas, acc_meas, dt):
        """하나의 IMU 측정을 preintegration에 추가한다.
        
        Parameters
        ----------
        gyro_meas : ndarray, shape (3,) — 자이로 측정 [rad/s]
        acc_meas : ndarray, shape (3,) — 가속도 측정 [m/s^2]
        dt : float — 시간 간격 [s]
        """
        # 바이어스 보정된 측정
        omega = gyro_meas - self.bg   # (3,)
        acc = acc_meas - self.ba       # (3,)
        
        # 중간 변수
        dR = exp_so3(omega * dt)       # (3, 3)  이 dt 동안의 회전
        Jr = right_jacobian_so3(omega * dt)  # (3, 3)
        
        # --- 바이어스 자코비안 재귀 갱신 (Step 3) ---
        # 위치 자코비안 (delta_p에 대한 바이어스 자코비안)
        self.d_p_d_bg += self.d_v_d_bg * dt - 0.5 * self.delta_R @ skew(acc) @ self.d_R_d_bg * dt**2
        self.d_p_d_ba += self.d_v_d_ba * dt - 0.5 * self.delta_R * dt**2
        # 속도 자코비안
        self.d_v_d_bg += -self.delta_R @ skew(acc) @ self.d_R_d_bg * dt
        self.d_v_d_ba += -self.delta_R * dt
        # 회전 자코비안
        self.d_R_d_bg = dR.T @ (self.d_R_d_bg - Jr * dt)
        
        # --- 공분산 전파 (Step 4) ---
        A = np.eye(9)
        A[0:3, 0:3] = dR.T
        A[3:6, 0:3] = -self.delta_R @ skew(acc) * dt
        A[6:9, 0:3] = -0.5 * self.delta_R @ skew(acc) * dt**2
        A[6:9, 3:6] = np.eye(3) * dt
        
        B = np.zeros((9, 6))
        B[0:3, 0:3] = Jr * dt
        B[3:6, 3:6] = self.delta_R * dt
        B[6:9, 3:6] = 0.5 * self.delta_R * dt**2
        
        Sigma_eta = np.zeros((6, 6))
        Sigma_eta[0:3, 0:3] = np.eye(3) * self.sigma_g**2
        Sigma_eta[3:6, 3:6] = np.eye(3) * self.sigma_a**2
        
        self.cov = A @ self.cov @ A.T + B @ Sigma_eta @ B.T
        
        # --- Preintegrated 측정 재귀 갱신 (Step 2) ---
        # 순서 중요: p를 먼저, 그 다음 v, 마지막 R
        self.delta_p += self.delta_v * dt + 0.5 * self.delta_R @ acc * dt**2
        self.delta_v += self.delta_R @ acc * dt
        self.delta_R = self.delta_R @ dR
        self.dt_sum += dt
    
    def predict(self, R_i, v_i, p_i, gravity):
        """Preintegrated 측정으로 키프레임 j의 상태를 예측한다.
        
        Parameters
        ----------
        R_i : ndarray, shape (3, 3) — 키프레임 i의 회전
        v_i : ndarray, shape (3,) — 키프레임 i의 속도
        p_i : ndarray, shape (3,) — 키프레임 i의 위치
        gravity : ndarray, shape (3,) — 중력 벡터 (e.g., [0, 0, -9.81])
        
        Returns
        -------
        R_j, v_j, p_j : 예측된 키프레임 j의 상태
        """
        dt = self.dt_sum
        R_j = R_i @ self.delta_R
        v_j = v_i + gravity * dt + R_i @ self.delta_v
        p_j = p_i + v_i * dt + 0.5 * gravity * dt**2 + R_i @ self.delta_p
        return R_j, v_j, p_j
    
    def compute_residual(self, R_i, v_i, p_i, R_j, v_j, p_j, gravity):
        """IMU factor의 잔차를 계산한다.
        
        Returns
        -------
        residual : ndarray, shape (9,) — [r_R(3), r_v(3), r_p(3)]
        """
        dt = self.dt_sum
        
        # 회전 잔차
        r_R = log_so3(self.delta_R.T @ R_i.T @ R_j)
        
        # 속도 잔차
        r_v = R_i.T @ (v_j - v_i - gravity * dt) - self.delta_v
        
        # 위치 잔차
        r_p = R_i.T @ (p_j - p_i - v_i * dt - 0.5 * gravity * dt**2) - self.delta_p
        
        return np.concatenate([r_R, r_v, r_p])

# GTSAM에서의 사용 예시
import gtsam

def create_imu_factor_gtsam():
    """GTSAM의 내장 IMU preintegration을 사용하는 예제."""
    # IMU 파라미터 설정
    imu_params = gtsam.PreintegrationParams.MakeSharedU(9.81)  # 중력 방향: +z
    imu_params.setAccelerometerCovariance(np.eye(3) * 0.01**2)
    imu_params.setGyroscopeCovariance(np.eye(3) * 0.001**2)
    imu_params.setIntegrationCovariance(np.eye(3) * 1e-8)
    
    # 초기 바이어스
    bias = gtsam.imuBias.ConstantBias(
        np.array([0.1, -0.05, 0.02]),   # accelerometer bias
        np.array([0.001, -0.002, 0.003]) # gyroscope bias
    )
    
    # Preintegration 객체 생성
    pim = gtsam.PreintegratedImuMeasurements(imu_params, bias)
    
    # IMU 측정 적분 (200Hz 가정, 0.1초 = 20개 측정)
    dt = 0.005  # 200Hz
    for k in range(20):
        acc_meas = np.array([0.0, 0.0, 9.81])  # 정지 상태 (중력만)
        gyro_meas = np.array([0.0, 0.0, 0.0])
        pim.integrateMeasurement(acc_meas, gyro_meas, dt)
    
    # Factor 생성
    # CombinedImuFactor는 키프레임 i, j의 pose, velocity, bias를 연결
    imu_factor = gtsam.ImuFactor(
        gtsam.symbol('x', 0),  # pose_i
        gtsam.symbol('v', 0),  # vel_i
        gtsam.symbol('x', 1),  # pose_j
        gtsam.symbol('v', 1),  # vel_j
        gtsam.symbol('b', 0),  # bias
        pim
    )
    
    return imu_factor
```

---

## 4.7 Marginalization & Sliding Window

### 4.7.1 왜 Marginalization이 필요한가

Factor graph 기반 SLAM/VIO 시스템은 새로운 키프레임이 들어올 때마다 변수와 factor가 추가된다. 무한정 키프레임을 유지하면 최적화의 계산량이 끝없이 증가한다.

슬라이딩 윈도우(sliding window) 방식은 최근 $N$개의 키프레임만 유지하고 오래된 키프레임을 제거한다. 그런데 단순히 삭제하면 그 키프레임과 연결된 관측 정보가 모두 사라져 정확도가 저하된다.

**Marginalization**은 오래된 변수를 그래프에서 제거하면서도, 그 변수를 통해 연결된 변수들 간의 정보를 보존하는 방법이다. 제거되는 변수의 정보가 나머지 변수들에 대한 **사전 분포**로 변환된다.

### 4.7.2 Schur Complement

Marginalization의 수학적 핵심은 **Schur complement**다. 정보 행렬 $\mathbf{H}$를 마지널라이즈할 변수($\mathbf{x}_m$)와 유지할 변수($\mathbf{x}_r$)로 분할한다. 정규 방정식을 $\mathbf{H}\Delta\mathbf{x} = \mathbf{b}$로 쓴다 (§4.5.3의 $-\mathbf{b}$를 $\mathbf{b}$로 재정의, 즉 $\mathbf{b} \triangleq -\mathbf{J}^\top \boldsymbol{\Sigma}^{-1} \mathbf{r}$):

$$\mathbf{H} \Delta\mathbf{x} = \mathbf{b}$$

$$\begin{bmatrix} \mathbf{H}_{mm} & \mathbf{H}_{mr} \\ \mathbf{H}_{rm} & \mathbf{H}_{rr} \end{bmatrix} \begin{bmatrix} \Delta\mathbf{x}_m \\ \Delta\mathbf{x}_r \end{bmatrix} = \begin{bmatrix} \mathbf{b}_m \\ \mathbf{b}_r \end{bmatrix}$$

여기서:
- $\mathbf{x}_m$: 마지널라이즈(제거)할 변수
- $\mathbf{x}_r$: 유지(retain)할 변수
- $\mathbf{H}_{mm} \in \mathbb{R}^{n_m \times n_m}$: 제거할 변수들끼리의 정보 블록
- $\mathbf{H}_{mr} \in \mathbb{R}^{n_m \times n_r}$: 교차 정보 블록
- $\mathbf{H}_{rr} \in \mathbb{R}^{n_r \times n_r}$: 유지할 변수들끼리의 정보 블록

위 블록 행렬에서 $\Delta\mathbf{x}_m$을 소거하면:

$$\underbrace{(\mathbf{H}_{rr} - \mathbf{H}_{rm} \mathbf{H}_{mm}^{-1} \mathbf{H}_{mr})}_{\mathbf{H}^*} \Delta\mathbf{x}_r = \underbrace{\mathbf{b}_r - \mathbf{H}_{rm} \mathbf{H}_{mm}^{-1} \mathbf{b}_m}_{\mathbf{b}^*}$$

$\mathbf{H}^* = \mathbf{H}_{rr} - \mathbf{H}_{rm} \mathbf{H}_{mm}^{-1} \mathbf{H}_{mr}$이 **Schur complement**이며, 이것이 마지널라이즈 후 유지되는 변수에 대한 사전 factor(prior factor)의 정보 행렬이 된다.

**직관적 의미**: $\mathbf{x}_m$을 통해 간접적으로 연결되어 있던 $\mathbf{x}_r$의 변수들 사이에 직접 연결(fill-in)이 생긴다. $\mathbf{H}^*$는 $\mathbf{H}_{rr}$보다 dense하며, 이것은 마지널라이즈 전에는 없던 변수 간 상관 관계가 명시적으로 등장했음을 뜻한다.

```python
import numpy as np

def marginalize(H, b, indices_to_marginalize, indices_to_keep):
    """Schur complement를 이용한 변수 마지널라이제이션.
    
    Parameters
    ----------
    H : ndarray, shape (N, N) — 정보 행렬 (Hessian)
    b : ndarray, shape (N,) — gradient 벡터
    indices_to_marginalize : list of int — 제거할 변수의 인덱스
    indices_to_keep : list of int — 유지할 변수의 인덱스
    
    Returns
    -------
    H_star : ndarray — 마지널라이즈 후 정보 행렬
    b_star : ndarray — 마지널라이즈 후 gradient
    """
    m = indices_to_marginalize
    r = indices_to_keep
    
    H_mm = H[np.ix_(m, m)]
    H_mr = H[np.ix_(m, r)]
    H_rm = H[np.ix_(r, m)]
    H_rr = H[np.ix_(r, r)]
    
    b_m = b[m]
    b_r = b[r]
    
    # Schur complement
    H_mm_inv = np.linalg.inv(H_mm)
    H_star = H_rr - H_rm @ H_mm_inv @ H_mr
    b_star = b_r - H_rm @ H_mm_inv @ b_m
    
    return H_star, b_star

# 예시: 3개 포즈 (각 2D, 3-DoF) 중 첫 번째를 마지널라이즈
# 상태: [x0(3), x1(3), x2(3)] = 9차원
np.random.seed(42)

# 희소 정보 행렬 (x0-x1, x1-x2 연결)
H = np.zeros((9, 9))
# x0 자체 (prior + odom factor)
H[0:3, 0:3] = np.diag([10, 10, 5])
# x0-x1 odometry factor
odom_info = np.diag([25, 25, 50])
H[0:3, 0:3] += odom_info
H[3:6, 3:6] += odom_info
H[0:3, 3:6] = -odom_info
H[3:6, 0:3] = -odom_info
# x1-x2 odometry factor
H[3:6, 3:6] += odom_info
H[6:9, 6:9] += odom_info
H[3:6, 6:9] = -odom_info
H[6:9, 3:6] = -odom_info

b = np.random.randn(9) * 0.1

print("마지널라이즈 전 H의 비영 패턴:")
print((np.abs(H) > 1e-10).astype(int))

# x0 (인덱스 0,1,2)를 마지널라이즈
H_star, b_star = marginalize(H, b, [0, 1, 2], [3, 4, 5, 6, 7, 8])

print("\n마지널라이즈 후 H*의 비영 패턴:")
print((np.abs(H_star) > 1e-10).astype(int))
print("→ x1과 x2 사이에 fill-in이 발생할 수 있음")
```

### 4.7.3 First-Estimate Jacobian (FEJ)

Marginalization에는 중요한 주의점이 있다. 마지널라이즈된 factor(prior)는 **고정된 선형화 지점**에서 계산된다. 이후 최적화 반복에서 유지된 변수의 추정치가 바뀌면, 마지널라이즈된 prior의 선형화 지점과 불일치가 생긴다.

이 불일치는 필터/스무더의 **일관성**을 해칠 수 있다. 구체적으로, 시스템의 관측 가능성(observability) 특성이 바뀌어, 실제로는 관측 불가능한 방향이 관측 가능한 것으로 잘못 처리될 수 있다.

**FEJ (First-Estimate Jacobian)** 전략: 마지널라이즈에 관련된 변수의 자코비안은 항상 **최초 추정치(first estimate)**에서 계산하고, 이후 추정치가 바뀌어도 자코비안을 갱신하지 않는다.

$$\mathbf{J}_{\text{FEJ}} = \left.\frac{\partial \mathbf{r}}{\partial \mathbf{x}}\right|_{\mathbf{x}^{(0)}}$$

여기서 $\mathbf{x}^{(0)}$는 해당 변수가 처음 추정된 시점의 값이다.

FEJ의 장점:
- 마지널라이즈된 prior와 현재 factor들이 같은 선형화 지점에서의 정보를 사용하므로 일관성이 유지된다
- MSCKF/OpenVINS에서 핵심적으로 사용 ([Li & Mourikis, 2013](https://doi.org/10.1177/0278364913481251))

FEJ의 단점:
- 최초 추정치가 부정확하면 수렴 속도가 느려질 수 있다
- 구현이 복잡해진다 (각 변수마다 "최초 추정치"를 별도 저장해야 함)

### 4.7.4 Sliding Window 구현의 실전 이슈

#### 이슈 1: 어떤 키프레임을 마지널라이즈할 것인가

VINS-Mono의 두 가지 전략:
- **최신 프레임이 키프레임이면**: 가장 오래된 키프레임을 마지널라이즈. 윈도우가 공간적으로 넓게 유지됨.
- **최신 프레임이 키프레임이 아니면**: 직전 non-keyframe을 마지널라이즈. 시각 정보만 버리고 IMU 정보는 인접 키프레임으로 전달.

#### 이슈 2: Fill-in에 의한 dense화

마지널라이즈가 반복되면 prior factor가 점점 dense해져서, 원래 희소했던 정보 행렬이 dense해질 수 있다. 이것은 계산 효율을 크게 저하시킨다.

대응 방법:
- Prior factor의 크기를 제한 (연결된 변수 수를 제한)
- 마지널라이즈 순서를 신중히 선택
- 정보 손실을 감수하고 일부 factor를 단순 삭제 (FAST-LIO2는 마지널라이즈 대신 오래된 점을 맵에서 삭제)

#### 이슈 3: 바이어스와 마지널라이제이션

IMU 바이어스는 모든 키프레임에 걸쳐 천천히 변하는 상태다. 키프레임을 마지널라이즈할 때 바이어스도 함께 마지널라이즈하면, 바이어스 정보가 prior에 고정되어 이후 바이어스 추정의 유연성이 줄어든다.

VINS-Mono의 접근: 바이어스를 마지널라이즈하지 않고 윈도우 내에서 계속 유지. 마지널라이즈 prior는 바이어스를 조건으로 하는 형태로 만든다.

#### 이슈 4: 수치 안정성

Schur complement 계산에서 $\mathbf{H}_{mm}^{-1}$의 역행렬이 필요하다. $\mathbf{H}_{mm}$이 나쁜 조건수(condition number)를 가지면 수치 불안정이 발생할 수 있다.

대응:
- $\mathbf{H}_{mm}$에 작은 정규화 항 추가: $(\mathbf{H}_{mm} + \epsilon \mathbf{I})^{-1}$
- LDL 분해를 사용하여 수치 안정성 확보
- 마지널라이즈 후 $\mathbf{H}^*$가 양의 반정치(positive semi-definite)인지 확인하고, 그렇지 않으면 가장 가까운 PSD 행렬로 보정

---

## 4장 요약

이 장은 센서 퓨전의 수학적 기반인 상태 추정 이론을 다룬다. 핵심 메시지를 정리한다:

1. **Bayesian Filtering Framework**는 prediction-update 재귀 구조로, 모든 상태 추정 방법의 공통 뼈대다. Chapman-Kolmogorov 방정식과 Bayes 정리가 이론적 근거이지만, 비선형 시스템에서는 근사가 필수적이다.

2. **Kalman Filter 계열**은 가우시안 근사를 통해 사후 분포를 평균과 공분산으로 추적한다. EKF는 1차 선형화, ESKF는 오차 상태에서의 선형화로 매니폴드 문제를 자연스럽게 처리하며, UKF는 sigma point 변환, IEKF는 반복 선형화로 비선형성에 대응한다. 현대 로봇 시스템에서는 ESKF가 사실상 표준이다.

3. **Particle Filter**는 다봉 분포와 강한 비선형을 다룰 수 있지만 차원의 저주로 고차원 문제에 부적합하다. RBPF(FastSLAM)로 일부 완화 가능하며, 2D SLAM과 global localization에서 여전히 활용된다.

4. **Filtering에서 Optimization으로의 전환**은 재선형화, 희소성 활용, 루프 클로저 처리 등 여러 이점 때문에 현대 SLAM의 주류가 되었다. 다만 필터 기반(MSCKF, FAST-LIO2)도 특정 조건에서 경쟁력을 유지한다.

5. **Factor Graph**는 확률적 추론을 모듈적으로 구성하고 MAP = NLS로 환원하는 강력한 프레임워크다. Gauss-Newton/LM으로 매니폴드 위에서 풀며, iSAM2의 incremental smoothing이 실시간 처리를 가능하게 한다.

6. **IMU Preintegration**은 고속 IMU 측정을 키프레임 간 factor로 압축하는 핵심 기술이다. On-manifold 유도와 바이어스 1차 보정 덕분에 재적분 없이 factor graph에 통합할 수 있다.

7. **Marginalization**은 슬라이딩 윈도우의 정보 보존 메커니즘이다. Schur complement가 핵심 연산이며, FEJ가 일관성 유지의 열쇠다.

이 장의 이론은 이후의 VIO(Ch.6), LIO(Ch.7), 멀티센서 퓨전(Ch.8) 챕터에서 실제 시스템의 설계와 구현을 이해하는 데 필수적인 기반이 된다.

> **2024-2025 연구 방향**: 두 흐름이 특히 주목받는다. **대칭 기반 필터**: Equivariant Filter(EqF)와 Invariant EKF가 Lie 군의 대칭 구조를 활용하여 일관성과 수렴성을 구조적으로 보장한다. **연속 시간 최적화**: Gaussian Process motion prior를 사용한 continuous-time factor graph가 비동기 멀티센서 퓨전의 새 패러다임으로 자리잡고 있다. 한편 [AI-Aided Kalman Filters (Revach et al., 2024)](https://arxiv.org/abs/2410.12289)처럼 RNN/Transformer로 칼만 이득이나 프로세스 모델을 학습하는 접근도 활발하나, 안전성 보장이 과제로 남아 있다.

---

# Ch.5 — Feature Matching & Correspondence: 기술 계보

Ch.4에서 상태 추정의 수학적 프레임워크를 세웠다. 하지만 칼만 필터든 팩터 그래프든, "어떤 관측이 어떤 랜드마크에 대응하는가"라는 **데이터 연관(data association)** 문제가 먼저 풀려야 한다. 이 챕터는 그 핵심인 특징점 매칭(feature matching)과 대응점 탐색(correspondence search)의 기술 계보를 추적한다.

> **이 챕터의 목적**: 센서 퓨전의 거의 모든 컴포넌트 — Visual Odometry, calibration, loop closure, point cloud registration — 가 **correspondence**(대응점)에 의존한다. 이 챕터는 mutual information에서 출발하여 RoMa까지 이어지는 기술적 흐름을 추적하며, 각 방법이 이전 세대의 어떤 한계를 해결했는지 살핀다.

---

## 5.1 Correspondence 문제란

### 5.1.1 "같은 것"을 찾는 문제

Correspondence 문제는 두 개 이상의 관측(observation)에서 **물리적으로 동일한 점, 영역, 또는 구조물**을 식별하는 문제다. 센서 퓨전 파이프라인의 거의 모든 단계가 correspondence를 전제로 한다.

- **Visual Odometry**: 연속된 프레임에서 같은 3D 점의 2D 투영을 찾아야 카메라 모션을 추정할 수 있다.
- **Calibration**: 카메라-LiDAR 외부 파라미터를 추정하려면 두 센서가 관측한 같은 물리적 점을 식별해야 한다.
- **Loop Closure**: 이전에 방문한 장소를 재인식하려면 현재 관측과 과거 관측 사이의 대응을 확인해야 한다.
- **Point Cloud Registration**: 두 스캔의 정합(alignment)은 대응점 쌍을 기반으로 강체 변환을 추정하는 과정이다.

### 5.1.2 Correspondence의 세 가지 유형

#### 2D-2D Correspondence

두 이미지 사이에서 같은 3D 점의 투영을 찾는 문제다. Visual Odometry, Stereo Matching, Image Stitching의 기초가 된다.

이미지 $I_1$의 점 $\mathbf{p}_1 = (u_1, v_1)$과 이미지 $I_2$의 점 $\mathbf{p}_2 = (u_2, v_2)$가 같은 3D 점 $\mathbf{X}$의 투영일 때, 이 쌍 $(\mathbf{p}_1, \mathbf{p}_2)$를 대응점(correspondence)이라 한다.

두 이미지 사이의 기하학적 관계는 **epipolar constraint**로 표현된다:

$$\mathbf{p}_2^\top \mathbf{F} \mathbf{p}_1 = 0$$

여기서 $\mathbf{F}$는 Fundamental matrix (3×3, rank 2)이다. 내부 파라미터를 알면 Essential matrix $\mathbf{E} = \mathbf{K}_2^\top \mathbf{F} \mathbf{K}_1$를 사용한다:

$$\hat{\mathbf{p}}_2^\top \mathbf{E} \hat{\mathbf{p}}_1 = 0$$

여기서 $\hat{\mathbf{p}} = \mathbf{K}^{-1} \mathbf{p}$는 정규화된 이미지 좌표다.

#### 2D-3D Correspondence

이미지의 2D 점과 3D 맵 포인트 사이의 대응. **PnP(Perspective-n-Point)** 문제의 기초이며, Visual Localization과 SLAM의 map-based tracking에서 핵심적이다.

3D 점 $\mathbf{X} = (X, Y, Z)^\top$과 그 2D 투영 $\mathbf{p} = (u, v)^\top$의 관계:

$$s \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \mathbf{K} [\mathbf{R} | \mathbf{t}] \begin{bmatrix} X \\ Y \\ Z \\ 1 \end{bmatrix}$$

최소 6개(DLT), 4개(P3P+1), 또는 3개(P3P) 대응점으로 카메라 포즈 $(\mathbf{R}, \mathbf{t})$를 추정한다.

#### 3D-3D Correspondence

두 점군(point cloud) 사이에서 같은 물리적 점을 찾는 문제. LiDAR scan-to-scan 정합, multi-LiDAR calibration, loop closure 시 point cloud registration에 쓰인다.

두 점군 $\mathcal{P} = \{\mathbf{p}_i\}$와 $\mathcal{Q} = \{\mathbf{q}_i\}$ 사이의 강체 변환 $(\mathbf{R}, \mathbf{t})$를 추정:

$$\min_{\mathbf{R}, \mathbf{t}} \sum_{i} \| \mathbf{q}_i - (\mathbf{R} \mathbf{p}_i + \mathbf{t}) \|^2$$

이 최적화 문제의 closed-form 해는 SVD를 이용하여 구할 수 있다 (ICP, Ch.7 참조).

### 5.1.3 왜 센서 퓨전의 핵심인가

센서 퓨전에서 correspondence는 시스템 전체의 정확도를 결정하는 병목이다. 잘못된 대응점(outlier) 하나가 포즈 추정을 완전히 망가뜨릴 수 있고, 대응점을 찾지 못하면 (텍스처 없는 환경) 시스템 자체가 동작하지 않는다. 이 챕터의 나머지 부분에서는 이 문제를 어떻게 해결해 왔는지의 기술적 계보를 추적한다.

---

## 5.2 전통적 Feature Detection & Description

전통적 correspondence 파이프라인은 **detect → describe → match**의 3단계로 구성된다. 이 절에서는 처음 두 단계 — 어디에서 특징점을 찾고(detection), 그 특징점을 어떻게 표현하는가(description) — 를 본다.

### 5.2.1 Corner Detection: Harris → FAST → ORB

#### Harris Corner Detector (1988)

Harris corner detector는 **이미지 패치를 이동시켰을 때 밝기 변화가 모든 방향으로 크게 일어나는 점**을 코너로 정의한다.

이미지 $I$에서 점 $(x, y)$ 주변의 윈도우를 $(\Delta u, \Delta v)$만큼 이동시킬 때의 밝기 변화:

$$E(\Delta u, \Delta v) = \sum_{x, y} w(x, y) [I(x + \Delta u, y + \Delta v) - I(x, y)]^2$$

$w(x, y)$는 가우시안 윈도우. Taylor 1차 전개를 적용하면:

$$E(\Delta u, \Delta v) \approx \begin{bmatrix} \Delta u & \Delta v \end{bmatrix} \mathbf{M} \begin{bmatrix} \Delta u \\ \Delta v \end{bmatrix}$$

여기서 **structure tensor**(또는 second moment matrix) $\mathbf{M}$은:

$$\mathbf{M} = \sum_{x, y} w(x, y) \begin{bmatrix} I_x^2 & I_x I_y \\ I_x I_y & I_y^2 \end{bmatrix}$$

$I_x, I_y$는 이미지의 x, y 방향 그래디언트. $\mathbf{M}$의 고유값 $(\lambda_1, \lambda_2)$에 따라:

- $\lambda_1 \approx 0, \lambda_2 \approx 0$: 평탄한 영역 (밝기 변화 없음)
- $\lambda_1 \gg \lambda_2 \approx 0$: 엣지 (한 방향으로만 변화)
- $\lambda_1, \lambda_2$ 모두 큼: 코너 (모든 방향으로 변화)

Harris는 고유값을 직접 계산하는 대신 **corner response function**을 정의:

$$R = \det(\mathbf{M}) - k \cdot \text{tr}(\mathbf{M})^2 = \lambda_1 \lambda_2 - k(\lambda_1 + \lambda_2)^2$$

여기서 $k$는 보통 0.04~0.06. $R > \text{threshold}$인 점을 코너로 선택하고, non-maximum suppression을 적용한다.

**Harris의 한계**: 회전 불변(rotation invariant)이지만 **스케일 불변이 아니다**. 카메라가 가까이 다가가면 코너가 엣지로 보일 수 있다. 이 한계가 SIFT의 scale-space 접근법의 동기가 되었다.

```python
import cv2
import numpy as np

# Harris Corner Detection
img = cv2.imread('scene.jpg', cv2.IMREAD_GRAYSCALE)
img_float = np.float32(img)

# blockSize: 이웃 크기, ksize: Sobel 커널, k: Harris 파라미터
harris_response = cv2.cornerHarris(img_float, blockSize=2, ksize=3, k=0.04)

# Non-maximum suppression & 임계값
corners = harris_response > 0.01 * harris_response.max()
```

#### FAST (Features from Accelerated Segment Test, 2006)

FAST는 Harris의 정확성보다 **속도를 극한으로 추구**한 검출기다. 로봇 비전에서 수십 FPS로 특징점을 검출해야 하는 실시간 요구를 해결하기 위해 [Rosten & Drummond (2006)](https://arxiv.org/abs/0810.2434)가 제안했다.

알고리즘은 단순하다:

1. 후보 픽셀 $p$를 중심으로 반지름 3의 원 위에 16개 픽셀을 배치 (Bresenham circle).
2. 원 위의 연속된 $N$개 픽셀(보통 $N=12$)이 모두 $p$보다 밝거나 모두 어두우면 $p$는 코너.
3. 고속 reject: 1, 5, 9, 13번 위치의 4개 픽셀만 먼저 확인 — 이 중 3개 이상이 조건을 만족하지 않으면 즉시 기각.

$$\text{FAST condition: } \exists \text{ contiguous arc of } N \text{ pixels on circle, all } > I_p + t \text{ or all } < I_p - t$$

여기서 $t$는 밝기 임계값.

**Decision tree 학습**: FAST는 추가로 머신러닝(ID3 decision tree)을 활용하여 검사 순서를 최적화한다. 어떤 픽셀을 먼저 검사해야 가장 빠르게 비-코너를 기각할 수 있는지를 학습한다.

**FAST의 한계**: 방향(orientation)과 스케일(scale) 정보가 없고, 디스크립터를 생성하지 않는다. 이를 보완한 것이 ORB다.

```python
# FAST Corner Detection
fast = cv2.FastFeatureDetector_create(threshold=20, nonmaxSuppression=True)
keypoints = fast.detect(img, None)
print(f"Detected {len(keypoints)} keypoints")
```

#### ORB (Oriented FAST and Rotated BRIEF, 2011)

ORB는 [Rublee et al. (2011)](https://ieeexplore.ieee.org/document/6126544)이 SIFT/SURF의 특허 문제와 속도 문제를 동시에 해결하기 위해 제안한 방법이다. **FAST 검출기에 방향성을 부여하고, BRIEF 디스크립터에 회전 불변성을 추가**한 조합이다.

**oFAST (Oriented FAST)**:
- FAST로 검출한 키포인트에 **intensity centroid** 방법으로 방향을 할당.
- 키포인트 주변 패치의 이미지 모멘트를 계산:

$$m_{pq} = \sum_{x, y} x^p y^q I(x, y)$$

- Centroid: $\mathbf{C} = (m_{10}/m_{00}, m_{01}/m_{00})$
- 방향: $\theta = \text{atan2}(m_{01}, m_{10})$

**rBRIEF (Rotated BRIEF)**:
- BRIEF는 패치 내의 랜덤 점 쌍 $(x_i, y_i)$의 밝기를 비교하여 이진 디스크립터를 생성:

$$\tau(\mathbf{p}; x_i, y_i) = \begin{cases} 1 & \text{if } I(\mathbf{p}, x_i) < I(\mathbf{p}, y_i) \\ 0 & \text{otherwise} \end{cases}$$

- 256개 비교 → 256-bit 이진 디스크립터.
- ORB는 키포인트 방향 $\theta$에 따라 비교 점 쌍을 회전시켜 **회전 불변성**을 확보.
- 추가로, 비교 점 쌍의 상관관계를 최소화하도록 그리디하게 선택하여 분별력(discriminability)을 높인다.

**멀티 스케일**: 이미지 피라미드(보통 8단계)를 구성하고 각 레벨에서 FAST를 수행하여 스케일 불변성을 근사한다.

ORB는 **ORB-SLAM 시리즈의 핵심 특징점**이며, 이진 디스크립터 덕분에 매칭이 Hamming distance로 수행되어 매우 빠르다.

```python
# ORB Detection & Description
orb = cv2.ORB_create(nfeatures=1000)
keypoints, descriptors = orb.detectAndCompute(img, None)
# descriptors.shape: (N, 32) — 256-bit = 32 bytes
```

### 5.2.2 Blob Detection: SIFT → SURF

#### SIFT (Scale-Invariant Feature Transform, 2004)

SIFT는 [Lowe (2004)](https://link.springer.com/article/10.1023/B:VISI.0000029664.99615.94)가 제안한, **스케일, 회전, 조명 변화에 불변한** 특징점 검출 및 기술 알고리즘이다. 20년간 특징점 매칭의 사실상 표준이었으며, 2020년 특허 만료 이후 자유롭게 사용 가능하다.

**Stage 1 — Scale-Space Extrema Detection (DoG)**:

이미지를 다양한 스케일의 가우시안으로 블러링한 scale-space를 구성한다:

$$L(x, y, \sigma) = G(x, y, \sigma) * I(x, y)$$

여기서 $G(x, y, \sigma) = \frac{1}{2\pi\sigma^2} \exp\left(-\frac{x^2 + y^2}{2\sigma^2}\right)$.

Laplacian of Gaussian (LoG)의 효율적 근사로 **Difference of Gaussians (DoG)**를 사용한다:

$$D(x, y, \sigma) = L(x, y, k\sigma) - L(x, y, \sigma) \approx (k-1)\sigma^2 \nabla^2 G$$

DoG 이미지에서 공간적(8개 이웃) + 스케일(상하 각 9개) = 26개 이웃과 비교하여 극값(extrema)을 찾는다.

**Stage 2 — Keypoint Localization**:

스케일-공간에서 Taylor 전개를 이용한 서브픽셀/서브스케일 위치 정밀화:

$$D(\mathbf{x}) = D + \frac{\partial D}{\partial \mathbf{x}}^\top \mathbf{x} + \frac{1}{2} \mathbf{x}^\top \frac{\partial^2 D}{\partial \mathbf{x}^2} \mathbf{x}$$

극값의 정밀 위치: $\hat{\mathbf{x}} = -\frac{\partial^2 D}{\partial \mathbf{x}^2}^{-1} \frac{\partial D}{\partial \mathbf{x}}$

저대비 키포인트 제거: $|D(\hat{\mathbf{x}})| < 0.03$이면 제거.
엣지 응답 제거: Hessian 행렬의 고유값 비를 이용하여 엣지 위의 불안정한 극값을 제거.

**Stage 3 — Orientation Assignment**:

키포인트 주변의 그래디언트 크기와 방향을 계산하고, 36-bin 방향 히스토그램을 생성한다 (가우시안 가중, $\sigma = 1.5 \times$ 키포인트 스케일). 최대 피크의 80% 이상인 피크가 있으면 해당 방향에도 별도 키포인트를 생성하여 **회전 불변성**을 확보한다.

**Stage 4 — Keypoint Descriptor**:

키포인트 주변 $16 \times 16$ 영역을 $4 \times 4$ 블록 16개로 분할. 각 블록에서 8-bin 방향 히스토그램을 생성한다:

$$\text{Descriptor} = 4 \times 4 \times 8 = 128\text{-dimensional vector}$$

L2 정규화 후 0.2 이상의 값을 클리핑하고 재정규화하여 비선형 조명 변화에 강건하게 만든다.

```python
# SIFT Detection & Description
sift = cv2.SIFT_create(nfeatures=2000)
keypoints, descriptors = sift.detectAndCompute(img, None)
# descriptors.shape: (N, 128) — 128-dimensional float32
```

#### SURF (Speeded-Up Robust Features, 2006)

SURF는 [Bay et al. (2006)](https://link.springer.com/chapter/10.1007/11744023_32)이 SIFT의 속도 문제를 해결하기 위해 제안했다. 핵심 아이디어:

- **Integral image**를 이용한 박스 필터로 LoG를 근사. 어떤 크기의 박스 필터든 O(1)에 계산.
- **Hessian determinant**를 검출 기준으로 사용 (DoG 대신):

$$\det(\mathbf{H}) = D_{xx} D_{yy} - (0.9 \cdot D_{xy})^2$$

- 64차원 디스크립터 (SIFT의 128차원 대비 절반): Haar wavelet 응답의 합과 절댓값 합.
- SIFT 대비 3~7배 빠르면서 유사한 정확도.

SURF는 특허 문제로 최근에는 잘 사용되지 않으며, 실시간 응용에서는 ORB가, 정확도가 중요한 응용에서는 SIFT나 학습 기반 방법이 선호된다.

### 5.2.3 Binary Descriptors: BRIEF, ORB, BRISK

이진 디스크립터는 패치 내 점 쌍의 밝기를 비교하여 0/1 비트열을 생성하는 디스크립터다. **Hamming distance**로 매칭하므로 float 디스크립터 대비 수십 배 빠르다.

| 디스크립터 | 비트 수 | 특징 |
|-----------|--------|------|
| BRIEF | 128/256/512 | 랜덤 점 쌍, 회전 불변 아님 |
| ORB | 256 | 학습된 점 쌍, 회전 불변 |
| BRISK | 512 | 동심원 샘플링, 스케일 불변 |

BRIEF의 비교 연산:

$$b_i = \begin{cases} 1 & \text{if } I(\mathbf{p}_i) < I(\mathbf{q}_i) \\ 0 & \text{otherwise} \end{cases}, \quad \text{descriptor} = \sum_{1 \le i \le n} 2^{i-1} b_i$$

Hamming distance는 XOR + popcount로 CPU에서 단일 명령어로 계산된다:

$$d_H(\mathbf{a}, \mathbf{b}) = \text{popcount}(\mathbf{a} \oplus \mathbf{b})$$

### 5.2.4 기술 계보: 정확도 vs 속도 트레이드오프의 역사

전통적 특징점의 역사는 **정확도와 속도의 트레이드오프**를 최적화하려는 시도의 연속이었다:

```
Harris (1988)     — 코너 검출의 수학적 정의
    ↓ 스케일 불변성 필요
SIFT (2004)       — scale-space + 128D float descriptor (정확하지만 느림)
    ↓ 속도 개선
SURF (2006)       — integral image + 64D (3~7× 빠름, 여전히 float)
    ↓ 실시간 요구
FAST (2006)       — 극한 속도 검출 (descriptor 없음)
    ↓ 검출+기술 통합
ORB (2011)        — oFAST + rBRIEF, 256-bit binary (Hamming 매칭)
```

이 트레이드오프는 딥러닝 시대에도 계속되며, SuperPoint는 SIFT급 정확도를 ORB급 속도로 달성하는 것을 목표로 했다.

---

## 5.3 전통적 Matching & Outlier Rejection

특징점을 검출하고 디스크립터를 추출한 뒤, **어떤 특징점 쌍이 실제로 같은 3D 점에 해당하는지**를 결정하는 매칭 단계가 필요하다.

### 5.3.1 Brute-Force Matching

가장 단순한 방법. 이미지 A의 모든 디스크립터를 이미지 B의 모든 디스크립터와 비교하여 가장 가까운 것을 매칭한다.

- Float 디스크립터 (SIFT): L2 거리
- Binary 디스크립터 (ORB): Hamming 거리
- 시간 복잡도: $O(N \cdot M \cdot D)$ — $N, M$은 각 이미지의 특징점 수, $D$는 디스크립터 차원

```python
# Brute-Force Matching
bf = cv2.BFMatcher(cv2.NORM_L2, crossCheck=True)  # SIFT
# bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)  # ORB
matches = bf.match(desc1, desc2)
matches = sorted(matches, key=lambda x: x.distance)
```

`crossCheck=True`는 **mutual nearest neighbor** 조건을 적용한다: A에서 B로의 최근접과 B에서 A로의 최근접이 일치할 때만 매칭을 수용.

### 5.3.2 FLANN (Fast Library for Approximate Nearest Neighbors)

대규모 디스크립터 집합에서는 brute-force가 비실용적이므로, **근사 최근접 이웃 탐색(ANN)**을 사용한다. FLANN은 kd-tree, randomized kd-tree, hierarchical k-means 등을 자동 선택한다.

```python
# FLANN Matching for SIFT
FLANN_INDEX_KDTREE = 1
index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5)
search_params = dict(checks=50)  # 탐색 노드 수 제한

flann = cv2.FlannBasedMatcher(index_params, search_params)
matches = flann.knnMatch(desc1, desc2, k=2)  # k=2 for ratio test
```

### 5.3.3 Lowe's Ratio Test

Lowe (2004)가 SIFT 논문에서 제안한 매칭 필터링 기법. **최근접 거리와 차근접 거리의 비율**이 임계값 이하일 때만 매칭을 수용한다:

$$\frac{d(\mathbf{f}, \mathbf{f}_1)}{d(\mathbf{f}, \mathbf{f}_2)} < \tau$$

여기서 $\mathbf{f}_1, \mathbf{f}_2$는 각각 최근접, 차근접 디스크립터, $\tau$는 보통 0.7~0.8.

직관: 올바른 매칭은 최근접이 확실히 가까우므로 $d_1 \ll d_2$. 잘못된 매칭은 여러 유사한 후보가 있으므로 $d_1 \approx d_2$.

```python
# Ratio test (Lowe's)
good_matches = []
for m, n in matches:  # m: best, n: second best
    if m.distance < 0.75 * n.distance:
        good_matches.append(m)
```

### 5.3.4 RANSAC 계열: RANSAC → PROSAC → MAGSAC++

매칭 단계 이후에도 **아웃라이어(잘못된 매칭)**가 존재한다. 기하학적 모델(Fundamental/Essential matrix)을 추정하면서 동시에 아웃라이어를 제거하는 것이 RANSAC 계열의 역할이다.

#### RANSAC (Random Sample Consensus, 1981)

[Fischler & Bolles (1981)](https://dl.acm.org/doi/10.1145/358669.358692)가 제안한 로버스트 추정 패러다임:

1. 전체 매칭 중 모델에 필요한 최소 $n$개를 무작위 추출 (예: Fundamental matrix는 8점, 7점, 또는 5점)
2. 추출한 점으로 모델 추정
3. 전체 매칭에서 모델과의 오차가 임계값 $t$ 이내인 점(인라이어)의 consensus set 구성
4. 가장 큰 consensus set을 가진 모델을 최종 선택
5. 인라이어 전체로 모델 재추정

**필요한 반복 횟수**:

$$k = \frac{\log(1 - p)}{\log(1 - w^n)}$$

- $p$: 원하는 성공 확률 (보통 0.99)
- $w$: 인라이어 비율
- $n$: 모델에 필요한 최소 점 수

예: 인라이어 50%, 8점 모델, 99% 신뢰 → $k \approx 1177$ 반복.

```python
# RANSAC으로 Fundamental matrix 추정
pts1 = np.float32([kp1[m.queryIdx].pt for m in good_matches])
pts2 = np.float32([kp2[m.trainIdx].pt for m in good_matches])

F, mask = cv2.findFundamentalMat(pts1, pts2, cv2.FM_RANSAC,
                                  ransacReprojThreshold=3.0,
                                  confidence=0.99)
inlier_matches = [m for m, flag in zip(good_matches, mask.ravel()) if flag]
```

#### PROSAC (Progressive Sample Consensus, 2005)

Chum & Matas가 제안했다. RANSAC이 균등 무작위 샘플링을 하는 반면, PROSAC은 **매칭 품질(예: 디스크립터 거리)** 순으로 정렬한 뒤 상위 매칭부터 점진적으로 샘플링한다.

직관: 좋은 매칭일수록 인라이어일 확률이 높으므로, 상위 매칭에서 먼저 모델을 시도하면 더 빨리 좋은 모델을 찾는다. RANSAC 대비 수~수십 배 빠른 수렴.

#### MAGSAC / MAGSAC++ (2019/2020)

Barath et al.이 제안. RANSAC의 핵심 문제 중 하나는 **임계값 $t$의 수동 설정**이다. MAGSAC은 이를 자동화한다:

- 모든 가능한 임계값 $\sigma$에 대해 모델의 품질을 marginalize:

$$Q(\theta) = \int_0^{\sigma_{\max}} q(\theta, \sigma) f(\sigma) d\sigma$$

여기서 $q(\theta, \sigma)$는 임계값 $\sigma$에서의 모델 $\theta$의 품질, $f(\sigma)$는 임계값의 사전 분포.

- MAGSAC++는 이를 더 효율적으로 구현하고, $\sigma$-consensus 기반 가중 최소자승 피팅을 추가.
- 임계값 선택에 대한 민감도가 크게 줄어든다.

```python
# OpenCV의 USAC (MAGSAC++ 포함)
F, mask = cv2.findFundamentalMat(
    pts1, pts2,
    cv2.USAC_MAGSAC,           # MAGSAC++ 사용
    ransacReprojThreshold=1.0,  # 덜 민감
    confidence=0.999,
    maxIters=10000
)
```

### 5.3.5 Fundamental / Essential Matrix Estimation

2D-2D 매칭으로부터 두 카메라 사이의 기하학적 관계를 추정하는 과정:

**Fundamental Matrix** ($\mathbf{F}$, 7 DOF):
- 내부 파라미터를 모를 때 사용
- 8-point algorithm: 최소 8개 대응점으로 선형 시스템을 풀고, rank-2 제약 적용
- 7-point algorithm: 최소 7개 대응점, 3차 다항식의 근으로 최대 3개 해

**Essential Matrix** ($\mathbf{E}$, 5 DOF):
- 내부 파라미터를 알 때 사용
- $\mathbf{E} = [\mathbf{t}]_\times \mathbf{R}$, $\mathbf{E} = \mathbf{K}_2^\top \mathbf{F} \mathbf{K}_1$
- 5-point algorithm (Nistér, 2004): 최소 5개 대응점, 10차 다항식으로 최대 10개 해

RANSAC과 결합하여 사용:

```python
# Essential matrix 추정 (calibrated camera)
E, mask = cv2.findEssentialMat(pts1, pts2, cameraMatrix=K,
                                method=cv2.RANSAC,
                                prob=0.999, threshold=1.0)

# Essential matrix에서 R, t 복원
_, R, t, mask_pose = cv2.recoverPose(E, pts1, pts2, cameraMatrix=K)
```

### 5.3.6 기술 계보: 로버스트 추정의 진화

```
Least Squares (아웃라이어에 취약)
    ↓ 아웃라이어 대응
RANSAC (1981)   — 최초의 로버스트 추정 패러다임
    ↓ 사전 정보 활용
PROSAC (2005)   — 매칭 품질 기반 진행적 샘플링
    ↓ 임계값 자동화
MAGSAC++ (2020) — 임계값-free 로버스트 추정
    ↓ 학습 기반 제거
GeoTransformer (2022) — RANSAC 없이 직접 변환 추정
```

---

## 5.4 Mutual Information & Intensity-Based Registration

### 5.4.1 MI의 정의와 직관

**Mutual Information (MI, 상호 정보량)**은 두 확률 변수가 서로에 대해 얼마나 많은 정보를 공유하는지를 측정하는 정보 이론적 척도다.

두 확률 변수 $X, Y$의 mutual information:

$$I(X; Y) = \sum_{x \in X} \sum_{y \in Y} p(x, y) \log \frac{p(x, y)}{p(x) p(y)}$$

연속 변수의 경우:

$$I(X; Y) = \int \int p(x, y) \log \frac{p(x, y)}{p(x) p(y)} \, dx \, dy$$

엔트로피(entropy)를 이용한 동치 표현:

$$I(X; Y) = H(X) + H(Y) - H(X, Y)$$

여기서:
- $H(X) = -\sum_x p(x) \log p(x)$: $X$의 엔트로피
- $H(X, Y) = -\sum_{x,y} p(x, y) \log p(x, y)$: 결합 엔트로피

**직관**: 두 이미지가 올바르게 정렬(aligned)되었을 때, 한 이미지의 픽셀 값을 알면 같은 위치의 다른 이미지 픽셀 값을 더 잘 예측할 수 있다. 즉 $I(X; Y)$가 최대가 된다. 정렬이 어긋나면 두 이미지의 관계가 약해져서 $I(X; Y)$가 감소한다.

핵심 성질: MI는 두 변수 사이의 **비선형적 통계적 의존성**을 측정한다. 단순한 상관계수(correlation coefficient)가 포착하지 못하는 관계도 잡아낸다.

### 5.4.2 MI 기반 다중 모달리티 정합

MI의 진정한 가치는 **서로 다른 모달리티의 센서 데이터를 정합**할 수 있다는 점이다. 원래 의료 영상(medical imaging)에서 CT-MRI 정합을 위해 개발된 방법이다.

왜 MI가 다중 모달리티에서 작동하는가?

- 같은 물체를 촬영한 CT와 MRI는 밝기 값의 분포가 완전히 다르다 (뼈가 CT에서 밝고, MRI에서 어두울 수 있다).
- 단순한 밝기 차이(SSD)나 상관관계(NCC)는 이러한 비선형 관계를 모델링하지 못한다.
- MI는 밝기 값 사이의 **통계적 의존성**만 측정하므로, 밝기 값의 단조(monotonic) 또는 비단조 관계에도 작동한다.

로보틱스에서의 응용: **카메라-LiDAR 정합**. LiDAR intensity 이미지와 카메라 이미지는 완전히 다른 물리적 양을 측정하지만, 같은 장면의 같은 구조물을 반영하므로 MI가 높다.

### 5.4.3 MI와 NMI의 실용적 계산

MI를 이미지 정합에 적용할 때, 확률 분포 $p(x), p(y), p(x, y)$는 **결합 히스토그램(joint histogram)**에서 추정한다.

이미지 $A$와 $B$가 변환 $T$로 정렬되어 있을 때:
1. 각 공통 위치 $(u, v)$에서 $A(u, v)$와 $B(T(u, v))$의 밝기 값 쌍을 수집.
2. 2D 히스토그램으로 결합 분포 $p(a, b)$를 추정.
3. 주변 분포 $p(a), p(b)$는 히스토그램의 행/열 합.
4. MI를 계산.

**Normalized Mutual Information (NMI)**는 MI를 정규화하여 오버랩 영역 크기에 대한 민감도를 줄인다:

$$NMI(A, B) = \frac{H(A) + H(B)}{H(A, B)}$$

또는:

$$NMI(A, B) = \frac{2 \cdot I(A; B)}{H(A) + H(B)}$$

NMI는 오버랩 영역이 변할 때도 안정적이므로 실용적으로 MI보다 선호된다.

### 5.4.4 MI Gradient 계산

MI를 정합의 목적 함수로 사용하려면 변환 파라미터에 대한 그래디언트를 계산해야 한다.

변환 $T_\xi$ (파라미터 $\xi$)에 의한 MI의 그래디언트:

$$\frac{\partial I}{\partial \xi} = \sum_{a, b} \frac{\partial p(a, b)}{\partial \xi} \left(1 + \log \frac{p(a, b)}{p(a) p(b)}\right)$$

결합 히스토그램이 이산적이면 그래디언트가 존재하지 않으므로, **Parzen 윈도우(커널 밀도 추정)** 또는 **B-spline** 기반의 미분 가능한 히스토그램 추정 방법을 사용한다.

실용적으로는 그래디언트 기반 최적화보다 **Nelder-Mead simplex** 같은 미분-free 최적화가 자주 사용되기도 한다 (Koide et al., 2023의 캘리브레이션 툴에서 사용).

### 5.4.5 왜 Calibration에서 MI가 쓰이는가

Ch.3의 targetless 카메라-LiDAR 캘리브레이션에서 MI(또는 NMI, NID)가 핵심 비용 함수로 사용된다:

1. LiDAR 점군을 현재 외부 파라미터 추정치로 카메라 이미지에 투영
2. 투영된 LiDAR intensity와 카메라 픽셀 밝기의 MI를 계산
3. 외부 파라미터를 조정하여 MI를 최대화

**Normalized Information Distance (NID)**는 Koide et al. (2023)의 범용 캘리브레이션 툴에서 사용된 MI 기반 거리 척도:

$$NID(A, B) = 1 - \frac{I(A; B)}{H(A, B)} = \frac{H(A|B) + H(B|A)}{H(A, B)}$$

NID는 0(완전 의존)에서 1(완전 독립)까지의 값을 가지며, 메트릭 공간의 성질을 만족한다.

```python
import numpy as np
from sklearn.metrics import mutual_info_score

def compute_mi(img_a, img_b, bins=256):
    """두 이미지의 Mutual Information 계산."""
    # Joint histogram
    hist_2d, _, _ = np.histogram2d(
        img_a.ravel(), img_b.ravel(), bins=bins
    )
    # Joint probability
    pxy = hist_2d / hist_2d.sum()
    px = pxy.sum(axis=1)
    py = pxy.sum(axis=0)
    
    # MI = H(X) + H(Y) - H(X,Y)
    hx = -np.sum(px[px > 0] * np.log(px[px > 0]))
    hy = -np.sum(py[py > 0] * np.log(py[py > 0]))
    hxy = -np.sum(pxy[pxy > 0] * np.log(pxy[pxy > 0]))
    
    return hx + hy - hxy

def compute_nid(img_a, img_b, bins=256):
    """Normalized Information Distance 계산."""
    mi = compute_mi(img_a, img_b, bins)
    hist_2d, _, _ = np.histogram2d(
        img_a.ravel(), img_b.ravel(), bins=bins
    )
    pxy = hist_2d / hist_2d.sum()
    hxy = -np.sum(pxy[pxy > 0] * np.log(pxy[pxy > 0]))
    
    return 1.0 - mi / hxy if hxy > 0 else 1.0
```

---

## 5.5 학습 기반 Feature Detection & Description

전통적 특징점은 밝기 그래디언트, 코너, 블롭 같은 **저수준 시각적 단서**에 의존한다. 이 때문에 조명 변화, 시점 변화, 날씨 변화에 취약하다. 딥러닝은 대량의 데이터로부터 더 강건한 특징 표현을 학습하여 이 한계를 극복했다.

### 5.5.1 SuperPoint (2018): 자기지도학습 기반 검출+기술의 통합

[DeTone et al. (2018)](https://arxiv.org/abs/1712.07629)의 SuperPoint는 **키포인트 검출과 디스크립터 추출을 단일 네트워크로 통합**한 최초의 실용적 딥러닝 파이프라인이다.

#### Homographic Adaptation: 핵심 학습 전략

SuperPoint의 가장 중요한 기술적 기여는 **라벨 없이 반복성 높은 키포인트 검출기를 학습하는 방법**이다.

1. 하나의 이미지에 무작위 호모그래피를 100회 이상 적용
2. 각 변환된 이미지에서 키포인트를 검출
3. 검출 결과를 원래 좌표계로 역변환하여 집계(aggregate)
4. 여러 변환에서 **일관되게 검출되는 점**만 pseudo ground-truth로 채택

수작업 라벨 없이도 반복성(repeatability) 높은 키포인트 검출기를 학습하는 방식이다.

#### 2단계 학습: MagicPoint → SuperPoint

- **1단계**: 합성 기하 도형(삼각형, 사각형, 선분 등)으로 구성된 Synthetic Shapes 데이터셋에서 코너/접합점 검출기(**MagicPoint**)를 사전 학습.
- **2단계**: MagicPoint를 MS-COCO 등 실제 이미지에 Homographic Adaptation과 함께 적용하여, 실제 장면에서의 pseudo ground-truth를 생성하고 SuperPoint를 학습.

#### 아키텍처

VGG 스타일 인코더(공유 백본) → 두 개의 디코더 헤드로 분기:

**Interest Point Decoder**: 
- 입력 이미지를 8×8 셀 그리드로 나눔
- 각 셀에서 65채널 (64개 위치 + 1 "no interest point") softmax 수행
- 셀 내 위치를 직접 예측하는 방식으로 픽셀 단위 키포인트 히트맵 생성

**Descriptor Decoder**: 
- 공유 백본의 feature map에서 256차원 디스크립터 맵을 출력
- 검출된 키포인트 위치에서 bi-cubic interpolation으로 샘플링
- L2 정규화 적용

#### 학습 손실

- 키포인트 검출: cross-entropy loss
- 디스크립터: 호모그래피로 대응점을 알고 있으므로 positive/negative pair에 대한 hinge loss:

$$L_{desc} = \sum_{(i,j) \in \text{pos}} \max(0, m_p - \mathbf{d}_i^\top \mathbf{d}_j) + \sum_{(i,j) \in \text{neg}} \max(0, \mathbf{d}_i^\top \mathbf{d}_j - m_n)$$

여기서 $m_p, m_n$은 positive/negative margin.

**성능**: 단일 포워드 패스로 검출+기술을 동시 수행. 640×480 이미지에서 약 70 FPS (GPU 기준).

```python
import torch
# SuperPoint 사용 예제 (hloc / kornia)
from kornia.feature import SuperPoint as KorniaSuperPoint

# 모델 로드
sp = KorniaSuperPoint(max_num_keypoints=2048)
sp = sp.eval()

# 추론
with torch.no_grad():
    img_tensor = torch.from_numpy(img).float().unsqueeze(0).unsqueeze(0) / 255.0
    pred = sp(img_tensor)
    keypoints = pred['keypoints']        # (1, N, 2)
    descriptors = pred['descriptors']    # (1, 256, N)
    scores = pred['scores']              # (1, N)
```

### 5.5.2 D2-Net (2019): Detect-and-Describe Jointly

[D2-Net (Dusmanu et al., 2019)](https://arxiv.org/abs/1905.03561)은 검출과 기술을 더 극단적으로 통합한 방법이다. SuperPoint가 여전히 검출 헤드와 기술 헤드를 분리한 반면, D2-Net은 **같은 특징 맵에서 검출과 기술을 동시에 수행**한다.

핵심 아이디어: VGG16의 중간 특징 맵 $\mathbf{F} \in \mathbb{R}^{H \times W \times C}$를 사용하여:
- **Detection**: 각 위치에서 채널 축 최대값을 취한 뒤, 공간적 NMS를 적용하여 키포인트 선택
- **Description**: 같은 위치의 $C$-차원 벡터를 디스크립터로 사용

장점: 높은 수준의 의미적(semantic) 특징을 사용하므로 큰 외관 변화에 강건.
단점: 검출 반복성이 SuperPoint보다 낮을 수 있고, 입력 해상도의 1/4까지만 로컬라이제이션 가능.

### 5.5.3 R2D2 (2019): Reliable and Repeatable Detector-Descriptor

[R2D2 (Revaud et al., 2019)](https://arxiv.org/abs/1906.06195)는 SuperPoint와 D2-Net의 한계를 분석하고, **반복성(repeatability)과 신뢰성(reliability)을 명시적으로 학습**하는 방법을 제안했다.

- **Repeatability**: 다양한 시점에서 같은 점이 검출되는가?
- **Reliability**: 검출된 점의 디스크립터가 매칭에 유용한가? (텍스처 없는 영역의 키포인트는 반복적이어도 매칭에 쓸모없다)

R2D2는 두 가지를 별도의 confidence map으로 예측하고, 이들의 곱으로 최종 키포인트 스코어를 결정한다.

### 5.5.4 DISK (2020)

[DISK (Tyszkiewicz et al., 2020)](https://arxiv.org/abs/2006.13566)는 **reinforcement learning** 관점에서 키포인트 검출을 학습한다. 매칭에 성공하면 보상(reward), 실패하면 벌칙(penalty)을 주어 검출기를 학습한다.

핵심 차별점: 매칭 정확도를 직접 최적화하는 방식으로, 검출과 매칭의 end-to-end 최적화에 한 발 더 다가갔다.

### 5.5.5 전통 대비 장점: Illumination/Viewpoint Invariance 향상

학습 기반 특징점이 전통적 방법 대비 우위를 가지는 구체적 시나리오:

| 시나리오 | SIFT/ORB 한계 | SuperPoint/D2-Net 개선 |
|---------|-------------|---------------------|
| 극한 조명 변화 (주야간) | DoG/그래디언트 기반 검출 실패 | 학습된 특징은 고수준 구조 포착 |
| 넓은 시점 변화 | affine 근사의 한계 | 대량의 훈련 데이터로 시점 불변성 학습 |
| 반복 패턴 | 디스크립터가 유사하여 모호 매칭 | 컨텍스트 정보를 포착하여 분별 |
| 모션 블러 | 그래디언트 약화 → 검출 실패 | CNN이 블러에 대한 강건성 학습 |

그러나 학습 기반 방법의 한계도 존재한다: 학습 데이터의 도메인에 의존적이며, 완전히 새로운 환경(예: 수중, 화성)에서는 성능이 저하될 수 있다.

---

## 5.6 학습 기반 Feature Matching

### 5.6.1 SuperGlue (2020): Attention 기반 매칭

[Sarlin et al. (2020)](https://arxiv.org/abs/1911.11763)의 SuperGlue는 키포인트 매칭을 **그래프 신경망(GNN)과 어텐션 메커니즘**으로 학습 가능한 문제로 재정의했다. 전통적인 nearest-neighbor 매칭을 학습 기반 매칭으로 대체한 최초의 상용급 시스템이다.

#### 문제 정의: 부분 할당(Partial Assignment)

두 이미지에서 추출된 키포인트 집합 $\mathcal{A} = \{(\mathbf{p}_i, \mathbf{d}_i)\}_{i=1}^{N}$과 $\mathcal{B} = \{(\mathbf{p}_j, \mathbf{d}_j)\}_{j=1}^{M}$ 사이의 대응을 찾되, **모든 키포인트가 대응점을 갖는 것은 아니다**. 이를 위해 "매칭 없음(dustbin)"이라는 가상 노드를 추가하여 unmatchable 포인트를 명시적으로 처리한다.

#### Keypoint Encoder

키포인트 위치 $(x, y)$와 검출 confidence $c$를 MLP로 임베딩하여 디스크립터 벡터에 더한다:

$$\mathbf{f}_i^{(0)} = \mathbf{d}_i + \text{MLP}_{\text{enc}}([\mathbf{p}_i, c_i])$$

이로써 기하학적 정보가 특징에 내재된다.

#### Attentional Graph Neural Network

키포인트를 그래프의 노드로 표현하고, Self-Attention과 Cross-Attention을 교대로 적용한다:

**Self-Attention (intra-image)**: 같은 이미지 내 키포인트들 사이의 관계를 학습. 예를 들어, 건물의 코너들이 직선 위에 정렬되어 있다는 구조적 정보를 포착한다.

$$\text{message}_i^{\text{self}} = \sum_{j \in \mathcal{A}} \text{softmax}\left(\frac{\mathbf{q}_i^\top \mathbf{k}_j}{\sqrt{d}}\right) \mathbf{v}_j$$

**Cross-Attention (inter-image)**: 두 이미지 간 키포인트 사이의 관계를 학습. "이 키포인트는 상대 이미지의 어떤 키포인트와 유사한가"를 어텐션으로 추론한다.

$$\text{message}_i^{\text{cross}} = \sum_{j \in \mathcal{B}} \text{softmax}\left(\frac{\mathbf{q}_i^\top \mathbf{k}_j}{\sqrt{d}}\right) \mathbf{v}_j$$

이를 $L$번(논문에서는 9회) 교대로 반복하여 점진적으로 매칭 정보를 정제한다.

#### Optimal Transport를 이용한 매칭

최종 매칭 점수 행렬을 GNN 출력의 내적으로 계산한 뒤, **Sinkhorn 알고리즘**(soft한 형태의 Hungarian algorithm)을 적용한다.

점수 행렬 $\mathbf{S} \in \mathbb{R}^{(N+1) \times (M+1)}$ (dustbin 포함):

$$S_{ij} = \langle \mathbf{f}_i^{(L)}, \mathbf{f}_j^{(L)} \rangle, \quad S_{i,M+1} = S_{N+1,j} = z$$

여기서 $z$는 학습 가능한 dustbin score.

Sinkhorn 정규화를 반복 적용(약 100회):

$$\mathbf{S} \leftarrow \text{row-normalize}(\mathbf{S}), \quad \mathbf{S} \leftarrow \text{col-normalize}(\mathbf{S})$$

수렴 후 soft assignment matrix를 threshold하여 최종 매칭을 결정한다.

#### 학습

Ground-truth 대응점(호모그래피 또는 상대 포즈 + 깊이 맵에서 생성)에 대한 negative log-likelihood 최대화로 end-to-end 학습:

$$L = -\sum_{(i,j) \in \mathcal{M}} \log \hat{P}_{ij} - \sum_{i \in \mathcal{U}_A} \log \hat{P}_{i, M+1} - \sum_{j \in \mathcal{U}_B} \log \hat{P}_{N+1, j}$$

여기서 $\mathcal{M}$은 매칭된 쌍, $\mathcal{U}_A, \mathcal{U}_B$는 매칭되지 않는 키포인트.

```python
# SuperGlue 사용 예제 (hloc)
from hloc import match_features, extract_features
from hloc.utils.io import list_h5_names

# SuperPoint 특징 추출
feature_conf = extract_features.confs['superpoint_aachen']
features_path = extract_features.main(feature_conf, images_dir)

# SuperGlue 매칭
match_conf = match_features.confs['superglue']
matches_path = match_features.main(match_conf, pairs_path, feature_conf['output'], features_path)
```

#### 기술 계보에서의 위치

SuperPoint가 검출+기술을 학습화했다면, SuperGlue는 **매칭 단계를 학습화**했다. 이로써 detect-then-describe-then-match 파이프라인의 **세 단계 모두가 딥러닝으로 대체**되었다. 그러나 여전히 파이프라인의 직렬적 3단계 구조 자체는 유지된다. 이 구조적 한계를 깨뜨린 것이 LoFTR이다.

### 5.6.2 LightGlue (2023): SuperGlue의 효율화

[Lindenberger et al. (2023)](https://arxiv.org/abs/2306.13643)의 LightGlue는 SuperGlue의 정확도를 유지하면서 **적응적 연산량 조절(adaptive computation)**로 속도를 3-5배 끌어올렸다.

#### SuperGlue의 문제점 진단

- 항상 고정된 9개 GNN 레이어와 100회 Sinkhorn 반복을 수행 → 쉬운 매칭에도 불필요하게 많은 연산.
- 키포인트 수 $N$에 대해 $O(N^2)$ attention이 반복되므로, 키포인트가 많을수록 급격히 느려짐.

#### 핵심 개선: Adaptive Depth & Width

**Adaptive Depth (레이어 조기 종료)**:
- 각 레이어 이후에 경량 classifier(MLP)가 매칭 확신도를 예측.
- 확신도가 충분히 높으면 네트워크를 조기 종료.
- 쉬운 이미지 쌍은 2-3 레이어만으로 처리, 어려운 쌍은 전체 9 레이어 사용.

**Adaptive Width (키포인트 pruning)**:
- 매칭 불가능하다고 판단된 키포인트를 중간 레이어에서 제거.
- Attention의 시퀀스 길이가 점진적으로 줄어들어 후반 레이어의 계산이 가벼워짐.

**Sinkhorn 제거**: 
Optimal Transport 대신 단순한 **dual-softmax + mutual nearest neighbor**로 매칭. Sinkhorn의 반복 비용을 완전히 제거하면서도 성능 저하가 거의 없다.

$$P_{ij} = \text{softmax}_j(S_{ij}) \cdot \text{softmax}_i(S_{ij})$$

#### 학습 전략

- 학습 시에는 적응적 종료를 사용하지 않고 전체 레이어를 통과시키되, 각 레이어의 출력에 supervision을 적용 (**deep supervision**).
- 추론 시에만 적응적 종료를 활성화.
- SuperPoint, DISK, ALIKED 등 다양한 로컬 특징 검출기와 호환되도록 범용 설계.

**성능**: SuperGlue 대비 동등한 정확도에서 **3-5배 빠름**. 쉬운 쌍에서는 최대 10배 이상 속도 향상.

```python
# LightGlue 사용 예제 (kornia / hloc)
from kornia.feature import LightGlue as KorniaLightGlue

# SuperPoint + LightGlue 조합
lg = KorniaLightGlue(features='superpoint')
lg = lg.eval()

with torch.no_grad():
    # pred0, pred1: SuperPoint 출력
    matches = lg({'image0': pred0, 'image1': pred1})
    # matches['matches']: (K, 2) — 매칭된 키포인트 인덱스 쌍
    # matches['scores']: (K,) — 매칭 신뢰도
```

### 5.6.3 기술 흐름: Detect → Describe → Match 분리 파이프라인의 딥러닝화

```
[전통적 파이프라인]
SIFT detect → SIFT descriptor → BF/FLANN + ratio test
                                        ↓
[학습 기반 대체]
SuperPoint detect+describe → SuperGlue attention matching → LightGlue 효율화
    (2018)                       (2020)                        (2023)
```

이 진화 경로의 핵심 서사: **파이프라인의 각 단계를 하나씩 딥러닝으로 대체하되, 3단계 직렬 구조 자체는 유지**. 이 구조의 장점은 모듈성과 해석 가능성이며, 단점은 검출 단계의 실패가 전체 파이프라인의 실패로 이어진다는 것이다. 이 구조적 한계를 깨뜨린 것이 다음 절의 Detector-Free 패러다임이다.

---

## 5.7 Detector-Free Matching (패러다임 전환)

### 5.7.1 왜 Detector-Free가 필요한가

Detect-then-match 파이프라인은 **검출기가 키포인트를 찾지 못하면 매칭 자체가 불가능**하다는 근본적 한계가 있다. 이는 다음과 같은 실전 환경에서 치명적이다:

- **텍스처 없는 영역**: 흰 벽, 콘크리트 바닥, 하늘 — 그래디언트가 약해 코너/블롭 검출 실패
- **반복 패턴**: 타일, 창문 격자 — 검출은 되지만 디스크립터가 유사하여 모호 매칭
- **넓은 시점 변화**: 극단적 시점 차이에서 로컬 패치의 외관이 완전히 달라짐

Detector-free 접근법은 검출 단계를 없애고, **이미지의 모든 위치를 잠재적 매칭 후보로 취급**하여 이 한계를 극복한다.

### 5.7.2 LoFTR (2021): Coarse-to-Fine Transformer Matching

[Sun et al. (2021)](https://arxiv.org/abs/2104.00680)의 LoFTR는 **검출기 없이 두 이미지 간 밀집 매칭을 수행하는 트랜스포머 기반 아키텍처**로, detector-free matching이라는 새로운 패러다임을 열었다.

#### 아키텍처

**1. Local Feature CNN**: ResNet-18 기반 FPN(Feature Pyramid Network)으로 두 이미지에서 coarse (1/8 해상도) 및 fine (1/2 해상도) 특징 맵을 추출한다.

**2. Coarse-Level Matching (Transformer)**:

1/8 특징 맵을 1D 시퀀스로 평탄화(flatten)하고, Self-Attention + Cross-Attention을 교대로 $N$번(보통 4회) 반복하는 트랜스포머 모듈을 적용한다.

위치 인코딩: sinusoidal positional encoding으로 공간 정보를 보존.

출력 특징들 간의 내적으로 score matrix를 계산하고, **dual-softmax**로 confidence matrix를 생성:

$$P_{ij} = \text{softmax}_j(\mathbf{f}_i^A \cdot \mathbf{f}_j^B) \cdot \text{softmax}_i(\mathbf{f}_i^A \cdot \mathbf{f}_j^B)$$

Threshold + mutual nearest neighbor 조건으로 coarse 매칭을 추출한다.

**3. Fine-Level Refinement**:

Coarse 매칭 각각에 대해, 1/2 해상도 특징 맵에서 해당 위치 주변 $w \times w$ 윈도우(보통 5×5)를 crop한다.
윈도우 내에서 다시 cross-attention 기반 correlation을 수행하여 **sub-pixel 정밀도**의 매칭 위치를 회귀(regression)한다.

#### Linear Transformer

계산량 절감을 위해 standard softmax attention 대신 **kernel-based linear attention**을 사용:

Standard attention: $O(N^2)$, $\text{Attn}(\mathbf{Q}, \mathbf{K}, \mathbf{V}) = \text{softmax}(\mathbf{Q}\mathbf{K}^\top / \sqrt{d}) \mathbf{V}$

Linear attention: $O(N)$, $\text{Attn}(\mathbf{Q}, \mathbf{K}, \mathbf{V}) = \phi(\mathbf{Q})(\phi(\mathbf{K})^\top \mathbf{V})$

여기서 $\phi$는 ELU 기반 커널 함수. 행렬 곱의 결합 순서를 바꿔 $O(N)$ 복잡도를 달성한다.

단, 후속 연구에서 standard attention이 정확도에서 우위라는 점이 밝혀졌다.

#### 학습

Ground-truth 포즈 + 깊이 맵에서 생성한 대응점을 supervision으로 사용:
- Coarse level: cross-entropy loss
- Fine level: L2 regression loss
- ScanNet(실내) 및 MegaDepth(실외) 데이터셋에서 학습

```python
# LoFTR 사용 예제 (kornia)
from kornia.feature import LoFTR as KorniaLoFTR

loftr = KorniaLoFTR(pretrained='outdoor')
loftr = loftr.eval()

with torch.no_grad():
    input_dict = {
        'image0': img0_tensor,  # (1, 1, H, W) grayscale
        'image1': img1_tensor,
    }
    result = loftr(input_dict)
    
    mkpts0 = result['keypoints0']     # (K, 2) — 이미지 0의 매칭 좌표
    mkpts1 = result['keypoints1']     # (K, 2) — 이미지 1의 매칭 좌표
    confidence = result['confidence'] # (K,) — 매칭 신뢰도
```

#### 기술 계보에서의 위치

LoFTR는 **패러다임 전환점**이다. SuperPoint→SuperGlue로 이어진 detect-then-match 파이프라인과 결별하고, 검출기를 완전히 제거했다. 트랜스포머의 어텐션이 검출과 매칭을 동시에 수행한다. 이미지의 모든 위치가 상대 이미지의 모든 위치와 직접 소통(cross-attention)하므로, 별도의 검출 단계 없이도 매칭이 가능해졌다.

### 5.7.3 QuadTree Attention: LoFTR의 효율화

LoFTR의 coarse-level transformer는 이미지의 모든 위치를 시퀀스로 평탄화하므로, 해상도가 높아지면 시퀀스 길이가 급격히 증가한다. QuadTree Attention (Tang et al., 2022)은 이를 해결한다.

핵심 아이디어: 어텐션을 계층적으로 수행한다.

1. 가장 거친(coarsest) 해상도에서 전체 어텐션을 수행
2. 높은 어텐션 점수를 가진 영역만 선택
3. 선택된 영역에서만 다음 해상도의 어텐션을 수행
4. 이를 반복하여 관련 영역에만 집중하는 계층적 어텐션 실현

이를 통해 $O(N^2)$에서 $O(N \log N)$으로 복잡도를 줄이면서도 LoFTR 수준의 정확도를 유지한다.

### 5.7.4 ASpanFormer (2022): Adaptive Span Attention

ASpanFormer (Chen et al., 2022)는 LoFTR의 또 다른 한계를 해결한다: **모든 위치가 같은 크기의 어텐션 범위를 가져야 하는가?**

핵심 아이디어: 각 위치마다 **적응적으로 어텐션 범위(span)**를 조절한다.

- 텍스처가 풍부한 영역: 좁은 범위로 정밀한 매칭
- 텍스처가 부족한 영역: 넓은 범위로 컨텍스트를 활용한 매칭

이를 통해 LoFTR이 텍스처 없는 영역에서 보이던 매칭 정확도 저하를 완화한다.

### 5.7.5 RoMa (2024): DINOv2 + Dense Matching의 성숙

[Edstedt et al. (2024)](https://arxiv.org/abs/2305.15404)의 RoMa는 detector-free 매칭의 **성숙 단계**를 대표한다. 두 가지 핵심 진화를 통해 LoFTR 계열을 큰 폭으로 상회한다.

#### Foundation Model 활용

LoFTR가 처음부터(scratch) 학습한 것과 달리, RoMa는 **사전학습된 DINOv2 ViT-Large를 특징 추출기로 사용**한다 (가중치 동결).

DINOv2는 대규모 자기지도학습으로 학습된 범용 시각 특징으로, 이미 풍부한 의미적(semantic) 정보를 내포한다. 이는 "특징 학습은 범용 모델에 맡기고, 매칭 로직만 학습하면 된다"는 철학적 전환이다.

#### 아키텍처

**1. Frozen DINOv2 Backbone**: 사전학습된 DINOv2 ViT-Large에서 1/14 해상도의 patch 특징을 추출한다.

**2. Coarse Matching (Warp Estimation)**:
- DINOv2 특징에 cross-attention (transformer decoder)을 적용.
- 이미지 A의 각 위치에 대해, 이미지 B에서의 대응 위치를 **확률 분포**로 예측:

$$p(\mathbf{x}_B | \mathbf{x}_A) = \sum_{k} w_k \mathcal{N}(\mathbf{x}_B; \mu_k, \Sigma_k)$$

단일 점이 아닌 확률 분포로 예측함으로써, 모호한 매칭의 불확실성을 명시적으로 표현한다.

**3. Fine Matching (Iterative Refinement)**:
- Coarse warp를 초기값으로, CNN 기반의 fine-level 특징을 사용하여 반복적으로 정밀화.
- 각 정밀화 단계에서 해상도를 높이며, 이전 단계의 warp를 기반으로 local correlation을 계산하고 잔차를 예측.
- RAFT와 유사한 iterative refinement 철학이지만, optical flow가 아닌 sparse-to-dense warp에 적용.

**4. Certainty Estimation**: 각 매칭에 대해 신뢰도(certainty)를 함께 예측하여, 후처리 시 고신뢰 매칭만 선택적으로 사용 가능.

#### Robust Regression

매칭 위치를 단순히 L2 loss로 회귀하는 대신, 예측된 확률 분포와 ground-truth 간의 **negative log-likelihood**를 최적화한다:

$$L = -\sum_{(\mathbf{x}_A, \mathbf{x}_B^*)} \log p(\mathbf{x}_B^* | \mathbf{x}_A)$$

이 접근법은 아웃라이어에 강건하다: 잘못된 ground-truth 대응점이 있더라도 분포의 꼬리(tail)로 흡수되어 학습이 안정적이다.

#### 성능

MegaDepth 및 ScanNet 벤치마크에서 LoFTR, ASpanFormer 등을 큰 폭으로 상회. 특히 **넓은 baseline(큰 시점 변화)**에서의 성능 향상이 두드러진다.

```python
# RoMa 사용 예제
from romatch import roma_outdoor

# 모델 로드 (DINOv2 backbone 포함)
roma_model = roma_outdoor(device='cuda')
roma_model.eval()

# 매칭
warp, certainty = roma_model.match(img0_path, img1_path)

# 고신뢰 매칭만 추출
matches, certainty_scores = roma_model.sample(
    warp, certainty,
    num=5000  # 최대 매칭 수
)
# matches: (K, 4) — [x0, y0, x1, y1] 정규화 좌표
```

#### 기술 계보에서의 위치

RoMa는 두 가지 핵심 전환을 체현한다:

1. **Foundation Model 활용**: 처음부터 학습 → 대규모 사전학습 모델의 특징을 기반으로 매칭 로직만 학습
2. **확률적 매칭**: 결정론적 점 예측 → 확률 분포 예측, 불확실한 매칭을 명시적으로 다룸

RoMa는 RAFT의 iterative refinement 아이디어와 LoFTR의 detector-free 사고방식을 결합하면서, DINOv2의 사전학습 특징을 도입한 종합적 발전이다.

### 5.7.6 최신 동향: 3D-Aware Dense Matching (2024-2025)

2024-2025년에는 2D 매칭을 넘어 **3D 기하학을 직접 예측하면서 매칭을 수행**하는 패러다임이 등장했다.

**DUSt3R (Leroy et al., 2024)**: [DUSt3R](https://arxiv.org/abs/2312.14132)는 캘리브레이션이나 포즈 정보 없이 임의의 이미지 쌍에서 직접 3D pointmap을 회귀하는 방법이다. 기존 매칭 파이프라인이 "2D 매칭 → 3D 복원"의 순서를 따른 반면, DUSt3R는 이를 뒤집어 **3D 구조 자체를 직접 예측하고, 대응점은 3D 공간에서 자연스럽게 얻어지는 부산물**로 다룬다.

**MASt3R (Leroy et al., 2024)**: [MASt3R](https://arxiv.org/abs/2406.09756)는 DUSt3R에 dense local feature head를 추가하여 밀집 매칭을 강화했다. Map-free localization 벤치마크에서 기존 최고 방법 대비 30%p(절대) VCRE AUC 향상을 달성했다.

**VGGT (Wang et al., 2025)**: [VGGT](https://arxiv.org/abs/2503.11651) (Visual Geometry Grounded Transformer)는 CVPR 2025 Best Paper로, 하나 이상의 이미지로부터 카메라 파라미터, pointmap, depth map, 3D point track을 **단일 feed-forward 패스로 동시에 추론**한다. 1초 이내의 추론 시간으로, 후처리 최적화가 필요한 기존 방법들을 능가하는 정확도를 보인다.

이 흐름은 "매칭은 2D 문제"라는 오랜 가정을 해체하고, **3D 기하학을 직접 추론하는 것이 결국 더 강건한 매칭을 낳는다**는 새로운 패러다임을 형성하고 있다.

---

## 5.8 3D-3D Correspondence

2D 이미지 매칭의 기술 계보와 병렬로, **3D 점군(point cloud) 사이의 대응점을 찾는 연구**도 진화해왔다. LiDAR scan-to-scan 정합, multi-session map merging, loop closure에서 핵심적이다.

### 5.8.1 FPFH (Fast Point Feature Histograms)

[FPFH (Rusu et al., 2009)](https://ieeexplore.ieee.org/document/5152473)는 3D 점군의 **기하학적 특징을 히스토그램으로 인코딩**하는 수작업 디스크립터다.

#### SPFH (Simple Point Feature Histogram)

쿼리 점 $\mathbf{p}$와 그 이웃 점 $\mathbf{p}_k$ 사이에서 로컬 좌표계를 설정하고, 세 가지 각도 특성을 계산한다:

법선 벡터 $\mathbf{n}_p, \mathbf{n}_k$와 방향 벡터 $\mathbf{d} = \mathbf{p}_k - \mathbf{p}$로부터:

$$\mathbf{u} = \mathbf{n}_p$$
$$\mathbf{v} = \mathbf{u} \times \frac{\mathbf{d}}{\|\mathbf{d}\|}$$
$$\mathbf{w} = \mathbf{u} \times \mathbf{v}$$

세 가지 각도 특성:
$$\alpha = \mathbf{v} \cdot \mathbf{n}_k, \quad \phi = \mathbf{u} \cdot \frac{\mathbf{d}}{\|\mathbf{d}\|}, \quad \theta = \text{atan2}(\mathbf{w} \cdot \mathbf{n}_k, \mathbf{u} \cdot \mathbf{n}_k)$$

각 특성을 $B$-bin 히스토그램으로 양자화.

#### FPFH: SPFH의 가속 버전

SPFH는 반경 $r$ 내의 모든 이웃 쌍의 특성을 계산하므로 $O(k^2)$. FPFH는 이를 근사하여 $O(k)$로 줄인다:

$$\text{FPFH}(\mathbf{p}) = \text{SPFH}(\mathbf{p}) + \frac{1}{k} \sum_{i=1}^{k} \frac{1}{w_i} \text{SPFH}(\mathbf{p}_i)$$

여기서 $w_i = \|\mathbf{p}_i - \mathbf{p}\|$. 33차원 히스토그램 (각 각도 11-bin × 3).

```python
import open3d as o3d

# FPFH 계산
pcd = o3d.io.read_point_cloud("scan.ply")
pcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))

fpfh = o3d.pipelines.registration.compute_fpfh_feature(
    pcd,
    o3d.geometry.KDTreeSearchParamHybrid(radius=0.25, max_nn=100)
)
# fpfh.data.shape: (33, N)
```

### 5.8.2 3DMatch (2017): Learned 3D Descriptors의 시작

[Zeng et al. (2017)](https://arxiv.org/abs/1603.08182)의 3DMatch는 RGB-D 데이터에서 **학습을 통해 3D 매칭 디스크립터를 추출**하는 최초의 시스템이다.

- **학습 데이터**: 62개 실내 장면의 RGB-D 재구성에서 자동 생성한 대응점 쌍
- **아키텍처**: 3D TDF(Truncated Distance Function) 볼륨을 입력으로 하는 3D CNN
- **출력**: 512차원 로컬 디스크립터

3DMatch는 학습 기반 3D 디스크립터의 출발점이며, 동시에 **3DMatch Benchmark**라는 표준 벤치마크를 제공하여 후속 연구의 평가 기준이 되었다.

### 5.8.3 FCGF (Fully Convolutional Geometric Features, 2019)

[Choy et al. (2019)](https://arxiv.org/abs/1904.09793)의 FCGF는 **sparse convolution**을 이용하여 전체 점군에서 한 번의 포워드 패스로 모든 점의 디스크립터를 추출한다.

3DMatch가 각 키포인트 주변의 로컬 볼륨을 개별적으로 처리하는 반면, FCGF는 전체 점군을 한꺼번에 처리하므로 **수십~수백 배 빠르다**. 32차원 디스크립터로 3DMatch의 512차원보다 간결하면서도 더 높은 정확도를 달성했다.

### 5.8.4 Predator (2021): Overlap-Aware 3D Matching

[Huang et al. (2021)](https://arxiv.org/abs/2011.13005)의 Predator는 두 점군의 **오버랩 영역을 명시적으로 예측**하는 접근법이다.

기존 방법들은 두 점군의 모든 영역에서 동일하게 디스크립터를 추출하지만, 실제로는 일부 영역만 겹친다. Predator는:

1. **Overlap attention**: cross-attention을 통해 각 점이 상대 점군과 겹치는 정도를 예측
2. **Matchability score**: 오버랩 영역 내에서 매칭에 유용한 점(독특한 기하학적 구조)을 별도로 스코어링
3. 오버랩 비율이 낮은(10-30%) 어려운 시나리오에서 큰 성능 향상

### 5.8.5 GeoTransformer (2022): 기하학적 트랜스포머

[Qin et al. (2022)](https://arxiv.org/abs/2202.06688)의 GeoTransformer는 3D point cloud registration에서 **기하학적 불변 특징 학습과 RANSAC 제거**를 동시에 달성한다.

#### 키포인트-프리 슈퍼포인트 매칭

반복 가능한 키포인트 검출 대신, 다운샘플링된 **슈퍼포인트(superpoint)**에서 대응점을 찾고 이를 밀집 포인트로 전파(propagation)한다.

#### 기하학적 트랜스포머 아키텍처

핵심은 **rigid transformation에 불변한 기하학적 특징**을 학습하는 것이다. 두 가지 기하학적 인코딩을 사용한다:

**1. 쌍별 거리 인코딩(Pairwise Distance)**:

슈퍼포인트 $\mathbf{p}_i, \mathbf{p}_j$ 사이의 유클리드 거리 $d_{ij} = \|\mathbf{p}_i - \mathbf{p}_j\|$는 강체 변환에 불변이다. 이를 positional encoding으로 사용:

$$\text{PE}(d_{ij}) = [\sin(\omega_1 d_{ij}), \cos(\omega_1 d_{ij}), \ldots, \sin(\omega_K d_{ij}), \cos(\omega_K d_{ij})]$$

**2. 삼중 각도 인코딩(Triplet Angle)**:

세 점 $\mathbf{p}_i, \mathbf{p}_j, \mathbf{p}_k$가 이루는 각도도 강체 변환에 불변이다. 이 각도 정보를 추가 인코딩으로 사용하여 기하학적 구조를 더 풍부하게 포착한다.

이 기하학적 인코딩은 트랜스포머의 attention bias로 주입된다:

$$\text{Attn}_{ij} = \frac{\mathbf{q}_i^\top \mathbf{k}_j}{\sqrt{d}} + b(\text{PE}(d_{ij}))$$

#### RANSAC-Free 변환 추정

슈퍼포인트 수준의 강건한 대응점이 높은 인라이어 비율을 달성하므로, **RANSAC 없이 직접 변환을 추정**할 수 있다. 이로 인해 **100배의 속도 향상**을 달성한다.

#### 성능

3DLoMatch 벤치마크에서 인라이어 비율 17-30%p 향상, 정합 리콜 7%p 이상 향상. 특히 **저오버랩(10-30%)** 시나리오에서 기존 방법 대비 큰 폭의 개선.

```python
# GeoTransformer 사용 예제
from geotransformer.utils.pointcloud import apply_transform
import torch

# 두 점군 로드 (N, 3)
src_points = torch.from_numpy(src_pcd.points).float().cuda()
ref_points = torch.from_numpy(ref_pcd.points).float().cuda()

# 다운샘플링 + 슈퍼포인트 추출
# ... (voxel downsampling)

# GeoTransformer 추론
output = model(src_points, ref_points, src_feats, ref_feats)
# output['estimated_transform']: (4, 4) 변환 행렬
# output['src_corr_points'], output['ref_corr_points']: 대응점
```

### 5.8.6 3D-3D Correspondence 기술 계보

```
[Handcrafted 3D Descriptors]
FPFH (2009)          — 기하학적 각도 히스토그램
SHOT (2010)          — 방향 히스토그램
    ↓ 학습 기반
[Learned 3D Descriptors]
3DMatch (2017)       — 최초의 학습 기반 3D 디스크립터 + 벤치마크
    ↓ 효율화
FCGF (2019)          — sparse conv, 전체 점군 한 번에 처리
    ↓ 오버랩 인식
Predator (2021)      — overlap-aware cross-attention
    ↓ RANSAC 제거
GeoTransformer (2022) — 기하학적 불변 트랜스포머, RANSAC-free
```

---

## 5.9 Cross-Modal Correspondence

### 5.9.1 2D-3D Matching: 카메라-LiDAR 간

가장 빈번한 cross-modal correspondence 문제는 **2D 이미지와 3D 점군 사이의 매칭**이다. 응용 시나리오:

- **카메라-LiDAR 외부 캘리브레이션**: 두 센서가 관측한 같은 물리적 점을 찾아 외부 파라미터 추정
- **Visual Localization against LiDAR Map**: 카메라 이미지를 LiDAR 맵에 대해 위치 추정
- **Loop Closure**: 카메라와 LiDAR 관측을 교차 검증

### 5.9.2 Image-to-Point Cloud 매칭 접근법

#### 투영 기반 접근 (Projection-based)

가장 직접적인 방법은 3D 점군을 2D 이미지 평면에 투영하여 2D-2D 매칭 문제로 환원하는 것이다:

1. LiDAR 점군을 가상 카메라 뷰로 렌더링하여 depth/intensity 이미지 생성
2. 생성된 2D 이미지와 카메라 이미지 사이에서 2D 매칭 (SuperGlue 등) 수행
3. 2D 매칭 결과를 3D 좌표로 역투영

Koide et al. (2023)의 캘리브레이션 툴이 이 접근법을 사용한다: LiDAR 밀집 점군을 가상 카메라로 렌더링하고, SuperGlue로 카메라 이미지와 교차 모달 2D-3D 대응점을 검출한다.

#### 학습 기반 직접 매칭

LCD (LiDAR-Camera Descriptor): 2D 이미지 패치와 3D 점군 패치의 공통 임베딩 공간을 학습한다.

P2-Net (Yu et al., 2021): patch-to-point 매칭을 학습하여 2D 이미지 패치와 3D 점의 직접 대응을 추론한다.

### 5.9.3 왜 Cross-Modal이 어려운가: Representation Gap

2D-3D cross-modal 매칭이 근본적으로 어려운 이유:

1. **표현의 이질성(Representation Heterogeneity)**: 2D 이미지는 정규 격자 위의 밝기/색상, 3D 점군은 불규칙하게 분포된 좌표+강도. 데이터 구조 자체가 다르다.

2. **정보 비대칭(Information Asymmetry)**: 이미지는 풍부한 텍스처 정보를 제공하지만 깊이가 없고, 점군은 정확한 기하학 정보를 제공하지만 텍스처가 희소하다.

3. **밀도 차이(Density Gap)**: 카메라 이미지의 해상도(수백만 픽셀)와 LiDAR 점군의 밀도(수만~수십만 점)가 크게 다르며, LiDAR 점군은 거리에 따라 밀도가 급격히 변한다.

4. **외관 도메인 갭(Appearance Domain Gap)**: 같은 물체라도 카메라의 반사율(albedo)과 LiDAR의 반사 강도(intensity)는 다른 물리적 양을 측정한다.

이러한 어려움 때문에, cross-modal correspondence는 아직 unimodal (2D-2D 또는 3D-3D) 매칭에 비해 성숙도가 낮은 연구 영역이다. MI 기반 접근법 (5.4절)은 이 도메인 갭을 통계적으로 우회하는 전략이며, 투영 기반 접근법은 문제를 같은 모달리티로 환원하는 전략이다.

---

## 5.10 Dense Matching & Optical Flow

지금까지 다룬 방법들은 **sparse correspondence** (희소 대응점)에 집중했다. 이 절에서는 이미지의 **모든 픽셀에 대해 대응을 찾는 dense matching**을 다룬다.

### 5.10.1 Classical Optical Flow: Lucas-Kanade, Horn-Schunck

#### Optical Flow의 정의

Optical flow는 이미지 시퀀스에서 **각 픽셀의 겉보기 운동(apparent motion)**을 2D 벡터 필드로 표현한 것이다:

$$\mathbf{u}(x, y) = (u(x, y), v(x, y))$$

**밝기 항상성 가정(brightness constancy assumption)**:

$$I(x, y, t) = I(x + u, y + v, t + 1)$$

Taylor 1차 전개로 **optical flow constraint equation**을 얻는다:

$$I_x u + I_y v + I_t = 0$$

여기서 $I_x = \frac{\partial I}{\partial x}$, $I_y = \frac{\partial I}{\partial y}$, $I_t = \frac{\partial I}{\partial t}$.

이 하나의 방정식에 두 개의 미지수 $(u, v)$가 있으므로, 추가 제약이 필요하다 (aperture problem).

#### Lucas-Kanade (1981)

**로컬 일관성 가정**: 작은 윈도우 $\Omega$ 내에서 optical flow가 일정하다고 가정한다. 윈도우 내의 모든 픽셀에 대해:

$$\begin{bmatrix} I_{x_1} & I_{y_1} \\ I_{x_2} & I_{y_2} \\ \vdots & \vdots \\ I_{x_n} & I_{y_n} \end{bmatrix} \begin{bmatrix} u \\ v \end{bmatrix} = -\begin{bmatrix} I_{t_1} \\ I_{t_2} \\ \vdots \\ I_{t_n} \end{bmatrix}$$

즉 $\mathbf{A} \mathbf{u} = -\mathbf{b}$. Overdetermined system이므로 최소자승법으로:

$$\begin{bmatrix} u \\ v \end{bmatrix} = (\mathbf{A}^\top \mathbf{A})^{-1} \mathbf{A}^\top (-\mathbf{b})$$

여기서 $\mathbf{A}^\top \mathbf{A} = \begin{bmatrix} \sum I_x^2 & \sum I_x I_y \\ \sum I_x I_y & \sum I_y^2 \end{bmatrix}$는 Harris corner detector의 structure tensor $\mathbf{M}$과 동일하다.

**한계**: 큰 변위(large displacement)에서는 로컬 선형 근사가 실패. 이를 해결하기 위해 **이미지 피라미드**에서 coarse-to-fine으로 적용한다 (Pyramidal LK).

```python
# Pyramidal Lucas-Kanade (OpenCV)
lk_params = dict(
    winSize=(21, 21),
    maxLevel=3,       # 피라미드 레벨
    criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 30, 0.01)
)

# 추적할 점 선택 (보통 FAST/Shi-Tomasi corners)
p0 = cv2.goodFeaturesToTrack(prev_gray, maxCorners=200, qualityLevel=0.3, minDistance=7)

# Lucas-Kanade 추적
p1, status, err = cv2.calcOpticalFlowPyrLK(prev_gray, next_gray, p0, None, **lk_params)
# status: 추적 성공 여부 (1/0)
```

#### Horn-Schunck (1981)

**글로벌 평활성 가정**: optical flow가 이미지 전체에서 부드럽게 변한다고 가정하고, 다음 에너지 함수를 최소화:

$$E = \iint \left[ (I_x u + I_y v + I_t)^2 + \lambda^2 (\|\nabla u\|^2 + \|\nabla v\|^2) \right] dx \, dy$$

첫 번째 항은 데이터 항(brightness constancy), 두 번째 항은 평활성 정규화. $\lambda$가 클수록 더 부드러운 flow.

Euler-Lagrange 방정식을 풀면 반복적 갱신 수식을 얻는다. Dense flow를 생성하지만, 경계에서의 불연속을 잘 처리하지 못한다는 한계가 있다.

### 5.10.2 Learning-Based Optical Flow: FlowNet → RAFT → FlowFormer → UniMatch

#### FlowNet / FlowNet 2.0 (2015/2017)

[Dosovitskiy et al. (2015)](https://arxiv.org/abs/1504.06852)의 FlowNet은 **optical flow를 CNN으로 직접 예측**한 최초의 딥러닝 방법이다. 두 이미지를 입력으로 받아 flow 필드를 출력하는 encoder-decoder 구조.

[FlowNet 2.0 (Ilg et al., 2017)](https://arxiv.org/abs/1612.01925)은 여러 FlowNet을 스택하여 정확도를 끌어올렸다.

#### RAFT (2020): Optical Flow의 새로운 표준

[Teed & Deng (2020)](https://arxiv.org/abs/2003.12039)의 RAFT는 **4D correlation volume + 반복적 GRU 업데이트**라는 새로운 아키텍처 패러다임을 제시하여, ECCV 2020 Best Paper를 수상했다.

**아키텍처 3단계**:

**1. Feature Encoder**: 입력 두 이미지 각각에 CNN(ResNet 변형)을 적용하여 1/8 해상도의 특징 맵 $\mathbf{g}_1, \mathbf{g}_2 \in \mathbb{R}^{H/8 \times W/8 \times D}$를 추출. 별도의 Context Encoder가 첫 번째 이미지에서 GRU의 초기 hidden state와 context feature를 추출.

**2. Correlation Volume 구성**: 두 특징 맵의 모든 픽셀 쌍에 대해 내적을 계산:

$$C_{ijkl} = \sum_d g_1(i, j, d) \cdot g_2(k, l, d)$$

4D correlation volume $\mathbf{C} \in \mathbb{R}^{H \times W \times H \times W}$ 생성. 이를 후반 두 차원에 대해 average pooling하여 4단계 **correlation pyramid**을 구축 (스케일 1, 2, 4, 8).

**coarse-to-fine이 아닌, single resolution에서 multi-scale lookup**을 수행한다는 점이 이전 방법과의 차이다.

**3. Iterative Update (GRU)**: ConvGRU가 현재 flow 추정치를 반복적으로 정제한다:

각 반복 $k$에서:
1. 현재 flow 추정 $\mathbf{f}^k$에 따라 correlation pyramid에서 값을 lookup (현재 대응 위치 주변의 local window 참조)
2. Correlation feature, 현재 flow, context feature를 결합
3. ConvGRU가 hidden state $\mathbf{h}^k$를 업데이트
4. Hidden state에서 flow 잔차(residual) $\Delta \mathbf{f}$ 예측
5. Flow 업데이트: $\mathbf{f}^{k+1} = \mathbf{f}^k + \Delta \mathbf{f}$

학습 시 12회, 추론 시 12~32회 반복. **반복 횟수를 늘리면 정확도가 향상**되는 특성(test-time adaptability).

**All-Pairs vs. Coarse-to-Fine**: 기존 PWC-Net, FlowNet 등은 피라미드에서 coarse flow를 먼저 추정하고 점진적으로 정밀화하는데, 큰 변위가 coarse 레벨에서 놓치면 복구 불가. RAFT는 **전체 해상도에서 모든 상관관계를 한 번에 보유**하므로 큰 변위도 놓치지 않는다.

**학습**: 모든 반복 단계의 예측에 대해 ground-truth flow와의 L1 loss를 적용하되, 후반 반복에 exponentially increasing weights:

$$L = \sum_{k=1}^{K} \gamma^{K-k} \| \mathbf{f}^k - \mathbf{f}^{gt} \|_1$$

여기서 $\gamma = 0.8$.

```python
# RAFT 사용 예제 (torchvision)
from torchvision.models.optical_flow import raft_large, Raft_Large_Weights

model = raft_large(weights=Raft_Large_Weights.DEFAULT)
model = model.eval().cuda()

with torch.no_grad():
    # img1, img2: (1, 3, H, W) float tensors, range [-1, 1]
    flow_predictions = model(img1, img2)
    # flow_predictions[-1]: 최종 flow (1, 2, H, W)
    flow = flow_predictions[-1]  # (u, v) per pixel
```

#### FlowFormer (2022)

[FlowFormer (Huang et al., 2022)](https://arxiv.org/abs/2203.16194)는 RAFT의 GRU 업데이트를 **트랜스포머로 대체**한 방법이다. Cost volume을 토큰화하고, 트랜스포머의 자기주의(self-attention)로 글로벌 컨텍스트를 포착하여 RAFT를 상회하는 정확도를 달성했다.

#### UniMatch (2023)

[Xu et al. (2023)](https://arxiv.org/abs/2211.05783)의 UniMatch는 **optical flow, stereo matching, depth estimation을 하나의 통합 프레임워크**로 처리한다. 핵심 아이디어는 세 태스크가 모두 "두 관측 사이의 dense correspondence"라는 공통 문제로 귀결된다는 것이다.

### 5.10.3 Dense Stereo: SGM → RAFT-Stereo → UniMatch

Stereo matching은 좌우 카메라 이미지에서 **수평 방향의 시차(disparity)**를 추정하는 문제다. Optical flow의 특수한 경우 (수직 방향 flow = 0)로 볼 수 있다.

#### SGM (Semi-Global Matching, Hirschmuller 2005)

전통적 dense stereo의 대표. 핵심 아이디어:
- 각 픽셀에서 모든 시차 후보에 대한 매칭 비용(cost) 계산
- 8(또는 16) 방향에서 1D 동적 프로그래밍으로 비용을 집계(aggregation)
- 집계된 비용에서 최소값을 선택하여 시차 결정

$$L_r(p, d) = C(p, d) + \min\begin{cases} L_r(p-r, d) \\ L_r(p-r, d-1) + P_1 \\ L_r(p-r, d+1) + P_1 \\ \min_i L_r(p-r, i) + P_2 \end{cases}$$

여기서 $P_1, P_2$는 평활성 페널티.

#### RAFT-Stereo (2021)

RAFT의 아키텍처를 stereo matching에 적용. 4D correlation volume을 3D (H×W×D)로 축소하고, 반복적 GRU로 시차를 정제한다.

#### UniMatch의 통합

UniMatch는 flow, stereo, depth를 통합하여, 하나의 모델이 태스크에 따라 cross-attention의 방향과 범위를 조절한다:

- **Stereo**: 수평 방향 1D correlation
- **Flow**: 2D all-pairs correlation
- **Depth**: monocular 특징에서 depth regression

### 5.10.4 기술 흐름: Sparse Feature → Dense Correspondence

```
[Sparse Correspondence]
Harris → SIFT → SuperPoint → SuperGlue → LightGlue
    ↓ 밀집화
[Dense Correspondence]
Lucas-Kanade (1981)  — 로컬 window, sparse-to-dense
Horn-Schunck (1981)  — 글로벌 최적화, 전 픽셀 flow
    ↓ 딥러닝
FlowNet (2015)       — CNN 직접 예측
[PWC-Net](https://arxiv.org/abs/1709.02371) (2018)       — cost volume + coarse-to-fine
    ↓ 패러다임 전환
RAFT (2020)          — all-pairs correlation + iterative GRU
    ↓ 트랜스포머
FlowFormer (2022)    — cost volume 토큰화 + transformer
    ↓ 태스크 통합
UniMatch (2023)      — flow + stereo + depth 통합
```

RAFT의 all-pairs correlation과 iterative refinement 아이디어는 dense matching뿐 아니라 **detector-free feature matching** (LoFTR, RoMa)과 **learned SLAM** (DROID-SLAM)에도 직접적으로 영향을 미쳤다.

---

## 기술 계보 요약

이 챕터에서 다룬 모든 기술의 흐름을 하나의 다이어그램으로 정리한다:

```
═══════════════════════════════════════════════════════════════════════════════
                    FEATURE MATCHING & CORRESPONDENCE 기술 계보
═══════════════════════════════════════════════════════════════════════════════

[2D Detection & Description]
                                                                              
Harris (1988) ─────→ SIFT (2004) ───→ FAST (2006) ─→ ORB (2011)             
  코너 검출           scale-space       극한 속도       FAST+BRIEF             
  structure tensor    DoG + 128D        검출만          binary descriptor       
                      float descriptor                                        
         │                │                                                   
         │   정확도 vs 속도 트레이드오프의 역사                               
         ▼                ▼                                                   
[학습 기반 Detection & Description]                                           
                                                                              
    SuperPoint (2018) ──→ D2-Net (2019) ──→ R2D2 (2019) ──→ DISK (2020)      
      self-supervised      detect=describe    reliability      RL 기반         
      homographic adapt    joint feature map  + repeatability                  
         │                                                                    
         │                                                                    
         ▼                                                                    
[학습 기반 Matching — 파이프라인 유지]                                        
                                                                              
    SuperGlue (2020) ──────────────→ LightGlue (2023)                        
      GNN + cross-attention           adaptive depth/width                    
      Sinkhorn optimal transport      dual-softmax (Sinkhorn 제거)            
      O(N²) × 9 layers               조기 종료, 3-5× 빠름                    
                                                                              
═══════════════════════ 패러다임 전환 ═══════════════════════════════════       
                                                                              
[Detector-Free Matching — 파이프라인 해체]                                    
                                                                              
    LoFTR (2021) ──→ QuadTree (2022) ──→ ASpanFormer (2022) ──→ RoMa (2024) 
      transformer         O(N log N)        adaptive span          DINOv2     
      coarse-to-fine       효율화           텍스처 적응             확률적 매칭 
      검출기 완전 제거                                              foundation 
                                                                    model 활용
                                                                        │
═══════════════════════ 3D-Aware 전환 ═══════════════════════════════════
                                                                        │
[3D-Aware Dense Matching — 2D 매칭을 넘어]                              ▼
                                                                              
    DUSt3R (2024) ──→ MASt3R (2024) ──→ VGGT (2025, CVPR Best Paper)  
      3D pointmap         dense local        feed-forward 3D 추론      
      직접 회귀            feature 추가       camera+depth+pointmap     
      매칭 = 3D 부산물     매칭 성능 강화     단일 패스 통합 추론
═══════════════════════════════════════════════════════════════════════════════

[Dense Matching & Optical Flow — 병렬 진화]                                   
                                                                              
    LK (1981) ──→ Horn-Schunck (1981) ──→ FlowNet (2015)                     
      로컬 window    글로벌 smoothness      CNN 직접 예측                      
         │                                      │                             
         ▼                                      ▼                             
    PWC-Net (2018) ──→ RAFT (2020) ──→ FlowFormer (2022) ──→ UniMatch (2023)
      cost volume       4D all-pairs      transformer           flow+stereo   
      coarse-to-fine    iterative GRU     cost tokenization     +depth 통합   
                            │                                                 
                            │ RAFT의 아이디어 전파                             
                            ├──→ LoFTR (all-pairs attention)                  
                            ├──→ RoMa (iterative refinement)                  
                            └──→ DROID-SLAM (correlation + DBA)               
                                                                              
═══════════════════════════════════════════════════════════════════════════════

[3D-3D Correspondence — 독립 진화]                                            
                                                                              
    FPFH (2009) ──→ SHOT (2010) ──→ 3DMatch (2017) ──→ FCGF (2019)          
      기하학 히스토그램   방향 히스토그램    학습 3D descriptor   sparse conv    
                                                │                             
                                                ▼                             
                                         Predator (2021)                      
                                           overlap-aware                      
                                                │                             
                                                ▼                             
                                        GeoTransformer (2022)                 
                                           기하학적 transformer                
                                           RANSAC-free                        
                                                                              
═══════════════════════════════════════════════════════════════════════════════

[Cross-Modal — MI 기반 우회 전략]                                             
                                                                              
    MI (정보 이론) ──→ NMI ──→ NID (Koide et al. 2023)                       
      다중 모달리티      정규화     LiDAR-Camera calibration                    
      통계적 의존성                                                            
                                                                              
═══════════════════════════════════════════════════════════════════════════════

핵심 서사:
  1. 전통적 3단계 파이프라인(detect → describe → match)을 딥러닝으로
     각각 대체하는 흐름: SIFT → SuperPoint → SuperGlue → LightGlue

2. 파이프라인 자체를 해체하고 end-to-end 밀집 매칭으로 전환하는 흐름:
     RAFT → LoFTR → RoMa

3. Foundation model 활용의 부상: DINOv2 특징을 매칭의 기반으로 활용하여,
     "특징 학습은 범용 모델에 맡기고 매칭 로직만 학습"하는 패러다임 전환.

4. 3D-Aware 매칭의 등장 (2024-2025): DUSt3R → MASt3R → VGGT로 이어지는
     흐름에서, "2D 매칭 후 3D 복원"이 아닌 "3D를 직접 추론하면 매칭은
     자연스럽게 따라온다"는 역발상이 큰 성공을 거두고 있다.

후자(2, 3, 4)가 점점 우세해지는 추세이나, 전자(1)의 효율성과 해석 가능성은
  여전히 실용적 가치가 있다.
```

이 챕터에서 다룬 매칭 기술들은 다음 챕터부터 본격적으로 활용된다. Ch.6에서는 이 기술들이 Visual Odometry의 frontend에서 어떻게 배치되는지, 그리고 Ch.4에서 다룬 상태 추정 방법이 backend에서 어떻게 결합되는지를 구체적인 시스템들을 통해 살펴본다.

---

# Ch.6 — Visual Odometry & Visual-Inertial Odometry

Ch.4에서 상태 추정 프레임워크를, Ch.5에서 특징점 매칭 기술을 다루었다. 이제 이 둘이 실제로 결합되는 첫 시스템, Visual Odometry(VO)와 Visual-Inertial Odometry(VIO)를 살펴본다.

VO는 카메라 영상만으로 카메라의 자기 운동(ego-motion)을 추정하고, VIO는 여기에 IMU를 결합해 스케일 관측 가능성과 강건성을 확보한다. 이 챕터에서는 VO·VIO의 내부 구조와 설계 선택을 파고든다.

VO의 기원은 [Nistér et al. (2004)](https://doi.org/10.1109/CVPR.2004.1315094)로 거슬러 올라간다. 이 논문은 "Visual Odometry"라는 용어를 처음 정의하고, 스테레오·단안 카메라로 실시간 자기 운동 추정 시스템을 제시했다. 스테레오 접근에서는 좌우 카메라에서 삼각측량한 3D 점을 3-point 알고리즘으로 프레임 간 강체 변환을 추정했고, 단안 접근에서는 5-point 알고리즘으로 Essential Matrix를 추정했다. 특징점 검출, 매칭, RANSAC, 모션 추정으로 이어지는 이 파이프라인은 20년이 지난 지금도 feature-based VO의 뼈대다.

VO·VIO 시스템은 세 축으로 분류된다.

1. **Feature-based vs Direct**: 기하학적 특징점(corner, edge)을 추출하여 매칭하는가, 아니면 픽셀 밝기 자체를 직접 사용하는가
2. **Filter vs Optimization**: 상태 추정에 칼만 필터 계열을 쓰는가, 비선형 최적화를 쓰는가
3. **Loosely coupled vs Tightly coupled**: IMU와 카메라를 독립적으로 처리한 뒤 결과를 합치는가, raw measurement를 하나의 최적화 문제에 넣는가

이 축들의 조합이 다양한 시스템을 낳았다. 챕터는 대표 시스템을 하나씩 해부하면서 각 설계 선택의 이유와 결과를 분석한다.

---

## 6.1 Feature-based Visual Odometry

Feature-based VO는 영상에서 기하학적 특징점을 추출하고, 프레임 간 대응 관계로 카메라 모션을 추정한다. 가장 오래된 VO 패러다임이며, ORB-SLAM3를 통해 지금도 현역으로 가동된다.

### 6.1.1 Frontend: Detection, Tracking, Outlier Rejection

Feature-based VO의 프론트엔드는 세 가지 핵심 작업을 수행한다.

**특징점 검출 (Feature Detection)**

프레임에서 추적 가능한 점을 찾는 단계다. 이상적인 특징점은 반복 가능성(repeatability)이 높아야 한다 — 같은 3D 점이 다른 시점에서 촬영되어도 비슷한 위치에서 검출되어야 한다.

Harris corner detector는 이미지 패치의 자기상관 행렬(autocorrelation matrix, 또는 second moment matrix) $\mathbf{M}$을 기반으로 코너를 검출한다:

$$\mathbf{M} = \sum_{(x,y) \in W} w(x,y) \begin{bmatrix} I_x^2 & I_xI_y \\ I_xI_y & I_y^2 \end{bmatrix}$$

여기서 $I_x, I_y$는 이미지 그래디언트, $W$는 윈도우, $w$는 가중치 함수다. $\mathbf{M}$의 두 고유값이 모두 크면 코너로 판정한다. Harris 응답 함수는:

$$R = \det(\mathbf{M}) - k \cdot \text{tr}(\mathbf{M})^2 = \lambda_1\lambda_2 - k(\lambda_1 + \lambda_2)^2$$

FAST (Features from Accelerated Segment Test)는 속도에 최적화된 검출기다. 후보 픽셀 $p$ 주위 반지름 3의 원(Bresenham circle) 위 16개 점 중 $n$개(보통 $n=9$) 이상이 연속으로 $p$보다 밝거나 어두우면 코너로 판정한다. 사전 테스트로 1, 5, 9, 13번 점만 먼저 검사하여 비코너를 빠르게 제거한다. Harris 대비 수십 배 빠르지만, 노이즈에 약하고 방향/스케일 불변성이 없다.

ORB (Oriented FAST and Rotated BRIEF)는 FAST 검출에 방향 정보를 추가하고 BRIEF 디스크립터를 회전 보정하여 실시간 SLAM에 적합한 특징점을 제공한다. 방향은 이미지 패치의 intensity centroid로 계산한다:

$$\theta = \text{atan2}(m_{01}, m_{10}), \quad m_{pq} = \sum_{x,y} x^p y^q I(x,y)$$

ORB-SLAM 시리즈는 스케일 불변성을 위해 이미지 피라미드(보통 8레벨, 스케일 팩터 1.2)에서 ORB를 추출한다.

**특징점 추적 (Feature Tracking)**

프레임 간 대응을 찾는 방법은 두 가지다:

1. **디스크립터 매칭**: 각 프레임에서 독립적으로 특징점을 검출하고, 디스크립터 간 거리(Hamming distance for binary descriptors, L2 for float descriptors)로 매칭한다. ORB-SLAM3는 이 접근을 사용한다. DBoW2 어휘 트리를 이용해 매칭 후보를 제한하여 속도를 높인다.

2. **광학 흐름 추적 (Optical Flow Tracking)**: Lucas-Kanade (LK) 추적기로 이전 프레임의 특징점 위치가 현재 프레임에서 어디로 이동했는지 추적한다. VINS-Mono는 이 접근을 사용한다. 밝기 항상성 가정(brightness constancy assumption) 하에:

$$I(x + u, y + v, t + \Delta t) = I(x, y, t)$$

1차 Taylor 전개 후 윈도우 $W$ 내에서 최소제곱:

$$\begin{bmatrix} u \\ v \end{bmatrix} = \left(\sum_W \begin{bmatrix} I_x^2 & I_xI_y \\ I_xI_y & I_y^2 \end{bmatrix}\right)^{-1} \sum_W \begin{bmatrix} -I_xI_t \\ -I_yI_t \end{bmatrix}$$

이미지 피라미드 위에서 coarse-to-fine으로 수행하면 큰 변위도 추적 가능하다.

두 접근의 트레이드오프: 디스크립터 매칭은 넓은 baseline(시점 변화가 큰 경우)에서 강하고 루프 클로저에 재사용 가능하지만, 검출+기술+매칭에 시간이 든다. LK 추적은 빠르고 서브픽셀 정밀도를 제공하지만, 큰 시점 변화에 실패하기 쉽다.

**아웃라이어 제거 (Outlier Rejection)**

매칭 결과에는 반드시 오매칭(outlier)이 섞인다. 이를 제거하지 않으면 모션 추정이 크게 틀어진다.

기본적인 접근은 RANSAC (Random Sample Consensus)이다. 최소 샘플로 모델을 추정하고, 전체 데이터에 대한 인라이어 수를 세어 최적 모델을 선택한다. VO에서는:

- **2D-2D**: 5-point 알고리즘으로 Essential Matrix $\mathbf{E}$를 추정. $\mathbf{E} = [\mathbf{t}]_\times \mathbf{R}$로 분해하여 상대 포즈 $(R, t)$ 획득.
- **3D-2D**: PnP (Perspective-n-Point) 알고리즘 + RANSAC. 이미 삼각측량된 3D 점과 현재 2D 관측의 대응으로 절대 포즈를 추정.
- **3D-3D**: ICP 변종으로 강체 변환 추정.

Epipolar constraint를 이용한 아웃라이어 제거의 핵심 수식은:

$$\mathbf{p}_2^T \mathbf{E} \mathbf{p}_1 = 0$$

여기서 $\mathbf{p}_1, \mathbf{p}_2$는 정규화된 이미지 좌표다. 이 등식을 만족하지 않는 대응은 아웃라이어로 판정한다.

ORB-SLAM3는 Fundamental Matrix와 Homography를 동시에 추정하여, 장면 구조(평면 vs 비평면)에 따라 적절한 모델을 선택한다. 평면 장면에서는 Homography가 더 적은 자유도를 가지므로 안정적이다.

### 6.1.2 Backend: PnP, Motion-only BA, Local BA

프론트엔드가 대응 관계를 제공하면, 백엔드는 이를 기반으로 카메라 포즈와 3D 구조를 추정한다.

**PnP (Perspective-n-Point)**

이미 삼각측량된 3D 맵포인트 $\mathbf{P}_j \in \mathbb{R}^3$와 현재 프레임에서의 2D 관측 $\mathbf{u}_j \in \mathbb{R}^2$의 대응이 주어졌을 때, 카메라 포즈 $\mathbf{T} = (\mathbf{R}, \mathbf{t}) \in SE(3)$를 추정하는 문제다.

$$\mathbf{u}_j = \pi(\mathbf{R}\mathbf{P}_j + \mathbf{t})$$

여기서 $\pi: \mathbb{R}^3 \to \mathbb{R}^2$는 카메라 투영 함수다. 최소 해는 4점(P3P의 경우 3점 + 1점 검증)으로 얻으며, RANSAC과 결합하여 아웃라이어를 처리한다.

초기 해를 구한 뒤에는 모든 인라이어에 대해 비선형 최적화(refinement)를 수행한다.

**Motion-only Bundle Adjustment**

PnP로 초기 포즈를 얻은 뒤, 3D 점 위치는 고정한 채 카메라 포즈만 최적화하는 단계다:

$$\mathbf{T}^* = \underset{\mathbf{T}}{\arg\min} \sum_{j} \rho\left(\left\|\mathbf{u}_j - \pi(\mathbf{T} \cdot \mathbf{P}_j)\right\|^2_{\Sigma_j}\right)$$

여기서 $\rho(\cdot)$는 robust kernel (Huber 등), $\Sigma_j$는 관측 공분산이다. Huber 함수는:

$$\rho(s) = \begin{cases} s & \text{if } s \leq \delta^2 \\ 2\delta\sqrt{s} - \delta^2 & \text{if } s > \delta^2 \end{cases}$$

이 최적화는 6-DoF(SE(3) 위의 최적화)이므로, Lie algebra $\mathfrak{se}(3)$에서의 증분 $\boldsymbol{\xi} \in \mathbb{R}^6$을 통해 업데이트한다:

$$\mathbf{T} \leftarrow \exp(\boldsymbol{\xi}^{\wedge}) \cdot \mathbf{T}$$

**Local Bundle Adjustment**

최근 키프레임 $N$개와 이들이 관측하는 맵포인트를 함께 최적화한다:

$$\{\mathbf{T}_i^*, \mathbf{P}_j^*\} = \underset{\{\mathbf{T}_i, \mathbf{P}_j\}}{\arg\min} \sum_{i,j} \rho\left(\left\|\mathbf{u}_{ij} - \pi(\mathbf{T}_i \cdot \mathbf{P}_j)\right\|^2_{\Sigma_{ij}}\right)$$

이것이 Bundle Adjustment(BA)의 핵심 형태다. "Bundle"은 각 3D 점에서 카메라들로 향하는 광선(ray)의 다발을 의미한다. BA의 정규 방정식(normal equations)은 특수한 희소 구조(Schur complement structure)를 가진다:

$$\begin{bmatrix} \mathbf{H}_{cc} & \mathbf{H}_{cp} \\ \mathbf{H}_{pc} & \mathbf{H}_{pp} \end{bmatrix} \begin{bmatrix} \delta\boldsymbol{\xi} \\ \delta\mathbf{p} \end{bmatrix} = \begin{bmatrix} \mathbf{b}_c \\ \mathbf{b}_p \end{bmatrix}$$

$\mathbf{H}_{pp}$는 블록 대각(각 점은 독립)이므로, Schur complement로 점 변수를 소거하면:

$$(\mathbf{H}_{cc} - \mathbf{H}_{cp}\mathbf{H}_{pp}^{-1}\mathbf{H}_{pc})\delta\boldsymbol{\xi} = \mathbf{b}_c - \mathbf{H}_{cp}\mathbf{H}_{pp}^{-1}\mathbf{b}_p$$

이 축소된 시스템의 크기는 카메라 수에만 의존하므로(점 수와 무관), 효율적으로 풀 수 있다. 이것이 BA가 수만 개의 점을 다루면서도 실시간에 가까운 속도를 달성하는 핵심 비결이다.

### 6.1.3 ORB-SLAM3 아키텍처 상세 분석

[ORB-SLAM3 (Campos et al., 2021)](https://doi.org/10.1109/TRO.2021.3075644)는 feature-based visual(-inertial) SLAM의 현재 사실상 표준이다. 단일 프레임워크에서 monocular/stereo/RGB-D 카메라와 IMU를 모두 지원하며, pinhole과 fisheye 렌즈 모델을 수용한다.

**전체 아키텍처**

ORB-SLAM3는 세 개의 병렬 스레드로 구성된다:

1. **Tracking Thread**: 매 프레임에서 ORB 특징점을 추출하고, 기존 맵포인트와 매칭하여 현재 포즈를 추정한다. Motion-only BA로 포즈를 정제한다.
2. **Local Mapping Thread**: 새 키프레임이 삽입되면 새 맵포인트를 삼각측량하고, local BA를 수행한다. 중복 키프레임/맵포인트를 제거(culling)하여 맵을 컴팩트하게 유지한다.
3. **Loop Closing & Map Merging Thread**: DBoW2로 루프 후보를 검출하고, Sim(3) (또는 SE(3)) 정합으로 검증한다. 확인되면 pose graph optimization 후 full BA를 수행한다.

**Tracking Thread 상세**

1. 이전 프레임 기반 초기 포즈 예측: 등속 모델(constant velocity model) 또는 IMU preintegration으로 현재 포즈를 예측한다.
2. 예측 포즈를 이용해 맵포인트를 현재 프레임에 투영(projection), ORB 매칭으로 대응을 찾는다.
3. Motion-only BA로 포즈를 정제한다.
4. 키프레임 여부를 결정한다. 키프레임 조건: (a) 마지막 키프레임으로부터 일정 프레임 이상 경과, (b) 현재 프레임의 추적된 맵포인트 비율이 임계값 이하, (c) 현재 프레임에서 충분한 수의 맵포인트가 관측됨.

**Visual-Inertial 모드**

ORB-SLAM3의 visual-inertial 모드는 [Forster et al. (2017)](https://doi.org/10.1109/TRO.2016.2597321)의 IMU preintegration을 기반으로 한다. 핵심은 MAP(Maximum-a-Posteriori) 추정으로, 시각 잔차와 IMU 잔차를 하나의 비용 함수에서 동시 최적화한다:

$$\mathcal{C} = \sum_{i,j} \rho\left(\left\|\mathbf{e}_{ij}^{\text{vis}}\right\|^2_{\Sigma_{ij}}\right) + \sum_k \left\|\mathbf{e}_k^{\text{IMU}}\right\|^2_{\Sigma_k^{\text{IMU}}} + \left\|\mathbf{e}^{\text{prior}}\right\|^2_{\Sigma^{\text{prior}}}$$

IMU 잔차 $\mathbf{e}_k^{\text{IMU}}$는 키프레임 $k$와 $k+1$ 사이의 preintegrated IMU 측정과 상태 추정치의 차이로 정의된다. 상태 벡터는 각 키프레임마다:

$$\mathbf{x}_k = [{}^W\mathbf{R}_k, {}^W\mathbf{p}_k, {}^W\mathbf{v}_k, \mathbf{b}_k^g, \mathbf{b}_k^a]$$

을 포함한다. 이전 챕터(Ch.4)에서 유도한 preintegrated 측정:

$$\Delta\mathbf{R}_{k,k+1}, \quad \Delta\mathbf{v}_{k,k+1}, \quad \Delta\mathbf{p}_{k,k+1}$$

을 이용해 IMU 잔차를 다음과 같이 구성한다:

$$\mathbf{e}^{\text{IMU}}_{\Delta R} = \text{Log}\left(\Delta\hat{\mathbf{R}}_{k,k+1}^T \cdot \mathbf{R}_k^T \mathbf{R}_{k+1}\right)$$
$$\mathbf{e}^{\text{IMU}}_{\Delta v} = \mathbf{R}_k^T(\mathbf{v}_{k+1} - \mathbf{v}_k - \mathbf{g}\Delta t) - \Delta\hat{\mathbf{v}}_{k,k+1}$$
$$\mathbf{e}^{\text{IMU}}_{\Delta p} = \mathbf{R}_k^T(\mathbf{p}_{k+1} - \mathbf{p}_k - \mathbf{v}_k\Delta t - \frac{1}{2}\mathbf{g}\Delta t^2) - \Delta\hat{\mathbf{p}}_{k,k+1}$$

**Multi-Map System (Atlas)**

ORB-SLAM3의 가장 중요한 기여 중 하나는 Atlas 구조다. 시각 정보가 부족한 구간(빠른 회전, 가림 등)에서 트래킹이 실패하면, 기존 시스템은 재초기화 후 이전 맵과의 연결을 잃는다. ORB-SLAM3의 Atlas는:

1. 트래킹 실패 시 새로운 맵(sub-map)을 생성한다.
2. 각 sub-map은 독립적으로 유지된다.
3. 장소 인식(DBoW2)을 통해 이전에 방문한 sub-map을 감지하면, 두 맵을 자동으로 병합(merging)한다.
4. 병합 시 Sim(3)(단안의 경우) 또는 SE(3)(스테레오/VI의 경우) 정합을 수행한다.

이 구조 덕분에 ORB-SLAM3는 트래킹 실패로부터 우아하게 복구할 수 있으며, 과거 세션의 맵을 재사용하는 multi-session SLAM도 지원한다.

**ORB-SLAM3의 성능**: EuRoC MAV 데이터셋에서 stereo-inertial 구성이 3.6cm, TUM-VI에서 9mm 정확도를 달성한다. 이는 feature-based 접근의 강점 — 정밀한 기하학적 제약, 안정적인 루프 클로저 — 을 잘 보여준다.

```python
# ORB-SLAM3 Tracking Thread 수도코드
def track(frame, map, last_keyframe):
    # 1. ORB 특징점 추출 (이미지 피라미드 8레벨)
    keypoints, descriptors = extract_orb(frame, n_features=1000, n_levels=8, scale=1.2)
    
    # 2. 초기 포즈 예측
    if imu_available:
        T_predict = imu_preintegrate(last_frame.T, imu_measurements)
    else:
        T_predict = constant_velocity_model(last_frame.T, last_frame.velocity)
    
    # 3. 맵포인트를 현재 프레임에 투영 → 매칭
    projected_points = project_map_points(map.local_points, T_predict, frame.camera)
    matches = match_by_projection(keypoints, descriptors, projected_points, radius=15)
    
    # 4. Motion-only BA (포즈만 최적화, 맵포인트 고정)
    T_refined = motion_only_BA(
        pose_init=T_predict,
        observations=[(kp, mp) for kp, mp in matches],
        robust_kernel='huber',
        n_iterations=10
    )
    
    # 5. 키프레임 결정
    tracked_ratio = len(matches) / len(last_keyframe.observations)
    if tracked_ratio < 0.9 and len(matches) > 50:
        insert_keyframe(frame, T_refined, matches)
    
    return T_refined
```

---

## 6.2 Direct Visual Odometry

Direct 방법은 특징점 추출 없이 픽셀 밝기(intensity) 자체를 관측으로 사용한다. 기본 아이디어는: 3D 점 $\mathbf{P}$가 두 프레임에서 같은 밝기를 가져야 한다는 가정(brightness constancy)이다.

### 6.2.1 Photometric Error

Direct VO의 핵심 잔차는 photometric error다. 카메라 포즈 $\mathbf{T}_i, \mathbf{T}_j$와 3D 점 $\mathbf{P}$(또는 호스트 프레임의 픽셀 $\mathbf{u}$와 역깊이 $d^{-1}$로 파라미터화)에 대해:

$$e_{\text{photo}} = I_j\left(\pi(\mathbf{T}_j \mathbf{T}_i^{-1} \pi^{-1}(\mathbf{u}_i, d_i^{-1}))\right) - I_i(\mathbf{u}_i)$$

여기서:
- $\pi^{-1}(\mathbf{u}, d^{-1})$: 2D 점과 역깊이로부터 3D 점 복원 (unprojection)
- $\pi(\cdot)$: 3D→2D 투영
- $I_i, I_j$: 프레임 $i, j$의 밝기 이미지

이 잔차를 최소화하여 포즈와 깊이를 추정한다. Feature-based 방법의 reprojection error와의 핵심 차이는:

| | Reprojection error | Photometric error |
|---|---|---|
| 관측 | 특징점 좌표 (2D) | 픽셀 밝기 (1D 또는 패치) |
| 데이터 연관 | 명시적 (매칭 필요) | 암묵적 (와핑으로 계산) |
| 그래디언트 | 기하학적 | 이미지 그래디언트 |
| 텍스처 요구 | 코너/에지 필요 | 그래디언트만 있으면 됨 |
| 조명 변화 | 불변 (기하학적) | 민감 (보정 필요) |

Photometric error의 장점은 명시적 특징점 매칭이 불필요하다는 것이다. 이미지 전체에서 그래디언트가 있는 모든 영역을 활용할 수 있으므로, 텍스처가 부족하지만 약한 그래디언트가 존재하는 환경(하얀 벽의 미세한 질감 등)에서도 동작할 수 있다.

한계는 두 가지다:
1. **밝기 항상성 위반**: 조명 변화, 자동 노출, 렌즈 비네팅 등으로 같은 3D 점의 밝기가 프레임마다 달라진다. 이를 보정하지 않으면 정확도가 급격히 떨어진다.
2. **좁은 수렴 영역 (Basin of convergence)**: 이미지 그래디언트 기반 최적화이므로, 초기 포즈 추정이 나쁘면 로컬 미니멈에 빠진다. 보통 1~2 픽셀 이내의 초기 정합이 필요하다.

### 6.2.2 DSO (Direct Sparse Odometry) 아키텍처 상세 분석

[DSO (Engel et al., 2018)](https://doi.org/10.1109/TPAMI.2017.2658577)는 direct 방법과 sparse 표현을 결합한 VO 시스템이다. 기존에는 "direct = dense" ([LSD-SLAM, Engel et al., 2014](https://doi.org/10.1007/978-3-319-10605-2_54)), "sparse = indirect" (ORB-SLAM)이라는 암묵적 등식이 있었는데, DSO는 이 두 축을 새롭게 조합했다.

**DSO의 핵심 설계 원칙**

1. **Direct**: 특징점 없이 픽셀 밝기를 직접 사용
2. **Sparse**: 이미지 전체가 아니라, 그래디언트가 있는 영역에서 균등하게 점을 샘플링
3. **Joint Optimization**: 포즈, 역깊이, 카메라 내부 파라미터(affine brightness parameters)를 동시 최적화

**완전한 Photometric Calibration**

DSO의 가장 중요한 기여 중 하나는 photometric calibration의 체계적 처리다. 실제 카메라에서 관측되는 밝기 $I'$은 장면의 실제 복사 휘도(irradiance) $B$와 다음 관계를 갖는다:

$$I'(\mathbf{u}) = G(t \cdot V(\mathbf{u}) \cdot B(\mathbf{u}))$$

여기서:
- $G(\cdot)$: 카메라의 비선형 응답 함수 (response function)
- $t$: 노출 시간 (exposure time)
- $V(\mathbf{u})$: 렌즈 비네팅 (vignetting) — 이미지 중심에서 멀어질수록 밝기 감소

이 세 요소를 모두 보정하여 photometrically corrected 이미지를 얻는다:

$$I(\mathbf{u}) = t^{-1} \cdot G^{-1}(I'(\mathbf{u})) / V(\mathbf{u})$$

이 보정 없이는 같은 3D 점이라도 이미지 중심과 가장자리, 또는 노출이 다른 프레임에서 다른 밝기를 가지므로 photometric error가 부정확해진다.

**Affine Brightness Transfer Function**

완벽한 photometric calibration이 불가능한 경우를 대비해, DSO는 프레임 간 밝기 변화를 affine 모델로 추가 보상한다:

$$e_{\mathbf{p}j} = \sum_{\mathbf{p} \in \mathcal{N}_\mathbf{p}} w_\mathbf{p} \left\| (I_j[\mathbf{p}'] - b_j) - \frac{t_j e^{a_j}}{t_i e^{a_i}}(I_i[\mathbf{p}] - b_i) \right\|_\gamma$$

여기서 $a_i, b_i, a_j, b_j$는 프레임별 affine brightness 파라미터로 함께 최적화된다. $\|\cdot\|_\gamma$는 Huber norm이다.

**점 선택 전략**

DSO는 이미지를 격자로 나누고, 각 셀에서 그래디언트 크기가 가장 큰 점을 선택한다. 핵심은 "균등 분포"다 — 한 영역에 특징이 집중되는 것을 방지한다. 약 2000개의 점을 선택하며, 그래디언트 임계값을 적응적으로 조절하여 텍스처가 적은 영역에서도 점을 확보한다.

**Sliding Window Optimization**

DSO는 최근 5~7개의 키프레임과 이들에 속한 점들의 역깊이를 슬라이딩 윈도우에서 joint 최적화한다. 최적화 변수는:

$$\boldsymbol{\theta} = \{\mathbf{T}_1, \ldots, \mathbf{T}_n, d_1^{-1}, \ldots, d_m^{-1}, a_1, b_1, \ldots, a_n, b_n\}$$

즉 카메라 포즈(SE(3)), 역깊이(inverse depth), affine brightness 파라미터를 모두 포함한다.

윈도우에서 빠지는 프레임/점은 Schur complement로 마지널라이즈되어 prior로 남는다. 이 마지널라이제이션은 Ch.4.7에서 다룬 Schur complement 기반 마지널라이제이션과 동일한 원리이지만, 시각 잔차만 다룬다는 점이 다르다.

**DSO의 한계와 확장**

DSO의 본래 설계에는 루프 클로저가 없다. direct 방법의 한계가 아니라 설계 선택이다. LDSO (Loop-closing DSO)는 DBoW와 direct 정합을 결합해 이 한계를 풀었고, VI-DSO·BASALT 등은 DSO에 IMU를 결합한 VIO 변종이다.

```python
# DSO 핵심 흐름 수도코드
def dso_track(frame, window, camera):
    # 1. Photometric calibration 적용
    frame.I = apply_photometric_correction(frame.raw, camera.G_inv, camera.V, frame.exposure)
    
    # 2. 직접 이미지 정합으로 초기 포즈 추정 (coarse-to-fine)
    T_init = direct_alignment_pyramid(
        ref_frame=window.latest_keyframe,
        cur_frame=frame,
        initial_guess=constant_velocity_predict(),
        levels=[4, 3, 2, 1]  # 이미지 피라미드 레벨
    )
    
    # 3. 점 선택 (그래디언트 기반, 균등 분포)
    candidate_points = select_points_gradient_based(frame, n_blocks=32*32, n_per_block=1)
    
    # 4. 역깊이 초기화 (에피폴라 서치)
    for p in candidate_points:
        p.inv_depth = epipolar_search(window.keyframes, frame, p.u)
    
    # 5. 키프레임이면: 슬라이딩 윈도우에 삽입 후 joint optimization
    if is_keyframe(frame, window):
        window.add(frame, candidate_points)
        # 포즈 + 역깊이 + affine 파라미터 동시 최적화
        gauss_newton_optimize(
            residuals=photometric_residuals(window),
            variables=[T, inv_depth, a, b],
            max_iter=6
        )
        # 오래된 프레임 마지널라이즈
        if len(window) > 7:
            marginalize_oldest(window)
    
    return T_init
```

### 6.2.3 Semi-Direct: SVO

[SVO (Semi-direct Visual Odometry, Forster et al., 2017)](https://doi.org/10.1109/TRO.2016.2623335)는 feature-based와 direct의 장점을 결합한 하이브리드 접근이다. "Semi-direct"라는 이름은 추적(tracking)에는 direct 방법을, 매핑(mapping)에는 feature-based 방법을 사용하기 때문이다.

**SVO의 핵심 아이디어**

1. **Sparse Model-based Image Alignment**: 기존 3D 맵포인트를 현재 프레임에 투영하고, 각 투영점 주위의 패치에 대해 photometric error를 최소화하여 프레임 포즈를 추정한다. 이는 DSO처럼 이미지 그래디언트를 직접 사용하지만, 전체 이미지가 아닌 이미 알고 있는 맵포인트 주위만 사용하므로 매우 빠르다.

2. **Feature Alignment**: 포즈가 추정된 후, 각 맵포인트의 투영 위치를 서브픽셀 정밀도로 정제한다. 이때도 패치 기반 direct 정합을 사용한다.

3. **Structure & Motion Refinement**: 정제된 2D 위치를 "가상 특징점"으로 취급하여, BA(reprojection error 최소화)로 포즈와 3D 구조를 함께 최적화한다.

SVO는 이 3단계 분리 덕분에 매우 빠르다 — 고해상도 이미지에서도 200~400Hz를 달성하며, 이는 고속 드론 같은 agile 로봇에 적합하다. 반면 루프 클로저가 없고, 순수 rotation(제자리 회전)에 취약하다는 한계가 있다.

---

## 6.3 Tightly-Coupled Visual-Inertial Odometry

VO만으로는 두 가지 구조적 한계가 있다: (1) 단안 카메라의 스케일 모호성, (2) 빠른 모션이나 텍스처 부족 시 트래킹 실패. IMU를 결합하면 두 문제를 동시에 해결할 수 있다. Tightly-coupled VIO는 카메라와 IMU의 raw 측정을 하나의 추정 프레임워크에서 함께 처리한다.

### 6.3.1 VINS-Mono 아키텍처 상세

[VINS-Mono (Qin et al., 2018)](https://doi.org/10.1109/TRO.2018.2853729)는 단안 카메라 + 저가 IMU만으로 강건한 6-DoF 상태 추정을 달성하는 완전한 VIO 시스템이다. 초기화, 오도메트리, 루프 클로저, 맵 재사용까지 전체 파이프라인을 하나의 시스템으로 통합했다.

**시스템 아키텍처**

VINS-Mono는 크게 세 모듈로 구성된다:

1. **프론트엔드 (Measurement Preprocessing)**: KLT 기반 특징점 추적 + IMU preintegration
2. **백엔드 (Estimator)**: 비선형 최적화 기반 tightly-coupled VIO
3. **루프 클로저 (Relocalization)**: DBoW2 기반 장소 인식 + 재위치추정

**강건한 초기화 (Robust Initialization)**

단안 VIO의 최대 난제는 초기화다. 단안 카메라만으로는 메트릭 스케일을 관측할 수 없고, IMU 바이어스도 알 수 없다. VINS-Mono의 초기화는 loosely-coupled 접근으로 이 문제를 해결한다:

**Step 1: 순수 비전 SfM**

처음 몇 프레임에서 순수 비전만으로 Structure from Motion을 수행한다. 5-point 알고리즘으로 Essential Matrix를 추정하고, 삼각측량으로 3D 점과 카메라 포즈를 복원한다. 이때 스케일은 임의적이다.

**Step 2: Visual-Inertial Alignment**

SfM 결과와 IMU preintegration을 정렬(alignment)한다. 이 과정에서 다음을 추정한다:

(a) 자이로 바이어스 $\mathbf{b}_g$: 연속 키프레임 쌍의 SfM 회전 $\mathbf{R}_{c_k c_{k+1}}^{\text{sfm}}$과 IMU preintegrated 회전 $\Delta\hat{\mathbf{R}}_{k,k+1}$을 비교:

$$\min_{\mathbf{b}_g} \sum_k \left\| \text{Log}\left(\Delta\hat{\mathbf{R}}_{k,k+1}(\mathbf{b}_g)^T \cdot \mathbf{R}_{c_k}^{c_{k+1}} \right) \right\|^2$$

바이어스 변화에 대한 1차 근사(Ch.4.6의 preintegration 자코비안)를 이용하면 이 문제는 선형으로 풀린다.

(b) 중력 방향, 속도, 메트릭 스케일을 동시 추정. 이는 다음 선형 시스템으로 정리된다. 각 키프레임 쌍 $(k, k+1)$에 대해:

$$s\mathbf{R}_{c_0}^w \mathbf{p}_{c_{k+1}}^{c_0} - s\mathbf{R}_{c_0}^w \mathbf{p}_{c_k}^{c_0} - \mathbf{v}_k^w \Delta t_k + \frac{1}{2}\mathbf{g}^w\Delta t_k^2 = \mathbf{R}_k^w \Delta\hat{\mathbf{p}}_{k,k+1}$$

여기서 미지수는 $s$ (스케일), $\mathbf{g}^w$ (중력 벡터), $\{\mathbf{v}_k^w\}$ (속도)다. 중력 크기 $\|\mathbf{g}\| = 9.81$이라는 구속 조건을 추가하여 정확도를 높인다.

이 loosely-coupled 초기화가 수렴하면 tightly-coupled 최적화로 전환한다.

**Tightly-Coupled Nonlinear Optimization**

VINS-Mono의 백엔드는 슬라이딩 윈도우 내에서 세 종류의 잔차를 동시 최적화한다:

$$\min_{\mathcal{X}} \left\{ \left\|\mathbf{r}_p - \mathbf{H}_p \mathcal{X}\right\|^2 + \sum_{k \in \mathcal{B}} \left\|\mathbf{r}_{\mathcal{B}}(\hat{\mathbf{z}}_{b_k b_{k+1}}, \mathcal{X})\right\|^2_{\mathbf{P}_{b_k b_{k+1}}^{-1}} + \sum_{(l,j) \in \mathcal{C}} \left\|\mathbf{r}_{\mathcal{C}}(\hat{\mathbf{z}}_{l}^{c_j}, \mathcal{X})\right\|^2_{(\mathbf{P}_{l}^{c_j})^{-1}} \right\}$$

여기서:
- $\mathbf{r}_p$: 마지널라이제이션 사전(prior) 잔차
- $\mathbf{r}_{\mathcal{B}}$: IMU preintegration 잔차
- $\mathbf{r}_{\mathcal{C}}$: 시각 reprojection error 잔차
- $\mathcal{X}$: 상태 변수 (슬라이딩 윈도우 내 키프레임 포즈, 속도, IMU 바이어스, 특징점 역깊이)

**시각 잔차의 상세 정의**:

특징점 $l$이 키프레임 $i$에서 처음 관측되고 역깊이 $\lambda_l$로 파라미터화되었을 때, 키프레임 $j$에서의 reprojection error는:

$$\mathbf{r}_{\mathcal{C}} = \begin{bmatrix} \bar{u}_l^{c_j} - u_l^{c_j} / z_l^{c_j} \\ \bar{v}_l^{c_j} - v_l^{c_j} / z_l^{c_j} \end{bmatrix}$$

여기서 $\begin{bmatrix} u_l^{c_j} & v_l^{c_j} & z_l^{c_j} \end{bmatrix}^T = \mathbf{R}_{b_j}^{c} (\mathbf{R}_{w}^{b_j}(\mathbf{R}_{b_i}^{w}(\mathbf{R}_{c}^{b_i} \frac{1}{\lambda_l}\begin{bmatrix} \bar{u}_l^{c_i} \\ \bar{v}_l^{c_i} \\ 1 \end{bmatrix} + \mathbf{p}_c^b) + \mathbf{p}_{b_i}^w - \mathbf{p}_{b_j}^w) - \mathbf{p}_c^b)$

**슬라이딩 윈도우 관리와 마지널라이제이션**

VINS-Mono는 윈도우 크기를 고정(보통 10개 키프레임)하되, 키프레임 여부에 따라 두 가지 마지널라이제이션 전략을 적용한다:

1. **최신 프레임이 키프레임인 경우**: 가장 오래된 키프레임을 마지널라이즈한다. Schur complement로 해당 프레임에 연결된 모든 측정을 prior로 변환한다.

2. **최신 프레임이 키프레임이 아닌 경우**: 직전 프레임(second-newest)을 마지널라이즈한다. 이때 시각 측정만 버리고, IMU 측정은 인접 키프레임 사이의 preintegration에 포함되므로 정보가 보존된다.

이 두 전략의 핵심은 **정보 보존**이다. 마지널라이제이션은 변수를 제거하되 그 변수가 제공하던 정보를 prior 형태로 남긴다. Schur complement의 수학적 메커니즘은 Ch.4.7에서 상세히 다루었다.

**4-DoF 포즈 그래프 최적화**

루프 클로저가 검출되면, VINS-Mono는 6-DoF가 아닌 4-DoF (yaw + 3D translation)로 포즈 그래프를 최적화한다. 왜 4-DoF인가? IMU의 가속도계가 중력 방향을 관측하므로, roll과 pitch는 IMU에 의해 이미 충분히 관측 가능(observable)하다. 드리프트는 yaw와 위치에서만 누적된다. 따라서 루프 클로저 시 roll/pitch는 건드리지 않고 yaw와 위치만 보정하는 것이 물리적으로 타당하다.

```python
# VINS-Mono 슬라이딩 윈도우 최적화 수도코드
class VINSEstimator:
    def __init__(self, window_size=10):
        self.window_size = window_size
        self.states = []  # [pose, velocity, bias_g, bias_a] per keyframe
        self.features = {}  # feature_id -> inverse_depth
        self.prior = None  # marginalization prior
    
    def optimize(self):
        # 비용 함수 구성
        cost = 0.0
        
        # 1. 마지널라이제이션 prior
        if self.prior is not None:
            cost += self.prior.evaluate(self.states)
        
        # 2. IMU preintegration 잔차
        for k in range(len(self.states) - 1):
            preint = self.imu_preintegrations[k]
            r_imu = compute_imu_residual(
                self.states[k], self.states[k+1], preint
            )
            cost += r_imu.T @ preint.info_matrix @ r_imu
        
        # 3. 시각 reprojection error
        for feat_id, inv_depth in self.features.items():
            for obs in self.observations[feat_id]:
                r_vis = compute_reprojection_error(
                    self.states[obs.host_frame], self.states[obs.target_frame],
                    inv_depth, obs.uv, self.T_cam_imu
                )
                cost += huber(r_vis.T @ obs.info_matrix @ r_vis)
        
        # Gauss-Newton / Levenberg-Marquardt 최적화
        solve_nonlinear_least_squares(cost, max_iterations=8)
    
    def marginalize(self, is_keyframe):
        if is_keyframe:
            # 가장 오래된 프레임을 마지널라이즈
            self.prior = schur_complement_marginalize(
                self.states[0], connected_factors(self.states[0])
            )
            self.states.pop(0)
        else:
            # 직전 프레임을 마지널라이즈 (시각만, IMU는 보존)
            self.prior = schur_complement_marginalize(
                self.states[-2], visual_factors(self.states[-2])
            )
            self.states.pop(-2)
```

### 6.3.2 OKVIS

[OKVIS (Open Keyframe-based Visual-Inertial SLAM, Leutenegger et al., 2015)](https://doi.org/10.1177/0278364914554813)는 VINS-Mono보다 먼저 나온 tightly-coupled VIO로, 키프레임 기반 슬라이딩 윈도우 최적화의 초기 형태를 제시했다.

**OKVIS의 핵심 특징**:

1. **Harris corner + BRISK descriptor**: ORB 대신 Harris 코너와 BRISK 디스크립터를 사용한다.
2. **Keyframe-based marginalization**: VINS-Mono와 유사하게 슬라이딩 윈도우에서 마지널라이제이션을 수행한다.
3. **Speed error term**: IMU preintegration 대신, 짧은 시간 간격의 IMU 적분을 직접 수행하고 속도 제약으로 사용한다. 이후 OKVIS2에서 preintegration으로 전환했다.
4. **Ceres Solver 기반**: 최적화에 Ceres Solver를 사용한다.

OKVIS는 VINS-Mono 대비 초기화가 간단하지만(스테레오 카메라를 기본으로 가정), 단안 모드에서의 강건한 초기화는 VINS-Mono가 더 우수하다.

### 6.3.3 MSCKF: Multi-State Constraint Kalman Filter

[MSCKF (Mourikis & Roumeliotis, 2007)](https://doi.org/10.1109/ROBOT.2007.364024)는 필터 기반 VIO의 대표 알고리즘이다. 최적화 기반(VINS-Mono, ORB-SLAM3)과 구조적으로 다른 접근을 취하며, 현재까지도 특정 응용에서 활발히 사용된다.

**MSCKF의 핵심 아이디어: 랜드마크를 상태에 넣지 않는다**

EKF-SLAM은 랜드마크(3D 점)를 상태 벡터에 포함시킨다. $N$개의 랜드마크가 있으면 상태 벡터 크기가 $3N + 15$가 되고, 공분산 행렬의 크기가 $(3N+15)^2$이 되어 랜드마크 수에 대해 $O(N^2)$ 공간, $O(N^3)$ 시간이 필요하다. 이는 실시간 처리에 치명적이다.

**랜드마크를 상태 벡터에서 제외하면서도, 랜드마크가 제공하는 기하학적 구속 정보를 보존할 수 있다**는 것이 MSCKF의 출발점이다.

**상태 벡터 구조**

MSCKF의 상태 벡터는 IMU 오차 상태(error-state)와 슬라이딩 윈도우 내 $N$개 카메라 포즈로 구성된다:

$$\tilde{\mathbf{x}} = [\tilde{\mathbf{x}}_{IMU}^T, \tilde{\mathbf{x}}_{C_1}^T, \ldots, \tilde{\mathbf{x}}_{C_N}^T]^T$$

여기서 IMU 오차 상태는:

$$\tilde{\mathbf{x}}_{IMU} = [\delta\boldsymbol{\theta}^T, \tilde{\mathbf{b}}_g^T, {}^G\tilde{\mathbf{v}}_I^T, \tilde{\mathbf{b}}_a^T, {}^G\tilde{\mathbf{p}}_I^T]^T \in \mathbb{R}^{15}$$

각 카메라 포즈 오차 상태는:

$$\tilde{\mathbf{x}}_{C_k} = [\delta\boldsymbol{\theta}_{C_k}^T, {}^G\tilde{\mathbf{p}}_{C_k}^T]^T \in \mathbb{R}^6$$

상태 벡터의 전체 크기는 $15 + 6N$이다. 랜드마크 수와 무관하다.

**Null-Space 투영: 핵심 수학**

하나의 정적 특징점 $\mathbf{p}_f$가 $M$개의 카메라 포즈에서 관측되었다고 하자. 관측 방정식을 선형화하면:

$$\mathbf{r} = \mathbf{H}_X \tilde{\mathbf{x}} + \mathbf{H}_f \tilde{\mathbf{p}}_f + \mathbf{n}$$

여기서:
- $\mathbf{r} \in \mathbb{R}^{2M}$: 잔차 벡터 (관측 - 예측)
- $\mathbf{H}_X \in \mathbb{R}^{2M \times (15+6N)}$: 상태에 대한 자코비안
- $\mathbf{H}_f \in \mathbb{R}^{2M \times 3}$: 특징점 위치에 대한 자코비안
- $\tilde{\mathbf{p}}_f \in \mathbb{R}^3$: 특징점 위치 오차

$\mathbf{H}_f$를 QR 분해한다:

$$\mathbf{H}_f = \begin{bmatrix} \mathbf{Q}_1 & \mathbf{Q}_2 \end{bmatrix} \begin{bmatrix} \mathbf{R}_1 \\ \mathbf{0} \end{bmatrix}$$

$\mathbf{Q}_2$는 $\mathbf{H}_f$의 left null space를 구성한다 ($\mathbf{Q}_2^T \mathbf{H}_f = \mathbf{0}$). 양변에 $\mathbf{Q}_2^T$를 곱하면:

$$\mathbf{r}_o = \mathbf{Q}_2^T \mathbf{r} = \mathbf{Q}_2^T \mathbf{H}_X \tilde{\mathbf{x}} + \mathbf{Q}_2^T \mathbf{n} = \mathbf{H}_o \tilde{\mathbf{x}} + \mathbf{n}_o$$

특징점 위치 $\tilde{\mathbf{p}}_f$가 완전히 소거되었다. $\mathbf{r}_o$와 $\mathbf{H}_o$만으로 EKF 업데이트를 수행할 수 있다. 이것이 MSCKF의 "multi-state constraint"다 — 하나의 특징점이 여러 카메라 포즈에 걸쳐 만드는 기하학적 구속을 직접 이용하되, 특징점 자체는 상태에서 제외한다.

**계산 복잡도**: 상태 벡터 크기가 $15 + 6N$ (카메라 포즈 수 $N$)으로 랜드마크 수 $M$과 무관하다. EKF-SLAM은 $M$개 랜드마크를 상태에 포함하여 상태 크기가 $O(M)$이 되고, 공분산 업데이트에 $O(M^2)$이 필요하다. MSCKF는 랜드마크를 상태에서 제외하므로, 카메라 수 $N \ll M$에만 의존하는 것이 핵심 장점이다.

**MSCKF 업데이트 절차**

1. **IMU propagation**: 새 IMU 측정이 들어오면 상태를 전파하고 공분산을 업데이트한다:
   $$\hat{\mathbf{x}}_{k+1|k} = f(\hat{\mathbf{x}}_{k|k}, \mathbf{u}_k)$$
   $$\mathbf{P}_{k+1|k} = \boldsymbol{\Phi}_k \mathbf{P}_{k|k} \boldsymbol{\Phi}_k^T + \mathbf{G}_k \mathbf{Q} \mathbf{G}_k^T$$

2. **State augmentation**: 새 이미지가 들어오면 현재 IMU 포즈를 복사하여 카메라 포즈를 상태에 추가한다. 공분산 행렬도 확장한다.

3. **MSCKF update**: 추적이 끝난 특징점(더 이상 관측되지 않는 점)에 대해:
   - 삼각측량으로 $\hat{\mathbf{p}}_f$를 추정한다.
   - null-space 투영으로 $\mathbf{r}_o, \mathbf{H}_o$를 계산한다.
   - 표준 EKF 업데이트를 수행한다:
     $$\mathbf{K} = \mathbf{P} \mathbf{H}_o^T (\mathbf{H}_o \mathbf{P} \mathbf{H}_o^T + \sigma^2 \mathbf{I})^{-1}$$
     $$\hat{\mathbf{x}} \leftarrow \hat{\mathbf{x}} + \mathbf{K} \mathbf{r}_o$$
     $$\mathbf{P} \leftarrow (\mathbf{I} - \mathbf{K}\mathbf{H}_o)\mathbf{P}$$

4. **State pruning**: 슬라이딩 윈도우에서 오래된 카메라 포즈를 제거하고 공분산을 축소한다.

```cpp
// MSCKF 핵심 업데이트 수도코드 (C++)
void MSCKF::msckf_update(const Feature& feature) {
    // 1. 삼각측량으로 특징점 3D 위치 추정
    Vector3d p_f = triangulate(feature.observations, cam_states);
    
    // 2. 관측 자코비안 계산
    int N_obs = feature.observations.size();
    MatrixXd H_X(2*N_obs, state_dim);  // 상태에 대한 자코비안
    MatrixXd H_f(2*N_obs, 3);          // 특징점에 대한 자코비안
    VectorXd r(2*N_obs);               // 잔차
    
    for (int i = 0; i < N_obs; i++) {
        auto& obs = feature.observations[i];
        auto& cam = cam_states[obs.cam_id];
        
        Vector3d p_c = cam.R_w2c * (p_f - cam.p_w);
        double X = p_c(0), Y = p_c(1), Z = p_c(2);
        
        // 투영 자코비안 (pinhole)
        Matrix<double, 2, 3> J_proj;
        J_proj << 1.0/Z, 0, -X/(Z*Z),
                  0, 1.0/Z, -Y/(Z*Z);
        
        // H_f 계산
        H_f.block<2,3>(2*i, 0) = J_proj * cam.R_w2c;
        
        // H_X 계산 (카메라 포즈에 대한 부분)
        // ... (rotation, translation 자코비안)
        
        // 잔차
        r.segment<2>(2*i) = obs.uv - Vector2d(X/Z, Y/Z);
    }
    
    // 3. QR 분해로 null-space 투영
    // H_f = Q * [R1; 0] → Q2^T * H_f = 0
    HouseholderQR<MatrixXd> qr(H_f);
    MatrixXd Q = qr.householderQ();
    MatrixXd Q2 = Q.rightCols(2*N_obs - 3);
    
    MatrixXd H_o = Q2.transpose() * H_X;
    VectorXd r_o = Q2.transpose() * r;
    
    // 4. 표준 EKF 업데이트
    MatrixXd S = H_o * P * H_o.transpose() + sigma2 * MatrixXd::Identity(r_o.size(), r_o.size());
    MatrixXd K = P * H_o.transpose() * S.inverse();
    
    VectorXd dx = K * r_o;
    // 상태 업데이트 (on-manifold)
    apply_correction(dx);
    // 공분산 업데이트
    P = (MatrixXd::Identity(state_dim, state_dim) - K * H_o) * P;
}
```

### 6.3.4 OpenVINS

[OpenVINS (Geneva et al., 2020)](https://doi.org/10.1109/ICRA40945.2020.9196524)는 MSCKF 기반 VIO의 가장 완성된 오픈소스 구현이다. 단순한 구현을 넘어, 다양한 VIO 알고리즘 변형을 모듈식으로 비교/실험할 수 있는 연구 플랫폼을 지향한다.

**OpenVINS의 핵심 특징**:

1. **On-Manifold Sliding Window EKF**: MSCKF를 기반으로 한 sliding window 칼만 필터. SO(3) 매니폴드 위에서 회전을 처리한다.

2. **온라인 캘리브레이션**: 카메라 intrinsic, camera-IMU extrinsic, 시간 오프셋(temporal offset)을 런타임에 자동 추정한다. 시간 오프셋 추정은 특히 중요한데, 카메라와 IMU의 타임스탬프가 완벽히 동기화되지 않으면 성능이 크게 저하되기 때문이다.

3. **SLAM 랜드마크 지원**: 순수 MSCKF는 특징점을 상태에 포함하지 않지만, OpenVINS는 선택적으로 일부 랜드마크를 SLAM feature로 상태에 포함할 수 있다. SLAM feature는 오래 추적되는 점으로, anchored inverse depth로 파라미터화된다.

4. **First-Estimates Jacobian (FEJ)**: EKF의 일관성(consistency) 문제를 해결하기 위한 기법이다. 표준 EKF는 매 업데이트마다 최신 상태 추정치에서 자코비안을 재계산하는데, 이는 관측 가능성(observability) 속성을 위반하여 공분산을 과도하게 줄이는 문제를 일으킨다. FEJ는 자코비안을 최초 추정치(first estimate)에서만 계산하여 올바른 관측 가능성을 유지한다.

5. **시뮬레이터**: VIO 알고리즘을 테스트하기 위한 시뮬레이터를 내장하고 있다. 다양한 궤적과 환경 설정, IMU 노이즈 모델을 지원하며, ground truth와의 비교를 통해 정량적 평가가 가능하다.

### 6.3.5 Basalt

[Basalt (Usenko et al., 2020)](https://doi.org/10.1109/LRA.2019.2961227)는 VINS-Mono와 유사한 tightly-coupled VIO이지만, 몇 가지 설계 선택에서 차별화된다.

**Basalt의 핵심 특징**:

1. **Visual-only Frontend**: KLT 대신 패치 기반 direct 정합(SVO와 유사)으로 서브픽셀 정밀도의 특징점 추적을 수행한다.

2. **Non-linear Factor Recovery (NFR)**: 마지널라이제이션의 대안이다. 마지널라이제이션은 선형화 지점에 의존하는 prior를 남기는데, 이 선형화 지점이 나중에 크게 변하면 정보 왜곡이 발생한다. Basalt의 NFR은 마지널라이즈된 정보를 비선형 factor로 근사하여, 재선형화가 가능하게 한다.

3. **Efficient Implementation**: Basalt는 factor graph의 구조를 활용한 효율적 구현으로, VINS-Mono 대비 더 빠른 처리 속도를 달성한다.

4. **Stereo/Multi-camera 지원**: 여러 카메라의 시각 정보를 자연스럽게 통합한다.

---

## 6.4 VIO 설계 선택지

VIO 시스템을 설계할 때 몇 가지 핵심적인 설계 선택이 있다. 이 섹션에서는 각 선택지의 장단점과 적용 시나리오를 분석한다.

### 6.4.1 Filter vs Optimization

Filter vs Optimization 논쟁은 VIO 분야에서 가장 오래된 것 중 하나다.

**Filter 기반 (MSCKF, OpenVINS)**

- 현재 상태만 유지하고, 새 측정이 올 때마다 순차적으로 업데이트
- 과거 상태는 현재 상태의 분포(mean + covariance)에 "흡수"됨
- 계산 복잡도: 업데이트당 $O(N^2)$ (N은 상태 차원)
- 장점: 일정한 계산 비용, 구현 간단
- 단점: 선형화 오차 누적 (한번 선형화하면 교정 불가), 일관성(consistency) 문제

**Optimization 기반 (VINS-Mono, ORB-SLAM3, Basalt)**

- 슬라이딩 윈도우 내 여러 상태를 동시에 유지하고, 모든 측정을 함께 최적화
- 반복 재선형화(iterative re-linearization)가 가능
- 계산 복잡도: 반복당 $O(N^3)$ (N은 윈도우 크기), 하지만 Schur complement로 효율화
- 장점: 더 정확 (재선형화로 선형화 오차 감소), 다양한 측정 통합 용이
- 단점: 계산 비용 높음, 윈도우 크기에 민감

**실험적 비교**: 같은 조건에서 최적화 기반이 필터 기반보다 일반적으로 더 정확하다. 그러나 MSCKF도 FEJ, 적절한 관측 가능성 분석 등을 적용하면 격차가 크게 줄어든다. 리소스 제약이 심한 환경(초저전력 MCU 등)에서는 필터 기반이 여전히 매력적이다.

### 6.4.2 Keyframe Selection 전략

키프레임 선택은 VIO 성능에 큰 영향을 미친다. 너무 자주 키프레임을 삽입하면 계산 부담이 커지고, 너무 드물게 삽입하면 정보 손실이 발생한다.

일반적인 키프레임 선택 기준:

1. **시간 기반**: 마지막 키프레임 이후 일정 시간 경과
2. **시차(parallax) 기반**: 현재 프레임과 마지막 키프레임 사이의 평균 특징점 시차가 임계값 초과. VINS-Mono는 이 기준을 사용한다.
3. **추적 품질 기반**: 추적된 특징점 수가 임계값 이하로 떨어지면 키프레임 삽입. ORB-SLAM3는 이 기준을 사용한다.
4. **Information gain 기반**: 새 키프레임이 제공할 정보량(정보 이득)을 추정하여 결정. 이론적으로 가장 합리적이지만 계산 비용이 높다.

키프레임 선택은 마지널라이제이션과 밀접하게 연결된다. VINS-Mono의 two-way marginalization 전략(6.3.1절 참조)은 키프레임 여부에 따라 마지널라이제이션 방향을 바꾸는 것으로, 이 연결을 잘 보여준다.

### 6.4.3 Feature Parameterization

3D 점을 어떻게 파라미터화하느냐도 중요한 설계 선택이다.

**XYZ (Euclidean 3D 좌표)**

가장 직관적이다. $\mathbf{P} = [X, Y, Z]^T \in \mathbb{R}^3$. 그러나 먼 점(far point)에서 불안정하다 — 작은 각도 변화에도 $Z$ 값이 크게 변한다.

**Inverse Depth**

점을 "호스트 프레임의 관측 방향 + 역깊이"로 표현한다:

$$\boldsymbol{\lambda} = [\theta, \phi, \rho]^T$$

여기서 $\theta, \phi$는 호스트 프레임에서의 방위각/고도각, $\rho = 1/d$는 역깊이다. 장점:

1. **원거리 점 처리**: $d \to \infty$일 때 $\rho \to 0$으로 수치적으로 안정적이다. XYZ에서는 $Z \to \infty$가 되어 불안정하다.
2. **선형성 개선**: 단안 초기화에서 깊이가 불확실할 때, 역깊이의 불확실성 분포가 가우시안에 더 가깝다.

VINS-Mono와 OpenVINS는 역깊이 파라미터화를 사용한다.

**Anchored Inverse Depth**

역깊이를 특정 "앵커" 키프레임에 고정(anchored)하여 표현한다:

$$\mathbf{P} = \mathbf{T}_{\text{anchor}} \cdot \frac{1}{\rho} [\bar{u}, \bar{v}, 1]^T$$

여기서 $(\bar{u}, \bar{v})$는 앵커 프레임에서의 정규화 좌표, $\rho$는 역깊이다. 이 파라미터화의 장점은 앵커 프레임의 포즈가 변해도 역깊이 자체는 변하지 않으므로, 부분적으로 선형화 오차를 줄인다. ORB-SLAM3와 OpenVINS에서 SLAM feature에 사용된다.

---

## 6.5 학습 기반 VO/VIO

전통적 VO/VIO는 "사람이 설계한 파이프라인"에 의존한다: 특징점 검출 → 매칭 → RANSAC → BA. 학습 기반 접근은 이 파이프라인의 일부 또는 전체를 신경망으로 대체하려 한다.

### 6.5.1 Supervised: DeepVO 계열

초기 학습 기반 VO ([DeepVO, Wang et al., 2017](https://doi.org/10.1109/ICRA.2017.7989236))는 연속 이미지 쌍을 입력으로 받아 상대 포즈를 직접 예측하는 end-to-end 네트워크를 훈련했다. CNN으로 시각 특징을 추출하고, LSTM으로 시간적 의존성을 모델링한다.

한계는 명백하다:
- 학습 데이터의 환경에 과적합 (generalization 부족)
- 기하학적 제약(에피폴라 기하 등)을 활용하지 않아 정확도가 전통 방법에 못 미침
- 스케일 드리프트가 심함

### 6.5.2 Self-supervised: 한계와 현재 위치

자기 지도 학습 VO (SfMLearner, Monodepth2 등)는 view synthesis를 통한 photometric loss로 깊이와 포즈를 동시에 학습한다. 레이블 없이 학습할 수 있다는 장점이 있지만, 움직이는 물체(moving objects), 텍스처 부족, occlusion 등에서 어려움을 겪는다.

현재 위치: 자기 지도 학습 VO는 단안 깊이 추정(monocular depth estimation)에서는 큰 성과를 거두었지만, 순수 VO/VIO 시스템으로서는 전통 방법에 크게 못 미친다. 특히 누적 드리프트 문제를 해결하지 못했다.

### 6.5.3 Hybrid: DROID-SLAM

[DROID-SLAM (Teed & Deng, 2021)](https://arxiv.org/abs/2108.10869)은 전통적 BA의 기하학적 엄밀성과 딥러닝의 강건한 매칭 능력을 하나의 미분 가능 파이프라인으로 통합한 시스템이다. DROID-SLAM은 학습 기반 SLAM이 전통적 시스템을 모든 지표에서 능가할 수 있음을 최초로 입증했다.

**아키텍처**

DROID-SLAM은 두 컴포넌트로 작동한다:

1. **RAFT 기반 반복 업데이트 연산자 (Iterative Update Operator)**

RAFT (Recurrent All-Pairs Field Transforms, Teed & Deng, 2020)에서 영감을 받은 구조다. 프레임 쌍 $(i, j)$에 대해:
- 두 프레임의 feature map에서 4D correlation volume을 계산한다.
- 현재 포즈/깊이 추정으로부터 유도된 correspondence field를 기준으로, correlation volume에서 특징을 인덱싱한다.
- 3×3 Convolutional GRU가 correlation 특징, 현재 flow, context 특징을 입력받아 **flow revision** $\Delta\mathbf{f}_{ij}$와 **confidence weight** $\mathbf{w}_{ij}$를 출력한다.

$$(\Delta\mathbf{f}_{ij}, \mathbf{w}_{ij}) = \text{ConvGRU}(\text{corr}_{ij}, \mathbf{f}_{ij}^{\text{curr}}, \text{context}_i)$$

이 업데이트는 반복적으로(iteratively) 수행되어 correspondence를 점진적으로 정제한다.

2. **미분 가능 Dense Bundle Adjustment (DBA) 레이어**

GRU가 출력한 flow revision을 기하학적 업데이트로 변환하는 레이어다. 핵심은: 모든 픽셀에 대해 reprojection error를 정의하고, 이를 카메라 포즈 $\mathbf{T}_i \in SE(3)$와 역깊이 $d_i$에 대해 Gauss-Newton으로 최소화한다:

$$\sum_{(i,j)} \sum_{\mathbf{p}} \left\| \mathbf{w}_{ij}^{\mathbf{p}} \circ (\mathbf{p}^{*}_{ij} - \pi(\mathbf{T}_j \circ \mathbf{T}_i^{-1} \circ \pi^{-1}(\mathbf{p}, d_i^{\mathbf{p}}))) \right\|^2$$

여기서 $\mathbf{p}^{*}_{ij} = \mathbf{p} + \mathbf{f}_{ij}^{\text{curr}} + \Delta\mathbf{f}_{ij}$는 target correspondence, $\mathbf{w}_{ij}^{\mathbf{p}}$는 confidence weight이다.

Gauss-Newton 업데이트를 유도하면 정규 방정식이 나온다:

$$\begin{bmatrix} \mathbf{B} & \mathbf{E} \\ \mathbf{E}^T & \mathbf{C} \end{bmatrix} \begin{bmatrix} \boldsymbol{\xi} \\ \delta\mathbf{d} \end{bmatrix} = \begin{bmatrix} \mathbf{v} \\ \mathbf{w} \end{bmatrix}$$

여기서 $\boldsymbol{\xi}$는 포즈 업데이트(SE(3) tangent space), $\delta\mathbf{d}$는 역깊이 업데이트, $\mathbf{B}$는 포즈 블록, $\mathbf{C}$는 깊이 블록(대각), $\mathbf{E}$는 교차 블록이다.

전통 BA와 동일하게 Schur complement를 적용한다:

$$(\mathbf{B} - \mathbf{E}\mathbf{C}^{-1}\mathbf{E}^T)\boldsymbol{\xi} = \mathbf{v} - \mathbf{E}\mathbf{C}^{-1}\mathbf{w}$$

$\mathbf{C}$는 대각이므로 $\mathbf{C}^{-1}$은 trivial하다. 축소된 시스템은 카메라 수에만 의존하므로 효율적이다.

전체 Gauss-Newton 솔버가 **미분 가능**하다는 것이 핵심이다. 역전파를 통해 GRU의 파라미터가 "좋은 correspondence"를 출력하도록 학습된다.

**프레임 그래프와 루프 클로저**

DROID-SLAM은 co-visibility 기반 프레임 그래프를 동적으로 구축한다. 새 프레임이 추가될 때 이웃 프레임과 엣지를 연결하고, 과거 프레임과의 co-visibility가 감지되면 장거리 엣지를 추가하여 루프 클로저를 수행한다. 백엔드에서는 전체 키프레임 히스토리에 대한 글로벌 BA를 수행한다.

**다중 모달리티 지원**

DROID-SLAM은 단안 영상만으로 학습했음에도 추론 시 스테레오와 RGB-D를 직접 활용할 수 있다:
- **스테레오**: 왼쪽/오른쪽 프레임을 별도 프레임으로 취급하되, 알려진 baseline으로 상대 포즈를 고정한다.
- **RGB-D**: 깊이 정보를 역깊이 초기값으로 사용하고, 깊이 관측을 추가 제약으로 반영한다.

**성능**

- TartanAir: 기존 최고 대비 오차 62% 감소
- EuRoC (monocular): 82% 감소
- ETH-3D: 32개 중 30개 시퀀스 성공 (기존 최고 19개)
- 합성 데이터(TartanAir)만으로 학습 후 4개 실제 데이터셋에서 모두 SOTA

**왜 중요한가**

DROID-SLAM은 기하학적 엄밀성(BA)과 데이터 기반 강건성(learned correspondence)을 결합한 최초의 실용적 시스템이다. 전통 방법이 실패하는 환경(반복 패턴, 텍스처 부족, 급격한 조명 변화)에서도 안정적으로 동작한다.

그러나 한계도 있다:
- **실시간성**: GPU가 필수이며, 현재 실시간보다 느리다.
- **IMU 미포함**: 순수 시각 시스템으로, IMU 결합은 아직 연구 과제다.
- **메모리**: 모든 프레임의 feature map과 correlation volume을 유지해야 하므로 메모리 사용량이 크다.

현재 후속 연구가 이 한계를 하나씩 해결하고 있으며, 학습 기반 VO/VIO는 빠르게 전통 방법을 추격하고 있다.

### 6.5.4 최근 동향 (2023-2025)

[DPVO (Teed & Deng, 2023)](https://arxiv.org/abs/2208.04726)는 DROID-SLAM의 dense flow를 sparse patch 기반 매칭으로 대체하여, 메모리를 1/3로 줄이고 속도를 3배 향상시키면서도 동등 이상의 정확도를 달성했다. 패치 단위 recurrent update operator와 미분 가능 BA를 결합하여 실시간에 가까운 학습 기반 VO를 실현했다.

[MAC-VO (Qu et al., 2024)](https://arxiv.org/abs/2409.09479)는 학습 기반 매칭 불확실성(metrics-aware covariance)을 스테레오 VO에 도입하여, 키포인트 선택과 포즈 그래프 최적화의 잔차 가중치를 불확실성으로 결정한다. 조명 변화와 텍스처 부족 환경에서 기존 VO/SLAM 시스템을 능가하며, ICRA 2025 Best Paper로 선정되었다.

---

## 6장 요약

| 시스템 | 유형 | 추정 방법 | 센서 | 핵심 특징 |
|--------|------|-----------|------|-----------|
| ORB-SLAM3 | Feature-based | Optimization (BA) | Mono/Stereo/RGBD + IMU | Multi-map Atlas, fisheye 지원 |
| DSO | Direct | Optimization (windowed) | Mono | Photometric calibration, sparse sampling |
| SVO | Semi-direct | Optimization (BA) | Mono/Stereo | 200-400Hz, 고속 드론 적합 |
| VINS-Mono | Feature-based | Optimization (sliding window) | Mono + IMU | 강건한 초기화, 4-DoF loop closure |
| MSCKF | Feature-based | EKF (sliding window) | Mono/Stereo + IMU | 랜드마크를 상태에서 제외, null-space 투영 |
| OpenVINS | Feature-based | EKF (MSCKF) | Mono/Stereo + IMU | 온라인 캘리브레이션, FEJ, 연구 플랫폼 |
| Basalt | Semi-direct | Optimization (sliding window) | Stereo + IMU | NFR, 효율적 구현 |
| DROID-SLAM | Learned | Differentiable BA | Mono/Stereo/RGBD | 미분 가능 BA, 합성 데이터 학습 |
| DPVO | Learned (sparse) | Differentiable BA | Mono | DROID 대비 3배 빠르고 메모리 1/3 |
| MAC-VO | Learned + Opt. | Pose graph opt. | Stereo | Metrics-aware covariance, ICRA 2025 Best Paper |

다음 챕터에서는 LiDAR 기반 오도메트리와 LiDAR-Inertial 결합 시스템을 다루며, 카메라 기반 시스템과의 상보적 관계를 분석한다.

---

# Ch.7 — LiDAR Odometry & LiDAR-Inertial Odometry

Ch.6에서 카메라(+IMU) 기반의 Visual Odometry를 다루었다. 이 챕터에서는 LiDAR 기반으로 동일한 자기 운동 추정 문제에 접근한다.

LiDAR는 카메라와 상보적인 센서다. 카메라가 풍부한 텍스처 정보를 제공하지만 조명에 민감하고 절대 거리를 알 수 없는 반면, LiDAR는 조명 불변의 정밀한 3D 거리 측정을 제공한다. 이 챕터에서는 LiDAR로부터 자기 운동을 추정하는 LiDAR Odometry(LO)와, IMU를 결합한 LiDAR-Inertial Odometry(LIO)의 내부 구조를 심도 있게 다룬다.

LiDAR 오도메트리의 핵심 문제는 **포인트 클라우드 정합(registration)**이다 — 연속된 두 스캔 사이의 강체 변환 $\mathbf{T} \in SE(3)$를 찾는 것이다. 이 단순해 보이는 문제가 실제로는 데이터 연관(correspondence), 노이즈 모델, 계산 효율, 모션 왜곡 보정 등 다양한 도전을 포함한다.

---

## 7.1 Point Cloud Registration 기초

포인트 클라우드 정합은 두 점군 $\mathcal{P} = \{\mathbf{p}_i\}$와 $\mathcal{Q} = \{\mathbf{q}_j\}$ 사이의 최적 강체 변환을 찾는 문제다:

$$\mathbf{T}^* = \underset{\mathbf{T} \in SE(3)}{\arg\min} \sum_i d(\mathbf{T} \cdot \mathbf{p}_i, \mathcal{Q})$$

여기서 $d(\cdot, \cdot)$는 변환된 소스 점과 타겟 점군 사이의 거리 메트릭이다. 이 거리의 정의에 따라 다양한 ICP 변종이 나뉜다.

### 7.1.1 ICP 변종들

**Point-to-Point ICP ([Besl & McKay, 1992](https://doi.org/10.1109/34.121791))**

가장 기본적인 형태로, 변환된 소스 점과 가장 가까운 타겟 점 사이의 유클리드 거리를 최소화한다:

$$\mathbf{T}^* = \underset{\mathbf{T}}{\arg\min} \sum_i \left\|\mathbf{T} \cdot \mathbf{p}_i - \mathbf{q}_{c(i)}\right\|^2$$

여기서 $c(i) = \arg\min_j \|\mathbf{T} \cdot \mathbf{p}_i - \mathbf{q}_j\|$는 최근접점(closest point) 대응이다. ICP는 두 단계를 반복한다:

1. **대응 찾기**: 현재 변환으로 소스 점을 변환한 뒤, 타겟에서 최근접점을 찾는다. kd-tree를 사용하면 $O(N\log N)$이다.

2. **변환 추정**: 대응 쌍이 주어지면, 최적 변환은 closed-form으로 구할 수 있다. SVD를 이용한 방법:
   
   양 점군의 중심을 뺀다:
   $$\bar{\mathbf{p}} = \frac{1}{N}\sum_i \mathbf{p}_i, \quad \bar{\mathbf{q}} = \frac{1}{N}\sum_i \mathbf{q}_{c(i)}$$
   
   교차 공분산 행렬을 계산한다:
   $$\mathbf{W} = \sum_i (\mathbf{p}_i - \bar{\mathbf{p}})(\mathbf{q}_{c(i)} - \bar{\mathbf{q}})^T$$
   
   SVD 분해: $\mathbf{W} = \mathbf{U}\boldsymbol{\Sigma}\mathbf{V}^T$
   
   최적 회전과 이동:
   $$\mathbf{R}^* = \mathbf{V} \text{diag}(1, 1, \det(\mathbf{V}\mathbf{U}^T)) \mathbf{U}^T, \quad \mathbf{t}^* = \bar{\mathbf{q}} - \mathbf{R}^*\bar{\mathbf{p}}$$
   
   $\det(\mathbf{V}\mathbf{U}^T) = 1$이면 $\mathbf{R}^* = \mathbf{V}\mathbf{U}^T$이고, $\det(\mathbf{V}\mathbf{U}^T) = -1$이면 반사(reflection)를 방지하기 위해 $\mathbf{V}$의 마지막 열 부호를 뒤집는다.

Point-to-point ICP의 한계:
- 평면에서의 슬라이딩 — 평면 위의 점들은 평면을 따라 미끄러져도 비용이 변하지 않아, 수렴이 느리다.
- 초기값 의존성 — 로컬 최소값에 빠지기 쉽다.
- 최근접점 대응의 부정확함 — 두 스캔의 샘플링 패턴이 다르면 진정한 대응이 아닐 수 있다.

**Point-to-Plane ICP**

평면 위의 점에 대해서는, 점 사이 거리보다 점에서 평면까지의 거리가 더 물리적으로 의미 있다:

$$\mathbf{T}^* = \underset{\mathbf{T}}{\arg\min} \sum_i \left((\mathbf{T} \cdot \mathbf{p}_i - \mathbf{q}_{c(i)})^T \mathbf{n}_{c(i)}\right)^2$$

여기서 $\mathbf{n}_{c(i)}$는 타겟 점 $\mathbf{q}_{c(i)}$에서의 표면 법선(surface normal)이다. 점과 점 사이 거리가 아니라 점에서 법선 방향으로의 거리만 측정하므로, 평면을 따른 슬라이딩은 비용에 기여하지 않는다.

장점: Point-to-point 대비 수렴 속도가 훨씬 빠르다. 특히 평면이 많은 실내/도시 환경에서 효과적이다.

단점: closed-form 해가 없어 반복 최적화(Gauss-Newton 등)가 필요하다. 법선 추정의 정확도에 의존한다.

법선은 각 점의 이웃점들에 대해 PCA(주성분 분석)를 수행하여 가장 작은 고유값에 대응하는 고유벡터로 추정한다. 이웃의 공분산 행렬:

$$\mathbf{C} = \frac{1}{k}\sum_{j \in \mathcal{N}(i)} (\mathbf{q}_j - \bar{\mathbf{q}})(\mathbf{q}_j - \bar{\mathbf{q}})^T$$

고유값 분해 $\mathbf{C} = \mathbf{V}\boldsymbol{\Lambda}\mathbf{V}^T$에서 $\lambda_{\min}$에 대응하는 고유벡터가 법선 방향이다.

### 7.1.2 GICP (Generalized ICP)

GICP ([Segal et al., 2009](https://doi.org/10.15607/RSS.2009.V.021))는 point-to-point, point-to-plane, plane-to-plane ICP를 하나의 확률적 프레임워크로 통합한다.

핵심 아이디어: 각 점이 국소 표면의 불확실성을 반영하는 공분산 $\mathbf{C}_i$를 가진다고 모델링한다. 비용 함수는:

$$\mathbf{T}^* = \underset{\mathbf{T}}{\arg\min} \sum_i (\mathbf{T} \cdot \mathbf{p}_i - \mathbf{q}_{c(i)})^T (\mathbf{C}_i^{\mathcal{Q}} + \mathbf{R}\mathbf{C}_i^{\mathcal{P}}\mathbf{R}^T)^{-1} (\mathbf{T} \cdot \mathbf{p}_i - \mathbf{q}_{c(i)})$$

여기서 $\mathbf{C}_i^{\mathcal{P}}, \mathbf{C}_i^{\mathcal{Q}}$는 각각 소스와 타겟 점의 국소 표면 공분산이다.

**공분산의 물리적 의미**:
- 평면 위의 점: 법선 방향으로 작은 분산, 접선 방향으로 큰 분산 → $\mathbf{C} = \mathbf{R}_s \text{diag}(\epsilon, 1, 1) \mathbf{R}_s^T$ ($\epsilon \ll 1$, $\mathbf{R}_s$는 법선을 첫 축에 정렬하는 회전)
- 이 경우 GICP는 자동으로 plane-to-plane 정합이 된다.
- $\mathbf{C}^{\mathcal{P}} = \mathbf{0}$이면 point-to-plane, $\mathbf{C}^{\mathcal{P}} = \mathbf{C}^{\mathcal{Q}} = \mathbf{I}$이면 point-to-point가 된다.

GICP는 이론적으로 가장 일반적인 ICP 프레임워크이며, 실제로도 다양한 환경에서 가장 정확한 결과를 보인다.

```python
# GICP 핵심 반복 수도코드
def gicp(P, Q, T_init, max_iter=50, tol=1e-6):
    """
    P: source point cloud (N x 3)
    Q: target point cloud (M x 3)
    T_init: initial transformation (4 x 4)
    """
    T = T_init.copy()
    
    # 각 점의 국소 표면 공분산 사전 계산
    C_P = compute_local_covariances(P, k_neighbors=20)  # N x 3 x 3
    C_Q = compute_local_covariances(Q, k_neighbors=20)  # M x 3 x 3
    
    # Target kd-tree 구축
    tree = KDTree(Q)
    
    for iteration in range(max_iter):
        # 1. 소스 점 변환
        P_transformed = apply_transform(T, P)
        
        # 2. 최근접점 대응 찾기
        distances, indices = tree.query(P_transformed)
        
        # 3. Gauss-Newton 업데이트
        H = np.zeros((6, 6))  # Hessian approximation
        b = np.zeros(6)       # gradient
        
        for i in range(len(P)):
            j = indices[i]
            residual = P_transformed[i] - Q[j]
            
            # 결합 공분산 (변환된 좌표계)
            R = T[:3, :3]
            Sigma = C_Q[j] + R @ C_P[i] @ R.T
            Sigma_inv = np.linalg.inv(Sigma)
            
            # SE(3) 자코비안
            J = compute_se3_jacobian(T, P[i])  # 2D: 3 x 6
            
            # 누적
            H += J.T @ Sigma_inv @ J
            b += J.T @ Sigma_inv @ residual
        
        # 4. 증분 계산 및 적용
        xi = np.linalg.solve(H, -b)
        T = se3_exp(xi) @ T
        
        if np.linalg.norm(xi) < tol:
            break
    
    return T
```

### 7.1.3 NDT (Normal Distributions Transform)

[NDT (Biber & Strasser, 2003)](https://doi.org/10.1109/IROS.2003.1249285)는 점군을 직접 사용하는 대신, 공간을 복셀(voxel)로 나누고 각 복셀 내 점 분포를 가우시안으로 모델링한다.

**NDT 절차**:

1. **타겟 점군의 NDT 표현 구축**: 공간을 3D 복셀 격자로 나누고, 각 복셀 $k$ 내의 점들로부터 가우시안 $\mathcal{N}(\boldsymbol{\mu}_k, \boldsymbol{\Sigma}_k)$를 계산한다:
   $$\boldsymbol{\mu}_k = \frac{1}{n_k}\sum_{i \in k} \mathbf{q}_i, \quad \boldsymbol{\Sigma}_k = \frac{1}{n_k-1}\sum_{i \in k} (\mathbf{q}_i - \boldsymbol{\mu}_k)(\mathbf{q}_i - \boldsymbol{\mu}_k)^T$$

2. **변환 최적화**: 변환된 소스 점이 타겟 NDT 분포에서 높은 가능도(likelihood)를 가지도록 최적화:
   $$\mathbf{T}^* = \underset{\mathbf{T}}{\arg\min} \sum_i -\log p(\mathbf{T} \cdot \mathbf{p}_i \mid \boldsymbol{\mu}_{k(i)}, \boldsymbol{\Sigma}_{k(i)})$$
   
   가우시안 가정 하에:
   $$\mathbf{T}^* = \underset{\mathbf{T}}{\arg\min} \sum_i (\mathbf{T} \cdot \mathbf{p}_i - \boldsymbol{\mu}_{k(i)})^T \boldsymbol{\Sigma}_{k(i)}^{-1} (\mathbf{T} \cdot \mathbf{p}_i - \boldsymbol{\mu}_{k(i)})$$

NDT의 장점:
- 명시적 대응 찾기가 불필요 — 점이 어느 복셀에 속하는지만 판단하면 된다. kd-tree 구축 비용이 없다.
- 복셀 크기로 정밀도와 수렴 영역을 조절할 수 있다 — 큰 복셀은 넓은 수렴 영역, 작은 복셀은 높은 정밀도.
- 비용 함수가 매끄러워 최적화가 안정적이다.

단점:
- 복셀 크기 선택에 민감하다.
- 점이 적은 복셀에서 공분산 추정이 불안정하다.
- 2D NDT는 자율주행에서 많이 쓰이지만, 3D NDT는 ICP/GICP 대비 정확도가 약간 떨어지는 경향이 있다.

### 7.1.4 수렴성과 초기값 의존성

모든 정합 알고리즘은 로컬 최적화이므로, 초기값이 중요하다. 초기 포즈 오차가 크면 잘못된 로컬 미니멈에 수렴한다.

실전에서의 초기값 제공 방법:
1. **등속 모델**: 이전 두 프레임의 변환을 외삽. 가장 간단하지만, 급격한 모션에서 실패.
2. **IMU 적분**: 짧은 시간 동안의 IMU 적분이 좋은 초기값을 제공. LIO 시스템이 LO보다 강건한 이유 중 하나.
3. **멀티 해상도 (Multi-resolution)**: coarse-to-fine 접근. 먼저 큰 복셀/다운샘플링으로 대략적 정합 후, 세밀한 정합으로 정제.
4. **RANSAC + 특징 매칭**: FPFH, SHOT 등의 3D 디스크립터로 대응을 찾고 RANSAC으로 초기 변환 추정. 일반적 정합에서는 유용하지만, odometry에서는 시간적 연속성으로 인해 등속 모델 + IMU가 더 실용적.

---

## 7.2 Feature-based LiDAR Odometry

### 7.2.1 LOAM (Lidar Odometry and Mapping in Real-time)

LOAM ([Zhang & Singh, 2014](https://doi.org/10.15607/RSS.2014.X.007))은 LiDAR 오도메트리의 기준점이다. KITTI 오도메트리 벤치마크에서 오랫동안 상위권을 유지했으며, 이후 LeGO-LOAM, LIO-SAM, A-LOAM 등 수많은 후속 시스템의 기반이 되었다.

LOAM의 핵심 통찰은 두 가지다:
1. LiDAR 스캔에서 기하학적 특징(edge, planar)을 추출하면, 전체 점군 대비 훨씬 적은 점으로 정확한 정합이 가능하다.
2. 빠른 odometry와 느린 mapping을 분리하면, 실시간성과 정확도를 동시에 달성할 수 있다.

**특징점 추출**

각 스캔 라인(scan line)에서 점의 국소 곡률(curvature)을 계산한다. 점 $\mathbf{p}_i$의 곡률:

$$c_i = \frac{1}{|\mathcal{S}_i| \cdot \|\mathbf{p}_i\|} \left\| \sum_{j \in \mathcal{S}_i} (\mathbf{p}_j - \mathbf{p}_i) \right\|$$

여기서 $\mathcal{S}_i$는 같은 스캔 라인에서 $\mathbf{p}_i$의 좌우 이웃점(보통 5개씩) 집합이고, $\|\mathbf{p}_i\|$는 센서로부터의 거리(range)로 정규화하여 가까운 점과 먼 점의 곡률을 비교 가능하게 한다.

- **Edge feature**: 곡률이 높은 점 ($c_i > c_{\text{thresh}}^e$). 물리적으로 모서리, 기둥 등 날카로운 경계에 해당.
- **Planar feature**: 곡률이 낮은 점 ($c_i < c_{\text{thresh}}^p$). 물리적으로 벽, 바닥 같은 평탄한 표면에 해당.

특징점 선택 시 추가 규칙:
- 각 스캔 라인을 4개 구간으로 나누어 균등 분포를 보장한다.
- 이웃 점에 이미 선택된 점이 있으면 제외(non-maximum suppression).
- 거의 수평인 표면이나 가림(occlusion) 경계의 점은 불안정하므로 제외한다.

**Odometry 모듈 (~10Hz)**

Scan-to-scan 매칭으로 빠른 모션 추정을 수행한다. 현재 스캔의 특징점을 이전 스캔의 특징점과 대응시키되, 거리 메트릭이 특징 유형에 따라 다르다:

**Edge point-to-edge distance**: 현재 스캔의 edge 점 $\mathbf{p}$에 대해, 이전 스캔에서 가장 가까운 edge 점 두 개 $\mathbf{a}, \mathbf{b}$를 찾는다. $\mathbf{p}$에서 직선 $\overline{\mathbf{ab}}$까지의 거리:

$$d_e = \frac{\|(\mathbf{p}-\mathbf{a}) \times (\mathbf{p}-\mathbf{b})\|}{\|\mathbf{a}-\mathbf{b}\|}$$

이것은 두 벡터의 외적 크기를 밑변 길이로 나눈 것으로, 삼각형의 높이(= 점에서 직선까지의 거리)와 같다.

**Planar point-to-plane distance**: 현재 스캔의 planar 점 $\mathbf{p}$에 대해, 이전 스캔에서 가장 가까운 planar 점 세 개 $\mathbf{a}, \mathbf{b}, \mathbf{c}$를 찾는다. $\mathbf{p}$에서 평면 $\triangle\mathbf{abc}$까지의 거리:

$$d_p = \frac{(\mathbf{p}-\mathbf{a})^T \left((\mathbf{a}-\mathbf{b}) \times (\mathbf{a}-\mathbf{c})\right)}{\|(\mathbf{a}-\mathbf{b}) \times (\mathbf{a}-\mathbf{c})\|}$$

분자는 혼합곱(scalar triple product)으로, 점에서 평면까지의 부호 있는 거리에 법선 크기를 곱한 것이다.

비용 함수:

$$\mathbf{T}^* = \underset{\mathbf{T}}{\arg\min} \sum_{\mathbf{p} \in \mathcal{E}} d_e(\mathbf{T}\cdot\mathbf{p})^2 + \sum_{\mathbf{p} \in \mathcal{P}} d_p(\mathbf{T}\cdot\mathbf{p})^2$$

Levenberg-Marquardt로 최적화한다.

**Mapping 모듈 (~1Hz)**

Scan-to-map 정합으로 정밀한 포즈 보정 및 맵 업데이트를 수행한다. 누적된 글로벌 맵에 대해 새 스캔을 정합하여 드리프트를 보정한다.

Mapping 모듈은 Odometry 모듈보다 느리지만 더 정확하다. 맵은 현재 위치 주변의 큐브 형태로 유지하며, voxel 다운샘플링으로 밀도를 관리한다.

**모션 왜곡 보정 (Motion Distortion Compensation)**

회전형(spinning) LiDAR는 한 스캔을 완성하는 데 약 100ms가 걸린다. 이 동안 로봇이 움직이므로, 스캔 내 각 점은 서로 다른 시점에서 획득된다. 이 모션 왜곡을 보정하지 않으면 정합 정확도가 크게 떨어진다.

보정 방법: 스캔 시작 시점 $t_s$와 끝 시점 $t_e$ 사이의 포즈 변화 $\mathbf{T}_{s \to e}$를 알면, 각 점의 타임스탬프 $t_k$에 대해 중간 포즈를 등속 보간으로 추정한다:

$$\mathbf{T}(t_k) = \text{Exp}\left(\frac{t_k - t_s}{t_e - t_s} \cdot \text{Log}(\mathbf{T}_{s \to e})\right)$$

그리고 각 점을 기준 시점(보통 스캔 시작)으로 변환한다:

$$\mathbf{p}_k^{\text{corrected}} = \mathbf{T}(t_k)^{-1} \cdot \mathbf{p}_k$$

IMU가 있으면 IMU 적분으로 더 정확한 중간 포즈를 구할 수 있다.

```python
# LOAM 핵심 파이프라인 수도코드
class LOAM:
    def __init__(self):
        self.map_edge = VoxelMap(resolution=0.2)
        self.map_planar = VoxelMap(resolution=0.4)
        self.T_odom = np.eye(4)     # odometry 누적 변환
        self.T_map = np.eye(4)      # mapping 보정 변환
    
    def extract_features(self, scan):
        """스캔에서 edge/planar feature 추출"""
        edge_points = []
        planar_points = []
        
        for scan_line in scan.lines:
            curvatures = []
            for i in range(5, len(scan_line) - 5):
                # 좌우 5개 이웃의 벡터 합, range로 정규화
                diff = sum(scan_line[j] - scan_line[i] for j in range(i-5, i+6) if j != i)
                c = np.linalg.norm(diff) / (10 * np.linalg.norm(scan_line[i]))
                curvatures.append((c, i))
            
            # 스캔 라인을 4구간으로 나눠 균등 추출
            for sector in split_into_4(curvatures):
                sector.sort(reverse=True)  # 곡률 내림차순
                
                n_edge, n_planar = 0, 0
                for c, idx in sector:
                    if c > EDGE_THRESH and n_edge < 2:
                        if not near_selected(idx, edge_points):
                            edge_points.append(scan_line[idx])
                            n_edge += 1
                
                sector.sort()  # 곡률 오름차순
                for c, idx in sector:
                    if c < PLANAR_THRESH and n_planar < 4:
                        if not near_selected(idx, planar_points):
                            planar_points.append(scan_line[idx])
                            n_planar += 1
        
        return edge_points, planar_points
    
    def odometry(self, edge_curr, planar_curr, edge_prev, planar_prev):
        """Scan-to-scan 매칭 (10Hz)"""
        T_relative = np.eye(4)
        tree_edge = KDTree(edge_prev)
        tree_planar = KDTree(planar_prev)
        
        for iter in range(25):
            residuals = []
            jacobians = []
            
            # Edge point-to-edge 잔차
            for p in edge_curr:
                p_t = apply_transform(T_relative, p)
                _, idx = tree_edge.query(p_t, k=2)
                a, b = edge_prev[idx[0]], edge_prev[idx[1]]
                
                d_e = point_to_line_distance(p_t, a, b)
                J_e = point_to_line_jacobian(T_relative, p, a, b)
                residuals.append(d_e)
                jacobians.append(J_e)
            
            # Planar point-to-plane 잔차
            for p in planar_curr:
                p_t = apply_transform(T_relative, p)
                _, idx = tree_planar.query(p_t, k=3)
                a, b, c = planar_prev[idx[0]], planar_prev[idx[1]], planar_prev[idx[2]]
                
                d_p = point_to_plane_distance(p_t, a, b, c)
                J_p = point_to_plane_jacobian(T_relative, p, a, b, c)
                residuals.append(d_p)
                jacobians.append(J_p)
            
            # LM update
            delta = levenberg_marquardt_step(residuals, jacobians)
            T_relative = se3_exp(delta) @ T_relative
            
            if np.linalg.norm(delta) < 1e-6:
                break
        
        self.T_odom = self.T_odom @ T_relative
        return self.T_odom
    
    def mapping(self, edge_curr, planar_curr):
        """Scan-to-map 정합 (1Hz)"""
        # 현재 위치 주변의 맵 추출
        local_edge_map = self.map_edge.get_points_near(self.T_odom[:3, 3], radius=50.0)
        local_planar_map = self.map_planar.get_points_near(self.T_odom[:3, 3], radius=50.0)
        
        # Scan-to-map 최적화 (odometry와 유사하지만 맵에 대해)
        T_correction = optimize_scan_to_map(edge_curr, planar_curr, 
                                            local_edge_map, local_planar_map)
        self.T_map = T_correction @ self.T_odom
        
        # 맵 업데이트
        self.map_edge.add_points(apply_transform(self.T_map, edge_curr))
        self.map_planar.add_points(apply_transform(self.T_map, planar_curr))
```

### 7.2.2 LeGO-LOAM

LeGO-LOAM ([Shan & Englot, 2018](https://doi.org/10.1109/IROS.2018.8594299))은 LOAM에 ground segmentation을 추가하고, 계산을 경량화하여 임베디드 시스템(Jetson TX2 등)에서도 실시간 동작을 달성했다.

**LeGO-LOAM의 핵심 추가 사항**:

1. **Ground Segmentation**: 포인트 클라우드를 range image로 변환한 뒤, 지면(ground)과 비지면을 분리한다. 인접 빔 간 기울기가 10° 미만이면 지면으로 판정한다. 지면점은 planar feature로 사용하고, 비지면점에서 edge feature를 추출한다.

2. **Point Cloud Segmentation**: 비지면점에 대해 range image 기반 클러스터링을 수행한다. 일정 크기 미만의 클러스터는 노이즈로 제거한다. 이 전처리가 LOAM 대비 특징점 품질을 크게 향상시킨다.

3. **2단계 LM 최적화**: LOAM이 6-DoF를 한 번에 최적화하는 반면, LeGO-LOAM은 ground planar feature로 먼저 $[t_z, \theta_{\text{roll}}, \theta_{\text{pitch}}]$를, 그 다음 edge feature로 $[t_x, t_y, \theta_{\text{yaw}}]$를 최적화한다. 이 분리가 수렴 속도와 안정성을 높인다.

4. **포즈 그래프 최적화**: LOAM에 없던 루프 클로저 + 포즈 그래프 최적화를 추가하여 글로벌 드리프트를 보정한다.

### 7.2.3 왜 LOAM 계열이 오래 살아남았는가

LOAM이 2014년에 발표된 이후 10년이 넘었지만, LOAM 계열의 아이디어는 여전히 LiDAR 오도메트리의 주류다. 그 이유:

1. **기하학적 명확성**: edge/planar feature는 물리적으로 의미 있는 기하학적 원시체(geometric primitive)에 대응한다. 이 구조화된 환경 가정은 대부분의 인공 환경에서 잘 맞는다.

2. **계산 효율**: 전체 점군(수만~수십만 점) 대신 수백~수천 개의 특징점만 사용하므로 빠르다.

3. **확장성**: 기본 프레임워크에 IMU(LIO-SAM), GPU(KISS-ICP), 카메라(LVI-SAM) 등을 모듈식으로 추가할 수 있다.

4. **강건성**: edge/planar 분류가 일종의 아웃라이어 필터 역할을 한다 — 노이즈나 동적 물체에 속하는 점은 일관된 곡률 패턴을 보이지 않으므로 자연스럽게 제외된다.

다만, LOAM 계열의 한계도 명확하다. 기하학적 특징이 부족한 환경(넓은 들판, 긴 터널)에서는 성능이 저하되며, solid-state LiDAR의 비반복 스캔 패턴에는 기존 특징 추출이 적합하지 않다. 이 한계를 극복한 것이 FAST-LIO2의 direct 접근이다.

---

## 7.3 Tightly-Coupled LiDAR-Inertial Odometry

LiDAR만으로는 빠른 모션에서 모션 왜곡이 심해지고, 초기값 제공이 어렵다. IMU를 tightly coupled로 결합하면 이 한계를 극복할 수 있다.

### 7.3.1 LIO-SAM

LIO-SAM ([Shan et al., 2020](https://arxiv.org/abs/2007.00258))은 factor graph 프레임워크 위에 LiDAR, IMU, GPS, loop closure를 통합한 LIO 시스템이다. LOAM 계열의 feature 기반 접근과 현대적 그래프 최적화를 결합했다.

**Factor Graph 기반 통합**

LIO-SAM의 핵심 혁신은 다양한 센서 측정을 factor graph의 factor로 모델링한다는 것이다:

1. **IMU Preintegration Factor**: Forster et al. (2017)의 on-manifold preintegration으로 연속 키프레임 간 IMU 제약을 표현한다. 이 factor는 두 키프레임의 상대 회전, 속도, 위치에 대한 제약과 함께, 바이어스 추정도 포함한다.

2. **LiDAR Odometry Factor**: LOAM 스타일의 scan-to-map 매칭으로 상대 포즈를 추정한다. 이 결과를 두 키프레임 간 상대 포즈 factor로 삽입한다.

3. **GPS Factor**: GPS 수신이 가능할 때, 위치 측정을 단항(unary) factor로 추가한다. GPS가 없는 구간에서는 이 factor가 없으므로, 시스템이 자연스럽게 LiDAR+IMU만으로 동작한다.

4. **Loop Closure Factor**: 장소 인식(Scan Context 등)으로 루프를 검출하고, ICP로 상대 포즈를 추정하여 이진(binary) factor로 추가한다.

이 모든 factor가 하나의 그래프에 들어가고, GTSAM의 iSAM2로 incremental 최적화를 수행한다. Factor graph의 강점은 **모듈성**이다 — 각 센서는 독립적으로 factor를 추가/제거할 수 있으며, 새 센서를 추가하는 것이 간단하다.

**IMU 기반 De-skewing**

LIO-SAM에서 IMU는 두 가지 역할을 한다:
1. **모션 왜곡 보정**: LiDAR 스캔 동안의 IMU 데이터로 각 점의 시점별 포즈를 정밀하게 보간하여 de-skewing한다.
2. **초기값 제공**: IMU preintegration으로 다음 키프레임의 포즈를 예측하여 scan-to-map 정합의 초기값으로 사용한다.

이 양방향 결합이 LIO-SAM의 "tightly-coupled"의 핵심이다: IMU가 LiDAR에 초기값과 de-skewing을 제공하고, LiDAR가 IMU에 포즈 보정과 바이어스 추정을 제공한다.

**Keyframe 기반 효율화**

전역 맵 대신, 현재 위치 주변의 키프레임들이 관측한 서브맵에 대해 scan matching을 수행한다. 이 슬라이딩 윈도우 기반 접근이 전역 맵 대비 계산량을 크게 줄인다.

```python
# LIO-SAM Factor Graph 구성 수도코드
import gtsam

class LIOSAM:
    def __init__(self):
        self.graph = gtsam.NonlinearFactorGraph()
        self.values = gtsam.Values()
        self.isam = gtsam.ISAM2()
        self.key_idx = 0
    
    def add_keyframe(self, lidar_scan, imu_data, gps_data=None):
        # 1. IMU Preintegration Factor
        preint = gtsam.PreintegratedImuMeasurements(self.imu_params, self.current_bias)
        for imu in imu_data:
            preint.integrateMeasurement(imu.acc, imu.gyro, imu.dt)
        
        imu_factor = gtsam.ImuFactor(
            X(self.key_idx - 1), V(self.key_idx - 1),
            X(self.key_idx), V(self.key_idx),
            B(self.key_idx - 1), preint
        )
        self.graph.add(imu_factor)
        
        # 2. LiDAR Odometry Factor
        # De-skewing with IMU
        deskewed_scan = self.deskew(lidar_scan, imu_data)
        
        # Feature extraction (LOAM-style)
        edge_pts, planar_pts = extract_features(deskewed_scan)
        
        # Scan-to-submap matching
        T_lidar = scan_to_map_match(edge_pts, planar_pts, self.local_map)
        
        lidar_factor = gtsam.BetweenFactorPose3(
            X(self.key_idx - 1), X(self.key_idx),
            T_lidar, self.lidar_noise
        )
        self.graph.add(lidar_factor)
        
        # 3. GPS Factor (if available)
        if gps_data is not None:
            gps_factor = gtsam.GPSFactor(
                X(self.key_idx), gps_data.position, self.gps_noise
            )
            self.graph.add(gps_factor)
        
        # 4. Loop Closure Factor
        loop_candidate = self.detect_loop(deskewed_scan)
        if loop_candidate is not None:
            T_loop = icp_align(deskewed_scan, loop_candidate.scan)
            loop_factor = gtsam.BetweenFactorPose3(
                X(loop_candidate.key_idx), X(self.key_idx),
                T_loop, self.loop_noise
            )
            self.graph.add(loop_factor)
        
        # Initial value (from IMU prediction)
        T_predict = preint.predict(self.current_state, self.current_bias)
        self.values.insert(X(self.key_idx), T_predict.pose())
        self.values.insert(V(self.key_idx), T_predict.velocity())
        self.values.insert(B(self.key_idx), self.current_bias)
        
        # 5. iSAM2 incremental update
        result = self.isam.update(self.graph, self.values)
        self.graph.resize(0)
        self.values.clear()
        
        self.key_idx += 1
```

### 7.3.2 FAST-LIO / FAST-LIO2

FAST-LIO2 ([Xu et al., 2022](https://doi.org/10.1109/TRO.2022.3141855))는 LOAM 계열과 완전히 다른 접근을 취한다. 특징 추출을 제거하고, raw LiDAR 점을 직접 맵에 정합하는 direct LiDAR-inertial odometry이다.

**1. Direct Point Registration (특징 추출 제거)**

LOAM이 edge/planar feature를 추출하는 것과 달리, FAST-LIO2는 모든 raw 점을 직접 사용한다. 각 점 $\mathbf{p}_k$에 대해 맵에서 최근접 평면을 찾고, point-to-plane 거리를 최소화한다:

$$d_k = \mathbf{n}_k^T (\mathbf{T} \cdot \mathbf{p}_k - \mathbf{q}_k)$$

여기서 $\mathbf{n}_k$는 맵 내 최근접 평면의 법선, $\mathbf{q}_k$는 최근접점이다.

왜 특징 추출을 제거하는가?
- 특징 추출은 정보 손실이다 — 분류 임계값에 따라 유용한 점이 버려질 수 있다.
- 다양한 LiDAR 스캔 패턴(spinning, solid-state, non-repetitive)에 범용으로 적용 가능하다. 특히 Livox 같은 solid-state LiDAR는 비반복 스캔이라 기존 곡률 기반 특징 추출이 적합하지 않다.
- 충분히 효율적인 맵 자료구조(ikd-Tree)가 있으면 raw 점 정합이 real-time 가능하다.

**2. ikd-Tree (Incremental k-d Tree)**

FAST-LIO2의 두 번째 핵심 혁신은 맵 자료구조다. 기존 kd-tree는 정적이라 점 삽입/삭제에 비효율적이다. ikd-Tree는:

- **점 삽입**: $O(\log N)$ 시간에 새 점을 삽입한다.
- **점 삭제**: 맵 영역 밖의 점을 lazy delete로 효율적 제거한다.
- **동적 re-balancing**: 삽입/삭제로 인해 트리가 불균형해지면 scapegoat tree 방식으로 부분 재구축한다.
- **Box 범위 삭제**: 현재 위치에서 먼 영역의 점을 박스 단위로 삭제하여 맵 크기를 관리한다.

ikd-Tree 덕분에 FAST-LIO2는 맵을 실시간으로 유지하면서 최근접점 검색도 빠르게 수행한다.

**3. Iterated Extended Kalman Filter (IEKF)**

FAST-LIO2는 최적화 기반(LIO-SAM)이 아닌 필터 기반(IEKF)을 사용한다.

표준 EKF는 관측 모델을 한 번만 선형화하는데, LiDAR 관측의 비선형성이 크면 이 선형화가 부정확하다. IEKF는 업데이트를 여러 번 반복하여 선형화 지점을 개선한다:

$$\hat{\mathbf{x}}^{(k+1)} = \hat{\mathbf{x}}^{-} + \mathbf{K}^{(k)} (\mathbf{z} - h(\hat{\mathbf{x}}^{(k)}) - \mathbf{H}^{(k)}(\hat{\mathbf{x}}^{-} - \hat{\mathbf{x}}^{(k)}))$$

여기서 $k$는 반복 인덱스, $\hat{\mathbf{x}}^{-}$는 prediction 결과, $\mathbf{H}^{(k)}$는 $\hat{\mathbf{x}}^{(k)}$에서의 자코비안이다.

칼만 게인:
$$\mathbf{K}^{(k)} = \mathbf{P}^{-} (\mathbf{H}^{(k)})^T (\mathbf{H}^{(k)} \mathbf{P}^{-} (\mathbf{H}^{(k)})^T + \mathbf{R})^{-1}$$

IEKF는 보통 3~5회 반복으로 수렴한다. Gauss-Newton 최적화와 유사한 효과를 내지만, 불확실성(공분산)을 자연스럽게 전파한다는 필터의 장점을 유지한다.

**상태 벡터**:

$$\mathbf{x} = [{}^G\mathbf{R}_I, {}^G\mathbf{p}_I, {}^G\mathbf{v}_I, \mathbf{b}_g, \mathbf{b}_a, {}^I\mathbf{R}_L, {}^I\mathbf{p}_L, \mathbf{g}]$$

회전 ${}^G\mathbf{R}_I$, 위치 ${}^G\mathbf{p}_I$, 속도 ${}^G\mathbf{v}_I$, 자이로 바이어스 $\mathbf{b}_g$, 가속도계 바이어스 $\mathbf{b}_a$ 외에, LiDAR-IMU extrinsic ${}^I\mathbf{R}_L, {}^I\mathbf{p}_L$과 중력 벡터 $\mathbf{g}$도 포함한다. 즉, 외부 캘리브레이션과 중력 방향까지 온라인으로 추정한다.

실외 환경에서 최대 100Hz odometry + mapping을 달성하며, multi-line spinning LiDAR, solid-state LiDAR(Livox), UAV/핸드헬드 플랫폼, Intel/ARM 프로세서에서 모두 동작한다.

```cpp
// FAST-LIO2 IEKF 업데이트 수도코드 (C++)
struct State {
    Matrix3d R_GI;    // world-to-IMU rotation
    Vector3d p_GI;    // IMU position in world
    Vector3d v_GI;    // IMU velocity in world
    Vector3d bg, ba;  // gyro/accel bias
    Matrix3d R_IL;    // IMU-to-LiDAR rotation
    Vector3d p_IL;    // IMU-to-LiDAR translation
    Vector3d gravity; // gravity vector
};

void FASTLIO2::iterated_ekf_update(const PointCloud& scan, State& x, MatrixXd& P) {
    State x_predict = x;  // prediction 결과 보관
    MatrixXd K;  // 최종 반복의 칼만 게인을 루프 밖에서 사용
    MatrixXd H;  // 최종 반복의 자코비안을 루프 밖에서 사용
    int n_valid = 0;
    
    for (int iter = 0; iter < MAX_ITER; iter++) {
        // 1. 현재 상태로 점을 world frame으로 변환
        PointCloud world_pts = transform_to_world(scan, x);
        
        // 2. ikd-tree에서 각 점의 최근접 평면 검색
        vector<Plane> planes = ikd_tree.find_nearest_planes(world_pts, k=5);
        
        // 3. 관측 자코비안 및 잔차 계산
        n_valid = 0;
        H.resize(scan.size(), STATE_DIM);
        VectorXd z(scan.size());
        
        for (int i = 0; i < scan.size(); i++) {
            if (!planes[i].valid) continue;
            
            Vector3d p_w = x.R_GI * (x.R_IL * scan[i] + x.p_IL) + x.p_GI;
            
            // Point-to-plane 잔차: 관측값은 0 (점이 평면 위에 있어야 함)
            // z = 0 - h(x) = -d_k
            z(n_valid) = -planes[i].normal.dot(p_w - planes[i].center);
            
            // 자코비안: d(residual) / d(state_error)
            // ∂z/∂δθ_GI = n^T * [-(R_GI(R_IL*p + p_IL))×]
            // ∂z/∂δp_GI = n^T
            // ∂z/∂δθ_IL = n^T * R_GI * [-(R_IL*p)×]
            // ∂z/∂δp_IL = n^T * R_GI
            H.row(n_valid) = compute_jacobian(x, scan[i], planes[i]);
            n_valid++;
        }
        
        H.conservativeResize(n_valid, STATE_DIM);
        z.conservativeResize(n_valid);
        
        // 4. IEKF 업데이트
        MatrixXd S = H * P * H.transpose() + R_meas * MatrixXd::Identity(n_valid, n_valid);
        K = P * H.transpose() * S.inverse();
        
        VectorXd dx = K * (z - H * state_difference(x_predict, x));
        
        // 5. 상태 보정 (on-manifold): x^{(k+1)} = x^{-} ⊞ dx
        x = state_plus(x_predict, dx);
        
        // 수렴 확인
        if (dx.norm() < CONVERGENCE_THRESH) break;
    }
    
    // 공분산 업데이트
    MatrixXd I_KH = MatrixXd::Identity(STATE_DIM, STATE_DIM) - K * H;
    P = I_KH * P * I_KH.transpose() + K * R_meas * K.transpose();
    
    // 맵 업데이트: 정합된 점을 ikd-tree에 삽입
    PointCloud aligned = transform_to_world(scan, x);
    ikd_tree.insert(aligned);
}
```

### 7.3.3 Faster-LIO

Faster-LIO는 FAST-LIO2의 ikd-Tree를 incremental voxel 구조로 대체하여 더 빠른 처리를 달성한다.

핵심 변경: kd-tree 대신 해시 맵 기반 voxel 구조를 사용한다. 각 voxel 내에서 평면을 유지하며, 점이 추가될 때마다 평면 파라미터를 incremental하게 업데이트한다. kd-tree의 $O(\log N)$ 검색 대신 해시 $O(1)$ 접근으로 속도를 높인다.

### 7.3.4 Point-LIO

Point-LIO ([He et al., 2023](https://doi.org/10.1002/aisy.202200459))는 FAST-LIO 시리즈의 극단적 확장이다. 스캔 단위가 아닌 **개별 점** 단위로 상태를 업데이트한다.

기존 LIO는 전체 스캔(~100ms)을 하나의 관측으로 처리한다. 이 동안 등속 보간으로 모션 왜곡을 보정하지만, 고속/고각속도 모션에서는 등속 가정이 깨진다.

Point-LIO는 각 점이 도착하는 즉시 ($\sim$μs 단위) EKF 업데이트를 수행한다. IMU의 고주파(~1kHz) 측정과 LiDAR 점의 타임스탬프를 이용해, 각 점에 대응하는 정확한 IMU 상태를 사용한다.

Point-LIO의 상태 전파는 IMU 측정 사이의 짧은 시간 간격에서 다음 연속 모델을 이산화한다:

$$\frac{d}{dt}\mathbf{R} = \mathbf{R}[\boldsymbol{\omega}]_\times, \quad \frac{d}{dt}\mathbf{v} = \mathbf{R}\mathbf{a} + \mathbf{g}, \quad \frac{d}{dt}\mathbf{p} = \mathbf{v}$$

점 하나가 올 때마다 state propagation → single-point update를 수행하므로, 사실상 연속 시간(continuous-time) 필터에 근접한다.

장점: 극단적으로 빠른 모션(초당 수백 도 회전)에서도 정확한 오도메트리. 모션 왜곡 보정이 암묵적으로 이루어진다(각 점이 이미 올바른 시점의 상태로 처리되므로).

단점: 점 수에 비례하는 업데이트 횟수로 계산 부담 증가. FAST-LIO2 대비 약 2~3배 느리다.

### 7.3.5 COIN-LIO

[COIN-LIO (Pfreundschuh et al., 2024)](https://arxiv.org/abs/2310.01235)는 LiDAR-Inertial 시스템에 **카메라 intensity** 정보를 추가한다. 전통적 LIO에서 카메라를 결합하려면 별도의 visual feature 추출/추적이 필요하지만, COIN-LIO는 더 간단한 접근을 취한다:

LiDAR 점에 해당하는 카메라 픽셀의 밝기(intensity)를 기록하고, 맵 포인트에도 intensity를 할당한다. 정합 시 기하학적 거리(point-to-plane)와 함께 intensity 차이도 비용에 포함한다:

$$e_k = \alpha \cdot d_{\text{geom}}(\mathbf{p}_k) + (1-\alpha) \cdot |I_{\text{obs}}(\mathbf{p}_k) - I_{\text{map}}(\mathbf{p}_k)|$$

기하학적으로 퇴화(degenerate)된 환경 — 예를 들어 긴 터널이나 빈 홀 — 에서 intensity 정보가 추가적인 구속을 제공하여 정확도를 유지한다. 카메라의 텍스처 정보를 활용하면서도 본격적인 VIO 파이프라인의 복잡성을 피하는 실용적 접근이다.

---

## 7.4 Continuous-Time LiDAR Odometry

기존 LiDAR 오도메트리는 이산 시간(discrete-time) 모델을 사용한다. 각 스캔에 하나의 포즈를 할당하고, 스캔 내 모션은 등속 보간으로 근사한다. Continuous-time 접근은 궤적을 연속 함수로 표현하여 이 한계를 극복한다.

### 7.4.1 CT-ICP

CT-ICP ([Dellenbach et al., 2022](https://arxiv.org/abs/2109.12979))는 각 스캔에 하나의 포즈가 아닌 **두 개의 포즈**(스캔 시작과 끝)를 할당한다.

스캔 내 각 점의 타임스탬프 $t_k \in [t_s, t_e]$에 대해, 포즈를 선형 보간한다:

$$\mathbf{T}(t_k) = \mathbf{T}_s \cdot \text{Exp}\left(\frac{t_k - t_s}{t_e - t_s} \cdot \text{Log}(\mathbf{T}_s^{-1}\mathbf{T}_e)\right)$$

이 두 포즈 $\mathbf{T}_s, \mathbf{T}_e$를 동시에 최적화한다. 기존 등속 보간과 달리, 최적화 과정에서 스캔 내 모션 모델이 함께 정제된다.

CT-ICP는 IMU 없이도 모션 왜곡을 효과적으로 보정할 수 있어, IMU가 없는 시스템에서 특히 유용하다.

### 7.4.2 B-Spline 기반 궤적 표현

더 일반적인 continuous-time 접근은 B-spline으로 궤적을 표현하는 것이다. B-spline은 제어점(control point) $\{\mathbf{T}_i\}$에 의해 정의되는 매끄러운 곡선이다:

$$\mathbf{T}(t) = \prod_{i=0}^{k} \text{Exp}\left(B_i(t) \cdot \text{Log}(\mathbf{T}_{i-1}^{-1}\mathbf{T}_i)\right)$$

여기서 $B_i(t)$는 B-spline 기저 함수(basis function)다. 3차(cubic) B-spline이 주로 사용되며, $C^2$ 연속성을 보장한다.

B-spline 궤적의 핵심 이점은 **임의 시점 질의**다. 어떤 시점 $t$에서든 포즈, 속도, 가속도를 미분으로 얻을 수 있어, 비동기(asynchronous) 센서 데이터를 자연스럽게 처리한다. 물리적으로 타당한 $C^2$ 연속 궤적을 보장하며, B-spline의 국소성 덕분에 한 제어점의 변경이 전체 궤적에 파급되지 않는다.

단점:
- 제어점 간격(knot spacing)이 주요 하이퍼파라미터다. 너무 조밀하면 과적합, 너무 듬성하면 고속 모션을 표현하지 못한다.
- 이산 시간 대비 계산량이 증가한다.

Kalibr(Ch.3 참조)의 camera-IMU 캘리브레이션도 B-spline 궤적 표현을 사용한다.

---

## 7.5 Solid-State LiDAR 특화

Solid-state LiDAR(Livox 시리즈 등)는 회전형 LiDAR와 전혀 다른 스캔 패턴을 가진다.

**회전형 vs Solid-state**:

| 특성 | 회전형 (Velodyne, Ouster) | Solid-state (Livox) |
|------|--------------------------|---------------------|
| 스캔 패턴 | 반복적 (매 회전 같은 패턴) | 비반복적 (꽃잎/로즈 패턴) |
| FoV | 360° 수평 | 제한적 (70~77°) |
| 점 밀도 | 균등 | 시간에 따라 누적, 불균등 |
| 가격 | 높음 | 낮음 |
| 크기/무게 | 큼 | 작음 |

**비반복 스캔이 특징 추출에 미치는 영향**

LOAM 스타일의 곡률 기반 특징 추출은 같은 스캔 라인의 이웃점을 이용한다. 그러나 solid-state LiDAR는 정의된 스캔 라인이 없고, 점들이 비규칙적으로 분포한다. 기존 라인 기반 곡률 계산은 쓸 수 없다. KNN(K-Nearest Neighbors) 기반 국소 곡률을 쓰거나, 아예 특징 추출을 포기하고 raw 점을 그대로 써야 한다.

**FAST-LIO가 solid-state에 강한 이유**

FAST-LIO/FAST-LIO2는 raw 점을 직접 사용하므로 스캔 패턴과 무관하게 동작한다. Solid-state LiDAR는 시간이 지남에 따라 FoV를 점점 더 조밀하게 채우는데, FAST-LIO2의 ikd-Tree 맵은 이 점진적 밀집화를 자연스럽게 수용하여 맵 품질이 시간이 지날수록 향상된다. FoV가 좁아 한 스캔의 정보가 제한적이지만, IMU와의 tight coupling이 이를 보상한다.

Livox 시리즈는 가격 대비 성능이 뛰어나 드론과 핸드헬드 플랫폼으로 빠르게 퍼졌으며, FAST-LIO2 + Livox 조합은 현재 가장 널리 쓰이는 LIO 구성 중 하나다.

---

## 7.6 학습 기반 LiDAR Odometry

### 7.6.1 DeepLO 계열

학습 기반 LiDAR 오도메트리는 포인트 클라우드 쌍을 입력으로 받아 상대 포즈를 예측하는 네트워크를 훈련한다.

대표적 접근:
- **LO-Net** (Li et al., 2019): LiDAR 스캔을 2D range image로 변환하고, CNN으로 특징을 추출하여 포즈를 예측한다. 법선 추정과 마스크 예측을 보조 작업으로 추가하여 기하학적 이해를 유도한다.
- **DeepLO** (Cho et al., 2020): PointNet 기반으로 3D 점군을 직접 처리하여 포즈를 예측한다.
- **PWCLO-Net** (Wang et al., 2021): Pyramid, Warping, Cost volume 구조를 LiDAR 오도메트리에 적용한다.

### 7.6.2 현재의 한계

학습 기반 LiDAR 오도메트리는 전통적 방법 대비 아직 큰 격차가 있다. 그 이유:

1. **LiDAR 데이터의 특성**: 포인트 클라우드는 이미지와 달리 비정형(unstructured)이고 순서가 없다. CNN이 자연스럽게 처리하기 어렵다.

2. **기하학의 충분함**: ICP/GICP/NDT 같은 기하학적 방법이 이미 매우 정확하다. 카메라 영역에서 학습이 빛나는 이유 — 조명 변화, 텍스처 부족 등 기하학만으로 해결하기 어려운 문제 — 가 LiDAR에는 적용되지 않는다.

3. **데이터 부족**: 대규모 LiDAR 오도메트리 학습 데이터가 이미지 데이터에 비해 훨씬 적다.

4. **일반화**: 특정 LiDAR/환경에서 학습한 모델이 다른 LiDAR/환경에 잘 일반화되지 않는다.

현재 학습은 LiDAR 오도메트리 자체보다 보조 컴포넌트에서 더 효과적이다. 루프 클로저 검출(Scan Context, PointNetVLAD), 정합 초기값 추정(GeoTransformer, Ch.5 참조), 동적 물체 제거를 위한 시맨틱 분할 등이 그 예다.

---

## 7.7 최근 동향 (2023-2024)

위에서 다룬 시스템들 외에, 최근 주목할 만한 LiDAR 오도메트리 연구들이 있다.

**[KISS-ICP (Vizzo et al., 2023)](https://arxiv.org/abs/2209.15397)**: Point-to-point ICP가 적응적 임계값(adaptive thresholding)과 강건 커널(robust kernel), 간단한 모션 보상만으로 SOTA 수준의 성능을 달성할 수 있음을 보였다. 자동차, UAV 등 센서 타입에 무관하게 튜닝 없이 동작하는 범용성이 핵심이다. LiDAR 오도메트리에서 "단순함의 힘"을 재확인시킨 연구다.

**[MAD-ICP (Ferrari et al., 2024)](https://arxiv.org/abs/2405.05828)**: PCA 기반 kd-tree를 활용하여 포인트 클라우드의 구조적 정보를 추출하고, point-to-plane 정합에 사용한다. 데이터 매칭 전략 자체의 중요성을 강조하며, 다양한 LiDAR 센서에서 도메인 특화 방법과 동등한 성능을 달성한다.

**[iG-LIO (Chen et al., 2024)](https://github.com/zijiechenrobotics/ig_lio)**: Incremental GICP를 tightly-coupled LIO에 통합한 시스템이다. Voxel 기반 표면 공분산 추정기(VSCE)로 GICP의 공분산 계산 효율을 높이고, incremental voxel map으로 최근접점 검색 비용을 줄였다. Faster-LIO보다 효율적이면서 SOTA 수준의 정확도를 유지한다.

---

## 7장 요약

| 시스템 | 접근 | 추정 방법 | 센서 | 핵심 특징 |
|--------|------|-----------|------|-----------|
| ICP/GICP/NDT | Registration | 반복 최적화 | LiDAR only | 기본 빌딩 블록 |
| LOAM | Feature-based | LM 최적화 | LiDAR only | Edge/planar feature, 2단계 아키텍처 |
| LeGO-LOAM | Feature-based | LM 최적화 | LiDAR only | Ground segmentation, 경량화 |
| LIO-SAM | Feature-based | Factor graph (iSAM2) | LiDAR + IMU + GPS | 모듈식 다중 센서 통합 |
| FAST-LIO2 | Direct | IEKF | LiDAR + IMU | 특징 추출 없음, ikd-Tree, 100Hz |
| Point-LIO | Direct | Point-wise EKF | LiDAR + IMU | 점 단위 업데이트, 고속 모션 |
| COIN-LIO | Direct + Intensity | IEKF | LiDAR + IMU + Camera(intensity) | Intensity 기반 degeneration 방지 |
| CT-ICP | Direct | 최적화 | LiDAR only | 연속 시간 모션 모델, IMU 불필요 |
| KISS-ICP | Direct (P2P) | 반복 최적화 | LiDAR only | 적응적 임계값, 튜닝 불필요, 범용 |
| MAD-ICP | Direct (P2Plane) | 반복 최적화 | LiDAR only | PCA 기반 구조 추출, 데이터 매칭 중심 |
| iG-LIO | Direct (GICP) | IEKF | LiDAR + IMU | Incremental GICP, voxel 공분산 추정 |

LOAM(2014) → LeGO-LOAM(2018) → LIO-SAM(2020) 계보는 **feature-based + factor graph** 방향으로 진화했다. FAST-LIO(2021) → FAST-LIO2(2022) → Point-LIO(2023) 계보는 **direct + Kalman filter** 방향으로 진화했다. 두 계보는 서로 다른 설계 철학을 대표하지만, 실전 성능은 비슷한 수준에 수렴하고 있다.

Feature-based 접근은 구조화된 환경(건물, 도시)에서 강하고, direct 접근은 비구조화된 환경(숲, 동굴)과 solid-state LiDAR에서 강하다. 다음 챕터에서는 이 두 센서 모달리티(카메라 + LiDAR)를 IMU와 함께 통합하는 multi-sensor fusion 아키텍처를 다룬다.

---

# Ch.8 — Multi-Sensor Fusion 아키텍처

> 개별 odometry를 넘어 **여러 센서를 어떻게 통합하는가**의 설계론.
> 앞 챕터에서 Visual Odometry, LiDAR Odometry를 각각 다뤘다면, 이 챕터에서는 이들을 하나의 시스템으로 엮는 아키텍처를 분석한다.

---

## 8.1 Fusion 아키텍처 분류

멀티센서 퓨전 시스템을 설계할 때 가장 먼저 결정해야 하는 것은 **센서 데이터를 어느 수준에서 결합할 것인가**이다. 이 결합의 깊이에 따라 시스템의 복잡도와 성능, 실패 모드가 갈린다.

### 8.1.1 Loosely Coupled (느슨한 결합)

각 센서를 독립적인 "전문가"로 본다. 각 전문가가 자기 데이터로 독립적으로 결론을 내린 뒤, 상위 레벨에서 이 결론들을 종합한다.

구체적으로, LiDAR odometry 모듈이 LiDAR 스캔으로부터 $\mathbf{T}_{L}$을, Visual odometry 모듈이 이미지로부터 $\mathbf{T}_{V}$를 각각 독립적으로 추정하고, 상위의 fusion 모듈이 이 두 추정치를 결합한다.

$$
\hat{\mathbf{x}} = \arg\min_{\mathbf{x}} \left\| \mathbf{x} - \mathbf{x}_{LiDAR} \right\|^2_{\mathbf{P}_{L}^{-1}} + \left\| \mathbf{x} - \mathbf{x}_{Visual} \right\|^2_{\mathbf{P}_{V}^{-1}}
$$

여기서 $\mathbf{P}_{L}$, $\mathbf{P}_{V}$는 각 서브시스템이 보고하는 공분산이다.

장점은 모듈성이다. 각 센서 모듈을 독립적으로 교체하거나 업그레이드할 수 있고, 문제가 어느 모듈에서 왔는지 추적하기 용이하다. 한 센서가 실패해도 나머지가 계속 동작한다.

단점은 정보 손실이다. 각 서브시스템이 이미 정보를 압축한 상태에서 결합하므로, 센서 간 상호 보완의 이점을 최대한 활용하지 못한다. 예를 들어 LiDAR의 정밀한 기하 정보가 카메라의 스케일 모호성을 해결할 수 있지만, loosely coupled에서는 이 상호작용이 제한적이다. 각 서브시스템이 보고하는 공분산의 일관성(consistency)도 보장되지 않아, 낙관적 공분산을 보고하면 fusion 결과가 왜곡된다.

### 8.1.2 Tightly Coupled (긴밀한 결합)

모든 센서의 **원시 측정(raw measurement)**을 하나의 추정기(estimator)에 직접 넣는다. "전문가"를 두지 않고, 하나의 추정기가 모든 원본 데이터를 직접 본다.

Factor graph 관점에서, 각 센서의 raw measurement가 독립적인 factor로 삽입된다:

$$
\hat{\mathbf{x}} = \arg\min_{\mathbf{x}} \sum_{i} \left\| \mathbf{r}^{\text{IMU}}_i(\mathbf{x}) \right\|^2_{\boldsymbol{\Sigma}^{-1}_{\text{IMU}}} + \sum_{j} \left\| \mathbf{r}^{\text{LiDAR}}_j(\mathbf{x}) \right\|^2_{\boldsymbol{\Sigma}^{-1}_{\text{LiDAR}}} + \sum_{k} \left\| \mathbf{r}^{\text{cam}}_k(\mathbf{x}) \right\|^2_{\boldsymbol{\Sigma}^{-1}_{\text{cam}}}
$$

여기서 $\mathbf{r}^{\text{IMU}}_i$는 IMU preintegration 잔차, $\mathbf{r}^{\text{LiDAR}}_j$는 point-to-plane 잔차, $\mathbf{r}^{\text{cam}}_k$는 reprojection error이다.

센서 간 상호작용을 최대한 활용한다는 것이 핵심 장점이다. IMU가 LiDAR의 motion distortion을 보정하고, LiDAR가 VIO의 스케일을 잡아준다. 정보 이론적으로 최적에 가까운 융합이 가능하다.

대신 시스템 복잡도가 높다. 모든 센서의 관측 모델, 노이즈 모델, 시간 동기화를 하나의 프레임워크에서 관리해야 하고, 한 센서의 이상 데이터가 전체 추정을 오염시킬 수 있어 outlier 처리가 필수다. 실시간성 확보도 어렵다.

대표 시스템으로는 [LIO-SAM (Shan et al. 2020)](https://arxiv.org/abs/2007.00258) (LiDAR+IMU+GPS), VINS-Mono (Camera+IMU), [R3LIVE (Lin et al. 2022)](https://arxiv.org/abs/2109.07982) (Camera+LiDAR+IMU)이 있다.

### 8.1.3 Ultra-Tightly Coupled (신호 수준 결합)

센서의 측정값이 아니라 **신호 자체**를 결합한다. 가장 극단적인 통합이다.

대표적으로 GNSS-INS ultra-tight coupling이 있다. 일반적인 GNSS 수신기는 위성 신호에서 의사거리(pseudorange)를 추출한 뒤 이를 INS와 결합하지만, ultra-tight에서는 INS가 추정한 위치/속도로 GNSS 수신기의 코드/반송파 추적 루프(tracking loop)를 직접 보조한다. 이렇게 하면 약한 신호 환경(도심 캐니언, 실내 진입 직후)에서도 GNSS 신호를 더 오래 추적할 수 있다.

$$
\text{NCO frequency} = f_{\text{nominal}} + \Delta f_{\text{INS-aided}}
$$

여기서 NCO (Numerically Controlled Oscillator)의 주파수를 INS가 예측한 도플러 이동으로 보정하여, 수신기의 추적 범위를 넓힌다.

**현실**: ultra-tight coupling은 GNSS 수신기의 하드웨어/펌웨어 수준 접근이 필요하므로, 군사/항공 분야 외에는 보기 어렵다. 대부분의 로보틱스 시스템은 tightly coupled까지가 실용적 한계이다.

### 8.1.4 세 가지 수준의 비교

```
측정 흐름:

[Loosely]    센서A → 서브시스템A → 포즈A ─┐
                                          ├→ Fusion → 최종 포즈
             센서B → 서브시스템B → 포즈B ─┘

[Tightly]    센서A → raw 측정A ──┐
                                 ├→ 단일 Optimizer → 최종 포즈
             센서B → raw 측정B ──┘

[Ultra-Tight] 센서A 신호 ←→ 센서B 추정 (양방향 신호 수준 결합)
```

```python
import numpy as np
from scipy.linalg import inv

def loosely_coupled_fusion(x_lidar, P_lidar, x_visual, P_visual):
    """
    Loosely coupled fusion: 두 서브시스템의 독립적 추정치를 공분산 가중 평균으로 결합.
    
    Parameters:
        x_lidar: LiDAR 서브시스템의 상태 추정 (n,)
        P_lidar: LiDAR 추정의 공분산 (n, n)
        x_visual: Visual 서브시스템의 상태 추정 (n,)
        P_visual: Visual 추정의 공분산 (n, n)
    
    Returns:
        x_fused: 융합된 상태 추정 (n,)
        P_fused: 융합된 공분산 (n, n)
    """
    # 정보 형식(information form)으로 변환
    I_lidar = inv(P_lidar)
    I_visual = inv(P_visual)
    
    # 정보 행렬의 합산
    I_fused = I_lidar + I_visual
    P_fused = inv(I_fused)
    
    # 정보 가중 평균
    x_fused = P_fused @ (I_lidar @ x_lidar + I_visual @ x_visual)
    
    return x_fused, P_fused

# 예시: 2D 위치 추정
x_lidar = np.array([10.1, 5.2])      # LiDAR가 추정한 위치
P_lidar = np.diag([0.01, 0.01])       # LiDAR는 정밀하지만 균일
x_visual = np.array([10.0, 5.0])      # Visual이 추정한 위치
P_visual = np.diag([0.1, 0.05])       # Visual은 수직 방향이 덜 정밀

x_fused, P_fused = loosely_coupled_fusion(x_lidar, P_lidar, x_visual, P_visual)
print(f"LiDAR:  {x_lidar}, P_diag: {np.diag(P_lidar)}")
print(f"Visual: {x_visual}, P_diag: {np.diag(P_visual)}")
print(f"Fused:  {x_fused}, P_diag: {np.diag(P_fused)}")
# Fused 결과는 LiDAR 쪽에 더 가까움 (공분산이 더 작으므로)
```

---

## 8.2 Camera + LiDAR + IMU 융합

카메라, LiDAR, IMU 세 센서의 조합은 현재 자율주행과 로보틱스에서 가장 풍부한 정보를 제공하는 센서 스위트다. 카메라는 텍스처와 색상 정보를, LiDAR는 정밀한 3D 기하 정보를, IMU는 고속 관성 측정을 제공하며, 이 세 센서는 서로의 약점을 보완한다:

| 상황 | 카메라 | LiDAR | IMU |
|------|--------|-------|-----|
| 어두운 환경 | ✗ | ✓ | ✓ |
| 텍스처 없는 벽 | ✗ | ✓ | ✓ |
| 기하적 퇴화 (긴 복도) | ✓ | ✗ | ✓ |
| 고속 회전 | ✗ | ✗ | ✓ |
| 스케일 관측 | ✗ (단안) | ✓ | ✗ |
| 색상/시맨틱 | ✓ | ✗ | ✗ |

이 세 센서를 통합하는 최신 시스템들을 분석한다.

### 8.2.1 R3LIVE / R3LIVE++

R3LIVE (Lin et al., 2022)는 LiDAR-Inertial Odometry(LIO)와 Visual-Inertial Odometry(VIO) 두 서브시스템을 tightly coupled로 결합한 시스템이다.

R3LIVE는 **이중 서브시스템(dual-subsystem)** 아키텍처를 채택한다. LIO 서브시스템이 기하학적 구조(geometry)를, VIO 서브시스템이 텍스처(photometric) 정보를 담당하되, 두 시스템이 **상태를 공유**하여 tightly coupled된다.

```
LiDAR scan ──→ [LIO 서브시스템] ──→ 상태 업데이트 (기하)
                     ↓                       ↓
                 IMU data ───────────→ 공유 상태 벡터
                     ↑                       ↑
Camera image ──→ [VIO 서브시스템] ──→ 상태 업데이트 (광도)
```

LIO 서브시스템은 FAST-LIO2와 동일한 방식으로, raw LiDAR point를 ikd-Tree 기반 맵에 직접 point-to-plane 정합한다. Iterated EKF로 상태를 업데이트한다.

VIO 서브시스템이 R3LIVE의 차별점이다. 일반적인 VIO는 특징점(feature point)의 reprojection error를 최소화하지만, R3LIVE는 **photometric error**(광도 오차)를 사용한다. LIO가 구축한 3D 맵의 각 포인트에 RGB 색상을 부여하고, 새 카메라 이미지가 들어올 때 이 맵 포인트들을 이미지에 투영하여 관측된 색상과 맵에 저장된 색상의 차이를 최소화한다:

$$
\mathbf{r}^{\text{photo}}_i = \mathbf{I}(\pi(\mathbf{T}_{CW} \mathbf{p}^W_i)) - \mathbf{c}_i^{\text{map}}
$$

여기서 $\mathbf{I}(\cdot)$는 이미지의 픽셀 강도, $\pi(\cdot)$는 3D→2D 투영 함수, $\mathbf{T}_{CW}$는 월드에서 카메라로의 변환, $\mathbf{p}^W_i$는 맵 포인트의 3D 좌표, $\mathbf{c}_i^{\text{map}}$는 맵에 저장된 해당 포인트의 색상이다.

LiDAR 또는 카메라 중 하나가 일시적으로 실패하더라도 나머지 센서로 계속 동작한다. LiDAR가 가려지면 VIO+IMU로, 카메라가 어두우면 LIO+IMU로 동작한다. 그 결과 SLAM과 동시에 컬러 3D 맵을 실시간으로 생성한다.

### 8.2.2 LVI-SAM

[LVI-SAM](https://arxiv.org/abs/2104.10831) (Shan et al., 2021)은 LIO-SAM의 확장으로, Visual-Inertial 서브시스템과 LiDAR-Inertial 서브시스템을 **양방향(bidirectional)**으로 결합한다.

**양방향 결합의 핵심**:

- **VIS → LIS 방향**: Visual-Inertial 서브시스템이 추정한 포즈를 LiDAR 스캔 매칭의 초기값으로 사용. 특히 LiDAR만으로는 초기값이 부정확한 경우(고속 회전, featureless 환경)에 VIS가 초기값을 제공하여 LiDAR 정합의 수렴을 돕는다.

- **LIS → VIS 방향**: LiDAR가 추정한 깊이 정보를 Visual 특징점에 부여하여, Visual 서브시스템의 깊이 초기화를 가속한다. 단안 VIO에서는 특징점의 깊이를 삼각측량으로 추정하는데, 충분한 시차(parallax)가 쌓이기 전에는 깊이가 부정확하다. LiDAR가 이 깊이를 직접 제공함으로써 즉시 초기화가 가능해진다.

```
         ┌─── VIS 초기 포즈 ───→ LIS 초기값
         │                            │
  [Visual-Inertial]            [LiDAR-Inertial]
         │                            │
         └←── LiDAR 깊이 ────────────┘
         
              ↓ 양쪽 factor 모두 ↓
           [Factor Graph (GTSAM/iSAM2)]
                     ↓
              최종 최적화된 포즈
```

**Factor Graph 설계**: LVI-SAM의 factor graph에는 다음 factor들이 삽입된다:
- IMU preintegration factor (연속 키프레임 사이)
- LiDAR odometry factor (scan matching 결과)
- Visual odometry factor (feature tracking 결과)
- GPS factor (가용 시)
- Loop closure factor (재방문 탐지 시)

### 8.2.3 FAST-LIVO / FAST-LIVO2

[FAST-LIVO2](https://arxiv.org/abs/2408.14035) (Zheng et al., 2024)는 FAST-LIO2 팀(HKU MARS Lab)이 개발한 Camera+LiDAR+IMU 직접(direct) 융합 시스템이다. "직접"이란 특징점 추출 없이 raw 데이터를 직접 사용한다는 뜻이다.

**설계 1 — 순차적 업데이트(Sequential Update)**:

이종 센서의 측정은 차원이 다르다. LiDAR는 3D point-to-plane 잔차를, 카메라는 2D photometric 잔차를 제공한다. 이들을 하나의 큰 잔차 벡터로 쌓아서 동시에 최적화하면 Jacobian 행렬의 구조가 복잡해지고 수치적으로 불안정해질 수 있다.

FAST-LIVO2는 이 문제를 **순차적 베이지안 업데이트**로 해결한다:

1. IMU로 상태를 예측 (prediction)
2. LiDAR 측정으로 상태를 업데이트 (1차 업데이트)
3. 카메라 측정으로 상태를 다시 업데이트 (2차 업데이트)

이론적으로, 측정이 독립이라면 순차적 업데이트는 동시 업데이트와 동일한 결과를 준다:

$$
p(\mathbf{x} | \mathbf{z}_L, \mathbf{z}_C) = p(\mathbf{x} | \mathbf{z}_C, \mathbf{z}_L) \propto p(\mathbf{z}_C | \mathbf{x}) \cdot p(\mathbf{z}_L | \mathbf{x}) \cdot p(\mathbf{x})
$$

순차적으로 하면:
$$
\underbrace{p(\mathbf{x} | \mathbf{z}_L)}_{\text{LiDAR 업데이트 후}} \propto p(\mathbf{z}_L | \mathbf{x}) \cdot p(\mathbf{x})
$$
$$
\underbrace{p(\mathbf{x} | \mathbf{z}_L, \mathbf{z}_C)}_{\text{카메라 업데이트 후}} \propto p(\mathbf{z}_C | \mathbf{x}) \cdot p(\mathbf{x} | \mathbf{z}_L)
$$

두 번째 식에서 $p(\mathbf{x} | \mathbf{z}_L)$이 prior 역할을 하며, 최종 결과는 동시 업데이트와 수학적으로 동등하다.

**설계 2 — 통합 적응형 복셀 맵**:

FAST-LIVO2는 해시 테이블 + 옥트리 기반의 단일 복셀 맵을 사용한다. LiDAR 모듈이 기하학적 구조(3D 좌표, 법선 벡터)를 구축하면, Visual 모듈이 같은 맵 포인트에 이미지 패치를 부착한다. 이렇게 하면 기하와 텍스처가 하나의 맵에서 일관되게 관리된다.

**설계 3 — LiDAR 법선 활용 어파인 워핑**:

카메라의 direct method에서 이미지 패치를 비교할 때, 표면의 기울기를 고려한 어파인 워핑이 정확도를 높인다. FAST-LIVO2는 LiDAR에서 추출한 평면 법선 벡터를 활용하여, 별도의 법선 추정 없이 정확한 어파인 워핑을 수행한다. 이것이 LiDAR-카메라 상호 보완의 구체적 예이다.

**설계 4 — 실시간 노출 보정**:

조명이 빠르게 변하는 환경(터널 진입·탈출)에서, FAST-LIVO2는 노출 시간(exposure time)을 온라인으로 추정해 photometric error를 보정한다.

```python
import numpy as np

def sequential_ekf_update(x_pred, P_pred, z_lidar, H_lidar, R_lidar, z_cam, H_cam, R_cam):
    """
    순차적 EKF 업데이트: LiDAR → Camera 순서.
    동시 업데이트와 수학적으로 동등하지만, 차원 불일치 문제를 회피.
    
    Parameters:
        x_pred: 예측 상태 (n,)
        P_pred: 예측 공분산 (n, n)
        z_lidar: LiDAR 측정 (m_L,)
        H_lidar: LiDAR 관측 자코비안 (m_L, n)
        R_lidar: LiDAR 측정 노이즈 (m_L, m_L)
        z_cam: 카메라 측정 (m_C,)
        H_cam: 카메라 관측 자코비안 (m_C, n)
        R_cam: 카메라 측정 노이즈 (m_C, m_C)
    
    Returns:
        x_updated: 최종 업데이트된 상태
        P_updated: 최종 업데이트된 공분산
    """
    # Step 1: LiDAR 업데이트
    S_L = H_lidar @ P_pred @ H_lidar.T + R_lidar
    K_L = P_pred @ H_lidar.T @ np.linalg.inv(S_L)
    y_L = z_lidar - H_lidar @ x_pred  # innovation
    x_after_lidar = x_pred + K_L @ y_L
    P_after_lidar = (np.eye(len(x_pred)) - K_L @ H_lidar) @ P_pred
    
    # Step 2: Camera 업데이트 (LiDAR 업데이트 결과를 prior로 사용)
    S_C = H_cam @ P_after_lidar @ H_cam.T + R_cam
    K_C = P_after_lidar @ H_cam.T @ np.linalg.inv(S_C)
    y_C = z_cam - H_cam @ x_after_lidar  # innovation
    x_updated = x_after_lidar + K_C @ y_C
    P_updated = (np.eye(len(x_pred)) - K_C @ H_cam) @ P_after_lidar
    
    return x_updated, P_updated
```

### 8.2.4 Multimodal Factor Graph 설계 비교

세 시스템의 설계를 factor graph 관점에서 비교한다:

| 측면 | R3LIVE | LVI-SAM | FAST-LIVO2 |
|------|--------|---------|------------|
| 백엔드 | IEKF (dual subsystem) | iSAM2 (factor graph) | IEKF (sequential) |
| LiDAR 처리 | Direct (point-to-plane) | Feature-based (LOAM) | Direct (point-to-plane) |
| 카메라 처리 | Direct (photometric) | Feature-based (ORB) | Direct (photometric) |
| 맵 표현 | ikd-Tree + RGB | Voxel map | Hash+Octree voxel map |
| 특징 추출 | 불필요 | 필요 (edge/planar, ORB) | 불필요 |
| GPS 통합 | 없음 | Factor로 통합 | 없음 |
| Loop closure | 없음 | Factor로 통합 | 없음 |
| 임베디드 지원 | 제한적 | 제한적 | ARM 실시간 가능 |

**선택 기준**:
- Loop closure와 GPS가 필요하면: LVI-SAM
- 최고 정밀도의 colored map이 필요하면: R3LIVE
- 임베디드 플랫폼에서 실시간이 필요하면: FAST-LIVO2
- 특징점이 부족한 환경(텍스처 없는 벽, 구조물 내부)에서: direct 방식 (R3LIVE, FAST-LIVO2)

---

## 8.3 GNSS 통합

GNSS (Global Navigation Satellite System)는 전역적 위치 참조(global position reference)를 제공하는 유일한 센서이다. IMU+LiDAR+카메라가 아무리 정밀해도, 이들은 모두 **상대적(relative)** 측정을 제공할 뿐이므로, 장시간 주행하면 드리프트가 누적된다. GNSS는 이 드리프트를 교정하는 앵커 역할을 한다.

### 8.3.1 GNSS Factor in Factor Graph (LIO-SAM 방식)

LIO-SAM (Shan et al. 2020)은 GNSS를 factor graph에 통합하는 방법을 보여준다. GNSS 수신기가 위치를 보고하면, 이를 **unary factor**로 포즈 노드에 연결한다:

$$
\mathbf{r}^{\text{GPS}}_i = \mathbf{T}^{-1}_{\text{ENU→map}} \cdot \mathbf{p}^{\text{ENU}}_{\text{GPS}} - \mathbf{p}^{\text{map}}_i - \mathbf{R}^{\text{map}}_i \cdot \mathbf{l}_{\text{antenna}}
$$

여기서:
- $\mathbf{p}^{\text{ENU}}_{\text{GPS}}$는 GNSS가 보고한 ENU 좌표
- $\mathbf{T}_{\text{ENU→map}}$은 ENU 좌표계에서 SLAM 맵 좌표계로의 변환
- $\mathbf{p}^{\text{map}}_i$는 SLAM이 추정한 로봇 위치
- $\mathbf{l}_{\text{antenna}}$는 GNSS 안테나와 로봇 body frame 사이의 lever arm 벡터
- $\mathbf{R}^{\text{map}}_i$는 로봇의 회전

**좌표계 정렬 문제**: SLAM의 로컬 맵 좌표계와 GNSS의 글로벌 좌표계(WGS84/ENU)는 다르다. 첫 번째 GNSS 수신 시점에서 ENU 원점을 설정하고, 초기 포즈들을 이용하여 맵↔ENU 변환을 추정해야 한다. 이 변환은 6-DoF (3 translation + 3 rotation)이지만, IMU가 중력 방향을 제공하므로 실제로는 4-DoF (yaw + 3 translation)만 추정하면 된다.

### 8.3.2 Loosely vs Tightly Coupled GNSS

**Loosely Coupled GNSS-INS**:
GNSS 수신기가 이미 계산한 위치/속도 해(PVT solution)를 EKF로 IMU 추정치와 결합한다. 대부분의 상용 시스템이 이 방식이다.

```python
def gnss_loose_coupling_ekf_update(x_ins, P_ins, gnss_position, R_gnss):
    """
    Loosely coupled GNSS-INS: GNSS PVT 솔루션으로 INS 상태 보정.
    
    x_ins: INS 상태 [position(3), velocity(3), attitude(3), biases(6)] = 15차원
    gnss_position: GNSS가 계산한 위치 (3,)
    R_gnss: GNSS 위치의 공분산 (3, 3) — 보통 HDOP * sigma_uere
    """
    n = len(x_ins)
    # 관측 행렬: GNSS는 위치만 관측
    H = np.zeros((3, n))
    H[0:3, 0:3] = np.eye(3)  # position 부분만 관측
    
    # Innovation
    y = gnss_position - H @ x_ins
    
    # Kalman gain
    S = H @ P_ins @ H.T + R_gnss
    K = P_ins @ H.T @ np.linalg.inv(S)
    
    # Update
    x_updated = x_ins + K @ y
    P_updated = (np.eye(n) - K @ H) @ P_ins
    
    return x_updated, P_updated
```

**Tightly Coupled GNSS-INS**:
GNSS 수신기의 PVT 솔루션이 아니라, 원시 의사거리(pseudorange)와 도플러(Doppler) 측정을 직접 사용한다. 각 위성에 대한 의사거리를 개별 factor로 삽입한다:

$$
\rho_i = \| \mathbf{p}_{\text{sat},i} - \mathbf{p}_{\text{rx}} \| + c \cdot \delta t_{\text{rx}} + I_i + T_i + \epsilon_i
$$

여기서 $\rho_i$는 위성 $i$에 대한 의사거리, $c \cdot \delta t_{\text{rx}}$는 수신기 시계 바이어스, $I_i$와 $T_i$는 전리층/대류층 지연이다.

Tightly coupled의 장점은, 위성이 4개 미만이어서 GNSS 자체적으로는 해를 구할 수 없는 상황에서도, 가용한 위성의 의사거리를 여전히 활용할 수 있다는 점이다. 도심 환경에서 건물에 의해 위성이 가려지는 경우가 빈번하므로, 이 장점은 실질적으로 매우 크다.

### 8.3.3 GNSS-Denied → GNSS-Available 전환 처리

실제 로봇 운행에서는 GNSS 신호가 수시로 끊기고 복원된다 (터널, 지하주차장, 고가도로 아래). 이 전환을 안정적으로 처리하는 것이 시스템 설계의 핵심 과제이다.

**전환 시 주의점**:
1. **좌표계 점프(coordinate jump) 방지**: GNSS 복원 직후 GNSS 위치와 IMU/LiDAR 추정 위치 사이에 큰 차이가 있을 수 있다. 이를 갑자기 교정하면 맵에 불연속이 생긴다. 해결책은 GNSS 불확실성을 초기에 크게 설정하고 점진적으로 줄이는 것이다.

2. **GNSS 품질 검증**: GNSS 복원 후 초기 몇 초의 측정은 multipath 등으로 정확도가 떨어질 수 있다. PDOP/HDOP, 위성 수, carrier phase 상태 등을 확인하여 충분히 신뢰할 수 있을 때만 factor에 포함한다.

3. **맵 좌표계 보정**: GNSS 장기 부재 후 드리프트가 누적되었다면, 복원 시 맵 좌표계 자체를 보정해야 할 수 있다. 이는 loop closure와 유사한 포즈 그래프 최적화로 처리한다.

---

## 8.4 Radar 퓨전

### 8.4.1 Radar의 재조명

전통적으로 자동차 레이더는 해상도가 낮아 SLAM/odometry 용으로는 적합하지 않다고 여겨졌다. 그러나 **4D imaging radar**의 등장으로 그 평가가 달라지고 있다.

**4D Radar란**: 기존 자동차 레이더가 거리(range), 속도(Doppler), 방위각(azimuth) 3가지를 측정했다면, 4D imaging radar는 여기에 **고도각(elevation)**을 추가하여 3D 포인트 클라우드를 생성한다. 해상도는 LiDAR에 비할 바가 아니지만(수백~수천 점 vs 수십만 점), 독보적인 장점들이 있다.

**Radar의 고유한 장점**:

1. **악천후 관통**: 비, 눈, 안개, 먼지를 관통한다. LiDAR(905nm/1550nm 레이저)는 이런 조건에서 성능이 크게 저하되지만, 레이더(mm-wave)는 영향을 거의 받지 않는다. 자율주행 안전성 관점에서 이 차이는 중요하다.

2. **직접 속도 측정**: FMCW (Frequency-Modulated Continuous Wave) 레이더는 도플러 효과를 이용하여 물체의 **상대 속도를 직접 측정**한다. 카메라나 LiDAR는 연속 프레임 비교로 속도를 간접 추정해야 하지만, 레이더는 단일 측정에서 속도를 얻는다.

3. **저렴한 비용**: 자동차 레이더 칩셋은 대량 생산으로 LiDAR 대비 한 자릿수 이상 저렴하다.

### 8.4.2 Radar Odometry

4D radar를 이용한 odometry는 2022년 이후 논문 수가 빠르게 늘고 있는 분야이다. 핵심 아이디어는 radar의 도플러 측정을 ego-motion 추정에 직접 활용하는 것이다.

FMCW radar의 각 측정점은 $(r, \theta, \phi, v_d)$ — 거리, 방위각, 고도각, 도플러 속도 — 를 제공한다. 로봇의 선속도 $\mathbf{v}$와 각속도 $\boldsymbol{\omega}$가 주어지면, 특정 방향 $\mathbf{d}_i = [\cos\phi_i \cos\theta_i, \cos\phi_i \sin\theta_i, \sin\phi_i]^T$의 점에서 관측되는 도플러 속도는:

$$
v_{d,i} = -\mathbf{d}_i^T (\mathbf{v} + \boldsymbol{\omega} \times \mathbf{p}_i) + n_i
$$

여기서 $\mathbf{p}_i = r_i \mathbf{d}_i$는 점의 3D 위치이다. 정적 점들만 사용하면 (움직이는 물체 제거 후), 이 방정식들의 집합으로부터 $(\mathbf{v}, \boldsymbol{\omega})$를 추정할 수 있다.

```python
import numpy as np

def radar_ego_velocity(radar_points, doppler_velocities):
    """
    Radar 도플러 측정으로부터 ego-velocity 추정.
    
    radar_points: (N, 3) — 각 점의 3D 좌표 (r*d_i)
    doppler_velocities: (N,) — 각 점의 관측 도플러 속도
    
    Returns:
        v_ego: (3,) — ego linear velocity
    """
    # 각 점의 방향 벡터 (단위 벡터)
    norms = np.linalg.norm(radar_points, axis=1, keepdims=True)
    directions = radar_points / (norms + 1e-8)  # (N, 3)
    
    # v_d = -d^T @ v_ego  (간단한 경우: 각속도 무시)
    # => A @ v_ego = b, 여기서 A = -directions, b = doppler_velocities
    A = -directions
    b = doppler_velocities
    
    # RANSAC으로 동적 물체 제거 후 최소자승
    # 간단한 버전: 전체 데이터로 최소자승
    v_ego, residuals, _, _ = np.linalg.lstsq(A, b, rcond=None)
    
    return v_ego
```

### 8.4.3 4D Radar + Camera Fusion

4D radar와 카메라의 조합은 "LiDAR-free" 자율주행의 유력한 대안으로 주목받고 있다. 두 센서의 상보성은 다음과 같다:

| 특성 | 카메라 | 4D Radar |
|------|--------|----------|
| 해상도 | 매우 높음 | 낮음 |
| 악천후 | 취약 | 강건 |
| 직접 깊이 측정 | ✗ | ✓ |
| 직접 속도 측정 | ✗ | ✓ |
| 시맨틱 이해 | 강함 | 약함 |
| 비용 | 매우 저렴 | 저렴 |

Fusion 접근법:
- **Early fusion**: Radar 포인트를 이미지에 투영하여 sparse depth cue로 활용. Mono depth estimation의 스케일 앵커로 사용.
- **Mid-level fusion**: 카메라 특징과 radar 특징을 네트워크 내부에서 결합. BEV (Bird's Eye View) 공간에서의 융합이 일반적.
- **Late fusion**: 각 센서로 독립적으로 물체 검출 후 결과를 결합.

### 8.4.4 Boreas 벤치마크

[Boreas](https://arxiv.org/abs/2203.10168) (Burnett et al., 2023)는 다양한 기상 조건(맑음, 비, 눈)에서 수집된 다중 센서 데이터셋으로, 특히 radar odometry의 벤치마킹에 중요하다. 카메라, LiDAR(Velodyne Alpha Prime), 4D radar(Navtech CIR304-H)를 동시에 탑재하고, 같은 경로를 다른 시간/계절에 반복 주행하여 long-term localization 연구에도 활용된다.

---

## 8.5 Multi-Robot / Decentralized Fusion

단일 로봇의 퓨전을 넘어, **여러 로봇이 협력적으로 환경을 인식**하는 문제는 난이도가 한 단계 더 높다. 통신 제약, 상대적 참조 프레임의 부재, 데이터 연관(data association)의 어려움이 추가되기 때문이다.

### 8.5.1 Multi-Robot SLAM의 핵심 과제

1. **상대 포즈 추정(Inter-Robot Relative Pose)**: 각 로봇은 자기만의 로컬 좌표계에서 SLAM을 수행한다. 두 로봇의 맵을 병합하려면 먼저 이들의 상대 좌표 변환을 알아야 한다. 이는 cross-robot place recognition + geometric verification으로 해결한다.

2. **통신 제약(Communication Constraint)**: 전체 맵이나 raw 센서 데이터를 전송하는 것은 대역폭 문제로 불가능한 경우가 많다. **어떤 정보를 압축하여 공유할 것인가**가 핵심 설계 결정이다.

3. **분산 최적화(Distributed Optimization)**: 중앙 서버에 모든 데이터를 모아 최적화하면 통신 병목과 단일 실패점(single point of failure) 문제가 생긴다. 각 로봇이 로컬 최적화를 하면서 이웃 로봇과 제한된 정보만 교환하는 분산 방식이 낫다.

### 8.5.2 Kimera-Multi

[Kimera-Multi](https://arxiv.org/abs/2106.14386) (Rosinol et al., 2021)는 MIT의 SPARK Lab에서 개발한 분산 멀티로봇 SLAM 시스템이다.

**아키텍처**:
- 각 로봇이 Kimera를 실행하여 로컬 metric-semantic SLAM 수행
- 로봇 간 조우(rendezvous) 시, **DBoW2** 기반 inter-robot place recognition으로 공통 장소를 탐지
- 탐지된 inter-robot loop closure를 분산 포즈 그래프 최적화에 반영
- **GNC (Graduated Non-Convexity)** 솔버로 이상치 loop closure를 걸러낸다

**분산 최적화**: 각 로봇이 자신의 포즈 그래프를 유지하면서 이웃 로봇과 inter-robot factor만 교환한다. Riemannian block-coordinate descent 등의 분산 최적화 알고리즘으로 수렴한다.

### 8.5.3 Swarm-SLAM

[Swarm-SLAM](https://arxiv.org/abs/2301.06230) (Lajoie et al., 2024)은 대규모 로봇 군집(swarm)을 위한 분산 SLAM으로, 통신 효율에 집중한다.

**핵심 설계**:
- **Place Recognition Descriptor 교환**: 전체 맵이 아니라 장소 인식 디스크립터(NetVLAD, Scan Context 등)만 교환하여 대역폭을 최소화
- **Inter-Robot Loop Closure**: 디스크립터 매칭으로 후보를 찾고, 최소한의 기하학적 정보(특징점 or 포인트 클라우드)만 교환하여 검증
- **인접 로봇 간 피어투피어 통신**: 중앙 서버 없이 인접 로봇 간 직접 통신
- **LiDAR/Visual/Multimodal 지원**: 카메라만, LiDAR만, 또는 혼합 센서 구성의 로봇들이 동시에 참여 가능

```python
# 분산 포즈 그래프 최적화의 개념적 구현
import numpy as np

class DistributedPoseGraphNode:
    """
    분산 포즈 그래프의 단일 로봇 노드.
    각 로봇은 자신의 로컬 그래프를 유지하고,
    이웃 로봇과 inter-robot factor만 교환한다.
    """
    def __init__(self, robot_id):
        self.robot_id = robot_id
        self.local_poses = []           # 자체 포즈 (로컬 좌표계)
        self.local_factors = []          # 로컬 odometry factor
        self.inter_robot_factors = []    # 다른 로봇과의 loop closure factor
        self.neighbor_info = {}          # 이웃 로봇으로부터 받은 경계 정보
    
    def add_odometry(self, delta_pose, covariance):
        """로컬 odometry factor 추가"""
        self.local_factors.append({
            'type': 'odom',
            'from': len(self.local_poses) - 1,
            'to': len(self.local_poses),
            'measurement': delta_pose,
            'covariance': covariance
        })
        self.local_poses.append(self.local_poses[-1] @ delta_pose)
    
    def add_inter_robot_factor(self, other_robot_id, other_pose_idx, 
                                relative_pose, covariance):
        """다른 로봇과의 loop closure factor 추가"""
        self.inter_robot_factors.append({
            'type': 'inter_robot',
            'robot': other_robot_id,
            'local_idx': len(self.local_poses) - 1,
            'remote_idx': other_pose_idx,
            'measurement': relative_pose,
            'covariance': covariance
        })
    
    def exchange_boundary_info(self, neighbor_node):
        """
        이웃 노드와 경계 정보(boundary variable의 추정치와 공분산)를 교환.
        전체 맵이 아니라 inter-robot factor에 관련된 변수만 교환.
        """
        boundary_poses = []
        for factor in self.inter_robot_factors:
            if factor['robot'] == neighbor_node.robot_id:
                idx = factor['local_idx']
                boundary_poses.append({
                    'idx': idx,
                    'pose': self.local_poses[idx]
                })
        neighbor_node.neighbor_info[self.robot_id] = boundary_poses
```

---

## 8.6 시스템 설계 실전

이론과 알고리즘을 넘어, 실제 멀티센서 퓨전 시스템을 설계하고 배포할 때 직면하는 실전적 문제들을 다룬다.

### 8.6.1 Sensor Suite 선정 가이드

센서 스위트의 선정은 **운용 환경(operational environment)**에 의해 결정된다:

| 환경 | 권장 최소 구성 | 선택 추가 센서 |
|------|---------------|---------------|
| 실내 (사무실/창고) | Camera + IMU | LiDAR (정밀 매핑 시) |
| 도심 자율주행 | Camera + LiDAR + IMU + GNSS | 4D Radar, Wheel Odom |
| 비포장/야외 | LiDAR + IMU + GNSS | Camera (시맨틱), Radar |
| 지하/터널 | LiDAR + IMU | Camera, UWB |
| 수중 | IMU + DVL (Doppler Velocity Log) | Sonar, Pressure |
| 항공/드론 | Camera + IMU + GNSS | LiDAR (매핑 시) |
| 악천후 (비/눈) | Radar + IMU | Camera, LiDAR |

**예산별 구성 예시**:
- **$500 이하**: Stereo Camera + IMU (Intel RealSense D435i)
- **$2,000 이하**: + 2D LiDAR (RPLidar)
- **$10,000 이하**: + 3D LiDAR (Livox Mid-360) + GNSS RTK
- **$30,000+**: Multi-LiDAR + Multi-Camera + 4D Radar + GNSS RTK

### 8.6.2 Timing Architecture (시간 동기화 설계)

멀티센서 시스템에서 **시간 동기화**는 정확도를 좌우하는 결정적 요소이다. 100km/h로 이동하는 차량에서 1ms의 시간 오차는 약 2.8cm의 위치 오차에 해당한다.

**하드웨어 동기화 (Hardware Sync)**:

가장 정밀한 방법은 하드웨어 트리거를 사용하는 것이다:

- **PPS (Pulse Per Second)**: GNSS 수신기가 1초마다 정밀한 펄스를 출력. 이 펄스를 다른 센서의 동기화 입력에 연결. 정밀도: ~50ns.
- **PTP (Precision Time Protocol, IEEE 1588)**: 이더넷 기반 시간 동기화. LiDAR(Velodyne, Ouster 등)가 지원. 정밀도: ~μs.
- **외부 트리거**: 마이크로컨트롤러가 카메라 셔터 트리거와 IMU 타임스탬프 캡처를 동시에 수행.

**소프트웨어 동기화 (Software Sync)**:

하드웨어 동기화가 불가능한 경우, 소프트웨어적으로 시간 오프셋을 추정한다:

- **Kalibr 방식**: B-spline 궤적으로 연속 시간 궤적을 표현하고, 센서 간 시간 오프셋을 최적화 변수로 포함하여 동시 추정.
- **상관 기반**: 두 센서의 운동 추정 결과 사이의 상호상관(cross-correlation)을 계산하여 시간 지연을 추정.

$$
\hat{\tau} = \arg\max_{\tau} \int \mathbf{a}_{\text{IMU}}(t) \cdot \dot{\mathbf{v}}_{\text{camera}}(t + \tau) \, dt
$$

```python
import numpy as np
from scipy.signal import correlate

def estimate_time_offset(timestamps_a, signal_a, timestamps_b, signal_b, max_offset_ms=100):
    """
    두 센서 신호의 상호상관으로 시간 오프셋 추정.
    
    예: IMU 각속도 vs 카메라 프레임 간 회전률
    """
    # 공통 타임라인에 리샘플링 (1kHz)
    dt = 0.001  # 1ms
    t_common = np.arange(
        max(timestamps_a[0], timestamps_b[0]),
        min(timestamps_a[-1], timestamps_b[-1]),
        dt
    )
    sig_a = np.interp(t_common, timestamps_a, signal_a)
    sig_b = np.interp(t_common, timestamps_b, signal_b)
    
    # 평균 제거
    sig_a -= np.mean(sig_a)
    sig_b -= np.mean(sig_b)
    
    # 상호상관
    correlation = correlate(sig_a, sig_b, mode='full')
    lags = np.arange(-len(sig_b) + 1, len(sig_a)) * dt
    
    # max_offset 범위 내에서 최대 상관 찾기
    mask = np.abs(lags) <= max_offset_ms / 1000
    valid_corr = correlation[mask]
    valid_lags = lags[mask]
    
    best_idx = np.argmax(valid_corr)
    estimated_offset = valid_lags[best_idx]
    
    return estimated_offset  # 초 단위

# 예시: IMU gyro Z축 vs Camera rotation rate
# offset = estimate_time_offset(imu_times, gyro_z, cam_times, cam_rotation_rate)
```

### 8.6.3 Failure Mode와 Degradation Handling

실제 시스템에서 센서는 반드시 실패한다. 강건한 시스템은 **graceful degradation** — 즉, 한 센서가 실패해도 성능이 다소 저하되면서 나머지 센서로 계속 동작하는 것 — 을 달성해야 한다.

**주요 실패 모드와 대응**:

| 실패 모드 | 증상 | 탐지 방법 | 대응 |
|-----------|------|-----------|------|
| 카메라 과노출/저노출 | 이미지 전체가 밝거나 어두움 | 히스토그램 분석 | 카메라 factor 비활성화, LIO만으로 동작 |
| LiDAR 기하 퇴화 (degenerate) | 긴 복도, 넓은 평지 | 정보 행렬의 고유값 분석 | 해당 DoF의 LiDAR 구속 완화, VIO로 보완 |
| IMU 포화 | 고속 충격 시 측정 범위 초과 | ADC 최대값 탐지 | 해당 시간대 IMU preintegration 불확실성 증가 |
| GNSS multipath | 건물 반사로 인한 큰 오차 | RAIM, 잔차 검사 | 해당 GNSS factor의 공분산 증가 또는 제거 |
| 센서 완전 단절 | 데이터 수신 없음 | Watchdog timer | 해당 센서의 모든 factor 비활성화 |

**LiDAR 기하 퇴화 탐지**:

LiDAR scan matching에서 정보 행렬(Hessian) $\mathbf{H} = \mathbf{J}^T \mathbf{J}$의 고유값 분석으로 기하 퇴화를 탐지할 수 있다. 만약 한 방향의 고유값이 다른 방향들에 비해 현저히 작으면, 그 방향으로의 구속이 약하다는 뜻이다.

$$
\mathbf{H} = \mathbf{U} \boldsymbol{\Lambda} \mathbf{U}^T, \quad \lambda_{\min} / \lambda_{\max} < \epsilon \Rightarrow \text{degenerate}
$$

예를 들어, 긴 복도에서 복도 축 방향의 구속이 약해지므로, 그 방향의 LiDAR 구속을 완화하고 카메라의 optical flow로 보완한다.

```python
import numpy as np

def check_lidar_degeneracy(jacobian, threshold=0.01):
    """
    LiDAR scan matching의 Hessian 고유값으로 기하 퇴화 검사.
    
    jacobian: (m, 6) — m개 point-to-plane 잔차의 6-DoF 자코비안
    threshold: 최소/최대 고유값 비의 임계값
    
    Returns:
        is_degenerate: bool
        degenerate_directions: (k, 6) — 퇴화된 방향의 고유벡터
        eigenvalues: (6,) — 정보 행렬의 고유값
    """
    # 정보 행렬 (근사 Hessian)
    H = jacobian.T @ jacobian
    
    # 고유값 분해
    eigenvalues, eigenvectors = np.linalg.eigh(H)
    
    # 고유값 비율 검사
    ratio = eigenvalues / (eigenvalues.max() + 1e-10)
    degenerate_mask = ratio < threshold
    
    is_degenerate = np.any(degenerate_mask)
    degenerate_directions = eigenvectors[:, degenerate_mask].T
    
    if is_degenerate:
        print(f"[경고] 기하 퇴화 감지!")
        print(f"  고유값: {eigenvalues}")
        print(f"  퇴화 방향 수: {degenerate_mask.sum()}")
    
    return is_degenerate, degenerate_directions, eigenvalues

def adaptive_fusion_weight(lidar_eigenvalues, camera_track_quality, 
                            lidar_min_eig_threshold=100.0):
    """
    LiDAR 퇴화 정도에 따라 카메라 가중치를 적응적으로 조절.
    """
    min_eig = lidar_eigenvalues.min()
    
    if min_eig > lidar_min_eig_threshold:
        # LiDAR 충분히 구속됨 → 기본 가중치
        lidar_weight = 1.0
        camera_weight = 0.3
    else:
        # LiDAR 퇴화 → 카메라 가중치 증가
        decay = min_eig / lidar_min_eig_threshold
        lidar_weight = decay
        camera_weight = 1.0
    
    return lidar_weight, camera_weight
```

### 8.6.5 최근 주목할 연구 (2024-2025)

- **[Gaussian-LIC (Lang et al., ICRA 2025)](https://arxiv.org/abs/2404.06926)**: 3D Gaussian Splatting을 LiDAR-Inertial-Camera tightly-coupled SLAM에 통합한 시스템. LiDAR의 정밀한 기하 정보와 카메라의 텍스처를 Gaussian 표현으로 융합하여, SLAM과 동시에 photo-realistic한 장면 복원을 달성한다.
- **[Snail-Radar (Huai et al., IJRR 2025)](https://arxiv.org/abs/2407.11705)**: 4D radar SLAM 평가를 위한 대규모 다양성 벤치마크. 다양한 환경(실내·실외, 도심·교외)과 플랫폼에서 4D radar 기반 odometry/SLAM 알고리즘을 체계적으로 비교한다.

### 8.6.4 시스템 설계 체크리스트

실제 멀티센서 퓨전 시스템을 설계할 때 반드시 확인해야 할 항목들:

**캘리브레이션**:
- [ ] 모든 센서 쌍의 extrinsic calibration 완료
- [ ] 시간 동기화 오프셋 측정/추정 완료
- [ ] 캘리브레이션 결과의 재현성 검증 (3회 이상 반복)
- [ ] 온라인 캘리브레이션 드리프트 보정 메커니즘 존재

**데이터 흐름**:
- [ ] 각 센서의 데이터 레이트와 시스템의 처리 레이트 매칭
- [ ] 센서 간 데이터 정렬 (temporal alignment) 방법 확정
- [ ] 버퍼 크기와 지연시간(latency) 분석

**Robustness**:
- [ ] 각 센서의 실패 모드 식별
- [ ] Degradation handling 전략 수립
- [ ] Outlier rejection 메커니즘 (robust kernel, chi-square test)
- [ ] 극단 환경 테스트 (어둠, 비, 진동, 기하 퇴화)

**성능**:
- [ ] 목표 정확도 (ATE/RPE) 정의
- [ ] 실시간 제약 충족 여부 (worst-case latency)
- [ ] 메모리 사용량 (장시간 운행 시 누적)
- [ ] CPU/GPU 점유율

---

## 8장 요약

멀티센서 퓨전의 아키텍처는 크게 loosely/tightly/ultra-tightly coupled로 분류되며, 현대 로보틱스에서는 **tightly coupled**가 주류이다. Camera+LiDAR+IMU 삼중 융합은 R3LIVE, LVI-SAM, FAST-LIVO2 등의 시스템으로 성숙 단계에 접어들었고, 각각 dual subsystem, factor graph, sequential update라는 서로 다른 설계 철학을 보여준다.

GNSS 통합은 전역 좌표 앵커를 제공해 장기 드리프트를 해결하고, 4D radar는 악천후 robustness와 직접 속도 측정이라는 고유 장점으로 활용 사례가 빠르게 늘고 있다. Multi-robot 퓨전은 통신 제약 하에서의 분산 최적화와 cross-robot place recognition이 핵심 과제이며, Kimera-Multi와 Swarm-SLAM이 이 분야를 이끈다.

마지막으로, 실전 시스템 설계에서는 센서 선정, 시간 동기화, failure mode 대응이 알고리즘만큼이나 중요하며, 이를 체계적으로 다루는 엔지니어링 역량이 성공적인 배포를 좌우한다.

Ch.6-8의 odometry·fusion 시스템은 로컬 정밀도가 높지만, 장시간 운행하면 드리프트가 쌓인다. 이 드리프트를 교정하려면 "과거에 방문했던 장소를 다시 인식하는" 능력이 필요하다. 다음 챕터의 주제 — **Place Recognition** — 이다.

---

# Ch.9 — Place Recognition & Retrieval

Ch.6-8에서 다룬 odometry/fusion 시스템은 시간이 지나면 드리프트가 누적된다. 이 드리프트를 교정하려면 로봇이 과거에 방문한 장소를 다시 인식할 수 있어야 한다 — 이것이 Place Recognition의 역할이다. 이 챕터에서 다루는 기술들은 Ch.10의 Loop Closure에서 직접 활용된다.

> 로봇이 "이 장소를 전에 본 적이 있는가?"를 판단하는 문제.
> Loop closure의 핵심 컴포넌트이자, multi-session SLAM과 재위치추정(relocalization)의 기반이다.

---

## 9.1 문제 정의

### 9.1.1 Place Recognition vs Loop Closure Detection

**Place Recognition (PR)**은 "현재 관측이 데이터베이스의 어떤 장소와 일치하는가?"를 판단하는 retrieval 문제이다. 이것은 standalone 문제로, SLAM 없이도 정의된다.

**Loop Closure Detection**은 SLAM 시스템 내에서 "로봇이 이전에 방문했던 장소를 다시 방문했는가?"를 탐지하는 것이다. Place recognition은 loop closure detection의 핵심 컴포넌트이지만, loop closure는 PR 이후에 **geometric verification**(기하학적 검증)까지 포함하는 더 넓은 파이프라인이다.

```
Loop Closure Detection Pipeline:
┌─────────────┐    ┌────────────────┐    ┌───────────────────┐    ┌─────────────┐
│  현재 관측   │───→│ Place          │───→│ Geometric         │───→│ Pose Graph  │
│  (query)     │    │ Recognition    │    │ Verification      │    │ Update      │
│              │    │ (후보 검색)     │    │ (기하학적 검증)    │    │ (최적화)     │
└─────────────┘    └────────────────┘    └───────────────────┘    └─────────────┘
```

SLAM에서 loop closure가 없으면 드리프트가 계속 누적된다. 그런데 brute-force로 현재 프레임을 모든 과거 프레임과 비교하는 것은 $O(N^2)$으로 불가능하다. Place recognition은 이 비교를 sub-linear (보통 $O(\log N)$ 또는 $O(1)$)로 줄여주는 핵심 기술이다.

### 9.1.2 Retrieval Pipeline

Place recognition의 일반적인 파이프라인은 정보 검색(Information Retrieval)의 패러다임을 따른다:

1. **Encoding**: 각 관측(이미지, 포인트 클라우드, 또는 둘 다)을 고정 길이의 **글로벌 디스크립터(global descriptor)**로 인코딩.
2. **Indexing**: 데이터베이스의 모든 디스크립터를 검색 가능한 인덱스 구조(예: kd-tree, FAISS)에 저장.
3. **Retrieval**: 쿼리 디스크립터와 가장 유사한 데이터베이스 디스크립터를 검색.
4. **Re-ranking & Verification**: 후보들을 기하학적으로 검증하여 최종 매칭을 확정.

$$
q^* = \arg\min_{q \in \mathcal{D}} d(\mathbf{f}(\text{query}), \mathbf{f}(q))
$$

여기서 $\mathbf{f}(\cdot)$는 관측을 글로벌 디스크립터로 변환하는 함수, $d(\cdot, \cdot)$는 거리 함수(보통 L2 또는 코사인 거리), $\mathcal{D}$는 데이터베이스이다.

### 9.1.3 평가 메트릭

PR 시스템의 성능은 다음 메트릭으로 평가한다:

**Recall@N**: 상위 $N$개 후보 중 올바른 매칭이 포함되는 비율. 가장 보편적인 메트릭이다.

$$
\text{Recall@N} = \frac{|\{q : \text{top-}N \text{ 후보 중 정답이 있는 쿼리 } q\}|}{|\text{전체 쿼리}|}
$$

**Recall@1**이 특히 중요한 이유: 실시간 SLAM에서는 보통 가장 유사한 1개의 후보만 검증할 여유가 있기 때문이다.

**Precision-Recall Curve**: 디스크립터 유사도에 임계값을 변화시키며 precision과 recall의 관계를 그린다. 높은 precision (false positive 최소화)이 SLAM에서 특히 중요하다 — false positive loop closure는 맵을 파괴적으로 왜곡시킨다.

**"정답"의 정의**: 보통 GPS 거리 기준 25m 이내를 같은 장소로 정의한다. 데이터셋에 따라 다르다.

```python
import numpy as np
from scipy.spatial.distance import cdist

def compute_recall_at_n(query_descriptors, db_descriptors, 
                         query_positions, db_positions,
                         n_values=[1, 5, 10], threshold_m=25.0):
    """
    Recall@N 계산.
    
    query_descriptors: (Q, D) — 쿼리 디스크립터
    db_descriptors: (M, D) — 데이터베이스 디스크립터
    query_positions: (Q, 2 or 3) — 쿼리의 GPS/GT 위치
    db_positions: (M, 2 or 3) — 데이터베이스의 GPS/GT 위치
    n_values: Recall@N에서 N 값들
    threshold_m: 같은 장소로 판정하는 거리 임계값 (미터)
    """
    # 디스크립터 유사도 행렬 (L2 거리)
    desc_dists = cdist(query_descriptors, db_descriptors, metric='euclidean')
    
    # 실제 거리 행렬
    geo_dists = cdist(query_positions, db_positions, metric='euclidean')
    
    recalls = {}
    for n in n_values:
        correct = 0
        for q in range(len(query_descriptors)):
            # 디스크립터 거리 기준 top-N 인덱스
            top_n_indices = np.argsort(desc_dists[q])[:n]
            
            # top-N 중 하나라도 실제 거리가 threshold 이내이면 정답
            min_geo_dist = geo_dists[q, top_n_indices].min()
            if min_geo_dist < threshold_m:
                correct += 1
        
        recalls[f"Recall@{n}"] = correct / len(query_descriptors)
    
    return recalls
```

---

## 9.2 Visual Place Recognition (VPR)

Visual Place Recognition은 이미지만으로 장소를 인식하는 문제이다. 가장 오래되고 가장 활발히 연구되는 PR 분야이다.

### 9.2.1 전통적 방법: Bag of Words (BoW)

**[Video Google (Sivic & Zisserman, 2003)](https://www.robots.ox.ac.uk/~vgg/publications/2003/Sivic03/)**이 제안한 **Bag of Visual Words** 모델은 VPR의 원점이다. 핵심 아이디어는 텍스트 검색(text retrieval)의 방법론을 시각 검색에 그대로 적용하는 것이다.

파이프라인:

1. **시각 어휘 구축(Visual Vocabulary Construction)**: 대량의 이미지에서 로컬 특징(SIFT, ORB 등)을 추출하고, 이 특징 기술자들을 k-means로 $K$개 클러스터로 그룹화한다. 각 클러스터 중심이 하나의 "시각 단어(visual word)"가 된다.

2. **이미지 표현**: 각 이미지에서 추출한 특징들을 가장 가까운 시각 단어에 할당(hard assignment)하고, 각 시각 단어의 등장 빈도를 세어 이미지를 $K$차원 히스토그램으로 표현한다.

3. **TF-IDF 가중**: 텍스트 검색에서 차용한 TF-IDF(Term Frequency – Inverse Document Frequency) 가중을 적용한다:

$$
w_{i,d} = \underbrace{\frac{n_{i,d}}{n_d}}_{\text{TF}} \cdot \underbrace{\log \frac{N}{N_i}}_{\text{IDF}}
$$

여기서 $n_{i,d}$는 이미지 $d$에서 시각 단어 $i$의 등장 횟수, $n_d$는 이미지 $d$의 전체 시각 단어 수, $N$은 데이터베이스의 전체 이미지 수, $N_i$는 시각 단어 $i$가 등장하는 이미지 수이다.

- **TF**: 해당 이미지에서 자주 등장하는 단어에 높은 가중치 → 이 이미지의 특징적 요소
- **IDF**: 전체 데이터베이스에서 희귀한 단어에 높은 가중치 → 변별력이 높은 요소

4. **역색인(Inverted Index)**: 각 시각 단어가 등장하는 이미지 목록을 미리 구축하여, 쿼리 시 전체 데이터베이스를 스캔하지 않고 해당 단어가 포함된 이미지만 빠르게 검색한다.

5. **유사도 랭킹**: 쿼리 이미지와 데이터베이스 이미지의 TF-IDF 벡터 간 코사인 유사도로 랭킹:

$$
\text{sim}(\mathbf{v}_q, \mathbf{v}_d) = \frac{\mathbf{v}_q \cdot \mathbf{v}_d}{\|\mathbf{v}_q\| \cdot \|\mathbf{v}_d\|}
$$

**DBoW2**: ORB-SLAM 시리즈에서 사용되는 BoW 구현이다. Hierarchical k-means tree를 사용하여 vocabulary 크기를 크게 늘리면서도 양자화 속도를 유지한다. 실시간 SLAM에서 loop closure 탐지의 사실상 표준이었다.

```python
import numpy as np
from collections import defaultdict

class SimpleBoW:
    """
    Bag of Visual Words의 간소화 구현.
    """
    def __init__(self, vocabulary):
        """
        vocabulary: (K, D) — K개 시각 단어의 기술자 (k-means 중심)
        """
        self.vocabulary = vocabulary  # (K, D)
        self.K = len(vocabulary)
        self.inverted_index = defaultdict(list)  # word_id -> [(img_id, tf)]
        self.idf = np.ones(self.K)
        self.N = 0  # 데이터베이스 이미지 수
        self.db_vectors = {}  # img_id -> tf-idf vector
    
    def quantize(self, descriptors):
        """로컬 기술자를 가장 가까운 시각 단어로 양자화."""
        # (N_desc, D) vs (K, D) → (N_desc, K) 거리 행렬
        dists = np.linalg.norm(
            descriptors[:, None, :] - self.vocabulary[None, :, :], axis=2
        )
        word_ids = np.argmin(dists, axis=1)
        return word_ids
    
    def compute_bow_vector(self, descriptors):
        """이미지의 BoW 벡터 (TF-IDF 가중) 계산."""
        word_ids = self.quantize(descriptors)
        
        # Term Frequency
        tf = np.zeros(self.K)
        for w in word_ids:
            tf[w] += 1
        tf /= len(word_ids)  # 정규화
        
        # TF-IDF
        tfidf = tf * self.idf
        
        # L2 정규화
        norm = np.linalg.norm(tfidf)
        if norm > 0:
            tfidf /= norm
        
        return tfidf
    
    def add_to_database(self, img_id, descriptors):
        """이미지를 데이터베이스에 추가."""
        bow_vector = self.compute_bow_vector(descriptors)
        self.db_vectors[img_id] = bow_vector
        self.N += 1
        
        # 역색인 업데이트
        word_ids = self.quantize(descriptors)
        unique_words = np.unique(word_ids)
        for w in unique_words:
            self.inverted_index[w].append(img_id)
        
        # IDF 재계산
        for w in range(self.K):
            n_w = len(set(self.inverted_index[w]))  # 해당 단어가 등장하는 이미지 수
            self.idf[w] = np.log(self.N / (n_w + 1e-6))
    
    def query(self, descriptors, top_k=5):
        """쿼리 이미지와 가장 유사한 데이터베이스 이미지 검색."""
        q_vector = self.compute_bow_vector(descriptors)
        
        scores = {}
        for img_id, db_vector in self.db_vectors.items():
            scores[img_id] = np.dot(q_vector, db_vector)
        
        # 유사도 순으로 정렬
        sorted_results = sorted(scores.items(), key=lambda x: x[1], reverse=True)
        return sorted_results[:top_k]
```

BoW의 한계는 세 가지다. 첫째, 양자화 과정에서 정보 손실이 크다(비슷한 특징이 같은 단어로 뭉침). 둘째, 조명·시점 변화에 대한 불변성이 기저 특징(SIFT, ORB)에 의존한다. 셋째, vocabulary 크기 $K$ 선택이 성능에 큰 영향을 미친다.

### 9.2.2 VLAD와 Fisher Vector

BoW의 한계를 극복하기 위해, 단순 빈도 히스토그램 대신 **잔차(residual) 정보**를 보존하는 집계 방법들이 등장했다.

**[VLAD (Vector of Locally Aggregated Descriptors, Jégou et al., 2010)](https://doi.org/10.1109/CVPR.2010.5540039)**:

BoW가 "어떤 시각 단어가 몇 번 등장했는가"만 기록하는 반면, VLAD는 "각 시각 단어와 실제 기술자 사이의 잔차가 어떤 방향으로 얼마나 큰가"를 기록한다.

각 클러스터 $k$에 대해, 해당 클러스터에 할당된 기술자들과 클러스터 중심의 잔차를 합산한다:

$$
\mathbf{V}_k = \sum_{i: \text{NN}(\mathbf{x}_i) = k} (\mathbf{x}_i - \mathbf{c}_k)
$$

최종 VLAD 디스크립터는 모든 $\mathbf{V}_k$를 연결(concatenate)한 벡터이다: $\mathbf{V} = [\mathbf{V}_1^T, \mathbf{V}_2^T, \ldots, \mathbf{V}_K^T]^T$. 차원은 $K \times D$이다.

**Fisher Vector**: VLAD보다 더 풍부한 표현. 시각 단어를 GMM (Gaussian Mixture Model)로 모델링하고, 각 가우시안 컴포넌트에 대한 1차/2차 통계량을 Fisher Information Matrix의 제곱근으로 정규화한다. 차원이 $2KD$로 VLAD의 2배지만, 일반적으로 더 높은 성능을 보인다.

### 9.2.3 NetVLAD: 학습 기반 VPR의 기준점

**[NetVLAD (Arandjelović et al., 2016)](https://arxiv.org/abs/1511.07247)**은 VLAD를 미분 가능한(differentiable) CNN 레이어로 재구성하여, end-to-end 학습을 가능하게 했다.

전통적 VLAD에서 각 기술자를 가장 가까운 클러스터에 hard assignment하는 것은 미분 불가능하다. 역전파를 위해서는 이 과정이 미분 가능해야 한다.

**해결: Soft Assignment**:

Hard assignment($\text{NN}(\mathbf{x}_i) = k$이면 1, 아니면 0)를 softmax로 대체한다:

$$
\bar{a}_k(\mathbf{x}_i) = \frac{e^{\mathbf{w}_k^T \mathbf{x}_i + b_k}}{\sum_{k'} e^{\mathbf{w}_{k'}^T \mathbf{x}_i + b_{k'}}}
$$

원래 VLAD의 유클리드 거리 기반 할당 $e^{-\alpha\|\mathbf{x}_i - \mathbf{c}_k\|^2}$을 전개하면, $\mathbf{w}_k = 2\alpha\mathbf{c}_k$, $b_k = -\alpha\|\mathbf{c}_k\|^2$로 해석할 수 있다. 이 파라미터들을 학습 가능하게 하면 클러스터 중심이 데이터에 맞게 조정된다.

**NetVLAD 레이어 출력**:

$$
\mathbf{V}(j, k) = \sum_{i=1}^{N} \bar{a}_k(\mathbf{x}_i) (x_i(j) - c_k(j))
$$

이 $D \times K$ 행렬을 L2 정규화하고 벡터로 펼치면 최종 글로벌 디스크립터가 된다.

Google Street View Time Machine 데이터를 활용한 약한 지도 학습(weakly supervised learning)으로 훈련된다. 같은 GPS 좌표의 다른 시간대 이미지를 positive pair, 먼 GPS 좌표의 이미지를 negative로 사용한다. Triplet ranking loss:

$$
\mathcal{L} = \sum_{(q, p^+, p^-)} \max\left(0, m + d(\mathbf{f}(q), \mathbf{f}(p^+)) - d(\mathbf{f}(q), \mathbf{f}(p^-))\right)
$$

여기서 $m$은 마진, $p^+$는 positive 이미지, $p^-$는 hard negative 이미지이다.

Pitts250k Recall@1 약 84.3%, Pitts30k Recall@1 약 86.3%로, 당시 hand-crafted 방법 대비 향상. VGG-16 백본 사용.

### 9.2.4 AnyLoc: Foundation Model 기반 범용 VPR

**[AnyLoc (Keetha et al., 2023)](https://arxiv.org/abs/2308.00688)**은 VPR 전용 학습 없이 범용적으로 작동하는 장소 인식이 가능한가를 묻는다. 대답은 DINOv2 같은 Foundation Model의 특징을 사용하면 가능하다는 것이다.

1. **DINOv2 특징 추출**: DINOv2 ViT-G14의 중간 레이어(31번째 레이어)에서 밀집(dense) 특징을 추출한다. CLS 토큰(이미지 전체를 하나의 벡터로 요약)이 아니라 모든 패치의 특징을 사용한다. CLS 토큰은 이미지 전체의 semantic을 포착하지만 장소를 구분하는 세밀한 구조적 차이를 놓칠 수 있다. 밀집 특징은 각 패치의 로컬 정보를 보존하므로 더 정밀한 장소 구분이 가능하다 (평균 23% 성능 향상).

2. **VLAD 집계**: 밀집 특징을 k-means로 클러스터링하여 시각 어휘를 구축하고, hard-assignment VLAD로 글로벌 디스크립터를 생성한다. NetVLAD처럼 학습하지 않고 비지도(unsupervised) VLAD를 사용한다.

3. **도메인별 어휘**: PCA 투영으로 Urban, Indoor, Aerial, SubT(지하), Degraded(악조건), Underwater(수중) 6개 도메인을 비지도적으로 발견하고, 도메인별 어휘를 구축하면 성능이 최대 19% 더 향상된다.

Foundation Model이 작동하는 이유:

[DINOv2 (Oquab et al., 2023)](https://arxiv.org/abs/2304.07193)는 1.42억 장의 이미지에서 자기지도 학습(self-supervised learning)으로 학습한 모델이다. 훈련 과정에서 장면의 구조적·시맨틱 특징을 범용적으로 익힌다. 특정 장소 인식 태스크에 학습하지 않았음에도 이 범용 특징이 장소를 구분하는 데 충분하다.

벤치마크 성능:
- 주야간 변화: 기존 SOTA(MixVPR, CosPlace) 대비 5-21% Recall@1 향상
- 계절 변화: 8-9% 향상
- 반대 시점(180도): 39-49% 향상
- 비정형 환경(수중, 지하): 기존 대비 최대 4배
- PCA-Whitening으로 49K 차원 → 512 차원 (100배 압축)하면서 SOTA 성능 유지

```python
import numpy as np

def anyloc_pipeline(images, dino_model, n_clusters=64, desc_dim=512):
    """
    AnyLoc 파이프라인의 개념적 구현.
    
    1. DINOv2에서 밀집 특징 추출
    2. k-means로 시각 어휘 구축
    3. VLAD 집계
    4. PCA 차원 축소
    """
    # Step 1: 밀집 특징 추출 (ViT 패치 토큰)
    all_patch_features = []
    image_patch_features = []
    
    for img in images:
        # DINOv2 forward: (1, N_patches, D_feat) 형태의 패치 토큰
        patch_tokens = dino_model.get_intermediate_layers(img, n=1)[0]
        # layer 31의 value facet 사용 (AnyLoc의 핵심 발견)
        image_patch_features.append(patch_tokens)
        all_patch_features.append(patch_tokens)
    
    # Step 2: k-means 시각 어휘 구축
    all_features = np.vstack(all_patch_features)  # (N_total_patches, D_feat)
    from sklearn.cluster import KMeans
    kmeans = KMeans(n_clusters=n_clusters, random_state=42)
    kmeans.fit(all_features)
    centers = kmeans.cluster_centers_  # (K, D_feat)
    
    # Step 3: 각 이미지에 대해 VLAD 디스크립터 계산
    vlad_descriptors = []
    D_feat = centers.shape[1]
    
    for patches in image_patch_features:
        # Hard assignment
        assignments = kmeans.predict(patches)  # (N_patches,)
        
        # VLAD: 각 클러스터에 대한 잔차 합산
        vlad = np.zeros((n_clusters, D_feat))
        for i, patch_feat in enumerate(patches):
            k = assignments[i]
            vlad[k] += patch_feat - centers[k]
        
        # Intra-normalization (각 클러스터 벡터를 개별 정규화)
        for k in range(n_clusters):
            norm = np.linalg.norm(vlad[k])
            if norm > 0:
                vlad[k] /= norm
        
        # Flatten + L2 normalization
        vlad_flat = vlad.flatten()
        vlad_flat /= (np.linalg.norm(vlad_flat) + 1e-10)
        
        vlad_descriptors.append(vlad_flat)
    
    vlad_descriptors = np.array(vlad_descriptors)
    
    # Step 4: PCA 차원 축소
    from sklearn.decomposition import PCA
    pca = PCA(n_components=desc_dim, whiten=True)
    reduced_descriptors = pca.fit_transform(vlad_descriptors)
    
    # 최종 L2 정규화
    norms = np.linalg.norm(reduced_descriptors, axis=1, keepdims=True)
    reduced_descriptors /= (norms + 1e-10)
    
    return reduced_descriptors  # (N_images, desc_dim)
```

### 9.2.5 EigenPlaces

**[EigenPlaces (Berton et al., 2023)](https://arxiv.org/abs/2308.10832)**는 [CosPlace (Berton et al., 2022)](https://arxiv.org/abs/2204.02287)의 후속작으로, PCA 기반 차원 축소를 학습 과정에 통합한 방법이다. CosPlace가 분류(classification) 기반 학습으로 triplet loss의 번거로운 hard negative mining을 대체한 것에서 더 나아가, 특징 공간의 구조를 PCA 관점에서 최적화한다.

### 9.2.6 Foundation Model 활용의 확장

DINOv2 외에도 다양한 Foundation Model이 VPR에 활용되고 있다:

- **CLIP**: 텍스트-이미지 대응 학습으로, "도시 거리", "숲 속 길" 같은 시맨틱 수준의 장소 인식에 활용 가능. 그러나 세밀한 구조적 차이 구분에서는 DINOv2에 뒤진다.
- **SAM (Segment Anything)**: 세그멘테이션 마스크를 장소의 구조적 표현으로 활용하는 연구가 진행 중.
- **DINOv2 + NetVLAD**: AnyLoc의 비지도 VLAD 대신, DINOv2 특징에 학습된 NetVLAD 레이어를 붙이면 성능이 더 향상된다는 후속 연구.

### 9.2.7 SeqSLAM과 시퀀스 매칭

**[SeqSLAM (Milford & Wyeth, 2012)](https://doi.org/10.1109/ICRA.2012.6224623)**은 개별 이미지 매칭 대신 **이미지 시퀀스 전체의 매칭**을 택했다.

단일 이미지로는 구분이 어려운 장소(예: 비슷하게 생긴 주택가)도, 연속된 이미지들의 패턴(좌회전 → 공원 → 건널목 순서)은 고유할 수 있다.

방법은 세 단계다. 첫째, 각 이미지를 극도로 간소화된 표현(저해상도 패치 또는 단순 디스크립터)으로 변환한다. 둘째, 쿼리 시퀀스와 데이터베이스 시퀀스의 **시퀀스 유사도 행렬(sequence similarity matrix)**을 구성한다. 셋째, 그 위에서 일정 속도 범위 내의 대각선 경로를 탐색하여 최적 시퀀스 매칭을 찾는다.

$$
S_{\text{seq}}(q_s, d_s) = \min_{\text{path}} \sum_{i=0}^{L-1} d(\mathbf{f}(q_{s+i}), \mathbf{f}(d_{s+\delta(i)}))
$$

여기서 $L$은 시퀀스 길이, $\delta(i)$는 속도 변화를 허용하는 경로 함수이다.

주간 → 야간, 여름 → 겨울 같은 외관 변화에서도 시퀀스 패턴이 보존되므로 강건하다. SeqNet (Garg et al., 2021)은 이 시퀀스 매칭을 학습 기반으로 발전시켰다.

---

## 9.3 LiDAR Place Recognition

3D LiDAR 포인트 클라우드로 장소를 인식하는 문제이다. 카메라와 달리 조명 변화에 영향을 받지 않지만, 계절에 따른 식생 변화나 시점(viewpoint) 변화에 취약하다.

### 9.3.1 Handcrafted 방법: Scan Context

**[Scan Context (Kim & Kim, 2018)](https://doi.org/10.1109/IROS.2018.8593953)**은 3D LiDAR 스캔을 histogram 없이 **공간 구조를 직접 보존하는 글로벌 디스크립터**로 변환한다. 학습이 불필요하고, LIO-SAM 등 주요 시스템에 loop closure 모듈로 널리 채택되어 있다.

디스크립터 생성 과정:

1. **극좌표 분할**: 센서 중심에서 바라보는 2D 평면을 방위각(azimuth) $N_s$개 섹터와 거리(range) $N_r$개 링으로 분할하여 $N_s \times N_r$ 그리드를 만든다 (보통 60×20).

2. **최대 높이 인코딩**: 각 빈(bin)에 속하는 3D 점들 중 최대 높이(max height)를 기록한다. 이것이 Scan Context 행렬 $\mathbf{SC} \in \mathbb{R}^{N_s \times N_r}$이다.

$$
\mathbf{SC}(i, j) = \max_{p \in \text{bin}(i,j)} z(p)
$$

평균 높이보다 최대 높이가 건물, 나무, 기둥 같은 돌출 구조물을 더 잘 포착한다.

3. **Egocentric 표현의 장점**: 센서 중심 좌표계에서 표현하므로, 같은 장소를 **반대 방향**에서 재방문해도 디스크립터의 행(row, 즉 섹터 축)이 순환 이동(circular shift)된 것에 불과하다. 이를 행 이동 매칭으로 처리한다.

**검색 전략 — Ring Key & Sector Key**:

전체 데이터베이스에 대해 Scan Context 행렬을 직접 비교하는 것은 비효율적이다. 2단계 검색으로 효율화한다:

1. **Ring Key**: SC 행렬의 각 링(열)에 대해 섹터 방향으로 평균한 값을 벡터로 추출 — $\mathbf{k}_r = [\bar{h}_1, \bar{h}_2, \ldots, \bar{h}_{N_r}]$. 이 벡터로 kd-tree 검색하여 후보를 빠르게 좁힌다.

2. **Sector Key**: 후보들에 대해, SC 행렬의 행(섹터 축) 이동을 시도하며 최적 매칭을 찾는다:

$$
d(\mathbf{SC}_q, \mathbf{SC}_d) = \min_{s \in [0, N_s)} \left\| \mathbf{SC}_q - \text{shift}(\mathbf{SC}_d, s) \right\|_F
$$

```python
import numpy as np

class ScanContext:
    """
    Scan Context 디스크립터 생성 및 매칭.
    """
    def __init__(self, n_sectors=60, n_rings=20, max_range=80.0):
        self.n_sectors = n_sectors
        self.n_rings = n_rings
        self.max_range = max_range
        self.database = []
        self.ring_keys = []
    
    def make_scan_context(self, point_cloud):
        """
        3D 포인트 클라우드 → Scan Context 행렬.
        
        point_cloud: (N, 3) — x, y, z 좌표
        """
        sc = np.zeros((self.n_sectors, self.n_rings))
        
        # 극좌표 변환
        x, y, z = point_cloud[:, 0], point_cloud[:, 1], point_cloud[:, 2]
        ranges = np.sqrt(x**2 + y**2)
        angles = np.arctan2(y, x)  # [-pi, pi]
        angles = (angles + np.pi) / (2 * np.pi)  # [0, 1]로 정규화
        
        # 범위 밖 점 제거
        valid = ranges < self.max_range
        ranges, angles, z = ranges[valid], angles[valid], z[valid]
        
        # 빈 인덱스 계산
        sector_idx = np.clip((angles * self.n_sectors).astype(int), 0, self.n_sectors - 1)
        ring_idx = np.clip((ranges / self.max_range * self.n_rings).astype(int), 0, self.n_rings - 1)
        
        # 각 빈의 최대 높이
        for i in range(len(z)):
            si, ri = sector_idx[i], ring_idx[i]
            sc[si, ri] = max(sc[si, ri], z[i])
        
        return sc
    
    def make_ring_key(self, sc):
        """Ring key: 각 링의 평균 높이."""
        return np.mean(sc, axis=0)  # (n_rings,)
    
    def add_to_database(self, point_cloud):
        """스캔을 데이터베이스에 추가."""
        sc = self.make_scan_context(point_cloud)
        rk = self.make_ring_key(sc)
        self.database.append(sc)
        self.ring_keys.append(rk)
    
    def query(self, point_cloud, top_k=5, n_candidates=20):
        """
        쿼리 스캔과 가장 유사한 데이터베이스 스캔 검색.
        """
        sc_query = self.make_scan_context(point_cloud)
        rk_query = self.make_ring_key(sc_query)
        
        # Stage 1: Ring key로 후보 선정
        rk_dists = [np.linalg.norm(rk_query - rk) for rk in self.ring_keys]
        candidate_indices = np.argsort(rk_dists)[:n_candidates]
        
        # Stage 2: Scan Context 열 이동 매칭
        scores = []
        for idx in candidate_indices:
            sc_db = self.database[idx]
            min_dist = float('inf')
            for shift in range(self.n_sectors):
                sc_shifted = np.roll(sc_db, shift, axis=0)
                dist = np.linalg.norm(sc_query - sc_shifted)
                min_dist = min(min_dist, dist)
            scores.append((idx, min_dist))
        
        scores.sort(key=lambda x: x[1])
        return scores[:top_k]
```

**후속작 — Scan Context++**: 시맨틱 세그멘테이션을 추가하여, 높이 대신 시맨틱 레이블(건물, 도로, 식생 등)을 인코딩. 시맨틱 정보는 계절 변화에 더 강건하다.

### 9.3.2 M2DP와 ESF

Scan Context 외의 handcrafted LiDAR 디스크립터:

- **M2DP (He et al., 2016)**: 포인트 클라우드를 여러 2D 평면에 투영하고, 각 투영의 밀도 분포를 SVD로 압축하여 192차원 디스크립터를 생성. 방향 불변(rotation invariant).
- **ESF (Ensemble of Shape Functions)**: 점쌍 간 거리, 각도, 면적 비율의 히스토그램을 조합한 디스크립터.

이들은 Scan Context에 비해 공간 구조 보존이 약하지만, 회전 불변성이나 간결함에서 장점이 있다.

### 9.3.3 Learning-based: PointNetVLAD

**[PointNetVLAD (Uy & Lee, 2018)](https://arxiv.org/abs/1804.03492)**는 NetVLAD의 아이디어를 3D 포인트 클라우드에 직접 적용한 최초의 연구이다. PointNet 백본으로 로컬 특징을 추출하고, NetVLAD 레이어로 글로벌 디스크립터로 집계하며, lazy triplet loss로 학습한다.

$$
\mathcal{L} = \max(0, m + \max_{p^+} d(q, p^+) - \min_{p^-} d(q, p^-))
$$

PointNet은 포인트 간 상호작용을 충분히 모델링하지 못하며, 대규모 포인트 클라우드 처리에 비효율적이다.

### 9.3.4 MinkLoc3D

**[MinkLoc3D (Komorowski, 2021)](https://arxiv.org/abs/2011.04530)**는 PointNetVLAD의 한계를 극복하기 위해 Minkowski Convolutional Neural Network을 백본으로 사용한다. 희소 3D 합성곱(sparse 3D convolution)을 통해 포인트 클라우드의 로컬 구조를 효과적으로 포착하며, GeM (Generalized Mean) 풀링으로 글로벌 디스크립터를 생성한다.

### 9.3.5 OverlapTransformer: Range Image 기반

**[OverlapTransformer (Ma et al., 2022)](https://arxiv.org/abs/2203.03397)**는 LiDAR 포인트 클라우드를 **range image**로 변환하여, 2D 이미지 처리 파이프라인을 활용하는 접근법이다.

**Range Image**: 회전형 LiDAR 스캔을 $(h, w)$ 크기의 2D 이미지로 변환한다. 각 픽셀 값은 해당 방향의 거리(range)이다. $h$는 레이저 빔 수, $w$는 수평 해상도에 대응한다.

아키텍처는 3단계로 구성된다. 첫째, range image를 lightweight CNN으로 처리하여 feature map을 추출한다. 둘째, NetVLAD 레이어로 글로벌 디스크립터를 생성한다. 셋째, Transformer encoder로 전체적인 context를 반영한다. 3D 포인트 클라우드 처리보다 2D CNN 처리가 빠르고, 기존 이미지 네트워크 아키텍처를 그대로 활용할 수 있다는 점이 이 방식의 실용적 장점이다.

### 9.3.6 BEV 기반 방법

Bird's Eye View(BEV)로 포인트 클라우드를 투영하여 2D 맵으로 변환한 뒤, 2D 이미지 기반 검색을 수행하는 방법들:

- **OverlapNet**: BEV 투영 이미지의 오버랩 정도를 직접 예측
- **BEVPlace**: BEV 이미지에서 NetVLAD 디스크립터를 추출

---

## 9.4 Cross-Modal Place Recognition

### 9.4.1 왜 Cross-Modal PR이 필요한가

실제 로봇 시스템에서는 **매핑 시의 센서와 로컬라이제이션 시의 센서가 다를 수 있다**. 예:
- LiDAR로 만든 맵에서 카메라만으로 위치를 찾아야 하는 경우
- 자율주행 차량(LiDAR+Camera)과 배달 로봇(Camera만) 간의 장소 인식
- 드론(Camera)이 차량(LiDAR)이 만든 맵에서 자신의 위치를 파악

이런 시나리오에서는 **LiDAR 관측과 카메라 관측 사이의 장소 인식**, 즉 cross-modal PR이 필요하다.

### 9.4.2 Cross-Modal PR의 근본적 어려움: Domain Gap

LiDAR 포인트 클라우드와 카메라 이미지는 근본적으로 다른 표현(representation)이다:

| 특성 | LiDAR 포인트 클라우드 | 카메라 이미지 |
|------|---------------------|-------------|
| 데이터 구조 | 비정형 3D 점 집합 | 정형 2D 그리드 |
| 정보 | 기하(geometry) | 외관(appearance) |
| 조명 의존 | 없음 | 매우 높음 |
| 텍스처 | 없음 | 풍부 |
| 밀도 | 거리에 반비례 | 균일 |

이 근본적 차이를 **domain gap**이라 하며, 같은 장소를 다른 모달리티로 관측했을 때 디스크립터 공간에서의 거리가 멀어지는 원인이 된다.

### 9.4.3 (LC)²: LiDAR-Camera Cross-Modal PR

**[(LC)² (Lee et al., 2023)](https://arxiv.org/abs/2304.08660)**는 LiDAR 포인트 클라우드와 카메라 이미지를 **공통 디스크립터 공간(shared embedding space)**에 매핑하는 방법을 내놓았다.

LiDAR 포인트 클라우드를 range image/BEV image로 변환하여 2D 표현으로 통일하고, 카메라 이미지와 LiDAR 투영 이미지를 각각 CNN으로 처리한다. **Contrastive learning**으로 같은 장소의 LiDAR-Camera 쌍을 임베딩 공간에서 가깝게, 다른 장소의 쌍을 멀게 학습한다.

$$
\mathcal{L}_{\text{contrastive}} = \sum_{(l, c) \in \mathcal{P}^+} \| \mathbf{f}_L(l) - \mathbf{f}_C(c) \|^2 + \sum_{(l, c) \in \mathcal{P}^-} \max(0, m - \| \mathbf{f}_L(l) - \mathbf{f}_C(c) \|)^2
$$

여기서 $\mathbf{f}_L$은 LiDAR 인코더, $\mathbf{f}_C$는 Camera 인코더, $\mathcal{P}^+$/$\mathcal{P}^-$는 positive/negative 쌍이다.

### 9.4.4 ModaLink

**ModaLink**는 (LC)²보다 더 범용적인 cross-modal 프레임워크를 지향하며, LiDAR, Camera, Radar 등 다양한 모달리티 조합에 대응한다.

### 9.4.5 Modality-Agnostic Descriptor 접근

궁극적 목표는 **모달리티에 무관한(modality-agnostic) 디스크립터**이다. 어떤 센서로 관측하든 같은 장소는 같은 디스크립터를 생성하는 것이다. 이를 위한 접근법:

- **Knowledge Distillation**: 정보가 풍부한 모달리티(LiDAR+Camera)의 디스크립터를 teacher로, 단일 모달리티를 student로 학습
- **Canonical Representation**: BEV 또는 semantic layout 같은 모달리티 중립적 표현으로 변환 후 비교
- **Foundation Model 기반**: DINOv2 같은 FM이 이미지에서 추출하는 특징이 LiDAR를 이미지로 렌더링한 것에서 추출한 특징과 비슷한 공간에 놓이는지 탐구하는 연구

---

## 9.5 Multi-Session & Long-Term Place Recognition

### 9.5.1 Long-Term VPR의 도전

동일한 장소도 시간에 따라 외관이 크게 변한다. 조명(낮 vs 밤), 계절(초록 나무 vs 눈 덮인 풍경), 날씨(맑음 vs 안개), 구조적 변화(건물 신축·철거, 도로 공사) 모두 같은 장소의 디스크립터를 변동시켜 PR 성능을 저하시킨다.

### 9.5.2 계절/시간/날씨 변화 대응 전략

네 가지 접근이 병행된다.

**Data Augmentation**: 학습 시 다양한 조건의 이미지를 포함한다. NetVLAD가 Google Street View Time Machine을 활용한 것이 대표적이다.

**Domain Invariant Feature**: 외관 변화에 불변하는 특징을 학습한다. 시맨틱 세그멘테이션 결과(건물, 도로, 하늘의 배치)는 조명에 불변적이다.

**Foundation Model 활용**: AnyLoc에서 보여주었듯이, DINOv2의 특징은 조명·계절 변화에 강건하다. 자기지도 학습 과정에서 다양한 augmentation에 불변하는 특징을 학습하기 때문이다.

**시퀀스 매칭**: SeqSLAM 방식이다. 개별 이미지의 외관이 달라도 시퀀스 패턴은 유지되는 경향이 있다.

### 9.5.3 Map Update 전략

장기 운용 시스템에서는 맵 자체를 업데이트해야 한다:

- **경험 기반 맵(Experience-Based Map)**: 같은 장소의 여러 시간대 관측을 모두 저장하여, 현재 조건과 가장 유사한 경험에서 매칭. 맵 크기가 시간에 비례하여 증가하는 문제.

- **적응형 디스크립터**: 새로운 관측으로 기존 장소의 디스크립터를 점진적으로 업데이트. Exponential moving average 등.

- **Change Detection**: 구조적 변화(건물 철거 등)를 탐지하여 해당 맵 영역을 무효화하고 재구축.

### 9.5.4 Lifelong Place Recognition

평생 동안 운용되는 로봇의 장소 인식은 아직 열린 연구 문제다. 맵이 무한히 커지지 않도록 정보를 압축·관리해야 하고, 오래된 관측을 점진적으로 망각(graceful forgetting)하면서 새로운 환경을 catastrophic forgetting 없이 학습해야 한다. continual learning/incremental learning과 밀접하게 연관된다.

---

## 9.6 Geometric Verification & Re-ranking

Place recognition이 후보를 찾았다면, 그것이 **진짜 같은 장소인지 기하학적으로 검증**해야 한다. 이 단계가 없으면 false positive loop closure가 발생하여 맵이 파괴될 수 있다.

### 9.6.1 PnP + RANSAC (Visual)

카메라 기반 PR 후보에 대해:

1. 쿼리 이미지와 후보 이미지 사이의 로컬 특징 매칭 (SuperPoint+LightGlue, ORB+BF 등)
2. **PnP (Perspective-n-Point)**: 2D-3D 대응점으로부터 카메라의 상대 포즈를 추정
3. **RANSAC**: 아웃라이어를 제거하며 포즈 추정

```python
import numpy as np

def geometric_verification_visual(query_keypoints, db_keypoints_3d, 
                                    K, ransac_threshold=5.0,
                                    min_inliers=30):
    """
    Visual geometric verification: PnP + RANSAC.
    
    query_keypoints: (N, 2) — 쿼리 이미지의 2D 키포인트
    db_keypoints_3d: (N, 3) — 매칭된 데이터베이스의 3D 포인트
    K: (3, 3) — 카메라 내부 파라미터
    """
    import cv2
    
    # PnP + RANSAC으로 카메라 포즈 추정
    success, rvec, tvec, inliers = cv2.solvePnPRansac(
        db_keypoints_3d.astype(np.float32),
        query_keypoints.astype(np.float32),
        K.astype(np.float32),
        distCoeffs=None,
        reprojectionError=ransac_threshold,
        confidence=0.99,
        iterationsCount=1000
    )
    
    if not success or inliers is None:
        return False, None, 0
    
    n_inliers = len(inliers)
    is_verified = n_inliers >= min_inliers
    
    # 상대 포즈
    R, _ = cv2.Rodrigues(rvec)
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = tvec.flatten()
    
    return is_verified, T, n_inliers
```

### 9.6.2 3D-3D Registration (LiDAR)

LiDAR 기반 PR 후보에 대해:

1. 두 포인트 클라우드 사이의 **3D-3D 대응점** 찾기 (FPFH, FCGF, GeoTransformer 등)
2. **RANSAC** 또는 **GeoTransformer의 RANSAC-free** 방식으로 강체 변환 추정
3. **ICP로 정밀 정합** (coarse-to-fine)

GeoTransformer (Qin et al., 2022)를 사용하면 RANSAC 없이도 강건한 정합이 가능하다. GeoTransformer는 쌍별 거리(pairwise distance)와 삼중 각도(triplet angle)를 인코딩하는 기하학적 트랜스포머로, rigid transformation에 불변한 특징을 학습하여 저오버랩 시나리오에서도 강건하다. 슈퍼포인트 수준의 대응에서 직접 변환을 추정하므로 RANSAC 대비 100배 빠르다.

```python
def geometric_verification_lidar(query_cloud, db_cloud, 
                                   voxel_size=0.5, 
                                   distance_threshold=0.5,
                                   fitness_threshold=0.3):
    """
    LiDAR geometric verification: FPFH + RANSAC + ICP.
    """
    import open3d as o3d
    
    # 다운샘플링
    q_down = query_cloud.voxel_down_sample(voxel_size)
    d_down = db_cloud.voxel_down_sample(voxel_size)
    
    # 법선 추정
    q_down.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size*2, max_nn=30))
    d_down.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size*2, max_nn=30))
    
    # FPFH 특징 추출
    q_fpfh = o3d.pipelines.registration.compute_fpfh_feature(
        q_down, o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size*5, max_nn=100))
    d_fpfh = o3d.pipelines.registration.compute_fpfh_feature(
        d_down, o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size*5, max_nn=100))
    
    # RANSAC 기반 정합
    result_ransac = o3d.pipelines.registration.registration_ransac_based_on_feature_matching(
        q_down, d_down, q_fpfh, d_fpfh,
        mutual_filter=True,
        max_correspondence_distance=distance_threshold * 2,
        estimation_method=o3d.pipelines.registration.TransformationEstimationPointToPoint(),
        ransac_n=3,
        criteria=o3d.pipelines.registration.RANSACConvergenceCriteria(100000, 0.999)
    )
    
    # ICP 정밀 정합
    result_icp = o3d.pipelines.registration.registration_icp(
        q_down, d_down,
        distance_threshold,
        result_ransac.transformation,
        o3d.pipelines.registration.TransformationEstimationPointToPlane()
    )
    
    is_verified = result_icp.fitness > fitness_threshold
    
    return is_verified, result_icp.transformation, result_icp.fitness
```

### 9.6.3 Spatial Re-ranking

디스크립터 유사도만으로 랭킹한 후보 목록을 기하학적 정보로 재랭킹하는 전략이 세 가지 있다.

**공간적 근접성 prior**: SLAM에서 현재 포즈 추정치 근처의 후보에 가산점을 부여한다. 드리프트가 작을 때만 유효하다.

**Inlier 수 기반 재랭킹**: 특징 매칭 후 인라이어 수가 많은 후보를 상위로 올린다.

**포즈 일관성 검사**: 추정된 상대 포즈가 odometry의 누적 포즈와 일관성이 있는지 확인한다. 큰 불일치가 있으면 false positive로 거부한다.

---

## 9.7 최신 동향

### 9.7.1 Foundation Model 기반 PR의 부상

VPR은 "환경별 전용 학습"에서 "범용 zero-shot 인식"으로 이동하고 있다. AnyLoc이 보여주었듯이, Foundation Model의 범용 특징이 VPR 전용 학습 없이도 대부분의 환경에서 SOTA를 달성한다.

향후 방향은 크게 두 가지다. 첫째, ViT-G14는 1B+ 파라미터로 임베디드 배포에 제약이 있어 경량 FM(ViT-S/B) + 도메인 적응의 조합이 연구되고 있다. 둘째, FM이 제공하는 패치 수준의 dense correspondence를 re-ranking이나 상대 포즈 추정에 직접 활용하는 연구가 진행 중이다.

### 9.7.2 Semantic Place Recognition

장소를 **시맨틱 구조**로 인식하는 접근:

"빨간 건물 옆의 큰 나무" → 건물(빨간)과 나무(큰)의 공간적 배치(spatial layout)로 장소를 표현

시맨틱 세그멘테이션으로 레이아웃을 비교하거나, object detection으로 scene graph를 구성하거나, CLIP으로 자연어 설명 기반 검색을 수행하는 방식이 있다. 시맨틱 구조는 조명·계절 변화에 강건하다. 나무의 잎 색깔이 변해도 "나무"라는 레이블은 유지된다. 다만 시맨틱 세그멘테이션의 정확도에 의존하며, 시맨틱 구조가 유사한 장소(비슷한 구조의 주택가)를 구분하기 어렵다.

### 9.7.3 4D Radar Place Recognition

4D imaging radar의 등장으로, radar 기반 PR도 연구되기 시작했다:

- Radar 포인트 클라우드를 Scan Context 변형으로 인코딩
- Range-Doppler image를 CNN으로 처리하여 글로벌 디스크립터 생성
- Radar-Camera cross-modal PR

비·눈·안개 환경에서 LiDAR PR과 Visual PR이 실패할 때 radar PR이 백업 역할을 할 수 있다. 다만 아직 초기 단계이며, radar의 낮은 해상도 때문에 장소 구별 능력(discriminability)이 LiDAR나 Visual에 비해 떨어진다. 4D radar의 해상도가 개선되고 있어 향후 주목할 분야다.

### 9.7.4 최근 연구 (2024-2025)

**[SALAD (Izquierdo & Civera, 2024)](https://arxiv.org/abs/2311.15937)**: NetVLAD의 feature-to-cluster 할당을 **optimal transport** 문제로 재정의하고, DINOv2를 백본으로 fine-tuning한다. Sinkhorn 알고리즘으로 soft assignment를 최적화하여 NetVLAD/CosPlace 대비 다수 벤치마크에서 SOTA를 달성한다.

**[EffoVPR (Taha et al., 2024)](https://arxiv.org/abs/2405.18065)**: DINOv2 등 Foundation Model의 특징을 효율적으로 활용하는 프레임워크. 128차원까지 압축된 디스크립터로도 SOTA 성능을 유지하며, 임베디드 배포에 유리한 경량 VPR을 보여준다.

### 9.7.5 기술 계보 요약

```
Visual Place Recognition 계보:

Sivic (2003) Video Google [BoW]
    ↓ 양자화 → 잔차로
Jégou (2010) VLAD
    ↓ hand-crafted → CNN end-to-end
Arandjelović (2016) NetVLAD [triplet, soft-assignment]
    ↓ triplet → classification
Berton (2022) CosPlace, (2023) EigenPlaces
    ↓ VPR-specific → Foundation Model
Keetha (2023) AnyLoc [DINOv2 + VLAD, zero-shot]
    ↓ assignment 최적화 + FM fine-tuning
Izquierdo (2024) SALAD [optimal transport + DINOv2]

LiDAR Place Recognition 계보:

Kim (2018) Scan Context [handcrafted, spatial]
    ↓ handcrafted → learned
Uy (2018) PointNetVLAD [PointNet + NetVLAD]
    ↓ PointNet → sparse convolution
Komorowski (2021) MinkLoc3D [sparse conv + GeM]
    ↓ 3D point → 2D range image
Ma (2022) OverlapTransformer [range image + transformer]

Cross-Modal:
Lee (2023) (LC)² [LiDAR ↔ Camera shared embedding]
```

---

## 9장 요약

Place Recognition은 SLAM 시스템에서 드리프트를 교정하는 loop closure의 핵심 컴포넌트이다. Visual PR은 BoW(Video Google) → VLAD → NetVLAD → AnyLoc으로 진화하며, Foundation Model(DINOv2) 기반의 범용 zero-shot 인식이 최근 패러다임이 되었다. LiDAR PR은 Scan Context(handcrafted) → PointNetVLAD → MinkLoc3D → OverlapTransformer로 발전하며, range image 기반 방법이 효율성 면에서 주목할 만하다.

Cross-modal PR은 domain gap이라는 근본적 어려움이 있으며, 공통 임베딩 공간 학습과 modality-agnostic 디스크립터가 연구되고 있다. Long-term PR은 계절/조명/구조적 변화에 대응해야 하며, Foundation Model의 강건한 특징이 이 문제에 유망하다.

Geometric verification은 PR 후보의 최종 검증 단계로, false positive를 방지하여 SLAM의 무결성을 보호한다. PnP+RANSAC(visual), ICP/GeoTransformer(LiDAR)가 표준적 방법이다.

최신 동향으로는 Foundation Model 기반 PR의 경량화, semantic PR, 4D radar PR이 활발히 연구되고 있다.

Place Recognition은 "이 장소를 본 적 있는가?"라는 질문에 답하지만, 그 답을 SLAM 시스템의 전역 일관성으로 전환하는 과정이 남아 있다. 다음 챕터에서는 PR 결과를 포즈 그래프에 통합하여 드리프트를 교정하는 **Loop Closure와 전역 최적화**를 다룬다.

---

# Ch.10 — Loop Closure & Global Optimization

Ch.9에서 Place Recognition — 과거 방문 장소의 인식 — 을 보았다. 이 챕터는 그 인식 결과를 SLAM 시스템에 통합하여 누적 드리프트를 실제로 교정하는 과정을 이야기한다.

SLAM 시스템에서 odometry는 필연적으로 드리프트(drift)를 누적한다. 아무리 정밀한 센서를 쓰더라도, 상대 pose 추정의 작은 오차들이 시간에 따라 쌓여 전역 일관성을 무너뜨린다. Loop closure는 "과거에 방문했던 장소를 재방문했음"을 인식하고, 그 정보를 활용하여 누적된 드리프트를 보정하는 메커니즘이다.

이 챕터에서는 loop closure 파이프라인(detection → verification → correction)을 살펴보고, 보정의 핵심인 pose graph optimization의 수학적 기초, global relocalization, multi-session SLAM까지 확장한다.

---

## 10.1 Loop Closure Pipeline

Loop closure는 Detection(후보 탐지), Verification(기하학적 검증), Correction(그래프 보정), 세 단계로 이어진다. 어느 하나라도 실패하면 전체 시스템의 일관성이 깨진다.

### 10.1.1 Detection: 후보 탐지

Loop closure detection은 "현재 센서 관측이 과거의 어떤 관측과 유사한가?"를 묻는 문제다. Ch.9에서 다룬 place recognition 문제와 같다.

**Visual loop closure detection**에서는 현재 이미지의 글로벌 디스크립터를 과거 키프레임들의 디스크립터 데이터베이스와 비교한다:

1. **BoW(Bag of Words) 기반**: DBoW2를 사용하여 ORB 특징점의 visual word histogram을 비교한다. ORB-SLAM3는 이 방식을 사용한다. 각 키프레임을 BoW 벡터 $\mathbf{v}_i$로 표현하고, 현재 프레임 $\mathbf{v}_q$와의 유사도를 $s(\mathbf{v}_q, \mathbf{v}_i) = 1 - \frac{1}{2} \left| \frac{\mathbf{v}_q}{\|\mathbf{v}_q\|} - \frac{\mathbf{v}_i}{\|\mathbf{v}_i\|} \right|$ (L1-score)로 계산한다.

2. **학습 기반**: [Arandjelovic et al. 2016](https://arxiv.org/abs/1511.07247)(NetVLAD), [Keetha et al. 2023](https://arxiv.org/abs/2308.00688)(AnyLoc) 등의 글로벌 디스크립터를 사용한다. AnyLoc은 [Oquab et al. 2023](https://arxiv.org/abs/2304.07193)(DINOv2)의 dense feature를 VLAD로 집계하여 환경 특화 학습 없이도 범용적으로 동작한다. 코사인 유사도로 후보를 랭킹한다:

$$s(\mathbf{d}_q, \mathbf{d}_i) = \frac{\mathbf{d}_q^\top \mathbf{d}_i}{\|\mathbf{d}_q\| \|\mathbf{d}_i\|}$$

**LiDAR loop closure detection**에서는 3D 포인트 클라우드 기반 디스크립터를 사용한다:

- **[Scan Context](https://doi.org/10.1109/IROS.2018.8593953)**: 센서 중심의 극좌표계에서 bin/sector별 최대 높이를 기록하여 공간 구조를 직접 보존하는 디스크립터다. ring key와 sector key를 이용한 2단계 검색으로 효율적 후보 탐색이 가능하며, 역방향 재방문에도 강건하다.
- **[PointNetVLAD](https://arxiv.org/abs/1804.03492), [OverlapTransformer](https://arxiv.org/abs/2203.03397)**: 학습 기반 3D place recognition으로, 대규모 환경에서 Scan Context보다 높은 recall을 보인다.

**시간적 필터링**: 최근 프레임과의 매칭은 loop closure가 아니라 연속 tracking이다. 시간적으로 충분히 떨어진 키프레임(예: 최소 30초 이상 경과)만 후보로 고려한다.

```python
import numpy as np

def detect_loop_candidates(query_descriptor, database_descriptors, 
                           timestamps, current_time, 
                           min_time_gap=30.0, top_k=5, threshold=0.7):
    """
    Loop closure 후보 탐지.
    
    Args:
        query_descriptor: 현재 프레임의 글로벌 디스크립터 (D,)
        database_descriptors: 과거 키프레임 디스크립터들 (N, D)
        timestamps: 각 키프레임의 타임스탬프 (N,)
        current_time: 현재 시간
        min_time_gap: 최소 시간 간격 (초)
        top_k: 반환할 후보 수
        threshold: 유사도 임계값
        
    Returns:
        candidates: [(index, similarity)] 리스트
    """
    # 시간적 필터링: 최근 프레임 제외
    time_mask = (current_time - timestamps) > min_time_gap
    
    if not np.any(time_mask):
        return []
    
    # 코사인 유사도 계산
    query_norm = query_descriptor / (np.linalg.norm(query_descriptor) + 1e-8)
    db_norms = database_descriptors / (
        np.linalg.norm(database_descriptors, axis=1, keepdims=True) + 1e-8
    )
    
    similarities = db_norms @ query_norm  # (N,)
    
    # 시간 필터 적용
    similarities[~time_mask] = -1.0
    
    # 상위 k개 후보 선택
    top_indices = np.argsort(similarities)[::-1][:top_k]
    candidates = [
        (idx, similarities[idx]) 
        for idx in top_indices 
        if similarities[idx] > threshold
    ]
    
    return candidates
```

### 10.1.2 Verification: 기하학적 검증

Detection 단계는 appearance similarity만으로 후보를 걸러내므로, false positive(실제로는 다른 장소인데 비슷하게 보이는 경우)가 섞인다. Perceptual aliasing — 시각적으로 유사하지만 실제로 다른 장소 — 은 특히 실내 환경(비슷한 복도, 반복적 구조)에서 빈번하다.

잘못된 loop closure 하나가 전체 맵을 뒤틀어버릴 수 있다. verification을 보수적으로 수행해야 한다. recall을 희생하더라도 precision을 극대화한다.

기하학적 검증은 다음 방법을 쓴다.

1. **2D-2D: Essential matrix 검증**: 현재 프레임과 후보 키프레임 사이에서 특징점 매칭을 수행하고, RANSAC으로 essential matrix $\mathbf{E}$를 추정한다. 인라이어 수가 충분하고(예: ≥ 20), 인라이어 비율이 높으면(예: ≥ 50%) 유효한 loop closure로 판단한다.

$$\mathbf{p}_2^\top \mathbf{E} \mathbf{p}_1 = 0, \quad \mathbf{E} = [\mathbf{t}]_\times \mathbf{R}$$

2. **3D-3D: Point cloud registration**: LiDAR 기반 시스템에서는 ICP나 GeoTransformer로 두 스캔 간의 상대 변환 $\mathbf{T}_{ij} \in SE(3)$를 추정한다. Fitness score(정합된 포인트 비율)와 RMSE로 검증한다.

3. **2D-3D: PnP**: 현재 2D 특징점과 후보 키프레임의 3D 맵 포인트 사이의 PnP 문제를 풀어 상대 pose를 추정한다.

4. **Temporal consistency**: 단일 매칭이 아니라, 연속된 여러 프레임에서 동일 장소와의 매칭이 일관되게 나타나는지 확인한다. ORB-SLAM3는 세 번 연속 동일 장소가 검출되어야 loop closure를 수용한다.

```python
import numpy as np

def verify_loop_closure(kp_current, kp_candidate, matches, K,
                        min_inliers=20, min_inlier_ratio=0.5):
    """
    Essential matrix 기반 loop closure 기하학적 검증.
    
    Args:
        kp_current: 현재 프레임 키포인트 좌표 (N, 2)
        kp_candidate: 후보 프레임 키포인트 좌표 (M, 2)
        matches: 매칭 인덱스 쌍 리스트 [(i, j), ...]
        K: 카메라 내부 파라미터 행렬 (3, 3)
        min_inliers: 최소 인라이어 수
        min_inlier_ratio: 최소 인라이어 비율
        
    Returns:
        is_valid: 유효한 loop closure인지 여부
        T_relative: 상대 변환 (4, 4) 또는 None
    """
    if len(matches) < min_inliers:
        return False, None
    
    pts1 = np.array([kp_current[m[0]] for m in matches], dtype=np.float64)
    pts2 = np.array([kp_candidate[m[1]] for m in matches], dtype=np.float64)
    
    # 정규화 좌표로 변환
    K_inv = np.linalg.inv(K)
    pts1_norm = (K_inv @ np.hstack([pts1, np.ones((len(pts1), 1))]).T).T[:, :2]
    pts2_norm = (K_inv @ np.hstack([pts2, np.ones((len(pts2), 1))]).T).T[:, :2]
    
    # RANSAC으로 Essential matrix 추정
    E, inlier_mask = estimate_essential_ransac(pts1_norm, pts2_norm, 
                                                threshold=1e-3, max_iter=1000)
    
    num_inliers = np.sum(inlier_mask)
    inlier_ratio = num_inliers / len(matches)
    
    if num_inliers < min_inliers or inlier_ratio < min_inlier_ratio:
        return False, None
    
    # E에서 R, t 복원
    R, t = decompose_essential(E, pts1_norm[inlier_mask], pts2_norm[inlier_mask])
    
    T_relative = np.eye(4)
    T_relative[:3, :3] = R
    T_relative[:3, 3] = t.flatten()
    
    return True, T_relative

def estimate_essential_ransac(pts1, pts2, threshold=1e-3, max_iter=1000):
    """5-point 알고리즘 + RANSAC으로 Essential matrix 추정."""
    best_E = None
    best_inliers = np.zeros(len(pts1), dtype=bool)
    
    for _ in range(max_iter):
        # 8개 점 랜덤 샘플링
        idx = np.random.choice(len(pts1), 8, replace=False)
        
        # 8-point 알고리즘으로 E 후보 생성 (5-point의 간략화 버전)
        E_candidate = eight_point_essential(pts1[idx], pts2[idx])
        
        if E_candidate is None:
            continue
        
        # Sampson error로 인라이어 판별
        errors = sampson_error(E_candidate, pts1, pts2)
        inliers = errors < threshold
        
        if np.sum(inliers) > np.sum(best_inliers):
            best_inliers = inliers
            best_E = E_candidate
    
    return best_E, best_inliers

def sampson_error(E, pts1, pts2):
    """Sampson error 계산 — epipolar constraint의 1차 근사 거리."""
    # pts를 homogeneous로 변환
    p1 = np.hstack([pts1, np.ones((len(pts1), 1))])  # (N, 3)
    p2 = np.hstack([pts2, np.ones((len(pts2), 1))])  # (N, 3)
    
    Ep1 = (E @ p1.T).T    # (N, 3)
    Etp2 = (E.T @ p2.T).T  # (N, 3)
    
    # p2^T E p1
    numerator = np.sum(p2 * Ep1, axis=1) ** 2
    denominator = Ep1[:, 0]**2 + Ep1[:, 1]**2 + Etp2[:, 0]**2 + Etp2[:, 1]**2
    
    return numerator / (denominator + 1e-10)
```

### 10.1.3 False Positive의 위험과 방지

False positive loop closure가 왜 그토록 위험한지 구체적으로 보자.

로봇이 두 개의 비슷하게 생긴 복도를 지나간다고 하자. 복도 A의 키프레임 $i$와 복도 B의 키프레임 $j$ 사이에 잘못된 loop closure가 발생하면, pose graph optimizer는 이 두 pose를 가깝게 끌어당긴다. 두 복도 사이의 모든 pose가 왜곡되어, 맵 전체가 접히거나 뒤틀린다.

방지 전략은 여러 계층으로 이어진다.

1. **다단계 검증**: appearance similarity → geometric consistency → temporal consistency 순으로 통과해야 한다.

2. **Robust kernel 사용** (§10.2에서 상세): optimizer 자체를 이상치 제약에 덜 민감하게 만든다.

3. **[Switchable constraints (Sünderhauf & Protzel, 2012)](https://doi.org/10.1109/IROS.2012.6385590)**: 각 loop closure factor에 on/off 스위치 변수를 추가하여, optimizer가 일관성이 떨어지는 loop closure를 자동으로 비활성화한다.

4. **[DCS (Dynamic Covariance Scaling) (Agarwal et al., 2013)](https://doi.org/10.1109/ICRA.2013.6630733)**: loop closure의 공분산(uncertainty)을 동적으로 조절하여, 이상치의 영향을 자동 감쇠시킨다.

5. **[PCM (Pairwise Consistency Maximization) (Mangelson et al., 2018)](https://doi.org/10.1109/ICRA.2018.8460217)**: 여러 loop closure 후보들 간의 pairwise consistency를 검사하여, 일관된 최대 집합만 수용한다.

### 10.1.4 Correction: 그래프 보정

검증을 통과한 loop closure는 pose graph에 새로운 제약으로 들어간다. Loop closure edge는 두 pose 사이의 상대 변환 $\mathbf{T}_{ij}$와 그 불확실성 $\boldsymbol{\Sigma}_{ij}$를 인코딩한다:

$$e_{ij} = \text{Log}(\mathbf{T}_{ij}^{-1} \cdot \mathbf{T}_i^{-1} \cdot \mathbf{T}_j)$$

이 edge가 추가되면 pose graph optimizer(§10.2)가 전체 그래프를 재최적화하여 드리프트를 보정한다. loop closure edge뿐 아니라 odometry edge들도 함께 조정되어, 오차가 경로 전체에 균등히 퍼진다.

---

## 10.2 Pose Graph Optimization

Pose graph optimization은 SLAM 백엔드의 핵심이다. 프론트엔드(odometry, loop closure)가 만든 상대 제약들을 만족시키며 전체 pose trajectory의 전역 일관성을 최적화한다.

### 10.2.1 SE(3) Pose Graph

Pose graph는 그래프 $\mathcal{G} = (\mathcal{V}, \mathcal{E})$로 표현된다:

- **노드** $\mathcal{V} = \{\mathbf{T}_1, \mathbf{T}_2, \ldots, \mathbf{T}_n\}$: 각 키프레임의 pose, $\mathbf{T}_i \in SE(3)$.
- **에지** $\mathcal{E}$: 두 노드 사이의 상대적 제약. odometry edge와 loop closure edge로 이루어진다.

각 에지 $(i, j) \in \mathcal{E}$는 측정된 상대 변환 $\tilde{\mathbf{T}}_{ij}$와 정보 행렬(information matrix) $\boldsymbol{\Omega}_{ij}$를 갖는다.

**SE(3)에서의 오차 정의**: pose graph의 오차는 유클리드 공간이 아니라 Lie group SE(3) 위에서 정의한다.

$$\mathbf{e}_{ij} = \text{Log}(\tilde{\mathbf{T}}_{ij}^{-1} \cdot \mathbf{T}_i^{-1} \cdot \mathbf{T}_j) \in \mathbb{R}^6$$

여기서 $\text{Log}: SE(3) \to \mathfrak{se}(3) \cong \mathbb{R}^6$은 행렬 로그(matrix logarithm)로 Lie algebra에 매핑한다. 이 6차원 벡터는 $($ 회전 3 + 병진 3 $)$의 오차를 인코딩한다.

**최적화 목표**: 모든 에지 오차의 가중 제곱합을 최소화한다:

$$\mathbf{T}^* = \arg\min_{\mathbf{T}_1, \ldots, \mathbf{T}_n} \sum_{(i,j) \in \mathcal{E}} \mathbf{e}_{ij}^\top \boldsymbol{\Omega}_{ij} \mathbf{e}_{ij}$$

nonlinear least squares 문제이며, Gauss-Newton 또는 Levenberg-Marquardt로 푼다.

**Gauss-Newton on manifold**: SE(3)는 유클리드 공간이 아니므로, 직접 더하기(+) 연산을 사용할 수 없다. Lie algebra에서 증분(increment) $\boldsymbol{\delta}$를 계산하고, exponential map으로 manifold 위에 적용한다:

$$\mathbf{T}_i \leftarrow \mathbf{T}_i \cdot \text{Exp}(\boldsymbol{\delta}_i)$$

한 반복(iteration)에서의 업데이트:

1. 현재 추정치에서 각 에지의 오차 $\mathbf{e}_{ij}$와 Jacobian $\mathbf{J}_{ij}$를 계산한다.
2. Normal equation을 구성한다: $\mathbf{H} \boldsymbol{\delta} = -\mathbf{b}$, 여기서

$$\mathbf{H} = \sum_{(i,j)} \mathbf{J}_{ij}^\top \boldsymbol{\Omega}_{ij} \mathbf{J}_{ij}, \quad \mathbf{b} = \sum_{(i,j)} \mathbf{J}_{ij}^\top \boldsymbol{\Omega}_{ij} \mathbf{e}_{ij}$$

3. $\mathbf{H}$는 sparse하므로 sparse Cholesky 분해로 효율적으로 풀 수 있다.
4. 증분을 적용한다: $\mathbf{T}_i \leftarrow \mathbf{T}_i \cdot \text{Exp}(\boldsymbol{\delta}_i)$.
5. 수렴할 때까지 반복한다.

**[g2o (General Graph Optimization)](https://doi.org/10.1109/ICRA.2011.5979949)**: Kümmerle et al. (2011)이 개발한 프레임워크로, 위 과정을 일반적인 그래프 최적화 문제에 적용한다. 사용자는 vertex(노드) 타입과 edge(제약) 타입만 정의하면, sparse 최적화 엔진이 자동으로 돌아간다.

```python
import numpy as np
from scipy.spatial.transform import Rotation

def se3_log(T):
    """SE(3) 행렬 → 6차원 벡터 (회전 + 병진)."""
    R = T[:3, :3]
    t = T[:3, 3]
    
    # SO(3) logarithm
    rot = Rotation.from_matrix(R)
    omega = rot.as_rotvec()  # (3,)
    
    theta = np.linalg.norm(omega)
    
    if theta < 1e-10:
        V_inv = np.eye(3)
    else:
        omega_hat = skew(omega)
        V_inv = (np.eye(3) 
                 - 0.5 * omega_hat 
                 + (1.0/theta**2) * (1 - theta/(2*np.tan(theta/2))) * omega_hat @ omega_hat)
    
    rho = V_inv @ t
    return np.concatenate([rho, omega])  # (6,) — [translation; rotation]

def se3_exp(xi):
    """6차원 벡터 → SE(3) 행렬."""
    rho = xi[:3]   # 병진 부분
    omega = xi[3:]  # 회전 부분
    
    theta = np.linalg.norm(omega)
    
    if theta < 1e-10:
        R = np.eye(3)
        V = np.eye(3)
    else:
        omega_hat = skew(omega)
        R = (np.eye(3) 
             + (np.sin(theta)/theta) * omega_hat 
             + ((1 - np.cos(theta))/theta**2) * omega_hat @ omega_hat)
        V = (np.eye(3) 
             + ((1 - np.cos(theta))/theta**2) * omega_hat 
             + ((theta - np.sin(theta))/theta**3) * omega_hat @ omega_hat)
    
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = V @ rho
    return T

def skew(v):
    """3차원 벡터 → 반대칭 행렬."""
    return np.array([
        [0, -v[2], v[1]],
        [v[2], 0, -v[0]],
        [-v[1], v[0], 0]
    ])

def pose_graph_error(T_i, T_j, T_ij_measured):
    """두 pose 사이의 오차 벡터 (6차원)."""
    T_ij_estimated = np.linalg.inv(T_i) @ T_j
    T_error = np.linalg.inv(T_ij_measured) @ T_ij_estimated
    return se3_log(T_error)

def pose_graph_cost(poses, edges, measurements, information_matrices):
    """
    전체 pose graph의 비용 함수.
    
    Args:
        poses: [T_0, T_1, ..., T_n] SE(3) 행렬 리스트
        edges: [(i, j), ...] 에지 인덱스 리스트
        measurements: [T_ij, ...] 측정된 상대 변환 리스트
        information_matrices: [Omega_ij, ...] 정보 행렬 리스트
        
    Returns:
        total_cost: 스칼라 비용
    """
    total_cost = 0.0
    
    for (i, j), T_ij, Omega_ij in zip(edges, measurements, information_matrices):
        e = pose_graph_error(poses[i], poses[j], T_ij)
        total_cost += e @ Omega_ij @ e
    
    return total_cost
```

### 10.2.2 Robust Kernel

실제 SLAM 시스템에서는 이상치(outlier) 측정이 불가피하다. 잘못된 loop closure, 센서 오류, 동적 객체가 원인이다. 표준 least squares 비용 함수 $\rho(x) = x^2$는 이상치에 극도로 민감하다 — 큰 오차가 비용을 지배하여 전체 해를 왜곡한다.

**Robust kernel** (M-estimator)은 큰 잔차의 영향을 제한하여 이상치에 강건한 최적화를 가능하게 한다.

| Kernel | $\rho(s)$ ($s = e^2$) | 특성 |
|--------|----------------------|------|
| Least Squares | $s$ | 이상치에 취약 |
| Huber | $\begin{cases} s & \text{if } \sqrt{s} \leq k \\ 2k\sqrt{s} - k^2 & \text{otherwise} \end{cases}$ | 임계값 $k$ 이상에서 선형 |
| Cauchy | $k^2 \log(1 + s/k^2)$ | 부드러운 감쇠 |
| Geman-McClure | $\frac{s}{k^2 + s}$ | 강한 이상치 억제 |

Robust kernel을 적용하면 비용 함수가 다음과 같이 바뀐다.

$$\sum_{(i,j)} \rho\left(\mathbf{e}_{ij}^\top \boldsymbol{\Omega}_{ij} \mathbf{e}_{ij}\right)$$

**IRLS (Iteratively Reweighted Least Squares)** 방식으로 푼다. 각 반복에서 잔차 크기에 따라 가중치를 재계산하고, 가중 least squares를 풀면 된다.

$$w_i = \rho'(s_i), \quad s_i = \mathbf{e}_i^\top \boldsymbol{\Omega}_i \mathbf{e}_i$$

여기서 $\rho'$은 위 테이블에 정의된 $\rho(s)$의 $s$에 대한 도함수이다. 이상치 에지는 작은 가중치를 받아 그 영향이 자동으로 줄어든다.

**[Switchable constraints](https://doi.org/10.1109/IROS.2012.6385590)**: Sünderhauf & Protzel (2012)은 각 loop closure factor에 이진 스위치 변수 $s_{ij} \in [0, 1]$을 도입하여, optimizer가 일관성이 없는 loop closure를 비활성화($s_{ij} \to 0$)할 수 있게 했다.

$$\rho_{\text{switch}}(\mathbf{e}_{ij}, s_{ij}) = s_{ij}^2 \mathbf{e}_{ij}^\top \boldsymbol{\Omega}_{ij} \mathbf{e}_{ij} + \lambda (1 - s_{ij})^2$$

첫째 항은 스위치가 켜져 있을 때 오차를 최소화하고, 둘째 항은 스위치를 끄는 것에 대한 페널티다. Optimizer가 이 둘을 자동으로 균형 잡는다.

**[DCS](https://doi.org/10.1109/ICRA.2013.6630733)** (Agarwal et al., 2013): 각 loop closure의 공분산을 잔차 크기에 따라 동적으로 조정한다. 이상치의 공분산이 자동으로 커져(= 불확실성 증가) 그 영향이 줄어든다.

```python
def huber_kernel(s, k=1.345):
    """Huber robust kernel."""
    sqrt_s = np.sqrt(s)
    if sqrt_s <= k:
        return s
    else:
        return 2 * k * sqrt_s - k**2

def cauchy_kernel(s, k=2.3849):
    """Cauchy robust kernel."""
    return k**2 * np.log(1 + s / k**2)

def huber_weight(s, k=1.345):
    """Huber kernel에 대한 IRLS 가중치."""
    sqrt_s = np.sqrt(s)
    if sqrt_s <= k:
        return 1.0
    else:
        return k / sqrt_s

def robust_pose_graph_cost(poses, edges, measurements, info_matrices, 
                           kernel='huber', k=1.345):
    """Robust kernel이 적용된 pose graph 비용."""
    total_cost = 0.0
    
    kernel_fn = {'huber': huber_kernel, 'cauchy': cauchy_kernel}[kernel]
    
    for (i, j), T_ij, Omega in zip(edges, measurements, info_matrices):
        e = pose_graph_error(poses[i], poses[j], T_ij)
        s = e @ Omega @ e  # Mahalanobis distance squared
        total_cost += kernel_fn(s, k)
    
    return total_cost
```

### 10.2.3 iSAM2: Incremental Smoothing and Mapping

새 키프레임이 추가될 때마다 전체 그래프를 재최적화하는 batch 방식은 대규모 환경에서 곧 한계를 드러낸다. 현대 SLAM의 대부분은 영향받는 부분만 선택적으로 업데이트하는 incremental 방식을 택한다. [iSAM2 (Kaess et al., 2012)](https://doi.org/10.1177/0278364911430419)가 그 핵심 알고리즘이다.

**핵심 아이디어**: 새 변수나 측정이 추가되면, 영향이 파급되는 범위를 파악하고 그 부분만 재계산한다.

**Bayes tree**: iSAM2의 핵심 자료구조다. Factor graph를 variable elimination하면 clique tree가 되는데, Bayes tree는 이 clique tree에 방향성을 부여한 것이다.

Factor graph의 MAP 추정은 다음과 같이 분해된다.

$$p(\mathbf{x} | \mathbf{z}) \propto \prod_k f_k(\mathbf{x}_k)$$

여기서 $f_k$는 각 factor(odometry, loop closure 등)이고, $\mathbf{x}_k$는 해당 factor에 관련된 변수들이다.

Variable elimination을 수행하면:

$$p(\mathbf{x} | \mathbf{z}) = \prod_i p(x_i | \text{Sep}(x_i))$$

여기서 $\text{Sep}(x_i)$는 clique tree에서 $x_i$의 separator — 즉, 이 변수를 제거할 때 남는 조건부 변수들이다. 이 조건부 분포 구조가 Bayes tree를 형성한다.

**Incremental update 과정**:

1. 새 factor가 추가되면 영향받는 clique만 식별한다.
2. 해당 clique와 그 조상(ancestor)만 QR 분해를 재수행한다.
3. 나머지 tree는 그대로 유지한다.

**Fluid relinearization**: 비선형 최적화에서 선형화 지점이 현재 추정치와 크게 달라진 변수만 재선형화한다. iSAM v1에서 필요했던 주기적 batch 재선형화를 완전히 없앤다.

**변수 재정렬(variable reordering)**: 새 변수 추가 시 전체 elimination order를 재계산하지 않고, 영향받는 부분만 incremental하게 재정렬한다.

iSAM2는 GTSAM 라이브러리의 핵심 엔진이며, LIO-SAM과 ORB-SLAM3의 백엔드에서 쓰인다. 대규모 환경에서도 일정 시간 내에 최적화를 마쳐 실시간 SLAM을 가능하게 한다.

```python
class SimpleIncrementalOptimizer:
    """
    iSAM2의 핵심 개념을 보여주는 간소화된 incremental optimizer.
    실제 iSAM2는 Bayes tree 기반이지만, 여기서는 직관적 이해를 위해
    affected-variable tracking 개념만 구현한다.
    """
    
    def __init__(self):
        self.poses = {}          # {id: SE(3) matrix}
        self.edges = []          # [(i, j, T_ij, Omega_ij)]
        self.affected = set()    # 재최적화 필요한 변수 ID
        
    def add_pose(self, pose_id, T_init):
        """새 pose 노드 추가."""
        self.poses[pose_id] = T_init.copy()
        self.affected.add(pose_id)
        
    def add_edge(self, i, j, T_ij, Omega_ij):
        """새 에지(제약) 추가. 연결된 변수를 affected로 표시."""
        self.edges.append((i, j, T_ij, Omega_ij))
        self.affected.add(i)
        self.affected.add(j)
        # 실제 iSAM2에서는 Bayes tree를 타고 올라가며
        # ancestor clique들도 affected로 표시한다.
        
    def add_loop_closure(self, i, j, T_ij, Omega_ij):
        """
        Loop closure 에지 추가.
        일반 에지와 동일하지만, loop closure는 먼 과거의 노드와
        현재 노드를 연결하므로 더 많은 변수가 affected된다.
        """
        self.add_edge(i, j, T_ij, Omega_ij)
        # Loop closure는 경로상의 모든 중간 pose에 영향을 미친다.
        # 실제 iSAM2에서는 Bayes tree 구조상
        # root에 가까운 clique까지 영향이 전파된다.
        for k in range(min(i, j) + 1, max(i, j)):
            if k in self.poses:
                self.affected.add(k)
    
    def optimize(self, max_iter=5):
        """
        Affected 변수들만 재최적화.
        실제 iSAM2에서는 Bayes tree의 부분적 재분해로 수행.
        """
        if not self.affected:
            return
            
        # 여기서는 간략화를 위해 전체 Gauss-Newton을 수행하되,
        # affected 변수만 업데이트한다.
        for iteration in range(max_iter):
            # 1. 관련 에지만 선별
            relevant_edges = [
                (i, j, T_ij, Omega) 
                for (i, j, T_ij, Omega) in self.edges
                if i in self.affected or j in self.affected
            ]
            
            # 2. 각 에지에 대해 오차와 Jacobian 계산
            # 3. Normal equation 구성 및 풀기
            # 4. Affected 변수에만 increment 적용
            
            # (실제 구현은 sparse linear system을 풀어야 하므로 생략)
            pass
        
        self.affected.clear()
```

---

## 10.3 Global Relocalization

Global relocalization은 로봇이 사전에 구축된 맵(prior map) 위에서 자신의 위치를 찾는 문제다. loop closure가 "이전에 내가 방문했던 곳"을 인식한다면, relocalization은 "다른 사람이 만든 맵에서 나는 어디인가"를 묻는다.

### 10.3.1 Map-Based Localization

이미 구축된 맵이 있을 때, 새로운 센서 관측을 이 맵에 정합하여 현재 pose를 추정한다.

**Visual relocalization 파이프라인**: 현재 이미지에서 특징점을 추출하고, 맵의 3D 포인트들과 2D-3D 대응을 찾은 뒤(visual word 기반 또는 직접 매칭), PnP + RANSAC으로 pose를 추정한다. 그 pose에서 tracking을 재개한다.

ORB-SLAM3의 relocalization은 DBoW2로 후보 키프레임을 검색하고, ORB 매칭으로 2D-3D 대응을 확보한 뒤, EPnP + RANSAC으로 pose를 추정한다. 이후 guided search로 추가 매칭을 얻어 정확도를 높인다.

**LiDAR relocalization**: 현재 LiDAR 스캔을 사전 맵(point cloud map)에 정합한다. Scan Context나 PointNetVLAD 같은 global descriptor로 근접 영역을 먼저 탐색하고, FPFH + RANSAC 또는 GeoTransformer로 coarse registration을 수행한 뒤, ICP/GICP로 정밀 정합한다.

### 10.3.2 Prior Map + Online Sensor

자율주행에서는 사전에 구축한 HD map(고정밀 지도) 위에서 실시간 센서 데이터로 localization하는 것이 일반적이다. 이때의 핵심 과제는:

- **맵과 환경의 불일치**: 시간이 지나면 건물이 바뀌고, 나뭇잎이 자란다. Prior map과 현재 관측 사이의 차이를 처리해야 한다.
- **Cross-modal matching**: HD map이 LiDAR로 만들어졌는데 현재 센서는 카메라뿐일 수 있다. 이종 센서 간 정합이 필요하다.
- **Initial pose 없음**: 로봇이 맵의 어디서 시작하는지 모를 때, 전체 맵에 대해 place recognition을 수행해야 한다.

### 10.3.3 Monte Carlo Localization (MCL)

MCL은 particle filter 기반의 global localization 방법이다. 로봇의 가능한 pose를 particle들로 표현하고, 센서 관측에 따라 particle 가중치를 갱신한다.

MCL은 네 단계로 반복된다.

1. **초기화**: 맵 전체에 particle을 균일하게 분포시킨다 (global uncertainty).
2. **Prediction**: 로봇 모션 모델에 따라 particle들을 이동시킨다:
$$x_t^{[k]} \sim p(x_t | u_t, x_{t-1}^{[k]})$$
3. **Update**: 현재 센서 관측과 맵에서의 예상 관측을 비교하여 각 particle의 가중치를 계산한다:
$$w_t^{[k]} = p(z_t | x_t^{[k]}, m)$$
4. **Resampling**: 가중치에 비례하여 particle을 재샘플링한다. 가중치가 높은 particle은 복제되고, 낮은 particle은 제거된다.

MCL의 핵심 장점은 multi-modal 분포를 표현한다는 것이다. 로봇이 여러 장소 중 어디에 있을지 모를 때 여러 가설을 동시에 유지한다. 관측이 쌓일수록 particle들이 올바른 위치로 수렴한다.

LiDAR 기반 MCL 예시:

```python
import numpy as np

class MonteCarloLocalization:
    """
    2D LiDAR 기반 Monte Carlo Localization.
    사전 맵: occupancy grid.
    """
    
    def __init__(self, occupancy_map, num_particles=1000, 
                 map_resolution=0.05):
        """
        Args:
            occupancy_map: 2D numpy array, 0=free, 1=occupied
            num_particles: particle 수
            map_resolution: 맵 해상도 (m/pixel)
        """
        self.map = occupancy_map
        self.resolution = map_resolution
        self.num_particles = num_particles
        
        # Particle 초기화: [x, y, theta, weight]
        self.particles = self._initialize_particles()
    
    def _initialize_particles(self):
        """맵의 free space에 균일하게 particle을 분포."""
        free_cells = np.argwhere(self.map == 0)
        
        if len(free_cells) == 0:
            raise ValueError("맵에 free space가 없습니다.")
        
        # 랜덤 free cell 선택
        indices = np.random.choice(len(free_cells), self.num_particles, 
                                   replace=True)
        selected = free_cells[indices]
        
        particles = np.zeros((self.num_particles, 4))
        particles[:, 0] = selected[:, 1] * self.resolution  # x
        particles[:, 1] = selected[:, 0] * self.resolution  # y
        particles[:, 2] = np.random.uniform(-np.pi, np.pi, 
                                             self.num_particles)  # theta
        particles[:, 3] = 1.0 / self.num_particles  # weight (균일)
        
        return particles
    
    def predict(self, delta_x, delta_y, delta_theta, 
                noise_std=[0.1, 0.1, 0.05]):
        """모션 모델로 particle 이동 + 노이즈."""
        noise = np.random.randn(self.num_particles, 3) * noise_std
        
        cos_theta = np.cos(self.particles[:, 2])
        sin_theta = np.sin(self.particles[:, 2])
        
        # 로봇 좌표계에서의 이동을 전역 좌표계로 변환
        self.particles[:, 0] += (delta_x * cos_theta 
                                  - delta_y * sin_theta + noise[:, 0])
        self.particles[:, 1] += (delta_x * sin_theta 
                                  + delta_y * cos_theta + noise[:, 1])
        self.particles[:, 2] += delta_theta + noise[:, 2]
        
        # 각도 정규화
        self.particles[:, 2] = np.arctan2(
            np.sin(self.particles[:, 2]), 
            np.cos(self.particles[:, 2])
        )
    
    def update(self, scan_ranges, scan_angles, sigma_hit=0.2):
        """
        센서 관측으로 particle 가중치 갱신.
        
        Args:
            scan_ranges: 실제 LiDAR 거리 측정 (N,)
            scan_angles: 각 빔의 각도 (N,)
            sigma_hit: 센서 노이즈 표준편차
        """
        for k in range(self.num_particles):
            x, y, theta = self.particles[k, :3]
            
            log_weight = 0.0
            for r_measured, angle in zip(scan_ranges, scan_angles):
                # 이 particle 위치에서의 예상 거리 계산 (ray casting)
                r_expected = self._ray_cast(x, y, theta + angle)
                
                # 가우시안 센서 모델
                diff = r_measured - r_expected
                log_weight += -0.5 * (diff / sigma_hit) ** 2
            
            self.particles[k, 3] = np.exp(log_weight)
        
        # 가중치 정규화
        total = np.sum(self.particles[:, 3])
        if total > 0:
            self.particles[:, 3] /= total
        else:
            self.particles[:, 3] = 1.0 / self.num_particles
    
    def resample(self):
        """Low-variance resampling."""
        weights = self.particles[:, 3]
        N = self.num_particles
        
        new_particles = np.zeros_like(self.particles)
        r = np.random.uniform(0, 1.0 / N)
        c = weights[0]
        i = 0
        
        for m in range(N):
            u = r + m / N
            while u > c:
                i += 1
                c += weights[i]
            new_particles[m] = self.particles[i].copy()
        
        new_particles[:, 3] = 1.0 / N
        self.particles = new_particles
    
    def get_estimate(self):
        """가중 평균으로 현재 pose 추정."""
        weights = self.particles[:, 3]
        x_est = np.average(self.particles[:, 0], weights=weights)
        y_est = np.average(self.particles[:, 1], weights=weights)
        
        # 각도의 가중 평균 (circular mean)
        sin_avg = np.average(np.sin(self.particles[:, 2]), weights=weights)
        cos_avg = np.average(np.cos(self.particles[:, 2]), weights=weights)
        theta_est = np.arctan2(sin_avg, cos_avg)
        
        return x_est, y_est, theta_est
    
    def _ray_cast(self, x, y, angle, max_range=30.0):
        """Bresenham 기반 간단한 ray casting."""
        dx = np.cos(angle) * self.resolution
        dy = np.sin(angle) * self.resolution
        
        cx, cy = x, y
        for step in range(int(max_range / self.resolution)):
            cx += dx
            cy += dy
            
            # 맵 좌표로 변환
            mx = int(cy / self.resolution)
            my = int(cx / self.resolution)
            
            if (mx < 0 or mx >= self.map.shape[0] or 
                my < 0 or my >= self.map.shape[1]):
                return max_range
            
            if self.map[mx, my] == 1:  # 장애물 히트
                return step * self.resolution
        
        return max_range
```

---

## 10.4 Multi-Session SLAM & Map Merging

실제 환경에서 SLAM은 한 번에 끝나지 않는 경우가 많다. 같은 건물을 여러 날에 걸쳐 매핑하거나, 여러 로봇이 동시에 다른 영역을 탐사하거나, tracking이 실패한 후 다른 지점에서 재시작하기도 한다. Multi-session SLAM은 개별 세션의 맵들을 하나의 일관된 전역 맵으로 통합한다.

### 10.4.1 Map Anchoring

여러 세션의 맵을 통합하려면, 각 맵의 좌표계(coordinate frame)를 정렬해야 한다. 이를 **map anchoring**이라 한다.

**방법 1 — 공유 랜드마크 기반**: 두 세션이 겹치는 영역에서 같은 3D 포인트나 특징점을 관측했다면, 이를 기반으로 두 맵 사이의 상대 변환 ${}^{A}\mathbf{T}_{B}$를 추정한다.

$${}^{A}\mathbf{T}_{B} = \arg\min_{\mathbf{T}} \sum_k \| \mathbf{p}_k^A - \mathbf{T} \cdot \mathbf{p}_k^B \|^2$$

**방법 2 — Place recognition 기반**: 공유 랜드마크가 없더라도, place recognition으로 두 세션에서 같은 장소를 방문한 키프레임 쌍을 찾아 정렬한다.

**방법 3 — GNSS 기반**: 두 세션 모두 GNSS 정보가 있다면, 이를 공유 좌표계로 사용하여 직접 정렬한다.

### 10.4.2 Inter-Session Loop Closure

Map anchoring이 초기 정렬을 제공하면, inter-session loop closure가 정밀한 정합을 수행한다. 원리는 일반 loop closure와 같지만 두 가지 도전이 더 있다.

1. **외관 변화**: 시간이 지나면 조명과 계절이 변한다. AnyLoc 같은 foundation model 기반 디스크립터가 이 문제에 강하다.

2. **좌표계 불일치**: 초기 정렬이 부정확할 수 있으므로, geometric verification의 tolerance를 높여야 한다.

### 10.4.3 ORB-SLAM3 Multi-Map System

ORB-SLAM3의 Atlas 시스템은 multi-session SLAM의 대표적 구현이다. 핵심 메커니즘은 다음과 같다:

1. **Active map**: 현재 tracking 중인 맵. 정상적으로 동작할 때는 이 맵에서 키프레임과 맵 포인트를 추가한다.

2. **Map creation**: tracking이 실패하면(예: 시야가 가려지거나, textureless 환경), 새 맵을 만들고 active map으로 설정한다. 이전 맵은 Atlas에 비활성 상태로 남는다.

3. **Map merging**: place recognition으로 현재 active map과 Atlas의 비활성 맵 사이에서 공통 장소가 검출되면, 두 맵을 병합한다:
   - 공통 키프레임 쌍에서 상대 변환 $\mathbf{T}_{merge}$를 추정한다.
   - 비활성 맵의 모든 키프레임과 맵 포인트를 $\mathbf{T}_{merge}$로 변환한다.
   - 공통 맵 포인트를 fusion한다.
   - Welding bundle adjustment로 병합 영역의 일관성을 확보한다.

4. **Multi-session 활용**: 이전 세션의 맵을 로드하고, 현재 세션에서 같은 장소를 방문하면 자동으로 병합된다. 여러 세션에 걸쳐 점진적으로 맵을 확장할 수 있다.

```python
class MultiMapAtlas:
    """
    ORB-SLAM3 Atlas 시스템의 핵심 개념.
    """
    
    def __init__(self):
        self.maps = {}           # {map_id: MapData}
        self.active_map_id = None
        self.next_map_id = 0
    
    def create_new_map(self, initial_pose):
        """새 맵을 생성하고 active로 설정."""
        map_id = self.next_map_id
        self.next_map_id += 1
        
        self.maps[map_id] = {
            'keyframes': [initial_pose],
            'map_points': [],
            'is_active': True,
            'origin': initial_pose.copy()
        }
        
        # 이전 active map 비활성화
        if self.active_map_id is not None:
            self.maps[self.active_map_id]['is_active'] = False
        
        self.active_map_id = map_id
        return map_id
    
    def on_tracking_lost(self, last_pose):
        """
        Tracking 실패 시: 현재 맵을 보관하고 새 맵 생성.
        """
        print(f"Tracking lost. Map {self.active_map_id} "
              f"saved with {len(self.maps[self.active_map_id]['keyframes'])} "
              f"keyframes.")
        self.create_new_map(last_pose)
    
    def try_merge_maps(self, current_kf_descriptor, current_kf_pose):
        """
        현재 키프레임의 디스크립터로 비활성 맵에서 매칭을 탐색.
        매칭이 발견되면 두 맵을 병합.
        """
        for map_id, map_data in self.maps.items():
            if map_id == self.active_map_id:
                continue
            
            # Place recognition으로 비활성 맵의 키프레임과 매칭 탐색
            match_idx = self._search_in_map(current_kf_descriptor, map_id)
            
            if match_idx is not None:
                # 기하학적 검증
                T_merge = self._compute_merge_transform(
                    current_kf_pose, 
                    map_data['keyframes'][match_idx]
                )
                
                if T_merge is not None:
                    self._merge_maps(self.active_map_id, map_id, T_merge)
                    return True
        
        return False
    
    def _merge_maps(self, active_id, merge_id, T_merge):
        """
        merge_id 맵을 active_id 맵으로 병합.
        merge_id의 모든 키프레임/맵포인트를 T_merge로 변환.
        """
        merge_map = self.maps[merge_id]
        active_map = self.maps[active_id]
        
        # 비활성 맵의 키프레임을 변환하여 active 맵에 추가
        for kf in merge_map['keyframes']:
            transformed_kf = T_merge @ kf
            active_map['keyframes'].append(transformed_kf)
        
        # 맵 포인트도 변환하여 추가
        for mp in merge_map['map_points']:
            transformed_mp = T_merge @ mp
            active_map['map_points'].append(transformed_mp)
        
        # 병합된 맵 제거
        del self.maps[merge_id]
        
        print(f"Maps {active_id} and {merge_id} merged. "
              f"Total keyframes: {len(active_map['keyframes'])}")
    
    def _search_in_map(self, descriptor, map_id):
        """비활성 맵에서 place recognition 수행 (placeholder)."""
        return None  # 실제 구현 시 디스크립터 비교
    
    def _compute_merge_transform(self, pose_a, pose_b):
        """두 pose 사이의 변환 계산 (placeholder)."""
        return None  # 실제 구현 시 기하학적 검증 포함
```

### 10.4.4 Multi-Robot Map Merging

여러 로봇이 동시에 탐사하는 경우, 각 로봇의 맵을 실시간으로 통합해야 한다. 이때 추가 제약이 붙는다:

- **통신 대역폭**: 전체 포인트 클라우드를 전송할 수 없으므로, 압축된 디스크립터(Scan Context, compact visual descriptor)만 교환한다.
- **분산 최적화**: 중앙 서버 없이 로봇들이 자율적으로 맵을 병합할 수 있어야 한다. Kimera-Multi와 Swarm-SLAM이 이 문제를 본다.
- **Relative pose 불확실성**: 로봇 간 초기 상대 pose가 알려져 있지 않으므로, inter-robot loop closure로 정렬해야 한다.

분산 pose graph optimization의 핵심 아이디어:

$$\mathbf{T}^* = \arg\min \sum_{\text{robot } r} \sum_{(i,j) \in \mathcal{E}_r} \rho(\mathbf{e}_{ij}) + \sum_{(i,j) \in \mathcal{E}_{\text{inter}}} \rho(\mathbf{e}_{ij})$$

각 로봇은 자신의 에지 $\mathcal{E}_r$에 대한 최적화를 로컬에서 수행하고, inter-robot 에지 $\mathcal{E}_{\text{inter}}$에 대해서만 정보를 교환한다. ADMM (Alternating Direction Method of Multipliers)이나 Gauss-Seidel iteration으로 분산적으로 수렴한다.

---

## 10.5 최근 연구 (2024-2025)

**[riSAM (McGann et al., 2023)](https://arxiv.org/abs/2209.14359)**: iSAM2에 Graduated Non-Convexity(GNC)를 통합하여 incremental SLAM에서 온라인으로 이상치 loop closure를 제거하는 robust backend다. 90% 이상의 outlier 측정에서도 강건하게 동작하며, 기존 offline 방법과 동등한 성능을 실시간으로 달성한다. GNC의 이론적 토대는 [Yang et al. 2020](https://arxiv.org/abs/1909.08605)이 마련했다.

**[Kimera2 (Abate et al., 2024)](https://arxiv.org/abs/2401.06323)**: Kimera SLAM 라이브러리의 차세대 버전으로, 백엔드의 outlier rejection을 기존 PCM에서 GNC로 교체하여 강건성을 높였다. 드론과 자율주행 차량을 포함한 다양한 플랫폼에서 실증했으며, metric-semantic SLAM의 실전 배포를 위한 종합적인 개선을 담았다.

**[Group-k Consistent Measurement Set Maximization (Forsgren & Kaess, 2022)](https://arxiv.org/abs/2209.02658)**: PCM의 pairwise consistency를 group-k consistency로 확장하여, 더 엄격한 이상치 탐지를 가능하게 한다. Multi-robot map merging에서 PCM보다 false positive를 더 억제한다.

---

Loop closure와 전역 최적화로 SLAM 시스템은 전역적으로 일관된 궤적과 맵을 만든다. 그런데 이 "맵"은 어떤 형태인가? 점군일 수도, 신경망 가중치일 수도 있다. 다음 챕터에서는 센서 퓨전의 최종 결과물인 공간 표현(spatial representation)의 다양한 형태와 그 장단점을 본다.

---

# Ch.11 — Spatial Representations

Ch.6-10에서 센서 데이터로부터 로봇의 궤적을 추정하고, loop closure로 전역 일관성을 확보하는 과정을 다루었다. 이 챕터에서는 그 과정의 부산물이자 궁극적 목적인 맵 — 로봇이 세상을 기억하고 활용하는 형태 — 을 다룬다.

센서 퓨전의 궁극적 결과물은 **맵(map)** — 환경의 공간적 표현 — 이다. SLAM 시스템이 추정하는 것은 로봇의 궤적뿐 아니라, 그 궤적을 통해 관측한 환경의 구조이다. 어떤 형태로 환경을 표현하느냐에 따라 로봇이 할 수 있는 일이 달라진다. 경로 계획은 free/occupied 정보를 요구하고, 시각적 렌더링은 텍스처를 요구하며, 인간과의 상호작용은 의미론적 레이블을 요구한다.

이 챕터에서는 metric map(정량적 기하 맵)에서 출발하여, mesh, neural representation, semantic map, 그리고 장기 유지(long-term maintenance)까지 공간 표현의 전체 스펙트럼을 다룬다.

---

## 11.1 Metric Maps

Metric map은 환경의 기하학적 구조를 정량적 좌표로 표현한다. 가장 기본적이면서도 여전히 가장 널리 사용되는 형태다.

### 11.1.1 Occupancy Grid (2D/3D)

Occupancy grid는 공간을 균일한 격자(grid)로 분할하고, 각 셀이 occupied/free/unknown 상태를 갖는 확률적 표현이다.

**2D occupancy grid**: 평면 환경(실내, 단층)에서 사용한다. 각 셀 $m_i$에 대해 occupancy 확률 $p(m_i \mid z_{1:t})$를 베이즈 갱신으로 유지한다.

$$\text{log-odds}(m_i \mid z_{1:t}) = \text{log-odds}(m_i \mid z_{1:t-1}) + \text{log-odds}(m_i \mid z_t) - \text{log-odds}(m_i)$$

여기서 log-odds 표현을 사용하면 곱셈이 덧셈이 되어 계산이 효율적이고, 수치적으로도 안정적이다:

$$l(m_i) = \log\frac{p(m_i)}{1 - p(m_i)}$$

**문제**: 3D 환경을 표현하려면 3D occupancy grid가 필요한데, 해상도 $r$로 $L \times W \times H$ 공간을 표현하면 $(L/r)(W/r)(H/r)$ 개의 셀이 필요하다. 예를 들어 100 m $\times$ 100 m $\times$ 10 m 공간을 5 cm 해상도로 표현하면 약 $8 \times 10^9$개의 셀, 약 32 GB 메모리가 필요하다. 이는 비현실적이다.

```python
import numpy as np

class OccupancyGrid2D:
    """2D Occupancy Grid — log-odds 기반 베이즈 갱신."""
    
    def __init__(self, width_m, height_m, resolution=0.05):
        """
        Args:
            width_m: 맵 너비 (미터)
            height_m: 맵 높이 (미터)
            resolution: 셀 크기 (미터)
        """
        self.resolution = resolution
        self.width = int(width_m / resolution)
        self.height = int(height_m / resolution)
        
        # Log-odds 맵: 0 = unknown (prior = 0.5)
        self.log_odds = np.zeros((self.height, self.width), dtype=np.float32)
        
        # 센서 모델 파라미터
        self.l_occ = 0.85   # log-odds(occupied | hit)
        self.l_free = -0.40  # log-odds(free | pass-through)
        
        # Clipping 범위 (확률 포화 방지)
        self.l_min = -5.0
        self.l_max = 5.0
    
    def update(self, robot_x, robot_y, scan_ranges, scan_angles):
        """
        LiDAR 스캔으로 맵 갱신.
        
        Args:
            robot_x, robot_y: 로봇 위치 (미터)
            scan_ranges: 각 빔의 거리 (N,)
            scan_angles: 각 빔의 각도 (N,)
        """
        rx = int(robot_x / self.resolution)
        ry = int(robot_y / self.resolution)
        
        for r, angle in zip(scan_ranges, scan_angles):
            # 빔의 끝점 (occupied)
            ex = int((robot_x + r * np.cos(angle)) / self.resolution)
            ey = int((robot_y + r * np.sin(angle)) / self.resolution)
            
            # Bresenham line: 로봇 → 끝점 사이 = free
            cells = self._bresenham(rx, ry, ex, ey)
            for cx, cy in cells[:-1]:  # 마지막 셀 제외 (그건 occupied)
                if 0 <= cx < self.width and 0 <= cy < self.height:
                    self.log_odds[cy, cx] = np.clip(
                        self.log_odds[cy, cx] + self.l_free,
                        self.l_min, self.l_max
                    )
            
            # 끝점 셀 = occupied
            if 0 <= ex < self.width and 0 <= ey < self.height:
                self.log_odds[ey, ex] = np.clip(
                    self.log_odds[ey, ex] + self.l_occ,
                    self.l_min, self.l_max
                )
    
    def get_probability_map(self):
        """Log-odds → 확률 변환."""
        return 1.0 - 1.0 / (1.0 + np.exp(self.log_odds))
    
    def _bresenham(self, x0, y0, x1, y1):
        """Bresenham 직선 알고리즘 — 두 점 사이의 격자 셀 반환."""
        cells = []
        dx = abs(x1 - x0)
        dy = abs(y1 - y0)
        sx = 1 if x0 < x1 else -1
        sy = 1 if y0 < y1 else -1
        err = dx - dy
        
        while True:
            cells.append((x0, y0))
            if x0 == x1 and y0 == y1:
                break
            e2 = 2 * err
            if e2 > -dy:
                err -= dy
                x0 += sx
            if e2 < dx:
                err += dx
                y0 += sy
        
        return cells
```

### 11.1.2 Voxel Maps: OctoMap, VDB, ikd-tree

균일 격자의 메모리 문제를 해결하기 위해, 다양한 적응적(adaptive) 자료구조가 제안되었다.

**[OctoMap](https://doi.org/10.1007/s10514-012-9321-0)** (Hornung et al. 2013): octree 기반의 확률적 3D occupancy map. 공간을 재귀적으로 8등분하여, occupied 또는 free인 영역은 조기에 분할을 중단(pruning)한다. 이를 통해 빈 공간은 큰 단위로, 세밀한 구조 근처만 작은 단위로 표현하여 메모리를 절약한다.

핵심 특성:
- **확률적 갱신**: occupancy grid와 동일한 log-odds 갱신 사용
- **적응적 해상도**: 최고 해상도(예: 2 cm, leaf node)부터 최저 해상도(예: 수 m, root 근처)까지 자동 조절
- **메모리 효율**: 64 m$^3$ 공간을 1 cm 해상도로 약 60 MB로 표현 가능 (균일 격자 대비 수백 배 절약)
- **한계**: 동적 삽입/삭제 시 tree 재균형 비용, 최근접 이웃 검색(kNN)이 느림

```python
class SimpleOctreeNode:
    """OctoMap의 핵심 개념 — 재귀적 8분할 octree."""
    
    def __init__(self, center, size, depth=0, max_depth=16):
        self.center = np.array(center)  # 노드 중심 좌표
        self.size = size                 # 노드 한 변 길이
        self.depth = depth
        self.max_depth = max_depth
        self.children = [None] * 8       # 8개 자식
        self.log_odds = 0.0              # occupancy log-odds
        self.is_leaf = True
    
    def get_child_index(self, point):
        """포인트가 어느 자식 octant에 속하는지 결정."""
        idx = 0
        if point[0] >= self.center[0]: idx |= 1
        if point[1] >= self.center[1]: idx |= 2
        if point[2] >= self.center[2]: idx |= 4
        return idx
    
    def get_child_center(self, child_idx):
        """자식 octant의 중심 좌표 계산."""
        offset = self.size / 4
        center = self.center.copy()
        center[0] += offset if (child_idx & 1) else -offset
        center[1] += offset if (child_idx & 2) else -offset
        center[2] += offset if (child_idx & 4) else -offset
        return center
    
    def update(self, point, is_occupied, l_occ=0.85, l_free=-0.40):
        """포인트로 octree 갱신."""
        if self.depth >= self.max_depth:
            # 최대 깊이 도달: leaf에서 log-odds 갱신
            self.log_odds += l_occ if is_occupied else l_free
            self.log_odds = np.clip(self.log_odds, -5.0, 5.0)
            return
        
        # 자식 octant 결정
        child_idx = self.get_child_index(point)
        
        if self.children[child_idx] is None:
            self.children[child_idx] = SimpleOctreeNode(
                self.get_child_center(child_idx),
                self.size / 2,
                self.depth + 1,
                self.max_depth
            )
            self.is_leaf = False
        
        self.children[child_idx].update(point, is_occupied, l_occ, l_free)
    
    def prune(self):
        """
        모든 자식이 같은 상태면 병합 (pruning).
        이것이 OctoMap의 핵심 메모리 절약 기법.
        """
        if self.is_leaf:
            return
        
        # 모든 자식이 존재하고 leaf이며 같은 상태인지 확인
        all_same = True
        first_odds = None
        
        for child in self.children:
            if child is None or not child.is_leaf:
                return  # pruning 불가
            
            if first_odds is None:
                first_odds = child.log_odds
            elif abs(child.log_odds - first_odds) > 0.01:
                all_same = False
                break
        
        if all_same and first_odds is not None:
            self.log_odds = first_odds
            self.children = [None] * 8
            self.is_leaf = True
```

**OpenVDB**: 영화 VFX 산업에서 유래한 sparse volumetric 자료구조. 해시 맵 기반으로 활성화된 voxel만 저장하여, 넓은 공간에서 극소수의 voxel만 occupied인 경우 매우 효율적이다. OctoMap보다 탐색이 빠르지만, 확률적 갱신 기능은 사용자가 직접 구현해야 한다.

**ikd-tree** (FAST-LIO2): incremental k-d tree로, 포인트 삽입과 삭제를 $O(\log n)$에 수행하며 동적 재균형을 지원한다. FAST-LIO2에서 맵 자료구조로 사용되어, LiDAR 포인트를 실시간으로 맵에 추가하면서 kNN을 효율적으로 수행한다.

ikd-tree의 핵심 연산:
- **삽입**: 새 포인트를 k-d tree에 삽입. 불균형이 임계값을 초과하면 부분적으로 재균형.
- **삭제**: 일정 범위 밖의 오래된 포인트를 lazy deletion으로 제거.
- **kNN 검색**: 관측 포인트의 최근접 맵 포인트를 찾아 point-to-plane 정합에 사용.

OctoMap vs ikd-tree 비교:

| 특성 | OctoMap | ikd-tree |
|------|---------|----------|
| 자료구조 | Octree | k-d tree |
| 확률적 갱신 | 내장 (log-odds) | 없음 (포인트 저장만) |
| kNN 검색 | 느림 | 빠름 |
| 동적 삽입/삭제 | 가능하지만 느림 | $O(\log n)$ |
| 용도 | 경로 계획, 탐사 | LiDAR odometry 맵 유지 |

### 11.1.3 Surfel Maps

Surfel(surface element)은 포인트에 법선 벡터와 반경 정보를 추가한 디스크(disk) 형태의 표면 원소다. 각 surfel은 $(\mathbf{p}, \mathbf{n}, r, c)$로 표현된다:

- $\mathbf{p} \in \mathbb{R}^3$: 위치
- $\mathbf{n} \in \mathbb{R}^3$: 법선 벡터 (단위)
- $r \in \mathbb{R}^+$: 반경
- $c$: 색상/신뢰도

**[ElasticFusion](https://doi.org/10.15607/RSS.2015.XI.001)** (Whelan et al. 2015): RGB-D 센서로부터 surfel map을 실시간 구축하는 dense SLAM 시스템. 핵심 아이디어는:

1. Frame-to-model tracking: 현재 프레임을 surfel map의 렌더링과 정합한다.
2. Map deformation: loop closure 시 embedded deformation graph로 surfel map 전체를 비강체적으로 변형한다.
3. Surfel fusion: 기존 surfel과 새 관측이 겹치면 가중 평균으로 병합한다.

Surfel은 명시적 mesh 없이도 연속적 표면을 표현할 수 있다. 포인트 클라우드보다 표면 정보가 풍부하면서도, mesh보다 갱신이 간단하다.

```python
class Surfel:
    """하나의 Surface Element."""
    
    def __init__(self, position, normal, radius, color, confidence=1.0):
        self.position = np.array(position, dtype=np.float64)  # (3,)
        self.normal = np.array(normal, dtype=np.float64)      # (3,)
        self.radius = float(radius)
        self.color = np.array(color, dtype=np.float64)        # (3,) RGB
        self.confidence = confidence
        self.update_count = 1
    
    def fuse(self, new_pos, new_normal, new_radius, new_color, 
             new_confidence=1.0):
        """새 관측으로 surfel 갱신 (가중 평균)."""
        total_w = self.confidence + new_confidence
        
        self.position = (self.confidence * self.position 
                         + new_confidence * new_pos) / total_w
        
        # 법선: 가중 평균 후 정규화
        avg_normal = (self.confidence * self.normal 
                      + new_confidence * new_normal) / total_w
        norm = np.linalg.norm(avg_normal)
        if norm > 1e-6:
            self.normal = avg_normal / norm
        
        self.radius = (self.confidence * self.radius 
                       + new_confidence * new_radius) / total_w
        self.color = (self.confidence * self.color 
                      + new_confidence * new_color) / total_w
        
        self.confidence = min(total_w, 100.0)  # confidence 상한
        self.update_count += 1

class SurfelMap:
    """간단한 Surfel Map 구현."""
    
    def __init__(self, fusion_distance=0.02, fusion_normal_threshold=0.9):
        self.surfels = []
        self.fusion_dist = fusion_distance
        self.fusion_normal_thresh = fusion_normal_threshold
    
    def integrate(self, points, normals, radii, colors):
        """
        새 관측을 맵에 통합.
        기존 surfel과 가까우면 fusion, 아니면 새로 추가.
        """
        for p, n, r, c in zip(points, normals, radii, colors):
            fused = False
            
            for surfel in self.surfels:
                dist = np.linalg.norm(surfel.position - p)
                normal_dot = abs(np.dot(surfel.normal, n))
                
                if (dist < self.fusion_dist and 
                    normal_dot > self.fusion_normal_thresh):
                    surfel.fuse(p, n, r, c)
                    fused = True
                    break
            
            if not fused:
                self.surfels.append(Surfel(p, n, r, c))
```

---

## 11.2 Mesh & CAD-level Maps

Mesh는 삼각형(triangle) 면의 집합으로 표면을 표현한다. Voxel이나 포인트 클라우드보다 시각적으로 자연스럽고, 물리 시뮬레이션(충돌 감지 등)에도 직접 활용할 수 있다.

### 11.2.1 TSDF (Truncated Signed Distance Function)

TSDF는 각 voxel에 가장 가까운 표면까지의 부호 거리(signed distance)를 저장한다:

- 양수: 표면 앞 (free space)
- 음수: 표면 뒤 (occupied)
- zero crossing: 실제 표면 위치

**TSDF 갱신**: 새 depth 관측이 들어올 때마다 해당 시선(ray)을 따라 voxel을 갱신한다:

$$\text{TSDF}(\mathbf{v}) \leftarrow \frac{W(\mathbf{v}) \cdot \text{TSDF}(\mathbf{v}) + w_{\text{new}} \cdot d_{\text{new}}}{W(\mathbf{v}) + w_{\text{new}}}$$

$$W(\mathbf{v}) \leftarrow \min(W(\mathbf{v}) + w_{\text{new}}, W_{\max})$$

여기서 $d_{\text{new}}$는 새로운 관측에서 계산한 부호 거리, $w_{\text{new}}$는 관측 가중치, $W(\mathbf{v})$는 누적 가중치다. 부호 거리는 truncation distance $\delta$ 이내로 제한(truncate)된다:

$$d_{\text{new}} = \text{clip}(D(\mathbf{u}) - \|\mathbf{v} - \mathbf{c}\|, -\delta, \delta)$$

$D(\mathbf{u})$는 픽셀 $\mathbf{u}$의 depth 값, $\mathbf{c}$는 카메라 위치, $\mathbf{v}$는 voxel 중심이다.

```python
class TSDFVolume:
    """
    TSDF 볼륨 — RGB-D 프레임으로부터 3D 재구성.
    """
    
    def __init__(self, volume_bounds, voxel_size=0.02):
        """
        Args:
            volume_bounds: [[x_min, x_max], [y_min, y_max], [z_min, z_max]]
            voxel_size: voxel 한 변 크기 (미터)
        """
        self.voxel_size = voxel_size
        self.bounds = np.array(volume_bounds)
        
        self.dims = np.ceil(
            (self.bounds[:, 1] - self.bounds[:, 0]) / voxel_size
        ).astype(int)
        
        # TSDF 값과 가중치
        self.tsdf = np.ones(self.dims) * 1.0  # 초기값: truncation 값
        self.weight = np.zeros(self.dims, dtype=np.float32)
        self.color = np.zeros((*self.dims, 3), dtype=np.float32)
        
        # Truncation distance
        self.trunc_dist = 3.0 * voxel_size
    
    def integrate(self, depth_image, color_image, K, T_camera_to_world):
        """
        하나의 RGB-D 프레임을 TSDF에 통합.
        
        Args:
            depth_image: (H, W) depth (미터)
            color_image: (H, W, 3) RGB (0~255)
            K: 카메라 내부 파라미터 (3, 3)
            T_camera_to_world: 카메라 pose (4, 4)
        """
        T_world_to_camera = np.linalg.inv(T_camera_to_world)
        cam_pos = T_camera_to_world[:3, 3]
        
        H, W = depth_image.shape
        fx, fy = K[0, 0], K[1, 1]
        cx, cy = K[0, 2], K[1, 2]
        
        # 각 voxel에 대해
        for ix in range(self.dims[0]):
            for iy in range(self.dims[1]):
                for iz in range(self.dims[2]):
                    # Voxel 중심의 월드 좌표
                    vx = self.bounds[0, 0] + (ix + 0.5) * self.voxel_size
                    vy = self.bounds[1, 0] + (iy + 0.5) * self.voxel_size
                    vz = self.bounds[2, 0] + (iz + 0.5) * self.voxel_size
                    
                    # 카메라 좌표로 변환
                    p_world = np.array([vx, vy, vz, 1.0])
                    p_cam = T_world_to_camera @ p_world
                    
                    if p_cam[2] <= 0:
                        continue
                    
                    # 이미지 좌표로 투영
                    u = int(fx * p_cam[0] / p_cam[2] + cx)
                    v = int(fy * p_cam[1] / p_cam[2] + cy)
                    
                    if u < 0 or u >= W or v < 0 or v >= H:
                        continue
                    
                    depth = depth_image[v, u]
                    if depth <= 0:
                        continue
                    
                    # 부호 거리 계산
                    sdf = depth - p_cam[2]
                    
                    if sdf < -self.trunc_dist:
                        continue
                    
                    tsdf_val = min(sdf / self.trunc_dist, 1.0)
                    
                    # 가중 평균 갱신
                    w_old = self.weight[ix, iy, iz]
                    w_new = 1.0
                    
                    self.tsdf[ix, iy, iz] = (
                        (w_old * self.tsdf[ix, iy, iz] + w_new * tsdf_val) 
                        / (w_old + w_new)
                    )
                    self.color[ix, iy, iz] = (
                        (w_old * self.color[ix, iy, iz] 
                         + w_new * color_image[v, u].astype(float))
                        / (w_old + w_new)
                    )
                    self.weight[ix, iy, iz] = min(w_old + w_new, 100.0)
    
    def extract_mesh(self):
        """
        Marching Cubes로 TSDF에서 mesh 추출.
        zero crossing을 찾아 삼각형 면을 생성.
        """
        # Marching Cubes 알고리즘:
        # 각 voxel cube의 8개 꼭짓점에서 TSDF 부호를 확인하고,
        # 부호가 바뀌는 edge 위에 vertex를 보간하여 삼각형을 생성.
        
        vertices = []
        faces = []
        
        for ix in range(self.dims[0] - 1):
            for iy in range(self.dims[1] - 1):
                for iz in range(self.dims[2] - 1):
                    # 8개 꼭짓점의 TSDF 값
                    cube = np.array([
                        self.tsdf[ix, iy, iz],
                        self.tsdf[ix+1, iy, iz],
                        self.tsdf[ix+1, iy+1, iz],
                        self.tsdf[ix, iy+1, iz],
                        self.tsdf[ix, iy, iz+1],
                        self.tsdf[ix+1, iy, iz+1],
                        self.tsdf[ix+1, iy+1, iz+1],
                        self.tsdf[ix, iy+1, iz+1],
                    ])
                    
                    # 부호 변화가 있는 경우에만 처리
                    if np.all(cube > 0) or np.all(cube < 0):
                        continue
                    
                    # 실제 Marching Cubes는 256가지 경우의
                    # look-up table을 사용하여 삼각형을 생성.
                    # 여기서는 개념만 표시.
                    pass
        
        return vertices, faces
```

### 11.2.2 Voxblox

**[Voxblox](https://arxiv.org/abs/1611.03631)** (Oleynikova et al. 2017)는 TSDF 기반 실시간 3D 재구성 시스템으로, 경로 계획에 필수적인 **ESDF (Euclidean Signed Distance Field)**를 효율적으로 계산한다.

핵심 파이프라인:

1. **TSDF 통합**: RGB-D 또는 depth 포인트 클라우드를 TSDF에 projective 방식으로 통합.
2. **Mesh 추출**: TSDF에서 Marching Cubes로 mesh를 incremental하게 추출. 변경된 voxel 블록만 재처리.
3. **ESDF 계산**: TSDF에서 ESDF를 계산. ESDF는 각 voxel에서 가장 가까운 장애물까지의 유클리드 거리를 저장한다.

ESDF가 중요한 이유: 경로 계획에서 로봇이 장애물로부터 안전 거리를 유지해야 하는데, ESDF가 있으면 임의의 점에서 장애물까지의 거리를 $O(1)$에 조회할 수 있다. 그래디언트 정보도 함께 제공하여, 장애물을 피하는 방향을 즉시 알 수 있다.

### 11.2.3 Poisson Surface Reconstruction

Poisson reconstruction은 oriented point cloud(위치 + 법선)로부터 수밀(watertight) mesh를 생성하는 방법이다.

**핵심 아이디어**: 법선 벡터를 gradient field로 해석하고, 이 gradient의 divergence를 구하여 Poisson 방정식을 푼다:

$$\nabla^2 \chi = \nabla \cdot \mathbf{V}$$

여기서 $\mathbf{V}$는 법선 벡터 필드, $\chi$는 indicator function(표면 안쪽 = 1, 바깥쪽 = 0)이다. 이 PDE를 octree 위에서 효율적으로 풀어 iso-surface를 추출한다.

장점은 노이즈 강건성과 수밀 mesh 생성이다. 포인트 밀도가 불균일해도 잘 동작한다. 실시간 SLAM에는 적합하지 않아 TSDF 기반 방법이 선호된다.

### 11.2.4 실시간 Mesh 생성

SLAM 시스템에서 실시간으로 mesh를 생성하는 현대적 접근:

1. **Voxblox incremental meshing**: TSDF가 갱신된 voxel 블록에서만 Marching Cubes를 재실행. 전체 볼륨을 재처리하지 않으므로 실시간 가능.
2. **FAST-LIVO2 mesh**: LiDAR 포인트에 카메라 색상을 부착하여 colored mesh를 실시간 생성. 통합 voxel map에서 Poisson 또는 Ball Pivoting으로 후처리.

---

## 11.3 Neural / Learned Representations

앞서 다룬 voxel, mesh, surfel 표현은 모두 명시적(explicit)이다 — 3D 구조를 직접 저장한다. 반면, neural representation은 **암묵적(implicit)** 방식으로 3D 장면을 신경망의 가중치에 인코딩한다.

### 11.3.1 NeRF-SLAM: Neural Implicit + Odometry

**[NeRF](https://arxiv.org/abs/2003.08934) (Neural Radiance Field)** (Mildenhall et al. 2020)는 3D 좌표와 시선 방향을 입력받아 색상과 밀도를 출력하는 MLP로 장면을 표현한다:

$$F_\theta: (\mathbf{x}, \mathbf{d}) \to (c, \sigma)$$

여기서 $\mathbf{x} \in \mathbb{R}^3$은 3D 위치, $\mathbf{d} \in \mathbb{S}^2$은 시선 방향, $c$는 RGB 색상, $\sigma$는 체적 밀도(volume density)다.

**볼륨 렌더링 방정식**: 카메라 ray $\mathbf{r}(t) = \mathbf{o} + t\mathbf{d}$를 따라 적분하여 픽셀 색상을 합성한다:

$$\hat{C}(\mathbf{r}) = \int_{t_n}^{t_f} T(t) \sigma(\mathbf{r}(t)) c(\mathbf{r}(t), \mathbf{d}) \, dt$$

여기서 $T(t) = \exp\left(-\int_{t_n}^{t} \sigma(\mathbf{r}(s)) ds\right)$는 누적 투과율(accumulated transmittance)이다.

**NeRF-SLAM 파이프라인**: 기존 NeRF가 offline reconstruction이었다면, NeRF-SLAM은 SLAM과 결합하여 online으로 동작한다:

1. **Tracking**: 현재 프레임의 camera pose를 기존 neural map에 대해 최적화한다 (photometric loss + depth loss).
2. **Mapping**: 추정된 pose에서 neural network 가중치를 갱신한다. 새로운 관측 영역을 학습하면서 기존 영역의 일관성도 유지해야 한다.

**[iMAP](https://arxiv.org/abs/2103.12352)** (Sucar et al. 2021): 최초의 neural implicit SLAM. 단일 MLP로 장면을 표현. 실시간성을 위해 keyframe 기반 학습과 active sampling 전략을 사용.

한계는 두 가지다. MLP 학습 속도(training speed)가 느려 실시간 mapping에 제약이 있고, 새 영역을 학습하면 이전 영역의 표현이 퇴화하는 catastrophic forgetting이 발생한다. 대규모 환경에서는 단일 MLP의 용량 한계도 드러난다.

개선 방향으로는 Instant-NGP가 주목된다. hash grid 기반 feature encoding으로 학습 속도를 수십 배 향상시켰고, 이를 활용한 NeRF-SLAM 변형이 등장했다. Instant-NGP의 핵심은 공간을 여러 해상도의 해시 테이블로 나누는 multi-resolution hash grid다.

**[NICE-SLAM](https://arxiv.org/abs/2112.12130)** (Zhu et al. 2022): iMAP의 확장성 문제를 해결하기 위해 계층적 feature grid와 사전학습된 기하 디코더를 도입한 dense SLAM 시스템. 대규모 실내 환경에서도 안정적으로 동작하며, CVPR 2022에서 발표되었다.

### 11.3.2 3DGS-SLAM: 3D Gaussian Splatting 기반 SLAM

**[3D Gaussian Splatting (3DGS)](https://arxiv.org/abs/2308.04079)** (Kerbl et al. 2023)은 NeRF의 대안으로 등장한 표현으로, 장면을 수백만 개의 3D Gaussian으로 표현한다. 각 Gaussian은:

- 위치 $\boldsymbol{\mu} \in \mathbb{R}^3$
- 공분산 $\boldsymbol{\Sigma} \in \mathbb{R}^{3 \times 3}$ (회전 + 스케일로 파라미터화)
- 불투명도 $\alpha \in [0, 1]$
- 색상 (spherical harmonics 계수)

렌더링은 splatting — 3D Gaussian을 이미지 평면에 투영하고, 깊이 순서대로 alpha blending — 으로 수행하며, NeRF의 ray marching보다 수십 배 빠르다.

**[3DGS-SLAM](https://arxiv.org/abs/2312.06741)** (Matsuki et al. 2024): 3DGS를 SLAM 표현으로 사용:

1. **Tracking**: Gaussian map을 렌더링한 예측 이미지와 실제 이미지의 photometric + geometric loss로 camera pose를 최적화.
2. **Mapping**: 새 관측에 따라 Gaussian을 추가(densification), 분할(splitting), 제거(pruning)한다.
3. **Loop closure**: pose 보정 시 Gaussian들의 위치도 함께 변형해야 한다.

**NeRF-SLAM vs 3DGS-SLAM 비교**:

| 특성 | NeRF-SLAM | 3DGS-SLAM |
|------|-----------|-----------|
| 표현 | MLP 가중치 | 명시적 3D Gaussian 집합 |
| 렌더링 속도 | 느림 (ray marching) | 빠름 (rasterization) |
| 학습 속도 | 느림 | 빠름 |
| 편집 가능성 | 어려움 | 용이 (개별 Gaussian 조작) |
| 메모리 | 고정 (모델 크기) | 가변 (Gaussian 수에 비례) |
| Loop closure 대응 | 어려움 (가중치 변형) | 상대적 용이 (Gaussian 변환) |

두 방법 모두 아직 전통적 맵 표현의 정확도와 실시간성을 모든 상황에서 능가하지는 못한다. 대규모 환경이나 장시간 SLAM에서는 TSDF 기반 방법이 여전히 더 안정적이다. 렌더링 품질에서는 neural representation이 압도적이며, 이 격차는 빠르게 좁혀지고 있다.

**최근 주요 발전 (2024~2025)**:

- **[SplaTAM](https://arxiv.org/abs/2312.02126)** (Keetha et al. CVPR 2024): RGB-D 카메라로부터 3D Gaussian을 실시간으로 추적·매핑하며, silhouette mask 기반의 구조화된 맵 확장으로 기존 방법 대비 camera pose 추정과 novel-view synthesis에서 2배 이상 성능 향상을 달성했다.
- **[MonoGS](https://arxiv.org/abs/2312.06741)** (Matsuki et al. CVPR 2024 Highlight): 최초의 monocular 3DGS SLAM으로, 3fps에서 단안 카메라만으로 tracking·mapping·렌더링을 통합 수행한다. geometric verification과 regularization으로 monocular 3D 재구성의 depth 모호성을 억제했다.
- **[MASt3R-SLAM](https://arxiv.org/abs/2412.12392)** (Murai et al. CVPR 2025): 3D reconstruction foundation model(MASt3R)을 SLAM에 통합한 시스템으로, 카메라 모델 가정 없이 15fps로 globally-consistent한 dense geometry를 복원한다.

```python
import numpy as np

class Gaussian3D:
    """단일 3D Gaussian 표현."""
    
    def __init__(self, mean, covariance, color, opacity):
        """
        Args:
            mean: 3D 위치 (3,)
            covariance: 3x3 공분산 행렬 (3, 3)
            color: RGB 색상 (3,)
            opacity: 불투명도 (scalar)
        """
        self.mean = np.array(mean, dtype=np.float64)
        self.covariance = np.array(covariance, dtype=np.float64)
        self.color = np.array(color, dtype=np.float64)
        self.opacity = float(opacity)
    
    def project_to_2d(self, T_world_to_cam, K):
        """
        3D Gaussian을 이미지 평면에 투영.
        
        Returns:
            mean_2d: 투영된 중심 (2,)
            cov_2d: 투영된 2D 공분산 (2, 2)
        """
        # 월드 → 카메라 좌표 변환
        R = T_world_to_cam[:3, :3]
        t = T_world_to_cam[:3, 3]
        
        mean_cam = R @ self.mean + t
        
        if mean_cam[2] <= 0:
            return None, None
        
        # 3D 공분산 → 카메라 좌표계
        cov_cam = R @ self.covariance @ R.T
        
        # 투영 Jacobian (pinhole model)
        fx, fy = K[0, 0], K[1, 1]
        z = mean_cam[2]
        
        J = np.array([
            [fx / z, 0, -fx * mean_cam[0] / z**2],
            [0, fy / z, -fy * mean_cam[1] / z**2]
        ])
        
        # 2D 공분산 (EWA splatting)
        cov_2d = J @ cov_cam @ J.T
        
        # 2D 중심
        mean_2d = np.array([
            fx * mean_cam[0] / z + K[0, 2],
            fy * mean_cam[1] / z + K[1, 2]
        ])
        
        return mean_2d, cov_2d

def render_gaussians(gaussians, T_world_to_cam, K, image_size):
    """
    3D Gaussian Splatting 렌더링 (간소화 버전).
    
    Args:
        gaussians: Gaussian3D 리스트
        T_world_to_cam: 카메라 pose (4, 4)
        K: 카메라 내부 파라미터 (3, 3)
        image_size: (H, W)
        
    Returns:
        rendered_image: (H, W, 3)
    """
    H, W = image_size
    image = np.zeros((H, W, 3), dtype=np.float64)
    accumulated_alpha = np.zeros((H, W), dtype=np.float64)
    
    # 깊이 순서로 정렬
    R = T_world_to_cam[:3, :3]
    t = T_world_to_cam[:3, 3]
    
    depths = []
    for g in gaussians:
        mean_cam = R @ g.mean + t
        depths.append(mean_cam[2])
    
    sorted_indices = np.argsort(depths)
    
    for idx in sorted_indices:
        g = gaussians[idx]
        mean_2d, cov_2d = g.project_to_2d(T_world_to_cam, K)
        
        if mean_2d is None:
            continue
        
        # 2D Gaussian의 영향 범위 (3 sigma)
        eigenvalues = np.linalg.eigvalsh(cov_2d)
        radius = int(3 * np.sqrt(max(eigenvalues))) + 1
        
        u_min = max(0, int(mean_2d[0]) - radius)
        u_max = min(W, int(mean_2d[0]) + radius + 1)
        v_min = max(0, int(mean_2d[1]) - radius)
        v_max = min(H, int(mean_2d[1]) + radius + 1)
        
        cov_2d_inv = np.linalg.inv(cov_2d + 1e-6 * np.eye(2))
        
        for v in range(v_min, v_max):
            for u in range(u_min, u_max):
                d = np.array([u - mean_2d[0], v - mean_2d[1]])
                power = -0.5 * d @ cov_2d_inv @ d
                
                if power < -4.0:  # 너무 먼 픽셀은 스킵
                    continue
                
                alpha = g.opacity * np.exp(power)
                
                # Front-to-back alpha blending
                remaining = 1.0 - accumulated_alpha[v, u]
                if remaining < 0.01:
                    continue
                
                weight = alpha * remaining
                image[v, u] += weight * g.color
                accumulated_alpha[v, u] += weight
    
    return np.clip(image, 0, 255).astype(np.uint8)
```

---

## 11.4 Semantic Maps

기하학적 맵은 "어디에 무엇이 있는가"의 "어디"만 답한다. **Semantic map(의미 맵)**은 "무엇"까지 답한다 — 이것이 벽인지, 문인지, 의자인지, 사람인지. 로봇이 인간 수준의 환경 이해를 하려면 이 레이블 정보가 필수적이다.

### 11.4.1 Object-Level Maps

가장 직관적인 semantic map은 환경의 객체(object)를 인식하고 그 위치와 크기를 맵에 등록하는 것이다.

파이프라인:
1. **2D detection/segmentation**: 카메라 이미지에서 객체를 검출 (YOLO, Mask R-CNN, SAM 등).
2. **3D lifting**: depth 정보와 camera pose를 이용하여 2D 검출을 3D 공간으로 역투영.
3. **Data association**: 여러 프레임에서 관측된 같은 객체를 연결 (tracking + re-identification).
4. **Map integration**: 객체의 3D 바운딩 박스 또는 형상 모델을 맵에 등록.

```python
class ObjectMap:
    """3D 객체 맵 — 2D 검출을 3D 공간에 통합."""
    
    def __init__(self, association_threshold=0.5):
        self.objects = []  # [{class, center_3d, bbox_3d, observations, id}]
        self.next_id = 0
        self.assoc_thresh = association_threshold
    
    def integrate_detection(self, class_label, mask_2d, depth_image, 
                            K, T_cam_to_world):
        """
        2D 검출을 3D 맵에 통합.
        
        Args:
            class_label: 객체 클래스 (str)
            mask_2d: 2D segmentation mask (H, W) bool
            depth_image: depth map (H, W)
            K: 카메라 내부 파라미터 (3, 3)
            T_cam_to_world: 카메라 → 월드 변환 (4, 4)
        """
        # 1. 마스크 영역의 3D 포인트 복원
        points_3d = self._backproject(mask_2d, depth_image, K, T_cam_to_world)
        
        if len(points_3d) < 10:
            return
        
        center = np.mean(points_3d, axis=0)
        bbox_min = np.min(points_3d, axis=0)
        bbox_max = np.max(points_3d, axis=0)
        
        # 2. Data association: 기존 객체와 매칭
        best_match = None
        best_dist = float('inf')
        
        for obj in self.objects:
            if obj['class'] != class_label:
                continue
            dist = np.linalg.norm(obj['center_3d'] - center)
            if dist < best_dist and dist < self.assoc_thresh:
                best_dist = dist
                best_match = obj
        
        if best_match is not None:
            # 기존 객체 갱신 (가중 평균)
            n = best_match['observations']
            best_match['center_3d'] = (
                (n * best_match['center_3d'] + center) / (n + 1)
            )
            best_match['observations'] = n + 1
        else:
            # 새 객체 등록
            self.objects.append({
                'class': class_label,
                'center_3d': center,
                'bbox_3d': (bbox_min, bbox_max),
                'observations': 1,
                'id': self.next_id
            })
            self.next_id += 1
    
    def _backproject(self, mask, depth, K, T):
        """마스크 영역의 2D 픽셀을 3D 월드 좌표로 역투영."""
        ys, xs = np.where(mask & (depth > 0))
        
        fx, fy = K[0, 0], K[1, 1]
        cx, cy = K[0, 2], K[1, 2]
        
        z = depth[ys, xs]
        x = (xs - cx) * z / fx
        y = (ys - cy) * z / fy
        
        points_cam = np.stack([x, y, z, np.ones_like(z)], axis=1)  # (N, 4)
        points_world = (T @ points_cam.T).T[:, :3]  # (N, 3)
        
        return points_world
```

### 11.4.2 3D Scene Graph: Hydra & S-Graphs

Object-level map은 개별 객체만 인식한다. 하지만 인간은 환경을 "이 방에 테이블이 있고, 그 위에 컵이 있고, 이 방은 거실이다"와 같은 계층적 관계로 이해한다. **3D Scene Graph**는 이러한 계층적, 관계적 환경 표현이다.

**[Hydra](https://arxiv.org/abs/2201.13360)** (Hughes et al. 2022)는 실시간으로 3D scene graph를 점진적으로 구축하는 최초의 시스템이다. 5개 계층으로 구성된다:

| Layer | 내용 | 구성 방법 |
|-------|------|-----------|
| 1 | Metric-Semantic 3D Mesh | TSDF 통합 + semantic segmentation |
| 2 | Objects & Agents | 3D bounding box detection |
| 3 | Places (topological) | GVD(Generalized Voronoi Diagram)에서 추출 |
| 4 | Rooms | 장소 그래프의 커뮤니티 검출 |
| 5 | Buildings | 방들의 상위 그룹핑 |

**Places layer의 구축**: 이 계층이 Hydra의 가장 독창적인 부분이다.

1. TSDF에서 ESDF를 계산한다.
2. ESDF에서 GVD를 점진적으로 추출한다. GVD의 꼭짓점은 장애물로부터 최대한 먼 지점 — 즉, 로봇이 통과하기 좋은 지점 — 이다.
3. GVD 꼭짓점들을 place 노드로, GVD 에지를 place 간 연결로 구성한다.
4. 이 place graph는 경로 계획에 직접 사용할 수 있는 topological map이다.

**Room detection**: place graph에서 방을 검출하는 방법:

1. Place 노드들의 에지를 장애물 근접도에 따라 가중치를 부여한다.
2. 문(doorway) 같은 좁은 통로에서 가중치가 높아진다 (통과하기 어렵다는 의미).
3. 커뮤니티 검출 알고리즘(예: dilation 기반)으로 place들을 방 단위로 그룹핑한다.

**계층적 loop closure**: Hydra는 scene graph의 계층 구조를 활용하여 loop closure의 품질을 높인다. 먼저 상위 계층(room, place)에서 후보를 좁히고, 하위 계층(visual feature, object)에서 TEASER++ 기반으로 기하학적 검증을 수행한다. 이 top-down/bottom-up 구조 덕분에 단순 BoW 방식보다 더 많은 loop closure를 더 정확하게 확보할 수 있다.

**S-Graphs** (Situational Graphs): Hydra와 유사한 계층적 scene graph이지만, factor graph 최적화에 계층 정보를 직접 포함시킨다. Room, wall, floor 같은 구조적 요소를 factor graph의 변수로 추가하여 SLAM 정확도를 향상시킨다.

### 11.4.3 Open-Vocabulary Semantic Mapping

전통적 semantic mapping은 사전에 정의된 클래스 집합(예: COCO의 80개 클래스)에서만 동작한다. **Open-vocabulary semantic mapping**은 임의의 텍스트 질의로 맵을 탐색할 수 있게 한다.

파이프라인은 단순하다. 각 관측(이미지 또는 패치)에서 CLIP/DINO feature를 추출하여 대응하는 3D 위치에 부착한다. 사용자가 "빨간 소화기"라고 질의하면, CLIP text encoder로 텍스트를 인코딩하고 맵의 visual feature와 코사인 유사도를 계산하여 해당 위치를 반환한다.

로봇이 사전에 학습하지 않은 객체에도 동작한다는 것이 핵심이다. 예측 불가능한 환경에서 운용되는 가정용 로봇이나 탐사 로봇에 특히 유용하다.

```python
class OpenVocabSemanticMap:
    """
    Open-vocabulary semantic mapping의 핵심 개념.
    각 3D 포인트에 CLIP feature를 부착.
    """
    
    def __init__(self, feature_dim=512):
        self.points_3d = []     # (N, 3)
        self.features = []      # (N, D) CLIP visual features
        self.feature_dim = feature_dim
    
    def add_observation(self, points_3d, clip_features):
        """
        3D 포인트와 대응하는 CLIP feature를 맵에 추가.
        
        Args:
            points_3d: (M, 3) 3D 포인트 좌표
            clip_features: (M, D) 각 포인트의 CLIP visual feature
        """
        self.points_3d.extend(points_3d)
        self.features.extend(clip_features)
    
    def query(self, text_feature, top_k=10):
        """
        텍스트 질의로 맵에서 관련 위치를 검색.
        
        Args:
            text_feature: (D,) CLIP text feature
            top_k: 반환할 상위 결과 수
            
        Returns:
            results: [(point_3d, similarity)] 리스트
        """
        if len(self.features) == 0:
            return []
        
        features_array = np.array(self.features)
        points_array = np.array(self.points_3d)
        
        # 코사인 유사도 계산
        text_norm = text_feature / (np.linalg.norm(text_feature) + 1e-8)
        feat_norms = features_array / (
            np.linalg.norm(features_array, axis=1, keepdims=True) + 1e-8
        )
        
        similarities = feat_norms @ text_norm
        
        top_indices = np.argsort(similarities)[::-1][:top_k]
        
        return [
            (points_array[idx], similarities[idx]) 
            for idx in top_indices
        ]
    
    def visualize_heatmap(self, text_feature):
        """텍스트 질의에 대한 관련도 히트맵 생성."""
        if len(self.features) == 0:
            return np.array([]), np.array([])
        
        features_array = np.array(self.features)
        points_array = np.array(self.points_3d)
        
        text_norm = text_feature / (np.linalg.norm(text_feature) + 1e-8)
        feat_norms = features_array / (
            np.linalg.norm(features_array, axis=1, keepdims=True) + 1e-8
        )
        
        similarities = feat_norms @ text_norm
        
        # 0~1로 정규화
        sim_min, sim_max = similarities.min(), similarities.max()
        if sim_max > sim_min:
            heatmap = (similarities - sim_min) / (sim_max - sim_min)
        else:
            heatmap = np.zeros_like(similarities)
        
        return points_array, heatmap
```

---

## 11.5 Long-Term & Dynamic Maps

실제 환경은 정적이지 않다. 사람이 오가고, 가구가 옮겨지고, 계절에 따라 외관이 달라진다. 장기간 운용되는 로봇은 이러한 변화에 적응해야 한다.

### 11.5.1 Change Detection (변화 감지)

맵과 현재 관측을 비교하여 변화된 부분을 식별한다.

**Geometric change detection**: 과거 맵의 예상 관측과 현재 센서 관측을 비교한다.

1. **Free-to-occupied**: 맵에서 free인 영역에 새 장애물이 감지되면 → 새 객체가 나타난 것.
2. **Occupied-to-free**: 맵에서 occupied인 영역을 현재 관측이 관통(pass-through)하면 → 기존 객체가 사라진 것.

```python
def detect_changes(occupancy_map, current_scan, robot_pose, threshold=0.3):
    """
    기존 맵과 현재 스캔을 비교하여 변화 감지.
    
    Returns:
        new_objects: 새로 나타난 장애물 위치들
        removed_objects: 사라진 장애물 위치들
    """
    new_objects = []
    removed_objects = []
    
    for point in current_scan:
        # 현재 스캔 포인트의 맵 좌표
        map_value = occupancy_map.get_probability(point)
        
        if map_value < threshold:
            # 맵에서는 free인데 현재 장애물 감지 → 새 객체
            new_objects.append(point)
    
    # 맵에서 occupied인데 현재 관통되는 영역 탐지
    for ray_endpoint in current_scan:
        ray_cells = trace_ray(robot_pose, ray_endpoint)
        for cell in ray_cells[:-1]:  # 끝점 제외 (여전히 occupied일 수 있음)
            map_value = occupancy_map.get_probability(cell)
            if map_value > (1.0 - threshold):
                # 맵에서 occupied인데 현재 ray가 관통 → 객체 제거됨
                removed_objects.append(cell)
    
    return new_objects, removed_objects
```

**Semantic change detection**: 기하학적 변화뿐 아니라 객체의 클래스나 상태 변화도 잡는다. "의자가 이동됨", "문이 열림/닫힘" 같은 사례가 여기 해당한다.

### 11.5.2 Map Maintenance: 유지 전략

장기 운용에서 맵의 모든 관측을 무한히 저장할 수는 없다. 어떤 정보를 유지하고 어떤 정보를 삭제할 것인가?

**Recency weighting**: 최근 관측에 더 높은 가중치를 주고 오래된 관측의 영향을 줄여간다. TSDF 가중치를 시간에 따라 감쇠하는 것이 대표적인 구현이다.

**Semi-static classification**: 환경 요소를 세 범주로 나눈다. 벽·바닥·건물 같은 정적(static) 요소는 영구 보존하고, 가구나 주차된 차량 같은 준정적(semi-static) 요소는 주기적으로 갱신하며, 보행자나 이동 차량 같은 동적(dynamic) 요소는 맵에서 제외한다.

**Multi-experience mapping**: 같은 장소의 여러 "경험(experience)"을 저장한다. 조명 조건이나 계절이 다른 여러 버전의 맵을 두고, 현재 관측과 가장 일치하는 경험을 고른다.

### 11.5.3 Dynamic Object 처리

SLAM에서 동적 객체(움직이는 사람, 차량)는 두 가지 문제를 일으킨다:

1. **Tracking 오류**: 동적 객체의 특징점을 정적 환경의 것으로 잘못 사용하면 pose 추정이 틀어진다.
2. **맵 오염**: 동적 객체가 맵에 기록되면 "유령 장애물"이 된다.

**대응 방법**:

1. **Semantic filtering**: semantic segmentation으로 동적 객체(사람, 차량) 클래스를 구분하고, 해당 관측을 SLAM 파이프라인에서 뺀다.

2. **Geometric consistency check**: 여러 프레임에 걸쳐 일관되지 않은 관측 — 한 프레임에서만 보이고 다음 프레임에서 사라지는 포인트 — 을 동적으로 분류한다.

3. **Background subtraction**: TSDF에서 free-to-occupied-to-free 패턴을 보이는 voxel을 동적으로 판별한다.

4. **SLAM with dynamic object tracking**: 동적 객체를 무시하는 대신 별도의 상태 변수로 추적한다. 동적 객체의 궤적도 함께 추정할 수 있다 (SLAM + MOT).

```python
class DynamicMapManager:
    """
    동적 환경 맵 관리자.
    관측을 static/semi-static/dynamic으로 분류하여 차별 처리.
    """
    
    def __init__(self):
        self.static_map = {}       # 영구 저장
        self.semistatic_map = {}   # 주기적 갱신
        self.dynamic_objects = []  # 추적 중인 동적 객체
        
        # 동적 클래스 정의
        self.dynamic_classes = {'person', 'car', 'bicycle', 'dog'}
        self.semistatic_classes = {'chair', 'box', 'parked_car'}
    
    def process_observation(self, point_3d, class_label, timestamp):
        """
        새 관측을 분류하고 적절한 맵에 통합.
        """
        if class_label in self.dynamic_classes:
            self._track_dynamic(point_3d, class_label, timestamp)
        elif class_label in self.semistatic_classes:
            self._update_semistatic(point_3d, class_label, timestamp)
        else:
            self._update_static(point_3d, class_label)
    
    def _track_dynamic(self, point, cls, timestamp):
        """동적 객체 추적. 맵에는 추가하지 않음."""
        # Data association으로 기존 동적 객체와 매칭
        matched = False
        for obj in self.dynamic_objects:
            if (obj['class'] == cls and 
                np.linalg.norm(obj['last_position'] - point) < 2.0):
                obj['trajectory'].append((timestamp, point.copy()))
                obj['last_position'] = point.copy()
                matched = True
                break
        
        if not matched:
            self.dynamic_objects.append({
                'class': cls,
                'last_position': point.copy(),
                'trajectory': [(timestamp, point.copy())]
            })
    
    def _update_semistatic(self, point, cls, timestamp):
        """준정적 객체 갱신."""
        key = self._spatial_key(point)
        self.semistatic_map[key] = {
            'position': point.copy(),
            'class': cls,
            'last_seen': timestamp
        }
    
    def _update_static(self, point, cls):
        """정적 맵 갱신."""
        key = self._spatial_key(point)
        self.static_map[key] = {
            'position': point.copy(),
            'class': cls
        }
    
    def cleanup_old_semistatic(self, current_time, max_age=3600):
        """오래된 준정적 관측 제거 (예: 1시간)."""
        keys_to_remove = [
            k for k, v in self.semistatic_map.items()
            if current_time - v['last_seen'] > max_age
        ]
        for k in keys_to_remove:
            del self.semistatic_map[k]
    
    def _spatial_key(self, point, resolution=0.1):
        """3D 포인트를 이산 격자 키로 변환."""
        return (
            int(point[0] / resolution),
            int(point[1] / resolution),
            int(point[2] / resolution)
        )
```

---

각 표현은 서로 다른 downstream task에 맞는다. 현대 시스템은 이들을 계층적으로 조합한다 — OctoMap으로 경로를 계획하고, surfel로 렌더링하고, scene graph로 의미 질의를 처리한다. 다음 챕터에서는 이 알고리즘과 표현들이 자율주행·드론·핸드헬드 플랫폼에서 실제로 어떻게 통합되는지, 그리고 성능을 어떻게 평가하는지를 본다.

---

# Ch.12 — 실전 시스템 & 벤치마크

Ch.2-11에서 센서 모델링부터 상태 추정, odometry, place recognition, 공간 표현까지 개별 알고리즘을 다루었다. 이 챕터에서는 시선을 알고리즘에서 시스템으로 넓혀, 이 기술들이 실제 플랫폼에서 어떻게 조합되는지를 살펴본다.

자율주행, 드론, 핸드헬드 매핑이라는 세 대표 플랫폼에서 센서 퓨전 아키텍처를 분석하고, 시스템을 평가하기 위한 벤치마크와 도구를 소개한다.

---

## 12.1 자율주행 Perception Stack

자율주행은 센서 퓨전이 가장 공격적으로 적용되는 분야다. 생사가 걸린 안전 요구사항 때문에, 단일 센서 실패에도 시스템이 동작해야 한다(redundancy). 여러 센서를 조합해야 한다.

### 12.1.1 Sensor Suite 구성 사례

**Waymo** (5th generation):
- 1 × 장거리 LiDAR (360°, 최대 300m)
- 4 × 단거리 LiDAR (근거리 사각지대 커버)
- 29 × 카메라 (360° 커버, 다양한 화각)
- 6 × radar (장거리 속도 측정)
- IMU, GNSS, wheel encoder

**[nuScenes](https://arxiv.org/abs/1903.11027) 데이터셋 기준 일반적 구성**:
- 1 × spinning LiDAR (32 또는 64 채널)
- 6 × 서라운드 카메라 (360° 커버)
- 5 × radar
- IMU, GNSS

**Tesla (Vision-only approach)**:
- 8 × 카메라만으로 360° 커버 (LiDAR/radar 제거)
- 뉴럴넷 기반 depth estimation으로 3D 인식
- 극도로 공격적인 접근이며, 업계에서 논쟁이 계속됨

### 12.1.2 Production-Level Fusion Pipeline

프로덕션 자율주행 시스템의 전형적인 센서 퓨전 파이프라인:

```
센서 동기화 (HW trigger + PTP)
    ↓
[LiDAR 처리]          [카메라 처리]         [Radar 처리]
- Motion compensation  - Object detection    - Doppler 속도
- Ground segmentation  - Semantic seg        - 장거리 탐지
- 3D object detection  - Lane detection
    ↓                      ↓                     ↓
         Late Fusion / Deep Fusion
    ↓
[Tracking & Prediction]
- Multi-object tracking (Kalman/JPDA)
- Trajectory prediction
    ↓
[Localization]
- HD map matching
- GNSS/IMU/LiDAR 통합
    ↓
[Planning & Control]
```

Late Fusion과 Deep Fusion은 서로 다른 설계 철학을 대표한다.

**Late fusion (전통)** 방식은 각 센서에서 독립적으로 3D bounding box를 검출하고 NMS(Non-Maximum Suppression)로 결합한다. 모듈화와 디버깅이 쉬운 반면, 센서 간 상보성을 충분히 활용하기 어렵다.

**Deep fusion (현대)** 방식은 BEV(Bird's Eye View) 공간에서 여러 센서의 feature를 직접 결합한다. [BEVFusion](https://arxiv.org/abs/2205.13542) (MIT/Nvidia), TransFusion 등이 대표적이며, 센서 간 상보적 정보를 네트워크가 학습으로 활용한다. 단, end-to-end 학습에 대규모 레이블 데이터가 필요하다는 부담이 있다.

```python
# BEV Fusion 개념도 (pseudo-code)

def bev_fusion_pipeline(lidar_points, camera_images, calibrations):
    """
    BEV 공간에서 LiDAR와 카메라 feature를 결합하는 fusion pipeline.
    """
    # 1. LiDAR → BEV feature
    # LiDAR 포인트를 voxelize하고 3D backbone으로 처리한 후
    # z축을 collapse하여 BEV feature map 생성
    lidar_voxels = voxelize(lidar_points, voxel_size=0.1)
    lidar_3d_features = sparse_3d_cnn(lidar_voxels)  # (X, Y, Z, C)
    lidar_bev = lidar_3d_features.max(dim='z')        # (X, Y, C)
    
    # 2. Camera → BEV feature
    # 각 카메라 이미지에서 feature를 추출하고,
    # depth estimation을 거쳐 BEV 공간으로 변환
    camera_features = []
    for img, calib in zip(camera_images, calibrations):
        feat_2d = image_backbone(img)  # (H', W', C)
        depth_dist = depth_net(feat_2d)  # (H', W', D) — depth 확률 분포
        
        # Lift: 2D feature를 3D로 (LSS 방식)
        feat_3d = outer_product(feat_2d, depth_dist)  # (H', W', D, C)
        
        # Splat: 3D feature를 BEV pillar로 합산
        feat_bev = splat_to_bev(feat_3d, calib)  # (X, Y, C)
        camera_features.append(feat_bev)
    
    camera_bev = sum(camera_features)  # 모든 카메라의 BEV feature 합산
    
    # 3. Fusion: BEV 공간에서 concatenate 또는 attention
    fused_bev = concat_and_conv(lidar_bev, camera_bev)  # (X, Y, C')
    
    # 4. Detection head
    detections = detection_head(fused_bev)  # 3D bounding boxes
    
    return detections
```

### 12.1.3 Localization Stack

자율주행 localization은 일반적으로 두 단계로 구성된다:

1. **Global localization**: GNSS (RTK 또는 PPP)로 초기 위치를 맵 위에 배치한다. 도심에서는 multipath 문제로 수 미터 오차가 발생할 수 있으므로, 이것만으로는 부족하다.

2. **Map-relative localization**: HD map(사전 구축 LiDAR 포인트 클라우드 맵)에 현재 LiDAR 스캔을 NDT/ICP로 정합하여 cm 수준 정확도를 달성한다. GNSS 신호가 차단되는 터널 등에서도 동작한다.

Factor graph 기반 통합:
$$\mathbf{x}^* = \arg\min \underbrace{f_{\text{IMU}}}_{\text{예측}} + \underbrace{f_{\text{LiDAR}}}_{\text{맵 정합}} + \underbrace{f_{\text{GNSS}}}_{\text{전역 앵커}} + \underbrace{f_{\text{wheel}}}_{\text{속도}}$$

---

## 12.2 드론/UAV

드론 환경의 센서 퓨전은 자율주행과는 상당히 다른 제약 아래에서 동작한다.

### 12.2.1 Visual-Inertial 중심 시스템

드론에서 가장 많이 사용되는 센서 조합은 카메라 + IMU다. 소형 드론은 무게와 전력 제약 때문에 LiDAR를 탑재하기 어렵고, 카메라와 IMU는 두 제약을 모두 만족한다(Livox Mid-360 같은 소형 solid-state LiDAR가 등장하면서 변화 중이긴 하다). 별도의 과제는 진동이다. 드론의 프로펠러 진동이 IMU 데이터에 노이즈를 추가하므로, 방진 마운트와 소프트웨어 필터링이 함께 필요하다.

대표적 VIO 시스템 for 드론:
- **VINS-Mono/Fusion**: tightly-coupled optimization 기반. PX4와 통합 가능.
- **MSCKF/OpenVINS**: filter 기반. 연산량이 적어 embedded 보드에 적합.
- **Basalt**: visual-inertial mapping with non-linear factor recovery.

### 12.2.2 GPS-Denied Navigation

드론의 핵심 도전은 GPS 신호가 없는 환경 — 실내, 터널, 숲 캐노피 아래, 전자전 환경 — 에서의 자율 비행이다.

접근 방식은 사전 인프라 여부에 따라 갈린다.

1. **VIO 단독**: 단기적으로 안정적이지만 드리프트가 누적된다. 수 분 이내의 단거리 미션에 적합하다.
2. **VIO + 지형 매칭**: 사전 구축된 지형·건물 맵과 현재 카메라 관측을 매칭해 드리프트를 억제한다. prior map이 필요하다.
3. **VIO + UWB**: 환경에 UWB 앵커를 설치하고 ranging 측정으로 드리프트를 보정한다. 인프라 사전 구축이 전제다.
4. **VIO + barometer**: 기압계를 보조 센서로 추가해 z축 드리프트를 잡는다. 별도 인프라 없이 적용 가능하다.

### 12.2.3 실시간 제약

드론은 고속 비행(5~15 m/s)과 급격한 자세 변화(flip, 급선회)를 수행한다. IMU rate는 200~1000 Hz가 필요하며, 고속 모션의 pose 변화를 충분히 캡처해야 한다. 카메라는 모션 블러를 줄이기 위해 짧은 노출 시간이 필요하지만, 이는 저조도에서의 노이즈 증가와 트레이드오프다. state estimation 결과는 30 ms 이내에 제어기에 전달되어야 하며, 지연이 길면 제어 불안정으로 이어진다.

**Point-LIO**는 포인트 단위로 처리하여 스캔 완료를 기다리지 않는 초저지연 LIO다. 드론의 고속 기동에 특히 유리하다.

```python
class DroneVIOConfig:
    """드론용 VIO 시스템 구성 예시."""
    
    # 센서 구성
    camera_fps = 30
    camera_resolution = (640, 480)  # 저해상도로 연산량 절감
    imu_rate = 400  # Hz
    
    # VIO 파라미터
    max_features = 150  # 특징점 수 제한 (연산량)
    keyframe_interval = 5  # 매 5프레임마다 키프레임
    sliding_window_size = 10  # 최적화 윈도우 크기
    
    # 드론 특화 설정
    gravity_magnitude = 9.81
    max_angular_velocity = 10.0  # rad/s — 급격한 회전 대응
    motion_blur_threshold = 0.3  # 블러 심한 프레임 제외
    
    # IMU 노이즈 (드론은 진동이 심하므로 값이 큼)
    gyro_noise_density = 0.004  # rad/s/sqrt(Hz)
    accel_noise_density = 0.05  # m/s^2/sqrt(Hz)
    gyro_random_walk = 0.0002   # rad/s^2/sqrt(Hz)
    accel_random_walk = 0.003   # m/s^3/sqrt(Hz)
    
    # 안전
    max_allowed_drift_m = 0.5   # 이 이상 드리프트 감지 시 경고
    min_tracked_features = 20   # 이하면 tracking quality 경고

def check_image_quality(image, angular_velocity, exposure_time):
    """
    드론 카메라 이미지 품질 검사.
    모션 블러가 심한 프레임은 VIO에서 제외.
    """
    # 모션 블러 추정: 각속도 × 노출 시간 × 초점 거리
    blur_pixels = abs(angular_velocity) * exposure_time * 300  # 대략적 focal length
    
    if blur_pixels > 5.0:  # 5 픽셀 이상 블러
        return False, f"Excessive motion blur: {blur_pixels:.1f} pixels"
    
    # 밝기 검사
    mean_brightness = image.mean()
    if mean_brightness < 20:
        return False, f"Too dark: mean={mean_brightness:.0f}"
    if mean_brightness > 240:
        return False, f"Too bright: mean={mean_brightness:.0f}"
    
    return True, "OK"
```

---

## 12.3 핸드헬드/백팩 매핑

핸드헬드 또는 백팩에 장착된 센서로 환경을 매핑하는 것은 측량(surveying), BIM(Building Information Modeling), 디지털 트윈 구축 등에 널리 사용된다.

### 12.3.1 SLAM as a Service

상용 핸드헬드 매핑 장비의 예:

- **Leica BLK2GO**: 핸드헬드 LiDAR 스캐너. LiDAR + IMU + 카메라 융합으로 실시간 SLAM 수행. 측량 등급(survey-grade) 정확도.
- **NavVis VLX**: 백팩 장착. 4개의 카메라 + LiDAR. 실내 매핑에 특화.
- **GeoSLAM ZEB**: 핸드헬드 모바일 매핑. 2D LiDAR를 수동으로 회전시키며 3D 스캔.

이러한 장비의 공통 파이프라인:

```
LiDAR + IMU → LIO (FAST-LIO2 또는 유사)
     ↓
Loop closure (Scan Context 등)
     ↓
Global optimization (iSAM2)
     ↓
Dense colorized point cloud (카메라 색상 부착)
     ↓
Post-processing (클라우드 정리, mesh 생성)
```

### 12.3.2 Survey-Grade Mapping

측량 등급 매핑에서의 핵심 요구사항:

- **절대 정확도**: GNSS 기준으로 수 cm 이내. 이를 위해 GCP(Ground Control Point)를 배치하고 후처리에서 맞춘다.
- **상대 정확도**: 맵 내부의 일관성. Loop closure와 global optimization이 핵심.
- **포인트 밀도**: 벽면 1 cm 간격 이상의 밀도. 후처리에서 다운샘플링.
- **색상 품질**: 정확한 HDR 색상 매핑. 카메라-LiDAR 시간 동기화와 extrinsic 캘리브레이션이 정밀해야 한다.

센서 퓨전에서의 실전 이슈:

1. **Degenerate environments**: 긴 복도, 빈 방 등 기하학적 특징이 부족한 환경. LiDAR-only에서 발생하는 drift를 카메라 또는 IMU가 보완. R3LIVE, FAST-LIVO2 같은 multi-modal 시스템이 효과적.

2. **다층 건물**: 엘리베이터/계단을 통한 층간 이동 시 loop closure가 필수. GNSS가 없으므로 z축 드리프트가 특히 문제. 기압계가 보조 센서로 유용.

3. **유리/거울**: LiDAR 빔이 투과하거나 반사. 카메라로 보완하거나, 반사 포인트 필터링.

---

## 12.4 벤치마크 & 평가

센서 퓨전 시스템을 공정하게 비교하려면 표준화된 데이터셋과 평가 메트릭이 필요하다.

### 12.4.1 주요 데이터셋

| 데이터셋 | 연도 | 환경 | 센서 | 특징 |
|----------|------|------|------|------|
| **[KITTI](https://doi.org/10.1177/0278364913491297)** | 2012 | 실외 (자율주행) | Stereo, LiDAR, GPS/IMU | SLAM/VO 벤치마크의 시초. 11개 training + 11개 test 시퀀스 |
| **[EuRoC](https://doi.org/10.1177/0278364915620033)** | 2016 | 실내 (MAV) | Stereo, IMU | VIO 벤치마크의 표준. Machine Hall + Vicon Room |
| **[TUM-RGBD](https://doi.org/10.1109/IROS.2012.6385773)** | 2012 | 실내 | RGB-D | Visual SLAM의 기본 벤치마크. Kinect v1 |
| **TUM-VI** | 2018 | 실내+실외 | Stereo, IMU | VIO 벤치마크. 다양한 모션 패턴 |
| **[Hilti](https://arxiv.org/abs/2109.11316)** | 2021~ | 건설 현장 | LiDAR, Camera, IMU | 산업 환경 특화. 도전적 조건 |
| **[HeLiPR](https://arxiv.org/abs/2309.14590)** | 2023 | 실외 (도심) | Heterogeneous LiDAR, Camera, IMU, GNSS | 이종 LiDAR 퓨전 연구용. Ouster+Velodyne+Livox+Aeva |
| **[nuScenes](https://arxiv.org/abs/1903.11027)** | 2020 | 실외 (자율주행) | Camera, LiDAR, Radar, GPS/IMU | 1000개 씬, 23 클래스 3D 어노테이션, 360° 서라운드 센서 |
| **Newer College** | 2020 | 실외+실내 | LiDAR, Camera, IMU | 옥스퍼드 대학 캠퍼스. Multi-session |

각 데이터셋의 특성과 용도:

**KITTI** — 2012년 공개로 역사가 깊지만 여전히 자율주행 SLAM 벤치마크의 표준이다. Velodyne 64채널 LiDAR, 스테레오 카메라, GPS/IMU를 제공한다. 한계도 뚜렷하다. 센서가 오래되었고 시퀀스가 짧다. ground truth는 GPS/INS 기반이라 cm 수준 정확도를 보장하지 않는다.

**EuRoC** — 드론(MAV)에 장착된 스테레오 카메라 + IMU 데이터로, VIO 시스템의 사실상 표준 벤치마크다. Ground truth는 Vicon 모션 캡처(sub-mm 정확도) 또는 Leica 레이저 트래커(mm 정확도)로 제공된다. 11개 시퀀스가 easy → medium → difficult로 분류되어 있다.

**Hilti** — 건설 현장이라는 도전적 환경(먼지, 진동, 반복 구조물)에서의 SLAM을 평가한다. 2021년부터 매년 SLAM 대회를 개최하여 최신 시스템의 한계를 드러내고 있다.

**HeLiPR** — 2023년 공개된 최신 데이터셋으로, 서로 다른 종류의 LiDAR(spinning, solid-state, FMCW)를 동시에 탑재한 것이 핵심 특징이다. 이종 LiDAR 퓨전이라는 새로운 연구 방향을 지원한다.

**Newer College** — 옥스퍼드 대학 캠퍼스를 여러 번 방문하며 수집한 데이터로, multi-session SLAM과 long-term mapping 연구에 적합하다. 핸드헬드 LiDAR로 수집되어 도전적인 모션 패턴을 포함한다.

2022년 이후 새로운 벤치마크가 빠르게 추가되고 있다.

[Hilti-Oxford](https://arxiv.org/abs/2208.09825) (2022)는 mm 수준 ground truth를 제공하는 건설 환경 SLAM 벤치마크로, 매년 SLAM challenge를 개최하며 최신 시스템의 한계를 드러낸다. [Boreas](https://arxiv.org/abs/2203.10168) (Burnett et al. 2023)는 동일 경로를 1년간 반복 주행하여 수집한 자율주행 데이터셋이다. LiDAR·radar·카메라를 포함하며 사계절 조건을 모두 담는다. [Snail-Radar](https://arxiv.org/abs/2407.11705) (Huai et al., IJRR 2025)는 4D radar SLAM 평가를 위한 대규모 벤치마크로, 다양한 환경과 플랫폼에서 4D radar odometry/SLAM을 체계적으로 비교한다.

### 12.4.2 평가 메트릭

**ATE (Absolute Trajectory Error)**: 추정 궤적과 ground truth 궤적의 전역적 차이를 측정한다.

$$\text{ATE} = \sqrt{\frac{1}{N} \sum_{i=1}^{N} \| \text{trans}(\mathbf{T}_{\text{gt},i}^{-1} \cdot \mathbf{T}_{\text{est},i}) \|^2}$$

평가 전에 두 궤적을 Sim(3) 또는 SE(3) 정렬(alignment)해야 한다. Monocular VO는 스케일이 모호하므로 Sim(3), stereo/LiDAR는 SE(3)를 사용한다.

$$\mathbf{S}^* = \arg\min_{\mathbf{S} \in \text{Sim}(3)} \sum_i \| \mathbf{p}_{\text{gt},i} - \mathbf{S} \cdot \mathbf{p}_{\text{est},i} \|^2$$

이 정렬은 Umeyama algorithm으로 closed-form 해를 구할 수 있다.

**RPE (Relative Pose Error)**: 짧은 구간에서의 상대적 정확도를 측정한다. 드리프트 성향을 나타낸다.

$$\text{RPE}(\Delta) = \sqrt{\frac{1}{M} \sum_{i=1}^{M} \| \text{trans}((\mathbf{T}_{\text{gt},i}^{-1} \mathbf{T}_{\text{gt},i+\Delta})^{-1} (\mathbf{T}_{\text{est},i}^{-1} \mathbf{T}_{\text{est},i+\Delta})) \|^2}$$

$\Delta$는 평가 구간 (프레임 수 또는 거리). 짧은 $\Delta$에서의 RPE는 odometry 정확도를, 긴 $\Delta$에서의 RPE는 드리프트를 반영한다.

Place recognition에는 별도의 메트릭이 사용된다. **Recall@N**은 상위 N개 후보 중 올바른 장소가 포함된 비율이며, Recall@1이 가장 엄격한 기준이다. **Precision-Recall curve**는 threshold에 따른 precision·recall 트레이드오프를 보여주고, **AUC**는 그 커브 아래 면적으로 전체 성능을 단일 수치로 요약한다.

```python
import numpy as np
from scipy.spatial.transform import Rotation

def compute_ate(poses_gt, poses_est, align='se3'):
    """
    Absolute Trajectory Error 계산.
    
    Args:
        poses_gt: ground truth poses [(4, 4), ...] 리스트
        poses_est: estimated poses [(4, 4), ...] 리스트
        align: 'se3' 또는 'sim3'
        
    Returns:
        ate_rmse: ATE RMSE (미터)
        ate_per_frame: 각 프레임의 ATE (N,)
    """
    positions_gt = np.array([T[:3, 3] for T in poses_gt])  # (N, 3)
    positions_est = np.array([T[:3, 3] for T in poses_est])  # (N, 3)
    
    # Umeyama alignment
    if align == 'sim3':
        S, R, t = umeyama_alignment(positions_est, positions_gt, 
                                     with_scale=True)
        positions_aligned = S * (R @ positions_est.T).T + t
    else:  # se3
        _, R, t = umeyama_alignment(positions_est, positions_gt, 
                                     with_scale=False)
        positions_aligned = (R @ positions_est.T).T + t
    
    # 프레임별 오차
    errors = np.linalg.norm(positions_gt - positions_aligned, axis=1)
    
    ate_rmse = np.sqrt(np.mean(errors ** 2))
    
    return ate_rmse, errors

def compute_rpe(poses_gt, poses_est, delta=10):
    """
    Relative Pose Error 계산.
    
    Args:
        poses_gt: ground truth poses 리스트
        poses_est: estimated poses 리스트
        delta: 평가 구간 (프레임 수)
        
    Returns:
        rpe_trans: RPE 병진 RMSE (미터)
        rpe_rot: RPE 회전 RMSE (도)
    """
    trans_errors = []
    rot_errors = []
    
    for i in range(len(poses_gt) - delta):
        # Ground truth 상대 변환
        T_gt_rel = np.linalg.inv(poses_gt[i]) @ poses_gt[i + delta]
        
        # Estimated 상대 변환
        T_est_rel = np.linalg.inv(poses_est[i]) @ poses_est[i + delta]
        
        # 오차
        T_error = np.linalg.inv(T_gt_rel) @ T_est_rel
        
        # 병진 오차
        trans_err = np.linalg.norm(T_error[:3, 3])
        trans_errors.append(trans_err)
        
        # 회전 오차
        rot = Rotation.from_matrix(T_error[:3, :3])
        rot_err = np.linalg.norm(rot.as_rotvec()) * 180 / np.pi  # 도 단위
        rot_errors.append(rot_err)
    
    rpe_trans = np.sqrt(np.mean(np.array(trans_errors) ** 2))
    rpe_rot = np.sqrt(np.mean(np.array(rot_errors) ** 2))
    
    return rpe_trans, rpe_rot

def umeyama_alignment(source, target, with_scale=True):
    """
    Umeyama alignment: source를 target에 정렬하는 
    최적 similarity/rigid 변환 계산.
    
    Args:
        source: (N, 3) 원본 포인트
        target: (N, 3) 대상 포인트
        with_scale: True면 Sim(3), False면 SE(3)
        
    Returns:
        scale: 스케일 (with_scale=False면 1.0)
        rotation: (3, 3) 회전 행렬
        translation: (3,) 병진 벡터
    """
    n = source.shape[0]
    
    # 중심 이동
    mu_source = np.mean(source, axis=0)
    mu_target = np.mean(target, axis=0)
    
    source_centered = source - mu_source
    target_centered = target - mu_target
    
    # 공분산 행렬
    sigma_source = np.sum(source_centered ** 2) / n
    H = (target_centered.T @ source_centered) / n
    
    # SVD
    U, D, Vt = np.linalg.svd(H)
    
    # 반사 보정
    d = np.linalg.det(U) * np.linalg.det(Vt)
    S = np.diag([1, 1, np.sign(d)])
    
    rotation = U @ S @ Vt
    
    if with_scale:
        scale = np.trace(np.diag(D) @ S) / sigma_source
    else:
        scale = 1.0
    
    translation = mu_target - scale * rotation @ mu_source
    
    return scale, rotation, translation
```

### 12.4.3 공정한 비교의 어려움

벤치마크 결과를 해석할 때 주의할 점:

1. **파라미터 튜닝**: 같은 알고리즘도 파라미터에 따라 성능이 크게 달라진다. 특정 데이터셋에 맞춰 튜닝하면 범용성이 떨어진다.

2. **하드웨어 의존성**: 실시간 성능은 하드웨어에 크게 좌우된다. "실시간"의 정의가 논문마다 다르다 (데스크톱 GPU vs 임베디드 ARM).

3. **Completeness**: 일부 시스템은 어려운 시퀀스에서 tracking loss가 발생하는데, 성공한 구간만으로 ATE를 계산하면 실패 빈도가 반영되지 않는다. **Completeness** (= 성공한 시퀀스 비율)도 함께 보고해야 한다.

4. **초기화 차이**: VIO 시스템의 초기화 방법과 시간이 다르면, 같은 시퀀스에서도 결과가 달라진다.

5. **Loop closure 포함 여부**: VO (loop closure 없음) vs SLAM (loop closure 있음)을 구분해야 한다. Loop closure가 있으면 ATE가 극적으로 좋아질 수 있다.

---

## 12.5 오픈소스 도구 가이드

센서 퓨전 연구와 실무에서 자주 쓰는 오픈소스 도구들을 묶었다.

### 12.5.1 최적화 라이브러리

**GTSAM** (Georgia Tech Smoothing and Mapping):
- Factor graph 기반 최적화 라이브러리
- iSAM2 구현 포함
- C++ with Python bindings (gtsam)
- LIO-SAM 등 많은 SLAM 시스템의 백엔드
- 장점: factor graph를 직관적으로 구성 가능, 다양한 factor 타입 내장

```python
# GTSAM Python 사용 예시 (개념 코드)

import gtsam
import numpy as np

def simple_pose_graph_gtsam():
    """GTSAM으로 간단한 pose graph optimization."""
    
    # Factor graph 생성
    graph = gtsam.NonlinearFactorGraph()
    
    # 초기값
    initial = gtsam.Values()
    
    # Prior factor: 첫 번째 pose 고정
    prior_noise = gtsam.noiseModel.Diagonal.Sigmas(
        np.array([0.01, 0.01, 0.01, 0.01, 0.01, 0.01])  # (rx, ry, rz, tx, ty, tz)
    )
    graph.add(gtsam.PriorFactorPose3(
        0, gtsam.Pose3(), prior_noise
    ))
    initial.insert(0, gtsam.Pose3())
    
    # Odometry factors
    odom_noise = gtsam.noiseModel.Diagonal.Sigmas(
        np.array([0.05, 0.05, 0.05, 0.1, 0.1, 0.1])
    )
    
    # Pose 1: 1m 전진
    T_01 = gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(1.0, 0.0, 0.0))
    graph.add(gtsam.BetweenFactorPose3(0, 1, T_01, odom_noise))
    initial.insert(1, gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(1.0, 0.1, 0.0)))
    
    # Pose 2: 90도 좌회전 + 1m 전진
    T_12 = gtsam.Pose3(
        gtsam.Rot3.Rz(np.pi / 2), 
        gtsam.Point3(1.0, 0.0, 0.0)
    )
    graph.add(gtsam.BetweenFactorPose3(1, 2, T_12, odom_noise))
    initial.insert(2, gtsam.Pose3(
        gtsam.Rot3.Rz(np.pi / 2), 
        gtsam.Point3(1.0, 1.1, 0.0)
    ))
    
    # Loop closure: pose 2 → pose 0
    loop_noise = gtsam.noiseModel.Diagonal.Sigmas(
        np.array([0.1, 0.1, 0.1, 0.2, 0.2, 0.2])
    )
    T_20 = gtsam.Pose3(
        gtsam.Rot3.Rz(np.pi / 2),
        gtsam.Point3(0.0, -1.0, 0.0)
    )
    graph.add(gtsam.BetweenFactorPose3(2, 0, T_20, loop_noise))
    
    # iSAM2로 최적화
    params = gtsam.ISAM2Params()
    isam = gtsam.ISAM2(params)
    isam.update(graph, initial)
    result = isam.calculateEstimate()
    
    # 결과 출력
    for i in range(3):
        pose = result.atPose3(i)
        print(f"Pose {i}: x={pose.x():.3f}, y={pose.y():.3f}, "
              f"z={pose.z():.3f}")
    
    return result
```

**Ceres Solver**:
- Google이 개발한 nonlinear least squares 최적화 라이브러리
- C++ 전용 (Python 바인딩은 제한적)
- 자동 미분(automatic differentiation) 지원이 핵심 장점
- VINS-Mono, ORB-SLAM 등에서 사용
- Factor graph 추상화 없이 순수 최적화 문제를 직접 정의

**g2o** (General Graph Optimization):
- Kümmerle et al. (2011) 개발
- C++ 라이브러리, 그래프 최적화 특화
- 다양한 vertex/edge 타입 사전 정의 (SE2, SE3, Sim3 등)
- GTSAM보다 가볍고 빠르지만, 유연성은 떨어짐

세 라이브러리 비교:

| 특성 | GTSAM | Ceres | g2o |
|------|-------|-------|-----|
| 추상화 수준 | Factor graph | Cost function | Graph vertex/edge |
| 자동 미분 | 부분 지원 | 완전 지원 | 미지원 |
| Incremental | iSAM2 | 미지원 | 미지원 |
| Python 지원 | 양호 | 제한적 | 제한적 |
| 대표 사용처 | LIO-SAM | VINS-Mono | ORB-SLAM |

### 12.5.2 캘리브레이션 도구

**Kalibr** (ethz-asl):
- Camera-IMU, Camera-Camera, multi-IMU 캘리브레이션
- Continuous-time B-spline trajectory 기반
- AprilGrid 타겟 사용
- 사실상 표준이지만 설치가 까다로움 (ROS 의존)

**OpenCalib** (2023):
- 자율주행 전체 센서 스택의 통합 캘리브레이션
- Camera, LiDAR, Radar, IMU 간 모든 조합 지원
- Target-based + Targetless 모두 포함

**[direct_visual_lidar_calibration](https://arxiv.org/abs/2302.05094)** (Koide et al. 2023):
- NID 기반 targetless LiDAR-카메라 캘리브레이션
- SuperGlue로 초기 추정, NID로 정밀 정합
- 단일 촬영만으로 동작

### 12.5.3 평가 도구

**evo** (MH Grupp):
- Python 기반 궤적 평가 도구
- ATE, RPE 계산 및 시각화
- TUM, KITTI, EuRoC 등 다양한 형식 지원
- 명령행 도구와 Python API 모두 제공

```bash
# evo 사용 예시
# ATE 계산
evo_ape tum groundtruth.txt estimated.txt -va --plot --plot_mode xz

# RPE 계산
evo_rpe tum groundtruth.txt estimated.txt -va --delta 100 --delta_unit f

# 두 시스템 비교
evo_traj tum system_a.txt system_b.txt --ref groundtruth.txt -p --plot_mode xz
```

```python
# evo Python API 사용 예시

from evo.core import metrics, sync
from evo.core.trajectory import PosePath3D, PoseTrajectory3D
from evo.tools import file_interface
import numpy as np

def evaluate_trajectory(gt_file, est_file, align=True):
    """
    evo를 이용한 궤적 평가.
    
    Args:
        gt_file: ground truth 파일 경로 (TUM 형식)
        est_file: estimated 궤적 파일 경로
        align: SE(3) 정렬 수행 여부
    """
    # 궤적 로드
    traj_gt = file_interface.read_tum_trajectory_file(gt_file)
    traj_est = file_interface.read_tum_trajectory_file(est_file)
    
    # 타임스탬프 동기화
    traj_gt, traj_est = sync.associate_trajectories(traj_gt, traj_est)
    
    # ATE 계산
    ate_metric = metrics.APE(metrics.PoseRelation.translation_part)
    
    if align:
        # Umeyama alignment
        traj_est_aligned = traj_est.align(traj_gt, correct_scale=False)
        ate_metric.process_data((traj_gt, traj_est_aligned))
    else:
        ate_metric.process_data((traj_gt, traj_est))
    
    stats = ate_metric.get_all_statistics()
    
    print(f"ATE RMSE: {stats['rmse']:.4f} m")
    print(f"ATE Mean: {stats['mean']:.4f} m")
    print(f"ATE Median: {stats['median']:.4f} m")
    print(f"ATE Max: {stats['max']:.4f} m")
    
    return stats

def compare_systems(gt_file, system_files, system_names):
    """여러 시스템의 ATE를 비교."""
    traj_gt = file_interface.read_tum_trajectory_file(gt_file)
    
    results = {}
    for name, est_file in zip(system_names, system_files):
        traj_est = file_interface.read_tum_trajectory_file(est_file)
        traj_gt_sync, traj_est_sync = sync.associate_trajectories(
            traj_gt, traj_est
        )
        
        traj_est_aligned = traj_est_sync.align(
            traj_gt_sync, correct_scale=False
        )
        
        ate = metrics.APE(metrics.PoseRelation.translation_part)
        ate.process_data((traj_gt_sync, traj_est_aligned))
        
        results[name] = ate.get_all_statistics()
    
    # 비교 테이블 출력
    print(f"{'System':<20} {'RMSE (m)':<12} {'Mean (m)':<12} {'Max (m)':<12}")
    print("-" * 56)
    for name, stats in results.items():
        print(f"{name:<20} {stats['rmse']:<12.4f} "
              f"{stats['mean']:<12.4f} {stats['max']:<12.4f}")
    
    return results
```

### 12.5.4 기타 필수 도구

**ROS 2** (Robot Operating System):
- 센서 퓨전 시스템의 통합 프레임워크
- 센서 드라이버, 시간 동기화, 메시지 전달 인프라 제공
- 대부분의 오픈소스 SLAM 시스템이 ROS 패키지로 나옴

**Open3D**:
- 3D 데이터 처리 Python 라이브러리
- Point cloud, mesh, TSDF 처리
- ICP, RANSAC, FPFH 등 기하학 알고리즘 내장
- 시각화 기능 우수

**CloudCompare**:
- 포인트 클라우드 비교/편집 GUI 도구
- 두 포인트 클라우드의 거리 비교(C2C, C2M)
- 포인트 클라우드 정합, 필터링, 다운샘플링

**COLMAP**:
- Structure from Motion (SfM) + Multi-View Stereo (MVS) 파이프라인
- 이미지 집합으로 3D 재구성
- 센서 퓨전에서 camera intrinsic 추정이나 ground truth 맵 구축에 쓴다

---

이 챕터에서 살펴본 실전 시스템과 벤치마크는 Ch.2-11의 이론이 실제 제품과 연구에서 어떻게 작동하는지를 보여준다. 마지막 챕터에서는 아직 성숙하지 않았지만 분야의 방향을 바꿀 수 있는 **연구 프런티어**(Foundation model, event camera, 4D radar, end-to-end SLAM)를 본다.

---

# Ch.13 — Frontiers & Emerging Directions

Ch.2-12에서 센서 퓨전의 확립된 이론과 실전 시스템을 체계적으로 다루었다. 이 마지막 챕터에서는 시선을 미래로 돌린다.

센서 퓨전 분야는 빠르게 진화하고 있다. 이 챕터에서는 아직 완전히 성숙하지 않았지만, 향후 몇 년간 분야의 방향을 바꿀 수 있는 연구 프런티어를 다룬다. Foundation model의 공간 지능(spatial AI)으로의 확장, end-to-end 학습 SLAM, scene graph 기반의 환경 이해, cross-modal representation, 그리고 event camera와 4D radar라는 새로운 센서 모달리티의 퓨전이 그 주제다.

---

## 13.1 Foundation Models for Spatial AI

Foundation model — 대규모 데이터로 사전학습된 범용 모델 (DINOv2, CLIP, SAM, GPT-4V 등) — 이 센서 퓨전과 SLAM 파이프라인에 빠르게 유입되고 있다. 이 모델들은 특정 태스크를 위해 학습되지 않았음에도, 풍부한 시각적/의미론적 표현을 제공하여 전통 파이프라인의 여러 모듈을 대체하거나 강화한다.

### 13.1.1 DINOv2/CLIP Feature를 SLAM에 활용

**DINOv2의 시각 특징**: [DINOv2](https://arxiv.org/abs/2304.07193) (Oquab et al. 2023)는 자기지도학습(self-supervised learning)으로 훈련된 ViT로, 픽셀 수준의 dense feature를 제공한다. 이 feature는:

- 조명·계절이 달라도 (주간/야간, 여름/겨울) 같은 장소에서 유사한 feature를 생성한다.
- 같은 종류의 객체(예: 모든 "의자")에 유사한 feature를 부여하는 의미론적 인식 능력이 있다.
- 기하학적 구조(모서리, 평면 등)도 feature에 반영된다.

**AnyLoc의 접근**: [AnyLoc](https://arxiv.org/abs/2308.00688) (Keetha et al. 2023)은 DINOv2의 dense feature를 VLAD로 집계하여 글로벌 장소 디스크립터를 생성한다. 이 디스크립터는:

- 도심, 실내, 항공, 수중, 지하 등 다양한 환경에서 VPR 전용 학습 없이 동작한다.
- 기존 학습 기반 VPR (NetVLAD, CosPlace 등)을 다양한 도메인에서 능가한다.
- DINOv2의 31번째 레이어 value facet의 dense feature가 CLS 토큰보다 23% 더 좋은 성능을 보인다.

```python
import numpy as np

class FoundationModelFeatureExtractor:
    """
    DINOv2 기반 dense feature 추출의 개념적 구현.
    실제로는 torch + DINOv2 모델을 사용한다.
    """
    
    def __init__(self, model_name='dinov2_vitg14', layer=31, facet='value'):
        """
        Args:
            model_name: DINOv2 모델 이름
            layer: 추출할 레이어 (AnyLoc에서는 31)
            facet: 'key', 'query', 'value' 중 선택 (AnyLoc에서는 'value')
        """
        self.model_name = model_name
        self.layer = layer
        self.facet = facet
        # 실제로는 여기서 torch 모델을 로드
        # self.model = torch.hub.load('facebookresearch/dinov2', model_name)
        
    def extract_dense_features(self, image):
        """
        이미지에서 픽셀 수준 dense feature를 추출.
        
        Args:
            image: (H, W, 3) RGB 이미지
            
        Returns:
            features: (H', W', D) dense feature map
                      H' = H/14, W' = W/14 (patch size = 14)
                      D = 1536 (ViT-G14의 feature 차원)
        """
        # 1. 이미지를 14x14 패치로 분할
        # 2. ViT를 통과시켜 지정 레이어의 feature 추출
        # 3. (num_patches, D) 형태로 반환
        
        # placeholder
        H, W = image.shape[:2]
        h, w = H // 14, W // 14
        D = 1536
        features = np.random.randn(h, w, D).astype(np.float32)
        return features
    
    def extract_global_descriptor(self, image, num_clusters=64):
        """
        AnyLoc 스타일: dense feature → VLAD → global descriptor.
        
        Args:
            image: (H, W, 3) RGB 이미지
            num_clusters: VLAD 클러스터 수
            
        Returns:
            descriptor: (num_clusters * D,) global descriptor
        """
        dense_features = self.extract_dense_features(image)
        
        # Flatten spatial dimensions
        h, w, D = dense_features.shape
        features_flat = dense_features.reshape(-1, D)  # (N, D)
        
        # VLAD aggregation (simplified)
        # 실제로는 k-means로 클러스터 중심을 사전 계산
        cluster_centers = np.random.randn(num_clusters, D)  # placeholder
        
        vlad = np.zeros((num_clusters, D))
        for feat in features_flat:
            # Hard assignment
            dists = np.linalg.norm(cluster_centers - feat, axis=1)
            closest = np.argmin(dists)
            vlad[closest] += feat - cluster_centers[closest]
        
        # L2 정규화 (intra + inter)
        for i in range(num_clusters):
            norm = np.linalg.norm(vlad[i])
            if norm > 1e-8:
                vlad[i] /= norm
        
        descriptor = vlad.flatten()
        descriptor /= (np.linalg.norm(descriptor) + 1e-8)
        
        return descriptor
```

**SLAM 파이프라인에서의 활용 지점**:

| 파이프라인 모듈 | 전통 방법 | FM 대체/강화 |
|---------------|-----------|-------------|
| Feature detection | FAST, ORB | SuperPoint + DINOv2 hybrid |
| Feature matching | BF + ratio test | SuperGlue/LightGlue, LoFTR |
| Place recognition | DBoW2, Scan Context | AnyLoc (DINOv2 + VLAD) |
| Semantic segmentation | 전용 모델 학습 | [SAM](https://arxiv.org/abs/2304.02643), open-vocab segmentation |
| Depth estimation | Stereo matching | [Depth Anything](https://arxiv.org/abs/2401.10891) (monocular) |
| Loop closure verification | Geometric only | FM descriptor consistency |

### 13.1.2 Open-Vocabulary 3D Understanding

CLIP의 vision-language alignment을 3D 맵에 확장하면, 로봇이 자연어로 환경을 이해하고 탐색할 수 있다.

**작동 방식**:

1. SLAM으로 3D 맵(point cloud, mesh, 3DGS)을 구축한다.
2. 각 관측 이미지의 각 영역에 대해 CLIP visual feature를 추출한다.
3. 2D feature를 3D 맵의 대응 위치에 역투영하여 부착한다.
4. 사용자가 "소화기를 찾아"라고 하면, CLIP text encoder로 텍스트를 인코딩하고, 3D 맵에서 가장 높은 유사도를 가진 위치를 반환한다.

**ConceptFusion, LERF, OpenScene**: 이러한 접근의 대표적 시스템들. 사전 정의된 클래스 집합 없이, 임의의 텍스트 질의로 3D 공간을 탐색할 수 있다.

**현재의 한계**:
- CLIP feature의 공간적 해상도가 낮다 (패치 단위). 작은 객체의 정확한 위치 파악이 어렵다.
- 3D 일관성 보장이 어렵다 — 같은 객체가 다른 시점에서 다른 feature를 가질 수 있다.
- Computational cost: 모든 이미지에서 FM feature를 추출하는 것은 비용이 크다.

### 13.1.3 FM이 전통 파이프라인을 얼마나 대체할 수 있는가

현재 상황(2025~2026년)에서의 솔직한 평가:

**이미 대체가 진행 중인 영역**:
- **Visual place recognition**: AnyLoc이 DBoW2를 대부분의 환경에서 능가. 특히 조건 변화(주야간, 계절)가 있는 경우 격차가 크다.
- **Feature matching**: LoFTR, RoMa가 전통적 detect-describe-match를 대체하는 추세. Textureless 환경에서 특히 강점.
- **Monocular depth**: [Depth Anything](https://arxiv.org/abs/2401.10891)이 단안 카메라의 metric depth를 합리적 수준으로 추정. 보조 센서로 활용 가능.

**아직 대체가 어려운 영역**:
- **LiDAR odometry**: 전통 방법(ICP, LOAM, FAST-LIO2)이 여전히 압도적. 학습 기반 LiDAR odometry는 일반화와 정확도 모두에서 뒤처진다.
- **IMU integration**: 물리 모델 기반 preintegration이 학습으로 대체할 수 없는 정확도와 이론적 보장을 제공한다.
- **Backend optimization**: factor graph, iSAM2 같은 최적화 프레임워크는 FM으로 대체할 대상이 아니다. 오히려 FM의 출력을 factor로 통합하는 것이 올바른 방향이다.

**하이브리드 접근이 가장 유망**: 전통 파이프라인의 구조적 엄밀함을 유지하되, FM이 제공하는 강건한 feature/semantic 정보를 모듈별로 주입하는 것이 현재 가장 실용적인 방향이다.

**최근 주요 발전 (2024~2025)**:

- **[MASt3R-SLAM](https://arxiv.org/abs/2412.12392)** (Murai et al. CVPR 2025): 3D reconstruction foundation model(MASt3R)로부터 학습된 기하학적 prior를 SLAM에 직접 통합하여, 카메라 모델 가정 없이 15fps에서 globally-consistent dense SLAM을 달성했다.
- **[Depth Anything V2](https://arxiv.org/abs/2406.09414)** (Yang et al. NeurIPS 2024): 합성 데이터로 teacher를 학습하고 대규모 pseudo-label로 student를 훈련하는 전략으로, monocular depth estimation의 정확도와 강건성을 크게 향상시켰다. 센서 퓨전에서 depth prior로 활용 가능하다.

---

## 13.2 End-to-End Learned SLAM

전통 SLAM은 모듈형 파이프라인 (feature extraction → matching → motion estimation → mapping → loop closure → optimization)으로 구성된다. End-to-end 학습은 이 전체 파이프라인을 하나의 미분 가능한 시스템으로 만들어, 입력(이미지/센서)에서 출력(pose, map)까지 직접 학습하는 것을 목표로 한다.

### 13.2.1 현재의 대표 시스템

**[DROID-SLAM](https://arxiv.org/abs/2108.10869)** (Teed & Deng 2021): 현재 가장 성공적인 학습 기반 SLAM 시스템.

핵심 아키텍처:

1. **RAFT 기반 반복 업데이트 연산자**: Convolutional GRU가 correlation volume에서 추출한 특징으로 optical flow를 반복적으로 보정한다. 이 flow 보정이 correspondence를 정제하는 역할을 한다.

2. **미분 가능 Dense Bundle Adjustment (DBA)**: flow 보정값을 카메라 pose (SE(3))와 픽셀 단위 inverse depth 업데이트로 변환한다. Gauss-Newton을 Schur complement로 효율적으로 풀되, 전체가 미분 가능하여 역전파로 학습 가능하다.

$$\begin{bmatrix} \mathbf{H}_{pp} & \mathbf{H}_{pd} \\ \mathbf{H}_{dp} & \mathbf{H}_{dd} \end{bmatrix} \begin{bmatrix} \Delta \boldsymbol{\xi} \\ \Delta \mathbf{d} \end{bmatrix} = \begin{bmatrix} \mathbf{b}_p \\ \mathbf{b}_d \end{bmatrix}$$

Schur complement로 pose만 먼저 풀 수 있다:

$$(\mathbf{H}_{pp} - \mathbf{H}_{pd} \mathbf{H}_{dd}^{-1} \mathbf{H}_{dp}) \Delta \boldsymbol{\xi} = \mathbf{b}_p - \mathbf{H}_{pd} \mathbf{H}_{dd}^{-1} \mathbf{b}_d$$

$\mathbf{H}_{dd}$는 대각 행렬이므로(각 depth는 독립) 역행렬이 $O(1)$이다. 이 구조가 전통 BA와 동일한 효율성을 학습 시스템 안에서 달성하는 방식이다.

3. **프레임 그래프 기반 루프 클로저**: co-visibility 기반으로 프레임 그래프를 동적 구축. 재방문 시 장거리 에지를 추가하여 implicit loop closure 수행.

4. **단일 모델로 monocular/stereo/RGB-D 지원**: 합성 데이터(TartanAir)만으로 학습 후, 4개 벤치마크에서 SOTA.

**DROID-SLAM의 성과와 의미**:
- TartanAir에서 이전 최고 대비 오차 62% 감소
- EuRoC monocular에서 82% 감소
- 학습 기반 SLAM이 처음으로 전통 시스템을 체계적으로 능가

### 13.2.2 Differentiable SLAM Components

완전한 end-to-end가 아니더라도, SLAM 파이프라인의 개별 컴포넌트를 미분 가능하게 만드는 연구가 활발하다:

**미분 가능 렌더링**: NeRF, 3DGS 자체가 미분 가능 렌더링 시스템이다. SLAM에서 pose estimation을 photometric loss의 역전파로 수행할 수 있다.

$$\hat{\mathbf{T}}^* = \arg\min_{\hat{\mathbf{T}}} \| I_{\text{real}} - \text{Render}(\text{Map}, \hat{\mathbf{T}}) \|^2$$

이때 $\text{Render}$ 함수가 미분 가능하므로, $\hat{\mathbf{T}}$에 대한 그래디언트를 직접 계산할 수 있다.

**미분 가능 ICP**: 전통 ICP의 nearest neighbor search와 SVD를 미분 가능하게 만들어, 포인트 클라우드 정합을 학습 루프에 포함시킬 수 있다.

**미분 가능 pose graph optimization**: iSAM2 같은 최적화를 미분 가능하게 만들면, 프론트엔드(feature extraction, matching)를 백엔드 오차 신호로 학습시킬 수 있다. "최적화 결과가 나쁘면 → feature extractor를 개선하라"는 end-to-end 학습 신호.

### 13.2.3 현재의 한계와 가능성

**한계**:
- **일반화**: 학습 데이터에 없는 환경에서의 성능 저하. DROID-SLAM은 합성 데이터로 학습하여 어느 정도 일반화하지만, LiDAR가 주도하는 대규모 실외 환경에서는 아직 전통 시스템에 미치지 못한다.
- **이론적 보장 부재**: 전통 최적화는 수렴성, 일관성 등의 이론적 보장이 있다. 학습 기반 시스템은 이러한 보장이 없어 안전 중요(safety-critical) 응용에 적용하기 어렵다.
- **Computational cost**: 학습 기반 시스템은 대부분 GPU가 필요하다. 임베디드 환경에서의 실시간 동작이 도전적.
- **Interpretability**: 실패 시 원인 분석이 어렵다. 전통 시스템은 "어느 모듈에서 실패했는가"를 추적할 수 있지만, end-to-end 시스템은 블랙박스에 가깝다.

**가능성**:
- FM의 발전으로 feature extraction과 matching의 품질이 계속 향상.
- 미분 가능 최적화 기법의 성숙으로, 전통 구조를 유지하면서 학습의 이점을 취하는 하이브리드 접근이 현실적.
- Multi-task learning: pose estimation, depth estimation, semantic segmentation을 동시에 학습하여 상호 보완.

---

## 13.3 Spatial Memory & Scene Graphs

로봇이 "공간을 기억하고 이해한다"는 것은 단순히 포인트 클라우드를 저장하는 것 이상이다. 인간은 "주방에 냉장고가 있고, 그 안에 우유가 있었다"는 식의 계층적, 관계적, 시간적 공간 기억을 가진다. 이 섹션은 이러한 고수준 공간 기억 시스템의 연구 프런티어를 다룬다.

### 13.3.1 Persistent Spatial Memory

전통 SLAM 맵은 "현재 이 순간의 환경 상태"를 반영한다. Persistent spatial memory는 시간에 따른 환경의 변화 이력까지 포함하는 장기 공간 기억이다.

**핵심 과제**:

1. **Episodic spatial memory**: "지난주 화요일에 여기에 상자가 있었다"와 같은 시간-장소-사건 연결.
2. **Semantic persistence**: 영구적 요소(벽, 건물)와 일시적 요소(사람, 차량)를 구분하여 장기 맵의 안정성 유지.
3. **Incremental forgetting**: 오래된 관측의 세부사항은 서서히 잊되, 핵심 구조는 유지. 인간의 기억 방식과 유사.

```python
class PersistentSpatialMemory:
    """
    시간적 공간 기억 — 환경 상태의 시계열 기록.
    """
    
    def __init__(self, decay_rate=0.01):
        self.memories = {}  # {location_key: [MemoryEntry, ...]}
        self.decay_rate = decay_rate
    
    def record(self, location, observation, timestamp, semantic_class=None):
        """새 관측을 공간 기억에 기록."""
        key = self._spatial_key(location)
        
        entry = {
            'timestamp': timestamp,
            'observation': observation,
            'semantic_class': semantic_class,
            'confidence': 1.0,
            'access_count': 0
        }
        
        if key not in self.memories:
            self.memories[key] = []
        self.memories[key].append(entry)
    
    def recall(self, location, time_query=None, semantic_query=None):
        """
        공간 기억에서 관련 정보를 회상.
        
        Args:
            location: 질의 위치
            time_query: 특정 시점 질의 (예: "3일 전")
            semantic_query: 의미론적 질의 (예: "의자")
        """
        key = self._spatial_key(location)
        if key not in self.memories:
            return []
        
        results = []
        for entry in self.memories[key]:
            relevance = entry['confidence']
            
            if time_query is not None:
                time_diff = abs(entry['timestamp'] - time_query)
                relevance *= np.exp(-self.decay_rate * time_diff)
            
            if semantic_query is not None:
                if entry['semantic_class'] != semantic_query:
                    continue
            
            if relevance > 0.1:
                results.append((entry, relevance))
                entry['access_count'] += 1
        
        return sorted(results, key=lambda x: x[1], reverse=True)
    
    def detect_changes(self, location, current_observation):
        """현재 관측과 기억을 비교하여 변화 감지."""
        key = self._spatial_key(location)
        if key not in self.memories:
            return 'new_location'
        
        latest = self.memories[key][-1]
        
        # 관측 비교 (placeholder — 실제로는 feature 비교)
        similarity = self._compare_observations(
            latest['observation'], current_observation
        )
        
        if similarity < 0.5:
            return 'significant_change'
        elif similarity < 0.8:
            return 'minor_change'
        else:
            return 'no_change'
    
    def consolidate(self, max_entries_per_location=10):
        """
        기억 정리: 오래되고 접근 빈도가 낮은 기억을 제거.
        핵심 구조적 정보는 보존.
        """
        for key in self.memories:
            entries = self.memories[key]
            
            if len(entries) <= max_entries_per_location:
                continue
            
            # 우선순위: 최근성 + 접근 빈도 + confidence
            scored = []
            for entry in entries:
                score = (entry['confidence'] * 
                         (1 + entry['access_count']) * 
                         (1.0 / (1 + self.decay_rate * 
                                 (entries[-1]['timestamp'] - entry['timestamp']))))
                scored.append((entry, score))
            
            scored.sort(key=lambda x: x[1], reverse=True)
            self.memories[key] = [
                e for e, _ in scored[:max_entries_per_location]
            ]
    
    def _spatial_key(self, location, resolution=0.5):
        return tuple((np.array(location) / resolution).astype(int))
    
    def _compare_observations(self, obs1, obs2):
        return 0.5  # placeholder
```

### 13.3.2 Scene Graph 기반 환경 이해

Ch.11에서 [Hydra](https://arxiv.org/abs/2201.13360)의 3D Scene Graph를 다루었다. 여기서는 scene graph가 열어주는 미래 방향을 탐색한다.

**Scene Graph + Language**: Scene graph에 자연어 인터페이스를 결합하면, 로봇에게 "거실 소파 옆의 테이블 위에 있는 리모콘을 가져와"라는 자연어 명령을 이해시킬 수 있다. 이 명령은 scene graph의 계층적 탐색으로 변환된다:

1. "거실" → Room 노드 탐색
2. "소파 옆의 테이블" → Room 내 Object 노드의 관계 탐색
3. "리모콘" → 해당 Table 근처의 Object 탐색
4. 경로 계획 및 manipulation

**Scene Graph + LLM**: GPT-4 같은 LLM이 scene graph를 입력으로 받아 고수준 추론을 수행한다. "이 방에 사람이 넘어지면 가장 가까운 전화기는 어디에 있는가?" 같은 질의에 답할 수 있다.

**동적 Scene Graph**: Hydra의 현재 구현은 정적 환경을 가정한다. 동적 scene graph는 사람, 차량 등 움직이는 에이전트를 노드로 포함하고, 그들의 관계를 실시간으로 갱신한다. 이는 사회적 내비게이션(social navigation), 인간-로봇 상호작용(HRI)의 핵심이다.

### 13.3.3 시계열 공간 기억 관리

장기 운용 로봇은 공간 기억의 **생성, 유지, 삭제** 전략이 필요하다.

**Hierarchical forgetting**: 세부 정보(정확한 텍스처, 개별 포인트)는 시간에 따라 해상도를 낮추고, 구조적 정보(방의 배치, 통로 연결)는 영구 유지한다. 이는 인간의 공간 기억과 유사한 전략이다.

**Event-triggered update**: 전체 맵을 주기적으로 갱신하는 대신, 변화가 감지된 영역만 선택적으로 업데이트한다.

**Compression**: 시간이 지남에 따라 맵을 점진적으로 압축한다. 예를 들어, dense point cloud → sparse landmarks → topological graph → semantic description.

---

## 13.4 Cross-Modal Representation

센서 퓨전의 핵심 도전 중 하나는 이종 센서의 관측을 **공통 표현 공간**에서 비교하는 것이다. LiDAR 포인트 클라우드와 카메라 이미지는 완전히 다른 데이터 형태이지만, 같은 물리적 환경을 관측한다. Cross-modal representation은 이 "표현 격차(representation gap)"를 해소하는 연구 방향이다.

### 13.4.1 이종 센서 간 표현 정렬 문제

**왜 어려운가**:

- **차원 불일치**: LiDAR는 3D 포인트, 카메라는 2D 이미지, radar는 range-Doppler map. 데이터 형태가 본질적으로 다르다.
- **정보 비대칭**: LiDAR는 정확한 거리 정보를 제공하지만 텍스처가 없다. 카메라는 풍부한 텍스처를 제공하지만 절대 거리 정보가 없다.
- **센서 특유의 아티팩트**: LiDAR의 motion distortion, 카메라의 rolling shutter, radar의 speckle noise 등 각 센서 고유의 노이즈 패턴이 다르다.

### 13.4.2 Contrastive Learning for Cross-Modal Alignment

Contrastive learning은 같은 장소/객체의 다른 모달리티 관측을 가까이, 다른 장소/객체의 관측을 멀리 배치하는 표현을 학습한다.

$$\mathcal{L}_{\text{contrastive}} = -\log \frac{\exp(\text{sim}(f_L(\mathbf{x}_L), f_C(\mathbf{x}_C)) / \tau)}{\sum_{j} \exp(\text{sim}(f_L(\mathbf{x}_L), f_C(\mathbf{x}_C^j)) / \tau)}$$

여기서 $f_L$은 LiDAR encoder, $f_C$는 카메라 encoder, $\tau$는 temperature, $(\mathbf{x}_L, \mathbf{x}_C)$는 같은 장소의 LiDAR-카메라 쌍, $\mathbf{x}_C^j$는 negative sample이다.

**Cross-modal place recognition**에서의 응용: LiDAR로 만든 맵에서 카메라만으로 localization하는 시나리오. LiDAR descriptor와 camera descriptor가 같은 공간에 있으면, 카메라 query로 LiDAR 맵을 검색할 수 있다.

**LC$^2$** (Lee et al. 2023): LiDAR-Camera cross-modal place recognition. LiDAR BEV 이미지와 카메라 이미지의 feature를 공통 공간으로 정렬한다.

### 13.4.3 Knowledge Distillation

한 모달리티(teacher)의 풍부한 정보를 다른 모달리티(student)로 전달하는 방법.

**LiDAR → Camera distillation**: LiDAR의 정확한 3D 정보로 학습된 모델의 지식을, 카메라만 사용하는 모델로 전달한다. 이를 통해 배포 시에는 카메라만으로 LiDAR 수준의 3D 이해를 근사할 수 있다.

**Camera → LiDAR distillation**: 카메라의 풍부한 의미론적 정보를 LiDAR 처리 모델에 전달한다. 예를 들어, CLIP의 의미론적 feature를 LiDAR 포인트에 부여하여, 텍스트 질의로 LiDAR 맵을 검색할 수 있게 한다.

### 13.4.4 아직 열린 질문들

1. **모달리티 독립적(modality-agnostic) 표현이 가능한가?** LiDAR, 카메라, radar, event camera 등 어떤 센서 입력이든 같은 표현 공간으로 매핑할 수 있는 범용 encoder가 가능할까?

2. **Temporal alignment**: 다른 모달리티의 관측은 시간적으로 완벽히 동기화되지 않는다. 비동기 관측을 어떻게 공통 표현으로 융합할 것인가?

3. **Partial observation**: 하나의 센서가 일시적으로 실패(LiDAR가 비에 영향, 카메라가 어둠에 영향)할 때, 사용 가능한 모달리티만으로 일관된 표현을 유지하는 방법.

---

## 13.5 Event Camera 기반 퓨전

이벤트 카메라(Event Camera, Dynamic Vision Sensor, DVS)는 전통적 프레임 기반 카메라와 근본적으로 다른 센서다. 각 픽셀이 독립적으로 밝기 변화를 감지하여, 변화가 일어난 시점에만 **이벤트**를 비동기적으로 출력한다.

### 13.5.1 Event Camera의 원리와 장점

각 이벤트는 $(x, y, t, p)$로 표현된다:

- $(x, y)$: 픽셀 좌표
- $t$: 마이크로초 단위 타임스탬프
- $p \in \{+1, -1\}$: 극성 (밝아짐 / 어두워짐)

이벤트가 발생하는 조건:

$$|\log I(x, y, t) - \log I(x, y, t_{\text{last}})| \geq C$$

이때 극성(polarity)은 $p = \text{sign}(\log I(x, y, t) - \log I(x, y, t_{\text{last}}))$로 결정된다. 여기서 $I$는 밝기, $C$는 대비 임계값(contrast threshold), $t_{\text{last}}$는 해당 픽셀에서 마지막으로 이벤트가 발생한 시점이다.

**장점**:

| 특성 | 프레임 카메라 | Event 카메라 |
|------|------------|-------------|
| 시간 해상도 | 30~120 fps | 마이크로초 단위 |
| 동적 범위 | ~60 dB | >120 dB |
| 모션 블러 | 있음 | 거의 없음 |
| 데이터 출력 | 균일 프레임 | 비동기 이벤트 |
| 정적 장면 | 정보 제공 | 이벤트 없음 (정보 없음) |
| 전력 소비 | 높음 | 매우 낮음 |

**왜 센서 퓨전에서 중요한가**: Event camera는 전통 카메라가 실패하는 극한 조건 — 고속 회전, 급격한 조명 변화(터널 진입/출구), 저조도 환경 — 에서 강건하다. 다른 센서의 취약점을 보완하는 역할을 한다.

### 13.5.2 Event + Frame 퓨전

Event camera와 전통 프레임 카메라를 결합하는 접근:

**Event-enhanced frame tracking**: 프레임 간의 고속 모션을 이벤트로 추적하여, 프레임 기반 VO의 프레임 간격 사이를 채운다. 빠른 카메라 모션에서도 tracking이 끊기지 않는다.

**Event-aided HDR**: 이벤트의 높은 동적 범위를 활용하여, 프레임 이미지의 under/over-exposed 영역의 정보를 보완한다.

### 13.5.3 Event + IMU 퓨전

**[EVO](https://doi.org/10.1109/LRA.2016.2645143)** (Rebecq et al. 2017): Event-based Visual Odometry. 이벤트만으로 camera pose를 추정한다. IMU와 결합하면 스케일을 복원하고 정확도를 높일 수 있다.

**[Ultimate SLAM](https://arxiv.org/abs/1709.06310)** (Vidal et al. 2018): Event camera + 프레임 카메라 + IMU를 결합한 시스템. 세 센서의 상보성을 활용:
- 프레임 카메라: 정적 장면에서의 풍부한 텍스처
- Event camera: 고속 모션에서의 연속적 추적
- IMU: 스케일 복원과 빠른 모션 예측

**현재 과제**:
- Event camera의 데이터 형식(비동기 이벤트 스트림)이 전통적 컴퓨터 비전 파이프라인(프레임 기반)과 호환되지 않는다. 이벤트를 프레임으로 변환(event frame)하면 장점을 잃는다.
- 상용 event camera가 아직 고가이며, 해상도가 낮다 (최신 모델도 1280 × 720 수준).
- 학습 데이터가 부족하다. 대부분의 데이터셋은 프레임 카메라용이다.

**최근 주요 발전 (2024~2025)**:

- **[EvenNICER-SLAM](https://arxiv.org/abs/2410.03812)** (2024): Event camera를 neural implicit SLAM에 통합한 시스템으로, 이벤트의 높은 시간 해상도를 활용하여 고속 모션에서의 tracking 강건성을 향상시켰다.
- **Event-based 3D reconstruction survey** ([arxiv:2505.08438](https://arxiv.org/abs/2505.08438), 2025): Event-driven 3D reconstruction 분야 서베이로, NeRF·3DGS 기반 이벤트 재구성, depth estimation, optical flow 등 최신 연구를 분류·정리했다.

```python
class EventProcessor:
    """
    Event camera 데이터 처리 유틸리티.
    """
    
    def __init__(self, width, height, time_window_us=33000):
        """
        Args:
            width, height: 센서 해상도
            time_window_us: 이벤트 누적 윈도우 (마이크로초)
        """
        self.width = width
        self.height = height
        self.time_window = time_window_us
    
    def events_to_frame(self, events, method='histogram'):
        """
        이벤트 스트림을 프레임으로 변환.
        
        Args:
            events: [(x, y, t, p), ...] 이벤트 리스트
            method: 'histogram' 또는 'time_surface'
            
        Returns:
            frame: (H, W) 또는 (H, W, 2) 이벤트 프레임
        """
        if method == 'histogram':
            return self._event_histogram(events)
        elif method == 'time_surface':
            return self._time_surface(events)
    
    def _event_histogram(self, events):
        """
        이벤트 히스토그램: 양/음 이벤트를 별도 채널로 누적.
        가장 단순한 변환이지만 시간 정보를 잃는다.
        """
        frame = np.zeros((self.height, self.width, 2), dtype=np.float32)
        
        for x, y, t, p in events:
            if 0 <= x < self.width and 0 <= y < self.height:
                channel = 0 if p > 0 else 1
                frame[int(y), int(x), channel] += 1
        
        return frame
    
    def _time_surface(self, events):
        """
        Time Surface: 각 픽셀의 가장 최근 이벤트 시간을 기록.
        시간 정보를 보존하면서 프레임 형태로 변환.
        """
        time_surface = np.zeros((self.height, self.width, 2), 
                                 dtype=np.float64)
        
        if len(events) == 0:
            return time_surface
        
        t_ref = events[-1][2]  # 참조 시간 (가장 최근)
        
        for x, y, t, p in events:
            if 0 <= x < self.width and 0 <= y < self.height:
                channel = 0 if p > 0 else 1
                time_surface[int(y), int(x), channel] = np.exp(
                    -(t_ref - t) / self.time_window
                )
        
        return time_surface
    
    def events_to_optical_flow(self, events, dt_us=10000):
        """
        이벤트에서 optical flow 추정 (contrast maximization 방식 간소화).
        
        핵심 아이디어: 올바른 optical flow로 이벤트를 시간적으로 정렬(warp)하면,
        이미지의 edge가 가장 선명해진다 (contrast가 최대화).
        """
        if len(events) < 100:
            return np.zeros((self.height, self.width, 2))
        
        # Contrast maximization:
        # argmax_{v} Var(I_warp(v))
        # 여기서 I_warp는 flow v로 warped된 이벤트 이미지
        
        # 간소화된 구현 (실제로는 최적화가 필요)
        best_flow = np.zeros(2)
        best_contrast = 0
        
        for vx in np.linspace(-2, 2, 20):
            for vy in np.linspace(-2, 2, 20):
                warped = np.zeros((self.height, self.width))
                
                t_ref = events[-1][2]
                for x, y, t, p in events:
                    dt = (t_ref - t) / 1e6  # 초 단위
                    wx = int(x + vx * dt)
                    wy = int(y + vy * dt)
                    
                    if 0 <= wx < self.width and 0 <= wy < self.height:
                        warped[wy, wx] += p
                
                contrast = np.var(warped)
                if contrast > best_contrast:
                    best_contrast = contrast
                    best_flow = np.array([vx, vy])
        
        flow = np.zeros((self.height, self.width, 2))
        flow[:, :] = best_flow
        return flow
```

---

## 13.6 4D Radar 퓨전

4D imaging radar는 센서 퓨전 분야에서 최근 주목받기 시작한 모달리티다. 전통적 automotive radar가 거리와 각도만 제공했다면, 4D radar는 거리(range), 방위각(azimuth), 고도(elevation), 도플러 속도(Doppler velocity)의 4차원 정보를 제공한다.

**FMCW radar의 거리/속도 측정 원리**: 4D radar의 대부분은 FMCW(Frequency-Modulated Continuous Wave) 방식을 사용한다. 송신 신호의 주파수를 시간에 따라 선형으로 증가(chirp)시키고, 반사 신호와의 비트 주파수(beat frequency)로 거리를, chirp 간 위상 변화로 속도를 측정한다:

$$R = \frac{c \cdot f_b}{2 \cdot S}$$

여기서 $R$은 타겟까지의 거리, $c$는 광속, $f_b$는 비트 주파수, $S$는 chirp의 주파수 변화율(Hz/s)이다.

$$v = \frac{\lambda \cdot \Delta\phi}{4\pi \cdot T_c}$$

여기서 $v$는 타겟의 radial velocity, $\lambda$는 캐리어 파장, $\Delta\phi$는 연속된 두 chirp 사이의 위상 변화, $T_c$는 chirp 주기이다.

### 13.6.1 악천후 Robustness

4D radar는 악천후에서도 작동한다. 이것이 이 센서의 핵심 가치다:

| 조건 | 카메라 | LiDAR | 4D Radar |
|------|--------|-------|----------|
| 맑은 날 | 최고 | 최고 | 양호 |
| 비 | 저하 | 약간 저하 | 정상 |
| 안개 | 심각 저하 | 심각 저하 | 정상 |
| 눈/먼지 | 심각 저하 | 심각 저하 | 정상 |
| 야간 | 심각 저하 | 정상 | 정상 |
| 직사광선 | 저하 | 정상 | 정상 |

Radar의 파장(밀리미터파)은 물방울, 안개 입자, 먼지보다 훨씬 크다. 이 입자들이 신호를 거의 산란시키지 못한다. LiDAR(근적외선)와 카메라(가시광선)가 갖는 한계를 여기서 보완한다.

### 13.6.2 4D Radar + Camera Fusion

4D radar와 카메라를 합치면 "악천후에서도 동작하는 저비용 인식 시스템"에 가까워진다:

**BEV 기반 퓨전**: 카메라 이미지에서 BEV feature를 추출하고 (LSS 또는 BEVFormer 방식), radar 포인트를 BEV 공간에 투영하여 결합한다.

**Radar의 Doppler 정보 활용**: 4D radar는 각 포인트의 시선 방향 속도(radial velocity)를 직접 측정한다. 이는 카메라나 LiDAR에는 없는 고유한 정보로:

- **동적 객체 분류**: 정적 배경과 움직이는 객체를 Doppler로 즉시 구분.
- **Ego-motion estimation**: 정적 포인트의 Doppler로 자차 속도를 추정 (IMU 없이도 가능).
- **Tracking 지원**: 객체의 속도 정보를 tracking에 직접 사용.

$$v_r = (\mathbf{v}_{\text{obj}} - \mathbf{v}_{\text{ego}}) \cdot \hat{\mathbf{r}}$$

여기서 $v_r$은 측정된 radial velocity, $\mathbf{v}_{\text{obj}}$는 객체 속도, $\mathbf{v}_{\text{ego}}$는 ego 속도, $\hat{\mathbf{r}}$은 radar → 타겟 방향 단위 벡터($\|\hat{\mathbf{r}}\| = 1$)다. 정적 객체($\mathbf{v}_{\text{obj}} = \mathbf{0}$)의 경우 $v_r = -\mathbf{v}_{\text{ego}} \cdot \hat{\mathbf{r}}$이 된다.

### 13.6.3 Radar Odometry의 최근 발전

Radar odometry는 2020년 이후 활발하게 연구되고 있다:

**FMCW radar odometry**: scanning FMCW radar (Navtech 등)에서의 odometry. Range-azimuth 이미지에서 특징점을 추출하고 매칭하여 ego-motion을 추정한다.

**4D radar odometry**: 4D radar 포인트 클라우드에서의 odometry. LiDAR odometry와 유사한 접근(ICP, feature matching)이 가능하지만, 해상도가 낮고 노이즈가 크다는 도전이 있다.

**Doppler 기반 ego-velocity estimation**: 정적 포인트의 Doppler 측정으로 ego-velocity를 직접 추정한다. RANSAC으로 동적 포인트를 제거하고, 정적 포인트의 Doppler로 $\mathbf{v}_{\text{ego}}$를 추정하는 방식:

$$v_r^{(k)} = -\mathbf{v}_{\text{ego}} \cdot \hat{\mathbf{r}}^{(k)} \quad \text{(정적 포인트)}$$

여기서 $k$는 포인트 인덱스. 최소 3개의 비공선(non-collinear) 포인트로 3D 속도 벡터를 추정할 수 있다.

```python
def estimate_ego_velocity_from_doppler(radar_points, doppler_values, 
                                        directions, ransac_threshold=0.3,
                                        max_iterations=100):
    """
    Radar Doppler 측정으로 ego-velocity 추정.
    
    정적 포인트의 Doppler: v_r = -v_ego · r_hat
    이는 선형 시스템으로 풀 수 있다.
    
    Args:
        radar_points: (N, 3) radar 포인트 좌표
        doppler_values: (N,) 각 포인트의 radial velocity
        directions: (N, 3) 각 포인트의 방향 단위 벡터
        ransac_threshold: RANSAC 인라이어 임계값 (m/s)
        max_iterations: RANSAC 최대 반복 수
        
    Returns:
        v_ego: (3,) ego-velocity 벡터
        inlier_mask: (N,) 정적 포인트 마스크
    """
    N = len(doppler_values)
    best_inliers = np.zeros(N, dtype=bool)
    best_v_ego = np.zeros(3)
    
    for _ in range(max_iterations):
        # 3개 포인트 랜덤 샘플링
        idx = np.random.choice(N, 3, replace=False)
        
        # 선형 시스템: v_r = -directions @ v_ego
        # A @ v_ego = b
        A = -directions[idx]  # (3, 3)
        b = doppler_values[idx]  # (3,)
        
        try:
            v_ego_candidate = np.linalg.solve(A, b)
        except np.linalg.LinAlgError:
            continue
        
        # 모든 포인트에 대한 잔차 계산
        predicted_doppler = -directions @ v_ego_candidate
        residuals = np.abs(doppler_values - predicted_doppler)
        
        inliers = residuals < ransac_threshold
        
        if np.sum(inliers) > np.sum(best_inliers):
            best_inliers = inliers
            best_v_ego = v_ego_candidate
    
    # 인라이어로 재추정 (least squares)
    if np.sum(best_inliers) >= 3:
        A = -directions[best_inliers]
        b = doppler_values[best_inliers]
        best_v_ego = np.linalg.lstsq(A, b, rcond=None)[0]
    
    return best_v_ego, best_inliers

def separate_static_dynamic(radar_points, doppler_values, directions,
                             v_ego, threshold=0.5):
    """
    Ego-velocity를 이용하여 정적/동적 포인트 분류.
    
    Args:
        v_ego: 추정된 ego-velocity (3,)
        threshold: 정적/동적 분류 임계값 (m/s)
        
    Returns:
        static_mask: (N,) 정적 포인트
        dynamic_mask: (N,) 동적 포인트
        object_velocities: (N,) 각 포인트의 추정 객체 속도 (radial)
    """
    # 정적 가정 하의 예상 Doppler
    expected_doppler = -directions @ v_ego
    
    # 잔차 = 실제 Doppler - 예상 Doppler
    residuals = doppler_values - expected_doppler
    
    static_mask = np.abs(residuals) < threshold
    dynamic_mask = ~static_mask
    
    # 동적 포인트의 객체 속도 (radial component)
    object_velocities = residuals  # v_obj · r_hat
    
    return static_mask, dynamic_mask, object_velocities
```

### 13.6.4 대표 데이터셋과 벤치마크

| 데이터셋 | 센서 | 환경 | 특징 |
|----------|------|------|------|
| **[Boreas](https://arxiv.org/abs/2203.10168)** (Burnett et al. 2023) | Camera, LiDAR, Radar, GNSS/IMU | 도심 (다양한 날씨) | 1년간 동일 경로 반복, 악천후 포함 |
| **RadarScenes** | Radar, Camera, LiDAR | 도심 | 기존 automotive radar 포인트 + semantic labels (point-level annotation) |
| **nuScenes** | Camera, LiDAR, Radar | 도심 | 5개 radar 포함, 악천후 일부 |
| **View-of-Delft** | Camera, LiDAR, 4D Radar | 도심 | 4D radar + 3D annotation |

**최근 주요 발전 (2024~2025)**:

- **[Snail-Radar](https://arxiv.org/abs/2407.11705)** (2024): 4D radar 기반 SLAM 평가 벤치마크로, 핸드헬드·자전거·SUV 세 플랫폼에서 다양한 날씨/조명 조건으로 수집된 44개 시퀀스를 제공한다.
- **[4D Radar-Inertial Odometry](https://arxiv.org/abs/2412.13639)** (2024): 3D Gaussian 기반 radar scene representation과 multi-hypothesis scan matching을 제안하여 voxel 방식 대비 더 정밀한 radar odometry를 달성했다.

4D radar 퓨전은 아직 초기 단계다. 특히 Doppler 정보를 활용한 ego-motion estimation과 동적 객체 분류는 LiDAR나 카메라로는 대체할 수 없는 고유한 기능이다.

---

## 마무리

이 가이드는 센서 모델링(Ch.2)에서 출발하여 캘리브레이션(Ch.3), 상태 추정 이론(Ch.4), 특징점 매칭(Ch.5), VO/VIO(Ch.6), LiDAR odometry(Ch.7), 멀티센서 퓨전(Ch.8), Place Recognition(Ch.9), Loop Closure(Ch.10), 공간 표현(Ch.11), 실전 시스템(Ch.12), 그리고 연구 프런티어(Ch.13)까지 — 센서 퓨전의 전체 파이프라인을 단계별로 다루었다.

이 분야의 핵심 서사를 다시 한번 정리하면:

1. **전통 방법은 여전히 기반이다.** 칼만 필터, ICP, RANSAC, 팩터 그래프 — 수십 년 전에 제안된 이 방법들이 현대 시스템의 뼈대를 이루고 있다.
2. **딥러닝은 지각(perception)에서 전통 방법을 밀어냈다.** 특징점 매칭, 깊이 추정, Place Recognition 등 "무엇을 보는가"의 영역에서 학습 기반 방법이 전통을 압도한다.
3. **추론(inference)에서는 전통과 학습이 공존한다.** 상태 추정 backend는 여전히 최적화 기반이 지배적이지만, DROID-SLAM처럼 미분 가능 최적화로 경계를 허무는 시도가 진행 중이다.
4. **Foundation model이 파이프라인을 바꾸고 있다.** DINOv2, SAM 등 범용 모델의 표현력이 센서 퓨전 파이프라인 곳곳에 유입되고 있다.

센서 퓨전은 불완전한 관측으로부터 세상을 이해하는 기술이다. 이 가이드가 그 이해의 출발점이 되기를 바란다.

# Ch.1 — Why Sensor Fusion?

This guide is an in-depth reference that systematically covers the theory and practice of sensor fusion. It is aimed at readers who are newcomers to robotics but have a working foundation in linear algebra and probability. The first chapter draws a map of the sensor fusion landscape as a whole — why a single sensor is insufficient, in what ways sensors can be combined, and what roles classical methods and deep-learning-based methods each play.

---

## 1.1 Limitations of a Single Sensor

When designing real-world robotic systems — autonomous vehicles, drones, service robots — the first question one encounters is, "Which sensor will we use to perceive the world?" Each sensor is grounded in a physical principle that lets it observe a particular facet of the environment, and that same physical principle imposes its intrinsic limitations.

### Camera Limitations

Cameras provide rich visual information about the environment, yet several fundamental limitations exist.

**Illumination dependence.** A camera is a passive sensor that detects light reflected from objects. Performance therefore degrades sharply under poor lighting conditions such as nighttime, tunnels, or backlight. Auto-exposure mitigates this to some extent, but in scenes that exceed the sensor's dynamic range, saturation or underexposure is unavoidable.

**Scale ambiguity.** A monocular camera projects the 3D world onto a 2D image and in doing so loses depth information. A 1 m object at 2 m distance and a 10 m object at 20 m distance can appear at identical size in the image. This scale ambiguity is the root cause of why monocular visual odometry cannot recover absolute scale. Without stereo cameras or fusion with other sensors, metric distance estimation in meters is fundamentally impossible.

**Textureless environments.** In environments lacking visual features — white walls, long corridors, wide paved roads — feature point extraction and tracking fail. Direct visual odometry methods face the same problem when the photometric gradient is insufficient.

**Motion blur.** During high-speed motion or sharp rotation, images blur, and feature point extraction and matching performance degrade significantly.

### LiDAR Limitations

LiDAR (Light Detection And Ranging) is an active sensor that emits laser pulses and measures the time-of-flight of the reflected wave to provide precise 3D range information. However, it has the following limitations.

**Absence of texture information.** LiDAR captures geometry precisely but provides no color or texture information about objects (some LiDARs report reflection intensity, but this is extremely limited compared with a camera image). As a result, place recognition becomes difficult in structurally similar places — for example, a street with repeated buildings of identical shape.

**Weather and environmental sensitivity.** In rain, fog, snow, dust, and similar conditions, the laser beam scatters, producing large numbers of ghost points or drastically reducing detection range. Measurements are also unstable on black objects or highly reflective surfaces (glass, metal).

**Low resolution and cost.** Mechanical spinning LiDARs have limited vertical resolution (e.g., 16-channel, 32-channel). High-resolution LiDARs cost from several thousand to tens of thousands of dollars. Solid-state LiDARs have recently reduced cost, but they trade off a narrower field of view (FoV).

### IMU Limitations

An inertial measurement unit (IMU) is a sensor that measures acceleration and angular velocity, providing high-frequency (typically 100 Hz–1 kHz) proprioceptive data. Its greatest advantage is independence from the external environment, but it has a critical limitation.

**Drift.** When IMU measurements are integrated to compute velocity and position, sensor bias and noise accumulate over time. Integrating acceleration twice to obtain position causes the error to grow in proportion to $t^2$. Even navigation-grade high-end IMUs exhibit substantial position error within minutes, while the MEMS-grade IMUs commonly used in robotics incur meter-level errors within seconds.

$$\delta \mathbf{p}(t) \approx \frac{1}{2} \mathbf{b}_a \, t^2 + \frac{1}{\sqrt{3}} \sigma_a \, t^{3/2}$$

Here $\mathbf{b}_a$ is the accelerometer bias and $\sigma_a$ is the acceleration noise density. A bias of only $0.01\,\text{m/s}^2$ yields a position error of 0.5 m after 10 seconds.

**Absence of an absolute reference.** The IMU measures only relative changes and provides no information about absolute position or absolute heading. Roll and pitch can be extracted from the gravity direction, but yaw is unobservable without a magnetometer (an observability issue).

### GNSS Limitations

A global navigation satellite system (GNSS) provides absolute global position, but has the following limitations.

**Obstructed environments.** Indoors, in tunnels, and between tall buildings in dense urban settings (urban canyons), satellite signals are blocked or reflected via multipath, producing errors of tens of meters.

**Update rate.** GNSS typically has a low update rate of 1–10 Hz, making it difficult to track fast dynamic motion.

**Precision limits.** The precision of standard single-point positioning is at the meter level. RTK (Real-Time Kinematic) improves this to centimeter level, but requires a base station and takes time for initial convergence.

### Complementarity of Sensor Limitations

Organizing the sensor limitations surveyed above in a table makes it clear that the weaknesses of one sensor can be compensated by the strengths of another.

| Property | Camera | LiDAR | IMU | GNSS |
|------|--------|-------|-----|------|
| Absolute position | ✗ | ✗ | ✗ | ✓ |
| Relative motion (short-term) | ✓ | ✓ | ✓ | ✗ |
| Relative motion (long-term) | △ (drift) | △ (drift) | ✗ (diverges) | ✓ |
| High-frequency motion capture | ✗ | ✗ | ✓ | ✗ |
| Illumination independence | ✗ | ✓ | ✓ | ✓ |
| Weather robustness | △ | ✗ | ✓ | ✓ |
| 3D geometric information | △ (depth ambiguous) | ✓ | ✗ | ✗ |
| Texture / semantic information | ✓ | ✗ | ✗ | ✗ |
| Indoor operation | ✓ | ✓ | ✓ | ✗ |
| Cost | Low | High | Medium~Low | Medium |

The message of this table is clear: **no single sensor is sufficient in all situations.** Sensor fusion is the systematic answer to this problem.

---

## 1.2 Taxonomy of Sensor Fusion

Sensor fusion is the technique of combining information from multiple sensors to achieve accuracy, robustness, and completeness beyond what any individual sensor can deliver. Depending on how the sensors are combined, we can classify sensor fusion into three categories.

### Complementary Fusion

In this form, sensors that measure different physical quantities compensate for each other's shortcomings. Each sensor observes a different subset of the full state, and combining them allows us to estimate the complete state — one that no single sensor alone can observe.

**Representative example: Camera + IMU (Visual-Inertial Odometry)**

- The camera provides relative changes of a 6-DoF pose, but its scale is ambiguous and it fails under high-speed motion.
- The IMU provides high-frequency acceleration/angular velocity, interpolating fast motion between camera frames and recovering scale from the gravity direction.
- The IMU supplies what the camera lacks — scale and high-frequency motion — while the camera supplies what the IMU lacks — drift correction.

**Representative example: GNSS + IMU**

- GNSS provides low-frequency (1–10 Hz) absolute position.
- The IMU provides high-frequency (hundreds of Hz) relative motion.
- When GNSS is lost inside a tunnel, the IMU carries navigation in the short term, and when GNSS returns, the accumulated IMU drift is corrected.

The key point of this type is the **extension of observability**. State variables that are unobservable to one sensor become observable through another sensor's observations.

### Competitive Fusion

In this form, sensors measuring the same physical quantity are deployed redundantly to improve reliability and accuracy.

**Representative example: multi-camera systems**

- Two cameras pointing in the same direction independently track feature points.
- Even if one camera is contaminated (lens fouling, failure), the system continues to operate with the remaining camera.
- Combining the two estimates yields lower variance than either individual estimate.

**Statistical foundation.** Optimally combining two independent observations $z_1 \sim \mathcal{N}(\mu, \sigma_1^2)$ and $z_2 \sim \mathcal{N}(\mu, \sigma_2^2)$ gives:

$$\hat{\mu} = \frac{\sigma_2^2 z_1 + \sigma_1^2 z_2}{\sigma_1^2 + \sigma_2^2}, \quad \sigma_{\text{fused}}^2 = \frac{\sigma_1^2 \sigma_2^2}{\sigma_1^2 + \sigma_2^2}$$

The variance of the fused estimate is always smaller than that of either individual estimate: $\sigma_{\text{fused}}^2 < \min(\sigma_1^2, \sigma_2^2)$. This is exactly the same principle as the update step of a Kalman filter.

**Representative example: multi-LiDAR systems**

In autonomous vehicles, it is common to arrange 4–6 LiDARs around the vehicle to secure a 360° field of view while also using redundant observations in overlapping regions to increase reliability.

### Cooperative Fusion

In this form, raw data from each sensor is combined to generate **new forms of information** that no single sensor could produce.

**Representative example: Stereo Vision**

- The left and right camera images are combined to compute disparity.
- Disparity yields 3D depth information.
- A single camera cannot determine depth, but the cooperation of two cameras generates a new physical quantity (depth).

**Representative example: Camera + LiDAR → colored point cloud**

- LiDAR points are projected onto the camera image to assign a color to each 3D point.
- The resulting colored point cloud carries both the precise geometry of LiDAR and the rich texture of the camera — something no individual sensor can produce.
- Systems such as [R3LIVE (Lin et al., 2022)](https://arxiv.org/abs/2109.07982) perform this in real time.

**Representative example: Radar + Camera → adverse-weather perception**

- Doppler measurements from radar and visual information from cameras are combined to simultaneously recognize the velocity and class of moving objects even in fog or rain.

The three categories are not mutually exclusive. Real-world systems often use all three simultaneously. For example, an autonomous vehicle's sensor fusion system includes cooperative fusion of camera + LiDAR (colored point cloud), complementary fusion of camera + IMU (VIO), and competitive fusion across multiple LiDARs (redundant coverage).

---

## 1.3 Classification by Coupling Level: Loosely vs Tightly vs Ultra-tightly Coupled

Another important axis for classifying sensor fusion is the **level at which sensor data is combined**. This classification directly affects a system's accuracy, complexity, and robustness.

### Loosely Coupled

In this approach, each sensor subsystem independently produces its own estimate, and these outputs are combined at a higher level.

**Structure:**
```
Sensor A → [Subsystem A] → Estimate A ─┐
                                        ├─→ [Fusion] → Final estimate
Sensor B → [Subsystem B] → Estimate B ─┘
```

**Representative example: Independent VO + independent LiDAR odometry → EKF fusion**

- Visual odometry independently outputs a pose.
- LiDAR odometry independently outputs a pose.
- A higher-level EKF combines the two poses to generate the final estimate.

**Advantages:**
- Modularity: each subsystem can be developed, tested, and replaced independently.
- Simplicity: the design of the fusion layer is relatively simple.
- Partial failure handling: even if one subsystem fails, the system can continue to operate using the outputs of the others.

**Disadvantages:**
- Information loss: each subsystem internally summarizes (compresses) its observations before outputting, so raw-observation detail (e.g., the individual uncertainty of each feature point) is lost.
- Ignoring correlations: when independent subsystems use a shared observation (e.g., the same IMU data), the two estimates become correlated, and ignoring this correlation leads to overconfidence. This is known as the "double counting" problem.
- Loss of optimality: because information is summarized, the overall system is not optimal in the information-theoretic sense.

### Tightly Coupled

In this approach, the **raw measurements** of all sensors are fed directly into a single estimation framework (a single estimator).

**Structure:**
```
Sensor A → raw measurement A ─┐
                                ├─→ [Single Estimator] → Final estimate
Sensor B → raw measurement B ─┘
```

**Representative example: [VINS-Mono (Qin et al., 2018)](https://arxiv.org/abs/1708.03852)**

- The camera's raw feature point observations and the IMU's raw acceleration/angular velocity measurements are placed together into a single nonlinear optimization (sliding-window optimization).
- The cost function of the optimization simultaneously minimizes the reprojection error and the IMU preintegration residual.

Here $\mathbf{r}_{\text{IMU}}$ is the IMU preintegration residual, $\mathbf{r}_{\text{cam}}$ is the visual reprojection residual, and $\mathcal{X}$ is the full state (pose, velocity, bias, landmarks).

**Representative example: [FAST-LIO2 (Xu et al., 2022)](https://arxiv.org/abs/2107.06829)**

- Individual LiDAR points and raw IMU measurements are fed directly into a single iterated error-state Kalman filter.

**Representative example: [LIO-SAM (Shan et al., 2020)](https://arxiv.org/abs/2007.00258)**

- LiDAR feature points, IMU preintegration, and GNSS position observations are jointly optimized in a single factor graph.

**Advantages:**
- Maximum information usage: all information in the raw observations is used, enabling information-theoretically superior estimation.
- Cross-calibration: inter-sensor cross-calibration happens naturally. For instance, camera observations contribute to IMU bias estimation, and IMU data stabilize camera feature point tracking.
- Graceful degradation: even when observations from one sensor drop (e.g., in feature-poor environments), observations from the others continue to support the estimate.

**Disadvantages:**
- Complexity: every sensor's observation model must be implemented in the single estimator, making system design and debugging complex.
- Computational cost: the state vector grows and the number of observations increases, raising computation.
- Sensor coupling: removing or replacing a specific sensor requires modifying the entire estimator.

### Ultra-tightly Coupled

In this approach, fusion occurs at the **signal level** of the sensors. This term is used mainly in GNSS/INS integration.

**Representative example: GNSS/INS ultra-tight integration**

- In typical tightly coupled schemes, the GNSS receiver outputs pseudoranges, which are fed into the navigation filter.
- In ultra-tight, the INS's predicted velocity is fed back into the code/carrier tracking loop inside the GNSS receiver.
- This narrows the receiver's tracking loop bandwidth, increasing noise immunity and maintaining satellite tracking in severely jammed or weak-signal environments.

**Analogous concepts in vision:**

In visual-inertial systems, the counterpart to ultra-tight coupling is using the IMU prediction to constrain the feature point search region of the camera, or to directly correct motion blur using IMU data. Setting the initial value of feature point tracking via the IMU prediction in VINS-Mono comes close to this.

### Comparison of Coupling Levels

| Property | Loosely Coupled | Tightly Coupled | Ultra-tightly Coupled |
|------|----------------|-----------------|----------------------|
| Fusion level | Output | Measurement | Signal |
| Information utilization | Low | High | Highest |
| Implementation complexity | Low | Medium~High | Very high |
| Modularity | High | Low | Very low |
| Partial failure handling | Easy | Requires design | Difficult |
| Representative systems | Independent VO + LO → EKF | VINS-Mono, LIO-SAM, FAST-LIO2, ORB-SLAM3 | GNSS/INS deep integration |

In modern robotics, the **tightly coupled** approach is mainstream. Loosely coupled is simple to implement but loses accuracy due to information loss, while ultra-tightly coupled requires access to specialized hardware and is therefore limited in scope. VINS-Mono, FAST-LIO2, LIO-SAM, and the other most widely used open-source systems today all adopt a tightly coupled architecture. Most recently, [FAST-LIVO2 (Zheng et al., 2024)](https://arxiv.org/abs/2408.14035), which tightly couples LiDAR, inertial, and visual sensors in a single framework, has shown results that substantially surpass existing systems in both accuracy and real-time performance.

---

## 1.4 Classical vs Learning-based: What Deep Learning Changed and What It Did Not

For decades, the field of sensor fusion was dominated by **classical** approaches grounded in probabilistic estimation theory (Kalman filter, factor graph) and geometric methods (epipolar geometry, ICP). Since the mid-2010s, as deep learning revolutionized almost every area of computer vision, learning-based methods have rapidly permeated sensor fusion as well. However, the extent of that penetration differs greatly by area.

### What Deep Learning Changed

**Feature extraction and matching.** Traditionally, handcrafted feature descriptors such as SIFT and ORB were used. [SuperPoint (DeTone et al., 2018)](https://arxiv.org/abs/1712.07629) performs keypoint detection and description jointly via self-supervised learning, greatly improving robustness to illumination and viewpoint changes. [SuperGlue (Sarlin et al., 2020)](https://arxiv.org/abs/1911.11763) revolutionized feature point matching using graph neural networks (GNNs) and attention mechanisms. Most recently, **detector-free** methods such as [LoFTR (Sun et al., 2021)](https://arxiv.org/abs/2104.00680) and [RoMa (Edstedt et al., 2024)](https://arxiv.org/abs/2305.15404) directly find dense correspondences without keypoints, succeeding at matching even in texture-scarce environments.

In this area, learning-based methods clearly surpass traditional methods — one can speak of a **paradigm shift**.

**Place Recognition.** The transition from Bag of Words (DBoW2) to [NetVLAD (Arandjelović et al., 2016)](https://arxiv.org/abs/1511.07247) was dramatic. CNN-based global descriptors enabled place recognition that is far more robust to illumination, seasonal, and viewpoint changes. More recently, [AnyLoc (Keetha et al., 2023)](https://arxiv.org/abs/2308.00688) leverages features from foundation models such as DINOv2 to deliver place recognition that works universally across diverse environments without any additional training.

**Monocular depth estimation.** Estimating depth from a single image is a task that is impossible with classical methods (geometric cues are insufficient). Models such as [Depth Anything (Yang et al., 2024)](https://arxiv.org/abs/2401.10891) have achieved remarkable levels of monocular depth estimation through training on large-scale data. Its successor, [Depth Anything V2 (Yang et al., 2024)](https://arxiv.org/abs/2406.09414), pushed precision further through synthetic-data training and large-scale pseudo-labeling, and [Metric3D v2 (Hu et al., 2024)](https://arxiv.org/abs/2404.15506) enables zero-shot absolute-scale depth estimation, opening the possibility of using metric depth information in sensor fusion without LiDAR. This technology has the potential to replace or complement LiDAR in sensor fusion.

**Map representations.** NeRF and 3D Gaussian Splatting opened a new paradigm for representing scenes with neural networks. NeRF-SLAM, Gaussian Splatting SLAM, and related systems provide photorealistic map representations that go beyond traditional point maps or voxel grids.

**Event cameras.** Event cameras, also called neuromorphic vision sensors, asynchronously detect brightness changes at each pixel, providing extremely high temporal resolution (microsecond level) and wide dynamic range. As a recent [event camera survey (Huang et al., 2024)](https://arxiv.org/abs/2408.13627) summarizes, event-based VIO and SLAM research is developing rapidly, and fusion with conventional frame-based cameras is opening new possibilities in high-speed motion and low-light environments.

### What Deep Learning Did Not Change

**State estimation backends.** Probabilistic estimation frameworks such as Kalman filters and factor graph optimization have not been replaced by deep learning. The reasons are clear:

1. **Rigorous propagation of uncertainty**: Kalman filters and factor graphs mathematically and rigorously track and propagate the uncertainty of observations. Deep learning models struggle to provide comparably calibrated uncertainty.
2. **Guaranteed physical laws**: physical laws are directly encoded into the state transition model (dynamics, kinematics), preventing physically impossible estimates. Learning-based methods cannot guarantee such hard constraints.
3. **Data efficiency**: probabilistic frameworks operate without any data, given only a sensor noise model and a system model. Learning-based methods require large-scale training data.
4. **Generalization**: learning-based odometry (e.g., DeepVO) tends to drop sharply in performance in environments that differ from the training data. Geometric methods are agnostic to the environment.

**LiDAR odometry.** Since [LOAM (Zhang & Singh, 2014)](https://frc.ri.cmu.edu/~zhangji/publications/RSS_2014.pdf), LiDAR odometry has remained overwhelmingly dominated by traditional methods. Point cloud registration methods such as ICP, GICP, and NDT are mathematically well understood, deliver excellent real-time performance, and apply immediately to new environments. Learning-based LiDAR odometry (DeepLO-family) has not yet reached the accuracy and generalization of traditional methods.

**Calibration.** For camera intrinsic calibration, the checkerboard method of [Zhang (2000)](https://doi.org/10.1109/34.888718) remains the standard. Learning-based methods for targetless calibration are under active research, but they have not yet surpassed target-based methods in precision.

### Hybrid Approach: The Current Mainstream

The most successful systems today adopt a **hybrid** structure that combines a learning-based frontend with a classical backend.

```
[Learning-based frontend]         [Classical backend]
 SuperPoint/SuperGlue          Factor Graph / EKF
 Mono Depth Estimation    →    Nonlinear Optimization
 Semantic Segmentation         Kalman Filtering
 Place Recognition              Pose Graph SLAM
```

- At the frontend, deep learning extracts high-level features (feature points, depth, semantic labels) from raw sensor data.
- At the backend, geometric/probabilistic frameworks integrate these features into temporally consistent state estimates.

[DROID-SLAM (Teed & Deng, 2021)](https://arxiv.org/abs/2108.10869) is a good example of this hybrid approach. It uses learned feature extraction and correspondence finding, while performing the final pose estimation with differentiable bundle adjustment.

### Technical Lineage Summary

This guide consistently shows, for each topic, the flow of **traditional methods → what deep learning enabled → where tradition is still needed**. The table below summarizes the technical lineage that runs through the entire guide.

| Area | Classical | Learning-based | Current mainstream |
|------|-----------|---------------|----------|
| Feature matching | SIFT/ORB + BF/FLANN | SuperPoint+SuperGlue → LoFTR → RoMa | Hybrid |
| Visual odometry | Feature-based (ORB) / Direct (DSO) | DROID-SLAM, DPV-SLAM | Tradition leads, learning catching up |
| LiDAR odometry | ICP/LOAM | DeepLO-family | Tradition overwhelmingly leads |
| Place recognition | BoW/VLAD | NetVLAD → AnyLoc | Learning leads |
| Depth estimation | Stereo matching | Mono depth (Depth Anything) | Learning leads (mono) |
| Calibration | Target-based | Targetless + learning | Tradition leads |
| Map representation | Occupancy/TSDF | NeRF / 3DGS | Coexistence |
| State estimation backend | KF / Factor Graph | End-to-end attempts | Tradition overwhelmingly leads |

A noteworthy pattern in this table is that **the closer to perception, the stronger learning is; the closer to inference/estimation, the stronger tradition is**. This pattern provides an important criterion for deciding where to invest deep learning and where to retain traditional methods when designing a sensor fusion system.

So far we have surveyed the classification of sensor fusion and the division of roles between classical and learning-based methods. We now introduce how this guide treats this broad field and in what order the reader might best proceed.

---

## 1.5 Scope and Organization of This Guide

### Relation to robotics-practice

This guide is the advanced companion to the robotics-practice guide. Whereas robotics-practice is an introductory survey covering Spatial AI broadly, this guide is an in-depth reference focused on **sensor fusion, localization, and retrieval**.

- Where robotics-practice introduces EKF/PF in one or two pages, this guide treats each topic at chapter depth — from the derivation of the Kalman filter through ESKF, IMU preintegration, and factor graph optimization.
- Where robotics-practice introduces sensors at a general level, this guide derives each sensor's **noise model and observation equations** in equation form.
- Overlapping introductory content is replaced by references to robotics-practice; this guide covers only the additional depth.

### Guide Organization

This guide follows the organization below:

1. **Ch.1 — Introduction** (this chapter): motivation and classification of sensor fusion
2. **Ch.2 — Sensor Modeling**: observation models and noise characteristics of each sensor (equation-focused)
3. **Ch.3 — Calibration**: calibration theory and practice for various sensor combinations
4. **Ch.4 — State Estimation Theory**: Bayesian filtering, the Kalman filter family, particle filters, factor graphs
5. **Ch.5 — Feature Matching & Correspondence**: technical lineage from SIFT to RoMa
6. **Ch.6 — Visual Odometry & VIO**: internal architecture of VO/VIO systems
7. **Ch.7 — LiDAR Odometry & LIO**: LiDAR-based odometry and LiDAR-inertial fusion
8. **Ch.8 — Multi-Sensor Fusion Architectures**: design principles for multi-sensor integration
9. **Ch.9 — Place Recognition & Retrieval**: from BoW to foundation models
10. **Ch.10 — Loop Closure & Global Optimization**: loop closure and global optimization
11. **Ch.11 — Spatial Representations**: from point maps to neural maps
12. **Ch.12 — Practical Systems & Benchmarks**: real applications and evaluation
13. **Ch.13 — Frontiers**: recent developments and open problems

### Target Audience

The intended audience of this guide is a robotics newcomer with the following background:

- **Linear algebra**: understanding of matrix operations, eigendecomposition, and SVD
- **Probability**: understanding of probability distributions, conditional probability, Bayes' theorem, and the Gaussian distribution
- **Basic optimization**: familiarity with least squares and gradient descent
- **Python**: ability to read and run code built on numpy and scipy

Each chapter of this guide follows the flow **intuition → equations → code/examples**. We first provide an intuitive understanding of a concept, then derive it rigorously in mathematics, and finally implement it in Python code to verify.

---

## 1.6 Cross-Cutting Themes of This Guide

Throughout this guide, the reader will repeatedly encounter several core questions:

1. **"Why was this traditional method important?"** — Understand the fundamental problem each traditional method solved and the elegance of its solution.
2. **"What did deep learning change?"** — See concretely which limitations of traditional methods learning-based methods overcame.
3. **"Where is tradition still needed?"** — Analyze the areas that deep learning has not replaced and why.
4. **"Where is the gap between theory and practice?"** — Examine the differences between papers and real systems, and the engineering issues encountered in practice.

Keeping these questions in mind while reading each chapter will allow the reader to see not only individual algorithms but also the full landscape of sensor fusion as a field.

In the next chapter, we treat the starting point of sensor fusion — **the observation model of each sensor** — mathematically. Precisely expressing in equations how a sensor "sees" the world is the foundation of every fusion algorithm.

---

# Ch.2 — Sensor Modeling

In Ch.1 we examined the taxonomy of sensor fusion and its design principles. We now turn in earnest to defining, in mathematical terms, how each sensor "sees" the world. Sensor observation models are substituted directly into the measurement function $h(\mathbf{x})$ of the Kalman filter and factor graph formulations covered in Ch.4, so the equations of this chapter form the foundation for every algorithm that follows.

> If robotics-practice Ch.2 introduced sensors at an overview level, this chapter focuses on **noise models and mathematical observation models**. To design a sensor fusion algorithm, we must know precisely not only "what a sensor measures" but also "what mathematical relationship the measurement bears to the underlying physical quantity" and "how the error is distributed."

---

## 2.1 Camera Observation Model

A camera is a sensor that projects points of the 3D world onto a 2D image plane. Modeling this projection mathematically is the crux of the camera observation model.

### 2.1.1 Pinhole Camera Model

The pinhole camera model is the most basic mathematical model of a camera. It assumes that a 3D point is projected onto the image plane along a straight line passing through the optical center.

For a 3D point $\mathbf{P}_c = [X_c, Y_c, Z_c]^\top$ in the camera coordinate frame, its projected point $\mathbf{p} = [u, v]^\top$ on the image plane is computed as:

$$\begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \frac{1}{Z_c} \mathbf{K} \begin{bmatrix} X_c \\ Y_c \\ Z_c \end{bmatrix}$$

Here $\mathbf{K}$ is the camera intrinsic matrix:

$$\mathbf{K} = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix}$$

Meaning of each parameter:
- $f_x, f_y$: focal length. The physical focal length $f$ divided by the pixel size $(\Delta x, \Delta y)$: $f_x = f / \Delta x$, $f_y = f / \Delta y$. Typically $f_x \approx f_y$, but the two values can differ for non-square pixels.
- $c_x, c_y$: principal point. The pixel coordinates at which the optical axis intersects the image plane. Ideally the image center, but manufacturing tolerances can shift it by several pixels.

In homogeneous coordinates:

$$s \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \mathbf{K} [\mathbf{R} | \mathbf{t}] \begin{bmatrix} X_w \\ Y_w \\ Z_w \\ 1 \end{bmatrix}$$

Here $[\mathbf{R} | \mathbf{t}]$ is the extrinsic parameters mapping the world frame to the camera frame, and $s = Z_c$ is the depth scale factor.

Denoting the projection function as $\pi(\cdot)$:

$$\mathbf{p} = \pi(\mathbf{P}_c) = \begin{bmatrix} f_x \frac{X_c}{Z_c} + c_x \\ f_y \frac{Y_c}{Z_c} + c_y \end{bmatrix}$$

The Jacobian of this nonlinear projection function plays a central role in state estimation:

$$\frac{\partial \pi}{\partial \mathbf{P}_c} = \begin{bmatrix} \frac{f_x}{Z_c} & 0 & -\frac{f_x X_c}{Z_c^2} \\ 0 & \frac{f_y}{Z_c} & -\frac{f_y Y_c}{Z_c^2} \end{bmatrix}$$

This $2 \times 3$ Jacobian is used directly to linearize the observation model in EKF-based VIO, and it is also the key building block of the residual Jacobian in nonlinear optimization.

### 2.1.2 Lens Distortion Model

Real camera lenses introduce distortion that deviates from the ideal projection of the pinhole model. Neglecting distortion can inflate the reprojection error from a few pixels to tens of pixels, so it must be corrected for any application demanding accurate sensor fusion.

#### Radial-Tangential Distortion (Brown-Conrady Model)

This is the most widely used distortion model and is OpenCV's default.

For normalized image coordinates $\mathbf{p}_n = [x_n, y_n]^\top = [X_c/Z_c, \, Y_c/Z_c]^\top$:

$$r^2 = x_n^2 + y_n^2$$

**Radial distortion:**

$$x_r = x_n (1 + k_1 r^2 + k_2 r^4 + k_3 r^6)$$
$$y_r = y_n (1 + k_1 r^2 + k_2 r^4 + k_3 r^6)$$

Radial distortion arises from lens curvature and grows with the distance from the image center. If $k_1 < 0$ the result is barrel distortion (magnification toward the center); if $k_1 > 0$ the result is pincushion distortion (magnification toward the edges).

**Tangential distortion:**

$$x_t = 2p_1 x_n y_n + p_2 (r^2 + 2x_n^2)$$
$$y_t = p_1 (r^2 + 2y_n^2) + 2p_2 x_n y_n$$

Tangential distortion appears when the lens is not perfectly parallel to the image sensor (decentering). It is much smaller in magnitude than radial distortion, but cannot be ignored in precision applications.

**Final distorted coordinates:**

$$\begin{bmatrix} x_d \\ y_d \end{bmatrix} = \begin{bmatrix} x_r + x_t \\ y_r + y_t \end{bmatrix}$$

$$\begin{bmatrix} u \\ v \end{bmatrix} = \begin{bmatrix} f_x x_d + c_x \\ f_y y_d + c_y \end{bmatrix}$$

The distortion parameters $[k_1, k_2, p_1, p_2, k_3]$ are estimated using the calibration method of [Zhang (2000)](https://doi.org/10.1109/34.888718); we cover this in detail in Ch.3.

#### Fisheye (Equidistant) Model

For fisheye lenses with a wide field of view (FoV of 180° or more), the radial-tangential model is inadequate: $r$ becomes very large near the image edges and the polynomial approximation diverges.

The generic camera model of [Kannala & Brandt (2006)](https://doi.org/10.1109/TPAMI.2006.153) is defined as follows.

Let the incidence angle $\theta$ denote the angle of the 3D point from the optical axis:

$$\theta = \arctan\left(\frac{\sqrt{X_c^2 + Y_c^2}}{Z_c}\right)$$

The distorted radius $r_d$ is modeled as an odd polynomial in $\theta$:

$$r_d = k_1 \theta + k_2 \theta^3 + k_3 \theta^5 + k_4 \theta^7 + k_5 \theta^9$$

In a pure equidistant projection, $r_d = f \cdot \theta$, corresponding to $k_1 = f$, $k_2 = k_3 = \cdots = 0$.

Projected coordinates:

$$\begin{bmatrix} u \\ v \end{bmatrix} = \begin{bmatrix} f_x \cdot r_d \cdot \frac{x_n}{\sqrt{x_n^2 + y_n^2}} + c_x \\ f_y \cdot r_d \cdot \frac{y_n}{\sqrt{x_n^2 + y_n^2}} + c_y \end{bmatrix}$$

Fisheye lenses offer a wide FoV that benefits perception of the surrounding environment, and they are supported by VIO systems such as [VINS-Mono](https://arxiv.org/abs/1708.03852) and Basalt. For calibration we use OCamCalib of [Scaramuzza et al. (2006)](https://rpg.ifi.uzh.ch/docs/IROS06_scaramuzza.pdf) or Kalibr.

```python
import numpy as np

def project_pinhole(P_c, K, dist_coeffs=None):
    """3D to 2D projection with the pinhole camera model.
    
    Args:
        P_c: (3,) 3D point in the camera frame [X, Y, Z]
        K: (3,3) intrinsic matrix
        dist_coeffs: (5,) distortion coefficients [k1, k2, p1, p2, k3] or None
    
    Returns:
        (2,) image coordinates [u, v]
    """
    X, Y, Z = P_c
    # Normalized coordinates
    x_n = X / Z
    y_n = Y / Z
    
    if dist_coeffs is not None:
        k1, k2, p1, p2, k3 = dist_coeffs
        r2 = x_n**2 + y_n**2
        r4 = r2**2
        r6 = r2 * r4
        
        # Radial distortion
        radial = 1 + k1 * r2 + k2 * r4 + k3 * r6
        x_d = x_n * radial + 2 * p1 * x_n * y_n + p2 * (r2 + 2 * x_n**2)
        y_d = y_n * radial + p1 * (r2 + 2 * y_n**2) + 2 * p2 * x_n * y_n
    else:
        x_d = x_n
        y_d = y_n
    
    fx, fy = K[0, 0], K[1, 1]
    cx, cy = K[0, 2], K[1, 2]
    
    u = fx * x_d + cx
    v = fy * y_d + cy
    return np.array([u, v])

def project_fisheye(P_c, K, fisheye_coeffs):
    """3D to 2D projection with the fisheye (equidistant) camera model.
    
    Uses the model of the OpenCV fisheye module:
      theta_d = theta * (1 + k1*theta^2 + k2*theta^4 + k3*theta^6 + k4*theta^8)
    This has a different parameterization from the 5-parameter Kannala-Brandt
    model above.
    
    Args:
        P_c: (3,) 3D point in the camera frame [X, Y, Z]
        K: (3,3) intrinsic matrix
        fisheye_coeffs: (4,) distortion coefficients [k1, k2, k3, k4] (OpenCV convention)
    
    Returns:
        (2,) image coordinates [u, v]
    """
    X, Y, Z = P_c
    r_xyz = np.sqrt(X**2 + Y**2)
    theta = np.arctan2(r_xyz, Z)
    
    k1, k2, k3, k4 = fisheye_coeffs
    theta2 = theta**2
    r_d = theta * (1 + k1 * theta2 + k2 * theta2**2 + k3 * theta2**3 + k4 * theta2**4)
    
    if r_xyz < 1e-10:
        return np.array([K[0, 2], K[1, 2]])
    
    u = K[0, 0] * r_d * (X / r_xyz) + K[0, 2]
    v = K[1, 1] * r_d * (Y / r_xyz) + K[1, 2]
    return np.array([u, v])
```

### 2.1.3 Reprojection Error

The reprojection error is the core cost function of nearly every algorithm that uses camera observations in sensor fusion.

For a 3D landmark $\mathbf{P}_w$, the reprojection error is the difference between the predicted image coordinate $\hat{\mathbf{p}}$ obtained by projecting $\mathbf{P}_w$ under the camera pose $\mathbf{T}_{cw} = [\mathbf{R}|\mathbf{t}]$, and the actually observed feature point $\mathbf{p}_{\text{obs}}$:

$$\mathbf{e}_{\text{reproj}} = \mathbf{p}_{\text{obs}} - \pi(\mathbf{T}_{cw} \cdot \mathbf{P}_w)$$

Here $\pi(\cdot)$ is the projection function defined above (including distortion).

In **bundle adjustment**, we minimize the sum of reprojection errors over all camera poses and landmarks:

$$\min_{\{\mathbf{T}_i\}, \{\mathbf{P}_j\}} \sum_{i,j} \rho\left(\| \mathbf{p}_{ij} - \pi(\mathbf{T}_i \cdot \mathbf{P}_j) \|^2_{\mathbf{\Sigma}_{ij}}\right)$$

where:
- $\mathbf{p}_{ij}$: image coordinates of landmark $j$ observed by camera $i$
- $\mathbf{\Sigma}_{ij}$: observation noise covariance (typically $\sigma^2 \mathbf{I}_2$, $\sigma \approx 1$ pixel)
- $\rho(\cdot)$: robust kernel (Huber, Cauchy, etc.) — suppresses the influence of outliers

The distribution of the reprojection error is typically modeled as Gaussian with $\sigma = 0.5$ to 2 pixels. This value depends on the precision of the feature detector; with sub-pixel corner detection it can drop to $\sigma \approx 0.5$ pixel. More recently, foundation models such as [Depth Anything V2 (Yang et al., 2024)](https://arxiv.org/abs/2406.09414) and [Metric3D v2 (Hu et al., 2024)](https://arxiv.org/abs/2404.15506) estimate dense depth from a single image, and are being leveraged to extend the camera observation model from 2D reprojection errors to 3D depth observations.

### 2.1.4 Rolling Shutter Model

Most low-cost cameras (smartphones, webcams) use CMOS image sensors and acquire images with a **rolling shutter**. Unlike a global shutter, which exposes all pixels simultaneously, a rolling shutter exposes rows sequentially. As a result, the top and bottom of the image are captured at different times.

The exposure time of row $k$ is:

$$t_k = t_0 + k \cdot t_r$$

where $t_0$ is the exposure time of the first row and $t_r$ is the row readout time. The total readout time across the whole image is $H \cdot t_r$ ($H$: image height), which ranges from a few to tens of milliseconds.

When a rolling-shutter image is captured while the camera moves, the following artifacts arise:
- **Geometric distortion**: vertical lines tilt, and moving objects deform as if made of jelly.
- **Feature position error**: each feature point is captured under a different camera pose, so a projection model that assumes a global shutter becomes inaccurate.

In a rolling-shutter-aware projection model, the camera pose at the time corresponding to each feature's row coordinate $v$ must be used:

$$\mathbf{p}_i = \pi\left(\mathbf{T}(t_{v_i}) \cdot \mathbf{P}_i\right)$$

where $\mathbf{T}(t_{v_i})$ is the camera pose at the time corresponding to row $v_i$ of the $i$-th feature. This pose is typically obtained by interpolation using IMU measurements:

$$\mathbf{T}(t_{v_i}) = \mathbf{T}(t_0) \cdot \text{Exp}\left(\frac{v_i}{H} \cdot \text{Log}(\mathbf{T}(t_0)^{-1} \mathbf{T}(t_0 + H \cdot t_r))\right)$$

Here $\text{Exp}$ and $\text{Log}$ are the exponential and logarithmic maps on the $SE(3)$ Lie group.

Rolling-shutter correction is optionally supported in VIO systems such as [VINS-Mono](https://arxiv.org/abs/1708.03852) and [ORB-SLAM3](https://arxiv.org/abs/2007.11898), and it is especially important for combinations of high-speed motion and low-cost sensors, such as smartphone or drone-mounted cameras.

```python
import numpy as np
from scipy.spatial.transform import Rotation, Slerp

def rolling_shutter_project(P_w, T_start, T_end, K, H, v_row):
    """Projection under a rolling-shutter camera model.
    
    Interpolates between the poses at the first row (T_start) and last row
    (T_end) of the image, and projects the 3D point using the pose of the
    corresponding row.
    
    Args:
        P_w: (3,) 3D point in world coordinates
        T_start: (4,4) camera-to-world transform at the exposure of the first row
        T_end: (4,4) camera-to-world transform at the exposure of the last row
        K: (3,3) intrinsic matrix
        H: image height (number of rows)
        v_row: row coordinate of the feature point to project
    
    Returns:
        (2,) image coordinates [u, v]
    """
    alpha = v_row / H  # interpolation ratio in [0, 1]
    
    # Rotation interpolation (SLERP)
    R_start = Rotation.from_matrix(T_start[:3, :3])
    R_end = Rotation.from_matrix(T_end[:3, :3])
    slerp = Slerp([0, 1], Rotation.concatenate([R_start, R_end]))
    R_interp = slerp(alpha).as_matrix()
    
    # Translation interpolation (linear)
    t_interp = (1 - alpha) * T_start[:3, 3] + alpha * T_end[:3, 3]
    
    # World-to-camera transform, then project
    P_c = R_interp.T @ (P_w - t_interp)
    
    u = K[0, 0] * P_c[0] / P_c[2] + K[0, 2]
    v = K[1, 1] * P_c[1] / P_c[2] + K[1, 2]
    return np.array([u, v])
```

---

## 2.2 LiDAR Observation Model

LiDAR is an active sensor that computes range by emitting laser pulses and measuring the time-of-flight or phase shift of the light reflected from objects. This section treats LiDAR's mathematical observation model and its error characteristics.

### 2.2.1 Range-Bearing Model

The basic LiDAR observation is a pair of **range** and **bearing** for each laser beam. In 3D LiDAR, each point observation is expressed in spherical coordinates $(r, \alpha, \omega)$:

- $r$: range
- $\alpha$: azimuth
- $\omega$: elevation

The observation model for a 3D point $\mathbf{P}_L = [x, y, z]^\top$ in the LiDAR frame is:

$$\begin{aligned}
r &= \sqrt{x^2 + y^2 + z^2} + n_r \\
\alpha &= \arctan2(y, x) + n_\alpha \\
\omega &= \arcsin\left(\frac{z}{\sqrt{x^2 + y^2 + z^2}}\right) + n_\omega
\end{aligned}$$

where $n_r, n_\alpha, n_\omega$ are the observation noise on range, azimuth, and elevation, respectively.

**Noise characteristics:**
- **Range noise** $n_r$: typically $\sigma_r \approx 1\text{–}3\,\text{cm}$ for mechanical LiDAR and $\sigma_r \approx 2\text{–}5\,\text{cm}$ for solid-state LiDAR. Noise grows with range because of beam divergence and decreasing received energy.
- **Angular noise** $n_\alpha, n_\omega$: determined by encoder precision. Typically $\sigma_\alpha, \sigma_\omega \approx 0.01°\text{–}0.1°$. These small values are amplified into position errors at long range — at 50 m, an angular error of $0.1°$ corresponds to roughly $8.7\,\text{cm}$ of lateral error.

Conversion from spherical to Cartesian coordinates:

$$\mathbf{P}_L = \begin{bmatrix} r \cos\omega \cos\alpha \\ r \cos\omega \sin\alpha \\ r \sin\omega \end{bmatrix}$$

**Beam divergence.** A LiDAR laser beam is not a perfect line but spreads as a narrow cone. The divergence angle is typically $0.1°\text{–}0.5°$; at long range this enlarges the beam footprint so that the measurement reports the average range over a region rather than a single reflection. When a beam partially straddles an object boundary and both the object and background contribute, the measured range falls between the two and a spurious point is produced — this is known as the **mixed pixel** effect.

### 2.2.2 Motion Distortion

A mechanical spinning LiDAR fires lasers while the sensor rotates through 360°. The Velodyne VLP-16, for example, takes roughly 100 ms per revolution. If the platform (vehicle, drone) moves during these 100 ms, the points within a single scan are measured in different coordinate frames. This is the **motion distortion** or **ego-motion compensation** problem.

This problem is essentially the same as rolling shutter for cameras. If the $i$-th point of a scan is measured at time $t_i$, we must transform it into the coordinate frame of the reference time $t_0$:

$$\mathbf{P}_L^{(t_0)} = \mathbf{T}(t_0)^{-1} \cdot \mathbf{T}(t_i) \cdot \mathbf{P}_L^{(t_i)}$$

where $\mathbf{T}(t_i)$ is the LiDAR pose at time $t_i$.

**Correction methods:**

1. **IMU-based interpolation**: interpolate the pose change over the scan duration using high-frequency IMU measurements. This is the most common approach and is used in LIO-SAM, FAST-LIO2, and others.
2. **Previous-frame odometry based**: apply a constant-velocity model with the velocity estimated in the immediately preceding frame.
3. **Continuous-time methods**: model the trajectory as a continuous function with, e.g., B-splines, and evaluate the pose at each point's time. [CT-ICP (Dellenbach et al., 2022)](https://arxiv.org/abs/2109.12979) is a representative example.

```python
import numpy as np
from scipy.spatial.transform import Rotation, Slerp

def undistort_scan(points, timestamps, T_start, T_end, t_start, t_end):
    """IMU-based motion distortion correction.
    
    Interpolates between the scan-start and scan-end poses and transforms each
    point into the coordinate frame of the start time.
    
    Args:
        points: (N, 3) LiDAR point cloud
        timestamps: (N,) timestamp of each point
        T_start: (4,4) LiDAR pose at the start of the scan (lidar-to-world)
        T_end: (4,4) LiDAR pose at the end of the scan (lidar-to-world)
        t_start, t_end: scan start/end times
    
    Returns:
        (N, 3) corrected point cloud
    """
    R_start = Rotation.from_matrix(T_start[:3, :3])
    R_end = Rotation.from_matrix(T_end[:3, :3])
    slerp = Slerp([t_start, t_end], Rotation.concatenate([R_start, R_end]))
    
    corrected = np.zeros_like(points)
    for i in range(len(points)):
        alpha = (timestamps[i] - t_start) / (t_end - t_start)
        alpha = np.clip(alpha, 0, 1)
        
        # Pose interpolation at that time
        R_i = slerp(timestamps[i]).as_matrix()
        t_i = (1 - alpha) * T_start[:3, 3] + alpha * T_end[:3, 3]
        
        # Frame at that time -> frame at start time
        p_world = R_i @ points[i] + t_i
        corrected[i] = T_start[:3, :3].T @ (p_world - T_start[:3, 3])
    
    return corrected
```

### 2.2.3 Impact of Spinning vs Solid-State LiDAR on Fusion

**Mechanical spinning LiDAR** (Velodyne, Ouster, Hesai) provides a 360° horizontal FoV, with each scan forming a complete annular point cloud. Algorithms in the LOAM family are designed around this property — extracting edge and planar features along horizontal scan lines and estimating 6-DoF pose from omnidirectional observations.

**Solid-state LiDAR** (Livox Mid-40/70, Avia, HAP, etc.) has no mechanical rotating part and uses a non-repetitive scan pattern within a restricted FoV (for example, roughly 70.4° circular for the Livox Mid-70). A characteristic feature is that coverage of the FoV grows gradually over time.

The impact of this difference on fusion algorithms:

| Property | Spinning LiDAR | Solid-State LiDAR |
|------|-------------|-------------------|
| FoV | 360° horizontal | Limited (40° to 120°) |
| Scan pattern | Repetitive (horizontal lines) | Non-repetitive (rosette, Lissajous, etc.) |
| Single-frame density | Uniform | Non-uniform (grows over time) |
| Feature extraction | Scan-line-based feasible | No scan-line structure |
| Suitable algorithms | LOAM, LeGO-LOAM | FAST-LIO/LIO2 (point-wise processing) |

FAST-LIO / [FAST-LIO2](https://arxiv.org/abs/2107.06829) are particularly strong on solid-state LiDAR because their iterated EKF structure does not depend on scan-line structure and instead **processes individual points sequentially**. In contrast, LOAM's edge/planar feature extraction presupposes scan-line structure and is hard to apply directly to solid-state sensors. More recently, [FAST-LIVO2 (Zheng et al., 2024)](https://arxiv.org/abs/2408.14035) extends this structure to sequentially fuse three sensors — LiDAR, inertial, and visual — within the same iterated EKF, using a direct method to process both LiDAR points and images without separate feature extraction.

---

## 2.3 IMU Model

An IMU (Inertial Measurement Unit) consists of a 3-axis accelerometer and a 3-axis gyroscope. In sensor fusion the IMU is the core sensor of virtually every system: it provides high-frequency observations (100 Hz to 1 kHz) that interpolate between camera or LiDAR frames, and it contributes to initialization and scale observability. This section treats IMU error models in detail.

### 2.3.1 Gyroscope Error Model

A gyroscope measures the 3-axis angular velocity $\boldsymbol{\omega}$. The actual measurement $\tilde{\boldsymbol{\omega}}$ is modeled as:

$$\tilde{\boldsymbol{\omega}} = \boldsymbol{\omega} + \mathbf{b}_g + \mathbf{n}_g$$

Meaning of each term:
- $\boldsymbol{\omega}$: true angular velocity (in the IMU body frame)
- $\mathbf{b}_g$: **bias** — a nearly constant offset that varies slowly in time
- $\mathbf{n}_g$: **measurement noise** — additive white Gaussian noise (AWGN)

**Bias dynamics.** The bias is not a constant but drifts slowly over time. We model it as a **random walk**:

$$\dot{\mathbf{b}}_g = \mathbf{n}_{bg}$$

where $\mathbf{n}_{bg} \sim \mathcal{N}(\mathbf{0}, \sigma_{bg}^2 \mathbf{I})$ is the driving noise for the bias. In discrete time:

$$\mathbf{b}_{g,k+1} = \mathbf{b}_{g,k} + \sigma_{bg} \sqrt{\Delta t} \cdot \mathbf{w}_k, \quad \mathbf{w}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{I})$$

Typical parameters for a MEMS-grade gyroscope:
- Measurement noise density: $\sigma_g \approx 0.004\,\text{rad/s}/\sqrt{\text{Hz}}$ (about $0.2\,°/\text{s}/\sqrt{\text{Hz}}$)
- In-run bias stability: $\sigma_{bg} \approx 10\text{–}100\,°/\text{hr}$
- Bias random walk: $\sigma_{bg} \approx 0.0002\,\text{rad/s}^2/\sqrt{\text{Hz}}$

### 2.3.2 Accelerometer Error Model

An accelerometer measures the 3-axis specific force $\mathbf{a}$. Specific force is the true acceleration minus gravity. The actual measurement $\tilde{\mathbf{a}}$ is:

$$\tilde{\mathbf{a}} = \mathbf{R}_{bw}(\mathbf{a}_w - \mathbf{g}_w) + \mathbf{b}_a + \mathbf{n}_a$$

Meaning of each term:
- $\mathbf{a}_w$: true acceleration in the world frame
- $\mathbf{g}_w = [0, 0, -g]^\top$: gravity vector ($g \approx 9.81\,\text{m/s}^2$)
- $\mathbf{R}_{bw}$: world-to-body rotation matrix
- $\mathbf{b}_a$: accelerometer bias
- $\mathbf{n}_a \sim \mathcal{N}(\mathbf{0}, \sigma_a^2 \mathbf{I})$: measurement noise

**Role of gravity.** The fact that the accelerometer "feels" gravity is of great importance in IMU-based fusion. Even at rest, the accelerometer measures $[0, 0, g]^\top$ (when z is up). From this gravity observation we can estimate roll and pitch. Yaw, however, is a rotation about the gravity vector and is therefore unobservable — this is why VIO/LIO systems require additional observations (e.g., motion of visual features) to estimate yaw during initialization.

**Bias dynamics.** As with the gyroscope, we model the bias as a random walk:

$$\dot{\mathbf{b}}_a = \mathbf{n}_{ba}, \quad \mathbf{n}_{ba} \sim \mathcal{N}(\mathbf{0}, \sigma_{ba}^2 \mathbf{I})$$

Typical parameters for a MEMS-grade accelerometer:
- Measurement noise density: $\sigma_a \approx 0.04\,\text{m/s}^2/\sqrt{\text{Hz}}$ (about $4\,\text{mg}/\sqrt{\text{Hz}}$)
- Bias stability: $\sigma_{ba} \approx 0.01\text{–}0.1\,\text{mg}$
- Bias random walk: $\sigma_{ba} \approx 0.001\,\text{m/s}^3/\sqrt{\text{Hz}}$

### 2.3.3 Allan Variance

Allan variance is the standard method for analyzing IMU noise characteristics. Data are collected at rest over a long period (several hours), and the variance is computed at various cluster times $\tau$.

Definition of the Allan variance $\sigma^2(\tau)$:

$$\sigma^2(\tau) = \frac{1}{2} \langle (\bar{y}_{k+1} - \bar{y}_k)^2 \rangle$$

where $\bar{y}_k$ is the mean output of the $k$-th interval of length $\tau$.

On a log-log plot, the slope of the Allan deviation $\sigma(\tau)$ identifies the noise type:

| Slope | Noise type | Physical meaning |
|--------|-----------|-----------|
| $-1/2$ | Angle/velocity random walk (ARW/VRW) | White noise $\mathbf{n}_g, \mathbf{n}_a$ |
| $0$ | Bias instability | Flicker noise; the minimum is the bias stability |
| $+1/2$ | Rate random walk (RRW) | Bias random walk $\mathbf{n}_{bg}, \mathbf{n}_{ba}$ |

**How to read a datasheet.** Extracting the key parameters needed for sensor fusion from an IMU datasheet:

1. **Angular random walk (ARW)**: units $°/\sqrt{\text{hr}}$ or $\text{rad/s}/\sqrt{\text{Hz}}$. Read the value at $\tau = 1\,\text{s}$ on the Allan deviation plot, or read from the slope-$-1/2$ region. This corresponds to $\sigma_g$.
2. **Velocity random walk (VRW)**: units $\text{m/s}/\sqrt{\text{hr}}$ or $\text{m/s}^2/\sqrt{\text{Hz}}$. The white-noise density of the accelerometer. This corresponds to $\sigma_a$.
3. **In-run bias stability**: the minimum of the Allan deviation plot. The theoretical lower bound on the bias estimate that the system can reach.
4. **Rate random walk**: the rate at which the bias changes over time. This corresponds to $\sigma_{bg}, \sigma_{ba}$.

```python
import numpy as np

def compute_allan_variance(data, dt, max_cluster_size=None):
    """Compute the Allan variance.
    
    Args:
        data: (N,) data for one IMU axis collected at rest
        dt: sampling period (seconds)
        max_cluster_size: maximum cluster size (defaults to N//2 when None)
    
    Returns:
        taus: array of cluster times
        adevs: array of Allan deviations
    """
    N = len(data)
    if max_cluster_size is None:
        max_cluster_size = N // 2
    
    # Cluster sizes generated on a log scale
    cluster_sizes = np.unique(np.logspace(
        0, np.log10(max_cluster_size), num=100
    ).astype(int))
    cluster_sizes = cluster_sizes[cluster_sizes > 0]
    
    taus = []
    adevs = []
    
    for m in cluster_sizes:
        tau = m * dt
        # Non-overlapping mean
        n_clusters = N // m
        if n_clusters < 2:
            break
        
        # Mean of each cluster
        truncated = data[:n_clusters * m].reshape(n_clusters, m)
        cluster_means = truncated.mean(axis=1)
        
        # Allan variance
        diff = np.diff(cluster_means)
        avar = 0.5 * np.mean(diff**2)
        
        taus.append(tau)
        adevs.append(np.sqrt(avar))
    
    return np.array(taus), np.array(adevs)

def extract_imu_params(taus, adevs):
    """Extract IMU noise parameters from an Allan deviation plot.
    
    Args:
        taus: array of cluster times
        adevs: array of Allan deviations
    
    Returns:
        dict: {
            'white_noise': value at tau=1 (ARW or VRW),
            'bias_instability': minimum value,
            'bias_instability_tau': tau at the minimum
        }
    """
    # White noise: Allan deviation at tau=1 (slope -1/2 region)
    idx_1s = np.argmin(np.abs(taus - 1.0))
    white_noise = adevs[idx_1s]
    
    # Bias instability: minimum of the Allan deviation
    idx_min = np.argmin(adevs)
    bias_instability = adevs[idx_min]
    bias_instability_tau = taus[idx_min]
    
    return {
        'white_noise': white_noise,
        'bias_instability': bias_instability,
        'bias_instability_tau': bias_instability_tau
    }
```

### 2.3.4 Strapdown Navigation Equation

The equations that integrate IMU measurements into pose (position, velocity, attitude) are called the strapdown navigation equations. "Strapdown" refers to the sensor being rigidly fixed (strapped down) to the platform, so that coordinate transformations are carried out in software rather than by a mechanical gimbal.

Continuous-time dynamics of the state $[\mathbf{R}, \mathbf{v}, \mathbf{p}]$ in the world (or navigation) frame:

$$\dot{\mathbf{R}} = \mathbf{R} \cdot [\tilde{\boldsymbol{\omega}} - \mathbf{b}_g - \mathbf{n}_g]_\times$$

$$\dot{\mathbf{v}} = \mathbf{R} (\tilde{\mathbf{a}} - \mathbf{b}_a - \mathbf{n}_a) + \mathbf{g}$$

$$\dot{\mathbf{p}} = \mathbf{v}$$

where:
- $\mathbf{R} \in SO(3)$: body-to-world rotation matrix
- $\mathbf{v} \in \mathbb{R}^3$: velocity in the world frame
- $\mathbf{p} \in \mathbb{R}^3$: position in the world frame
- $[\cdot]_\times$: skew-symmetric matrix (expressing the vector cross product as a matrix multiplication)

$$[\boldsymbol{\omega}]_\times = \begin{bmatrix} 0 & -\omega_z & \omega_y \\ \omega_z & 0 & -\omega_x \\ -\omega_y & \omega_x & 0 \end{bmatrix}$$

**Discrete-time integration.** State propagation from time $t_k$ to $t_{k+1} = t_k + \Delta t$:

$$\mathbf{R}_{k+1} = \mathbf{R}_k \cdot \text{Exp}\left((\tilde{\boldsymbol{\omega}}_k - \mathbf{b}_{g,k}) \Delta t\right)$$

$$\mathbf{v}_{k+1} = \mathbf{v}_k + \mathbf{g} \Delta t + \mathbf{R}_k (\tilde{\mathbf{a}}_k - \mathbf{b}_{a,k}) \Delta t$$

$$\mathbf{p}_{k+1} = \mathbf{p}_k + \mathbf{v}_k \Delta t + \frac{1}{2} \mathbf{g} \Delta t^2 + \frac{1}{2} \mathbf{R}_k (\tilde{\mathbf{a}}_k - \mathbf{b}_{a,k}) \Delta t^2$$

Here $\text{Exp}(\boldsymbol{\phi})$ is the exponential map on $SO(3)$ that converts a rotation vector $\boldsymbol{\phi}$ into a rotation matrix. It can be computed via Rodrigues' formula:

**Midpoint integration.** The Euler integration above is first-order accurate. For second-order accuracy one can average two consecutive IMU measurements:

$$\bar{\boldsymbol{\omega}} = \frac{1}{2}(\tilde{\boldsymbol{\omega}}_k + \tilde{\boldsymbol{\omega}}_{k+1}) - \mathbf{b}_{g,k}$$

$$\mathbf{R}_{k+1} = \mathbf{R}_k \cdot \text{Exp}(\bar{\boldsymbol{\omega}} \Delta t)$$

$$\bar{\mathbf{a}} = \frac{1}{2}\left(\mathbf{R}_k(\tilde{\mathbf{a}}_k - \mathbf{b}_{a,k}) + \mathbf{R}_{k+1}(\tilde{\mathbf{a}}_{k+1} - \mathbf{b}_{a,k})\right)$$

$$\mathbf{v}_{k+1} = \mathbf{v}_k + (\bar{\mathbf{a}} + \mathbf{g}) \Delta t$$

$$\mathbf{p}_{k+1} = \mathbf{p}_k + \mathbf{v}_k \Delta t + \frac{1}{2}(\bar{\mathbf{a}} + \mathbf{g}) \Delta t^2$$

This midpoint integration is the default scheme used in VINS-Mono, FAST-LIO2, and similar systems. Higher-order fourth-order Runge-Kutta (RK4) integration is also possible, but midpoint integration is sufficient at typical IMU rates (200 to 400 Hz).

```python
import numpy as np
from scipy.spatial.transform import Rotation

def skew(v):
    """Skew-symmetric matrix of a 3D vector."""
    return np.array([
        [0, -v[2], v[1]],
        [v[2], 0, -v[0]],
        [-v[1], v[0], 0]
    ])

def exp_so3(phi):
    """SO(3) exponential map: rotation vector -> rotation matrix (Rodrigues' formula)."""
    angle = np.linalg.norm(phi)
    if angle < 1e-10:
        return np.eye(3) + skew(phi)
    
    axis = phi / angle
    K = skew(axis)
    return np.eye(3) + np.sin(angle) * K + (1 - np.cos(angle)) * K @ K

def imu_strapdown(gyro_data, accel_data, dt, R0, v0, p0, bg, ba, gravity):
    """Strapdown inertial navigation (midpoint integration).
    
    Args:
        gyro_data: (N, 3) gyroscope measurements [rad/s]
        accel_data: (N, 3) accelerometer measurements [m/s^2]
        dt: sampling period [s]
        R0: (3,3) initial rotation matrix (body-to-world)
        v0: (3,) initial velocity [m/s] (world frame)
        p0: (3,) initial position [m] (world frame)
        bg: (3,) gyroscope bias
        ba: (3,) accelerometer bias
        gravity: (3,) gravity vector (e.g., [0, 0, -9.81])
    
    Returns:
        Rs: (N+1, 3, 3) rotation history
        vs: (N+1, 3) velocity history
        ps: (N+1, 3) position history
    """
    N = len(gyro_data)
    Rs = np.zeros((N + 1, 3, 3))
    vs = np.zeros((N + 1, 3))
    ps = np.zeros((N + 1, 3))
    
    Rs[0] = R0
    vs[0] = v0
    ps[0] = p0
    
    for k in range(N - 1):
        # Midpoint gyroscope
        omega_k = gyro_data[k] - bg
        omega_k1 = gyro_data[k + 1] - bg
        omega_mid = 0.5 * (omega_k + omega_k1)
        
        # Rotation update
        Rs[k + 1] = Rs[k] @ exp_so3(omega_mid * dt)
        
        # Midpoint acceleration (world frame)
        a_k = Rs[k] @ (accel_data[k] - ba)
        a_k1 = Rs[k + 1] @ (accel_data[k + 1] - ba)
        a_mid = 0.5 * (a_k + a_k1)
        
        # Velocity/position update
        vs[k + 1] = vs[k] + (a_mid + gravity) * dt
        ps[k + 1] = ps[k] + vs[k] * dt + 0.5 * (a_mid + gravity) * dt**2
    
    # Last step (simple Euler)
    k = N - 1
    omega_k = gyro_data[k] - bg
    Rs[k + 1] = Rs[k] @ exp_so3(omega_k * dt)
    a_k = Rs[k] @ (accel_data[k] - ba)
    vs[k + 1] = vs[k] + (a_k + gravity) * dt
    ps[k + 1] = ps[k] + vs[k] * dt + 0.5 * (a_k + gravity) * dt**2
    
    return Rs, vs, ps
```

**Numerical meaning of drift.** Consider how quickly the strapdown integration above diverges when the bias is not corrected. For an accelerometer bias of $b_a = 0.01\,\text{m/s}^2$ (about $1\,\text{mg}$, typical for MEMS):

- Position error after 1 s: $\frac{1}{2} \times 0.01 \times 1^2 = 0.005\,\text{m}$ (5 mm)
- After 10 s: $\frac{1}{2} \times 0.01 \times 100 = 0.5\,\text{m}$
- After 60 s: $\frac{1}{2} \times 0.01 \times 3600 = 18\,\text{m}$

This is why standalone inertial navigation is infeasible, and why we must continually estimate and correct the bias through sensor fusion. In VIO/LIO systems the biases $\mathbf{b}_g, \mathbf{b}_a$ are **included as part of the state vector** and are updated continuously from the observations of other sensors. Research on deep-learning-based inertial odometry has also been active. [AirIO (Chen et al., 2025)](https://arxiv.org/abs/2501.15659) strengthens the observability of IMU features and reports an accuracy improvement of more than 50 % over prior learning-based inertial odometry in drone settings.

### 2.3.5 IMU Grade Classification

IMUs are broadly divided into three grades by performance. The grade chosen for a sensor fusion system determines the frequency and type of external-sensor aiding required.

| Grade | ARW | Bias stability (gyro) | Price | Standalone navigation time | Examples |
|------|-----|----------------------|-------|-------------|------|
| Navigation grade | $< 0.002°/\sqrt{\text{hr}}$ | $< 0.01°/\text{hr}$ | $>$ \$10k | hours | HG1700, LN-200 |
| Tactical grade | $0.05\text{–}0.5°/\sqrt{\text{hr}}$ | $0.1\text{–}10°/\text{hr}$ | \$1k–10k | minutes | STIM300, ADIS16490 |
| MEMS | $0.1\text{–}1°/\sqrt{\text{hr}}$ | $1\text{–}100°/\text{hr}$ | $<$ \$100 | seconds | BMI088, ICM-42688 |

Robotics and autonomous driving use MEMS-grade IMUs almost exclusively, which makes tight fusion with cameras, LiDAR, and other sensors essential.

---

## 2.4 GNSS Model

GNSS (Global Navigation Satellite System) is a general term for satellite navigation systems such as GPS, GLONASS, Galileo, and BeiDou. This section treats the mathematical models needed to use GNSS observations in sensor fusion.

### 2.4.1 Pseudorange Observation Model

A GNSS receiver measures a **pseudorange** $\rho$ from each satellite. The pseudorange is the actual geometric range to the satellite plus error terms such as the receiver clock offset:

$$\rho^s = r^s + c \cdot \delta t_r - c \cdot \delta t^s + I^s + T^s + \epsilon_\rho$$

Meaning of each term:
- $r^s = \|\mathbf{p}_r - \mathbf{p}^s\|$: geometric distance between the receiver position $\mathbf{p}_r$ and the satellite $s$ position $\mathbf{p}^s$
- $c \cdot \delta t_r$: receiver clock offset (unknown, estimated together with position)
- $c \cdot \delta t^s$: satellite clock offset (can be corrected from the navigation message)
- $I^s$: ionospheric delay — signal delay caused by free electrons in the ionosphere. Can be removed with a dual-frequency L1/L2 receiver.
- $T^s$: tropospheric delay — delay caused by water vapor and air in the troposphere. Corrected with the Saastamoinen model or similar.
- $\epsilon_\rho$: residual noise (thermal noise, multipath, etc.). Typically $\sigma_\rho \approx 1\text{–}5\,\text{m}$ for single-point positioning and $\sigma_\rho \approx 0.1\text{–}0.3\,\text{m}$ with dual frequency and corrections.

**Positioning principle.** With pseudoranges from four or more satellites we can solve for the four unknowns: the receiver position $(x, y, z)$ and the clock offset $\delta t_r$. The nonlinear equations are solved by least squares or a Kalman filter — this is the basis of standard Single Point Positioning (SPP).

**Multipath.** In urban environments, satellite signals reflected off buildings can arrive along paths other than the direct line of sight, adding several to tens of meters of error to the pseudorange. Multipath is hard to model and varies with the environment, so outlier handling is especially important when using GNSS in sensor fusion.

### 2.4.2 Carrier Phase Observation Model

Carrier phase observations are far more precise than pseudoranges (millimeter level). The L1 carrier (about 1575.42 MHz) has a wavelength of roughly 19 cm, and resolving the phase to only 1 % yields a range precision of roughly 2 mm.

$$\Phi^s = r^s + c \cdot \delta t_r - c \cdot \delta t^s + \lambda N^s - I^s + T^s + \epsilon_\Phi$$

where:
- $\lambda$: carrier wavelength
- $N^s$: **integer ambiguity** — the integer number of full wavelengths between receiver and satellite. An unknown integer whose accurate resolution is the crux of RTK/PPP.
- $\epsilon_\Phi \approx 1\text{–}5\,\text{mm}$: carrier-phase noise (about 1/100 of the pseudorange noise)

A notable point is that the ionospheric delay enters with the opposite sign from the pseudorange (group velocity vs phase velocity). This allows the ionospheric delay to be canceled with dual-frequency observations.

### 2.4.3 RTK (Real-Time Kinematic)

RTK is a technique that uses **differencing** observations against a nearby (within a few km) base station to eliminate common errors (satellite clock, ionosphere, troposphere) and resolve the carrier-phase integer ambiguities in real time, achieving **centimeter-level** positioning.

**Double differencing.** The double difference between rover and base for satellite $s$ and reference satellite $r$:

$$\nabla\Delta\Phi_{br}^{sr} = \nabla\Delta r_{br}^{sr} + \lambda \nabla\Delta N_{br}^{sr} + \epsilon$$

Double differencing removes the receiver and satellite clock offsets and nearly cancels ionospheric and tropospheric errors when the base station is close. The remaining unknowns are the geometric range (which depends on position) and the integer ambiguities, which are resolved with algorithms such as LAMBDA.

### 2.4.4 PPP (Precise Point Positioning)

PPP is a technique that achieves centimeter-level positioning with a single receiver, without a base station. Precise orbits and precise clock corrections received from an external service remove satellite-related errors, while ionospheric and tropospheric errors are included in the state vector and estimated.

**RTK vs PPP:**

| Property | RTK | PPP |
|------|-----|-----|
| Base station required | Yes (within a few km) | No |
| Convergence time | seconds to tens of seconds | tens of minutes (traditional), minutes (PPP-AR) |
| Precision (after convergence) | $\sim 2\,\text{cm}$ | $\sim 5\,\text{cm}$ |
| Coverage | Near the base station | Global |

**Using GNSS in sensor fusion.** Because GNSS provides absolute position, combining it with VIO/LIO can completely eliminate long-term drift. [LIO-SAM (Shan et al., 2020)](https://arxiv.org/abs/2007.00258) is a representative example that adds a GNSS factor directly to the factor graph. Key considerations when incorporating GNSS observations into fusion:

1. **Coordinate frame transformation**: GNSS is output in WGS84 (latitude, longitude, ellipsoidal height), while robotics systems use a local frame such as ENU (East-North-Up) or NED (North-East-Down). A transformation is required.
2. **Use of covariance**: The DOP (Dilution of Precision) values or position covariances output by the GNSS receiver should be used as the observation covariance in the fusion system.
3. **Outlier handling**: In multipath environments, GNSS positioning can have errors of tens of meters, so robust kernels or $\chi^2$ tests must be used to detect and reject anomalous observations.

```python
import numpy as np

def pseudorange_model(p_receiver, p_satellites, clock_bias):
    """Pseudorange observation model (simplified).
    
    Args:
        p_receiver: (3,) receiver position in ECEF [m]
        p_satellites: (N, 3) ECEF positions of each satellite [m]
        clock_bias: receiver clock offset [m] (c * dt_r)
    
    Returns:
        pseudoranges: (N,) predicted pseudoranges [m]
        H: (N, 4) observation Jacobian (at the linearization point)
    """
    N = len(p_satellites)
    pseudoranges = np.zeros(N)
    H = np.zeros((N, 4))
    
    for i in range(N):
        diff = p_receiver - p_satellites[i]
        r = np.linalg.norm(diff)
        pseudoranges[i] = r + clock_bias
        
        # Jacobian: d(rho)/d(x,y,z,cb)
        e = diff / r  # unit vector (receiver-to-satellite direction)
        H[i, :3] = e
        H[i, 3] = 1.0  # partial derivative with respect to clock bias
    
    return pseudoranges, H

def geodetic_to_enu(lat, lon, alt, lat0, lon0, alt0):
    """Convert WGS84 coordinates to local ENU coordinates.
    
    Args:
        lat, lon: latitude and longitude of the point to convert [rad]
        alt: ellipsoidal altitude [m]
        lat0, lon0, alt0: latitude, longitude, altitude of the origin [rad, rad, m]
    
    Returns:
        (3,) ENU coordinates [m]
    """
    # Simplified conversion (WGS84 Earth radius)
    a = 6378137.0  # WGS84 semi-major axis
    e2 = 0.00669437999014  # squared eccentricity
    
    sin_lat0 = np.sin(lat0)
    N0 = a / np.sqrt(1 - e2 * sin_lat0**2)
    
    dlat = lat - lat0
    dlon = lon - lon0
    dalt = alt - alt0
    
    east = (N0 + alt0) * np.cos(lat0) * dlon
    north = (N0 * (1 - e2) + alt0) * dlat
    up = dalt
    
    return np.array([east, north, up])
```

---

## 2.5 Radar Model

Radar is an active sensor that uses radio waves. Its key advantage is reliable operation in **adverse weather (rain, fog, snow, dust)** where cameras and LiDAR degrade. It also has the unique property of measuring relative **velocity directly** via the Doppler effect. Radar is rapidly gaining importance in autonomous driving.

### 2.5.1 FMCW Radar Principles

Most radars used in automotive and robotic systems are **FMCW (Frequency Modulated Continuous Wave)**. They transmit a continuously frequency-modulated wave (chirp) and extract range and velocity from the frequency difference (beat frequency) between the transmitted and reflected waves.

**Chirp signal.** The instantaneous frequency of the transmitted signal increases linearly with time:

$$f_{\text{TX}}(t) = f_0 + \frac{B}{T_c} t$$

where $f_0$ is the starting frequency, $B$ is the frequency bandwidth, and $T_c$ is the chirp duration.

**Beat frequency.** A reflected wave from an object at range $R$ has a time delay $\tau = 2R/c$ ($c$: speed of light). Mixing the transmitted and received waves yields a beat frequency:

$$f_b = \frac{2BR}{cT_c}$$

So the range is:

$$R = \frac{f_b \cdot c \cdot T_c}{2B}$$

**Range resolution.** The minimum range difference at which two objects can be distinguished is determined by the bandwidth:

$$\Delta R = \frac{c}{2B}$$

For example, with a 77 GHz radar and $B = 4\,\text{GHz}$, $\Delta R \approx 3.75\,\text{cm}$.

**Doppler measurement.** By transmitting successive chirps and observing the phase change across reflections from the same object, the radial (line-of-sight) velocity can be measured:

$$v_r = \frac{\lambda \cdot f_d}{2}$$

where $f_d$ is the Doppler frequency and $\lambda$ is the carrier wavelength. For a 77 GHz radar, $\lambda \approx 3.9\,\text{mm}$, so even very small velocity changes can be detected.

**Range-Doppler map.** A 2D FFT over multiple chirps produces a 2D range-velocity map (Range-Doppler map). Each peak in this map corresponds to a reflector, and its position gives the range and radial velocity simultaneously.

**Azimuth measurement.** The angle of arrival is estimated from the phase differences across an antenna array. A horizontal array provides azimuth and a vertical array provides elevation.

### 2.5.2 Recent Trends in 4D Imaging Radar

Traditional automotive radar provides three-dimensional information — range, velocity, and azimuth — with very poor vertical resolution. **4D imaging radar** is a next-generation radar that provides four dimensions — range, Doppler, azimuth, and **elevation** — at high resolution.

The key to 4D imaging radar is implementing a large virtual antenna array with MIMO (Multiple Input Multiple Output) technology. For example, 12 transmit antennas × 16 receive antennas = 192 virtual antennas, which achieves sufficient angular resolution both horizontally and vertically.

**4D Radar vs LiDAR:**

| Property | 4D Imaging Radar | LiDAR |
|------|-----------------|-------|
| Adverse-weather operation | Excellent | Weak |
| Point density | Medium (thousands of points/frame) | High (hundreds of thousands of points/frame) |
| Velocity measurement | Direct (Doppler) | Not available (requires two-frame differencing) |
| Angular resolution | $\sim 1°$ | $\sim 0.1°$ |
| Cost | Low to medium | Medium to high |
| Static-object detection | Limited (Doppler = 0) | Excellent |

**Using radar in sensor fusion.** Radar's Doppler measurement provides unique value in sensor fusion:

1. **Ego-velocity estimation**: By fitting the radial velocities measured from static reflectors, we can estimate the sensor's own 3D velocity. This is more direct than integrating accelerometer readings and has no drift.

$$v_r^{(i)} = -\mathbf{e}^{(i)\top} \mathbf{v}_{\text{ego}}$$

where $\mathbf{e}^{(i)}$ is the unit vector toward the $i$-th reflector and $\mathbf{v}_{\text{ego}}$ is the ego-velocity. Observations from multiple reflectors allow $\mathbf{v}_{\text{ego}}$ to be estimated by least squares.

2. **Dynamic-object detection**: Moving objects are identified from the discrepancy between the predicted Doppler for the static environment and the actual observation.
3. **Adverse-weather backup**: When rain or fog causes cameras and LiDAR to fail, radar functions as the only exteroceptive sensor.

```python
import numpy as np

def fmcw_range_from_beat(f_beat, bandwidth, chirp_time, c=3e8):
    """Compute range from an FMCW radar beat frequency.
    
    Args:
        f_beat: beat frequency [Hz]
        bandwidth: frequency bandwidth [Hz]
        chirp_time: chirp duration [s]
        c: speed of light [m/s]
    
    Returns:
        range_m: range [m]
    """
    return f_beat * c * chirp_time / (2 * bandwidth)

def estimate_ego_velocity(bearings, doppler_velocities):
    """Estimate ego-velocity from Doppler observations of static reflectors.
    
    v_doppler_i = -e_i^T @ v_ego  (assumes a static environment)
    
    Args:
        bearings: (N, 3) unit vectors toward each reflector
        doppler_velocities: (N,) radial velocity of each reflector [m/s]
    
    Returns:
        v_ego: (3,) estimated ego-velocity [m/s]
    """
    # -E @ v_ego = v_doppler  ->  v_ego = -(E^T E)^{-1} E^T v_doppler
    E = bearings  # (N, 3)
    v_ego = -np.linalg.lstsq(E, doppler_velocities, rcond=None)[0]
    return v_ego
```

Radar's direct velocity measurement is information that other sensors cannot easily provide, and this is why radar is increasingly emerging as a core sensor in the multi-sensor fusion architectures covered in Ch.8.

---

## 2.6 Other Sensors

Beyond cameras, LiDAR, IMU, GNSS, and radar, sensor fusion systems make use of a variety of auxiliary sensors. These are not sufficient for navigation on their own, but they are useful for compensating for the limitations of the main sensors.

### 2.6.1 Wheel Odometry

A wheel encoder is the most basic proprioceptive sensor: it estimates travel distance by measuring wheel rotation.

**Observation model.** For a differential-drive robot, from the rotation angles of the left and right wheels $\Delta \theta_L, \Delta \theta_R$:

$$\Delta s = \frac{r(\Delta \theta_L + \Delta \theta_R)}{2}, \quad \Delta \psi = \frac{r(\Delta \theta_R - \Delta \theta_L)}{d}$$

where $r$ is the wheel radius, $d$ is the distance between the left and right wheels (tread), $\Delta s$ is the forward distance, and $\Delta \psi$ is the change in yaw angle.

**Ackermann steering model** (cars):

$$\dot{x} = v \cos\psi, \quad \dot{y} = v \sin\psi, \quad \dot{\psi} = \frac{v \tan\delta}{L}$$

where $v$ is the rear-wheel velocity, $\delta$ is the steering angle, and $L$ is the wheelbase.

**Slip model.** In reality the wheel slips. Slip is especially large on wet roads, off-road surfaces, and during rapid acceleration or braking. The slip ratio is:

$$s = \frac{v_{\text{wheel}} - v_{\text{actual}}}{v_{\text{actual}}}$$

Under heavy slip, the reliability of wheel odometry drops sharply. In sensor fusion we handle this with an **adaptive observation covariance** — when slip is detected, the uncertainty of wheel odometry is enlarged so that other sensors dominate.

**Role in sensor fusion.** Wheel odometry is used in VIO/LIO systems as an additional velocity/position observation. Its short-term accuracy is particularly high on straight-line motion, so it compensates for IMU drift in environments with few visual features (tunnels, long straight roads). There have been works that extend VINS-Mono by adding a wheel-odometry factor.

### 2.6.2 Barometer

A barometer measures atmospheric pressure to estimate **altitude**.

**Observation model.** The pressure-altitude relation under the International Standard Atmosphere (ISA):

$$h = \frac{T_0}{L}\left(1 - \left(\frac{P}{P_0}\right)^{\frac{RL}{g_0}}\right)$$

where:
- $P$: measured pressure [Pa]
- $P_0 = 101325\,\text{Pa}$: standard sea-level pressure
- $T_0 = 288.15\,\text{K}$: standard sea-level temperature
- $L = 0.0065\,\text{K/m}$: lapse rate
- $R = 287.053\,\text{J/(kg·K)}$: specific gas constant for air
- $g_0 = 9.80665\,\text{m/s}^2$: standard gravitational acceleration

Simplified approximation (low altitude):

$$\Delta h \approx -\frac{\Delta P}{\rho g} \approx -\frac{\Delta P}{12.0}\,[\text{m}], \quad (\Delta P\text{ in Pa})$$

Near sea level, a pressure change of about $8.5\,\text{Pa}$ corresponds to an altitude change of $1\,\text{m}$.

**Noise characteristics:**
- Short-term precision: $\pm 0.1\text{–}0.5\,\text{m}$ (excellent)
- Long-term precision: $\pm 1\text{–}10\,\text{m}$ (pressure drift from weather changes)

**Role in sensor fusion.** The barometer provides a vertical (altitude) observation. This suppresses the vertical drift of the IMU and is especially useful for holding vertical position during drone hover. However, long-term drift from weather changes means it is more appropriate to use relative altitude changes rather than absolute altitude. Indoors, one must be careful of pressure changes from air conditioning or opening and closing doors.

### 2.6.3 Magnetometer

A magnetometer measures the 3-axis magnetic field vector. From the Earth's magnetic field we can extract an absolute **yaw heading**.

**Observation model.** The magnetometer reading is:

$$\tilde{\mathbf{m}} = \mathbf{R}_{bw} \mathbf{m}_w + \mathbf{b}_{\text{hard}} + \mathbf{S}_{\text{soft}} \mathbf{R}_{bw} \mathbf{m}_w + \mathbf{n}_m$$

where:
- $\mathbf{m}_w$: Earth's magnetic field vector in the world frame (magnitude about $25\text{–}65\,\mu\text{T}$, varies with location)
- $\mathbf{b}_{\text{hard}}$: **hard-iron bias** — a constant magnetic field caused by nearby permanent magnets or metal
- $\mathbf{S}_{\text{soft}}$: **soft-iron distortion** — direction-dependent distortion of the magnetic field caused by surrounding ferromagnetic material
- $\mathbf{n}_m$: measurement noise

**Yaw extraction.** If roll and pitch are known (from the accelerometer), the yaw angle can be computed from the magnetometer reading:

$$\psi = \arctan2(m_y \cos\phi - m_z \sin\phi, \, m_x \cos\theta + m_y \sin\theta \sin\phi + m_z \sin\theta \cos\phi)$$

where $\phi$ is roll, $\theta$ is pitch, and $m_x, m_y, m_z$ are the hard-iron-corrected magnetometer readings.

**Limitations and cautions:**
- Magnetic distortion is very large indoors, inside vehicles, and near buildings. Near reinforced-concrete buildings, heading errors of tens of degrees can occur.
- Sensitive to electromagnetic interference from motors, wires, and other currents.
- For these reasons, the magnetometer plays only an auxiliary role in sensor fusion, and is used only when its reading is trusted or with adaptive weighting.

### 2.6.4 UWB (Ultra-Wideband)

UWB is a wireless technology that uses ultra-short pulses over a very wide bandwidth (500 MHz or more) to measure **range** between nodes with high precision.

**Observation model (TWR — Two-Way Ranging):**

$$d = \frac{c \cdot (t_{\text{round}} - t_{\text{reply}})}{2} + n_d$$

where:
- $t_{\text{round}}$: time from transmission of the request pulse to reception of the reply pulse
- $t_{\text{reply}}$: processing time at the responding node
- $n_d$: range noise, typically $\sigma_d \approx 5\text{–}30\,\text{cm}$ in LOS environments

**NLOS (Non-Line-of-Sight) problem.** UWB achieves high precision when a direct line of sight (LOS) is available, but when the signal passes through walls or obstacles (NLOS), it is delayed and reports a range larger than the true one. NLOS detection and mitigation is the central challenge of UWB-based positioning.

**Role in sensor fusion.** By pre-installing UWB anchors in the environment, one can estimate an absolute position by trilaterating the ranges to each anchor. This can substitute for GNSS indoors, and combined with VIO it corrects long-term drift.

**Observation equations (trilateration):**

$$d_i = \|\mathbf{p} - \mathbf{a}_i\| + n_{d,i}, \quad i = 1, \ldots, N_a$$

where $\mathbf{p}$ is the unknown tag position and $\mathbf{a}_i$ is the known position of the $i$-th anchor. Three or more anchors suffice to estimate a 2D position, and four or more for 3D.

```python
import numpy as np

def uwb_trilateration(anchor_positions, ranges):
    """UWB trilateration (least squares).
    
    Args:
        anchor_positions: (N, 3) anchor positions [m]
        ranges: (N,) measured range to each anchor [m]
    
    Returns:
        position: (3,) estimated position [m]
    """
    N = len(ranges)
    # Linearization: difference relative to the first anchor
    # ||p - a_i||^2 - ||p - a_0||^2 = d_i^2 - d_0^2
    # 2(a_0 - a_i)^T p = d_i^2 - d_0^2 - ||a_i||^2 + ||a_0||^2
    
    A = np.zeros((N - 1, 3))
    b = np.zeros(N - 1)
    
    a0 = anchor_positions[0]
    d0 = ranges[0]
    
    for i in range(1, N):
        ai = anchor_positions[i]
        di = ranges[i]
        A[i - 1] = 2 * (a0 - ai)
        b[i - 1] = di**2 - d0**2 - np.dot(ai, ai) + np.dot(a0, a0)
    
    # Least-squares solution
    position = np.linalg.lstsq(A, b, rcond=None)[0]
    return position
```

---

## 2.7 Sensor Modeling Summary

We summarize the observation models and key characteristics of the sensors covered in this chapter.

| Sensor | Observation | Observation model | Main noise sources | Typical noise level |
|------|--------|----------|-------------|------------------|
| Camera | 2D image coordinates | $\pi(\mathbf{T} \cdot \mathbf{P})$ (pinhole + distortion) | Detection noise, distortion residual | $0.5\text{–}2$ pixels |
| LiDAR | 3D point $(r, \alpha, \omega)$ | Range-bearing | Range noise, beam divergence, mixed pixel | $1\text{–}5\,\text{cm}$ |
| IMU (gyro) | Angular velocity $\boldsymbol{\omega}$ | $\tilde{\boldsymbol{\omega}} = \boldsymbol{\omega} + \mathbf{b}_g + \mathbf{n}_g$ | Bias, random walk | ARW $\sim 0.1\text{–}1°/\sqrt{\text{hr}}$ |
| IMU (accel) | Specific force $\mathbf{a}$ | $\tilde{\mathbf{a}} = \mathbf{R}(\mathbf{a}-\mathbf{g}) + \mathbf{b}_a + \mathbf{n}_a$ | Bias, random walk | VRW $\sim 0.02\text{–}0.2\,\text{m/s}/\sqrt{\text{hr}}$ |
| GNSS | Pseudorange $\rho$ | $\rho = r + c\delta t_r + I + T + \epsilon$ | Multipath, ionosphere, troposphere | $1\text{–}5\,\text{m}$ (SPP) |
| Radar | Range, Doppler, bearing | FMCW beat frequency | Clutter, multipath | $\Delta R \sim 4\,\text{cm}$, $v \sim 0.1\,\text{m/s}$ |
| Wheel odom. | Rotation count | $\Delta s = r \Delta\theta$ | Slip | $1\text{–}5\%$ of travel distance |
| Barometer | Pressure $P$ | $h = f(P)$ | Weather change | $0.1\text{–}0.5\,\text{m}$ (short term) |
| Magnetometer | Magnetic field $\mathbf{m}$ | $\tilde{\mathbf{m}} = \mathbf{R}\mathbf{m}_w + \mathbf{b} + \mathbf{n}$ | Hard/soft iron | $1\text{–}5°$ (after calibration) |
| UWB | Range $d$ | $d = \|\mathbf{p} - \mathbf{a}\| + n$ | NLOS | $5\text{–}30\,\text{cm}$ (LOS) |

Accurately understanding each sensor's observation model is the first step of sensor fusion. In the next chapter we cover the prerequisite for actually using these sensors together — **calibration**, the process of precisely determining the geometric and temporal relationships between sensors. No matter how accurate the observation models, fusion performance degrades substantially if the relative sensor positions and time synchronization are inaccurate.

---

# Ch.3 — Calibration Deep Dive

Ch.2 defined the observation model of each sensor mathematically. However, applying these models to real sensor data requires one prerequisite — the parameters of the model must be known precisely. The camera's focal length, the relative position between LiDAR and IMU, the time offset between sensors — the process that determines these values precisely is calibration.

> **Key message**: The accuracy of sensor fusion cannot exceed the accuracy of calibration. This chapter covers every aspect of calibration, from camera intrinsics to multi-sensor extrinsics and time synchronization.

Calibration is the first problem that must be solved in a sensor fusion pipeline. No matter how sophisticated the state estimation algorithm, if the internal model of a sensor is inaccurate or if the relative position/orientation between sensors is wrong, the fusion result diverges. In LiDAR-camera fusion in particular, an error of even 1 degree in the extrinsic parameters produces roughly 87 cm of registration error for an object 50 m away. In this chapter, we derive the mathematical foundation of each calibration problem and provide code and tools that can be used directly in practice.

---

## 3.1 Camera Intrinsic Calibration

Camera intrinsic calibration is the process of estimating the parameters that determine where a point in the 3D world is projected onto the 2D image. These parameters include the focal length, the principal point, and the lens distortion coefficients.

### 3.1.1 Pinhole Camera Model (Review)

A 3D world coordinate $\mathbf{P}_w = [X, Y, Z]^\top$ is transformed into the camera frame as $\mathbf{P}_c = [X_c, Y_c, Z_c]^\top$ and then projected onto the image plane as follows:

Here:
- $f_x, f_y$: focal length in pixel units. $f_x = f / p_x$, where $f$ is the physical focal length (mm) and $p_x$ is the pixel size (mm/pixel).
- $(c_x, c_y)$: principal point. The intersection of the optical axis with the image sensor center.
- $\gamma$: skew coefficient. Nearly 0 on modern cameras.
- $s = Z_c$: scale factor (depth).

$\mathbf{K}$ is called the **camera intrinsic matrix** or **calibration matrix**. This matrix has 5 degrees of freedom ($f_x, f_y, c_x, c_y, \gamma$). In practice, $\gamma$ is often set to 0, reducing it to 4.

### 3.1.2 Lens Distortion Model

Real lenses do not follow the ideal straight-line projection of the pinhole model. Distortion is broadly divided into radial distortion and tangential distortion.

First, define **normalized coordinates**:

$$
x = X_c / Z_c, \quad y = Y_c / Z_c, \quad r^2 = x^2 + y^2
$$

**Radial distortion**:
$$
x_{\text{radial}} = x(1 + k_1 r^2 + k_2 r^4 + k_3 r^6)
$$
$$
y_{\text{radial}} = y(1 + k_1 r^2 + k_2 r^4 + k_3 r^6)
$$

Barrel distortion occurs when $k_1 < 0$ and pincushion distortion when $k_1 > 0$. Most lenses are adequately modeled using only $k_1$ and $k_2$.

**Tangential distortion**:
$$
x_{\text{tangential}} = 2p_1 xy + p_2(r^2 + 2x^2)
$$
$$
y_{\text{tangential}} = p_1(r^2 + 2y^2) + 2p_2 xy
$$

This arises when the lens elements are not perfectly parallel to the image sensor plane.

**Final distorted coordinates**:
$$
x_d = x_{\text{radial}} + x_{\text{tangential}}, \quad y_d = y_{\text{radial}} + y_{\text{tangential}}
$$

$$
u = f_x \cdot x_d + c_x, \quad v = f_y \cdot y_d + c_y
$$

The distortion parameter vector is expressed as $\mathbf{d} = [k_1, k_2, p_1, p_2, k_3]$, and OpenCV's `calibrateCamera()` follows this ordering.

### 3.1.3 Zhang's Method: Homography-Based Calibration

The method proposed by [Zhang (2000)](https://ieeexplore.ieee.org/document/888718) estimates camera parameters from images of a planar pattern (a checkerboard) captured in various poses. Since it requires no 3D calibration apparatus — only a printer-produced checkerboard — it is currently the most widely used calibration method.

**Key idea**: When the pattern lies on the $Z = 0$ plane, the 3D-to-2D projection simplifies to a homography.

#### Step 1: Homography Extraction

Since the checkerboard lies on the $Z = 0$ plane, the relation between world coordinate $\mathbf{M} = [X, Y, 0]^\top$ and image coordinate $\mathbf{m} = [u, v]^\top$ in homogeneous coordinates is:

Since $Z = 0$, the $\mathbf{r}_3$ column vanishes. From this:

$$
s \tilde{\mathbf{m}} = \mathbf{H} \tilde{\mathbf{M}}, \quad \mathbf{H} = \mathbf{K} [\mathbf{r}_1 \quad \mathbf{r}_2 \quad \mathbf{t}]
$$

$\mathbf{H}$ is a $3 \times 3$ homography matrix. In each image, a minimum of 4 corresponding points allow $\mathbf{H}$ to be estimated via the DLT (Direct Linear Transform). In practice, since a checkerboard has dozens of corners, we solve the over-determined system using SVD and remove outliers with RANSAC.

**Homography estimation via DLT**: From the correspondence $(\mathbf{M}_j, \mathbf{m}_j)$, using $\tilde{\mathbf{m}}_j \times \mathbf{H} \tilde{\mathbf{M}}_j = \mathbf{0}$, we obtain the linear system:

Here $\mathbf{h}$ is the vectorization of the 9 elements of $\mathbf{H}$. Given $n$ correspondences, we form the $2n \times 9$ matrix $\mathbf{A}$ and find $\mathbf{h}$ that minimizes $\|\mathbf{A}\mathbf{h}\|$ as the right singular vector corresponding to the smallest singular value in the SVD of $\mathbf{A}$.

#### Step 2: Deriving Constraints on the Intrinsic Parameters

Splitting $\mathbf{H} = [\mathbf{h}_1 \quad \mathbf{h}_2 \quad \mathbf{h}_3]$ by columns:

$$
[\mathbf{h}_1 \quad \mathbf{h}_2 \quad \mathbf{h}_3] = \lambda \mathbf{K} [\mathbf{r}_1 \quad \mathbf{r}_2 \quad \mathbf{t}]
$$

Here $\lambda$ is an arbitrary scale factor. From the orthogonality of the rotation matrix we obtain two constraints:

1. **Orthogonality condition**: $\mathbf{r}_1^\top \mathbf{r}_2 = 0$
   $$\mathbf{h}_1^\top \mathbf{K}^{-\top} \mathbf{K}^{-1} \mathbf{h}_2 = 0$$

2. **Equal-length condition**: $\|\mathbf{r}_1\| = \|\mathbf{r}_2\|$
   $$\mathbf{h}_1^\top \mathbf{K}^{-\top} \mathbf{K}^{-1} \mathbf{h}_1 = \mathbf{h}_2^\top \mathbf{K}^{-\top} \mathbf{K}^{-1} \mathbf{h}_2$$

Define $\mathbf{B} = \mathbf{K}^{-\top} \mathbf{K}^{-1}$. Since $\mathbf{B}$ is a symmetric positive-definite matrix, it has 6 independent elements:

$$
\mathbf{B} = \begin{bmatrix} B_{11} & B_{12} & B_{13} \\ B_{12} & B_{22} & B_{23} \\ B_{13} & B_{23} & B_{33} \end{bmatrix}
$$

Written as a vector, $\mathbf{b} = [B_{11}, B_{12}, B_{22}, B_{13}, B_{23}, B_{33}]^\top$.

#### Step 3: Constructing the Linear System

$\mathbf{h}_i^\top \mathbf{B} \mathbf{h}_j$ can be expressed as an inner product with respect to $\mathbf{b}$:

$$
\mathbf{h}_i^\top \mathbf{B} \mathbf{h}_j = \mathbf{v}_{ij}^\top \mathbf{b}
$$

Here:
$$
\mathbf{v}_{ij} = \begin{bmatrix} h_{1i}h_{1j} \\ h_{1i}h_{2j} + h_{2i}h_{1j} \\ h_{2i}h_{2j} \\ h_{3i}h_{1j} + h_{1i}h_{3j} \\ h_{3i}h_{2j} + h_{2i}h_{3j} \\ h_{3i}h_{3j} \end{bmatrix}
$$

From the two constraints on each image:

$$
\begin{bmatrix} \mathbf{v}_{12}^\top \\ (\mathbf{v}_{11} - \mathbf{v}_{22})^\top \end{bmatrix} \mathbf{b} = \mathbf{0}
$$

Given $n$ images, we obtain a $2n \times 6$ system.

**Minimum number of images**: Setting $\gamma = 0$ (adding the constraint $B_{12} = 0$) leaves 5 unknowns, so a minimum of 3 images suffices. The general 5-parameter model also requires a minimum of 3 images. In practice, 15-25 images are captured.

#### Step 4: Recovering K

Once $\mathbf{b}$ is solved, $\mathbf{B} = \mathbf{K}^{-\top} \mathbf{K}^{-1}$ can be recovered, and the Cholesky decomposition $\mathbf{B} = \mathbf{L}\mathbf{L}^\top$ yields $\mathbf{K}^{-1} = \mathbf{L}^\top$, from which $\mathbf{K}$ is obtained. Concretely:

#### Step 5: Computing Extrinsic Parameters

Once $\mathbf{K}$ is known, for each image $i$:

$$
\mathbf{r}_1 = \lambda \mathbf{K}^{-1} \mathbf{h}_1, \quad \mathbf{r}_2 = \lambda \mathbf{K}^{-1} \mathbf{h}_2, \quad \mathbf{t} = \lambda \mathbf{K}^{-1} \mathbf{h}_3
$$

where $\lambda = 1 / \|\mathbf{K}^{-1} \mathbf{h}_1\|$. We obtain $\mathbf{r}_3 = \mathbf{r}_1 \times \mathbf{r}_2$. Because the estimated $[\mathbf{r}_1, \mathbf{r}_2, \mathbf{r}_3]$ is generally not an exact rotation matrix (due to noise), it is projected onto the nearest rotation matrix using SVD: $\mathbf{R} = \mathbf{U}\mathbf{V}^\top$ (with $\det(\mathbf{R}) = 1$).

#### Step 6: Nonlinear Optimization (MLE)

The linear solution provides a good initial value but does not account for distortion parameters and is not optimal under noise. In the final stage, the Levenberg-Marquardt (LM) algorithm minimizes the **reprojection error**:

Here $n$ is the number of images, $m$ is the number of corners per image, and $\hat{\mathbf{m}}(\cdot)$ is the full projection function including distortion.

The Jacobian of this optimization consists of the partial derivatives of the projection function with respect to each parameter. OpenCV's `calibrateCamera()` implements this entire pipeline (DLT → linear K estimation → LM optimization).

### 3.1.4 Calibration Code with OpenCV

```python
import numpy as np
import cv2
import glob

# Checkerboard settings
CHECKERBOARD = (9, 6)  # Number of interior corners (columns, rows)
SQUARE_SIZE = 0.025    # 25mm square

# Generate 3D world coordinates (Z=0 plane)
objp = np.zeros((CHECKERBOARD[0] * CHECKERBOARD[1], 3), np.float32)
objp[:, :2] = np.mgrid[0:CHECKERBOARD[0], 0:CHECKERBOARD[1]].T.reshape(-1, 2)
objp *= SQUARE_SIZE

obj_points = []  # 3D points (same in all images)
img_points = []  # 2D points (different per image)

images = sorted(glob.glob("calibration_images/*.jpg"))

for fname in images:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    
    # Corner detection
    ret, corners = cv2.findChessboardCorners(
        gray, CHECKERBOARD,
        cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_FAST_CHECK + cv2.CALIB_CB_NORMALIZE_IMAGE
    )
    
    if ret:
        # Refine corner locations to sub-pixel precision
        criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
        corners_refined = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
        
        obj_points.append(objp)
        img_points.append(corners_refined)

# Perform calibration (Zhang's method)
ret, K, dist, rvecs, tvecs = cv2.calibrateCamera(
    obj_points, img_points, gray.shape[::-1], None, None
)

print(f"RMS reprojection error: {ret:.4f} pixels")
print(f"Camera matrix K:\n{K}")
print(f"Distortion coefficients: {dist.ravel()}")

# Reprojection error analysis
errors = []
for i in range(len(obj_points)):
    img_points_proj, _ = cv2.projectPoints(obj_points[i], rvecs[i], tvecs[i], K, dist)
    error = cv2.norm(img_points[i], img_points_proj, cv2.NORM_L2) / len(img_points_proj)
    errors.append(error)
    
print(f"Per-image mean error: {np.mean(errors):.4f} pixels")
print(f"Max error image: {np.argmax(errors)} ({max(errors):.4f} px)")
```

### 3.1.5 Practical Tips: Conditions for Good Calibration

The quality of calibration is determined during the data collection process. Below are key conditions that have been repeatedly confirmed in practice.

**Pose Diversity**: the most important factor. The checkerboard should be captured from diverse angles and positions. Concretely:
- Place the checkerboard in every region of the image — top, bottom, left, right, and center (essential for principal-point estimation)
- Tilt the board by more than 45 degrees when photographing (improves the accuracy of focal-length estimation)
- Place the board close to and far from the camera to obtain a variety of scales
- A minimum of 15-25 images, ideally more than 50

**Corner Accuracy**: always secure sub-pixel precision with `cv2.cornerSubPix()`. Exclude images where corner detection fails.

**Illumination Conditions**: uniform lighting is ideal, but slight shadows do not affect corner detection. Glare interferes with corner detection, so use a checkerboard printed on matte paper.

**Reprojection Error Criteria**:
- $< 0.3$ pixels: excellent
- $0.3 - 0.5$ pixels: good
- $0.5 - 1.0$ pixels: consider recollection
- $> 1.0$ pixels: problematic (insufficient pose diversity, corner-detection errors, etc.)

**Detecting Outlier Images**: compute the per-image reprojection error and remove any image with error 2-3 times larger than the mean, then recalibrate. The `errors` array above shows this.

**Warning — accuracy of the square size**: `SQUARE_SIZE` must exactly match the actual printed checkerboard square size. Printer scaling can cause the physical size to differ from the specified size, so always measure with a ruler. If this value is wrong, `tvecs` (the translation vector) will be incorrect, but `K` and the distortion coefficients are unaffected (since the distortion model is defined in normalized coordinates).

### 3.1.6 Fisheye / Omnidirectional Calibration

For fisheye lenses with a FoV exceeding 180 degrees, the standard radial-tangential distortion model fails. The distortion is so severe that the polynomial approximation does not converge.

**Equidistant Projection Model**:

The general camera model proposed by [Kannala & Brandt (2006)](https://ieeexplore.ieee.org/document/1642666) expresses the relation between the incidence angle $\theta$ and the image radius $r$ as a polynomial:

$$
r(\theta) = k_1 \theta + k_2 \theta^3 + k_3 \theta^5 + k_4 \theta^7 + k_5 \theta^9
$$

Here $\theta = \arctan\left(\sqrt{x^2 + y^2}\right)$ is the incidence angle and $x, y$ are normalized coordinates. The ideal equidistant projection is $r = f\theta$, and the higher-order polynomial terms correct for the actual lens deviation.

OpenCV implements a variant of this model in the `cv2.fisheye` module:

$$
\theta_d = \theta(1 + k_1\theta^2 + k_2\theta^4 + k_3\theta^6 + k_4\theta^8)
$$

```python
# Fisheye calibration (OpenCV fisheye module)
import cv2
import numpy as np

calibration_flags = (
    cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC
    + cv2.fisheye.CALIB_CHECK_COND
    + cv2.fisheye.CALIB_FIX_SKEW
)

K_fisheye = np.zeros((3, 3))
D_fisheye = np.zeros((4, 1))  # k1, k2, k3, k4

print(f"Fisheye RMS error: {ret:.4f}")
print(f"K:\n{K_fisheye}")
print(f"D: {D_fisheye.ravel()}")
```

**Scaramuzza OCamCalib (Omnidirectional Camera Calibration)**:

The OCamCalib of [Scaramuzza et al. (2006)](https://sites.google.com/site/scarabotix/ocamcalib-omnidirectional-camera-calibration-toolbox-for-matlab) is a unified calibration tool for catadioptric systems (mirror + lens) and ultra-wide-angle fisheye lenses. It models the projection function directly as a polynomial, independent of sensor type:

$$
\begin{bmatrix} u \\ v \end{bmatrix} = \begin{bmatrix} x_c \\ y_c \end{bmatrix} + \mathbf{A} \cdot \rho(\theta) \begin{bmatrix} \cos(\phi) \\ \sin(\phi) \end{bmatrix}
$$

Here $\mathbf{A}$ is an affine transformation matrix (for stretch and non-square pixel correction) and $\rho(\theta)$ is the image radius as a function of incidence angle.

### 3.1.7 Camera Calibration with Kalibr

[Kalibr](https://github.com/ethz-asl/kalibr), developed at ETH Zurich, is the de facto standard for camera-IMU calibration (Section 3.4). For camera intrinsic calibration as well, it often yields more refined results than OpenCV.

**Camera models supported by Kalibr**:

| Model | # Parameters | Suitable Lens |
|------|-----------|-----------|
| `pinhole-radtan` | 4 + 4 | Standard lens (same as OpenCV) |
| `pinhole-equi` | 4 + 4 | Fisheye lens |
| `omni-radtan` | 5 + 4 | Ultra-wide / catadioptric |
| `ds` (Double Sphere) | 6 | Wide-angle lens (modern model) |
| `eucm` (Extended UCM) | 6 | Wide-angle lens |

**AprilGrid vs Checkerboard**: Kalibr recommends the AprilGrid target. Each tag in an AprilGrid has a unique ID encoded, so corner detection works even under partial occlusion, and corner ordering is identified automatically.

**Kalibr run example** (camera intrinsic only):
```bash
# Generate the AprilGrid target file
kalibr_create_target_pdf --type apriltag \
    --nx 6 --ny 6 --tsize 0.024 --tspace 0.3

# Camera calibration
kalibr_calibrate_cameras \
    --target april_6x6_24x24mm.yaml \
    --bag camera_calibration.bag \
    --models pinhole-radtan \
    --topics /cam0/image_raw
```

Kalibr's internal operation proceeds as follows:
1. Detect AprilGrid corners in each frame
2. Estimate homographies from corner correspondences (similar to Step 1 of Zhang's method)
3. Perform batch optimization over the full parameter set of the camera model
4. Visualize the optimization result and the distribution of residuals

Kalibr's advantages over OpenCV's default calibration are:
- **B-spline trajectory representation**: a continuous-time model that naturally handles motion blur effects
- **Variety of camera models**: supports modern models such as DS and EUCM
- **Multi-camera**: can simultaneously estimate the relative poses of several cameras
- **IMU integration**: connects naturally to the camera-IMU calibration covered in Section 3.4

---

## 3.2 Camera-Camera (Stereo) Extrinsic

In a stereo camera system, calibrating the relative pose (extrinsic) between the two cameras is a prerequisite for depth estimation.

### 3.2.1 Stereo Calibration

Given two cameras $C_L$ (left) and $C_R$ (right), for the same 3D point $\mathbf{P}$:

$$
\mathbf{P}_{C_R} = \mathbf{R} \cdot \mathbf{P}_{C_L} + \mathbf{t}
$$

Here $(\mathbf{R}, \mathbf{t})$ is the transformation from the left camera frame to the right camera frame. To estimate it, we capture the same checkerboard simultaneously with both cameras.

OpenCV's `cv2.stereoCalibrate()` estimates $(\mathbf{R}, \mathbf{t})$ while either fixing the intrinsics of both cameras or refining them simultaneously:

```python
# Assume the intrinsics of both cameras are already calibrated
# K_L, dist_L, K_R, dist_R: intrinsic parameters of each camera

flags = cv2.CALIB_FIX_INTRINSIC  # Fix intrinsics

print(f"Stereo RMS error: {ret:.4f} pixels")
print(f"Baseline: {np.linalg.norm(t):.4f} m")
print(f"R:\n{R}")
print(f"t: {t.ravel()}")
```

### 3.2.2 Stereo Rectification

After calibration, the images from both cameras are transformed by **parallel rectification** so that the epipolar lines become horizontal. After rectification, the correspondence search is reduced to a 1D problem (search along the same row), greatly improving stereo matching efficiency.

**Epipolar Geometry Review**:

The plane formed by the two projection centers $O_L, O_R$ and a 3D point $\mathbf{P}$ is called the epipolar plane. The lines where this plane intersects each image plane are the epipolar lines. The point $\mathbf{m}_R$ in the right image corresponding to a point $\mathbf{m}_L$ in the left image must lie on the corresponding epipolar line.

Expressed as an equation:

$$
\tilde{\mathbf{m}}_R^\top \mathbf{F} \tilde{\mathbf{m}}_L = 0
$$

Here $\mathbf{F}$ is the **fundamental matrix**. For calibrated cameras, we use the **essential matrix** $\mathbf{E}$:

$$
\hat{\mathbf{m}}_R^\top \mathbf{E} \hat{\mathbf{m}}_L = 0, \quad \mathbf{E} = [\mathbf{t}]_\times \mathbf{R}
$$

Here $\hat{\mathbf{m}} = \mathbf{K}^{-1}\tilde{\mathbf{m}}$ is the normalized coordinate and $[\mathbf{t}]_\times$ is the skew-symmetric matrix of the translation vector.

**Rectification Procedure**:

Rectification is the process of finding homographies that transform both camera image planes onto a common virtual plane. This common plane is set to be parallel to the baseline vector of the two cameras.

OpenCV's `cv2.stereoRectify()` uses Bouguet's algorithm:

```python
# Compute the rectification mapping
R_L, R_R, P_L, P_R, Q, roi_L, roi_R = cv2.stereoRectify(
    K_L, dist_L, K_R, dist_R,
    gray.shape[::-1],
    R, t,
    alpha=0  # 0: keep only valid pixels, 1: keep all original pixels
)

# Create undistort + rectify maps
map_Lx, map_Ly = cv2.initUndistortRectifyMap(
    K_L, dist_L, R_L, P_L, gray.shape[::-1], cv2.CV_32FC1
)
map_Rx, map_Ry = cv2.initUndistortRectifyMap(
    K_R, dist_R, R_R, P_R, gray.shape[::-1], cv2.CV_32FC1
)

# Rectify images
img_L_rect = cv2.remap(img_L, map_Lx, map_Ly, cv2.INTER_LINEAR)
img_R_rect = cv2.remap(img_R, map_Rx, map_Ry, cv2.INTER_LINEAR)
```

**Verification via the epipolar constraint**: The most intuitive way to confirm that rectification is correct is to draw horizontal lines on the rectified left and right images. If corresponding points lie on the same horizontal line, the rectification is accurate.

```python
# Visualize epipolar-line verification
def draw_epilines(img_L_rect, img_R_rect, num_lines=20):
    h, w = img_L_rect.shape[:2]
    canvas = np.hstack([img_L_rect, img_R_rect])
    for y in np.linspace(0, h-1, num_lines, dtype=int):
        color = tuple(np.random.randint(0, 255, 3).tolist())
        cv2.line(canvas, (0, y), (2*w, y), color, 1)
    return canvas
```

**From disparity to depth**: In rectified stereo images, the horizontal displacement $d = u_L - u_R$ between corresponding left-right points is called the disparity. The depth is:

$$
Z = \frac{f \cdot B}{d}
$$

Here $f$ is the focal length (in pixel units) and $B = \|\mathbf{t}\|$ is the baseline length. Using the $Q$ matrix, 3D point clouds can be computed directly from the disparity map: $\mathbf{P}_{3D} = Q \cdot [u, v, d, 1]^\top$.

---

## 3.3 Camera-LiDAR Extrinsic Calibration

Estimating the extrinsic parameters $(\mathbf{R}, \mathbf{t})$ between a camera and a LiDAR is a core prerequisite for multi-modal sensor fusion. To project LiDAR point clouds onto images or to place image features in 3D space, this transformation must be accurate.

### 3.3.1 Target-based Calibration

The most traditional approach, in which a known geometric target (checkerboard, AprilTag, etc.) is observed simultaneously by the camera and the LiDAR to create correspondences.

**Principle**: The corners of a checkerboard are observed as 2D points in the camera image and as a 3D plane in the LiDAR point cloud. We extract the LiDAR points that fit the checkerboard plane and use the plane's normal and boundary to build 3D-2D correspondences.

**3D-2D correspondence-based method**:

1. Detect checkerboard corners in the camera image → 2D points $\{\mathbf{m}_j\}$
2. Fit the checkerboard plane in the LiDAR point cloud with RANSAC → plane equation $\mathbf{n}^\top \mathbf{p} + d = 0$
3. Extract the checkerboard boundary from the points on the plane to estimate the 3D coordinates of the corners $\{\mathbf{P}_j\}$
4. Estimate $(\mathbf{R}, \mathbf{t})$ from the 3D-2D correspondences $\{(\mathbf{P}_j, \mathbf{m}_j)\}$ via the PnP (Perspective-n-Point) algorithm

**Plane-constraint-based method**:

When the precise 3D positions of corners are hard to estimate, calibration is possible using only plane constraints. Back-project the corners detected by the camera to form 3D rays, and use as correspondences the points where those rays intersect the plane estimated by the LiDAR.

Plane constraints from $n$ checkerboard poses:

$$
\mathbf{n}_i^\top (\mathbf{R} \mathbf{p}_{L,i} + \mathbf{t}) + d_i = 0, \quad \forall i = 1, \ldots, n
$$

Here $\mathbf{n}_i$ is the plane normal in the camera frame and $\mathbf{p}_{L,i}$ is a point on the plane in the LiDAR frame.

```python
import numpy as np
import cv2

def calibrate_camera_lidar_target(
    img_corners_list,    # 2D checkerboard corners in each image [N_imgs x N_corners x 2]
    lidar_planes_list,   # LiDAR plane parameters per observation [N_imgs x (n, d)]
    K, dist,             # Camera intrinsics
    board_corners_3d     # 3D corners in the checkerboard frame [N_corners x 3]
):
    """Target-based Camera-LiDAR extrinsic calibration via PnP."""
    
    # Estimate camera-to-checkerboard transform for each image
    all_points_3d_lidar = []
    all_points_2d_camera = []
    
    for i, (corners_2d, (normal, d)) in enumerate(zip(img_corners_list, lidar_planes_list)):
        # Checkerboard pose as seen from the camera (PnP)
        ret, rvec, tvec = cv2.solvePnP(
            board_corners_3d, corners_2d, K, dist
        )
        R_cam_board, _ = cv2.Rodrigues(rvec)
        
        # Transform checkerboard corners into the camera frame
        corners_cam = (R_cam_board @ board_corners_3d.T + tvec).T
        
        # Collect points on the LiDAR plane
        # (in practice, use plane inliers from the LiDAR point cloud)
        all_points_2d_camera.append(corners_2d)
    
    # The final result is refined by nonlinear optimization
    return R_cam_lidar, t_cam_lidar
```

**Practical considerations**:
- The checkerboard must be large enough for a sufficient number of LiDAR points to fall on it. At minimum A2 size, ideally A0.
- 10-20 observations from various distances and angles are required.
- Enough LiDAR beams must strike the checkerboard plane. This is difficult with sparse LiDAR (16-channel).

### 3.3.2 Targetless Calibration

In practice, preparing and installing a calibration target is often cumbersome or impossible. Targetless calibration estimates the transformation between sensors using only information from natural scenes.

#### Mutual Information (MI) Based Method

**Intuition**: When a LiDAR point cloud is projected onto a camera image, the transformation for which the statistical dependency between the two data is maximized is the correct calibration.

**Definition of Mutual Information**:

The mutual information between two random variables $X$ (LiDAR reflectivity or depth) and $Y$ (image intensity):

$$
\text{MI}(X; Y) = \sum_{x}\sum_{y} p(x, y) \log \frac{p(x, y)}{p(x)p(y)}
$$

Or in entropy form:

$$
\text{MI}(X; Y) = H(X) + H(Y) - H(X, Y)
$$

If $X$ and $Y$ are independent, $\text{MI} = 0$; if perfectly dependent, $\text{MI} = H(X) = H(Y)$.

**Normalized Information Distance (NID)**:

[Koide et al. (2023)](https://arxiv.org/abs/2302.05094) use NID, a normalized MI, as the cost function:

$$
\text{NID}(X; Y) = 1 - \frac{\text{MI}(X; Y)}{H(X, Y)} = \frac{H(X, Y) - \text{MI}(X; Y)}{H(X, Y)}
$$

NID has a range of $[0, 1]$, and smaller values indicate better registration of the two datasets.

**Histogram-based MI computation**:

In practice, MI is estimated from the joint histogram:

```python
import numpy as np

def compute_nid(lidar_intensity, image_intensity, bins=64):
    """
    Compute NID (Normalized Information Distance).
    lidar_intensity: LiDAR reflectivity [N]
    image_intensity: image intensity at the projected location [N]
    """
    # Compute joint histogram
    hist_2d, _, _ = np.histogram2d(
        lidar_intensity, image_intensity, bins=bins,
        range=[[0, 255], [0, 255]]
    )
    
    # Normalize to a probability
    pxy = hist_2d / hist_2d.sum()
    px = pxy.sum(axis=1)  # marginal X
    py = pxy.sum(axis=0)  # marginal Y
    
    # Entropy computation (handle 0 log 0 = 0)
    eps = 1e-10
    H_xy = -np.sum(pxy[pxy > eps] * np.log(pxy[pxy > eps]))
    H_x = -np.sum(px[px > eps] * np.log(px[px > eps]))
    H_y = -np.sum(py[py > eps] * np.log(py[py > eps]))
    
    MI = H_x + H_y - H_xy
    NID = 1.0 - MI / (H_xy + eps)
    
    return NID, MI
```

The optimization of MI-based calibration has no explicit gradient, so we use gradient-free optimizers such as Nelder-Mead, or numerical gradients. [Pandey et al. (2015)](https://onlinelibrary.wiley.com/doi/abs/10.1002/rob.21542) were the first to apply this approach to LiDAR-camera calibration.

#### Edge Alignment Based Method

**Intuition**: Optimize the transformation so that depth-discontinuity edges extracted from the LiDAR point cloud register with edges extracted from the camera image.

Let $\mathbf{e}_L$ denote edges extracted from a LiDAR depth image, and $\mathbf{e}_C$ edges from the camera image:

$$
\min_{\mathbf{R}, \mathbf{t}} \sum_i \text{dist}(\pi(\mathbf{R}\mathbf{p}_{L,i} + \mathbf{t}), \mathbf{e}_C)
$$

Here $\pi(\cdot)$ is the camera projection function and $\text{dist}(\cdot, \mathbf{e}_C)$ is the distance to the nearest edge (using a distance transform).

#### Learning-based Methods

Deep learning approaches such as RegNet and CalibNet take the depth image of the LiDAR point cloud and the camera image as input and directly regress the 6-DoF transformation. These methods can perform calibration without initial values, but they currently fall short of traditional methods in accuracy and depend on the domain of the training data.

### 3.3.3 Koide et al. (2023) — State-of-the-Art Targetless Calibration

The `direct_visual_lidar_calibration` of [Koide et al. (2023)](https://arxiv.org/abs/2302.05094) is currently the most practical targetless LiDAR-camera calibration tool. Its core pipeline:

**Stage 1: LiDAR point cloud densification**

A single scan from a rotational LiDAR (Ouster, Velodyne, etc.) is too sparse for MI registration. The CT-ICP (Continuous-Time ICP) algorithm precisely accumulates several seconds of continuous scans to generate a dense point cloud. Solid-state LiDAR (Livox, etc.) densifies naturally thanks to its non-repetitive scan pattern.

**Stage 2: SuperGlue-based initial estimation**

The dense point cloud is rendered from a virtual camera viewpoint to produce a LiDAR intensity image. SuperGlue (learning-based matching) detects 2D-2D correspondences between this rendered image and the actual camera image. These are converted to 2D-3D correspondences, from which the initial transformation is estimated via RANSAC + PnP.

This stage is innovative because SuperGlue solves the cross-modal correspondence problem of matching images from different modalities (LiDAR intensity vs. camera RGB). The success rate of the initial estimation is over 80%.

**Stage 3: NID-based fine registration**

Starting from the initial estimate, a Nelder-Mead optimization minimizes the NID. View-based hidden point removal is used to discard LiDAR points not visible from the camera, improving registration quality.

**Result**: Mean translation error of 0.043 m and rotation error of 0.374 degrees. It operates across a wide variety of combinations — rotational/solid-state LiDAR, pinhole/fisheye/omnidirectional cameras.

### 3.3.4 Practical Tool Comparison

| Tool | Approach | Target needed | Accuracy | Automation level |
|------|------|----------|--------|-----------|
| Autoware Calibration Toolkit | Target-based | O | High | Semi-automatic |
| `direct_visual_lidar_calibration` (Koide) | Targetless (NID) | X | High | Automatic |
| ACSC (Automatic Calibration) | Target-based + auto corner | O | High | Automatic |
| LiveCalib | Online | X | Medium | Fully automatic |

Recently, [MFCalib (2024)](https://arxiv.org/abs/2409.00992) significantly improved the accuracy of single-shot targetless calibration by simultaneously leveraging depth-continuity/discontinuity edges and intensity-discontinuity edges. A distinguishing feature is that it resolves the edge inflation problem by modeling the physical measurement principle of the LiDAR beam.

In practice, a dual strategy is effective: first perform a precise target-based calibration, and then monitor calibration drift during operation with a targetless method.

---

## 3.4 Camera-IMU Extrinsic + Temporal Calibration

The key is to simultaneously estimate not only the spatial displacement (extrinsic) between the camera and the IMU but also the temporal offset. Modern VIO (Visual-Inertial Odometry) systems depend strongly on this calibration.

### 3.4.1 Why Time Offset Matters

The camera and the IMU generate data on different clocks. When a time offset $t_d$ exists between the two sensors, the IMU data corresponding to a camera timestamp $t_c$ is actually from time $t_c + t_d$.

A typical camera-IMU time offset is on the order of several to tens of milliseconds. Ignoring this offset dramatically increases the reprojection error under fast rotation. For example, if the time offset is 10 ms and the camera is rotating at 100 deg/s, a 1-degree rotation error results.

### 3.4.2 Kalibr: Continuous-Time B-Spline-Based Calibration

Kalibr, proposed by [Furgale et al. (2013)](https://ieeexplore.ieee.org/document/6696514), is the de facto standard for camera-IMU calibration.

**Key idea**: Represent the trajectory not as a sequence of discrete poses but as a continuous-time B-spline. This naturally handles sensors with different sampling rates (camera: 20-30 Hz, IMU: 200-1000 Hz).

**B-Spline trajectory representation**:

In a cubic B-spline, the pose $\mathbf{T}(t) \in SE(3)$ at time $t$ is expressed as a weighted combination of control points $\{\mathbf{T}_i\}$:

$$
\mathbf{T}(t) = \mathbf{T}_i \prod_{j=1}^{3} \text{Exp}(\mathbf{B}_j(u) \cdot \Omega_{i+j})
$$

Here:
- $u = (t - t_i) / \Delta t$ is the normalized time ($0 \leq u < 1$)
- $\mathbf{B}_j(u)$ are the cubic B-spline basis coefficients
- $\Omega_{i+j} = \text{Log}(\mathbf{T}_{i+j-1}^{-1} \mathbf{T}_{i+j})$ is the Lie algebra representation of the relative transform between adjacent control points
- $\text{Exp}, \text{Log}$ are the exponential/logarithm maps of $SE(3)$

Key advantages of the B-spline:
1. **Differentiable**: velocity and acceleration at any time can be computed analytically → direct connection to the IMU observation model
2. **Asynchronous sensor handling**: not constrained by each sensor's timestamp
3. **Locality**: each basis function affects only 4 control points → sparse optimization is possible

**Observation model**:

Camera observation: project a 3D landmark using the trajectory pose at time $t_c + t_d$ (time-offset-corrected):

$$
\mathbf{e}_{\text{cam},k} = \mathbf{m}_k - \pi\left(\mathbf{T}_{CB} \cdot \mathbf{T}(t_{c,k} + t_d) \cdot \mathbf{p}_w\right)
$$

Here $\mathbf{T}_{CB}$ is the camera-IMU extrinsic (the target of estimation).

IMU observation: predict acceleration and angular velocity from the derivatives of the trajectory at time $t_{\text{imu}}$:

**Optimization problem**:

This is a nonlinear least-squares problem that can be solved with Gauss-Newton or LM. Thanks to the locality of the B-spline, the Jacobian is sparse, so even large-scale problems are handled efficiently.

### 3.4.3 Kalibr Execution Guide

```bash
# 1. Prepare the AprilGrid target
kalibr_create_target_pdf --type apriltag \
    --nx 6 --ny 6 --tsize 0.024 --tspace 0.3

# 2. Collect data (ROS bag)
#    - Place the target in the camera's view and move the sensor rig diversely
#    - Include both rotation and translation along all axes
#    - Minimum 60 seconds, ideally more than 2 minutes

# 3. Run the Camera-IMU calibration
kalibr_calibrate_imu_camera \
    --target april_6x6_24x24mm.yaml \
    --cam camchain.yaml \
    --imu imu.yaml \
    --bag calibration.bag \
    --bag-freq 20.0 \
    --timeoffset-padding 0.1
```

**IMU configuration file (imu.yaml) example**:
```yaml
# imu.yaml
rostopic: /imu0
update_rate: 200.0  # Hz

# IMU noise parameters (measured via Allan variance, see Section 3.4.4)
accelerometer_noise_density: 0.01    # m/s^2/sqrt(Hz)
accelerometer_random_walk: 0.0002   # m/s^3/sqrt(Hz)
gyroscope_noise_density: 0.005      # rad/s/sqrt(Hz)
gyroscope_random_walk: 4.0e-06      # rad/s^2/sqrt(Hz)
```

**Key tips for data collection**:
1. **Motion diversity**: excite all 6-DoF. Rotation about each axis is especially important.
2. **Motion speed**: too slow makes IMU bias hard to estimate, too fast blurs the images.
3. **Target visibility**: the target should be visible during more than 80% of the total collection time.
4. **Start and end**: begin and end at rest to facilitate IMU bias initialization.

**Interpreting the results**: Items to check in Kalibr's output:
- `T_cam_imu`: camera-IMU extrinsic (4x4 transformation matrix)
- `timeshift_cam_imu`: time offset $t_d$ (usually several ms)
- Reprojection-error distribution: a mean of 0.2-0.5 px is ideal
- Accelerometer/gyro residuals: must match the noise model

### 3.4.4 Allan Variance Measurement Hands-On

Accurately knowing the IMU noise parameters directly affects not only Kalibr calibration but the performance of every IMU-based system. Allan variance is a technique for analyzing the noise characteristics of a time series according to cluster time, originally developed to measure the stability of atomic clocks.

**Allan Variance Definition**:

For a time series $\{x_k\}$, the Allan variance at cluster time $\tau = n \cdot \tau_0$ ($\tau_0$: sampling period):

$$
\sigma^2(\tau) = \frac{1}{2(N-2n)} \sum_{k=1}^{N-2n} \left[\bar{x}_{k+n} - \bar{x}_k\right]^2
$$

Here $\bar{x}_k = \frac{1}{n}\sum_{j=0}^{n-1} x_{k+j}$ is the cluster mean.

**Noise identification on a log-log plot**:

Plotting the Allan deviation $\sigma(\tau)$ against $\tau$ on a log-log scale, each noise source appears as a line with a distinct slope:

| Noise type | Slope | $\sigma(\tau)$ |
|-----------|-------|---------------|
| Quantization noise | $-1$ | $\propto \tau^{-1}$ |
| **Angle/Velocity random walk** | $-1/2$ | $\propto \tau^{-1/2}$ |
| **Bias instability** | $0$ | constant (minimum) |
| Rate random walk | $+1/2$ | $\propto \tau^{+1/2}$ |
| Rate ramp | $+1$ | $\propto \tau$ |

**Hands-on code**:

```python
import numpy as np
import matplotlib.pyplot as plt

def compute_allan_variance(data, dt, max_clusters=100):
    """
    Compute the Allan variance.
    data: 1D time series (e.g., gyro x-axis raw data) [N]
    dt: sampling period [s]
    max_clusters: number of clusters distributed on a log scale
    """
    N = len(data)
    max_n = N // 2
    
    # Choose cluster sizes on a log scale
    n_values = np.unique(
        np.logspace(0, np.log10(max_n), max_clusters).astype(int)
    )
    
    taus = []
    allan_vars = []
    
    for n in n_values:
        tau = n * dt
        
        # Compute cluster means
        n_clusters = N // n
        trimmed = data[:n_clusters * n]
        clusters = trimmed.reshape(n_clusters, n)
        cluster_means = clusters.mean(axis=1)
        
        # Allan variance
        diffs = np.diff(cluster_means)
        avar = 0.5 * np.mean(diffs**2)
        
        taus.append(tau)
        allan_vars.append(avar)
    
    return np.array(taus), np.array(allan_vars)

def extract_imu_noise_params(taus, allan_vars, dt):
    """
    Extract IMU noise parameters from the Allan variance plot.
    """
    adev = np.sqrt(allan_vars)
    
    # 1. Random walk (slope -1/2): value at tau=1
    # N (noise density) = sigma(tau=1) 
    idx_1s = np.argmin(np.abs(taus - 1.0))
    noise_density = adev[idx_1s]
    
    # 2. Bias instability: minimum of the Allan deviation
    bias_instability = np.min(adev)
    tau_min = taus[np.argmin(adev)]
    
    # 3. Random walk (slope +1/2): extracted from the long-term slope
    # K (rate random walk) = sigma(tau=3) * sqrt(3) (approximation)
    # In practice, extract precisely via linear regression
    
    return {
        'noise_density': noise_density,
        'bias_instability': bias_instability,
        'tau_min': tau_min
    }

# Usage example
# 1. Record the IMU stationary for at least 2 hours
# 2. Analyze raw data from each gyro/accelerometer axis

# Example: 200 Hz IMU data, 2 hours long
dt = 1.0 / 200  # 5ms
# gyro_x = np.loadtxt("imu_static_gyro_x.txt")  # actual data

# Demo with simulated data
np.random.seed(42)
N = 200 * 3600 * 2  # 2 hours
noise_density = 0.005  # rad/s/sqrt(Hz)
bias = 0.0001  # rad/s (slow drift)
gyro_x = noise_density * np.sqrt(1/dt) * np.random.randn(N)
gyro_x += bias * np.cumsum(np.random.randn(N)) * np.sqrt(dt)

taus, avars = compute_allan_variance(gyro_x, dt)
params = extract_imu_noise_params(taus, avars, dt)

print(f"Gyroscope noise density: {params['noise_density']:.6f} rad/s/sqrt(Hz)")
print(f"Bias instability: {params['bias_instability']:.6f} rad/s")
print(f"Min at tau = {params['tau_min']:.1f} s")

# Allan deviation plot
plt.figure(figsize=(10, 6))
plt.loglog(taus, np.sqrt(avars), 'b-', linewidth=1.5)
plt.xlabel('Cluster time τ (s)')
plt.ylabel('Allan deviation σ(τ)')
plt.title('Gyroscope Allan Deviation')
plt.grid(True, which='both', alpha=0.3)

# Slope reference lines
tau_ref = np.array([0.01, 100])
plt.loglog(tau_ref, params['noise_density'] / np.sqrt(tau_ref), 
           'r--', label='Random walk (-1/2)')
plt.axhline(y=params['bias_instability'], color='g', linestyle='--', 
            label=f'Bias instability ({params["bias_instability"]:.1e})')
plt.legend()
plt.savefig('allan_deviation.png', dpi=150)
plt.show()
```

**Practical tips for Allan variance measurement**:
- **Stationary data collection**: place the IMU on a rigid surface free from vibration and record for at least 2 hours (ideally 6 hours). With short records, the minimum of the bias instability cannot be identified accurately.
- **Comparison with the data sheet**: compare the noise density value in the manufacturer's data sheet with the value extracted from the Allan variance to verify the sensor's health.
- **Connection to Kalibr**: the `accelerometer_noise_density` and `gyroscope_noise_density` entered into Kalibr's `imu.yaml` are exactly the Allan deviation at $\tau = 1$ second.
- **Temperature stabilization**: IMUs are sensitive to temperature, so warm up for 15-30 minutes after power-on before starting the recording.

---

## 3.5 LiDAR-IMU Extrinsic Calibration

The problem of estimating the extrinsic parameters between LiDAR and IMU reduces to the classical **hand-eye calibration** problem.

### 3.5.1 Hand-Eye Calibration (AX = XB)

The archetypal hand-eye calibration problem asks for the relation between the motion of a robot arm's end (hand), observed from the robot's base, and the motion of a target, observed from a camera mounted at the hand ([Tsai & Lenz, 1989](https://ieeexplore.ieee.org/document/34770)).

**Problem definition**:

Given sensors $A$ (e.g., LiDAR) and $B$ (e.g., IMU) rigidly mounted on a rigid body, let the relative motions observed by each sensor between two instants $i, j$ be $\mathbf{A}_{ij}$ and $\mathbf{B}_{ij}$:

$$
\mathbf{A}_{ij} = \mathbf{T}_A^{-1}(t_i) \cdot \mathbf{T}_A(t_j) \quad \text{(relative motion of sensor A)}
$$
$$
\mathbf{B}_{ij} = \mathbf{T}_B^{-1}(t_i) \cdot \mathbf{T}_B(t_j) \quad \text{(relative motion of sensor B)}
$$

The fixed transformation $\mathbf{X} = \mathbf{T}_{AB}$ between the two sensors satisfies:

$$
\mathbf{A}_{ij} \mathbf{X} = \mathbf{X} \mathbf{B}_{ij}
$$

This is the $\mathbf{AX} = \mathbf{XB}$ equation. We must solve for $\mathbf{X} \in SE(3)$.

**Separating rotation from translation**:

Separated into rotation and translation:

$$
\mathbf{R}_A \mathbf{R}_X = \mathbf{R}_X \mathbf{R}_B \quad \text{(rotation)}
$$
$$
\mathbf{R}_A \mathbf{t}_X + \mathbf{t}_A = \mathbf{R}_X \mathbf{t}_B + \mathbf{t}_X \quad \text{(translation)}
$$

The rotation equation is independent of $\mathbf{t}_X$, so we first solve for $\mathbf{R}_X$ and then solve for $\mathbf{t}_X$ from the translation equation.

**[Tsai & Lenz (1989)](https://ieeexplore.ieee.org/document/34770) solution**:

Convert the rotation equation into angle-axis representation. If the rotation axis of $\mathbf{R}_A$ is $\hat{\mathbf{a}}$ and the rotation angle is $\alpha$, then using modified Rodrigues parameters:

$$
\text{skew}(\hat{\mathbf{a}}_A + \hat{\mathbf{a}}_B) \cdot \mathbf{r}_X = \hat{\mathbf{a}}_A - \hat{\mathbf{a}}_B
$$

Here $\mathbf{r}_X$ is the modified Rodrigues vector of $\mathbf{R}_X$. Stacking this equation over many motion pairs yields a linear system $\mathbf{C} \mathbf{r}_X = \mathbf{d}$, solvable with a minimum of 2 motion pairs (with non-parallel rotation axes). The translation vector $\mathbf{t}_X$ is solved by a similar linear system.

**Leveraging more motion pairs**: in practice, dozens to hundreds of motion pairs are used; the resulting overdetermined system is solved by least squares and refined by LM optimization.

### 3.5.2 Motion-based Automatic Calibration

The key requirement of hand-eye calibration is that the motion estimates (odometry) for each sensor must already exist. Since LiDAR odometry and IMU integration operate independently, data can be collected by moving the sensor freely without any calibration target.

```python
import numpy as np
from scipy.spatial.transform import Rotation

def hand_eye_calibration_tsai(A_rotations, A_translations, 
                                B_rotations, B_translations):
    """
    Tsai-Lenz hand-eye calibration (AX = XB).
    
    A_rotations: [N x 3 x 3] relative rotation matrices of sensor A
    A_translations: [N x 3] relative translations of sensor A
    B_rotations: [N x 3 x 3] relative rotation matrices of sensor B
    B_translations: [N x 3] relative translations of sensor B
    """
    N = len(A_rotations)
    
    # Step 1: Estimate rotation RX
    C = []
    d = []
    for i in range(N):
        # Convert to angle-axis representation
        rA = Rotation.from_matrix(A_rotations[i]).as_rotvec()
        rB = Rotation.from_matrix(B_rotations[i]).as_rotvec()
        
        alpha = np.linalg.norm(rA)
        beta = np.linalg.norm(rB)
        
        if alpha < 1e-6 or beta < 1e-6:
            continue  # Ignore small rotations
        
        # Modified Rodrigues parameters
        a_prime = np.tan(alpha / 2) * rA / alpha
        b_prime = np.tan(beta / 2) * rB / beta
        
        # skew(a' + b') * rX = a' - b'
        skew_sum = np.array([
            [0, -(a_prime[2]+b_prime[2]), a_prime[1]+b_prime[1]],
            [a_prime[2]+b_prime[2], 0, -(a_prime[0]+b_prime[0])],
            [-(a_prime[1]+b_prime[1]), a_prime[0]+b_prime[0], 0]
        ])
        
        C.append(skew_sum)
        d.append(a_prime - b_prime)
    
    C = np.vstack(C)
    d = np.concatenate(d)
    
    # Least-squares solve
    rX, _, _, _ = np.linalg.lstsq(C, d, rcond=None)
    
    # Modified Rodrigues → rotation matrix
    angle = 2 * np.arctan(np.linalg.norm(rX))
    if angle > 1e-6:
        axis = rX / np.linalg.norm(rX)
        R_X = Rotation.from_rotvec(angle * axis).as_matrix()
    else:
        R_X = np.eye(3)
    
    # Step 2: Estimate translation tX
    C_t = []
    d_t = []
    for i in range(N):
        C_t.append(A_rotations[i] - np.eye(3))
        d_t.append(R_X @ B_translations[i] - A_translations[i])
    
    C_t = np.vstack(C_t)
    d_t = np.concatenate(d_t)
    
    t_X, _, _, _ = np.linalg.lstsq(C_t, d_t, rcond=None)
    
    return R_X, t_X
```

### 3.5.3 LI-Init (FAST-LIO Lineage)

Modern LIO systems such as FAST-LIO2 include a feature that automatically initializes the LiDAR-IMU extrinsic parameters online. The key idea of the **LI-Init** approach:

1. Estimate the attitude using only IMU data, and estimate the pose via LiDAR matching
2. Iteratively refine the relative transform from the difference of the two estimates
3. Include the LiDAR-IMU extrinsic in the state vector of the Error-State Iterated Kalman Filter (ESIKF) to estimate it online

This approach estimates the extrinsic parameters automatically at the start of the LIO system, with no separate calibration procedure. Convergence occurs after a few seconds of sufficiently diverse motion.

**Advantages**: no separate tools or procedures needed. Immediately usable in the field.
**Disadvantages**: may not converge or may be inaccurate if initial motion is insufficient. Accuracy can be lower than with target-based methods.

**GRIL-Calib**: When motion is confined to a plane, as with ground robots, existing methods suffer reduced accuracy because some axes are poorly observable. [GRIL-Calib (Kim et al., 2024)](https://arxiv.org/abs/2312.14035) leverages the ground-plane residual in LiDAR odometry and integrates a ground-plane motion (GPM) constraint into the optimization, enabling 6-DoF calibration parameters to be estimated from planar motion alone.

So far we have addressed calibration between a single LiDAR and a single IMU. However, in systems that use multiple LiDARs, such as autonomous vehicles, the relative poses between LiDARs must also be determined.

---

## 3.6 LiDAR-LiDAR Extrinsic Calibration

This is the problem of estimating the relative poses between each LiDAR in a multi-LiDAR rig. It is essential when LiDARs are placed at the front/side/rear of an autonomous vehicle.

### 3.6.1 Problem Setup

Given $n$ LiDARs $\{L_1, L_2, \ldots, L_n\}$ rigidly mounted on a vehicle body, estimate the relative transformations $\{\mathbf{T}_{L_1 L_i}\}_{i=2}^{n}$ of the other LiDARs with respect to the reference LiDAR $L_1$.

### 3.6.2 Target-based Methods

Use a large planar target (panel) so that several LiDARs can observe it simultaneously. Fit the target plane in each LiDAR with RANSAC, and estimate the relative transform from constraints on the plane parameters.

### 3.6.3 Targetless: ICP-Based

The most natural targetless method is to register the point clouds of two LiDARs with overlapping regions via ICP.

**When there is an overlapping region**:

If the FoVs of two LiDARs overlap, apply ICP directly to the point clouds in that region:

$$
\min_{\mathbf{R}, \mathbf{t}} \sum_{i} \|\mathbf{p}_{L_2,i} - (\mathbf{R} \mathbf{p}_{L_1,i'} + \mathbf{t})\|^2
$$

Point-to-plane ICP generally converges faster:

$$
\min_{\mathbf{R}, \mathbf{t}} \sum_{i} \left[(\mathbf{R} \mathbf{p}_{L_1,i'} + \mathbf{t} - \mathbf{p}_{L_2,i})^\top \mathbf{n}_{L_2,i}\right]^2
$$

Here $\mathbf{n}_{L_2,i}$ is the normal vector at the target point.

**When there is no overlapping region**:

If the FoVs do not overlap, direct registration is impossible. In this case:
1. Hand-eye calibration (Section 3.5.1): extract relative motions from each LiDAR's odometry to solve AX=XB.
2. SLAM-based: each LiDAR performs SLAM independently, and the relative transform is estimated from matching within a global map of the common environment.

### 3.6.4 Feature-based Methods

In structured environments (buildings, roads, etc.), geometric features such as planes, pillars, and edges are extracted and used for matching. This method is more robust to noise than point-wise matching:

- Extract the same large plane (wall, floor) from multiple LiDARs
- Compute the plane parameters $(n_i, d_i)$ in each LiDAR frame
- Estimate the relative transform from pairs of corresponding planes

A minimum of 3 non-coplanar plane correspondences is required.

The calibrations covered so far (3.1-3.6) concerned relations among cameras, LiDARs, and IMUs. In outdoor systems that exploit GNSS, the spatial relation between the GNSS antenna and the IMU must also be known precisely.

---

## 3.7 GNSS-IMU Lever Arm & Boresight

The spatial relation between the GNSS antenna and the IMU is called the **lever arm**. This is a core calibration parameter in GNSS/INS integrated navigation.

### 3.7.1 Lever Arm Vector

The 3D vector $\mathbf{l} = [l_x, l_y, l_z]^\top$ between the phase center of the GNSS antenna and the IMU's origin is defined in the IMU body frame.

GNSS measures the position of the antenna's phase center, but what we want is the position of the IMU (or vehicle reference point):

$$
\mathbf{p}_{\text{IMU}} = \mathbf{p}_{\text{GNSS}} - \mathbf{R}_{\text{body}}^{\text{nav}} \cdot \mathbf{l}
$$

Here $\mathbf{R}_{\text{body}}^{\text{nav}}$ is the rotation matrix from the body frame to the navigation frame. As the vehicle rotates, the GNSS antenna's position changes, so failing to correct for the lever arm produces position errors. If the lever arm is 1 m and the vehicle tilts by 10 deg, a position error of about 17 cm results.

### 3.7.2 Lever Arm Estimation Methods

**Method 1: Physical measurement**

The most intuitive method is to measure directly with a tape measure, laser rangefinder, or similar. The accuracy is on the order of several cm, which is sufficient for most applications.

**Method 2: Filter-based online estimation**

Include the lever arm $\mathbf{l}$ in the EKF state vector and estimate it online. For the lever arm to be observable in the GNSS observation model, sufficient rotational motion is needed. Straight-line motion alone makes it hard to estimate the forward component ($l_x$) of the lever arm.

**Method 3: Post-processing**

Process the collected GNSS/IMU data with a forward-backward smoother to optimize the lever arm. Primarily used in precision surveying.

### 3.7.3 GNSS Antenna Phase Center

The antenna phase center (APC) of a GNSS antenna does not coincide with the antenna's physical center and varies with the satellite's elevation angle and frequency. This variation is called the Phase Center Variation (PCV), and is on the order of mm to cm.

In precision positioning (RTK/PPP), antenna phase-center correction data (ANTEX files) must be applied. For robotics-grade applications this can usually be ignored, but when survey-grade accuracy is required it must be considered.

---

## 3.8 Online / Continuous Calibration

Calibration parameters may change during operation. Temperature variations, vibration, or shocks can slightly deform the sensor mount, causing the initial calibration to gradually become inaccurate. **Online calibration** is needed to correct for this.

### 3.8.1 Self-Calibration during SLAM

This approach includes calibration parameters in the SLAM system's state vector and estimates them during operation.

**EKF-based**: OpenVINS includes the camera intrinsics, camera-IMU extrinsic, and time offset in its state vector and estimates them online.

State vector extension:
$$
\mathbf{x} = \begin{bmatrix} \mathbf{x}_{\text{nav}} \\ \mathbf{x}_{\text{calib}} \end{bmatrix}
= \begin{bmatrix} \mathbf{q}, \mathbf{p}, \mathbf{v}, \mathbf{b}_g, \mathbf{b}_a \\ \mathbf{q}_{CI}, \mathbf{p}_{CI}, t_d, f_x, f_y, c_x, c_y, k_1, \ldots \end{bmatrix}
$$

The process model for the calibration parameters is typically a random walk:

$$
\dot{\mathbf{x}}_{\text{calib}} = \mathbf{w}_{\text{calib}}, \quad \mathbf{w}_{\text{calib}} \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_{\text{calib}})
$$

The magnitude of $\mathbf{Q}_{\text{calib}}$ reflects how quickly the calibration parameters can change. If too large, the parameters fluctuate unstably; if too small, real changes cannot be tracked.

**Factor graph-based**: LIO-SAM and VINS-Mono can also include the extrinsic parameters as optimization variables. The noise model of the between factor controls the stability of the calibration.

### 3.8.2 Correcting Extrinsic Drift

For long-duration systems (autonomous vehicles, robots), strategies for detecting and correcting drift in the extrinsic parameters:

1. **Reprojection-error monitoring**: continuously monitor the edge-registration quality when LiDAR point clouds are projected onto the image. When the registration quality drops, trigger a recalibration.

2. **Periodic recalibration**: periodically rerun targetless calibration using operational data.

3. **Online fine-tuning**: use the current calibration as the initial value and continuously optimize within a small range.

Recently, [CalibRefine (2025)](https://arxiv.org/abs/2502.17648) proposed a deep-learning-based framework that takes raw LiDAR point clouds and camera images directly as input, performs online targetless calibration, and improves accuracy through iterative post-refinement automatically.

### 3.8.3 OpenCalib: An Integrated Calibration Framework for Autonomous Driving

[OpenCalib (Yan et al., 2023)](https://arxiv.org/abs/2205.14087) is an open-source tool that integrates the various calibrations needed by autonomous-driving systems into a single framework.

**Calibration types supported**:

| Sensor pair | Method | Target |
|---------|------|------|
| Camera intrinsic | Zhang's method | Checkerboard/AprilTag |
| Camera-Camera | Stereo calibration | Shared target |
| Camera-LiDAR | PnP / Edge alignment | With/without |
| LiDAR-LiDAR | ICP / Feature matching | None |
| Camera-Ground | Vanishing point | None |
| Online correction | Monitoring + recalibration | None |

**OpenCalib's design philosophy**:
- **Modularity**: each calibration type is implemented as an independent module, so only what is needed can be used
- **Unified interface**: a ROS-based unified interface handles diverse sensor inputs
- **Visualization**: calibration results are visualized in real time so quality can be assessed intuitively

Calibration in an autonomous-driving system matters not only for a single sensor pair but also for the consistency of the **sensor chain**. For example, if a vehicle has 6 cameras and 1 LiDAR, even if the camera-LiDAR calibrations are performed independently, the relative poses among the cameras must remain consistent. OpenCalib integrates such consistency constraints into a global optimization.

---

## 3.9 Temporal Calibration

Time synchronization between sensors is as important as spatial calibration. On a fast-moving platform, even a few milliseconds of time error translate into centimeter-level position error.

### 3.9.1 Hardware Synchronization

Hardware synchronization is the most accurate and reliable method.

**Trigger-based synchronization**:

A single master timer sends hardware trigger signals to all sensors, causing them to capture data simultaneously.

- **Camera trigger**: the exposure start time is controlled via a GPIO pin connected to the external trigger input
- **LiDAR sync**: some LiDARs (Ouster, etc.) support a PPS (Pulse Per Second) input to synchronize the start time of the scan
- **IMU sync**: the IMU's sampling clock is locked to an external reference

**PPS (Pulse Per Second)**:

A GNSS receiver outputs a 1 Hz electrical pulse (PPS) synchronized with GPS time. Since the rising edge of this pulse corresponds exactly to a GPS-second boundary, it can be used as a reference to align every sensor's local timestamp to GPS time.

The essence of PPS synchronization is to measure the offset between each sensor's local timestamp and the PPS pulse, thereby aligning all data to a common time axis (GPS time).

### 3.9.2 PTP (Precision Time Protocol)

IEEE 1588 PTP is a protocol that provides microsecond-level time synchronization over Ethernet. It is far more precise than the millisecond-level accuracy of NTP (Network Time Protocol).

**How PTP works**:

1. **Master-slave structure**: one master clock (Grandmaster) in the network provides the reference time
2. **Sync message**: the master periodically broadcasts a Sync message
3. **Follow-up**: the precise transmission timestamp is sent in a separate message
4. **Delay Request/Response**: the slave measures the network delay
5. **Offset computation**: $\text{offset} = \frac{(t_2 - t_1) - (t_4 - t_3)}{2}$

```
   Master (Grandmaster)              Slave (Sensor)
         |                                |
         |------- Sync (t1) ------------->|  (t2: reception time)
         |                                |
         |------- Follow-up (t1) -------->|  (conveys the exact t1)
         |                                |
         |<------ Delay_Req (t3) ---------|  (t3: transmission time)
         |                                |
         |------- Delay_Resp (t4) ------->|  (conveys the reception time t4)
         |                                |
         
   offset = [(t2 - t1) - (t4 - t3)] / 2
   delay  = [(t2 - t1) + (t4 - t3)] / 2
```

Modern LiDARs (Ouster, Hesai, etc.) and industrial cameras (FLIR/Lucid, etc.) support PTP hardware timestamping, enabling microsecond-level synchronization without software overhead.

### 3.9.3 Software Synchronization

When hardware synchronization is not available, time alignment is done in software.

**Host clock based**: when all sensor data arrives at the host computer, the host's system clock timestamps them. Simple, but the jitter of USB/network delays introduces uncertainty on the order of milliseconds.

**ROS Time**: in ROS, `ros::Time::now()` records the message reception time. When the driver converts the sensor's local timestamp to ROS time, the accuracy of that conversion determines the overall synchronization precision.

### 3.9.4 Online Estimation of the Time Offset

[Li & Mourikis (2014)](https://journals.sagepub.com/doi/abs/10.1177/0278364913515286) proposed including the time offset $t_d$ in the state vector of VIO and estimating it online. The key idea:

**Reflecting the time offset in the observation model**:

The actual sensor pose at camera observation time $t_c$ is at $t_c + t_d$. Reflecting this in the IMU preintegration:

$$
\mathbf{z}(t_c) = \pi(\mathbf{T}(t_c + t_d) \cdot \mathbf{p}_w)
$$

Assuming $t_d$ is small, a first-order Taylor expansion gives:

$$
\mathbf{T}(t_c + t_d) \approx \mathbf{T}(t_c) \cdot \text{Exp}(\boldsymbol{\xi} \cdot t_d)
$$

Here $\boldsymbol{\xi}$ is the body velocity (angular + linear velocity) at $t_c$.

Through this approximation, the Jacobian with respect to $t_d$ can be computed analytically, so it can be estimated jointly with the other state variables in an EKF or optimization framework.

Recently, [iKalibr (Chen et al., 2024)](https://arxiv.org/abs/2407.11420) extended this idea to multiple sensors and proposed a unified calibration tool that estimates spatio-temporal parameters between heterogeneous sensors such as LiDAR, camera, IMU, and radar **all at once** in a B-spline continuous-time framework (IEEE T-RO 2025).

**Observability conditions**: For the time offset to be observable, the platform must undergo sufficient accelerated motion. With constant-velocity straight-line motion, the time offset cannot be estimated (temporal shift is indistinguishable from spatial shift).

Kalibr (Section 3.4.2), OpenVINS, VINS-Mono, and other modern VIO systems all implement variants of this method.

### 3.9.5 Practical Synchronization Strategy Guide

| Precision required | Recommended method | Cost |
|-----------|----------|------|
| $< 1\mu s$ | PPS + HW trigger | High (dedicated HW required) |
| $1\mu s - 100\mu s$ | PTP | Medium (PTP-capable equipment) |
| $100\mu s - 1ms$ | NTP + online estimation | Low |
| $> 1ms$ | Host clock + online estimation | None |

**Autonomous-driving grade**: PPS + PTP combination is standard. All sensors are synchronized to GPS time.

**Research/prototyping grade**: software synchronization + online time-offset estimation is often sufficient. Kalibr estimates the initial offset and the VIO system fine-tunes it online.

**Core principle**: if hardware synchronization is available, use it. Software synchronization complements, but does not replace, hardware synchronization.

---

## 3.10 Chapter Summary

We summarize the overall system of calibrations covered in this chapter.

| Calibration type | Parameters estimated | Key method | Minimum requirement |
|----------------|-------------|----------|-------------|
| Camera intrinsic | $\mathbf{K}, \mathbf{d}$ | Zhang's method | 15-25 checkerboard images |
| Stereo extrinsic | $\mathbf{R}, \mathbf{t}$ | Shared target + stereoCalibrate | 15-25 simultaneous observation pairs |
| Camera-LiDAR | $\mathbf{T}_{CL}$ | Target-based / NID | 10-20 target observations / natural scene |
| Camera-IMU | $\mathbf{T}_{CI}, t_d$ | Kalibr (B-spline) | 60+ seconds of diverse motion |
| LiDAR-IMU | $\mathbf{T}_{LI}$ | Hand-eye (AX=XB) / LI-Init | Diverse motion |
| LiDAR-LiDAR | $\mathbf{T}_{L_1 L_2}$ | ICP / Feature matching | Overlapping region or motion data |
| GNSS-IMU | $\mathbf{l}$ (lever arm) | Physical measurement / EKF estimation | Rotational motion |
| Temporal | $t_d$ | PPS/PTP + online estimation | Accelerated motion |

**Recommended calibration order**:

1. Intrinsic of each camera (performed independently)
2. Stereo extrinsic (if applicable)
3. Camera-IMU extrinsic + temporal (Kalibr)
4. LiDAR-IMU extrinsic (hand-eye or LI-Init)
5. Camera-LiDAR extrinsic (computed indirectly as the chain of camera-IMU and LiDAR-IMU, or calibrated directly)
6. GNSS lever arm
7. Set up online calibration (in-operation correction)

Example of indirect computation: the camera-LiDAR transform can be obtained as $\mathbf{T}_{CL} = \mathbf{T}_{CI} \cdot \mathbf{T}_{IL}$. However, errors accumulate, so verifying it via direct calibration is recommended.

**Key paper summary**:

- [Zhang (2000)](https://ieeexplore.ieee.org/document/888718): camera intrinsic — flexible homography-based calibration
- [Furgale et al. (2013)](https://ieeexplore.ieee.org/document/6696514): camera-IMU — simultaneous spatio-temporal calibration with B-spline continuous-time trajectories
- [Tsai & Lenz (1989)](https://ieeexplore.ieee.org/document/34770): hand-eye — the original AX=XB
- [Koide et al. (2023)](https://arxiv.org/abs/2302.05094): camera-LiDAR targetless — automatic calibration based on NID + SuperGlue
- [Li & Mourikis (2014)](https://journals.sagepub.com/doi/abs/10.1177/0278364913515286): temporal — online estimation of the time offset
- [OpenCalib (2023)](https://arxiv.org/abs/2205.14087): an integrated calibration framework for autonomous driving
- [GRIL-Calib (Kim et al., 2024)](https://arxiv.org/abs/2312.14035): targetless IMU-LiDAR calibration in ground-robot settings using planar-motion constraints. 6-DoF estimation is possible even from constrained motion.
- [MFCalib (2024)](https://arxiv.org/abs/2409.00992): single-shot targetless LiDAR-camera calibration that leverages multi-feature edges (depth continuity/discontinuity, intensity discontinuity). The edge inflation problem is solved with a LiDAR beam model.
- [iKalibr (Chen et al., 2024)](https://arxiv.org/abs/2407.11420): temporal — unified spatio-temporal calibration of heterogeneous multi-sensors (LiDAR, camera, IMU, radar) based on B-spline continuous time (IEEE T-RO 2025).

---

# Ch.4 — State Estimation Theory

Ch.2 covered sensor observation models, and Ch.3 addressed inter-sensor calibration. It is now time for the **algorithms** that estimate the robot's state (position, attitude, velocity, etc.) from this observation data. The Kalman filter, factor graph, and IMU preintegration covered in this chapter are the mathematical engines behind every odometry and fusion system we examine in Ch.6–8.

> **Goal**: We systematically treat state estimation theory, the mathematical foundation of sensor fusion. Starting from Bayesian filtering, we follow the technical lineage through the Kalman filter family and the particle filter, up to factor-graph-based optimization, which is at the heart of modern SLAM. We show each method's derivation in detail and explain why modern robotics systems moved from filtering to optimization.

---

## 4.1 Bayesian Filtering Framework

### 4.1.1 Definition of the State Estimation Problem

The robot state estimation problem is fundamentally a **conditional probabilistic inference** problem. What we wish to know is the posterior over the state $\mathbf{x}_k$ given all observations up to the present $\mathbf{z}_{1:k}$ and control inputs $\mathbf{u}_{1:k}$:

$$p(\mathbf{x}_k \mid \mathbf{z}_{1:k}, \mathbf{u}_{1:k})$$

Here:
- $\mathbf{x}_k \in \mathbb{R}^n$: state vector at time $k$ (position, velocity, attitude, bias, etc.)
- $\mathbf{z}_k \in \mathbb{R}^m$: observation vector at time $k$ (camera, LiDAR, IMU, etc.)
- $\mathbf{u}_k \in \mathbb{R}^l$: control input at time $k$

To solve this problem we define two models:

**Motion Model (Process Model)**:
$$\mathbf{x}_k = f(\mathbf{x}_{k-1}, \mathbf{u}_k) + \mathbf{w}_k, \quad \mathbf{w}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_k)$$

$$p(\mathbf{x}_k \mid \mathbf{x}_{k-1}, \mathbf{u}_k)$$

This represents the transition probability from the previous state to the current state given the control input $\mathbf{u}_k$. The process noise $\mathbf{w}_k$ reflects the model's uncertainty.

**Observation Model (Measurement Model)**:
$$\mathbf{z}_k = h(\mathbf{x}_k) + \mathbf{v}_k, \quad \mathbf{v}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{R}_k)$$

$$p(\mathbf{z}_k \mid \mathbf{x}_k)$$

This is the likelihood of the observation given the current state. The measurement noise $\mathbf{v}_k$ reflects the sensor's uncertainty.

Intuitively, the robot cannot know exactly "where it is," but by combining "how it moved" (motion model) and "what it sees" (observation model), it progressively refines a **belief** about its own position.

### 4.1.2 Markov Assumptions and Recursive Estimation

For practical filtering, we introduce the **Markov assumptions**:

1. **First-order Markov process**: The current state $\mathbf{x}_k$ depends only on the immediately previous state $\mathbf{x}_{k-1}$ and the control $\mathbf{u}_k$; it is conditionally independent of earlier states and observations:
$$p(\mathbf{x}_k \mid \mathbf{x}_{0:k-1}, \mathbf{u}_{1:k}, \mathbf{z}_{1:k-1}) = p(\mathbf{x}_k \mid \mathbf{x}_{k-1}, \mathbf{u}_k)$$

2. **Conditional observation independence**: Given the current state $\mathbf{x}_k$, the observation $\mathbf{z}_k$ is independent of everything else:
$$p(\mathbf{z}_k \mid \mathbf{x}_{0:k}, \mathbf{u}_{1:k}, \mathbf{z}_{1:k-1}) = p(\mathbf{z}_k \mid \mathbf{x}_k)$$

Thanks to these two assumptions, we can update the current estimate using only the previous estimate, without storing the entire observation history $\mathbf{z}_{1:k}$. This is the crux of **recursive estimation**.

### 4.1.3 Prediction-Update Cycle

The Bayesian filter iterates two steps:

**Prediction Step**: Using the motion model, predict the current prior from the previous posterior.

This integral is the **Chapman-Kolmogorov equation**. Its physical meaning is:
- $p(\mathbf{x}_{k-1} \mid \mathbf{z}_{1:k-1})$: posterior at time $k-1$ (result of the previous step)
- $p(\mathbf{x}_k \mid \mathbf{x}_{k-1}, \mathbf{u}_k)$: state transition probability (motion model)
- The predicted distribution is obtained by a weighted average of the transition probability over all possible values of the previous state.

**Update Step**: When a new observation $\mathbf{z}_k$ arrives, update the posterior via Bayes' rule.

Here:
- Numerator $p(\mathbf{z}_k \mid \mathbf{x}_k)$: likelihood (observation model)
- Numerator $p(\mathbf{x}_k \mid \mathbf{z}_{1:k-1})$: prior obtained in the prediction step
- Denominator $p(\mathbf{z}_k \mid \mathbf{z}_{1:k-1}) = \int p(\mathbf{z}_k \mid \mathbf{x}_k) p(\mathbf{x}_k \mid \mathbf{z}_{1:k-1}) d\mathbf{x}_k$: normalization constant (evidence)

Intuitively, each time an observation arrives, we ask "how consistent is this observation with my prediction?" and revise our belief accordingly.

### 4.1.4 Why Closed-Form Solutions Fail

Although the prediction-update formulation above is theoretically perfect, in practice the Chapman-Kolmogorov integral $\int p(\mathbf{x}_k \mid \mathbf{x}_{k-1}) p(\mathbf{x}_{k-1} \mid \mathbf{z}_{1:k-1}) d\mathbf{x}_{k-1}$ cannot be solved analytically in most cases. The reasons are:

1. **Nonlinear motion/observation models**: If $f(\cdot)$ and $h(\cdot)$ are nonlinear, propagating a Gaussian distribution does not preserve Gaussianity. The posterior can take arbitrarily complex shapes.

2. **High-dimensional state spaces**: As the dimensionality of the state vector grows, the computational cost of the integral grows exponentially (curse of dimensionality).

3. **Multimodal distributions**: When the robot could be at multiple locations (e.g., global localization in a symmetric environment), the posterior becomes multimodal.

Therefore, approximations are essential in practice, and different filters arise depending on the approximation:

| Approximation | Filter | Distribution representation | Regime |
|----------|------|---------|----------|
| Linear-Gaussian assumption | Kalman Filter | $\mathcal{N}(\hat{\mathbf{x}}, \mathbf{P})$ | Linear systems |
| First-order linearization + Gaussian | EKF, ESKF | $\mathcal{N}(\hat{\mathbf{x}}, \mathbf{P})$ | Mildly nonlinear |
| Sigma point transform | UKF | $\mathcal{N}(\hat{\mathbf{x}}, \mathbf{P})$ | Moderately nonlinear |
| Sample-based | Particle Filter | $\{(\mathbf{x}^{(i)}, w^{(i)})\}_{i=1}^N$ | Strongly nonlinear, multimodal |
| Iterated linearization | IEKF | $\mathcal{N}(\hat{\mathbf{x}}, \mathbf{P})$ | Strongly nonlinear |

We derive each method in detail in what follows.

---

## 4.2 Kalman Filter Family

### 4.2.1 Kalman Filter: Derivation and Optimality

The Kalman Filter (KF) is the exact solution of the Bayesian filter under the assumptions of a **linear system + Gaussian noise**. Rudolf Kalman introduced it in his 1960 paper ["A New Approach to Linear Filtering and Prediction Problems"](https://doi.org/10.1115/1.3662552), and its practicality was established when it was applied to orbit estimation in the Apollo program.

#### Linear System Model

We assume a system with linear state transition and observation:

Here:
- $\mathbf{F}_k \in \mathbb{R}^{n \times n}$: state transition matrix
- $\mathbf{B}_k \in \mathbb{R}^{n \times l}$: control input matrix
- $\mathbf{H}_k \in \mathbb{R}^{m \times n}$: observation matrix
- $\mathbf{Q}_k \in \mathbb{R}^{n \times n}$: process noise covariance (positive semi-definite, symmetric)
- $\mathbf{R}_k \in \mathbb{R}^{m \times m}$: measurement noise covariance (positive definite, symmetric)

Key property: applying a linear transformation to a Gaussian yields another Gaussian. Thus the posterior remains Gaussian at all times and is fully described by its mean and covariance.

#### MMSE Derivation — Why the Kalman Filter Is Optimal

We derive the Kalman filter's optimality from the Minimum Mean Square Error (MMSE) perspective. What we want is the estimator that minimizes the mean squared error:

$$\hat{\mathbf{x}}_k = \arg\min_{\hat{\mathbf{x}}} \mathbb{E}[\|\mathbf{x}_k - \hat{\mathbf{x}}\|^2 \mid \mathbf{z}_{1:k}]$$

The MMSE estimator is the conditional expectation $\hat{\mathbf{x}}_k = \mathbb{E}[\mathbf{x}_k \mid \mathbf{z}_{1:k}]$. We show that in a linear-Gaussian system this coincides exactly with the Kalman filter's state update equations.

**Bayesian derivation**: Assume the posterior at time $k-1$ is Gaussian:

$$p(\mathbf{x}_{k-1} \mid \mathbf{z}_{1:k-1}) = \mathcal{N}(\hat{\mathbf{x}}_{k-1|k-1}, \mathbf{P}_{k-1|k-1})$$

**Step 1: Prediction — evaluate the Chapman-Kolmogorov integral**

Under the linear motion model:
$$\mathbf{x}_k = \mathbf{F}_k \mathbf{x}_{k-1} + \mathbf{B}_k \mathbf{u}_k + \mathbf{w}_k$$

Since $\mathbf{x}_{k-1}$ is Gaussian and $\mathbf{w}_k$ is independent Gaussian, $\mathbf{x}_k$ is also Gaussian. Computing its mean and covariance:

$$\hat{\mathbf{x}}_{k|k-1} = \mathbb{E}[\mathbf{x}_k \mid \mathbf{z}_{1:k-1}] = \mathbf{F}_k \hat{\mathbf{x}}_{k-1|k-1} + \mathbf{B}_k \mathbf{u}_k$$

The covariance follows from $\tilde{\mathbf{x}}_{k|k-1} = \mathbf{x}_k - \hat{\mathbf{x}}_{k|k-1} = \mathbf{F}_k \tilde{\mathbf{x}}_{k-1|k-1} + \mathbf{w}_k$:

$$\mathbf{P}_{k|k-1} = \mathbb{E}[\tilde{\mathbf{x}}_{k|k-1} \tilde{\mathbf{x}}_{k|k-1}^\top] = \mathbf{F}_k \mathbf{P}_{k-1|k-1} \mathbf{F}_k^\top + \mathbf{Q}_k$$

Predicted distribution: $p(\mathbf{x}_k \mid \mathbf{z}_{1:k-1}) = \mathcal{N}(\hat{\mathbf{x}}_{k|k-1}, \mathbf{P}_{k|k-1})$.

**Step 2: Update — apply Bayes' rule**

From the observation model $\mathbf{z}_k = \mathbf{H}_k \mathbf{x}_k + \mathbf{v}_k$, construct the joint distribution of the predicted state and the observation:

Here $\mathbf{S}_k = \mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^\top + \mathbf{R}_k \in \mathbb{R}^{m \times m}$ is the innovation covariance.

Applying the formula for the conditional of a Gaussian joint (the key Gaussian property: if the joint is Gaussian, the conditional is also Gaussian, and the conditional mean equals the original mean plus a correction proportional to the correlation with the observation):

$$\hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + \mathbf{P}_{k|k-1} \mathbf{H}_k^\top \mathbf{S}_k^{-1} (\mathbf{z}_k - \mathbf{H}_k \hat{\mathbf{x}}_{k|k-1})$$

Defining the **Kalman gain** $\mathbf{K}_k \in \mathbb{R}^{n \times m}$:

$$\boxed{\mathbf{K}_k = \mathbf{P}_{k|k-1} \mathbf{H}_k^\top \mathbf{S}_k^{-1} = \mathbf{P}_{k|k-1} \mathbf{H}_k^\top (\mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^\top + \mathbf{R}_k)^{-1}}$$

Intuitive meaning of the Kalman gain:
- $\mathbf{R}_k \to \mathbf{0}$ (observation is very accurate): $\mathbf{K}_k \to \mathbf{H}_k^{-1}$ → trust the observation almost entirely
- $\mathbf{P}_{k|k-1} \to \mathbf{0}$ (prediction is very accurate): $\mathbf{K}_k \to \mathbf{0}$ → ignore the observation and trust the prediction
- The Kalman gain automatically determines the **optimal weighting** between the uncertainty of prediction and that of observation.

**Innovation**:
$$\tilde{\mathbf{y}}_k = \mathbf{z}_k - \mathbf{H}_k \hat{\mathbf{x}}_{k|k-1} \in \mathbb{R}^m$$

The difference between the predicted observation and the actual one. If it is zero, the prediction was perfect.

**State update**:
$$\boxed{\hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_k \tilde{\mathbf{y}}_k}$$

**Covariance update**:
$$\boxed{\mathbf{P}_{k|k} = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k|k-1}}$$

This covariance update is numerically more stable when written in the Joseph form $\mathbf{P}_{k|k} = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k|k-1} (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k)^\top + \mathbf{K}_k \mathbf{R}_k \mathbf{K}_k^\top$. Even if $\mathbf{K}_k$ has numerical errors, the Joseph form guarantees that $\mathbf{P}_{k|k}$ remains symmetric and positive semi-definite.

#### KF Optimality Theorem

**Theorem**: In a linear-Gaussian system, the Kalman filter is optimal in the following three senses:
1. **MMSE (Minimum Mean Square Error) estimator**: minimizes the mean squared error
2. **MAP (Maximum A Posteriori) estimator**: in a Gaussian, mean equals mode, so MAP and MMSE coincide
3. **BLUE (Best Linear Unbiased Estimator)**: even without the Gaussian assumption, it is the linear unbiased estimator with minimum variance

Kalman's 1960 paper replaced Wiener's frequency-domain approach with a state-space, time-domain formulation, enabling natural extensions to time-varying and multivariate systems — an innovation of far-reaching consequences.

#### Python Implementation

```python
import numpy as np

class KalmanFilter:
    """Linear Kalman Filter implementation.
    
    State-space model:
        x_k = F @ x_{k-1} + B @ u_k + w_k,  w_k ~ N(0, Q)
        z_k = H @ x_k + v_k,                 v_k ~ N(0, R)
    """
    def __init__(self, F, H, Q, R, B=None):
        """
        Parameters
        ----------
        F : ndarray, shape (n, n) — state transition matrix
        H : ndarray, shape (m, n) — observation matrix
        Q : ndarray, shape (n, n) — process noise covariance
        R : ndarray, shape (m, m) — measurement noise covariance
        B : ndarray, shape (n, l) — control input matrix (optional)
        """
        self.F = F
        self.H = H
        self.Q = Q
        self.R = R
        self.B = B
        self.n = F.shape[0]
        
    def predict(self, x, P, u=None):
        """Prediction step.
        
        Parameters
        ----------
        x : ndarray, shape (n,) — previous state estimate
        P : ndarray, shape (n, n) — previous covariance
        u : ndarray, shape (l,) — control input (optional)
        
        Returns
        -------
        x_pred : ndarray, shape (n,) — predicted state
        P_pred : ndarray, shape (n, n) — predicted covariance
        """
        x_pred = self.F @ x
        if self.B is not None and u is not None:
            x_pred += self.B @ u
        P_pred = self.F @ P @ self.F.T + self.Q
        return x_pred, P_pred
    
    def update(self, x_pred, P_pred, z):
        """Update step.
        
        Parameters
        ----------
        x_pred : ndarray, shape (n,) — predicted state
        P_pred : ndarray, shape (n, n) — predicted covariance
        z : ndarray, shape (m,) — observation
        
        Returns
        -------
        x_upd : ndarray, shape (n,) — updated state
        P_upd : ndarray, shape (n, n) — updated covariance
        """
        # innovation
        y = z - self.H @ x_pred                          # (m,)
        # innovation covariance
        S = self.H @ P_pred @ self.H.T + self.R           # (m, m)
        # Kalman gain
        K = P_pred @ self.H.T @ np.linalg.inv(S)          # (n, m)
        # state update
        x_upd = x_pred + K @ y                            # (n,)
        # covariance update (Joseph form for numerical stability)
        I_KH = np.eye(self.n) - K @ self.H                # (n, n)
        P_upd = I_KH @ P_pred @ I_KH.T + K @ self.R @ K.T  # (n, n)
        return x_upd, P_upd

# Example: position estimation under a 1D constant-velocity model
# state: [position, velocity]^T
dt = 0.1  # time step
F = np.array([[1, dt],
              [0, 1]])      # (2, 2) constant-velocity transition matrix
H = np.array([[1, 0]])       # (1, 2) observe position only
Q = np.array([[dt**3/3, dt**2/2],
              [dt**2/2, dt]]) * 0.1  # (2, 2) process noise (constant-acceleration model)
R = np.array([[1.0]])         # (1, 1) observation noise variance

kf = KalmanFilter(F, H, Q, R)

# initial state
x = np.array([0.0, 1.0])  # position=0, velocity=1
P = np.eye(2) * 10.0       # large initial uncertainty

# simulation
np.random.seed(42)
true_positions = []
estimated_positions = []

for k in range(100):
    # ground truth
    true_pos = 0.0 + 1.0 * k * dt  # constant-velocity motion
    true_positions.append(true_pos)
    
    # predict
    x, P = kf.predict(x, P)
    
    # noisy observation
    z = np.array([true_pos + np.random.randn() * 1.0])
    
    # update
    x, P = kf.update(x, P, z)
    estimated_positions.append(x[0])

print(f"final position estimate: {x[0]:.3f}, true: {true_positions[-1]:.3f}")
print(f"final position uncertainty (1 sigma): {np.sqrt(P[0,0]):.3f}")
```

### 4.2.2 Extended Kalman Filter (EKF)

Real robotic systems are almost always nonlinear. The 3D→2D projection of a camera, the quaternion-based rotation of an IMU, LiDAR scan matching — all are nonlinear functions. The EKF is the most direct way to extend the Kalman filter to nonlinear systems.

#### Core Idea: First-Order Taylor Expansion (Linearization)

Nonlinear system model:
$$\mathbf{x}_k = f(\mathbf{x}_{k-1}, \mathbf{u}_k) + \mathbf{w}_k$$
$$\mathbf{z}_k = h(\mathbf{x}_k) + \mathbf{v}_k$$

In this system, passing a Gaussian through a nonlinear function no longer yields a Gaussian. The EKF's key approximation is to **linearize the function about the current estimate via a first-order Taylor expansion**.

Linearization of the motion model (about the estimate $\hat{\mathbf{x}}_{k-1|k-1}$):

$$f(\mathbf{x}_{k-1}, \mathbf{u}_k) \approx f(\hat{\mathbf{x}}_{k-1|k-1}, \mathbf{u}_k) + \mathbf{F}_k (\mathbf{x}_{k-1} - \hat{\mathbf{x}}_{k-1|k-1})$$

$$\mathbf{F}_k = \left.\frac{\partial f}{\partial \mathbf{x}}\right|_{\hat{\mathbf{x}}_{k-1|k-1}, \mathbf{u}_k} \in \mathbb{R}^{n \times n}$$

Linearization of the observation model (about the predicted estimate $\hat{\mathbf{x}}_{k|k-1}$):

$$h(\mathbf{x}_k) \approx h(\hat{\mathbf{x}}_{k|k-1}) + \mathbf{H}_k (\mathbf{x}_k - \hat{\mathbf{x}}_{k|k-1})$$

$$\mathbf{H}_k = \left.\frac{\partial h}{\partial \mathbf{x}}\right|_{\hat{\mathbf{x}}_{k|k-1}} \in \mathbb{R}^{m \times n}$$

#### EKF Algorithm

**Prediction step**:
$$\hat{\mathbf{x}}_{k|k-1} = f(\hat{\mathbf{x}}_{k-1|k-1}, \mathbf{u}_k)$$
$$\mathbf{P}_{k|k-1} = \mathbf{F}_k \mathbf{P}_{k-1|k-1} \mathbf{F}_k^\top + \mathbf{Q}_k$$

Note: the state prediction uses the nonlinear function $f$ directly, while the covariance propagation uses the Jacobian $\mathbf{F}_k$.

**Update step**:
$$\tilde{\mathbf{y}}_k = \mathbf{z}_k - h(\hat{\mathbf{x}}_{k|k-1})$$
$$\mathbf{S}_k = \mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^\top + \mathbf{R}_k$$
$$\mathbf{K}_k = \mathbf{P}_{k|k-1} \mathbf{H}_k^\top \mathbf{S}_k^{-1}$$
$$\hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_k \tilde{\mathbf{y}}_k$$
$$\mathbf{P}_{k|k} = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k|k-1}$$

Observe that the innovation $\tilde{\mathbf{y}}_k$ also uses the nonlinear function $h$ directly.

#### Limitations of the EKF

1. **Linearization error**: The stronger the nonlinearity, the larger the error of the first-order approximation. This can hurt the filter's consistency — the actual error may be larger than the uncertainty the filter reports.

2. **Jacobian computation burden**: We must compute the analytical derivatives $\mathbf{F}_k$ and $\mathbf{H}_k$ at every time step. When the system is complex, the Jacobian derivation becomes cumbersome and error-prone.

3. **Unimodal assumption**: Gaussians are always unimodal, so multimodal posteriors cannot be represented.

### 4.2.3 Error-State Kalman Filter (ESKF)

The ESKF (Error-State Kalman Filter) is the most widely used filter form in modern robotic sensor fusion systems. Nearly all major VIO/LIO systems — MSCKF, VINS-Mono, OpenVINS, FAST-LIO, and others — adopt the ESKF.

#### Why ESKF Instead of EKF

When estimating a robot state — especially one that includes 3D orientation — using the EKF directly causes several problems:

**Problem 1: Non-Euclidean nature of rotation**

3D rotation lives on the SO(3) manifold, not $\mathbb{R}^n$. Representing it with a quaternion $\mathbf{q} \in \mathbb{H}$ imposes $\|\mathbf{q}\| = 1$; representing it with a rotation matrix $\mathbf{R} \in SO(3)$ imposes $\mathbf{R}^\top \mathbf{R} = \mathbf{I}$, $\det(\mathbf{R}) = 1$.

In the EKF state update $\hat{\mathbf{x}} \leftarrow \hat{\mathbf{x}} + \mathbf{K} \tilde{\mathbf{y}}$, the "+" is Euclidean addition. Adding a vector to a quaternion breaks the unit norm. Re-normalizing after the update is a makeshift fix that is not theoretically correct and leads to consistency problems.

**Problem 2: The error state is "almost zero"**

The error state $\delta\mathbf{x} = \mathbf{x} - \hat{\mathbf{x}}$ stays near zero by construction (it is reset every update). Therefore the first-order linearization is extremely accurate. In contrast, linearizing about the original state becomes inaccurate when motion is large.

**Problem 3: Separation of slow-varying and fast-varying states**

Separating slow-varying states such as IMU biases from fast-varying states such as velocity and attitude allows update strategies tailored to each.

#### ESKF Structure

The ESKF maintains two states simultaneously:

1. **Nominal state** $\hat{\mathbf{x}}$: integrated through the nonlinear motion model without noise terms. It does not track uncertainty.

2. **Error state** $\delta\mathbf{x}$: the difference between the nominal state and the true state. It is estimated with a Kalman filter. Since the error state is a "small value" by construction, linearization error is minimized.

The true state is recovered by composition of the two:

$$\mathbf{x}_{\text{true}} = \hat{\mathbf{x}} \boxplus \delta\mathbf{x}$$

Here $\boxplus$ is the composition operation on the manifold. For Euclidean components it is ordinary addition; for rotation components it is:

$$\mathbf{R}_{\text{true}} = \hat{\mathbf{R}} \cdot \text{Exp}(\delta\boldsymbol{\theta})$$

or, in quaternion form:

$$\mathbf{q}_{\text{true}} = \hat{\mathbf{q}} \otimes \begin{bmatrix} 1 \\ \frac{1}{2}\delta\boldsymbol{\theta} \end{bmatrix} \approx \hat{\mathbf{q}} \otimes \delta\mathbf{q}$$

Here $\delta\boldsymbol{\theta} \in \mathbb{R}^3$ is the angle-axis representation of the rotational error.

#### State Vector of an IMU-Based ESKF

Typical state vector in an IMU-camera/LiDAR fusion system:

**Nominal state** (16-dimensional, with a quaternion):
$$\hat{\mathbf{x}} = \begin{bmatrix} {}^W\hat{\mathbf{p}} \\ {}^W\hat{\mathbf{v}} \\ \hat{\mathbf{q}}_{WB} \\ \hat{\mathbf{b}}_a \\ \hat{\mathbf{b}}_g \end{bmatrix} \in \mathbb{R}^{3} \times \mathbb{R}^{3} \times \mathbb{S}^3 \times \mathbb{R}^{3} \times \mathbb{R}^{3}$$

**Error state** (15-dimensional — minimal parameterization of rotation):
$$\delta\mathbf{x} = \begin{bmatrix} \delta\mathbf{p} \\ \delta\mathbf{v} \\ \delta\boldsymbol{\theta} \\ \delta\mathbf{b}_a \\ \delta\mathbf{b}_g \end{bmatrix} \in \mathbb{R}^{15}$$

The key point: the error representation of the quaternion (4D) is the 3D vector $\delta\boldsymbol{\theta}$. Because of the unit-quaternion constraint the actual DoF is 3, and the ESKF naturally uses this minimal parameterization. Putting the quaternion directly into the state of an EKF introduces one redundant DoF in its 4D representation, which makes the covariance matrix singular.

#### ESKF Algorithm

**Step 1: Nominal state propagation (IMU mechanization)**

Integrate the nominal state from the IMU measurements $(\tilde{\boldsymbol{\omega}}_k, \tilde{\mathbf{a}}_k)$. Subtract the biases and ignore the noise terms:

Here $\hat{\mathbf{R}}_k = \mathbf{R}(\hat{\mathbf{q}}_k) \in SO(3)$, and $\mathbf{g} = [0, 0, -9.81]^\top \, \text{m/s}^2$ is the gravity vector.

**Step 2: Error-state propagation (prediction)**

We derive the continuous-time dynamics of the error state. Substituting the true IMU measurements $\tilde{\boldsymbol{\omega}} = \boldsymbol{\omega} + \mathbf{b}_g + \mathbf{n}_g$, $\tilde{\mathbf{a}} = \mathbf{R}^\top(\mathbf{a}_W - \mathbf{g}) + \mathbf{b}_a + \mathbf{n}_a$, and subtracting the nominal state:

Here $[\mathbf{a}]_\times$ is the skew-symmetric matrix of the vector $\mathbf{a}$:

$$[\mathbf{a}]_\times = \begin{bmatrix} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{bmatrix} \in \mathbb{R}^{3 \times 3}$$

In matrix form:

$$\delta\dot{\mathbf{x}} = \mathbf{F}_c \delta\mathbf{x} + \mathbf{G}_c \mathbf{n}$$

Discretization (first-order approximation): $\mathbf{F}_d \approx \mathbf{I} + \mathbf{F}_c \Delta t$.

$$\mathbf{P}_{k+1|k} = \mathbf{F}_d \mathbf{P}_{k|k} \mathbf{F}_d^\top + \mathbf{G}_d \mathbf{Q}_d \mathbf{G}_d^\top$$

**Step 3: Observation update**

When a camera/LiDAR observation arrives, perform a standard EKF update on the error state. Linearize the observation model $\mathbf{z} = h(\mathbf{x}_{\text{true}})$ with respect to the error state:

$$\mathbf{z} - h(\hat{\mathbf{x}}) \approx \mathbf{H} \delta\mathbf{x} + \mathbf{v}$$

$$\mathbf{H} = \frac{\partial h}{\partial \delta\mathbf{x}}\bigg|_{\hat{\mathbf{x}}} \in \mathbb{R}^{m \times 15}$$

This Jacobian is computed by the chain rule:

$$\mathbf{H} = \frac{\partial h}{\partial \mathbf{x}_{\text{true}}} \cdot \frac{\partial \mathbf{x}_{\text{true}}}{\partial \delta\mathbf{x}}\bigg|_{\delta\mathbf{x}=\mathbf{0}}$$

Standard Kalman update:
$$\mathbf{K} = \mathbf{P}_{k|k-1} \mathbf{H}^\top (\mathbf{H} \mathbf{P}_{k|k-1} \mathbf{H}^\top + \mathbf{R})^{-1}$$
$$\delta\hat{\mathbf{x}} = \mathbf{K}(\mathbf{z} - h(\hat{\mathbf{x}}))$$
$$\mathbf{P}_{k|k} = (\mathbf{I} - \mathbf{K}\mathbf{H})\mathbf{P}_{k|k-1}$$

**Step 4: Injection of the error state and reset**

Inject the error-state estimate $\delta\hat{\mathbf{x}}$ into the nominal state:

After injection, reset the error state to $\delta\hat{\mathbf{x}} \leftarrow \mathbf{0}$. The covariance must also be transformed by the reset Jacobian:

$$\mathbf{P} \leftarrow \mathbf{G} \mathbf{P} \mathbf{G}^\top$$

Here $\mathbf{G} = \frac{\partial (\delta\mathbf{x} \boxminus \delta\hat{\mathbf{x}})}{\partial \delta\mathbf{x}}\big|_{\delta\hat{\mathbf{x}}}$. In practice, when $\delta\hat{\boldsymbol{\theta}}$ is small, we often approximate $\mathbf{G} \approx \mathbf{I}$.

### 4.2.4 Unscented Kalman Filter (UKF)

The UKF rests on the insight that "approximating a distribution is easier than approximating a nonlinear function." The EKF approximates nonlinear functions by linearizing them, whereas the UKF leaves the nonlinear function intact and approximates the distribution with a finite set of **sigma points**.

#### Unscented Transform

For an $n$-dimensional Gaussian random variable $\mathbf{x} \sim \mathcal{N}(\hat{\mathbf{x}}, \mathbf{P})$, we generate $2n+1$ sigma points and weights:

Here:
- $\lambda = \alpha^2(n + \kappa) - n$ is a scaling parameter ($\alpha$: controls the sigma-point spread; $\kappa$: auxiliary parameter, typically $\kappa = 0$ or $3-n$)
- $\left(\sqrt{(n+\lambda)\mathbf{P}}\right)_i$ is the $i$-th column of the Cholesky factor of $(n+\lambda)\mathbf{P}$

Pass each sigma point through the nonlinear function:

$$\boldsymbol{\gamma}_i = f(\boldsymbol{\chi}_i)$$

Recover the mean and covariance from the transformed sigma points:

The weights $w_i^{(m)}$ and $w_i^{(c)}$ are used for the mean and covariance respectively; $w_0^{(c)} = w_0^{(m)} + (1 - \alpha^2 + \beta)$ ($\beta = 2$ is optimal for Gaussians), and the remaining weights are identical.

#### UKF Algorithm

**Prediction step**:
1. Generate sigma points from the current state $(\hat{\mathbf{x}}_{k-1|k-1}, \mathbf{P}_{k-1|k-1})$
2. Pass each sigma point through the motion model: $\boldsymbol{\chi}_{k|k-1}^{(i)} = f(\boldsymbol{\chi}_{k-1|k-1}^{(i)}, \mathbf{u}_k)$
3. Compute the predicted mean and covariance: $\hat{\mathbf{x}}_{k|k-1} = \sum w_i^{(m)} \boldsymbol{\chi}_{k|k-1}^{(i)}$
4. $\mathbf{P}_{k|k-1} = \sum w_i^{(c)} (\boldsymbol{\chi}_{k|k-1}^{(i)} - \hat{\mathbf{x}}_{k|k-1})(\cdots)^\top + \mathbf{Q}_k$

**Update step**:
1. Regenerate sigma points from the predicted state (or reuse those from the prediction step)
2. Pass them through the observation model: $\boldsymbol{\zeta}_k^{(i)} = h(\boldsymbol{\chi}_{k|k-1}^{(i)})$
3. Predicted observation mean: $\hat{\mathbf{z}}_k = \sum w_i^{(m)} \boldsymbol{\zeta}_k^{(i)}$
4. Observation covariance: $\mathbf{P}_{zz} = \sum w_i^{(c)} (\boldsymbol{\zeta}_k^{(i)} - \hat{\mathbf{z}}_k)(\cdots)^\top + \mathbf{R}_k$
5. Cross-covariance: $\mathbf{P}_{xz} = \sum w_i^{(c)} (\boldsymbol{\chi}_{k|k-1}^{(i)} - \hat{\mathbf{x}}_{k|k-1})(\boldsymbol{\zeta}_k^{(i)} - \hat{\mathbf{z}}_k)^\top$
6. Kalman gain: $\mathbf{K}_k = \mathbf{P}_{xz} \mathbf{P}_{zz}^{-1}$
7. Update: $\hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_k (\mathbf{z}_k - \hat{\mathbf{z}}_k)$
8. $\mathbf{P}_{k|k} = \mathbf{P}_{k|k-1} - \mathbf{K}_k \mathbf{P}_{zz} \mathbf{K}_k^\top$

#### Pros and Cons of the UKF

**Pros**:
- No Jacobian computation. Big advantage for complex observation models (e.g., camera projection with distortion).
- Captures nonlinearity up to second order exactly (EKF only up to first order).
- Can be simpler to implement than the EKF (function calls replace Jacobian derivation).

**Cons**:
- Each of the $2n+1$ sigma points must be pushed through the nonlinear function, so the computational cost grows with the state dimension $n$.
- Handling manifold states (e.g., SO(3)) requires replacing sigma-point generation and statistical aggregation with manifold operations, which is not clean.
- Why the ESKF tends to be preferred over the UKF in practice: the ESKF already operates on the error state (a small value), so first-order linearization is accurate enough, manifolds are handled naturally, and the cost is lower.

### 4.2.5 Iterated Extended Kalman Filter (IEKF)

The IEKF improves the accuracy of nonlinear observation handling by relinearizing iteratively in the update step, instead of linearizing only once.

#### Motivation: Linearization Error in the EKF Update

In the EKF, the Jacobian $\mathbf{H}_k$ of the observation model is computed at the predicted estimate $\hat{\mathbf{x}}_{k|k-1}$. However, if the post-update estimate $\hat{\mathbf{x}}_{k|k}$ differs significantly from $\hat{\mathbf{x}}_{k|k-1}$, the linearization point is no longer optimal. The IEKF mitigates this by relinearizing at the post-update estimate and repeating the update.

#### IEKF Algorithm

The prediction step is identical to the EKF. In the update step we iterate:

Initialization: $\hat{\mathbf{x}}^{(0)} = \hat{\mathbf{x}}_{k|k-1}$

For $j = 0, 1, 2, \ldots$ until convergence:

$$\mathbf{H}^{(j)} = \left.\frac{\partial h}{\partial \mathbf{x}}\right|_{\hat{\mathbf{x}}^{(j)}}$$

$$\mathbf{K}^{(j)} = \mathbf{P}_{k|k-1} \mathbf{H}^{(j)\top} (\mathbf{H}^{(j)} \mathbf{P}_{k|k-1} \mathbf{H}^{(j)\top} + \mathbf{R}_k)^{-1}$$

$$\hat{\mathbf{x}}^{(j+1)} = \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}^{(j)} \left[\mathbf{z}_k - h(\hat{\mathbf{x}}^{(j)}) - \mathbf{H}^{(j)}(\hat{\mathbf{x}}_{k|k-1} - \hat{\mathbf{x}}^{(j)})\right]$$

After convergence: $\hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}^{(j+1)}$, $\mathbf{P}_{k|k} = (\mathbf{I} - \mathbf{K}^{(j)} \mathbf{H}^{(j)}) \mathbf{P}_{k|k-1}$.

The IEKF is effectively equivalent to performing **Gauss-Newton optimization** in the observation update step. This perspective is important for understanding the connection to factor-graph-based optimization that we establish later.

Why FAST-LIO2 adopts the IEKF: the LiDAR point-to-plane/point-to-edge observation model is strongly nonlinear, and hundreds to thousands of points must be updated together. In such settings the IEKF's iterated linearization yields a considerably more accurate result than a single EKF update.

---

## 4.3 Particle Filter

### 4.3.1 Overview of Sequential Monte Carlo (SMC)

The particle filter (PF), or Sequential Monte Carlo (SMC) method, represents the posterior as a set of weighted samples (particles). Because it does not require the Gaussian assumption, it can handle multimodal distributions and strongly nonlinear systems.

Particle approximation of the posterior:

$$p(\mathbf{x}_k \mid \mathbf{z}_{1:k}) \approx \sum_{i=1}^{N} w_k^{(i)} \delta(\mathbf{x}_k - \mathbf{x}_k^{(i)})$$

Here:
- $N$: number of particles
- $\mathbf{x}_k^{(i)}$: state of the $i$-th particle
- $w_k^{(i)}$: weight of the $i$-th particle ($\sum_{i=1}^N w_k^{(i)} = 1$)
- $\delta(\cdot)$: Dirac delta function

As $N \to \infty$, the particle distribution converges to the true posterior. In practice we approximate with a finite number of particles; the number is chosen based on the complexity of the problem and the dimensionality of the state space.

### 4.3.2 Importance Sampling

Sampling directly from the posterior $p(\mathbf{x}_k \mid \mathbf{z}_{1:k})$ is generally impossible. Instead, we draw samples from a **proposal distribution** $q(\mathbf{x}_k \mid \mathbf{x}_{k-1}, \mathbf{z}_k)$ and correct them with importance weights.

We derive the recursive weight update. From Bayes' rule:

Dividing by the proposal gives the importance ratio:

**Simplest proposal**: use the transition prior as the proposal, i.e., $q(\mathbf{x}_k \mid \mathbf{x}_{k-1}, \mathbf{z}_k) = p(\mathbf{x}_k \mid \mathbf{x}_{k-1})$. The weights then simplify:

$$w_k^{(i)} \propto w_{k-1}^{(i)} \cdot p(\mathbf{z}_k \mid \mathbf{x}_k^{(i)})$$

Each particle's weight is proportional to the observation likelihood at that particle's location. Intuitively, particles consistent with the observation receive high weights, while inconsistent particles receive low weights.

The optimal proposal is $q^*(\mathbf{x}_k \mid \mathbf{x}_{k-1}^{(i)}, \mathbf{z}_k) = p(\mathbf{x}_k \mid \mathbf{x}_{k-1}^{(i)}, \mathbf{z}_k)$, but in most cases this cannot be obtained.

### 4.3.3 Resampling

Propagating particles through the proposal leads to a **weight degeneracy** problem: a few particles accumulate most of the weight while the others become negligibly small. In effect, only a small subset of particles contribute, and the quality of the approximation deteriorates rapidly.

We diagnose degeneracy using the effective sample size:

$$N_{\text{eff}} = \frac{1}{\sum_{i=1}^N (w_k^{(i)})^2}$$

If $N_{\text{eff}} < N_{\text{threshold}}$ (typically $N/2$), we perform resampling.

**Resampling**: the process of duplicating high-weight particles and removing low-weight ones to make the weights uniform.

Main resampling strategies:

**Multinomial resampling**: draw $N$ independent samples using the weights as probabilities. The most intuitive but has high variance.

**Systematic resampling**: generate a single uniform random number $U_0 \sim \text{Uniform}(0, 1/N)$, then traverse the CDF with $U_i = U_0 + (i-1)/N$ to resample. Has the smallest variance and is the most used in practice.

**Stratified resampling**: use independent uniform random numbers within each stratum. Intermediate between systematic and multinomial.

```python
import numpy as np

def systematic_resampling(weights, N):
    """Systematic resampling.
    
    Parameters
    ----------
    weights : ndarray, shape (N,) — normalized weights
    N : int — number of particles to resample
    
    Returns
    -------
    indices : ndarray, shape (N,) — indices of the selected particles
    """
    cumsum = np.cumsum(weights)
    u0 = np.random.uniform(0, 1.0 / N)
    u = u0 + np.arange(N) / N
    indices = np.searchsorted(cumsum, u)
    return indices

def bootstrap_particle_filter(f, h, Q, R, z_seq, N=1000, x0_sampler=None):
    """Bootstrap Particle Filter (SIR: Sampling Importance Resampling).
    
    proposal = transition prior (the most basic PF)
    
    Parameters
    ----------
    f : callable — state transition function f(x, noise) -> x_next
    h : callable — observation function h(x) -> z_predicted
    Q : ndarray — process noise covariance
    R : ndarray — observation noise covariance
    z_seq : list of ndarray — observation sequence
    N : int — number of particles
    x0_sampler : callable — initial particle sampler (defaults to N(0, I))
    
    Returns
    -------
    x_est : list of ndarray — weighted mean estimate at each time step
    """
    n = Q.shape[0]
    m = R.shape[0]
    T = len(z_seq)
    
    # initialization
    if x0_sampler:
        particles = np.array([x0_sampler() for _ in range(N)])  # (N, n)
    else:
        particles = np.random.randn(N, n)
    weights = np.ones(N) / N
    
    x_est = []
    L_Q = np.linalg.cholesky(Q)
    
    for k in range(T):
        # 1. prediction: propagate particles through the transition model
        noise = (L_Q @ np.random.randn(n, N)).T  # (N, n)
        particles = np.array([f(particles[i], noise[i]) for i in range(N)])
        
        # 2. weight update: observation likelihood
        for i in range(N):
            z_pred = h(particles[i])
            innovation = z_seq[k] - z_pred
            # Gaussian likelihood
            log_w = -0.5 * innovation @ np.linalg.solve(R, innovation)
            weights[i] *= np.exp(log_w)
        
        # normalize
        weights /= np.sum(weights)
        
        # weighted-mean estimate
        x_est.append(np.average(particles, weights=weights, axis=0))
        
        # 3. resampling (if the effective sample size is below the threshold)
        N_eff = 1.0 / np.sum(weights ** 2)
        if N_eff < N / 2:
            indices = systematic_resampling(weights, N)
            particles = particles[indices]
            weights = np.ones(N) / N
    
    return x_est
```

### 4.3.4 Rao-Blackwellized Particle Filter (RBPF)

The RBPF splits the state space into two parts, estimating one with particles and the other analytically (e.g., with a Kalman filter). This reduces the dimensionality the particle filter must cover, yielding a good approximation with far fewer particles.

Let the state be partitioned as $\mathbf{x} = [\mathbf{x}_1, \mathbf{x}_2]$. If, given $\mathbf{x}_1$, the conditional distribution of $\mathbf{x}_2$ is analytically tractable (e.g., Gaussian):

$$p(\mathbf{x}_1, \mathbf{x}_2 \mid \mathbf{z}_{1:k}) = p(\mathbf{x}_2 \mid \mathbf{x}_1, \mathbf{z}_{1:k}) \cdot p(\mathbf{x}_1 \mid \mathbf{z}_{1:k})$$

- $p(\mathbf{x}_1 \mid \mathbf{z}_{1:k})$: approximated by a particle filter
- $p(\mathbf{x}_2 \mid \mathbf{x}_1, \mathbf{z}_{1:k})$: tracked by a Kalman filter attached to each particle

**Rao-Blackwell theorem**: the variance of the estimator under this partition is always less than or equal to the variance of the pure particle filter.

$$\text{Var}[\hat{\mathbf{x}}_{\text{RBPF}}] \leq \text{Var}[\hat{\mathbf{x}}_{\text{PF}}]$$

**Connection to FastSLAM**: FastSLAM is a representative application of the RBPF. It represents the robot trajectory with particles and tracks landmark positions with individual EKFs attached to each particle.

- $\mathbf{x}_1 = \mathbf{x}_{0:k}^{\text{robot}}$ (robot trajectory) → particle filter
- $\mathbf{x}_2 = \{\mathbf{m}_1, \ldots, \mathbf{m}_M\}$ (landmarks) → $M$ independent 2D EKFs per particle

Given the robot trajectory, observations of each landmark become mutually independent (conditional independence), so $M$ small EKFs can be run independently instead of one giant EKF. This is the key insight that reduces the $O(M^2)$ complexity of EKF-SLAM to the $O(M \log M)$ of FastSLAM.

### 4.3.5 Limitations of the Particle Filter and Its Current Role

The greatest limitation of the PF is the **curse of dimensionality**. As the state-space dimension grows, the number of particles needed for a meaningful approximation grows exponentially. A typical VIO/LIO state vector is 15-dimensional or more, so a pure PF is impractical.

For this reason, the PF plays a limited role in modern robotic systems:

- **2D SLAM (RBPF-based)**: still used in 2D occupancy-grid SLAM such as GMapping. Only the robot pose (3-DoF) is represented by particles, with the map represented by a grid attached to each particle.
- **Global localization (MCL)**: when the robot's initial position is unknown in an existing map (the kidnapped-robot problem). The PF's ability to represent multimodal distributions naturally makes it a good fit.
- **Low-dimensional nonlinear estimation**: specialized problems with a low-dimensional state and strong nonlinearity.

High-dimensional state estimation is dominated by the Kalman filter family (especially the ESKF) and factor-graph-based optimization.

---

## 4.4 Smoothing vs Filtering

### 4.4.1 The Difference Between Filtering and Smoothing

**Filtering**: uses observations up to the present to estimate the current state.
$$p(\mathbf{x}_k \mid \mathbf{z}_{1:k})$$

**Smoothing**: uses all observations (including future ones) to estimate past states.
$$p(\mathbf{x}_k \mid \mathbf{z}_{1:T}), \quad k < T$$

A smoother leverages "future observations," so its estimate at the same time is always at least as accurate as the filter's. However, real-time estimation requires a filter; smoothers are used in batch (post-processing) or fixed-lag form.

### 4.4.2 Fixed-Lag Smoother

A fixed-lag smoother uses observations up to the current time $k$ to estimate the state at $k-L$, $L$ steps in the past:

$$p(\mathbf{x}_{k-L} \mid \mathbf{z}_{1:k})$$

This is a compromise between filtering and full smoothing. Allowing a latency of $L$ yields a better estimate.

The sliding-window optimization systems of VINS-Mono, ORB-SLAM3, and so on are effectively fixed-lag smoothers. Keyframes within the window are optimized jointly, which is more accurate than simple filtering.

### 4.4.3 Full Smoothing (Batch Optimization)

Estimate all states of the entire trajectory simultaneously using all observations:

$$p(\mathbf{x}_{0:T} \mid \mathbf{z}_{1:T})$$

Solving this as MAP (Maximum A Posteriori) estimation gives:

$$\mathbf{x}_{0:T}^* = \arg\max_{\mathbf{x}_{0:T}} p(\mathbf{x}_{0:T} \mid \mathbf{z}_{1:T})$$

Under Gaussian noise assumptions, MAP becomes a Nonlinear Least Squares (NLS) problem. This is the starting point of factor-graph-based optimization.

### 4.4.4 Why Modern SLAM Moved From Filtering To Optimization

Until the early 2000s, EKF-SLAM was the mainstream of SLAM. Gradually, the field shifted to graph-based optimization (= batch smoothing). The reasons:

**1. The linearization-point problem**

The EKF is "linearize once, and you're done." The Jacobian at time $k$ is computed at the time-$k$ estimate, and if a better estimate is later obtained, the past Jacobians are not revised. In contrast, batch optimization can iteratively relinearize the Jacobians of the entire trajectory at the current estimate.

[Strasdat et al. (2012) "Visual SLAM: Why Filter?"](https://doi.org/10.1016/j.imavis.2012.02.009) presented this argument systematically: given the same computational budget, adding more keyframes to optimization yields higher accuracy than adding more observations to filtering.

**2. Consistency issues**

EKF-SLAM tends to violate observability conditions. In SLAM, the first pose should be unobservable (and therefore fixed), but the EKF's linearization breaks this, making four unobservable directions observable and introducing inconsistency. The First-Estimate Jacobian (FEJ) alleviates this problem but does not fully resolve it.

**3. Scalability**

In EKF-SLAM with $M$ landmarks, the covariance matrix has size $O((n + 3M)^2)$ and each update has cost $O((n+3M)^2)$. This becomes intractable as the map grows.

In graph-based SLAM the information matrix (Hessian) is **sparse**. Each variable (pose, landmark) is connected only to the variables it directly observed, so most of the information matrix is zero. Exploiting this sparsity allows optimization in time near-linear in the number of variables.

**4. Natural handling of loop closures**

In filter-based systems, handling a loop closure requires retaining the covariance information for past states, which dramatically increases the cost. In graph-based systems, a loop closure is simply a new factor (constraint) to add, and the whole graph is re-optimized.

| Aspect | Filtering (EKF) | Optimization (Graph) |
|------|----------------|---------------------|
| Linearization | Once, fixed | Can be repeatedly relinearized |
| Past states | Marginalized out | All retained |
| Loop closure | Difficult and costly | Natural via adding a factor |
| Information-matrix structure | Dense | Sparse |
| Cost per step ($M$ landmarks) | $O(M^2)$ | $O(M)$ (exploiting sparsity) |
| Consistency | Requires FEJ and the like | Naturally mitigated by relinearization |

That said, filter-based approaches have not disappeared entirely. [MSCKF (Mourikis & Roumeliotis, 2007)](https://ieeexplore.ieee.org/document/4209642) and OpenVINS are EKF-based yet exhibit competitive performance, and remain useful in environments with extremely limited compute (e.g., micro UAVs). FAST-LIO2's IEKF is also filter-based, yet combined with ikd-tree it achieves accuracy on par with optimization-based systems.

---

## 4.5 Factor Graph & Optimization

### 4.5.1 Factor Graph Representation

A factor graph is a kind of probabilistic graphical model — a bipartite graph consisting of variables and factors.

$$p(\mathbf{X} \mid \mathbf{Z}) \propto \prod_{i} f_i(\mathbf{X}_i)$$

Here:
- $\mathbf{X} = \{\mathbf{x}_0, \mathbf{x}_1, \ldots, \mathbf{x}_T, \mathbf{l}_1, \ldots, \mathbf{l}_M\}$: variable nodes (poses, landmarks, biases, etc.)
- $f_i(\mathbf{X}_i)$: the $i$-th factor. An "energy function" or "probabilistic constraint" on the subset $\mathbf{X}_i$ of variables
- $\mathbf{Z}$: all observations

Each factor corresponds to a specific observation or piece of prior information:

| Factor type | Connected variables | Meaning |
|------------|-------------|------|
| Prior factor | $\mathbf{x}_0$ | Prior on the initial state |
| Odometry factor | $\mathbf{x}_{k-1}, \mathbf{x}_k$ | Relative motion between consecutive poses |
| IMU preintegration factor | $\mathbf{x}_{i}, \mathbf{x}_{j}, \mathbf{v}_i, \mathbf{v}_j, \mathbf{b}_i$ | IMU integration between keyframes |
| Vision factor | $\mathbf{x}_k, \mathbf{l}_m$ | Camera observation of a landmark |
| LiDAR factor | $\mathbf{x}_k$ | Point-to-plane/point-to-edge registration |
| GPS factor | $\mathbf{x}_k$ | Absolute-position observation |
| Loop closure factor | $\mathbf{x}_i, \mathbf{x}_j$ | Loop-closure relative pose |

The key strength of factor graphs is **modularity**. To add a new sensor, one defines the corresponding factor and adds it to the graph — existing factors need not change.

### 4.5.2 MAP Inference = Nonlinear Least Squares

Under a Gaussian noise model, each factor takes the form:

$$f_i(\mathbf{X}_i) \propto \exp\left(-\frac{1}{2} \|\mathbf{r}_i(\mathbf{X}_i)\|^2_{\boldsymbol{\Sigma}_i}\right)$$

Here $\mathbf{r}_i(\mathbf{X}_i)$ is the residual and $\|\mathbf{r}\|^2_{\boldsymbol{\Sigma}} = \mathbf{r}^\top \boldsymbol{\Sigma}^{-1} \mathbf{r}$ is the squared Mahalanobis distance.

For an observation factor, for example:
$$\mathbf{r}_i = \mathbf{z}_i - h_i(\mathbf{X}_i), \quad \boldsymbol{\Sigma}_i = \mathbf{R}_i \text{ (observation noise covariance)}$$

MAP estimate of the full posterior:

$$\mathbf{X}^* = \arg\max_\mathbf{X} p(\mathbf{X} \mid \mathbf{Z}) = \arg\max_\mathbf{X} \prod_i f_i(\mathbf{X}_i)$$

Taking logarithms and flipping signs:

$$\boxed{\mathbf{X}^* = \arg\min_\mathbf{X} \sum_i \|\mathbf{r}_i(\mathbf{X}_i)\|^2_{\boldsymbol{\Sigma}_i}}$$

This is the **Nonlinear Least Squares (NLS)** problem. The probabilistic inference problem has been converted into an optimization problem. The 139-page tutorial [Dellaert & Kaess (2017) "Factor Graphs for Robot Perception"](https://doi.org/10.1561/2300000043) systematically explains this process.

### 4.5.3 Gauss-Newton Method

To solve the NLS problem we use the Gauss-Newton (GN) method. Expand the residuals to first order about the current estimate $\mathbf{X}^{(0)}$:

$$\mathbf{r}_i(\mathbf{X}^{(0)} \boxplus \Delta\mathbf{X}) \approx \mathbf{r}_i(\mathbf{X}^{(0)}) + \mathbf{J}_i \Delta\mathbf{X}$$

Here $\mathbf{J}_i = \frac{\partial \mathbf{r}_i}{\partial \mathbf{X}}\big|_{\mathbf{X}^{(0)}}$ is the residual Jacobian, and $\boxplus$ is the increment operation on the manifold.

Substituting:

$$\sum_i \|\mathbf{r}_i + \mathbf{J}_i \Delta\mathbf{X}\|^2_{\boldsymbol{\Sigma}_i}$$

Whitening via $\mathbf{r}_i' = \boldsymbol{\Sigma}_i^{-1/2} \mathbf{r}_i$, $\mathbf{J}_i' = \boldsymbol{\Sigma}_i^{-1/2} \mathbf{J}_i$ yields a standard least-squares problem:

$$\sum_i \|\mathbf{r}_i' + \mathbf{J}_i' \Delta\mathbf{X}\|^2$$

Differentiating with respect to $\Delta\mathbf{X}$ and setting to zero yields the **normal equation**:

$$\underbrace{\left(\sum_i \mathbf{J}_i'^\top \mathbf{J}_i'\right)}_{\mathbf{H}} \Delta\mathbf{X} = -\underbrace{\sum_i \mathbf{J}_i'^\top \mathbf{r}_i'}_{\mathbf{b}}$$

$$\boxed{\mathbf{H} \Delta\mathbf{X} = -\mathbf{b}}$$

Here $\mathbf{H} = \mathbf{J}^\top \boldsymbol{\Sigma}^{-1} \mathbf{J} \in \mathbb{R}^{N \times N}$ is the approximate Hessian (information matrix), and $\mathbf{b} = \mathbf{J}^\top \boldsymbol{\Sigma}^{-1} \mathbf{r}$ is the gradient.

In SLAM problems $\mathbf{H}$ is **sparse**. Each factor's Jacobian $\mathbf{J}_i$ has nonzero entries only in the columns corresponding to the variables it connects, and zero elsewhere. Thus the nonzero entries of $\mathbf{H}$ correspond to edges in the factor graph: when the graph is sparse, $\mathbf{H}$ is sparse.

Gauss-Newton iteration:

$$\mathbf{X}^{(k+1)} = \mathbf{X}^{(k)} \boxplus \Delta\mathbf{X}^{(k)}$$

Each iteration solves a normal equation. Sparse linear systems are solved with **sparse Cholesky decomposition** ($\mathbf{H} = \mathbf{L}\mathbf{L}^\top$, forward/back substitution); since the fill-in of $\mathbf{L}$ depends on the variable ordering, an approximate minimum-degree ordering such as COLAMD is used.

### 4.5.4 Levenberg-Marquardt Method

Gauss-Newton is a purely approximate second-order method, but with a bad initial estimate or strong nonlinearity it may diverge. The **Levenberg-Marquardt (LM)** method is a compromise between GN and gradient descent, adding a regularization term:

$$(\mathbf{H} + \lambda \mathbf{I}) \Delta\mathbf{X} = -\mathbf{b}$$

- Small $\lambda$ → closer to GN (fast, quadratic convergence)
- Large $\lambda$ → closer to gradient descent (small step, safe)

Strategy for $\lambda$: if the update decreases the cost, decrease $\lambda$ (GN mode); if it increases the cost, increase $\lambda$ (conservative mode).

### 4.5.5 Optimization on Manifolds

When optimizing a 3D pose $\mathbf{T} \in SE(3)$, $SE(3)$ is a manifold, not Euclidean space, so we cannot use ordinary addition. The standard way to handle this is to use a **retraction** (or **exponential map**).

Near the current estimate $\mathbf{T}^{(k)}$, define the increment $\boldsymbol{\xi} \in \mathbb{R}^6$ in the tangent space and set:

$$\mathbf{T}^{(k+1)} = \mathbf{T}^{(k)} \cdot \text{Exp}(\boldsymbol{\xi})$$

or:

$$\mathbf{T}^{(k+1)} = \text{Exp}(\boldsymbol{\xi}) \cdot \mathbf{T}^{(k)}$$

(the choice of left/right increment depends on convention)

Here $\text{Exp}: \mathbb{R}^6 \to SE(3)$ is the Lie group exponential map. In $\boldsymbol{\xi} = [\boldsymbol{\rho}^\top, \boldsymbol{\phi}^\top]^\top$, $\boldsymbol{\rho} \in \mathbb{R}^3$ is translation and $\boldsymbol{\phi} \in \mathbb{R}^3$ is rotation (angle-axis).

Exponential map on SO(3) (Rodrigues' formula):

$$\text{Exp}(\boldsymbol{\phi}) = \mathbf{I} + \frac{\sin\theta}{\theta}[\boldsymbol{\phi}]_\times + \frac{1 - \cos\theta}{\theta^2}[\boldsymbol{\phi}]_\times^2 \in \mathbb{R}^{3 \times 3}$$

where $\theta = \|\boldsymbol{\phi}\|$.

Conversely, $\text{Log}: SE(3) \to \mathbb{R}^6$ is the logarithmic map.

The Gauss-Newton/LM normal equation is solved for the tangent-space increment $\boldsymbol{\xi}$, and the manifold state is updated via the exponential map. Jacobians are computed with respect to the tangent space:

$$\mathbf{J}_i = \frac{\partial \mathbf{r}_i}{\partial \boldsymbol{\xi}}\bigg|_{\boldsymbol{\xi}=\mathbf{0}}$$

### 4.5.6 iSAM2: Incremental Smoothing

Running batch optimization from scratch at every keyframe takes unrealistically long. **iSAM2** ([Kaess et al., 2012](https://doi.org/10.1177/0278364911430419)) uses the Bayes tree data structure to perform optimization incrementally.

Core ideas:

1. **Bayes tree**: a directed-tree representation of the elimination result of a factor graph. Each node stores the conditional density over a clique (subset of variables).

2. **Incremental update**: when a new factor is added, only the affected cliques are recomputed. Most of the tree is unchanged.

3. **Fluid relinearization**: only variables whose linearization point has drifted significantly from the current estimate are selectively relinearized. No periodic batch pass is needed.

4. **Variable reordering**: when new variables/factors are added, no full reordering is performed; only the affected portion is locally reordered.

iSAM2 is the core algorithm of the GTSAM library, serving as the backend of many modern SLAM systems such as LIO-SAM and VINS-Mono.

> **Recent trends — continuous-time factor graph**: there is active research extending discrete keyframe-based factor graphs to **continuous time**. [Wong et al. (2024)](https://arxiv.org/abs/2402.06174) use a Gaussian Process motion prior to unify radar-inertial and LiDAR-inertial odometry in a continuous-time factor graph, and show that asynchronous sensor measurements can be handled naturally.

### 4.5.7 GTSAM / Ceres / g2o Comparison

| Aspect | GTSAM | Ceres Solver | g2o |
|------|-------|-------------|-----|
| Developer | Georgia Tech ([Dellaert](https://gtsam.org/)) | Google ([Ceres](http://ceres-solver.org/)) | [Kümmerle et al.](https://doi.org/10.1109/ICRA.2011.5979949) |
| Core philosophy | Factor graph + Bayes tree | General-purpose NLS solver | Graph optimization |
| Incremental | iSAM2 (native) | None (batch) | None (batch) |
| Manifolds | Built-in (Rot2, Rot3, Pose2, Pose3, ...) | Local parameterization | Built-in |
| IMU preintegration | Built-in (`PreintegratedImuMeasurements`) | User-defined | User-defined |
| Automatic differentiation | Numerical differentiation available | Auto-diff (ceres::AutoDiffCostFunction) | None |
| Language | C++ (Python bindings) | C++ | C++ |
| Representative users | LIO-SAM, VINS-Mono | Cartographer, ORB-SLAM3 BA | Pose graphs in many SLAM systems |
| Learning curve | Just define factors | Define cost functions | Define vertices/edges |

```python
# Simple pose graph optimization example using GTSAM
import gtsam
import numpy as np

# 1. create factor graph
graph = gtsam.NonlinearFactorGraph()

# 2. initial estimates
initial = gtsam.Values()

# 3. prior factor on the first pose
prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))  # (x, y, theta)
graph.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(0.0, 0.0, 0.0), prior_noise))
initial.insert(0, gtsam.Pose2(0.0, 0.0, 0.0))

# 4. odometry factors (between consecutive poses)
odom_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))

# square trajectory: 4 poses (forward 2 m + left turn 90 deg on each side)
odometry = [
    gtsam.Pose2(2.0, 0.0, np.pi / 2),   # x0 -> x1: forward 2 m + left turn 90 deg
    gtsam.Pose2(2.0, 0.0, np.pi / 2),   # x1 -> x2: forward 2 m + left turn 90 deg
    gtsam.Pose2(2.0, 0.0, np.pi / 2),   # x2 -> x3: forward 2 m + left turn 90 deg
    gtsam.Pose2(2.0, 0.0, np.pi / 2),   # x3 -> x4: forward 2 m + left turn 90 deg
]

# add odometry factors and set initial values
pose = gtsam.Pose2(0.0, 0.0, 0.0)
for i, odom in enumerate(odometry):
    graph.add(gtsam.BetweenFactorPose2(i, i + 1, odom, odom_noise))
    pose = pose.compose(odom)
    # add a little noise to the initial guess (in reality this is the accumulated odometry)
    initial.insert(i + 1, pose)

# 5. loop-closure factor: x4 and x0 are at the same position (the square trajectory closes)
loop_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 0.05]))
graph.add(gtsam.BetweenFactorPose2(4, 0, gtsam.Pose2(0.0, 0.0, 0.0), loop_noise))

# 6. optimize with iSAM2
params = gtsam.ISAM2Params()
isam = gtsam.ISAM2(params)
isam.update(graph, initial)
result = isam.calculateEstimate()

# print results
for i in range(5):
    pose_i = result.atPose2(i)
    print(f"Pose {i}: x={pose_i.x():.3f}, y={pose_i.y():.3f}, theta={pose_i.theta():.3f}")
```

---

## 4.6 IMU Preintegration

### 4.6.1 Why Preintegration Is Needed

An IMU usually measures acceleration and angular velocity at 200 Hz to 1000 Hz, while camera/LiDAR keyframes arrive at 10 Hz to 30 Hz intervals. Hundreds of IMU measurements occur between two keyframes $i, j$.

**Naive approach: direct integration**

Integrating the IMU measurements between two keyframes to obtain the relative pose gives:

The problem: this integration depends on the state $(\mathbf{R}_i, \mathbf{v}_i, \mathbf{p}_i)$ at keyframe $i$ and on the biases $(\mathbf{b}_g, \mathbf{b}_a)$. In the optimization loop, whenever the estimates of $\mathbf{R}_i, \mathbf{v}_i, \mathbf{p}_i$ change, all intermediate states must be re-integrated. The same is true when the bias estimates change. This means hundreds of exponential-map evaluations per optimization iteration.

**Preintegration's remedy**: define the relative change in the body frame of keyframe $i$, so that this quantity is independent of keyframe $i$'s global pose. For biases, first-order Jacobian correction avoids re-integration.

### 4.6.2 On-Manifold Preintegration Derivation

We derive the on-manifold preintegration of [Forster et al. (2017)](https://doi.org/10.1109/TRO.2016.2597321) step by step. The original paper is also available on [arXiv:1512.02363](https://arxiv.org/abs/1512.02363).

#### Step 1: Define relative quantities

Rearrange the integration equations in the global frame so they are expressed in the body frame of keyframe $i$. Move the global variables ($\mathbf{R}_i, \mathbf{v}_i, \mathbf{p}_i$) to the left-hand side:

$$\Delta\mathbf{R}_{ij} \triangleq \mathbf{R}_i^\top \mathbf{R}_j = \prod_{k=i}^{j-1} \text{Exp}((\tilde{\boldsymbol{\omega}}_k - \mathbf{b}_g^i)\Delta t) \in SO(3)$$

Key observation: **the right-hand sides depend only on the IMU measurements and the bias estimates, and are independent of the global pose $(\mathbf{R}_i, \mathbf{v}_i, \mathbf{p}_i)$ at keyframe $i$.** Therefore, when the keyframe pose changes during optimization, the right-hand sides do not need to be recomputed.

#### Step 2: Recursive computation (on-manifold)

The preintegrated measurements can be accumulated recursively:

Initial values: $\Delta\mathbf{R}_{ii} = \mathbf{I}_{3\times 3}$, $\Delta\mathbf{v}_{ii} = \mathbf{0}$, $\Delta\mathbf{p}_{ii} = \mathbf{0}$.

The meaning of "on-manifold": the rotation $\Delta\mathbf{R}_{ij}$ is accumulated directly on $SO(3)$. Makeshift workarounds such as Euler angles or quaternion re-normalization are not needed.

#### Step 3: First-order correction for bias change

During optimization, the bias estimate changes as $\mathbf{b} \to \mathbf{b} + \delta\mathbf{b}$, and the preintegrated measurements change accordingly. To avoid full re-integration, we apply a first-order Taylor correction:

**Rotation correction**:
$$\Delta\hat{\mathbf{R}}_{ij}(\mathbf{b}_g + \delta\mathbf{b}_g) \approx \Delta\hat{\mathbf{R}}_{ij}(\mathbf{b}_g) \cdot \text{Exp}\left(\frac{\partial \Delta\bar{\mathbf{R}}_{ij}}{\partial \mathbf{b}_g} \delta\mathbf{b}_g\right)$$

**Velocity correction**:
$$\Delta\hat{\mathbf{v}}_{ij}(\mathbf{b} + \delta\mathbf{b}) \approx \Delta\hat{\mathbf{v}}_{ij}(\mathbf{b}) + \frac{\partial \Delta\bar{\mathbf{v}}_{ij}}{\partial \mathbf{b}_g} \delta\mathbf{b}_g + \frac{\partial \Delta\bar{\mathbf{v}}_{ij}}{\partial \mathbf{b}_a} \delta\mathbf{b}_a$$

**Position correction**:
$$\Delta\hat{\mathbf{p}}_{ij}(\mathbf{b} + \delta\mathbf{b}) \approx \Delta\hat{\mathbf{p}}_{ij}(\mathbf{b}) + \frac{\partial \Delta\bar{\mathbf{p}}_{ij}}{\partial \mathbf{b}_g} \delta\mathbf{b}_g + \frac{\partial \Delta\bar{\mathbf{p}}_{ij}}{\partial \mathbf{b}_a} \delta\mathbf{b}_a$$

The Jacobians $\frac{\partial \Delta\bar{\mathbf{R}}_{ij}}{\partial \mathbf{b}_g}$ and the like are accumulated recursively during preintegration. For example, the bias Jacobian of the rotation:

where $\text{Jr}(\boldsymbol{\phi})$ is the right Jacobian of SO(3):

When the bias change is small ($\|\delta\mathbf{b}\|$ is small), this first-order correction is sufficiently accurate. When the bias change is large, the preintegration is recomputed from scratch, but this rarely occurs in practice.

#### Step 4: Covariance propagation

The IMU measurement noise $\mathbf{n}_g, \mathbf{n}_a \sim \mathcal{N}(\mathbf{0}, \boldsymbol{\Sigma})$ propagates through the preintegration. The covariance is also computed recursively:

$$\boldsymbol{\Sigma}_{k+1} = \mathbf{A}_k \boldsymbol{\Sigma}_k \mathbf{A}_k^\top + \mathbf{B}_k \boldsymbol{\Sigma}_\eta \mathbf{B}_k^\top$$

Here $\mathbf{A}_k, \mathbf{B}_k$ are the Jacobians of the recursive propagation and $\boldsymbol{\Sigma}_\eta$ is the IMU noise covariance. The covariance $\boldsymbol{\Sigma}_{ij}$ is used as the information matrix $\boldsymbol{\Sigma}_{ij}^{-1}$ of the IMU factor in the factor graph.

#### Step 5: IMU Factor in the Factor Graph

Finally, the preintegrated measurement is inserted as a factor between two keyframes $i, j$. The residual:

$$\mathbf{r}_{\Delta\mathbf{R}_{ij}} = \text{Log}\left(\Delta\hat{\mathbf{R}}_{ij}^\top \mathbf{R}_i^\top \mathbf{R}_j\right) \in \mathbb{R}^3$$

$$\mathbf{r}_{\Delta\mathbf{v}_{ij}} = \mathbf{R}_i^\top(\mathbf{v}_j - \mathbf{v}_i - \mathbf{g}\Delta t_{ij}) - \Delta\hat{\mathbf{v}}_{ij} \in \mathbb{R}^3$$

This 9-dimensional residual is the IMU preintegration factor's residual, and its squared Mahalanobis distance $\|\mathbf{r}\|^2_{\boldsymbol{\Sigma}_{ij}}$ is added to the cost function.

### 4.6.3 Preintegration in Code

```python
import numpy as np
from scipy.spatial.transform import Rotation

def skew(v):
    """3D vector -> skew-symmetric matrix.
    
    Parameters
    ----------
    v : ndarray, shape (3,)
    
    Returns
    -------
    S : ndarray, shape (3, 3)
    """
    return np.array([[0, -v[2], v[1]],
                     [v[2], 0, -v[0]],
                     [-v[1], v[0], 0]])

def exp_so3(phi):
    """so(3) -> SO(3) exponential map (Rodrigues' formula).
    
    Parameters
    ----------
    phi : ndarray, shape (3,) — angle-axis vector
    
    Returns
    -------
    R : ndarray, shape (3, 3) — rotation matrix
    """
    theta = np.linalg.norm(phi)
    if theta < 1e-10:
        return np.eye(3) + skew(phi)
    axis = phi / theta
    K = skew(axis)
    return np.eye(3) + np.sin(theta) * K + (1 - np.cos(theta)) * K @ K

def log_so3(R):
    """SO(3) -> so(3) logarithmic map.
    
    Parameters
    ----------
    R : ndarray, shape (3, 3) — rotation matrix
    
    Returns
    -------
    phi : ndarray, shape (3,) — angle-axis vector
    """
    cos_theta = (np.trace(R) - 1) / 2
    cos_theta = np.clip(cos_theta, -1, 1)
    theta = np.arccos(cos_theta)
    if theta < 1e-10:
        return np.array([R[2,1] - R[1,2], R[0,2] - R[2,0], R[1,0] - R[0,1]]) / 2
    return theta / (2 * np.sin(theta)) * np.array([R[2,1] - R[1,2],
                                                    R[0,2] - R[2,0],
                                                    R[1,0] - R[0,1]])

def right_jacobian_so3(phi):
    """Right Jacobian of SO(3).
    
    Parameters
    ----------
    phi : ndarray, shape (3,)
    
    Returns
    -------
    Jr : ndarray, shape (3, 3)
    """
    theta = np.linalg.norm(phi)
    if theta < 1e-10:
        return np.eye(3) - 0.5 * skew(phi)
    K = skew(phi)
    return (np.eye(3) 
            - (1 - np.cos(theta)) / theta**2 * K 
            + (theta - np.sin(theta)) / theta**3 * K @ K)

class IMUPreintegration:
    """On-manifold IMU preintegration (Forster et al., 2017).
    
    Preintegrates IMU measurements between two keyframes 
    into a form usable as an IMU factor in a factor graph.
    """
    def __init__(self, bias_gyro, bias_acc, 
                 gyro_noise_density, acc_noise_density,
                 gyro_random_walk, acc_random_walk):
        """
        Parameters
        ----------
        bias_gyro : ndarray, shape (3,) — initial gyro bias estimate [rad/s]
        bias_acc : ndarray, shape (3,) — initial accel bias estimate [m/s^2]
        gyro_noise_density : float — gyro noise density [rad/s/sqrt(Hz)]
        acc_noise_density : float — accel noise density [m/s^2/sqrt(Hz)]
        gyro_random_walk : float — gyro bias random walk [rad/s^2/sqrt(Hz)]
        acc_random_walk : float — accel bias random walk [m/s^3/sqrt(Hz)]
        """
        self.bg = bias_gyro.copy()
        self.ba = bias_acc.copy()
        
        # preintegrated measurements (initial values)
        self.delta_R = np.eye(3)     # relative rotation on SO(3)
        self.delta_v = np.zeros(3)   # relative velocity change
        self.delta_p = np.zeros(3)   # relative position change
        self.dt_sum = 0.0            # total integration time
        
        # covariance (9x9: rotation, velocity, position)
        self.cov = np.zeros((9, 9))
        
        # bias Jacobians (for bias correction)
        self.d_R_d_bg = np.zeros((3, 3))
        self.d_v_d_bg = np.zeros((3, 3))
        self.d_v_d_ba = np.zeros((3, 3))
        self.d_p_d_bg = np.zeros((3, 3))
        self.d_p_d_ba = np.zeros((3, 3))
        
        # noise parameters
        self.sigma_g = gyro_noise_density
        self.sigma_a = acc_noise_density
        self.sigma_bg = gyro_random_walk
        self.sigma_ba = acc_random_walk
        
    def integrate(self, gyro_meas, acc_meas, dt):
        """Add a single IMU measurement to the preintegration.
        
        Parameters
        ----------
        gyro_meas : ndarray, shape (3,) — gyro measurement [rad/s]
        acc_meas : ndarray, shape (3,) — accelerometer measurement [m/s^2]
        dt : float — time step [s]
        """
        # bias-corrected measurements
        omega = gyro_meas - self.bg   # (3,)
        acc = acc_meas - self.ba       # (3,)
        
        # intermediate quantities
        dR = exp_so3(omega * dt)       # (3, 3)  rotation over this dt
        Jr = right_jacobian_so3(omega * dt)  # (3, 3)
        
        # --- recursive update of bias Jacobians (Step 3) ---
        # position Jacobian (bias Jacobian of delta_p)
        self.d_p_d_bg += self.d_v_d_bg * dt - 0.5 * self.delta_R @ skew(acc) @ self.d_R_d_bg * dt**2
        self.d_p_d_ba += self.d_v_d_ba * dt - 0.5 * self.delta_R * dt**2
        # velocity Jacobian
        self.d_v_d_bg += -self.delta_R @ skew(acc) @ self.d_R_d_bg * dt
        self.d_v_d_ba += -self.delta_R * dt
        # rotation Jacobian
        self.d_R_d_bg = dR.T @ (self.d_R_d_bg - Jr * dt)
        
        # --- covariance propagation (Step 4) ---
        A = np.eye(9)
        A[0:3, 0:3] = dR.T
        A[3:6, 0:3] = -self.delta_R @ skew(acc) * dt
        A[6:9, 0:3] = -0.5 * self.delta_R @ skew(acc) * dt**2
        A[6:9, 3:6] = np.eye(3) * dt
        
        B = np.zeros((9, 6))
        B[0:3, 0:3] = Jr * dt
        B[3:6, 3:6] = self.delta_R * dt
        B[6:9, 3:6] = 0.5 * self.delta_R * dt**2
        
        Sigma_eta = np.zeros((6, 6))
        Sigma_eta[0:3, 0:3] = np.eye(3) * self.sigma_g**2
        Sigma_eta[3:6, 3:6] = np.eye(3) * self.sigma_a**2
        
        self.cov = A @ self.cov @ A.T + B @ Sigma_eta @ B.T
        
        # --- recursive update of preintegrated measurements (Step 2) ---
        # order matters: first p, then v, finally R
        self.delta_p += self.delta_v * dt + 0.5 * self.delta_R @ acc * dt**2
        self.delta_v += self.delta_R @ acc * dt
        self.delta_R = self.delta_R @ dR
        self.dt_sum += dt
    
    def predict(self, R_i, v_i, p_i, gravity):
        """Predict the state at keyframe j from the preintegrated measurements.
        
        Parameters
        ----------
        R_i : ndarray, shape (3, 3) — rotation at keyframe i
        v_i : ndarray, shape (3,) — velocity at keyframe i
        p_i : ndarray, shape (3,) — position at keyframe i
        gravity : ndarray, shape (3,) — gravity vector (e.g., [0, 0, -9.81])
        
        Returns
        -------
        R_j, v_j, p_j : predicted state at keyframe j
        """
        dt = self.dt_sum
        R_j = R_i @ self.delta_R
        v_j = v_i + gravity * dt + R_i @ self.delta_v
        p_j = p_i + v_i * dt + 0.5 * gravity * dt**2 + R_i @ self.delta_p
        return R_j, v_j, p_j
    
    def compute_residual(self, R_i, v_i, p_i, R_j, v_j, p_j, gravity):
        """Compute the IMU factor residual.
        
        Returns
        -------
        residual : ndarray, shape (9,) — [r_R(3), r_v(3), r_p(3)]
        """
        dt = self.dt_sum
        
        # rotation residual
        r_R = log_so3(self.delta_R.T @ R_i.T @ R_j)
        
        # velocity residual
        r_v = R_i.T @ (v_j - v_i - gravity * dt) - self.delta_v
        
        # position residual
        r_p = R_i.T @ (p_j - p_i - v_i * dt - 0.5 * gravity * dt**2) - self.delta_p
        
        return np.concatenate([r_R, r_v, r_p])

# usage example with GTSAM
import gtsam

def create_imu_factor_gtsam():
    """Example using GTSAM's built-in IMU preintegration."""
    # IMU parameters
    imu_params = gtsam.PreintegrationParams.MakeSharedU(9.81)  # gravity direction: +z
    imu_params.setAccelerometerCovariance(np.eye(3) * 0.01**2)
    imu_params.setGyroscopeCovariance(np.eye(3) * 0.001**2)
    imu_params.setIntegrationCovariance(np.eye(3) * 1e-8)
    
    # initial bias
    bias = gtsam.imuBias.ConstantBias(
        np.array([0.1, -0.05, 0.02]),   # accelerometer bias
        np.array([0.001, -0.002, 0.003]) # gyroscope bias
    )
    
    # create preintegration object
    pim = gtsam.PreintegratedImuMeasurements(imu_params, bias)
    
    # integrate IMU measurements (assume 200 Hz, 0.1 s = 20 measurements)
    dt = 0.005  # 200 Hz
    for k in range(20):
        acc_meas = np.array([0.0, 0.0, 9.81])  # static (gravity only)
        gyro_meas = np.array([0.0, 0.0, 0.0])
        pim.integrateMeasurement(acc_meas, gyro_meas, dt)
    
    # create factor
    # CombinedImuFactor connects pose, velocity, and bias at keyframes i, j
    imu_factor = gtsam.ImuFactor(
        gtsam.symbol('x', 0),  # pose_i
        gtsam.symbol('v', 0),  # vel_i
        gtsam.symbol('x', 1),  # pose_j
        gtsam.symbol('v', 1),  # vel_j
        gtsam.symbol('b', 0),  # bias
        pim
    )
    
    return imu_factor
```

---

## 4.7 Marginalization & Sliding Window

### 4.7.1 Why Marginalization Is Needed

Factor-graph-based SLAM/VIO systems add variables and factors whenever a new keyframe arrives. Keeping keyframes indefinitely causes the optimization cost to grow without bound.

The sliding-window approach retains only the most recent $N$ keyframes and removes older ones. However, simple deletion loses all the observation information associated with those keyframes and degrades accuracy.

**Marginalization** is the method for removing old variables from the graph while preserving the information they carried between the variables they connected. The information of the eliminated variable is converted into a **prior** on the remaining variables.

### 4.7.2 Schur Complement

The mathematical core of marginalization is the **Schur complement**. Partition the information matrix (Hessian) $\mathbf{H}$ into variables to marginalize ($\mathbf{x}_m$) and variables to retain ($\mathbf{x}_r$). Write the normal equation as $\mathbf{H}\Delta\mathbf{x} = \mathbf{b}$ (redefining the $-\mathbf{b}$ of §4.5.3 as $\mathbf{b}$, i.e., $\mathbf{b} \triangleq -\mathbf{J}^\top \boldsymbol{\Sigma}^{-1} \mathbf{r}$):

$$\mathbf{H} \Delta\mathbf{x} = \mathbf{b}$$

Here:
- $\mathbf{x}_m$: variables to marginalize (remove)
- $\mathbf{x}_r$: variables to retain
- $\mathbf{H}_{mm} \in \mathbb{R}^{n_m \times n_m}$: information block among the variables to remove
- $\mathbf{H}_{mr} \in \mathbb{R}^{n_m \times n_r}$: cross-information block
- $\mathbf{H}_{rr} \in \mathbb{R}^{n_r \times n_r}$: information block among the variables to retain

Eliminating $\Delta\mathbf{x}_m$ from the block matrix above yields:

$\mathbf{H}^* = \mathbf{H}_{rr} - \mathbf{H}_{rm} \mathbf{H}_{mm}^{-1} \mathbf{H}_{mr}$ is the **Schur complement**, and it becomes the information matrix of the prior factor on the retained variables after marginalization.

**Intuitive meaning**: variables in $\mathbf{x}_r$ that were indirectly connected through $\mathbf{x}_m$ become directly connected (fill-in). $\mathbf{H}^*$ is denser than $\mathbf{H}_{rr}$, which reflects that variable-to-variable correlations hidden before marginalization now appear explicitly.

```python
import numpy as np

def marginalize(H, b, indices_to_marginalize, indices_to_keep):
    """Variable marginalization via the Schur complement.
    
    Parameters
    ----------
    H : ndarray, shape (N, N) — information matrix (Hessian)
    b : ndarray, shape (N,) — gradient vector
    indices_to_marginalize : list of int — indices of variables to remove
    indices_to_keep : list of int — indices of variables to retain
    
    Returns
    -------
    H_star : ndarray — information matrix after marginalization
    b_star : ndarray — gradient after marginalization
    """
    m = indices_to_marginalize
    r = indices_to_keep
    
    H_mm = H[np.ix_(m, m)]
    H_mr = H[np.ix_(m, r)]
    H_rm = H[np.ix_(r, m)]
    H_rr = H[np.ix_(r, r)]
    
    b_m = b[m]
    b_r = b[r]
    
    # Schur complement
    H_mm_inv = np.linalg.inv(H_mm)
    H_star = H_rr - H_rm @ H_mm_inv @ H_mr
    b_star = b_r - H_rm @ H_mm_inv @ b_m
    
    return H_star, b_star

# example: 3 poses (each 2D, 3-DoF), marginalize the first one
# state: [x0(3), x1(3), x2(3)] = 9D
np.random.seed(42)

# sparse information matrix (x0-x1, x1-x2 connections)
H = np.zeros((9, 9))
# x0 itself (prior + odom factor)
H[0:3, 0:3] = np.diag([10, 10, 5])
# x0-x1 odometry factor
odom_info = np.diag([25, 25, 50])
H[0:3, 0:3] += odom_info
H[3:6, 3:6] += odom_info
H[0:3, 3:6] = -odom_info
H[3:6, 0:3] = -odom_info
# x1-x2 odometry factor
H[3:6, 3:6] += odom_info
H[6:9, 6:9] += odom_info
H[3:6, 6:9] = -odom_info
H[6:9, 3:6] = -odom_info

b = np.random.randn(9) * 0.1

print("nonzero pattern of H before marginalization:")
print((np.abs(H) > 1e-10).astype(int))

# marginalize x0 (indices 0, 1, 2)
H_star, b_star = marginalize(H, b, [0, 1, 2], [3, 4, 5, 6, 7, 8])

print("\nnonzero pattern of H* after marginalization:")
print((np.abs(H_star) > 1e-10).astype(int))
print("-> fill-in may appear between x1 and x2")
```

### 4.7.3 First-Estimate Jacobian (FEJ)

There is an important caveat in marginalization. The marginalized factor (prior) is computed at a **fixed linearization point**. In subsequent optimization iterations, if the retained variables' estimates change, a mismatch arises between the marginalized prior's linearization point and the current one.

This mismatch can hurt the **consistency** of the filter/smoother. Specifically, the system's observability properties can change, so directions that are actually unobservable may be incorrectly treated as observable.

The **FEJ (First-Estimate Jacobian)** strategy: Jacobians related to marginalization are always computed at the **first estimate** of the relevant variable, and are not updated even when the estimate later changes.

$$\mathbf{J}_{\text{FEJ}} = \left.\frac{\partial \mathbf{r}}{\partial \mathbf{x}}\right|_{\mathbf{x}^{(0)}}$$

Here $\mathbf{x}^{(0)}$ is the value of the variable when it was first estimated.

Advantages of FEJ:
- The marginalized prior and the current factors use information from the same linearization point, preserving consistency.
- Used centrally in MSCKF/OpenVINS ([Li & Mourikis, 2013](https://doi.org/10.1177/0278364913481251)).

Disadvantages of FEJ:
- If the first estimate is inaccurate, convergence may slow down.
- Implementation becomes more complex (the "first estimate" of each variable must be stored separately).

### 4.7.4 Practical Issues of Sliding-Window Implementation

#### Issue 1: Which keyframe to marginalize

The two strategies of VINS-Mono:
- **If the latest frame is a keyframe**: marginalize the oldest keyframe. The window remains spatially wide.
- **If the latest frame is not a keyframe**: marginalize the immediately previous non-keyframe. Only the visual information is discarded, while the IMU information is forwarded to the neighboring keyframe.

#### Issue 2: Densification due to fill-in

As marginalization is repeated, the prior factor becomes progressively denser, and what was originally a sparse information matrix can become dense. This severely degrades computational efficiency.

Mitigations:
- Limit the size of the prior factor (limit the number of connected variables)
- Choose the marginalization order carefully
- Accept some loss of information and simply delete certain factors (FAST-LIO2 removes old points from the map rather than marginalizing)

#### Issue 3: Biases and marginalization

The IMU bias is a slowly varying state that spans all keyframes. Marginalizing the bias together with a keyframe fixes the bias information in the prior, reducing the flexibility of subsequent bias estimation.

VINS-Mono's approach: the bias is not marginalized, but kept within the window. The marginalization prior is formed conditional on the bias.

#### Issue 4: Numerical stability

The Schur complement computation requires the inverse $\mathbf{H}_{mm}^{-1}$. If $\mathbf{H}_{mm}$ has a bad condition number, numerical instability can arise.

Mitigations:
- Add a small regularization term to $\mathbf{H}_{mm}$: $(\mathbf{H}_{mm} + \epsilon \mathbf{I})^{-1}$
- Use LDL decomposition for numerical stability
- After marginalization, check that $\mathbf{H}^*$ is positive semi-definite and, if not, project it onto the nearest PSD matrix

---

## Chapter 4 Summary

In this chapter we covered state estimation theory, the mathematical foundation of sensor fusion. The key messages:

1. The **Bayesian Filtering Framework** is a prediction-update recursion that forms the common skeleton of every state estimation method. The Chapman-Kolmogorov equation and Bayes' rule are the theoretical underpinnings, but in nonlinear systems approximation is essential.

2. The **Kalman Filter family** tracks the posterior via Gaussian approximation in terms of mean and covariance. The EKF linearizes to first order; the ESKF linearizes in the error state, handling manifold issues naturally; the UKF uses the sigma-point transform; and the IEKF uses iterated linearization to cope with strong nonlinearity. In modern robotics systems, the ESKF is effectively the standard.

3. The **particle filter** can handle multimodal distributions and strong nonlinearity, but the curse of dimensionality makes it unsuitable for high-dimensional problems. The RBPF (FastSLAM) partially alleviates this, and the PF is still used in 2D SLAM and global localization.

4. The **shift from filtering to optimization** has become the mainstream of modern SLAM thanks to benefits such as relinearization, exploitation of sparsity, and natural handling of loop closures. That said, filter-based approaches (MSCKF, FAST-LIO2) remain competitive under specific conditions.

5. The **factor graph** is a powerful framework that modularly composes probabilistic inference and reduces MAP to NLS. It is solved on the manifold with Gauss-Newton/LM, and iSAM2's incremental smoothing makes real-time processing possible.

6. **IMU preintegration** is the key technique for compressing high-rate IMU measurements into a factor between keyframes. The on-manifold derivation and first-order bias correction allow integration into the factor graph without re-integration.

7. **Marginalization** is the information-preservation mechanism of the sliding window. The Schur complement is the core operation, and FEJ is the key to maintaining consistency.

This chapter's theory is an indispensable foundation for understanding the design and implementation of real systems in the subsequent VIO (Ch.6), LIO (Ch.7), and multi-sensor fusion (Ch.8) chapters.

> **2024-2025 research directions**: state estimation is evolving along three major axes. (1) **Symmetry-based filters**: the Equivariant Filter (EqF) and the Invariant EKF exploit the symmetry structure of Lie groups to structurally guarantee consistency and convergence. (2) **Continuous-time optimization**: continuous-time factor graphs with Gaussian Process motion priors are becoming a new paradigm for asynchronous multi-sensor fusion. (3) **Learning-based hybrids**: approaches such as [AI-Aided Kalman Filters (Revach et al., 2024)](https://arxiv.org/abs/2410.12289) that learn the Kalman gain or process model with RNNs/Transformers are active, though providing safety guarantees remains a challenge.

---

# Ch.5 — Feature Matching & Correspondence: Technical Lineage

Ch.4 established the mathematical framework of state estimation. But whether we use a Kalman filter or a factor graph, the **data association** problem — "which observation corresponds to which landmark?" — must be solved first. This chapter covers the technical lineage of its core: feature matching and correspondence search.

> **Purpose of this chapter**: Almost every component of sensor fusion — Visual Odometry, calibration, loop closure, point cloud registration — depends on **correspondence**. This chapter traces the technical flow starting from mutual information and continuing through to RoMa, clearly showing which limitation of the previous generation each method addressed.

---

## 5.1 What Is the Correspondence Problem

### 5.1.1 The Problem of Finding "the Same Thing"

The correspondence problem is the problem of identifying **physically identical points, regions, or structures** across two or more observations. To understand why this problem is fundamental in robotics, we must recognize that virtually every stage of the sensor fusion pipeline presupposes correspondence.

- **Visual Odometry**: Camera motion can only be estimated by finding the 2D projections of the same 3D point across consecutive frames.
- **Calibration**: Estimating the camera-LiDAR extrinsic parameters requires identifying the same physical point observed by both sensors.
- **Loop Closure**: Recognizing a previously visited place requires confirming correspondences between current observations and past observations.
- **Point Cloud Registration**: Alignment of two scans is the process of estimating a rigid transformation based on corresponding point pairs.

### 5.1.2 Three Types of Correspondence

#### 2D-2D Correspondence

This is the problem of finding the projections of the same 3D point across two images. It forms the basis of Visual Odometry, stereo matching, and image stitching.

When the point $\mathbf{p}_1 = (u_1, v_1)$ in image $I_1$ and the point $\mathbf{p}_2 = (u_2, v_2)$ in image $I_2$ are projections of the same 3D point $\mathbf{X}$, the pair $(\mathbf{p}_1, \mathbf{p}_2)$ is called a correspondence.

The geometric relationship between the two images is expressed by the **epipolar constraint**:

$$\mathbf{p}_2^\top \mathbf{F} \mathbf{p}_1 = 0$$

Here, $\mathbf{F}$ is the fundamental matrix (3×3, rank 2). When the intrinsic parameters are known, we use the essential matrix $\mathbf{E} = \mathbf{K}_2^\top \mathbf{F} \mathbf{K}_1$:

$$\hat{\mathbf{p}}_2^\top \mathbf{E} \hat{\mathbf{p}}_1 = 0$$

Here, $\hat{\mathbf{p}} = \mathbf{K}^{-1} \mathbf{p}$ are the normalized image coordinates.

#### 2D-3D Correspondence

The correspondence between a 2D point in an image and a 3D map point. This is the basis of the **PnP (Perspective-n-Point)** problem and is central to visual localization and map-based tracking in SLAM.

Relationship between a 3D point $\mathbf{X} = (X, Y, Z)^\top$ and its 2D projection $\mathbf{p} = (u, v)^\top$:

$$s \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \mathbf{K} [\mathbf{R} | \mathbf{t}] \begin{bmatrix} X \\ Y \\ Z \\ 1 \end{bmatrix}$$

With a minimum of 6 (DLT), 4 (P3P+1), or 3 (P3P) correspondences, the camera pose $(\mathbf{R}, \mathbf{t})$ is estimated.

#### 3D-3D Correspondence

The problem of finding the same physical point between two point clouds. It is used for LiDAR scan-to-scan registration, multi-LiDAR calibration, and point cloud registration during loop closure.

Estimating the rigid transformation $(\mathbf{R}, \mathbf{t})$ between two point clouds $\mathcal{P} = \{\mathbf{p}_i\}$ and $\mathcal{Q} = \{\mathbf{q}_i\}$:

$$\min_{\mathbf{R}, \mathbf{t}} \sum_{i} \| \mathbf{q}_i - (\mathbf{R} \mathbf{p}_i + \mathbf{t}) \|^2$$

The closed-form solution to this optimization problem can be obtained via SVD (ICP, see Ch.7).

### 5.1.3 Why Correspondence Is Central to Sensor Fusion

In sensor fusion, correspondence is not merely preprocessing but **the bottleneck that determines the accuracy of the entire system**. A single incorrect correspondence (outlier) can completely ruin pose estimation, and if correspondences cannot be found (as in textureless environments) the system itself fails to operate. The remainder of this chapter traces the technical lineage of how this problem has been solved.

---

## 5.2 Traditional Feature Detection & Description

The traditional correspondence pipeline consists of three stages: **detect → describe → match**. This section covers the first two stages — where to find keypoints (detection) and how to represent those keypoints (description).

### 5.2.1 Corner Detection: Harris → FAST → ORB

#### Harris Corner Detector (1988)

The Harris corner detector defines a corner as **a point where the intensity changes significantly in all directions when an image patch is shifted**.

The intensity change when the window around a point $(x, y)$ in image $I$ is shifted by $(\Delta u, \Delta v)$:

$$E(\Delta u, \Delta v) = \sum_{x, y} w(x, y) [I(x + \Delta u, y + \Delta v) - I(x, y)]^2$$

$w(x, y)$ is a Gaussian window. Applying a first-order Taylor expansion:

$$E(\Delta u, \Delta v) \approx \begin{bmatrix} \Delta u & \Delta v \end{bmatrix} \mathbf{M} \begin{bmatrix} \Delta u \\ \Delta v \end{bmatrix}$$

Here, the **structure tensor** (or second moment matrix) $\mathbf{M}$ is:

$$\mathbf{M} = \sum_{x, y} w(x, y) \begin{bmatrix} I_x^2 & I_x I_y \\ I_x I_y & I_y^2 \end{bmatrix}$$

$I_x, I_y$ are the image gradients in the x and y directions. Depending on the eigenvalues $(\lambda_1, \lambda_2)$ of $\mathbf{M}$:

- $\lambda_1 \approx 0, \lambda_2 \approx 0$: flat region (no intensity change)
- $\lambda_1 \gg \lambda_2 \approx 0$: edge (change in only one direction)
- Both $\lambda_1, \lambda_2$ large: corner (change in all directions)

Instead of computing the eigenvalues directly, Harris defines a **corner response function**:

$$R = \det(\mathbf{M}) - k \cdot \text{tr}(\mathbf{M})^2 = \lambda_1 \lambda_2 - k(\lambda_1 + \lambda_2)^2$$

Here, $k$ is typically 0.04-0.06. Points with $R > \text{threshold}$ are selected as corners, and non-maximum suppression is applied.

**Limitations of Harris**: It is rotation invariant but **not scale invariant**. As the camera moves closer, a corner may appear as an edge. This limitation motivated the scale-space approach of SIFT.

```python
import cv2
import numpy as np

# Harris Corner Detection
img = cv2.imread('scene.jpg', cv2.IMREAD_GRAYSCALE)
img_float = np.float32(img)

# blockSize: neighborhood size, ksize: Sobel kernel, k: Harris parameter
harris_response = cv2.cornerHarris(img_float, blockSize=2, ksize=3, k=0.04)

# Non-maximum suppression & threshold
corners = harris_response > 0.01 * harris_response.max()
```

#### FAST (Features from Accelerated Segment Test, 2006)

FAST is a detector that pursues **extreme speed** over the accuracy of Harris. It was proposed by [Rosten & Drummond (2006)](https://arxiv.org/abs/0810.2434) to meet the real-time demand of detecting keypoints at tens of FPS in robot vision.

The algorithm is surprisingly simple:

1. Place 16 pixels on a circle of radius 3 centered on a candidate pixel $p$ (Bresenham circle).
2. If $N$ consecutive pixels on the circle (typically $N=12$) are all brighter or all darker than $p$, then $p$ is a corner.
3. Fast reject: first check only the 4 pixels at positions 1, 5, 9, 13 — if at least 3 of them fail the condition, immediately reject.

$$\text{FAST condition: } \exists \text{ contiguous arc of } N \text{ pixels on circle, all } > I_p + t \text{ or all } < I_p - t$$

Here, $t$ is the intensity threshold.

**Decision tree learning**: FAST additionally uses machine learning (ID3 decision tree) to optimize the inspection order. It learns which pixels should be inspected first to reject non-corners as quickly as possible.

**Limitations of FAST**: It has no orientation or scale information, and does not generate a descriptor. ORB addresses these shortcomings.

```python
# FAST Corner Detection
fast = cv2.FastFeatureDetector_create(threshold=20, nonmaxSuppression=True)
keypoints = fast.detect(img, None)
print(f"Detected {len(keypoints)} keypoints")
```

#### ORB (Oriented FAST and Rotated BRIEF, 2011)

ORB was proposed by [Rublee et al. (2011)](https://ieeexplore.ieee.org/document/6126544) to simultaneously address the patent and speed problems of SIFT/SURF. It is a combination that **adds orientation to the FAST detector and rotation invariance to the BRIEF descriptor**.

**oFAST (Oriented FAST)**:
- Orientations are assigned to keypoints detected by FAST using the **intensity centroid** method.
- Image moments of the patch around a keypoint are computed:

$$m_{pq} = \sum_{x, y} x^p y^q I(x, y)$$

- Centroid: $\mathbf{C} = (m_{10}/m_{00}, m_{01}/m_{00})$
- Orientation: $\theta = \text{atan2}(m_{01}, m_{10})$

**rBRIEF (Rotated BRIEF)**:
- BRIEF generates a binary descriptor by comparing the intensities of random point pairs $(x_i, y_i)$ within a patch:

$$\tau(\mathbf{p}; x_i, y_i) = \begin{cases} 1 & \text{if } I(\mathbf{p}, x_i) < I(\mathbf{p}, y_i) \\ 0 & \text{otherwise} \end{cases}$$

- 256 comparisons → 256-bit binary descriptor.
- ORB rotates the comparison point pairs according to the keypoint orientation $\theta$ to secure **rotation invariance**.
- In addition, the comparison point pairs are selected greedily to minimize their correlation, increasing discriminability.

**Multi-scale**: An image pyramid (typically 8 levels) is constructed and FAST is performed at each level, approximating scale invariance.

ORB is **the core feature of the ORB-SLAM series**, and thanks to its binary descriptor, matching is performed with Hamming distance, making it very fast.

```python
# ORB Detection & Description
orb = cv2.ORB_create(nfeatures=1000)
keypoints, descriptors = orb.detectAndCompute(img, None)
# descriptors.shape: (N, 32) — 256-bit = 32 bytes
```

### 5.2.2 Blob Detection: SIFT → SURF

#### SIFT (Scale-Invariant Feature Transform, 2004)

SIFT, proposed by [Lowe (2004)](https://link.springer.com/article/10.1023/B:VISI.0000029664.99615.94), is a keypoint detection and description algorithm that is **invariant to scale, rotation, and illumination changes**. It was the de facto standard for feature matching for 20 years, and since the patent expired in 2020 it can be used freely.

**Stage 1 — Scale-Space Extrema Detection (DoG)**:

A scale-space is constructed by blurring the image with Gaussians at various scales:

$$L(x, y, \sigma) = G(x, y, \sigma) * I(x, y)$$

Here, $G(x, y, \sigma) = \frac{1}{2\pi\sigma^2} \exp\left(-\frac{x^2 + y^2}{2\sigma^2}\right)$.

As an efficient approximation of the Laplacian of Gaussian (LoG), the **Difference of Gaussians (DoG)** is used:

$$D(x, y, \sigma) = L(x, y, k\sigma) - L(x, y, \sigma) \approx (k-1)\sigma^2 \nabla^2 G$$

In the DoG images, extrema are found by comparing against 26 neighbors (spatial: 8 neighbors + scale: 9 each above and below).

**Stage 2 — Keypoint Localization**:

Sub-pixel/sub-scale localization in scale-space using a Taylor expansion:

$$D(\mathbf{x}) = D + \frac{\partial D}{\partial \mathbf{x}}^\top \mathbf{x} + \frac{1}{2} \mathbf{x}^\top \frac{\partial^2 D}{\partial \mathbf{x}^2} \mathbf{x}$$

Refined position of the extremum: $\hat{\mathbf{x}} = -\frac{\partial^2 D}{\partial \mathbf{x}^2}^{-1} \frac{\partial D}{\partial \mathbf{x}}$

Low-contrast keypoint removal: remove if $|D(\hat{\mathbf{x}})| < 0.03$.
Edge response removal: unstable extrema on edges are removed by the ratio of eigenvalues of the Hessian matrix.

**Stage 3 — Orientation Assignment**:

The gradient magnitudes and orientations around a keypoint are computed, and a 36-bin orientation histogram is built (Gaussian weighted, $\sigma = 1.5 \times$ keypoint scale). If there is a peak at least 80% of the maximum peak, a separate keypoint is created for that orientation as well to secure **rotation invariance**.

**Stage 4 — Keypoint Descriptor**:

The $16 \times 16$ region around a keypoint is divided into 16 blocks of $4 \times 4$. An 8-bin orientation histogram is built for each block:

$$\text{Descriptor} = 4 \times 4 \times 8 = 128\text{-dimensional vector}$$

After L2 normalization, values above 0.2 are clipped and renormalized, making it robust to nonlinear illumination changes.

```python
# SIFT Detection & Description
sift = cv2.SIFT_create(nfeatures=2000)
keypoints, descriptors = sift.detectAndCompute(img, None)
# descriptors.shape: (N, 128) — 128-dimensional float32
```

#### SURF (Speeded-Up Robust Features, 2006)

SURF was proposed by [Bay et al. (2006)](https://link.springer.com/chapter/10.1007/11744023_32) to address the speed issue of SIFT. Key ideas:

- Approximate LoG with box filters using the **integral image**. Any box filter size can be computed in O(1).
- Use the **Hessian determinant** as the detection criterion (instead of DoG):

$$\det(\mathbf{H}) = D_{xx} D_{yy} - (0.9 \cdot D_{xy})^2$$

- A 64-dimensional descriptor (half the dimensionality of SIFT's 128): sums and sums-of-absolute-values of Haar wavelet responses.
- 3-7× faster than SIFT with comparable accuracy.

Due to patent issues SURF is rarely used in recent practice; in real-time applications ORB is preferred, while for accuracy-critical applications SIFT or learning-based methods are preferred.

### 5.2.3 Binary Descriptors: BRIEF, ORB, BRISK

Binary descriptors generate a 0/1 bit string by comparing the intensities of point pairs within a patch. Since matching is done with **Hamming distance**, they are tens of times faster than float descriptors.

| Descriptor | Bits | Features |
|-----------|--------|------|
| BRIEF | 128/256/512 | Random point pairs, not rotation invariant |
| ORB | 256 | Learned point pairs, rotation invariant |
| BRISK | 512 | Concentric sampling, scale invariant |

BRIEF's comparison operation:

$$b_i = \begin{cases} 1 & \text{if } I(\mathbf{p}_i) < I(\mathbf{q}_i) \\ 0 & \text{otherwise} \end{cases}, \quad \text{descriptor} = \sum_{1 \le i \le n} 2^{i-1} b_i$$

Hamming distance is computed with XOR + popcount in a single CPU instruction:

$$d_H(\mathbf{a}, \mathbf{b}) = \text{popcount}(\mathbf{a} \oplus \mathbf{b})$$

### 5.2.4 Technical Lineage: A History of the Accuracy vs. Speed Trade-off

The history of traditional feature points is a continuous effort to optimize **the trade-off between accuracy and speed**:

```
Harris (1988)     — Mathematical definition of corner detection
    ↓ need for scale invariance
SIFT (2004)       — scale-space + 128D float descriptor (accurate but slow)
    ↓ speed improvement
SURF (2006)       — integral image + 64D (3-7× faster, still float)
    ↓ real-time demand
FAST (2006)       — extreme-speed detection (no descriptor)
    ↓ unifying detection+description
ORB (2011)        — oFAST + rBRIEF, 256-bit binary (Hamming matching)
```

This trade-off continues in the deep learning era as well, and SuperPoint aimed to achieve SIFT-level accuracy at ORB-level speed.

---

## 5.3 Traditional Matching & Outlier Rejection

After detecting keypoints and extracting descriptors, a matching stage is needed **to decide which feature-point pairs actually correspond to the same 3D point**.

### 5.3.1 Brute-Force Matching

The simplest method. Every descriptor in image A is compared with every descriptor in image B, and the closest one is matched.

- Float descriptors (SIFT): L2 distance
- Binary descriptors (ORB): Hamming distance
- Time complexity: $O(N \cdot M \cdot D)$ — $N, M$ are the number of keypoints per image, $D$ is the descriptor dimension.

`crossCheck=True` applies the **mutual nearest neighbor** condition: a match is accepted only when the nearest neighbor from A to B and from B to A agree.

### 5.3.2 FLANN (Fast Library for Approximate Nearest Neighbors)

For large descriptor sets, brute-force is impractical, so **approximate nearest neighbor (ANN) search** is used. FLANN automatically selects among kd-tree, randomized kd-tree, hierarchical k-means, etc.

```python
# FLANN Matching for SIFT
FLANN_INDEX_KDTREE = 1
index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5)
search_params = dict(checks=50)  # limit on number of nodes explored

flann = cv2.FlannBasedMatcher(index_params, search_params)
matches = flann.knnMatch(desc1, desc2, k=2)  # k=2 for ratio test
```

### 5.3.3 Lowe's Ratio Test

A match filtering technique proposed by Lowe (2004) in the SIFT paper. A match is accepted only if the **ratio of the nearest distance to the second-nearest distance** is below a threshold:

$$\frac{d(\mathbf{f}, \mathbf{f}_1)}{d(\mathbf{f}, \mathbf{f}_2)} < \tau$$

Here, $\mathbf{f}_1, \mathbf{f}_2$ are the nearest and second-nearest descriptors respectively, and $\tau$ is typically 0.7-0.8.

Intuition: a correct match has a clearly close nearest neighbor, so $d_1 \ll d_2$. An incorrect match has multiple similar candidates, so $d_1 \approx d_2$.

```python
# Ratio test (Lowe's)
good_matches = []
for m, n in matches:  # m: best, n: second best
    if m.distance < 0.75 * n.distance:
        good_matches.append(m)
```

### 5.3.4 The RANSAC Family: RANSAC → PROSAC → MAGSAC++

Even after the matching stage, **outliers (incorrect matches)** remain. Estimating a geometric model (fundamental/essential matrix) while simultaneously rejecting outliers is the role of the RANSAC family.

#### RANSAC (Random Sample Consensus, 1981)

A robust estimation paradigm proposed by [Fischler & Bolles (1981)](https://dl.acm.org/doi/10.1145/358669.358692):

1. Randomly sample the minimum $n$ matches required for the model (e.g., fundamental matrix with 8, 7, or 5 points)
2. Estimate the model from the sampled points
3. Form a consensus set of points (inliers) whose error to the model is within a threshold $t$ over all matches
4. Select the model with the largest consensus set
5. Re-estimate the model using all inliers

**Required number of iterations**:

$$k = \frac{\log(1 - p)}{\log(1 - w^n)}$$

- $p$: desired success probability (typically 0.99)
- $w$: inlier ratio
- $n$: minimum number of points required by the model

Example: with 50% inliers, an 8-point model, and 99% confidence → $k \approx 1177$ iterations.

```python
# Estimate the fundamental matrix with RANSAC
pts1 = np.float32([kp1[m.queryIdx].pt for m in good_matches])
pts2 = np.float32([kp2[m.trainIdx].pt for m in good_matches])

#### PROSAC (Progressive Sample Consensus, 2005)

Proposed by Chum & Matas. While RANSAC samples uniformly at random, PROSAC **sorts matches by quality (e.g., descriptor distance)** and progressively samples from the top.

Intuition: better matches have higher probability of being inliers, so trying the model on the top matches first finds a good model more quickly. Tens of times faster convergence than RANSAC.

#### MAGSAC / MAGSAC++ (2019/2020)

Proposed by Barath et al. One of the core problems of RANSAC is the **manual setting of the threshold $t$**. MAGSAC automates this:

- Marginalize the quality of the model over all possible thresholds $\sigma$:

$$Q(\theta) = \int_0^{\sigma_{\max}} q(\theta, \sigma) f(\sigma) d\sigma$$

Here, $q(\theta, \sigma)$ is the quality of the model $\theta$ at threshold $\sigma$, and $f(\sigma)$ is the prior over thresholds.

- MAGSAC++ implements this more efficiently and adds a weighted least-squares fit based on $\sigma$-consensus.
- As a result, sensitivity to threshold selection is substantially reduced.

```python
# OpenCV's USAC (includes MAGSAC++)
F, mask = cv2.findFundamentalMat(
    pts1, pts2,
    cv2.USAC_MAGSAC,           # use MAGSAC++
    ransacReprojThreshold=1.0,  # less sensitive
    confidence=0.999,
    maxIters=10000
)
```

### 5.3.5 Fundamental / Essential Matrix Estimation

The process of estimating the geometric relationship between two cameras from 2D-2D matches:

**Fundamental Matrix** ($\mathbf{F}$, 7 DOF):
- Used when intrinsic parameters are unknown
- 8-point algorithm: solve the linear system from at least 8 correspondences and enforce the rank-2 constraint
- 7-point algorithm: at least 7 correspondences, up to 3 solutions as roots of a cubic polynomial

**Essential Matrix** ($\mathbf{E}$, 5 DOF):
- Used when intrinsic parameters are known
- $\mathbf{E} = [\mathbf{t}]_\times \mathbf{R}$, $\mathbf{E} = \mathbf{K}_2^\top \mathbf{F} \mathbf{K}_1$
- 5-point algorithm (Nistér, 2004): at least 5 correspondences, up to 10 solutions via a 10th-order polynomial

Used in combination with RANSAC:

```python
# Essential matrix estimation (calibrated camera)
E, mask = cv2.findEssentialMat(pts1, pts2, cameraMatrix=K,
                                method=cv2.RANSAC,
                                prob=0.999, threshold=1.0)

# Recover R, t from the essential matrix
_, R, t, mask_pose = cv2.recoverPose(E, pts1, pts2, cameraMatrix=K)
```

### 5.3.6 Technical Lineage: The Evolution of Robust Estimation

```
Least Squares (vulnerable to outliers)
    ↓ handling outliers
RANSAC (1981)   — the first robust estimation paradigm
    ↓ exploiting prior information
PROSAC (2005)   — progressive sampling based on match quality
    ↓ threshold automation
MAGSAC++ (2020) — threshold-free robust estimation
    ↓ eliminating learning-based routines
GeoTransformer (2022) — direct transformation estimation without RANSAC
```

---

## 5.4 Mutual Information & Intensity-Based Registration

### 5.4.1 Definition and Intuition of MI

**Mutual Information (MI)** is an information-theoretic measure of how much information two random variables share about each other.

Mutual information of two random variables $X, Y$:

$$I(X; Y) = \sum_{x \in X} \sum_{y \in Y} p(x, y) \log \frac{p(x, y)}{p(x) p(y)}$$

For continuous variables:

$$I(X; Y) = \int \int p(x, y) \log \frac{p(x, y)}{p(x) p(y)} \, dx \, dy$$

Equivalent expression using entropy:

$$I(X; Y) = H(X) + H(Y) - H(X, Y)$$

Here:
- $H(X) = -\sum_x p(x) \log p(x)$: entropy of $X$
- $H(X, Y) = -\sum_{x,y} p(x, y) \log p(x, y)$: joint entropy

**Intuition**: when two images are correctly aligned, knowing a pixel value in one image allows better prediction of the corresponding pixel in the other. That is, $I(X; Y)$ is maximized. When alignment drifts, the relationship between the two images weakens and $I(X; Y)$ decreases.

Key property: MI measures **nonlinear statistical dependence** between two variables. This means it can capture relationships that a simple correlation coefficient cannot.

### 5.4.2 MI-Based Multi-Modality Registration

The true value of MI lies in its ability **to register sensor data across different modalities**. This is related to the fact that the method was originally developed for medical imaging to register CT-MRI.

Why does MI work across multiple modalities?

- CT and MRI of the same object have completely different intensity distributions (bone can be bright in CT and dark in MRI).
- A simple intensity difference (SSD) or correlation (NCC) cannot model such nonlinear relationships.
- MI measures only the **statistical dependence** between intensity values, so it works for monotonic or non-monotonic relationships between intensities.

Application in robotics: **camera-LiDAR registration**. LiDAR intensity images and camera images measure completely different physical quantities, but they reflect the same structures in the same scene, so MI is high.

### 5.4.3 Practical Computation of MI and NMI

When applying MI to image registration, the probability distributions $p(x), p(y), p(x, y)$ are estimated from a **joint histogram**.

When images $A$ and $B$ are aligned by the transformation $T$:
1. At each common location $(u, v)$, collect intensity pairs $A(u, v)$ and $B(T(u, v))$.
2. Estimate the joint distribution $p(a, b)$ from a 2D histogram.
3. Marginal distributions $p(a), p(b)$ are the row/column sums of the histogram.
4. Compute MI.

**Normalized Mutual Information (NMI)** normalizes MI to reduce sensitivity to overlap area size:

$$NMI(A, B) = \frac{H(A) + H(B)}{H(A, B)}$$

Or:

$$NMI(A, B) = \frac{2 \cdot I(A; B)}{H(A) + H(B)}$$

NMI remains stable when the overlap area varies, so it is preferred over MI in practice.

### 5.4.4 MI Gradient Computation

To use MI as the objective for registration, we must compute the gradient with respect to the transformation parameters.

The gradient of MI with respect to a transformation $T_\xi$ (parameters $\xi$):

$$\frac{\partial I}{\partial \xi} = \sum_{a, b} \frac{\partial p(a, b)}{\partial \xi} \left(1 + \log \frac{p(a, b)}{p(a) p(b)}\right)$$

When the joint histogram is discrete the gradient does not exist, so differentiable histogram estimation methods based on a **Parzen window (kernel density estimation)** or **B-splines** are used.

In practice, derivative-free optimization such as **Nelder-Mead simplex** is often used instead of gradient-based optimization (used in the calibration tool of Koide et al., 2023).

### 5.4.5 Why MI Is Used for Calibration

In the targetless camera-LiDAR calibration of Ch.3, MI (or NMI, NID) is used as the core cost function:

1. Project the LiDAR point cloud onto the camera image with the current extrinsic estimate
2. Compute the MI between the projected LiDAR intensity and the camera pixel intensity
3. Adjust the extrinsic parameters to maximize MI

**Normalized Information Distance (NID)** is an MI-based distance metric used in the general-purpose calibration tool of Koide et al. (2023):

$$NID(A, B) = 1 - \frac{I(A; B)}{H(A, B)} = \frac{H(A|B) + H(B|A)}{H(A, B)}$$

NID takes values from 0 (full dependence) to 1 (full independence) and satisfies the properties of a metric space.

```python
import numpy as np
from sklearn.metrics import mutual_info_score

def compute_mi(img_a, img_b, bins=256):
    """Compute the mutual information of two images."""
    # Joint histogram
    hist_2d, _, _ = np.histogram2d(
        img_a.ravel(), img_b.ravel(), bins=bins
    )
    # Joint probability
    pxy = hist_2d / hist_2d.sum()
    px = pxy.sum(axis=1)
    py = pxy.sum(axis=0)
    
    # MI = H(X) + H(Y) - H(X,Y)
    hx = -np.sum(px[px > 0] * np.log(px[px > 0]))
    hy = -np.sum(py[py > 0] * np.log(py[py > 0]))
    hxy = -np.sum(pxy[pxy > 0] * np.log(pxy[pxy > 0]))
    
    return hx + hy - hxy

def compute_nid(img_a, img_b, bins=256):
    """Compute the Normalized Information Distance."""
    mi = compute_mi(img_a, img_b, bins)
    hist_2d, _, _ = np.histogram2d(
        img_a.ravel(), img_b.ravel(), bins=bins
    )
    pxy = hist_2d / hist_2d.sum()
    hxy = -np.sum(pxy[pxy > 0] * np.log(pxy[pxy > 0]))
    
    return 1.0 - mi / hxy if hxy > 0 else 1.0
```

---

## 5.5 Learning-Based Feature Detection & Description

Traditional feature points rely on **low-level visual cues** such as intensity gradients, corners, and blobs. For this reason they are vulnerable to illumination changes, viewpoint changes, and weather changes. Deep learning has overcome this limitation by learning more robust feature representations from large amounts of data.

### 5.5.1 SuperPoint (2018): Self-Supervised Integration of Detection and Description

[DeTone et al. (2018)](https://arxiv.org/abs/1712.07629)'s SuperPoint is the first practical deep learning pipeline to **unify keypoint detection and descriptor extraction into a single network**.

#### Homographic Adaptation: The Key Training Strategy

The most important technical contribution of SuperPoint is **a method for training a highly repeatable keypoint detector without labels**.

1. Apply 100+ random homographies to a single image
2. Detect keypoints in each transformed image
3. Inversely transform the detection results back to the original coordinate frame and aggregate
4. Adopt only the points **consistently detected across transformations** as pseudo ground-truth

Through this process, a highly repeatable keypoint detector is trained without manual labels.

#### Two-Stage Training: MagicPoint → SuperPoint

- **Stage 1**: Pretrain a corner/junction detector (**MagicPoint**) on the Synthetic Shapes dataset composed of synthetic geometric figures (triangles, rectangles, line segments, etc.).
- **Stage 2**: Apply MagicPoint with Homographic Adaptation to real images such as MS-COCO to generate pseudo ground-truth in real scenes and train SuperPoint.

#### Architecture

A VGG-style encoder (shared backbone) → branches into two decoder heads:

**Interest Point Decoder**: 
- Divide the input image into a grid of 8×8 cells
- Perform a 65-channel (64 positions + 1 "no interest point") softmax in each cell
- Generate a pixel-level keypoint heatmap by directly predicting the position within the cell

**Descriptor Decoder**: 
- Output a 256-dimensional descriptor map from the shared backbone's feature map
- Sample at detected keypoint positions using bi-cubic interpolation
- Apply L2 normalization

#### Training Loss

- Keypoint detection: cross-entropy loss
- Descriptor: since correspondences are known via the homography, hinge loss on positive/negative pairs:

$$L_{desc} = \sum_{(i,j) \in \text{pos}} \max(0, m_p - \mathbf{d}_i^\top \mathbf{d}_j) + \sum_{(i,j) \in \text{neg}} \max(0, \mathbf{d}_i^\top \mathbf{d}_j - m_n)$$

Here, $m_p, m_n$ are the positive/negative margins.

**Performance**: detection and description are performed jointly in a single forward pass. About 70 FPS on a 640×480 image (GPU).

```python
import torch
# SuperPoint usage example (hloc / kornia)
from kornia.feature import SuperPoint as KorniaSuperPoint

# Load model
sp = KorniaSuperPoint(max_num_keypoints=2048)
sp = sp.eval()

# Inference
with torch.no_grad():
    img_tensor = torch.from_numpy(img).float().unsqueeze(0).unsqueeze(0) / 255.0
    pred = sp(img_tensor)
    keypoints = pred['keypoints']        # (1, N, 2)
    descriptors = pred['descriptors']    # (1, 256, N)
    scores = pred['scores']              # (1, N)
```

### 5.5.2 D2-Net (2019): Detect-and-Describe Jointly

[D2-Net (Dusmanu et al., 2019)](https://arxiv.org/abs/1905.03561) is a method that integrates detection and description even more aggressively. Whereas SuperPoint still separates the detection head and the description head, D2-Net **performs detection and description simultaneously from the same feature map**.

Key idea: using the intermediate feature map $\mathbf{F} \in \mathbb{R}^{H \times W \times C}$ of VGG16:
- **Detection**: take the maximum value along the channel axis at each position and apply spatial NMS to select keypoints
- **Description**: use the $C$-dimensional vector at the same location as the descriptor

Advantage: uses higher-level semantic features, so robust to large appearance changes.
Disadvantage: detection repeatability may be lower than SuperPoint, and localization is only possible up to 1/4 of the input resolution.

### 5.5.3 R2D2 (2019): Reliable and Repeatable Detector-Descriptor

[R2D2 (Revaud et al., 2019)](https://arxiv.org/abs/1906.06195) analyzes the limitations of SuperPoint and D2-Net and proposes a method that **explicitly trains for repeatability and reliability**.

- **Repeatability**: is the same point detected across various viewpoints?
- **Reliability**: is the descriptor of the detected point useful for matching? (A keypoint in a textureless area may be repeatable but useless for matching.)

R2D2 predicts the two as separate confidence maps and multiplies them to determine the final keypoint score.

### 5.5.4 DISK (2020)

[DISK (Tyszkiewicz et al., 2020)](https://arxiv.org/abs/2006.13566) trains keypoint detection from a **reinforcement learning** perspective. The detector is trained by rewarding successful matches and penalizing failed ones.

Key differentiator: by directly optimizing matching accuracy, it takes one more step toward end-to-end optimization of detection and matching.

### 5.5.5 Advantages Over Traditional Methods: Improved Illumination/Viewpoint Invariance

Specific scenarios in which learning-based feature points have an edge over traditional methods:

| Scenario | SIFT/ORB Limitations | SuperPoint/D2-Net Improvements |
|---------|-------------|---------------------|
| Extreme illumination change (day-night) | DoG/gradient-based detection fails | Learned features capture high-level structures |
| Wide viewpoint change | Limitations of affine approximation | Viewpoint invariance learned from large training data |
| Repetitive patterns | Similar descriptors lead to ambiguous matches | Context information captured for discrimination |
| Motion blur | Weakened gradients → detection failure | CNN learns robustness to blur |

However, learning-based methods also have limitations: they depend on the training data domain and performance can degrade in entirely new environments (e.g., underwater, Mars).

---

## 5.6 Learning-Based Feature Matching

### 5.6.1 SuperGlue (2020): Attention-Based Matching

[Sarlin et al. (2020)](https://arxiv.org/abs/1911.11763)'s SuperGlue recast keypoint matching as a learnable problem using **graph neural networks (GNN) and the attention mechanism**. It is the first production-grade system to replace traditional nearest-neighbor matching with learning-based matching.

#### Problem Definition: Partial Assignment

Between the keypoint sets $\mathcal{A} = \{(\mathbf{p}_i, \mathbf{d}_i)\}_{i=1}^{N}$ and $\mathcal{B} = \{(\mathbf{p}_j, \mathbf{d}_j)\}_{j=1}^{M}$ extracted from two images, we seek correspondences, but **not every keypoint has a correspondence**. To handle this, a virtual node called a "dustbin" is added to explicitly handle unmatchable points.

#### Keypoint Encoder

The keypoint position $(x, y)$ and detection confidence $c$ are embedded via an MLP and added to the descriptor vector:

$$\mathbf{f}_i^{(0)} = \mathbf{d}_i + \text{MLP}_{\text{enc}}([\mathbf{p}_i, c_i])$$

In this way, geometric information is baked into the feature.

#### Attentional Graph Neural Network

Keypoints are represented as nodes of a graph, and self-attention and cross-attention are applied alternately:

**Self-Attention (intra-image)**: learns relationships among keypoints within the same image. For example, it captures structural information such as building corners being collinear.

$$\text{message}_i^{\text{self}} = \sum_{j \in \mathcal{A}} \text{softmax}\left(\frac{\mathbf{q}_i^\top \mathbf{k}_j}{\sqrt{d}}\right) \mathbf{v}_j$$

**Cross-Attention (inter-image)**: learns relationships between keypoints across the two images. It infers by attention "which keypoint in the other image is this keypoint similar to?"

$$\text{message}_i^{\text{cross}} = \sum_{j \in \mathcal{B}} \text{softmax}\left(\frac{\mathbf{q}_i^\top \mathbf{k}_j}{\sqrt{d}}\right) \mathbf{v}_j$$

This is alternated $L$ times (9 in the paper), progressively refining the matching information.

#### Matching via Optimal Transport

The final matching score matrix is computed as the inner product of the GNN outputs, and the **Sinkhorn algorithm** (a soft form of the Hungarian algorithm) is applied.

The score matrix $\mathbf{S} \in \mathbb{R}^{(N+1) \times (M+1)}$ (including dustbin):

$$S_{ij} = \langle \mathbf{f}_i^{(L)}, \mathbf{f}_j^{(L)} \rangle, \quad S_{i,M+1} = S_{N+1,j} = z$$

Here, $z$ is a learnable dustbin score.

Sinkhorn normalization is applied iteratively (about 100 times):

$$\mathbf{S} \leftarrow \text{row-normalize}(\mathbf{S}), \quad \mathbf{S} \leftarrow \text{col-normalize}(\mathbf{S})$$

After convergence, the soft assignment matrix is thresholded to produce the final matches.

#### Training

End-to-end training by maximizing the negative log-likelihood over ground-truth correspondences (generated from a homography or from relative pose + depth map):

$$L = -\sum_{(i,j) \in \mathcal{M}} \log \hat{P}_{ij} - \sum_{i \in \mathcal{U}_A} \log \hat{P}_{i, M+1} - \sum_{j \in \mathcal{U}_B} \log \hat{P}_{N+1, j}$$

Here, $\mathcal{M}$ is the set of matched pairs and $\mathcal{U}_A, \mathcal{U}_B$ are the sets of unmatched keypoints.

```python
# SuperGlue usage example (hloc)
from hloc import match_features, extract_features
from hloc.utils.io import list_h5_names

# SuperPoint feature extraction
feature_conf = extract_features.confs['superpoint_aachen']
features_path = extract_features.main(feature_conf, images_dir)

# SuperGlue matching
match_conf = match_features.confs['superglue']
matches_path = match_features.main(match_conf, pairs_path, feature_conf['output'], features_path)
```

#### Position in the Technical Lineage

If SuperPoint learned detection+description, SuperGlue **learned the matching stage**. With this, **all three stages** of the detect-then-describe-then-match pipeline **were replaced by deep learning**. However, the sequential three-stage structure of the pipeline itself is still maintained. It is LoFTR that broke this structural limitation.

### 5.6.2 LightGlue (2023): Making SuperGlue Efficient

[Lindenberger et al. (2023)](https://arxiv.org/abs/2306.13643)'s LightGlue maintains SuperGlue's accuracy while drastically improving speed through **adaptive computation**.

#### Diagnosis of SuperGlue's Problems

- Always performs a fixed 9 GNN layers and 100 Sinkhorn iterations → unnecessarily many operations even for easy matches.
- $O(N^2)$ attention is repeated over the number of keypoints $N$, so it slows down drastically as the number of keypoints grows.

#### Key Improvements: Adaptive Depth & Width

**Adaptive Depth (early layer exit)**:
- A lightweight classifier (MLP) after each layer predicts matching confidence.
- If confidence is sufficiently high, the network terminates early.
- Easy image pairs are processed in only 2-3 layers, while difficult pairs use all 9 layers.

**Adaptive Width (keypoint pruning)**:
- Keypoints deemed unmatchable are pruned in intermediate layers.
- The sequence length of attention gradually decreases, lightening the computation of later layers.

**Removing Sinkhorn**: 
Instead of optimal transport, matching is done with simple **dual-softmax + mutual nearest neighbor**. The iteration cost of Sinkhorn is eliminated entirely with virtually no performance degradation.

$$P_{ij} = \text{softmax}_j(S_{ij}) \cdot \text{softmax}_i(S_{ij})$$

#### Training Strategy

- During training, adaptive termination is not used; the full network is run and supervision is applied to the output of each layer (**deep supervision**).
- Adaptive termination is activated only at inference.
- Designed for general compatibility with various local feature detectors including SuperPoint, DISK, and ALIKED.

**Performance**: at accuracy on par with SuperGlue, it is **3-5× faster**. On easy pairs, speedups of up to 10× or more are achieved.

```python
# LightGlue usage example (kornia / hloc)
from kornia.feature import LightGlue as KorniaLightGlue

# SuperPoint + LightGlue combination
lg = KorniaLightGlue(features='superpoint')
lg = lg.eval()

with torch.no_grad():
    # pred0, pred1: SuperPoint outputs
    matches = lg({'image0': pred0, 'image1': pred1})
    # matches['matches']: (K, 2) — pairs of matched keypoint indices
    # matches['scores']: (K,) — match confidence
```

### 5.6.3 Technical Flow: Deep-Learning Replacement of the Separated Detect → Describe → Match Pipeline

```
[Traditional pipeline]
SIFT detect → SIFT descriptor → BF/FLANN + ratio test
                                        ↓
[Learning-based replacements]
SuperPoint detect+describe → SuperGlue attention matching → LightGlue efficiency
    (2018)                       (2020)                        (2023)
```

The core narrative of this evolution: **each stage of the pipeline is replaced one by one with deep learning, while the three-stage serial structure itself is preserved**. The advantage of this structure is modularity and interpretability; the disadvantage is that a failure in the detection stage leads to failure of the entire pipeline. The next section's detector-free paradigm breaks this fundamental limitation.

---

## 5.7 Detector-Free Matching (Paradigm Shift)

### 5.7.1 Why Detector-Free Is Needed

The detect-then-match pipeline has the fundamental limitation that **matching is impossible if the detector fails to find keypoints**. This is critical in real-world environments such as:

- **Textureless regions**: white walls, concrete floors, sky — weak gradients cause corner/blob detection to fail
- **Repetitive patterns**: tiles, window grids — detection succeeds but descriptors are similar, leading to ambiguous matches
- **Wide viewpoint changes**: at extreme viewpoint differences, the appearance of local patches changes completely

The detector-free approach eliminates the detection stage and **treats every position in the image as a potential matching candidate**, overcoming this limitation.

### 5.7.2 LoFTR (2021): Coarse-to-Fine Transformer Matching

[Sun et al. (2021)](https://arxiv.org/abs/2104.00680)'s LoFTR is **a transformer-based architecture that performs dense matching between two images without a detector**, opening a new paradigm of detector-free matching.

#### Architecture

**1. Local Feature CNN**: a ResNet-18-based FPN (Feature Pyramid Network) extracts coarse (1/8 resolution) and fine (1/2 resolution) feature maps from the two images.

**2. Coarse-Level Matching (Transformer)**:

The 1/8 feature maps are flattened into 1D sequences, and a transformer module with self-attention + cross-attention applied alternately $N$ times (typically 4) is used.

Positional encoding: sinusoidal positional encoding preserves spatial information.

A score matrix is computed by inner products of the output features, and **dual-softmax** produces a confidence matrix:

$$P_{ij} = \text{softmax}_j(\mathbf{f}_i^A \cdot \mathbf{f}_j^B) \cdot \text{softmax}_i(\mathbf{f}_i^A \cdot \mathbf{f}_j^B)$$

Coarse matches are extracted by thresholding + the mutual nearest neighbor condition.

**3. Fine-Level Refinement**:

For each coarse match, a $w \times w$ window (typically 5×5) around the corresponding position is cropped from the 1/2-resolution feature map.
Cross-attention-based correlation is again performed within the window to regress the **sub-pixel-accurate** match location.

#### Linear Transformer

To reduce computation, **kernel-based linear attention** is used instead of standard softmax attention:

Standard attention: $O(N^2)$, $\text{Attn}(\mathbf{Q}, \mathbf{K}, \mathbf{V}) = \text{softmax}(\mathbf{Q}\mathbf{K}^\top / \sqrt{d}) \mathbf{V}$

Linear attention: $O(N)$, $\text{Attn}(\mathbf{Q}, \mathbf{K}, \mathbf{V}) = \phi(\mathbf{Q})(\phi(\mathbf{K})^\top \mathbf{V})$

Here, $\phi$ is an ELU-based kernel function. By changing the associativity of the matrix products, $O(N)$ complexity is achieved.

However, subsequent work revealed that standard attention has the edge in accuracy.

#### Training

Correspondences generated from ground-truth pose + depth map are used as supervision:
- Coarse level: cross-entropy loss
- Fine level: L2 regression loss
- Trained on ScanNet (indoor) and MegaDepth (outdoor) datasets

```python
# LoFTR usage example (kornia)
from kornia.feature import LoFTR as KorniaLoFTR

loftr = KorniaLoFTR(pretrained='outdoor')
loftr = loftr.eval()

with torch.no_grad():
    input_dict = {
        'image0': img0_tensor,  # (1, 1, H, W) grayscale
        'image1': img1_tensor,
    }
    result = loftr(input_dict)
    
    mkpts0 = result['keypoints0']     # (K, 2) — matched coordinates in image 0
    mkpts1 = result['keypoints1']     # (K, 2) — matched coordinates in image 1
    confidence = result['confidence'] # (K,) — match confidence
```

#### Position in the Technical Lineage

LoFTR is a **paradigm shift**. It breaks away from the detect-then-match pipeline that flowed from SuperPoint to SuperGlue and removes the detector entirely. The key insight is **that the transformer's attention can perform detection and matching simultaneously**. Since every position in one image communicates directly (via cross-attention) with every position in the other, matching is possible without a separate detection stage.

### 5.7.3 QuadTree Attention: Making LoFTR Efficient

LoFTR's coarse-level transformer flattens every location in the image into a sequence, so the sequence length grows rapidly as resolution increases. QuadTree Attention (Tang et al., 2022) addresses this.

Key idea: perform attention hierarchically.

1. Perform full attention at the coarsest resolution
2. Select only the regions with high attention scores
3. Perform the next resolution's attention only in the selected regions
4. Repeat to realize hierarchical attention that concentrates on relevant regions

This reduces complexity from $O(N^2)$ to $O(N \log N)$ while maintaining LoFTR-level accuracy.

### 5.7.4 ASpanFormer (2022): Adaptive Span Attention

ASpanFormer (Chen et al., 2022) addresses another limitation of LoFTR: **must every location have the same attention span?**

Key idea: adaptively adjust the **attention span** per location.

- Texture-rich regions: precise matching with a narrow span
- Texture-poor regions: context-aware matching with a wide span

This mitigates the matching accuracy degradation LoFTR exhibited in textureless regions.

### 5.7.5 RoMa (2024): Maturation of DINOv2 + Dense Matching

[Edstedt et al. (2024)](https://arxiv.org/abs/2305.15404)'s RoMa represents the **maturation** of detector-free matching. Through two key evolutions it substantially surpasses the LoFTR family.

#### Leveraging a Foundation Model

Unlike LoFTR, which was trained from scratch, RoMa **uses a pretrained DINOv2 ViT-Large as the feature extractor** (weights frozen).

DINOv2 is a general-purpose visual feature learned by large-scale self-supervised learning, and it already contains rich semantic information. This is a philosophical shift: "leave feature learning to the general model and only learn the matching logic."

#### Architecture

**1. Frozen DINOv2 Backbone**: extracts patch features at 1/14 resolution from a pretrained DINOv2 ViT-Large.

**2. Coarse Matching (Warp Estimation)**:
- Cross-attention (transformer decoder) is applied to DINOv2 features.
- For each location in image A, the corresponding location in image B is predicted as a **probability distribution**:

$$p(\mathbf{x}_B | \mathbf{x}_A) = \sum_{k} w_k \mathcal{N}(\mathbf{x}_B; \mu_k, \Sigma_k)$$

By predicting a probability distribution rather than a single point, the uncertainty of ambiguous matches is explicitly represented.

**3. Fine Matching (Iterative Refinement)**:
- Starting from the coarse warp, CNN-based fine-level features are used to iteratively refine.
- At each refinement step, the resolution is increased; based on the previous warp, local correlation is computed and the residual is predicted.
- The philosophy is similar to RAFT's iterative refinement, but applied to sparse-to-dense warp rather than optical flow.

**4. Certainty Estimation**: certainty is predicted together with each match, so that only high-confidence matches can be used selectively in post-processing.

#### Robust Regression

Instead of regressing the matching position with a simple L2 loss, the **negative log-likelihood** between the predicted probability distribution and the ground truth is optimized:

$$L = -\sum_{(\mathbf{x}_A, \mathbf{x}_B^*)} \log p(\mathbf{x}_B^* | \mathbf{x}_A)$$

This approach is robust to outliers: even if there are incorrect ground-truth correspondences, they are absorbed into the tail of the distribution, keeping training stable.

#### Performance

On the MegaDepth and ScanNet benchmarks it substantially surpasses LoFTR, ASpanFormer, and others. The performance improvement is especially pronounced under **wide baselines (large viewpoint changes)**.

```python
# RoMa usage example
from romatch import roma_outdoor

# Load model (includes DINOv2 backbone)
roma_model = roma_outdoor(device='cuda')
roma_model.eval()

# Matching
warp, certainty = roma_model.match(img0_path, img1_path)

# Extract only high-confidence matches
matches, certainty_scores = roma_model.sample(
    warp, certainty,
    num=5000  # maximum number of matches
)
# matches: (K, 4) — [x0, y0, x1, y1] normalized coordinates
```

#### Position in the Technical Lineage

RoMa embodies two key transitions:

1. **Leveraging a foundation model**: training from scratch → training only the matching logic on top of features from a large pretrained model
2. **Probabilistic matching**: deterministic point prediction → probability distribution prediction that explicitly handles uncertain matches

It combines RAFT's iterative refinement idea with LoFTR's detector-free mindset while introducing the powerful pretrained features of DINOv2 — a comprehensive advance.

### 5.7.6 Recent Developments: 3D-Aware Dense Matching (2024-2025)

In 2024-2025, a paradigm has emerged that goes beyond 2D matching to **directly predict 3D geometry while performing matching**.

**DUSt3R (Leroy et al., 2024)**: [DUSt3R](https://arxiv.org/abs/2312.14132) is a method that directly regresses a 3D pointmap from an arbitrary image pair without any calibration or pose information. Whereas existing matching pipelines followed the order "2D matching → 3D reconstruction," DUSt3R reverses this to **directly predict the 3D structure itself and treat correspondences as a natural byproduct obtained in 3D space**.

**MASt3R (Leroy et al., 2024)**: [MASt3R](https://arxiv.org/abs/2406.09756) adds a dense local feature head to DUSt3R, **substantially strengthening dense matching performance** along with 3D pointmap prediction. On the map-free localization benchmark it achieves a 30%p (absolute) VCRE AUC improvement over the previous best method.

**VGGT (Wang et al., 2025)**: [VGGT](https://arxiv.org/abs/2503.11651) (Visual Geometry Grounded Transformer) is the CVPR 2025 Best Paper, which **simultaneously infers camera parameters, pointmap, depth map, and 3D point tracks in a single feed-forward pass** from one or more images. With inference time under one second, it achieves accuracy exceeding existing methods that require post-processing optimization.

This trend dismantles the long-standing assumption that "matching is a 2D problem" and forms a new paradigm that **directly reasoning about 3D geometry ultimately produces more robust matching**.

---

## 5.8 3D-3D Correspondence

In parallel with the technical lineage of 2D image matching, **research on finding correspondences between 3D point clouds** has also evolved. It is central to LiDAR scan-to-scan registration, multi-session map merging, and loop closure.

### 5.8.1 FPFH (Fast Point Feature Histograms)

[FPFH (Rusu et al., 2009)](https://ieeexplore.ieee.org/document/5152473) is a handcrafted descriptor that **encodes the geometric features of a 3D point cloud as a histogram**.

#### SPFH (Simple Point Feature Histogram)

A local coordinate frame is set up between a query point $\mathbf{p}$ and its neighbor $\mathbf{p}_k$, and three angular features are computed.

From the normal vectors $\mathbf{n}_p, \mathbf{n}_k$ and the direction vector $\mathbf{d} = \mathbf{p}_k - \mathbf{p}$:

$$\mathbf{u} = \mathbf{n}_p$$
$$\mathbf{v} = \mathbf{u} \times \frac{\mathbf{d}}{\|\mathbf{d}\|}$$
$$\mathbf{w} = \mathbf{u} \times \mathbf{v}$$

Three angular features:
$$\alpha = \mathbf{v} \cdot \mathbf{n}_k, \quad \phi = \mathbf{u} \cdot \frac{\mathbf{d}}{\|\mathbf{d}\|}, \quad \theta = \text{atan2}(\mathbf{w} \cdot \mathbf{n}_k, \mathbf{u} \cdot \mathbf{n}_k)$$

Each feature is quantized into a $B$-bin histogram.

#### FPFH: An Accelerated Version of SPFH

SPFH computes features over all neighbor pairs within radius $r$, so its complexity is $O(k^2)$. FPFH approximates this to reduce it to $O(k)$:

$$\text{FPFH}(\mathbf{p}) = \text{SPFH}(\mathbf{p}) + \frac{1}{k} \sum_{i=1}^{k} \frac{1}{w_i} \text{SPFH}(\mathbf{p}_i)$$

Here, $w_i = \|\mathbf{p}_i - \mathbf{p}\|$. A 33-dimensional histogram (11 bins × 3 angles).

```python
import open3d as o3d

# Compute FPFH
pcd = o3d.io.read_point_cloud("scan.ply")
pcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))

fpfh = o3d.pipelines.registration.compute_fpfh_feature(
    pcd,
    o3d.geometry.KDTreeSearchParamHybrid(radius=0.25, max_nn=100)
)
# fpfh.data.shape: (33, N)
```

### 5.8.2 3DMatch (2017): The Beginning of Learned 3D Descriptors

[Zeng et al. (2017)](https://arxiv.org/abs/1603.08182)'s 3DMatch is the first system to **extract 3D matching descriptors through learning** on RGB-D data.

- **Training data**: correspondence pairs automatically generated from RGB-D reconstructions of 62 indoor scenes
- **Architecture**: a 3D CNN with a 3D TDF (Truncated Distance Function) volume as input
- **Output**: 512-dimensional local descriptor

3DMatch is the starting point of learned 3D descriptors, and it also provided a standard benchmark called the **3DMatch Benchmark**, serving as the evaluation standard for subsequent research.

### 5.8.3 FCGF (Fully Convolutional Geometric Features, 2019)

[Choy et al. (2019)](https://arxiv.org/abs/1904.09793)'s FCGF uses **sparse convolution** to extract descriptors for all points in the entire point cloud in a single forward pass.

While 3DMatch processes each keypoint's local volume individually, FCGF processes the entire point cloud at once, so it is **tens to hundreds of times faster**. With a 32-dimensional descriptor — more compact than 3DMatch's 512 dimensions — it achieved even higher accuracy.

### 5.8.4 Predator (2021): Overlap-Aware 3D Matching

[Huang et al. (2021)](https://arxiv.org/abs/2011.13005)'s Predator is an approach that **explicitly predicts the overlap region** of two point clouds.

Existing methods extract descriptors uniformly over all regions of both point clouds, but in reality only parts of them overlap. Predator:

1. **Overlap attention**: predicts the degree to which each point overlaps with the other point cloud via cross-attention
2. **Matchability score**: separately scores points useful for matching (distinctive geometric structures) within the overlap region
3. Achieves substantial gains in difficult scenarios with low overlap (10-30%)

### 5.8.5 GeoTransformer (2022): Geometric Transformer

[Qin et al. (2022)](https://arxiv.org/abs/2202.06688)'s GeoTransformer is an innovative method in 3D point cloud registration that simultaneously achieves **learning of geometric invariant features and removal of RANSAC**.

#### Keypoint-Free Superpoint Matching

Instead of repeatable keypoint detection, correspondences are found on downsampled **superpoints**, then propagated to dense points.

#### Geometric Transformer Architecture

The core is learning **geometric features invariant to rigid transformations**. Two geometric encodings are used:

**1. Pairwise Distance Encoding**:

The Euclidean distance $d_{ij} = \|\mathbf{p}_i - \mathbf{p}_j\|$ between superpoints $\mathbf{p}_i, \mathbf{p}_j$ is invariant to rigid transformations. It is used as a positional encoding:

$$\text{PE}(d_{ij}) = [\sin(\omega_1 d_{ij}), \cos(\omega_1 d_{ij}), \ldots, \sin(\omega_K d_{ij}), \cos(\omega_K d_{ij})]$$

**2. Triplet Angle Encoding**:

The angle formed by three points $\mathbf{p}_i, \mathbf{p}_j, \mathbf{p}_k$ is also invariant to rigid transformations. This angle information is used as an additional encoding to more richly capture geometric structure.

These geometric encodings are injected as attention biases into the transformer:

$$\text{Attn}_{ij} = \frac{\mathbf{q}_i^\top \mathbf{k}_j}{\sqrt{d}} + b(\text{PE}(d_{ij}))$$

#### RANSAC-Free Transformation Estimation

Because robust correspondences at the superpoint level achieve high inlier ratios, **the transformation can be estimated directly without RANSAC**. This yields a **100× speedup**.

#### Performance

On the 3DLoMatch benchmark, the inlier ratio improves by 17-30%p and registration recall by over 7%p. In particular, large improvements over existing methods in the **low-overlap (10-30%)** regime.

```python
# GeoTransformer usage example
from geotransformer.utils.pointcloud import apply_transform
import torch

# Load two point clouds (N, 3)
src_points = torch.from_numpy(src_pcd.points).float().cuda()
ref_points = torch.from_numpy(ref_pcd.points).float().cuda()

# Downsample + extract superpoints
# ... (voxel downsampling)

# GeoTransformer inference
output = model(src_points, ref_points, src_feats, ref_feats)
# output['estimated_transform']: (4, 4) transformation matrix
# output['src_corr_points'], output['ref_corr_points']: correspondences
```

### 5.8.6 Technical Lineage of 3D-3D Correspondence

```
[Handcrafted 3D Descriptors]
FPFH (2009)          — geometric angular histograms
SHOT (2010)          — orientation histograms
    ↓ learning-based
[Learned 3D Descriptors]
3DMatch (2017)       — first learned 3D descriptor + benchmark
    ↓ efficiency
FCGF (2019)          — sparse conv, processes entire point cloud at once
    ↓ overlap-aware
Predator (2021)      — overlap-aware cross-attention
    ↓ RANSAC removal
GeoTransformer (2022) — geometric invariant transformer, RANSAC-free
```

---

## 5.9 Cross-Modal Correspondence

### 5.9.1 2D-3D Matching: Camera-LiDAR

The most common cross-modal correspondence problem is **matching between 2D images and 3D point clouds**. Application scenarios:

- **Camera-LiDAR extrinsic calibration**: find the same physical point observed by both sensors to estimate the extrinsic parameters
- **Visual localization against a LiDAR map**: localize a camera image against a LiDAR map
- **Loop closure**: cross-validate camera and LiDAR observations

### 5.9.2 Image-to-Point Cloud Matching Approaches

#### Projection-Based Approach

The most straightforward method is to project the 3D point cloud onto the 2D image plane to reduce the problem to 2D-2D matching:

1. Render the LiDAR point cloud into a virtual camera view to generate depth/intensity images
2. Perform 2D matching (e.g., SuperGlue) between the generated 2D image and the camera image
3. Back-project the 2D matching results to 3D coordinates

The calibration tool of Koide et al. (2023) uses this approach: the LiDAR dense point cloud is rendered with a virtual camera, and SuperGlue is used to detect cross-modal 2D-3D correspondences with the camera image.

#### Learning-Based Direct Matching

LCD (LiDAR-Camera Descriptor): learns a common embedding space for 2D image patches and 3D point cloud patches.

P2-Net (Yu et al., 2021): learns patch-to-point matching to infer direct correspondences between 2D image patches and 3D points.

### 5.9.3 Why Cross-Modal Is Difficult: The Representation Gap

Why 2D-3D cross-modal matching is fundamentally difficult:

1. **Representation heterogeneity**: 2D images are intensity/color values on a regular grid, while 3D point clouds are irregularly distributed coordinates + intensity. The data structures themselves are different.

2. **Information asymmetry**: images provide rich texture information but lack depth, while point clouds provide accurate geometric information but have sparse texture.

3. **Density gap**: the resolution of camera images (millions of pixels) differs greatly from the density of LiDAR point clouds (tens of thousands to hundreds of thousands of points), and LiDAR density changes drastically with distance.

4. **Appearance domain gap**: even for the same object, camera albedo and LiDAR reflection intensity measure different physical quantities.

Because of these difficulties, cross-modal correspondence is still a less mature research area than unimodal (2D-2D or 3D-3D) matching. The MI-based approach (Section 5.4) is a strategy that statistically bypasses this domain gap, while the projection-based approach is a strategy that reduces the problem to the same modality.

---

## 5.10 Dense Matching & Optical Flow

The methods discussed so far have focused on **sparse correspondence**. This section covers **dense matching**, which finds correspondences for every pixel of an image.

### 5.10.1 Classical Optical Flow: Lucas-Kanade, Horn-Schunck

#### Definition of Optical Flow

Optical flow represents the **apparent motion of each pixel** in an image sequence as a 2D vector field:

$$\mathbf{u}(x, y) = (u(x, y), v(x, y))$$

**Brightness constancy assumption**:

$$I(x, y, t) = I(x + u, y + v, t + 1)$$

A first-order Taylor expansion yields the **optical flow constraint equation**:

$$I_x u + I_y v + I_t = 0$$

Here, $I_x = \frac{\partial I}{\partial x}$, $I_y = \frac{\partial I}{\partial y}$, $I_t = \frac{\partial I}{\partial t}$.

Because this single equation has two unknowns $(u, v)$, an additional constraint is needed (aperture problem).

#### Lucas-Kanade (1981)

**Local consistency assumption**: optical flow is assumed constant within a small window $\Omega$. For all pixels in the window:

That is, $\mathbf{A} \mathbf{u} = -\mathbf{b}$. Since this is an overdetermined system, least squares gives:

$$\begin{bmatrix} u \\ v \end{bmatrix} = (\mathbf{A}^\top \mathbf{A})^{-1} \mathbf{A}^\top (-\mathbf{b})$$

Here, $\mathbf{A}^\top \mathbf{A} = \begin{bmatrix} \sum I_x^2 & \sum I_x I_y \\ \sum I_x I_y & \sum I_y^2 \end{bmatrix}$ is identical to the structure tensor $\mathbf{M}$ of the Harris corner detector.

**Limitation**: for large displacements the local linear approximation fails. To address this, it is applied coarse-to-fine over an **image pyramid** (Pyramidal LK).

```python
# Pyramidal Lucas-Kanade (OpenCV)
lk_params = dict(
    winSize=(21, 21),
    maxLevel=3,       # pyramid levels
    criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 30, 0.01)
)

# Select points to track (typically FAST/Shi-Tomasi corners)
p0 = cv2.goodFeaturesToTrack(prev_gray, maxCorners=200, qualityLevel=0.3, minDistance=7)

# Lucas-Kanade tracking
p1, status, err = cv2.calcOpticalFlowPyrLK(prev_gray, next_gray, p0, None, **lk_params)
# status: whether tracking succeeded (1/0)
```

#### Horn-Schunck (1981)

**Global smoothness assumption**: optical flow is assumed to vary smoothly across the image, and the following energy function is minimized:

$$E = \iint \left[ (I_x u + I_y v + I_t)^2 + \lambda^2 (\|\nabla u\|^2 + \|\nabla v\|^2) \right] dx \, dy$$

The first term is the data term (brightness constancy) and the second is smoothness regularization. The larger $\lambda$, the smoother the flow.

Solving the Euler-Lagrange equations yields an iterative update formula. It produces a dense flow, but has the limitation of not handling discontinuities at boundaries well.

### 5.10.2 Learning-Based Optical Flow: FlowNet → RAFT → FlowFormer → UniMatch

#### FlowNet / FlowNet 2.0 (2015/2017)

[Dosovitskiy et al. (2015)](https://arxiv.org/abs/1504.06852)'s FlowNet is the first deep learning method to **directly predict optical flow with a CNN**. An encoder-decoder structure that takes two images as input and outputs the flow field.

[FlowNet 2.0 (Ilg et al., 2017)](https://arxiv.org/abs/1612.01925) stacks multiple FlowNets to substantially improve accuracy.

#### RAFT (2020): A New Standard for Optical Flow

[Teed & Deng (2020)](https://arxiv.org/abs/2003.12039)'s RAFT presents a new architectural paradigm of **4D correlation volume + iterative GRU update**, winning the ECCV 2020 Best Paper.

**Architecture in 3 stages**:

**1. Feature Encoder**: a CNN (ResNet variant) is applied to each of the two input images to extract feature maps $\mathbf{g}_1, \mathbf{g}_2 \in \mathbb{R}^{H/8 \times W/8 \times D}$ at 1/8 resolution. A separate Context Encoder extracts the initial hidden state and context feature for the GRU from the first image.

**2. Correlation Volume Construction**: the inner product is computed for every pixel pair of the two feature maps:

$$C_{ijkl} = \sum_d g_1(i, j, d) \cdot g_2(k, l, d)$$

Generates a 4D correlation volume $\mathbf{C} \in \mathbb{R}^{H \times W \times H \times W}$. This is average-pooled over the last two dimensions to build a 4-level **correlation pyramid** (scales 1, 2, 4, 8).

Key insight: **rather than coarse-to-fine, multi-scale lookup is performed at a single resolution**.

**3. Iterative Update (GRU)**: a ConvGRU iteratively refines the current flow estimate:

At each iteration $k$:
1. Based on the current flow estimate $\mathbf{f}^k$, look up values from the correlation pyramid (references a local window around the current correspondence location)
2. Combine the correlation feature, current flow, and context feature
3. ConvGRU updates the hidden state $\mathbf{h}^k$
4. Predict the flow residual $\Delta \mathbf{f}$ from the hidden state
5. Flow update: $\mathbf{f}^{k+1} = \mathbf{f}^k + \Delta \mathbf{f}$

12 iterations at training, 12-32 at inference. It has the property that **increasing the number of iterations improves accuracy** (test-time adaptability).

**All-Pairs vs. Coarse-to-Fine**: existing methods such as PWC-Net and FlowNet first estimate coarse flow at the pyramid and then progressively refine, so large displacements missed at the coarse level cannot be recovered. RAFT **keeps all correlations at full resolution at once**, so large displacements are not missed.

**Training**: an L1 loss with ground-truth flow is applied to the predictions at every iteration, with exponentially increasing weights on later iterations:

$$L = \sum_{k=1}^{K} \gamma^{K-k} \| \mathbf{f}^k - \mathbf{f}^{gt} \|_1$$

Here, $\gamma = 0.8$.

```python
# RAFT usage example (torchvision)
from torchvision.models.optical_flow import raft_large, Raft_Large_Weights

model = raft_large(weights=Raft_Large_Weights.DEFAULT)
model = model.eval().cuda()

with torch.no_grad():
    # img1, img2: (1, 3, H, W) float tensors, range [-1, 1]
    flow_predictions = model(img1, img2)
    # flow_predictions[-1]: final flow (1, 2, H, W)
    flow = flow_predictions[-1]  # (u, v) per pixel
```

#### FlowFormer (2022)

[FlowFormer (Huang et al., 2022)](https://arxiv.org/abs/2203.16194) is a method that **replaces RAFT's GRU update with a transformer**. It tokenizes the cost volume and uses transformer self-attention to capture global context, achieving accuracy surpassing RAFT.

#### UniMatch (2023)

[Xu et al. (2023)](https://arxiv.org/abs/2211.05783)'s UniMatch handles **optical flow, stereo matching, and depth estimation in a single unified framework**. The key idea is that all three tasks reduce to the common problem of "dense correspondence between two observations."

### 5.10.3 Dense Stereo: SGM → RAFT-Stereo → UniMatch

Stereo matching is the problem of estimating the **horizontal disparity** between left and right camera images. It can be viewed as a special case of optical flow (vertical flow = 0).

#### SGM (Semi-Global Matching, Hirschmuller 2005)

The representative of traditional dense stereo. Key ideas:
- Compute the matching cost for every disparity candidate at each pixel
- Aggregate costs by 1D dynamic programming along 8 (or 16) directions
- Select the minimum of the aggregated costs to determine the disparity

$$L_r(p, d) = C(p, d) + \min\begin{cases} L_r(p-r, d) \\ L_r(p-r, d-1) + P_1 \\ L_r(p-r, d+1) + P_1 \\ \min_i L_r(p-r, i) + P_2 \end{cases}$$

Here, $P_1, P_2$ are smoothness penalties.

#### RAFT-Stereo (2021)

Applies the RAFT architecture to stereo matching. The 4D correlation volume is reduced to 3D (H×W×D), and an iterative GRU refines the disparity.

#### Unification in UniMatch

UniMatch unifies flow, stereo, and depth, so a single model adjusts the direction and range of cross-attention depending on the task:

- **Stereo**: 1D horizontal correlation
- **Flow**: 2D all-pairs correlation
- **Depth**: depth regression from monocular features

### 5.10.4 Technical Flow: From Sparse Feature to Dense Correspondence

```
[Sparse Correspondence]
Harris → SIFT → SuperPoint → SuperGlue → LightGlue
    ↓ densification
[Dense Correspondence]
Lucas-Kanade (1981)  — local window, sparse-to-dense
Horn-Schunck (1981)  — global optimization, per-pixel flow
    ↓ deep learning
FlowNet (2015)       — CNN direct prediction
[PWC-Net](https://arxiv.org/abs/1709.02371) (2018)       — cost volume + coarse-to-fine
    ↓ paradigm shift
RAFT (2020)          — all-pairs correlation + iterative GRU
    ↓ transformer
FlowFormer (2022)    — cost volume tokenization + transformer
    ↓ task unification
UniMatch (2023)      — flow + stereo + depth unified
```

RAFT's all-pairs correlation and iterative refinement ideas directly influenced not only dense matching but also **detector-free feature matching** (LoFTR, RoMa) and **learned SLAM** (DROID-SLAM).

---

## Technical Lineage Summary

We consolidate the flow of all technologies covered in this chapter into a single diagram:

```
═══════════════════════════════════════════════════════════════════════════════
                    FEATURE MATCHING & CORRESPONDENCE TECHNICAL LINEAGE
═══════════════════════════════════════════════════════════════════════════════

[2D Detection & Description]
                                                                              
Harris (1988) ─────→ SIFT (2004) ───→ FAST (2006) ─→ ORB (2011)             
  corner detection    scale-space       extreme speed    FAST+BRIEF             
  structure tensor    DoG + 128D        detection only   binary descriptor       
                      float descriptor                                        
         │                │                                                   
         │   history of the accuracy vs. speed trade-off                      
         ▼                ▼                                                   
[Learning-Based Detection & Description]                                      
                                                                              
    SuperPoint (2018) ──→ D2-Net (2019) ──→ R2D2 (2019) ──→ DISK (2020)      
      self-supervised      detect=describe    reliability      RL-based         
      homographic adapt    joint feature map  + repeatability                  
         │                                                                    
         │                                                                    
         ▼                                                                    
[Learning-Based Matching — Pipeline Preserved]                                
                                                                              
    SuperGlue (2020) ──────────────→ LightGlue (2023)                        
      GNN + cross-attention           adaptive depth/width                    
      Sinkhorn optimal transport      dual-softmax (Sinkhorn removed)         
      O(N²) × 9 layers                early exit, 3-5× faster                 
                                                                              
═══════════════════════ Paradigm Shift ═══════════════════════════════════       
                                                                              
[Detector-Free Matching — Pipeline Dissolved]                                 
                                                                              
    LoFTR (2021) ──→ QuadTree (2022) ──→ ASpanFormer (2022) ──→ RoMa (2024) 
      transformer         O(N log N)        adaptive span          DINOv2     
      coarse-to-fine       efficiency        texture-adaptive       probabilistic matching 
      detector fully removed                                        foundation 
                                                                    model leveraged
                                                                        │
═══════════════════════ 3D-Aware Shift ═══════════════════════════════════
                                                                        │
[3D-Aware Dense Matching — Beyond 2D Matching]                          ▼
                                                                              
    DUSt3R (2024) ──→ MASt3R (2024) ──→ VGGT (2025, CVPR Best Paper)  
      3D pointmap         dense local        feed-forward 3D inference 
      direct regression   feature added      camera+depth+pointmap     
      matching = 3D byproduct  stronger matching  unified single-pass inference
═══════════════════════════════════════════════════════════════════════════════

[Dense Matching & Optical Flow — Parallel Evolution]                          
                                                                              
    LK (1981) ──→ Horn-Schunck (1981) ──→ FlowNet (2015)                     
      local window   global smoothness      CNN direct prediction              
         │                                      │                             
         ▼                                      ▼                             
    PWC-Net (2018) ──→ RAFT (2020) ──→ FlowFormer (2022) ──→ UniMatch (2023)
      cost volume       4D all-pairs      transformer           flow+stereo   
      coarse-to-fine    iterative GRU     cost tokenization     +depth unified
                            │                                                 
                            │ RAFT's ideas propagate                           
                            ├──→ LoFTR (all-pairs attention)                  
                            ├──→ RoMa (iterative refinement)                  
                            └──→ DROID-SLAM (correlation + DBA)               
                                                                              
═══════════════════════════════════════════════════════════════════════════════

[3D-3D Correspondence — Independent Evolution]                                
                                                                              
    FPFH (2009) ──→ SHOT (2010) ──→ 3DMatch (2017) ──→ FCGF (2019)          
      geometric histograms  orientation histograms  learned 3D descriptor  sparse conv
                                                │                             
                                                ▼                             
                                         Predator (2021)                      
                                           overlap-aware                      
                                                │                             
                                                ▼                             
                                        GeoTransformer (2022)                 
                                           geometric transformer              
                                           RANSAC-free                        
                                                                              
═══════════════════════════════════════════════════════════════════════════════

[Cross-Modal — MI-Based Bypass Strategy]                                      
                                                                              
    MI (information theory) ──→ NMI ──→ NID (Koide et al. 2023)              
      multi-modality              normalization  LiDAR-Camera calibration    
      statistical dependence                                                  
                                                                              
═══════════════════════════════════════════════════════════════════════════════

Key narratives:
  1. A flow that replaces the traditional three-stage pipeline
     (detect → describe → match) with deep learning one step at a time:
     SIFT → SuperPoint → SuperGlue → LightGlue

2. A flow that dismantles the pipeline itself and transitions to
     end-to-end dense matching: RAFT → LoFTR → RoMa

3. The rise of leveraging foundation models: using DINOv2 features as
     the basis for matching — a paradigm shift of "leave feature learning
     to the general model and only learn the matching logic."

4. The emergence of 3D-aware matching (2024-2025): in the flow from
     DUSt3R → MASt3R → VGGT, the reverse intuition of "directly inferring
     3D lets matching follow naturally" — rather than "2D matching then
     3D reconstruction" — is achieving great success.

The latter (2, 3, 4) is increasingly dominant, but the efficiency and
  interpretability of the former (1) still hold practical value.
```

The matching techniques covered in this chapter are used in earnest from the next chapter onward. In Ch.6 we examine, through concrete systems, how these techniques are deployed in the frontend of Visual Odometry, and how the state estimation methods covered in Ch.4 are combined in the backend.

---

# Ch.6 — Visual Odometry & Visual-Inertial Odometry

Ch.4 covered the state estimation framework and Ch.5 covered feature matching techniques. We now examine the first system in which these two are actually combined: Visual Odometry (VO) and Visual-Inertial Odometry (VIO).

Visual Odometry (VO) estimates the ego-motion of a camera from image data alone, while Visual-Inertial Odometry (VIO) couples it with an IMU to gain scale observability and robustness. This chapter examines the internal structure of VO/VIO and the design choices involved in depth.

The origin of VO traces back to [Nistér et al. (2004)](https://doi.org/10.1109/CVPR.2004.1315094). That paper first defined the term "Visual Odometry" and presented a real-time ego-motion estimation system for stereo and monocular cameras. The stereo approach triangulated 3D points from the left/right cameras and then estimated the rigid-body transform between frames with a 3-point algorithm; the monocular approach estimated the Essential Matrix with a 5-point algorithm. This basic pipeline — feature detection → matching → RANSAC → motion estimation — still forms the skeleton of feature-based VO two decades later.

VO/VIO systems can be classified along three main axes:

1. **Feature-based vs Direct**: whether geometric features (corners, edges) are extracted and matched, or pixel intensities themselves are used directly.
2. **Filter vs Optimization**: whether state estimation relies on Kalman-filter-family methods or nonlinear optimization.
3. **Loosely coupled vs Tightly coupled**: whether IMU and camera are processed independently and their results fused, or their raw measurements are placed in a single optimization problem.

The combinations of these three axes have produced a wide variety of systems. This chapter dissects representative systems one by one, analyzing the rationale and consequences of each design choice.

---

## 6.1 Feature-based Visual Odometry

Feature-based VO extracts geometric features from images, establishes inter-frame correspondences, and estimates camera motion from them. It is the oldest and best-understood VO paradigm, and it remains the most widely deployed today through ORB-SLAM3.

### 6.1.1 Frontend: Detection, Tracking, Outlier Rejection

The frontend of a feature-based VO performs three core tasks.

**Feature Detection**

This step finds trackable points in a frame. Ideal feature points must have high repeatability — the same 3D point should be detected at similar locations when imaged from different viewpoints.

The Harris corner detector detects corners based on the autocorrelation matrix (also called the second moment matrix) $\mathbf{M}$ of an image patch:

$$\mathbf{M} = \sum_{(x,y) \in W} w(x,y) \begin{bmatrix} I_x^2 & I_xI_y \\ I_xI_y & I_y^2 \end{bmatrix}$$

Here $I_x, I_y$ are the image gradients, $W$ is the window, and $w$ is the weighting function. A pixel is labeled a corner if both eigenvalues of $\mathbf{M}$ are large. The Harris response function is:

$$R = \det(\mathbf{M}) - k \cdot \text{tr}(\mathbf{M})^2 = \lambda_1\lambda_2 - k(\lambda_1 + \lambda_2)^2$$

FAST (Features from Accelerated Segment Test) is a detector optimized for speed. It declares a candidate pixel $p$ a corner if at least $n$ (typically $n=9$) contiguous pixels on a radius-3 Bresenham circle around $p$ are all brighter or all darker than $p$. A pre-test examines only pixels 1, 5, 9, and 13 first to quickly reject non-corners. It is tens of times faster than Harris but is noise-sensitive and lacks orientation/scale invariance.

ORB (Oriented FAST and Rotated BRIEF) augments FAST detection with orientation and rotates the BRIEF descriptor accordingly, yielding features suited to real-time SLAM. Orientation is computed from the intensity centroid of the image patch:

$$\theta = \text{atan2}(m_{01}, m_{10}), \quad m_{pq} = \sum_{x,y} x^p y^q I(x,y)$$

The ORB-SLAM family extracts ORB features from an image pyramid (typically 8 levels, scale factor 1.2) to obtain scale invariance.

**Feature Tracking**

There are two ways to establish inter-frame correspondences:

1. **Descriptor matching**: detect features independently in each frame and match them by descriptor distance (Hamming distance for binary descriptors, L2 for float descriptors). ORB-SLAM3 uses this approach. A DBoW2 vocabulary tree restricts match candidates to speed up matching.

2. **Optical Flow Tracking**: the Lucas-Kanade (LK) tracker tracks where each feature point of the previous frame moves in the current frame. VINS-Mono uses this approach. Under the brightness constancy assumption:

$$I(x + u, y + v, t + \Delta t) = I(x, y, t)$$

A first-order Taylor expansion followed by least squares within a window $W$ yields:

$$\begin{bmatrix} u \\ v \end{bmatrix} = \left(\sum_W \begin{bmatrix} I_x^2 & I_xI_y \\ I_xI_y & I_y^2 \end{bmatrix}\right)^{-1} \sum_W \begin{bmatrix} -I_xI_t \\ -I_yI_t \end{bmatrix}$$

Running this coarse-to-fine over an image pyramid makes it possible to track even large displacements.

Trade-off between the two approaches: descriptor matching is strong under wide baselines (large viewpoint changes) and can be reused for loop closure, but detection + description + matching costs time. LK tracking is fast and offers subpixel accuracy, but tends to fail under large viewpoint changes.

**Outlier Rejection**

Mismatches (outliers) are inevitable in matching results. If they are not removed, motion estimation can be thrown off badly.

The basic approach is RANSAC (Random Sample Consensus). A model is estimated from a minimal sample, inliers are counted across the full data, and the model with the most inliers is retained. In VO:

- **2D-2D**: estimate the Essential Matrix $\mathbf{E}$ with the 5-point algorithm. Decompose $\mathbf{E} = [\mathbf{t}]_\times \mathbf{R}$ to obtain the relative pose $(R, t)$.
- **3D-2D**: PnP (Perspective-n-Point) algorithm + RANSAC. Estimate the absolute pose from correspondences between already triangulated 3D points and current 2D observations.
- **3D-3D**: an ICP variant estimates the rigid-body transform.

The core equation for outlier rejection using the epipolar constraint is:

$$\mathbf{p}_2^T \mathbf{E} \mathbf{p}_1 = 0$$

where $\mathbf{p}_1, \mathbf{p}_2$ are normalized image coordinates. Correspondences that fail to satisfy this equation are labeled outliers.

ORB-SLAM3 estimates the Fundamental Matrix and a Homography in parallel and picks the appropriate model according to scene structure (planar vs non-planar). For planar scenes, the Homography has fewer degrees of freedom and is thus more stable.

### 6.1.2 Backend: PnP, Motion-only BA, Local BA

Once the frontend provides correspondences, the backend uses them to estimate camera poses and 3D structure.

**PnP (Perspective-n-Point)**

Given correspondences between already triangulated 3D map points $\mathbf{P}_j \in \mathbb{R}^3$ and 2D observations $\mathbf{u}_j \in \mathbb{R}^2$ in the current frame, the problem is to estimate the camera pose $\mathbf{T} = (\mathbf{R}, \mathbf{t}) \in SE(3)$:

$$\mathbf{u}_j = \pi(\mathbf{R}\mathbf{P}_j + \mathbf{t})$$

Here $\pi: \mathbb{R}^3 \to \mathbb{R}^2$ is the camera projection function. A minimal solution uses four points (three points plus one for verification in the case of P3P), combined with RANSAC to handle outliers.

Once an initial solution is obtained, a nonlinear refinement is performed over all inliers.

**Motion-only Bundle Adjustment**

After obtaining an initial pose from PnP, this step fixes the 3D point positions and optimizes only the camera pose:

$$\mathbf{T}^* = \underset{\mathbf{T}}{\arg\min} \sum_{j} \rho\left(\left\|\mathbf{u}_j - \pi(\mathbf{T} \cdot \mathbf{P}_j)\right\|^2_{\Sigma_j}\right)$$

where $\rho(\cdot)$ is a robust kernel (Huber, etc.) and $\Sigma_j$ is the observation covariance. The Huber function is:

$$\rho(s) = \begin{cases} s & \text{if } s \leq \delta^2 \\ 2\delta\sqrt{s} - \delta^2 & \text{if } s > \delta^2 \end{cases}$$

This optimization is 6-DoF (optimization on SE(3)), so we update via an increment $\boldsymbol{\xi} \in \mathbb{R}^6$ in the Lie algebra $\mathfrak{se}(3)$:

$$\mathbf{T} \leftarrow \exp(\boldsymbol{\xi}^{\wedge}) \cdot \mathbf{T}$$

**Local Bundle Adjustment**

This jointly optimizes the most recent $N$ keyframes together with the map points they observe:

This is the canonical form of Bundle Adjustment (BA). "Bundle" refers to the bundles of rays emanating from each 3D point toward the cameras. The normal equations of BA have a special sparse structure (Schur complement structure):

$\mathbf{H}_{pp}$ is block-diagonal (each point is independent), so eliminating the point variables via the Schur complement yields:

$$(\mathbf{H}_{cc} - \mathbf{H}_{cp}\mathbf{H}_{pp}^{-1}\mathbf{H}_{pc})\delta\boldsymbol{\xi} = \mathbf{b}_c - \mathbf{H}_{cp}\mathbf{H}_{pp}^{-1}\mathbf{b}_p$$

The size of this reduced system depends only on the number of cameras (independent of the number of points) and can be solved efficiently. This is the key reason BA handles tens of thousands of points while still approaching real-time speed.

### 6.1.3 ORB-SLAM3 Architecture Deep Dive

[ORB-SLAM3 (Campos et al., 2021)](https://doi.org/10.1109/TRO.2021.3075644) is the current de facto standard for feature-based visual(-inertial) SLAM. In a single framework it supports monocular, stereo, and RGB-D cameras as well as IMUs, and it accommodates both pinhole and fisheye lens models.

**Overall Architecture**

ORB-SLAM3 consists of three parallel threads:

1. **Tracking Thread**: extracts ORB features in every frame, matches them with existing map points to estimate the current pose, and refines the pose with motion-only BA.
2. **Local Mapping Thread**: when a new keyframe is inserted, it triangulates new map points and performs local BA. Redundant keyframes and map points are culled to keep the map compact.
3. **Loop Closing & Map Merging Thread**: detects loop candidates with DBoW2 and verifies them via Sim(3) (or SE(3)) registration. Once confirmed, pose graph optimization is run and then a full BA.

**Tracking Thread in Detail**

1. Initial pose prediction from the previous frame: predict the current pose with a constant velocity model or IMU preintegration.
2. Using the predicted pose, project map points onto the current frame and find correspondences via ORB matching.
3. Refine the pose with motion-only BA.
4. Decide whether to insert a keyframe. Keyframe conditions: (a) a minimum number of frames has elapsed since the last keyframe, (b) the fraction of tracked map points in the current frame is below a threshold, and (c) the current frame observes a sufficient number of map points.

**Visual-Inertial Mode**

The visual-inertial mode of ORB-SLAM3 is based on the IMU preintegration of [Forster et al. (2017)](https://doi.org/10.1109/TRO.2016.2623335). The key is MAP (Maximum-a-Posteriori) estimation: visual residuals and IMU residuals are optimized jointly in a single cost function:

The IMU residual $\mathbf{e}_k^{\text{IMU}}$ is defined as the difference between the preintegrated IMU measurement between keyframes $k$ and $k+1$ and the state estimate. The state vector for each keyframe includes:

$$\mathbf{x}_k = [{}^W\mathbf{R}_k, {}^W\mathbf{p}_k, {}^W\mathbf{v}_k, \mathbf{b}_k^g, \mathbf{b}_k^a]$$

Using the preintegrated measurements derived in the previous chapter (Ch.4):

$$\Delta\mathbf{R}_{k,k+1}, \quad \Delta\mathbf{v}_{k,k+1}, \quad \Delta\mathbf{p}_{k,k+1}$$

the IMU residual is constructed as:

**Multi-Map System (Atlas)**

One of the most important contributions of ORB-SLAM3 is the Atlas structure. When tracking fails in regions with insufficient visual information (fast rotation, occlusion, etc.), existing systems reinitialize and lose the connection to the previous map. The Atlas of ORB-SLAM3:

1. Creates a new map (sub-map) when tracking fails.
2. Maintains each sub-map independently.
3. When place recognition (DBoW2) detects a previously visited sub-map, automatically merges the two maps.
4. Performs Sim(3) registration for monocular or SE(3) registration for stereo/VI upon merging.

Thanks to this structure, ORB-SLAM3 can recover gracefully from tracking failure and also supports multi-session SLAM that reuses maps from past sessions.

**ORB-SLAM3 Performance**: the stereo-inertial configuration achieves 3.6 cm accuracy on the EuRoC MAV dataset and 9 mm on TUM-VI. This showcases the strengths of the feature-based approach — precise geometric constraints and stable loop closure.

```python
# ORB-SLAM3 Tracking Thread pseudocode
def track(frame, map, last_keyframe):
    # 1. Extract ORB features (8-level image pyramid)
    keypoints, descriptors = extract_orb(frame, n_features=1000, n_levels=8, scale=1.2)
    
    # 2. Initial pose prediction
    if imu_available:
        T_predict = imu_preintegrate(last_frame.T, imu_measurements)
    else:
        T_predict = constant_velocity_model(last_frame.T, last_frame.velocity)
    
    # 3. Project map points onto the current frame → match
    projected_points = project_map_points(map.local_points, T_predict, frame.camera)
    matches = match_by_projection(keypoints, descriptors, projected_points, radius=15)
    
    # 4. Motion-only BA (optimize pose only, fix map points)
    T_refined = motion_only_BA(
        pose_init=T_predict,
        observations=[(kp, mp) for kp, mp in matches],
        robust_kernel='huber',
        n_iterations=10
    )
    
    # 5. Keyframe decision
    tracked_ratio = len(matches) / len(last_keyframe.observations)
    if tracked_ratio < 0.9 and len(matches) > 50:
        insert_keyframe(frame, T_refined, matches)
    
    return T_refined
```

---

## 6.2 Direct Visual Odometry

Direct methods use pixel intensity itself as the observation, without extracting feature points. The basic idea is: a 3D point $\mathbf{P}$ should have the same intensity in two frames (brightness constancy).

### 6.2.1 Photometric Error

The core residual of direct VO is the photometric error. For camera poses $\mathbf{T}_i, \mathbf{T}_j$ and a 3D point $\mathbf{P}$ (or a pixel $\mathbf{u}$ and inverse depth $d^{-1}$ in the host frame):

$$e_{\text{photo}} = I_j\left(\pi(\mathbf{T}_j \mathbf{T}_i^{-1} \pi^{-1}(\mathbf{u}_i, d_i^{-1}))\right) - I_i(\mathbf{u}_i)$$

where:
- $\pi^{-1}(\mathbf{u}, d^{-1})$: reconstructing the 3D point from a 2D point and inverse depth (unprojection)
- $\pi(\cdot)$: 3D → 2D projection
- $I_i, I_j$: intensity images of frames $i, j$

Pose and depth are estimated by minimizing this residual. The key differences from the reprojection error of feature-based methods are:

| | Reprojection error | Photometric error |
|---|---|---|
| Observation | Feature coordinates (2D) | Pixel intensity (1D or patch) |
| Data association | Explicit (matching required) | Implicit (computed via warping) |
| Gradient | Geometric | Image gradient |
| Texture requirement | Corners/edges needed | Only gradient needed |
| Illumination change | Invariant (geometric) | Sensitive (correction needed) |

The advantage of the photometric error is that explicit feature matching is unnecessary. Every region with a gradient across the entire image can be exploited, so the method can operate even in environments that lack texture but have weak gradients (e.g., the subtle texture of a white wall).

There are two limitations:
1. **Brightness constancy violation**: illumination changes, auto exposure, and lens vignetting cause the intensity of the same 3D point to vary across frames. Without correction, accuracy drops sharply.
2. **Narrow basin of convergence**: since optimization is based on image gradients, a poor initial pose estimate leads to local minima. Typically an initial alignment within 1–2 pixels is required.

### 6.2.2 DSO (Direct Sparse Odometry) Architecture Deep Dive

[DSO (Engel et al., 2018)](https://doi.org/10.1109/TPAMI.2017.2658577) is a landmark VO system that combines a direct method with a sparse representation. Prior to DSO there was an implicit equation of "direct = dense" (LSD-SLAM) and "sparse = indirect" (ORB-SLAM), but DSO recombined these two axes in a new way.

**Core Design Principles of DSO**

1. **Direct**: uses pixel intensity directly, without feature points.
2. **Sparse**: instead of using the entire image, samples points evenly in regions with gradient.
3. **Joint Optimization**: simultaneously optimizes poses, inverse depths, and camera intrinsic parameters (affine brightness parameters).

**Full Photometric Calibration**

One of the most important contributions of DSO is its systematic treatment of photometric calibration. In a real camera, the observed intensity $I'$ relates to the true scene irradiance $B$ as:

$$I'(\mathbf{u}) = G(t \cdot V(\mathbf{u}) \cdot B(\mathbf{u}))$$

where:
- $G(\cdot)$: the camera's nonlinear response function
- $t$: exposure time
- $V(\mathbf{u})$: lens vignetting — intensity decreases as we move away from the image center

Correcting all three factors yields a photometrically corrected image:

$$I(\mathbf{u}) = t^{-1} \cdot G^{-1}(I'(\mathbf{u})) / V(\mathbf{u})$$

Without this correction, the same 3D point has different intensities at the image center versus the edge, or in frames with different exposures, so the photometric error becomes inaccurate.

**Affine Brightness Transfer Function**

For cases in which complete photometric calibration is infeasible, DSO further compensates for inter-frame brightness changes with an affine model:

$$e_{\mathbf{p}j} = \sum_{\mathbf{p} \in \mathcal{N}_\mathbf{p}} w_\mathbf{p} \left\| (I_j[\mathbf{p}'] - b_j) - \frac{t_j e^{a_j}}{t_i e^{a_i}}(I_i[\mathbf{p}] - b_i) \right\|_\gamma$$

Here $a_i, b_i, a_j, b_j$ are per-frame affine brightness parameters optimized jointly. $\|\cdot\|_\gamma$ is the Huber norm.

**Point Selection Strategy**

DSO divides the image into a grid and selects the point with the largest gradient magnitude in each cell. The key is "uniform distribution" — it prevents features from concentrating in a single region. About 2000 points are selected, and the gradient threshold is adaptively adjusted to secure points even in low-texture regions.

**Sliding Window Optimization**

DSO jointly optimizes the most recent 5–7 keyframes and the inverse depths of the points belonging to them within a sliding window. The optimization variables are:

$$\boldsymbol{\theta} = \{\mathbf{T}_1, \ldots, \mathbf{T}_n, d_1^{-1}, \ldots, d_m^{-1}, a_1, b_1, \ldots, a_n, b_n\}$$

that is, camera poses (SE(3)), inverse depths, and affine brightness parameters.

Frames/points that drop out of the window are marginalized via the Schur complement and remain as a prior. This marginalization follows the same Schur-complement-based principle covered in Ch.4.7, differing only in that only visual residuals are involved.

**Limitations and Extensions of DSO**

The original DSO design lacks loop closure. This is not a fundamental limitation of direct methods but a design choice. LDSO (Loop-closing DSO) addresses this by combining DBoW with direct alignment. Similarly, VI-DSO, BASALT, and others are VIO variants that couple DSO with an IMU.

```python
# DSO core flow pseudocode
def dso_track(frame, window, camera):
    # 1. Apply photometric calibration
    frame.I = apply_photometric_correction(frame.raw, camera.G_inv, camera.V, frame.exposure)
    
    # 2. Initial pose estimate via direct image alignment (coarse-to-fine)
    T_init = direct_alignment_pyramid(
        ref_frame=window.latest_keyframe,
        cur_frame=frame,
        initial_guess=constant_velocity_predict(),
        levels=[4, 3, 2, 1]  # image pyramid levels
    )
    
    # 3. Point selection (gradient-based, uniform distribution)
    candidate_points = select_points_gradient_based(frame, n_blocks=32*32, n_per_block=1)
    
    # 4. Inverse depth initialization (epipolar search)
    for p in candidate_points:
        p.inv_depth = epipolar_search(window.keyframes, frame, p.u)
    
    # 5. If keyframe: insert into sliding window and run joint optimization
    if is_keyframe(frame, window):
        window.add(frame, candidate_points)
        # Joint optimization of pose + inverse depth + affine parameters
        gauss_newton_optimize(
            residuals=photometric_residuals(window),
            variables=[T, inv_depth, a, b],
            max_iter=6
        )
        # Marginalize the oldest frame
        if len(window) > 7:
            marginalize_oldest(window)
    
    return T_init
```

### 6.2.3 Semi-Direct: SVO

[SVO (Semi-direct Visual Odometry, Forster et al., 2017)](https://doi.org/10.1109/TRO.2016.2623335) is a hybrid approach that combines the advantages of feature-based and direct methods. The name "semi-direct" comes from the fact that tracking uses a direct method while mapping uses a feature-based method.

**Core Ideas of SVO**

1. **Sparse Model-based Image Alignment**: existing 3D map points are projected onto the current frame, and the frame pose is estimated by minimizing the photometric error over a patch around each projected point. This uses image gradients directly, as DSO does, but only around already-known map points rather than the whole image, so it is very fast.

2. **Feature Alignment**: once the pose is estimated, the projected location of each map point is refined to subpixel accuracy. Patch-based direct alignment is again used here.

3. **Structure & Motion Refinement**: the refined 2D locations are treated as "virtual feature points" and BA (reprojection error minimization) jointly optimizes the pose and 3D structure.

Thanks to this three-stage decomposition SVO is extremely fast — it reaches 200–400 Hz even on high-resolution images, making it suitable for agile robots such as high-speed drones. On the other hand, it lacks loop closure and is vulnerable to pure rotation (rotation in place).

---

## 6.3 Tightly-Coupled Visual-Inertial Odometry

VO alone has two fundamental limitations: (1) scale ambiguity with a monocular camera, and (2) tracking failure under fast motion or texture scarcity. Combining an IMU can resolve both problems simultaneously. Tightly-coupled VIO processes the raw measurements of camera and IMU within a single estimation framework.

### 6.3.1 VINS-Mono Architecture in Detail

[VINS-Mono (Qin et al., 2018)](https://doi.org/10.1109/TRO.2018.2853729) is a complete VIO system that achieves robust 6-DoF state estimation using only a monocular camera and a low-cost IMU. It integrates the entire pipeline — initialization, odometry, loop closure, and map reuse — in a single system.

**System Architecture**

VINS-Mono consists of three main modules:

1. **Frontend (Measurement Preprocessing)**: KLT-based feature tracking + IMU preintegration
2. **Backend (Estimator)**: nonlinear-optimization-based tightly-coupled VIO
3. **Loop Closure (Relocalization)**: DBoW2-based place recognition + relocalization

**Robust Initialization**

The greatest challenge for monocular VIO is initialization. A monocular camera alone cannot observe metric scale, and IMU biases are also unknown. VINS-Mono's initialization solves this with a loosely-coupled approach:

**Step 1: Vision-only SfM**

For the first few frames, Structure from Motion is run using vision alone. The Essential Matrix is estimated with the 5-point algorithm and triangulation recovers 3D points and camera poses. Scale at this stage is arbitrary.

**Step 2: Visual-Inertial Alignment**

The SfM result is aligned with IMU preintegration. During this process, the following are estimated:

(a) The gyro bias $\mathbf{b}_g$: compare the SfM rotation $\mathbf{R}_{c_k c_{k+1}}^{\text{sfm}}$ between consecutive keyframe pairs with the IMU preintegrated rotation $\Delta\hat{\mathbf{R}}_{k,k+1}$:

$$\min_{\mathbf{b}_g} \sum_k \left\| \text{Log}\left(\Delta\hat{\mathbf{R}}_{k,k+1}(\mathbf{b}_g)^T \cdot \mathbf{R}_{c_k}^{c_{k+1}} \right) \right\|^2$$

Using the first-order approximation to bias change (the preintegration Jacobian from Ch.4.6), this problem becomes linear.

(b) The gravity direction, velocities, and metric scale are estimated jointly. This reduces to the following linear system. For each keyframe pair $(k, k+1)$:

The unknowns are $s$ (scale), $\mathbf{g}^w$ (gravity vector), and $\{\mathbf{v}_k^w\}$ (velocities). The constraint $\|\mathbf{g}\| = 9.81$ on the magnitude of gravity is added to improve accuracy.

Once this loosely-coupled initialization converges, the system transitions to tightly-coupled optimization.

**Tightly-Coupled Nonlinear Optimization**

The VINS-Mono backend jointly optimizes three kinds of residuals within the sliding window:

where:
- $\mathbf{r}_p$: marginalization prior residual
- $\mathbf{r}_{\mathcal{B}}$: IMU preintegration residual
- $\mathbf{r}_{\mathcal{C}}$: visual reprojection error residual
- $\mathcal{X}$: state variables (keyframe poses, velocities, IMU biases in the sliding window, and feature inverse depths)

**Detailed Definition of the Visual Residual**:

For a feature $l$ first observed in keyframe $i$ and parameterized by inverse depth $\lambda_l$, the reprojection error in keyframe $j$ is:

$$\mathbf{r}_{\mathcal{C}} = \begin{bmatrix} \bar{u}_l^{c_j} - u_l^{c_j} / z_l^{c_j} \\ \bar{v}_l^{c_j} - v_l^{c_j} / z_l^{c_j} \end{bmatrix}$$

where $\begin{bmatrix} u_l^{c_j} & v_l^{c_j} & z_l^{c_j} \end{bmatrix}^T = \mathbf{R}_{b_j}^{c} (\mathbf{R}_{w}^{b_j}(\mathbf{R}_{b_i}^{w}(\mathbf{R}_{c}^{b_i} \frac{1}{\lambda_l}\begin{bmatrix} \bar{u}_l^{c_i} \\ \bar{v}_l^{c_i} \\ 1 \end{bmatrix} + \mathbf{p}_c^b) + \mathbf{p}_{b_i}^w - \mathbf{p}_{b_j}^w) - \mathbf{p}_c^b)$

**Sliding Window Management and Marginalization**

VINS-Mono fixes the window size (typically 10 keyframes) and applies one of two marginalization strategies depending on whether the newest frame is a keyframe:

1. **If the newest frame is a keyframe**: marginalize the oldest keyframe. All measurements connected to that frame are converted to priors via the Schur complement.

2. **If the newest frame is not a keyframe**: marginalize the previous frame (second-newest). In this case only the visual measurements are discarded, since the IMU measurements are included in the preintegration between adjacent keyframes and thus information is preserved.

The key point of both strategies is **information preservation**. Marginalization removes a variable while retaining the information it contributed as a prior. The mathematical mechanism of the Schur complement is treated in detail in Ch.4.7.

**4-DoF Pose Graph Optimization**

Once a loop closure is detected, VINS-Mono optimizes the pose graph in 4-DoF (yaw + 3D translation) rather than 6-DoF. Why 4-DoF? Because the IMU accelerometer observes the direction of gravity, roll and pitch are already sufficiently observable via the IMU. Drift accumulates only in yaw and position. Therefore, on loop closure it is physically sensible to leave roll/pitch untouched and correct only yaw and position.

```python
# VINS-Mono sliding window optimization pseudocode
class VINSEstimator:
    def __init__(self, window_size=10):
        self.window_size = window_size
        self.states = []  # [pose, velocity, bias_g, bias_a] per keyframe
        self.features = {}  # feature_id -> inverse_depth
        self.prior = None  # marginalization prior
    
    def optimize(self):
        # Construct the cost function
        cost = 0.0
        
        # 1. Marginalization prior
        if self.prior is not None:
            cost += self.prior.evaluate(self.states)
        
        # 2. IMU preintegration residual
        for k in range(len(self.states) - 1):
            preint = self.imu_preintegrations[k]
            r_imu = compute_imu_residual(
                self.states[k], self.states[k+1], preint
            )
            cost += r_imu.T @ preint.info_matrix @ r_imu
        
        # 3. Visual reprojection error
        for feat_id, inv_depth in self.features.items():
            for obs in self.observations[feat_id]:
                r_vis = compute_reprojection_error(
                    self.states[obs.host_frame], self.states[obs.target_frame],
                    inv_depth, obs.uv, self.T_cam_imu
                )
                cost += huber(r_vis.T @ obs.info_matrix @ r_vis)
        
        # Gauss-Newton / Levenberg-Marquardt optimization
        solve_nonlinear_least_squares(cost, max_iterations=8)
    
    def marginalize(self, is_keyframe):
        if is_keyframe:
            # Marginalize the oldest frame
            self.prior = schur_complement_marginalize(
                self.states[0], connected_factors(self.states[0])
            )
            self.states.pop(0)
        else:
            # Marginalize the previous frame (visual only; IMU preserved)
            self.prior = schur_complement_marginalize(
                self.states[-2], visual_factors(self.states[-2])
            )
            self.states.pop(-2)
```

### 6.3.2 OKVIS

[OKVIS (Open Keyframe-based Visual-Inertial SLAM, Leutenegger et al., 2015)](https://doi.org/10.1177/0278364914554813) is a tightly-coupled VIO that preceded VINS-Mono and presented an early form of keyframe-based sliding window optimization.

**Key Features of OKVIS**:

1. **Harris corner + BRISK descriptor**: uses Harris corners and BRISK descriptors instead of ORB.
2. **Keyframe-based marginalization**: performs marginalization within a sliding window, similarly to VINS-Mono.
3. **Speed error term**: rather than IMU preintegration, it directly integrates IMU over a short time interval and uses it as a velocity constraint. OKVIS2 later switched to preintegration.
4. **Ceres Solver-based**: uses Ceres Solver for optimization.

OKVIS has simpler initialization than VINS-Mono (it assumes a stereo camera by default), but VINS-Mono is superior for robust initialization in the monocular case.

### 6.3.3 MSCKF: Multi-State Constraint Kalman Filter

[MSCKF (Mourikis & Roumeliotis, 2007)](https://doi.org/10.1109/ROBOT.2007.364024) is the representative algorithm for filter-based VIO. Its approach is fundamentally different from optimization-based systems (VINS-Mono, ORB-SLAM3), and it remains actively used in certain applications today.

**Core Idea of MSCKF: Do Not Put Landmarks in the State**

EKF-SLAM includes landmarks (3D points) in the state vector. With $N$ landmarks, the state vector has size $3N + 15$ and the covariance matrix has size $(3N+15)^2$, requiring $O(N^2)$ space and $O(N^3)$ time in the number of landmarks. This is fatal for real-time processing.

The key insight of MSCKF is: **it is possible to exclude landmarks from the state vector while still preserving the geometric constraint information they provide**.

**State Vector Structure**

The MSCKF state vector comprises the IMU error state and $N$ camera poses within a sliding window:

$$\tilde{\mathbf{x}} = [\tilde{\mathbf{x}}_{IMU}^T, \tilde{\mathbf{x}}_{C_1}^T, \ldots, \tilde{\mathbf{x}}_{C_N}^T]^T$$

where the IMU error state is:

$$\tilde{\mathbf{x}}_{IMU} = [\delta\boldsymbol{\theta}^T, \tilde{\mathbf{b}}_g^T, {}^G\tilde{\mathbf{v}}_I^T, \tilde{\mathbf{b}}_a^T, {}^G\tilde{\mathbf{p}}_I^T]^T \in \mathbb{R}^{15}$$

and each camera pose error state is:

$$\tilde{\mathbf{x}}_{C_k} = [\delta\boldsymbol{\theta}_{C_k}^T, {}^G\tilde{\mathbf{p}}_{C_k}^T]^T \in \mathbb{R}^6$$

The total size of the state vector is $15 + 6N$, independent of the number of landmarks.

**Null-Space Projection: The Core Mathematics**

Suppose a single static feature $\mathbf{p}_f$ is observed across $M$ camera poses. Linearizing the observation equation:

$$\mathbf{r} = \mathbf{H}_X \tilde{\mathbf{x}} + \mathbf{H}_f \tilde{\mathbf{p}}_f + \mathbf{n}$$

where:
- $\mathbf{r} \in \mathbb{R}^{2M}$: residual vector (observation − prediction)
- $\mathbf{H}_X \in \mathbb{R}^{2M \times (15+6N)}$: Jacobian with respect to the state
- $\mathbf{H}_f \in \mathbb{R}^{2M \times 3}$: Jacobian with respect to the feature position
- $\tilde{\mathbf{p}}_f \in \mathbb{R}^3$: feature position error

QR-decompose $\mathbf{H}_f$:

$$\mathbf{H}_f = \begin{bmatrix} \mathbf{Q}_1 & \mathbf{Q}_2 \end{bmatrix} \begin{bmatrix} \mathbf{R}_1 \\ \mathbf{0} \end{bmatrix}$$

$\mathbf{Q}_2$ spans the left null space of $\mathbf{H}_f$ ($\mathbf{Q}_2^T \mathbf{H}_f = \mathbf{0}$). Left-multiplying both sides by $\mathbf{Q}_2^T$:

$$\mathbf{r}_o = \mathbf{Q}_2^T \mathbf{r} = \mathbf{Q}_2^T \mathbf{H}_X \tilde{\mathbf{x}} + \mathbf{Q}_2^T \mathbf{n} = \mathbf{H}_o \tilde{\mathbf{x}} + \mathbf{n}_o$$

The feature position $\tilde{\mathbf{p}}_f$ has been fully eliminated. The EKF update can be performed using only $\mathbf{r}_o$ and $\mathbf{H}_o$. This is the "multi-state constraint" of MSCKF — exploiting the geometric constraint that a single feature imposes across multiple camera poses directly, while excluding the feature itself from the state.

**Computational Complexity**: the state vector size is $15 + 6N$ (in the number of camera poses $N$), independent of the number of landmarks $M$. EKF-SLAM includes $M$ landmarks in the state so the state size is $O(M)$ and the covariance update requires $O(M^2)$. MSCKF excludes landmarks from the state and therefore depends only on the number of cameras $N \ll M$ — this is its core advantage.

**MSCKF Update Procedure**

1. **IMU propagation**: when a new IMU measurement arrives, propagate the state and update the covariance:
   $$\hat{\mathbf{x}}_{k+1|k} = f(\hat{\mathbf{x}}_{k|k}, \mathbf{u}_k)$$
   $$\mathbf{P}_{k+1|k} = \boldsymbol{\Phi}_k \mathbf{P}_{k|k} \boldsymbol{\Phi}_k^T + \mathbf{G}_k \mathbf{Q} \mathbf{G}_k^T$$

2. **State augmentation**: when a new image arrives, copy the current IMU pose to add a camera pose to the state. The covariance matrix is also expanded.

3. **MSCKF update**: for a feature whose tracking has ended (no longer observed):
   - Estimate $\hat{\mathbf{p}}_f$ by triangulation.
   - Compute $\mathbf{r}_o, \mathbf{H}_o$ via null-space projection.
   - Perform the standard EKF update:
     $$\mathbf{K} = \mathbf{P} \mathbf{H}_o^T (\mathbf{H}_o \mathbf{P} \mathbf{H}_o^T + \sigma^2 \mathbf{I})^{-1}$$
     $$\hat{\mathbf{x}} \leftarrow \hat{\mathbf{x}} + \mathbf{K} \mathbf{r}_o$$
     $$\mathbf{P} \leftarrow (\mathbf{I} - \mathbf{K}\mathbf{H}_o)\mathbf{P}$$

4. **State pruning**: remove old camera poses from the sliding window and shrink the covariance accordingly.

```cpp
// MSCKF core update pseudocode (C++)
void MSCKF::msckf_update(const Feature& feature) {
    // 1. Estimate the 3D position of the feature via triangulation
    Vector3d p_f = triangulate(feature.observations, cam_states);
    
    // 2. Compute observation Jacobians
    int N_obs = feature.observations.size();
    MatrixXd H_X(2*N_obs, state_dim);  // Jacobian with respect to the state
    MatrixXd H_f(2*N_obs, 3);          // Jacobian with respect to the feature
    VectorXd r(2*N_obs);               // residual
    
    for (int i = 0; i < N_obs; i++) {
        auto& obs = feature.observations[i];
        auto& cam = cam_states[obs.cam_id];
        
        Vector3d p_c = cam.R_w2c * (p_f - cam.p_w);
        double X = p_c(0), Y = p_c(1), Z = p_c(2);
        
        // Projection Jacobian (pinhole)
        Matrix<double, 2, 3> J_proj;
        J_proj << 1.0/Z, 0, -X/(Z*Z),
                  0, 1.0/Z, -Y/(Z*Z);
        
        // Compute H_f
        H_f.block<2,3>(2*i, 0) = J_proj * cam.R_w2c;
        
        // Compute H_X (blocks for camera poses)
        // ... (rotation, translation Jacobians)
        
        // Residual
        r.segment<2>(2*i) = obs.uv - Vector2d(X/Z, Y/Z);
    }
    
    // 3. Null-space projection via QR decomposition
    // H_f = Q * [R1; 0] → Q2^T * H_f = 0
    HouseholderQR<MatrixXd> qr(H_f);
    MatrixXd Q = qr.householderQ();
    MatrixXd Q2 = Q.rightCols(2*N_obs - 3);
    
    MatrixXd H_o = Q2.transpose() * H_X;
    VectorXd r_o = Q2.transpose() * r;
    
    // 4. Standard EKF update
    MatrixXd S = H_o * P * H_o.transpose() + sigma2 * MatrixXd::Identity(r_o.size(), r_o.size());
    MatrixXd K = P * H_o.transpose() * S.inverse();
    
    VectorXd dx = K * r_o;
    // Update the state (on-manifold)
    apply_correction(dx);
    // Update the covariance
    P = (MatrixXd::Identity(state_dim, state_dim) - K * H_o) * P;
}
```

### 6.3.4 OpenVINS

[OpenVINS (Geneva et al., 2020)](https://doi.org/10.1109/ICRA40945.2020.9196524) is the most complete open-source implementation of MSCKF-based VIO. Beyond being a straightforward implementation, it aims to be a research platform where various VIO algorithm variants can be modularly compared and experimented with.

**Key Features of OpenVINS**:

1. **On-Manifold Sliding Window EKF**: a sliding window Kalman filter based on MSCKF. Rotations are handled on the SO(3) manifold.

2. **Online calibration**: camera intrinsics, camera-IMU extrinsics, and the temporal offset are estimated automatically at runtime. Temporal offset estimation is particularly important because imperfect synchronization of camera and IMU timestamps severely degrades performance.

3. **SLAM landmark support**: pure MSCKF does not include features in the state, but OpenVINS can optionally include a subset of landmarks in the state as SLAM features. SLAM features are long-tracked points, parameterized as anchored inverse depth.

4. **First-Estimates Jacobian (FEJ)**: a technique for addressing the consistency problem of the EKF. The standard EKF recomputes Jacobians at the latest state estimate at every update, which violates observability properties and causes the covariance to shrink excessively. FEJ computes Jacobians only at the first estimate, preserving correct observability.

5. **Simulator**: a simulator for testing VIO algorithms is included. It supports a variety of trajectories, environment configurations, and IMU noise models, enabling quantitative evaluation via comparison with ground truth.

### 6.3.5 Basalt

[Basalt (Usenko et al., 2020)](https://doi.org/10.1109/LRA.2019.2961227) is a tightly-coupled VIO similar to VINS-Mono but differs in several design choices.

**Key Features of Basalt**:

1. **Visual-only Frontend**: instead of KLT, it performs patch-based direct alignment (similar to SVO) for subpixel-accurate feature tracking.

2. **Non-linear Factor Recovery (NFR)**: an alternative to marginalization. Marginalization leaves a prior that depends on the linearization point, and information distortion occurs if that linearization point later changes significantly. Basalt's NFR approximates the marginalized information as a nonlinear factor, enabling relinearization.

3. **Efficient Implementation**: Basalt exploits the structure of the factor graph for an efficient implementation, achieving higher processing speed than VINS-Mono.

4. **Stereo/Multi-camera support**: it naturally integrates visual information from multiple cameras.

---

## 6.4 VIO Design Choices

When designing a VIO system, several core design choices arise. This section analyzes the pros and cons of each option and the scenarios to which it applies.

### 6.4.1 Filter vs Optimization

This is one of the oldest debates in the VIO field.

**Filter-based (MSCKF, OpenVINS)**

- Maintains only the current state and updates it sequentially as new measurements arrive
- Past states are "absorbed" into the current state distribution (mean + covariance)
- Computational complexity: $O(N^2)$ per update ($N$ is the state dimension)
- Advantages: constant per-update cost, simple implementation
- Disadvantages: accumulated linearization error (once linearized, it cannot be corrected), consistency problems

**Optimization-based (VINS-Mono, ORB-SLAM3, Basalt)**

- Maintains multiple states within a sliding window and optimizes them jointly with all measurements
- Iterative re-linearization is possible
- Computational complexity: $O(N^3)$ per iteration ($N$ is the window size), but made efficient with the Schur complement
- Advantages: more accurate (iterative relinearization reduces linearization error), easy integration of diverse measurements
- Disadvantages: higher computational cost, sensitivity to window size

**Empirical Comparison**: under equal conditions, optimization-based methods are generally more accurate than filter-based ones. However, when FEJ and proper observability analysis are applied, MSCKF closes much of the gap. In severely resource-constrained environments (ultra-low-power MCUs, etc.), filter-based approaches remain attractive.

### 6.4.2 Keyframe Selection Strategies

Keyframe selection has a large impact on VIO performance. Inserting keyframes too often increases the computational burden; too rarely, and information is lost.

Common keyframe selection criteria:

1. **Time-based**: a minimum time has elapsed since the last keyframe.
2. **Parallax-based**: the mean feature parallax between the current frame and the last keyframe exceeds a threshold. VINS-Mono uses this criterion.
3. **Tracking quality-based**: insert a keyframe when the number of tracked features falls below a threshold. ORB-SLAM3 uses this criterion.
4. **Information gain-based**: decide based on the estimated information gain (amount of information) that a new keyframe would provide. Theoretically the most principled but computationally expensive.

Keyframe selection is closely tied to marginalization. The two-way marginalization strategy of VINS-Mono (see Section 6.3.1) switches the direction of marginalization depending on whether the frame is a keyframe, illustrating this connection clearly.

### 6.4.3 Feature Parameterization

How 3D points are parameterized is another important design choice.

**XYZ (Euclidean 3D coordinates)**

The most intuitive option. $\mathbf{P} = [X, Y, Z]^T \in \mathbb{R}^3$. However, it is unstable for far points — small angular changes cause large variations in $Z$.

**Inverse Depth**

A point is expressed as "observation direction in the host frame + inverse depth":

$$\boldsymbol{\lambda} = [\theta, \phi, \rho]^T$$

Here $\theta, \phi$ are the azimuth/elevation in the host frame and $\rho = 1/d$ is the inverse depth. Advantages:

1. **Handling far points**: as $d \to \infty$, $\rho \to 0$ is numerically stable. In XYZ, $Z \to \infty$ leads to instability.
2. **Improved linearity**: when depth is uncertain in monocular initialization, the uncertainty distribution of inverse depth is closer to Gaussian.

VINS-Mono and OpenVINS use the inverse depth parameterization.

**Anchored Inverse Depth**

The inverse depth is anchored to a specific "anchor" keyframe:

$$\mathbf{P} = \mathbf{T}_{\text{anchor}} \cdot \frac{1}{\rho} [\bar{u}, \bar{v}, 1]^T$$

where $(\bar{u}, \bar{v})$ are the normalized coordinates in the anchor frame and $\rho$ is the inverse depth. The advantage of this parameterization is that even when the anchor frame's pose changes, the inverse depth itself does not, partially reducing linearization error. It is used for SLAM features in ORB-SLAM3 and OpenVINS.

---

## 6.5 Learning-Based VO/VIO

Traditional VO/VIO relies on a "human-designed pipeline": feature detection → matching → RANSAC → BA. Learning-based approaches aim to replace part or all of this pipeline with neural networks.

### 6.5.1 Supervised: the DeepVO Family

Early learning-based VO ([DeepVO, Wang et al., 2017](https://doi.org/10.1109/ICRA.2017.7989236)) trained an end-to-end network that takes consecutive image pairs as input and directly predicts the relative pose. A CNN extracts visual features and an LSTM models temporal dependencies.

The limitations are clear:
- Overfitting to the environment of the training data (poor generalization)
- Not exploiting geometric constraints (e.g., epipolar geometry), so accuracy falls short of traditional methods
- Severe scale drift

### 6.5.2 Self-supervised: Limitations and Current Position

Self-supervised VO (SfMLearner, Monodepth2, etc.) learns depth and pose jointly using photometric loss via view synthesis. It has the advantage of training without labels, but struggles with moving objects, texture scarcity, and occlusion.

Current position: self-supervised VO has achieved great success in monocular depth estimation, but as a standalone VO/VIO system it falls well short of traditional methods. In particular it has not resolved the accumulated drift problem.

### 6.5.3 Hybrid: DROID-SLAM

[DROID-SLAM (Teed & Deng, 2021)](https://arxiv.org/abs/2108.10869) is a system that integrates the geometric rigor of traditional BA with the robust matching ability of deep learning in a single differentiable pipeline. It was the first to demonstrate that learning-based SLAM can surpass traditional systems on every metric.

**Architecture**

The core of DROID-SLAM is the combination of two components:

1. **RAFT-Inspired Iterative Update Operator**

A structure inspired by RAFT (Recurrent All-Pairs Field Transforms, Teed & Deng, 2020). For a frame pair $(i, j)$:
- A 4D correlation volume is computed from the two frames' feature maps.
- Features are indexed from the correlation volume using the correspondence field derived from the current pose/depth estimate.
- A 3×3 Convolutional GRU takes correlation features, the current flow, and context features as input and outputs a **flow revision** $\Delta\mathbf{f}_{ij}$ and a **confidence weight** $\mathbf{w}_{ij}$.

$$(\Delta\mathbf{f}_{ij}, \mathbf{w}_{ij}) = \text{ConvGRU}(\text{corr}_{ij}, \mathbf{f}_{ij}^{\text{curr}}, \text{context}_i)$$

This update is applied iteratively to progressively refine the correspondences.

2. **Differentiable Dense Bundle Adjustment (DBA) Layer**

A layer that turns the flow revision output by the GRU into a geometric update. The key idea: define a reprojection error for every pixel and minimize it via Gauss-Newton over camera poses $\mathbf{T}_i \in SE(3)$ and inverse depths $d_i$:

$$\sum_{(i,j)} \sum_{\mathbf{p}} \left\| \mathbf{w}_{ij}^{\mathbf{p}} \circ (\mathbf{p}^{*}_{ij} - \pi(\mathbf{T}_j \circ \mathbf{T}_i^{-1} \circ \pi^{-1}(\mathbf{p}, d_i^{\mathbf{p}}))) \right\|^2$$

where $\mathbf{p}^{*}_{ij} = \mathbf{p} + \mathbf{f}_{ij}^{\text{curr}} + \Delta\mathbf{f}_{ij}$ is the target correspondence and $\mathbf{w}_{ij}^{\mathbf{p}}$ is the confidence weight.

Deriving the Gauss-Newton update gives the normal equations:

where $\boldsymbol{\xi}$ is the pose update (in the SE(3) tangent space), $\delta\mathbf{d}$ is the inverse depth update, $\mathbf{B}$ is the pose block, $\mathbf{C}$ is the depth block (diagonal), and $\mathbf{E}$ is the cross block.

The Schur complement is applied as in traditional BA:

$$(\mathbf{B} - \mathbf{E}\mathbf{C}^{-1}\mathbf{E}^T)\boldsymbol{\xi} = \mathbf{v} - \mathbf{E}\mathbf{C}^{-1}\mathbf{w}$$

Since $\mathbf{C}$ is diagonal, $\mathbf{C}^{-1}$ is trivial. The reduced system depends only on the number of cameras and is therefore efficient.

The core innovation is that the entire Gauss-Newton solver is **differentiable**. Backpropagation trains the GRU's parameters to output "good correspondences."

**Frame Graph and Loop Closure**

DROID-SLAM builds a co-visibility-based frame graph dynamically. When a new frame is added, edges are connected to its neighbors; when co-visibility with past frames is detected, long-range edges are added to perform loop closure. The backend runs a global BA over the entire keyframe history.

**Multi-Modality Support**

Although DROID-SLAM is trained only on monocular video, it can directly use stereo and RGB-D inputs at inference time:
- **Stereo**: treat left/right frames as separate frames while fixing their relative pose at the known baseline.
- **RGB-D**: use depth information as the initial inverse depth and incorporate depth observations as additional constraints.

**Performance**

- TartanAir: 62% error reduction versus the previous best
- EuRoC (monocular): 82% reduction
- ETH-3D: 30 out of 32 sequences succeed (previous best: 19)
- Trained only on synthetic data (TartanAir), it achieves SOTA on all four real datasets

**Why It Matters**

DROID-SLAM is the first practical system that combines geometric rigor (BA) with data-driven robustness (learned correspondence). It operates stably even in environments where traditional methods fail (repetitive patterns, texture scarcity, abrupt illumination change).

It has limitations, however:
- **Real time**: a GPU is required, and it currently runs slower than real time.
- **No IMU**: it is a vision-only system; coupling an IMU remains an open research problem.
- **Memory**: feature maps and correlation volumes for every frame must be retained, so memory usage is high.

Follow-up research is addressing these limitations one by one, and learning-based VO/VIO is rapidly catching up with traditional methods.

### 6.5.4 Recent Trends (2023-2025)

[DPVO (Teed & Deng, 2023)](https://arxiv.org/abs/2208.04726) replaces DROID-SLAM's dense flow with sparse patch-based matching, reducing memory by a factor of three and improving speed threefold while achieving comparable or better accuracy. Combining a patch-wise recurrent update operator with a differentiable BA, it realizes near-real-time learning-based VO.

[MAC-VO (Qu et al., 2024)](https://arxiv.org/abs/2409.09479) introduces learning-based matching uncertainty (metrics-aware covariance) into stereo VO, using the uncertainty to determine keypoint selection and the residual weights in pose graph optimization. It outperforms existing VO/SLAM systems in environments with illumination changes and texture scarcity, and was selected as ICRA 2025 Best Paper.

---

## Chapter 6 Summary

| System | Type | Estimation | Sensors | Key Features |
|--------|------|-----------|------|-----------|
| ORB-SLAM3 | Feature-based | Optimization (BA) | Mono/Stereo/RGBD + IMU | Multi-map Atlas, fisheye support |
| DSO | Direct | Optimization (windowed) | Mono | Photometric calibration, sparse sampling |
| SVO | Semi-direct | Optimization (BA) | Mono/Stereo | 200-400 Hz, suitable for high-speed drones |
| VINS-Mono | Feature-based | Optimization (sliding window) | Mono + IMU | Robust initialization, 4-DoF loop closure |
| MSCKF | Feature-based | EKF (sliding window) | Mono/Stereo + IMU | Excludes landmarks from state, null-space projection |
| OpenVINS | Feature-based | EKF (MSCKF) | Mono/Stereo + IMU | Online calibration, FEJ, research platform |
| Basalt | Semi-direct | Optimization (sliding window) | Stereo + IMU | NFR, efficient implementation |
| DROID-SLAM | Learned | Differentiable BA | Mono/Stereo/RGBD | Differentiable BA, trained on synthetic data |
| DPVO | Learned (sparse) | Differentiable BA | Mono | 3x faster than DROID with 1/3 memory |
| MAC-VO | Learned + Opt. | Pose graph opt. | Stereo | Metrics-aware covariance, ICRA 2025 Best Paper |

The next chapter covers LiDAR-based odometry and LiDAR-Inertial fusion systems, analyzing their complementary relationship with camera-based systems.

---

# Ch.7 — LiDAR Odometry & LiDAR-Inertial Odometry

In Ch.6 we covered camera(+IMU)-based Visual Odometry. In this chapter we approach the same ego-motion estimation problem from the LiDAR side.

LiDAR is a sensor that complements the camera. While the camera provides rich texture information but is sensitive to illumination and cannot recover absolute range, LiDAR delivers illumination-invariant, precise 3D range measurements. This chapter examines in depth the internal structure of LiDAR Odometry (LO), which estimates ego-motion from LiDAR alone, and of LiDAR-Inertial Odometry (LIO), which couples LiDAR with an IMU.

The central problem in LiDAR odometry is **point cloud registration** — finding the rigid-body transformation $\mathbf{T} \in SE(3)$ between two consecutive scans. Despite its seeming simplicity, this problem in practice involves a variety of challenges: data association (correspondence), noise modeling, computational efficiency, and motion distortion compensation.

---

## 7.1 Point Cloud Registration Fundamentals

Point cloud registration is the problem of finding the optimal rigid-body transformation between two point clouds $\mathcal{P} = \{\mathbf{p}_i\}$ and $\mathcal{Q} = \{\mathbf{q}_j\}$:

$$\mathbf{T}^* = \underset{\mathbf{T} \in SE(3)}{\arg\min} \sum_i d(\mathbf{T} \cdot \mathbf{p}_i, \mathcal{Q})$$

Here $d(\cdot, \cdot)$ is a distance metric between a transformed source point and the target point cloud. Different definitions of this distance give rise to different ICP variants.

### 7.1.1 ICP Variants

**Point-to-Point ICP ([Besl & McKay, 1992](https://doi.org/10.1109/34.121791))**

In its most basic form, it minimizes the Euclidean distance between a transformed source point and its closest target point:

$$\mathbf{T}^* = \underset{\mathbf{T}}{\arg\min} \sum_i \left\|\mathbf{T} \cdot \mathbf{p}_i - \mathbf{q}_{c(i)}\right\|^2$$

where $c(i) = \arg\min_j \|\mathbf{T} \cdot \mathbf{p}_i - \mathbf{q}_j\|$ is the closest-point correspondence. ICP iterates two steps:

1. **Correspondence search (closest point)**: Using the current transformation, transform the source points and then find the closest point in the target. Using a kd-tree this takes $O(N\log N)$.

2. **Transformation estimation**: Given the correspondence pairs, the optimal transformation can be obtained in closed form. Using SVD:

Subtract the centroids of both point sets:
   $$\bar{\mathbf{p}} = \frac{1}{N}\sum_i \mathbf{p}_i, \quad \bar{\mathbf{q}} = \frac{1}{N}\sum_i \mathbf{q}_{c(i)}$$

Compute the cross-covariance matrix:
   $$\mathbf{W} = \sum_i (\mathbf{p}_i - \bar{\mathbf{p}})(\mathbf{q}_{c(i)} - \bar{\mathbf{q}})^T$$

SVD: $\mathbf{W} = \mathbf{U}\boldsymbol{\Sigma}\mathbf{V}^T$

Optimal rotation and translation:
   $$\mathbf{R}^* = \mathbf{V} \text{diag}(1, 1, \det(\mathbf{V}\mathbf{U}^T)) \mathbf{U}^T, \quad \mathbf{t}^* = \bar{\mathbf{q}} - \mathbf{R}^*\bar{\mathbf{p}}$$

When $\det(\mathbf{V}\mathbf{U}^T) = 1$, we have $\mathbf{R}^* = \mathbf{V}\mathbf{U}^T$; when $\det(\mathbf{V}\mathbf{U}^T) = -1$, the sign of the last column of $\mathbf{V}$ is flipped to prevent a reflection.

Limitations of point-to-point ICP:
- Sliding on planes — points on a plane can slide along the plane without changing the cost, leading to slow convergence.
- Initialization dependence — it easily falls into local minima.
- Inaccurate closest-point correspondences — if the two scans have different sampling patterns, the nearest neighbor may not be the true correspondence.

**Point-to-Plane ICP**

For points on a planar surface, the distance from a point to the plane is physically more meaningful than the distance between two points:

$$\mathbf{T}^* = \underset{\mathbf{T}}{\arg\min} \sum_i \left((\mathbf{T} \cdot \mathbf{p}_i - \mathbf{q}_{c(i)})^T \mathbf{n}_{c(i)}\right)^2$$

where $\mathbf{n}_{c(i)}$ is the surface normal at the target point $\mathbf{q}_{c(i)}$. Because only the normal-direction component of the distance is measured, sliding along the plane does not contribute to the cost.

Advantages: Convergence is much faster than point-to-point. It is particularly effective in indoor/urban environments rich in planar structures.

Disadvantages: There is no closed-form solution, so iterative optimization (e.g., Gauss-Newton) is required. Accuracy depends on the quality of normal estimation.

Normal estimation is performed by running PCA (principal component analysis) on the neighboring points of each point and taking the eigenvector corresponding to the smallest eigenvalue as the normal. The neighborhood covariance matrix is

$$\mathbf{C} = \frac{1}{k}\sum_{j \in \mathcal{N}(i)} (\mathbf{q}_j - \bar{\mathbf{q}})(\mathbf{q}_j - \bar{\mathbf{q}})^T$$

and from the eigendecomposition $\mathbf{C} = \mathbf{V}\boldsymbol{\Lambda}\mathbf{V}^T$ the eigenvector corresponding to $\lambda_{\min}$ is the normal direction.

### 7.1.2 GICP (Generalized ICP)

GICP ([Segal et al., 2009](https://doi.org/10.15607/RSS.2009.V.021)) unifies point-to-point, point-to-plane, and plane-to-plane ICP into a single probabilistic framework.

The key idea: each point is modeled as carrying a covariance $\mathbf{C}_i$ that reflects the uncertainty of the local surface. The cost function is

where $\mathbf{C}_i^{\mathcal{P}}, \mathbf{C}_i^{\mathcal{Q}}$ are the local surface covariances at the source and target points, respectively.

**Physical meaning of the covariance**:
- For a point on a plane: small variance along the normal, large variance along the tangent directions $\rightarrow \mathbf{C} = \mathbf{R}_s \text{diag}(\epsilon, 1, 1) \mathbf{R}_s^T$ ($\epsilon \ll 1$; $\mathbf{R}_s$ is the rotation aligning the normal to the first axis).
- In this case GICP automatically becomes plane-to-plane registration.
- When $\mathbf{C}^{\mathcal{P}} = \mathbf{0}$ it reduces to point-to-plane, and when $\mathbf{C}^{\mathcal{P}} = \mathbf{C}^{\mathcal{Q}} = \mathbf{I}$ it reduces to point-to-point.

GICP is theoretically the most general ICP framework, and in practice it delivers the most accurate results across a wide range of environments.

```python
# GICP core iteration pseudocode
def gicp(P, Q, T_init, max_iter=50, tol=1e-6):
    """
    P: source point cloud (N x 3)
    Q: target point cloud (M x 3)
    T_init: initial transformation (4 x 4)
    """
    T = T_init.copy()
    
    # Precompute local surface covariance for each point
    C_P = compute_local_covariances(P, k_neighbors=20)  # N x 3 x 3
    C_Q = compute_local_covariances(Q, k_neighbors=20)  # M x 3 x 3
    
    # Build target kd-tree
    tree = KDTree(Q)
    
    for iteration in range(max_iter):
        # 1. Transform source points
        P_transformed = apply_transform(T, P)
        
        # 2. Find closest-point correspondences
        distances, indices = tree.query(P_transformed)
        
        # 3. Gauss-Newton update
        H = np.zeros((6, 6))  # Hessian approximation
        b = np.zeros(6)       # gradient
        
        for i in range(len(P)):
            j = indices[i]
            residual = P_transformed[i] - Q[j]
            
            # Combined covariance (in the transformed frame)
            R = T[:3, :3]
            Sigma = C_Q[j] + R @ C_P[i] @ R.T
            Sigma_inv = np.linalg.inv(Sigma)
            
            # SE(3) Jacobian
            J = compute_se3_jacobian(T, P[i])  # 2D: 3 x 6
            
            # Accumulate
            H += J.T @ Sigma_inv @ J
            b += J.T @ Sigma_inv @ residual
        
        # 4. Compute and apply the increment
        xi = np.linalg.solve(H, -b)
        T = se3_exp(xi) @ T
        
        if np.linalg.norm(xi) < tol:
            break
    
    return T
```

### 7.1.3 NDT (Normal Distributions Transform)

[NDT (Biber & Strasser, 2003)](https://doi.org/10.1109/IROS.2003.1249285) does not use the point cloud directly; instead it partitions space into voxels and models the distribution of points within each voxel as a Gaussian.

**NDT procedure**:

1. **Build the NDT representation of the target point cloud**: Partition space into a 3D voxel grid and, from the points in each voxel $k$, compute a Gaussian $\mathcal{N}(\boldsymbol{\mu}_k, \boldsymbol{\Sigma}_k)$:
   $$\boldsymbol{\mu}_k = \frac{1}{n_k}\sum_{i \in k} \mathbf{q}_i, \quad \boldsymbol{\Sigma}_k = \frac{1}{n_k-1}\sum_{i \in k} (\mathbf{q}_i - \boldsymbol{\mu}_k)(\mathbf{q}_i - \boldsymbol{\mu}_k)^T$$

2. **Optimize the transformation**: Optimize so that the transformed source points have high likelihood under the target NDT distribution:
   $$\mathbf{T}^* = \underset{\mathbf{T}}{\arg\min} \sum_i -\log p(\mathbf{T} \cdot \mathbf{p}_i \mid \boldsymbol{\mu}_{k(i)}, \boldsymbol{\Sigma}_{k(i)})$$
   
   Under the Gaussian assumption,
   $$\mathbf{T}^* = \underset{\mathbf{T}}{\arg\min} \sum_i (\mathbf{T} \cdot \mathbf{p}_i - \boldsymbol{\mu}_{k(i)})^T \boldsymbol{\Sigma}_{k(i)}^{-1} (\mathbf{T} \cdot \mathbf{p}_i - \boldsymbol{\mu}_{k(i)})$$

Advantages of NDT:
- No explicit correspondence search is required — we only need to determine which voxel a point belongs to. There is no kd-tree construction cost.
- The voxel size controls the trade-off between precision and the convergence basin — larger voxels give a wider convergence basin, smaller voxels give higher precision.
- The cost function is smooth, so the optimization is stable.

Disadvantages:
- Sensitive to the choice of voxel size.
- Covariance estimation is unstable in voxels with few points.
- 2D NDT is widely used in autonomous driving, but 3D NDT tends to be slightly less accurate than ICP/GICP.

### 7.1.4 Convergence and Initialization Dependence

All registration algorithms are local optimizers, so the initial guess matters. If the initial pose error is large, the optimization converges to the wrong local minimum.

Practical ways to supply an initial guess:
1. **Constant-velocity model**: Extrapolate the transformation from the previous two frames. The simplest option, but it fails under abrupt motion.
2. **IMU integration**: Integrating the IMU over the short scan interval provides an excellent initial guess. This is one reason LIO systems are more robust than LO.
3. **Multi-resolution**: A coarse-to-fine approach. Perform a rough registration first with large voxels or downsampling, then refine with a finer registration.
4. **RANSAC + feature matching**: Establish correspondences using 3D descriptors such as FPFH or SHOT, and estimate the initial transformation with RANSAC. Useful for general registration, but in odometry the constant-velocity model combined with IMU is more practical because of temporal continuity.

---

## 7.2 Feature-based LiDAR Odometry

### 7.2.1 LOAM (Lidar Odometry and Mapping in Real-time)

LOAM ([Zhang & Singh, 2014](https://doi.org/10.15607/RSS.2014.X.007)) is the reference point of LiDAR odometry. It held a top position on the KITTI odometry benchmark for a long time and became the foundation of many follow-up systems such as LeGO-LOAM, LIO-SAM, and A-LOAM.

**Key idea: feature extraction + two-stage architecture**

LOAM's core insights are twofold:
1. Extracting geometric features (edge, planar) from a LiDAR scan enables accurate registration using far fewer points than the full point cloud.
2. Separating fast odometry from slow mapping achieves real-time operation and high accuracy at the same time.

**Feature extraction**

On each scan line, the local curvature at each point is computed. The curvature at point $\mathbf{p}_i$ is

$$c_i = \frac{1}{|\mathcal{S}_i| \cdot \|\mathbf{p}_i\|} \left\| \sum_{j \in \mathcal{S}_i} (\mathbf{p}_j - \mathbf{p}_i) \right\|$$

where $\mathcal{S}_i$ is the set of left and right neighbors of $\mathbf{p}_i$ on the same scan line (typically five on each side), and $\|\mathbf{p}_i\|$ is the range from the sensor, used for normalization so that curvatures of near and far points can be compared.

- **Edge feature**: point of high curvature ($c_i > c_{\text{thresh}}^e$). Physically corresponds to corners, poles, and sharp boundaries.
- **Planar feature**: point of low curvature ($c_i < c_{\text{thresh}}^p$). Physically corresponds to walls, floors, and ceilings.

Additional rules when selecting feature points:
- Divide each scan line into four sectors to ensure a uniform distribution.
- If a neighbor has already been selected, exclude the current point (non-maximum suppression).
- Exclude points on near-horizontal surfaces or at occlusion boundaries because they are unstable.

**Odometry module (~10 Hz)**

A fast motion estimate is obtained via scan-to-scan matching. Feature points from the current scan are associated with feature points from the previous scan, but the distance metric depends on the feature type.

**Edge point-to-edge distance**: For an edge point $\mathbf{p}$ in the current scan, find the two closest edge points $\mathbf{a}, \mathbf{b}$ in the previous scan. The distance from $\mathbf{p}$ to the line $\overline{\mathbf{ab}}$ is

$$d_e = \frac{\|(\mathbf{p}-\mathbf{a}) \times (\mathbf{p}-\mathbf{b})\|}{\|\mathbf{a}-\mathbf{b}\|}$$

This is the magnitude of the cross product of the two vectors divided by the base length, which equals the height of the triangle (i.e., the distance from the point to the line).

**Planar point-to-plane distance**: For a planar point $\mathbf{p}$ in the current scan, find the three closest planar points $\mathbf{a}, \mathbf{b}, \mathbf{c}$ in the previous scan. The distance from $\mathbf{p}$ to the plane $\triangle\mathbf{abc}$ is

$$d_p = \frac{(\mathbf{p}-\mathbf{a})^T \left((\mathbf{a}-\mathbf{b}) \times (\mathbf{a}-\mathbf{c})\right)}{\|(\mathbf{a}-\mathbf{b}) \times (\mathbf{a}-\mathbf{c})\|}$$

The numerator is the scalar triple product, which equals the signed distance from the point to the plane multiplied by the magnitude of the normal.

Cost function:

$$\mathbf{T}^* = \underset{\mathbf{T}}{\arg\min} \sum_{\mathbf{p} \in \mathcal{E}} d_e(\mathbf{T}\cdot\mathbf{p})^2 + \sum_{\mathbf{p} \in \mathcal{P}} d_p(\mathbf{T}\cdot\mathbf{p})^2$$

It is optimized with Levenberg-Marquardt.

**Mapping module (~1 Hz)**

Scan-to-map registration provides precise pose refinement and map updates. The new scan is registered against the accumulated global map to correct drift.

The mapping module is slower than the odometry module but more accurate. The map is maintained as a cube centered on the current position, with voxel downsampling to control density.

**Motion Distortion Compensation**

A spinning LiDAR takes about 100 ms to complete a single scan. During that time the robot is moving, so the points in a scan are acquired at different instants. Without compensating for this motion distortion, registration accuracy suffers significantly.

Compensation method: If the pose change $\mathbf{T}_{s \to e}$ between the scan start $t_s$ and scan end $t_e$ is known, we estimate the intermediate pose at each point's timestamp $t_k$ by constant-velocity interpolation:

$$\mathbf{T}(t_k) = \text{Exp}\left(\frac{t_k - t_s}{t_e - t_s} \cdot \text{Log}(\mathbf{T}_{s \to e})\right)$$

Each point is then transformed to a reference time (typically the start of the scan):

$$\mathbf{p}_k^{\text{corrected}} = \mathbf{T}(t_k)^{-1} \cdot \mathbf{p}_k$$

When an IMU is available, IMU integration provides more accurate intermediate poses.

```python
# LOAM core pipeline pseudocode
class LOAM:
    def __init__(self):
        self.map_edge = VoxelMap(resolution=0.2)
        self.map_planar = VoxelMap(resolution=0.4)
        self.T_odom = np.eye(4)     # accumulated odometry transformation
        self.T_map = np.eye(4)      # mapping correction transformation
    
    def extract_features(self, scan):
        """Extract edge/planar features from a scan"""
        edge_points = []
        planar_points = []
        
        for scan_line in scan.lines:
            curvatures = []
            for i in range(5, len(scan_line) - 5):
                # Sum of vectors to 5 neighbors on each side, normalized by range
                diff = sum(scan_line[j] - scan_line[i] for j in range(i-5, i+6) if j != i)
                c = np.linalg.norm(diff) / (10 * np.linalg.norm(scan_line[i]))
                curvatures.append((c, i))
            
            # Split the scan line into 4 sectors for uniform extraction
            for sector in split_into_4(curvatures):
                sector.sort(reverse=True)  # sort by descending curvature
                
                n_edge, n_planar = 0, 0
                for c, idx in sector:
                    if c > EDGE_THRESH and n_edge < 2:
                        if not near_selected(idx, edge_points):
                            edge_points.append(scan_line[idx])
                            n_edge += 1
                
                sector.sort()  # sort by ascending curvature
                for c, idx in sector:
                    if c < PLANAR_THRESH and n_planar < 4:
                        if not near_selected(idx, planar_points):
                            planar_points.append(scan_line[idx])
                            n_planar += 1
        
        return edge_points, planar_points
    
    def odometry(self, edge_curr, planar_curr, edge_prev, planar_prev):
        """Scan-to-scan matching (10 Hz)"""
        T_relative = np.eye(4)
        tree_edge = KDTree(edge_prev)
        tree_planar = KDTree(planar_prev)
        
        for iter in range(25):
            residuals = []
            jacobians = []
            
            # Edge point-to-edge residuals
            for p in edge_curr:
                p_t = apply_transform(T_relative, p)
                _, idx = tree_edge.query(p_t, k=2)
                a, b = edge_prev[idx[0]], edge_prev[idx[1]]
                
                d_e = point_to_line_distance(p_t, a, b)
                J_e = point_to_line_jacobian(T_relative, p, a, b)
                residuals.append(d_e)
                jacobians.append(J_e)
            
            # Planar point-to-plane residuals
            for p in planar_curr:
                p_t = apply_transform(T_relative, p)
                _, idx = tree_planar.query(p_t, k=3)
                a, b, c = planar_prev[idx[0]], planar_prev[idx[1]], planar_prev[idx[2]]
                
                d_p = point_to_plane_distance(p_t, a, b, c)
                J_p = point_to_plane_jacobian(T_relative, p, a, b, c)
                residuals.append(d_p)
                jacobians.append(J_p)
            
            # LM update
            delta = levenberg_marquardt_step(residuals, jacobians)
            T_relative = se3_exp(delta) @ T_relative
            
            if np.linalg.norm(delta) < 1e-6:
                break
        
        self.T_odom = self.T_odom @ T_relative
        return self.T_odom
    
    def mapping(self, edge_curr, planar_curr):
        """Scan-to-map registration (1 Hz)"""
        # Extract the local map around the current position
        local_edge_map = self.map_edge.get_points_near(self.T_odom[:3, 3], radius=50.0)
        local_planar_map = self.map_planar.get_points_near(self.T_odom[:3, 3], radius=50.0)
        
        # Scan-to-map optimization (similar to odometry but against the map)
        T_correction = optimize_scan_to_map(edge_curr, planar_curr, 
                                            local_edge_map, local_planar_map)
        self.T_map = T_correction @ self.T_odom
        
        # Update the map
        self.map_edge.add_points(apply_transform(self.T_map, edge_curr))
        self.map_planar.add_points(apply_transform(self.T_map, planar_curr))
```

### 7.2.2 LeGO-LOAM

LeGO-LOAM ([Shan & Englot, 2018](https://doi.org/10.1109/IROS.2018.8594299)) adds ground segmentation to LOAM and trims the computation to achieve real-time operation even on embedded systems such as the Jetson TX2.

**Key additions in LeGO-LOAM**:

1. **Ground segmentation**: The point cloud is converted to a range image, and ground points are separated from non-ground points. A point is classified as ground if the slope between adjacent beams is below 10°. Ground points are used as planar features, and edge features are extracted from non-ground points.

2. **Point cloud segmentation**: On the non-ground points, range-image-based clustering is performed. Clusters below a certain size are discarded as noise. This preprocessing significantly improves feature quality compared to LOAM.

3. **Two-stage LM optimization**: Whereas LOAM optimizes all 6-DoF at once, LeGO-LOAM first estimates $[t_z, \theta_{\text{roll}}, \theta_{\text{pitch}}]$ from ground planar features and then $[t_x, t_y, \theta_{\text{yaw}}]$ from edge features. This decomposition improves convergence speed and stability.

4. **Pose graph optimization**: LeGO-LOAM adds loop closure and pose graph optimization, which were absent in LOAM, to correct global drift.

### 7.2.3 Why the LOAM Family Has Endured

Over ten years have passed since LOAM was published in 2014, yet the LOAM-family ideas remain the mainstream of LiDAR odometry. The reasons:

1. **Geometric clarity**: Edge and planar features correspond to physically meaningful geometric primitives. This structured-environment assumption fits most man-made environments well.

2. **Computational efficiency**: Using hundreds to thousands of feature points instead of the full point cloud (tens to hundreds of thousands of points) makes the system fast.

3. **Extensibility**: IMU (LIO-SAM), GPU (KISS-ICP), camera (LVI-SAM), and other modules can be plugged into the base framework.

4. **Robustness**: The edge/planar classification acts as a kind of outlier filter — points belonging to noise or dynamic objects do not show consistent curvature patterns and are naturally excluded.

That said, the limitations of the LOAM family are equally clear. Performance degrades in environments lacking geometric features (open fields, long tunnels), and the existing feature extraction is not well suited to the non-repetitive scan patterns of solid-state LiDARs. The direct approach of FAST-LIO2 addresses these limitations.

---

## 7.3 Tightly-Coupled LiDAR-Inertial Odometry

With LiDAR alone, motion distortion becomes severe under fast motion, and providing a good initial guess is difficult. A tightly coupled combination with an IMU overcomes these limitations.

### 7.3.1 LIO-SAM

LIO-SAM ([Shan et al., 2020](https://arxiv.org/abs/2007.00258)) is a LIO system that integrates LiDAR, IMU, GPS, and loop closure on top of a factor graph framework. It combines the LOAM-family feature-based approach with modern graph optimization.

**Factor-graph-based integration**

The core innovation of LIO-SAM is modeling the various sensor measurements as factors of a factor graph:

1. **IMU Preintegration Factor**: On-manifold preintegration from Forster et al. (2017) expresses the IMU constraint between consecutive keyframes. This factor provides constraints on the relative rotation, velocity, and position between two keyframes, along with bias estimation.

2. **LiDAR Odometry Factor**: LOAM-style scan-to-map matching estimates the relative pose. The result is inserted as a relative-pose factor between two keyframes.

3. **GPS Factor**: When GPS is available, the position measurement is added as a unary factor. In segments without GPS, this factor is absent, so the system naturally falls back to LiDAR+IMU only.

4. **Loop Closure Factor**: Place recognition (e.g., Scan Context) detects a loop, ICP estimates the relative pose, and the result is added as a binary factor.

All of these factors enter a single graph, and GTSAM's iSAM2 performs incremental optimization. The strength of the factor graph is its **modularity** — each sensor can independently add or remove factors, and adding a new sensor is straightforward.

**IMU-based de-skewing**

In LIO-SAM the IMU plays two roles:
1. **Motion distortion compensation**: IMU data during the LiDAR scan is used to precisely interpolate the per-timestamp pose of each point for de-skewing.
2. **Initial guess**: IMU preintegration predicts the pose of the next keyframe, which is used as the initial guess for scan-to-map registration.

This bidirectional coupling is the essence of the "tightly-coupled" nature of LIO-SAM: the IMU provides the LiDAR with the initial guess and de-skewing, while the LiDAR provides the IMU with pose correction and bias estimation.

**Keyframe-based efficiency**

Instead of a global map, scan matching is performed against a submap observed by the keyframes around the current position. This sliding-window approach significantly reduces computation compared to the global map.

```python
# LIO-SAM factor graph construction pseudocode
import gtsam

### 7.3.2 FAST-LIO / FAST-LIO2

FAST-LIO2 ([Xu et al., 2022](https://doi.org/10.1109/TRO.2022.3141855)) takes a completely different approach from the LOAM family. It eliminates feature extraction and is a direct LiDAR-inertial odometry that registers raw LiDAR points directly to the map.

**Three core innovations**:

**1. Direct point registration (no feature extraction)**

Unlike LOAM, which extracts edge/planar features, FAST-LIO2 uses every raw point directly. For each point $\mathbf{p}_k$ it finds the closest plane in the map and minimizes the point-to-plane distance:

$$d_k = \mathbf{n}_k^T (\mathbf{T} \cdot \mathbf{p}_k - \mathbf{q}_k)$$

where $\mathbf{n}_k$ is the normal of the closest plane in the map and $\mathbf{q}_k$ is the closest point.

Why eliminate feature extraction?
- Feature extraction is information loss — useful points can be discarded depending on the classification threshold.
- It applies generically across diverse LiDAR scan patterns (spinning, solid-state, non-repetitive). In particular, solid-state LiDARs such as Livox use non-repetitive scanning, which is poorly suited to the existing curvature-based feature extraction.
- With a sufficiently efficient map data structure (ikd-Tree), raw-point registration can be performed in real time.

**2. ikd-Tree (Incremental k-d Tree)**

The second key innovation of FAST-LIO2 is the map data structure. A conventional kd-tree is static and inefficient for point insertion/deletion. The ikd-Tree supports:

- **Point insertion**: insert a new point in $O(\log N)$ time.
- **Point deletion**: efficient removal of points outside the map region via lazy deletion.
- **Dynamic re-balancing**: when insertions/deletions unbalance the tree, partial rebuilding in a scapegoat-tree style restores balance.
- **Box-range deletion**: points in regions far from the current position are deleted in box units to manage map size.

Thanks to the ikd-Tree, FAST-LIO2 maintains the map in real time while also performing fast nearest-neighbor search.

**3. Iterated Extended Kalman Filter (IEKF)**

FAST-LIO2 uses a filter-based approach (IEKF), not an optimization-based one (as in LIO-SAM).

A standard EKF linearizes the observation model only once; when the LiDAR observation is highly nonlinear, this linearization is inaccurate. IEKF repeats the update several times to improve the linearization point:

$$\hat{\mathbf{x}}^{(k+1)} = \hat{\mathbf{x}}^{-} + \mathbf{K}^{(k)} (\mathbf{z} - h(\hat{\mathbf{x}}^{(k)}) - \mathbf{H}^{(k)}(\hat{\mathbf{x}}^{-} - \hat{\mathbf{x}}^{(k)}))$$

where $k$ is the iteration index, $\hat{\mathbf{x}}^{-}$ is the prediction, and $\mathbf{H}^{(k)}$ is the Jacobian at $\hat{\mathbf{x}}^{(k)}$.

Kalman gain:
$$\mathbf{K}^{(k)} = \mathbf{P}^{-} (\mathbf{H}^{(k)})^T (\mathbf{H}^{(k)} \mathbf{P}^{-} (\mathbf{H}^{(k)})^T + \mathbf{R})^{-1}$$

IEKF typically converges in 3-5 iterations. It achieves an effect similar to Gauss-Newton optimization while retaining the filter's advantage of naturally propagating uncertainty (covariance).

**State vector**:

$$\mathbf{x} = [{}^G\mathbf{R}_I, {}^G\mathbf{p}_I, {}^G\mathbf{v}_I, \mathbf{b}_g, \mathbf{b}_a, {}^I\mathbf{R}_L, {}^I\mathbf{p}_L, \mathbf{g}]$$

In addition to the rotation ${}^G\mathbf{R}_I$, position ${}^G\mathbf{p}_I$, velocity ${}^G\mathbf{v}_I$, gyro bias $\mathbf{b}_g$, and accelerometer bias $\mathbf{b}_a$, the state also includes the LiDAR-IMU extrinsics ${}^I\mathbf{R}_L, {}^I\mathbf{p}_L$ and the gravity vector $\mathbf{g}$. In other words, the extrinsic calibration and the gravity direction are also estimated online.

**Performance of FAST-LIO2**: Up to 100 Hz odometry + mapping is achieved in outdoor environments. It runs on multi-line spinning LiDARs, solid-state LiDARs (Livox), UAV/handheld platforms, and Intel/ARM processors alike.

```cpp
// FAST-LIO2 IEKF update pseudocode (C++)
struct State {
    Matrix3d R_GI;    // world-to-IMU rotation
    Vector3d p_GI;    // IMU position in world
    Vector3d v_GI;    // IMU velocity in world
    Vector3d bg, ba;  // gyro/accel bias
    Matrix3d R_IL;    // IMU-to-LiDAR rotation
    Vector3d p_IL;    // IMU-to-LiDAR translation
    Vector3d gravity; // gravity vector
};

void FASTLIO2::iterated_ekf_update(const PointCloud& scan, State& x, MatrixXd& P) {
    State x_predict = x;  // keep the prediction result
    MatrixXd K;  // reuse the Kalman gain of the final iteration outside the loop
    MatrixXd H;  // reuse the Jacobian of the final iteration outside the loop
    int n_valid = 0;
    
    for (int iter = 0; iter < MAX_ITER; iter++) {
        // 1. Transform points to the world frame using the current state
        PointCloud world_pts = transform_to_world(scan, x);
        
        // 2. For each point, find the nearest plane in the ikd-tree
        vector<Plane> planes = ikd_tree.find_nearest_planes(world_pts, k=5);
        
        // 3. Compute the observation Jacobians and residuals
        n_valid = 0;
        H.resize(scan.size(), STATE_DIM);
        VectorXd z(scan.size());
        
        for (int i = 0; i < scan.size(); i++) {
            if (!planes[i].valid) continue;
            
            Vector3d p_w = x.R_GI * (x.R_IL * scan[i] + x.p_IL) + x.p_GI;
            
            // Point-to-plane residual: the observation is 0 (point should lie on the plane)
            // z = 0 - h(x) = -d_k
            z(n_valid) = -planes[i].normal.dot(p_w - planes[i].center);
            
            // Jacobian: d(residual) / d(state_error)
            // ∂z/∂δθ_GI = n^T * [-(R_GI(R_IL*p + p_IL))×]
            // ∂z/∂δp_GI = n^T
            // ∂z/∂δθ_IL = n^T * R_GI * [-(R_IL*p)×]
            // ∂z/∂δp_IL = n^T * R_GI
            H.row(n_valid) = compute_jacobian(x, scan[i], planes[i]);
            n_valid++;
        }
        
        H.conservativeResize(n_valid, STATE_DIM);
        z.conservativeResize(n_valid);
        
        // 4. IEKF update
        MatrixXd S = H * P * H.transpose() + R_meas * MatrixXd::Identity(n_valid, n_valid);
        K = P * H.transpose() * S.inverse();
        
        VectorXd dx = K * (z - H * state_difference(x_predict, x));
        
        // 5. State correction (on-manifold): x^{(k+1)} = x^{-} ⊞ dx
        x = state_plus(x_predict, dx);
        
        // Check convergence
        if (dx.norm() < CONVERGENCE_THRESH) break;
    }
    
    // Covariance update
    MatrixXd I_KH = MatrixXd::Identity(STATE_DIM, STATE_DIM) - K * H;
    P = I_KH * P * I_KH.transpose() + K * R_meas * K.transpose();
    
    // Map update: insert the registered points into the ikd-tree
    PointCloud aligned = transform_to_world(scan, x);
    ikd_tree.insert(aligned);
}
```

### 7.3.3 Faster-LIO

Faster-LIO replaces the ikd-Tree of FAST-LIO2 with an incremental voxel structure to achieve even faster processing.

The key change: instead of a kd-tree, a hash-map-based voxel structure is used. A plane is maintained within each voxel, and the plane parameters are updated incrementally whenever a point is added. The $O(\log N)$ kd-tree search is replaced by $O(1)$ hash access, boosting speed.

### 7.3.4 Point-LIO

Point-LIO ([He et al., 2023](https://doi.org/10.1002/aisy.202200459)) is an extreme extension of the FAST-LIO series. It updates the state at the granularity of **individual points** rather than scans.

**Why per-point processing?**

Conventional LIO treats an entire scan (~100 ms) as a single observation. During that interval, motion distortion is corrected by constant-velocity interpolation, but under fast/high-angular-rate motion the constant-velocity assumption breaks down.

Point-LIO performs an EKF update as soon as each point arrives (on the order of $\sim$μs). Using the IMU's high-rate (~1 kHz) measurements together with the LiDAR point timestamps, it uses the IMU state that exactly corresponds to each point.

**Mathematical core**: Point-LIO's state propagation model, over the short interval between IMU measurements, discretizes

$$\frac{d}{dt}\mathbf{R} = \mathbf{R}[\boldsymbol{\omega}]_\times, \quad \frac{d}{dt}\mathbf{v} = \mathbf{R}\mathbf{a} + \mathbf{g}, \quad \frac{d}{dt}\mathbf{p} = \mathbf{v}$$

Whenever a point arrives, it performs state propagation followed by a single-point update, effectively approximating a continuous-time filter.

Advantages: Accurate odometry even under extremely fast motion (hundreds of degrees per second of rotation). Motion distortion compensation happens implicitly (each point is already processed with the correct per-timestamp state).

Disadvantages: The number of updates scales with the number of points, increasing the computational burden. It is about 2-3 times slower than FAST-LIO2.

### 7.3.5 COIN-LIO

[COIN-LIO (Pfreundschuh et al., 2024)](https://arxiv.org/abs/2310.01235) adds **camera intensity** information to a LiDAR-Inertial system. Coupling a camera into a traditional LIO typically requires a separate visual feature extraction/tracking stack, but COIN-LIO takes a simpler approach:

It records the brightness (intensity) of the camera pixel corresponding to each LiDAR point and assigns an intensity to each map point as well. During registration, the intensity difference is included in the cost together with the geometric distance (point-to-plane):

$$e_k = \alpha \cdot d_{\text{geom}}(\mathbf{p}_k) + (1-\alpha) \cdot |I_{\text{obs}}(\mathbf{p}_k) - I_{\text{map}}(\mathbf{p}_k)|$$

In geometrically degenerate environments — for example long tunnels or empty halls — the intensity information provides additional constraints and preserves accuracy. It is a pragmatic approach that exploits the camera's texture information while avoiding the complexity of a full VIO pipeline.

---

## 7.4 Continuous-Time LiDAR Odometry

Existing LiDAR odometry uses a discrete-time model. It assigns one pose per scan and approximates intra-scan motion by constant-velocity interpolation. The continuous-time approach overcomes this limitation by representing the trajectory as a continuous function.

### 7.4.1 CT-ICP

CT-ICP ([Dellenbach et al., 2022](https://arxiv.org/abs/2109.12979)) assigns not one but **two poses** per scan (at the start and end of the scan).

For each point's timestamp $t_k \in [t_s, t_e]$ within the scan, the pose is linearly interpolated:

$$\mathbf{T}(t_k) = \mathbf{T}_s \cdot \text{Exp}\left(\frac{t_k - t_s}{t_e - t_s} \cdot \text{Log}(\mathbf{T}_s^{-1}\mathbf{T}_e)\right)$$

The two poses $\mathbf{T}_s, \mathbf{T}_e$ are optimized jointly. Unlike constant-velocity interpolation, the intra-scan motion model is refined along with the optimization.

CT-ICP effectively compensates for motion distortion without an IMU, making it especially useful for IMU-less systems.

### 7.4.2 B-Spline-Based Trajectory Representation

A more general continuous-time approach represents the trajectory with a B-spline. A B-spline is a smooth curve defined by control points $\{\mathbf{T}_i\}$:

$$\mathbf{T}(t) = \prod_{i=0}^{k} \text{Exp}\left(B_i(t) \cdot \text{Log}(\mathbf{T}_{i-1}^{-1}\mathbf{T}_i)\right)$$

where $B_i(t)$ is a B-spline basis function. Cubic B-splines are commonly used and guarantee $C^2$ continuity.

Advantages of a B-spline trajectory:
1. **Query at arbitrary times**: At any time $t$, the pose, velocity, and acceleration can be obtained via differentiation. This enables natural handling of asynchronous sensor data.
2. **Smooth trajectory**: A physically plausible smooth trajectory is guaranteed.
3. **Local control**: Thanks to the locality of B-splines, modifying one control point does not affect the entire trajectory.

Disadvantages:
- The control-point (knot) spacing is the primary hyperparameter. Too dense causes overfitting; too sparse fails to represent fast motion.
- Computation increases compared to the discrete-time case.

The camera-IMU calibration in Kalibr (see Ch.3) also uses a B-spline trajectory representation.

---

## 7.5 Solid-State LiDAR Specifics

Solid-state LiDARs (e.g., the Livox series) have a fundamentally different scan pattern from spinning LiDARs.

**Spinning vs solid-state**:

| Property | Spinning (Velodyne, Ouster) | Solid-state (Livox) |
|------|--------------------------|---------------------|
| Scan pattern | Repetitive (same pattern every rotation) | Non-repetitive (petal / rose pattern) |
| FoV | 360° horizontal | Limited (70-77°) |
| Point density | Uniform | Accumulates over time, non-uniform |
| Price | High | Low |
| Size / weight | Large | Small |

**Impact of non-repetitive scanning on feature extraction**

LOAM-style curvature-based feature extraction uses neighbors on the same scan line. However, solid-state LiDARs have no defined scan line and their points are distributed irregularly. Consequently:

1. The existing line-based curvature computation does not apply.
2. Instead, one must use KNN (K-Nearest Neighbors)-based local curvature, or abandon feature extraction altogether and use raw points.

**Why FAST-LIO is strong on solid-state**

FAST-LIO/FAST-LIO2 work well on solid-state LiDARs for three reasons:

1. **No feature extraction needed**: Using raw points directly, the system is agnostic to the scan pattern.
2. **Leverages the non-repetitive scan**: A solid-state LiDAR gradually fills the FoV more densely over time. FAST-LIO2's ikd-Tree map naturally accommodates this progressive densification, so map quality improves over time.
3. **Compensates for the narrow FoV**: A narrow FoV means less information per scan, but tight coupling with the IMU compensates for this.

The Livox series is rapidly spreading in drones, handheld devices, and small robots thanks to its strong price/performance, and the FAST-LIO2 + Livox combination is currently one of the most popular LIO configurations.

---

## 7.6 Learning-Based LiDAR Odometry

### 7.6.1 The DeepLO Family

Learning-based LiDAR odometry trains a network that takes a pair of point clouds as input and predicts the relative pose.

Representative approaches:
- **LO-Net** (Li et al., 2019): Converts a LiDAR scan to a 2D range image and uses a CNN to extract features and predict the pose. Normal estimation and mask prediction are added as auxiliary tasks to encourage geometric understanding.
- **DeepLO** (Cho et al., 2020): Predicts the pose by processing 3D point clouds directly using a PointNet backbone.
- **PWCLO-Net** (Wang et al., 2021): Applies the Pyramid, Warping, and Cost-volume architecture to LiDAR odometry.

### 7.6.2 Current Limitations

Learning-based LiDAR odometry still lags significantly behind classical methods. The reasons:

1. **Characteristics of LiDAR data**: Unlike images, point clouds are unstructured, sparse, and unordered. CNNs cannot process them naturally.

2. **Geometry is already sufficient**: Geometric methods such as ICP/GICP/NDT are already very accurate. The reasons learning shines in the camera domain — illumination changes, lack of texture, and other issues that are hard to solve with geometry alone — do not apply to LiDAR.

3. **Lack of data**: Large-scale LiDAR odometry training data is much scarcer than image data.

4. **Generalization**: A model trained on a particular LiDAR/environment does not generalize well to other LiDARs/environments.

Today, learning is more effective as auxiliary components than as LiDAR odometry itself:
- **Loop closure detection**: Scan Context, PointNetVLAD, etc.
- **Point cloud registration initialization**: GeoTransformer (see Ch.5)
- **Semantic segmentation**: dynamic-object removal, road/building classification

---

## 7.7 Recent Trends (2023-2024)

Beyond the systems discussed above, several recent studies in LiDAR odometry are worth noting.

**[KISS-ICP (Vizzo et al., 2023)](https://arxiv.org/abs/2209.15397)**: Shows that point-to-point ICP, combined only with adaptive thresholding, a robust kernel, and simple motion compensation, can achieve SOTA-level performance. The key is generality — it operates without tuning, regardless of the sensor type (automotive, UAV, handheld). The work reaffirms the "power of simplicity" in LiDAR odometry.

**[MAD-ICP (Ferrari et al., 2024)](https://arxiv.org/abs/2405.05828)**: Uses a PCA-based kd-tree to extract the structural information of the point cloud and applies it to point-to-plane registration. It emphasizes the importance of the data-matching strategy itself and matches the performance of domain-specific methods across a variety of LiDAR sensors.

**[iG-LIO (Chen et al., 2024)](https://github.com/zijiechenrobotics/ig_lio)**: A system that integrates incremental GICP into a tightly coupled LIO. A voxel-based surface covariance estimator (VSCE) improves the efficiency of GICP's covariance computation, and an incremental voxel map reduces the nearest-neighbor search cost. It is more efficient than Faster-LIO while retaining SOTA-level accuracy.

---

## Chapter 7 Summary

| System | Approach | Estimation | Sensors | Key feature |
|--------|------|-----------|------|-----------|
| ICP/GICP/NDT | Registration | Iterative optimization | LiDAR only | Fundamental building block |
| LOAM | Feature-based | LM optimization | LiDAR only | Edge/planar feature, two-stage architecture |
| LeGO-LOAM | Feature-based | LM optimization | LiDAR only | Ground segmentation, lightweight |
| LIO-SAM | Feature-based | Factor graph (iSAM2) | LiDAR + IMU + GPS | Modular multi-sensor integration |
| FAST-LIO2 | Direct | IEKF | LiDAR + IMU | No feature extraction, ikd-Tree, 100 Hz |
| Point-LIO | Direct | Point-wise EKF | LiDAR + IMU | Per-point update, fast motion |
| COIN-LIO | Direct + intensity | IEKF | LiDAR + IMU + camera (intensity) | Intensity-based degeneration mitigation |
| CT-ICP | Direct | Optimization | LiDAR only | Continuous-time motion model, no IMU needed |
| KISS-ICP | Direct (P2P) | Iterative optimization | LiDAR only | Adaptive threshold, tuning-free, general-purpose |
| MAD-ICP | Direct (P2Plane) | Iterative optimization | LiDAR only | PCA-based structural extraction, data-matching focus |
| iG-LIO | Direct (GICP) | IEKF | LiDAR + IMU | Incremental GICP, voxel covariance estimation |

**The big picture of LiDAR odometry**:

The LOAM (2014) → LeGO-LOAM (2018) → LIO-SAM (2020) lineage evolved in the direction of **feature-based + factor graph**. The FAST-LIO (2021) → FAST-LIO2 (2022) → Point-LIO (2023) lineage evolved in the direction of **direct + Kalman filter**. The two lineages represent different design philosophies, yet their practical performance is converging to similar levels.

The feature-based approach is strong in structured environments (buildings, cities), and the direct approach is strong in unstructured environments (forests, caves) and on solid-state LiDARs. In the next chapter we take up multi-sensor fusion architectures that integrate these two sensor modalities (camera + LiDAR) together with an IMU.

---

# Ch.8 — Multi-Sensor Fusion Architectures

> A design discipline that goes beyond individual odometry to address **how multiple sensors are integrated**.
> Having covered Visual Odometry and LiDAR Odometry separately in previous chapters, this chapter provides an in-depth analysis of the architectures that weave them into a single system.

---

## 8.1 Taxonomy of Fusion Architectures

The first decision in designing a multi-sensor fusion system is **at what level the sensor data will be combined**. The depth of this coupling fundamentally determines the system's complexity, performance, and failure modes.

### 8.1.1 Loosely Coupled

**Intuition**: Each sensor is viewed as an independent "expert." Each expert draws its own conclusion (pose, velocity, etc.) from its own data, and a higher-level stage then synthesizes these conclusions.

Concretely, the LiDAR odometry module independently estimates $\mathbf{T}_{L}$ from LiDAR scans and the Visual odometry module independently estimates $\mathbf{T}_{V}$ from images, and a higher-level fusion module combines these two estimates.

$$
\hat{\mathbf{x}} = \arg\min_{\mathbf{x}} \left\| \mathbf{x} - \mathbf{x}_{LiDAR} \right\|^2_{\mathbf{P}_{L}^{-1}} + \left\| \mathbf{x} - \mathbf{x}_{Visual} \right\|^2_{\mathbf{P}_{V}^{-1}}
$$

Here $\mathbf{P}_{L}$ and $\mathbf{P}_{V}$ are the covariances reported by each subsystem.

**Advantages**:
- High modularity. Each sensor module can be swapped or upgraded independently.
- Easier debugging. It is easy to trace which module a problem came from.
- If one sensor fails, the others continue to operate.

**Disadvantages**:
- Because each subsystem combines its output after information has already been lost, the full benefit of complementary interaction between sensors cannot be exploited. For example, LiDAR's precise geometric information can resolve a camera's scale ambiguity, but in a loosely coupled design this interaction is limited.
- The consistency of the covariances reported by each subsystem is not guaranteed. If a subsystem reports an overly optimistic covariance, the fusion result is distorted.

### 8.1.2 Tightly Coupled

**Intuition**: All sensors' **raw measurements** are fed directly into a single estimator. Instead of consulting "experts," a single "chief analyst" inspects all raw data directly.

From a factor graph perspective, each sensor's raw measurements are inserted as independent factors:

Here $\mathbf{r}^{\text{IMU}}_i$ is the IMU preintegration residual, $\mathbf{r}^{\text{LiDAR}}_j$ is the point-to-plane residual, and $\mathbf{r}^{\text{cam}}_k$ is the reprojection error.

**Advantages**:
- Sensor-to-sensor interaction is exploited to the fullest. For example, the IMU corrects the LiDAR's motion distortion, and the LiDAR anchors the VIO's scale.
- Fusion is close to information-theoretically optimal.

**Disadvantages**:
- System complexity is high. The observation models, noise models, and time synchronization of all sensors must be managed within a single framework.
- Anomalous data from one sensor can contaminate the entire estimate (outlier handling is essential).
- Achieving real-time performance is difficult.

**Representative systems**: [LIO-SAM](https://arxiv.org/abs/2007.00258) (LiDAR+IMU+GPS), VINS-Mono (Camera+IMU), [R3LIVE](https://arxiv.org/abs/2109.07982) (Camera+LiDAR+IMU).

### 8.1.3 Ultra-Tightly Coupled (Signal-Level Coupling)

**Intuition**: Not the sensor's measurements but the **signals themselves** are combined. This is the most extreme form of integration.

A representative example is GNSS-INS ultra-tight coupling. A typical GNSS receiver extracts pseudoranges from satellite signals and then combines them with the INS, but in the ultra-tight approach the position and velocity estimated by the INS directly aid the GNSS receiver's code/carrier tracking loops. Doing so lets the receiver keep tracking GNSS signals longer even in weak-signal environments (urban canyons, right after entering indoors).

$$
\text{NCO frequency} = f_{\text{nominal}} + \Delta f_{\text{INS-aided}}
$$

Here the frequency of the NCO (Numerically Controlled Oscillator) is corrected by the Doppler shift predicted by the INS, widening the receiver's tracking range.

**Reality**: Ultra-tight coupling requires access to the GNSS receiver at the hardware/firmware level, so it is rarely seen outside military and aviation applications. For most robotics systems, tightly coupled is the practical limit.

### 8.1.4 Comparison of the Three Levels

```
Measurement flow:

[Loosely]    Sensor A → Subsystem A → Pose A ─┐
                                              ├→ Fusion → Final pose
             Sensor B → Subsystem B → Pose B ─┘

[Tightly]    Sensor A → raw meas. A ──┐
                                      ├→ Single Optimizer → Final pose
             Sensor B → raw meas. B ──┘

[Ultra-Tight] Sensor A signal ←→ Sensor B estimate (bidirectional signal-level coupling)
```

```python
import numpy as np
from scipy.linalg import inv

def loosely_coupled_fusion(x_lidar, P_lidar, x_visual, P_visual):
    """
    Loosely coupled fusion: combine independent estimates from two subsystems
    via a covariance-weighted average.

Parameters:
        x_lidar: state estimate from the LiDAR subsystem (n,)
        P_lidar: covariance of the LiDAR estimate (n, n)
        x_visual: state estimate from the Visual subsystem (n,)
        P_visual: covariance of the Visual estimate (n, n)

Returns:
        x_fused: fused state estimate (n,)
        P_fused: fused covariance (n, n)
    """
    # Convert to information form
    I_lidar = inv(P_lidar)
    I_visual = inv(P_visual)

# Sum of information matrices
    I_fused = I_lidar + I_visual
    P_fused = inv(I_fused)

# Information-weighted mean
    x_fused = P_fused @ (I_lidar @ x_lidar + I_visual @ x_visual)

return x_fused, P_fused

# Example: 2D position estimation
x_lidar = np.array([10.1, 5.2])      # Position estimated by LiDAR
P_lidar = np.diag([0.01, 0.01])       # LiDAR is precise and isotropic
x_visual = np.array([10.0, 5.0])      # Position estimated by Visual
P_visual = np.diag([0.1, 0.05])       # Visual is less precise in the vertical direction

x_fused, P_fused = loosely_coupled_fusion(x_lidar, P_lidar, x_visual, P_visual)
print(f"LiDAR:  {x_lidar}, P_diag: {np.diag(P_lidar)}")
print(f"Visual: {x_visual}, P_diag: {np.diag(P_visual)}")
print(f"Fused:  {x_fused}, P_diag: {np.diag(P_fused)}")
# The fused result is closer to the LiDAR estimate (since its covariance is smaller)
```

---

## 8.2 Camera + LiDAR + IMU Fusion

The combination of camera, LiDAR, and IMU currently constitutes the most information-rich sensor suite in autonomous driving and robotics. The camera provides texture and color information, the LiDAR provides precise 3D geometry, and the IMU provides high-rate inertial measurements. These three sensors complement one another's weaknesses:

| Situation | Camera | LiDAR | IMU |
|------|--------|-------|-----|
| Dark environment | ✗ | ✓ | ✓ |
| Textureless wall | ✗ | ✓ | ✓ |
| Geometric degeneracy (long corridor) | ✓ | ✗ | ✓ |
| High-speed rotation | ✗ | ✗ | ✓ |
| Scale observability | ✗ (monocular) | ✓ | ✗ |
| Color/semantics | ✓ | ✗ | ✗ |

We analyze recent state-of-the-art systems that integrate these three sensors.

### 8.2.1 R3LIVE / R3LIVE++

[R3LIVE](https://arxiv.org/abs/2109.07982) (Lin et al., 2022) is a system that tightly couples two subsystems: LiDAR-Inertial Odometry (LIO) and Visual-Inertial Odometry (VIO).

**Core architectural idea**:

R3LIVE adopts a **dual-subsystem** architecture. The LIO subsystem is responsible for geometry and the VIO subsystem is responsible for photometric (texture) information, and the two subsystems are tightly coupled by **sharing a single state**.

```
LiDAR scan ──→ [LIO subsystem] ──→ state update (geometry)
                     ↓                       ↓
                 IMU data ───────────→ shared state vector
                     ↑                       ↑
Camera image ──→ [VIO subsystem] ──→ state update (photometric)
```

**LIO subsystem**: Identical to FAST-LIO2, raw LiDAR points are directly point-to-plane registered to an ikd-Tree-based map. The state is updated by an Iterated EKF.

**VIO subsystem**: This is where R3LIVE's originality shines. A typical VIO minimizes the reprojection error of feature points, whereas R3LIVE uses the **photometric error**. Specifically, each point in the 3D map built by LIO is assigned an RGB color, and when a new camera image arrives, these map points are projected into the image to minimize the **difference between the observed color and the color stored in the map**:

$$
\mathbf{r}^{\text{photo}}_i = \mathbf{I}(\pi(\mathbf{T}_{CW} \mathbf{p}^W_i)) - \mathbf{c}_i^{\text{map}}
$$

Here $\mathbf{I}(\cdot)$ is the pixel intensity of the image, $\pi(\cdot)$ is the 3D→2D projection function, $\mathbf{T}_{CW}$ is the world-to-camera transformation, $\mathbf{p}^W_i$ is the 3D coordinate of a map point, and $\mathbf{c}_i^{\text{map}}$ is the color stored in the map for that point.

**Robustness**: The key design benefit is that if either the LiDAR or the camera temporarily fails, the system continues to operate with the remaining sensors. When the LiDAR is occluded, VIO+IMU operate; when the camera is dark, LIO+IMU operate.

**Result**: It produces a survey-grade colored 3D map in real time while performing SLAM.

### 8.2.2 LVI-SAM

[LVI-SAM](https://arxiv.org/abs/2104.10831) (Shan et al., 2021) is an extension of LIO-SAM that couples a Visual-Inertial subsystem and a LiDAR-Inertial subsystem **bidirectionally**.

**The essence of bidirectional coupling**:

- **VIS → LIS direction**: The pose estimated by the Visual-Inertial subsystem is used as the initial guess for LiDAR scan matching. Especially when the LiDAR alone yields an inaccurate initial guess (high-speed rotation, featureless environments), VIS provides the initial guess and helps LiDAR registration converge.

- **LIS → VIS direction**: The depth information estimated by the LiDAR is assigned to Visual feature points, accelerating depth initialization in the Visual subsystem. In monocular VIO, feature depth is estimated by triangulation, but depth is inaccurate until sufficient parallax accumulates. By providing this depth directly from the LiDAR, immediate initialization becomes possible.

```
         ┌─── VIS initial pose ───→ LIS initial guess
         │                            │
  [Visual-Inertial]            [LiDAR-Inertial]
         │                            │
         └←── LiDAR depth ────────────┘

↓ both factors ↓
           [Factor Graph (GTSAM/iSAM2)]
                     ↓
              Final optimized pose
```

**Factor graph design**: The following factors are inserted into the LVI-SAM factor graph:
- IMU preintegration factor (between successive keyframes)
- LiDAR odometry factor (scan matching result)
- Visual odometry factor (feature tracking result)
- GPS factor (when available)
- Loop closure factor (upon revisit detection)

### 8.2.3 FAST-LIVO / FAST-LIVO2

[FAST-LIVO2](https://arxiv.org/abs/2408.14035) (Zheng et al., 2024) is a direct Camera+LiDAR+IMU fusion system developed by the FAST-LIO2 team (HKU MARS Lab). "Direct" means raw data is used without feature extraction.

**Key innovation 1 — Sequential Update**:

Measurements from heterogeneous sensors have different dimensionalities. LiDAR provides 3D point-to-plane residuals, while the camera provides 2D photometric residuals. Stacking them into a single large residual vector and optimizing simultaneously complicates the Jacobian matrix structure and can be numerically unstable.

FAST-LIVO2 solves this problem with **sequential Bayesian updates**:

1. Predict the state with the IMU (prediction)
2. Update the state with LiDAR measurements (first update)
3. Update the state again with camera measurements (second update)

Theoretically, if the measurements are independent, sequential updating gives the same result as simultaneous updating:

$$
p(\mathbf{x} | \mathbf{z}_L, \mathbf{z}_C) = p(\mathbf{x} | \mathbf{z}_C, \mathbf{z}_L) \propto p(\mathbf{z}_C | \mathbf{x}) \cdot p(\mathbf{z}_L | \mathbf{x}) \cdot p(\mathbf{x})
$$

Sequentially:
$$
\underbrace{p(\mathbf{x} | \mathbf{z}_L)}_{\text{after LiDAR update}} \propto p(\mathbf{z}_L | \mathbf{x}) \cdot p(\mathbf{x})
$$
$$
\underbrace{p(\mathbf{x} | \mathbf{z}_L, \mathbf{z}_C)}_{\text{after camera update}} \propto p(\mathbf{z}_C | \mathbf{x}) \cdot p(\mathbf{x} | \mathbf{z}_L)
$$

In the second equation, $p(\mathbf{x} | \mathbf{z}_L)$ serves as the prior, and the final result is mathematically equivalent to the simultaneous update.

**Key innovation 2 — Unified adaptive voxel map**:

FAST-LIVO2 uses a single voxel map based on a hash table plus an octree. The LiDAR module builds the geometric structure (3D coordinates, normal vectors), and the Visual module attaches image patches to the same map points. Geometry and texture are thus managed consistently within a single map.

**Key innovation 3 — Affine warping using LiDAR normals**:

When comparing image patches in a camera direct method, affine warping that accounts for surface tilt improves accuracy. FAST-LIVO2 leverages the planar normal vectors extracted from the LiDAR to perform accurate affine warping without any separate normal estimation. This is a concrete example of LiDAR-camera complementarity.

**Key innovation 4 — Real-time exposure compensation**:

In environments with rapidly changing illumination (entering/exiting a tunnel), FAST-LIVO2 estimates the exposure time online and corrects the photometric error accordingly.

```python
import numpy as np

def sequential_ekf_update(x_pred, P_pred, z_lidar, H_lidar, R_lidar, z_cam, H_cam, R_cam):
    """
    Sequential EKF update in the order LiDAR → Camera.
    Mathematically equivalent to a simultaneous update but avoids the
    dimensionality-mismatch issue.

Parameters:
        x_pred: predicted state (n,)
        P_pred: predicted covariance (n, n)
        z_lidar: LiDAR measurement (m_L,)
        H_lidar: LiDAR observation Jacobian (m_L, n)
        R_lidar: LiDAR measurement noise (m_L, m_L)
        z_cam: camera measurement (m_C,)
        H_cam: camera observation Jacobian (m_C, n)
        R_cam: camera measurement noise (m_C, m_C)

Returns:
        x_updated: final updated state
        P_updated: final updated covariance
    """
    # Step 1: LiDAR update
    S_L = H_lidar @ P_pred @ H_lidar.T + R_lidar
    K_L = P_pred @ H_lidar.T @ np.linalg.inv(S_L)
    y_L = z_lidar - H_lidar @ x_pred  # innovation
    x_after_lidar = x_pred + K_L @ y_L
    P_after_lidar = (np.eye(len(x_pred)) - K_L @ H_lidar) @ P_pred

# Step 2: Camera update (use the post-LiDAR result as the prior)
    S_C = H_cam @ P_after_lidar @ H_cam.T + R_cam
    K_C = P_after_lidar @ H_cam.T @ np.linalg.inv(S_C)
    y_C = z_cam - H_cam @ x_after_lidar  # innovation
    x_updated = x_after_lidar + K_C @ y_C
    P_updated = (np.eye(len(x_pred)) - K_C @ H_cam) @ P_after_lidar

return x_updated, P_updated
```

### 8.2.4 Comparison of Multimodal Factor Graph Designs

We compare the designs of the three systems from a factor graph perspective:

| Aspect | R3LIVE | LVI-SAM | FAST-LIVO2 |
|------|--------|---------|------------|
| Backend | IEKF (dual subsystem) | iSAM2 (factor graph) | IEKF (sequential) |
| LiDAR processing | Direct (point-to-plane) | Feature-based (LOAM) | Direct (point-to-plane) |
| Camera processing | Direct (photometric) | Feature-based (ORB) | Direct (photometric) |
| Map representation | ikd-Tree + RGB | Voxel map | Hash+Octree voxel map |
| Feature extraction | Not required | Required (edge/planar, ORB) | Not required |
| GPS integration | None | Integrated as factor | None |
| Loop closure | None | Integrated as factor | None |
| Embedded support | Limited | Limited | ARM real-time capable |

**Selection criteria**:
- If loop closure and GPS are needed: LVI-SAM
- If the highest-precision colored map is needed: R3LIVE
- If real-time operation on embedded platforms is needed: FAST-LIVO2
- In environments with scarce feature points (textureless walls, interiors of structures): direct methods (R3LIVE, FAST-LIVO2)

---

## 8.3 GNSS Integration

GNSS (Global Navigation Satellite System) is the only sensor that provides a global position reference. No matter how precise IMU+LiDAR+camera are, they all provide only **relative** measurements, so drift accumulates over long-duration operation. GNSS serves as an anchor that corrects this drift.

### 8.3.1 GNSS Factor in the Factor Graph (LIO-SAM Approach)

[LIO-SAM](https://arxiv.org/abs/2007.00258) (Shan et al., 2020) shows a clean way to integrate GNSS into a factor graph. When the GNSS receiver reports a position, it is connected to a pose node as a **unary factor**:

where:
- $\mathbf{p}^{\text{ENU}}_{\text{GPS}}$ is the ENU coordinate reported by the GNSS
- $\mathbf{T}_{\text{ENU→map}}$ is the transformation from the ENU frame to the SLAM map frame
- $\mathbf{p}^{\text{map}}_i$ is the robot position estimated by SLAM
- $\mathbf{l}_{\text{antenna}}$ is the lever-arm vector between the GNSS antenna and the robot's body frame
- $\mathbf{R}^{\text{map}}_i$ is the robot's rotation

**Frame alignment issue**: The SLAM local map frame and the GNSS global frame (WGS84/ENU) are different. At the first GNSS reception, the ENU origin is set, and the map↔ENU transformation is estimated using the initial poses. This transformation has 6-DoF (3 translation + 3 rotation), but because the IMU provides the gravity direction, only 4-DoF (yaw + 3 translation) need to be estimated in practice.

### 8.3.2 Loosely vs Tightly Coupled GNSS

**Loosely Coupled GNSS-INS**:
The position/velocity solution (PVT solution) already computed by the GNSS receiver is combined with the IMU estimate by an EKF. Most commercial systems use this approach.

```python
def gnss_loose_coupling_ekf_update(x_ins, P_ins, gnss_position, R_gnss):
    """
    Loosely coupled GNSS-INS: correct the INS state using the GNSS PVT solution.

x_ins: INS state [position(3), velocity(3), attitude(3), biases(6)] = 15 dim
    gnss_position: position computed by GNSS (3,)
    R_gnss: covariance of the GNSS position (3, 3) — typically HDOP * sigma_uere
    """
    n = len(x_ins)
    # Observation matrix: GNSS observes position only
    H = np.zeros((3, n))
    H[0:3, 0:3] = np.eye(3)  # only the position portion is observed

# Innovation
    y = gnss_position - H @ x_ins

# Kalman gain
    S = H @ P_ins @ H.T + R_gnss
    K = P_ins @ H.T @ np.linalg.inv(S)

# Update
    x_updated = x_ins + K @ y
    P_updated = (np.eye(n) - K @ H) @ P_ins

return x_updated, P_updated
```

**Tightly Coupled GNSS-INS**:
Instead of the GNSS receiver's PVT solution, the raw pseudorange and Doppler measurements are used directly. The pseudorange to each satellite is inserted as an individual factor:

$$
\rho_i = \| \mathbf{p}_{\text{sat},i} - \mathbf{p}_{\text{rx}} \| + c \cdot \delta t_{\text{rx}} + I_i + T_i + \epsilon_i
$$

Here $\rho_i$ is the pseudorange to satellite $i$, $c \cdot \delta t_{\text{rx}}$ is the receiver clock bias, and $I_i$ and $T_i$ are the ionospheric/tropospheric delays.

The advantage of tight coupling is that even when fewer than four satellites are visible — so that GNSS cannot produce a solution on its own — the pseudoranges from the available satellites can still be exploited. Because buildings frequently occlude satellites in urban environments, this advantage is substantial in practice.

### 8.3.3 Handling GNSS-Denied → GNSS-Available Transitions

In real robot operation, GNSS signals are repeatedly lost and recovered (tunnels, underground parking lots, under overpasses). Handling these transitions reliably is a central challenge of system design.

**Transition considerations**:
1. **Avoid coordinate jumps**: Immediately after GNSS recovery, there can be a large discrepancy between the GNSS position and the IMU/LiDAR-estimated position. Correcting this abruptly introduces discontinuities in the map. The solution is to initially set the GNSS uncertainty to a large value and decrease it gradually.

2. **GNSS quality verification**: Measurements in the first few seconds after recovery can lose accuracy due to multipath and similar effects. Include them as factors only after verifying PDOP/HDOP, satellite count, carrier phase status, and the like to ensure sufficient reliability.

3. **Map frame correction**: If drift has accumulated during a prolonged GNSS outage, the map frame itself may need to be corrected upon recovery. This is handled by pose graph optimization similar to loop closure.

---

## 8.4 Radar Fusion

### 8.4.1 Radar Revisited

Traditionally, automotive radar was considered unsuitable for SLAM/odometry because of its low resolution. However, the emergence of **4D imaging radar** is rapidly changing the situation.

**What is 4D radar**: Whereas conventional automotive radars measured three quantities — range, Doppler velocity, and azimuth — 4D imaging radar adds **elevation** to produce a 3D point cloud. The resolution is nowhere near that of LiDAR (hundreds to thousands of points vs. hundreds of thousands), but it has unique strengths.

**Unique advantages of radar**:

1. **Adverse-weather penetration**: It penetrates rain, snow, fog, and dust. LiDAR (905 nm/1550 nm lasers) degrades sharply under such conditions, whereas radar (mm-wave) is nearly unaffected. From the standpoint of autonomous-driving safety, this is decisive.

2. **Direct velocity measurement**: FMCW (Frequency-Modulated Continuous Wave) radar uses the Doppler effect to **measure the relative velocity of objects directly**. Cameras and LiDAR must infer velocity indirectly by comparing consecutive frames, whereas radar obtains velocity from a single measurement.

3. **Low cost**: Automotive radar chipsets are produced at mass-market scale, making them at least an order of magnitude cheaper than LiDAR.

### 8.4.2 Radar Odometry

Odometry using 4D radar is a rapidly emerging field. The key idea is to exploit radar Doppler measurements directly for ego-motion estimation.

Each measurement point of an FMCW radar provides $(r, \theta, \phi, v_d)$ — range, azimuth, elevation, Doppler velocity. Given the robot's linear velocity $\mathbf{v}$ and angular velocity $\boldsymbol{\omega}$, the Doppler velocity observed at a point in direction $\mathbf{d}_i = [\cos\phi_i \cos\theta_i, \cos\phi_i \sin\theta_i, \sin\phi_i]^T$ is:

$$
v_{d,i} = -\mathbf{d}_i^T (\mathbf{v} + \boldsymbol{\omega} \times \mathbf{p}_i) + n_i
$$

where $\mathbf{p}_i = r_i \mathbf{d}_i$ is the 3D position of the point. Using only static points (after removing moving objects), $(\mathbf{v}, \boldsymbol{\omega})$ can be estimated from this set of equations.

```python
import numpy as np

def radar_ego_velocity(radar_points, doppler_velocities):
    """
    Estimate ego-velocity from radar Doppler measurements.

radar_points: (N, 3) — 3D coordinates of each point (r*d_i)
    doppler_velocities: (N,) — observed Doppler velocity of each point

Returns:
        v_ego: (3,) — ego linear velocity
    """
    # Direction vectors (unit vectors) for each point
    norms = np.linalg.norm(radar_points, axis=1, keepdims=True)
    directions = radar_points / (norms + 1e-8)  # (N, 3)

# v_d = -d^T @ v_ego  (simple case: ignoring angular velocity)
    # => A @ v_ego = b, where A = -directions, b = doppler_velocities
    A = -directions
    b = doppler_velocities

# After removing dynamic objects via RANSAC, apply least squares
    # Simplified version: least squares over the full data
    v_ego, residuals, _, _ = np.linalg.lstsq(A, b, rcond=None)

return v_ego
```

### 8.4.3 4D Radar + Camera Fusion

The combination of 4D radar and camera is drawing attention as a compelling alternative for "LiDAR-free" autonomous driving. The complementarity of the two sensors is as follows:

| Property | Camera | 4D Radar |
|------|--------|----------|
| Resolution | Very high | Low |
| Adverse weather | Weak | Robust |
| Direct depth measurement | ✗ | ✓ |
| Direct velocity measurement | ✗ | ✓ |
| Semantic understanding | Strong | Weak |
| Cost | Very low | Low |

Fusion approaches:
- **Early fusion**: Project radar points into the image and use them as sparse depth cues. Used as scale anchors for monocular depth estimation.
- **Mid-level fusion**: Combine camera features and radar features inside a network. Fusion in the BEV (Bird's Eye View) space is common.
- **Late fusion**: Detect objects independently with each sensor and then combine the results.

### 8.4.4 Boreas Benchmark

[Boreas](https://arxiv.org/abs/2203.10168) (Burnett et al., 2023) is a multi-sensor dataset collected under diverse weather conditions (clear, rain, snow), and it is particularly important for benchmarking radar odometry. It simultaneously mounts a camera, LiDAR (Velodyne Alpha Prime), and 4D radar (Navtech CIR304-H), and repeatedly drives the same route across different times/seasons, making it useful for long-term localization research as well.

---

## 8.5 Multi-Robot / Decentralized Fusion

Moving beyond single-robot fusion, the problem of having **multiple robots cooperatively perceive the environment** is a level more difficult. This is because communication constraints, the absence of a common relative reference frame, and the difficulty of data association are added.

### 8.5.1 Core Challenges of Multi-Robot SLAM

1. **Inter-Robot Relative Pose**: Each robot runs SLAM in its own local frame. To merge the maps of two robots, their relative coordinate transformation must first be known. This is solved by cross-robot place recognition plus geometric verification.

2. **Communication Constraint**: Transmitting the full map or raw sensor data is often impossible due to bandwidth limits. Thus **what information to compress and share** is a key design decision.

3. **Distributed Optimization**: Collecting all data at a central server for optimization has communication-bottleneck and single-point-of-failure problems. It is desirable for each robot to perform local optimization in a distributed fashion, exchanging only limited information with neighboring robots.

### 8.5.2 Kimera-Multi

[Kimera-Multi](https://arxiv.org/abs/2106.14386) (Rosinol et al., 2021) is a distributed multi-robot SLAM system developed by MIT's SPARK Lab.

**Architecture**:
- Each robot runs Kimera and performs local metric-semantic SLAM
- Upon rendezvous between robots, common places are detected by **DBoW2**-based inter-robot place recognition
- Detected inter-robot loop closures are incorporated into the distributed pose graph optimization
- A **GNC (Graduated Non-Convexity)** solver robustly rejects outlier loop closures

**Distributed optimization**: Each robot maintains its own pose graph and exchanges only inter-robot factors with neighboring robots. A distributed optimization algorithm such as Riemannian block-coordinate descent is used to reach convergence.

### 8.5.3 Swarm-SLAM

[Swarm-SLAM](https://arxiv.org/abs/2301.06230) (Lajoie et al., 2024) is a distributed SLAM for large-scale robot swarms that places particular emphasis on communication efficiency.

**Core design**:
- **Place recognition descriptor exchange**: Only place recognition descriptors (NetVLAD, Scan Context, etc.), rather than the full map, are exchanged to minimize bandwidth
- **Inter-robot loop closure**: Candidates are found by descriptor matching, and only a minimal amount of geometric information (feature points or a point cloud) is exchanged for verification
- **Peer-to-peer communication between neighboring robots**: Direct communication between adjacent robots without a central server
- **LiDAR/Visual/Multimodal support**: Robots with camera-only, LiDAR-only, or mixed sensor configurations can participate simultaneously

```python
# Conceptual implementation of distributed pose graph optimization
import numpy as np

class DistributedPoseGraphNode:
    """
    A single robot node in the distributed pose graph.
    Each robot maintains its own local graph and
    exchanges only inter-robot factors with neighbors.
    """
    def __init__(self, robot_id):
        self.robot_id = robot_id
        self.local_poses = []           # Own poses (local frame)
        self.local_factors = []          # Local odometry factors
        self.inter_robot_factors = []    # Loop closure factors with other robots
        self.neighbor_info = {}          # Boundary info received from neighbors

def add_odometry(self, delta_pose, covariance):
        """Add a local odometry factor"""
        self.local_factors.append({
            'type': 'odom',
            'from': len(self.local_poses) - 1,
            'to': len(self.local_poses),
            'measurement': delta_pose,
            'covariance': covariance
        })
        self.local_poses.append(self.local_poses[-1] @ delta_pose)

def add_inter_robot_factor(self, other_robot_id, other_pose_idx,
                                relative_pose, covariance):
        """Add a loop closure factor with another robot"""
        self.inter_robot_factors.append({
            'type': 'inter_robot',
            'robot': other_robot_id,
            'local_idx': len(self.local_poses) - 1,
            'remote_idx': other_pose_idx,
            'measurement': relative_pose,
            'covariance': covariance
        })

def exchange_boundary_info(self, neighbor_node):
        """
        Exchange boundary information (estimates and covariances of
        boundary variables) with a neighbor node. Only variables related
        to inter-robot factors are exchanged, not the full map.
        """
        boundary_poses = []
        for factor in self.inter_robot_factors:
            if factor['robot'] == neighbor_node.robot_id:
                idx = factor['local_idx']
                boundary_poses.append({
                    'idx': idx,
                    'pose': self.local_poses[idx]
                })
        neighbor_node.neighbor_info[self.robot_id] = boundary_poses
```

---

## 8.6 System Design in Practice

Beyond theory and algorithms, we address the practical problems encountered when designing and deploying a real multi-sensor fusion system.

### 8.6.1 Sensor Suite Selection Guide

The choice of sensor suite is dictated by the operational environment:

| Environment | Recommended minimum | Optional additional sensors |
|------|---------------|---------------|
| Indoor (office/warehouse) | Camera + IMU | LiDAR (for precise mapping) |
| Urban autonomous driving | Camera + LiDAR + IMU + GNSS | 4D Radar, Wheel Odom |
| Off-road/outdoor | LiDAR + IMU + GNSS | Camera (semantics), Radar |
| Underground/tunnel | LiDAR + IMU | Camera, UWB |
| Underwater | IMU + DVL (Doppler Velocity Log) | Sonar, Pressure |
| Aerial/drone | Camera + IMU + GNSS | LiDAR (for mapping) |
| Adverse weather (rain/snow) | Radar + IMU | Camera, LiDAR |

**Example configurations by budget**:
- **Under $500**: Stereo Camera + IMU (Intel RealSense D435i)
- **Under $2,000**: + 2D LiDAR (RPLidar)
- **Under $10,000**: + 3D LiDAR (Livox Mid-360) + GNSS RTK
- **$30,000+**: Multi-LiDAR + Multi-Camera + 4D Radar + GNSS RTK

### 8.6.2 Timing Architecture (Time Synchronization Design)

In a multi-sensor system, **time synchronization** is a decisive factor for accuracy. On a vehicle moving at 100 km/h, a 1 ms time error corresponds to roughly a 2.8 cm position error.

**Hardware Sync**:

The most precise method is to use hardware triggers:

- **PPS (Pulse Per Second)**: The GNSS receiver outputs a precise pulse once per second. This pulse is wired into the synchronization input of other sensors. Precision: ~50 ns.
- **PTP (Precision Time Protocol, IEEE 1588)**: Ethernet-based time synchronization. Supported by LiDARs (Velodyne, Ouster, etc.). Precision: ~μs.
- **External trigger**: A microcontroller simultaneously triggers the camera shutter and captures the IMU timestamp.

**Software Sync**:

When hardware synchronization is not possible, the time offset is estimated in software:

- **Kalibr approach**: Represent the continuous-time trajectory with a B-spline and include the inter-sensor time offset as an optimization variable, estimating everything jointly.
- **Correlation-based**: Compute the cross-correlation between the motion estimates of two sensors to estimate the time delay.

$$
\hat{\tau} = \arg\max_{\tau} \int \mathbf{a}_{\text{IMU}}(t) \cdot \dot{\mathbf{v}}_{\text{camera}}(t + \tau) \, dt
$$

```python
import numpy as np
from scipy.signal import correlate

def estimate_time_offset(timestamps_a, signal_a, timestamps_b, signal_b, max_offset_ms=100):
    """
    Estimate the time offset between two sensor signals via cross-correlation.

Example: IMU angular velocity vs. inter-frame rotation rate of the camera
    """
    # Resample to a common timeline (1 kHz)
    dt = 0.001  # 1 ms
    t_common = np.arange(
        max(timestamps_a[0], timestamps_b[0]),
        min(timestamps_a[-1], timestamps_b[-1]),
        dt
    )
    sig_a = np.interp(t_common, timestamps_a, signal_a)
    sig_b = np.interp(t_common, timestamps_b, signal_b)

# Remove the mean
    sig_a -= np.mean(sig_a)
    sig_b -= np.mean(sig_b)

# Cross-correlation
    correlation = correlate(sig_a, sig_b, mode='full')
    lags = np.arange(-len(sig_b) + 1, len(sig_a)) * dt

# Find the maximum correlation within the max_offset range
    mask = np.abs(lags) <= max_offset_ms / 1000
    valid_corr = correlation[mask]
    valid_lags = lags[mask]

best_idx = np.argmax(valid_corr)
    estimated_offset = valid_lags[best_idx]

return estimated_offset  # in seconds

# Example: IMU gyro Z-axis vs. Camera rotation rate
# offset = estimate_time_offset(imu_times, gyro_z, cam_times, cam_rotation_rate)
```

### 8.6.3 Failure Modes and Degradation Handling

In real systems, sensors inevitably fail. A robust system must achieve **graceful degradation** — that is, it must continue to operate with the remaining sensors, even at reduced performance, when one sensor fails.

**Key failure modes and responses**:

| Failure mode | Symptom | Detection | Response |
|-----------|------|-----------|------|
| Camera over-/under-exposure | Entire image is bright or dark | Histogram analysis | Disable camera factor, operate with LIO only |
| LiDAR geometric degeneracy | Long corridor, wide flat plane | Eigenvalue analysis of the information matrix | Relax LiDAR constraint on the affected DoF, compensate with VIO |
| IMU saturation | Measurement range exceeded under high-speed impact | Detect ADC maximum values | Increase IMU preintegration uncertainty for the affected interval |
| GNSS multipath | Large error due to reflections from buildings | RAIM, residual check | Increase the covariance of the affected GNSS factor or remove it |
| Total sensor dropout | No data received | Watchdog timer | Disable all factors for the affected sensor |

**Detecting LiDAR geometric degeneracy**:

In LiDAR scan matching, geometric degeneracy can be detected by eigenvalue analysis of the information matrix (Hessian) $\mathbf{H} = \mathbf{J}^T \mathbf{J}$. If the eigenvalue along one direction is significantly smaller than those along the others, the constraint in that direction is weak.

$$
\mathbf{H} = \mathbf{U} \boldsymbol{\Lambda} \mathbf{U}^T, \quad \lambda_{\min} / \lambda_{\max} < \epsilon \Rightarrow \text{degenerate}
$$

For instance, in a long corridor the constraint along the corridor axis becomes weak, so the LiDAR constraint in that direction is relaxed and complemented by the camera's optical flow.

```python
import numpy as np

def check_lidar_degeneracy(jacobian, threshold=0.01):
    """
    Check for geometric degeneracy using the eigenvalues of the Hessian
    from LiDAR scan matching.

jacobian: (m, 6) — 6-DoF Jacobian of m point-to-plane residuals
    threshold: threshold for the min-to-max eigenvalue ratio

Returns:
        is_degenerate: bool
        degenerate_directions: (k, 6) — eigenvectors of degenerate directions
        eigenvalues: (6,) — eigenvalues of the information matrix
    """
    # Information matrix (approximate Hessian)
    H = jacobian.T @ jacobian

# Eigen-decomposition
    eigenvalues, eigenvectors = np.linalg.eigh(H)

# Eigenvalue ratio check
    ratio = eigenvalues / (eigenvalues.max() + 1e-10)
    degenerate_mask = ratio < threshold

is_degenerate = np.any(degenerate_mask)
    degenerate_directions = eigenvectors[:, degenerate_mask].T

if is_degenerate:
        print(f"[Warning] Geometric degeneracy detected!")
        print(f"  Eigenvalues: {eigenvalues}")
        print(f"  Number of degenerate directions: {degenerate_mask.sum()}")

return is_degenerate, degenerate_directions, eigenvalues

def adaptive_fusion_weight(lidar_eigenvalues, camera_track_quality,
                            lidar_min_eig_threshold=100.0):
    """
    Adaptively adjust the camera weight according to the degree of LiDAR degeneracy.
    """
    min_eig = lidar_eigenvalues.min()

if min_eig > lidar_min_eig_threshold:
        # LiDAR is sufficiently constrained → default weights
        lidar_weight = 1.0
        camera_weight = 0.3
    else:
        # LiDAR is degenerate → increase the camera weight
        decay = min_eig / lidar_min_eig_threshold
        lidar_weight = decay
        camera_weight = 1.0

return lidar_weight, camera_weight
```

### 8.6.5 Notable Recent Research (2024-2025)

- **[Gaussian-LIC (Lang et al., ICRA 2025)](https://arxiv.org/abs/2404.06926)**: A system that integrates 3D Gaussian Splatting into tightly-coupled LiDAR-Inertial-Camera SLAM. By fusing the precise geometric information from the LiDAR with the camera's texture using a Gaussian representation, it achieves photo-realistic scene reconstruction concurrently with SLAM.
- **[Snail-Radar (Huai et al., IJRR 2025)](https://arxiv.org/abs/2407.11705)**: A large-scale diversity benchmark for evaluating 4D radar SLAM. It systematically compares 4D radar-based odometry/SLAM algorithms across diverse environments (indoor/outdoor, urban/suburban) and platforms.

### 8.6.4 System Design Checklist

Items that must always be checked when designing a real multi-sensor fusion system:

**Calibration**:
- [ ] Extrinsic calibration completed for every sensor pair
- [ ] Time-synchronization offsets measured/estimated
- [ ] Calibration results verified for reproducibility (at least three repetitions)
- [ ] Mechanism in place for online calibration drift correction

**Data flow**:
- [ ] Each sensor's data rate matches the system's processing rate
- [ ] Temporal alignment method between sensors is finalized
- [ ] Buffer sizes and latency are analyzed

**Robustness**:
- [ ] Failure modes of each sensor identified
- [ ] Degradation handling strategy established
- [ ] Outlier rejection mechanisms (robust kernel, chi-square test)
- [ ] Tested under extreme conditions (darkness, rain, vibration, geometric degeneracy)

**Performance**:
- [ ] Target accuracy (ATE/RPE) defined
- [ ] Real-time constraints satisfied (worst-case latency)
- [ ] Memory usage (accumulation over long-duration operation)
- [ ] CPU/GPU utilization

---

## Chapter 8 Summary

Multi-sensor fusion architectures are broadly classified as loosely/tightly/ultra-tightly coupled, and in modern robotics **tightly coupled** is the mainstream choice. Triple Camera+LiDAR+IMU fusion has reached a mature stage through systems such as R3LIVE, LVI-SAM, and FAST-LIVO2, each representing a distinct design philosophy — dual subsystem, factor graph, and sequential update, respectively.

GNSS integration provides a global coordinate anchor that resolves long-term drift, and 4D radar is rapidly rising thanks to its unique advantages of adverse-weather robustness and direct velocity measurement. Multi-robot fusion faces the core challenges of distributed optimization and cross-robot place recognition under communication constraints, with Kimera-Multi and Swarm-SLAM leading this area.

Finally, in practical system design, sensor selection, time synchronization, and failure-mode handling are as important as the algorithms themselves, and the engineering capability to address these systematically determines successful deployment.

The odometry/fusion systems covered in Ch.6-8 are highly accurate locally, but drift accumulates over long-duration operation. To correct this drift, the ability to "recognize places previously visited" is required. The next chapter addresses this key component, **Place Recognition**.

---

# Ch.9 — Place Recognition & Retrieval

The odometry/fusion systems covered in Ch.6-8 accumulate drift over time. To correct this drift, the robot must be able to recognize places it has visited before — this is the role of Place Recognition. The techniques covered in this chapter are used directly in the Loop Closure discussion of Ch.10.

> The problem of judging, "Have I seen this place before?"
> We systematically cover the techniques that form the core component of loop closure and the foundation for multi-session SLAM and relocalization.

---

## 9.1 Problem Definition

### 9.1.1 Place Recognition vs Loop Closure Detection

**Place Recognition (PR)** is the **retrieval problem** of judging, "Which place in the database does the current observation match?" It is a standalone problem that is defined even without SLAM.

**Loop Closure Detection** is, within a SLAM system, the detection of "Has the robot revisited a place it visited before?" Place recognition is the core component of loop closure detection, but loop closure is a broader pipeline that also includes **geometric verification** after PR.

```
Loop Closure Detection Pipeline:
┌─────────────┐    ┌────────────────┐    ┌───────────────────┐    ┌─────────────┐
│  Current    │───→│ Place          │───→│ Geometric         │───→│ Pose Graph  │
│  observation│    │ Recognition    │    │ Verification      │    │ Update      │
│  (query)    │    │ (candidate     │    │ (geometric        │    │ (optimize)  │
│             │    │  retrieval)    │    │  verification)    │    │             │
└─────────────┘    └────────────────┘    └───────────────────┘    └─────────────┘
```

**Why PR matters**: In SLAM, without loop closure, drift keeps accumulating. Yet brute-force comparison of the current frame against all past frames is $O(N^2)$ and infeasible. Place recognition is the key technique that reduces this comparison to **sub-linear** time (typically $O(\log N)$ or $O(1)$).

### 9.1.2 Retrieval Pipeline

The general place recognition pipeline follows the information retrieval paradigm:

1. **Encoding**: Encode each observation (image, point cloud, or both) into a fixed-length **global descriptor**.
2. **Indexing**: Store all database descriptors in a searchable index structure (e.g., kd-tree, FAISS).
3. **Retrieval**: Retrieve the database descriptors most similar to the query descriptor.
4. **Re-ranking & Verification**: Geometrically verify the candidates to confirm the final match.

$$
q^* = \arg\min_{q \in \mathcal{D}} d(\mathbf{f}(\text{query}), \mathbf{f}(q))
$$

Here $\mathbf{f}(\cdot)$ is the function that converts an observation into a global descriptor, $d(\cdot, \cdot)$ is a distance function (typically L2 or cosine distance), and $\mathcal{D}$ is the database.

### 9.1.3 Evaluation Metrics

PR system performance is evaluated with the following metrics:

**Recall@N**: The fraction of queries for which the correct match is among the top $N$ candidates. This is the most universal metric.

$$
\text{Recall@N} = \frac{|\{q : \text{top-}N \text{ candidates contain the correct match for query } q\}|}{|\text{total queries}|}
$$

**Recall@1** is especially important because in real-time SLAM we typically can only afford to verify the single most similar candidate.

**Precision-Recall Curve**: Varying a threshold on descriptor similarity traces out the relationship between precision and recall. High precision (minimizing false positives) is particularly important in SLAM — a false-positive loop closure can destructively distort the map.

**Definition of "correct match"**: Typically defined as being within 25 m by GPS distance, though this varies by dataset.

```python
import numpy as np
from scipy.spatial.distance import cdist

def compute_recall_at_n(query_descriptors, db_descriptors, 
                         query_positions, db_positions,
                         n_values=[1, 5, 10], threshold_m=25.0):
    """
    Compute Recall@N.
    
    query_descriptors: (Q, D) — query descriptors
    db_descriptors: (M, D) — database descriptors
    query_positions: (Q, 2 or 3) — GPS/GT positions of queries
    db_positions: (M, 2 or 3) — GPS/GT positions of database entries
    n_values: N values for Recall@N
    threshold_m: distance threshold (meters) to count as the same place
    """
    # Descriptor similarity matrix (L2 distance)
    desc_dists = cdist(query_descriptors, db_descriptors, metric='euclidean')
    
    # True distance matrix
    geo_dists = cdist(query_positions, db_positions, metric='euclidean')
    
    recalls = {}
    for n in n_values:
        correct = 0
        for q in range(len(query_descriptors)):
            # Top-N indices by descriptor distance
            top_n_indices = np.argsort(desc_dists[q])[:n]
            
            # Correct if any of the top-N is within the geographic threshold
            min_geo_dist = geo_dists[q, top_n_indices].min()
            if min_geo_dist < threshold_m:
                correct += 1
        
        recalls[f"Recall@{n}"] = correct / len(query_descriptors)
    
    return recalls
```

---

## 9.2 Visual Place Recognition (VPR)

Visual Place Recognition is the problem of recognizing a place from images alone. It is the oldest and most actively studied branch of PR.

### 9.2.1 Classical Method: Bag of Words (BoW)

The **Bag of Visual Words** model proposed in **[Video Google (Sivic & Zisserman, 2003)](https://www.robots.ox.ac.uk/~vgg/publications/2003/Sivic03/)** is the origin point of VPR. The key idea is to directly apply the methodology of text retrieval to visual retrieval.

**Pipeline**:

1. **Visual Vocabulary Construction**: Extract local features (SIFT, ORB, etc.) from a large corpus of images, and group these feature descriptors into $K$ clusters with k-means. Each cluster center becomes a "visual word."

2. **Image representation**: Assign features extracted from each image to the nearest visual word (hard assignment) and count the occurrence frequency of each visual word, representing the image as a $K$-dimensional histogram.

3. **TF-IDF weighting**: Apply TF-IDF (Term Frequency – Inverse Document Frequency) weighting borrowed from text retrieval:

$$
w_{i,d} = \underbrace{\frac{n_{i,d}}{n_d}}_{\text{TF}} \cdot \underbrace{\log \frac{N}{N_i}}_{\text{IDF}}
$$

Here $n_{i,d}$ is the count of visual word $i$ in image $d$, $n_d$ is the total number of visual words in image $d$, $N$ is the total number of images in the database, and $N_i$ is the number of images containing visual word $i$.

- **TF**: Higher weight for words that appear frequently in this image → distinctive features of this image
- **IDF**: Higher weight for rare words across the entire database → more discriminative features

4. **Inverted Index**: Pre-build a list of images containing each visual word so that, at query time, rather than scanning the entire database, only images containing the relevant word are retrieved quickly.

5. **Similarity ranking**: Rank by cosine similarity between the TF-IDF vectors of the query image and the database images:

$$
\text{sim}(\mathbf{v}_q, \mathbf{v}_d) = \frac{\mathbf{v}_q \cdot \mathbf{v}_d}{\|\mathbf{v}_q\| \cdot \|\mathbf{v}_d\|}
$$

**DBoW2**: The BoW implementation used in the ORB-SLAM series. It uses a hierarchical k-means tree to scale vocabulary size substantially while keeping quantization fast. It was effectively the de facto standard for loop closure detection in real-time SLAM.

```python
import numpy as np
from collections import defaultdict

class SimpleBoW:
    """
    Simplified implementation of Bag of Visual Words.
    """
    def __init__(self, vocabulary):
        """
        vocabulary: (K, D) — descriptors of K visual words (k-means centers)
        """
        self.vocabulary = vocabulary  # (K, D)
        self.K = len(vocabulary)
        self.inverted_index = defaultdict(list)  # word_id -> [(img_id, tf)]
        self.idf = np.ones(self.K)
        self.N = 0  # number of images in the database
        self.db_vectors = {}  # img_id -> tf-idf vector
    
    def quantize(self, descriptors):
        """Quantize local descriptors to the nearest visual word."""
        # (N_desc, D) vs (K, D) → (N_desc, K) distance matrix
        dists = np.linalg.norm(
            descriptors[:, None, :] - self.vocabulary[None, :, :], axis=2
        )
        word_ids = np.argmin(dists, axis=1)
        return word_ids
    
    def compute_bow_vector(self, descriptors):
        """Compute the image's BoW vector (with TF-IDF weighting)."""
        word_ids = self.quantize(descriptors)
        
        # Term Frequency
        tf = np.zeros(self.K)
        for w in word_ids:
            tf[w] += 1
        tf /= len(word_ids)  # normalize
        
        # TF-IDF
        tfidf = tf * self.idf
        
        # L2 normalization
        norm = np.linalg.norm(tfidf)
        if norm > 0:
            tfidf /= norm
        
        return tfidf
    
    def add_to_database(self, img_id, descriptors):
        """Add an image to the database."""
        bow_vector = self.compute_bow_vector(descriptors)
        self.db_vectors[img_id] = bow_vector
        self.N += 1
        
        # Update inverted index
        word_ids = self.quantize(descriptors)
        unique_words = np.unique(word_ids)
        for w in unique_words:
            self.inverted_index[w].append(img_id)
        
        # Recompute IDF
        for w in range(self.K):
            n_w = len(set(self.inverted_index[w]))  # number of images containing this word
            self.idf[w] = np.log(self.N / (n_w + 1e-6))
    
    def query(self, descriptors, top_k=5):
        """Retrieve the database images most similar to the query image."""
        q_vector = self.compute_bow_vector(descriptors)
        
        scores = {}
        for img_id, db_vector in self.db_vectors.items():
            scores[img_id] = np.dot(q_vector, db_vector)
        
        # Sort by similarity
        sorted_results = sorted(scores.items(), key=lambda x: x[1], reverse=True)
        return sorted_results[:top_k]
```

**Limitations of BoW**:
- Substantial information loss in quantization (all similar features collapse into the same word).
- Invariance to illumination and viewpoint changes is inherited from the underlying features (SIFT, ORB).
- The choice of vocabulary size $K$ has a large effect on performance.

### 9.2.2 VLAD and Fisher Vector

To overcome the limitations of BoW, aggregation methods that preserve **residual information** rather than simple frequency histograms emerged.

**[VLAD (Vector of Locally Aggregated Descriptors, Jégou et al., 2010)](https://doi.org/10.1109/CVPR.2010.5540039)**:

Whereas BoW records only "which visual words appeared how many times," VLAD records "in which direction and how large the residual is between each visual word and the actual descriptor."

For each cluster $k$, sum the residuals between the descriptors assigned to that cluster and the cluster center:

$$
\mathbf{V}_k = \sum_{i: \text{NN}(\mathbf{x}_i) = k} (\mathbf{x}_i - \mathbf{c}_k)
$$

The final VLAD descriptor is the concatenation of all $\mathbf{V}_k$: $\mathbf{V} = [\mathbf{V}_1^T, \mathbf{V}_2^T, \ldots, \mathbf{V}_K^T]^T$. Its dimensionality is $K \times D$.

**Fisher Vector**: A richer representation than VLAD. Visual words are modeled as a Gaussian Mixture Model (GMM), and first- and second-order statistics for each Gaussian component are normalized by the square root of the Fisher Information Matrix. The dimensionality is $2KD$, twice that of VLAD, but it typically yields higher performance.

### 9.2.3 NetVLAD: The Baseline for Learning-Based VPR

**[NetVLAD (Arandjelović et al., 2016)](https://arxiv.org/abs/1511.07247)** is a seminal work that reformulated VLAD as a **differentiable CNN layer**, enabling end-to-end training.

**Core problem**: In classical VLAD, hard-assigning each descriptor to its nearest cluster is non-differentiable. Backpropagation requires this process to be differentiable.

**Solution: Soft Assignment**:

Replace the hard assignment (1 if $\text{NN}(\mathbf{x}_i) = k$, else 0) with a softmax:

$$
\bar{a}_k(\mathbf{x}_i) = \frac{e^{\mathbf{w}_k^T \mathbf{x}_i + b_k}}{\sum_{k'} e^{\mathbf{w}_{k'}^T \mathbf{x}_i + b_{k'}}}
$$

Expanding the original VLAD's Euclidean-distance-based assignment $e^{-\alpha\|\mathbf{x}_i - \mathbf{c}_k\|^2}$, we can interpret $\mathbf{w}_k = 2\alpha\mathbf{c}_k$ and $b_k = -\alpha\|\mathbf{c}_k\|^2$. Making these parameters learnable lets the cluster centers adapt to the data.

**NetVLAD layer output**:

$$
\mathbf{V}(j, k) = \sum_{i=1}^{N} \bar{a}_k(\mathbf{x}_i) (x_i(j) - c_k(j))
$$

L2-normalizing this $D \times K$ matrix and flattening it into a vector yields the final global descriptor.

**Training strategy**: **Weakly supervised learning** using Google Street View Time Machine data. Images of the same GPS coordinate from different times are used as positive pairs, and images from distant GPS coordinates as negatives. Triplet ranking loss:

$$
\mathcal{L} = \sum_{(q, p^+, p^-)} \max\left(0, m + d(\mathbf{f}(q), \mathbf{f}(p^+)) - d(\mathbf{f}(q), \mathbf{f}(p^-))\right)
$$

Here $m$ is the margin, $p^+$ is a positive image, and $p^-$ is a hard-negative image.

**Performance**: Approximately 84.3% Recall@1 on Pitts250k and about 86.3% Recall@1 on Pitts30k — a substantial improvement over the hand-crafted methods of the time. VGG-16 backbone.

### 9.2.4 AnyLoc: Foundation-Model-Based Universal VPR

**[AnyLoc (Keetha et al., 2023)](https://arxiv.org/abs/2308.00688)** fundamentally shifted the paradigm of VPR. The central question is "Is universal place recognition possible without VPR-specific training?" and the answer is "Yes, using features from a Foundation Model like DINOv2."

**Approach**:

1. **DINOv2 feature extraction**: Extract **dense features** from a middle layer (the 31st layer) of DINOv2 ViT-G14. Use the features of all patches, not the CLS token (which summarizes the whole image into a single vector).

Why dense features? The CLS token captures the semantic content of the entire image, but it may miss the fine-grained structural differences that distinguish places. Dense features preserve the local information of each patch, enabling more precise place discrimination (on average a 23% improvement).

2. **VLAD aggregation**: Cluster the dense features with k-means to build a visual vocabulary, and produce global descriptors with hard-assignment VLAD. Unlike NetVLAD, this is unsupervised VLAD without any training.

3. **Domain-specific vocabularies**: PCA projections reveal six domains in an unsupervised manner — Urban, Indoor, Aerial, SubT (subterranean), Degraded (adverse conditions), and Underwater — and using domain-specific vocabularies yields up to a further 19% improvement.

**Why does a Foundation Model work?**:

[DINOv2 (Oquab et al., 2023)](https://arxiv.org/abs/2304.07193) was trained with self-supervised learning on 142 million images. In this process, the model learns universal **structural and semantic features** of scenes. Even though it was never trained for a specific place recognition task, these universal features are sufficient for discriminating places.

**Performance**:
- Day-night changes: 5-21% Recall@1 improvement over prior SOTA (MixVPR, CosPlace)
- Seasonal changes: 8-9% improvement
- Opposite viewpoint (180 degrees): 39-49% improvement
- Unstructured environments (underwater, subterranean): up to 4x prior performance
- PCA-Whitening compresses 49K dimensions to 512 (100x compression) while maintaining SOTA performance

```python
import numpy as np

def anyloc_pipeline(images, dino_model, n_clusters=64, desc_dim=512):
    """
    Conceptual implementation of the AnyLoc pipeline.
    
    1. Extract dense features from DINOv2
    2. Build a visual vocabulary with k-means
    3. VLAD aggregation
    4. PCA dimensionality reduction
    """
    # Step 1: Dense feature extraction (ViT patch tokens)
    all_patch_features = []
    image_patch_features = []
    
    for img in images:
        # DINOv2 forward: patch tokens of shape (1, N_patches, D_feat)
        patch_tokens = dino_model.get_intermediate_layers(img, n=1)[0]
        # Use the value facet of layer 31 (AnyLoc's key finding)
        image_patch_features.append(patch_tokens)
        all_patch_features.append(patch_tokens)
    
    # Step 2: Build k-means visual vocabulary
    all_features = np.vstack(all_patch_features)  # (N_total_patches, D_feat)
    from sklearn.cluster import KMeans
    kmeans = KMeans(n_clusters=n_clusters, random_state=42)
    kmeans.fit(all_features)
    centers = kmeans.cluster_centers_  # (K, D_feat)
    
    # Step 3: Compute VLAD descriptor for each image
    vlad_descriptors = []
    D_feat = centers.shape[1]
    
    for patches in image_patch_features:
        # Hard assignment
        assignments = kmeans.predict(patches)  # (N_patches,)
        
        # VLAD: sum residuals for each cluster
        vlad = np.zeros((n_clusters, D_feat))
        for i, patch_feat in enumerate(patches):
            k = assignments[i]
            vlad[k] += patch_feat - centers[k]
        
        # Intra-normalization (normalize each cluster vector individually)
        for k in range(n_clusters):
            norm = np.linalg.norm(vlad[k])
            if norm > 0:
                vlad[k] /= norm
        
        # Flatten + L2 normalization
        vlad_flat = vlad.flatten()
        vlad_flat /= (np.linalg.norm(vlad_flat) + 1e-10)
        
        vlad_descriptors.append(vlad_flat)
    
    vlad_descriptors = np.array(vlad_descriptors)
    
    # Step 4: PCA dimensionality reduction
    from sklearn.decomposition import PCA
    pca = PCA(n_components=desc_dim, whiten=True)
    reduced_descriptors = pca.fit_transform(vlad_descriptors)
    
    # Final L2 normalization
    norms = np.linalg.norm(reduced_descriptors, axis=1, keepdims=True)
    reduced_descriptors /= (norms + 1e-10)
    
    return reduced_descriptors  # (N_images, desc_dim)
```

### 9.2.5 EigenPlaces

**[EigenPlaces (Berton et al., 2023)](https://arxiv.org/abs/2308.10832)** is the follow-up to [CosPlace (Berton et al., 2022)](https://arxiv.org/abs/2204.02287), integrating PCA-based dimensionality reduction into the training process. Building on CosPlace — which replaced the cumbersome hard-negative mining of triplet loss with classification-based training — EigenPlaces goes further and optimizes the feature-space structure from a PCA perspective.

### 9.2.6 Expanding the Use of Foundation Models

Beyond DINOv2, various Foundation Models are being used for VPR:

- **CLIP**: With text-image correspondence training, it can be used for semantic-level place recognition such as "city street" or "forest trail." However, it lags behind DINOv2 in distinguishing fine-grained structural differences.
- **SAM (Segment Anything)**: Research is underway on using segmentation masks as a structural representation of places.
- **DINOv2 + NetVLAD**: Follow-up work shows that attaching a trained NetVLAD layer to DINOv2 features, instead of AnyLoc's unsupervised VLAD, yields further performance gains.

### 9.2.7 SeqSLAM and Sequence Matching

**[SeqSLAM (Milford & Wyeth, 2012)](https://doi.org/10.1109/ICRA.2012.6224623)** proposes matching **entire image sequences** instead of individual images.

**Key intuition**: Places that are hard to distinguish from a single image (e.g., visually similar residential areas) can have unique patterns across consecutive images (left turn → park → crosswalk).

**Method**:
1. Convert each image into an extremely simplified representation (low-resolution patch or a simple descriptor)
2. Construct a **sequence similarity matrix** between the query sequence and the database sequence
3. Search for diagonal paths within a constrained velocity range over the matrix to find the best sequence match

$$
S_{\text{seq}}(q_s, d_s) = \min_{\text{path}} \sum_{i=0}^{L-1} d(\mathbf{f}(q_{s+i}), \mathbf{f}(d_{s+\delta(i)}))
$$

Here $L$ is the sequence length and $\delta(i)$ is a path function that admits velocity variations.

**Advantages**: Robust under dramatic appearance changes (day → night, summer → winter) because the sequence pattern is preserved.

**Follow-up**: SeqNet (Garg et al., 2021) performs sequence matching with a learning-based approach.

---

## 9.3 LiDAR Place Recognition

This is the problem of recognizing places from 3D LiDAR point clouds. Unlike cameras, it is not affected by illumination changes, but it is vulnerable to seasonal vegetation changes and viewpoint changes.

### 9.3.1 Handcrafted Method: Scan Context

**[Scan Context (Kim & Kim, 2018)](https://doi.org/10.1109/IROS.2018.8593953)** converts a 3D LiDAR scan into a **global descriptor that directly preserves spatial structure** without a histogram. No training is needed, and it is widely adopted as a loop closure module in major systems such as LIO-SAM.

**Descriptor generation**:

1. **Polar partitioning**: Partition the 2D plane viewed from the sensor center into $N_s$ azimuth sectors and $N_r$ range rings, forming an $N_s \times N_r$ grid (typically 60x20).

2. **Max-height encoding**: For each bin, record the **maximum height (max height)** among the 3D points falling into it. This yields the Scan Context matrix $\mathbf{SC} \in \mathbb{R}^{N_s \times N_r}$.

$$
\mathbf{SC}(i, j) = \max_{p \in \text{bin}(i,j)} z(p)
$$

**Why maximum height?** Max height captures protruding structures like buildings, trees, and poles better than mean height does.

3. **Advantage of an egocentric representation**: Because it is expressed in the sensor-centered frame, revisiting the same place from the **opposite direction** merely circularly shifts the rows (sector axis) of the descriptor. This is handled via row-shift matching.

**Search strategy — Ring Key & Sector Key**:

Directly comparing Scan Context matrices against the entire database is inefficient. A two-stage search is used for efficiency:

1. **Ring Key**: For each ring (column) of the SC matrix, take the mean over the sector direction to produce a vector — $\mathbf{k}_r = [\bar{h}_1, \bar{h}_2, \ldots, \bar{h}_{N_r}]$. This vector is used for fast kd-tree search to narrow down candidates.

2. **Sector Key**: For each candidate, try row shifts (sector axis) of the SC matrix to find the best match:

$$
d(\mathbf{SC}_q, \mathbf{SC}_d) = \min_{s \in [0, N_s)} \left\| \mathbf{SC}_q - \text{shift}(\mathbf{SC}_d, s) \right\|_F
$$

```python
import numpy as np

class ScanContext:
    """
    Scan Context descriptor generation and matching.
    """
    def __init__(self, n_sectors=60, n_rings=20, max_range=80.0):
        self.n_sectors = n_sectors
        self.n_rings = n_rings
        self.max_range = max_range
        self.database = []
        self.ring_keys = []
    
    def make_scan_context(self, point_cloud):
        """
        3D point cloud → Scan Context matrix.
        
        point_cloud: (N, 3) — x, y, z coordinates
        """
        sc = np.zeros((self.n_sectors, self.n_rings))
        
        # Polar conversion
        x, y, z = point_cloud[:, 0], point_cloud[:, 1], point_cloud[:, 2]
        ranges = np.sqrt(x**2 + y**2)
        angles = np.arctan2(y, x)  # [-pi, pi]
        angles = (angles + np.pi) / (2 * np.pi)  # normalize to [0, 1]
        
        # Remove out-of-range points
        valid = ranges < self.max_range
        ranges, angles, z = ranges[valid], angles[valid], z[valid]
        
        # Compute bin indices
        sector_idx = np.clip((angles * self.n_sectors).astype(int), 0, self.n_sectors - 1)
        ring_idx = np.clip((ranges / self.max_range * self.n_rings).astype(int), 0, self.n_rings - 1)
        
        # Max height per bin
        for i in range(len(z)):
            si, ri = sector_idx[i], ring_idx[i]
            sc[si, ri] = max(sc[si, ri], z[i])
        
        return sc
    
    def make_ring_key(self, sc):
        """Ring key: mean height of each ring."""
        return np.mean(sc, axis=0)  # (n_rings,)
    
    def add_to_database(self, point_cloud):
        """Add a scan to the database."""
        sc = self.make_scan_context(point_cloud)
        rk = self.make_ring_key(sc)
        self.database.append(sc)
        self.ring_keys.append(rk)
    
    def query(self, point_cloud, top_k=5, n_candidates=20):
        """
        Retrieve the database scans most similar to the query scan.
        """
        sc_query = self.make_scan_context(point_cloud)
        rk_query = self.make_ring_key(sc_query)
        
        # Stage 1: Candidate selection via ring key
        rk_dists = [np.linalg.norm(rk_query - rk) for rk in self.ring_keys]
        candidate_indices = np.argsort(rk_dists)[:n_candidates]
        
        # Stage 2: Scan Context column-shift matching
        scores = []
        for idx in candidate_indices:
            sc_db = self.database[idx]
            min_dist = float('inf')
            for shift in range(self.n_sectors):
                sc_shifted = np.roll(sc_db, shift, axis=0)
                dist = np.linalg.norm(sc_query - sc_shifted)
                min_dist = min(min_dist, dist)
            scores.append((idx, min_dist))
        
        scores.sort(key=lambda x: x[1])
        return scores[:top_k]
```

**Follow-up — Scan Context++**: Adds semantic segmentation, encoding semantic labels (buildings, roads, vegetation, etc.) instead of height. Semantic information is more robust to seasonal changes.

### 9.3.2 M2DP and ESF

Handcrafted LiDAR descriptors beyond Scan Context:

- **M2DP (He et al., 2016)**: Projects the point cloud onto several 2D planes and compresses the density distribution of each projection with SVD to produce a 192-dimensional descriptor. Rotation invariant.
- **ESF (Ensemble of Shape Functions)**: A descriptor combining histograms of pairwise distances, angles, and area ratios among point pairs.

These are weaker than Scan Context in preserving spatial structure, but have advantages in rotation invariance or compactness.

### 9.3.3 Learning-based: PointNetVLAD

**[PointNetVLAD (Uy & Lee, 2018)](https://arxiv.org/abs/1804.03492)** is the first work to apply the NetVLAD idea directly to 3D point clouds.

**Architecture**:
1. Extract local features from the point cloud with a **PointNet** backbone
2. Aggregate local features into a global descriptor with a **NetVLAD layer**
3. Train with lazy triplet loss

$$
\mathcal{L} = \max(0, m + \max_{p^+} d(q, p^+) - \min_{p^-} d(q, p^-))
$$

**Limitations**: PointNet does not sufficiently model inter-point interactions and is inefficient for processing large-scale point clouds.

### 9.3.4 MinkLoc3D

**[MinkLoc3D (Komorowski, 2021)](https://github.com/jac99/MinkLoc3D)** addresses the limitations of PointNetVLAD by using a **Minkowski Convolutional Neural Network** as its backbone. Sparse 3D convolutions effectively capture local point-cloud structure, and GeM (Generalized Mean) pooling produces the global descriptor.

### 9.3.5 OverlapTransformer: Range-Image-Based

**[OverlapTransformer (Ma et al., 2022)](https://arxiv.org/abs/2203.03397)** is an approach that converts LiDAR point clouds into **range images** to leverage 2D image processing pipelines.

**Range Image**: A rotating LiDAR scan is converted into a 2D image of size $(h, w)$. Each pixel value is the range in that direction. $h$ corresponds to the number of laser beams and $w$ to the horizontal resolution.

**Architecture**:
1. Process the range image with a lightweight CNN to extract a feature map
2. Produce a global descriptor with a **NetVLAD** layer
3. Incorporate overall context with a Transformer encoder

**Advantages**: 2D CNN processing is much faster than 3D point cloud processing, and existing image-network architectures can be directly leveraged.

### 9.3.6 BEV-Based Methods

Methods that project point clouds into a Bird's Eye View (BEV) 2D map and perform 2D image-based retrieval:

- **OverlapNet**: Directly predicts the degree of overlap between BEV projection images
- **BEVPlace**: Extracts NetVLAD descriptors from BEV images

---

## 9.4 Cross-Modal Place Recognition

### 9.4.1 Why Cross-Modal PR Is Needed

In real-world robot systems, **the sensor used during mapping may differ from the sensor used during localization**. Examples:
- Localizing with a camera only on a map built with LiDAR
- Place recognition between an autonomous vehicle (LiDAR+Camera) and a delivery robot (Camera only)
- A drone (Camera) localizing itself on a map built by a vehicle (LiDAR)

In these scenarios, **place recognition between LiDAR observations and camera observations** — that is, cross-modal PR — is needed.

### 9.4.2 The Fundamental Difficulty of Cross-Modal PR: Domain Gap

LiDAR point clouds and camera images are fundamentally different representations:

| Property | LiDAR point cloud | Camera image |
|------|---------------------|-------------|
| Data structure | Unstructured 3D point set | Structured 2D grid |
| Information | Geometry | Appearance |
| Illumination dependence | None | Very high |
| Texture | None | Rich |
| Density | Inversely proportional to range | Uniform |

This fundamental difference is called the **domain gap**, and it is the reason that descriptors of the same place observed in different modalities are far apart in descriptor space.

### 9.4.3 (LC)²: LiDAR-Camera Cross-Modal PR

**[(LC)² (Lee et al., 2023)](https://arxiv.org/abs/2304.08660)** proposes a method to map LiDAR point clouds and camera images into a **shared embedding space**.

**Approach**:
1. Convert LiDAR point clouds into range images / BEV images, unifying them as a 2D representation
2. Process camera images and LiDAR projection images with CNNs
3. Train with **contrastive learning** so that LiDAR-Camera pairs from the same place are close in the embedding space and pairs from different places are far apart

Here $\mathbf{f}_L$ is the LiDAR encoder, $\mathbf{f}_C$ is the camera encoder, and $\mathcal{P}^+$/$\mathcal{P}^-$ are positive/negative pairs.

### 9.4.4 ModaLink

**ModaLink** aims at a more general cross-modal framework than (LC)², handling various modality combinations such as LiDAR, Camera, and Radar.

### 9.4.5 Modality-Agnostic Descriptor Approach

The ultimate goal is a **modality-agnostic descriptor** — regardless of which sensor is used, the same place produces the same descriptor. Approaches toward this:

- **Knowledge Distillation**: Use descriptors from an information-rich modality (LiDAR+Camera) as teacher and a single modality as student
- **Canonical Representation**: Convert to a modality-neutral representation such as BEV or semantic layout before comparison
- **Foundation-Model-based**: Research exploring whether the features a Foundation Model like DINOv2 extracts from an image lie in a similar space to those extracted from a rendered LiDAR image

---

## 9.5 Multi-Session & Long-Term Place Recognition

### 9.5.1 Challenges of Long-Term VPR

The same place can undergo **dramatic appearance changes over time**:

- **Illumination changes**: Day vs. night, clear vs. overcast
- **Seasonal changes**: Green trees vs. bare branches vs. snow-covered scenery
- **Weather changes**: Clear vs. rain vs. fog
- **Structural changes**: New construction/demolition, parked vehicles, road work

Such changes cause descriptors of the same place to drift over time, degrading PR performance.

### 9.5.2 Strategies for Seasonal/Time-of-Day/Weather Changes

1. **Data Augmentation based**: Include images under diverse conditions during training. NetVLAD's use of Google Street View Time Machine is a canonical example.

2. **Learning Domain-Invariant Features**: Learn features that are invariant to appearance changes. For example, semantic segmentation results (the arrangement of buildings, roads, sky) are invariant to illumination.

3. **Leveraging Foundation Models**: As shown by AnyLoc, DINOv2 features exhibit strong robustness to illumination/seasonal changes. This is because the self-supervised training process learns features invariant to diverse augmentations.

4. **Sequence matching**: The SeqSLAM approach. Even when the appearance of individual images changes, the pattern across an image sequence tends to be preserved.

### 9.5.3 Map Update Strategies

For long-term systems, the map itself must be updated:

- **Experience-Based Map**: Store observations of the same place from multiple time periods and match against the experience most similar to the current conditions. The drawback is that map size grows with time.

- **Adaptive descriptors**: Incrementally update the descriptor of an existing place with new observations, e.g., using an exponential moving average.

- **Change Detection**: Detect structural changes (e.g., building demolition) to invalidate and rebuild the affected map regions.

### 9.5.4 Lifelong Place Recognition

The ultimate goal is **place recognition for a robot operated over its lifetime**. This requires:

- Compressing/managing information so the map does not grow unboundedly
- Graceful forgetting of old observations
- Continuous adaptation to environmental change
- Learning new environments without catastrophic forgetting

This area is still an open research problem and is closely related to continual / incremental learning.

---

## 9.6 Geometric Verification & Re-ranking

Once place recognition has identified candidates, it is necessary to **verify geometrically whether they are truly the same place**. Without this step, false-positive loop closures can occur and destroy the map.

### 9.6.1 PnP + RANSAC (Visual)

For camera-based PR candidates:

1. **Local feature matching** between the query and candidate images (SuperPoint+LightGlue, ORB+BF, etc.)
2. **PnP (Perspective-n-Point)**: Estimate the relative camera pose from 2D-3D correspondences
3. **RANSAC**: Estimate the pose while rejecting outliers

```python
import numpy as np

def geometric_verification_visual(query_keypoints, db_keypoints_3d, 
                                    K, ransac_threshold=5.0,
                                    min_inliers=30):
    """
    Visual geometric verification: PnP + RANSAC.
    
    query_keypoints: (N, 2) — 2D keypoints in the query image
    db_keypoints_3d: (N, 3) — matched 3D points from the database
    K: (3, 3) — camera intrinsic parameters
    """
    import cv2
    
    # Estimate camera pose via PnP + RANSAC
    success, rvec, tvec, inliers = cv2.solvePnPRansac(
        db_keypoints_3d.astype(np.float32),
        query_keypoints.astype(np.float32),
        K.astype(np.float32),
        distCoeffs=None,
        reprojectionError=ransac_threshold,
        confidence=0.99,
        iterationsCount=1000
    )
    
    if not success or inliers is None:
        return False, None, 0
    
    n_inliers = len(inliers)
    is_verified = n_inliers >= min_inliers
    
    # Relative pose
    R, _ = cv2.Rodrigues(rvec)
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = tvec.flatten()
    
    return is_verified, T, n_inliers
```

### 9.6.2 3D-3D Registration (LiDAR)

For LiDAR-based PR candidates:

1. Find **3D-3D correspondences** between the two point clouds (FPFH, FCGF, GeoTransformer, etc.)
2. Estimate the rigid transformation via **RANSAC** or a **RANSAC-free** approach like GeoTransformer
3. **Refine with ICP** (coarse-to-fine)

GeoTransformer (Qin et al., 2022) enables robust registration without RANSAC. It is a geometric transformer that encodes pairwise distances and triplet angles and learns features invariant to rigid transformation, remaining robust in low-overlap scenarios. It estimates the transformation directly from superpoint-level correspondences and is 100x faster than RANSAC.

```python
def geometric_verification_lidar(query_cloud, db_cloud, 
                                   voxel_size=0.5, 
                                   distance_threshold=0.5,
                                   fitness_threshold=0.3):
    """
    LiDAR geometric verification: FPFH + RANSAC + ICP.
    """
    import open3d as o3d
    
    # Downsample
    q_down = query_cloud.voxel_down_sample(voxel_size)
    d_down = db_cloud.voxel_down_sample(voxel_size)
    
    # Normal estimation
    q_down.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size*2, max_nn=30))
    d_down.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size*2, max_nn=30))
    
    # FPFH feature extraction
    q_fpfh = o3d.pipelines.registration.compute_fpfh_feature(
        q_down, o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size*5, max_nn=100))
    d_fpfh = o3d.pipelines.registration.compute_fpfh_feature(
        d_down, o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size*5, max_nn=100))
    
    # RANSAC-based registration
    result_ransac = o3d.pipelines.registration.registration_ransac_based_on_feature_matching(
        q_down, d_down, q_fpfh, d_fpfh,
        mutual_filter=True,
        max_correspondence_distance=distance_threshold * 2,
        estimation_method=o3d.pipelines.registration.TransformationEstimationPointToPoint(),
        ransac_n=3,
        criteria=o3d.pipelines.registration.RANSACConvergenceCriteria(100000, 0.999)
    )
    
    # ICP refinement
    result_icp = o3d.pipelines.registration.registration_icp(
        q_down, d_down,
        distance_threshold,
        result_ransac.transformation,
        o3d.pipelines.registration.TransformationEstimationPointToPlane()
    )
    
    is_verified = result_icp.fitness > fitness_threshold
    
    return is_verified, result_icp.transformation, result_icp.fitness
```

### 9.6.3 Spatial Re-ranking

Strategies for **re-ranking** the candidate list — originally ordered by descriptor similarity — using geometric information:

1. **Spatial proximity prior**: In SLAM, add a bonus to candidates near the current pose estimate. This is valid only when drift is small.

2. **Inlier-count-based re-ranking**: After feature matching, promote candidates with more inliers.

3. **Pose consistency check**: Verify that the estimated relative pose is consistent with the accumulated odometry pose. Reject as false positive if there is a large discrepancy.

---

## 9.7 Recent Developments

### 9.7.1 The Rise of Foundation-Model-Based PR

**Paradigm shift**: VPR is transitioning from "environment-specific dedicated training" to "universal zero-shot recognition." As AnyLoc showed, the universal features of a Foundation Model achieve SOTA in most environments without any VPR-specific training.

**Future directions**:
- The VPR performance of next-generation FMs beyond DINOv2 (Vision Foundation Model v2, RADIO, etc.)
- **Lightweighting** FM features: Current ViT-G14 has 1B+ parameters, limiting embedded deployment. Combinations of lightweight FMs (ViT-S/B) + domain adaptation are being actively studied
- **Leveraging fine-grained spatial information** from FM features: Directly using patch-level correspondences (dense correspondence) provided by FMs for re-ranking or relative pose estimation

### 9.7.2 Semantic Place Recognition

An approach that recognizes a place by its **semantic structure**:

"The big tree next to the red building" → represents the place by the spatial layout of the building (red) and the tree (big)

**Approaches**:
- Semantic segmentation → semantic layout comparison
- Object detection → object graph (scene graph) comparison
- Open-vocabulary: natural-language-description-based place retrieval with CLIP

**Advantages**: Semantic structure is robust to illumination/seasonal changes. Even if the color of a tree's leaves changes, the semantic label "tree" is preserved.

**Limitations**: Depends on the accuracy of semantic segmentation, and it is difficult to distinguish places with similar semantic structure (e.g., similarly structured residential areas).

### 9.7.3 4D Radar Place Recognition

With the advent of 4D imaging radar, radar-based PR is beginning to be studied:

- Encoding radar point clouds with Scan Context variants
- Producing global descriptors by processing Range-Doppler images with CNNs
- Radar-Camera cross-modal PR

**Advantages**: PR systems that operate in adverse weather. When LiDAR PR and visual PR fail in rain, snow, or fog, radar PR can serve as a backup.

**Current status**: Still in early stages, and discriminability falls short of LiDAR and Visual PR due to radar's low resolution. However, 4D radar resolution is improving rapidly, making this a promising area for the future.

### 9.7.4 Recent Research (2024-2025)

**[SALAD (Izquierdo & Civera, 2024)](https://arxiv.org/abs/2311.15937)**: Redefines NetVLAD's feature-to-cluster assignment as an **optimal transport** problem and fine-tunes DINOv2 as the backbone. The Sinkhorn algorithm is used to optimize soft assignment, achieving SOTA on numerous benchmarks over NetVLAD/CosPlace.

**[EffoVPR (Taha et al., 2024)](https://arxiv.org/abs/2405.18065)**: A framework for efficiently leveraging the features of Foundation Models like DINOv2. It maintains SOTA performance even with descriptors compressed to 128 dimensions, presenting a lightweight VPR suitable for embedded deployment.

### 9.7.5 Technical Lineage Summary

```
Visual Place Recognition lineage:

Sivic (2003) Video Google [BoW]
    ↓ quantization → residual
Jégou (2010) VLAD
    ↓ hand-crafted → CNN end-to-end
Arandjelović (2016) NetVLAD [triplet, soft-assignment]
    ↓ triplet → classification
Berton (2022) CosPlace, (2023) EigenPlaces
    ↓ VPR-specific → Foundation Model
Keetha (2023) AnyLoc [DINOv2 + VLAD, zero-shot]
    ↓ assignment optimization + FM fine-tuning
Izquierdo (2024) SALAD [optimal transport + DINOv2]

LiDAR Place Recognition lineage:

Cross-Modal:
Lee (2023) (LC)² [LiDAR ↔ Camera shared embedding]
```

---

## Chapter 9 Summary

Place Recognition is the core component of loop closure that corrects drift in SLAM systems. Visual PR has evolved from BoW (Video Google) → VLAD → NetVLAD → AnyLoc, and universal zero-shot recognition based on Foundation Models (DINOv2) has recently become the paradigm. LiDAR PR has progressed from Scan Context (handcrafted) → PointNetVLAD → MinkLoc3D → OverlapTransformer, with range-image-based methods drawing attention for their efficiency.

Cross-modal PR faces the fundamental difficulty of the domain gap, and shared-embedding-space learning and modality-agnostic descriptors are active research directions. Long-term PR must cope with seasonal/illumination/structural changes, and the robust features of Foundation Models offer a promising solution to this problem.

Geometric verification is the final verification stage for PR candidates, preventing false positives and protecting the integrity of SLAM. PnP+RANSAC (visual) and ICP/GeoTransformer (LiDAR) are the standard methods.

Recent trends include the lightweighting of Foundation-Model-based PR, semantic PR, and 4D radar PR, all of which are active areas of research.

Place Recognition answers the question "Have I seen this place before?", but the process of converting that answer into global consistency for a SLAM system remains. The next chapter covers **Loop Closure and global optimization**, which integrate PR results into the pose graph to correct drift.

---

# Ch.10 — Loop Closure & Global Optimization

In Ch.9 we covered place recognition — recognizing previously visited locations. This chapter addresses how those recognition results are integrated into a SLAM system to actually correct the accumulated drift.

In a SLAM system, odometry inevitably accumulates drift. No matter how precise the sensors are, small errors in relative pose estimation build up over time and break global consistency. Loop closure is the mechanism that recognizes "we have revisited a previously visited place" and uses that information to correct the accumulated drift all at once.

In this chapter we walk through the full loop closure pipeline (detection → verification → correction), the mathematical foundations of pose graph optimization at the heart of the correction step, and we extend the discussion to global relocalization and multi-session SLAM.

---

## 10.1 Loop Closure Pipeline

Loop closure consists of three stages: **Detection** (candidate search), **Verification** (geometric verification), and **Correction** (graph correction). Each stage plays a distinct role, and failure in any one of them can break the consistency of the entire system.

### 10.1.1 Detection: Candidate Search

Loop closure detection asks: "Which past observation is similar to the current sensor observation?" This is essentially the same problem as place recognition covered in Ch.9.

In **visual loop closure detection**, the global descriptor of the current image is compared against a descriptor database of past keyframes:

1. **BoW-based (traditional)**: DBoW2 is used to compare the visual word histograms of ORB keypoints. ORB-SLAM3 uses this approach. Each keyframe is represented as a bag-of-words vector $\mathbf{v}_i$, and its similarity to the current frame $\mathbf{v}_q$ is computed as $s(\mathbf{v}_q, \mathbf{v}_i) = 1 - \frac{1}{2} \left| \frac{\mathbf{v}_q}{\|\mathbf{v}_q\|} - \frac{\mathbf{v}_i}{\|\mathbf{v}_i\|} \right|$ (L1-score).

2. **Learning-based (modern)**: Global descriptors such as [NetVLAD](https://arxiv.org/abs/1511.07247) and [AnyLoc](https://arxiv.org/abs/2308.00688) are used. AnyLoc aggregates the dense features of [DINOv2](https://arxiv.org/abs/2304.07193) with VLAD, providing general-purpose operation without environment-specific training. Candidates are ranked by cosine similarity:

$$s(\mathbf{d}_q, \mathbf{d}_i) = \frac{\mathbf{d}_q^\top \mathbf{d}_i}{\|\mathbf{d}_q\| \|\mathbf{d}_i\|}$$

In **LiDAR loop closure detection**, 3D point cloud descriptors are used:

- **[Scan Context](https://doi.org/10.1109/IROS.2018.8593953)**: A descriptor that directly preserves spatial structure by recording the maximum height per bin/sector in a sensor-centered polar coordinate system. Efficient candidate search is possible through a two-stage search using a ring key and a sector key, and it is robust even to reverse revisits.
- **[PointNetVLAD](https://arxiv.org/abs/1804.03492), [OverlapTransformer](https://arxiv.org/abs/2203.03397)**: Learning-based 3D place recognition methods that achieve higher recall than Scan Context in large-scale environments.

**Temporal filtering**: Matching against recent frames is not loop closure but simply continuous tracking. Therefore, only keyframes that are sufficiently separated in time (e.g., at least 30 seconds apart) are considered as candidates.

```python
import numpy as np

def detect_loop_candidates(query_descriptor, database_descriptors, 
                           timestamps, current_time, 
                           min_time_gap=30.0, top_k=5, threshold=0.7):
    """
    Loop closure candidate detection.
    
    Args:
        query_descriptor: global descriptor of the current frame (D,)
        database_descriptors: descriptors of past keyframes (N, D)
        timestamps: timestamp of each keyframe (N,)
        current_time: current time
        min_time_gap: minimum time gap (seconds)
        top_k: number of candidates to return
        threshold: similarity threshold
        
    Returns:
        candidates: list of [(index, similarity)]
    """
    # Temporal filtering: exclude recent frames
    time_mask = (current_time - timestamps) > min_time_gap
    
    if not np.any(time_mask):
        return []
    
    # Compute cosine similarity
    query_norm = query_descriptor / (np.linalg.norm(query_descriptor) + 1e-8)
    db_norms = database_descriptors / (
        np.linalg.norm(database_descriptors, axis=1, keepdims=True) + 1e-8
    )
    
    similarities = db_norms @ query_norm  # (N,)
    
    # Apply time filter
    similarities[~time_mask] = -1.0
    
    # Select top-k candidates
    top_indices = np.argsort(similarities)[::-1][:top_k]
    candidates = [
        (idx, similarities[idx]) 
        for idx in top_indices 
        if similarities[idx] > threshold
    ]
    
    return candidates
```

### 10.1.2 Verification: Geometric Verification

Candidates found in the detection stage are selected based on appearance similarity alone, so **false positives** (actually different places that merely look similar) can be included. Perceptual aliasing — visually similar but actually distinct places — is especially frequent in indoor environments (similar corridors, repetitive structures).

A false positive loop closure is **catastrophic**. A single incorrect loop closure can warp the entire map. Verification must therefore be performed conservatively — maximize precision even at some cost to recall.

**Geometric verification methods**:

1. **2D-2D: Essential matrix verification**: Feature point matching is performed between the current frame and the candidate keyframe, and the essential matrix $\mathbf{E}$ is estimated with RANSAC. If the number of inliers is sufficient (e.g., ≥ 20) and the inlier ratio is high (e.g., ≥ 50%), the loop closure is deemed valid.

$$\mathbf{p}_2^\top \mathbf{E} \mathbf{p}_1 = 0, \quad \mathbf{E} = [\mathbf{t}]_\times \mathbf{R}$$

2. **3D-3D: Point cloud registration**: In LiDAR-based systems, the relative transform $\mathbf{T}_{ij} \in SE(3)$ between two scans is estimated with ICP or GeoTransformer. It is verified by the fitness score (ratio of registered points) and RMSE.

3. **2D-3D: PnP**: The relative pose is estimated by solving a PnP problem between the current 2D feature points and the 3D map points of the candidate keyframe.

4. **Temporal consistency**: Rather than a single match, check whether matches against the same place appear consistently across several consecutive frames. ORB-SLAM3 accepts a loop closure only when the same place is detected three times in a row.

```python
import numpy as np

def verify_loop_closure(kp_current, kp_candidate, matches, K,
                        min_inliers=20, min_inlier_ratio=0.5):
    """
    Essential matrix-based geometric verification of loop closure.
    
    Args:
        kp_current: keypoint coordinates of the current frame (N, 2)
        kp_candidate: keypoint coordinates of the candidate frame (M, 2)
        matches: list of match index pairs [(i, j), ...]
        K: camera intrinsic matrix (3, 3)
        min_inliers: minimum number of inliers
        min_inlier_ratio: minimum inlier ratio
        
    Returns:
        is_valid: whether it is a valid loop closure
        T_relative: relative transform (4, 4) or None
    """
    if len(matches) < min_inliers:
        return False, None
    
    pts1 = np.array([kp_current[m[0]] for m in matches], dtype=np.float64)
    pts2 = np.array([kp_candidate[m[1]] for m in matches], dtype=np.float64)
    
    # Convert to normalized coordinates
    K_inv = np.linalg.inv(K)
    pts1_norm = (K_inv @ np.hstack([pts1, np.ones((len(pts1), 1))]).T).T[:, :2]
    pts2_norm = (K_inv @ np.hstack([pts2, np.ones((len(pts2), 1))]).T).T[:, :2]
    
    # Estimate essential matrix with RANSAC
    E, inlier_mask = estimate_essential_ransac(pts1_norm, pts2_norm, 
                                                threshold=1e-3, max_iter=1000)
    
    num_inliers = np.sum(inlier_mask)
    inlier_ratio = num_inliers / len(matches)
    
    if num_inliers < min_inliers or inlier_ratio < min_inlier_ratio:
        return False, None
    
    # Recover R, t from E
    R, t = decompose_essential(E, pts1_norm[inlier_mask], pts2_norm[inlier_mask])
    
    T_relative = np.eye(4)
    T_relative[:3, :3] = R
    T_relative[:3, 3] = t.flatten()
    
    return True, T_relative

def estimate_essential_ransac(pts1, pts2, threshold=1e-3, max_iter=1000):
    """Estimate essential matrix via the 5-point algorithm + RANSAC."""
    best_E = None
    best_inliers = np.zeros(len(pts1), dtype=bool)
    
    for _ in range(max_iter):
        # Randomly sample 8 points
        idx = np.random.choice(len(pts1), 8, replace=False)
        
        # Generate an E candidate with the 8-point algorithm (simplified 5-point variant)
        E_candidate = eight_point_essential(pts1[idx], pts2[idx])
        
        if E_candidate is None:
            continue
        
        # Inlier test via Sampson error
        errors = sampson_error(E_candidate, pts1, pts2)
        inliers = errors < threshold
        
        if np.sum(inliers) > np.sum(best_inliers):
            best_inliers = inliers
            best_E = E_candidate
    
    return best_E, best_inliers

def sampson_error(E, pts1, pts2):
    """Sampson error — first-order approximate distance of the epipolar constraint."""
    # Convert pts to homogeneous
    p1 = np.hstack([pts1, np.ones((len(pts1), 1))])  # (N, 3)
    p2 = np.hstack([pts2, np.ones((len(pts2), 1))])  # (N, 3)
    
    Ep1 = (E @ p1.T).T    # (N, 3)
    Etp2 = (E.T @ p2.T).T  # (N, 3)
    
    # p2^T E p1
    numerator = np.sum(p2 * Ep1, axis=1) ** 2
    denominator = Ep1[:, 0]**2 + Ep1[:, 1]**2 + Etp2[:, 0]**2 + Etp2[:, 1]**2
    
    return numerator / (denominator + 1e-10)
```

### 10.1.3 The Danger of False Positives and How to Prevent Them

Let us examine concretely why a false positive loop closure is so dangerous.

**Scenario**: Suppose the robot traverses two similar-looking corridors. If an incorrect loop closure is formed between keyframe $i$ in corridor A and keyframe $j$ in corridor B, the pose graph optimizer pulls these two poses close together. As a result, all poses between the two corridors are distorted, and the entire map folds or twists.

**Prevention strategies**:

1. **Multi-stage verification**: The candidate must sequentially pass appearance similarity → geometric consistency → temporal consistency.

2. **Use robust kernels** (detailed in §10.2): Make the optimizer itself less sensitive to outlier constraints.

3. **[Switchable constraints (Sünderhauf & Protzel, 2012)](https://doi.org/10.1109/IROS.2012.6385590)**: Add an on/off switch variable to each loop closure factor so that the optimizer automatically deactivates loop closures with poor consistency.

4. **[DCS (Dynamic Covariance Scaling) (Agarwal et al., 2013)](https://doi.org/10.1109/ICRA.2013.6630733)**: Dynamically adjust the covariance (uncertainty) of each loop closure so that the influence of outliers is attenuated automatically.

5. **[PCM (Pairwise Consistency Maximization) (Mangelson et al., 2018)](https://doi.org/10.1109/ICRA.2018.8460217)**: Check pairwise consistency among multiple loop closure candidates and accept only the largest consistent subset.

### 10.1.4 Correction: Graph Correction

A loop closure that passes verification is added to the pose graph as a new constraint. The loop closure edge encodes the relative transform $\mathbf{T}_{ij}$ between the two poses and its uncertainty $\boldsymbol{\Sigma}_{ij}$:

$$e_{ij} = \text{Log}(\mathbf{T}_{ij}^{-1} \cdot \mathbf{T}_i^{-1} \cdot \mathbf{T}_j)$$

Once this edge is added, the pose graph optimizer (§10.2) re-optimizes the entire graph and corrects the drift. In this process, not only the loop closure edge but also the odometry edges are adjusted, so that the error is distributed evenly over the full trajectory.

---

## 10.2 Pose Graph Optimization

Pose graph optimization is the core of the SLAM backend. It optimizes the global consistency of the entire pose trajectory while satisfying the relative constraints produced by the frontend (odometry, loop closure).

### 10.2.1 SE(3) Pose Graph

The pose graph is represented as a graph $\mathcal{G} = (\mathcal{V}, \mathcal{E})$:

- **Nodes** $\mathcal{V} = \{\mathbf{T}_1, \mathbf{T}_2, \ldots, \mathbf{T}_n\}$: the pose of each keyframe, $\mathbf{T}_i \in SE(3)$.
- **Edges** $\mathcal{E}$: relative constraints between two nodes, divided into odometry edges and loop closure edges.

Each edge $(i, j) \in \mathcal{E}$ has a measured relative transform $\tilde{\mathbf{T}}_{ij}$ and an information matrix $\boldsymbol{\Omega}_{ij}$.

**Error definition on SE(3)**: In a pose graph, the error is defined not on Euclidean space but on the Lie group SE(3):

$$\mathbf{e}_{ij} = \text{Log}(\tilde{\mathbf{T}}_{ij}^{-1} \cdot \mathbf{T}_i^{-1} \cdot \mathbf{T}_j) \in \mathbb{R}^6$$

Here, $\text{Log}: SE(3) \to \mathfrak{se}(3) \cong \mathbb{R}^6$ is the mapping to the Lie algebra via the matrix logarithm. This 6-dimensional vector encodes the error in $($ 3 rotation + 3 translation $)$.

**Optimization objective**: Minimize the weighted sum of squared edge errors:

$$\mathbf{T}^* = \arg\min_{\mathbf{T}_1, \ldots, \mathbf{T}_n} \sum_{(i,j) \in \mathcal{E}} \mathbf{e}_{ij}^\top \boldsymbol{\Omega}_{ij} \mathbf{e}_{ij}$$

This is a nonlinear least squares problem that can be solved with Gauss-Newton or Levenberg-Marquardt.

**Gauss-Newton on manifold**: Since SE(3) is not a Euclidean space, we cannot use direct addition (+). Instead, we compute an increment $\boldsymbol{\delta}$ on the Lie algebra and apply it on the manifold via the exponential map:

$$\mathbf{T}_i \leftarrow \mathbf{T}_i \cdot \text{Exp}(\boldsymbol{\delta}_i)$$

The update at a single iteration:

1. At the current estimate, compute the error $\mathbf{e}_{ij}$ and Jacobian $\mathbf{J}_{ij}$ for each edge.
2. Form the normal equations: $\mathbf{H} \boldsymbol{\delta} = -\mathbf{b}$, where

$$\mathbf{H} = \sum_{(i,j)} \mathbf{J}_{ij}^\top \boldsymbol{\Omega}_{ij} \mathbf{J}_{ij}, \quad \mathbf{b} = \sum_{(i,j)} \mathbf{J}_{ij}^\top \boldsymbol{\Omega}_{ij} \mathbf{e}_{ij}$$

3. Since $\mathbf{H}$ is sparse, it can be solved efficiently by sparse Cholesky decomposition.
4. Apply the increment: $\mathbf{T}_i \leftarrow \mathbf{T}_i \cdot \text{Exp}(\boldsymbol{\delta}_i)$.
5. Iterate until convergence.

**[g2o (General Graph Optimization)](https://doi.org/10.1109/ICRA.2011.5979949)**: A framework developed by Kümmerle et al. (2011) that implements the above procedure for general graph optimization problems. The user defines only the vertex (node) and edge (constraint) types, and the sparse optimization engine handles the rest automatically.

```python
import numpy as np
from scipy.spatial.transform import Rotation

def se3_log(T):
    """SE(3) matrix -> 6-dimensional vector (rotation + translation)."""
    R = T[:3, :3]
    t = T[:3, 3]
    
    # SO(3) logarithm
    rot = Rotation.from_matrix(R)
    omega = rot.as_rotvec()  # (3,)
    
    theta = np.linalg.norm(omega)
    
    if theta < 1e-10:
        V_inv = np.eye(3)
    else:
        omega_hat = skew(omega)
        V_inv = (np.eye(3) 
                 - 0.5 * omega_hat 
                 + (1.0/theta**2) * (1 - theta/(2*np.tan(theta/2))) * omega_hat @ omega_hat)
    
    rho = V_inv @ t
    return np.concatenate([rho, omega])  # (6,) — [translation; rotation]

def se3_exp(xi):
    """6-dimensional vector -> SE(3) matrix."""
    rho = xi[:3]   # translation part
    omega = xi[3:]  # rotation part
    
    theta = np.linalg.norm(omega)
    
    if theta < 1e-10:
        R = np.eye(3)
        V = np.eye(3)
    else:
        omega_hat = skew(omega)
        R = (np.eye(3) 
             + (np.sin(theta)/theta) * omega_hat 
             + ((1 - np.cos(theta))/theta**2) * omega_hat @ omega_hat)
        V = (np.eye(3) 
             + ((1 - np.cos(theta))/theta**2) * omega_hat 
             + ((theta - np.sin(theta))/theta**3) * omega_hat @ omega_hat)
    
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = V @ rho
    return T

def skew(v):
    """3-dimensional vector -> skew-symmetric matrix."""
    return np.array([
        [0, -v[2], v[1]],
        [v[2], 0, -v[0]],
        [-v[1], v[0], 0]
    ])

def pose_graph_error(T_i, T_j, T_ij_measured):
    """Error vector (6-dimensional) between two poses."""
    T_ij_estimated = np.linalg.inv(T_i) @ T_j
    T_error = np.linalg.inv(T_ij_measured) @ T_ij_estimated
    return se3_log(T_error)

def pose_graph_cost(poses, edges, measurements, information_matrices):
    """
    Cost function of the full pose graph.
    
    Args:
        poses: list of SE(3) matrices [T_0, T_1, ..., T_n]
        edges: list of edge indices [(i, j), ...]
        measurements: list of measured relative transforms [T_ij, ...]
        information_matrices: list of information matrices [Omega_ij, ...]
        
    Returns:
        total_cost: scalar cost
    """
    total_cost = 0.0
    
    for (i, j), T_ij, Omega_ij in zip(edges, measurements, information_matrices):
        e = pose_graph_error(poses[i], poses[j], T_ij)
        total_cost += e @ Omega_ij @ e
    
    return total_cost
```

### 10.2.2 Robust Kernel

In practical SLAM systems, outlier measurements are unavoidable. Incorrect loop closures, sensor errors, dynamic objects, and more are the causes. The standard least squares cost function $\rho(x) = x^2$ is extremely sensitive to outliers — large errors dominate the cost and distort the whole solution.

A **robust kernel** (M-estimator) limits the influence of large residuals and enables optimization that is robust to outliers:

| Kernel | $\rho(s)$ ($s = e^2$) | Characteristics |
|--------|----------------------|------|
| Least Squares | $s$ | Vulnerable to outliers |
| Huber | $\begin{cases} s & \text{if } \sqrt{s} \leq k \\ 2k\sqrt{s} - k^2 & \text{otherwise} \end{cases}$ | Linear beyond threshold $k$ |
| Cauchy | $k^2 \log(1 + s/k^2)$ | Smooth attenuation |
| Geman-McClure | $\frac{s}{k^2 + s}$ | Strong outlier suppression |

Applying a robust kernel changes the cost function to:

$$\sum_{(i,j)} \rho\left(\mathbf{e}_{ij}^\top \boldsymbol{\Omega}_{ij} \mathbf{e}_{ij}\right)$$

This can be solved via **IRLS (Iteratively Reweighted Least Squares)**: at each iteration, the weights are recomputed based on the residual magnitudes and a weighted least squares problem is solved.

$$w_i = \rho'(s_i), \quad s_i = \mathbf{e}_i^\top \boldsymbol{\Omega}_i \mathbf{e}_i$$

Here $\rho'$ is the derivative with respect to $s$ of the $\rho(s)$ defined in the table above. Outlier edges are assigned small weights so that their influence is reduced automatically.

**[Switchable constraints](https://doi.org/10.1109/IROS.2012.6385590)**: Sünderhauf & Protzel (2012) introduced a binary switch variable $s_{ij} \in [0, 1]$ on each loop closure factor, allowing the optimizer to deactivate inconsistent loop closures ($s_{ij} \to 0$):

$$\rho_{\text{switch}}(\mathbf{e}_{ij}, s_{ij}) = s_{ij}^2 \mathbf{e}_{ij}^\top \boldsymbol{\Omega}_{ij} \mathbf{e}_{ij} + \lambda (1 - s_{ij})^2$$

The first term minimizes the error when the switch is on, and the second term is a penalty for turning the switch off. The optimizer automatically balances these two.

**[DCS (Dynamic Covariance Scaling)](https://doi.org/10.1109/ICRA.2013.6630733)** (Agarwal et al., 2013): Dynamically scales the covariance of each loop closure according to the residual magnitude. The covariance of an outlier automatically grows (= increased uncertainty), so its influence decreases.

```python
def huber_kernel(s, k=1.345):
    """Huber robust kernel."""
    sqrt_s = np.sqrt(s)
    if sqrt_s <= k:
        return s
    else:
        return 2 * k * sqrt_s - k**2

def cauchy_kernel(s, k=2.3849):
    """Cauchy robust kernel."""
    return k**2 * np.log(1 + s / k**2)

def huber_weight(s, k=1.345):
    """IRLS weight for the Huber kernel."""
    sqrt_s = np.sqrt(s)
    if sqrt_s <= k:
        return 1.0
    else:
        return k / sqrt_s

def robust_pose_graph_cost(poses, edges, measurements, info_matrices, 
                           kernel='huber', k=1.345):
    """Pose graph cost with a robust kernel applied."""
    total_cost = 0.0
    
    kernel_fn = {'huber': huber_kernel, 'cauchy': cauchy_kernel}[kernel]
    
    for (i, j), T_ij, Omega in zip(edges, measurements, info_matrices):
        e = pose_graph_error(poses[i], poses[j], T_ij)
        s = e @ Omega @ e  # Mahalanobis distance squared
        total_cost += kernel_fn(s, k)
    
    return total_cost
```

### 10.2.3 iSAM2: Incremental Smoothing and Mapping

Most SLAM systems, rather than re-optimizing the entire graph in a batch fashion whenever a new keyframe is added, use an incremental approach that selectively updates only the affected parts. [iSAM2 (Kaess et al., 2012)](https://doi.org/10.1177/0278364911430419) is the key algorithm for this incremental optimization.

**Core idea**: When a new variable or measurement is added, precisely identify the range over which its influence propagates, and recompute only that part.

**Bayes tree**: The core data structure of iSAM2. Variable elimination on a factor graph yields a clique tree, and the Bayes tree is this clique tree endowed with direction.

MAP estimation on a factor graph is decomposed as follows:

$$p(\mathbf{x} | \mathbf{z}) \propto \prod_k f_k(\mathbf{x}_k)$$

Here $f_k$ is each factor (odometry, loop closure, etc.) and $\mathbf{x}_k$ is the set of variables associated with that factor.

Performing variable elimination yields:

$$p(\mathbf{x} | \mathbf{z}) = \prod_i p(x_i | \text{Sep}(x_i))$$

Here $\text{Sep}(x_i)$ is the separator of $x_i$ in the clique tree — that is, the conditioning variables that remain when this variable is eliminated. This conditional distribution structure forms the Bayes tree.

**Incremental update procedure**:

1. When a new factor is added, identify only the affected cliques (typically a small number).
2. Redo the QR decomposition only for those cliques and their ancestors.
3. Keep the rest of the tree unchanged.

**Fluid relinearization**: In nonlinear optimization, only variables whose linearization point has drifted significantly from the current estimate are relinearized. This completely removes the periodic batch relinearization that was required in iSAM v1.

**Variable reordering**: When a new variable is added, the full elimination order is not recomputed; only the affected part is reordered incrementally.

**Practical impact**: iSAM2 is the core engine of the GTSAM library and is used in the backend of nearly every modern SLAM system, including LIO-SAM and ORB-SLAM3. Because it can complete optimization within bounded time even in large environments, it enables real-time SLAM.

```python
class SimpleIncrementalOptimizer:
    """
    Simplified incremental optimizer illustrating the core concepts of iSAM2.
    The real iSAM2 is Bayes-tree-based; here we implement only the
    affected-variable tracking concept for intuitive understanding.
    """
    
    def __init__(self):
        self.poses = {}          # {id: SE(3) matrix}
        self.edges = []          # [(i, j, T_ij, Omega_ij)]
        self.affected = set()    # variable IDs that need re-optimization
        
    def add_pose(self, pose_id, T_init):
        """Add a new pose node."""
        self.poses[pose_id] = T_init.copy()
        self.affected.add(pose_id)
        
    def add_edge(self, i, j, T_ij, Omega_ij):
        """Add a new edge (constraint). Mark the connected variables as affected."""
        self.edges.append((i, j, T_ij, Omega_ij))
        self.affected.add(i)
        self.affected.add(j)
        # In the real iSAM2, ancestor cliques are also marked as affected
        # by walking up the Bayes tree.
        
    def add_loop_closure(self, i, j, T_ij, Omega_ij):
        """
        Add a loop closure edge.
        Identical to a regular edge, but a loop closure connects a node from
        the distant past with the current node, so more variables are affected.
        """
        self.add_edge(i, j, T_ij, Omega_ij)
        # A loop closure influences every intermediate pose along the trajectory.
        # In the real iSAM2, due to the Bayes tree structure,
        # the influence propagates up to cliques near the root.
        for k in range(min(i, j) + 1, max(i, j)):
            if k in self.poses:
                self.affected.add(k)
    
    def optimize(self, max_iter=5):
        """
        Re-optimize only the affected variables.
        In the real iSAM2, this is performed via partial re-decomposition of the Bayes tree.
        """
        if not self.affected:
            return
            
        # For simplicity we run full Gauss-Newton here, but
        # update only the affected variables.
        for iteration in range(max_iter):
            # 1. Select only the relevant edges
            relevant_edges = [
                (i, j, T_ij, Omega) 
                for (i, j, T_ij, Omega) in self.edges
                if i in self.affected or j in self.affected
            ]
            
            # 2. Compute error and Jacobian for each edge
            # 3. Form and solve the normal equations
            # 4. Apply the increment only to the affected variables
            
            # (The actual implementation would require solving a sparse linear system; omitted)
            pass
        
        self.affected.clear()
```

---

## 10.3 Global Relocalization

Global relocalization is the problem of finding the robot's location on a pre-built map (prior map). If loop closure is about recognizing "a place I have visited before," relocalization asks, "Where am I within a map built by someone else?"

### 10.3.1 Map-Based Localization

When a pre-built map is available, the current pose is estimated by registering new sensor observations to this map.

**Visual relocalization pipeline**:

1. Extract feature points from the current image.
2. Find 2D-3D correspondences with the 3D points of the map (via visual words or direct matching).
3. Estimate the pose with PnP + RANSAC.
4. Resume tracking using the estimated pose as a starting point.

ORB-SLAM3's relocalization performs this procedure as follows:
- Retrieve candidate keyframes with DBoW2.
- Obtain 2D-3D correspondences via ORB matching.
- Estimate the pose with EPnP + RANSAC.
- Improve accuracy by finding additional matches via guided search.

**LiDAR relocalization**: Register the current LiDAR scan to a prior (point cloud) map.

1. Use a **global descriptor** (Scan Context, PointNetVLAD, etc.) to search for nearby regions in the map.
2. **Coarse registration**: Perform initial registration with FPFH + RANSAC or GeoTransformer.
3. **Fine registration**: Refine alignment with ICP/GICP.

### 10.3.2 Prior Map + Online Sensor

In autonomous driving, it is common to localize in real time with live sensor data against a pre-built HD map. The key challenges are:

- **Discrepancies between the map and the current environment**: Over time, buildings change, vehicles park, and foliage grows. We must handle differences between the prior map and current observations.
- **Cross-modal matching**: The HD map may have been built with LiDAR while the current sensor is only a camera. Registration across heterogeneous sensors is required.
- **No initial pose**: If the robot does not know where in the map it starts, place recognition must be performed against the entire map.

### 10.3.3 Monte Carlo Localization (MCL)

MCL is a particle-filter-based global localization method. It represents the robot's possible poses as particles and updates particle weights according to sensor observations.

**Algorithm**:

1. **Initialization**: Distribute particles uniformly over the entire map (global uncertainty).
2. **Prediction**: Move particles according to the robot's motion model:
$$x_t^{[k]} \sim p(x_t | u_t, x_{t-1}^{[k]})$$
3. **Update**: Compute the weight of each particle by comparing the current sensor observation with the expected observation from the map:
$$w_t^{[k]} = p(z_t | x_t^{[k]}, m)$$
4. **Resampling**: Resample particles in proportion to their weights. Particles with high weights (close to the true location) are duplicated, and low-weight particles are eliminated.

The key advantage of MCL is that it can represent multi-modal distributions. When the robot does not know which of several places it might be in, it can maintain several hypotheses simultaneously. As observations accumulate, the particles gradually converge to the correct location.

**Example of LiDAR-based MCL**:

```python
import numpy as np

class MonteCarloLocalization:
    """
    2D LiDAR-based Monte Carlo Localization.
    Prior map: occupancy grid.
    """
    
    def __init__(self, occupancy_map, num_particles=1000, 
                 map_resolution=0.05):
        """
        Args:
            occupancy_map: 2D numpy array, 0=free, 1=occupied
            num_particles: number of particles
            map_resolution: map resolution (m/pixel)
        """
        self.map = occupancy_map
        self.resolution = map_resolution
        self.num_particles = num_particles
        
        # Particle initialization: [x, y, theta, weight]
        self.particles = self._initialize_particles()
    
    def _initialize_particles(self):
        """Distribute particles uniformly over the free space of the map."""
        free_cells = np.argwhere(self.map == 0)
        
        if len(free_cells) == 0:
            raise ValueError("The map has no free space.")
        
        # Randomly select free cells
        indices = np.random.choice(len(free_cells), self.num_particles, 
                                   replace=True)
        selected = free_cells[indices]
        
        particles = np.zeros((self.num_particles, 4))
        particles[:, 0] = selected[:, 1] * self.resolution  # x
        particles[:, 1] = selected[:, 0] * self.resolution  # y
        particles[:, 2] = np.random.uniform(-np.pi, np.pi, 
                                             self.num_particles)  # theta
        particles[:, 3] = 1.0 / self.num_particles  # weight (uniform)
        
        return particles
    
    def predict(self, delta_x, delta_y, delta_theta, 
                noise_std=[0.1, 0.1, 0.05]):
        """Move particles with the motion model + noise."""
        noise = np.random.randn(self.num_particles, 3) * noise_std
        
        cos_theta = np.cos(self.particles[:, 2])
        sin_theta = np.sin(self.particles[:, 2])
        
        # Convert motion in the robot frame to the global frame
        self.particles[:, 0] += (delta_x * cos_theta 
                                  - delta_y * sin_theta + noise[:, 0])
        self.particles[:, 1] += (delta_x * sin_theta 
                                  + delta_y * cos_theta + noise[:, 1])
        self.particles[:, 2] += delta_theta + noise[:, 2]
        
        # Normalize angles
        self.particles[:, 2] = np.arctan2(
            np.sin(self.particles[:, 2]), 
            np.cos(self.particles[:, 2])
        )
    
    def update(self, scan_ranges, scan_angles, sigma_hit=0.2):
        """
        Update particle weights with sensor observations.
        
        Args:
            scan_ranges: actual LiDAR range measurements (N,)
            scan_angles: angle of each beam (N,)
            sigma_hit: sensor noise standard deviation
        """
        for k in range(self.num_particles):
            x, y, theta = self.particles[k, :3]
            
            log_weight = 0.0
            for r_measured, angle in zip(scan_ranges, scan_angles):
                # Compute expected range at this particle's location (ray casting)
                r_expected = self._ray_cast(x, y, theta + angle)
                
                # Gaussian sensor model
                diff = r_measured - r_expected
                log_weight += -0.5 * (diff / sigma_hit) ** 2
            
            self.particles[k, 3] = np.exp(log_weight)
        
        # Normalize weights
        total = np.sum(self.particles[:, 3])
        if total > 0:
            self.particles[:, 3] /= total
        else:
            self.particles[:, 3] = 1.0 / self.num_particles
    
    def resample(self):
        """Low-variance resampling."""
        weights = self.particles[:, 3]
        N = self.num_particles
        
        new_particles = np.zeros_like(self.particles)
        r = np.random.uniform(0, 1.0 / N)
        c = weights[0]
        i = 0
        
        for m in range(N):
            u = r + m / N
            while u > c:
                i += 1
                c += weights[i]
            new_particles[m] = self.particles[i].copy()
        
        new_particles[:, 3] = 1.0 / N
        self.particles = new_particles
    
    def get_estimate(self):
        """Estimate the current pose as a weighted average."""
        weights = self.particles[:, 3]
        x_est = np.average(self.particles[:, 0], weights=weights)
        y_est = np.average(self.particles[:, 1], weights=weights)
        
        # Weighted average of angles (circular mean)
        sin_avg = np.average(np.sin(self.particles[:, 2]), weights=weights)
        cos_avg = np.average(np.cos(self.particles[:, 2]), weights=weights)
        theta_est = np.arctan2(sin_avg, cos_avg)
        
        return x_est, y_est, theta_est
    
    def _ray_cast(self, x, y, angle, max_range=30.0):
        """Simple Bresenham-based ray casting."""
        dx = np.cos(angle) * self.resolution
        dy = np.sin(angle) * self.resolution
        
        cx, cy = x, y
        for step in range(int(max_range / self.resolution)):
            cx += dx
            cy += dy
            
            # Convert to map coordinates
            mx = int(cy / self.resolution)
            my = int(cx / self.resolution)
            
            if (mx < 0 or mx >= self.map.shape[0] or 
                my < 0 or my >= self.map.shape[1]):
                return max_range
            
            if self.map[mx, my] == 1:  # obstacle hit
                return step * self.resolution
        
        return max_range
```

---

## 10.4 Multi-Session SLAM & Map Merging

In practical environments, SLAM is often not completed in a single run. The same building may be mapped over several days, multiple robots may simultaneously explore different areas, or tracking may fail and then resume from a different point. Multi-session SLAM integrates the maps from such separate sessions into a single globally consistent map.

### 10.4.1 Map Anchoring

To integrate maps from multiple sessions, the coordinate frame of each map must be aligned. This is called **map anchoring**.

**Method 1: Based on shared landmarks**: If two sessions observe the same 3D points or feature points in an overlapping region, the relative transform ${}^{A}\mathbf{T}_{B}$ between the two maps is estimated from them.

$${}^{A}\mathbf{T}_{B} = \arg\min_{\mathbf{T}} \sum_k \| \mathbf{p}_k^A - \mathbf{T} \cdot \mathbf{p}_k^B \|^2$$

**Method 2: Based on place recognition**: Even when shared landmarks are not explicitly available, place recognition can find keyframe pairs from the two sessions that visited the same location, and alignment is performed from these pairs.

**Method 3: Based on GNSS**: If both sessions have GNSS information, it can be used as a shared coordinate frame for direct alignment.

### 10.4.2 Inter-Session Loop Closure

Once map anchoring provides an initial alignment, inter-session loop closure performs the precise registration. The principle is the same as standard loop closure, but two additional challenges arise:

1. **Appearance change**: Over time, lighting, season, and furniture arrangement change. Foundation-model-based descriptors such as AnyLoc are robust to this problem.

2. **Frame misalignment**: Because the initial alignment may be inaccurate, the tolerance of geometric verification must be increased.

### 10.4.3 ORB-SLAM3 Multi-Map System

The Atlas system of ORB-SLAM3 is a representative implementation of multi-session SLAM. The core mechanisms are as follows:

1. **Active map**: The map currently being tracked. When operating normally, keyframes and map points are added to this map.

2. **Map creation**: If tracking fails (e.g., due to occlusion or a textureless environment), a new map is created and set as the active map. The previous map is kept in an inactive state inside Atlas.

3. **Map merging**: When place recognition detects a common location between the current active map and an inactive map in Atlas, the two maps are merged:
   - Estimate the relative transform $\mathbf{T}_{merge}$ from the common keyframe pair.
   - Transform all keyframes and map points of the inactive map by $\mathbf{T}_{merge}$.
   - Fuse common map points.
   - Ensure consistency in the merged region via welding bundle adjustment.

4. **Leveraging multi-session**: A map from a previous session can be loaded, and when the current session visits the same location, it is automatically merged. This allows the map to be extended incrementally across multiple sessions.

```python
class MultiMapAtlas:
    """
    Core concepts of the ORB-SLAM3 Atlas system.
    """
    
    def __init__(self):
        self.maps = {}           # {map_id: MapData}
        self.active_map_id = None
        self.next_map_id = 0
    
    def create_new_map(self, initial_pose):
        """Create a new map and set it as active."""
        map_id = self.next_map_id
        self.next_map_id += 1
        
        self.maps[map_id] = {
            'keyframes': [initial_pose],
            'map_points': [],
            'is_active': True,
            'origin': initial_pose.copy()
        }
        
        # Deactivate the previous active map
        if self.active_map_id is not None:
            self.maps[self.active_map_id]['is_active'] = False
        
        self.active_map_id = map_id
        return map_id
    
    def on_tracking_lost(self, last_pose):
        """
        On tracking failure: preserve the current map and create a new one.
        """
        print(f"Tracking lost. Map {self.active_map_id} "
              f"saved with {len(self.maps[self.active_map_id]['keyframes'])} "
              f"keyframes.")
        self.create_new_map(last_pose)
    
    def try_merge_maps(self, current_kf_descriptor, current_kf_pose):
        """
        Search inactive maps for a match using the current keyframe's descriptor.
        If a match is found, merge the two maps.
        """
        for map_id, map_data in self.maps.items():
            if map_id == self.active_map_id:
                continue
            
            # Search inactive-map keyframes via place recognition
            match_idx = self._search_in_map(current_kf_descriptor, map_id)
            
            if match_idx is not None:
                # Geometric verification
                T_merge = self._compute_merge_transform(
                    current_kf_pose, 
                    map_data['keyframes'][match_idx]
                )
                
                if T_merge is not None:
                    self._merge_maps(self.active_map_id, map_id, T_merge)
                    return True
        
        return False
    
    def _merge_maps(self, active_id, merge_id, T_merge):
        """
        Merge the merge_id map into the active_id map.
        Transform all keyframes/map points of merge_id by T_merge.
        """
        merge_map = self.maps[merge_id]
        active_map = self.maps[active_id]
        
        # Transform the keyframes of the inactive map and add them to the active map
        for kf in merge_map['keyframes']:
            transformed_kf = T_merge @ kf
            active_map['keyframes'].append(transformed_kf)
        
        # Map points are also transformed and added
        for mp in merge_map['map_points']:
            transformed_mp = T_merge @ mp
            active_map['map_points'].append(transformed_mp)
        
        # Remove the merged map
        del self.maps[merge_id]
        
        print(f"Maps {active_id} and {merge_id} merged. "
              f"Total keyframes: {len(active_map['keyframes'])}")
    
    def _search_in_map(self, descriptor, map_id):
        """Perform place recognition in an inactive map (placeholder)."""
        return None  # Actual implementation would compare descriptors
    
    def _compute_merge_transform(self, pose_a, pose_b):
        """Compute the transform between two poses (placeholder)."""
        return None  # Actual implementation would include geometric verification
```

### 10.4.4 Multi-Robot Map Merging

When multiple robots explore simultaneously, each robot's map must be integrated in real time. The additional constraints are:

- **Communication bandwidth**: Full point clouds cannot be transmitted, so only compressed descriptors (Scan Context, compact visual descriptors) are exchanged.
- **Distributed optimization**: Robots must be able to merge maps autonomously without a central server. Kimera-Multi and Swarm-SLAM address this problem.
- **Relative pose uncertainty**: Because the initial relative pose between robots is unknown, alignment must be achieved via inter-robot loop closures.

The core idea of distributed pose graph optimization:

$$\mathbf{T}^* = \arg\min \sum_{\text{robot } r} \sum_{(i,j) \in \mathcal{E}_r} \rho(\mathbf{e}_{ij}) + \sum_{(i,j) \in \mathcal{E}_{\text{inter}}} \rho(\mathbf{e}_{ij})$$

Each robot performs the optimization over its own edges $\mathcal{E}_r$ locally, and exchanges information only for the inter-robot edges $\mathcal{E}_{\text{inter}}$. Distributed convergence can be achieved via ADMM (Alternating Direction Method of Multipliers) or Gauss-Seidel iteration.

---

## 10.5 Recent Research (2024-2025)

**[riSAM (McGann et al., 2023)](https://arxiv.org/abs/2209.14359)**: A robust backend that integrates **Graduated Non-Convexity (GNC)** into iSAM2 to remove outlier loop closures online in incremental SLAM. It operates robustly even with more than 90% outlier measurements and achieves, in real time, performance comparable to existing offline methods. [The theoretical foundation of GNC was presented by Yang et al. (2020)](https://arxiv.org/abs/1909.08605).

**[Kimera2 (Abate et al., 2024)](https://arxiv.org/abs/2401.06323)**: The next-generation version of the Kimera SLAM library, which replaces the backend's outlier rejection from PCM with GNC, significantly improving robustness. It has been validated on diverse platforms such as drones, quadruped robots, and autonomous vehicles, and includes comprehensive improvements for the practical deployment of metric-semantic SLAM.

**[Group-k Consistent Measurement Set Maximization (Forsgren & Kaess, 2022)](https://arxiv.org/abs/2209.02658)**: Extends PCM's pairwise consistency to group-k consistency, enabling stricter outlier detection. In multi-robot map merging, it further suppresses false positives relative to PCM.

---

Through loop closure and global optimization, a SLAM system produces a globally consistent trajectory and map. But what form does this "map" take? Is it a point cloud, a grid, or a neural network? In the next chapter, we examine the various forms of **spatial representation** — the final output of sensor fusion — along with their strengths and weaknesses.

---

# Ch.11 — Spatial Representations

In Ch.6-10 we covered the process of estimating the robot's trajectory from sensor data and securing global consistency through loop closure. This chapter treats the byproduct and ultimate goal of that process: the **map** — the form in which the robot remembers and uses the world.

The ultimate output of sensor fusion is a **map** — a spatial representation of the environment. What a SLAM system estimates is not only the robot's trajectory but also the structure of the environment observed along that trajectory. The way in which the environment is represented determines what the robot can do: path planning requires free/occupied information, visual rendering requires texture information, and human interaction requires semantic information.

In this chapter, starting from metric maps (quantitative geometric maps), we cover the full spectrum of spatial representations — mesh, neural representation, semantic map, and long-term maintenance.

---

## 11.1 Metric Maps

A metric map represents the geometric structure of the environment in quantitative coordinates. It is the most basic form and is still the most widely used.

### 11.1.1 Occupancy Grid (2D/3D)

An occupancy grid partitions space into a uniform grid and represents each cell probabilistically as occupied/free/unknown.

**2D occupancy grid**: used for planar environments (indoor, single-floor). For each cell $m_i$, the occupancy probability $p(m_i \mid z_{1:t})$ is maintained via Bayesian update.

$$\text{log-odds}(m_i \mid z_{1:t}) = \text{log-odds}(m_i \mid z_{1:t-1}) + \text{log-odds}(m_i \mid z_t) - \text{log-odds}(m_i)$$

Using the log-odds representation turns multiplication into addition, making computation efficient and numerically stable:

$$l(m_i) = \log\frac{p(m_i)}{1 - p(m_i)}$$

**Problem**: representing a 3D environment requires a 3D occupancy grid. Representing an $L \times W \times H$ space at resolution $r$ requires $(L/r)(W/r)(H/r)$ cells. For instance, representing a 100 m $\times$ 100 m $\times$ 10 m space at 5 cm resolution requires about $8 \times 10^9$ cells, roughly 32 GB of memory. This is impractical.

```python
import numpy as np

class OccupancyGrid2D:
    """2D Occupancy Grid — log-odds-based Bayesian update."""
    
    def __init__(self, width_m, height_m, resolution=0.05):
        """
        Args:
            width_m: map width (meters)
            height_m: map height (meters)
            resolution: cell size (meters)
        """
        self.resolution = resolution
        self.width = int(width_m / resolution)
        self.height = int(height_m / resolution)
        
        # Log-odds map: 0 = unknown (prior = 0.5)
        self.log_odds = np.zeros((self.height, self.width), dtype=np.float32)
        
        # Sensor model parameters
        self.l_occ = 0.85   # log-odds(occupied | hit)
        self.l_free = -0.40  # log-odds(free | pass-through)
        
        # Clipping range (prevents probability saturation)
        self.l_min = -5.0
        self.l_max = 5.0
    
    def update(self, robot_x, robot_y, scan_ranges, scan_angles):
        """
        Update the map with a LiDAR scan.
        
        Args:
            robot_x, robot_y: robot position (meters)
            scan_ranges: range of each beam (N,)
            scan_angles: angle of each beam (N,)
        """
        rx = int(robot_x / self.resolution)
        ry = int(robot_y / self.resolution)
        
        for r, angle in zip(scan_ranges, scan_angles):
            # Endpoint of the beam (occupied)
            ex = int((robot_x + r * np.cos(angle)) / self.resolution)
            ey = int((robot_y + r * np.sin(angle)) / self.resolution)
            
            # Bresenham line: robot -> endpoint = free
            cells = self._bresenham(rx, ry, ex, ey)
            for cx, cy in cells[:-1]:  # exclude last cell (that one is occupied)
                if 0 <= cx < self.width and 0 <= cy < self.height:
                    self.log_odds[cy, cx] = np.clip(
                        self.log_odds[cy, cx] + self.l_free,
                        self.l_min, self.l_max
                    )
            
            # Endpoint cell = occupied
            if 0 <= ex < self.width and 0 <= ey < self.height:
                self.log_odds[ey, ex] = np.clip(
                    self.log_odds[ey, ex] + self.l_occ,
                    self.l_min, self.l_max
                )
    
    def get_probability_map(self):
        """Log-odds -> probability conversion."""
        return 1.0 - 1.0 / (1.0 + np.exp(self.log_odds))
    
    def _bresenham(self, x0, y0, x1, y1):
        """Bresenham line algorithm — returns grid cells between two points."""
        cells = []
        dx = abs(x1 - x0)
        dy = abs(y1 - y0)
        sx = 1 if x0 < x1 else -1
        sy = 1 if y0 < y1 else -1
        err = dx - dy
        
        while True:
            cells.append((x0, y0))
            if x0 == x1 and y0 == y1:
                break
            e2 = 2 * err
            if e2 > -dy:
                err -= dy
                x0 += sx
            if e2 < dx:
                err += dx
                y0 += sy
        
        return cells
```

### 11.1.2 Voxel Maps: OctoMap, VDB, ikd-tree

To address the memory problem of uniform grids, various adaptive data structures have been proposed.

**[OctoMap](https://doi.org/10.1007/s10514-012-9321-0)** (Hornung et al. 2013): an octree-based probabilistic 3D occupancy map. Space is recursively subdivided into octants, and regions that are entirely occupied or entirely free stop subdividing early (pruning). Through this, empty space is represented with large units and only regions near fine structure are represented with small units, saving memory.

Key properties:
- **Probabilistic update**: uses the same log-odds update as an occupancy grid.
- **Adaptive resolution**: automatically adjusts from the finest resolution (e.g., 2 cm, leaf node) to the coarsest resolution (e.g., several m, near the root).
- **Memory efficiency**: a 64 m$^3$ space can be represented at 1 cm resolution in about 60 MB (hundreds of times smaller than a uniform grid).
- **Limitations**: the cost of tree rebalancing on dynamic insertion/deletion, and slow nearest-neighbor (kNN) search.

```python
class SimpleOctreeNode:
    """Core idea of OctoMap — a recursively 8-way subdivided octree."""
    
    def __init__(self, center, size, depth=0, max_depth=16):
        self.center = np.array(center)  # node center coordinate
        self.size = size                 # edge length of the node
        self.depth = depth
        self.max_depth = max_depth
        self.children = [None] * 8       # 8 children
        self.log_odds = 0.0              # occupancy log-odds
        self.is_leaf = True
    
    def get_child_index(self, point):
        """Determine which child octant the point belongs to."""
        idx = 0
        if point[0] >= self.center[0]: idx |= 1
        if point[1] >= self.center[1]: idx |= 2
        if point[2] >= self.center[2]: idx |= 4
        return idx
    
    def get_child_center(self, child_idx):
        """Compute the center coordinate of a child octant."""
        offset = self.size / 4
        center = self.center.copy()
        center[0] += offset if (child_idx & 1) else -offset
        center[1] += offset if (child_idx & 2) else -offset
        center[2] += offset if (child_idx & 4) else -offset
        return center
    
    def update(self, point, is_occupied, l_occ=0.85, l_free=-0.40):
        """Update the octree with a point."""
        if self.depth >= self.max_depth:
            # Reached maximum depth: update log-odds at the leaf
            self.log_odds += l_occ if is_occupied else l_free
            self.log_odds = np.clip(self.log_odds, -5.0, 5.0)
            return
        
        # Decide the child octant
        child_idx = self.get_child_index(point)
        
        if self.children[child_idx] is None:
            self.children[child_idx] = SimpleOctreeNode(
                self.get_child_center(child_idx),
                self.size / 2,
                self.depth + 1,
                self.max_depth
            )
            self.is_leaf = False
        
        self.children[child_idx].update(point, is_occupied, l_occ, l_free)
    
    def prune(self):
        """
        Merge children if they are all in the same state (pruning).
        This is the core memory-saving technique of OctoMap.
        """
        if self.is_leaf:
            return
        
        # Check that all children exist, are leaves, and are in the same state
        all_same = True
        first_odds = None
        
        for child in self.children:
            if child is None or not child.is_leaf:
                return  # cannot prune
            
            if first_odds is None:
                first_odds = child.log_odds
            elif abs(child.log_odds - first_odds) > 0.01:
                all_same = False
                break
        
        if all_same and first_odds is not None:
            self.log_odds = first_odds
            self.children = [None] * 8
            self.is_leaf = True
```

**OpenVDB**: a sparse volumetric data structure originating from the film VFX industry. Based on a hash map, it stores only activated voxels, making it highly efficient when only a tiny fraction of voxels in a large space are occupied. It is faster to traverse than OctoMap, but the user must implement probabilistic updates directly.

**ikd-tree** (FAST-LIO2): an incremental k-d tree that performs point insertion and deletion in $O(\log n)$ and supports dynamic rebalancing. Used as the map data structure in FAST-LIO2, it adds LiDAR points to the map in real time while efficiently performing nearest-neighbor (kNN) search.

Core operations of ikd-tree:
- **Insertion**: insert a new point into the k-d tree. Partial rebalancing occurs when the imbalance exceeds a threshold.
- **Deletion**: remove old points outside a certain range via lazy deletion.
- **kNN search**: find the nearest map points to an observed point and use them for point-to-plane registration.

OctoMap vs. ikd-tree:

| Property | OctoMap | ikd-tree |
|------|---------|----------|
| Data structure | Octree | k-d tree |
| Probabilistic update | Built-in (log-odds) | None (only point storage) |
| kNN search | Slow | Fast |
| Dynamic insertion/deletion | Possible but slow | $O(\log n)$ |
| Use case | Path planning, exploration | LiDAR odometry map maintenance |

### 11.1.3 Surfel Maps

A surfel (surface element) is a disk-shaped surface primitive that augments a point with a normal vector and radius information. Each surfel is represented as $(\mathbf{p}, \mathbf{n}, r, c)$:

- $\mathbf{p} \in \mathbb{R}^3$: position
- $\mathbf{n} \in \mathbb{R}^3$: normal vector (unit)
- $r \in \mathbb{R}^+$: radius
- $c$: color/confidence

**[ElasticFusion](https://doi.org/10.15607/RSS.2015.XI.001)** (Whelan et al. 2015): a dense SLAM system that builds a surfel map in real time from an RGB-D sensor. The core ideas are:

1. Frame-to-model tracking: register the current frame against a rendering of the surfel map.
2. Map deformation: upon loop closure, non-rigidly deform the entire surfel map via an embedded deformation graph.
3. Surfel fusion: when existing surfels overlap with new observations, merge them via a weighted average.

The advantage of surfels is that they can represent a continuous surface without an explicit mesh. They carry richer surface information than a point cloud while being simpler to update than a mesh.

```python
class Surfel:
    """A single Surface Element."""
    
    def __init__(self, position, normal, radius, color, confidence=1.0):
        self.position = np.array(position, dtype=np.float64)  # (3,)
        self.normal = np.array(normal, dtype=np.float64)      # (3,)
        self.radius = float(radius)
        self.color = np.array(color, dtype=np.float64)        # (3,) RGB
        self.confidence = confidence
        self.update_count = 1
    
    def fuse(self, new_pos, new_normal, new_radius, new_color, 
             new_confidence=1.0):
        """Update the surfel with a new observation (weighted average)."""
        total_w = self.confidence + new_confidence
        
        self.position = (self.confidence * self.position 
                         + new_confidence * new_pos) / total_w
        
        # Normal: weighted average then normalize
        avg_normal = (self.confidence * self.normal 
                      + new_confidence * new_normal) / total_w
        norm = np.linalg.norm(avg_normal)
        if norm > 1e-6:
            self.normal = avg_normal / norm
        
        self.radius = (self.confidence * self.radius 
                       + new_confidence * new_radius) / total_w
        self.color = (self.confidence * self.color 
                      + new_confidence * new_color) / total_w
        
        self.confidence = min(total_w, 100.0)  # confidence upper bound
        self.update_count += 1

class SurfelMap:
    """A simple surfel map implementation."""
    
    def __init__(self, fusion_distance=0.02, fusion_normal_threshold=0.9):
        self.surfels = []
        self.fusion_dist = fusion_distance
        self.fusion_normal_thresh = fusion_normal_threshold
    
    def integrate(self, points, normals, radii, colors):
        """
        Integrate new observations into the map.
        Fuse if close to an existing surfel; otherwise add a new one.
        """
        for p, n, r, c in zip(points, normals, radii, colors):
            fused = False
            
            for surfel in self.surfels:
                dist = np.linalg.norm(surfel.position - p)
                normal_dot = abs(np.dot(surfel.normal, n))
                
                if (dist < self.fusion_dist and 
                    normal_dot > self.fusion_normal_thresh):
                    surfel.fuse(p, n, r, c)
                    fused = True
                    break
            
            if not fused:
                self.surfels.append(Surfel(p, n, r, c))
```

---

## 11.2 Mesh & CAD-level Maps

A mesh represents surfaces as a collection of triangle faces. It is visually more natural than voxels or point clouds and can be used directly in physical simulation (collision detection, etc.).

### 11.2.1 TSDF (Truncated Signed Distance Function)

A TSDF stores the signed distance from each voxel to the nearest surface:

- Positive: in front of the surface (free space)
- Negative: behind the surface (occupied)
- Zero crossing: the actual surface location

**TSDF update**: whenever a new depth observation arrives, the voxels along the corresponding ray are updated:

$$\text{TSDF}(\mathbf{v}) \leftarrow \frac{W(\mathbf{v}) \cdot \text{TSDF}(\mathbf{v}) + w_{\text{new}} \cdot d_{\text{new}}}{W(\mathbf{v}) + w_{\text{new}}}$$

$$W(\mathbf{v}) \leftarrow \min(W(\mathbf{v}) + w_{\text{new}}, W_{\max})$$

Here $d_{\text{new}}$ is the signed distance computed from the new observation, $w_{\text{new}}$ is the observation weight, and $W(\mathbf{v})$ is the accumulated weight. The signed distance is truncated within the truncation distance $\delta$:

$$d_{\text{new}} = \text{clip}(D(\mathbf{u}) - \|\mathbf{v} - \mathbf{c}\|, -\delta, \delta)$$

$D(\mathbf{u})$ is the depth value at pixel $\mathbf{u}$, $\mathbf{c}$ is the camera position, and $\mathbf{v}$ is the voxel center.

```python
class TSDFVolume:
    """
    TSDF volume — 3D reconstruction from RGB-D frames.
    """
    
    def __init__(self, volume_bounds, voxel_size=0.02):
        """
        Args:
            volume_bounds: [[x_min, x_max], [y_min, y_max], [z_min, z_max]]
            voxel_size: voxel edge length (meters)
        """
        self.voxel_size = voxel_size
        self.bounds = np.array(volume_bounds)
        
        self.dims = np.ceil(
            (self.bounds[:, 1] - self.bounds[:, 0]) / voxel_size
        ).astype(int)
        
        # TSDF values and weights
        self.tsdf = np.ones(self.dims) * 1.0  # initial value: truncation value
        self.weight = np.zeros(self.dims, dtype=np.float32)
        self.color = np.zeros((*self.dims, 3), dtype=np.float32)
        
        # Truncation distance
        self.trunc_dist = 3.0 * voxel_size
    
    def integrate(self, depth_image, color_image, K, T_camera_to_world):
        """
        Integrate a single RGB-D frame into the TSDF.
        
        Args:
            depth_image: (H, W) depth (meters)
            color_image: (H, W, 3) RGB (0-255)
            K: camera intrinsic parameters (3, 3)
            T_camera_to_world: camera pose (4, 4)
        """
        T_world_to_camera = np.linalg.inv(T_camera_to_world)
        cam_pos = T_camera_to_world[:3, 3]
        
        H, W = depth_image.shape
        fx, fy = K[0, 0], K[1, 1]
        cx, cy = K[0, 2], K[1, 2]
        
        # For each voxel
        for ix in range(self.dims[0]):
            for iy in range(self.dims[1]):
                for iz in range(self.dims[2]):
                    # World coordinates of the voxel center
                    vx = self.bounds[0, 0] + (ix + 0.5) * self.voxel_size
                    vy = self.bounds[1, 0] + (iy + 0.5) * self.voxel_size
                    vz = self.bounds[2, 0] + (iz + 0.5) * self.voxel_size
                    
                    # Transform to camera coordinates
                    p_world = np.array([vx, vy, vz, 1.0])
                    p_cam = T_world_to_camera @ p_world
                    
                    if p_cam[2] <= 0:
                        continue
                    
                    # Project to image coordinates
                    u = int(fx * p_cam[0] / p_cam[2] + cx)
                    v = int(fy * p_cam[1] / p_cam[2] + cy)
                    
                    if u < 0 or u >= W or v < 0 or v >= H:
                        continue
                    
                    depth = depth_image[v, u]
                    if depth <= 0:
                        continue
                    
                    # Compute signed distance
                    sdf = depth - p_cam[2]
                    
                    if sdf < -self.trunc_dist:
                        continue
                    
                    tsdf_val = min(sdf / self.trunc_dist, 1.0)
                    
                    # Weighted-average update
                    w_old = self.weight[ix, iy, iz]
                    w_new = 1.0
                    
                    self.tsdf[ix, iy, iz] = (
                        (w_old * self.tsdf[ix, iy, iz] + w_new * tsdf_val) 
                        / (w_old + w_new)
                    )
                    self.color[ix, iy, iz] = (
                        (w_old * self.color[ix, iy, iz] 
                         + w_new * color_image[v, u].astype(float))
                        / (w_old + w_new)
                    )
                    self.weight[ix, iy, iz] = min(w_old + w_new, 100.0)
    
    def extract_mesh(self):
        """
        Extract a mesh from the TSDF via Marching Cubes.
        Find zero crossings and generate triangle faces.
        """
        # Marching Cubes algorithm:
        # check the TSDF sign at the 8 corners of each voxel cube and
        # interpolate vertices on edges where the sign changes to generate triangles.
        
        vertices = []
        faces = []
        
        for ix in range(self.dims[0] - 1):
            for iy in range(self.dims[1] - 1):
                for iz in range(self.dims[2] - 1):
                    # TSDF values at the 8 corners
                    cube = np.array([
                        self.tsdf[ix, iy, iz],
                        self.tsdf[ix+1, iy, iz],
                        self.tsdf[ix+1, iy+1, iz],
                        self.tsdf[ix, iy+1, iz],
                        self.tsdf[ix, iy, iz+1],
                        self.tsdf[ix+1, iy, iz+1],
                        self.tsdf[ix+1, iy+1, iz+1],
                        self.tsdf[ix, iy+1, iz+1],
                    ])
                    
                    # Process only if there is a sign change
                    if np.all(cube > 0) or np.all(cube < 0):
                        continue
                    
                    # The actual Marching Cubes uses a 256-case
                    # look-up table to generate triangles.
                    # Here we only show the concept.
                    pass
        
        return vertices, faces
```

### 11.2.2 Voxblox

**[Voxblox](https://arxiv.org/abs/1611.03631)** (Oleynikova et al. 2017) is a TSDF-based real-time 3D reconstruction system that efficiently computes the **ESDF (Euclidean Signed Distance Field)** essential for path planning.

Core pipeline:

1. **TSDF integration**: integrate RGB-D or depth point clouds into the TSDF in a projective manner.
2. **Mesh extraction**: extract a mesh from the TSDF incrementally via Marching Cubes. Reprocess only changed voxel blocks.
3. **ESDF computation**: compute the ESDF from the TSDF. The ESDF stores the Euclidean distance from each voxel to the nearest obstacle.

Why ESDF matters: in path planning the robot must maintain a safety distance from obstacles, and having an ESDF allows querying the distance from an arbitrary point to the nearest obstacle in $O(1)$. It also provides gradient information, so the direction to avoid obstacles is immediately available.

### 11.2.3 Poisson Surface Reconstruction

Poisson reconstruction is a method that generates a watertight mesh from an oriented point cloud (position + normals).

**Core idea**: interpret the normal vectors as a gradient field, take the divergence of this gradient, and solve the Poisson equation:

$$\nabla^2 \chi = \nabla \cdot \mathbf{V}$$

Here $\mathbf{V}$ is the normal vector field, and $\chi$ is the indicator function (1 inside the surface, 0 outside). This PDE is solved efficiently on an octree, and an iso-surface is extracted.

Advantages: robust to noise, produces a watertight mesh, and works well even when the input points have non-uniform density. Disadvantages: suited to offline processing; for real-time SLAM, TSDF-based methods are preferred.

### 11.2.4 Real-Time Mesh Generation

Modern approaches to real-time mesh generation in SLAM systems:

1. **Voxblox incremental meshing**: re-run Marching Cubes only on voxel blocks whose TSDF has been updated. Real time is achievable because the entire volume is not reprocessed.
2. **FAST-LIVO2 mesh**: attach camera colors to LiDAR points to generate a colored mesh in real time. Post-process from the unified voxel map via Poisson or Ball Pivoting.

---

## 11.3 Neural / Learned Representations

Traditional map representations (voxel, mesh, surfel) are explicit — they store 3D structure directly. Neural representations, in contrast, encode the 3D scene into the weights of a neural network in an **implicit** or **parametric** manner.

### 11.3.1 NeRF-SLAM: Neural Implicit + Odometry

**[NeRF](https://arxiv.org/abs/2003.08934) (Neural Radiance Field)** (Mildenhall et al. 2020) represents a scene with an MLP that takes a 3D coordinate and viewing direction as input and outputs color and density:

$$F_\theta: (\mathbf{x}, \mathbf{d}) \to (c, \sigma)$$

Here $\mathbf{x} \in \mathbb{R}^3$ is a 3D position, $\mathbf{d} \in \mathbb{S}^2$ is the viewing direction, $c$ is RGB color, and $\sigma$ is volume density.

**Volume rendering equation**: integrate along the camera ray $\mathbf{r}(t) = \mathbf{o} + t\mathbf{d}$ to synthesize pixel color:

$$\hat{C}(\mathbf{r}) = \int_{t_n}^{t_f} T(t) \sigma(\mathbf{r}(t)) c(\mathbf{r}(t), \mathbf{d}) \, dt$$

Here $T(t) = \exp\left(-\int_{t_n}^{t} \sigma(\mathbf{r}(s)) ds\right)$ is the accumulated transmittance.

**NeRF-SLAM pipeline**: whereas the original NeRF performed offline reconstruction, NeRF-SLAM couples with SLAM to operate online:

1. **Tracking**: optimize the camera pose of the current frame against the existing neural map (photometric loss + depth loss).
2. **Mapping**: at the estimated pose, update the neural network weights. While learning newly observed regions, the representation of previously observed regions must remain consistent.

**[iMAP](https://arxiv.org/abs/2103.12352)** (Sucar et al. 2021): the first neural implicit SLAM. It represents the scene with a single MLP. For real-time performance, it uses keyframe-based learning and an active sampling strategy.

Limitations:
- **Training speed**: MLP training is slow, constraining real-time mapping.
- **Forgetting**: learning new regions degrades the representation of previously seen regions (catastrophic forgetting).
- **Scalability**: the capacity limit of a single MLP in large-scale environments.

Directions of improvement:
- **Instant-NGP**: hash-grid-based feature encoding accelerates training by tens of times. NeRF-SLAM variants leveraging this have emerged.
- **Multi-resolution hash grid**: partition space into hash tables at multiple resolutions, storing and querying local features efficiently.

**[NICE-SLAM](https://arxiv.org/abs/2112.12130)** (Zhu et al. 2022): a dense SLAM system that addresses iMAP's scalability problem by introducing a hierarchical feature grid and a pre-trained geometry decoder. It operates stably even in large-scale indoor environments and was presented at CVPR 2022.

### 11.3.2 3DGS-SLAM: 3D Gaussian Splatting-Based SLAM

**[3D Gaussian Splatting (3DGS)](https://arxiv.org/abs/2308.04079)** (Kerbl et al. 2023) is a representation that emerged as an alternative to NeRF, representing a scene with millions of 3D Gaussians. Each Gaussian consists of:

- Position $\boldsymbol{\mu} \in \mathbb{R}^3$
- Covariance $\boldsymbol{\Sigma} \in \mathbb{R}^{3 \times 3}$ (parameterized by rotation + scale)
- Opacity $\alpha \in [0, 1]$
- Color (spherical harmonics coefficients)

Rendering is performed by splatting — projecting 3D Gaussians onto the image plane and alpha-blending them in depth order — which is tens of times faster than NeRF's ray marching.

**[3DGS-SLAM](https://arxiv.org/abs/2312.06741)** (Matsuki et al. 2024): uses 3DGS as the SLAM representation:

1. **Tracking**: optimize the camera pose using photometric + geometric loss between the predicted image rendered from the Gaussian map and the actual image.
2. **Mapping**: according to new observations, add (densification), split (splitting), and remove (pruning) Gaussians.
3. **Loop closure**: when correcting poses, the positions of the Gaussians must be deformed together.

**NeRF-SLAM vs. 3DGS-SLAM**:

| Property | NeRF-SLAM | 3DGS-SLAM |
|------|-----------|-----------|
| Representation | MLP weights | Explicit 3D Gaussian set |
| Rendering speed | Slow (ray marching) | Fast (rasterization) |
| Training speed | Slow | Fast |
| Editability | Hard | Easy (manipulate individual Gaussians) |
| Memory | Fixed (model size) | Variable (proportional to number of Gaussians) |
| Loop closure handling | Hard (weight deformation) | Relatively easy (Gaussian transformation) |

**Current limitations**: neither method yet surpasses traditional map representations (TSDF, surfel) in accuracy and real-time performance across all situations. Especially in large-scale environments, dynamic scenes, and long-term SLAM, traditional methods remain more stable. However, in rendering quality neural representations are overwhelmingly superior, and this gap is closing rapidly.

**Recent major advances (2024-2025)**:

- **[SplaTAM](https://arxiv.org/abs/2312.02126)** (Keetha et al. CVPR 2024): tracks and maps 3D Gaussians in real time from an RGB-D camera, and with silhouette-mask-based structured map expansion achieves more than 2x improvement over prior methods in camera pose estimation and novel-view synthesis.
- **[MonoGS](https://arxiv.org/abs/2312.06741)** (Matsuki et al. CVPR 2024 Highlight): the first monocular 3DGS SLAM, performing accurate tracking, mapping, and high-quality rendering in an integrated pipeline at 3fps with only a monocular camera. It resolves the ambiguity of monocular 3D reconstruction with geometric verification and regularization.
- **[MASt3R-SLAM](https://arxiv.org/abs/2412.12392)** (Murai et al. CVPR 2025): a system that integrates a 3D reconstruction foundation model (MASt3R) into SLAM, recovering globally-consistent dense geometry at 15fps without camera-model assumptions.

```python
import numpy as np

class Gaussian3D:
    """Single 3D Gaussian representation."""
    
    def __init__(self, mean, covariance, color, opacity):
        """
        Args:
            mean: 3D position (3,)
            covariance: 3x3 covariance matrix (3, 3)
            color: RGB color (3,)
            opacity: opacity (scalar)
        """
        self.mean = np.array(mean, dtype=np.float64)
        self.covariance = np.array(covariance, dtype=np.float64)
        self.color = np.array(color, dtype=np.float64)
        self.opacity = float(opacity)
    
    def project_to_2d(self, T_world_to_cam, K):
        """
        Project a 3D Gaussian onto the image plane.
        
        Returns:
            mean_2d: projected center (2,)
            cov_2d: projected 2D covariance (2, 2)
        """
        # World -> camera coordinate transform
        R = T_world_to_cam[:3, :3]
        t = T_world_to_cam[:3, 3]
        
        mean_cam = R @ self.mean + t
        
        if mean_cam[2] <= 0:
            return None, None
        
        # 3D covariance -> camera frame
        cov_cam = R @ self.covariance @ R.T
        
        # Projection Jacobian (pinhole model)
        fx, fy = K[0, 0], K[1, 1]
        z = mean_cam[2]
        
        J = np.array([
            [fx / z, 0, -fx * mean_cam[0] / z**2],
            [0, fy / z, -fy * mean_cam[1] / z**2]
        ])
        
        # 2D covariance (EWA splatting)
        cov_2d = J @ cov_cam @ J.T
        
        # 2D center
        mean_2d = np.array([
            fx * mean_cam[0] / z + K[0, 2],
            fy * mean_cam[1] / z + K[1, 2]
        ])
        
        return mean_2d, cov_2d

def render_gaussians(gaussians, T_world_to_cam, K, image_size):
    """
    3D Gaussian Splatting rendering (simplified version).
    
    Args:
        gaussians: list of Gaussian3D
        T_world_to_cam: camera pose (4, 4)
        K: camera intrinsic parameters (3, 3)
        image_size: (H, W)
        
    Returns:
        rendered_image: (H, W, 3)
    """
    H, W = image_size
    image = np.zeros((H, W, 3), dtype=np.float64)
    accumulated_alpha = np.zeros((H, W), dtype=np.float64)
    
    # Sort by depth
    R = T_world_to_cam[:3, :3]
    t = T_world_to_cam[:3, 3]
    
    depths = []
    for g in gaussians:
        mean_cam = R @ g.mean + t
        depths.append(mean_cam[2])
    
    sorted_indices = np.argsort(depths)
    
    for idx in sorted_indices:
        g = gaussians[idx]
        mean_2d, cov_2d = g.project_to_2d(T_world_to_cam, K)
        
        if mean_2d is None:
            continue
        
        # Influence range of the 2D Gaussian (3 sigma)
        eigenvalues = np.linalg.eigvalsh(cov_2d)
        radius = int(3 * np.sqrt(max(eigenvalues))) + 1
        
        u_min = max(0, int(mean_2d[0]) - radius)
        u_max = min(W, int(mean_2d[0]) + radius + 1)
        v_min = max(0, int(mean_2d[1]) - radius)
        v_max = min(H, int(mean_2d[1]) + radius + 1)
        
        cov_2d_inv = np.linalg.inv(cov_2d + 1e-6 * np.eye(2))
        
        for v in range(v_min, v_max):
            for u in range(u_min, u_max):
                d = np.array([u - mean_2d[0], v - mean_2d[1]])
                power = -0.5 * d @ cov_2d_inv @ d
                
                if power < -4.0:  # skip pixels that are too far
                    continue
                
                alpha = g.opacity * np.exp(power)
                
                # Front-to-back alpha blending
                remaining = 1.0 - accumulated_alpha[v, u]
                if remaining < 0.01:
                    continue
                
                weight = alpha * remaining
                image[v, u] += weight * g.color
                accumulated_alpha[v, u] += weight
    
    return np.clip(image, 0, 255).astype(np.uint8)
```

---

## 11.4 Semantic Maps

A geometric map answers only the "where" of "where is what." A semantic map also answers the "what" — whether this is a wall, a door, a chair, or a person. For the robot to achieve human-level environmental understanding, semantic information is essential.

### 11.4.1 Object-Level Maps

The most intuitive semantic map recognizes the objects in an environment and registers their positions and sizes in the map.

Pipeline:
1. **2D detection/segmentation**: detect objects in camera images (YOLO, Mask R-CNN, SAM, etc.).
2. **3D lifting**: back-project 2D detections into 3D space using depth information and the camera pose.
3. **Data association**: link the same object observed across multiple frames (tracking + re-identification).
4. **Map integration**: register the 3D bounding box or shape model of the object into the map.

```python
class ObjectMap:
    """3D object map — integrate 2D detections into 3D space."""
    
    def __init__(self, association_threshold=0.5):
        self.objects = []  # [{class, center_3d, bbox_3d, observations, id}]
        self.next_id = 0
        self.assoc_thresh = association_threshold
    
    def integrate_detection(self, class_label, mask_2d, depth_image, 
                            K, T_cam_to_world):
        """
        Integrate a 2D detection into the 3D map.
        
        Args:
            class_label: object class (str)
            mask_2d: 2D segmentation mask (H, W) bool
            depth_image: depth map (H, W)
            K: camera intrinsic parameters (3, 3)
            T_cam_to_world: camera -> world transform (4, 4)
        """
        # 1. Recover 3D points of the masked region
        points_3d = self._backproject(mask_2d, depth_image, K, T_cam_to_world)
        
        if len(points_3d) < 10:
            return
        
        center = np.mean(points_3d, axis=0)
        bbox_min = np.min(points_3d, axis=0)
        bbox_max = np.max(points_3d, axis=0)
        
        # 2. Data association: match against existing objects
        best_match = None
        best_dist = float('inf')
        
        for obj in self.objects:
            if obj['class'] != class_label:
                continue
            dist = np.linalg.norm(obj['center_3d'] - center)
            if dist < best_dist and dist < self.assoc_thresh:
                best_dist = dist
                best_match = obj
        
        if best_match is not None:
            # Update existing object (weighted average)
            n = best_match['observations']
            best_match['center_3d'] = (
                (n * best_match['center_3d'] + center) / (n + 1)
            )
            best_match['observations'] = n + 1
        else:
            # Register a new object
            self.objects.append({
                'class': class_label,
                'center_3d': center,
                'bbox_3d': (bbox_min, bbox_max),
                'observations': 1,
                'id': self.next_id
            })
            self.next_id += 1
    
    def _backproject(self, mask, depth, K, T):
        """Back-project 2D pixels of the masked region to 3D world coordinates."""
        ys, xs = np.where(mask & (depth > 0))
        
        fx, fy = K[0, 0], K[1, 1]
        cx, cy = K[0, 2], K[1, 2]
        
        z = depth[ys, xs]
        x = (xs - cx) * z / fx
        y = (ys - cy) * z / fy
        
        points_cam = np.stack([x, y, z, np.ones_like(z)], axis=1)  # (N, 4)
        points_world = (T @ points_cam.T).T[:, :3]  # (N, 3)
        
        return points_world
```

### 11.4.2 3D Scene Graph: Hydra & S-Graphs

An object-level map only recognizes individual objects. But humans understand environments as hierarchical relations: "this room has a table, on top of which is a cup, and this room is a living room." A **3D Scene Graph** is such a hierarchical, relational environmental representation.

**[Hydra](https://arxiv.org/abs/2201.13360)** (Hughes et al. 2022) is the first system to build a 3D scene graph incrementally in real time. It consists of five layers:

| Layer | Contents | Construction method |
|-------|------|-----------|
| 1 | Metric-Semantic 3D Mesh | TSDF integration + semantic segmentation |
| 2 | Objects & Agents | 3D bounding box detection |
| 3 | Places (topological) | Extracted from GVD (Generalized Voronoi Diagram) |
| 4 | Rooms | Community detection on the place graph |
| 5 | Buildings | Higher-level grouping of rooms |

**Construction of the Places layer**: this layer is the most original part of Hydra.

1. Compute the ESDF from the TSDF.
2. Incrementally extract the GVD from the ESDF. Vertices of the GVD are points maximally far from obstacles — i.e., good points for the robot to pass through.
3. Configure GVD vertices as place nodes and GVD edges as connections between places.
4. This place graph is a topological map that can be used directly for path planning.

**Room detection**: a method for detecting rooms from the place graph:

1. Weight the edges between place nodes according to their proximity to obstacles.
2. At narrow passages such as doorways, weights become high (indicating difficulty of passage).
3. Group places into rooms using a community detection algorithm (e.g., dilation-based).

**Hierarchical loop closure**: Hydra leverages the hierarchical structure of the scene graph to improve the quality of loop closure:

- **Top-down**: first find candidates at higher layers (room, place).
- **Bottom-up**: perform geometric verification at lower layers (visual feature, object) (TEASER++ based).
- This hierarchical approach detects more and more accurate loop closures than a simple BoW approach.

**S-Graphs** (Situational Graphs): a hierarchical scene graph similar to Hydra, but directly incorporating hierarchical information into factor graph optimization. Structural elements such as rooms, walls, and floors are added as variables of the factor graph to improve SLAM accuracy.

### 11.4.3 Open-Vocabulary Semantic Mapping

Traditional semantic mapping operates only over a predefined class set (e.g., COCO's 80 classes). **Open-vocabulary semantic mapping** enables searching the map with arbitrary text queries.

Core techniques:

1. **CLIP/DINO feature embedding**: extract features from a vision-language model for each observation (image or image patch).
2. **Feature -> 3D map**: store the extracted features at the corresponding 3D location (attached to a point cloud or voxel).
3. **Text query**: when the user queries, say, "red fire extinguisher," encode the text with the CLIP text encoder, compute similarity against the map's visual features, and return the corresponding location.

Impact of this approach: the robot can recognize and locate objects it was not trained on in advance. It works even for "things never seen before." This is essential in unpredictable environments such as household robots and exploration robots.

```python
class OpenVocabSemanticMap:
    """
    Core idea of open-vocabulary semantic mapping.
    Attach CLIP features to each 3D point.
    """
    
    def __init__(self, feature_dim=512):
        self.points_3d = []     # (N, 3)
        self.features = []      # (N, D) CLIP visual features
        self.feature_dim = feature_dim
    
    def add_observation(self, points_3d, clip_features):
        """
        Add 3D points and corresponding CLIP features to the map.
        
        Args:
            points_3d: (M, 3) 3D point coordinates
            clip_features: (M, D) CLIP visual feature of each point
        """
        self.points_3d.extend(points_3d)
        self.features.extend(clip_features)
    
    def query(self, text_feature, top_k=10):
        """
        Search the map for relevant locations given a text query.
        
        Args:
            text_feature: (D,) CLIP text feature
            top_k: number of top results to return
            
        Returns:
            results: list of (point_3d, similarity)
        """
        if len(self.features) == 0:
            return []
        
        features_array = np.array(self.features)
        points_array = np.array(self.points_3d)
        
        # Cosine similarity
        text_norm = text_feature / (np.linalg.norm(text_feature) + 1e-8)
        feat_norms = features_array / (
            np.linalg.norm(features_array, axis=1, keepdims=True) + 1e-8
        )
        
        similarities = feat_norms @ text_norm
        
        top_indices = np.argsort(similarities)[::-1][:top_k]
        
        return [
            (points_array[idx], similarities[idx]) 
            for idx in top_indices
        ]
    
    def visualize_heatmap(self, text_feature):
        """Generate a relevance heatmap for a text query."""
        if len(self.features) == 0:
            return np.array([]), np.array([])
        
        features_array = np.array(self.features)
        points_array = np.array(self.points_3d)
        
        text_norm = text_feature / (np.linalg.norm(text_feature) + 1e-8)
        feat_norms = features_array / (
            np.linalg.norm(features_array, axis=1, keepdims=True) + 1e-8
        )
        
        similarities = feat_norms @ text_norm
        
        # Normalize to 0-1
        sim_min, sim_max = similarities.min(), similarities.max()
        if sim_max > sim_min:
            heatmap = (similarities - sim_min) / (sim_max - sim_min)
        else:
            heatmap = np.zeros_like(similarities)
        
        return points_array, heatmap
```

---

## 11.5 Long-Term & Dynamic Maps

Real environments are not static. Vehicles move, furniture is rearranged, and buildings are newly constructed. Robots operating over long periods must adapt to such changes.

### 11.5.1 Change Detection

Identify changes by comparing the map with current observations.

**Geometric change detection**: compare expected observations from the past map with current sensor observations.

1. **Free-to-occupied**: if a new obstacle is detected in a region marked free on the map -> a new object has been added.
2. **Occupied-to-free**: if a region marked occupied on the map is passed through in the current observation -> an existing object has been removed.

```python
def detect_changes(occupancy_map, current_scan, robot_pose, threshold=0.3):
    """
    Detect changes by comparing the existing map with the current scan.
    
    Returns:
        new_objects: locations of newly appeared obstacles
        removed_objects: locations of vanished obstacles
    """
    new_objects = []
    removed_objects = []
    
    for point in current_scan:
        # Map coordinate of the current scan point
        map_value = occupancy_map.get_probability(point)
        
        if map_value < threshold:
            # Map says free but an obstacle is currently detected -> new object
            new_objects.append(point)
    
    # Detect regions that were occupied on the map but are currently passed through
    for ray_endpoint in current_scan:
        ray_cells = trace_ray(robot_pose, ray_endpoint)
        for cell in ray_cells[:-1]:  # exclude endpoint (may still be occupied)
            map_value = occupancy_map.get_probability(cell)
            if map_value > (1.0 - threshold):
                # Map says occupied but current ray passes through -> object removed
                removed_objects.append(cell)
    
    return new_objects, removed_objects
```

**Semantic change detection**: detect not only geometric changes but also changes in an object's class or state. For example, "chair moved" or "door opened/closed."

### 11.5.2 Map Maintenance: Retention Strategies

In long-term operation, we cannot store all observations of the map indefinitely. Which information do we keep and which do we discard?

**Strategy 1: Recency weighting**: assign higher weights to recent observations and gradually decay the influence of older ones. A scheme that decays the TSDF weight over time.

**Strategy 2: Semi-static classification**: classify each element of the environment as static, semi-static, or dynamic:
- **Static**: walls, floors, buildings -> permanently preserved.
- **Semi-static**: furniture, parked vehicles -> periodically updated.
- **Dynamic**: pedestrians, moving vehicles -> removed from the map.

**Strategy 3: Multi-experience mapping**: store multiple "experiences" of the same place. Maintain several versions of the map under different lighting, season, and furniture arrangement, and select the experience that best matches the current observation.

### 11.5.3 Handling Dynamic Objects

In SLAM, dynamic objects (moving people, vehicles) cause two problems:

1. **Tracking errors**: if feature points of dynamic objects are mistakenly used as those of the static environment, pose estimation breaks.
2. **Map contamination**: if dynamic objects are recorded in the map, they become "ghost obstacles."

**Countermeasures**:

1. **Semantic filtering**: identify dynamic object classes (person, vehicle) via semantic classification and exclude those observations from the SLAM pipeline.

2. **Geometric consistency check**: classify as dynamic the observations that are inconsistent across multiple frames (points visible in one frame but disappearing in the next).

3. **Background subtraction**: classify as dynamic the voxels in the TSDF that show a free-to-occupied-to-free pattern.

4. **SLAM with dynamic object tracking**: instead of simply ignoring dynamic objects, track them with separate state variables. This allows estimating the trajectories of dynamic objects as well (SLAM + MOT).

```python
class DynamicMapManager:
    """
    Dynamic environment map manager.
    Classifies observations as static/semi-static/dynamic for differential handling.
    """
    
    def __init__(self):
        self.static_map = {}       # permanent storage
        self.semistatic_map = {}   # periodic update
        self.dynamic_objects = []  # dynamic objects being tracked
        
        # Dynamic class definitions
        self.dynamic_classes = {'person', 'car', 'bicycle', 'dog'}
        self.semistatic_classes = {'chair', 'box', 'parked_car'}
    
    def process_observation(self, point_3d, class_label, timestamp):
        """
        Classify a new observation and integrate it into the appropriate map.
        """
        if class_label in self.dynamic_classes:
            self._track_dynamic(point_3d, class_label, timestamp)
        elif class_label in self.semistatic_classes:
            self._update_semistatic(point_3d, class_label, timestamp)
        else:
            self._update_static(point_3d, class_label)
    
    def _track_dynamic(self, point, cls, timestamp):
        """Track a dynamic object. Do not add to the map."""
        # Match against existing dynamic objects via data association
        matched = False
        for obj in self.dynamic_objects:
            if (obj['class'] == cls and 
                np.linalg.norm(obj['last_position'] - point) < 2.0):
                obj['trajectory'].append((timestamp, point.copy()))
                obj['last_position'] = point.copy()
                matched = True
                break
        
        if not matched:
            self.dynamic_objects.append({
                'class': cls,
                'last_position': point.copy(),
                'trajectory': [(timestamp, point.copy())]
            })
    
    def _update_semistatic(self, point, cls, timestamp):
        """Update a semi-static object."""
        key = self._spatial_key(point)
        self.semistatic_map[key] = {
            'position': point.copy(),
            'class': cls,
            'last_seen': timestamp
        }
    
    def _update_static(self, point, cls):
        """Update the static map."""
        key = self._spatial_key(point)
        self.static_map[key] = {
            'position': point.copy(),
            'class': cls
        }
    
    def cleanup_old_semistatic(self, current_time, max_age=3600):
        """Remove old semi-static observations (e.g., 1 hour)."""
        keys_to_remove = [
            k for k, v in self.semistatic_map.items()
            if current_time - v['last_seen'] > max_age
        ]
        for k in keys_to_remove:
            del self.semistatic_map[k]
    
    def _spatial_key(self, point, resolution=0.1):
        """Convert a 3D point to a discrete grid key."""
        return (
            int(point[0] / resolution),
            int(point[1] / resolution),
            int(point[2] / resolution)
        )
```

---

In this chapter we surveyed the full spectrum of spatial representations, from point-based representations (OctoMap) through continuous surfaces (TSDF/Mesh), neural representations (NeRF/3DGS), and semantic hierarchies (Scene Graph). Each representation is suited to different downstream tasks, and modern systems trend toward combining them hierarchically. In the next chapter we examine how the algorithms and representations covered so far are integrated in **real platforms** (autonomous driving, drones, handheld) and how performance is evaluated.

---

# Ch.12 — Practical Systems & Benchmarks

In Ch.2-11 we covered individual algorithms ranging from sensor modeling and state estimation to odometry, place recognition, and spatial representation. In this chapter we broaden our view from algorithms to **systems** and examine how these techniques are combined on real platforms.

Having covered the theory and algorithms of sensor fusion, we now examine how they are integrated in real systems. We analyze sensor fusion architectures on three representative platforms — autonomous driving, drones, and handheld mapping — and introduce benchmarks and tools for evaluating these systems.

---

## 12.1 Autonomous Driving Perception Stack

Autonomous driving is the domain where sensor fusion is applied most aggressively. Because safety requirements are a matter of life and death, the system must remain operational even under single-sensor failures (redundancy). Combining multiple sensors is not optional — it is mandatory.

### 12.1.1 Sensor Suite Configuration Examples

**Waymo** (5th generation):
- 1 × long-range LiDAR (360°, up to 300 m)
- 4 × short-range LiDAR (covering near-field blind spots)
- 29 × cameras (360° coverage, varied fields of view)
- 6 × radar (long-range velocity measurement)
- IMU, GNSS, wheel encoder

**Typical configuration based on the [nuScenes](https://arxiv.org/abs/1903.11027) dataset**:
- 1 × spinning LiDAR (32 or 64 channels)
- 6 × surround cameras (360° coverage)
- 5 × radar
- IMU, GNSS

**Tesla (Vision-only approach)**:
- 360° coverage using only 8 × cameras (LiDAR/radar removed)
- 3D perception via neural-network-based depth estimation
- An extremely aggressive approach that remains controversial in the industry

### 12.1.2 Production-Level Fusion Pipeline

A typical sensor fusion pipeline in a production autonomous driving system:

```
Sensor synchronization (HW trigger + PTP)
    ↓
[LiDAR processing]     [Camera processing]   [Radar processing]
- Motion compensation  - Object detection    - Doppler velocity
- Ground segmentation  - Semantic seg        - Long-range detection
- 3D object detection  - Lane detection
    ↓                      ↓                     ↓
         Late Fusion / Deep Fusion
    ↓
[Tracking & Prediction]
- Multi-object tracking (Kalman/JPDA)
- Trajectory prediction
    ↓
[Localization]
- HD map matching
- GNSS/IMU/LiDAR integration
    ↓
[Planning & Control]
```

**Late Fusion vs Deep Fusion**:

- **Late fusion (traditional)**: Each sensor independently detects 3D bounding boxes, which are then combined via NMS (Non-Maximum Suppression). Advantages: modularity, ease of debugging. Disadvantages: difficult to exploit cross-sensor complementarity.

- **Deep fusion (modern)**: Features from multiple sensors are combined directly in BEV (Bird's Eye View) space. Representative systems include [BEVFusion](https://arxiv.org/abs/2205.13542) (MIT/Nvidia) and TransFusion. Advantages: the network learns to exploit complementary cross-sensor information. Disadvantages: end-to-end training requires large-scale labeled data.

```python
# BEV Fusion conceptual diagram (pseudo-code)

def bev_fusion_pipeline(lidar_points, camera_images, calibrations):
    """
    Fusion pipeline that combines LiDAR and camera features in BEV space.
    """
    # 1. LiDAR → BEV feature
    # Voxelize the LiDAR points, process them with a 3D backbone, and
    # collapse the z axis to produce a BEV feature map
    lidar_voxels = voxelize(lidar_points, voxel_size=0.1)
    lidar_3d_features = sparse_3d_cnn(lidar_voxels)  # (X, Y, Z, C)
    lidar_bev = lidar_3d_features.max(dim='z')        # (X, Y, C)
    
    # 2. Camera → BEV feature
    # Extract features from each camera image and transform them
    # into BEV space via depth estimation
    camera_features = []
    for img, calib in zip(camera_images, calibrations):
        feat_2d = image_backbone(img)  # (H', W', C)
        depth_dist = depth_net(feat_2d)  # (H', W', D) — depth probability distribution
        
        # Lift: 2D feature to 3D (LSS scheme)
        feat_3d = outer_product(feat_2d, depth_dist)  # (H', W', D, C)
        
        # Splat: aggregate 3D features into BEV pillars
        feat_bev = splat_to_bev(feat_3d, calib)  # (X, Y, C)
        camera_features.append(feat_bev)
    
    camera_bev = sum(camera_features)  # sum BEV features across all cameras
    
    # 3. Fusion: concatenate or attention in BEV space
    fused_bev = concat_and_conv(lidar_bev, camera_bev)  # (X, Y, C')
    
    # 4. Detection head
    detections = detection_head(fused_bev)  # 3D bounding boxes
    
    return detections
```

### 12.1.3 Localization Stack

Autonomous driving localization is typically organized in two stages:

1. **Global localization**: GNSS (RTK or PPP) provides an initial position on the map. In urban environments, multipath can cause errors of several meters, so GNSS alone is insufficient.

2. **Map-relative localization**: The current LiDAR scan is registered to an HD map (a pre-built LiDAR point cloud map) using NDT/ICP, achieving cm-level accuracy. This also works where GNSS is blocked, such as in tunnels.

Factor-graph-based integration:
$$\mathbf{x}^* = \arg\min \underbrace{f_{\text{IMU}}}_{\text{prediction}} + \underbrace{f_{\text{LiDAR}}}_{\text{map matching}} + \underbrace{f_{\text{GNSS}}}_{\text{global anchor}} + \underbrace{f_{\text{wheel}}}_{\text{velocity}}$$

---

## 12.2 Drones/UAVs

Sensor fusion for drones operates under substantially different constraints than autonomous driving.

### 12.2.1 Visual-Inertial-Centric Systems

The most common sensor combination on drones is **camera + IMU**. Reasons:

- **Weight/size constraints**: Small drones cannot easily carry a LiDAR (though this is changing with compact solid-state LiDARs such as the Livox Mid-360).
- **Power constraints**: Cameras and IMUs consume little power.
- **Vibration**: Propeller vibration adds noise to IMU data. Vibration-isolating mounts and software filtering are required.

Representative VIO systems for drones:
- **VINS-Mono/Fusion**: tightly-coupled optimization-based. Can be integrated with PX4.
- **MSCKF/OpenVINS**: filter-based. Low computational cost makes it suitable for embedded boards.
- **Basalt**: visual-inertial mapping with non-linear factor recovery.

### 12.2.2 GPS-Denied Navigation

A core challenge for drones is autonomous flight in GPS-denied environments — indoors, tunnels, under forest canopy, and in electronic-warfare conditions.

**Solution approaches**:

1. **VIO alone**: Stable in the short term but accumulates drift. Suitable for missions of a few minutes.
2. **VIO + terrain matching**: Match current camera observations against a pre-built terrain/building map. A prior map is required.
3. **VIO + UWB**: Install UWB anchors in the environment and use ranging measurements to correct drift. Prior infrastructure is required.
4. **VIO + barometer**: Use a barometer as an auxiliary sensor for altitude estimation, correcting VIO's z-axis drift.

### 12.2.3 Real-Time Constraints

Drones perform high-speed flight (5-15 m/s) and aggressive attitude changes (flips, sharp turns). The corresponding sensor fusion requirements:

- **IMU rate**: 200-1000 Hz. Must adequately capture pose changes during high-speed motion.
- **Camera exposure**: Short exposure times are needed to reduce motion blur, which trades off against increased noise in low light.
- **Processing latency**: State estimation results must be delivered to the controller within 30 ms. Longer delays cause control instability.
- **Point-LIO**: An ultra-low-latency LIO that processes points individually without waiting for scan completion. Especially advantageous for high-agility drone maneuvers.

```python
class DroneVIOConfig:
    """Example VIO system configuration for drones."""
    
    # Sensor configuration
    camera_fps = 30
    camera_resolution = (640, 480)  # low resolution to reduce compute
    imu_rate = 400  # Hz
    
    # VIO parameters
    max_features = 150  # cap on number of features (compute)
    keyframe_interval = 5  # one keyframe every 5 frames
    sliding_window_size = 10  # optimization window size
    
    # Drone-specific settings
    gravity_magnitude = 9.81
    max_angular_velocity = 10.0  # rad/s — handle aggressive rotations
    motion_blur_threshold = 0.3  # drop heavily blurred frames
    
    # IMU noise (large values since drones vibrate heavily)
    gyro_noise_density = 0.004  # rad/s/sqrt(Hz)
    accel_noise_density = 0.05  # m/s^2/sqrt(Hz)
    gyro_random_walk = 0.0002   # rad/s^2/sqrt(Hz)
    accel_random_walk = 0.003   # m/s^3/sqrt(Hz)
    
    # Safety
    max_allowed_drift_m = 0.5   # warn if drift exceeds this value
    min_tracked_features = 20   # warn about tracking quality if below

def check_image_quality(image, angular_velocity, exposure_time):
    """
    Drone camera image quality check.
    Exclude heavily motion-blurred frames from VIO.
    """
    # Motion-blur estimate: angular velocity × exposure time × focal length
    blur_pixels = abs(angular_velocity) * exposure_time * 300  # approximate focal length
    
    if blur_pixels > 5.0:  # blur exceeds 5 pixels
        return False, f"Excessive motion blur: {blur_pixels:.1f} pixels"
    
    # Brightness check
    mean_brightness = image.mean()
    if mean_brightness < 20:
        return False, f"Too dark: mean={mean_brightness:.0f}"
    if mean_brightness > 240:
        return False, f"Too bright: mean={mean_brightness:.0f}"
    
    return True, "OK"
```

---

## 12.3 Handheld/Backpack Mapping

Mapping environments with handheld or backpack-mounted sensors is widely used in surveying, BIM (Building Information Modeling), and digital twin construction.

### 12.3.1 SLAM as a Service

Examples of commercial handheld mapping devices:

- **Leica BLK2GO**: Handheld LiDAR scanner. Performs real-time SLAM by fusing LiDAR + IMU + camera. Survey-grade accuracy.
- **NavVis VLX**: Backpack-mounted. Four cameras + LiDAR. Specialized for indoor mapping.
- **GeoSLAM ZEB**: Handheld mobile mapping. A 2D LiDAR is rotated manually to produce 3D scans.

Common pipeline across these devices:

```
LiDAR + IMU → LIO (FAST-LIO2 or similar)
     ↓
Loop closure (Scan Context, etc.)
     ↓
Global optimization (iSAM2)
     ↓
Dense colorized point cloud (camera color overlaid)
     ↓
Post-processing (cloud cleanup, mesh generation)
```

### 12.3.2 Survey-Grade Mapping

Key requirements for survey-grade mapping:

- **Absolute accuracy**: Within a few cm relative to GNSS. This is achieved by placing GCPs (Ground Control Points) and aligning in post-processing.
- **Relative accuracy**: Internal consistency of the map. Loop closure and global optimization are essential.
- **Point density**: Density of at least 1 cm spacing on walls, with downsampling in post-processing.
- **Color quality**: Accurate HDR color mapping. Camera-LiDAR time synchronization and extrinsic calibration must be precise.

Practical issues in sensor fusion:

1. **Degenerate environments**: Long corridors, empty rooms, and other environments lacking geometric features. Drift that occurs in LiDAR-only systems is compensated by cameras or IMU. Multi-modal systems such as R3LIVE and FAST-LIVO2 are effective.

2. **Multi-story buildings**: Loop closure is essential when moving between floors via elevators or stairs. With no GNSS, z-axis drift is especially problematic. A barometer serves as a useful auxiliary sensor.

3. **Glass/mirrors**: LiDAR beams either pass through or reflect. Compensate with cameras or filter out reflected points.

---

## 12.4 Benchmarks & Evaluation

Fair comparison of sensor fusion systems requires standardized datasets and evaluation metrics.

### 12.4.1 Major Datasets

| Dataset | Year | Environment | Sensors | Features |
|---------|------|-------------|---------|----------|
| **[KITTI](https://doi.org/10.1177/0278364913491297)** | 2012 | Outdoor (autonomous driving) | Stereo, LiDAR, GPS/IMU | The original SLAM/VO benchmark. 11 training + 11 test sequences |
| **[EuRoC](https://doi.org/10.1177/0278364915620033)** | 2016 | Indoor (MAV) | Stereo, IMU | The standard VIO benchmark. Machine Hall + Vicon Room |
| **[TUM-RGBD](https://doi.org/10.1109/IROS.2012.6385773)** | 2012 | Indoor | RGB-D | Canonical visual SLAM benchmark. Kinect v1 |
| **TUM-VI** | 2018 | Indoor + outdoor | Stereo, IMU | VIO benchmark. Diverse motion patterns |
| **[Hilti](https://arxiv.org/abs/2109.11316)** | 2021- | Construction sites | LiDAR, Camera, IMU | Industrial-environment-specific. Challenging conditions |
| **[HeLiPR](https://arxiv.org/abs/2309.14590)** | 2023 | Outdoor (urban) | Heterogeneous LiDAR, Camera, IMU, GNSS | For heterogeneous LiDAR fusion research. Ouster+Velodyne+Livox+Aeva |
| **[nuScenes](https://arxiv.org/abs/1903.11027)** | 2020 | Outdoor (autonomous driving) | Camera, LiDAR, Radar, GPS/IMU | 1000 scenes, 23-class 3D annotations, 360° surround sensors |
| **Newer College** | 2020 | Outdoor + indoor | LiDAR, Camera, IMU | Oxford University campus. Multi-session |

Characteristics and uses of each dataset:

**KITTI** — Released in 2012 and therefore long in the tooth, it nonetheless remains the standard benchmark for autonomous driving SLAM. It provides a Velodyne 64-channel LiDAR, stereo cameras, and GPS/IMU. Limitations: the sensors are dated, sequences are relatively short, and ground truth is GPS/INS-based, so it may not be accurate at the cm level.

**EuRoC** — Stereo camera + IMU data captured on a drone (MAV). It is the de facto standard benchmark for VIO systems. Ground truth is provided by Vicon motion capture (sub-mm accuracy) or a Leica laser tracker (mm accuracy). The 11 sequences are classified as easy → medium → difficult.

**Hilti** — Evaluates SLAM in the challenging environment of construction sites (dust, vibration, repetitive structure). Since 2021, an annual SLAM challenge has been held that exposes the limits of state-of-the-art systems.

**HeLiPR** — A recent dataset released in 2023. Its defining feature is that different types of LiDAR (spinning, solid-state, FMCW) are mounted simultaneously. It supports the emerging research direction of heterogeneous LiDAR fusion.

**Newer College** — Collected by visiting the Oxford University campus multiple times, this dataset is well suited to multi-session SLAM and long-term mapping research. Captured with a handheld LiDAR, it contains challenging motion patterns.

**Recent benchmark trends (2024-2025)**:

- **[Hilti-Oxford](https://arxiv.org/abs/2208.09825)** (2022): A construction-environment SLAM benchmark that provides mm-level-accurate ground truth. An annual SLAM challenge has been held since 2022.
- **[Boreas](https://arxiv.org/abs/2203.10168)** (Burnett et al. 2023): An autonomous driving dataset collected by repeatedly driving the same route over a year. It includes LiDAR, radar, and cameras and captures all four seasons as well as adverse weather conditions.
- **[Snail-Radar](https://arxiv.org/abs/2407.11705)** (Huai et al., IJRR 2025): A large-scale benchmark for evaluating 4D radar SLAM that systematically compares 4D radar odometry/SLAM across diverse environments and platforms.

### 12.4.2 Evaluation Metrics

**ATE (Absolute Trajectory Error)**: measures the global difference between the estimated trajectory and the ground truth trajectory.

$$\text{ATE} = \sqrt{\frac{1}{N} \sum_{i=1}^{N} \| \text{trans}(\mathbf{T}_{\text{gt},i}^{-1} \cdot \mathbf{T}_{\text{est},i}) \|^2}$$

Before evaluation, the two trajectories must be aligned in Sim(3) or SE(3). Monocular VO has scale ambiguity, so Sim(3) is used; for stereo/LiDAR, SE(3) is used.

$$\mathbf{S}^* = \arg\min_{\mathbf{S} \in \text{Sim}(3)} \sum_i \| \mathbf{p}_{\text{gt},i} - \mathbf{S} \cdot \mathbf{p}_{\text{est},i} \|^2$$

This alignment admits a closed-form solution via the Umeyama algorithm.

**RPE (Relative Pose Error)**: measures relative accuracy over short intervals. It reflects drift tendency.

$\Delta$ is the evaluation interval (in frames or distance). RPE at short $\Delta$ reflects odometry accuracy, while RPE at long $\Delta$ reflects drift.

**Place recognition metrics**:
- **Recall@N**: the fraction of queries for which the correct place is included among the top-N candidates. Recall@1 is the strictest.
- **Precision-Recall curve**: the precision vs recall trade-off as a function of threshold.
- **AUC**: the area under the PR curve.

```python
import numpy as np
from scipy.spatial.transform import Rotation

def compute_ate(poses_gt, poses_est, align='se3'):
    """
    Compute Absolute Trajectory Error.
    
    Args:
        poses_gt: list of ground truth poses [(4, 4), ...]
        poses_est: list of estimated poses [(4, 4), ...]
        align: 'se3' or 'sim3'
        
    Returns:
        ate_rmse: ATE RMSE (meters)
        ate_per_frame: per-frame ATE (N,)
    """
    positions_gt = np.array([T[:3, 3] for T in poses_gt])  # (N, 3)
    positions_est = np.array([T[:3, 3] for T in poses_est])  # (N, 3)
    
    # Umeyama alignment
    if align == 'sim3':
        S, R, t = umeyama_alignment(positions_est, positions_gt, 
                                     with_scale=True)
        positions_aligned = S * (R @ positions_est.T).T + t
    else:  # se3
        _, R, t = umeyama_alignment(positions_est, positions_gt, 
                                     with_scale=False)
        positions_aligned = (R @ positions_est.T).T + t
    
    # Per-frame error
    errors = np.linalg.norm(positions_gt - positions_aligned, axis=1)
    
    ate_rmse = np.sqrt(np.mean(errors ** 2))
    
    return ate_rmse, errors

def compute_rpe(poses_gt, poses_est, delta=10):
    """
    Compute Relative Pose Error.
    
    Args:
        poses_gt: list of ground truth poses
        poses_est: list of estimated poses
        delta: evaluation interval (in frames)
        
    Returns:
        rpe_trans: RPE translation RMSE (meters)
        rpe_rot: RPE rotation RMSE (degrees)
    """
    trans_errors = []
    rot_errors = []
    
    for i in range(len(poses_gt) - delta):
        # Ground truth relative transform
        T_gt_rel = np.linalg.inv(poses_gt[i]) @ poses_gt[i + delta]
        
        # Estimated relative transform
        T_est_rel = np.linalg.inv(poses_est[i]) @ poses_est[i + delta]
        
        # Error
        T_error = np.linalg.inv(T_gt_rel) @ T_est_rel
        
        # Translation error
        trans_err = np.linalg.norm(T_error[:3, 3])
        trans_errors.append(trans_err)
        
        # Rotation error
        rot = Rotation.from_matrix(T_error[:3, :3])
        rot_err = np.linalg.norm(rot.as_rotvec()) * 180 / np.pi  # in degrees
        rot_errors.append(rot_err)
    
    rpe_trans = np.sqrt(np.mean(np.array(trans_errors) ** 2))
    rpe_rot = np.sqrt(np.mean(np.array(rot_errors) ** 2))
    
    return rpe_trans, rpe_rot

def umeyama_alignment(source, target, with_scale=True):
    """
    Umeyama alignment: compute the optimal similarity/rigid
    transform that aligns source to target.
    
    Args:
        source: (N, 3) source points
        target: (N, 3) target points
        with_scale: True for Sim(3), False for SE(3)
        
    Returns:
        scale: scale (1.0 if with_scale=False)
        rotation: (3, 3) rotation matrix
        translation: (3,) translation vector
    """
    n = source.shape[0]
    
    # Recenter
    mu_source = np.mean(source, axis=0)
    mu_target = np.mean(target, axis=0)
    
    source_centered = source - mu_source
    target_centered = target - mu_target
    
    # Covariance matrix
    sigma_source = np.sum(source_centered ** 2) / n
    H = (target_centered.T @ source_centered) / n
    
    # SVD
    U, D, Vt = np.linalg.svd(H)
    
    # Reflection correction
    d = np.linalg.det(U) * np.linalg.det(Vt)
    S = np.diag([1, 1, np.sign(d)])
    
    rotation = U @ S @ Vt
    
    if with_scale:
        scale = np.trace(np.diag(D) @ S) / sigma_source
    else:
        scale = 1.0
    
    translation = mu_target - scale * rotation @ mu_source
    
    return scale, rotation, translation
```

### 12.4.3 Difficulties of Fair Comparison

Caveats when interpreting benchmark results:

1. **Parameter tuning**: The same algorithm can perform very differently depending on its parameters. Tuning for a specific dataset reduces generality.

2. **Hardware dependence**: Real-time performance depends heavily on hardware. The definition of "real-time" varies across papers (desktop GPU vs embedded ARM).

3. **Completeness**: Some systems experience tracking loss on difficult sequences; computing ATE only over successful segments fails to reflect the failure rate. **Completeness** (= the fraction of sequences that succeed) should be reported alongside.

4. **Initialization differences**: Different initialization methods and times in VIO systems can yield different results on the same sequence.

5. **Whether loop closure is included**: VO (no loop closure) vs SLAM (with loop closure) must be distinguished. Loop closure can dramatically improve ATE.

---

## 12.5 Open-Source Tool Guide

We summarize open-source tools commonly used in sensor fusion research and practice.

### 12.5.1 Optimization Libraries

**GTSAM** (Georgia Tech Smoothing and Mapping):
- Factor-graph-based optimization library
- Includes an iSAM2 implementation
- C++ with Python bindings (gtsam)
- Backend of many SLAM systems including LIO-SAM
- Strengths: intuitive factor graph construction, many built-in factor types

```python
# GTSAM Python usage example (conceptual code)

import gtsam
import numpy as np

def simple_pose_graph_gtsam():
    """Simple pose graph optimization with GTSAM."""
    
    # Create the factor graph
    graph = gtsam.NonlinearFactorGraph()
    
    # Initial values
    initial = gtsam.Values()
    
    # Prior factor: fix the first pose
    prior_noise = gtsam.noiseModel.Diagonal.Sigmas(
        np.array([0.01, 0.01, 0.01, 0.01, 0.01, 0.01])  # (rx, ry, rz, tx, ty, tz)
    )
    graph.add(gtsam.PriorFactorPose3(
        0, gtsam.Pose3(), prior_noise
    ))
    initial.insert(0, gtsam.Pose3())
    
    # Odometry factors
    odom_noise = gtsam.noiseModel.Diagonal.Sigmas(
        np.array([0.05, 0.05, 0.05, 0.1, 0.1, 0.1])
    )
    
    # Pose 1: 1 m forward
    T_01 = gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(1.0, 0.0, 0.0))
    graph.add(gtsam.BetweenFactorPose3(0, 1, T_01, odom_noise))
    initial.insert(1, gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(1.0, 0.1, 0.0)))
    
    # Pose 2: 90-degree left turn + 1 m forward
    T_12 = gtsam.Pose3(
        gtsam.Rot3.Rz(np.pi / 2), 
        gtsam.Point3(1.0, 0.0, 0.0)
    )
    graph.add(gtsam.BetweenFactorPose3(1, 2, T_12, odom_noise))
    initial.insert(2, gtsam.Pose3(
        gtsam.Rot3.Rz(np.pi / 2), 
        gtsam.Point3(1.0, 1.1, 0.0)
    ))
    
    # Loop closure: pose 2 → pose 0
    loop_noise = gtsam.noiseModel.Diagonal.Sigmas(
        np.array([0.1, 0.1, 0.1, 0.2, 0.2, 0.2])
    )
    T_20 = gtsam.Pose3(
        gtsam.Rot3.Rz(np.pi / 2),
        gtsam.Point3(0.0, -1.0, 0.0)
    )
    graph.add(gtsam.BetweenFactorPose3(2, 0, T_20, loop_noise))
    
    # Optimize with iSAM2
    params = gtsam.ISAM2Params()
    isam = gtsam.ISAM2(params)
    isam.update(graph, initial)
    result = isam.calculateEstimate()
    
    # Print results
    for i in range(3):
        pose = result.atPose3(i)
        print(f"Pose {i}: x={pose.x():.3f}, y={pose.y():.3f}, "
              f"z={pose.z():.3f}")
    
    return result
```

**Ceres Solver**:
- A nonlinear least squares optimization library developed by Google
- C++ only (Python bindings are limited)
- Automatic differentiation support is a core strength
- Used by VINS-Mono, ORB-SLAM, and others
- Defines the optimization problem directly, without a factor graph abstraction

**g2o** (General Graph Optimization):
- Developed by Kümmerle et al. (2011)
- C++ library specialized for graph optimization
- Predefines various vertex/edge types (SE2, SE3, Sim3, etc.)
- Lighter and faster than GTSAM but less flexible

Comparison of the three libraries:

| Property | GTSAM | Ceres | g2o |
|----------|-------|-------|-----|
| Abstraction level | Factor graph | Cost function | Graph vertex/edge |
| Automatic differentiation | Partial | Full | None |
| Incremental | iSAM2 | Unsupported | Unsupported |
| Python support | Good | Limited | Limited |
| Representative use | LIO-SAM | VINS-Mono | ORB-SLAM |

### 12.5.2 Calibration Tools

**Kalibr** (ethz-asl):
- Camera-IMU, Camera-Camera, and multi-IMU calibration
- Continuous-time B-spline trajectory-based
- Uses an AprilGrid target
- The de facto standard, though installation is tricky (ROS dependency)

**OpenCalib** (2023):
- Unified calibration across the full autonomous driving sensor stack
- Supports all combinations among Camera, LiDAR, Radar, and IMU
- Covers both target-based and targetless methods

**[direct_visual_lidar_calibration](https://arxiv.org/abs/2302.05094)** (Koide et al. 2023):
- NID-based targetless LiDAR-camera calibration
- Initial estimate via SuperGlue, refined via NID registration
- Operates from a single capture

### 12.5.3 Evaluation Tools

**evo** (MH Grupp):
- Python-based trajectory evaluation tool
- Computes and visualizes ATE and RPE
- Supports a variety of formats including TUM, KITTI, and EuRoC
- Provides both command-line tools and a Python API

```bash
# evo usage examples
# Compute ATE
evo_ape tum groundtruth.txt estimated.txt -va --plot --plot_mode xz

# Compute RPE
evo_rpe tum groundtruth.txt estimated.txt -va --delta 100 --delta_unit f

# Compare two systems
evo_traj tum system_a.txt system_b.txt --ref groundtruth.txt -p --plot_mode xz
```

```python
# evo Python API usage example

from evo.core import metrics, sync
from evo.core.trajectory import PosePath3D, PoseTrajectory3D
from evo.tools import file_interface
import numpy as np

def evaluate_trajectory(gt_file, est_file, align=True):
    """
    Trajectory evaluation using evo.
    
    Args:
        gt_file: path to ground truth file (TUM format)
        est_file: path to estimated trajectory file
        align: whether to perform SE(3) alignment
    """
    # Load trajectories
    traj_gt = file_interface.read_tum_trajectory_file(gt_file)
    traj_est = file_interface.read_tum_trajectory_file(est_file)
    
    # Synchronize timestamps
    traj_gt, traj_est = sync.associate_trajectories(traj_gt, traj_est)
    
    # Compute ATE
    ate_metric = metrics.APE(metrics.PoseRelation.translation_part)
    
    if align:
        # Umeyama alignment
        traj_est_aligned = traj_est.align(traj_gt, correct_scale=False)
        ate_metric.process_data((traj_gt, traj_est_aligned))
    else:
        ate_metric.process_data((traj_gt, traj_est))
    
    stats = ate_metric.get_all_statistics()
    
    print(f"ATE RMSE: {stats['rmse']:.4f} m")
    print(f"ATE Mean: {stats['mean']:.4f} m")
    print(f"ATE Median: {stats['median']:.4f} m")
    print(f"ATE Max: {stats['max']:.4f} m")
    
    return stats

def compare_systems(gt_file, system_files, system_names):
    """Compare the ATE of multiple systems."""
    traj_gt = file_interface.read_tum_trajectory_file(gt_file)
    
    results = {}
    for name, est_file in zip(system_names, system_files):
        traj_est = file_interface.read_tum_trajectory_file(est_file)
        traj_gt_sync, traj_est_sync = sync.associate_trajectories(
            traj_gt, traj_est
        )
        
        traj_est_aligned = traj_est_sync.align(
            traj_gt_sync, correct_scale=False
        )
        
        ate = metrics.APE(metrics.PoseRelation.translation_part)
        ate.process_data((traj_gt_sync, traj_est_aligned))
        
        results[name] = ate.get_all_statistics()
    
    # Print comparison table
    print(f"{'System':<20} {'RMSE (m)':<12} {'Mean (m)':<12} {'Max (m)':<12}")
    print("-" * 56)
    for name, stats in results.items():
        print(f"{name:<20} {stats['rmse']:<12.4f} "
              f"{stats['mean']:<12.4f} {stats['max']:<12.4f}")
    
    return results
```

### 12.5.4 Other Essential Tools

**ROS 2** (Robot Operating System):
- Integration framework for sensor fusion systems
- Provides sensor drivers, time synchronization, and message-passing infrastructure
- Most open-source SLAM systems are distributed as ROS packages

**Open3D**:
- Python library for 3D data processing
- Point cloud, mesh, and TSDF processing
- Built-in geometric algorithms such as ICP, RANSAC, and FPFH
- Excellent visualization

**CloudCompare**:
- GUI tool for point cloud comparison/editing
- Distance comparison between two point clouds (C2C, C2M)
- Point cloud registration, filtering, and downsampling

**COLMAP**:
- Structure from Motion (SfM) + Multi-View Stereo (MVS) pipeline
- 3D reconstruction from image collections
- Used in sensor fusion for camera intrinsic estimation and ground truth map construction

---

The practical systems and benchmarks covered in this chapter show how the theory of Ch.2-11 is applied in real products and research. The final chapter addresses **research frontiers** — foundation models, event cameras, 4D radar, and end-to-end SLAM — that are not yet mature but may reshape the field's future.

---

# Ch.13 — Frontiers & Emerging Directions

Ch.2-12 systematically covered the established theory and practical systems of sensor fusion. In this final chapter, we turn our gaze to the future.

The field of sensor fusion is evolving rapidly. This chapter surveys research frontiers that are not yet fully mature but could reshape the field's direction over the next several years. The topics include the extension of foundation models to spatial AI, end-to-end learned SLAM, scene-graph-based environmental understanding, cross-modal representation, and the fusion of new sensor modalities such as event cameras and 4D radar.

---

## 13.1 Foundation Models for Spatial AI

Foundation models — general-purpose models pretrained on large-scale data (DINOv2, CLIP, SAM, GPT-4V, etc.) — are rapidly permeating sensor fusion and SLAM pipelines. Although these models were not trained for any specific task, they provide rich visual and semantic representations that replace or augment multiple modules of traditional pipelines.

### 13.1.1 Leveraging DINOv2/CLIP Features in SLAM

**Visual features of DINOv2**: [DINOv2](https://arxiv.org/abs/2304.07193) (Oquab et al. 2024) is a ViT trained with self-supervised learning that provides pixel-level dense features. These features are:

- **Illumination/season invariant**: they produce similar features for the same place under different conditions (day/night, summer/winter).
- **Semantically aware**: they assign similar features to objects of the same kind (e.g., every "chair").
- **Structure aware**: geometric structure (edges, planes, etc.) is also reflected in the features.

**AnyLoc's approach**: [AnyLoc](https://arxiv.org/abs/2308.00688) (Keetha et al. 2023) aggregates DINOv2's dense features with VLAD to produce a global place descriptor. This descriptor:

- works in **every environment** — urban, indoor, aerial, underwater, subterranean — without any VPR-specific training.
- outperforms existing learning-based VPR methods (NetVLAD, CosPlace, etc.) across diverse domains.
- Key insight: dense features from the value facet of DINOv2's 31st layer perform 23% better than the CLS token.

```python
import numpy as np

class FoundationModelFeatureExtractor:
    """
    Conceptual implementation of DINOv2-based dense feature extraction.
    In practice, torch + the DINOv2 model is used.
    """
    
    def __init__(self, model_name='dinov2_vitg14', layer=31, facet='value'):
        """
        Args:
            model_name: DINOv2 model name
            layer: layer to extract from (31 in AnyLoc)
            facet: one of 'key', 'query', 'value' ('value' in AnyLoc)
        """
        self.model_name = model_name
        self.layer = layer
        self.facet = facet
        # In practice, load the torch model here
        # self.model = torch.hub.load('facebookresearch/dinov2', model_name)
        
    def extract_dense_features(self, image):
        """
        Extract pixel-level dense features from an image.
        
        Args:
            image: (H, W, 3) RGB image
            
        Returns:
            features: (H', W', D) dense feature map
                      H' = H/14, W' = W/14 (patch size = 14)
                      D = 1536 (feature dimension of ViT-G14)
        """
        # 1. Split the image into 14x14 patches
        # 2. Pass through the ViT to extract features from the specified layer
        # 3. Return in (num_patches, D) shape
        
        # placeholder
        H, W = image.shape[:2]
        h, w = H // 14, W // 14
        D = 1536
        features = np.random.randn(h, w, D).astype(np.float32)
        return features
    
    def extract_global_descriptor(self, image, num_clusters=64):
        """
        AnyLoc style: dense feature -> VLAD -> global descriptor.
        
        Args:
            image: (H, W, 3) RGB image
            num_clusters: number of VLAD clusters
            
        Returns:
            descriptor: (num_clusters * D,) global descriptor
        """
        dense_features = self.extract_dense_features(image)
        
        # Flatten spatial dimensions
        h, w, D = dense_features.shape
        features_flat = dense_features.reshape(-1, D)  # (N, D)
        
        # VLAD aggregation (simplified)
        # In practice, cluster centers are precomputed with k-means
        cluster_centers = np.random.randn(num_clusters, D)  # placeholder
        
        vlad = np.zeros((num_clusters, D))
        for feat in features_flat:
            # Hard assignment
            dists = np.linalg.norm(cluster_centers - feat, axis=1)
            closest = np.argmin(dists)
            vlad[closest] += feat - cluster_centers[closest]
        
        # L2 normalization (intra + inter)
        for i in range(num_clusters):
            norm = np.linalg.norm(vlad[i])
            if norm > 1e-8:
                vlad[i] /= norm
        
        descriptor = vlad.flatten()
        descriptor /= (np.linalg.norm(descriptor) + 1e-8)
        
        return descriptor
```

**Where foundation models fit in the SLAM pipeline**:

| Pipeline module | Traditional method | FM replacement/augmentation |
|---------------|-----------|-------------|
| Feature detection | FAST, ORB | SuperPoint + DINOv2 hybrid |
| Feature matching | BF + ratio test | SuperGlue/LightGlue, LoFTR |
| Place recognition | DBoW2, Scan Context | AnyLoc (DINOv2 + VLAD) |
| Semantic segmentation | Task-specific model training | [SAM](https://arxiv.org/abs/2304.02643), open-vocabulary segmentation |
| Depth estimation | Stereo matching | [Depth Anything](https://arxiv.org/abs/2401.10891) (monocular) |
| Loop closure verification | Geometric only | FM descriptor consistency |

### 13.1.2 Open-Vocabulary 3D Understanding

Extending CLIP's vision-language alignment to 3D maps allows a robot to understand and navigate its environment via natural language.

**How it works**:

1. Build a 3D map (point cloud, mesh, 3DGS) with SLAM.
2. Extract CLIP visual features for each region of each observation image.
3. Back-project the 2D features to their corresponding locations in the 3D map and attach them.
4. When the user says "find the fire extinguisher," the CLIP text encoder encodes the text and returns the location in the 3D map with the highest similarity.

**ConceptFusion, LERF, OpenScene**: representative systems of this approach. The core value is that 3D space can be queried by arbitrary text without a predefined class set.

**Current limitations**:
- CLIP features have low spatial resolution (patch-level). Precise localization of small objects is difficult.
- 3D consistency is hard to guarantee — the same object can have different features from different viewpoints.
- Computational cost: extracting FM features from every image is expensive.

### 13.1.3 How Much of the Traditional Pipeline Can FMs Replace?

An honest assessment as of 2025-2026:

**Areas where replacement is already under way**:
- **Visual place recognition**: AnyLoc outperforms DBoW2 in most environments. The gap is especially large when there are condition changes (day/night, seasonal).
- **Feature matching**: LoFTR and RoMa are trending toward replacing the traditional detect-describe-match pipeline. They are particularly strong in textureless environments.
- **Monocular depth**: [Depth Anything](https://arxiv.org/abs/2401.10891) estimates monocular metric depth to a reasonable level. It can be used as an auxiliary sensor.

**Areas where replacement is still difficult**:
- **LiDAR odometry**: Traditional methods (ICP, LOAM, FAST-LIO2) remain dominant. Learning-based LiDAR odometry lags in both generalization and accuracy.
- **IMU integration**: Physics-based preintegration provides accuracy and theoretical guarantees that learning cannot replace.
- **Backend optimization**: Optimization frameworks such as factor graphs and iSAM2 are not targets for FM replacement. The correct direction is to integrate FM outputs as factors.

**Hybrid approaches are the most promising**: Preserving the structural rigor of the traditional pipeline while injecting the robust features and semantic information that FMs provide on a module-by-module basis is currently the most practical direction.

**Recent key developments (2024-2025)**:

- **[MASt3R-SLAM](https://arxiv.org/abs/2412.12392)** (Murai et al. CVPR 2025): By directly integrating the geometric prior learned by the 3D reconstruction foundation model MASt3R into SLAM, this system achieves globally-consistent dense SLAM at 15 fps without assumptions about the camera model.
- **[Depth Anything V2](https://arxiv.org/abs/2406.09414)** (Yang et al. NeurIPS 2024): Using a strategy that trains a teacher on synthetic data and a student on large-scale pseudo-labels, this work significantly improves the accuracy and robustness of monocular depth estimation. It can be used as a depth prior in sensor fusion.

---

## 13.2 End-to-End Learned SLAM

Traditional SLAM is built as a modular pipeline (feature extraction -> matching -> motion estimation -> mapping -> loop closure -> optimization). End-to-end learning has the ambitious goal of turning this entire pipeline into a single differentiable system that maps directly from input (images/sensors) to output (pose, map).

### 13.2.1 Current Representative Systems

**[DROID-SLAM](https://arxiv.org/abs/2108.10869)** (Teed & Deng 2021): the most successful learning-based SLAM system to date.

Core architecture:

1. **RAFT-based iterative update operator**: A convolutional GRU iteratively refines the optical flow using features extracted from a correlation volume. This flow correction serves as a refinement of correspondences.

2. **Differentiable Dense Bundle Adjustment (DBA)**: The flow corrections are converted into camera pose (SE(3)) and per-pixel inverse-depth updates. Gauss-Newton is solved efficiently via the Schur complement, and the entire operation is differentiable, so it can be trained via backpropagation.

The Schur complement lets us solve for pose first:

$$(\mathbf{H}_{pp} - \mathbf{H}_{pd} \mathbf{H}_{dd}^{-1} \mathbf{H}_{dp}) \Delta \boldsymbol{\xi} = \mathbf{b}_p - \mathbf{H}_{pd} \mathbf{H}_{dd}^{-1} \mathbf{b}_d$$

Since $\mathbf{H}_{dd}$ is diagonal (each depth is independent), its inverse is $O(1)$. This implements the same structural efficiency as traditional BA inside a learning system.

3. **Frame-graph-based loop closure**: A frame graph is built dynamically based on co-visibility. On revisit, long-range edges are added to perform implicit loop closure.

4. **A single model supports monocular/stereo/RGB-D**: Trained only on synthetic data (TartanAir), it achieves SOTA on four benchmarks.

**DROID-SLAM's results and significance**:
- 62% error reduction over the previous best on TartanAir
- 82% reduction on EuRoC monocular
- The first learning-based SLAM system to systematically surpass traditional systems

### 13.2.2 Differentiable SLAM Components

Even without full end-to-end learning, research on making individual SLAM pipeline components differentiable is very active:

**Differentiable rendering**: NeRF and 3DGS are themselves differentiable rendering systems. In SLAM, pose estimation can be performed by backpropagating a photometric loss.

$$\hat{\mathbf{T}}^* = \arg\min_{\hat{\mathbf{T}}} \| I_{\text{real}} - \text{Render}(\text{Map}, \hat{\mathbf{T}}) \|^2$$

Because the $\text{Render}$ function is differentiable, the gradient with respect to $\hat{\mathbf{T}}$ can be computed directly.

**Differentiable ICP**: By making the nearest-neighbor search and SVD of traditional ICP differentiable, point cloud registration can be embedded inside a learning loop.

**Differentiable pose graph optimization**: If optimizers such as iSAM2 are made differentiable, the frontend (feature extraction, matching) can be trained with error signals from the backend. "If the optimization result is poor, improve the feature extractor" becomes an end-to-end training signal.

### 13.2.3 Current Limitations and Possibilities

**Limitations**:
- **Generalization**: Performance degrades in environments outside the training distribution. DROID-SLAM generalizes reasonably well since it is trained on synthetic data, but it still falls short of traditional systems in large-scale outdoor environments where LiDAR dominates.
- **Lack of theoretical guarantees**: Traditional optimization provides theoretical guarantees such as convergence and consistency. Learning-based systems lack such guarantees, making them difficult to apply to safety-critical use cases.
- **Computational cost**: Most learning-based systems require a GPU. Real-time operation on embedded platforms is challenging.
- **Interpretability**: Failure analysis is difficult. Traditional systems allow tracing "which module failed," but end-to-end systems are closer to black boxes.

**Possibilities**:
- As FMs advance, the quality of feature extraction and matching continues to improve.
- As differentiable optimization techniques mature, hybrid approaches that preserve traditional structure while taking advantage of learning become realistic.
- Multi-task learning: jointly training pose estimation, depth estimation, and semantic segmentation produces mutual benefits.

---

## 13.3 Spatial Memory & Scene Graphs

A robot's ability to "remember and understand space" is more than simply storing point clouds. Humans possess hierarchical, relational, and temporal spatial memory — "there is a refrigerator in the kitchen, and there was milk inside it." This section surveys research frontiers in such high-level spatial memory systems.

### 13.3.1 Persistent Spatial Memory

A traditional SLAM map reflects "the state of the environment at this moment." Persistent spatial memory is long-term spatial memory that also includes the history of how the environment changes over time.

**Core challenges**:

1. **Episodic spatial memory**: linking time, place, and event, as in "there was a box here last Tuesday."
2. **Semantic persistence**: distinguishing permanent elements (walls, buildings) from transient ones (people, vehicles) to preserve the stability of long-term maps.
3. **Incremental forgetting**: gradually forgetting the details of old observations while retaining core structure. This mirrors human memory.

```python
class PersistentSpatialMemory:
    """
    Temporal spatial memory -- a time series record of environment state.
    """
    
    def __init__(self, decay_rate=0.01):
        self.memories = {}  # {location_key: [MemoryEntry, ...]}
        self.decay_rate = decay_rate
    
    def record(self, location, observation, timestamp, semantic_class=None):
        """Record a new observation into spatial memory."""
        key = self._spatial_key(location)
        
        entry = {
            'timestamp': timestamp,
            'observation': observation,
            'semantic_class': semantic_class,
            'confidence': 1.0,
            'access_count': 0
        }
        
        if key not in self.memories:
            self.memories[key] = []
        self.memories[key].append(entry)
    
    def recall(self, location, time_query=None, semantic_query=None):
        """
        Recall relevant information from spatial memory.
        
        Args:
            location: query location
            time_query: query at a specific time (e.g., "3 days ago")
            semantic_query: semantic query (e.g., "chair")
        """
        key = self._spatial_key(location)
        if key not in self.memories:
            return []
        
        results = []
        for entry in self.memories[key]:
            relevance = entry['confidence']
            
            if time_query is not None:
                time_diff = abs(entry['timestamp'] - time_query)
                relevance *= np.exp(-self.decay_rate * time_diff)
            
            if semantic_query is not None:
                if entry['semantic_class'] != semantic_query:
                    continue
            
            if relevance > 0.1:
                results.append((entry, relevance))
                entry['access_count'] += 1
        
        return sorted(results, key=lambda x: x[1], reverse=True)
    
    def detect_changes(self, location, current_observation):
        """Compare the current observation with memory to detect changes."""
        key = self._spatial_key(location)
        if key not in self.memories:
            return 'new_location'
        
        latest = self.memories[key][-1]
        
        # Compare observations (placeholder -- in practice, compare features)
        similarity = self._compare_observations(
            latest['observation'], current_observation
        )
        
        if similarity < 0.5:
            return 'significant_change'
        elif similarity < 0.8:
            return 'minor_change'
        else:
            return 'no_change'
    
    def consolidate(self, max_entries_per_location=10):
        """
        Memory consolidation: remove old, rarely accessed memories.
        Preserve essential structural information.
        """
        for key in self.memories:
            entries = self.memories[key]
            
            if len(entries) <= max_entries_per_location:
                continue
            
            # Priority: recency + access frequency + confidence
            scored = []
            for entry in entries:
                score = (entry['confidence'] * 
                         (1 + entry['access_count']) * 
                         (1.0 / (1 + self.decay_rate * 
                                 (entries[-1]['timestamp'] - entry['timestamp']))))
                scored.append((entry, score))
            
            scored.sort(key=lambda x: x[1], reverse=True)
            self.memories[key] = [
                e for e, _ in scored[:max_entries_per_location]
            ]
    
    def _spatial_key(self, location, resolution=0.5):
        return tuple((np.array(location) / resolution).astype(int))
    
    def _compare_observations(self, obs1, obs2):
        return 0.5  # placeholder
```

### 13.3.2 Scene-Graph-Based Environmental Understanding

In Ch.11 we discussed the 3D Scene Graph of [Hydra](https://arxiv.org/abs/2201.13360). Here we explore the future directions that scene graphs open up.

**Scene Graph + Language**: Combining a scene graph with a natural language interface allows a robot to understand commands such as "bring me the remote on the table next to the sofa in the living room." This command is translated into a hierarchical traversal of the scene graph:

1. "living room" -> search the Room node
2. "table next to the sofa" -> search the relations among Object nodes within the Room
3. "remote" -> search Object nodes near that Table
4. Path planning and manipulation

**Scene Graph + LLM**: An LLM such as GPT-4 takes a scene graph as input and performs high-level reasoning. It can answer queries such as "if a person falls in this room, where is the nearest phone?"

**Dynamic scene graphs**: Hydra's current implementation assumes a static environment. Dynamic scene graphs include moving agents (people, vehicles) as nodes and update their relations in real time. This is central to social navigation and human-robot interaction (HRI).

### 13.3.3 Time-Series Spatial Memory Management

Robots operating over long time spans require strategies for the **creation, maintenance, and deletion** of spatial memory.

**Hierarchical forgetting**: The resolution of detail (exact texture, individual points) is reduced over time, while structural information (room layout, passage connectivity) is retained permanently. This parallels human spatial memory.

**Event-triggered update**: Instead of refreshing the entire map periodically, only regions where change is detected are updated selectively.

**Compression**: The map is progressively compressed over time. For example, dense point cloud -> sparse landmarks -> topological graph -> semantic description.

---

## 13.4 Cross-Modal Representation

One of the fundamental challenges of sensor fusion is comparing heterogeneous sensor observations in a **common representation space**. A LiDAR point cloud and a camera image are completely different data forms, yet they observe the same physical environment. Cross-modal representation is the research direction that seeks to close this "representation gap."

### 13.4.1 Aligning Representations Across Heterogeneous Sensors

**Why is it difficult?**

- **Dimensional mismatch**: LiDAR returns 3D points, a camera returns a 2D image, and radar returns a range-Doppler map. The data forms are intrinsically different.
- **Information asymmetry**: LiDAR provides accurate range information but no texture. A camera provides rich texture but no absolute distance.
- **Sensor-specific artifacts**: LiDAR motion distortion, camera rolling shutter, radar speckle noise — each sensor has its own noise pattern.

### 13.4.2 Contrastive Learning for Cross-Modal Alignment

Contrastive learning learns a representation in which observations from different modalities of the same place/object are placed close together, while those from different places/objects are pushed apart.

$$\mathcal{L}_{\text{contrastive}} = -\log \frac{\exp(\text{sim}(f_L(\mathbf{x}_L), f_C(\mathbf{x}_C)) / \tau)}{\sum_{j} \exp(\text{sim}(f_L(\mathbf{x}_L), f_C(\mathbf{x}_C^j)) / \tau)}$$

Here $f_L$ is the LiDAR encoder, $f_C$ is the camera encoder, $\tau$ is the temperature, $(\mathbf{x}_L, \mathbf{x}_C)$ is a LiDAR-camera pair from the same place, and $\mathbf{x}_C^j$ is a negative sample.

**Application to cross-modal place recognition**: A scenario in which a map built with LiDAR is localized against using only a camera. If the LiDAR descriptor and the camera descriptor live in the same space, a camera query can retrieve from a LiDAR map.

**LC$^2$** (Lee et al. 2023): LiDAR-Camera cross-modal place recognition. It aligns features from a LiDAR BEV image and a camera image into a common space.

### 13.4.3 Knowledge Distillation

A method for transferring rich information from one modality (teacher) to another (student).

**LiDAR -> Camera distillation**: The knowledge of a model trained with LiDAR's accurate 3D information is transferred to a camera-only model. At deployment, the camera alone can then approximate LiDAR-level 3D understanding.

**Camera -> LiDAR distillation**: Rich semantic information from cameras is transferred to LiDAR processing models. For example, attaching CLIP's semantic features to LiDAR points allows a LiDAR map to be queried by text.

### 13.4.4 Still-Open Questions

1. **Is a modality-agnostic representation possible?** Can a universal encoder map inputs from any sensor — LiDAR, camera, radar, event camera — into the same representation space?

2. **Temporal alignment**: Observations from different modalities are not perfectly synchronized in time. How should asynchronous observations be fused into a common representation?

3. **Partial observation**: When one sensor fails temporarily (LiDAR affected by rain, camera affected by darkness), how can a consistent representation be maintained from only the available modalities?

---

## 13.5 Event-Camera-Based Fusion

The event camera (Dynamic Vision Sensor, DVS) is a sensor that is fundamentally different from traditional frame-based cameras. Each pixel independently senses brightness changes and asynchronously emits an **event** only at the moment a change occurs.

### 13.5.1 Principles and Advantages of the Event Camera

Each event is represented as $(x, y, t, p)$:

- $(x, y)$: pixel coordinates
- $t$: microsecond-resolution timestamp
- $p \in \{+1, -1\}$: polarity (brighter / darker)

An event is triggered when:

$$|\log I(x, y, t) - \log I(x, y, t_{\text{last}})| \geq C$$

The polarity is $p = \text{sign}(\log I(x, y, t) - \log I(x, y, t_{\text{last}}))$. Here $I$ is brightness, $C$ is the contrast threshold, and $t_{\text{last}}$ is the most recent time at which the pixel fired an event.

**Advantages**:

| Property | Frame camera | Event camera |
|------|------------|-------------|
| Temporal resolution | 30-120 fps | microseconds |
| Dynamic range | ~60 dB | >120 dB |
| Motion blur | present | nearly none |
| Data output | uniform frames | asynchronous events |
| Static scene | provides information | no events (no information) |
| Power consumption | high | very low |

**Why is it important for sensor fusion?** Event cameras are robust in extreme conditions where traditional cameras fail — fast rotations, abrupt illumination changes (entering/exiting tunnels), low-light environments. This makes them an ideal complementary sensor that covers the weaknesses of other sensors.

### 13.5.2 Event + Frame Fusion

Approaches that combine an event camera with a traditional frame camera:

**Event-enhanced frame tracking**: Fast motion between frames is tracked with events, filling the gap between frame-based VO frames. This maintains tracking even during fast camera motion.

**Event-aided HDR**: Using the event camera's high dynamic range, information in the under/over-exposed regions of frame images is recovered.

### 13.5.3 Event + IMU Fusion

**[EVO](https://doi.org/10.1109/LRA.2016.2645143)** (Rebecq et al. 2017): Event-based Visual Odometry. It estimates camera pose from events alone. Combining with an IMU recovers scale and improves accuracy.

**[Ultimate SLAM](https://arxiv.org/abs/1709.06310)** (Vidal et al. 2018): A system that combines an event camera, a frame camera, and an IMU. It exploits the complementarity of the three sensors:
- Frame camera: rich texture in static scenes
- Event camera: continuous tracking under fast motion
- IMU: scale recovery and fast motion prediction

**Current challenges**:
- The data format of event cameras (an asynchronous event stream) is not compatible with traditional computer vision pipelines (frame-based). Converting events to frames (event frame) sacrifices the advantages.
- Commercial event cameras remain expensive and have low resolution (even recent models are around 1280 x 720).
- Training data is scarce. Most datasets are designed for frame cameras.

**Recent key developments (2024-2025)**:

- **[EvenNICER-SLAM](https://arxiv.org/abs/2410.03812)** (2024): A system that integrates an event camera into neural implicit SLAM, using the high temporal resolution of events to improve tracking robustness under fast motion.
- **Event-based 3D reconstruction survey** ([arxiv:2505.08438](https://arxiv.org/abs/2505.08438), 2025): The first comprehensive survey of event-driven 3D reconstruction, systematically organizing recent work on NeRF/3DGS-based event reconstruction, depth estimation, optical flow, and related topics.

```python
class EventProcessor:
    """
    Utilities for processing event camera data.
    """
    
    def __init__(self, width, height, time_window_us=33000):
        """
        Args:
            width, height: sensor resolution
            time_window_us: event accumulation window (microseconds)
        """
        self.width = width
        self.height = height
        self.time_window = time_window_us
    
    def events_to_frame(self, events, method='histogram'):
        """
        Convert an event stream into a frame.
        
        Args:
            events: [(x, y, t, p), ...] list of events
            method: 'histogram' or 'time_surface'
            
        Returns:
            frame: (H, W) or (H, W, 2) event frame
        """
        if method == 'histogram':
            return self._event_histogram(events)
        elif method == 'time_surface':
            return self._time_surface(events)
    
    def _event_histogram(self, events):
        """
        Event histogram: accumulate positive and negative events in separate channels.
        Simplest conversion, but loses temporal information.
        """
        frame = np.zeros((self.height, self.width, 2), dtype=np.float32)
        
        for x, y, t, p in events:
            if 0 <= x < self.width and 0 <= y < self.height:
                channel = 0 if p > 0 else 1
                frame[int(y), int(x), channel] += 1
        
        return frame
    
    def _time_surface(self, events):
        """
        Time Surface: records the most recent event time for each pixel.
        Preserves temporal information while converting to a frame-like form.
        """
        time_surface = np.zeros((self.height, self.width, 2), 
                                 dtype=np.float64)
        
        if len(events) == 0:
            return time_surface
        
        t_ref = events[-1][2]  # reference time (most recent)
        
        for x, y, t, p in events:
            if 0 <= x < self.width and 0 <= y < self.height:
                channel = 0 if p > 0 else 1
                time_surface[int(y), int(x), channel] = np.exp(
                    -(t_ref - t) / self.time_window
                )
        
        return time_surface
    
    def events_to_optical_flow(self, events, dt_us=10000):
        """
        Estimate optical flow from events (simplified contrast maximization).
        
        Key idea: temporally warping events with the correct optical flow
        maximizes the sharpness of image edges (maximal contrast).
        """
        if len(events) < 100:
            return np.zeros((self.height, self.width, 2))
        
        # Contrast maximization:
        # argmax_{v} Var(I_warp(v))
        # where I_warp is the event image warped by flow v
        
        # Simplified implementation (optimization is needed in practice)
        best_flow = np.zeros(2)
        best_contrast = 0
        
        for vx in np.linspace(-2, 2, 20):
            for vy in np.linspace(-2, 2, 20):
                warped = np.zeros((self.height, self.width))
                
                t_ref = events[-1][2]
                for x, y, t, p in events:
                    dt = (t_ref - t) / 1e6  # in seconds
                    wx = int(x + vx * dt)
                    wy = int(y + vy * dt)
                    
                    if 0 <= wx < self.width and 0 <= wy < self.height:
                        warped[wy, wx] += p
                
                contrast = np.var(warped)
                if contrast > best_contrast:
                    best_contrast = contrast
                    best_flow = np.array([vx, vy])
        
        flow = np.zeros((self.height, self.width, 2))
        flow[:, :] = best_flow
        return flow
```

---

## 13.6 4D Radar Fusion

4D imaging radar is the fastest-rising new modality in sensor fusion. Whereas traditional automotive radar provided only range and angle, 4D radar provides four-dimensional information: range, azimuth, elevation, and Doppler velocity.

**Principle of range/velocity measurement in FMCW radar**: Most 4D radars use FMCW (Frequency-Modulated Continuous Wave). The transmitted signal's frequency increases linearly over time (chirp); range is measured from the beat frequency of the reflected signal, and velocity from the phase change between chirps:

$$R = \frac{c \cdot f_b}{2 \cdot S}$$

Here $R$ is the range to the target, $c$ is the speed of light, $f_b$ is the beat frequency, and $S$ is the chirp's frequency slope (Hz/s).

$$v = \frac{\lambda \cdot \Delta\phi}{4\pi \cdot T_c}$$

Here $v$ is the radial velocity of the target, $\lambda$ is the carrier wavelength, $\Delta\phi$ is the phase change between two consecutive chirps, and $T_c$ is the chirp period.

### 13.6.1 Adverse-Weather Robustness

The core value of 4D radar lies in its **robustness under adverse weather**:

| Condition | Camera | LiDAR | 4D Radar |
|------|--------|-------|----------|
| Clear day | best | best | good |
| Rain | degraded | slightly degraded | normal |
| Fog | severely degraded | severely degraded | normal |
| Snow/dust | severely degraded | severely degraded | normal |
| Night | severely degraded | normal | normal |
| Direct sunlight | degraded | normal | normal |

Because radar's wavelength (millimeter wave) is much larger than raindrops, fog particles, and dust, scattering by these media is almost negligible. This complements the fundamental limitations of LiDAR (near-infrared) and cameras (visible light).

### 13.6.2 4D Radar + Camera Fusion

The fusion of 4D radar and a camera aims at a "low-cost perception system that works even in adverse weather":

**BEV-based fusion**: BEV features are extracted from camera images (as in LSS or BEVFormer) and radar points are projected into the BEV space to be combined.

**Leveraging radar's Doppler information**: 4D radar directly measures the radial velocity along the line of sight for each point. This is information unique to radar, absent from cameras and LiDAR:

- **Dynamic object classification**: the Doppler channel immediately separates static background from moving objects.
- **Ego-motion estimation**: the Doppler of static points allows ego-velocity estimation (even without an IMU).
- **Tracking support**: the velocity information of an object can be used directly in tracking.

$$v_r = (\mathbf{v}_{\text{obj}} - \mathbf{v}_{\text{ego}}) \cdot \hat{\mathbf{r}}$$

Here $v_r$ is the measured radial velocity, $\mathbf{v}_{\text{obj}}$ is the object velocity, $\mathbf{v}_{\text{ego}}$ is the ego velocity, and $\hat{\mathbf{r}}$ is the unit direction vector from the radar to the target ($\|\hat{\mathbf{r}}\| = 1$). For a static object ($\mathbf{v}_{\text{obj}} = \mathbf{0}$), $v_r = -\mathbf{v}_{\text{ego}} \cdot \hat{\mathbf{r}}$.

### 13.6.3 Recent Developments in Radar Odometry

Radar odometry has advanced rapidly since 2020:

**FMCW radar odometry**: odometry on scanning FMCW radar (Navtech, etc.). Feature points are extracted and matched in range-azimuth images to estimate ego-motion.

**4D radar odometry**: odometry on 4D radar point clouds. Approaches similar to LiDAR odometry (ICP, feature matching) are feasible, but they face the challenges of low resolution and high noise.

**Doppler-based ego-velocity estimation**: Ego velocity is estimated directly from the Doppler measurements of static points. Dynamic points are removed with RANSAC, and $\mathbf{v}_{\text{ego}}$ is estimated from the Doppler of static points:

$$v_r^{(k)} = -\mathbf{v}_{\text{ego}} \cdot \hat{\mathbf{r}}^{(k)} \quad \text{(static point)}$$

Here $k$ is the point index. At least three non-collinear points are required to estimate the 3D velocity vector.

```python
def estimate_ego_velocity_from_doppler(radar_points, doppler_values, 
                                        directions, ransac_threshold=0.3,
                                        max_iterations=100):
    """
    Estimate ego-velocity from radar Doppler measurements.
    
    Doppler of static points: v_r = -v_ego . r_hat
    This is a linear system.
    
    Args:
        radar_points: (N, 3) radar point coordinates
        doppler_values: (N,) radial velocity for each point
        directions: (N, 3) unit direction vector for each point
        ransac_threshold: RANSAC inlier threshold (m/s)
        max_iterations: maximum number of RANSAC iterations
        
    Returns:
        v_ego: (3,) ego-velocity vector
        inlier_mask: (N,) static point mask
    """
    N = len(doppler_values)
    best_inliers = np.zeros(N, dtype=bool)
    best_v_ego = np.zeros(3)
    
    for _ in range(max_iterations):
        # Randomly sample 3 points
        idx = np.random.choice(N, 3, replace=False)
        
        # Linear system: v_r = -directions @ v_ego
        # A @ v_ego = b
        A = -directions[idx]  # (3, 3)
        b = doppler_values[idx]  # (3,)
        
        try:
            v_ego_candidate = np.linalg.solve(A, b)
        except np.linalg.LinAlgError:
            continue
        
        # Compute residuals for all points
        predicted_doppler = -directions @ v_ego_candidate
        residuals = np.abs(doppler_values - predicted_doppler)
        
        inliers = residuals < ransac_threshold
        
        if np.sum(inliers) > np.sum(best_inliers):
            best_inliers = inliers
            best_v_ego = v_ego_candidate
    
    # Re-estimate with inliers (least squares)
    if np.sum(best_inliers) >= 3:
        A = -directions[best_inliers]
        b = doppler_values[best_inliers]
        best_v_ego = np.linalg.lstsq(A, b, rcond=None)[0]
    
    return best_v_ego, best_inliers

def separate_static_dynamic(radar_points, doppler_values, directions,
                             v_ego, threshold=0.5):
    """
    Use the ego-velocity to classify points as static or dynamic.
    
    Args:
        v_ego: estimated ego-velocity (3,)
        threshold: static/dynamic classification threshold (m/s)
        
    Returns:
        static_mask: (N,) static points
        dynamic_mask: (N,) dynamic points
        object_velocities: (N,) estimated object velocity for each point (radial)
    """
    # Expected Doppler under the static assumption
    expected_doppler = -directions @ v_ego
    
    # Residual = actual Doppler - expected Doppler
    residuals = doppler_values - expected_doppler
    
    static_mask = np.abs(residuals) < threshold
    dynamic_mask = ~static_mask
    
    # Radial component of dynamic point object velocity
    object_velocities = residuals  # v_obj . r_hat
    
    return static_mask, dynamic_mask, object_velocities
```

### 13.6.4 Representative Datasets and Benchmarks

| Dataset | Sensors | Environment | Features |
|----------|------|------|------|
| **[Boreas](https://arxiv.org/abs/2203.10168)** (Burnett et al. 2023) | Camera, LiDAR, Radar, GNSS/IMU | Urban (various weather) | Same route repeated for one year, including adverse weather |
| **RadarScenes** | Radar, Camera, LiDAR | Urban | Traditional automotive radar points + semantic labels (point-level annotation) |
| **nuScenes** | Camera, LiDAR, Radar | Urban | Includes 5 radars, some adverse weather |
| **View-of-Delft** | Camera, LiDAR, 4D Radar | Urban | 4D radar + 3D annotation |

**Recent key developments (2024-2025)**:

- **[Snail-Radar](https://arxiv.org/abs/2407.11705)** (2024): The first large-scale, diverse benchmark for evaluating 4D radar-based SLAM, providing 44 sequences collected across three platforms (handheld, bicycle, SUV) under diverse weather and lighting conditions.
- **[4D Radar-Inertial Odometry](https://arxiv.org/abs/2412.13639)** (2024): Proposes a 3D Gaussian radar scene representation and multi-hypothesis scan matching, achieving more precise radar odometry than voxel-based methods.

4D radar fusion is still in its early stages, but it is developing rapidly as a key technology for all-weather operation in autonomous driving. In particular, ego-motion estimation and dynamic object classification that exploit Doppler information are unique capabilities that cameras and LiDAR cannot provide.

---

## Closing

Starting from sensor modeling (Ch.2), this guide has traced the entire sensor fusion pipeline — through calibration (Ch.3), state estimation theory (Ch.4), feature matching (Ch.5), VO/VIO (Ch.6), LiDAR odometry (Ch.7), multi-sensor fusion (Ch.8), Place Recognition (Ch.9), Loop Closure (Ch.10), spatial representation (Ch.11), practical systems (Ch.12), and the research frontiers of Ch.13.

To restate the core narrative of the field once more:

1. **Traditional methods remain the foundation.** Kalman filters, ICP, RANSAC, factor graphs — methods proposed decades ago form the skeleton of modern systems.
2. **Deep learning has revolutionized perception.** In the domain of "what is being seen" — feature matching, depth estimation, Place Recognition — learning-based methods overwhelm traditional ones.
3. **In inference, traditional and learned methods coexist.** State estimation backends are still dominated by optimization-based methods, but efforts such as DROID-SLAM are dissolving the boundary through differentiable optimization.
4. **Foundation models are the new inflection point.** The expressive power of general-purpose models such as DINOv2 and SAM is permeating every part of the sensor fusion pipeline, and this trend will accelerate.

Sensor fusion is not "the technology of combining sensor data" but **the technology of understanding the world from incomplete observations**. We hope this guide serves as a starting point for that understanding.