센서 퓨전 심화 가이드

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채널이고, KITTI의 HDL-64E 같은 표준 참조 센서는 64채널, 상위 모델은 128채널까지 있다. 고해상도 모델은 수천에서 수만 달러를 호가한다. 최근 솔리드 스테이트(solid-state) LiDAR가 가격을 낮추고 있지만, 좁아진 시야각(FoV)이 트레이드오프다.

### IMU의 한계

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

**드리프트(drift).** IMU 측정을 적분하여 속도와 위치를 계산하면, 센서 바이어스(bias)와 노이즈가 시간에 따라 적분되어 오차가 누적된다. 일정한 가속도계 바이어스가 만드는 위치 오차 항은 $t^2$에 비례한다. 실제 드리프트 속도는 센서 등급, 온도 보정, 초기화, 운동과 외부 보정 관측에 따라 크게 달라지므로, 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)를 정렬할 수 있지만, standalone IMU의 요(yaw)는 관측되지 않는다. 자력계, GNSS course, 카메라·LiDAR 지도 정합 같은 외부 관측이 있어야 heading 기준을 붙일 수 있다.

### 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는 고주파의 가속도/각속도를 제공하여 카메라 프레임 사이의 빠른 모션을 보간한다. 운동 여기(motion excitation)가 충분한 경우에는 메트릭 가속도(metric acceleration)와 시각 제약(vision constraint)의 결합으로 스케일을 관측할 수 있고, 가속도계는 중력 방향 추정에도 기여한다.
- 두 센서는 서로 보완한다. 카메라가 제공하지 못하는 스케일·고주파 모션을 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}$$

위 식처럼 두 관측이 unbiased·independent Gaussian이고 분산을 정확히 안다는 가정에서는 $\sigma_{\text{fused}}^2 < \min(\sigma_1^2, \sigma_2^2)$가 성립한다. 상관관계를 무시하거나 분산을 잘못 정하면 이 보장은 깨진다. 이 식은 scalar independent-measurement Kalman update의 특수한 경우다.

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

차량 주위에 여러 LiDAR를 배치하면 field of view를 넓히고 겹치는 영역에서 중복 관측을 얻을 수 있다. 센서 수와 배치는 range·occlusion·cost·failure coverage 요구에 따라 달라진다.

### 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)" 문제로 알려져 있다.
- 최적성 저하 가능성: 중간 출력이 충분통계량이 아니거나 cross-correlation을 전달하지 않으면, raw 관측을 일관되게 결합한 estimator보다 정보를 잃을 수 있다.

### 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)에서 통합 최적화한다.

**장점:**
- 정보 보존 가능성: 개별 residual과 covariance를 함께 다루므로 중간 pose 출력만 합치는 방식보다 정보를 더 보존할 수 있다. 실제 품질은 model·linearization·outlier 처리에 달려 있다.
- 교차 상태 추정: extrinsic·time offset·bias를 상태에 포함하고 motion이 관측 가능성을 제공하면, camera 관측이 IMU bias 추정에 기여하는 식의 상호 보정이 가능하다.
- 열화 대응 여지: 한 sensor의 관측 수가 줄었을 때 다른 sensor가 해당 상태를 충분히 관측하면 추정을 이어갈 수 있다. 이를 보장하려면 failure detection과 observability 분석이 필요하다.

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

### 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 데이터를 직접 사용하는 것이다. IMU 예측으로 특징점 탐색 영역을 좁히는 프런트엔드 설계가 이에 가깝다. VINS-Mono의 프런트엔드는 KLT optical flow로 추적하고 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 |

현대의 공개 VIO·LIO 시스템에서는 **tightly coupled** 설계를 흔히 볼 수 있다. Loosely coupled 방식은 구현과 모듈 교체가 쉽지만 중간 결과로 정보를 압축하면서 원시 관측의 일부를 잃을 수 있다. Ultra-tightly coupled 방식은 센서 내부 신호나 추적 루프에 접근해야 해 적용 범위가 좁다. VINS-Mono, FAST-LIO2, LIO-SAM은 서로 다른 센서 조합을 tightly coupled 방식으로 융합하는 대표적인 공개 구현이다. [FAST-LIVO2 (Zheng et al., 2024)](https://arxiv.org/abs/2408.14035)는 LiDAR·IMU·카메라를 한 프레임워크에 결합하며, 원 논문의 데이터셋과 비교 설정에서 정확도와 처리 속도 개선을 보고한다.

---

## 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)을 찾아, 텍스처가 부족한 환경에서도 매칭한다.

학습 기반 matcher가 여러 공개 benchmark에서 handcrafted baseline보다 높은 결과를 보이지만, latency·memory·domain shift까지 포괄한 종합적 실효성은 배포 대상 임베디드 하드웨어와 실제 운용 환경에 직결된다.

**장소 인식(Place Recognition).** [NetVLAD (Arandjelović et al., 2016)](https://arxiv.org/abs/1511.07247)은 CNN feature를 trainable VLAD layer로 모아 place descriptor를 학습했고, 논문 benchmark에서 기존 image-retrieval baseline보다 높은 recall을 보고했다. [AnyLoc (Keetha et al., 2023)](https://arxiv.org/abs/2308.00688)은 DINOv2 feature와 비지도 VLAD를 여러 domain에 평가했다. DBoW2와 입력 feature·compute가 달라, 결과는 같은 dataset과 verification pipeline에서 비교해야 한다.

**단안 깊이 추정.** 단일 영상만의 투영 기하로는 장면의 절대 스케일을 유일하게 정할 수 없다. [Depth Anything (Yang et al., 2024)](https://arxiv.org/abs/2401.10891)은 대규모 데이터에서 학습한 단안 상대 깊이 모델이다. 후속작인 [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 metric depth를 추정한다. 센서 퓨전에 사용할 때는 목표 카메라와 환경에서 스케일 편향과 불확실성을 따로 검증해야 한다.

**맵 표현.** NeRF는 장면을 신경망으로, 3D Gaussian Splatting은 최적화된 3D 가우시안들의 집합으로 표현한다. NeRF-SLAM, Gaussian Splatting SLAM 등은 전통적인 점 지도(point map)나 복셀 격자(voxel grid)와 다른 포토리얼리스틱 맵 표현을 제공한다.

**이벤트 카메라(Event Camera).** 딥러닝의 발전과 별개로, 뉴로모픽 비전 센서로 불리는 이벤트 카메라는 각 픽셀이 밝기 변화를 비동기적으로 감지하여, 마이크로초 수준의 시간 분해능과 넓은 다이내믹 레인지를 제공한다. [이벤트 카메라 서베이 (Chakravarthi et al., 2024)](https://arxiv.org/abs/2408.13627)가 정리하듯, 이벤트 기반 VIO와 SLAM은 기존 프레임 기반 카메라와 결합되어 고속 모션과 저조도 환경에 쓰인다.

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

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

1. **명시적 불확실성 전파**: 칼만 필터와 팩터 그래프는 가정한 model과 covariance 아래에서 불확실성을 계산한다. Model mismatch·linearization·correlation을 잘못 다루면 이 covariance도 일관적이지 않을 수 있으며, learned observation에는 별도의 calibration이 필요하다.
2. **물리 model과 constraint**: 상태 전이와 residual에 dynamics·kinematics를 넣을 수 있다. 이것만으로 모든 estimate가 물리적으로 가능해지는 것은 아니며, 필요한 hard constraint는 optimization이나 parameterization에 명시해야 한다.
3. **학습 데이터 의존성**: model-based estimator는 대규모 supervised training 없이 실행할 수 있지만, sensor calibration·noise tuning·validation data는 여전히 필요하다. Learned component는 training distribution과 label·self-supervision 조건도 함께 관리해야 한다.
4. **일반화**: 학습 기반 오도메트리는 학습 분포와 다른 환경에서 성능이 떨어질 수 있다. 기하학적 방법도 텍스처, 조명, 동적 물체, 센서 노이즈의 영향을 받지만 재학습 없이 적용할 수 있다는 장점이 있다.

**LiDAR 오도메트리.** [LOAM (Zhang & Singh, 2014)](https://frc.ri.cmu.edu/~zhangji/publications/RSS_2014.pdf) 이후에도 기하 기반 LiDAR 오도메트리가 널리 쓰인다. ICP, GICP, NDT 같은 점군 정합 방법은 모델과 실패 조건이 비교적 잘 알려져 있고 재학습 없이 새 환경에 적용할 수 있다. 학습 기반 LiDAR 오도메트리는 데이터셋별로 경쟁력 있는 결과를 내지만, 분포 이동과 센서 변경까지 포함한 일반화는 별도로 검증해야 한다.

**캘리브레이션.** [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 | 정확도·compute·domain 조건별 선택 |
| LiDAR odometry | ICP/LOAM | DeepLO 계열 | Geometry 중심, learned component 병용 |
| Place recognition | BoW/VLAD | NetVLAD → AnyLoc | 후보 검색은 혼용, geometry로 검증 |
| Depth estimation | Stereo matching | Mono depth (Depth Anything) | Metric stereo와 learned prior의 역할 구분 |
| Calibration | Target-based | Targetless + learning | Target·targetless를 요구조건별 선택 |
| Map representation | Occupancy/TSDF | NeRF / 3DGS | 공존 |
| State estimation backend | KF / Factor Graph | End-to-end 시도 | Model-based estimator와 learned observation 결합 |

대체로 learned feature는 perception module에, model-based estimator는 geometry와 uncertainty 결합에 쓰이는 경우가 많다. 이는 절대적인 경계가 아니라 각 module의 input·failure mode·검증 지표를 나누어 설계하기 위한 출발점이다.

---

## 1.5 범위와 구성

### 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.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에서 기본 지원하는 왜곡 모델로, pinhole camera 기반 SLAM·VIO 파이프라인에서 널리 활용된다. Wide-FoV lens에는 fisheye나 omnidirectional model이 더 적합할 수 있다.

정규화된 이미지 좌표 $\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 = \operatorname{atan2}\left(\sqrt{X_c^2 + Y_c^2},\; Z_c\right)$$

$Z_c < 0$인 후방 반구(180° 이상)까지 다루려면 두 인자 atan2가 필요하고, 방향 단위벡터도 $Z_c$로 나누지 않고 $X_c/\sqrt{X_c^2+Y_c^2}$ 형태로 구한다. 아래 파이썬 구현이 그 방식이다.

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

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

여기서 $r_d$는 무차원 정규화 반경이다. 순수 등거리(equidistant) 투영에서는 $r_d = \theta$이며, 이는 $k_1 = 1$, $k_2 = k_3 = \cdots = 0$에 해당한다. 픽셀 단위 초점 거리는 아래 투영식의 $f_x$, $f_y$로 적용한다.

투영 좌표:

$$\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-1) \cdot t_r$ ($H$: 이미지 높이)이며, 각 행의 노출 지속 시간과는 다르다. 전체 readout 기간은 대략 $H \cdot t_r$로, 수 밀리초에서 수십 밀리초에 달한다.

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

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

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

여기서 $\mathbf{T}(t_{v_i})$는 $i$번째 특징점의 행 $v_i$에 대응하는 시각의 월드→카메라 변환이다. 첫 행과 마지막 행의 포즈가 주어지면, 두 시각 사이에서 일정한 twist를 가정해 다음과 같이 근사 보간할 수 있다 ($H>1$):

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

여기서 $\text{Exp}$와 $\text{Log}$는 $SE(3)$ 리 군 위의 지수/로그 맵이다. 아래 코드는 카메라→월드 변환을 입력받고, 회전 SLERP와 이동의 선형 보간을 따로 적용하는 근사 예제이다.

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

```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 - 1) if H > 1 else 0.0  # 첫 행 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를 비롯한 LIO 시스템에서 쓰이는 방식이다.
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 포즈를 추정한다.

**제한된 FoV의 Livox LiDAR** (Mid-40/70, Avia, HAP 등)는 비반복(non-repetitive) 스캔 모드를 제공한다. 이 가운데 Mid-40은 내부의 회전 프리즘으로 빔을 조향하므로 기계적 회전부가 없는 센서는 아니다. Avia는 반복 스캔 모드도 지원한다. 비반복 모드에서는 제한된 FoV(예: Mid-70은 약 70.4° 원형) 내 커버리지가 시간이 지남에 따라 증가한다.

아래 표는 스피닝 LiDAR와 제한된 FoV의 비반복 스캔 모드를 비교한다:

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

FAST-LIO / [FAST-LIO2](https://arxiv.org/abs/2107.06829)는 스캔 라인 구조에 의존하지 않고 **개별 포인트를 순차적으로 처리**하는 iterated EKF 구조를 사용하므로 스캔 라인이 없는 비반복 스캔 LiDAR에 잘 맞는다. 반면 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)로 구성된다. 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})$$

자이로스코프의 노이즈 파라미터는 장치와 운용 조건에 맞춰 정한다.

- **백색 노이즈 밀도**: 이름이 명시된 장치의 데이터시트와 Allan deviation의 $-1/2$ 기울기 구간에서 추정한다. 단위와 연속 시간 노이즈 밀도 정의가 구현의 이산 공분산 변환과 일치하는지 확인한다.
- **In-run bias stability**: 충분히 긴 정지 데이터의 Allan deviation에서 기울기가 거의 0인 구간을 확인한다. 온도, 장착 응력, 진동, 측정 시간에 따라 달라지므로 데이터시트의 한 수치를 그대로 쓰지 않는다.
- **바이어스 랜덤 워크 구동 노이즈**: 장시간 구간의 양의 기울기에서 추정한다. 측정 노이즈 $\sigma_g$와 바이어스 구동 노이즈 $\sigma_{bg}$를 구분하고, 연속 시간 밀도와 이산 시간 공분산의 변환 규약을 기록한다.

### 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})$$

가속도계도 같은 절차로 파라미터를 정한다.

- **백색 노이즈 밀도**: 장치 데이터시트와 Allan deviation의 $-1/2$ 기울기 구간을 함께 사용하고, $\text{m/s}^2/\sqrt{\text{Hz}}$, $\text{mg}/\sqrt{\text{Hz}}$, VRW 사이의 단위 변환을 명시한다.
- **In-run bias stability**: 정지·온도 시험에서 거의 평평한 Allan 구간과 반복 실험의 분산을 확인한다. 중력 투영 오차와 장착 자세 변화가 바이어스로 섞이지 않게 한다.
- **바이어스 랜덤 워크 구동 노이즈**: 장시간 데이터의 해당 기울기에서 $\sigma_{ba}$를 추정하고, 필터가 사용하는 샘플 주기에 맞춰 이산 공분산으로 변환한다.

### 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 같은 VIO 구현에서 흔히 쓰인다. FAST-LIO2를 포함한 각 system의 propagation 식과 discretization은 구현별로 확인해야 한다. IMU rate가 높더라도 angular acceleration과 sampling jitter가 크면 적분 오차가 남으므로, 중간점·RK4·preintegration 선택은 목표 motion과 error budget으로 검증한다.

```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}$)로 둔다.

- 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}$

이 예는 보조 센서가 없는 저가형 MEMS 관성 추정의 위치 오차가 짧은 임무에서도 빠르게 허용 범위를 넘을 수 있음을 보여준다. 실제 지속 가능한 시간은 IMU 성능 등급, 주변 온도, 운동 형태, 오차 허용 한계에 따라 차이가 난다. 고성능 INS의 경우 외부 보조 없이 더 오래 버틸 수 있으나 시간 경과에 따른 드리프트 누적 자체를 완전히 피하기는 어렵다. 이에 따라 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 등급 분류

항법등급, 전술등급, 산업용, 소비자용 같은 명칭은 제조사와 분야마다 경계가 다르다. 또한 MEMS는 제작 기술을 가리키므로 하나의 성능 등급과 같은 뜻이 아니다. 따라서 등급명이나 고정 가격표보다 이름이 명시된 장치의 사양과 목표 플랫폼 시험으로 선택한다.

| 선택 기준 | 확인할 항목 | 검증 자료 | 설계 영향 |
|----------|------------|----------|----------|
| 백색 노이즈 | 자이로·가속도계 노이즈 밀도, ARW/VRW 단위 | 장치 데이터시트 + Allan 시험 | 단기 상태 전파 오차 |
| 바이어스 | In-run stability, 랜덤 워크, 온도 계수 | 정지·온도 반복 시험 + Allan 시험 | 무보조 드리프트와 외부 관측 주기 |
| 동적 범위 | 측정 범위, 대역폭, 포화·클리핑 | 목표 운동·진동 프로파일 시험 | 급격한 운동에서의 관측 손실 |
| 시간 특성 | 출력률, 타임스탬프, clock/trigger 지원 | 지연·지터 측정 | 시간 보정과 퓨전 일관성 |
| 환경 내성 | 온도, 진동, 충격, 장착 응력 | 제조사 qualification + 목표 플랫폼 시험 | 반복성과 장기 안정성 |

외부 센서의 종류와 보조 주기는 이 측정값을 임무의 오차 예산에 넣어 정한다. 가격은 시점과 수량에 따라 달라지므로 현재 견적과 필요한 캘리브레이션·동기화 비용까지 함께 비교한다.

---

## 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)

반송파 위상의 fractional-phase 관측 잡음은 의사거리보다 훨씬 작아 millimeter 수준일 수 있다. 그러나 이것이 곧바로 millimeter positioning accuracy를 뜻하지는 않는다. L1 반송파(약 1575.42 MHz)의 파장은 약 19 cm이며, 정수 모호성·cycle slip·multipath·대기 오차를 함께 풀어야 한다.

$$\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의 fixed 해와 PPP-AR는 모호성을 정수로 결정하며, 전통적인 PPP는 모호성을 실수값으로 추정한다.
- $\epsilon_\Phi \approx 1\text{–}5\,\text{mm}$: 반송파 위상 노이즈 (의사거리 노이즈의 약 1/100)

전리층 지연의 부호는 의사거리와 반대이다(군속도 vs 위상속도). 이중 주파수 조합으로 1차 전리층 항을 추정·완화할 수 있지만, 고차 항(higher-order term)과 다른 오차 원인은 남는다.

### 2.4.3 RTK (Real-Time Kinematic)

RTK는 기준국 또는 network correction과의 **차분(differencing)** 관측으로 공통 오차를 줄이고 carrier-phase integer ambiguity를 실시간으로 해결하는 기법이다. 기준선(baseline)이 길어질수록 대기 오차의 상관이 약해지며, 정수 모호성이 고정(fix)되고 신호·보정 정보(correction)의 품질이 좋은 조건에서는 센티미터급(centimeter-class) 결과를 낼 수 있다.

**이중 차분(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의 전역 위치 관측(global position observation)은 VIO/LIO의 장기 드리프트를 제한하거나 보정할 수 있다. 다만 outage·multipath·frame alignment·lever arm·time offset이 남으면 드리프트와 바이어스도 남는다. [LIO-SAM (Shan et al., 2020)](https://arxiv.org/abs/2007.00258)은 조건을 만족하는 GNSS 관측을 factor graph에 추가할 수 있는 대표적 예다. GNSS 관측을 fusion에 포함할 때는 세 가지를 확인해야 한다:

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  # 단위 벡터 (위성→수신기 방향; diff = p_receiver - p_satellite)
        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) / (1 - e2 * sin_lat0**2) + alt0) * dlat  # 자오선 곡률반경 M
    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 이미징 레이더는 MIMO(Multiple Input Multiple Output) 기술로 대규모 가상 안테나 어레이(virtual antenna array)를 구현한다. 예를 들어, 12개 송신 안테나 × 16개 수신 안테나 = 192개 가상 채널을 구성할 수 있다. 수평·수직 각도 분해능은 가상 배열의 각 방향 개구와 배치에 따라 달라진다.

**4D Radar vs LiDAR:**

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

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

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 단위})$$

해수면 근처에서 약 $12.0\,\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축 자기장 벡터를 측정한다. 롤(Roll)·피치(pitch) 보정과 hard/soft-iron calibration(하드·소프트 아이언 보정)이 유효하고 주변 자기 교란이 작다면, 지구 자기장에 대한 방위를 얻을 수 있다. 진북(True north)이 필요하면 지역 자기 편차(magnetic declination)도 보정해야 한다.

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

$$\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) 신호가 지연되어 실제보다 먼 거리를 보고한다. UWB 기반 측위에서는 NLOS를 탐지하고 그 영향을 줄여야 한다.

**센서 퓨전에서의 역할.** UWB 앵커(Anchor)를 환경에 미리 설치하면, 각 앵커까지의 거리(range)로 앵커 좌표계(anchor frame) 안의 위치를 추정할 수 있다. 앵커 좌표를 측량해 전역 좌표계(global frame)에 묶은 경우에만 전역 절대 위치(global absolute position)가 된다. VIO와 결합하면 기하 배치와 NLOS 조건이 충분히 양호한 구간에서 드리프트를 제한할 수 있다.

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

$$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})$ (핀홀 + 왜곡) | 검출 노이즈, 왜곡 잔차 | 목표 검출기와 별도 검증 영상의 재투영 잔차 |
| LiDAR | 3D 점 $(r, \alpha, \omega)$ | Range-bearing | 거리 노이즈, 빔 확산, 혼합 픽셀 | 장치 데이터시트 + 거리·표면·입사각별 잔차 시험 |
| IMU (자이로) | 각속도 $\boldsymbol{\omega}$ | $\tilde{\boldsymbol{\omega}} = \boldsymbol{\omega} + \mathbf{b}_g + \mathbf{n}_g$ | 바이어스, 랜덤 워크 | 장치 데이터시트 + Allan·온도·진동 시험 |
| IMU (가속도) | 비력 $\mathbf{a}$ | $\tilde{\mathbf{a}} = \mathbf{R}(\mathbf{a}-\mathbf{g}) + \mathbf{b}_a + \mathbf{n}_a$ | 바이어스, 랜덤 워크 | 장치 데이터시트 + Allan·온도·진동 시험 |
| GNSS | 의사거리 $\rho$ | $\rho = r + c\delta t_r + I + T + \epsilon$ | 다중경로, 전리층, 대류권 | 수신기·보정 상태별 목표 환경 주행 로그 |
| Radar | Range, Doppler, 방위 | FMCW beat frequency | 클러터, 다중경로 | 장치의 거리·속도·각도 해상도 + 타겟·RCS·기상별 시험 |
| Wheel Odom. | 회전수 | $\Delta s = r \Delta\theta$ | 슬립 | 노면·하중·슬립 조건별 반복 궤적 |
| 기압계 | 기압 $P$ | $h = f(P)$ | 기상 변화 | 정지·온도·기상 조건별 반복 측정 |
| 자력계 | 자기장 $\mathbf{m}$ | $\tilde{\mathbf{m}} = \mathbf{R}\mathbf{m}_w + \mathbf{b} + \mathbf{n}$ | 하드/소프트 아이언 | 캘리브레이션 잔차 + 운용 장소의 자기 간섭 시험 |
| UWB | 거리 $d$ | $d = \|\mathbf{p} - \mathbf{a}\| + n$ | NLOS | 앵커별 LOS/NLOS 거리 잔차 |

관측 모델이 정확해도 센서 간 상대 위치와 시간 동기가 어긋나면 퓨전 성능이 무너진다. 센서를 함께 쓰려면 센서 간 기하·시간 관계를 정하는 **캘리브레이션(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 calibration rig 없이 평면 타깃으로 수행할 수 있어 널리 쓰이는 정식화이며, OpenCV의 `calibrateCamera()`로 같은 종류의 2D-3D 대응을 비선형 최적화할 수 있다.

패턴이 $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개 동차 성분은 스케일을 제외하고 4자유도이므로, 비퇴화 조건에서 최소 2장이 필요하다. 일반적인 5-파라미터 모델도 선형 해를 구하려면 서로 다른 평면 포즈가 최소 3개 필요하다. 다만 이 수는 대수적 최솟값일 뿐이다. 실제 촬영 수는 이미지 수 자체보다 시야 전역의 위치·거리·기울기를 얼마나 잘 포괄하는지와 추정 문제의 조건수로 정한다.

#### 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)
    residual = img_points[i].reshape(-1, 2) - img_points_proj.reshape(-1, 2)
    error = np.sqrt(np.mean(np.sum(residual ** 2, axis=1)))
    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 실전 팁: 좋은 캘리브레이션을 위한 조건

캘리브레이션의 품질은 데이터 수집 과정에 크게 좌우된다. 다음 조건은 특정 장수나 각도를 기계적으로 맞추기 위한 규칙이 아니라, 파라미터가 서로 구분되도록 관측을 구성하는 원칙이다.

**포즈 다양성 (Pose Diversity)**: 체커보드를 다양한 각도와 위치에서 촬영해야 한다. 구체적인 조건은 다음과 같다.
- 체커보드를 이미지의 상하좌우와 중앙에 배치해 시야 전역을 관측
- 정면에 가까운 포즈만 반복하지 말고 여러 축으로 충분히 기울여 초점 거리와 외부 파라미터를 구분
- 보드를 카메라에 가깝게/멀리 배치하여 다양한 스케일 확보
- 검출에 성공한 장수보다 포즈 다양성과 재투영 잔차의 공간적 패턴을 기준으로 추가 촬영 여부를 결정

**코너 정확도**: 사용하는 검출기가 별도의 서브픽셀 정제를 제공하지 않는다면 `cv2.cornerSubPix()` 같은 방법으로 코너를 정제한다. 코너 검출이 실패했거나 보드가 흐리고 반사된 이미지는 제외한다.

**조명 조건**: 균일한 조명은 코너의 대비를 안정시킨다. 강한 그림자와 반사(glare)는 검출 위치를 흔들 수 있으므로 무광 타겟을 사용하고 노출된 이미지를 직접 확인한다.

**재투영 오차 해석**: 절대 임계값 하나로 성공 여부를 판정하지 않는다. 해상도, 렌즈 모델, 타겟 크기, 검출기의 불확실성에 따라 기대 오차가 달라진다. 전체 RMS와 함께 이미지별 잔차, 영상 위치에 따른 방향성, 보정 전후 직선의 휨, 별도 검증 영상의 오차를 확인한다.

**이상치 이미지 감지**: 위 코드의 `errors`처럼 이미지별 재투영 오차를 비교한다. 큰 오차를 보이는 이미지는 자동으로 삭제하지 말고 검출 실패, 모션 블러, 반사, 타겟 평면 오차를 먼저 확인한다. 제외 전후의 파라미터 안정성과 별도 검증 오차를 비교해 제외 여부를 결정한다.

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

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

광각·어안 렌즈에서는 표준 radial-tangential 모델이 시야 가장자리의 투영을 충분히 표현하지 못할 수 있다. 적용 가능성을 FoV의 단일 경계값으로 나눌 수는 없으며, 렌즈 투영과 잔차 패턴에 맞춰 equidistant나 Kannala–Brandt 같은 모델을 선택한다.

**등거리 투영 모델 (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
)
# CALIB_CHECK_COND는 조건수가 나쁜 이미지가 하나라도 있으면 예외로 중단한다.
# 그때는 해당 이미지를 제외하거나 이 플래그를 뺀다.

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

# fisheye 모듈은 객체점을 (N, 1, 3), 이미지점을 (N, 1, 2)로 요구한다 (3.1.4의 (N, 3) 객체점을 변환)
obj_points_f = [o.reshape(-1, 1, 3).astype(np.float64) for o in obj_points]
img_points_f = [p.reshape(-1, 1, 2).astype(np.float64) for p in img_points]

ret, K_fisheye, D_fisheye, rvecs, tvecs = cv2.fisheye.calibrate(
    obj_points_f,
    img_points_f,
    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 toolbox](https://sites.google.com/site/scarabotix/ocamcalib-omnidirectional-camera-calibration-toolbox-for-matlab)는 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절)에 널리 쓰인다. 여러 카메라 모델과 batch 최적화를 지원하므로 카메라 intrinsic도 같은 도구 안에서 추정할 수 있다. OpenCV와의 정밀도 차이는 모델, 타깃 관측, 초기값과 최적화 설정으로 비교해야 한다.

**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 궤적 표현**: camera-IMU 캘리브레이션에서 연속 시간 궤적으로 비동기 관측을 처리
- **다양한 카메라 모델**: 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 점군을 계산할 수 있다: $[X,Y,Z,W]^\top = Q \cdot [u,v,d,1]^\top$로 동차 좌표를 구한 뒤, $\mathbf{P}_{3D} = [X/W,Y/W,Z/W]^\top$로 정규화한다.

---

## 3.3 Camera-LiDAR Extrinsic Calibration

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

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

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 위치를 추정하기 어려운 경우, 평면 제약만으로도 캘리브레이션이 가능하다. 카메라에서 검출한 코너와 알려진 체커보드 형상으로 카메라 좌표계의 평면을 추정한다. 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 = []

# 각 관측에서 (카메라 좌표계 평면, LiDAR 좌표계 평면) 쌍을 모은다. 평면은 n·x + d = 0
    planes_cam, planes_lidar = [], []
    for corners_2d, (normal_l, d_l) in 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)
        # 체커보드 평면을 카메라 좌표계로: 법선은 보드 z축, 거리는 보드 원점에서
        n_c = R_cam_board[:, 2]
        d_c = -n_c @ tvec.ravel()
        planes_cam.append((n_c, d_c))
        planes_lidar.append((normal_l, d_l))

# 회전: 법선 쌍 정렬 (Kabsch/SVD), n_c = R n_l
    N_c = np.stack([n for n, _ in planes_cam])
    N_l = np.stack([n for n, _ in planes_lidar])
    U, _, Vt = np.linalg.svd(N_c.T @ N_l)
    R_cam_lidar = U @ np.diag([1, 1, np.sign(np.linalg.det(U @ Vt))]) @ Vt

# 이동: 평면 거리 제약 n_c · t = d_l - d_c 를 최소자승으로 (평면 3개 이상 필요)
    b = np.array([d_l - d_c for (_, d_c), (_, d_l) in zip(planes_cam, planes_lidar)])
    t_cam_lidar, *_ = np.linalg.lstsq(N_c, b, rcond=None)

# 최종 결과는 비선형 최적화로 정제
    return R_cam_lidar, t_cam_lidar
```

**실전 주의사항**:
- 체커보드 크기는 거리, 각 해상도, 빔 패턴에 맞춰 한 관측에서 평면을 안정적으로 추정할 만큼 점이 들어오도록 정한다.
- 거리와 입사각을 바꾼 여러 관측을 모으고, 관측 수보다 평면의 방향·위치 다양성과 최적화의 조건수를 확인한다.
- 희소한 LiDAR에서는 타겟에 닿는 점이 적을 수 있으므로 실제 점 분포를 확인한 뒤 타겟 크기와 거리를 조정한다.

### 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 보정량을 회귀한다. 동작 범위는 학습 때 사용한 센서·장면·오정렬 분포에 의존한다. 따라서 새로운 장비에서는 보유 데이터로 검증하고, 필요하면 기하 최적화의 초기값으로 사용한다.

### 3.3.3 Koide et al. (2023) — 공개 Targetless Calibration 구현

[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로 초기 변환을 추정한다.

SuperGlue가 LiDAR 강도 렌더링과 카메라 RGB 사이의 후보 대응점을 만든다. 논문은 저자들의 데이터와 성공 판정 기준에서 이 초기화 단계의 성공률이 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 | 타겟 검출 자동화 | 구현이 요구하는 타겟과 센서 모델 |
| 온라인 extrinsic 추정 (VIO/LIO 상태 증강) | Online | X | 주행 중 추정 | 운동의 관측 가능성과 drift 감시 기준 |

최근 [MFCalib (2024)](https://arxiv.org/abs/2409.00992)은 깊이 연속·불연속 에지와 강도 불연속 에지를 함께 사용한다. 저자들은 제안한 빔 모델로 edge-inflation 현상을 다루고, 논문의 데이터셋과 지표에서 비교 방법보다 낮은 보정 오차를 보고했다.

Koide et al. (2023)은 초기 target-based 결과를 기준값으로 두고 targetless NID로 변화를 감시하는 운용안을 제시한다. 실제 재보정 여부는 NID 변화와 작업 오차 사이의 검증된 관계로 결정해야 한다.

---

## 3.4 Camera-IMU Extrinsic + Temporal Calibration

카메라와 IMU 사이의 공간적 변위(extrinsic)뿐 아니라 시간적 오프셋(temporal offset)도 동시에 추정해야 한다. VINS-Mono, OpenVINS 같은 VIO(Visual-Inertial Odometry) 시스템은 이 두 파라미터를 초기값으로 받아 운용 중 정제하고, Kalibr 같은 캘리브레이션 도구는 바로 이 두 값을 추정해 낸다.

### 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)^{-1} \cdot \mathbf{p}_w\right)
$$

여기서 $\mathbf{T}(t)$는 IMU(body)→월드 변환이며, $\mathbf{T}_{CB}$는 IMU(body)→카메라 외부 변환 (추정 대상)이다.

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)
#    - 타겟을 카메라 시야에 놓고, 센서 리그를 다양하게 움직임
#    - 모든 축에서 회전 + 병진 운동을 포함
#    - 고정된 시간보다 모든 축의 운동, 타겟 검출, 잔차의 조건을 충족할 만큼 수집

# 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. **타겟 가시성**: 카메라가 타겟을 지속적으로 검출하면서 각 축의 운동을 충분히 포함하도록 궤적을 설계한다.
4. **시작과 끝**: 정지 상태에서 시작/끝나야 IMU bias 초기화가 용이하다.

**결과 해석**: Kalibr 출력에서 확인해야 할 항목:
- `T_cam_imu`: 카메라-IMU 외부 파라미터 (4x4 변환 행렬)
- `timeshift_cam_imu`: 이 데이터에서 추정된 시간 오프셋 $t_d$와 부호 규약
- 재투영 오차 분포: 전체 평균뿐 아니라 시간·영상 위치에 따른 잔차 패턴과 이상치를 확인
- 가속도계/자이로 잔차: 노이즈 모델과 일치해야 함

### 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)
    # 큰 tau에서도 통계량이 남도록 최소 10개 cluster를 확보한다.
    max_n = max(1, N // 10)

# 로그 스케일로 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):
    """
    Allan variance 플롯에서 IMU 노이즈 파라미터를 추출.
    """
    adev = np.sqrt(allan_vars)

valid = (taus > 0) & (adev > 0) & np.isfinite(adev)
    taus, adev = taus[valid], adev[valid]
    slopes = np.gradient(np.log(adev), np.log(taus))

# -1/2 구간 전체에서 sigma(tau) = N / sqrt(tau)의 계수 N을 적합한다.
    white = np.abs(slopes + 0.5) < 0.1
    if not np.any(white):
        raise ValueError("No stable -1/2-slope region; collect or inspect more data")
    noise_coefficient = np.median(adev[white] * np.sqrt(taus[white]))

# 0 기울기 구간이 있을 때만 bias-instability 계수를 보고한다.
    flat = np.abs(slopes) < 0.1
    bias_instability = np.min(adev[flat]) / 0.664 if np.any(flat) else np.nan
    tau_min = taus[np.argmin(adev)]

return {
        'noise_coefficient': noise_coefficient,
        'bias_instability': bias_instability,
        'tau_min': tau_min
    }

# 사용 예시
# 1. 분석하려는 최대 cluster time에서 충분한 cluster가 남도록 정지 데이터를 녹화
# 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)

print(f"White-noise coefficient: {params['noise_coefficient']:.6f}")
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_coefficient'] / 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를 진동이 없는 단단한 표면 위에 놓는다. 기록 길이는 분석하려는 가장 긴 $\tau$에서 서로 겹치지 않는 cluster가 충분히 남도록 정한다. bias-instability 평탄부가 나타나지 않으면 시간을 늘리되, 고정된 시간만으로 식별을 보장하지는 않는다.
- **데이터시트와의 비교**: 제조사 데이터시트의 noise density 값과 Allan variance에서 추출한 값을 비교하여 센서 상태를 검증한다.
- **Kalibr와의 연결**: $-1/2$ 기울기 구간 전체를 적합해 white-noise 계수를 구한 뒤, Allan 편차의 정의와 단위가 Kalibr의 연속시간 noise-density 규약과 일치하는지 확인한다. $\tau=1$초에 가장 가까운 표본 하나를 그대로 복사하면 기울기 이탈과 단위 규약을 놓칠 수 있다.
- **온도 안정화**: 전원을 켠 뒤 센서 온도와 출력 평균이 안정된 구간에서 측정을 시작하고 온도를 함께 기록한다. 충분한 열적 평형에 도달하는 데 걸리는 시간은 IMU 패키징 형태와 외기 노출 상태에 좌우된다.

---

## 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)의 해법**:

회전축과 회전각을 각각 $\hat{\mathbf{a}}_A, \alpha$와 $\hat{\mathbf{a}}_B, \beta$로 두고, $\mathbf{p}_A = 2\sin(\alpha/2)\hat{\mathbf{a}}_A$, $\mathbf{p}_B = 2\sin(\beta/2)\hat{\mathbf{a}}_B$를 사용하면:

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

여기서 $\mathbf{r}_X = \tan(\theta_X/2)\hat{\mathbf{a}}_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  # 작은 회전은 무시

# Scaled quaternion vector parts: 2 sin(angle / 2) * axis
        a_prime = 2 * np.sin(alpha / 2) * rA / alpha
        b_prime = 2 * np.sin(beta / 2) * rB / beta

# skew(a' + b') * rX = b' - a'
        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(b_prime - a_prime)

C = np.vstack(C)
    d = np.concatenate(d)

# 최소자승 풀이
    rX, _, _, _ = np.linalg.lstsq(C, d, rcond=None)

# 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는 운용 중 LiDAR-IMU extrinsic을 추정할 수 있지만, 좋은 초기값과 충분한 운동이 필요하다. [**LI-Init**](https://arxiv.org/abs/2202.11006)은 시간 오프셋, extrinsic, 중력, IMU bias를 초기화한 뒤 FAST-LIO2로 넘기는 별도의 초기화 모듈이며, 세 단계로 진행된다:

1. IMU 데이터만으로 자세(attitude)를 추정하고, LiDAR 매칭으로 포즈를 추정
2. 두 추정의 차이로부터 상대 변환을 반복적으로 정제
3. 결과를 FAST-LIO2에 넘김. 이후의 온라인 정제는 FAST-LIO2의 ESIKF가 상태 벡터에 포함된 LiDAR-IMU extrinsic을 갱신하는 방식으로 이뤄진다

별도의 타겟이나 추가 센서 없이, LIO 기동 시 초기화 모듈이 외부 파라미터를 추정한다. 충분한 운동 여기가 필요하며, 공개 구현은 초기 정지 구간과 초기화 후 15–30초의 온라인 정제를 권장한다.

**장점**: 별도의 캘리브레이션 타겟이나 추가 센서가 필요 없음. 현장에서 초기화 가능.
**단점**: 초기 운동이 불충분하면 수렴하지 않거나 부정확할 수 있음. 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를 쓰는 시스템에서는 LiDAR-IMU 외부 파라미터와 함께 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 좌표계에서 계산
- 대응 평면 쌍으로부터 상대 변환 추정

법선 벡터가 비공면(non-coplanar)인 평면 대응이 최소 3개 필요하다.

지금까지 다룬 캘리브레이션(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을 보정하지 않으면 위치 오차가 발생한다. 예를 들어 회전축에 수직인 1m lever arm을 기준 자세에서만 보정하고 이후의 10 deg 회전을 반영하지 않으면, 약 17cm의 위치 오차가 생긴다.

### 3.7.2 Lever Arm 추정 방법

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

가장 직관적인 방법은 줄자, 레이저 거리 측정기 등으로 직접 측정하는 것이다. 필요한 측정 정밀도는 안테나와 IMU 사이 거리, 플랫폼의 회전 범위, 허용 위치 오차로 계산한다. 수 cm의 물리 측정 오차가 허용 오차 예산에 들어오면 직접 측정으로 충분하고, 그렇지 않으면 정밀 측량이나 추정을 사용한다.

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

EKF 상태 벡터에 lever arm $\mathbf{l}$을 포함하여 온라인으로 추정한다. GNSS 관측 모델에서 lever arm이 관측 가능(observable)하려면 충분한 회전 운동이 필요하다. 자세가 일정한 직선 주행에서는 $\mathbf{R}\mathbf{l}$이 상수 편향으로 흡수되어 lever arm의 세 성분이 모두 관측되지 않고, yaw 회전만 있으면 회전축과 나란한 수직 성분($l_z$)이 관측되지 않은 채 남는다.

**방법 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

공유 clock·trigger·timestamp 경로를 명확히 구성할 수 있다면 하드웨어 동기화가 우선 선택지다. 다만 장치별 latency와 timestamp 기준 시점은 별도로 검증해야 한다.

**Trigger 기반 동기화**:

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

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

**PPS (Pulse Per Second)**:

GNSS 수신기는 GNSS 시간에 동기된 1Hz 전기 펄스(PPS)를 출력할 수 있다. 수신기가 상승 에지를 GNSS 초 경계에 놓도록 설정됐다면 이 에지를 공통 기준으로 사용할 수 있다. 수신기 설정, 케이블·입력 회로 지연, 각 장치의 timestamp 기준점은 별도로 확인한다.

```
                   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는 이더넷 네트워크에서 클록을 동기화하는 프로토콜이다. 하드웨어 타임스탬핑과 PTP-aware 네트워크를 갖추면 마이크로초 이하 오차도 가능하지만, 실제 정확도는 장비와 네트워크 구성에 좌우된다.

**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와 산업용 카메라는 PTP와 하드웨어 타임스탬핑을 지원한다. 지원 여부와 timestamp 기준점은 모델별 데이터시트에서 확인해야 한다.

### 3.9.3 Software Synchronization

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

**Host Clock 기반**: 센서 데이터가 호스트 컴퓨터에 도착했을 때 시스템 시계로 타임스탬프를 찍는다. 이 값에는 USB·네트워크·드라이버 queue의 지연과 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) 조건**: 시간 오프셋이 관측 가능하려면 플랫폼이 충분한 가속 운동을 해야 한다. 등속 직선 운동에서는 시간 오프셋을 추정할 수 없다(시간 이동이 공간 이동과 구별 불가).

OpenVINS, VINS-Mono 같은 현대의 VIO 시스템이 이 방법의 변형을 구현하고 있다. 오프라인 도구인 Kalibr(3.4.2절)는 연속시간 B-spline 배치 최적화로 같은 값을 따로 추정한다.

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

| 구성 조건 | 우선 검토할 방법 | 반드시 확인할 항목 |
|-----------|------------------|---------------------|
| 공통 트리거 입력과 clock을 배선할 수 있음 | 하드웨어 트리거 + 공통 clock/PPS | 노출·샘플·스캔 중 timestamp 기준 시점, 고정 latency |
| 모든 장치와 스위치가 PTP 및 하드웨어 timestamp를 지원 | PTP | grandmaster 선택, PTP profile, 경로 비대칭, lock 상태 |
| 장치 clock은 읽을 수 있으나 공통 신호가 없음 | clock 변환 + 오프라인/온라인 offset 추정 | clock drift, 운동의 관측 가능성, offset의 시간 변화 |
| 수신 시각만 사용할 수 있음 | host timestamp를 임시 사용하고 jitter 측정 | USB·네트워크 queue 지연; 요구 오차를 만족하지 못하면 장비 구성 변경 |

**하드웨어 동기화가 필요한 구성**: PPS와 PTP를 함께 사용해 여러 센서를 GNSS 기준 시간에 맞추는 방식이 있다.

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

하드웨어 동기화를 구성할 수 있으면 우선 검토하되, 실제 timestamp 의미와 고정·가변 latency를 측정한다. 소프트웨어 offset 추정은 하드웨어 구성을 보완하거나 공유 trigger가 불가능한 경우에 사용한다.

---

## 3.10 챕터 요약

표는 센서 조합별 캘리브레이션 파라미터와 방법을 정리한다.

| 캘리브레이션 유형 | 추정 파라미터 | 방법 | 최소 요구 조건 |
|----------------|-------------|----------|-------------|
| Camera intrinsic | $\mathbf{K}, \mathbf{d}$ | Zhang's method | 시야 전역을 포괄하는 다양한 타겟 포즈 |
| Stereo extrinsic | $\mathbf{R}, \mathbf{t}$ | 공유 타겟 + stereoCalibrate | 두 카메라가 동시에 검출한 다양한 포즈 |
| Camera-LiDAR | $\mathbf{T}_{CL}$ | Target-based / NID | 여러 시점의 타겟 관측 / 자연 장면 |
| Camera-IMU | $\mathbf{T}_{CI}, t_d$ | Kalibr (B-spline) | 충분한 시간의 다양한 운동 |
| 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.4의 **상태 추정 이론**은 칼만 필터에서 팩터 그래프까지 이 계산을 설명한다.

---

# Ch.4 — State Estimation 이론

센서의 관측 모델과 캘리브레이션 파라미터는 로봇의 상태(위치, 자세, 속도 등)를 추정하는 **알고리즘**의 입력이다. 칼만 필터, 팩터 그래프, IMU 사전적분은 Ch.6-8의 odometry/fusion 시스템에서 이 입력을 처리한다.

> 상태 추정(state estimation)은 Bayesian filtering에서 Kalman 필터 계열과 Particle Filter를 거쳐 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{H}_k^{-1}$ 표기는 $\mathbf{H}_k$가 정사각·가역인 특수한 경우에만 성립한다
- $\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이면 그 측정 잔차가 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$으로 쓰면 수치적으로 더 안정적이다. $\mathbf{P}_{k|k-1}$과 $\mathbf{R}_k$가 양의 준정부호일 때 Joseph form은 이 대수적 성질을 엄밀히 보존한다. 실제 구현에서는 부동소수점 반올림 오차를 방지하기 위해 주기적 대칭화나 $LDL^\top$ 분해 기반 풀이를 병행한다.

#### 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)는 IMU가 포함된 상태 추정에서 널리 쓰이는 정식화다. MSCKF·OpenVINS 같은 필터 기반 VIO와 FAST-LIO 계열은 오차 상태 또는 그에 가까운 매니폴드 필터 정식화를 채택했다. 한편 [VINS-Mono](https://doi.org/10.1109/TRO.2018.2853729)의 경우 필터 대신 sliding-window 기반 비선형 최적화를 중심으로 동작한다.

#### 왜 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}}$의 "+"는 유클리드 덧셈이다. 쿼터니언 네 성분에 그대로 증분을 더하면 단위 노름 제약을 벗어난다. 정규화와 공분산 투영을 일관되게 설계한 직접 쿼터니언 EKF도 가능하지만, 단순히 갱신 뒤 값만 정규화하면 제약 방향의 불확실성을 잘못 처리할 수 있다.

**특성 2: 오차 상태는 "거의 0"**

오차 상태(error state) $\delta\mathbf{x} = \mathbf{x} \boxminus \hat{\mathbf{x}}$는 갱신 뒤 명목 상태에 주입되고 0으로 리셋된다. 명목 궤적이 실제 상태에 충분히 가까우면 작은 접선 공간에서 선형화할 수 있다. 다만 리셋 자체가 작은 선형화 오차를 보장하지 않으므로, 초기화·관측 가능성·모델 오차를 함께 확인해야 한다.

**문제 3: 명목 상태와 오차 상태의 서로 다른 동역학**

큰 운동 변화는 명목 상태에서 적분하고, 그 주변의 작은 오차는 오차 상태에서 추적한다. 이 구조에서는 고주파 IMU로 예측하면서 더 낮은 주파수의 외부 관측이 도착할 때 오차를 보정할 수 있다.

#### ESKF 구조

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

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

2. **오차 상태(Error State)** $\delta\mathbf{x}$: 명목 상태와 실제 상태의 국소 차이. 칼만 필터로 추정한다. 명목 상태가 참값 근방에 머무는 동안 접선 공간의 1차 선형 근사가 성립하며, 상태 주입 직후에는 reset Jacobian을 적용해 오차 공분산 좌표계를 재정렬한다.

실제 상태는 두 상태의 합성(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 상태에 직접 넣는 방식도 가능하지만, 정규화와 공분산의 제약 방향을 일관되게 처리해야 한다.

#### 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. 프로세스 잡음 $\mathbf{Q}_k$가 포함된 예측 평균과 공분산에서 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의 장단점

**장점**:
- 자코비안 계산이 불필요하다. 복잡한 관측 모델(예: 카메라 투영 + 왜곡)에서 큰 이점.
- 비선형 함수를 통과시킨 sigma point로 평균과 공분산의 곡률 효과를 근사한다. 정확도 차수는 분포, 함수, sigma-point 파라미터에 따라 달라진다.
- 구현이 EKF보다 간단할 수 있다 (자코비안 유도 대신 함수 호출만 필요).

**단점**:
- $2n+1$개 sigma point 각각을 비선형 함수에 통과시켜야 하므로, 상태 차원 $n$이 클 때 연산량이 증가한다.
- 매니폴드 위의 상태(SO(3) 등)를 다루려면 sigma point의 생성과 통계 계산을 매니폴드 연산으로 대체해야 하므로 구현이 단순하지 않다.
- IMU 상태 추정에서 ESKF가 자주 선택되는 이유: 접선 공간 오차로 매니폴드를 다루고, sigma point를 모두 전파하지 않아 계산량을 줄일 수 있다. 어느 필터가 더 정확한지는 모델, 초기 오차, 조정값에 따라 달라진다.

### 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는 다수의 LiDAR point-to-plane 잔차를 상태 갱신에 넣고 반복 재선형화하는 매니폴드 IEKF를 사용한다. 한 번의 선형화에 비해 잔차의 비선형성을 충실히 반영할 수 있으나, 최종적인 정확도 향상 폭과 수렴 반복 수는 초기 추정 오차, 주변 환경의 기하 구조, 수렴 판정 임계치에 좌우된다.

---

## 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를 타서 리샘플링한다. 구현 비용이 낮고 multinomial resampling보다 분산을 줄이는 경우가 많지만, 모든 가중치 배열에서 최소 분산을 보장하지는 않는다.

**Stratified Resampling**: 각 층에서 독립 균등 난수를 사용. Systematic처럼 구간을 층으로 나누되, 하나의 난수를 공유하지 않고 각 층에서 따로 추출한다.

```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를 독립적으로 운영할 수 있다. 이것이 FastSLAM의 효율성의 핵심이다. 대응 랜드마크가 알려진 관측 한 건에 대해, 균형 트리로 맵을 관리하는 FastSLAM의 갱신 비용은 입자 수 $N$과 랜드마크 수 $M$에 대해 $O(N \log M)$이다. EKF-SLAM의 대응 갱신은 $O(M^2)$이다.

### 4.3.5 Particle Filter의 한계와 현재 위치

PF의 큰 한계는 **차원의 저주(curse of dimensionality)**다. 일반적인 고차원 문제에서는 의미 있는 근사를 위한 입자 수가 매우 빠르게 늘어난다. VIO/LIO의 상태는 보통 자세·속도·바이어스를 함께 포함하므로, 강한 proposal이나 Rao–Blackwellization 없이 전체 상태를 입자로 표현하는 방식은 실시간 구현에서 대개 비효율적이다.

따라서 현대 로봇 시스템에서 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는 평균적인 Bayes risk를 줄일 수 있다. 그러나 개별 궤적의 실제 오차나 근사 최적화 결과가 filter보다 항상 작다는 보장은 없다. 실시간 추정에는 filter뿐 아니라 incremental smoother와 fixed-lag smoother도 사용되며, batch smoother는 후처리에도 쓰인다.

### 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 smoothing에 가까운 구조다. 윈도우 내 키프레임을 함께 재선형화할 수 있다는 장점이 있지만, 정확도 비교는 계산 budget, 모델과 데이터셋을 맞춰야 한다.

### 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)가 이 논증을 체계적으로 제시했다: 같은 계산량이 주어지면 정확도를 올리는 데는 프레임 수보다 특징점 수를 늘리는 쪽이 유리하고, 비용이 특징점 수에 선형인 bundle adjustment가 3차인 filtering보다 그 점에서 효율적이다.

**2. 일관성(Consistency) 문제**

SLAM에는 외부 기준이 없을 때 전역 좌표계를 함께 옮겨도 관측이 같아지는 gauge freedom이 있다. unobservable direction의 수는 2D SLAM, 3D SLAM, visual-inertial 추정에서 각각 다르며, 예를 들어 VIO는 보통 전역 위치 3축과 중력축 yaw의 네 방향을 갖는다. 구현에서는 첫 포즈나 일부 자유도를 고정해 gauge를 정하지만, EKF가 매 시점 다른 추정치에서 선형화하면 이 방향에 허위 정보가 생겨 inconsistency가 발생할 수 있다. FEJ(First-Estimate Jacobian)와 observability-constrained 계열은 선형화가 null space를 더 잘 보존하도록 설계한다.

**3. 스케일러빌리티(Scalability)**

EKF-SLAM에서 $M$개 랜드마크의 공분산 행렬 크기는 $O((n + 3M)^2)$이고, 각 갱신의 계산량은 $O((n+3M)^2)$이다. 맵이 커지면 감당할 수 없다.

Graph-based SLAM에서는 정보 행렬(information matrix, Hessian)이 **희소(sparse)**하다. 각 변수(포즈, 랜드마크)는 직접 관측한 다른 변수들과만 연결되므로, 정보 행렬의 많은 항이 0이다. 희소성을 이용하면 dense 풀이보다 계산을 줄일 수 있지만, 실제 복잡도는 그래프의 연결 구조, 소거 순서, 루프 클로저가 만드는 fill-in에 따라 달라진다.

**4. 루프 클로저(Loop Closure) 처리의 자연스러움**

필터 기반 시스템에서 루프 클로저를 처리하려면 과거 상태의 공분산 정보를 유지해야 하며, 이것이 계산량을 크게 증가시킨다. Graph-based 시스템에서는 루프 클로저를 단순히 새로운 factor(constraint)로 추가하고 전체 그래프를 재최적화하면 된다.

| 관점 | Filtering (EKF) | Optimization (Graph) |
|------|----------------|---------------------|
| 선형화 | 기본 EKF는 갱신 시 한 번 | batch/smoother는 반복 재선형화 가능 |
| 과거 상태 | 기본 필터는 제거하지만 상태 증강 가능 | full smoothing은 유지, fixed-lag는 마지널라이즈 |
| 루프 클로저 | 과거 상태·상관을 유지해야 함 | 새 factor를 추가한 뒤 영향 구간 갱신 |
| 행렬 구조 | 공분산은 대체로 dense | factor/Hessian의 희소성을 활용 가능 |
| 계산량 | 상태와 관측 구조에 의존 | 그래프 구조·소거 순서·fill-in에 의존 |
| 일관성 | 선형화·관측 가능성 처리가 필요 | 재선형화만으로 보장되지 않으며 gauge·마지널라이제이션 처리가 필요 |

필터 기반도 계속 쓰인다. [MSCKF (Mourikis & Roumeliotis, 2007)](https://ieeexplore.ieee.org/document/4209642)와 OpenVINS는 계산량을 제한해야 하는 환경에 적용되며, 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$에 대한 "확률적 잠재 함수(potential)" 또는 "확률적 구속 조건"
- $\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에 함께 연결된 변수들 사이의 결합을 나타낸다. 각 factor가 소수의 변수만 연결하는 SLAM 문제에서는 이 구조로 인해 $\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차 미분 항을 생략하고 $\mathbf{J}^\top\mathbf{J}$로 Hessian을 근사한다. 초기값이 나쁘거나 비선형성이 심하면 발산할 수 있다. **Levenberg-Marquardt(LM)** 방법은 GN과 경사 하강법의 성격을 조절하도록 감쇠 항을 추가한다:

$$(\mathbf{H} + \lambda \mathbf{I}) \Delta\mathbf{X} = -\mathbf{b}$$

- $\lambda$가 작으면 → GN에 가까움 (해 근처의 조건이 좋으면 빠르게 수렴)
- $\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하게 최적화를 수행한다.

iSAM2는 네 가지 장치를 사용한다:

1. **Bayes Tree**: factor graph의 elimination 결과를 방향성 트리로 표현. 각 노드는 clique(변수의 부분집합)에 대한 조건부 확률 밀도를 저장.

2. **Incremental Update**: 새 factor가 추가되면, 영향을 받는 clique만 재계산. 트리의 대부분은 변경되지 않음.

3. **Fluid Relinearization**: 선형화 지점이 현재 추정치와 크게 달라진 변수만 선택적으로 재선형화. 주기적 batch 처리 불필요.

4. **변수 순서 재정렬(Variable Reordering)**: 새 변수/factor 추가 시 전체 재정렬 없이 영향받는 부분만 지역적으로 재정렬.

GTSAM은 iSAM2를 구현하며, [LIO-SAM](https://arxiv.org/abs/2007.00258) 같은 시스템이 이를 백엔드로 쓴다. VINS-Mono는 GTSAM/iSAM2가 아니라 Ceres 기반 sliding-window 최적화를 사용한다.

> **최근 동향 — Continuous-Time Factor Graph**: 이산 키프레임 기반 factor graph를 **연속 시간(continuous-time)**으로 확장하는 연구가 활발하다. [Burnett 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`) | 사용자 정의 | 사용자 정의 |
| 자동 미분 | Expression 기반 지원 (수치 미분도 가능) | 자동 미분 (ceres::AutoDiffCostFunction) | AutoDifferentiation 기반 지원 |
| 언어 | C++ (Python 바인딩) | C++ | C++ |
| 대표 사용처 | LIO-SAM | Cartographer, VINS-Mono | ORB-SLAM3, 많은 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$ 사이에 대략 7~100개의 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 + \sum_{k=i}^{j-1}\left[\mathbf{v}_k \Delta t + \frac{1}{2}\left(\mathbf{R}_k(\tilde{\mathbf{a}}_k - \mathbf{b}_a) + \mathbf{g}\right)\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 \frac{\partial \Delta\bar{\mathbf{R}}_{ik}}{\partial \mathbf{b}_g} - \text{Jr}((\tilde{\boldsymbol{\omega}}_k - \mathbf{b}_g)\Delta t) \Delta t$$

여기서 $\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차 보정을 사용할 수 있다. 보정 뒤 잔차나 바이어스 변화가 구현의 허용 범위를 넘으면 저장한 IMU 표본으로 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   # 새 항에는 dR.T가 곱해지지 않는다
        
        # --- 공분산 전파 (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))
        # 연속 시간 노이즈 밀도 → 이산 시간 공분산: 밀도²/dt (B에 dt가 이미 곱해져 있다)
        Sigma_eta[0:3, 0:3] = np.eye(3) * self.sigma_g**2 / dt
        Sigma_eta[3:6, 3:6] = np.eye(3) * self.sigma_a**2 / dt
        
        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-up 프레임용. 내부 중력 벡터는 (0, 0, -9.81)
    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 생성
    # ImuFactor는 i, j의 pose·velocity와 i의 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}^*$는 이 fill-in 때문에 $\mathbf{H}_{rr}$보다 조밀해질 수 있으며, 기존의 간접 의존성이 변수 간 직접 결합으로 표현된다.

```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 정리가 이론적 근거이며, 일반 비선형 시스템에서는 적분을 근사한다. Batch smoothing과 factor graph는 관련 확률 모델을 다른 계산 구조로 푼다.

2. **Kalman Filter 계열**은 가우시안 근사를 통해 사후 분포를 평균과 공분산으로 추적한다. EKF는 1차 테일러 선형화, ESKF는 오차 상태 공간에서의 선형화로 리 다양체 회전을 처리한다. 한편 UKF의 sigma point 표본 변환이나 IEKF의 반복 재선형화는 강한 비선형성에 대응하는 대표 기법이다. IMU가 포함된 센서 퓨전 필터에서는 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차 테일러 확장으로 보정하며, 바이어스 변화가 과도하게 커질 때만 사전 적분을 재수행한다.

7. **Marginalization**은 제거할 변수의 선형화 정보를 prior로 옮긴다. Schur complement를 사용하며, FEJ는 관측 불가능 방향을 보존해 일관성 저하를 줄이는 한 방법이다.

같은 이론은 VIO(Ch.6), LIO(Ch.7), 멀티센서 퓨전(Ch.8)의 시스템 설계와 구현에도 쓰인다.

> **2024-2025 연구 방향**: Equivariant Filter와 Invariant EKF는 Lie 군의 대칭을 이용해 특정 시스템과 가정에서 오차 동역학·일관성·수렴 분석을 개선한다. 연속 시간 최적화에서는 Gaussian Process motion prior를 사용한 factor graph가 비동기 멀티센서 퓨전에 쓰인다. [AI-Aided Kalman Filters (Shlezinger et al., 2024)](https://arxiv.org/abs/2410.12289)처럼 칼만 이득이나 모델 일부를 학습하는 접근도 연구되며, 분포 변화와 안전성 검증이 남는다.

---

# 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 외부 파라미터를 추정하려면 두 센서 관측 사이의 대응을 찾아야 한다. target을 쓰는 방법은 같은 물리적 점을 대응으로 삼고, targetless MI 기반 방법(§5.4)은 겹치는 영역의 통계적 의존성을 대응으로 삼는다.
- **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)이라 한다. 아래 기하 식에서는 이 점을 동차 좌표 $(u, v, 1)^\top$로 쓴다. $3 \times 3$ 행렬과의 곱과 $\mathbf{K}^{-1}$ 적용이 모두 동차 좌표에서 정의되기 때문이다.

두 이미지 사이의 기하학적 관계는 **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)

# 임계값 적용 — cornerHarris는 응답 맵만 돌려주므로 NMS는 별도 단계가 필요하다
corners = harris_response > 0.01 * harris_response.max()
```

#### FAST (Features from Accelerated Segment Test, 2006)

FAST는 Harris의 정확성보다 **속도를 극한으로 추구**한 검출기다. 로봇 비전에서 수십 FPS로 특징점을 검출해야 하는 실시간 요구를 해결하기 위해 [Rosten & Drummond (ECCV 2006; 링크는 Rosten, Porter & Drummond의 확장판, TPAMI 2010)](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개 이상이 모두 밝거나 모두 어두운 조건을 만족하지 못하면 즉시 기각. 이 단축 검사는 $N=12$에서 성립하고 $N=9$ 같은 변형에는 그대로 쓸 수 없어서, 아래의 결정 트리 학습이 그 자리를 대신한다.

$$\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)가 제안한 특징점 검출·기술 알고리즘이다. 스케일과 평면 내 회전에 불변하도록 설계됐고, 정규화된 gradient descriptor 덕분에 일정 범위의 조명 변화와 affine 왜곡에도 강건하다. 오랫동안 특징점 매칭의 대표 기준선으로 쓰였으며 미국 특허는 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 응답의 합과 절댓값 합.

integral image와 box filter로 Hessian 근사를 빠르게 계산하도록 설계됐다. 속도와 정확도 차이는 구현, 영상 크기, 하드웨어에 따라 달라진다.

SURF는 과거 특허 문제로 채택률이 낮아졌다. 현재 실시간성이 중요한 파이프라인에서는 ORB를 주로 쓰고, 정밀한 복원이 필요한 분야에서는 SIFT나 최신 학습 기반 정합 기법을 활용한다.

### 5.2.3 Binary Descriptors: BRIEF, ORB, BRISK

이진 디스크립터는 패치 내 점 쌍의 밝기를 비교하여 0/1 비트열을 만든다. XOR와 popcount로 Hamming distance를 계산할 수 있어 부동소수점 거리보다 낮은 비용으로 구현되는 경우가 많지만, 실제 속도비는 디스크립터 길이와 라이브러리·하드웨어에 따라 달라진다.

| 디스크립터 | 비트 수 | 특징 |
|-----------|--------|------|
| 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로 계산한다. popcount는 워드 단위 명령이므로 256비트 디스크립터는 64비트 워드 4개의 XOR·popcount와 합산으로, 512비트는 그 두 배로 처리된다:

$$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 (속도 지향, float)
    ↓ 실시간 요구
FAST (2006)       — 극한 속도 검출 (descriptor 없음)
    ↓ 검출+기술 통합
ORB (2011)        — oFAST + rBRIEF, 256-bit binary (Hamming 매칭)
```

이러한 속도와 정확도의 상충 관계는 딥러닝 기반 모델에서도 주요 설계 쟁점이다. 예컨대 SuperPoint는 검출과 기술을 함께 학습해 시점·조명 변화에 강건한 정합을 겨냥했고, 속도는 GPU 환경에서 640×480 기준 약 70 FPS(§5.5.1)로 하드웨어 조건에 묶인다.

---

## 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점, Essential matrix는 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은 **매칭 품질(예: 디스크립터 거리)** 순으로 정렬한 뒤 상위 매칭부터 점진적으로 샘플링한다.

직관: 순위 점수가 인라이어 확률과 상관된다면 상위 매칭부터 모델을 시도해 유효한 가설을 더 일찍 찾을 수 있다. 이점은 점수의 보정과 실제 인라이어 순위에 의존한다.

#### MAGSAC / MAGSAC++ (2019/2020)

Barath et al.이 제안. RANSAC의 핵심 문제 중 하나는 **임계값 $t$의 수동 설정**이다. MAGSAC은 이를 자동화한다:

- 임계값 $\sigma$를 $[0, \sigma_{\max}]$ 구간에서 marginalize하여 모델의 품질을 구한다:

$$Q(\theta) = \int_0^{\sigma_{\max}} q(\theta, \sigma) f(\sigma) d\sigma$$

여기서 $q(\theta, \sigma)$는 임계값 $\sigma$에서의 모델 $\theta$의 품질, $f(\sigma)$는 임계값의 사전 분포.

- MAGSAC++는 이를 더 효율적으로 구현하고, $\sigma$-consensus 기반 가중 최소자승 피팅을 추가.
- 인라이어/아웃라이어 판정 임계값이 사라지므로 그 선택에 대한 민감도가 크게 줄어든다. 다만 적분 상한 $\sigma_{\max}$는 여전히 지정해야 하며, 아래 코드가 `ransacReprojThreshold`를 넘기는 것도 그 상한에 해당한다.

```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)   — 무작위 표본 기반 robust estimation의 대표적 출발점
    ↓ 사전 정보 활용
PROSAC (2005)   — 매칭 품질 기반 진행적 샘플링
    ↓ 임계값 자동화
MAGSAC++ (2020) — 인라이어 임계값을 없앤 로버스트 추정
    ↓ 학습 기반 제거
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** 같은 미분이 필요 없는(gradient-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는 **키포인트 검출과 디스크립터 추출을 단일 fully convolutional network에서 함께 계산**하고, Homographic Adaptation으로 자기지도 학습한다.

#### 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차원 디스크립터 맵을 출력
- 검출된 키포인트 위치에서 bicubic 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.

**논문 보고 성능**: 단일 포워드 패스로 검출과 기술을 함께 수행한다. SuperPoint 원 논문에서는 저자들의 GPU 환경에서 640×480 해상도 기준 약 70 FPS의 처리 속도를 제시했다. 실무 적용 시의 실제 처리량은 타깃 하드웨어 스펙과 추출 후보점 수에 맞춰 개별적으로 벤치마크해야 한다.

```python
import torch
# SuperPoint 사용 예제 (cvg/LightGlue 패키지. kornia.feature에는 SuperPoint 클래스가 없다)
from lightglue import SuperPoint

# 모델 로드
sp = SuperPoint(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({'image': img_tensor})   # 반환 키: keypoints, keypoint_scores, descriptors
    keypoints = pred['keypoints']        # (1, N, 2)
    descriptors = pred['descriptors']    # (1, 256, N)
    scores = pred['keypoint_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은 **같은 특징 맵에서 검출과 기술을 동시에 수행**한다.

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)을 주어 검출기를 학습한다.

DISK는 매칭 정확도를 직접 최적화하여 검출과 매칭을 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는 키포인트 매칭과 unmatched point rejection을 **그래프 신경망(GNN), 어텐션, optimal transport**로 함께 푸는 학습 기반 matcher다.

#### 문제 정의: 부분 할당(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는 **적응적 연산량 조절(adaptive computation)**을 사용한다. 논문은 평가 설정의 평균에서 SuperGlue와 비슷한 정확도와 3–5배 속도 향상을 보고했으며, 실제 비율은 키포인트 수와 하드웨어에 따라 달라진다.

#### SuperGlue의 문제점 진단

- 공개 SuperGlue 구성은 고정된 GNN 깊이와 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 등 다양한 로컬 특징 검출기와 호환되도록 범용 설계.

**성능**: LightGlue 논문은 평가 설정에서 SuperGlue와 비슷한 정확도를 유지하면서 평균 **3–5배**, 쉬운 image pair에서는 그보다 큰 속도 향상을 보고한다. 실제 배율은 keypoint 수와 하드웨어에 따라 달라진다.

```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는 **검출기 없이 두 이미지 간 밀집 매칭을 수행하는 트랜스포머 기반 아키텍처**다.

#### 아키텍처

**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)$ 복잡도를 달성한다.

linear attention은 계산을 줄이는 대신 attention의 표현력을 제한한다. 이어지는 QuadTree Attention과 ASpanFormer는 그 제한을 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. 이를 반복하여 관련 영역에만 집중하는 계층적 어텐션 실현

이를 통해 standard attention의 $O(N^2)$ 비용을 $O(N)$으로 줄이면서 정확도를 유지한다. 비교의 기준은 standard attention이다. LoFTR가 쓰는 linear attention도 이미 $O(N)$이며, 원 논문은 linear attention과 비슷한 연산량으로 정확도를 높였다고 보고한다.

### 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는 DINOv2 특징과 detector-free dense matching을 결합한다. LoFTR 계열과 구별되는 두 가지 설계를 쓴다.

#### Foundation Model 활용

LoFTR가 처음부터(scratch) 학습한 것과 달리, RoMa는 **사전학습된 DINOv2 ViT-Large를 특징 추출기로 사용**한다 (가중치 동결).

DINOv2는 대규모 자기지도학습으로 학습된 범용 시각 특징으로, 풍부한 의미적(semantic) 정보를 담고 있다. RoMa는 이 특징을 고정한 채 매칭 로직만 학습한다.

#### 아키텍처

**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)$$

분포를 예측하면 모호한 매칭에 낮은 확률을 배정해 손실이 한 대응에 지배되지 않게 할 수 있다. 다만 강건성은 분포 계열이 정한다. Gaussian 계열은 꼬리가 가벼워 큰 오차에 제곱 벌점을 주므로, 무거운 꼬리 분포나 명시적 outlier 성분, 또는 회귀를 분류로 바꾸는 설계를 함께 써야 잘못된 ground-truth의 영향이 실제로 줄어든다.

#### 성능

RoMa 원 논문의 relative-pose 실험에서는 MegaDepth·ScanNet에서 비교한 LoFTR·ASpanFormer 계열보다 높은 AUC를 보고했다. 개선 폭은 dataset과 각도 threshold에 따라 다르며, 논문은 큰 viewpoint·appearance 변화가 포함된 WxBS에서도 36% 상대 향상을 강조한다.

```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 (Wang 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 설정에서 이전 비교 방법보다 VCRE AUC가 30%p 높았다고 보고한다.

**VGGT (Wang et al., 2025)**: [VGGT](https://arxiv.org/abs/2503.11651) (Visual Geometry Grounded Transformer)는 CVPR 2025 Best Paper로, 하나 이상의 이미지로부터 camera parameter, pointmap, depth map, 3D point track을 **단일 feed-forward pass로 동시에 추론**한다. 원 논문은 자체 평가 설정에서 image reconstruction을 1초 이내에 처리하고, 비교한 geometry-optimization 기반 방법보다 높은 여러 3D task 성능을 보고한다.

이들 방법은 2D 대응점을 먼저 구한 뒤 3D를 복원하던 순서를 바꾸어, 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: PFH의 가속 버전

PFH는 반경 $r$ 내의 이웃 쌍 특성을 계산하므로 포인트당 $O(k^2)$의 계산이 필요하다. FPFH는 query와 각 이웃 사이의 특성으로 SPFH를 구하고 이웃의 SPFH를 가중 결합하여 포인트당 $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 reconstruction에서 자동 수집한 correspondence로 local volumetric descriptor를 학습한 초기의 영향력 있는 시스템이다.

- **학습 데이터**: 62개 실내 장면의 RGB-D 재구성에서 자동 생성한 대응점 쌍
- **아키텍처**: 3D TDF(Truncated Distance Function) 볼륨을 입력으로 하는 3D CNN
- **출력**: 512차원 로컬 디스크립터

3DMatch는 학습 기반 3D descriptor 연구를 촉발한 선구적 작업이다. 논문과 함께 공개된 **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는 sparse convolution을 도입해 전체 점군의 특징을 단일 패스로 계산한다. FCGF 논문은 해당 구현·하드웨어·벤치마크에서 큰 처리시간 단축과 32차원 디스크립터의 높은 registration 성능을 보고했다.

### 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)한다.

#### 기하학적 트랜스포머 아키텍처

GeoTransformer는 **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 변환 추정

GeoTransformer는 슈퍼포인트 대응을 고른 뒤 local-to-global registration으로 변환 가설을 집계하므로 별도의 외부 RANSAC 단계를 생략할 수 있다. 저자들은 논문의 구현과 비교 설정에서 이 단계의 큰 처리시간 단축을 보고했지만, 대응점이 오염된 다른 데이터에서도 같은 비율이나 성공을 보장하지는 않는다.

#### 성능

논문은 3DLoMatch의 비교 설정에서 기준 방법에 따라 인라이어 비율 17–30%p, registration recall 7%p 이상의 향상을 보고했다. 이 수치는 저오버랩으로 정의된 해당 벤치마크와 평가 프로토콜에 한정된다.

```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)       — 초기 learned 3D descriptor + benchmark
    ↓ 효율화
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 (Learned Cross-Domain Descriptors): 2D 이미지 패치와 3D 점군 패치의 공통 임베딩 공간을 학습한다.

P2-Net (Wang 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)**: 같은 물체를 두 센서가 서로 다른 양으로 잰다. 카메라는 조명과 표면 반사 특성이 함께 결정하는 밝기를 기록하고, LiDAR는 레이저 파장에서 되돌아온 광량(intensity)을 측정한다. 표면 고유의 반사율(albedo)은 어느 쪽에서도 직접 얻어지지 않는다.

이런 어려움 때문에 cross-modal correspondence는 아직 unimodal (2D-2D 또는 3D-3D) 매칭보다 덜 성숙한 연구 영역이다. MI 기반 접근법 (5.4절)은 이 도메인 갭을 통계적으로 우회하고, 투영 기반 접근법은 두 관측을 같은 2D 영상 형식으로 표현한다.

---

## 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 field를 end-to-end supervised learning으로 직접 예측한 초기 CNN 시스템이다. 두 이미지를 입력으로 받아 flow field를 출력하는 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/8 \times W/8 \times H/8 \times W/8}$ 생성. 축은 모두 특징 맵 해상도 기준이다. 이를 후반 두 차원에 대해 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회 갱신을 사용한다. 추가 반복이 논문 실험에서 오차를 낮추기도 했지만, 수렴 뒤에는 개선이 보장되지 않으므로 지연시간과 검증 오차로 횟수를 정한다.

**All-Pairs vs. Coarse-to-Fine**: coarse-to-fine 방법은 거친 단계의 대응 오류를 후속 단계에서 복구하기 어려울 수 있다. RAFT는 모든 feature 쌍의 상관을 만들어 반복 갱신 중 여러 스케일에서 조회하므로 큰 변위 후보에도 접근할 수 있다. 다만 반복 최적화와 특징이 실패하면 큰 변위를 여전히 놓칠 수 있다.

**학습**: 모든 반복 단계의 예측에 대해 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)는 cost volume을 토큰화하고 트랜스포머로 cost memory를 인코딩한다. 이어 recurrent transformer decoder가 이 표현을 조회하며 optical flow를 반복 갱신한다. 논문에서는 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)

SGM은 전통적인 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**: 자세를 아는 두 시점 사이의 cross-view matching

### 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)        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

Visual Odometry(VO)와 Visual-Inertial Odometry(VIO)는 Ch.4의 상태 추정 프레임워크와 Ch.5의 특징점 매칭 기술을 한 시스템 안에서 결합한다.

VO는 카메라 영상만으로 카메라의 자기 운동(ego-motion)을 추정하고, VIO는 여기에 IMU를 결합해 스케일 관측 가능성과 강건성을 확보한다. 두 계열은 내부 구조와 설계 선택에서 갈린다.

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는 영상에서 기하학적 특징점을 추출하고, 프레임 간 대응 관계로 카메라 모션을 추정한다. 오래 연구되어 구성 요소와 failure mode가 비교적 잘 알려진 패러다임이며, 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번 점만 먼저 보는 4점 사전 검사는 $n=12$에서 성립하는 단축법이라 $n=9$에는 그대로 쓸 수 없고, 이 경우에는 학습된 결정 트리가 검사 순서를 정한다. Harris와의 속도 차이는 구현과 하드웨어에 따라 달라지며, 기본 FAST에는 방향·스케일 불변성이 없다.

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를 쓰는 이유는 자유도가 적어서가 아니다. $\mathbf{F}$는 7 자유도, $\mathbf{H}$는 8 자유도로 $\mathbf{H}$가 더 많다. 평면 위의 대응만으로는 $\mathbf{F}$가 유일하게 결정되지 않는 퇴화가 일어나기 때문이다.

### 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와 TUM-VI에서 stereo-inertial 구성을 평가하고, 각 데이터셋의 프로토콜에 따른 궤적 오차를 보고한다. 수치를 비교하려면 ATE/RPE 중 어떤 지표인지, 대상 시퀀스와 집계 방식, 초기화 및 트래킹 실패 처리까지 함께 밝혀야 한다. 새 카메라·IMU 조합에서는 목표 시퀀스의 ATE/RPE, 실패율, 처리 시간과 메모리를 다시 측정한다.

```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. **제한된 수렴 영역**: 이미지 그래디언트 기반 최적화는 초기 포즈가 나쁘면 다른 로컬 미니멈으로 갈 수 있다. 허용되는 초기 오차는 피라미드, 텍스처, 시점·밝기 변화에 따라 달라지므로 coarse-to-fine 정렬과 검증 잔차로 확인한다.

### 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 또는 semi-dense" (Newcombe et al.의 DTAM(ICCV 2011)이 dense, [LSD-SLAM, Engel et al., 2014](https://doi.org/10.1007/978-3-319-10605-2_54)가 semi-dense), "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}, \mathbf{c}, a_1, b_1, \ldots, a_n, b_n\}$$

즉 카메라 포즈(SE(3)), 역깊이(inverse depth), 카메라 내부 파라미터 $\mathbf{c}$, affine brightness 파라미터를 모두 포함한다.

윈도우에서 빠지는 프레임/점은 Schur complement로 마지널라이즈되어 prior로 남는다. 이 마지널라이제이션은 Ch.4.7에서 다룬 Schur complement 기반 마지널라이제이션과 동일한 원리이지만, 시각 잔차만 다룬다는 점이 다르다.

**DSO의 한계와 확장**

DSO의 본래 설계에는 루프 클로저가 없다. direct 방법의 한계가 아니라 설계 선택이다. LDSO (Loop-closing DSO)는 DBoW와 direct 정합을 결합해 이 한계를 풀었고, VI-DSO는 DSO에 IMU를 결합한 확장판이다. Basalt는 §6.3.5에서 다루듯 패치 기반 direct frontend와 Non-linear Factor Recovery를 갖춘 별도 시스템으로, DSO의 변종이 아니다.

```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단계 분리로 저자들의 해상도·하드웨어 설정에서 수백 Hz의 처리율을 보고했다. 실제 rate는 카메라 해상도, 맵 크기, CPU와 구현에 따라 달라지므로 target platform에서 측정해야 한다. 기본 구성에는 loop closure가 없고, depth update를 위한 translation이 부족한 pure 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}=[0,0,-9.81]^\top$, Ch.4)을 따른다. VINS-Mono 원 논문은 $\mathbf{g}^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개 안팎의 frame state를 유지하는 fixed-size window를 사용하며, 키프레임 여부에 따라 두 가지 마지널라이제이션 전략을 적용한다:

1. **최신 프레임이 키프레임인 경우**: 가장 오래된 키프레임을 마지널라이즈한다. Schur complement로 해당 프레임에 연결된 모든 측정을 prior로 변환한다.

2. **최신 프레임이 키프레임이 아닌 경우**: 직전 프레임(second-newest)을 마지널라이즈한다. 이때 시각 측정만 버리고, IMU 측정은 인접 키프레임 사이의 preintegration에 포함되므로 정보가 보존된다.

두 전략은 정보를 남기는 방식이 다르다. 키프레임을 마지널라이즈할 때는 연결된 측정을 Schur complement로 prior로 바꿔 남기고, 비키프레임을 마지널라이즈할 때는 그 프레임의 시각 측정을 prior로 만들지 않고 버리며 IMU만 인접 키프레임 사이의 preintegration으로 이어 붙인다. Schur complement의 수학적 메커니즘은 Ch.4.7에서 상세히 다루었다.

**4-DoF 포즈 그래프 최적화**

루프 클로저가 검출되면 VINS-Mono는 4-DoF(yaw + 3D translation) 포즈 그래프를 최적화한다. 충분한 운동과 올바른 중력·바이어스 모델 아래에서 VIO는 roll과 pitch를 중력 방향에 맞출 수 있지만, 전역 yaw와 위치에는 gauge freedom이 남는다. 이 설계는 그 가정에 맞춰 roll/pitch를 local VIO에 맡기고 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는 preintegration으로 이어 붙인다
            drop_visual_factors(self.states[-2])
            merge_imu_preintegration(self.states[-3], self.states[-2], self.states[-1])
            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. **IMU error term**: IMU preintegration 대신, 두 프레임 사이의 IMU 운동학을 재전파(repropagation)해 자세·속도·위치·바이어스 전체에 대한 15차원 잔차를 만든다. "speed and bias"는 오차항이 아니라 상태 블록 $[\mathbf{v}, \mathbf{b}_g, \mathbf{b}_a]$의 이름이다. 이후 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 점)를 상태 벡터에 포함시킨다. $L$개의 랜드마크가 있으면 상태 벡터 크기가 $3L + 15$가 되고, 공분산 행렬의 크기가 $(3L+15)^2$이 되어 랜드마크 수에 대해 $O(L^2)$ 공간과 갱신마다 $O(L^2)$ 연산이 필요하다. 혁신 공분산은 측정 차원의 작은 행렬이라 전체 행렬 분해가 필요하지 않으므로 이 비용은 제곱에서 멈춘다. 그래도 랜드마크가 늘면 실시간 처리에 치명적이다.

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$)으로 랜드마크 수 $L$과 무관하다. EKF-SLAM은 $L$개 랜드마크를 상태에 포함하여 상태 크기가 $O(L)$이 되고, 공분산 업데이트에 $O(L^2)$이 필요하다. MSCKF는 랜드마크를 상태에서 제외하므로 카메라 수 $N \ll L$에만 의존한다.

**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. **온라인 캘리브레이션**: OpenVINS는 설정에 따라 카메라 intrinsic, camera-IMU extrinsic, 시간 오프셋을 상태에 포함할 수 있다. 충분한 운동과 장면 기하가 없으면 안정적으로 추정되지 않으므로, 하드웨어 동기화·오프라인 보정된 시스템에서는 고정할 수도 있다.

3. **SLAM 랜드마크 지원**: 순수 MSCKF는 특징점을 상태에 포함하지 않지만, OpenVINS는 선택적으로 일부 랜드마크를 SLAM feature로 상태에 포함할 수 있다. SLAM feature는 오래 추적되는 점으로, anchored inverse depth로 파라미터화된다.

4. **First-Estimates Jacobian (FEJ)**: 자코비안의 일부를 first estimate에서 평가해 선형화된 시스템의 관측 불가능 방향이 잘못 줄어드는 현상을 완화한다. FEJ는 중요한 일관성 장치지만 모델 오차·noise tuning·모든 선형화 문제를 해결하지는 않는다.

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 추적(inverse-compositional, SE(2) warp)으로 서브픽셀 정밀도의 특징점 추적을 수행한다. VINS-Mono의 점 단위 KLT와 계열은 같고 패치·warp 설계가 다르다.

2. **Non-linear Factor Recovery (NFR)**: 마지널라이제이션의 대안이다. 마지널라이제이션은 선형화 지점에 의존하는 prior를 남기는데, 이 선형화 지점이 나중에 크게 변하면 정보 왜곡이 발생한다. Basalt의 NFR은 마지널라이즈된 정보를 비선형 factor로 근사하므로 나중에 다시 선형화할 수 있다.

3. **Efficient Implementation**: Basalt는 factor graph의 희소 구조를 활용해 구현을 최적화했다. 다른 시스템과의 처리 속도 비교는 센서 구성·해상도·하드웨어가 함께 정해져야 성립하며, Basalt는 stereo+IMU라 mono+IMU 시스템과 같은 조건의 비교가 아니다.

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$는 역깊이다. 이 파라미터화의 장점은 앵커 프레임의 포즈가 변해도 역깊이 자체는 변하지 않으므로, 부분적으로 선형화 오차를 줄인다. OpenVINS가 SLAM feature의 표현으로 이 방식을 쓴다. ORB-SLAM3의 맵포인트는 세계 좌표계의 3D 위치로 저장·최적화되므로 여기에 해당하지 않는다.

---

## 6.5 학습 기반 VO/VIO

전통적 VO/VIO는 "사람이 설계한 파이프라인"에 의존한다. Feature-based 방식의 예는 특징점 검출 → 매칭 → 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 등에서 어려움을 겪는다.

자기 지도 학습은 단안 깊이 추정(monocular depth estimation)에 적용되지만, 독립형 VO/VIO 시스템에서는 전통 방법과 성능 차이가 남아 있다. 특히 누적 드리프트 문제는 해결되지 않았다.

### 6.5.3 Hybrid: DROID-SLAM

[DROID-SLAM (Teed & Deng, 2021)](https://arxiv.org/abs/2108.10869)은 BA의 기하 구조와 학습된 correspondence update를 하나의 미분 가능 파이프라인으로 통합한 시스템이다. 원 논문은 TartanAir, EuRoC, TUM-RGBD 등 논문이 선택한 benchmark와 metric에서 기존 학습·기하 기반 baseline보다 큰 정확도 향상과 적은 catastrophic failure를 보고했다.

**아키텍처**

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**: 깊이 정보를 역깊이 초기값으로 사용하고, 깊이 관측을 추가 제약으로 반영한다.

**성능**

다음 수치는 DROID-SLAM 원 논문의 표와 비교 프로토콜에서 보고된 결과다:

- TartanAir: DROID-SLAM 논문의 비교 설정에서 이전 비교 방법보다 오차 62% 감소
- EuRoC monocular: 같은 논문의 프로토콜에서 비교 방법보다 오차 82% 감소
- ETH-3D: 32개 중 30개 시퀀스 성공 (기존 최고 19개)
- 합성 데이터(TartanAir)만으로 학습한 뒤, 논문이 평가한 네 실제 데이터셋의 선택 지표에서 비교 방법보다 낮은 오차를 보고

**기하 백엔드와 학습 대응 관계**

DROID-SLAM은 미분 가능한 BA와 학습된 대응 관계를 한 반복 갱신 구조에 결합한다. 원 논문은 반복 패턴, 텍스처 부족, 조명 변화가 있는 시퀀스를 포함한 평가에서 기존 비교 방법보다 낮은 오차와 적은 추적 실패를 보고했다.

그러나 한계도 있다:
- **연산량**: 공개 구현은 GPU를 전제로 하며, 프레임 속도는 해상도·키프레임 수·하드웨어와 설정에 따라 달라진다.
- **IMU 미포함**: 순수 시각 시스템으로, IMU 결합은 아직 연구 과제다.
- **메모리**: 모든 프레임의 feature map과 correlation volume을 유지해야 하므로 메모리 사용량이 크다.

후속 연구는 sparse patch, 메모리 축소, 관성 센서 결합 등 서로 다른 방향에서 이 한계를 다룬다.

### 6.5.4 최근 동향 (2023-2025)

[DPVO (Teed, Lipson & Deng, 2023)](https://arxiv.org/abs/2208.04726)는 DROID-SLAM의 dense flow를 sparse patch 기반 매칭으로 대체한다. 원 논문은 자체 비교 설정에서 메모리 약 1/3, 속도 약 3배와 경쟁력 있는 정확도를 보고한다. 패치 단위 recurrent update operator와 미분 가능 BA를 결합한 구조다.

[MAC-VO (Qiu et al., 2024)](https://arxiv.org/abs/2409.09479)는 학습 기반 매칭 불확실성(metrics-aware covariance)을 스테레오 VO에 도입하여, 키포인트 선택과 포즈 그래프 최적화의 잔차 가중치를 불확실성으로 결정한다. 원 논문은 조명 변화와 텍스처 부족을 포함한 공개 benchmark에서 비교 방법보다 낮은 오차를 보고했고, 이 논문은 ICRA 2025 Best Paper Award on Robot Perception을 받았다.

---

## 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 | Semi-direct front end, sparse depth, 고주기 처리를 겨냥한 설계 |
| 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 | Sparse patch; 논문 설정에서 DROID-SLAM보다 낮은 연산·메모리 비용 보고 |
| MAC-VO | Learned + Opt. | Pose graph opt. | Stereo | Metrics-aware covariance, ICRA 2025 Best Paper on Robot Perception |

LiDAR 기반 오도메트리와 LiDAR-Inertial 결합 시스템은 같은 자기 운동 추정 문제를 다른 센서 조건에서 푼다. 카메라 기반 시스템과의 상보 관계도 거기서 나온다.

---

# Ch.7 — LiDAR Odometry & LiDAR-Inertial Odometry

같은 자기 운동 추정 문제도 카메라(+IMU) 대신 LiDAR로 풀면 조건이 바뀐다.

LiDAR는 카메라와 상보적인 센서다. 카메라는 풍부한 텍스처를 제공하지만 가시광 조명에 민감하고 단안 기하만으로 절대 거리를 정할 수 없다. LiDAR는 가시광 조명 변화에 덜 민감한 3D 거리를 측정하지만, 비·안개·반사율·다중경로의 영향을 받을 수 있다. LiDAR Odometry(LO)는 LiDAR 점군을 기반으로 자체 움직임을 추정하는 기법이다. 여기에 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)이다. 이는 점 사이의 3차원 유클리드 거리가 아닌 접평면 법선 방향 오차만을 투영해 측정한다. 따라서 평면을 따라 미끄러지는 움직임은 잔차 비용에 불필요한 페널티를 주지 않는다.

장점: 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$는 첫 열이 법선인 회전, 즉 $\mathbf{R}_s \mathbf{e}_1 = \mathbf{n}$). $\mathbf{C}$의 고유벡터가 $\mathbf{R}_s$의 열이므로 첫 열이 법선일 때 그 방향의 분산이 $\epsilon$이 된다.
- 이 경우 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는 point-to-point와 point-to-plane ICP를 covariance 기반의 확률적 framework로 묶는다. 원 논문 실험에서는 두 baseline보다 높은 정확도와 correspondence error에 대한 강건성을 보고했지만, 실제 결과는 초기값·overlap·sampling·outlier 처리에 따라 달라진다.

```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\max} \sum_i \exp\left(-\tfrac{1}{2}(\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)})\right)$$
   
   가우시안 값을 로그 없이 더하므로 잔차가 큰 점의 기여가 포화된다. 이를 로그가능도의 합, 즉 마할라노비스 제곱합
   $$\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)})$$
   의 최소화로 바꾸면 outlier의 영향이 무한히 커지는 복셀 기반 마할라노비스 ICP가 되어 원 NDT와 다른 알고리즘이 된다. 실제 구현은 가우시안에 균등분포를 섞은 근사를 쓴다.

NDT의 장점:
- 명시적 대응 찾기가 불필요 — 점이 어느 복셀에 속하는지만 판단하면 된다. kd-tree 구축 비용이 없다.
- 복셀 크기로 정밀도와 수렴 영역을 조절할 수 있다 — 큰 복셀은 넓은 수렴 영역, 작은 복셀은 높은 정밀도.
- 점수 함수가 매끄럽고, 멀리 떨어진 점의 기여가 포화되어 outlier에 덜 끌린다.

단점:
- 복셀 크기 선택에 민감하다.
- 점이 적은 복셀에서 공분산 추정이 불안정하다.
- 3D NDT는 Autoware의 ndt_scan_matcher처럼 자율주행의 사전 지도 기반 localization에 널리 쓰인다. scan-to-scan 오도메트리에서는 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{a}$의 인접 스캔 라인에서 가장 가까운 edge 점 $\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의 한 스캔 시간은 회전 주파수의 역수다. 예컨대 10Hz 회전 주파수라면 1회 스캔에 약 100ms가 소요된다. 이 시간 동안 이동체가 계속 주행하므로 스캔 내의 각 측정점은 서로 다른 위치와 시점에서 기록된다. 모션 왜곡의 크기는 스캔 시간과 운동에 비례하므로 실제 timestamp와 궤적으로 보정한다.

보정 방법: 스캔 시작 시점 $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)  # 실제 LOAM은 두 점을 서로 다른 스캔 라인에서 고른다
                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)  # 실제 LOAM은 같은 라인 두 점 + 인접 라인 한 점
                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 기반 클러스터링을 수행한다. 일정 크기 미만의 클러스터는 노이즈로 제거한다. LeGO-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. **확장성**: LiDAR frontend를 IMU(LIO-SAM)나 카메라(LVI-SAM)와 결합하는 식으로 센서 구성을 확장할 수 있다. KISS-ICP는 이런 센서 추가형 확장이 아닌, 단순한 LiDAR-only ICP 설계를 택한 대안적인 접근 방식을 취한다.

4. **강건성**: edge/planar 분류가 잡음과 불안정한 입사 기하에 대한 필터 역할을 한다 — 곡률 패턴이 일관되지 않은 점은 선택되지 않는다. 다만 곡률은 한 스캔 라인의 이웃점에서 계산하는 기하량이라 운동을 판별하지 못한다. 움직이는 차량의 모서리와 평면도 좋은 특징으로 선택되므로, 동적 물체 제거는 별도 단계가 필요하다.

다만, LOAM 계열의 한계도 명확하다. 첫째, 비반복 주사 패턴을 사용하는 solid-state LiDAR에서는 링 기반 특징 추출 기법을 그대로 적용하기 어렵다. FAST-LIO2의 direct 접근이 특징 추출을 없애 이 쪽을 푼다. 둘째, 기하학적 특징이 희소한 개활지나 직선 터널에서는 오도메트리 정확도가 크게 떨어진다. 이것은 환경의 관측 기하가 퇴화한 문제이므로 특징을 추출하든 raw 점을 쓰든 남는다.

---

## 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 최적화하는 전역 포즈 그래프에 들어간다. LIO-SAM은 여기에 더해 IMU odometry용 그래프를 따로 두고 주기적으로 초기화하여, IMU 주기의 상태 전파와 바이어스 추정을 담당하게 한다. Factor graph 구조의 핵심 강점은 뛰어난 **모듈성**에 있다. 각 센서 관측을 독립적인 factor 형태로 손쉽게 탈부착할 수 있어 새로운 센서 모달리티의 추가가 수월하다.

**IMU 기반 De-skewing**

LIO-SAM에서 IMU는 두 가지 역할을 한다:
1. **모션 왜곡 보정**: LiDAR 스캔 동안의 IMU 데이터로 각 점의 시점별 포즈를 정밀하게 보간하여 de-skewing한다.
2. **초기값 제공**: IMU preintegration으로 다음 키프레임의 포즈를 예측하여 scan-to-map 정합의 초기값으로 사용한다.

이 양방향 결합 구조에서 IMU는 스캔 왜곡 보정과 점군 정합 초기값을 제공한다. 반대로 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)
        # 새 바이어스 변수를 구속하는 랜덤워크 factor. 없으면 미구속 변수로 iSAM2 update가 실패한다
        self.graph.add(gtsam.BetweenFactorConstantBias(
            B(self.key_idx - 1), B(self.key_idx), gtsam.imuBias.ConstantBias(), self.bias_noise))
        
        # 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 반복 횟수는 잔차 감소나 상태 증분 기준으로 정한다. FAST-LIO 계열의 설정에서는 소수 회 반복하는 경우가 많지만, 필요한 횟수는 초기값과 장면 기하에 따라 달라진다. 이 갱신은 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}$도 포함한다. 즉, 외부 캘리브레이션과 중력 방향까지 온라인으로 추정한다.

FAST-LIO2 논문은 저자들의 하드웨어와 데이터에서 최대 100Hz의 odometry·mapping 처리율을 보고하고, multi-line spinning 및 solid-state LiDAR와 여러 플랫폼·프로세서의 실험을 제시한다. 현재 시스템의 처리율은 점 수, CPU, map 크기로 다시 측정해야 한다.

```cpp
// FAST-LIO2 IEKF 업데이트 수도코드 (C++)
struct State {
    Matrix3d R_GI;    // IMU -> world rotation (앞첨자 표기 G_R_I)
    Vector3d p_GI;    // IMU position in world
    Vector3d v_GI;    // IMU velocity in world
    Vector3d bg, ba;  // gyro/accel bias
    Matrix3d R_IL;    // LiDAR -> IMU rotation (앞첨자 표기 I_R_L)
    Vector3d p_IL;    // LiDAR 원점의 IMU 좌표계 위치
    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)을 하나의 관측으로 처리한다. 스캔 안의 왜곡은 IMU 적분으로 각 점 시각의 포즈를 구해 보정하지만(§7.3.1), 갱신 자체는 스캔 단위이므로 그 구간을 하나의 운동으로 묶는 가정이 남는다. 고속·고각속도 모션에서는 이 가정이 깨진다.

Point-LIO는 점 timestamp 순서로 상태를 전파하고 point-wise update를 수행한다. 고주파 IMU와 LiDAR 점의 timestamp로 각 관측 시각의 상태를 추정해 scan-level deskew의 단일 운동 가정보다 시간 해상도를 높이지만, 그 상태도 IMU noise·bias·동기화 오차를 포함한 추정치다.

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) 필터에 근접한다.

장점: 극단적으로 빠른 모션(초당 수백 도 회전)에서도 정확한 오도메트리. 모션 왜곡 보정이 암묵적으로 이루어진다(각 점이 이미 올바른 시점의 상태로 처리되므로).

단점: 점 수에 따라 업데이트 부담이 늘어난다. Point-LIO 논문이 보고한 FAST-LIO2와의 처리시간 비율은 해당 데이터·하드웨어·설정에 한정되므로 현재 구현에서 profile해야 한다.

### 7.3.5 COIN-LIO

[COIN-LIO (Pfreundschuh et al., 2024)](https://arxiv.org/abs/2310.01235)는 LiDAR-Inertial 시스템에 **LiDAR 반사 강도(intensity)** 정보를 결합한다. LiDAR가 측정한 반사 강도를 intensity image로 투영하고, 영상 내부와 관측 사이의 밝기 일관성을 개선하는 필터링을 수행한다.

포인트 클라우드 정합에서 구속이 약한 방향을 찾고, 그 방향을 보완하는 intensity image 패치를 선택한다. 선택한 패치의 photometric residual은 IMU 측정 및 point-to-plane residual과 함께 iterated EKF에서 융합된다.

긴 터널이나 평탄한 개활지처럼 기하학적으로 퇴화한 환경에서는 LiDAR 반사 강도의 공간적 변화가 추가 구속을 제공할 수 있다. 이 구속의 효과는 해당 방향에서 관측할 수 있는 intensity 패턴에 달려 있다.

---

## 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) = \mathbf{T}_0 \prod_{i=1}^{k} \text{Exp}\left(\tilde{B}_i(t) \cdot \text{Log}(\mathbf{T}_{i-1}^{-1}\mathbf{T}_i)\right)$$

여기서 $\tilde{B}_i(t)$는 누적(cumulative) B-spline 기저 함수(basis function)다. 3차(cubic) B-spline이 주로 사용되며, $C^2$ 연속성을 보장한다.

B-spline 궤적에서는 **임의 시점 질의**가 가능하다. 어떤 시점 $t$에서든 궤적을 평가해 포즈를 얻고, 미분으로 속도와 가속도를 구해 비동기 센서 데이터를 처리한다. 3차 spline은 매듭 설정이 적절할 때 $C^2$ 연속성을 제공하지만, 이 수학적 매끄러움만으로 동역학적 실행 가능성이 보장되지는 않는다. 국소 지지 덕분에 한 제어점의 변경은 인접 구간에 주로 영향을 준다.

단점:
- 제어점 간격(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° 수평 | 모델별 (Mid-40 38.4° 원형, Avia 70.4° 원형, Horizon 81.7°×25.1°) |
| 점 밀도 | 균등 | 시간에 따라 누적, 불균등 |
| 가격 | 높음 | 낮음 |
| 크기/무게 | 큼 | 작음 |

**비반복 스캔이 특징 추출에 미치는 영향**

LOAM 스타일의 곡률 기반 특징 추출은 같은 스캔 라인의 이웃점을 이용한다. 그러나 solid-state LiDAR는 정의된 스캔 라인이 없고, 점들이 비규칙적으로 분포한다. 기존 라인 기반 곡률 계산은 쓸 수 없다. KNN(K-Nearest Neighbors) 기반 국소 곡률을 쓰거나, 아예 특징 추출을 포기하고 raw 점을 그대로 써야 한다.

**FAST-LIO2가 solid-state에 강한 이유**

FAST-LIO2는 raw 점을 직접 사용하므로 스캔 패턴과 무관하게 동작한다. FAST-LIO는 아직 edge/planar 특징을 추출하며, 특징 추출을 없앤 것은 FAST-LIO2의 기여다. Solid-state LiDAR는 시간이 지남에 따라 FoV를 점점 더 조밀하게 채우는데, FAST-LIO2의 ikd-Tree 맵은 이 점진적 밀집화를 자연스럽게 수용하여 맵 품질이 시간이 지날수록 향상된다. FoV가 좁아 한 스캔의 정보가 제한적이지만, IMU와의 tight coupling이 이를 보상한다.

Livox 계열은 비반복 스캔 패턴과 작은 폼팩터 때문에 드론·핸드헬드·소형 로봇의 공개 연구에서 자주 사용된다. FAST-LIO2가 이 스캔 패턴을 지원하므로 Livox와 조합한 공개 예제와 데이터셋도 쉽게 찾을 수 있다. 실제 선택에서는 가격뿐 아니라 FoV, 거리, 시간 동기화, 점 분포와 목표 플랫폼을 함께 비교한다.

---

## 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): 구면 투영한 vertex map·normal map을 CNN에 넣고, point-to-plane ICP 잔차 형태의 기하 인지 손실로 비지도 학습한다.
- **PWCLO-Net** (Wang et al., 2021): Pyramid, Warping, Cost volume 구조를 LiDAR 오도메트리에 적용한다.

### 7.6.2 현재의 한계

학습 기반 LiDAR 오도메트리는 전통적 방법 대비 아직 큰 격차가 있다. 네 가지 이유가 있다.

1. **LiDAR 데이터의 특성**: 포인트 클라우드는 이미지와 달리 비정형(unstructured)이고 순서가 없다. CNN이 자연스럽게 처리하기 어렵다.

2. **강한 기하 기준선**: ICP/GICP/NDT는 충분한 중첩과 구조가 있을 때 학습 없이도 강한 기준선을 제공한다. 그러나 반복 구조, 동적 객체, 강수·먼지, 희소·저중첩 장면에서는 여전히 실패할 수 있어 학습은 대응점·동적점·불확실성 추정 등에 보조적으로 쓰인다.

3. **데이터 부족**: 대규모 LiDAR 오도메트리 학습 데이터가 이미지 데이터에 비해 훨씬 적다.

4. **일반화**: 특정 LiDAR/환경에서 학습한 모델이 다른 LiDAR/환경에 잘 일반화되지 않는다.

현재 학습은 LiDAR 오도메트리 자체보다 보조 컴포넌트에서 더 효과적이다. 루프 클로저 검출(PointNetVLAD), 정합 초기값 추정(GeoTransformer, Ch.5 참조), 동적 물체 제거를 위한 시맨틱 분할 등이 그 예다. 같은 자리에서 잘 쓰이는 Scan Context는 학습이 아니라 수작업 설계 디스크립터이므로 학습의 예가 아니라 비교 대상이다.

---

## 7.7 최근 동향 (2023-2024)

2023~2024년에는 간결성과 적응성을 앞세운 신규 LiDAR 오도메트리 파이프라인들이 주목받았다.

**[KISS-ICP (Vizzo et al., 2023)](https://arxiv.org/abs/2209.15397)**: Point-to-point ICP에 적응적 임계값, 강건 커널, 모션 보상을 결합한다. 원 논문은 자동차·UAV·handheld 데이터셋 전반에서 단일 파라미터 세트로 경쟁력 있는 궤적 추정 성능을 달성할 수 있음을 보고했다. 벤치마크마다 개별 파라미터를 미세 조정해야 하는 부담을 대폭 줄였다.

**[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으로 최근접점 검색 비용을 줄였다. 공개 구현과 논문은 선택한 benchmark에서 Faster-LIO와 계산량·정확도를 비교한다.

---

## 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 | LiDAR intensity로 기하학적 퇴화 보완 |
| 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** 방향을 보여준다. 어느 쪽이 더 정확하거나 빠른지는 sensor pattern, motion, map scale, 하드웨어, benchmark protocol에 따라 달라진다.

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)**이 하나의 공유 상태를 직접 제약한다. "전문가"를 두고 그 결과를 합치는 것이 아니라, 원본 데이터가 상태 추정에 직접 들어간다. 기준은 측정이 들어가는 수준이며 optimizer가 하나인지가 아니다. R3LIVE처럼 두 하위 시스템이 하나의 공유 상태를 갱신하는 구성이나 FAST-LIVO2처럼 순차 갱신을 쓰는 구성도 tightly coupled다. LVI-SAM처럼 두 하위 시스템이 각자 그래프를 두더라도 서로의 측정(LiDAR 깊이)과 상태(초기값)를 직접 주고받으면 같은 수준으로 분류한다.

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이 있다. 일반적인 tightly coupled GNSS-INS 구성에서는 수신기가 위성 신호에서 추출한 의사거리(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 수신기의 correlator나 tracking loop에 접근해야 하므로 일반 수신기의 측정 API만으로는 구현할 수 없다. 수신기 SDK·FPGA·펌웨어를 제어할 수 있는 통합 GNSS/INS 제품과 연구 플랫폼에서 사용되며, 보통의 로보틱스 개발에서는 raw pseudorange·carrier·Doppler를 쓰는 tightly coupled 구성이 접근하기 쉽다.

### 8.1.4 세 가지 수준의 비교

```
측정 흐름:

[Loosely]    센서A → 서브시스템A → 포즈A ─┐
                                          ├→ Fusion → 최종 포즈
             센서B → 서브시스템B → 포즈B ─┘

[Tightly]    센서A → raw 측정A ──┐
                                 ├→ 공유 상태 추정기 → 최종 포즈
             센서B → raw 측정B ──┘   (단일 optimizer, 순차 갱신, 공유 상태를 쓰는
                                      두 하위 시스템 모두 이 수준에 해당)

[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 방향이 덜 정밀)

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 조합은 텍스처·색상, 3D 거리, 고속 관성을 함께 제공한다. 다만 비용·전력·기상·가용 거리와 실패 상관관계까지 고려해야 하며, 세 센서를 장착했다는 사실만으로 강건성이 보장되지는 않는다:

| 상황 | 카메라 | LiDAR | IMU |
|------|--------|-------|-----|
| 어두운 환경 | ✗ | ✓ | ✓ |
| 텍스처 없는 벽 | ✗ | ✓ | ✓ |
| 기하적 퇴화 (긴 복도) | ✓ | ✗ | ✓ |
| 고속 회전 | ✗ | ✗ | ✓ |
| 스케일 관측 | ✗ (단안) | ✓ | ✓ (가속도가 metric 단위; 운동 여기 필요) |
| 색상/시맨틱 | ✓ | ✗ | ✗ |

이 세 센서를 효과적으로 결합하기 위해 다양한 밀결합 융합 아키텍처가 제안되었다.

### 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}}$는 맵에 저장된 해당 포인트의 색상이다.

R3LIVE의 공유 상태는 한 모달리티에서 사용할 수 있는 잔차의 수가 줄어들 때도 다른 갱신을 계속 사용할 수 있다. 그러나 LiDAR 차단 때 visual update가 유지되는 범위는 기존 colored map의 가시성에, 어두운 영상에서 LIO가 유지되는 범위는 LiDAR 기하와 IMU 품질에 달려 있다. 논문은 이 구조로 online colored 3D mapping을 시연한다.

### 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 특징점에 부여해 깊이 초기화를 돕는다. 외부 파라미터·시간 동기화·가시성·occlusion 검사가 맞는 점에 한해 삼각측량의 시차를 기다리지 않고 metric depth prior를 줄 수 있다.

```
         ┌─── VIS 초기 포즈 ───→ LIS 초기값
         │                            │
  [Visual-Inertial]            [LiDAR-Inertial]
         │                            │
         └←── LiDAR 깊이 ────────────┘
         
              ↓ LiDAR·IMU factor ↓
           [Factor Graph (GTSAM/iSAM2)]
                     ↓
              최종 최적화된 포즈
```

**Factor Graph 설계**: LVI-SAM은 시각-관성 시스템(VIS)과 LiDAR-관성 시스템(LIS)의 두 하위 시스템으로 이뤄지고, factor graph는 LIO-SAM을 계승한 LIS가 유지한다. 여기에 들어가는 factor는 다음과 같다.
- IMU preintegration factor (연속 키프레임 사이)
- LiDAR odometry factor (scan matching 결과)
- GPS factor (가용 시)
- Loop closure factor (재방문 탐지 시)

VIS는 이 그래프에 자기 odometry를 factor로 넣는 대신, LIS에 초기값을 제공하고 LiDAR 깊이를 받아 자신의 스케일을 잡는 방향으로 결합한다.

### 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는 점마다 point-to-plane 잔차 하나를, 카메라는 패치 픽셀마다 photometric 잔차 하나를 주는데, 두 측정 집합의 크기·단위·잡음 규모가 달라 원 논문은 이 dimension mismatch를 이유로 순차 갱신을 택했다.

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 순서.
    동시 업데이트와 수학적으로 동등하지만, 이종 측정의 dimension mismatch를 한 번에 다루지 않아도 된다.
    
    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 (Shi-Tomasi + KLT) | Direct (photometric) |
| 맵 표현 | ikd-Tree + RGB | Voxel map | Hash+Octree voxel map |
| 특징 추출 | 불필요 | 필요 (LiDAR edge/planar, 시각 코너) | 불필요 |
| GPS 통합 | 없음 | Factor로 통합 | 없음 |
| Loop closure | 없음 | Factor로 통합 | 없음 |
| 임베디드 검증 | 목표 하드웨어 benchmark 필요 | 목표 하드웨어 benchmark 필요 | ARM 구현 보고; 목표 설정에서 재검증 |

**적용 후보를 좁히는 기준**:
- Loop closure와 GPS factor의 통합이 필요하면 LVI-SAM을 평가한다. 센서 구성, ROS 의존성, 목표 데이터에서의 재방문 성능을 함께 확인한다.
- Direct photometric update와 colored mapping이 필요하면 R3LIVE를 평가한다. 목표 장면에서 색상, 궤적, 맵 품질 지표를 비교한다.
- ARM 배포 후보로는 FAST-LIVO2를 평가하되, 목표 센서 해상도와 설정에서 처리율, 최악 지연, 메모리를 측정한다.
- 시각 특징점이나 LOAM 특징이 부족한 환경에서는 R3LIVE와 FAST-LIVO2 같은 direct 방식을 후보에 넣고, photometric calibration과 동적 객체 민감도를 검증한다.

---

## 8.3 GNSS 통합

GNSS(Global Navigation Satellite System)는 전역 좌표계의 절대 위치 참조를 제공한다. 이 장에서 다룬 IMU, LiDAR odometry, visual odometry는 주로 **상대적(relative)** 운동을 제공하므로 장시간 주행하면 드리프트가 누적된다. GNSS는 이 드리프트를 교정하는 앵커가 될 수 있다. 측량 기준점, UWB beacon, motion capture처럼 다른 절대 참조원도 있으므로 GNSS만이 유일한 전역 센서인 것은 아니다.

### 8.3.1 GNSS Factor in Factor Graph (LIO-SAM 방식)

[LIO-SAM (Shan et al. 2020)](https://arxiv.org/abs/2007.00258)은 GNSS 위치를 factor graph의 포즈 노드에 **unary factor**로 연결한다:

$$
\mathbf{r}^{\text{GPS}}_i = \mathbf{T}_{\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 맵 좌표계로의 변환이다. 위 식은 GNSS 위치를 맵 좌표계로 옮겨 비교하므로 이 변환을 그대로 적용해야 하며, $\mathbf{T}^{-1}$ 표기는 반대 방향인 $\mathbf{T}_{\text{map→ENU}}$를 쓸 때의 형태다
- $\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) — 표준편차의 제곱을 대각에 넣는다.
            수평은 sigma_h = HDOP * sigma_uere, 수직은 VDOP를 쓴다.
            즉 diag(sigma_E**2, sigma_N**2, sigma_U**2) 형태다.
    """
    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와 4D Imaging

전통적으로 자동차 레이더는 해상도가 낮아 SLAM/odometry 용으로는 적합하지 않다고 여겨졌다. 그러나 **4D imaging radar**의 등장으로 그 평가가 달라지고 있다.

**4D Radar란**: 기존 자동차 레이더가 거리(range), 속도(Doppler), 방위각(azimuth) 3가지를 측정했다면, 4D imaging radar는 여기에 **고도각(elevation)**을 추가하여 3D 포인트 클라우드를 생성한다. 점 개수는 LiDAR보다 적지만(수백~수천 점 vs 수십만 점), 다음 세 특성을 갖는다.

1. **악천후 내성**: mm-wave radar는 가시광 카메라나 일부 LiDAR보다 안개·비·눈의 영향이 작은 경우가 많다. 그러나 강수 attenuation, 물방울·노면 multipath와 clutter가 남으므로 조건별 검증이 필요하다.

2. **직접 radial velocity 측정**: FMCW radar의 Doppler는 한 frame 안의 여러 chirp에 걸친 위상 변화에서 line-of-sight 상대 속도 성분을 준다. 한 chirp만으로는 거리 정보에 머물고 속도가 분리되지 않는다. 한 점의 완전한 3D 속도나 물체 운동을 직접 주는 것은 아니며, 여러 방향·시간·추적 또는 다른 센서가 필요하다.

3. **다른 비용 구조**: mass-market radar chipset은 저가일 수 있지만 imaging radar와 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}_S + n_i
$$

여기서 $\mathbf{v}_S$는 radar 위상 중심의 선속도다. 정적 반사점이 여러 방향에 충분히 분포하면 이 식들로 $\mathbf{v}_S$를 추정할 수 있다. 단일 monostatic radar의 점별 Doppler만으로 센서 원점의 각속도를 독립 추정할 수는 없으며, body 원점으로 옮길 때는 알려진 lever arm과 IMU 각속도 또는 scan registration을 함께 사용한다.

```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 |
|------|--------|----------|
| 해상도 | 매우 높음 | 낮음 |
| 악천후 | 취약 | 강건 |
| 직접 깊이 측정 | ✗ | ✓ |
| radial velocity 측정 | ✗ | ✓ |
| 시맨틱 이해 | 강함 | 약함 |
| 비용 | 렌즈·카메라 수·연산에 의존 | 안테나·채널·연산에 의존 |

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)는 맑음·비·눈과 여러 계절에 반복 수집된 다중 센서 데이터셋이다. 카메라, Velodyne Alpha Prime LiDAR, Navtech CIR304-H **평면 scanning radar** 등을 동기화한다. 이 Navtech 장비는 elevation을 포함한 automotive 4D imaging radar가 아니므로, Boreas 결과를 4D radar 성능으로 해석하면 안 된다.

---

## 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) (Tian 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 같은 분산 최적화가 반복마다 이웃과 맞닿은 경계(separator) 포즈의 현재 추정치를 주고받으며 수렴한다. 제약을 한 번 공유하는 것과 최적화 변수를 반복 교환하는 것은 다른 통신이다.

### 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 = [np.eye(4)]  # 자체 포즈 (로컬 좌표계). 원점 초기 포즈를 넣어 둔다
        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 선정 가이드

다음 표는 센서 스위트 검토의 출발점이다. 최종 구성은 **운용 환경**, 안전 요구, 가용 거리, 전력·질량, 실패 시나리오로 결정한다:

| 환경 | 권장 최소 구성 | 선택 추가 센서 |
|------|---------------|---------------|
| 실내 (사무실/창고) | 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 |

**비용 검토 순서**: 센서 본체 가격은 시점·지역·수량에 따라 바뀐다. 먼저 필요한 측정량과 정확도·rate·FoV를 정하고, 센서 가격에 케이블·동기화 장치·GNSS 보정 서비스·연산 장치·마운트·예비 부품·통합 시간을 더한 총비용을 현재 견적으로 계산한다. 저비용 prototype은 camera+IMU에서 시작할 수 있고, 2D/3D LiDAR, RTK GNSS, radar, sensor redundancy는 검증된 실패 모드와 오차 예산에 따라 추가한다.

### 8.6.2 Timing Architecture (시간 동기화 설계)

멀티센서 시스템에서 **시간 동기화**는 정확도를 좌우한다. 100km/h로 이동하는 차량에서 1ms의 시간 오차는 약 2.8cm의 위치 오차에 해당한다.

**하드웨어 동기화 (Hardware Sync)**:

공유 clock이나 trigger를 구성할 수 있으면 software 수신 시각보다 지연 경로를 명확히 만들 수 있다. 다만 pulse의 정확도와 실제 측정 timestamp의 정확도는 다르므로 end-to-end로 검증한다:

- **PPS (Pulse Per Second)**: GNSS 수신기의 초 경계 pulse를 장치 clock의 공통 기준으로 사용한다. receiver time-pulse 사양, cable·input delay, timestamp 기준점을 확인한다.
- **PTP (Precision Time Protocol, IEEE 1588)**: 이더넷 clock을 동기화한다. 실제 오차는 hardware timestamp 지원, switch, PTP profile, 경로 비대칭과 lock 상태에 좌우된다.
- **외부 트리거**: 마이크로컨트롤러가 카메라 셔터 트리거와 IMU 타임스탬프 캡처를 동시에 수행.

**소프트웨어 동기화 (Software Sync)**:

하드웨어 동기화가 불가능한 경우, 소프트웨어적으로 시간 오프셋을 추정한다:

- **Kalibr 방식**: B-spline 궤적으로 연속 시간 궤적을 표현하고, 센서 간 시간 오프셋을 최적화 변수로 포함하여 동시 추정.
- **상관 기반**: 두 센서가 같은 운동을 관측한 신호 사이의 상호상관(cross-correlation)으로 시간 지연을 추정한다. 가속도를 쓰려면 IMU 측정에서 중력을 제거하고 두 신호를 같은 좌표계로 옮겨야 하며, 단안 카메라에서는 속도의 스케일이 미정이다. 그래서 실무에서는 이 세 조건이 필요 없는 자이로 각속도와 카메라 회전율의 비교를 주로 쓴다.

$$
\hat{\tau} = \arg\max_{\tau} \int \boldsymbol{\omega}_{\text{IMU}}(t) \cdot \boldsymbol{\omega}_{\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.4 최근 시스템과 연구 (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.5 시스템 설계 체크리스트

실제 멀티센서 퓨전 시스템을 설계할 때 반드시 확인해야 할 항목들:

**캘리브레이션**:
- [ ] 모든 센서 쌍의 extrinsic calibration 완료
- [ ] 시간 동기화 오프셋 측정/추정 완료
- [ ] 캘리브레이션 결과의 재현성 검증 (3회 이상 반복)
- [ ] 캘리브레이션 변화의 감시와 대응 절차 존재 (온라인 추정 또는 주기적 재교정). 온라인 extrinsic 추정은 관측 가능성을 위해 충분한 운동 여기를 요구하므로 모든 시스템의 필수 조건은 아니다

**데이터 흐름**:
- [ ] 각 센서의 데이터 레이트와 시스템의 처리 레이트 매칭
- [ ] 센서 간 데이터 정렬 (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의 경우 안개나 눈비 같은 악천후에서도 도플러 기반 반경 방향 속도(radial velocity)를 측정하는 데 도움이 될 수 있지만, 오차는 조건별로 확인해야 한다. Kimera-Multi와 Swarm-SLAM 같은 multi-robot 시스템은 통신 제약 아래에서 분산 추정과 cross-robot place recognition을 결합한다.

실전 시스템의 배포 결과는 알고리즘뿐 아니라 센서 선정, 시간 동기화, failure mode 대응에도 좌우된다.

Ch.6-8의 odometry·fusion 시스템은 로컬 정밀도가 높지만, 장시간 운행하면 드리프트가 쌓인다. 이 드리프트를 loop closure로 교정하려면 과거에 방문했던 장소를 다시 인식하는 **Place Recognition**이 필요하다.

---

# Ch.9 — Place Recognition & Retrieval

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 파이프라인이 **geometric verification**(기하학적 검증)을 거쳐 확정한다.

```
Loop Closure Detection Pipeline:
┌─────────────┐    ┌────────────────┐    ┌───────────────────┐    ┌─────────────┐
│  현재 관측   │───→│ Place          │───→│ Geometric         │───→│ Pose Graph  │
│  (query)     │    │ Recognition    │    │ Verification      │    │ Update      │
│              │    │ (후보 검색)     │    │ (기하학적 검증)    │    │ (최적화)     │
└─────────────┘    └────────────────┘    └───────────────────┘    └─────────────┘
```

SLAM에서 loop closure가 없으면 드리프트가 누적된다. 현재 query를 $N$개 과거 frame과 모두 비교하는 비용은 query당 $O(N)$이고, 전체 sequence의 all-pairs 비교는 $O(N^2)$이다. Global descriptor와 exact scan만 사용하면 여전히 $O(N)$이지만, inverted index나 tree, approximate-nearest-neighbor index를 쓰면 데이터 분포와 recall trade-off 아래에서 평균 query 비용을 줄일 수 있다.

### 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은 지연시간과 false-positive 비용에 따라 top-$k$ 후보를 기하 검증하므로 Recall@$k$, precision-recall, query latency도 함께 봐야 한다.

**Precision-Recall Curve**: 디스크립터 유사도에 임계값을 변화시키며 precision과 recall의 관계를 그린다. 높은 precision (false positive 최소화)이 SLAM에서 특히 중요하다 — false positive loop closure는 맵을 파괴적으로 왜곡시킨다.

**"정답"의 정의**: 위치 거리, 시야 중첩, 자세 차이 등 데이터셋의 annotation protocol이 정한다. 예를 들어 일부 VPR benchmark는 특정 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** 모델은 텍스트 검색(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를 효율적으로 검색하며, feature-based real-time SLAM의 loop-candidate retrieval에 널리 쓰인 구현 중 하나다.

```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_words = {}  # img_id -> word_ids. TF-IDF 벡터는 질의 시점의 IDF로 다시 계산한다
    
    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 가중) 계산."""
        return self.bow_from_words(self.quantize(descriptors))
    
    def bow_from_words(self, word_ids):
        """양자화된 단어 목록에서 현재 IDF로 TF-IDF 벡터 계산."""
        
        # 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):
        """이미지를 데이터베이스에 추가."""
        word_ids = self.quantize(descriptors)
        self.db_words[img_id] = word_ids   # 벡터는 저장하지 않고 단어만 보관한다
        self.N += 1
        
        # 역색인 업데이트
        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)
        
        # 역색인으로 후보를 좁힌다: 쿼리 단어를 하나라도 공유하는 이미지만
        candidates = set()
        for w in np.unique(self.quantize(descriptors)):
            candidates.update(self.inverted_index[w])
        
        scores = {}
        for img_id in candidates:
            db_vector = self.bow_from_words(self.db_words[img_id])   # 현재 IDF로 계산
            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배다. 검색 성능의 우열은 지표·데이터·정규화 설정에 따라 갈리며, VLAD를 Fisher Vector의 단순화로 보고 같은 차원에서 비슷하거나 VLAD가 앞선다는 보고도 있다.

### 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가 가깝다고 같은 장면이 보인다는 보장이 없기 때문이다. 그래서 지리적으로 가까운 이미지들을 potential positive로 두고 그중 특징 공간에서 가장 잘 맞는 하나를 positive로 고르며, negative는 먼 이미지에서 hard 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 특징 추출**: AnyLoc 구성은 DINOv2 ViT-G/14의 중간 layer에서 dense patch feature를 추출한다. CLS token 대신 patch 정보를 보존하며, 저자들은 논문의 비교 집합과 지표에서 dense feature 사용의 평균 향상을 보고했다. layer와 향상 폭은 backbone·dataset에 따라 달라진다.

2. **VLAD 집계**: 밀집 특징을 k-means로 클러스터링하여 시각 어휘를 구축하고, hard-assignment VLAD로 글로벌 디스크립터를 생성한다. NetVLAD처럼 학습하지 않고 비지도(unsupervised) VLAD를 사용한다.

3. **도메인별 어휘**: 논문은 PCA 투영에서 Urban, Indoor, Aerial, SubT, Degraded, Underwater 집단을 분석하고 domain-specific vocabulary를 사용한다. 보고된 최대 향상은 해당 평가 설정의 결과이며 새 domain에서는 vocabulary 선택을 검증해야 한다.

Foundation Model이 VPR에 유용한 배경:

[DINOv2 (Oquab et al., 2023)](https://arxiv.org/abs/2304.07193)는 1.42억 장의 이미지로 자기지도 학습한 모델이다. 특정 place-recognition label로 fine-tune하지 않은 feature도 여러 benchmark에서 유용했지만, 반복 구조·극단적 viewpoint·미포함 sensor domain에서 식별 가능성을 보장하지는 않는다.

AnyLoc 원 논문이 사용한 benchmark·protocol에서 보고한 상대 비교:
- 주야간 변화: 비교한 MixVPR·CosPlace 계열보다 Recall@1이 5–21%p 높은 경우
- 계절 변화: 8–9%p 높은 경우
- 반대 시점(180도): 39–49%p 높은 경우
- 수중·지하 등 일부 비정형 환경: baseline의 최대 약 4배
- PCA-Whitening으로 49K 차원을 512차원으로 줄인 실험도 제시하지만, recall 변화는 dataset별로 확인해야 한다

```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]
        # 아래는 마지막 블록의 출력 토큰이다. AnyLoc의 핵심 발견인
        # 특정 레이어의 value facet을 쓰려면 attention 모듈에 hook을 걸어야 한다.
        image_patch_features.append(patch_tokens)
        all_patch_features.append(patch_tokens)
    
    # Step 2: k-means 시각 어휘 구축
    all_features = np.concatenate([p.reshape(-1, p.shape[-1]) for p in all_patch_features], axis=0)  # (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
        patches = patches.reshape(-1, patches.shape[-1])   # (1, N, D) → (N, D)
        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)의 후속작으로, 시점 변화에 강건한 디스크립터를 학습하는 방법이다. CosPlace가 분류(classification) 기반 학습으로 triplet loss의 번거로운 hard negative mining을 대체한 데서 더 나아가, 각 클래스 안의 카메라 시점 분포에 PCA를 적용해 장면의 주 방향을 찾고 그 방향을 향하는 focal point 기준으로 학습 뷰를 구성한다. PCA가 적용되는 대상은 디스크립터 특징 공간이 아니라 학습 데이터의 시점 기하다.

### 9.2.6 Foundation Model 활용의 확장

DINOv2 외에도 다양한 Foundation Model이 VPR에 활용되고 있다:

- **CLIP**: 텍스트-이미지 대응 학습으로, "도시 거리", "숲 속 길" 같은 시맨틱 수준의 장소 인식에 활용 가능. AnyLoc의 foundation model 비교에서는 자신들의 레이어·facet·집계 설정과 데이터셋에서 DINOv2 특징이 더 나은 장소 식별 성능을 보였다.
- **SAM (Segment Anything)**: 세그멘테이션 마스크를 장소의 구조적 표현으로 활용하는 연구가 진행 중.
- **DINOv2 + 학습된 집계**: AnyLoc의 비지도 VLAD 대신 DINOv2 특징 위에 집계를 학습시키는 계열이 이어졌다. SALAD(§9.7.4)는 VLAD의 할당을 최적 수송으로 바꾸고, SelaVPR(Lu et al., ICLR 2024)은 사전학습 모델을 VPR에 적응시킨다.

### 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 없이 **공간 구조를 직접 보존하는 글로벌 디스크립터**로 변환한다. 학습이 불필요하고 여러 LiDAR SLAM 시스템의 loop closure 모듈로 쓰인다. 다만 LIO-SAM의 공개 구현은 반경 검색과 ICP 기반 루프 검출을 기본으로 쓰고, Scan Context는 SC-LIO-SAM 같은 별도 통합 저장소로 제공된다.

디스크립터 생성 과정:

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 표현의 장점**: 센서 중심 좌표계에서 표현하므로, 센서 위치가 같고 yaw만 다르면 디스크립터의 행(row, 즉 섹터 축)이 순환 이동(circular shift)된 형태가 된다. 이를 행 이동 매칭으로 처리한다. 실제 반대 방향 재방문에는 횡방향 이동과 가림 변화가 함께 오므로 이동만으로 대응하지 않으며, 원 논문이 root shifting 증강을 따로 두는 이유가 그것이다.

**검색 전략 — 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**: 섹터별로 링 축을 집계한 회전 가변 키다. 두 스캔의 sector key를 비교해 최적 이동량의 초기 추정을 얻고, 그 근방의 소수 이동에 대해서만 아래의 거리를 계산한다(아래 식과 코드는 단순화를 위해 모든 이동을 탐색한다):

$$
d(\mathbf{SC}_q, \mathbf{SC}_d) = \min_{s \in [0, N_s)} \frac{1}{N_s}\sum_{i=1}^{N_s}\left(1 - \frac{\mathbf{c}^q_i \cdot \mathbf{c}^d_{i+s}}{\|\mathbf{c}^q_i\|\,\|\mathbf{c}^d_{i+s}\|}\right)
$$

```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)
                # 섹터(행)별 코사인 거리의 평균 — 원 논문의 거리 정의
                num = np.sum(sc_query * sc_shifted, axis=1)
                den = np.linalg.norm(sc_query, axis=1) * np.linalg.norm(sc_shifted, axis=1) + 1e-9
                dist = np.mean(1.0 - num / den)
                min_dist = min(min_dist, dist)
            scores.append((idx, min_dist))
        
        scores.sort(key=lambda x: x[1])
        return scores[:top_k]
```

**후속작 — [Scan Context++](https://arxiv.org/abs/2109.13494)**: 구조 기반 디스크립터를 확장하여, roll·pitch 변화가 크지 않은 환경에서 heading 회전과 횡방향 위치 변화에 대한 강건성을 높인다. 장소 검색 뒤에는 1-DOF 상대 정렬도 추정한다.

### 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)**는 PointNet과 NetVLAD를 결합해 point cloud place retrieval을 end-to-end로 학습한 초기의 영향력 있는 연구다. PointNet backbone으로 특징을 추출하고, NetVLAD layer로 global descriptor를 집계하며, lazy triplet·quadruplet 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)** ([code](https://github.com/jac99/MinkLoc3D))는 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를 경량 CNN에 통과시켜 로컬 feature map을 추출한다. 이어 Transformer encoder가 특징 사이의 문맥을 반영하고, NetVLAD 레이어가 이를 글로벌 디스크립터로 집계한다. 이 방식은 2D range image와 경량 네트워크를 이용해 빠른 처리를 목표로 한다.

**관련 방법 — [OverlapNet](https://github.com/PRBonn/OverlapNet)**: LiDAR의 range image 표현을 이용해 두 스캔의 overlap과 상대 yaw를 예측한다.

### 9.3.6 BEV 기반 방법

Bird's Eye View(BEV)로 포인트 클라우드를 투영하여 2D 맵으로 변환한 뒤, 2D 이미지 기반 검색을 수행하는 방법들:

- **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로 투영하고 카메라 이미지도 disparity/depth 표현으로 바꾸어 두 모달리티를 같은 range image 영역에서 맞춘다. 두 표현을 각각의 인코더로 처리한다. **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** (Xie et al., IROS 2024)는 이미지에서 포인트 클라우드를 찾는 cross-modal 장소 인식을 효율적으로 푸는 프레임워크다. 대상 모달리티는 카메라와 LiDAR이며, 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)**: 1단계의 이미지 간 매칭을 후보 이미지에 연관된 3D 맵 포인트로 옮겨 2D-3D 대응을 만들고, 그 대응에서 카메라 포즈를 추정한다. 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
    
    # solvePnP의 (rvec, tvec)는 3D 점 좌표계 → 카메라 변환이다.
    # 3D 점이 맵 좌표계면 T는 맵→질의 카메라 변환이고, 후보와의 상대 포즈는 후보 포즈와 합성해야 한다.
    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)는 pairwise distance와 triplet angle을 인코딩하고 local-to-global registration으로 별도 외부 RANSAC 단계를 생략한다. 논문은 3DLoMatch를 포함한 비교 설정에서 저오버랩 성능과 큰 처리시간 단축을 보고했지만, 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의 누적 포즈와 일관성이 있는지 확인한다. 기준은 차이의 크기가 아니라 odometry 공분산에 대한 Mahalanobis 거리다. 루프 클로저의 목적이 누적 드리프트 교정이므로 긴 루프에서는 정상 검출도 큰 차이를 보이며, 크기만으로 거부하면 가장 유용한 루프를 버린다.

---

## 9.7 최신 동향

### 9.7.1 Foundation Model 기반 PR의 부상

VPR에서는 환경별 전용 학습과 foundation-model 기반 zero-shot 인식을 함께 연구한다. AnyLoc은 VPR 전용 학습 없이도 원 논문이 선택한 여러 환경에서 경쟁력 있는 결과를 보고했다.

향후 방향은 크게 두 가지다. 첫째, ViT-G/14는 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가 선택한 VPR benchmark에서 NetVLAD·CosPlace보다 높은 recall을 보였다고 보고한다.

**[EffoVPR (Taha et al., 2024)](https://arxiv.org/abs/2405.18065)**: DINOv2 등 foundation model의 특징을 효율적으로 활용하는 프레임워크다. 원 논문은 128차원 descriptor 설정을 포함한 평가에서 정확도와 저장·검색 비용의 절충을 보고한다.

### 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 위험을 낮춘다. Visual에서는 PnP·essential matrix와 robust estimation을, LiDAR에서는 ICP 계열이나 learned registration을 사용하며, 어느 방법도 잘못된 loop를 완전히 배제하지는 못한다.

최신 동향으로는 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. **학습 기반**: [NetVLAD](https://arxiv.org/abs/1511.07247), [AnyLoc](https://arxiv.org/abs/2308.00688) 등의 global descriptor를 사용한다. AnyLoc은 [DINOv2](https://arxiv.org/abs/2304.07193) dense feature를 VLAD로 집계해 place-specific label 학습 없이 여러 domain에 적용하지만, 새 sensor·환경에서 recall을 검증해야 한다. 코사인 유사도로 후보를 랭킹한다:

$$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 기반 kd-tree 검색으로 후보를 좁히고 섹터 이동 정렬로 재순위를 매기는 2단계 검색으로 효율적 후보 탐색이 가능하며, 역방향 재방문에도 강건하다.
- **[PointNetVLAD](https://arxiv.org/abs/1804.03492), [OverlapTransformer](https://arxiv.org/abs/2203.03397)**: 학습 기반 3D place recognition이다. Scan Context와의 recall 순위는 데이터셋, sensor pattern, 학습 domain에 따라 달라진다.

**시간적 필터링**: 최근 frame과의 matching은 보통 연속 tracking이므로 인접 keyframe을 후보에서 제외한다. 시간이나 이동거리 간격은 platform 속도, keyframe rate, 재방문 형태로 조정한다. 아래 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 검증**: 현재 frame과 후보 keyframe 사이에서 특징점을 matching하고 RANSAC으로 essential matrix를 추정한다. 인라이어 수·비율뿐 아니라 공간 분포, pose plausibility, 반복 검출을 확인한다. 아래 20개·50%는 예시이며 detector와 scene에 맞춰 검증한다.

$$\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 문제를 푼다. 3D 점을 후보 키프레임 좌표계로 두면 결과가 곧 loop edge에 필요한 상대 변환 $\mathbf{T}_{ij}$이고, 맵 좌표계로 두면 질의 프레임의 절대 pose가 나오므로 후보의 pose와 합성해야 한다.

4. **Temporal consistency**: 단일 매칭이 아니라, 연속된 여러 프레임에서 동일 장소와의 매칭이 일관되게 나타나는지 확인한다. ORB-SLAM2는 세 번 연속 동일 장소가 검출되어야 수용했다. ORB-SLAM3는 이 시간 조건을 local window 기반 기하 검증으로 바꿨다. 질의 키프레임과 covisible한 키프레임 둘, 총 셋에서 변환이 지지되면 수용하는데, 그 셋이 대개 이미 맵에 있어 새 키프레임을 기다리지 않아도 되므로 루프를 더 이르게 잡는다.

```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.
                    Essential 분해의 t는 크기가 정해지지 않은 단위 방향이므로
                    이 값을 pose graph의 loop edge로 쓰려면 스테레오·깊이·맵
                    포인트에서 metric scale을 따로 얻어야 한다.
    """
    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):
    """8-point 선형 해법 + RANSAC으로 Essential matrix 추정.

최소 표본 5개로 푸는 Nister의 5-point 알고리즘은 별도 해법이며
    최소 표본 크기가 다르므로 필요한 RANSAC 반복 수도 다르다.
    """
    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)가 전체 그래프를 재최적화하여 드리프트를 보정한다. 조정되는 것은 측정값인 edge가 아니라 pose 변수이며, 그 결과 각 edge의 잔차가 재분배된다. 분배는 균등하지 않고 각 edge의 정보 행렬 $\boldsymbol{\Omega}_{ij}$와 그래프 위상이 정하므로, 정보가 큰 odometry 구간은 보정을 덜 받는다.

---

## 10.2 Pose Graph Optimization

Pose graph optimization은 프론트엔드(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 분해로 효율적으로 풀 수 있다. 단 상대 제약만 있으면 목적식이 전역 SE(3) 변환에 불변이라 $\mathbf{H}$가 6차원 영공간을 갖는다. 기준 pose 하나를 고정하거나 prior factor를 넣어야 정부호가 되어 분해가 성립한다.
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)는 이 incremental optimization을 구현한다.

새 변수나 측정이 추가되면, 영향이 파급되는 범위를 파악하고 그 부분만 재계산한다.

**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처럼 정해진 주기의 full-batch 재선형화를 요구하지 않지만, 큰 loop closure는 영향 범위를 넓힐 수 있다.

**변수 재정렬(variable reordering)**: 새 변수 추가 시 전체 elimination order를 재계산하지 않고, 영향받는 부분만 incremental하게 재정렬한다.

GTSAM 라이브러리는 iSAM2를 제공하며 [LIO-SAM](https://arxiv.org/abs/2007.00258)이 이를 백엔드에 사용한다. 한편 ORB-SLAM3의 경우 자체 커스텀 g2o 엔진을 기반으로 비선형 최적화를 수행한다. iSAM2는 새 factor의 영향을 받는 Bayes-tree 부분을 갱신해 매번 전체 batch 문제를 다시 푸는 비용을 줄인다.

```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 대응을 확보한 뒤, MLPnP + RANSAC으로 pose를 추정한다(EPnP는 ORB-SLAM2까지의 방식이고 ORB-SLAM3는 카메라 모델에 독립적인 MLPnP로 바꿨다). 이후 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은 초기화 뒤 Prediction·Update·Resampling의 세 단계를 반복한다.

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 분포를 표현할 수 있다. 로봇이 여러 장소 중 어디에 있을지 모를 때 여러 가설을 동시에 유지한다. 관측이 장소를 구별해 주고 정답 근방의 입자가 남아 있으면 가설이 하나로 줄어든다. 대칭 복도처럼 관측이 장소를 가르지 못하면 다중 모드가 계속 남고, 입자가 부족하거나 가능도가 지나치게 뾰족하면 정답 모드가 고갈되어 잘못된 위치로 굳는다.

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] = log_weight   # 로그 가중치를 먼저 모은다
        
        # 가중치 정규화 — 최대 로그가중치를 뺀 뒤 exp (빔 수백 개의 로그합은 그대로 exp하면 언더플로)
        log_w = self.particles[:, 3]
        self.particles[:, 3] = np.exp(log_w - np.max(log_w))
        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):
        """고정 간격 샘플링 기반 간단한 ray casting.

실수 좌표에 방향 벡터를 더하며 셀을 조회하므로 셀을 건너뛰거나
        중복 조회할 수 있다. 직선이 지나는 셀을 빠짐없이 순회해야 하면
        Bresenham이나 Amanatides-Woo DDA를 쓴다.
        """
        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 + 1) * self.resolution   # 전진 후 검사하므로 검사한 점의 거리는 (step+1)칸
        
        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. **좌표계 불일치**: 초기 정렬이 부정확할 수 있으므로 후보 탐색 범위를 넓힌다. pose prior의 게이트를 키우고 후보 수와 RANSAC 반복을 늘리며 정합 초기값의 허용 영역을 넓히는 쪽이다. 기하 검증의 잔차 수용 기준은 풀지 않는다. 그쪽을 완화하면 잘못된 루프가 그대로 들어와 맵이 깨진다.

### 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)만 교환한다. 이후 기하 검증에는 희소 특징이나 부분 표본 점군이, 분산 최적화에는 경계 pose 추정치와 불확실성이 추가로 오간다.
- **분산 최적화**: 중앙 서버 없이 로봇들이 자율적으로 맵을 병합할 수 있어야 한다. 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이나 Gauss-Seidel 계열 블록 좌표 하강을 분산으로 돌린다. 다만 회전이 들어간 pose graph 목적식은 비볼록이라 ADMM의 볼록 수렴 보장이 적용되지 않고, 도달하는 것은 초기값과 통신 그래프에 의존하는 1차 임계점이다. 전역 최적성은 사후 검증을 붙인 certifiably correct 계열에서 특정 조건에서만 얻는다.

---

## 10.5 최근 연구

**[riSAM (McGann et al., 2023)](https://arxiv.org/abs/2209.14359)**: iSAM2에 [Graduated Non-Convexity](https://arxiv.org/abs/1909.08605)를 통합한 incremental robust backend다. 저자들은 논문의 합성·실험 설정에서 90%를 넘는 outlier 비율과 offline baseline 비교 결과를 보고한다. 이 수치는 outlier 구조와 초기값이 다른 문제의 보장이 아니다.

**[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

로봇은 센서 데이터로 궤적을 추정하고 loop closure로 전역 일관성을 맞춘 뒤, 그 결과를 맵으로 남긴다. 맵은 로봇이 세상을 기억하고 활용하는 형태다.

**맵(map)**은 환경의 공간적 표현이다. SLAM 시스템은 로봇의 궤적뿐 아니라, 그 궤적을 통해 관측한 환경의 구조도 추정한다. 어떤 형태로 환경을 표현하느냐에 따라 로봇이 할 수 있는 일이 달라진다. 경로 계획은 free/occupied 정보를 요구하고, 시각적 렌더링은 텍스처를 요구하며, 인간과의 상호작용은 의미론적 레이블을 요구한다.

출발점은 metric map(정량적 기하 맵)이고, 그 뒤에 mesh, neural representation, semantic map, 장기 유지(long-term maintenance)가 이어진다.

---

## 11.1 Metric Maps

Metric map은 환경의 기하학적 구조를 정량적 좌표로 표현한다. Localization·planning에 직접 연결하기 쉬워 여러 로봇 시스템에서 널리 쓰이지만, 필요한 표현은 작업과 센서에 따라 달라진다.

### 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)}$$

**문제**: 해상도 $r$로 $L \times W \times H$ 공간을 dense 3D grid로 표현하면 $(L/r)(W/r)(H/r)$개의 cell이 필요하다. 예를 들어 100 m $\times$ 100 m $\times$ 10 m를 5 cm로 나누면 $2000 \times 2000 \times 200 = 8 \times 10^8$개 cell이다. cell당 float 하나만 저장해도 약 3.2 GB이고, weight·semantic·자료구조 overhead를 더하면 커진다.

```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(occupied | miss), 통과 관측의 갱신값
        
        # 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)한다. 빈 공간은 큰 단위로, 세밀한 구조 근처만 작은 단위로 표현해 메모리를 절약한다.

OctoMap은 다음 특성을 갖는다.
- **확률적 갱신**: occupancy grid와 동일한 log-odds 갱신 사용
- **적응적 해상도**: 최고 해상도(예: 2 cm, leaf node)부터 최저 해상도(예: 수 m, root 근처)까지 자동 조절
- **메모리 효율**: 관측된 경계 근처만 세분화하므로 free/unknown 영역이 큰 장면에서 dense grid보다 적은 memory를 쓸 수 있다. 절감률은 점유 구조와 tree overhead에 따라 달라진다.
- **한계**: pointer/tree overhead가 있고, 표준 occupancy query에는 적합하지만 kNN·surface registration용 index를 대신하지는 않는다. 동적 객체를 지우려면 inverse sensor update와 시간 정책을 별도로 설계해야 한다.

```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 자료구조다. 고정 깊이의 넓고 얕은 hierarchical tree와 tile/value 압축으로 background 영역을 생략한다. OctoMap과 구조·API 목적이 달라 속도를 일반적으로 서열화할 수 없으며, occupancy 확률 갱신은 응용에서 정의해야 한다.

**ikd-tree** (FAST-LIO2): incremental k-d tree로, 포인트 하나의 삽입을 분할 상환 $O(\log n)$에 수행하며 동적 재균형을 지원한다. 삭제는 범위 단위 lazy deletion이고 재균형은 기준을 넘을 때 부분 트리에 대해 일어나므로, 이 두 연산의 비용은 점 단위 $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, w)$로 표현된다:

- $\mathbf{p} \in \mathbb{R}^3$: 위치
- $\mathbf{n} \in \mathbb{R}^3$: 법선 벡터 (단위)
- $r \in \mathbb{R}^+$: 반경
- $c$: 색상 (관측된 값)
- $w$: 융합 신뢰도 (누적 가중치)

**[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)를 저장한다. 모든 방향의 최근접 표면까지의 거리를 담는 것은 뒤에 나오는 ESDF(§11.2.2)이고, 여기서 쓰는 것은 depth 관측을 투영해 얻는 projective TSDF다:

- 양수: 표면 앞 (free space)
- 음수: 관측된 표면 뒤 (truncation band 안)
- zero crossing: 실제 표면 위치

부호는 관측된 시선을 따라 정의된다. 표면 앞쪽은 $+\delta$로 잘라 갱신하고, 표면 뒤 $-\delta$를 넘는 물체 내부는 아래 코드처럼 갱신에서 건너뛰어 기본값으로 남긴다. 따라서 음수 TSDF는 §11.1의 occupied 상태와 같은 점유 확정이 아니다.

**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}\left(D(\mathbf{u}) - \lambda^{-1}\|\mathbf{v} - \mathbf{c}\|, -\delta, \delta\right), \quad \lambda = \|\mathbf{K}^{-1}\dot{\mathbf{u}}\|$$

$D(\mathbf{u})$는 픽셀 $\mathbf{u}$의 depth 값, $\mathbf{c}$는 카메라 위치, $\mathbf{v}$는 voxel 중심이다. $D(\mathbf{u})$가 광축 방향 깊이이므로 시선 길이 $\|\mathbf{v} - \mathbf{c}\|$를 $\lambda$로 나눠 같은 기준으로 맞춘 뒤 뺀다.

```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)**를 효율적으로 계산한다. 표본 기반·탐색 기반 계획은 점유 맵에서 직접 동작하므로 ESDF가 계획 일반의 전제는 아니다.

Voxblox는 세 단계로 동작한다.

1. **TSDF 통합**: RGB-D 또는 depth 포인트 클라우드를 TSDF에 projective 방식으로 통합.
2. **Mesh 추출**: TSDF에서 Marching Cubes로 mesh를 incremental하게 추출. 변경된 voxel 블록만 재처리.
3. **ESDF 계산**: TSDF에서 ESDF를 계산. ESDF는 각 voxel에서 가장 가까운 장애물까지의 유클리드 거리를 저장한다.

경로 계획에서는 로봇과 장애물 사이의 안전 거리를 유지해야 한다. ESDF가 있으면 임의의 점에서 장애물까지의 거리를 $O(1)$에 조회할 수 있다. 그래디언트 정보도 함께 제공하므로 장애물을 피하는 방향도 바로 구할 수 있다.

### 11.2.3 Poisson Surface Reconstruction

Poisson reconstruction은 oriented point cloud(위치 + 법선)로부터 수밀(watertight) mesh를 생성하는 방법이다.

Poisson reconstruction은 법선 벡터를 gradient field로 해석하고, 이 gradient의 divergence를 구하여 Poisson 방정식을 푼다.

$$\nabla^2 \chi = \nabla \cdot \mathbf{V}$$

여기서 $\mathbf{V}$는 법선 벡터 필드, $\chi$는 indicator function(표면 안쪽 = 1, 바깥쪽 = 0)이다. 이 PDE를 octree 위에서 효율적으로 풀어 iso-surface를 추출한다.

Poisson reconstruction은 전역 implicit 함수를 풀어 연결된 surface를 만들며, oriented normal의 품질과 depth·screening 설정에 민감하다. 입력 바깥을 닫아 watertight surface를 만들 수 있지만 원하지 않는 막이 생길 수도 있다. 계산비용 때문에 online incremental mapping보다 후처리에 자주 쓰이고, TSDF meshing은 depth integration과 incremental update가 필요한 경우에 적합하다.

### 11.2.4 실시간 Mesh 생성

SLAM 시스템은 TSDF를 incremental meshing하거나, online point/surfel map을 별도 surface reconstruction 단계로 넘길 수 있다.

1. **Voxblox incremental meshing**: TSDF가 갱신된 voxel 블록에서만 Marching Cubes를 재실행. 전체 볼륨을 재처리하지 않으므로 실시간 가능.
2. **FAST-LIVO2 colored point map + 후처리**: FAST-LIVO2는 camera intensity를 이용한 visual update와 colored point map을 제공하지만, 원 시스템의 online output을 mesh라고 부르면 안 된다. Mesh가 필요하면 exported point map의 normals·density를 확인한 뒤 Poisson, Ball Pivoting 같은 별도 reconstruction을 수행한다.

---

## 11.3 Neural / Learned Representations

Voxel, mesh, surfel 표현은 3D 구조를 직접 저장하는 명시적(explicit) 방식이다. 이 절이 다루는 학습된 표현은 그 안에서 다시 갈린다. 단일 MLP가 장면을 가중치에 담는 **암묵적(implicit)** 방식이 한쪽 끝이고, hash-grid나 계층 grid처럼 공간 feature를 명시적 자료구조에 두고 작은 MLP로 복호하는 방식이 중간이며, 3D Gaussian처럼 명시적 primitive를 쓰는 방식이 반대쪽 끝이다.

### 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과 결합하여 온라인으로 동작한다:

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)은 단일 MLP를 handheld RGB-D 실시간 SLAM의 유일한 장면 표현으로 사용한 초기 시스템이다. 원 논문은 keyframe 구조와 정보 기반 pixel sampling으로 tracking 10 Hz, global map update 2 Hz를 보고한다.

한계는 세 가지다. MLP 학습 속도(training speed)가 느려 실시간 mapping에 제약이 있고, 새 영역을 학습하면 이전 영역의 표현이 퇴화하는 catastrophic forgetting이 발생한다. 대규모 환경에서는 단일 MLP의 용량 한계도 드러난다.

Instant-NGP는 multi-resolution hash-grid feature encoding으로 신경 장면 표현의 학습과 렌더링을 가속했고, 이를 활용한 NeRF-SLAM 변형도 등장했다. 공간의 여러 해상도에서 해시 테이블로 local feature를 저장하고 조회한다.

**[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 — 으로 수행한다. 명시적 primitive와 rasterization 덕분에 원 3DGS 논문은 자체 실험의 장면·해상도·하드웨어 설정에서 실시간 렌더링을 보고했다.

**[Gaussian Splatting SLAM](https://arxiv.org/abs/2312.06741)** (Matsuki et al. CVPR 2024, 공개 구현 이름은 MonoGS): 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 조작) |
| 메모리 | 단일 MLP는 고정 (모델 크기), 공간 feature grid 변형은 장면 규모에 따라 증가 | 가변 (Gaussian 수에 비례) |
| Loop closure 대응 | 어려움 (가중치 변형) | 표현상으로는 상대적 용이 (Gaussian 변환). 시스템 구현은 아직 드묾 |

Neural map과 TSDF·surfel map의 우열은 센서, 장면 규모, 동적 물체, 장시간 일관성, 렌더링 요구에 따라 달라진다. Neural representation은 novel-view rendering을 함께 최적화할 수 있지만, 대규모·장시간 운용에서는 메모리 증가와 loop correction을 별도로 평가해야 한다.

**최근 시스템 (2024~2025)**:

- **[SplaTAM](https://arxiv.org/abs/2312.02126)** (Keetha et al. CVPR 2024): RGB-D 카메라로부터 3D Gaussian을 online tracking·mapping하며 silhouette mask로 맵을 확장한다. 원 논문은 비교한 지표와 장면에 따라 camera pose, map construction, novel-view synthesis에서 최대 2배 개선을 보고한다.
- **[MonoGS](https://arxiv.org/abs/2312.06741)** (Matsuki et al. CVPR 2024 Highlight): 3D Gaussian을 단안 SLAM의 유일한 3D 표현으로 사용해 tracking·mapping·렌더링을 통합한다. 원 논문은 3 fps 운용을 보고하며, geometric verification과 regularization으로 단안 재구성의 모호성을 다룬다.
- **[MASt3R-SLAM](https://arxiv.org/abs/2412.12392)** (Murai et al. CVPR 2025): 3D reconstruction foundation model(MASt3R)을 SLAM에 통합한다. 지도 표현은 Gaussian이 아니라 MASt3R가 예측하는 dense pointmap이다. 원 논문은 카메라 모델을 미리 가정하지 않는 dense SLAM과 15 fps 처리 결과를 보고한다.

```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

Object-level map은 환경의 객체(object)를 인식하고 그 위치와 크기를 맵에 등록한다.

Object-level map은 네 단계로 구축한다.
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의 여러 계층을 online으로 점진 구축하고 loop closure 뒤 전체 계층을 함께 보정하는 초기 공개 시스템이다. 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 노드에 장애물까지의 거리(clearance)를 값으로 둔다.
2. 문(doorway) 같은 좁은 통로에서는 이 값이 낮다.
3. clearance 임계를 차례로 올려(dilation) 좁은 연결을 끊은 뒤, 남은 부분 그래프에 modularity 기반 커뮤니티 검출을 적용해 place들을 방으로 묶는다.

**계층적 loop closure**: Hydra는 scene graph의 여러 계층을 활용하여 loop closure의 품질을 높인다. place와 object 계층의 디스크립터로 후보를 좁히고, 두 scene graph 부분 사이의 정합을 TEASER++로 검증한다. 원 논문은 이 구조가 자신들의 실험에서 외관 기반 BoW 방식보다 더 많은 루프를 더 정확하게 잡았다고 보고한다.

**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 image feature를 추출하여 대응하는 3D 위치에 부착한다. 사용자가 "빨간 소화기"라고 질의하면, CLIP text encoder로 텍스트를 인코딩하고 맵의 CLIP feature와 코사인 유사도를 계산하여 해당 위치를 반환한다. 텍스트와 공유 임베딩 공간을 갖는 것은 CLIP image encoder뿐이므로, DINO 특징은 이 비교에 쓸 수 없고 분할이나 관측 일관성 같은 별도 채널로 쓴다.

이 방식은 로봇이 사전에 학습하지 않은 객체에도 동작한다. 예측 불가능한 환경에서 운용되는 가정용 로봇이나 탐사 로봇에 특히 유용하다.

```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 — 실전 시스템 & 벤치마크

센서 모델링, 상태 추정, odometry, place recognition, 공간 표현은 실제 플랫폼에서 하나의 시스템으로 조합된다.

자율주행, 드론, 핸드헬드 매핑은 센서 퓨전이 핵심으로 쓰이는 대표 플랫폼들이다. 각 환경에 맞추어 구성된 시스템은 표준 벤치마크와 전용 진단 도구로 정량 평가한다.

---

## 12.1 플랫폼별 시스템 설계

### 12.1.1 자율주행 (Autonomous Driving)

자율주행의 안전 아키텍처는 단일 센서 결함을 감지하고 위험 전파를 차단하도록 설계해야 한다. 운행을 유지할지 최소 위험 상태로 천이할지는 운행 설계 영역(ODD)과 안전 사례에 의해 결정된다. 다양한 센서 모달리티는 이 과정에서 결함 포용적 중복성과 상호 검증 장치를 제공한다.

#### 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 × 32-beam spinning LiDAR
- 6 × 서라운드 카메라 (360° 커버)
- 5 × radar
- IMU, GNSS

**Camera-centric production example**:
- 다중 surround camera와 neural perception을 중심으로 구성할 수 있다.
- 실제 장착 sensor와 radar 사용 여부는 차량 세대·시장·시점에 따라 바뀌므로 특정 회사 전체를 고정된 "vision-only" 구성으로 일반화하지 않는다.

### 12.1.2 Production-Level Fusion Pipeline

프로덕션 자율주행 시스템의 센서 퓨전 파이프라인은 대체로 다음 요소로 구성된다. 아래 화살표는 처리 단계의 개념적 순서이며 모듈 사이의 유일한 의존 관계가 아니다. 특히 위치 추정은 추적·예측의 하류가 아니라 GNSS·IMU·LiDAR·wheel 관측으로 병행 수행되고 그 결과가 인식·예측·계획에 함께 쓰인다.

```
센서 동기화 (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** 방식은 센서의 feature 단계에서 학습으로 결합한다. 결합 위치는 방법마다 다르다. [BEVFusion](https://arxiv.org/abs/2205.13542) (MIT)은 카메라와 LiDAR 특징을 공유 BEV 표현으로 변환해 그 공간에서 합치고, TransFusion은 LiDAR BEV 특징에서 출발한 object query가 이미지 특징에 cross-attention하는 transformer decoder 구조다. 어느 쪽이든 센서 간 상보적 정보를 네트워크가 학습으로 활용한다. 단, 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**: 현재 LiDAR scan을 HD point-cloud map에 NDT/ICP로 정합한다. 충분한 map 정확도·중첩·기하·초기값 아래에서 GNSS가 약한 구간의 위치를 제약할 수 있지만, cm-level 오차는 map과 평가 조건으로 검증해야 한다.

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 파이프라인들이 있다.
- **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 단독**: 드리프트가 누적되므로 허용되는 임무(mission) 길이는 운동 양상(motion), 텍스처(texture), 캘리브레이션(calibration), 루프 클로저(loop closure)와 오차 예산으로 정한다.
2. **VIO + 지형 매칭**: 사전 구축된 지형·건물 맵과 현재 카메라 관측을 매칭해 드리프트를 억제한다. 사전 지도(prior map)가 필요하다.
3. **VIO + UWB**: 환경에 UWB 앵커를 설치하고 거리 측정(ranging)으로 드리프트를 보정한다. 인프라 사전 구축이 전제다.
4. **VIO + 기압계(barometer)**: 기압(pressure) 변화로 상대 고도(altitude) 추정을 보조한다. 기상·온도·프로펠러 후류(prop wash)에 따른 바이어스(bias)가 있어 절대 z축 드리프트를 단독으로 제거하지는 못한다.

### 12.2.3 실시간 제약

드론의 센서 주기와 허용 지연은 최대 각가속도, 제어 대역폭, 카메라 노출, 통신 오버헤드를 종합 고려해 산정한다. IMU는 에일리어싱과 적분 드리프트를 억제할 수 있는 고주파 샘플링 주기를 확보해야 한다. 카메라 셔터 노출 시간은 모션 블러와 저조도 노이즈의 절충점에서 결정된다. 상태 추정치의 종단간(end-to-end) 허용 지연 역시 임의의 고정 수치가 아니라 폐루프 제어 안정성 해석과 비행 시험을 통해 규정해야 한다.

**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 수행. 정확도는 제조사가 명시한 조건(스캔 길이, 폐합 여부, 검증 방식)에서 읽고, 발주 규격은 아래 12.3.2의 기준으로 따로 확인한다.
- **NavVis VLX**: 백팩 장착. 4개의 카메라 + LiDAR. 실내 매핑에 특화.
- **GeoSLAM ZEB**: 핸드헬드 모바일 매핑. 초기 세대는 2D LiDAR를 스프링에 매달아 보행 중 수동으로 진동시키는 구조였고, ZEB-REVO 이후 세대는 모터로 회전하는 헤드를 쓴다. 세대에 따라 스캔 기구가 다르다.

이런 장비가 수행하는 작업의 개념 흐름은 다음과 같다. 세 제품은 내부 알고리즘을 공개하지 않으므로 아래 이름들은 같은 자리를 채우는 공개 방법의 예시이며 실제 채택을 뜻하지 않는다.

```
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

측량용 매핑은 발주 규격과 검증 절차를 먼저 정한다.

- **절대 정확도**: 요구 수평·수직 오차와 신뢰수준을 정하고, 독립 check point로 검증한다. GNSS·total station·GCP는 현장 조건에 맞춰 사용한다.
- **상대 정확도**: 맵 내부의 일관성. Loop closure와 global optimization이 이 일관성을 맞춘다.
- **포인트 밀도**: 대상 크기와 deliverable에 필요한 point spacing·완전성을 정한다. 1cm 같은 값은 현장 규격일 수 있지만 보편적 기준은 아니다.
- **색상 품질**: 정확한 HDR 색상 매핑. 카메라-LiDAR 시간 동기화와 extrinsic 캘리브레이션이 정밀해야 한다.

실제 센서 퓨전에서는 다음 문제를 처리해야 한다.

1. **Degenerate environments**: 긴 복도, 빈 방 등 기하학적 특징이 부족한 환경. LiDAR-only에서 발생하는 drift를 카메라 또는 IMU가 보완. R3LIVE, FAST-LIVO2 같은 multi-modal 시스템이 효과적.

2. **다층 건물**: GNSS가 없으므로 층간 이동에서 z축 드리프트가 특히 문제다. 같은 층을 재방문하는 경로라면 loop closure가 이 오차를 크게 줄인다. 단일 통과 경로에는 닫을 루프가 없으므로 기압계, 층고·도면 prior, 기준점 같은 다른 관측으로 제약한다.

3. **유리/거울**: LiDAR 빔이 투과하거나 반사. 카메라로 보완하거나, 반사 포인트 필터링.

---

## 12.4 벤치마크 & 평가

센서 퓨전 시스템을 공정하게 비교하려면 표준화된 데이터셋과 평가 메트릭이 필요하다.

### 12.4.1 주요 데이터셋

| 데이터셋 | 연도 | 환경 | 센서 | 특징 |
|----------|------|------|------|------|
| **[KITTI](https://doi.org/10.1177/0278364913491297)** | 2012 | 실외 (자율주행) | Stereo, LiDAR, GPS/IMU | Odometry 평가용 11개 training + 11개 test 시퀀스 |
| **[EuRoC](https://doi.org/10.1177/0278364915620033)** | 2016 | 실내 (MAV) | Stereo, IMU | Machine Hall + Vicon Room의 VIO 시퀀스 |
| **[TUM-RGBD](https://doi.org/10.1109/IROS.2012.6385773)** | 2012 | 실내 | RGB-D | Kinect v1 기반 Visual SLAM 시퀀스와 평가 도구 |
| **TUM-VI** | 2018 | 실내+실외 | Stereo, IMU | VIO 벤치마크. 다양한 모션 패턴 |
| **[Hilti](https://arxiv.org/abs/2109.11316)** | 2021–2023 challenge editions | 건설 현장 | LiDAR, Camera, IMU | 산업 환경 특화. 도전적 조건 |
| **[HeLiPR](https://arxiv.org/abs/2309.14590)** | 2023 | 실외 (도심) | Heterogeneous LiDAR, IMU, GNSS/INS | 이종 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 | 옥스퍼드 New College 구내. 핸드헬드 단일 세션, 루프 다수 |

각 데이터셋의 특성과 용도:

**KITTI** — 2012년에 공개된 자율주행 odometry·SLAM의 장기 기준선이다. Velodyne 64채널 LiDAR, 스테레오 카메라, GPS/IMU를 제공한다. 한계도 뚜렷하다. 센서가 오래되었고 시퀀스가 짧다. ground truth는 GPS/INS 기반이라 모든 구간에서 cm 수준 정확도를 보장하지 않는다.

**EuRoC** — 드론(MAV)에 장착된 스테레오 카메라 + IMU 데이터로, VIO에서 널리 쓰이는 benchmark다. Ground truth는 구간에 따라 Vicon motion capture 또는 Leica laser tracker로 기록됐다. 11개 시퀀스가 easy → medium → difficult로 분류되어 있다.

**Hilti** — 건설 현장의 분진, 진동, 반복 기하 구조라는 가혹한 환경에서 SLAM 성능을 검증하는 챌린지다. 2021년, 2022년, 2023년에 걸쳐 개최되었으며, 회차마다 센서 탑재 구성과 정합 평가 규칙이 달라지므로 세부 명세는 공식 릴리스 논문을 참조해야 한다.

**HeLiPR** — 2023년 공개되었으며, 서로 다른 종류의 LiDAR(spinning, solid-state, FMCW)를 동시에 탑재했다. 이종 LiDAR 사이의 장소 인식(inter-LiDAR place recognition) 연구에 쓰인다.

**Newer College** — 옥스퍼드 New College 구내를 핸드헬드 장비로 한 세션에 걸으며 수집한 데이터로, 같은 구역을 반복 통과해 loop closure 평가에 적합하다. 시점·계절 변화를 담은 long-term 연구에는 Boreas 같은 데이터셋이 맞다. 핸드헬드 LiDAR로 수집되어 도전적인 모션 패턴을 포함한다.

2022년 이후 새로운 벤치마크가 빠르게 추가되고 있다.

[Hilti-Oxford](https://arxiv.org/abs/2208.09825) (2022)는 mm 수준 ground truth를 제공하는 건설 환경 SLAM benchmark로 2022 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 평가를 위한 benchmark로, 다양한 환경과 플랫폼의 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}$$

평가 전에 두 궤적을 정렬(alignment)하는 경우가 많고, 정렬의 자유도는 센서 이름이 아니라 관측 불가능한 자유도와 벤치마크 규약이 정한다. 스케일이 관측되지 않는 순수 단안에서는 Sim(3), 스케일이 관측되는 stereo·LiDAR·단안+IMU에서는 SE(3)를 쓴다. KITTI odometry의 공식 지표처럼 Sim(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는 드리프트를 반영한다. KITTI처럼 거리 구간(100~800 m)으로 평가하려면 각 시작 프레임에서 누적 이동 거리가 목표값에 도달하는 프레임을 먼저 찾아 대응시켜야 한다.

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 포함 여부**: 루프 클로저를 켠 결과와 끈 결과를 섞어 비교하면 안 된다. Loop closure가 있으면 ATE가 크게 낮아질 수 있다. 이름으로 판단할 수는 없다. VO로 불리는 DSO는 루프 클로저가 없지만 LDSO는 더했고, ORB-SLAM3는 루프 클로저를 끄고 돌릴 수 있다. 실제 설정을 확인한다.

---

## 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(0.0, 1.0, 0.0)   # 병진은 pose1 좌표계에서 해석되므로, 회전 후 전진은 +y
    )
    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(-1.0, 1.0, 0.0)   # pose2.between(pose0): pose2 = (Rz(90°), (1,1,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 등에서 사용
- Factor graph 추상화 없이 순수 최적화 문제를 직접 정의

**g2o** (General Graph Optimization):
- Kümmerle et al. (2011) 개발
- C++ 라이브러리, 그래프 최적화 특화
- 다양한 vertex/edge 타입 사전 정의 (SE2, SE3, Sim3 등)
- GTSAM의 factor 타입 계층 대신 vertex/(hyper-)edge 그래프 추상화를 직접 정의하는 구조. 처리 속도 비교는 문제 크기, 변수 순서화, 선형 solver, 빌드 설정에 따라 뒤집히므로 목표 문제에서 직접 측정한다

세 라이브러리 비교:

| 특성 | GTSAM | Ceres | g2o |
|------|-------|-------|-----|
| 추상화 수준 | Factor graph | Cost function | Graph vertex/edge |
| 자동 미분 | 부분 지원 (Expression 기반) | 완전 지원 | 기본은 수치 미분 대체, 버전에 따라 자동 미분 제공 |
| 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 타겟 사용
- Camera-IMU calibration에 널리 쓰이지만 설치가 까다로움 (ROS 의존)

**OpenCalib** (2023):
- 자율주행 전체 센서 스택의 통합 캘리브레이션
- 카메라 내부, LiDAR-카메라, LiDAR-IMU, radar-카메라, LiDAR-LiDAR 등 센서 쌍별 도구의 모음. 네 센서의 모든 조합에 대응하는 모듈이 있는 것은 아니다
- Target-based + Targetless 모두 포함

**[direct_visual_lidar_calibration](https://arxiv.org/abs/2302.05094)** (Koide et al. 2023):
- NID 기반 targetless LiDAR-카메라 캘리브레이션
- SuperGlue로 초기 추정, NID로 정밀 정합
- 카메라 이미지 한 장과 LiDAR 점군 한 세트의 짝으로 동작 (single-shot). 회전형 LiDAR에서는 누적 점군을 쓰는 경우가 있어 필요한 점 밀도를 함께 확인한다

### 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
import copy

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 = copy.deepcopy(traj_est)
        traj_est_aligned.align(traj_gt, correct_scale=False)   # align은 제자리 변환, 반환값은 (R, t, s)
        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 = copy.deepcopy(traj_est_sync)
        traj_est_aligned.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 맵 구축에 쓴다

---

앞에서 다룬 센서 퓨전 이론은 이러한 실전 시스템과 벤치마크를 통해 제품과 연구에 적용된다. Foundation model, event camera, 4D radar, end-to-end SLAM은 현재 연구가 활발한 **연구 프런티어**다.

---

# Ch.13 — Frontiers & Emerging Directions

센서 퓨전에는 확립된 이론과 실전 시스템뿐 아니라, 아직 검증이 진행 중인 연구 방향도 있다.

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 등)—의 표현을 센서 퓨전과 SLAM pipeline에 사용하는 연구가 이어지고 있다. 이 모델의 feature를 retrieval·matching·semantic prior에 넣을 수 있지만, geometry와 uncertainty 처리를 자동으로 대신하는 것은 아니다.

### 13.1.1 DINOv2/CLIP Feature를 SLAM에 활용

**DINOv2의 시각 특징**: [DINOv2](https://arxiv.org/abs/2304.07193)는 자기지도학습(self-supervised learning)으로 훈련된 ViT다. Image patch마다 얻은 token을 dense feature처럼 사용할 수 있으며, downstream 실험에서는 다음 정보가 관찰된다.

- 일부 조명·계절 변화에서 같은 장소의 대응을 찾는 데 유용한 feature가 나타난다.
- 객체 category와 장면 semantics가 feature similarity에 반영될 수 있다.
- Patch-level structure가 correspondence·segmentation 같은 downstream task에 활용된다.

**AnyLoc의 접근**: [AnyLoc](https://arxiv.org/abs/2308.00688) (Keetha et al. 2023)은 DINOv2의 dense feature를 VLAD로 집계하여 글로벌 장소 디스크립터를 생성한다. 이 디스크립터는:

- 도심, 실내, 항공, 수중, 지하를 포함한 논문 benchmark에서 VPR 전용 fine-tuning 없이 평가됐다.
- 원 논문의 여러 dataset에서 비교한 NetVLAD·CosPlace 계열보다 높은 recall을 보고했지만, 모든 환경에 대한 보장은 아니다.
- 31번째 layer의 value-facet dense feature가 전역 CLS token 디스크립터보다 좋았다는 논문 내 ablation을 제시한다. 개선 폭은 지표와 평가 대상에 따라 다르므로 원 논문의 ablation 표에서 지표 이름과 평균 범위를 함께 읽는다.

```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):
        """
        이미지에서 패치 단위(1/14 해상도) 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 파이프라인에서의 활용 지점**:

| 파이프라인 모듈 | 전통 방법 | 학습 feature를 넣는 예 |
|---------------|-----------|-------------|
| 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)의 relative-depth prior |
| Loop closure candidate | DBoW2, Scan Context | FM descriptor similarity 후 geometric verification |

### 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 모듈별 근거와 경계

파운데이션 모델 통합(Foundation-model integration)의 근거 수준은 과제와 벤치마크마다 다르다. 다음 구분은 대체 순위가 아니라, 어떤 주장을 검증해야 하는지 보여준다.

**논문 benchmark에서 활용 근거가 있는 영역**:
- **Visual place recognition**: AnyLoc은 DINOv2+VLAD를 여러 domain에서 평가해 강한 recall을 보고했다. DBoW2와 입력 feature·학습 조건·계산량이 달라 보편적 대체 관계로 해석하지 않고, 목표 환경에서 false-positive rate와 latency를 함께 비교한다.
- **Feature matching**: LoFTR·RoMa는 texture와 viewpoint가 어려운 benchmark에서 강한 결과를 보고한다. Classical sparse feature는 compute budget, calibration, repeatability 조건에 따라 여전히 유효한 선택지다.
- **Monocular depth**: [Depth Anything](https://arxiv.org/abs/2401.10891)의 기본 출력은 relative depth다. Metric scale이 필요한 fusion에서는 metric-depth용 fine-tuning이나 다른 sensor constraint가 필요하며, 출력 uncertainty도 따로 다뤄야 한다.

**별도의 estimator가 계속 필요한 영역**:
- **LiDAR odometry**: ICP·LOAM·FAST-LIO2 같은 geometry-based 방법과 learned method의 비교 결과는 sensor와 dataset에 따라 달라진다. Learned feature나 correspondence를 넣더라도 motion model과 registration 검증은 남는다.
- **IMU integration**: Preintegration은 kinematics와 noise model을 factor에 압축한다. Learned bias·noise correction을 결합할 수 있지만, 그것이 preintegration의 관측 모델과 uncertainty propagation을 자동으로 대체하지는 않는다.
- **Backend optimization**: Factor graph와 iSAM2는 관측을 결합하는 estimator다. FM output을 factor로 넣으려면 covariance, outlier, correlation을 명시해야 한다.

따라서 실용적인 기본형은 기존 estimator의 geometry·uncertainty 구조를 유지하고, learned feature·semantic prior를 검증 가능한 module로 넣는 hybrid다.

**최근 시스템 (2024~2025)**:

- **[MASt3R-SLAM](https://arxiv.org/abs/2412.12392)** (Murai et al. CVPR 2025): MASt3R의 learned geometry를 SLAM에 통합하며, 원 논문은 미리 정한 camera model 없이 dense SLAM을 15 fps로 처리한 결과를 보고한다.
- **[Depth Anything V2](https://arxiv.org/abs/2406.09414)** (Yang et al. NeurIPS 2024): Synthetic-data teacher와 large-scale pseudo-label student 학습을 사용한다. 원 논문 benchmark 결과를 depth prior 후보로 볼 수 있지만, fusion에는 scale·uncertainty 검증이 추가로 필요하다.

---

## 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)은 학습 기반 visual SLAM의 대표적인 공개 시스템이다.

DROID-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}$는 대각 행렬이므로 각 대각 원소의 역은 $O(1)$에 구할 수 있고, 전체 depth block 처리는 depth 변수 수에 선형이다. 이 구조는 Schur complement를 효율적으로 계산하는 데 쓰인다.

3. **프레임 그래프 기반 루프 클로저**: co-visibility 기반으로 프레임 그래프를 동적 구축. 재방문 시 장거리 에지를 추가하여 implicit loop closure 수행.

4. **단일 모델로 monocular/stereo/RGB-D 지원**: 합성 데이터(TartanAir)로 학습한 한 모델을 세 입력 형태에 적용한다. 원 논문은 네 benchmark의 선택한 metric에서 기존 baseline보다 높은 정확도와 적은 catastrophic failure를 보고한다.

**DROID-SLAM이 보고한 결과**:
- TartanAir에서 이전 최고 대비 오차 62% 감소
- EuRoC monocular에서 82% 감소
- 이 수치는 원 논문의 특정 split·metric·baseline에 대한 상대 비교

### 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의 학습 신호가 된다.

### 13.2.3 현재의 한계와 가능성

**한계**:
- **일반화**: 학습 분포 밖의 camera, motion, 조명, 동적 장면에서 성능이 달라질 수 있다. DROID-SLAM의 원 논문은 synthetic-to-real generalization을 평가하지만 모든 환경과 sensor modality를 포괄하지는 않는다.
- **검증 범위**: 전통적 pipeline은 residual과 각 module을 따로 검사하기 쉽지만, 학습된 update operator의 거동은 training distribution과 평가 protocol에 더 강하게 묶인다. 어느 쪽도 일반적인 비선형 SLAM 문제에서 전역 수렴이나 일관성을 자동으로 보장하지 않는다.
- **계산 비용(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는 시간에 따른 환경의 변화 이력까지 포함하는 장기 공간 기억이다.

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 기반 환경 이해

[Hydra](https://arxiv.org/abs/2201.13360)가 구축하는 것과 같은 3D Scene Graph 표현 위에서 다음과 같은 확장이 연구되고 있다. Hydra 자체는 계층 구축과 계층적 loop closure까지를 다룬다.

**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는 사람 agent를 노드로 두지만(§11.4.2의 계층 표), 기하·의미 mesh와 place·room 계층 그리고 그 위의 최적화는 정적 구조를 가정한다. 동적 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$는 분모의 후보 표본이다. 합은 양성 표본 $\mathbf{x}_C$와 negative sample을 모두 포함한다.

**Cross-modal place recognition**에서의 응용: LiDAR로 만든 맵에서 카메라만으로 localization하는 시나리오. LiDAR descriptor와 camera descriptor가 같은 공간에 있으면, 카메라 질의로 LiDAR 맵을 검색할 수 있다.

**LC$^2$** (Lee et al. 2023): LiDAR-Camera cross-modal place recognition. LiDAR를 range image로 투영하고 카메라 이미지도 disparity/depth 표현으로 바꾸어, 두 표현의 feature를 공통 공간으로 정렬한다(§9.4.3).

### 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의 프레임 간격 사이를 채운다. 빠른 카메라 모션에서 추적 강건성이 올라간다. 대비가 부족해 이벤트가 적거나 반대로 이벤트가 포화해 대역폭이 막히면 여전히 끊길 수 있다.

**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의 데이터 형식(비동기 이벤트 스트림)이 전통적 컴퓨터 비전 파이프라인(프레임 기반)과 호환되지 않는다. 프레임으로 집계하면 표현마다 잃는 것이 다르다. 이벤트 수 histogram은 윈도 안의 시각 정보를 버리고, time surface는 픽셀마다 최근 이벤트 시각을 남겨 순서를 상당 부분 보존한다. 로그 강도 감지에서 오는 높은 동적 범위는 집계해도 남는다.
- Event camera의 가격과 해상도는 모델별 차이가 크며, 같은 가격대의 frame camera보다 선택지가 제한적이다.
- 학습 데이터가 부족하다. 대부분의 데이터셋은 프레임 카메라용이다.

**최근 시스템과 연구 (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(-200, 200, 20):   # px/s. 이벤트 카메라의 flow는 보통 수십~수백 px/s
            for vy in np.linspace(-200, 200, 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는 거리(range), 방위각(azimuth), 고각(elevation), 도플러 속도(Doppler velocity)의 4차원 정보를 제공한다. 거리·방위각·도플러를 측정하는 automotive radar에 고각 분해능을 더해 3차원 공간 위치와 방사 방향 속도를 함께 관측한다.

**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 imaging radar는 가시광·근적외선 센서가 저하되는 조건에서도 유효한 거리·Doppler 검출을 유지하는 경우가 많지만, 악천후의 영향을 받지 않는 것은 아니다. 다음 표는 일반적인 상대 경향이며 특정 장치의 성능 보증이 아니다.

| 조건 | 카메라의 경향 | LiDAR의 경향 | 4D Radar의 경향 | 주요 변수 |
|------|---------------|--------------|------------------|----------|
| 맑은 날 | 색·텍스처 정보가 풍부함 | 조밀한 기하 정보에 유리함 | 거리·Doppler를 주지만 각 해상도가 제한될 수 있음 | 타겟 RCS, 거리, 각 해상도 |
| 비 | 대비 저하와 렌즈 젖음 | 후방 산란과 감쇠 | 상대적으로 덜 저하될 수 있으나 rain clutter와 젖은 radome 영향 | 강우량, radome 상태, 주파수 |
| 안개 | 대비와 가시거리 저하 | 후방 산란과 감쇠 | 대체로 상대적 강건성이 높음 | 안개 농도, 거리, 타겟 RCS |
| 눈/먼지 | 가림과 대비 저하 | 입자 후방 산란 | clutter·multipath와 표면 적설 영향이 남음 | 입자 농도, 축적, 타겟 RCS |
| 야간 | 조명과 노출에 의존 | 능동 거리 측정 유지 | 능동 거리·Doppler 측정 유지 | 조명, 표면 반사율, 타겟 RCS |
| 직사광선 | glare와 포화 가능 | 수광부의 태양 배경광 영향 가능 | 광학적 영향은 작지만 RF 간섭은 남음 | 수광부·필터, RF 간섭, 장착 방향 |

밀리미터파는 일반적으로 가시광·근적외선보다 안개 크기의 입자에 덜 민감하지만 영향이 0인 것은 아니다. 강한 강수, 젖거나 얼어붙은 radome, multipath, RF 간섭, 낮은 RCS의 타겟, 제한된 각 해상도는 탐지 거리를 줄이거나 오경보를 늘릴 수 있다. 목표 기상 조건에서 탐지 거리, 오경보율, Doppler 오차를 장치별로 검증한다.

### 13.6.2 4D Radar + Camera Fusion

4D radar와 카메라를 결합하면 서로 다른 failure mode를 보완할 수 있다. 비용과 악천후 성능은 센서 모델, 장착, 처리 장치, 목표 데이터셋에서 따로 검증한다.

**BEV 기반 퓨전**: 카메라 이미지에서 BEV feature를 추출하고 (LSS 또는 BEVFormer 방식), radar 포인트를 BEV 공간에 투영하여 결합한다.

**Radar의 Doppler 정보 활용**: 4D radar는 각 포인트의 시선 방향 속도(radial velocity)를 직접 측정한다. 카메라와 pulsed time-of-flight LiDAR에는 없는 정보다. FMCW 방식 LiDAR는 같은 원리로 점별 속도를 재므로 예외다:

- **동적 객체 분류**: 자차 속도를 보상한 Doppler 잔차로 정적 배경과 움직이는 객체의 후보를 가른다. 시선에 직교해 움직이는 객체는 기여가 0이라 이 관측만으로는 구별되지 않으며, 임계값은 radar 속도 잡음과 자차 속도 추정 오차를 함께 반영해야 한다.
- **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$는 포인트 인덱스를 뜻한다. 3D 속도 벡터를 추정하려면 최소 3개의 정적 포인트가 필요하며, 이들의 시선 방향 단위 벡터가 선형 독립이어야 한다.

```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 (참고용), Odometry | 도심 | 기존 automotive radar 포인트 + semantic labels (point-level annotation). LiDAR는 포함되지 않는다 |
| **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과 동적 객체 분류는 카메라나 pulsed LiDAR가 제공하지 않는 기능이다(FMCW 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?

Autonomous vehicles, drones, and service robots start from one question: which sensors should the system use to perceive the world? No single sensor answers it alone. Sensor fusion begins there, with the limits of individual sensors, the ways sensors can be combined, and the division of labor between classical methods and deep-learning-based methods.

---

## 1.1 Limitations of a Single Sensor

Each sensor observes a particular aspect of the environment through a physical principle, and the same principle also imposes limitations.

### Camera Limitations

Cameras provide rich visual information about the environment, but have clear limitations.

**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 backlighting. 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 a distance of 2 m and a 10 m object at a distance of 20 m can appear the same size in the image. This scale ambiguity prevents monocular visual odometry from recovering absolute scale. Without stereo cameras, fusion with other sensors, or prior knowledge of metric size, monocular projective geometry alone cannot uniquely determine metric distance.

**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 almost 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 offer 16 to 32 channels of vertical resolution in low-cost models, 64 channels in standard reference sensors such as the HDL-64E used in KITTI, and up to 128 channels in high-end models. High-resolution LiDARs cost from several thousand to tens of thousands of dollars. Solid-state LiDARs have recently reduced cost, but at the cost of a narrower field of view (FoV).

### IMU Limitations

An inertial measurement unit (IMU) measures acceleration and angular velocity, providing high-frequency (typically 100 Hz–1 kHz) proprioceptive data. It does not depend on the external environment, but its measurement errors accumulate through integration.

**Drift.** When IMU measurements are integrated to compute velocity and position, sensor bias and noise accumulate over time. The position-error term caused by a constant accelerometer bias grows in proportion to $t^2$. Actual drift depends strongly on sensor grade, temperature compensation, initialization, motion, and external correction measurements, so an IMU grade alone does not determine a fixed error after a given number of seconds or minutes.

$$\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.** An IMU measures relative changes and does not by itself provide absolute position or heading. During stationary intervals, or when the accelerometer reliably indicates gravity, roll and pitch can be aligned; yaw remains unobservable for a standalone IMU. A magnetometer, GNSS course, or camera/LiDAR map alignment is needed to attach a heading reference.

### 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 |

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 and robustness 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 state components 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 and angular velocity, helping bridge motion between camera frames. With sufficient motion excitation, metric acceleration combined with visual constraints can make scale observable, while the accelerometer also contributes to estimating 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.

This form of fusion **extends 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 develops a problem (lens fouling or 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}$$

Under the assumptions used by the formula — unbiased, independent Gaussian observations with correctly known variances — $\sigma_{\text{fused}}^2 < \min(\sigma_1^2, \sigma_2^2)$. The guarantee fails if correlation is ignored or the variances are wrong. This is a special case of a scalar Kalman update with an independent measurement.

**Representative example: multi-LiDAR systems**

Multiple LiDARs placed around a vehicle can extend field of view and provide redundant observations in overlapping regions. The number and layout depend on range, occlusion, cost, and failure-coverage requirements.

### 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

Sensor fusion can also be classified by the **level at which sensor data is combined**. This level 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.
- Possible loss of optimality: if the intermediate output is not a sufficient statistic or does not carry cross-correlation, information can be lost relative to an estimator that combines the raw observations consistently.

### 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:**
- Potential information preservation: retaining individual residuals and covariances can preserve more information than fusing intermediate poses. Actual quality still depends on modeling, linearization, and outlier handling.
- Cross-state estimation: when extrinsics, time offsets, or biases are included in the state and the motion provides observability, camera observations can contribute to IMU-bias estimation and similar cross-calibration effects.
- Room for graceful degradation: if another sensor still observes the required states, it may carry the estimate when one sensor loses observations. This requires failure detection and observability analysis rather than following automatically from tight coupling.

**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 integration, the INS's predicted velocity is fed back into the code/carrier tracking loop inside the GNSS receiver.
- This feedback narrows the receiver's tracking-loop bandwidth. The receiver becomes more resistant to noise and can maintain satellite tracking under severe interference or weak signals.

**Analogous concepts in vision:**

In visual-inertial systems, the counterpart to ultra-tight coupling is using the IMU prediction to constrain the camera's feature-point search region or directly correcting motion blur with IMU data. A front-end design that uses the IMU prediction to narrow the feature-point search region comes close to this. VINS-Mono is not a fitting example here: its front end tracks features with KLT optical flow, and the IMU enters through pre-integration in the back end and initialization.

### 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 |

Many public VIO and LIO systems use a **tightly coupled** design. Loosely coupled systems are easier to implement and swap as modules, but compressing measurements into intermediate estimates can discard information. Ultra-tightly coupled systems require access to internal sensor signals or tracking loops and therefore have a narrower scope. VINS-Mono, FAST-LIO2, and LIO-SAM are representative public implementations that tightly couple different sensor combinations. [FAST-LIVO2 (Zheng et al., 2024)](https://arxiv.org/abs/2408.14035) combines LiDAR, IMU, and cameras in one framework and reports accuracy and throughput improvements under the datasets and comparison protocol in its paper.

---

## 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, learning-based methods have entered sensor fusion along with the broader spread of deep learning in computer vision. Their reach and performance differ 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, improving robustness to illumination and viewpoint changes. [SuperGlue (Sarlin et al., 2020)](https://arxiv.org/abs/1911.11763) applies graph neural networks (GNNs) and attention mechanisms to feature matching and reported a lower mismatch rate than nearest-neighbor matching based on handcrafted descriptors. **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 and can match texture-scarce environments.

Learning-based matchers report higher results than handcrafted baselines on several public benchmarks, but deployment comparisons must also include latency, memory, and domain shift.

**Place Recognition.** [NetVLAD (Arandjelović et al., 2016)](https://arxiv.org/abs/1511.07247) aggregates CNN features with a trainable VLAD layer and reports higher recall than the tested image-retrieval baselines. [AnyLoc (Keetha et al., 2023)](https://arxiv.org/abs/2308.00688) evaluates DINOv2 features with unsupervised VLAD across several domains. Because these methods differ from DBoW2 in input features and compute, compare them on the same dataset and verification pipeline.

**Monocular depth estimation.** Projective geometry from one image cannot uniquely determine a scene's absolute scale. [Depth Anything (Yang et al., 2024)](https://arxiv.org/abs/2401.10891) is a monocular relative-depth model trained on large-scale data. Its successor, [Depth Anything V2 (Yang et al., 2024)](https://arxiv.org/abs/2406.09414), improves precision through synthetic-data training and large-scale pseudo-labeling, and [Metric3D v2 (Hu et al., 2024)](https://arxiv.org/abs/2404.15506) uses learned priors for zero-shot metric-depth estimation. Before using these predictions in fusion, validate scale bias and uncertainty on the target camera and environment.

**Map representations.** NeRF represents scenes with a neural network, while 3D Gaussian Splatting uses a set of optimized 3D Gaussians. NeRF-SLAM, Gaussian Splatting SLAM, and related systems provide photorealistic map representations distinct from traditional point maps or voxel grids.

**Event cameras.** Alongside these developments in deep learning, event cameras, also called neuromorphic vision sensors, asynchronously detect brightness changes at each pixel, providing microsecond-scale temporal resolution and wide dynamic range. As an [event camera survey (Chakravarthi et al., 2024)](https://arxiv.org/abs/2408.13627) summarizes, event-based VIO and SLAM combine event cameras with conventional frame-based cameras for 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. There are four reasons:

1. **Explicit uncertainty propagation**: Kalman filters and factor graphs compute uncertainty under an assumed model and covariance. Their covariance can still become inconsistent under model mismatch, linearization error, or ignored correlation, and learned observations require separate calibration.
2. **Physical models and constraints**: dynamics and kinematics can be encoded in state transitions and residuals. This alone does not prevent every physically impossible estimate; required hard constraints must be expressed in the optimization or parameterization.
3. **Dependence on training data**: model-based estimators can run without large supervised training sets, but still require sensor calibration, noise tuning, and validation data. Learned components additionally require management of the training distribution and supervision assumptions.
4. **Generalization**: learning-based odometry can degrade outside its training distribution. Geometric methods are also affected by texture, illumination, dynamic objects, and sensor noise, but can be applied without retraining.

**LiDAR odometry.** Geometry-based LiDAR odometry remains widely used after [LOAM (Zhang & Singh, 2014)](https://frc.ri.cmu.edu/~zhangji/publications/RSS_2014.pdf). The models and failure conditions of point-cloud registration methods such as ICP, GICP, and NDT are comparatively well understood, and the methods can be applied to new environments without retraining. Learning-based LiDAR odometry can be competitive on individual datasets, but generalization across distribution and sensor changes must be tested separately.

**Calibration.** The planar-target method of [Zhang (2000)](https://doi.org/10.1109/34.888718) remains a widely used foundation for camera intrinsic calibration. Targetless and learning-based methods are also active research areas, but precision and observability must be compared with target-based methods for the specific sensor setup and data-collection protocol.

### Hybrid Approach

A **hybrid** structure that combines a learning-based frontend with a classical backend is a common design choice.

```
[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) illustrates this hybrid approach. It uses learned feature extraction and correspondence finding, while performing the final pose estimation with differentiable bundle adjustment.

### Technical Lineage Summary

For each topic, the guide follows this sequence: **traditional methods → what deep learning enabled → where tradition is still needed**. The table below summarizes the technical lineage.

| Area | Classical | Learning-based | Relationship in practice |
|------|-----------|---------------|----------|
| Feature matching | SIFT/ORB + BF/FLANN | SuperPoint+SuperGlue → LoFTR → RoMa | Hybrid |
| Visual odometry | Feature-based (ORB) / Direct (DSO) | DROID-SLAM, DPV-SLAM | Choice depends on accuracy, compute, and domain |
| LiDAR odometry | ICP/LOAM | DeepLO-family | Geometry-centered, sometimes with learned components |
| Place recognition | BoW/VLAD | NetVLAD → AnyLoc | Mixed retrieval frontends, followed by geometric verification |
| Depth estimation | Stereo matching | Mono depth (Depth Anything) | Separate roles for metric stereo and learned priors |
| Calibration | Target-based | Targetless + learning | Select target or targetless methods by requirements |
| Map representation | Occupancy/TSDF | NeRF / 3DGS | Coexistence |
| State estimation backend | KF / Factor Graph | End-to-end attempts | Model-based estimator with learned observations |

Learned features are often used in perception modules, while model-based estimators combine geometry and uncertainty. This is not an absolute boundary; it is a starting point for separating each module's inputs, failure modes, and validation metrics.

---

## 1.5 Scope and Organization

### Relation to robotics-practice

robotics-practice surveys Spatial AI broadly. This guide focuses on **sensor fusion, localization, and retrieval**.

- Where robotics-practice introduces EKF/PF in one or two pages, this text 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 text derives each sensor's **noise model and observation equations** in equation form.
- Overlapping introductory content is replaced by references to robotics-practice; only the additional depth remains here.

### Guide Organization

The organization is:

1. **Ch.1 — Introduction**: 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 concept moves from intuition to equations, with Python code and examples used to check the result.

---

## 1.6 Cross-Cutting Themes of This Guide

The guide returns to four questions throughout:

1. **"Why was this traditional method important?"** — Understand the problem each traditional method solved and 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.

Following these questions reveals a broader map of sensor fusion beyond individual algorithms.

Every fusion algorithm starts by expressing, in equations, how a sensor "sees" the world through its observation model.

---

# Ch.2 — Sensor Modeling

How a sensor "sees" the world is defined by its mathematical observation model. Ch.4 uses these models directly as the measurement function $h(\mathbf{x})$ in Kalman filters and factor graphs.

> Going one level beyond the overview in robotics-practice Ch.2, the focus here is on **noise models and mathematical observation models**. A fusion algorithm needs the mathematical relationship between a measurement and its physical quantity, along with the error distribution.

---

## 2.1 Camera Observation Model

A camera projects points of the 3D world onto a 2D image plane. The camera observation model expresses this projection mathematically.

### 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$. For square pixels ($\Delta x = \Delta y$), $f_x = f_y$; for rectangular pixels, the two values differ.
- $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}]$ represents 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}$$

State estimation uses the Jacobian of this nonlinear projection function directly:

$$\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 linearizes the observation model in EKF-based VIO and forms 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 a common distortion model supported by OpenCV and often used with pinhole cameras in SLAM and VIO. Fisheye or omnidirectional models can be more appropriate for wide-FoV lenses.

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 = \operatorname{atan2}\left(\sqrt{X_c^2 + Y_c^2},\; Z_c\right)$$

Handling the rear hemisphere ($Z_c < 0$, beyond 180°) requires the two-argument atan2, and the direction unit vector is likewise computed as $X_c/\sqrt{X_c^2+Y_c^2}$ rather than by dividing by $Z_c$. The Python implementation below follows this approach.

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$$

Here $r_d$ is a dimensionless normalized radius. In a pure equidistant projection, $r_d = \theta$, corresponding to $k_1 = 1$, $k_2 = k_3 = \cdots = 0$. The focal lengths in pixels are applied as $f_x$ and $f_y$ in the projection below.

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, OCamCalib of [Scaramuzza et al. (2006)](https://rpg.ifi.uzh.ch/docs/IROS06_scaramuzza.pdf) or Kalibr can be used.

```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 cost function used by nearly every sensor fusion algorithm that incorporates camera observations.

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 (in an isotropic model, $\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 \sim 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 exposure-start offset between the first and last rows is $(H-1) \cdot t_r$ ($H$: image height), which differs from each row's exposure duration. The total readout period is approximately $H \cdot t_r$, ranging 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 world-to-camera transform at the time corresponding to row $v_i$ of the $i$-th feature. Given the first- and last-row poses, constant-twist interpolation approximates the transform between them as follows ($H>1$):

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

Here $\text{Exp}$ and $\text{Log}$ are the exponential and logarithmic maps on the $SE(3)$ Lie group. The code below instead accepts camera-to-world transforms and approximates motion with separate rotational SLERP and linear translation interpolation.

Rolling-shutter correction is optionally supported in some VIO systems (for example, the readout-time setting in [VINS-Mono](https://arxiv.org/abs/1708.03852); ORB-SLAM3 assumes a global shutter), 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 - 1) if H > 1 else 0.0  # first row 0, last row 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. LIO systems including LIO-SAM and FAST-LIO2 use this approach.
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.

**Limited-FoV Livox LiDARs** (Mid-40/70, Avia, HAP, etc.) offer non-repetitive scanning modes. The Mid-40 steers its beam with internal rotating prisms, so it is not free of mechanical rotating parts. Avia also supports repetitive scanning. In non-repetitive mode, coverage within the restricted FoV (for example, roughly 70.4° circular for the Mid-70) grows over time.

The table below compares spinning LiDAR with limited-FoV non-repetitive scanning modes:

| Property | Spinning LiDAR | Limited-FoV Non-repetitive Mode |
|------|-------------|-------------------|
| 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) suit non-repetitive scanning LiDAR, which has no scan lines, because their iterated EKF structure does not depend on scan lines 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 non-repetitive scanning. 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. Its high-frequency observations (100 Hz to 1 kHz) interpolate between camera or LiDAR frames and provide the observability needed for initialization and scale estimation. Fusion also needs an error model for these measurements.

### 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 varies slowly over time and is modeled 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})$$

Set gyroscope noise parameters for the named device and its operating conditions.

- **White-noise density**: estimate it from both the device datasheet and the $-1/2$ slope region of the Allan deviation. Check that the units and continuous-time density convention match the implementation's discrete covariance conversion.
- **In-run bias stability**: identify the nearly flat region of an Allan deviation computed from a sufficiently long static recording. Temperature, mounting stress, vibration, and recording duration all affect it, so do not copy a single datasheet value uncritically.
- **Bias-random-walk driving noise**: estimate it from the positive long-term slope. Keep measurement noise $\sigma_g$ distinct from bias driving noise $\sigma_{bg}$, and document the conversion from continuous-time density to discrete-time covariance.

### 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 from gravity. VIO/LIO sets an arbitrary initial yaw and estimates relative rotations. Global yaw requires observations tied to an external heading reference, such as a magnetometer or a known map.

**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})$$

Apply the same parameter-selection procedure to the accelerometer.

- **White-noise density**: use both the device datasheet and the $-1/2$ slope region of the Allan deviation, and state the conversion among $\text{m/s}^2/\sqrt{\text{Hz}}$, $\text{mg}/\sqrt{\text{Hz}}$, and VRW.
- **In-run bias stability**: inspect the nearly flat Allan region and variance across repeated static and temperature tests. Prevent gravity-projection error and mounting-orientation changes from being absorbed into the bias estimate.
- **Bias-random-walk driving noise**: estimate $\sigma_{ba}$ from the corresponding long-term slope and convert it to the discrete covariance for the filter's sampling interval.

### 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.** The parameters needed for sensor fusion can be extracted from an IMU datasheet as follows:

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. It characterizes the stability of time-averaged stationary sensor output, rather than a theoretical lower bound on the bias-estimation error of a fusion system.
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 a navigation state (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$$

Midpoint integration is common in VIO implementations such as VINS-Mono. The propagation equations and discretization used by each system, including FAST-LIO2, should be checked in its implementation. A high IMU rate does not remove integration error under large angular acceleration or sampling jitter, so choose among midpoint, RK4, and preintegration using the target motion and error budget.

```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.** A simple assumption illustrates how error accumulates when the strapdown integration above does not correct bias. For this example, assume a constant accelerometer bias of $b_a = 0.01\,\text{m/s}^2$ (about $1\,\text{mg}$).

- 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 example shows that position error in an unaided, low-cost MEMS inertial estimate can quickly exceed a mission budget even over a short interval. The useful duration depends on IMU performance, thermal environment, motion, initialization, and the mission's error budget. Higher-performance INS units can operate unaided for longer, but drift still accumulates. VIO/LIO systems **include** the biases $\mathbf{b}_g, \mathbf{b}_a$ **in the state vector** and update them from observations by 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

Labels such as navigation grade, tactical grade, industrial, and consumer do not have uniform boundaries across vendors or fields. MEMS denotes a fabrication technology, not a single performance grade. Select a device from its named specifications and tests on the target platform instead of relying on a grade label or fixed price table.

| Selection criterion | What to inspect | Validation evidence | Design impact |
|----------|------------|----------|----------|
| White noise | Gyro/accelerometer noise density and ARW/VRW units | Device datasheet + Allan test | Short-term propagation error |
| Bias | In-run stability, random walk, temperature coefficient | Repeated static/temperature tests + Allan test | Unaided drift and aiding rate |
| Dynamic range | Range, bandwidth, saturation, and clipping | Target motion/vibration profile | Observation loss during aggressive motion |
| Timing | Output rate, timestamps, and clock/trigger support | Latency and jitter measurements | Temporal calibration and fusion consistency |
| Environmental tolerance | Temperature, vibration, shock, and mounting stress | Vendor qualification + target-platform test | Repeatability and long-term stability |

Determine the external sensors and aiding rate by inserting these measurements into the mission error budget. Because price depends on date and volume, compare current quotes together with the required calibration and synchronization costs.

---

## 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

Fractional carrier-phase observations can have millimeter-scale noise, much smaller than pseudorange noise. That does not imply millimeter positioning accuracy by itself. The L1 carrier (about 1575.42 MHz) has a wavelength of roughly 19 cm, and integer ambiguity, cycle slips, multipath, and atmospheric errors must also be resolved or modeled.

$$\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. Fixed RTK and PPP-AR resolve integer ambiguities, while conventional PPP estimates real-valued ambiguities.
- $\epsilon_\Phi \approx 1\text{–}5\,\text{mm}$: carrier-phase noise (about 1/100 of the pseudorange noise)

The ionospheric delay enters with the opposite sign from the pseudorange (group velocity versus phase velocity). Dual-frequency combinations can estimate or mitigate the first-order ionospheric term, while higher-order terms and other error sources remain.

### 2.4.3 RTK (Real-Time Kinematic)

RTK uses **differenced** observations from a base station or correction network to reduce common errors and resolve carrier-phase integer ambiguities in real time. Atmospheric-error correlation weakens with baseline length; centimeter-class results require a fixed ambiguity solution and favorable signal and correction quality.

**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.** Global position observations from GNSS can bound or correct long-term VIO/LIO drift. Drift and bias can remain during outages or under multipath, frame-alignment, lever-arm, and time-offset errors. [LIO-SAM (Shan et al., 2020)](https://arxiv.org/abs/2007.00258) is a representative system that can add qualified GNSS measurements to its factor graph. Three checks apply 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**: Transform the receiver's position covariance into the observation frame for use in fusion. DOP (Dilution of Precision) is a dimensionless geometry measure, so it cannot be inserted directly as a covariance; use it with a measurement-error scale to estimate position uncertainty.
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 (satellite-to-receiver direction; diff = p_receiver - p_satellite)
        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) / (1 - e2 * sin_lat0**2) + alt0) * dlat  # meridional radius of curvature M
    up = dalt
    
    return np.array([east, north, up])
```

---

## 2.5 Radar Model

Radar is an active sensor that uses radio waves. It operates reliably in **adverse weather (rain, fog, snow, dust)** where cameras and LiDAR degrade, and measures relative **velocity directly** via the Doppler effect.

### 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.

4D imaging radar implements a large virtual antenna array with MIMO (Multiple Input Multiple Output) technology. For example, 12 transmit antennas × 16 receive antennas = 192 virtual channels can be formed. Horizontal and vertical angular resolution depend on the aperture and layout of the virtual array in each direction.

**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 | Possible (depends on detection and clutter-removal settings) | 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 weakens camera and LiDAR observations, radar provides an independent exteroceptive measurement. Performance still depends on precipitation, frequency band, antenna, and signal-processing settings.

```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 measures velocity directly, information that other sensors cannot easily provide. This property is driving increased use of radar 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). Studies have extended 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 $12.0\,\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. When roll/pitch compensation and hard/soft-iron calibration are valid and local magnetic disturbance is small, it can provide a heading relative to the geomagnetic field. True-north heading also requires the local magnetic declination.

**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. UWB-based positioning must detect NLOS and reduce its effects.

**Role in sensor fusion.** Preinstalled UWB anchors allow range-based position estimation in the anchor frame. The result becomes a global absolute position only if the anchor coordinates are surveyed into that global frame. Combined with VIO, UWB can bound drift where anchor geometry and NLOS conditions are adequate.

**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

The table summarizes each sensor's observation model and main characteristics.

| Sensor | Observation | Observation model | Main noise sources | Validation basis |
|------|--------|----------|-------------|------------------|
| Camera | 2D image coordinates | $\pi(\mathbf{T} \cdot \mathbf{P})$ (pinhole + distortion) | Detection noise, distortion residual | Target detector and reprojection residuals on held-out images |
| LiDAR | 3D point $(r, \alpha, \omega)$ | Range-bearing | Range noise, beam divergence, mixed pixel | Device datasheet + residual tests by range, surface, and incidence angle |
| IMU (gyro) | Angular velocity $\boldsymbol{\omega}$ | $\tilde{\boldsymbol{\omega}} = \boldsymbol{\omega} + \mathbf{b}_g + \mathbf{n}_g$ | Bias, random walk | Device datasheet + Allan, temperature, and vibration tests |
| IMU (accel) | Specific force $\mathbf{a}$ | $\tilde{\mathbf{a}} = \mathbf{R}(\mathbf{a}-\mathbf{g}) + \mathbf{b}_a + \mathbf{n}_a$ | Bias, random walk | Device datasheet + Allan, temperature, and vibration tests |
| GNSS | Pseudorange $\rho$ | $\rho = r + c\delta t_r + I + T + \epsilon$ | Multipath, ionosphere, troposphere | Target-environment logs by receiver and correction status |
| Radar | Range, Doppler, bearing | FMCW beat frequency | Clutter, multipath | Device range/velocity/angular resolution + tests by target, RCS, and weather |
| Wheel odom. | Rotation count | $\Delta s = r \Delta\theta$ | Slip | Repeated trajectories by surface, load, and slip condition |
| Barometer | Pressure $P$ | $h = f(P)$ | Weather change | Repeated static, temperature, and weather-condition measurements |
| Magnetometer | Magnetic field $\mathbf{m}$ | $\tilde{\mathbf{m}} = \mathbf{R}\mathbf{m}_w + \mathbf{b} + \mathbf{n}$ | Hard/soft iron | Calibration residual + magnetic-interference test at the operating site |
| UWB | Range $d$ | $d = \|\mathbf{p} - \mathbf{a}\| + n$ | NLOS | Per-anchor LOS/NLOS range residuals |

Even with accurate observation models, fusion performance breaks down when the relative sensor positions or time synchronization are wrong. Using sensors together requires **calibration**, which determines their geometric and temporal relationships.

---

# Ch.3 — Calibration Deep Dive

Ch.2 defined the observation model of each sensor mathematically. Applying these models to real sensor data requires precise knowledge of their parameters. Examples include the camera's focal length, the relative position between LiDAR and IMU, and the time offset between sensors. Calibration is the process of determining these values precisely.

> Calibration error introduces systematic bias into sensor fusion. Calibration spans camera intrinsics, multi-sensor extrinsics, and time synchronization.

Calibration is one of the first checks in a sensor-fusion pipeline. An inaccurate sensor model or relative pose biases the estimate and can make a filter or optimizer unstable under some conditions. In LiDAR-camera fusion, for example, a one-degree angular error gives a small-angle lateral-error approximation of about 87 cm at 50 m. Addressing these errors requires both mathematical foundations and practical tools.

---

## 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 plane.
- $\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 captured in several poses. It is a widely used formulation because it does not require a 3D calibration rig. OpenCV's `calibrateCamera()` can optimize the same kind of 2D–3D correspondences with nonlinear refinement.

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 five homogeneous entries with four degrees of freedom after removing scale, so two nondegenerate views suffice. The general 5-parameter model likewise needs at least three distinct planar poses for its linear solution. This is only an algebraic minimum. In practice, collect enough views to cover positions, distances, and tilts across the image and to obtain a well-conditioned estimate.

#### 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)
    residual = img_points[i].reshape(-1, 2) - img_points_proj.reshape(-1, 2)
    error = np.sqrt(np.mean(np.sum(residual ** 2, axis=1)))
    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

Calibration quality depends strongly on data collection. The following conditions are observability principles, not fixed quotas for image count or board angle.

**Pose Diversity**: The checkerboard should be captured from diverse angles and positions under the following conditions:
- Place the checkerboard near the top, bottom, left, right, and center so observations cover the image
- Avoid repeating nearly fronto-parallel poses; tilt the board about several axes so focal length and extrinsics are distinguishable
- Place the board close to and far from the camera to obtain a variety of scales
- Decide whether to collect more images from pose diversity, conditioning, and the spatial pattern of residuals, not from image count alone

**Corner Accuracy**: if the detector does not already provide refined coordinates, refine corners with a method such as `cv2.cornerSubPix()`. Exclude images with failed detections, motion blur, or glare on the target.

**Illumination Conditions**: uniform lighting stabilizes corner contrast. Strong shadows and glare can move or hide detections, so use a matte target and inspect the exposed images.

**Interpreting Reprojection Error**: no single pixel threshold establishes success. The expected error depends on resolution, lens model, target size, and detector uncertainty. Inspect the global RMS together with per-image residuals, directional patterns across the image, straight-line curvature before and after correction, and error on held-out validation images.

**Detecting Outlier Images**: compare per-image errors using the `errors` array above. Do not discard a high-error image automatically; first inspect it for a failed detection, motion blur, glare, or a non-planar target. Compare parameter stability and held-out error before and after exclusion.

**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 wide-angle and fisheye lenses, the standard radial-tangential model may not describe projection near the image boundary adequately. There is no single FoV cutoff that determines validity; select an equidistant or Kannala–Brandt model according to the lens projection and residual pattern.

**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
)
# CALIB_CHECK_COND aborts with an exception if even one image is badly conditioned.
# In that case, drop the offending image or remove this flag.

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

# The fisheye module requires object points as (N, 1, 3) and image points as (N, 1, 2) (convert the (N, 3) object points from 3.1.4)
obj_points_f = [o.reshape(-1, 1, 3).astype(np.float64) for o in obj_points]
img_points_f = [p.reshape(-1, 1, 2).astype(np.float64) for p in img_points]

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 toolbox](https://sites.google.com/site/scarabotix/ocamcalib-omnidirectional-camera-calibration-toolbox-for-matlab), described by [Scaramuzza et al. (2006)](https://rpg.ifi.uzh.ch/docs/IROS06_scaramuzza.pdf), calibrates 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 a widely used public tool for camera-IMU calibration (Section 3.4). It supports several camera models and batch optimization, so intrinsics can be estimated in the same tool. Precision relative to OpenCV depends on the model, target observations, initialization, and optimization settings.

**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 trajectory handles asynchronous observations in camera-IMU calibration
- **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: first compute homogeneous coordinates $[X,Y,Z,W]^\top = Q \cdot [u,v,d,1]^\top$, then normalize to $\mathbf{P}_{3D} = [X/W,Y/W,Z/W]^\top$.

---

## 3.3 Camera-LiDAR Extrinsic Calibration

Multi-modal sensor fusion requires estimating the extrinsic parameters $(\mathbf{R}, \mathbf{t})$ between a camera and a LiDAR. 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

Target-based calibration creates correspondences by observing a known geometric target (checkerboard, AprilTag, etc.) simultaneously with the camera and LiDAR.

**Principle**: Checkerboard corners are observed as 2D points in the camera image, while the board surface is observed 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. Estimate the target plane in camera coordinates from detected corners and the known checkerboard geometry. Constrain LiDAR points on the checkerboard to lie on this plane after applying the unknown extrinsic transform.

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 = []

# Collect (camera-frame plane, LiDAR-frame plane) pairs from each observation. Planes are n·x + d = 0
    planes_cam, planes_lidar = [], []
    for corners_2d, (normal_l, d_l) in 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)
        # Checkerboard plane in the camera frame: normal is the board z-axis, distance from the board origin
        n_c = R_cam_board[:, 2]
        d_c = -n_c @ tvec.ravel()
        planes_cam.append((n_c, d_c))
        planes_lidar.append((normal_l, d_l))

# Rotation: align normal pairs (Kabsch/SVD), n_c = R n_l
    N_c = np.stack([n for n, _ in planes_cam])
    N_l = np.stack([n for n, _ in planes_lidar])
    U, _, Vt = np.linalg.svd(N_c.T @ N_l)
    R_cam_lidar = U @ np.diag([1, 1, np.sign(np.linalg.det(U @ Vt))]) @ Vt

# Translation: plane-distance constraint n_c · t = d_l - d_c by least squares (needs at least 3 planes)
    b = np.array([d_l - d_c for (_, d_c), (_, d_l) in zip(planes_cam, planes_lidar)])
    t_cam_lidar, *_ = np.linalg.lstsq(N_c, b, rcond=None)

# The final result is refined by nonlinear optimization
    return R_cam_lidar, t_cam_lidar
```

**Practical considerations**:
- Choose the checkerboard size from range, angular resolution, and beam pattern so that each observation contains enough points for a stable plane estimate.
- Collect views at several ranges and incidence angles; judge the dataset by plane-orientation and position diversity and by conditioning, not by a fixed observation count.
- With a sparse LiDAR, inspect the actual hit pattern and adjust target size and distance accordingly.

### 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 one variable is a deterministic function of the other, their mutual information is bounded by the smaller entropy; for an invertible one-to-one relation, $\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://arxiv.org/abs/1507.07595) applied 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 a LiDAR depth image and a camera image as input and regress a 6-DoF correction. Their operating range depends on the sensors, scenes, and miscalibration distribution used for training. On new hardware, validate them with representative data and, when appropriate, use the estimate to initialize geometric optimization.

### 3.3.3 Koide et al. (2023) — An Open Targetless Calibration Implementation

The `direct_visual_lidar_calibration` of [Koide et al. (2023)](https://arxiv.org/abs/2302.05094) is a public implementation of targetless LiDAR-camera calibration. Its pipeline has three stages:

**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.

SuperGlue proposes correspondences between a rendered LiDAR-intensity image and camera RGB. The paper reports an initialization success rate above 80% under its datasets and success criterion; this is not a guarantee for other sensors or scenes.

**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.

**Reported result**: under the authors' evaluation, the mean translation error was 0.043 m and the mean rotation error was 0.374 degrees. Their experiments included several combinations of rotating and solid-state LiDARs with pinhole, fisheye, and omnidirectional cameras.

### 3.3.4 Practical Tool Comparison

| Tool | Approach | Target needed | Operating mode | Check before use |
|------|----------|---------------|----------------|------------------|
| Autoware Calibration Toolkit | Target-based | Yes | Offline workflow with operator steps | Supported sensors and target; initialization |
| `direct_visual_lidar_calibration` (Koide) | Targetless (NID) | No | Automated offline pipeline | Scene overlap, intensity rendering, initialization success |
| ACSC (Automatic Calibration) | Target-based + automatic corner extraction | Yes | Automated target detection | Required target and supported sensor model |
| Online extrinsic estimation (VIO/LIO state augmentation) | Online | No | Estimation during operation | Motion observability and drift-monitoring criterion |

Recently, [MFCalib (2024)](https://arxiv.org/abs/2409.00992) combined depth-continuity, depth-discontinuity, and intensity-discontinuity edges. Its authors model LiDAR-beam formation to address edge inflation and report lower calibration errors than the compared methods under the paper's datasets and metrics.

Koide et al. (2023) propose using an initial target-based result as the reference and targetless NID to monitor change during operation. Whether to recalibrate should be decided from a validated relationship between NID changes and task error.

---

## 3.4 Camera-IMU Extrinsic + Temporal Calibration

The spatial displacement (extrinsic) and temporal offset between the camera and IMU must be estimated together. VIO (Visual-Inertial Odometry) systems such as VINS-Mono and OpenVINS take both parameters as initial values and refine them during operation, whereas calibration tools such as Kalibr are what estimate these two values in the first place.

### 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$.

Furgale et al. (2013) included camera-IMU time offsets from several to tens of milliseconds in their experiments. Ignoring an offset can increase reprojection error during rapid rotation. For example, a 10 ms offset at 100 deg/s corresponds to a one-degree mismatch in observation time.

### 3.4.2 Kalibr: Continuous-Time B-Spline-Based Calibration

Kalibr, proposed by [Furgale et al. (2013)](https://ieeexplore.ieee.org/document/6696514), has been adopted as a calibration tool for major VIO systems including VINS-Mono, OpenVINS, and MSCKF.

The trajectory is represented not as a sequence of discrete poses but as a continuous-time B-spline. This representation 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)$

Three properties 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**: evaluate the trajectory at each sensor timestamp without resampling everything to common frame times
3. **Locality**: the trajectory at each time depends on only 4 neighboring 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)^{-1} \cdot \mathbf{p}_w\right)
$$

Here $\mathbf{T}(t)$ maps the IMU (body) frame to the world, and $\mathbf{T}_{CB}$ maps the IMU (body) frame to the camera (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
#    - Record until all axes are excited, the target remains detectable, and residuals are well conditioned

# 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)
```

**Data collection**:
1. **Motion diversity**: provide sufficient excitation in all 6-DoF motion components. 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**: design a trajectory that keeps the target detectable while providing motion about all relevant axes.
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$ estimated from this dataset, including its sign convention
- Reprojection-error distribution: inspect temporal and image-space residual patterns and outliers in addition to the global mean
- Accelerometer/gyro residuals: must match the noise model

### 3.4.4 Allan Variance Measurement Hands-On

Accurate IMU noise parameters affect Kalibr calibration and the performance of IMU-based systems. Allan variance analyzes the noise characteristics of a time series across cluster times; it was originally developed to measure atomic-clock stability.

**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)
    # Retain at least ten clusters at the largest tau.
    max_n = max(1, N // 10)

# 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):
    """
    Extract IMU noise parameters from the Allan variance plot.
    """
    adev = np.sqrt(allan_vars)

valid = (taus > 0) & (adev > 0) & np.isfinite(adev)
    taus, adev = taus[valid], adev[valid]
    slopes = np.gradient(np.log(adev), np.log(taus))

# Fit the coefficient N of sigma(tau) = N / sqrt(tau) over a -1/2 region.
    white = np.abs(slopes + 0.5) < 0.1
    if not np.any(white):
        raise ValueError("No stable -1/2-slope region; collect or inspect more data")
    noise_coefficient = np.median(adev[white] * np.sqrt(taus[white]))

# Report bias instability only if a near-flat region is present.
    flat = np.abs(slopes) < 0.1
    bias_instability = np.min(adev[flat]) / 0.664 if np.any(flat) else np.nan
    tau_min = taus[np.argmin(adev)]

return {
        'noise_coefficient': noise_coefficient,
        'bias_instability': bias_instability,
        'tau_min': tau_min
    }

# Usage example
# 1. Record enough stationary data to retain many clusters at the largest tau of interest
# 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)

print(f"White-noise coefficient: {params['noise_coefficient']:.6f}")
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_coefficient'] / 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. Choose the recording length so that many non-overlapping clusters remain at the largest $\tau$ of interest. If no bias-instability plateau appears, extend the record, but no fixed duration guarantees identification.
- **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**: fit the white-noise coefficient over the $-1/2$-slope region, then check that the Allan-deviation definition and units match Kalibr's continuous-time noise-density convention. Copying the sample nearest $\tau=1$ second can hide slope and unit mismatches.
- **Temperature stabilization**: begin after sensor temperature and output mean have stabilized, and record temperature with the data. Warm-up time depends on the IMU, mounting, and ambient conditions.

---

## 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**:

Let the rotation axes and angles be $\hat{\mathbf{a}}_A, \alpha$ and $\hat{\mathbf{a}}_B, \beta$. Define $\mathbf{p}_A = 2\sin(\alpha/2)\hat{\mathbf{a}}_A$ and $\mathbf{p}_B = 2\sin(\beta/2)\hat{\mathbf{a}}_B$. Then:

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

Here $\mathbf{r}_X = \tan(\theta_X/2)\hat{\mathbf{a}}_X$ is the 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

# Scaled quaternion vector parts: 2 sin(angle / 2) * axis
        a_prime = 2 * np.sin(alpha / 2) * rA / alpha
        b_prime = 2 * np.sin(beta / 2) * rB / beta

C.append(skew_sum)
        d.append(b_prime - a_prime)

C = np.vstack(C)
    d = np.concatenate(d)

# Least-squares solve
    rX, _, _, _ = np.linalg.lstsq(C, d, rcond=None)

# Rodrigues vector → 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)

FAST-LIO2 can estimate LiDAR-IMU extrinsics during operation, but it still requires a useful initial value and sufficient excitation. [**LI-Init**](https://arxiv.org/abs/2202.11006) is a separate initialization module that estimates the time offset, extrinsics, gravity, and IMU bias before handing the state to FAST-LIO2. It proceeds in three steps:

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. Hand the result to FAST-LIO2. Subsequent online refinement is carried out by FAST-LIO2's Error-State Iterated Kalman Filter (ESIKF), which updates the LiDAR-IMU extrinsic included in its state vector

The initialization module estimates extrinsics when LIO starts, without a separate target or additional sensor. Sufficient motion excitation is required; the public implementation recommends an initial stationary period and 15–30 seconds of online refinement after initialization.

**Advantages**: no separate calibration target or additional sensor needed. Initialization can be performed 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.

Systems with multiple LiDARs, such as autonomous vehicles, need the relative poses between LiDARs as well as the LiDAR-IMU extrinsic parameters.

---

## 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

At least 3 plane correspondences with non-coplanar normal vectors are 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**. GNSS/INS integrated navigation requires this parameter to be calibrated.

### 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. For example, if a 1 m lever arm perpendicular to the rotation axis is corrected only at a reference attitude and a subsequent 10 deg rotation is ignored, the position error is about 17 cm.

### 3.7.2 Lever Arm Estimation Methods

**Method 1: Physical measurement**

The most direct method uses a tape measure, laser rangefinder, or similar tool. Derive the required accuracy from the antenna-to-IMU distance, expected rotation, and allowable position error. Direct measurement is adequate if an error of several centimeters fits that error budget; otherwise use precision surveying or estimation.

**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. In straight-line motion at constant attitude, $\mathbf{R}\mathbf{l}$ is absorbed into a constant bias, so none of the three lever-arm components is observable; with yaw rotation only, the vertical component ($l_z$), parallel to the rotation axis, remains unobserved.

**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 RTK or PPP processing, apply antenna phase-center corrections when required by the error budget and processing convention. The decision depends on the antenna model, base/rover processing, attitude changes, and target accuracy.

---

## 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 an automated 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.

### 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 must remain consistent across the entire **sensor chain**. For example, a vehicle with six cameras and one LiDAR may calibrate each camera-LiDAR pair independently, but the relative poses among the cameras must still agree. OpenCalib includes these consistency constraints in 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

When a shared clock, trigger, and timestamp path can be configured clearly, hardware synchronization is the preferred starting point. Device-specific latency and the timestamp reference instant still need separate validation.

**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 can output a 1 Hz electrical pulse synchronized with GNSS time. If the receiver is configured to place its rising edge on the GNSS-second boundary, the edge can serve as a common reference. Verify receiver configuration, cable and input-circuit delay, and each device's timestamp reference point.

PPS synchronization measures 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 synchronizes clocks over Ethernet. With hardware timestamping and a PTP-aware network, sub-microsecond errors can be possible, but actual accuracy depends on the devices and network configuration.

**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
```

Some LiDARs and industrial cameras support PTP and hardware timestamping. Support and the timestamp reference point must be checked in the datasheet for the specific model.

### 3.9.3 Software Synchronization

When hardware synchronization is not available, time alignment is done in software.

**Host-clock based**: timestamp data with the host system clock when it arrives. The value then includes USB, network, and driver-queue latency and jitter; their magnitude depends on the device and host load and must be measured.

**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://arxiv.org/abs/1310.7863) proposed including the time offset $t_d$ in the state vector of VIO and estimating it online. The observation model incorporates the offset as follows:

**Reflecting the time offset in the observation model**:

The sensor pose corresponding to camera observation time $t_c$ is evaluated at the corrected time $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).

Modern VIO systems such as OpenVINS and VINS-Mono implement variants of this method. Kalibr (Section 3.4.2), an offline tool, estimates the same quantities separately through continuous-time B-spline batch optimization.

### 3.9.5 Practical Synchronization Strategy Guide

| Configuration | First method to consider | What to verify |
|---------------|--------------------------|----------------|
| A shared trigger and clock can be wired | Hardware trigger + common clock/PPS | Timestamp reference within exposure, sample, or scan; fixed latency |
| Every device and switch supports PTP and hardware timestamps | PTP | Grandmaster selection, PTP profile, path asymmetry, lock state |
| Device clocks are readable but no shared signal exists | Clock conversion + offline/online offset estimation | Clock drift, motion observability, time variation of the offset |
| Only receive timestamps are available | Use host timestamps provisionally and measure jitter | USB/network queueing; change hardware if the error budget is not met |

**For systems requiring hardware synchronization**: PPS and PTP can be combined to align multiple sensors to a GNSS-referenced clock.

**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.

When hardware synchronization is available, consider it first and still measure timestamp semantics and fixed or variable latency. Software offset estimation supplements the hardware path or serves setups in which a shared trigger is unavailable.

---

## 3.10 Chapter Summary

The table summarizes the parameters and methods for each sensor combination.

| Calibration type | Parameters estimated | Method | Minimum requirement |
|----------------|-------------|----------|-------------|
| Camera intrinsic | $\mathbf{K}, \mathbf{d}$ | Zhang's method | Diverse target poses covering the image |
| Stereo extrinsic | $\mathbf{R}, \mathbf{t}$ | Shared target + stereoCalibrate | Diverse poses detected simultaneously by both cameras |
| Camera-LiDAR | $\mathbf{T}_{CL}$ | Target-based / NID | Multiple target views / natural scene |
| Camera-IMU | $\mathbf{T}_{CI}, t_d$ | Kalibr (B-spline) | A sufficiently long sequence with 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.

**Related papers**:

- [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://arxiv.org/abs/1310.7863): 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).

Once the sensor models and calibration parameters are in place, the robot's state can be estimated from sensor data. Ch.4, **State Estimation Theory**, explains this computation from Kalman filters through factor graphs.

---

# Ch.4 — State Estimation Theory

Sensor observation models and inter-sensor calibration parameters are inputs to the **algorithms** that estimate a robot's state (position, attitude, velocity, etc.). Kalman filters, factor graphs, and IMU preintegration process those inputs in the odometry and fusion systems of Ch.6–8.

> State estimation runs from Bayesian filtering through the Kalman filter family and the particle filter to factor-graph-based optimization. The derivations also explain why modern robotics systems moved from filtering to optimization.

---

## 4.1 Bayesian Filtering Framework

### 4.1.1 Definition of the State Estimation Problem

Robot state estimation is a **conditional probabilistic inference** problem. It seeks 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)$$

These two assumptions let us update the current estimate from the previous estimate without storing the entire observation history $\mathbf{z}_{1:k}$. This procedure is **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)

Applying a linear transformation to a Gaussian yields another Gaussian. Thus the posterior remains Gaussian 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.

For a jointly Gaussian distribution, the conditional is also Gaussian, and its 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}$ (small measurement noise): the update follows the measurement constraint more strongly in the observable subspace. The notation $\mathbf{H}_k^{-1}$ applies only when $\mathbf{H}_k$ is square and invertible
- $\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
- If the linear model and specified noise covariances are correct, the Kalman gain gives the conditional-mean weighting.

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

This is the difference between the predicted and actual observation. A zero innovation means that this measurement residual is zero, not that unobserved state components or model error vanish.

**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 in 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$. If $\mathbf{P}_{k|k-1}$ and $\mathbf{R}_k$ are positive semidefinite, the expression preserves that algebraic structure; implementations may also symmetrize the result or use factorized solvers to limit roundoff.

#### 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 that extends to time-varying and multivariate systems.

#### 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-velocity model + continuous-time white-noise acceleration)
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, and GPS coordinate transformations are all nonlinear functions. The EKF is the most direct way to extend the Kalman filter to nonlinear systems.

#### 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 **linearizes 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 evaluate the Jacobians $\mathbf{F}_k$ and $\mathbf{H}_k$ at each time step's linearization points. 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 widely used in state estimation with an IMU. Filter-based VIO systems such as MSCKF and OpenVINS, and the FAST-LIO family, use error-state or closely related manifold-filter formulations. [VINS-Mono](https://doi.org/10.1109/TRO.2018.2853729), by contrast, is based on sliding-window nonlinear optimization rather than an 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 a naive EKF state update $\hat{\mathbf{x}} \leftarrow \hat{\mathbf{x}} + \mathbf{K} \tilde{\mathbf{y}}$, the "+" is Euclidean addition. Adding an increment directly to four quaternion components breaks the unit norm. A direct quaternion EKF can be designed with consistent normalization and covariance projection, but normalizing only the value after an additive update can mishandle uncertainty in the constrained direction.

**Property 2: The error state is "almost zero"**

After an update, the error state $\delta\mathbf{x} = \mathbf{x} \boxminus \hat{\mathbf{x}}$ is injected into the nominal state and reset to zero. If the nominal trajectory remains close to the true state, the model can be linearized in a small tangent-space neighborhood. Resetting alone does not guarantee a small linearization error, so initialization, observability, and model error still matter.

**Problem 3: Different dynamics of the nominal and error states**

Large motion changes are integrated in the nominal state, while small deviations around it are tracked in the error state. This supports prediction with high-rate IMU data and error correction when lower-rate external observations arrive.

#### 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 local difference between the nominal and true states, estimated by a Kalman filter. While the nominal state remains close, a first-order tangent-space approximation is useful; after injection, a reset Jacobian changes the covariance coordinates consistently.

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 quaternion's 4D error is represented by 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. An EKF can keep all four quaternion components, but it must handle normalization and the constrained covariance direction consistently.

#### 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 mean and covariance, including process noise $\mathbf{Q}_k$
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).
- Propagates deterministic sigma points through the nonlinear function, approximating curvature in the transformed mean and covariance. The formal order depends on the distribution, function, and sigma-point parameters.
- 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 straightforward.
- ESKF formulations are common for IMU state estimation because they handle local manifold errors without propagating every sigma point. Which filter is more accurate depends on the model, initial error, and tuning.

### 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 in Section 4.5.

FAST-LIO2 uses a manifold IEKF that repeatedly relinearizes many LiDAR point-to-plane residuals during the state update. This can account for residual nonlinearity beyond a single linearization, but the gain and required iteration count depend on the initial error, scene geometry, and stopping rule.

---

## 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 previous weight is multiplied by the observation likelihood at that particle's location. Given equal previous weights, particles more consistent with the observation receive higher 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$. It is inexpensive and often reduces variance relative to multinomial resampling, but it does not guarantee the minimum variance for every weight configuration.

**Stratified resampling**: use independent uniform random numbers within each stratum. Like systematic resampling it divides the interval into strata, but draws independently within each stratum rather than sharing one random offset.

```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 conditional independence is central to FastSLAM's efficiency. For one observation with known landmark association, balanced-tree map management gives an update cost of $O(N \log M)$ for $N$ particles and $M$ landmarks, versus $O(M^2)$ for the corresponding EKF-SLAM update.

### 4.3.5 Limitations of the Particle Filter and Its Current Role

A major limitation of the PF is the **curse of dimensionality**. In general high-dimensional problems, the particle count needed for a useful approximation can grow very rapidly. VIO and LIO states usually include attitude, velocity, and biases, so representing the full state with particles is often inefficient in real time unless a strong proposal or Rao–Blackwellization reduces the sampled dimension.

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. Recovery after an unexpected relocation during operation (the kidnapped-robot problem) also requires a recovery strategy, such as redistributing particles. 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$$

Under a correct probabilistic model and the same loss, conditioning on future observations can reduce a smoother's expected Bayes risk. This does not guarantee lower realized error on every trajectory or for every approximate optimizer. Real-time estimation uses filters as well as incremental and fixed-lag smoothers; batch smoothers are also used for post-processing.

### 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$ lets the estimator revisit a past state with additional observations.

The sliding-window optimization systems of VINS-Mono, ORB-SLAM3, and similar systems are close to fixed-lag smoothers. They can relinearize keyframes jointly inside the window, but an accuracy comparison with filtering must hold compute budget, model, and dataset constant.

### 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) for four 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, increasing the number of features improves accuracy more than increasing the number of frames, and bundle adjustment, whose cost is linear in the number of features, is more efficient in that respect than filtering, whose cost is cubic.

**2. Consistency issues**

Without an external reference, SLAM has gauge freedoms: transforming the global frame can leave all observations unchanged. Their number differs among 2D SLAM, 3D SLAM, and visual-inertial estimation; VIO, for example, normally has four unobservable directions, three for global position and one for yaw about gravity. Implementations anchor the first pose or selected degrees of freedom to choose a gauge. If an EKF linearizes at changing estimates, it can nevertheless inject spurious information along these directions and become inconsistent. First-Estimate Jacobian (FEJ) and observability-constrained formulations are designed to preserve this null space more faithfully.

**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 is connected only to variables that share factors, so many matrix entries are zero. Exploiting this sparsity can reduce work relative to a dense solve, but actual complexity depends on graph topology, elimination ordering, and fill-in introduced by loop closures.

**4. Natural handling of loop closures**

In filter-based systems, handling a loop closure requires retaining the covariance information for past states, which increases the computational 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 | A basic EKF linearizes once per update | Batch/smoothing methods can relinearize repeatedly |
| Past states | A basic filter removes them, but state augmentation is possible | Full smoothing retains them; fixed-lag smoothing marginalizes them |
| Loop closure | Requires retaining past state and correlations | Add a factor and update the affected region |
| Matrix structure | Covariance is usually dense | Factor/Hessian sparsity can be exploited |
| Cost | Depends on state and observation structure | Depends on graph topology, ordering, and fill-in |
| Consistency | Requires care with linearization and observability | Relinearization alone is insufficient; gauge and marginalization still matter |

Filter-based approaches remain in use. [MSCKF (Mourikis & Roumeliotis, 2007)](https://ieeexplore.ieee.org/document/4209642) and OpenVINS target bounded-compute settings, and FAST-LIO2 combines an IEKF with an ikd-tree. Accuracy comparisons between filtering and optimization require matched datasets, sensors, and compute budgets.

---

## 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. A "probabilistic potential" 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 |

Factor graphs are **modular**. Adding a sensor requires defining its factor and adding 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 blocks of $\mathbf{H}$ represent couplings between variables that share a factor. In SLAM problems where each factor connects only a few variables, this structure makes $\mathbf{H}$ 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 drops second derivatives of the residuals and approximates the Hessian with $\mathbf{J}^\top\mathbf{J}$. It may diverge from a poor initial estimate or under strong nonlinearity. **Levenberg-Marquardt (LM)** adds a damping term that shifts behavior between GN-like and gradient-descent-like steps:

$$(\mathbf{H} + \lambda \mathbf{I}) \Delta\mathbf{X} = -\mathbf{b}$$

- Small $\lambda$ → closer to GN (fast near a well-conditioned solution)
- 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.

iSAM2 uses four mechanisms:

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.

GTSAM implements iSAM2, which serves as the backend of systems such as [LIO-SAM](https://arxiv.org/abs/2007.00258). VINS-Mono instead uses Ceres-based sliding-window optimization.

> **Recent trends — continuous-time factor graph**: there is active research extending discrete keyframe-based factor graphs to **continuous time**. [Burnett 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) |
| Design | 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 | Expression-based support (numerical differentiation also available) | Auto-diff (ceres::AutoDiffCostFunction) | AutoDifferentiation support |
| Language | C++ (Python bindings) | C++ | C++ |
| Representative users | LIO-SAM | Cartographer, VINS-Mono | ORB-SLAM3 and 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)
    # use accumulated odometry as the initial guess
    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. At these rates, approximately 7–100 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)$$

**The right-hand sides depend only on the IMU measurements and the bias estimates, and are independent of the global navigation state $(\mathbf{R}_i, \mathbf{v}_i, \mathbf{p}_i)$ at keyframe $i$.** Therefore, when the keyframe navigation state 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)$. Rotation composition is handled with manifold operations and can be implemented using rotation matrices or unit quaternions.

#### 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), a first-order correction can be used. If the corrected residual or bias change exceeds the implementation's validity threshold, recompute preintegration from the stored IMU samples.

#### 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   # the new term is not multiplied by dR.T
        
        # --- 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))
        # continuous-time noise density -> discrete-time covariance: density^2/dt (B already carries a factor of dt)
        Sigma_eta[0:3, 0:3] = np.eye(3) * self.sigma_g**2 / dt
        Sigma_eta[3:6, 3:6] = np.eye(3) * self.sigma_a**2 / dt
        
        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)  # for a Z-up frame; the internal gravity vector is (0, 0, -9.81)
    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
    # ImuFactor connects pose and velocity at i, j and the bias at i
    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

Marginalization uses 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}^*$ can become denser than $\mathbf{H}_{rr}$ through this fill-in, making previously indirect dependencies explicit as direct variable couplings.

```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,) — negative 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 — negative 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:
- Keeping linearization points consistent between marginalized priors and current factors suppresses spurious information gain along unobservable directions.
- 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

VINS-Mono uses two strategies:
- **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's local-map point deletion is a separate operation for controlling map size.

#### 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.

When removing the oldest keyframe, VINS-Mono marginalizes that frame's velocity and biases along with its pose. Biases of the remaining keyframes continue to be estimated within the window and remain connected to the prior.

#### 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

The state estimation methods relate as follows:

1. The **Bayesian Filtering Framework** is the prediction-update recursion for sequential filters. The Chapman-Kolmogorov equation and Bayes' rule are its basis, while general nonlinear systems require approximate integration. Batch smoothing and factor graphs solve related probabilistic models with a different computational structure.

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. ESKF-family formulations are widely used in IMU-based filters.

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** modularly composes probabilistic inference and reduces MAP to NLS. Gauss-Newton or LM solves it on the manifold, while iSAM2 performs incremental smoothing in real time.

6. **IMU preintegration** compresses high-rate IMU measurements into a factor between keyframes. The on-manifold derivation avoids reintegration when global poses change; small bias changes use a first-order correction, while large changes require reintegration.

7. **Marginalization** transfers linearized information from removed variables into a prior through the Schur complement. FEJ is one way to preserve unobservable directions and reduce inconsistency.

The same machinery appears in VIO (Ch.6), LIO (Ch.7), and multi-sensor fusion (Ch.8).

> **2024-2025 research directions**: the Equivariant Filter and Invariant EKF exploit Lie-group symmetries to improve error dynamics, consistency, and convergence analysis for specific systems and assumptions. Continuous-time factor graphs with Gaussian-process motion priors support asynchronous multi-sensor fusion. Work such as [AI-Aided Kalman Filters (Shlezinger et al., 2024)](https://arxiv.org/abs/2410.12289) learns a gain or part of the model; distribution shift and safety validation remain open.

---

# 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. Its core is feature matching and correspondence search.

> Almost every component of sensor fusion — Visual Odometry, calibration, loop closure, point cloud registration — depends on **correspondence**. The technical flow runs from mutual information to RoMa, and each step addresses a limitation of the previous generation.

---

## 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. 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 a correspondence between the two sensors' observations. Target-based methods use the same physical point; targetless MI-based methods (§5.4) use the statistical dependence over the overlapping region instead.
- **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. In the geometric equations below the point is written in homogeneous coordinates $(u, v, 1)^\top$, since both the product with a $3 \times 3$ matrix and the application of $\mathbf{K}^{-1}$ are defined there.

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 The Role of Correspondence in Sensor Fusion

Sensor-fusion accuracy depends heavily on correspondence quality. Incorrect correspondences (outliers) can destabilize pose estimation, while conditions with too few usable correspondences, such as texture-poor scenes, can make pose estimation impossible. The methods developed to address these failures range from traditional feature detection and description to learned matching.

---

## 5.2 Traditional Feature Detection & Description

The traditional correspondence pipeline consists of three stages: **detect → describe → match**. Detection selects keypoints, and description represents them for comparison.

### 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)

# Threshold only — cornerHarris returns the response map, so NMS is a separate step
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 (ECCV 2006; the link is the extended version by Rosten, Porter & Drummond, TPAMI 2010)](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 — reject immediately unless at least 3 of them are all brighter or all darker. This shortcut holds for $N=12$ and does not carry over to variants such as $N=9$, which is why the learned decision tree below takes its place.

$$\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 detector and descriptor designed to be invariant to scale and in-plane rotation. Its normalized gradient descriptor is robust to a range of illumination changes and affine distortion. It has long served as a representative feature-matching baseline, and its US patent expired in 2020.

**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. It reduces computation through three design choices:

- 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.

SURF is designed to approximate Hessian responses efficiently with integral images and box filters. Speed and accuracy differences depend on implementation, image size, and hardware.

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 intensities at point pairs. Hamming distance can be implemented with XOR and popcount and is often cheaper than a floating-point distance, but the ratio depends on descriptor length, library, and hardware.

| 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 and popcount. popcount is a word-level instruction, so a 256-bit descriptor takes four 64-bit XOR and popcount operations plus the sum, and a 512-bit descriptor twice that:

$$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 (speed-oriented, 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. SuperPoint, for example, learns detection and description jointly to target matching that is robust to viewpoint and illumination changes, while its speed, about 70 FPS for 640×480 input on a GPU (§5.5.1), is tied to the hardware conditions.

---

## 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

It is 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)

[Fischler & Bolles (1981)](https://dl.acm.org/doi/10.1145/358669.358692) perform robust estimation as follows:

1. Randomly sample the minimum $n$ matches required for the model (e.g., 8 or 7 points for a fundamental matrix, or 5 points for an essential matrix)
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.

If the ranking score correlates with inlier probability, trying high-ranked matches first can produce a valid hypothesis earlier. The gain depends on score calibration and the actual inlier ordering.

#### 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 the threshold $\sigma$ on the range $[0, \sigma_{\max}]$:

$$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.
- The inlier/outlier decision threshold disappears, so sensitivity to its selection is substantially reduced. The integration bound $\sigma_{\max}$ still has to be specified, which is what the `ransacReprojThreshold` argument below supplies.

```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)   — a landmark random-sampling paradigm for robust estimation
    ↓ exploiting prior information
PROSAC (2005)   — progressive sampling based on match quality
    ↓ threshold automation
MAGSAC++ (2020) — robust estimation without an inlier threshold
    ↓ learning-based rejection
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.

MI measures **nonlinear statistical dependence** between two variables, so it can capture relationships that a simple correlation coefficient cannot.

### 5.4.2 MI-Based Multi-Modality Registration

MI can **register sensor data across different modalities**. The method was originally developed in medical imaging for CT-MRI registration.

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 optimize an MI registration objective with a gradient-based method, we must compute its 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, which makes them vulnerable to illumination, viewpoint, and weather changes. Deep learning overcame these limitations by learning more robust feature representations from large-scale 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 **computes keypoint locations and descriptors in one fully convolutional network** and trains them with self-supervised Homographic Adaptation.

#### Homographic Adaptation Training Strategy

SuperPoint provides **a way to train a highly repeatable keypoint detector without manual 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 bicubic 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.

**Reported performance**: detection and description share one forward pass. The SuperPoint paper reports about 70 FPS for 640×480 input in the authors' GPU implementation; throughput must be remeasured for the current hardware and keypoint count.

```python
import torch
# SuperPoint usage example (cvg/LightGlue package; kornia.feature has no SuperPoint class)
from lightglue import SuperPoint

# Load model
sp = SuperPoint(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({'image': img_tensor})   # returned keys: keypoints, keypoint_scores, descriptors
    keypoints = pred['keypoints']        # (1, N, 2)
    descriptors = pred['descriptors']    # (1, 256, N)
    scores = pred['keypoint_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**.

D2-Net uses the intermediate VGG16 feature map $\mathbf{F} \in \mathbb{R}^{H \times W \times C}$ for both tasks:
- **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 the detection grid sits at 1/4 of the input resolution, so localization precision is tied to that spacing. The original paper applies a local refinement on this grid to recover sub-pixel positions.

### 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.

DISK directly optimizes matching accuracy and learns detection and matching end to end.

### 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 jointly solves keypoint matching and unmatched-point rejection with a **graph neural network, attention, and optimal transport**.

#### 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 minimizing 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 uses **adaptive computation**. The paper reports accuracy comparable to SuperGlue with an average 3–5× speedup in its evaluation; the actual ratio depends on the number of keypoints and the hardware.

#### Diagnosis of SuperGlue's Problems

- The public SuperGlue configuration uses a fixed GNN depth and number of Sinkhorn iterations, so it does not reduce computation for easier inputs.
- $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**, so Sinkhorn iterations are not used. The accuracy difference must be evaluated for the dataset and configuration.

$$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**: The LightGlue paper reports accuracy comparable to SuperGlue with an average **3–5×** speedup in its evaluation, and larger gains on easy image pairs. The actual ratio depends on keypoint count and hardware.

```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)
```

Even as deep learning replaced each stage of the pipeline, the three-stage serial structure remained. Its advantages are modularity and interpretability; its disadvantage is that a failure in detection causes the entire pipeline to fail. The detector-free methods in the next section remove that detection stage.

---

## 5.7 Detector-Free Matching

### 5.7.1 Why Detector-Free Is Needed

In a detect-then-match pipeline, **matching cannot proceed if the detector fails to find keypoints**. This problem is especially visible in the following environments:

- **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**.

#### 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.

Linear attention cuts computation at the cost of limiting what attention can express. QuadTree Attention and ASpanFormer, described next, address that limit by changing the structure of attention.

#### 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 removes the detector from the detect-then-match pipeline used by SuperPoint and SuperGlue. Transformer attention performs detection and matching together. Since every position in one image communicates directly with every position in the other through cross-attention, matching no longer requires 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.

Attention is organized 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 the $O(N^2)$ cost of standard attention to $O(N)$ while maintaining accuracy. The baseline for that comparison is standard attention; the linear attention LoFTR uses is also already $O(N)$, and the original paper reports improved accuracy at a computational cost comparable to linear attention.

### 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?**

The **attention span** is adjusted independently at each 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): DINOv2 + Dense Matching

[Edstedt et al. (2024)](https://arxiv.org/abs/2305.15404)'s RoMa combines DINOv2 features with detector-free dense matching. Two design choices distinguish it from 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 provides general-purpose visual features learned through large-scale self-supervision. RoMa freezes those features and trains only 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)$$

Predicting a distribution lets an ambiguous match receive low probability so the loss is not dominated by a single correspondence. Robustness, however, depends on the family of distribution. Gaussian families have light tails and penalize large errors quadratically, so a heavy-tailed distribution, an explicit outlier component, or recasting regression as classification is needed before incorrect ground truth actually loses its influence.

#### Performance

In the RoMa paper's relative-pose experiments, it reports higher AUC than the tested LoFTR- and ASpanFormer-family baselines on MegaDepth and ScanNet. The margin depends on the dataset and angular threshold; the paper also highlights a 36% relative improvement on WxBS, which includes large viewpoint and appearance 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 combines two design choices:

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 with LoFTR's detector-free design and pretrained DINOv2 features.

### 5.7.6 Recent Developments: 3D-Aware Dense Matching (2024-2025)

In 2024-2025, methods emerged that go beyond 2D matching to **directly predict 3D geometry while performing matching**.

**DUSt3R (Wang 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 of the 3D prediction**.

**MASt3R (Leroy et al., 2024)**: [MASt3R](https://arxiv.org/abs/2406.09756) adds a dense local-feature head to DUSt3R. Its authors report a 30-percentage-point VCRE AUC improvement over the previous compared method under the paper's map-free localization setup.

**VGGT (Wang et al., 2025)**: [VGGT](https://arxiv.org/abs/2503.11651) (Visual Geometry Grounded Transformer) received the CVPR 2025 Best Paper award. It **simultaneously infers camera parameters, pointmaps, depth maps, and 3D point tracks in a single feed-forward pass** from one or more images. Under the paper's evaluation settings, it reports image reconstruction in under one second and higher results on several 3D tasks than the tested alternatives that use geometry-optimization post-processing.

These methods reverse the usual order of first finding 2D correspondences and then reconstructing 3D. They infer 3D geometry directly and obtain correspondences from the result.

---

## 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 PFH

PFH computes features for pairs in the local neighborhood, requiring $O(k^2)$ work per point. FPFH computes SPFH from query-neighbor pairs and combines neighboring SPFHs to obtain an $O(k)$ approximation per point:

$$\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 an early influential system that learns a local volumetric descriptor from correspondences mined from RGB-D reconstructions.

- **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 became a major reference point for learned 3D descriptors, and its accompanying **3DMatch Benchmark** became a widely used evaluation set in subsequent work.

### 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.

Where 3DMatch processes local volumes separately, FCGF computes features over the point cloud with sparse convolutions. The FCGF paper reports a large runtime reduction and strong registration results with 32-dimensional descriptors under its implementation, hardware, and benchmarks.

### 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 simultaneously achieves **learning of geometric invariant features and removal of RANSAC** in 3D point cloud registration.

#### 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

GeoTransformer selects superpoint correspondences and aggregates transformation hypotheses with local-to-global registration, so it can omit a separate external RANSAC stage. The paper reports a large runtime reduction under its implementation and comparison setup; the same ratio or success is not guaranteed on differently contaminated correspondences.

#### Performance

Under the paper's 3DLoMatch protocol, the reported gain over compared methods ranges from 17 to 30 percentage points in inlier ratio and exceeds 7 points in registration recall. These values are specific to that low-overlap benchmark and evaluation setup.

```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)       — early 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**: establish a correspondence between the two sensors — the same physical point for target-based methods, statistical dependence over the overlap for targetless MI-based methods
- **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 (Learned Cross-Domain Descriptors): learns a common embedding space for 2D image patches and 3D point cloud patches.

P2-Net (Wang 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

Four differences make 2D-3D cross-modal matching 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**: the two sensors measure different quantities of the same object. The camera records brightness set jointly by illumination and surface reflectance, while LiDAR measures the light returned at the laser wavelength (intensity). Surface albedo itself is not obtained directly from either.

Because of these difficulties, cross-modal correspondence remains less mature than unimodal (2D-2D or 3D-3D) matching. MI-based methods (Section 5.4) statistically bypass this domain gap, while projection-based methods express both observations in a common 2D image format.

---

## 5.10 Dense Matching & Optical Flow

The methods above cover **sparse correspondence** and some dense matching methods. **Dense matching** seeks a correspondence for every image pixel.

### 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 an early CNN system that predicts an optical-flow field directly with end-to-end supervised learning. Its encoder-decoder 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 combines a **4D correlation volume with iterative GRU updates** and received the ECCV 2020 Best Paper award.

**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/8 \times W/8 \times H/8 \times W/8}$, with every axis at feature-map resolution. This is average-pooled over the last two dimensions to build a 4-level **correlation pyramid** (scales 1, 2, 4, 8).

The important twist is that **multi-scale lookup is performed at a single resolution**, rather than coarse-to-fine.

**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}$

The original configuration uses 12 updates during training and 12–32 at inference. Extra iterations reduced error in some paper experiments, but improvement is not guaranteed after convergence; choose the count from latency and validation error.

**All-Pairs vs. Coarse-to-Fine**: coarse-to-fine methods can struggle to recover a correspondence missed at a coarse level. RAFT constructs all-pairs feature correlations and queries them at several scales during recurrent updates, preserving access to large-displacement candidates. It can still fail when the features or iterative update choose the wrong match.

**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) tokenizes the cost volume and encodes it into cost memory with a transformer. A recurrent transformer decoder queries this representation to iteratively update optical flow. The paper reports higher accuracy than 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**. It formulates all three tasks as 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)

SGM is a traditional dense-stereo method that estimates disparity in three steps:
- 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**: cross-view matching between two views with known poses

### 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
    ↓ global correlation + iterative updates
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 influenced dense matching, **detector-free feature matching** (LoFTR, RoMa), and **learned SLAM** (DROID-SLAM).

---

## Technical Lineage Summary

The diagram below traces how these technical lineages connect:

```
═══════════════════════════════════════════════════════════════════════════════
                    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

═══════════════════════ Shift to Detector-Free Matching ═══════════════════

[Detector-Free Matching]

LoFTR (2021) ──→ QuadTree (2022) ──→ ASpanFormer (2022) ──→ RoMa (2024)
      transformer         O(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

Summary:
  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. Foundation models: fixing DINOv2 features as the basis for matching
     and training the matching logic on top of them.

4. The emergence of 3D-aware matching (2024-2025): DUSt3R → MASt3R →
     VGGT reverse the usual order. They infer 3D structure directly and
     derive correspondences from that structure, rather than matching in
     2D before reconstruction.

Approaches 2, 3, and 4 extend matching beyond a fixed detector-descriptor
  pipeline, while approach 1 remains useful when efficiency and
  interpretability matter.
```

Ch.6 places these matching methods in the frontend of Visual Odometry and connects them to the backend state-estimation methods from Ch.4.

---

# Ch.6 — Visual Odometry & Visual-Inertial Odometry

Visual Odometry (VO) and Visual-Inertial Odometry (VIO) combine the state estimation framework of Ch.4 with the feature matching techniques of Ch.5.

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. The two families differ in internal structure and design choices.

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 combined within a single estimation framework.

Combinations of these axes produce different internal structures and operating characteristics.

---

## 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 has been studied for a long time, so its components and failure modes are comparatively well understood, and public systems such as ORB-SLAM3 continue to use it.

### 6.1.1 Frontend: Detection, Tracking, Outlier Rejection

The frontend of a feature-based VO performs three tasks.

**Feature Detection**

This step finds trackable points in a frame. Ideal feature points must have high repeatability — the projection of the same 3D point should be repeatedly detected in images taken 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 optimized for speed. It declares a candidate pixel $p$ a corner if at least $n$ (typically $n=9$) of the 16 pixels on a radius-3 Bresenham circle are contiguous and all brighter or all darker than $p$. The four-point pre-test that examines only pixels 1, 5, 9, and 13 is a shortcut valid for $n=12$, so it does not carry over unchanged to $n=9$; in that case a learned decision tree determines the order in which pixels are examined. Its speed relative to Harris depends on implementation and hardware, and basic FAST has no orientation or 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.

Outlier rejection with the epipolar constraint uses:

$$\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 selects the model that fits the scene structure (planar vs non-planar) from the ratio of the two models' scores. The reason for using the Homography on a plane is not that it has fewer degrees of freedom: $\mathbf{F}$ has 7 and $\mathbf{H}$ has 8, so $\mathbf{H}$ has more. It is that correspondences confined to a plane make $\mathbf{F}$ degenerate — it is no longer uniquely determined.

### 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 reduction lets BA handle 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 a widely used public reference system 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 uses the IMU preintegration of [Forster et al. (2017)](https://doi.org/10.1109/TRO.2016.2597321) with MAP (Maximum-a-Posteriori) estimation. Visual 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)**

ORB-SLAM3 introduced 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 operates as follows:

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.

After tracking failure, this structure lets ORB-SLAM3 create a new map and merge it with an existing one. It also supports multi-session SLAM that reuses maps from past sessions.

**Scope of the ORB-SLAM3 evaluation**: the paper evaluates its stereo-inertial configuration on EuRoC MAV and TUM-VI and reports trajectory error under each dataset's protocol. A numerical comparison must also name the metric (ATE or RPE), the evaluated sequences and aggregation, and the handling of initialization and tracking failures. For a new camera–IMU rig, remeasure ATE/RPE, failure rate, processing time, and memory on the target sequences.

```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

Direct VO uses photometric error as its residual. 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. It differs from the reprojection error of feature-based methods as follows:

| | 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. **Limited basin of convergence**: gradient-based alignment can enter a different local minimum from a poor initial pose. The tolerable error depends on pyramid level, texture, viewpoint, and photometric change; evaluate it with coarse-to-fine alignment and residual checks rather than a fixed pixel threshold.

### 6.2.2 DSO (Direct Sparse Odometry) Architecture Deep Dive

[DSO (Engel et al., 2018)](https://doi.org/10.1109/TPAMI.2017.2658577) combines a direct method with a sparse representation. Prior to DSO there was an implicit equation of "direct = dense or semi-dense" (DTAM by Newcombe et al. (ICCV 2011) being dense, and [LSD-SLAM, Engel et al., 2014](https://doi.org/10.1007/978-3-319-10605-2_54) semi-dense) and "sparse = indirect" (ORB-SLAM), but DSO recombined the two independent axes of residual formulation and representation density.

**DSO Design**

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, and affine brightness parameters.

**Full Photometric Calibration**

DSO incorporates photometric calibration into its model. In a real camera, the observed intensity $I'$ relates to the irradiance $B$ incident on the image plane 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, preventing features from concentrating in one 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}, \mathbf{c}, a_1, b_1, \ldots, a_n, b_n\}$$

that is, camera poses (SE(3)), inverse depths, camera intrinsic parameters $\mathbf{c}$, 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 an inherent limitation of direct methods but a design choice. LDSO (Loop-closing DSO) addresses this by combining DBoW with direct alignment, and VI-DSO is an extension that couples DSO with an IMU. Basalt, as covered in §6.3.5, is a separate system with a patch-based direct frontend and Non-linear Factor Recovery, not a DSO variant.

```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.

**Three Stages 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. Processing only the neighborhoods of known map points reduces computation compared with processing the whole image.

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.

The SVO paper reports several-hundred-hertz processing under the authors' resolution and hardware setup. The rate depends on camera resolution, map size, CPU, and implementation, so measure it on the target platform. The base system has no loop closure, and pure rotation provides insufficient translation for ordinary depth updates unless handled separately.

---

## 6.3 Tightly-Coupled Visual-Inertial Odometry

VO alone has two structural 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 sign of gravity follows this guide's convention ($\mathbf{g}=[0,0,-9.81]^\top$, Ch.4). The original VINS-Mono paper takes $\mathbf{g}^w$ with the opposite sign, so the sign differs from the equation in the original. 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**

The VINS-Mono paper and public configuration maintain roughly ten frame states in a fixed-size window and apply 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 two strategies retain information in different ways. Marginalizing a keyframe converts the measurements connected to it into a prior via the Schur complement; marginalizing a non-keyframe discards that frame's visual measurements without turning them into a prior, and only the IMU information is carried across through the preintegration between adjacent keyframes. 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 a 4-DoF pose graph (yaw + 3D translation). Under sufficient excitation and a correct gravity and bias model, VIO aligns roll and pitch with gravity while global yaw and position retain gauge freedom. The design therefore leaves local roll and pitch to VIO and applies global correction to yaw and position under those assumptions.

```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:
            # Drop the previous (non-keyframe) frame: discard its visual measurements and chain the IMU via preintegration
            drop_visual_factors(self.states[-2])
            merge_imu_preintegration(self.states[-3], self.states[-2], self.states[-1])
            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.

**OKVIS Design**:

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. **IMU error term**: rather than IMU preintegration, it repropagates the IMU kinematics between two frames to form a 15-dimensional residual over orientation, velocity, position, and biases. "Speed and bias" is the name of the state block $[\mathbf{v}, \mathbf{b}_g, \mathbf{b}_a]$, not of an error term. 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 a representative algorithm for filter-based VIO. It uses a different estimation structure from optimization-based systems (VINS-Mono, ORB-SLAM3), and remains in use in certain applications.

**MSCKF Excludes Landmarks from the State**

EKF-SLAM includes landmarks (3D points) in the state vector. With $L$ landmarks, the state vector has size $3L + 15$ and the covariance matrix has size $(3L+15)^2$, requiring $O(L^2)$ space and $O(L^2)$ operations per update in the number of landmarks. The innovation covariance is a small matrix of the measurement dimension, so no factorization of the full matrix is needed and the cost stops at quadratic. Even so, the growth precludes real-time processing as the number of landmarks increases.

MSCKF **excludes landmarks from the state vector while 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**

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 $L$. EKF-SLAM includes $L$ landmarks in the state so the state size is $O(L)$ and the covariance update requires $O(L^2)$. MSCKF excludes landmarks from the state and therefore depends only on the number of cameras $N \ll L$.

**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 an open-source research platform for MSCKF-based VIO. Its modular design supports comparisons and experiments across several VIO variants.

**OpenVINS Components**:

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**: OpenVINS can, when configured, include camera intrinsics, camera-IMU extrinsics, and temporal offset in the state. These quantities are stable only under sufficiently observable motion and scene geometry; a hardware-synchronized, offline-calibrated system may keep them fixed instead.

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)**: evaluates selected Jacobians at first estimates so that unobservable directions of the linearized system are less likely to collapse spuriously. It is an important consistency device, but it does not correct model error, noise mistuning, or every linearization problem.

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.

**Basalt Components**:

1. **Visual-only Frontend**: patch-wise KLT tracking (inverse-compositional, SE(2) warp) for subpixel-accurate feature tracking. It belongs to the same family as VINS-Mono's point-wise KLT but differs in its patch and warp design.

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 that can be relinearized later.

3. **Efficient Implementation**: Basalt optimizes its implementation by exploiting the sparse structure of the factor graph. Processing-speed comparisons against other systems only hold once sensor configuration, resolution, and hardware are fixed together, and Basalt is stereo+IMU, so it is not an equal-footing comparison against a mono+IMU system.

4. **Stereo/Multi-camera support**: it naturally integrates visual information from multiple cameras.

---

## 6.4 VIO Design Choices

VIO design choices change computational cost, accuracy, and operating conditions.

### 6.4.1 Filter vs Optimization

This is one of the oldest debates in the VIO field.

**Filter-based (MSCKF, OpenVINS)**

- Sequentially updates the maintained state, including some past camera poses, as new measurements arrive
- Information from removed past states is "absorbed" into the maintained 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.

### 6.4.3 Feature Parameterization

How 3D points are parameterized is another 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. OpenVINS uses this representation for its SLAM features. ORB-SLAM3 map points are stored and optimized as 3D positions in the world frame, so they do not fall under this parameterization.

---

## 6.5 Learning-Based VO/VIO

Traditional VO/VIO relies on a "human-designed pipeline." A feature-based example is 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 remaining limitations are:
- 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.

Self-supervised learning is used for monocular depth estimation, but standalone VO/VIO systems still show a performance gap from traditional methods. Accumulated drift also remains unresolved.

### 6.5.3 Hybrid: DROID-SLAM

[DROID-SLAM (Teed & Deng, 2021)](https://arxiv.org/abs/2108.10869) integrates the geometry of BA with learned correspondence updates in a differentiable pipeline. On the benchmarks and metrics selected in the paper, including TartanAir, EuRoC, and TUM-RGBD, it reports large accuracy gains and fewer catastrophic failures than the tested learned and geometric baselines.

**Architecture**

DROID-SLAM has 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**

This layer turns the flow revision output by the GRU into a geometric update. It defines a reprojection error for every pixel and minimizes 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 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**

The following values are those reported under the tables and comparison protocol in the DROID-SLAM paper:

- TartanAir: the DROID-SLAM paper reports a 62% error reduction against its previous compared method
- EuRoC monocular: the same paper reports an 82% reduction under its protocol
- ETH-3D: 30 out of 32 sequences succeed (previous best: 19)
- After training only on synthetic data (TartanAir), it reports lower error than the compared methods on the selected metrics of four real-world datasets

**Geometric Backend and Learned Correspondence**

DROID-SLAM combines differentiable BA and learned correspondence in one recurrent update structure. On evaluations that include repetitive patterns, texture scarcity, and illumination changes, the paper reports lower error and fewer tracking failures than its compared methods.

It has limitations, however:
- **Compute**: the public implementation assumes a GPU, and frame rate depends on resolution, keyframe count, hardware, and configuration.
- **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 work addresses these limits through sparse patches, lower memory use, inertial coupling, and other design choices.

### 6.5.4 Recent Trends (2023-2025)

[DPVO (Teed, Lipson & Deng, 2023)](https://arxiv.org/abs/2208.04726) replaces DROID-SLAM's dense flow with sparse patch-based matching. Under its own comparison setup, the paper reports roughly one-third the memory, about three times the speed, and competitive accuracy. Its architecture combines a patch-wise recurrent update operator with differentiable BA.

[MAC-VO (Qiu et al., 2024)](https://arxiv.org/abs/2409.09479) introduces learned matching uncertainty (metrics-aware covariance) into stereo VO, using it for keypoint selection and residual weighting in pose-graph optimization. The paper reports lower error than its compared methods on public benchmarks that include illumination changes and texture scarcity; it received the ICRA 2025 Best Paper Award on Robot Perception.

---

## Chapter 6 Summary

| System | Type | Estimation | Sensors | 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 | Semi-direct frontend, sparse depth, designed for high-rate processing |
| 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 | Sparse patches; paper reports lower compute and memory cost than DROID-SLAM under its setup |
| MAC-VO | Learned + Opt. | Pose graph opt. | Stereo | Metrics-aware covariance, ICRA 2025 Best Paper on Robot Perception |

LiDAR-based odometry and LiDAR-Inertial systems solve the same ego-motion estimation problem under different sensing conditions. This is also what makes them complementary to camera-based systems.

---

# Ch.7 — LiDAR Odometry & LiDAR-Inertial Odometry

The same ego-motion estimation problem changes shape when LiDAR replaces the camera(+IMU).

LiDAR complements the camera. A camera provides rich texture but is sensitive to visible-light illumination, and monocular geometry alone does not determine absolute range. LiDAR measures 3D range with less sensitivity to visible-light changes, but rain, fog, reflectivity, and multipath can still affect it. LiDAR Odometry (LO) estimates ego-motion from LiDAR alone, and LiDAR-Inertial Odometry (LIO) couples LiDAR with an IMU.

LiDAR odometry uses **point cloud registration** to find the rigid-body transformation $\mathbf{T} \in SE(3)$ between two consecutive scans. The process involves 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 — on a plane, a small tangential displacement is met by re-association at the next iteration, which simply picks another nearby point, so the cost surface is nearly flat along that direction and convergence is slow. With the correspondences held fixed, a tangential shift does raise the cost; the flattening comes from the re-association, not from the cost function.
- 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.

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 whose first column is the normal, i.e., $\mathbf{R}_s \mathbf{e}_1 = \mathbf{n}$). The eigenvectors of $\mathbf{C}$ are the columns of $\mathbf{R}_s$, so putting the normal in the first column is what makes the variance along that direction $\epsilon$.
- 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 places point-to-point and point-to-plane ICP in a covariance-based probabilistic framework. Its paper reports better accuracy and robustness to correspondence errors than those two baselines in the tested data, but practical results still depend on initialization, overlap, sampling, and outlier handling.

```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\max} \sum_i \exp\left(-\tfrac{1}{2}(\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)})\right)$$
   
   Because the Gaussian values are summed without a logarithm, the contribution of a point with a large residual saturates. Replacing this with the sum of log-likelihoods, i.e. minimizing the sum of squared Mahalanobis distances
   $$\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)})$$
   yields a voxel-based Mahalanobis ICP in which the influence of outliers grows without bound — a different algorithm from the original NDT. Practical implementations use an approximation that mixes a uniform distribution into the Gaussian.

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 score function is smooth, and the contribution of distant points saturates, so it is less pulled by outliers.

Disadvantages:
- Sensitive to the choice of voxel size.
- Covariance estimation is unstable in voxels with few points.
- 3D NDT is widely used for prior-map-based localization in autonomous driving, as in Autoware's ndt_scan_matcher. For scan-to-scan odometry it is reported to be slightly less accurate than ICP/GICP, though the outcome varies by dataset.

### 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.

LOAM combines two elements:
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 and floors.

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 surfaces nearly parallel to the laser beam and points on occlusion boundaries because they are unstable. The criterion is the incidence geometry relative to the beam, not horizontality in the world frame: a surface that the beam merely grazes yields an unstable range estimate, and a point on an occlusion boundary disappears under a small change of viewpoint.

**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 closest edge point $\mathbf{a}$ in the previous scan and the closest edge point $\mathbf{b}$ on a scan line adjacent to $\mathbf{a}$'s (two points from the same line make the line degenerate). 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's scan duration is the reciprocal of its rotation rate: at 10 Hz, for example, it is about 100 ms. The platform moves during that interval, so points have different acquisition times. Distortion scales with scan duration and motion and should be corrected using timestamps and a trajectory estimate.

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)  # The real LOAM picks the two points from different scan lines
                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)  # The real LOAM uses two points on the same line + one on an adjacent line
                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.

LeGO-LOAM adds four elements:

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. LeGO-LOAM uses this preprocessing to select non-ground features in ground-vehicle environments.

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. Four properties account for this longevity.

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**: A LiDAR frontend can be combined with an IMU (LIO-SAM) or camera (LVI-SAM). KISS-ICP belongs to a different lineage: it is a deliberately simple LiDAR-only ICP system, not an additional-sensor extension of LOAM.

4. **Robustness**: The edge/planar classification acts as a filter against noise and unstable incidence geometry — points whose curvature pattern is not consistent are not selected. Curvature, however, is a geometric quantity computed from neighbors on a single scan line, so it cannot distinguish motion. The corners and planes of a moving vehicle are selected as good features just the same, so removing dynamic objects requires a separate stage.

That said, the limitations of the LOAM family are equally clear. First, the ring-based feature extraction does not carry over directly to solid-state LiDARs with non-repetitive scan patterns; the direct approach of FAST-LIO2 solves that one by removing feature extraction altogether. Second, odometry accuracy drops sharply in environments where geometric features are scarce, such as open fields and straight tunnels. That is a degeneracy of the environment's observation geometry, so it remains whether one extracts features or works with raw points.

---

## 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**

LIO-SAM models 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.

These factors enter a global pose graph that GTSAM's iSAM2 optimizes incrementally. On top of that, LIO-SAM keeps a separate graph for IMU odometry and resets it periodically, letting it handle state propagation at the IMU rate and bias estimation. 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.

In this bidirectional coupling, 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

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)
        # Random-walk factor constraining the new bias variable. Without it the variable is unconstrained and the iSAM2 update fails
        self.graph.add(gtsam.BetweenFactorConstantBias(
            B(self.key_idx - 1), B(self.key_idx), gtsam.imuBias.ConstantBias(), self.bias_noise))
        
        # 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)) 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.

**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 change in FAST-LIO2 is its 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}$$

Set IEKF iterations from residual reduction or state-increment criteria. FAST-LIO-family configurations often use a small number, but the count depends on initialization and scene geometry. The update uses Gauss-Newton-related iterative linearization while also propagating 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.

**Reported FAST-LIO2 performance**: the paper reports odometry and mapping rates up to 100 Hz under the authors' hardware and data and evaluates multi-line spinning and solid-state LiDARs across several platforms and processors. Remeasure throughput for the current point count, CPU, and map size.

```cpp
// FAST-LIO2 IEKF update pseudocode (C++)
struct State {
    Matrix3d R_GI;    // IMU -> world rotation (prefix notation G_R_I)
    Vector3d p_GI;    // IMU position in world
    Vector3d v_GI;    // IMU velocity in world
    Vector3d bg, ba;  // gyro/accel bias
    Matrix3d R_IL;    // LiDAR -> IMU rotation (prefix notation I_R_L)
    Vector3d p_IL;    // LiDAR origin expressed in the IMU frame
    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 increase processing speed.

Instead of a kd-tree, the method uses a hash-map-based voxel structure. 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.

### 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.

Conventional LIO treats an entire scan (~100 ms) as a single observation. Distortion within the scan is corrected by integrating the IMU to recover the pose at each point's timestamp (§7.3.1), but the update itself is per scan, so the assumption that binds the whole interval into one motion remains. Under fast, high-angular-rate motion that assumption breaks down.

Point-LIO propagates the state in point-timestamp order and performs point-wise updates. Using high-rate IMU measurements and LiDAR timestamps, it estimates the state at each observation time and achieves higher temporal resolution than the single-motion assumption of scan-level deskew. That state remains an estimate subject to IMU noise, bias, and synchronization error.

Point-LIO discretizes the following continuous model over the short interval between IMU measurements:

$$\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: update work grows with the point count. The runtime ratio to FAST-LIO2 reported by Point-LIO is specific to that paper's data, hardware, and configuration, so profile the current implementation.

### 7.3.5 COIN-LIO

[COIN-LIO (Pfreundschuh et al., 2024)](https://arxiv.org/abs/2310.01235) combines **LiDAR return intensity** with a LiDAR-inertial system. It projects LiDAR intensity returns into an intensity image and filters it to improve brightness consistency within and across observations.

The method identifies directions that are weakly constrained by point-cloud registration and selects intensity-image patches that provide complementary information. Their photometric residuals are fused with IMU measurements and point-to-plane residuals in an iterated EKF.

In geometrically degenerate environments such as long tunnels or flat open fields, spatial variation in LiDAR intensity can provide additional constraints. Its benefit depends on the intensity patterns observable along those directions.

---

## 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) = \mathbf{T}_0 \prod_{i=1}^{k} \text{Exp}\left(\tilde{B}_i(t) \cdot \text{Log}(\mathbf{T}_{i-1}^{-1}\mathbf{T}_i)\right)$$

where $\tilde{B}_i(t)$ is a cumulative B-spline basis function. A cubic B-spline is $C^2$ across ordinary interior knots; repeated knots and boundary choices can reduce continuity.

Advantages of a B-spline trajectory:
1. **Query at arbitrary times**: At any time $t$, the pose is obtained by evaluating the trajectory, while velocity and acceleration are obtained by differentiation. This enables natural handling of asynchronous sensor data.
2. **Smooth trajectory**: continuity is controlled by the spline degree. Smoothness alone does not guarantee dynamic feasibility.
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) use non-repetitive scan patterns rather than the repeated scan lines of 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 | Model-dependent (Mid-40 38.4° circular, Avia 70.4° circular, Horizon 81.7°×25.1°) |
| 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. The existing line-based curvature computation therefore does not apply; one must use KNN (K-Nearest Neighbors)-based local curvature or abandon feature extraction and use raw points.

**Why FAST-LIO2 is strong on solid-state**

FAST-LIO2 uses raw points directly and is therefore agnostic to the scan pattern. FAST-LIO still extracts edge/planar features; removing feature extraction is FAST-LIO2's contribution. A solid-state LiDAR gradually fills its FoV more densely over time, and FAST-LIO2's ikd-Tree map naturally accommodates this progressive densification, so map quality improves over time. A narrow FoV means less information per scan, but tight coupling with the IMU compensates for this.

Livox sensors appear frequently in public work on drones, handheld devices, and small robots because of their non-repetitive scan pattern and compact form factor. FAST-LIO2 supports that pattern, so public examples and datasets using the pair are easy to find. Actual selection should compare field of view, range, time synchronization, point distribution, and the target platform in addition to price.

---

## 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 include the following:
- **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): Feeds spherically projected vertex and normal maps to a CNN and trains it unsupervised with a geometry-aware loss in the form of a point-to-plane ICP residual.
- **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 behind classical methods for four reasons:

1. **Characteristics of LiDAR data**: Unlike images, point clouds are unstructured, sparse, and unordered. CNNs cannot process them naturally.

2. **Strong geometric baselines**: ICP, GICP, and NDT work without training when overlap and structure are adequate. They can still fail in repetitive geometry, dynamic scenes, precipitation or dust, and sparse low-overlap scans; learning is therefore used selectively for correspondence, dynamic-point rejection, or uncertainty.

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**: PointNetVLAD, etc.
- **Point cloud registration initialization**: GeoTransformer (see Ch.5)
- **Semantic segmentation**: dynamic-object removal

Scan Context, widely used in the same role, is a hand-crafted descriptor rather than a learned one, so it belongs on the comparison side rather than among the examples of learning.

---

## 7.7 Recent Trends (2023-2024)

LiDAR odometry systems published in 2023-2024 include the following.

**[KISS-ICP (Vizzo et al., 2023)](https://arxiv.org/abs/2209.15397)**: Combines point-to-point ICP with adaptive thresholding, a robust kernel, and motion compensation. The paper reports competitive accuracy with one common configuration across automotive, UAV, and handheld datasets, emphasizing reduced dataset-specific parameter tuning.

**[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 focuses on the data-matching strategy 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)**: Integrates incremental GICP into a tightly coupled LIO. A voxel-based surface covariance estimator (VSCE) reduces the cost of GICP covariance computation, and an incremental voxel map reduces nearest-neighbor search cost. Its paper and public implementation compare computation and accuracy with Faster-LIO on the selected benchmarks.

---

## Chapter 7 Summary

| System | Approach | Estimation | Sensors | Feature |
|--------|------|-----------|------|-----------|
| ICP/GICP/NDT | Registration | Iterative optimization | LiDAR only | Basic 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, paper reports up to 100 Hz |
| Point-LIO | Direct | Point-wise EKF | LiDAR + IMU | Per-point update, fast motion |
| COIN-LIO | Direct + intensity | IEKF | LiDAR + IMU | LiDAR intensity complements weak geometry |
| 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 LOAM (2014) → LeGO-LOAM (2018) → LIO-SAM (2020) lineage illustrates **feature-based + factor graph** design. FAST-LIO (2021) → FAST-LIO2 (2022) → Point-LIO (2023) illustrates **direct + Kalman filter** design. Relative accuracy and runtime depend on the sensor pattern, motion, map scale, hardware, and benchmark protocol.

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. Multi-sensor fusion architectures 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**.
> Combining Visual Odometry and LiDAR Odometry in one system requires choices about coupling level and estimator structure.

---

## 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 determines the system's complexity, performance, and failure modes.

### 8.1.1 Loosely Coupled

Each sensor is viewed as an independent "expert." Each expert produces an independent estimate (pose, velocity, etc.) from its own data, and a higher-level stage then combines the results.

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 fusion occurs only after each subsystem has already compressed its measurements into an output estimate, 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

All sensors' **raw measurements** directly constrain a single shared state. Instead of running "experts" and merging their outputs, the raw data enters the state estimate itself. What defines the level is where the measurements enter, not whether there is only one optimizer. A design in which two subsystems update one shared state (R3LIVE) or one that applies sequential updates (FAST-LIVO2) is tightly coupled as well. Even when two subsystems keep separate graphs, as in LVI-SAM, the design is classified at the same level if they exchange measurements (LiDAR depth) and state (initial guesses) directly with each other.

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 (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 (Signal-Level Coupling)

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 tightly coupled GNSS-INS system combines receiver-derived pseudoranges with the inertial navigation solution, 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.

**Implementation boundary**: ultra-tight coupling requires access to correlators or tracking loops and cannot be built from a generic measurement API alone. It appears in integrated GNSS/INS products and research platforms with receiver SDK, FPGA, or firmware access; ordinary robotics projects more readily use tightly coupled raw pseudorange, carrier, and Doppler measurements.

### 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 ──┐
                                      ├→ Shared-state estimator → Final pose
             Sensor B → raw meas. B ──┘   (a single optimizer, sequential updates,
                                           and two subsystems sharing one state
                                           all belong to this level)

[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 precision differs per axis (less precise along x)

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

Camera, LiDAR, and IMU together provide texture and color, 3D range, and high-rate inertia. Cost, power, weather, range, and correlated failures still matter; installing all three does not by itself guarantee robustness:

| Situation | Camera | LiDAR | IMU |
|------|--------|-------|-----|
| Dark environment | ✗ | ✓ | ✓ |
| Textureless wall | ✗ | ✓ | ✓ |
| Geometric degeneracy (long corridor) | ✓ | ✗ | ✓ |
| High-speed rotation | ✗ | ✗ | ✓ |
| Scale observability | ✗ (monocular) | ✓ | ✓ (acceleration is in metric units; requires motion excitation) |
| Color/semantics | ✓ | ✗ | ✗ |

The following systems integrate these three sensors.

### 8.2.1 R3LIVE / R3LIVE++

R3LIVE (Lin et al., 2022) is a system that tightly couples two subsystems: LiDAR-Inertial Odometry (LIO) and Visual-Inertial Odometry (VIO).

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)
```

The LIO subsystem directly registers raw LiDAR points to an ikd-Tree-based map with point-to-plane matching, as FAST-LIO2 does. An iterated EKF updates the state.

The VIO subsystem distinguishes R3LIVE. A typical VIO minimizes the reprojection error of feature points, whereas R3LIVE uses the **photometric error**. Each point in the 3D map built by LIO is assigned an RGB color. 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.

R3LIVE's shared state can continue receiving valid updates when one modality temporarily contributes fewer usable residuals. The duration of visual updates during LiDAR blockage depends on visibility of the existing colored map; LIO during dark imagery depends on LiDAR geometry and IMU quality. The paper demonstrates online colored 3D mapping with this structure.

### 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**.

**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**: assign LiDAR depth to visual features with valid spatial and temporal correspondence. With correct extrinsics, synchronization, visibility, and occlusion handling, this provides a metric-depth prior without waiting for triangulation parallax.

```
         ┌─── VIS initial pose ───→ LIS initial guess
         │                            │
  [Visual-Inertial]            [LiDAR-Inertial]
         │                            │
         └←── LiDAR depth ────────────┘

↓ LiDAR·IMU factors ↓
           [Factor Graph (GTSAM/iSAM2)]
                     ↓
              Final optimized pose
```

**Factor graph design**: LVI-SAM consists of two subsystems, a visual-inertial system (VIS) and a LiDAR-inertial system (LIS), and the factor graph is maintained by the LIS, which inherits it from LIO-SAM. The factors it holds are:
- IMU preintegration factor (between successive keyframes)
- LiDAR odometry factor (scan matching result)
- GPS factor (when available)
- Loop closure factor (upon revisit detection)

Rather than inserting its own odometry into this graph as a factor, the VIS couples in by supplying the LIS with an initial guess and by taking LiDAR depth to fix its own scale.

### 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.

**Design 1 — Sequential Update**:

Measurements from heterogeneous sensors differ greatly in number and in nature. LiDAR provides one point-to-plane residual per point, while the camera provides one photometric residual per patch pixel; the two measurement sets differ in size, units, and noise magnitude, and the original paper cites this dimension mismatch as the reason for choosing sequential updates.

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.

**Design 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.

**Design 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 illustrates LiDAR-camera complementarity.

**Design 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 the dimension
    mismatch of the heterogeneous measurements need not be handled at once.

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 (Shi-Tomasi + KLT) | Direct (photometric) |
| Map representation | ikd-Tree + RGB | Voxel map | Hash+Octree voxel map |
| Feature extraction | Not required | Required (LiDAR edge/planar, visual corners) | Not required |
| GPS integration | None | Integrated as factor | None |
| Loop closure | None | Integrated as factor | None |
| Embedded validation | Benchmark on target hardware | Benchmark on target hardware | ARM implementations reported; revalidate under the target setup |

**Criteria for narrowing the candidates**:
- Evaluate LVI-SAM when integrated loop closure and GPS factors are required. Also check the sensor configuration, ROS dependencies, and revisit performance on the target data.
- Evaluate R3LIVE when direct photometric updates and colored mapping are required. Compare color, trajectory, and map-quality metrics in the target scenes.
- Evaluate FAST-LIVO2 as an ARM deployment candidate, but measure throughput, worst-case latency, and memory at the target sensor resolution and configuration.
- In environments with scarce visual or LOAM features, include direct methods such as R3LIVE and FAST-LIVO2 as candidates, then validate photometric calibration and sensitivity to dynamic objects.

---

## 8.3 GNSS Integration

GNSS (Global Navigation Satellite System) provides an absolute position reference in a global coordinate frame. The IMU, LiDAR odometry, and visual odometry discussed here primarily provide **relative** motion, so drift accumulates over long-duration operation. GNSS can serve as an anchor that corrects this drift. Surveyed landmarks, UWB beacons, and motion-capture systems can also provide absolute references, so GNSS is not the only possible global sensor.

### 8.3.1 GNSS Factor in the Factor Graph (LIO-SAM Approach)

[LIO-SAM](https://arxiv.org/abs/2007.00258) (Shan et al., 2020) connects each GNSS position report to a factor-graph pose node as a **unary factor**:

$$
\mathbf{r}^{\text{GPS}}_i = \mathbf{T}_{\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}}
$$

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. The equation above brings the GNSS position into the map frame for comparison, so this transformation is applied as is; the $\mathbf{T}^{-1}$ form is what one writes when using the opposite direction, $\mathbf{T}_{\text{map→ENU}}$
- $\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) — put squared standard deviations
            on the diagonal. Horizontally, sigma_h = HDOP * sigma_uere; vertically,
            use VDOP. That is, diag(sigma_E**2, sigma_N**2, sigma_U**2).
    """
    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 and 4D Imaging

Traditionally, automotive radar was considered unsuitable for SLAM/odometry because of its low resolution. However, the emergence of **4D imaging radar** is 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. Its point count is lower than that of LiDAR (hundreds to thousands of points vs. hundreds of thousands).

Radar provides the following properties:

1. **Adverse-weather tolerance**: mm-wave radar is often less affected by fog, rain, and snow than visible cameras or some LiDARs. Heavy precipitation, wet-road multipath, attenuation, and clutter remain, so performance must be tested by condition.

2. **Direct radial-velocity measurement**: FMCW Doppler gives the line-of-sight component of relative velocity from the phase change across the several chirps within one frame. A single chirp alone yields only range information, with velocity not yet separated out. It does not directly provide a point's full 3D velocity or an object's motion; multiple directions, time, tracking, or another sensor are needed.

3. **Different cost structure**: mass-market radar chipsets can be inexpensive, but imaging-radar and LiDAR module prices overlap depending on channels, antennas, compute, and production volume. Compare current quotes against required performance rather than assuming a fixed ratio.

### 8.4.2 Radar Odometry

The number of papers on odometry using 4D radar has grown rapidly since 2022. These methods directly use radar Doppler measurements 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}_S + n_i
$$

Here $\mathbf{v}_S$ is the linear velocity of the radar phase center. If static returns span enough directions, the equations constrain $\mathbf{v}_S$. Point Doppler from a single monostatic radar does not independently determine angular velocity about the sensor origin; transform to a body origin with a known lever arm and angular rate from an IMU or scan registration.

```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

Combining 4D radar with cameras can produce a "LiDAR-free" autonomous-driving system. The two sensors complement each other as follows:

| Property | Camera | 4D Radar |
|------|--------|----------|
| Resolution | Very high | Low |
| Adverse weather | Weak | Robust |
| Direct depth measurement | ✗ | ✓ |
| Radial-velocity measurement | ✗ | ✓ |
| Semantic understanding | Strong | Weak |
| Cost | Depends on lenses, camera count, and compute | Depends on antennas, channels, and compute |

Fusion uses three 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) repeats multi-sensor routes across clear, rainy, snowy, and seasonal conditions. It synchronizes cameras, a Velodyne Alpha Prime LiDAR, and a Navtech CIR304-H **planar scanning radar**, among other sensors. That Navtech unit is not an automotive 4D imaging radar with elevation, so Boreas radar-odometry results should not be presented as 4D-radar performance.

---

## 8.5 Multi-Robot / Decentralized Fusion

For **multiple robots to perceive the environment cooperatively**, they must address communication constraints, the absence of a common reference frame, and data association.

### 8.5.1 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. The system must therefore choose **what information to compress and share**.

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) (Tian 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. When a loop is detected, the inter-robot factor is shared; from then on a distributed optimizer such as Riemannian block-coordinate descent converges by exchanging, at every iteration, the current estimates of the separator poses it shares with its neighbors. Sharing a constraint once and repeatedly exchanging optimization variables are two different kinds of communication.

### 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.

**Swarm-SLAM 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 inter-robot factors and separator pose estimates with neighbors.
    """
    def __init__(self, robot_id):
        self.robot_id = robot_id
        self.local_poses = [np.eye(4)]  # Own poses (local frame); seeded with the identity pose at the origin
        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

Designing and deploying a real multi-sensor fusion system also introduces problems beyond theory and algorithms.

### 8.6.1 Sensor Suite Selection Guide

The following table is a starting point. Final selection depends on the operational environment, safety requirements, range, power and mass, and failure scenarios:

| 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 |

**Costing sequence**: sensor prices vary by date, region, and volume. First specify the required measurement, accuracy, rate, and FoV. Then price the complete system: sensors, cables, synchronization, GNSS corrections, compute, mounts, spares, and integration time. A low-cost prototype may begin with camera and IMU; add 2D or 3D LiDAR, RTK GNSS, radar, or redundancy only for failure modes and error budgets established by testing.

### 8.6.2 Timing Architecture (Time Synchronization Design)

In a multi-sensor system, **time synchronization** directly affects 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**:

A shared clock or trigger can make the latency path clearer than software receive timestamps. Pulse accuracy and measurement-timestamp accuracy are not identical, so validate the path end to end:

- **PPS (Pulse Per Second)**: use a GNSS receiver's second-boundary pulse as a common clock reference. Check the receiver time-pulse specification, cable and input delay, and timestamp reference point.
- **PTP (Precision Time Protocol, IEEE 1588)**: synchronize Ethernet clocks. Actual error depends on hardware timestamps, switches, PTP profile, path asymmetry, and lock state.
- **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**: Estimate the time delay from the cross-correlation between two signals in which the sensors observed the same motion. Using acceleration requires removing gravity from the IMU measurement and bringing both signals into the same frame, and with a monocular camera the scale of the velocity is undetermined. Practice therefore mostly compares gyro angular rate against the camera's rotation rate, which needs none of those three conditions.

$$
\hat{\tau} = \arg\max_{\tau} \int \boldsymbol{\omega}_{\text{IMU}}(t) \cdot \boldsymbol{\omega}_{\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**: when one sensor fails, it must continue operating with the remaining sensors, even at reduced performance.

**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 | Stop adding new factors. If the failure time is uncertain, down-weight or remove only the interval after the suspect point |

**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.4 Recent Systems and 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.5 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)
- [ ] Procedure in place for monitoring and responding to calibration changes (online estimation or periodic recalibration). Online extrinsic estimation requires sufficient motion excitation for observability, so it is not a mandatory condition for every system

**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. Three-sensor fusion with camera, LiDAR, and IMU is implemented in systems such as R3LIVE, LVI-SAM, and FAST-LIVO2, which use a dual subsystem, factor graph, and sequential update, respectively.

GNSS integration constrains drift with a global-coordinate observation, while 4D radar can add weather tolerance and radial-velocity measurements. Multi-robot systems such as Kimera-Multi and Swarm-SLAM combine distributed estimation with cross-robot place recognition under communication constraints.

In practical system design, sensor selection, time synchronization, and failure-mode handling matter as much as the algorithms. Engineering decisions in these areas affect deployment results.

The odometry/fusion systems covered in Ch.6-8 are highly accurate locally, but drift accumulates over long-duration operation. Correcting that drift through loop closure requires the ability to recognize previously visited places: **Place Recognition**.

---

# Ch.9 — Place Recognition & Retrieval

Odometry and fusion systems accumulate drift over time. Place Recognition finds previously visited locations and supplies candidates for correcting that drift through the Loop Closure pipeline in Ch.10.

> The problem of judging, "Have I seen this place before?"
> Place recognition supplies candidates for loop closure and supports 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** determines whether the robot has revisited a place within a SLAM system. Place recognition supplies candidates, after which the loop closure pipeline performs **geometric verification**.

```
Loop Closure Detection Pipeline:
┌─────────────┐    ┌────────────────┐    ┌───────────────────┐    ┌─────────────┐
│  Current    │───→│ Place          │───→│ Geometric         │───→│ Pose Graph  │
│  observation│    │ Recognition    │    │ Verification      │    │ Update      │
│  (query)    │    │ (candidate     │    │ (geometric        │    │ (optimize)  │
│             │    │  retrieval)    │    │  verification)    │    │             │
└─────────────┘    └────────────────┘    └───────────────────┘    └─────────────┘
```

Without loop closure, drift accumulates in SLAM. Comparing one query with all $N$ past frames costs $O(N)$; all-pairs comparison over a sequence costs $O(N^2)$. Global descriptors with an exact scan remain $O(N)$, while inverted, tree, or approximate-nearest-neighbor indexes can reduce average query cost under data-distribution and recall tradeoffs.

### 9.1.2 Retrieval Pipeline

Place recognition uses the same pipeline as information retrieval:

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 metric is widely used.

$$
\text{Recall@N} = \frac{|\{q : \text{top-}N \text{ candidates contain the correct match for query } q\}|}{|\text{total queries}|}
$$

**Recall@1** measures whether the first result succeeds. A SLAM system may geometrically verify top-$k$ candidates according to latency and false-positive cost, so Recall@$k$, precision-recall, and query latency should also be reported.

**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"**: the dataset protocol may use position distance, field-of-view overlap, or pose difference. Some VPR benchmarks use a GPS radius, but 25 m is not a universal rule.

```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/)** applied the methodology of text retrieval directly 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**: A BoW implementation used in the ORB-SLAM series. Its hierarchical k-means tree supports efficient search over a large vocabulary, and it became one of the widely used implementations for loop-candidate retrieval in feature-based 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_words = {}  # img_id -> word_ids. TF-IDF vectors are recomputed at query time with the current IDF
    
    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)."""
        return self.bow_from_words(self.quantize(descriptors))
    
    def bow_from_words(self, word_ids):
        """Compute the TF-IDF vector from a list of quantized words using the current IDF."""
        
        # 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."""
        word_ids = self.quantize(descriptors)
        self.db_words[img_id] = word_ids   # store only the words, not the vector
        self.N += 1
        
        # Update inverted index
        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)
        
        # Narrow candidates with the inverted index: only images sharing at least one query word
        candidates = set()
        for w in np.unique(self.quantize(descriptors)):
            candidates.update(self.inverted_index[w])
        
        scores = {}
        for img_id in candidates:
            db_vector = self.bow_from_words(self.db_words[img_id])   # computed with the current IDF
            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. Which of the two retrieves better depends on the metric, the data, and the normalization used; VLAD can be read as a simplification of the Fisher Vector, and at equal dimensionality some reports find the two comparable or VLAD ahead.

### 9.2.3 NetVLAD: The Baseline for Learning-Based VPR

**[NetVLAD (Arandjelović et al., 2016)](https://arxiv.org/abs/1511.07247)** reformulated VLAD as a **differentiable CNN layer**, enabling end-to-end training.

In classical VLAD, hard-assigning each descriptor to its nearest cluster is non-differentiable. Backpropagation therefore requires a differentiable assignment.

**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. The supervision is weak because a nearby GPS coordinate does not guarantee that the same scene is visible. Geographically close images are therefore treated as potential positives, and the single best-matching one in feature space is chosen as the positive, while negatives are mined as hard negatives from geographically distant images. 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**: Recall@1 is approximately 84.3% on Pitts250k and 86.3% on Pitts30k, improving on the hand-crafted methods used for comparison at the time. The model uses a VGG-16 backbone.

### 9.2.4 AnyLoc: Foundation-Model-Based Universal VPR

**[AnyLoc (Keetha et al., 2023)](https://arxiv.org/abs/2308.00688)** examines whether place recognition can work across domains without VPR-specific training. The paper demonstrates this possibility using features from a foundation model such as DINOv2.

1. **DINOv2 feature extraction**: Extract **dense features** from a middle layer of DINOv2 ViT-G/14. Use the features of all patches, not the CLS token, because patch features retain local information that a single CLS token can discard. AnyLoc reports an average gain under its compared datasets and metrics; the layer and magnitude depend on backbone and domain.

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**: the paper analyzes PCA groupings for Urban, Indoor, Aerial, SubT, Degraded, and Underwater data and uses domain-specific vocabularies. Its maximum reported gain belongs to that evaluation; vocabulary choice must be validated for a new domain.

**Why are foundation-model features useful for VPR?**

[DINOv2 (Oquab et al., 2023)](https://arxiv.org/abs/2304.07193) was self-supervised on 142 million images. Features without place-recognition-label fine-tuning proved useful on several benchmarks, but do not guarantee discrimination in repetitive geometry, extreme viewpoint changes, or unseen sensor domains.

**Relative comparisons reported under the benchmarks and protocols in the AnyLoc paper**:
- Day-night changes: cases with Recall@1 5–21 percentage points above the tested MixVPR- and CosPlace-family baselines
- Seasonal changes: cases 8–9 percentage points higher
- Opposite viewpoint (180 degrees): cases 39–49 percentage points higher
- Selected unstructured environments, including underwater and subterranean data: up to about 4x the baseline
- The paper also evaluates PCA whitening from 49K to 512 dimensions; the recall change should be checked per dataset

```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]
        # These are the output tokens of the last block. Using the value facet
        # of a specific layer — AnyLoc's key finding — requires hooking the
        # attention module.
        image_patch_features.append(patch_tokens)
        all_patch_features.append(patch_tokens)
    
    # Step 2: Build k-means visual vocabulary
    all_features = np.concatenate([p.reshape(-1, p.shape[-1]) for p in all_patch_features], axis=0)  # (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
        patches = patches.reshape(-1, patches.shape[-1])   # (1, N, D) → (N, D)
        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), and it learns descriptors that are robust to viewpoint change. Building on CosPlace — which replaced the cumbersome hard-negative mining of triplet loss with classification-based training — EigenPlaces goes further and applies PCA to the distribution of camera viewpoints within each class to find the dominant direction of the scene, then composes the training views around a focal point along that direction. PCA is applied to the viewpoint geometry of the training data, not to the descriptor feature space.

### 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." In AnyLoc's comparison of foundation models, DINOv2 features gave better place discrimination under the authors' own layer, facet, and aggregation settings and on their datasets.
- **SAM (Segment Anything)**: Research is underway on using segmentation masks as a structural representation of places.
- **DINOv2 + learned aggregation**: A line of follow-up work replaces AnyLoc's unsupervised VLAD by learning the aggregation on top of DINOv2 features. SALAD (§9.7.4) recasts VLAD's assignment as optimal transport, and SelaVPR (Lu et al., ICLR 2024) adapts the pre-trained model to VPR.

### 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.

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).

The method has three steps:
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.

The sequence pattern is preserved under appearance changes such as day → night and summer → winter. SeqNet (Garg et al., 2021) develops this sequence matching into 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 used as the loop closure module in several LiDAR SLAM systems. Note, however, that the public LIO-SAM implementation defaults to radius search with ICP-based loop detection; Scan Context is provided through a separate integration repository such as SC-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, a revisit from the same sensor position that differs only in yaw leaves the descriptor a **circular shift** of the rows (the sector axis). This is handled via row-shift matching. A real reverse-direction revisit also brings lateral displacement and changes in occlusion, so a shift alone does not account for it — which is why the original paper adds root shifting augmentation.

**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**: A rotation-variant key that aggregates the ring axis per sector. Comparing the sector keys of two scans gives an initial estimate of the best shift, and the distance below is computed only for the few shifts around it (the equation and code below search all shifts for simplicity):

$$
d(\mathbf{SC}_q, \mathbf{SC}_d) = \min_{s \in [0, N_s)} \frac{1}{N_s}\sum_{i=1}^{N_s}\left(1 - \frac{\mathbf{c}^q_i \cdot \mathbf{c}^d_{i+s}}{\|\mathbf{c}^q_i\|\,\|\mathbf{c}^d_{i+s}\|}\right)
$$

```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 row-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)
                # Mean of per-sector (row) cosine distances — the distance defined in the original paper
                num = np.sum(sc_query * sc_shifted, axis=1)
                den = np.linalg.norm(sc_query, axis=1) * np.linalg.norm(sc_shifted, axis=1) + 1e-9
                dist = np.mean(1.0 - num / den)
                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++](https://arxiv.org/abs/2109.13494)**: Extends structural descriptors to improve robustness to heading rotations and lateral displacement when roll and pitch changes are limited. Place retrieval is followed by a 1-DOF relative alignment estimate.

### 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 an early influential system that combines PointNet and NetVLAD for end-to-end point-cloud place retrieval. A PointNet backbone extracts features, a NetVLAD layer aggregates them into a global descriptor, and lazy triplet and quadruplet losses train the model.

$$
\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://arxiv.org/abs/2011.04530)** ([code](https://github.com/jac99/MinkLoc3D)) addresses the limitations of PointNetVLAD by using a **Minkowski Convolutional Neural Network** as its backbone. Sparse 3D convolutions 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.

The architecture first extracts local features from the range image with a lightweight CNN. A Transformer encoder incorporates context among these features, and a NetVLAD layer aggregates them into a global descriptor. This design uses 2D range images and a lightweight network for fast processing.

**Related method — [OverlapNet](https://github.com/PRBonn/OverlapNet)**: Uses range-image representations of LiDAR scans to predict their overlap and relative yaw.

### 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:

- **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 Domain Gap in Cross-Modal PR

LiDAR point clouds and camera images use different data 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 | Point spacing grows with range (density per unit surface area falls with the square of range and also depends on incidence angle) | Uniform in the image plane |

This difference is called the **domain gap** and causes descriptors of the same place observed in different modalities to lie 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. Project LiDAR point clouds into range images and convert camera images into a disparity/depth representation, bringing both modalities into the same range-image domain
2. Process the two representations with their respective encoders
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** (Xie et al., IROS 2024) is a framework for efficiently solving cross-modal place recognition that retrieves point clouds from an image. Its target modalities are camera and LiDAR; combinations that also include radar are out of its scope.

### 9.4.5 Modality-Agnostic Descriptor Approach

A **modality-agnostic descriptor** aims to produce the same descriptor for a place regardless of which sensor observes it. Approaches include the following:

- **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 appearance of the same place can change substantially over time. Illumination (day vs. night), seasons (green trees vs. snow-covered scenery), weather (clear vs. fog), and structural changes (new construction or demolition, road work) all shift the descriptor of the same place and reduce PR performance.

### 9.5.2 Strategies for Seasonal/Time-of-Day/Weather Changes

Four approaches are used in combination.

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 relatively robust to illumination changes. Under conditions where the segmentation itself degrades, however — night or strong backlighting — the label map degrades with it.

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

Place recognition for a robot operating over its lifetime remains an open research problem. Information must be compressed and managed so that the map does not grow without bound. The system must adapt continuously to environmental change, gradually forget old observations, and learn new environments without catastrophic forgetting. This problem is closely related to continual and 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)**: Lift the image-to-image matches from step 1 onto the 3D map points associated with the candidate image to form 2D-3D correspondences, and estimate the camera pose from them. When the 3D points live in the map frame, the result is the absolute pose of the query camera, so the pose relative to the candidate is obtained by composing it with the candidate's known pose
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
    
    # solvePnP's (rvec, tvec) is the transform from the 3D-point frame to the camera.
    # If the 3D points are in the map frame, T is the map→query-camera transform, and the relative
    # pose to the candidate must be obtained by composing it with the candidate's 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) encodes pairwise distances and triplet angles and uses local-to-global registration, omitting a separate external RANSAC stage. The paper reports low-overlap gains and a large runtime reduction in comparisons including 3DLoMatch; its 100× figure is specific to the implementation, hardware, and compared stage.

```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. The criterion is not the raw size of the discrepancy but its Mahalanobis distance under the odometry covariance. Since the purpose of loop closure is to correct accumulated drift, a correct detection on a long loop also shows a large discrepancy; rejecting on magnitude alone discards the most useful loops.

---

## 9.7 Recent Developments

### 9.7.1 The Rise of Foundation-Model-Based PR

VPR research includes both environment-specific training and foundation-model-based zero-shot recognition. AnyLoc reports competitive results without VPR-specific training across several environments selected in its paper.

Two directions are under study. ViT-G/14 has more than 1B parameters, which limits embedded deployment, so researchers are combining lightweight FMs (ViT-S/B) with domain adaptation. They are also using the patch-level dense correspondences provided by FMs directly 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)

The system can compare layouts from semantic segmentation, construct a scene graph with object detection, or retrieve places from natural-language descriptions with CLIP. Semantic structure is robust to illumination and seasonal changes: even if the color of a tree's leaves changes, the label "tree" remains. The method still depends on semantic-segmentation accuracy and has difficulty distinguishing places with similar semantic structures, such as residential areas with similar layouts.

### 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

Radar PR can serve as a backup when LiDAR PR and visual PR fail in rain, snow, or fog. It is still at an early stage, and its low resolution makes places harder to distinguish than with LiDAR or visual PR. The resolution of 4D radar continues to improve.

### 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 paper reports higher recall than NetVLAD and CosPlace on its selected VPR benchmarks using Sinkhorn-based soft assignment.

**[EffoVPR (Taha et al., 2024)](https://arxiv.org/abs/2405.18065)**: A framework for efficiently using features from foundation models such as DINOv2. Its evaluation includes 128-dimensional descriptors and reports the tradeoff between accuracy and storage or retrieval cost.

### 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 supplies loop-closure candidates that can constrain drift. Visual PR spans BoW, VLAD, NetVLAD, and systems such as AnyLoc that reuse foundation-model features without task-specific labels. LiDAR PR spans Scan Context, PointNetVLAD, MinkLoc3D, and OverlapTransformer; range-image methods offer a different efficiency–invariance tradeoff.

Cross-modal PR faces a domain gap, and shared-embedding-space learning and modality-agnostic descriptors are active research directions. Long-term PR must cope with seasonal, illumination, and structural changes; researchers are applying Foundation Model features to this problem.

Geometric verification tests whether a retrieved candidate satisfies geometric constraints and reduces false-positive risk. Visual systems use PnP or essential geometry with robust estimation; LiDAR systems use ICP-family or learned registration. None eliminates bad loop closures under every ambiguity.

Recent trends include lightweight 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 a SLAM system still has to convert that answer into global consistency. **Loop Closure and global optimization** integrate PR results into the pose graph and correct drift.

---

# Ch.10 — Loop Closure & Global Optimization

In Ch.9 we covered place recognition — recognizing previously visited locations. Those recognition results must enter the SLAM system before they can correct 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.

The loop closure pipeline runs detection → verification → correction, and pose graph optimization sits at the center of the correction step. The same structure extends 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). Missing a candidate loses an opportunity for correction, while accepting a false loop 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**: global descriptors such as [NetVLAD](https://arxiv.org/abs/1511.07247) and [AnyLoc](https://arxiv.org/abs/2308.00688) are used. AnyLoc aggregates [DINOv2](https://arxiv.org/abs/2304.07193) dense features with VLAD and works across several domains without place-specific labels, but recall must be validated for a new sensor and environment. 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 — a ring-key-based kd-tree lookup narrows the candidates, then sector-shift alignment re-ranks them — and it is robust even to reverse revisits.
- **[PointNetVLAD](https://arxiv.org/abs/1804.03492), [OverlapTransformer](https://arxiv.org/abs/2203.03397)**: learned 3D place-recognition methods. Their recall relative to Scan Context depends on dataset, sensor pattern, and training domain.

**Temporal filtering**: matching recent frames is usually continuous tracking, so adjacent keyframes are excluded. Tune the time or travel-distance gap to platform speed, keyframe rate, and revisit pattern. The 30-second value below is illustrative.

```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**: match features and estimate $\mathbf{E}$ with RANSAC. Check inlier count and ratio together with spatial coverage, pose plausibility, and repeated detection. The 20-inlier and 50% settings below are examples to tune for the detector and scene.

$$\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**: Solve a PnP problem between the current 2D feature points and the 3D map points held by the candidate keyframe. If the 3D points are expressed in the candidate keyframe's frame, the solution is directly the relative transform $\mathbf{T}_{ij}$ the loop edge needs; if they are expressed in the map frame, the solution is the absolute pose of the query frame and must be composed with the candidate's pose.

4. **Temporal consistency**: Rather than a single match, check whether matches against the same place appear consistently across several consecutive frames. ORB-SLAM2 accepted a loop closure only when the same place was detected three times in a row. ORB-SLAM3 replaces that temporal condition with geometric verification over a local window: it accepts the loop when the transformation is supported by the query keyframe plus two keyframes covisible with it, three in total. Because those three are usually already in the map, there is no need to wait for new keyframes — which lets it close loops earlier.

```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.
                    The t recovered from an essential matrix is a unit
                    direction with no determined magnitude, so using this
                    value as a pose graph loop edge requires obtaining the
                    metric scale separately from stereo, depth, or map points.
    """
    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 8-point linear solver + RANSAC.

Nister's 5-point algorithm, which solves from a minimal sample of 5,
    is a separate solver; its minimal sample size differs, so the number of
    RANSAC iterations it requires differs as well.
    """
    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 linear solver (a separate solver from 5-point, not a simplification of it)
        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

The corridor example below shows why a false positive loop closure is 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. What gets adjusted is the pose variables, not the edges — the edges are measurements — and the result is a redistribution of each edge's residual. That redistribution is not uniform: it is set by each edge's information matrix $\boldsymbol{\Omega}_{ij}$ and by the graph topology, so odometry stretches with large information absorb less of the correction.

---

## 10.2 Pose Graph Optimization

Pose graph optimization improves 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 six-dimensional vector encodes three rotational and three translational error components.

**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. With relative constraints alone, however, the objective is invariant to a global SE(3) transform, so $\mathbf{H}$ has a six-dimensional null space. Fixing one reference pose or adding a prior factor is what makes it positive definite and the decomposition valid.
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 [translation; rotation]."""
    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 reduces sensitivity 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 continuous 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) implements this incremental optimization.

When a new variable or measurement is added, iSAM2 identifies the range over which its influence propagates and recomputes only that part.

**Bayes tree**: iSAM2 uses this data structure. Variable elimination on a factor graph yields a clique tree, and the Bayes tree is this clique tree endowed with direction.

The posterior distribution used for 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**: selectively relinearize variables whose estimate has moved beyond a threshold. Unlike iSAM v1, this does not require a scheduled full-batch relinearization, although a large loop closure can affect a broad part of the Bayes tree.

**Variable reordering**: When a new variable is added, the full elimination order is not recomputed; only the affected part is reordered incrementally.

GTSAM provides iSAM2, and [LIO-SAM](https://arxiv.org/abs/2007.00258) uses it in the backend. ORB-SLAM3 instead performs nonlinear optimization with a modified version of g2o. By updating the affected part of the Bayes tree, iSAM2 avoids solving the full batch problem after every new factor.

```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 pre-built map?"

### 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**: Extract feature points from the current image, find 2D-3D correspondences with the map's 3D points via visual words or direct matching, estimate the pose with PnP + RANSAC, and resume tracking from that pose.

ORB-SLAM3 retrieves candidate keyframes with DBoW2, obtains 2D-3D correspondences via ORB matching, estimates the pose with MLPnP + RANSAC (EPnP was the method up through ORB-SLAM2; ORB-SLAM3 switched to MLPnP, which is independent of the camera model), and then finds additional matches through guided search to improve accuracy.

**LiDAR relocalization**: Register the current LiDAR scan to a prior point-cloud map. First search for nearby regions with a global descriptor such as Scan Context or PointNetVLAD, then perform coarse registration with FPFH + RANSAC or GeoTransformer, and finally refine the 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. Three challenges follow:

- **Discrepancies between the map and the current environment**: Over time, buildings change 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 against the entire map can be used to find the starting location.

### 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.

After initialization, MCL repeats prediction, update, and resampling.

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.

MCL 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. The hypotheses collapse to one only if the observations actually discriminate between the places and particles remain near the true pose. Where observations cannot tell places apart — a symmetric corridor, say — multiple modes persist; and if the particle count is too low or the likelihood too peaked, the correct mode is depleted and the estimate locks onto the wrong 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] = log_weight   # collect log-weights first
        
        # Normalize weights -- subtract the max log-weight before exp (the log-sum over hundreds of beams underflows if exponentiated directly)
        log_w = self.particles[:, 3]
        self.particles[:, 3] = np.exp(log_w - np.max(log_w))
        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 ray casting based on fixed-step sampling.

Cells are looked up by adding a direction vector to real-valued
        coordinates, so cells can be skipped or queried more than once.
        When every cell the line passes through must be visited exactly
        once, use Bresenham or the Amanatides-Woo DDA.
        """
        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 + 1) * self.resolution   # we advance before checking, so the checked point lies (step+1) cells away
        
        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 and seasons change. Foundation-model-based descriptors such as AnyLoc are robust to this problem.

2. **Frame misalignment**: Because the initial alignment may be inaccurate, widen the candidate search instead. Open up the pose prior's gate, raise the number of candidates and RANSAC iterations, and enlarge the basin of initial guesses handed to registration. Do not loosen the residual acceptance criterion of geometric verification: relax that and false loops pass straight through and break the map.

### 10.4.3 ORB-SLAM3 Multi-Map System

The Atlas system of ORB-SLAM3 is a representative implementation of multi-session SLAM. It operates 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 candidate search exchanges only compressed descriptors (Scan Context, compact visual descriptors). Geometric verification then additionally exchanges sparse features or subsampled point clouds, and distributed optimization exchanges boundary pose estimates and their uncertainties.
- **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.

Distributed pose graph optimization can be written as:

$$\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 for the boundary states touched by the inter-robot edges $\mathcal{E}_{\text{inter}}$. ADMM or Gauss-Seidel-style block coordinate descent is run in a distributed fashion. Note, though, that a pose graph objective containing rotations is non-convex, so ADMM's convex convergence guarantees do not apply; what is reached is a first-order critical point that depends on the initialization and the communication graph. Global optimality is obtained only under specific conditions, by certifiably correct methods that attach an a posteriori verification.

---

## 10.5 Recent Research

**[riSAM (McGann et al., 2023)](https://arxiv.org/abs/2209.14359)** integrates [Graduated Non-Convexity](https://arxiv.org/abs/1909.08605) into an incremental robust backend. The paper reports results above 90% outlier rate and comparisons with offline baselines under its synthetic and experimental setups; this is not a guarantee for other outlier structures or initializations.

**[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 to improve robustness. It has been validated on diverse platforms such as drones 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 and applies a stricter outlier test. 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 or a neural network? That question leads to **spatial representation**, the final output of sensor fusion.

---

# Ch.11 — Spatial Representations

A robot estimates its trajectory from sensor data, establishes global consistency through loop closure, and records the result as a **map**. The map is the form in which the robot remembers and uses the world.

A **map** is a spatial representation of the environment. A SLAM system estimates the robot's trajectory together with the structure observed along it. The representation determines which tasks the robot can perform: path planning requires free/occupied information, visual rendering requires texture, and human interaction requires semantics.

The starting point is metric maps (quantitative geometric maps), followed by 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 widely used because it connects directly to localization and planning, although the appropriate representation depends on the task and sensors.

### 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**: a dense 3D grid over an $L \times W \times H$ space at resolution $r$ requires $(L/r)(W/r)(H/r)$ cells. A 100 m $\times$ 100 m $\times$ 10 m volume at 5 cm contains $2000 \times 2000 \times 200 = 8 \times 10^8$ cells. One float per cell is about 3.2 GB before weights, semantics, and data-structure overhead.

```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(occupied | miss) — update for a pass-through observation
        
        # 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, while regions that are entirely occupied or entirely free stop subdividing early (pruning). Large cells represent empty space, and small cells are reserved for fine structure, reducing memory use.

OctoMap has the following 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**: refining near observed boundaries can use less memory than a dense grid when large regions are free or unknown. The ratio depends on occupancy structure and tree overhead.
- **Limitations**: pointer and tree overhead remain. It supports occupancy queries but does not replace a kNN or surface-registration index, and removing dynamic objects requires an inverse sensor update and temporal policy.

```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 structure from film VFX. It uses a fixed-depth, wide hierarchical tree with tile/value compression to omit background regions. Its structure and API differ from OctoMap, so traversal speed cannot be ranked universally; an application must define probabilistic occupancy updates.

**ikd-tree** (FAST-LIO2): an incremental k-d tree that inserts a single point in amortized $O(\log n)$ and supports dynamic rebalancing. Deletion is a range-wise lazy deletion, and rebalancing is applied to a subtree once an imbalance criterion is exceeded, so the cost of these two operations is separate from the per-point $O(\log n)$. 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.

An ikd-tree provides three operations:
- **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 | Point insertion is amortized $O(\log n)$; range deletion and rebalancing carry separate costs |
| 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, w)$:

- $\mathbf{p} \in \mathbb{R}^3$: position
- $\mathbf{n} \in \mathbb{R}^3$: normal vector (unit)
- $r \in \mathbb{R}^+$: radius
- $c$: color (the observed value)
- $w$: fusion confidence (accumulated weight)

**[ElasticFusion](https://doi.org/10.15607/RSS.2015.XI.001)** (Whelan et al. 2015) is a dense SLAM system that builds a surfel map in real time from an RGB-D sensor. It uses three processes:

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, at each voxel, the signed distance along the observed line of sight. The field that holds the distance to the nearest surface in every direction is the ESDF introduced later (§11.2.2); what is used here is a projective TSDF obtained by projecting depth observations:

- Positive: in front of the surface (free space)
- Negative: behind the observed surface (within the truncation band)
- Zero crossing: the actual surface location

The sign is defined along the observed ray. In front of the surface the distance is clipped to $+\delta$ and updated; the interior of an object beyond $-\delta$ behind the surface is skipped during the update, as in the code below, and keeps its default value. A negative TSDF is therefore not the same commitment to occupancy as the occupied state of §11.1.

**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}\left(D(\mathbf{u}) - \lambda^{-1}\|\mathbf{v} - \mathbf{c}\|, -\delta, \delta\right), \quad \lambda = \|\mathbf{K}^{-1}\dot{\mathbf{u}}\|$$

$D(\mathbf{u})$ is the depth value at pixel $\mathbf{u}$, $\mathbf{c}$ is the camera position, and $\mathbf{v}$ is the voxel center. Because $D(\mathbf{u})$ is a depth along the optical axis, the ray length $\|\mathbf{v} - \mathbf{c}\|$ is divided by $\lambda$ to bring it onto the same basis before the subtraction.

```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)** useful for gradient-based and optimization-based path planning. Sampling-based and search-based planners operate directly on an occupancy map, so an ESDF is not a prerequisite for planning in general.

Voxblox operates in three stages:

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.

Path planning must maintain a safe distance between the robot and obstacles. An ESDF allows the distance from an arbitrary point to the nearest obstacle to be queried in $O(1)$. It also provides gradient information from which the avoidance direction can be obtained directly.

### 11.2.3 Poisson Surface Reconstruction

Poisson reconstruction is a method that generates a watertight mesh from an oriented point cloud (position + normals).

Poisson reconstruction interprets the normal vectors as a gradient field, takes the divergence of this gradient, and solves 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.

Poisson reconstruction solves for a global implicit function and can close a surface, but depends on oriented-normal quality and depth or screening settings; it may also seal openings that should remain open. Its cost makes it common in post-processing, whereas TSDF meshing fits systems that need incremental depth integration.

### 11.2.4 Real-Time Mesh Generation

SLAM systems can mesh an incremental TSDF or pass an online point/surfel map to a separate surface-reconstruction stage:

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 colored point map + post-processing**: FAST-LIVO2 uses camera intensity for visual updates and can output a colored point map; its online output should not be described as a mesh. If a mesh is needed, inspect normals and density in the exported point map before a separate Poisson or Ball-Pivoting reconstruction.

---

## 11.3 Neural / Learned Representations

Traditional map representations (voxel, mesh, surfel) are explicit — they store 3D structure directly. The learned representations covered in this section divide further among themselves. At one end sits the **implicit** approach, in which a single MLP holds the scene in its weights; in the middle sit hash grids and hierarchical grids, which keep spatial features in an explicit data structure and decode them with a small MLP; at the other end sit approaches built on explicit primitives such as 3D Gaussians.

### 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) was an early system to use a single MLP as the sole scene representation in real-time handheld RGB-D SLAM. The paper reports 10 Hz tracking and 2 Hz global-map updates using a keyframe structure and information-guided pixel sampling.

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.

Recent variants use the following components:
- **Instant-NGP**: multi-resolution hash-grid feature encoding accelerates neural scene training and rendering. NeRF-SLAM variants leveraging it 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. With explicit primitives and rasterization, the original 3DGS paper reports real-time rendering under its scenes, resolutions, and hardware setup.

**[Gaussian Splatting SLAM](https://arxiv.org/abs/2312.06741)** (Matsuki et al. CVPR 2024; the public implementation is named MonoGS) uses 3DGS as the SLAM representation. The three problems this line of work addresses are the following.

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. This remains an open problem for this line of work, and the paper above does not implement loop closure.

**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 for a single MLP (model size); grows with scene scale for spatial feature-grid variants | Variable (proportional to number of Gaussians) |
| Loop closure handling | Hard (weight deformation) | Relatively easy at the representation level (Gaussian transformation); full-system implementations remain rare |

The tradeoff between neural maps and TSDF or surfel maps depends on the sensor, scene scale, dynamic objects, long-term consistency, and rendering requirements. Neural representations can optimize novel-view rendering jointly with mapping, but memory growth and loop correction must be evaluated separately for large-scale or long-running systems.

**Recent systems (2024-2025)**:

- **[SplaTAM](https://arxiv.org/abs/2312.02126)** (Keetha et al. CVPR 2024): performs online tracking and mapping of 3D Gaussians from an RGB-D camera and expands the map using a silhouette mask. Depending on the metric and scene, the paper reports improvements of up to 2x in camera pose, map construction, and novel-view synthesis.
- **[MonoGS](https://arxiv.org/abs/2312.06741)** (Matsuki et al. CVPR 2024 Highlight): uses 3D Gaussians as the sole 3D representation for integrated monocular tracking, mapping, and rendering. The paper reports operation at 3 fps and uses geometric verification and regularization to address monocular reconstruction ambiguities.
- **[MASt3R-SLAM](https://arxiv.org/abs/2412.12392)** (Murai et al. CVPR 2025): integrates the MASt3R 3D-reconstruction foundation model into SLAM. Its map representation is not Gaussians but the dense pointmap predicted by MASt3R. The paper reports dense SLAM without a predefined camera model and throughput of 15 fps.

```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. A robot needs semantic information to distinguish the objects and categories in its environment.

### 11.4.1 Object-Level Maps

An object-level map recognizes the objects in an environment and registers their positions and sizes in the map.

An object-level map is built in four stages:
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 an early public system that incrementally builds several layers of a 3D scene graph online and jointly corrects them after loop closure. 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**: Hydra builds this layer as follows.

1. Compute the ESDF from the TSDF.
2. Incrementally extract the GVD from the ESDF. The GVD is the set of points equidistant from two or more nearest obstacles, and a vertex of the graph is a point equidistant from three or more. Such a point is where clearance is locally maximal, which makes it a good point 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. Assign each place node the distance to the nearest obstacle (its clearance) as a value.
2. At narrow passages such as doorways, this value is low.
3. Raise the clearance threshold step by step (dilation) to cut the narrow connections, then apply modularity-based community detection to the remaining subgraph to group the places into rooms.

**Hierarchical loop closure**: Hydra leverages several layers of the scene graph to improve loop closure. Descriptors from the place and object layers narrow the candidates, and the registration between two scene-graph portions is verified with TEASER++. The paper reports that in its own experiments this structure detected more loops, and did so more accurately, than an appearance-based 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.

The system extracts CLIP image features from each observation (an image or patch) and stores them at the corresponding 3D location. When the user queries "red fire extinguisher," the system encodes the text with the CLIP text encoder, compares it with the map's CLIP features using cosine similarity, and returns the corresponding location. Only the CLIP image encoder shares an embedding space with text, so DINO features cannot serve this comparison; they are used as a separate channel, for segmentation or observation consistency.

The robot can therefore locate objects on which it was not trained in advance. This is useful for household and exploration robots operating in unpredictable environments.

```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. People come and go, furniture is moved, and appearance changes with the seasons. 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 changes in an object's class or state in addition to geometric changes. Examples include "chair moved" and "door opened/closed."

### 11.5.2 Map Maintenance: Retention Strategies

Long-term operation cannot retain every observation indefinitely, so map maintenance needs criteria for keeping and discarding information.

**Strategy 1: Recency weighting**: assign higher weights to recent observations and gradually decay the influence of older ones. Decaying the TSDF weight over time is one implementation.

**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 conditions or seasons, 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 points that are predicted to be visible but whose re-observed position cannot be explained by ego-motion. Observations also disappear for static reasons — leaving the field of view, occlusion by static structure, association failure — so the criterion is visibility reasoning and reprojection residual, not the disappearance itself.

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)
        )
```

---

Each representation serves different downstream tasks. Modern systems combine them hierarchically—planning paths with OctoMap, rendering with surfels, and answering semantic queries with scene graphs. On a real platform, these algorithms and representations must be integrated and evaluated within autonomous-driving, drone, or handheld systems.

---

# Ch.12 — Practical Systems & Benchmarks

Real platforms combine sensor modeling, state estimation, odometry, place recognition, and spatial representation into a single system.

Representative platforms include autonomous driving, drones, and handheld mapping, and each system is evaluated with standard benchmarks and tools.

---

## 12.1 Autonomous Driving Perception Stack

An autonomous-driving safety design must detect single-sensor failures and limit risk. Whether the vehicle continues or transitions to a minimal-risk condition depends on its ODD and safety case; multiple sensors can provide redundancy and cross-checks.

### 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

**[nuScenes](https://arxiv.org/abs/1903.11027) collection vehicle**:
- 1 × 32-beam spinning LiDAR
- 6 × surround cameras (360° coverage)
- 5 × radar
- IMU, GNSS

**Camera-centric production example**:
- A system may center perception on multiple surround cameras and neural models.
- Installed sensors and radar use vary by vehicle generation, market, and date, so do not generalize an entire manufacturer as one fixed "vision-only" configuration.

### 12.1.2 Production-Level Fusion Pipeline

The sensor fusion pipeline of a production autonomous-driving system is generally built from the following elements. The arrows below give a conceptual order of processing stages, not the only dependencies among the modules. Localization in particular is not downstream of tracking and prediction: it runs in parallel from GNSS, IMU, LiDAR, and wheel measurements, and its output feeds perception, prediction, and planning alike.

```
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**: Each sensor independently detects 3D bounding boxes, which are then combined via NMS (Non-Maximum Suppression). It is modular and easy to debug, but has difficulty exploiting cross-sensor complementarity.

- **Deep fusion**: Sensors are combined by learning at the feature stage, and where that combination happens differs by method. [BEVFusion](https://arxiv.org/abs/2205.13542) (MIT) transforms camera and LiDAR features into a shared BEV (Bird's Eye View) representation and merges them in that space, whereas TransFusion is a transformer decoder in which object queries initialized from LiDAR BEV features cross-attend to image features. Either way, the network learns to exploit complementary cross-sensor information, but 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**: register the current LiDAR scan to an HD point-cloud map with NDT or ICP. With adequate map accuracy, overlap, geometry, and initialization, this constrains position where GNSS is weak; centimeter-level error must be demonstrated under the actual map and protocol.

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

Camera + IMU is a common sensor combination on drones. Small drones cannot easily carry a LiDAR because of weight and power constraints, whereas cameras and IMUs satisfy both constraints; compact solid-state LiDARs such as the Livox Mid-360 are changing the available options. Propeller vibration is a separate issue because it adds noise to IMU data, so vibration-isolating mounts and software filtering are both required.

Representative VIO systems used on drones include the following:
- **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

Autonomous flight in GPS-denied environments — indoors, tunnels, under forest canopy, and in electronic-warfare conditions — remains a challenge for drones.

The available approaches depend on whether prior infrastructure is available:

1. **VIO alone**: drift accumulates, so allowable mission duration follows from motion, texture, calibration, loop closure, and the error budget.
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**: pressure change provides a relative-altitude cue. Weather, temperature, and prop wash introduce bias, so it does not remove absolute z drift by itself.

### 12.2.3 Real-Time Constraints

Set a drone's sensor rates and latency budget from maximum angular rate and acceleration, control bandwidth, exposure, estimator delay, and communication delay. The IMU rate must limit aliasing and integration error under the specified motion, while camera exposure balances allowable image motion against low-light noise. The end-to-end state-estimation latency must come from closed-loop stability analysis and flight testing; 30 ms is not universal.

**Point-LIO** reduces latency by processing points individually without waiting for scan completion. It can be used 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. Read its accuracy under the conditions the manufacturer states (scan length, whether the loop is closed, validation method), and check the contract specification separately against the criteria in 12.3.2 below.
- **NavVis VLX**: Backpack-mounted. Four cameras + LiDAR. Specialized for indoor mapping.
- **GeoSLAM ZEB**: Handheld mobile mapping. Early generations suspended a 2D LiDAR on a spring so that walking oscillated it passively, while generations from ZEB-REVO onward use a motor-rotated head. The scanning mechanism differs by generation.

The conceptual flow of what these devices do is as follows. None of the three products publishes its internal algorithms, so the names below are examples of published methods that fill the same slots, not an indication of what the products actually use.

```
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

Survey mapping begins with the contract specification and validation procedure:

- **Absolute accuracy**: specify horizontal and vertical error and confidence, then validate against independent checkpoints. Use GNSS, total station, or GCPs as site conditions require.
- **Relative accuracy**: Internal consistency of the map. Loop closure and global optimization establish this consistency.
- **Point density**: specify spacing and completeness from object size and deliverable. A 1 cm value may be a project requirement, not a universal survey standard.
- **Color quality**: Accurate HDR color mapping. Camera-LiDAR time synchronization and extrinsic calibration must be precise.

Deployed sensor fusion systems must address the following problems:

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 an IMU. Multi-modal systems such as R3LIVE and FAST-LIVO2 are effective.

2. **Multi-story buildings**: With no GNSS, z-axis drift is especially problematic when moving between floors. On a route that revisits the same floor, loop closure reduces this error substantially. A single-pass route has no loop to close, so constrain it with other observations instead — a barometer, floor-height or floor-plan priors, or control points.

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 | 11 training + 11 test sequences for odometry evaluation |
| **[EuRoC](https://doi.org/10.1177/0278364915620033)** | 2016 | Indoor (MAV) | Stereo, IMU | VIO sequences in Machine Hall + Vicon Room |
| **[TUM-RGBD](https://doi.org/10.1109/IROS.2012.6385773)** | 2012 | Indoor | RGB-D | Kinect v1 visual-SLAM sequences and evaluation tools |
| **TUM-VI** | 2018 | Indoor + outdoor | Stereo, IMU | VIO benchmark. Diverse motion patterns |
| **[Hilti](https://arxiv.org/abs/2109.11316)** | 2021–2023 challenge editions | Construction sites | LiDAR, Camera, IMU | Industrial-environment-specific. Challenging conditions |
| **[HeLiPR](https://arxiv.org/abs/2309.14590)** | 2023 | Outdoor (urban) | Heterogeneous LiDAR, IMU, GNSS/INS | For place recognition research across heterogeneous LiDARs. Ouster+Velodyne+Livox+Aeva. Check whether cameras are included in the per-release sensor list |
| **[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 (campus) | LiDAR, Camera, IMU | New College grounds, Oxford. Handheld single session, many loops |

Characteristics and uses of each dataset:

**KITTI** — Released in 2012, it remains a long-standing reference benchmark for autonomous-driving odometry and SLAM. It provides a Velodyne 64-channel LiDAR, stereo cameras, and GPS/IMU. Its sensors are dated, its sequences are relatively short, and its GPS/INS-based ground truth does not guarantee centimeter-level accuracy in every segment.

**EuRoC** — Stereo camera + IMU data captured on a drone (MAV). It is a widely used VIO benchmark. Depending on the sequence, ground truth was recorded with a Vicon motion-capture system or a Leica laser tracker. The 11 sequences are classified as easy → medium → difficult.

**Hilti** — Evaluates SLAM in challenging construction-site environments with dust, vibration, and repetitive structures. Public sources document challenge editions in 2021, 2022, and 2023; the sensor suites and evaluation rules differ by edition.

**HeLiPR** — Released in 2023, it mounts different types of LiDAR (spinning, solid-state, FMCW) simultaneously. It is used for place recognition research across heterogeneous LiDARs (inter-LiDAR place recognition).

**Newer College** — Collected in a single session by walking the grounds of New College, Oxford with a handheld rig, this dataset passes through the same areas repeatedly and is well suited to loop closure evaluation. For long-term research covering time-of-day and seasonal change, a dataset such as Boreas is the better fit. Captured with a handheld LiDAR, it contains challenging motion patterns.

**Additional benchmarks since 2022**:

- **[Hilti-Oxford](https://arxiv.org/abs/2208.09825)** (2022): A construction-environment SLAM benchmark with millimeter-level ground truth used for the 2022 challenge.
- **[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.
- **[Snail-Radar](https://arxiv.org/abs/2407.11705)** (Huai et al., IJRR 2025): A benchmark for evaluating 4D radar SLAM that provides 4D radar odometry/SLAM results 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}$$

The two trajectories are often aligned before evaluation, and the degrees of freedom of that alignment are set by which degrees of freedom are unobservable and by the benchmark protocol — not by the sensor's name. Sim(3) is used where scale is unobserved, as in pure monocular; SE(3) is used where scale is observed, as in stereo, LiDAR, and monocular + IMU. Some protocols use no Sim(3) alignment at all, such as the official KITTI odometry metrics, and some cases look at absolute-coordinate error with no post-hoc alignment, as in the surveying section above.

$$\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.

In the expression above, $\Delta$ is a frame-index interval. RPE at short $\Delta$ reflects odometry accuracy, while RPE at long $\Delta$ reflects drift. To evaluate over distance intervals instead, as KITTI does (100–800 m), you must first find, for each start frame, the frame at which the accumulated travel distance reaches the target value, and pair the two frames accordingly.

**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

The following caveats apply when interpreting benchmark results:

1. **Parameter tuning**: The same algorithm can perform very differently depending on its parameters. A number obtained by tuning for a specific dataset is a result for that configuration and cannot be read across to another environment. Check whether the tuning conditions were reported alongside it.

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**: Results obtained with loop closure enabled must not be mixed with results obtained with it disabled. Loop closure can substantially improve ATE. The name is no guide: DSO, called a VO system, has no loop closure, but LDSO added one, and ORB-SLAM3 can be run with loop closure turned off. Check the actual configuration.

---

## 12.5 Open-Source Tool Guide

Sensor fusion research and practice commonly use the following open-source tools.

### 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(0.0, 1.0, 0.0)   # translation is expressed in pose 1's frame, so forward after the turn is +y
    )
    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(-1.0, 1.0, 0.0)   # pose2.between(pose0): from pose2 = (Rz(90°), (1,1,0)) back to the origin
    )
    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)
- Supports automatic differentiation
- Used by VINS-Mono 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.)
- Defines a vertex/(hyper-)edge graph abstraction directly, in place of GTSAM's factor type hierarchy. Comparisons of processing speed can reverse with problem size, variable ordering, linear solver, and build settings, so measure it on the target problem

Comparison of the three libraries:

| Property | GTSAM | Ceres | g2o |
|----------|-------|-------|-----|
| Abstraction level | Factor graph | Cost function | Graph vertex/edge |
| Automatic differentiation | Partial (Expression-based) | Full | Numerical differentiation by default; automatic differentiation available in some versions |
| 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
- Widely used for camera-IMU calibration, though installation is tricky (ROS dependency)

**OpenCalib** (2023):
- Unified calibration across the full autonomous driving sensor stack
- A collection of per-sensor-pair tools: camera intrinsics, LiDAR-camera, LiDAR-IMU, radar-camera, LiDAR-LiDAR, and others. There is not a module for every combination of the four sensors
- 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 on a pair of one camera image and one LiDAR point cloud (single-shot). Spinning LiDARs sometimes require an accumulated point cloud, so check the point density it needs as well

### 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
import copy

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 = copy.deepcopy(traj_est)
        traj_est_aligned.align(traj_gt, correct_scale=False)   # align transforms in place; the return value is (R, t, s)
        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 = copy.deepcopy(traj_est_sync)
        traj_est_aligned.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 sensor-fusion methods covered here reach products and research through these systems and benchmarks. Foundation models, event cameras, 4D radar, and end-to-end SLAM remain active **research frontiers**.

---

# Ch.13 — Frontiers & Emerging Directions

Sensor fusion includes established theory and deployed systems as well as research directions that are still being validated.

These directions include extending foundation models to spatial AI, end-to-end learned SLAM, scene-graph-based environmental understanding, cross-modal representation, and the fusion of event cameras and 4D radar.

---

## 13.1 Foundation Models for Spatial AI

Research increasingly uses representations from foundation models — general-purpose models pretrained on large-scale data, such as DINOv2, CLIP, and SAM — inside sensor-fusion and SLAM pipelines. Their features can support retrieval, matching, and semantic priors, but they do not automatically replace geometry or uncertainty handling.

### 13.1.1 Leveraging DINOv2/CLIP Features in SLAM

**Visual features of DINOv2**: [DINOv2](https://arxiv.org/abs/2304.07193) is a ViT trained with self-supervised learning. Tokens obtained for image patches can be used as dense features, and downstream experiments reveal the following properties.

- Under some illumination and seasonal changes, the features help recover same-place correspondences.
- Object categories and scene semantics can affect feature similarity.
- Patch-level structure can support downstream correspondence and segmentation tasks.

**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:

- was evaluated without VPR-specific fine-tuning on paper benchmarks spanning urban, indoor, aerial, underwater, and subterranean settings.
- reports higher recall than the tested NetVLAD- and CosPlace-family baselines on several datasets, not a guarantee for every environment.
- includes an ablation in which value-facet dense features from layer 31 outperform the global CLS-token descriptor. The size of the improvement depends on the metric and on what is being evaluated, so read the metric name together with the reported average range in the paper's own ablation table.

```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 patch-level (1/14 resolution) 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 | Example use of learned features |
|---------------|-----------|-------------|
| 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 | Relative-depth prior from [Depth Anything](https://arxiv.org/abs/2401.10891) |
| Loop-closure candidate | DBoW2, Scan Context | FM descriptor similarity followed by geometric verification |

### 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.

The system operates as follows:

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** are representative systems of this approach. They allow 3D space to be queried with arbitrary text without a predefined class set.

Limits remain:
- 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 Evidence and Boundaries by Module

The evidence for foundation-model integration varies by task and benchmark. The following is not a replacement ranking; it identifies what must be validated for each use.

**Areas with evidence on published benchmarks**:
- **Visual place recognition**: AnyLoc reports strong recall for DINOv2+VLAD across several domains. Because its inputs, training assumptions, and compute differ from DBoW2, treat them as alternatives to test in the target environment and compare both false-positive rate and latency.
- **Feature matching**: LoFTR and RoMa report strong results on benchmarks with difficult texture and viewpoint changes. Classical sparse features remain viable when compute, calibration, or repeatability requirements favor them.
- **Monocular depth**: The base output of [Depth Anything](https://arxiv.org/abs/2401.10891) is relative depth. Fusion that requires metric scale needs a metric-depth fine-tune or constraints from another sensor, plus a separate treatment of output uncertainty.

**Areas that still require a separate estimator**:
- **LiDAR odometry**: Comparisons between geometry-based methods such as ICP, LOAM, and FAST-LIO2 and learned methods depend on the sensor and dataset. Learned features or correspondences do not remove motion modeling and registration validation.
- **IMU integration**: Preintegration compresses kinematics and a noise model into a factor. Learned bias or noise corrections can be added, but they do not automatically replace the observation model and uncertainty propagation.
- **Backend optimization**: Factor graphs and iSAM2 combine observations. An FM output used as a factor still needs explicit covariance, outlier handling, and correlation analysis.

A practical default is therefore a hybrid that retains the geometry and uncertainty structure of an estimator while adding learned features or semantic priors as testable modules.

**Recent systems (2024-2025)**:

- **[MASt3R-SLAM](https://arxiv.org/abs/2412.12392)** (Murai et al. CVPR 2025): This system integrates MASt3R's learned geometry into SLAM; the paper reports dense SLAM at 15 fps without a prespecified camera model.
- **[Depth Anything V2](https://arxiv.org/abs/2406.09414)** (Yang et al. NeurIPS 2024): This work trains a teacher on synthetic data and a student on large-scale pseudo-labels. Its benchmark results make it a candidate depth prior, but fusion still requires validation of scale and uncertainty.

---

## 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 turns this 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) is a representative public system for learned visual SLAM.

DROID-SLAM has the following structure:

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 diagonal inverse costs $O(1)$ and processing the full depth block is linear in the number of depth variables. This structure makes the Schur complement efficient to compute.

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**: One model trained on synthetic TartanAir data is applied to all three input types. The paper reports higher accuracy and fewer catastrophic failures than the tested baselines on selected metrics across four benchmarks.

**Results reported for DROID-SLAM**:
- 62% error reduction over the previous best on TartanAir
- 82% reduction on EuRoC monocular
- These percentages are relative comparisons under the paper's specific splits, metrics, and baselines

### 13.2.2 Differentiable SLAM Components

Individual SLAM pipeline components can be made differentiable even without full end-to-end learning.

**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. Optimization error becomes a training signal for the feature extractor.

### 13.2.3 Current Limitations and Possibilities

**Limitations**:
- **Generalization**: Performance can change with cameras, motion, lighting, and dynamic scenes outside the training distribution. The DROID-SLAM paper evaluates synthetic-to-real generalization, but not every environment or sensor modality.
- **Scope of verification**: Traditional pipelines make it easier to inspect residuals and modules separately, whereas a learned update operator is tied more strongly to its training distribution and evaluation protocol. Neither approach automatically guarantees global convergence or consistency for a general nonlinear SLAM problem.
- **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 develop, learning-based modules can be integrated into traditional structures.
- Multi-task learning: jointly training pose estimation, depth estimation, and semantic segmentation produces mutual benefits.

---

## 13.3 Spatial Memory & Scene Graphs

Spatial memory includes hierarchical, relational, and temporal information as well as point clouds. Humans remember space in statements such as "there is a refrigerator in the kitchen, and there was milk inside it." High-level spatial memory systems apply this structure to a robot's map.

### 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.

Persistent spatial memory poses three 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

The following extensions are being studied on top of 3D Scene Graph representations such as the one [Hydra](https://arxiv.org/abs/2201.13360) builds. Hydra itself covers hierarchy construction and hierarchical loop closure.

**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 remote-control Object nodes with an on-top-of relation to 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 does place human agents as nodes (see the layer table in §11.4.2), but its geometric and semantic mesh, its place and room layers, and the optimization built on top of them all assume a static structure. Dynamic scene graphs aim to update moving agents — vehicles included — and their relations inside the map structure in real time. Social navigation and human-robot interaction (HRI) can use this information.

### 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 challenge in sensor fusion is comparing heterogeneous sensor observations in a **common representation space**. A LiDAR point cloud and a camera image have different data forms, yet they observe the same physical environment. Cross-modal representations reduce this "representation gap."

### 13.4.1 Aligning Representations Across Heterogeneous Sensors

Three differences make representation alignment difficult:

- **Dimensional mismatch**: LiDAR returns 3D points, a camera returns a 2D image, and radar returns a range-Doppler map. The data forms differ.
- **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$ indexes the candidate samples in the denominator. The sum includes the positive sample $\mathbf{x}_C$ and the negative samples.

**Application to cross-modal place recognition**: A camera-only system can localize against a map built with LiDAR. If the LiDAR descriptor and the camera descriptor occupy the same space, a camera query can retrieve locations from a LiDAR map.

**LC$^2$** (Lee et al. 2023): LiDAR-Camera cross-modal place recognition. It projects the LiDAR scan into a range image and converts the camera image into a disparity/depth representation, then aligns the features of the two representations into a common space (§9.4.3).

### 13.4.3 Knowledge Distillation

Knowledge distillation transfers 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 (rain affects LiDAR or darkness affects the camera), 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) operates differently from traditional frame-based cameras. Each pixel independently senses brightness changes and asynchronously emits an **event** only when 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 |
| No pixel brightness change | provides frame information | ideally no events |
| Power consumption | varies widely by device | the sensor itself stays low and scales with activity; system power including readout and processing grows with the event rate |

Event cameras provide information during fast rotations, abrupt illumination changes (entering/exiting tunnels), and low-light operation. They therefore complement traditional cameras and other sensors.

### 13.5.2 Event + Frame Fusion

Event cameras and traditional frame cameras are combined in the following ways:

**Event-enhanced frame tracking**: Fast motion between frames is tracked with events, filling the gap between frame-based VO frames. This improves tracking robustness under fast camera motion. Tracking can still break when low contrast yields too few events, or when the opposite happens and a saturating event rate exhausts the available bandwidth.

**Event-aided HDR**: The event camera's high dynamic range supplements information in underexposed or overexposed regions of frame images.

### 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). What is lost when events are aggregated into frames depends on the representation: an event-count histogram discards the timing information inside the window, whereas a time surface keeps the most recent event time at each pixel and so preserves much of the ordering. The high dynamic range that comes from log-intensity sensing survives aggregation.
- Event-camera price and resolution vary by model, and the product range remains narrower than for conventional frame cameras.
- Training data is scarce. Most datasets are designed for frame cameras.

**Recent systems and research (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): A survey of event-driven 3D reconstruction that organizes 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(-200, 200, 20):   # px/s. Event-camera flow is typically tens to hundreds of px/s
            for vy in np.linspace(-200, 200, 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 provides four-dimensional information: range, azimuth, elevation angle, and Doppler velocity. It adds elevation resolution to automotive radar measurements of range, azimuth, and Doppler, observing 3D position together with radial 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

4D imaging radar often retains useful range and Doppler detections in conditions that degrade visible and near-infrared sensors, but it is not immune to adverse weather. The following table describes qualitative tendencies, not a performance guarantee for a particular device.

| Condition | Camera tendency | LiDAR tendency | 4D radar tendency | Main variables |
|------|-----------------|----------------|-------------------|----------------|
| Clear day | Rich color and texture information | Favors dense geometric detail | Provides range and Doppler, with potentially limited angular resolution | Target RCS, range, angular resolution |
| Rain | Contrast loss and lens wetting | Backscatter and attenuation | Often less degraded, but rain clutter and a wet radome still matter | Rain rate, radome condition, frequency |
| Fog | Reduced contrast and visibility | Backscatter and attenuation | Often relatively robust | Fog density, range, target RCS |
| Snow/dust | Occlusion and contrast loss | Particle backscatter | Clutter, multipath, and surface accumulation remain | Particle density, accumulation, target RCS |
| Night | Depends on illumination and exposure | Active ranging remains available | Active range and Doppler remain available | Illumination, surface reflectivity, target RCS |
| Direct sunlight | Glare and saturation are possible | Solar background can affect the receiver | Optically insensitive, but RF interference remains | Receiver/filter, RF interference, mounting direction |

Millimeter-wave radar is generally less sensitive to fog-sized particles than visible or near-infrared sensing, but it is still affected by them. Heavy precipitation, a wet or iced radome, multipath, RF interference, low-RCS targets, and limited angular resolution can reduce detection range or increase false alarms. Validate detection range, false-alarm rate, and Doppler error for each device under the target weather conditions.

### 13.6.2 4D Radar + Camera Fusion

Combining 4D radar with a camera can compensate for their different failure modes. Validate cost and adverse-weather performance for the sensor models, mounting, compute platform, and target dataset.

**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. Cameras and pulsed time-of-flight LiDAR do not provide this. FMCW LiDAR is the exception, since it measures per-point velocity by the same principle:

- **Dynamic object classification**: the Doppler residual left after compensating for the ego velocity separates candidate static background from candidate moving objects. An object moving perpendicular to the line of sight contributes zero radial velocity and cannot be distinguished from this observation alone, and the threshold has to reflect both the radar's velocity noise and the error in the ego-velocity estimate.
- **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 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 low resolution and high noise pose challenges.

**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$ denotes the point index. Estimating the 3D velocity vector requires at least three static points whose line-of-sight unit vectors are linearly independent.

```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 (reference), Odometry | Urban | Traditional automotive radar points + semantic labels (point-level annotation). LiDAR is not included |
| **nuScenes** | Camera, LiDAR, Radar | Urban | Includes 5 radars, some adverse weather |
| **View-of-Delft** | Camera, LiDAR, 4D Radar | Urban | 4D radar + 3D annotation |

**Recent systems and datasets (2024-2025)**:

- **[Snail-Radar](https://arxiv.org/abs/2407.11705)** (2024): A 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.

The deployment scope of 4D radar fusion remains limited. Ego-motion estimation and dynamic object classification based on Doppler information provide capabilities that cameras and pulsed LiDAR do not (FMCW LiDAR is the exception).

---

## Closing

The sensor fusion pipeline runs from sensor modeling (Ch.2) 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 research frontiers (Ch.13).

Four trends emerge from this sequence:

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 is widely used in perception.** In feature matching, depth estimation, and Place Recognition, learned methods have reported higher scores than traditional baselines on several public benchmarks. The scope of that advantage depends on the dataset and evaluation protocol.
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 being integrated into the pipeline.** Sensor-fusion pipelines use representations from general-purpose models such as DINOv2 and SAM at several stages.

Sensor fusion combines incomplete observations to estimate the state of the environment. A system design should consider sensor models, estimators, and failure modes together.