Guide on Becoming a Roboticist

가이드 렌더링 중...

# Ch.1 — 서론: Spatial AI란?

각 기술의 위치를 보려면 먼저 전체 지도가 필요하다.

## 1.1 Spatial AI의 정의

**Spatial AI**는 기계가 3차원 공간을 이해하고 그 안에서 행동할 수 있도록 하는 인공지능 기술의 총칭이다. 구체적으로 다음 질문에 답해야 한다.

- "나는 지금 어디에 있는가?" (Localization)
- "주변 환경은 어떻게 생겼는가?" (Mapping)
- "저 물체는 무엇이고, 어디에 있는가?" (Object Detection & Localization)
- "어떻게 목적지까지 갈 수 있는가?" (Navigation & Planning)

일반적인 AI/딥러닝은 "이미지에 고양이가 있는가?"에 답한다. Spatial AI는 여기에 더해 "저 고양이는 나로부터 몇 미터 떨어져 있고 어느 방향으로 움직이는가? 고양이를 피해 어떻게 이동할 것인가?"까지 답해야 한다. 핵심은 공간적 맥락이다.

Spatial AI는 다음 기술을 아우른다.
- Computer Vision: 카메라 이미지에서 정보 추출
- 3D Vision: 깊이 인식, 포인트 클라우드 처리
- SLAM: 동시적 위치 추정과 지도 작성
- Deep Learning: 학습 기반 인식 및 예측
- Sensor Fusion: 여러 센서 정보의 통합

이 모든 기술은 불확실성이라는 공통 난관을 마주한다. Thrun, Burgard, Fox (2005) *Probabilistic Robotics* §1.1은 그 원천을 다섯 가지로 정리한다. 첫째, 환경을 완전히 예측할 수 없다. 둘째, 모든 측정에는 분해능의 한계와 노이즈가 따른다. 셋째, 모터 토크의 편차와 슬립 때문에 명령한 동작과 실제 동작이 달라진다. 넷째, 복잡한 물리 환경과 로봇을 수식으로 추상화하는 순간 모델은 필연적인 근사물에 불과하다. 다섯째, 실시간 제약 아래에서는 최적해 대신 근사해를 써야 한다. 이 다섯 가지는 베이즈 필터부터 SLAM까지 이 가이드에서 다루는 알고리즘의 공통 동기다. 자세한 내용은 ch.3 §3.9~3.11과 ch.14 §14.16에서 다룬다.

> **추천 자료**
> - [Andrew Davison — From SLAM to Spatial AI (MIT Robotics)](https://www.youtube.com/watch?v=BRRtlR0C_CY) — Andrew Davison 교수가 Spatial AI의 비전을 설명하는 강연이다. 분야의 큰 흐름을 파악하는 데 도움이 된다.
> - [FutureMapping 논문 (arXiv:1803.11288)](https://arxiv.org/abs/1803.11288) — 2018년에 SLAM이 기하·의미를 통합한 Spatial AI 인지 능력으로 진화하는 흐름을 논의하고, 그 알고리즘의 계산 구조와 프로세서·센서 co-design을 검토한 포지션 논문.
> - [Cyrill Stachniss — SLAM Course (2013)](https://www.youtube.com/playlist?list=PLgnQpQtFTOGQrZ4O5QzbIHgl3b1JHimN_) — Cyrill Stachniss 교수의 프라이부르크 시절 SLAM 강의. EKF-SLAM·FastSLAM·graph SLAM을 다루는 대학원 수준 자료다. Spatial AI의 기반 개념들을 잘 정리해서 강의한다.

## 1.2 왜 중요한가?

관심 있는 응용 분야에 따라 깊이 공부할 기술이 달라진다. 자율주행에서는 LiDAR와 센서 퓨전이 중요하고, AR/VR에서는 Visual-Inertial 시스템이 중요하다. 전체 응용 분야를 살펴보면 자신에게 맞는 학습 경로를 정할 수 있다.

Spatial AI는 다음 분야의 핵심 기술이다:

| 분야 | 응용 예시 |
| --- | --- |
| **자율주행** | 차량의 위치 인식, 장애물 감지, 경로 계획 |
| **서비스 로봇** | 실내 내비게이션, 물체 조작, 인간과 협업 |
| **드론** | 자율 비행, 3D 지도 생성, 검사/배송 |
| **AR/VR** | 공간 추적, 가상 객체 배치, 손 추적 |
| **산업 자동화** | 물류 로봇, 품질 검사, 조립 자동화 |

## 1.3 로보틱스는 왜 어려운가

"AI가 발전하면 로보틱스의 문제도 모두 풀리지 않나?"라는 질문을 종종 받는다. 그렇지는 않다. AI가 개선하는 영역과 로보틱스를 어렵게 만드는 요인이 서로 다르기 때문이다.

로보틱스의 어려움은 주로 물리 세계와 맞닿는 지점에서 생긴다.

- 코드 버그가 충돌로 이어지면 장비가 파손되거나 사람이 다칠 수 있어 소프트웨어처럼 간단히 롤백할 수 없다.
- 코드 수정, 업로드, 환경 초기화, 안전 확보, 실행, 물리적 확인을 거치는 한 차례의 실험에 수 분에서 수십 분이 걸린다.
- 센서 데이터에는 역광, 모션 블러, 드리프트, 프레임 드롭이 섞인다. 깨끗한 데이터에서의 성능만으로는 실제 환경의 동작을 판단하기 어렵다.
- 장애물 회피처럼 실시간성이 필요한 기능은 정확도와 응답 지연에 대한 요구를 함께 충족해야 한다.
- 드물게 발생하는 엣지 케이스도 충돌이나 안전사고로 이어질 수 있다.
- 시뮬레이터의 마찰 계수, 관성, 노이즈는 현실의 근사치이므로 오차가 누적되면 실제 로봇의 행동이 달라질 수 있다.

| 일반 소프트웨어 | 로보틱스 |
|---|---|
| 버그 → 로그 → 수정 → 재배포 | 버그 → 충돌 → 파손 → 수리 → 재시도 |
| 이터레이션 수 초 | 이터레이션 수 분~수 시간 |
| 입력이 정형화됨 | 센서 데이터가 노이즈 투성이 |
| 99% 정확도면 훌륭 | 99.9999%도 부족할 수 있음 |
| 응답 지연 → 불편 | 응답 지연 → 사고 |
| 같은 입력 → 같은 출력 | 같은 코드라도 환경에 따라 결과가 다름 |

AI의 발전은 인식 정확도나 자연어 명령 이해를 개선한다. 그러나 위 표의 오른쪽에 놓인 이터레이션 속도, 센서 노이즈, 실시간 제약, 파손 위험은 물리 세계에서 생기는 문제다. 모델의 규모를 키우는 것만으로는 해결되지 않으며, AI 모델 하나를 학습했다고 해서 로봇 시스템이 곧바로 작동하는 것도 아니다.

### AI 시대에 로보티시스트가 하는 일

AI가 코드를 짜고, 논문을 요약하고, 실험을 제안하는 시대에 로보티시스트의 가치는 어디에 있는가?

- **문제 정의**: AI는 주어진 문제를 푸는 일을 도울 수 있지만, 어떤 문제를 풀어야 하는지는 스스로 판단하지 못한다. 어떤 센서 조합이 환경에 적합한지, 어느 정도의 정확도가 응용에 충분한지, 어떤 절충을 받아들일 수 있는지는 분야를 아는 사람이 판단해야 한다. (*문제 정의의 기본 틀은 [「연구노트」 Ch.1 — 리뷰 논문에서 시작하라](../research-notes/guide.html#chapter-1)와 [「연구노트」 Ch.2 — 문제 정의](../research-notes/guide.html#chapter-2)에서 다룬다.*)
- **시스템 통합**: 인식 모듈, 제어 모듈, 통신 스택, 하드웨어를 하나의 시스템으로 작동하게 만드는 일이다. AI는 각 모듈의 코드를 짤 수 있지만, 모듈 간 인터페이스와 타이밍, 예외 처리를 설계하는 일은 엔지니어의 몫이다.
- **물리 세계와의 접점**: 케이블이 빠졌는지, 센서 렌즈에 먼지가 꼈는지, 모터가 과열됐는지는 원격 접속만으로 확인하기 어렵다. 로봇 앞에서 장비를 직접 살피는 사람이 필요하다.
- **신뢰성 판단**: AI가 "99% 정확도"라고 보고해도, 그 값이 무엇을 분모로 무엇을 세었는지, 남은 1%가 어떤 실패인지는 엔지니어가 확인해야 한다. 같은 99%라도 실패가 인명 사고로 이어지는 작업과 재시도로 끝나는 작업에서 충분성이 다르다. 판단 기준은 산업 분야 이름이 아니라 실패의 비용과 노출 빈도다.

AI 도구가 발전해도 이 네 가지를 대신할 수는 없다.

## 1.4 읽는 방법

이 문서는 필요할 때 찾아보는 참고서(reference)처럼 활용하면 된다.

1. 처음 읽을 때: 목차를 훑어보고 전체 그림을 파악
2. 연구 시작할 때: 관련 섹션을 깊이 읽고 추천 자료 학습
3. 막힐 때: 부록의 용어 사전과 트러블슈팅 참고

추천 학습 순서:

```
수학적 기초 → 센서 → 컴퓨터 비전 기초 → SLAM → 딥러닝 → VFM/VLA → 연구실 방향
```

수학 없이 SLAM 논문을 읽으면 수식에서 막히고, 센서 특성을 모르면 왜 알고리즘이 특정 상황에서 실패하는지 이해할 수 없다. 기초부터 순서대로 쌓는 것이 결국 빠른 길이다.

### 단계별 학습 경로

배경에 따라 속도를 조절하되, 각 단계를 건너뛰지 않는다.

**입문 단계 — 도구 손에 익히기**

이 단계의 목표는 연구에 필요한 기본 도구를 자유롭게 다루는 것이다. 코드를 읽고, 돌려보고, 결과를 해석할 수 있어야 한다.

학습 내용:
1. **C++ 코드 읽기** — 연구실의 핵심 코드(SLAM, ROS 패키지)가 C++이다. 처음부터 짤 필요는 없지만, 구조를 읽고 수정할 수 있어야 한다.
2. **Python 기초** — 딥러닝 학습 스크립트, 데이터 전처리, 시각화에 사용. AI 에이전트의 도움을 받기 좋은 언어이다.
3. **선형대수, 확률/통계 복습** — 학부 때 배운 내용을 로보틱스 관점에서 다시 정리. 3장의 내용을 참고하자.
4. **ROS2 기본** — 토픽, 서비스, 액션, launch 파일. 연구실에서 사용하는 로봇 프레임워크이다.
5. **Git 사용법** — branch, merge, rebase까지. 연구실에서 코드 관리는 Git으로 한다.

실습 과제:
- OpenCV로 이미지 처리 파이프라인 구축 (읽기 → 필터링 → 특징점 추출 → 시각화)
- 간단한 ROS2 노드 작성 (퍼블리셔/서브스크라이버)
- 카메라 캘리브레이션 수행 (체스보드 패턴 사용)

**중급 단계 — 핵심 기술 익히기**

이 단계에서는 Spatial AI의 핵심 알고리즘을 직접 돌려보고 결과를 분석할 수 있어야 한다. 논문 읽기도 이때부터 시작한다.

학습 내용:
1. **딥러닝 기초 (PyTorch)** — 텐서, 자동 미분, 학습 루프, 모델 설계. TensorFlow보다 PyTorch가 연구에서 주류이다.
2. **Object Detection (YOLO 계열)** — 바운딩 박스, NMS, mAP 등 인식 파이프라인의 기본 개념 이해.
3. **Visual SLAM 이해 (ORB-SLAM3)** — 특징점 기반 SLAM의 대표 알고리즘. 돌려보고 코드를 뜯어보자.
4. **포인트 클라우드 처리 (Open3D)** — 3D 데이터를 다루는 법. 필터링, 정합, 시각화.
5. **VFM 이해 및 활용 (DINOv2, SAM)** — Foundation Model이 기존 파이프라인을 어떻게 바꾸는지 이해.

실습 과제:
- KITTI 데이터셋으로 벤치마크 실험 수행
- YOLOv8 파인튜닝 (커스텀 데이터셋)
- ORB-SLAM3 실행 및 궤적 분석
- TUM RGB-D 데이터셋으로 SLAM 정확도 평가

**고급 단계 — 연구자로서의 첫걸음**

이 단계부터는 논문 읽기, 실험, 글쓰기로 이어지는 연구의 전체 사이클을 경험한다.

학습 내용:
1. **논문 읽기 및 구현** — 주당 최소 1~2편 논문 읽기. 핵심 논문은 코드까지 분석.
2. **새로운 아이디어 실험** — 기존 방법의 한계를 파악하고 개선 아이디어를 실험.
3. **벤치마크 평가** — 공정한 비교를 위한 평가 프로토콜 숙지.

실습 과제:
- 최신 논문 코드 분석 및 재현
- 자체 개선 아이디어 실험 및 정량적 비교
- 논문 작성 시도 (학회 워크숍 투고 목표)

> **추천 자료**
> - [Missing Semester of Your CS Education (MIT)](https://missing.csail.mit.edu/) — Git, Shell, 디버깅 등 연구에 필요한 실전 도구를 가르쳐주는 MIT 강의
> - [ROS2 공식 튜토리얼](https://docs.ros.org/en/humble/Tutorials.html) — ROS2 Humble 기준 공식 학습 자료
> - [Andrej Karpathy — Neural Networks: Zero to Hero](https://www.youtube.com/playlist?list=PLAqhIrjkxbuWI23v9cThsA9GvCAUhRvKZ) — 딥러닝을 밑바닥부터 구현하며 배우는 명강의

## 1.5 선수 지식 체크리스트

연구를 시작하기 전에 다음 항목을 확인하자. 각 항목이 필요한 이유도 함께 적었으니, 체크하는 데 그치지 말고 "이걸 왜 해야 하는지"를 이해하고 넘어가자.

**필수**:
- [ ] **C++ 읽기 능력**
  - 연구실의 핵심 코드(SLAM, 실시간 제어, ROS 패키지)가 C++이다. ORB-SLAM3, LOAM 같은 오픈소스를 이해하고 수정하려면 C++에 익숙해야 한다.
- [ ] **Linux 기본 명령어 (cd, ls, cp, mv, grep)**
  - 연구실 서버는 거의 100% Ubuntu이다. GPU 서버에 SSH로 접속해서 실험을 돌리려면 터미널이 편해야 한다.
- [ ] **Git 기본 사용법 (clone, commit, push, pull)**
  - 연구 코드 관리, 논문 코드 받기, 연구실 내부 코드 공유 전부 Git으로 한다. GitHub에서 오픈소스 코드를 clone해서 돌려보는 것이 일상이다.

**권장**:
- [ ] **Python 기초 (함수, 클래스, 모듈)**
  - 딥러닝 학습 스크립트, 데이터 전처리, 시각화에 사용된다. AI 에이전트가 잘 다루는 언어이므로 직접 작성할 일은 줄고 있지만, 읽고 이해할 수 있어야 한다.
- [ ] **NumPy 기본 사용법**
  - 행렬 연산, 브로드캐스팅, 인덱싱. 센서 데이터 처리와 좌표 변환에 사용된다.
- [ ] **선형대수 기초 (행렬 연산, 고유값)**
  - 3D 변환, 카메라 모델, 최적화 전부 선형대수이다. "이 수식이 뭘 의미하는지"를 이해하려면 행렬의 기하학적 의미를 알아야 한다. 3장에서 이어진다.
- [ ] **확률/통계 기초 (정규분포, 베이즈 정리)**
  - 센서 노이즈 모델링, 상태 추정, 필터링 전부 확률 기반이다. "이 센서의 측정값을 얼마나 믿을 수 있는가?"를 수학적으로 표현하려면 이 지식이 필요하다.
- [ ] **미적분학 기초 (편미분, 체인 룰)**
  - Gradient descent, Jacobian, 최적화 알고리즘의 기본이다. 딥러닝의 역전파도, SLAM의 Bundle Adjustment도 결국 미분이다.

> **추천 자료**
> - [3Blue1Brown — Essence of Linear Algebra](https://www.youtube.com/playlist?list=PLZHQObOWTQDPD3MizzM2xVFitgF8hE_ab) — 선형대수의 기하학적 직관을 잘 설명하는 영상. 수식에 들어가기 전에 보면 도움이 된다.
> - [3Blue1Brown — Essence of Calculus](https://www.youtube.com/playlist?list=PLZHQObOWTQDMsr9K-rj53DwVRMYO3t5Yr) — 미적분의 직관적 이해. 체인 룰과 편미분이 왜 중요한지 시각적으로 보여준다.
> - [Python for Data Analysis (Wes McKinney)](https://wesmckinney.com/book/) — NumPy, Pandas 등 데이터 분석 도구의 표준 교재. 무료 온라인 버전 제공.

> **기술 흐름: Spatial AI 분야 전체**
> - **~2005**: 고전적 로보틱스 — 수학적 모델 기반, Kalman Filter, EKF-SLAM. 수작업 특징점과 기하학적 방법이 주류.
> - **2007~2015**: 실시간 Visual SLAM의 등장 — MonoSLAM(2007), PTAM(2007), ORB-SLAM(2015). 카메라만으로 실시간 위치 추정과 지도 생성이 가능해졌다.
> - **2012~2018**: 딥러닝 혁명 — AlexNet(2012)을 시작으로 ResNet(2015), Faster R-CNN(2015) 등 인식 성능 급상승. Spatial AI에도 학습 기반 방법 도입 시작.
> - **2020~2023**: Foundation Model 시대 — CLIP(2021), SAM(2023), DINOv2(2023) 등 대규모 사전학습 모델 등장. 이전에는 새 환경마다 데이터 수집→라벨링→학습을 반복해야 했으나, zero-shot으로 처리 가능한 태스크가 크게 늘었다.
> - **2024~**: End-to-End 시스템과 Embodied AI — VLA(Vision-Language-Action) 모델, World Models, 3D Gaussian Splatting + SLAM 등 인식·계획·제어의 경계를 줄이는 연구가 이어진다. 실물 시스템에서는 안전성, 지연, 검증 요구에 따라 end-to-end와 모듈형 구성을 함께 비교한다.
> - **최근 흐름**: Foundation Model을 로봇 인식에 접목한 open-vocabulary SLAM과 VFM 기반 scene understanding, 고전적 기하학과 학습 기반 component를 결합하는 연구가 이어지고 있다.

> 실습 자료: 이 문서의 주요 개념에 대한 interactive 실습은 [여기](https://alexjunholee.github.io/robotics-practice/)에서 확인할 수 있다.

---

# Ch.2 — 센서 (Sensors)

로봇이 환경을 인식하려면 센서가 필요하다. 센서별 특성을 알아야 상황에 맞게 고르고 알고리즘도 설계할 수 있다.

SLAM 추적 실패가 rolling shutter, LiDAR 반사 특성, IMU bias 가운데 어디에서 시작됐는지 가려내려면 센서의 측정 원리와 오차를 알아야 한다. 센서 모델은 뒤따르는 인식·추정 알고리즘이 어떤 데이터를 받는지 규정한다.

## 2.1 카메라 (Camera)

카메라는 색상과 텍스처를 높은 공간 해상도로 측정한다. 단안·스테레오·RGB-D·event camera는 깊이 측정 방식, 시간 해상도, 조명 변화에 대한 반응이 서로 다르므로 작업 조건에 맞춰 선택한다.

### 2.1.1 Monocular Camera (단안 카메라)

가장 기본적인 시각 센서로, 하나의 렌즈로 2D 이미지를 촬영한다.

단안 카메라는 렌즈 하나로 색상과 텍스처를 얻어 Visual SLAM, 객체 인식, 시맨틱 이해에 사용한다. 깊이를 직접 측정하지 못하므로 monocular depth estimation이나 SfM으로 장면 구조를 추정한다. 스테레오·깊이 카메라는 이 깊이 모호성을 다른 측정 방식으로 보완한다.

**장점**:
- 저렴하고 가벼움
- 풍부한 색상 및 텍스처 정보
- 높은 해상도

**단점**:
- 단일 이미지에서 깊이(depth) 직접 측정 불가
- Scale ambiguity: 물체의 실제 크기를 알 수 없음

**주요 사양**:
- 해상도: 720p, 1080p, 4K 등
- Frame rate: 30 fps, 60 fps, 120 fps 등
- Field of View (FoV): 좁은 화각 vs 광각 (fisheye)
- Global shutter vs Rolling shutter

```
일반적인 카메라 센서:
- 웹캠: Logitech C920, C930e
- 산업용: FLIR (Point Grey), Basler, Allied Vision
- 임베디드: Raspberry Pi Camera, OAK-D
```

> **추천 자료**
> - [First Principles of Computer Vision — Camera and Imaging](https://www.youtube.com/playlist?list=PL2zRqk16wsdoCCLpou-dGo7QQNks1Ppzo) — Columbia 대학교 Shree Nayar 교수의 카메라 원리 강의. 핀홀 모델부터 렌즈 왜곡까지 설명.
> - [OpenCV Camera Calibration Tutorial](https://docs.opencv.org/4.x/dc/dbb/tutorial_py_calibration.html) — 카메라 캘리브레이션을 직접 해보는 실습 가이드

### 2.1.2 Stereo Camera (스테레오 카메라)

두 개의 카메라를 일정 간격(baseline)으로 배치하여 깊이를 측정한다. 인간의 양안 시각과 같은 원리다.

실외 환경에서 깊이를 얻는 대표적인 패시브(능동적 빛 방출 없이) 방식이 스테레오 카메라다. Structured Light나 ToF는 햇빛 간섭으로 실외에서 성능이 저하될 수 있다. 에피폴라 기하학(Epipolar Geometry)과 직결되므로 수학적 기초와도 연결된다.

**깊이 계산 원리**:

```
Depth (Z) = (focal_length × baseline) / disparity
```

- **Disparity**: 좌우 이미지에서 동일 점의 x좌표 차이
- **Baseline**: 두 카메라 사이의 거리

**장점**:
- 패시브 센서 (능동 조명 장치 불필요)
- 실외 환경에서도 사용 가능
- RGB 정보와 깊이를 동시에 획득

**단점**:
- 텍스처가 없는 표면에서 매칭 실패 (흰 벽, 유리)
- 계산 비용이 높음
- Baseline에 따라 측정 범위 제한

**대표 제품**:
- Intel RealSense D435/D455: Active IR 패턴 투사로 매칭 보조
- ZED 2: 넓은 baseline, 장거리 측정
- OAK-D: 엣지 AI 내장

> **추천 자료**
> - [Cyrill Stachniss — Stereo Vision](https://www.youtube.com/watch?v=SyB7Wg1e62A) — 스테레오 비전의 수학적 원리를 명확하게 설명
> - [Stanford CS231A — Epipolar Geometry and Stereo](https://web.stanford.edu/class/cs231a/) — Stanford의 Computer Vision 강의. 에피폴라 기하학을 잘 다룬다.

> **실습**: [Stereo Disparity 시각화](https://alexjunholee.github.io/robotics-practice/app.html#stereo_disparity)
> 스테레오 이미지 쌍에서 disparity를 계산하고, baseline과 focal length가 깊이 추정에 미치는 영향을 확인할 수 있다.

### 2.1.3 RGB-D Camera (깊이 카메라)

RGB 이미지와 Depth 이미지를 직접 제공하는 센서이다.

연구실에서 처음 다루게 되는 센서는 대개 RGB-D 카메라다. 데스크탑 환경에서 SLAM이나 3D 복원을 실험하기에 편리하기 때문이다. 다만 ToF와 Structured Light 방식의 차이를 모르면 실외에서 깊이 값이 사라지는 이유나 여러 대를 동시에 쓸 때 생기는 간섭을 이해하기 어렵다.

**ToF (Time of Flight) 방식**:
- 적외선을 발사하고 돌아오는 시간을 측정
- 장점: 텍스처 무관, 실시간 처리
- 단점: 햇빛 간섭, 반사 표면 문제
- 예시: Microsoft Azure Kinect, PMD Pico Flexx

**Structured Light 방식**:
- 알려진 패턴을 투사하고 변형을 분석
- 장점: 높은 정확도, 저비용
- 단점: 실외 사용 어려움, 다중 센서 간섭
- 예시: Orbbec Astra

**Active IR Stereo 방식**:
- 좌우 적외선 카메라의 스테레오 매칭을 IR 프로젝터로 보조
- 예시: Intel RealSense D400 시리즈

**비교**:
| 특성 | ToF | Structured Light |
|------|-----|------------------|
| 실외 사용 | 제한적 | 어려움 |
| 정확도 | 중간 | 높음 |
| 범위 | 0.2~5 m | 0.2~10 m |
| 다중 센서 | 가능 | 간섭 발생 |

> **추천 자료**
> - [Intel RealSense — Depth Cameras D415 & D435](https://www.youtube.com/watch?v=A4Kjvosvx5I) — Intel에서 직접 설명하는 깊이 카메라 원리
> - [Open3D RGB-D Reconstruction Tutorial](http://www.open3d.org/docs/release/tutorial/pipelines/rgbd_integration.html) — RGB-D 데이터로 3D 복원을 실습하는 튜토리얼

**RealSense 드라이버 설치 (Ubuntu 22.04)**

```bash
# Intel RealSense SDK 설치
sudo mkdir -p /etc/apt/keyrings
curl -sSf https://librealsense.intel.com/Debian/librealsense.pgp | sudo tee /etc/apt/keyrings/librealsense.pgp > /dev/null
echo "deb [signed-by=/etc/apt/keyrings/librealsense.pgp] https://librealsense.intel.com/Debian/apt-repo `lsb_release -cs` main" | \
    sudo tee /etc/apt/sources.list.d/librealsense.list
sudo apt-get update
sudo apt-get install -y librealsense2-dkms librealsense2-utils librealsense2-dev

# 테스트
realsense-viewer
```

ROS2에서 사용하려면 추가로:
```bash
sudo apt install ros-humble-realsense2-camera
ros2 launch realsense2_camera rs_launch.py
```

(참고: [정진용 블로그](https://jinyongjeong.github.io/2020/06/20/Realsense-Ubuntu-driver-%EC%84%A4%EC%B9%98/))

### 2.1.4 Event Camera (이벤트 카메라)

기존 카메라와 다른 패러다임의 센서이다. 프레임 단위 촬영 대신, 각 픽셀이 **밝기 변화**가 생길 때만 비동기적으로 이벤트를 출력한다.

Event camera 연구는 고속 운동과 HDR처럼 프레임 카메라가 어려움을 겪는 조건을 중심으로 꾸준히 확장되어 왔다. 이벤트 센서는 픽셀별 밝기 변화를 비동기적으로 기록하므로 프레임 노출에서 생기는 모션 블러를 피할 수 있다. 고속 운동이나 HDR 조건을 다룬다면 Gallego et al. survey (TPAMI 2020)와 rpg_dvs_ros 패키지부터 살펴보라.

**이벤트 출력 형식**:

```
(x, y, timestamp, polarity)
- x, y: 픽셀 좌표
- timestamp: 마이크로초 단위 시간
- polarity: 밝아짐(+1) 또는 어두워짐(-1)
```

**장점**:
- 매우 높은 시간 해상도 (마이크로초 단위)
- 높은 다이나믹 레인지 (140 dB vs 일반 카메라 60 dB)
- 낮은 전력 소모, 낮은 지연
- 모션 블러 없음

**단점**:
- 밝기 변화가 없으면 출력 없음 (장면과 카메라가 모두 정지하고 조명이 일정할 때)
- 전통적인 CV 알고리즘 적용 어려움
- 비교적 높은 가격

**대표 제품**:
- Prophesee: 고해상도 이벤트 센서
- iniVation: DAVIS (이벤트 + 프레임 동시 출력)
- Samsung: 모바일용 이벤트 센서 개발 중

> **추천 자료**
> - [Davide Scaramuzza — Event Cameras: A Paradigm Shift for Computer Vision](https://www.youtube.com/watch?v=LauQ6LWTkxM) — Event Camera 분야의 선구자인 Scaramuzza 교수의 개요 강연
> - [Gallego et al. — Event-based Vision: A Survey (TPAMI 2020)](https://arxiv.org/abs/1904.08405) — Event Camera 기술의 종합 서베이 논문. 이 분야를 이해하는 데 좋은 출발점이다.
> - [rpg_dvs_ros — Event Camera ROS 드라이버](https://github.com/uzh-rpg/rpg_dvs_ros) — Event Camera를 ROS에서 다루는 오픈소스 패키지

## 2.2 LiDAR

**LiDAR (Light Detection and Ranging)**는 레이저를 이용하여 거리를 측정하는 센서이다. 출력의 차원은 종류에 따라 다르다. 3D LiDAR는 포인트 클라우드를 직접 만들고, 2D LiDAR는 단일 평면의 스캔을 낸다.

카메라는 색과 texture를 기록하지만 수동 monocular 영상만으로 metric depth가 직접 정해지지는 않는다. LiDAR는 각 return의 range를 측정해 점을 만든다(3D LiDAR는 3D 점, 2D LiDAR는 평면 위의 점). 측정 범위와 오차는 모델, 표면 반사율, 입사각, 대기, 햇빛, return mode에 따라 달라지며, 자동차용 장거리 LiDAR 가운데 일부는 지정 반사율 조건에서 100 m 이상의 range를 제공한다. 수동 단안 카메라와 비교하면 이 직접 거리 측정이 LiDAR의 핵심 강점이다. ToF·structured-light 방식 RGB-D와 radar도 거리를 직접 재므로, LiDAR의 차별점은 측정 자체가 아니라 각분해능과 장거리에서의 거리 정확도에 있다.

Solid-State LiDAR와 Spinning(기계식) LiDAR는 요구 조건에 따라 함께 쓰이고 있다. Solid-State LiDAR는 움직이는 부품을 줄일 수 있어 내구성과 대량 생산 측면에서 자동차 양산에 유리할 수 있다. Livox의 비반복 스캔처럼 기존 회전식 센서와 다른 패턴도 등장하면서, 포인트 클라우드 처리 알고리즘은 이런 차이를 함께 고려해야 한다.

### 2.2.1 2D LiDAR vs 3D LiDAR

**2D LiDAR**:
- 단일 평면 스캔
- 용도: 실내 로봇 내비게이션, 장애물 회피
- 예시: SICK TiM, Hokuyo URG, RPLIDAR

**3D LiDAR**:
- 다중 레이어 또는 회전 스캔으로 3D 포인트 클라우드 생성
- 용도: 자율주행, 대규모 매핑
- 예시: Velodyne VLP-16·VLP-32C·HDL-64E, Ouster OS1, Hesai

> **추천 자료**
> - [Cyrill Stachniss — LiDAR-based SLAM](https://www.youtube.com/watch?v=vrdlk2p9AZI) — LiDAR 데이터를 이용한 SLAM의 원리를 설명
> - [PCL (Point Cloud Library) 공식 튜토리얼](https://pcl.readthedocs.io/projects/tutorials/en/latest/) — 널리 쓰이는 공개 포인트 클라우드 처리 라이브러리

### 2.2.2 Spinning vs Solid-State

**Spinning (기계식)**:
- 레이저와 수광부가 회전
- 360° FoV 제공
- 단점: 움직이는 부품으로 인한 내구성 이슈
- 예시: Velodyne, Ouster

**Solid-State**:
- 대형 회전 기구 없음 (MEMS·flash 방식은 가동부가 없고, Livox의 비반복 스캔은 내부 회전 프리즘을 쓴다)
- 제한된 FoV (보통 120° 이하)
- 장점: 높은 내구성, 저비용 가능성
- 예시: Livox (비반복 스캔 패턴), Innoviz

알고리즘 설계에 직접 영향을 미치는 차이다. Spinning LiDAR는 360° 균일한 포인트 클라우드를 생성하므로, 기존 SLAM 알고리즘(LOAM, LeGO-LOAM 등)이 이 특성을 전제로 설계되었다. Solid-State LiDAR로 넘어가면 스캔 패턴이 크게 달라져 알고리즘 수정이 필요하다. Livox의 비반복 스캔을 겨냥해 FAST-LIO2 같은 알고리즘이 등장한 것도 그 때문이다.

### 2.2.3 주요 사양

| 사양 | 설명 |
| --- | --- |
| Channels | 수직 레이어 수 (16, 32, 64, 128) |
| Range | 최대 측정 거리 (50 m~300 m) |
| Points/sec | 초당 포인트 수 (300K~2M) |
| Accuracy | 측정 정확도 (±2 cm~±5 cm) |
| FoV | 수평/수직 화각 |

> **추천 자료**
> - [Livox 기술 문서](https://www.livoxtech.com/downloads) — Solid-State LiDAR의 비반복 스캔 패턴과 그 장점을 설명하는 기술 자료
> - [Xu et al. — FAST-LIO2 (T-RO 2022)](https://arxiv.org/abs/2107.06829) — 특징 추출을 없앤 direct 정합과 ikd-Tree로 spinning·solid-state LiDAR를 모두 다루는 LiDAR-Inertial Odometry 논문

## 2.3 IMU (Inertial Measurement Unit)

IMU는 관성을 이용하여 움직임을 측정하는 센서이다.

SLAM을 돌렸는데 드리프트가 심할 때, IMU 특성을 모르면 원인조차 파악하기 어렵다. "IMU 바이어스가 제대로 보정되고 있나?", "이 등급의 IMU로 이 정도 정확도를 기대할 수 있나?" 같은 질문에 답하려면 오차 모델을 이해해야 한다. Visual-Inertial Odometry(VIO)나 LiDAR-Inertial Odometry(LIO)에서 IMU는 카메라/LiDAR 프레임 사이를 메워주는 역할을 한다. 그 역할을 제대로 하려면 IMU 데이터의 한계를 알아야 한다.

### 2.3.1 구성 요소

**Accelerometer (가속도계)**:
- 3축 선형 가속도 측정 (m/s²)
- 중력 가속도 포함

**Gyroscope (자이로스코프)**:
- 3축 각속도 측정 (rad/s 또는 deg/s)
- 회전 속도 감지

**Magnetometer (지자기 센서)** (일부 IMU):
- 3축 자기장 측정
- 절대 방위(heading) 추정 가능
- 자기장 왜곡에 취약

### 2.3.2 주요 오차 특성

IMU 오차를 모델링하지 못하면 센서 퓨전 시스템 전체가 흔들린다.

**Bias (바이어스)**:
- 정지 상태에서도 0이 아닌 출력
- 시간에 따라 천천히 표류 (bias instability, Allan deviation 곡선의 바닥값으로 정의). 온도 변화에 따른 thermal drift는 별개 항목이다

**Noise**:
- 고주파 랜덤 노이즈
- Allan Variance로 특성화

**Integration Drift**:
- 가속도 이중 적분 → 위치 오차 누적
- 각속도 적분 → 자세 오차 누적
- 외부 관측 없이는 시간이 지날수록 오차 누적

가속도를 두 번 적분해 위치를 구하면 noise, bias, scale factor와 초기 자세 오차가 누적된다. 오차 증가는 센서와 운동, 보정, 온도, 초기화에 따라 달라져 고정된 시간 하나로 설명할 수 없다. 저가 MEMS IMU의 unaided 위치 추정은 장기 위치 오차 예산을 빠르게 넘기기 쉬우므로, 장시간 위치가 필요한 시스템은 camera, LiDAR, GNSS 같은 외부 관측으로 drift를 제한한다.

**IMU 등급**은 단순한 가격대 구분을 넘어 bias stability, noise density, scale-factor error, 온도 보정, 진동 내성과 인증 요구로 구분한다. Consumer MEMS는 크기와 전력을 우선하고, industrial·tactical·navigation 계열로 갈수록 장기 안정성과 보정 범위를 강화하는 경향이 있다. `VN-100`, `MTi`, `KVH 1750`, `HG1700` 같은 제품을 비교할 때는 등급 이름보다 같은 단위의 최신 datasheet와 Allan 측정 결과를 확인한다.

> **추천 자료**
> - [Probabilistic Robotics, Ch.5–6 — Robot Motion / Robot Perception (Thrun, Burgard, Fox)](https://www.probabilistic-robotics.org/) — 모션 모델(5장)과 센서 측정 모델(6장)의 핵심 참고서. IMU 오차 모델(bias, random walk, Allan variance)은 다루지 않으므로 그 부분은 Titterton & Weston을 본다.
> - [Titterton & Weston — Strapdown Inertial Navigation Technology](https://ieeexplore.ieee.org/book/5765860) — IMU 원리와 관성 항법의 교과서
> - [Cyrill Stachniss — IMU and Inertial Navigation](https://www.youtube.com/watch?v=uHbRKvD8TWg) — IMU의 작동 원리와 오차 특성을 시각적으로 설명
> - [Allan Variance — IMU 노이즈 분석 가이드 (Vectornav)](https://www.vectornav.com/resources/inertial-navigation-primer/specifications--background/specifications--allan-variance) — Allan Variance를 이용한 IMU 노이즈 파라미터 추출 방법
> - [정진용 블로그 — IMU Filter (AHRS)](https://jinyongjeong.github.io/2020/01/10/IMU_filter/) — IMU 센서의 AHRS 필터 개요. Madgwick 필터와 ROS 패키지 소개

---

## 2.4 GNSS / GPS

GNSS는 실외 자율주행차와 드론에 지구 기준의 전역 좌표를 제공한다. SLAM이 출발점에 대한 상대 위치를 추정한다면, GNSS는 위도·경도·고도로 위치를 나타낸다. 실외 로봇 환경에서는 두 기준계를 함께 결합하며, 특히 RTK-GPS 측정치는 고정밀 측위 성능을 검증하는 ground truth로 널리 활용된다.

**정확도 해석**: open-sky 환경의 standalone code solution이 대략 meter-class 수준을 보이는 데 비해, differential correction을 거치면 환경 조건에 따라 sub-meter나 centimeter 수준까지 오차가 줄어든다. multipath가 심한 도심 협곡에서는 RTK라도 ambiguity 해상에 실패해 오차가 수 미터까지 튈 수 있다. 도시 자율주행이 GNSS에만 의존하지 않고 LiDAR·카메라 SLAM을 결합하는 이유다. 수치를 인용할 때는 CEP·RMS·95% 같은 metric, horizontal/vertical, baseline, correction link와 fix 상태를 함께 적는다.

**RTK-GPS 원리**:
- 고정된 Base Station이 보정 데이터 제공
- Rover가 보정 데이터를 수신하여 정확도 향상
- 실시간 통신 필요 (Radio 또는 인터넷)

**한계**:
- 실내·터널에서 사용이 어렵고, 도심 캐년에서 수신 성능 저하
- 멀티패스 오차 (건물 반사)
- 고도 정확도는 수평보다 낮음

> **추천 자료**
> - [Cyrill Stachniss — Robot Localization Overview](https://www.youtube.com/watch?v=8VJ-A9OlhAE) — 로봇 위치 추정의 원리와 방법론 개요
> - [u-blox GNSS 가이드](https://www.u-blox.com/en/technologies/gnss) — GNSS 기초부터 RTK까지 실용적 가이드

## 2.5 기타 센서

**Radar**

자율주행과 로보틱스에서는 radar를 camera·LiDAR와 함께 사용한다. 전파는 가시광과 일부 LiDAR 파장보다 안개·비·먼지·역광에 상대적으로 강건할 수 있지만, 강우 감쇠, clutter, multipath, wet radome과 낮은 각해상도는 남는다. 가격대도 antenna 수, bandwidth, imaging capability와 자동차 인증에 따라 LiDAR 제품군과 겹칠 수 있으므로 현재 견적으로 비교한다.

**FMCW (Frequency Modulated Continuous Wave) Radar**:
- 주파수를 시간에 따라 변조하여 송신하고, 반사파와의 주파수 차이로 거리와 속도를 동시에 측정한다.
- 출력: Range-Doppler map (거리 × 속도 2D 맵), Range-Azimuth map
- 자동차용 77 GHz radar가 가장 흔하다.

**로보틱스에서의 활용**:
- 자율주행: 전방 충돌 감지, 적응형 크루즈 컨트롤 (ACC)
- Radar odometry: 레이더만으로 자기 위치 변화 추정
- Radar SLAM: 레이더 기반 지도 작성 + 위치 추정

**카메라/LiDAR와의 비교**:

| 특성 | 카메라 | LiDAR | Radar |
|------|--------|-------|-------|
| 해상도 | 매우 높음 | 높음 | 낮음 |
| 거리 측정 | 불가 (단안) | 정확 | 가능 |
| 속도 측정 | 불가 | 불가 (직접) | 가능 (Doppler) |
| 악천후 | 취약 | 취약 (비, 안개) | 강건 |
| 가격 | 저렴 | 비쌈 | 중간 |
| 야간 | 불가 | 가능 | 가능 |

**대표 제품**: Texas Instruments AWR1843, Continental ARS548, Navtech CTS350-X (spinning radar)

> **추천 자료**
> - [김기섭 블로그 — ICRA 2021 Radar in Robotics Workshop 요약](https://gisbi-kim.github.io/blog/2021/05/31/icra21-radar-ws.html) — 레이더 로보틱스의 전반적 동향 정리
> - [김기섭 블로그 — Radar Odometry Results on MulRan dataset](https://gisbi-kim.github.io/blog/2021/05/30/yeti-radar-odom-mulran1.html) — 레이더 오도메트리 실험 결과. 도시 환경에서 LiDAR급 성능
> - [Kim et al., "MulRan: Multimodal Range Dataset for Urban Place Recognition" (ICRA 2020)](https://sites.google.com/view/mulran-pr/home) — LiDAR + radar + GPS 멀티모달 데이터셋

**Ultrasonic (초음파)**:
- 가까운 거리 장애물 감지 (0.2~5 m)
- 저비용
- 주차 보조, 근접 센서

**Wheel Encoder (휠 엔코더)**:
- 바퀴 회전량 측정
- Dead reckoning 기반 위치 추정
- 슬립에 취약

이 센서들을 "기타"로 분류했다고 중요하지 않은 것은 아니다. Radar는 자율주행 시스템에서 LiDAR가 취약한 악천후 상황의 안전망으로 기능하며, Wheel Encoder를 통해서는 지상 로봇의 가장 기초적인 오도메트리 정보를 확보한다. 센서 퓨전에서는 이런 "보조" 센서가 시스템 전체의 로버스트니스를 좌우한다.

> **추천 자료**
> - [Probabilistic Robotics, Ch.6 — Robot Perception (Thrun, Burgard, Fox)](https://www.probabilistic-robotics.org/) — 각종 센서의 확률 모델을 꼼꼼하게 다룬다. 센서 모델링의 교과서.

## 2.6 센서 퓨전 (Sensor Fusion)

단일 센서에는 저마다 한계가 있다. 여러 센서를 결합하면 서로의 약점을 보완할 수 있다.

단일 센서는 조명·거리·가림·드리프트 같은 조건을 모두 감당하지 못한다. 자율주행 센서 구성은 차량과 운행 조건에 따라 다르며, 카메라·LiDAR·Radar·IMU·GNSS 가운데 여러 종류를 조합한다. 선택한 센서들의 데이터를 언제, 어디서, 어떻게 결합하느냐가 시스템 성능을 좌우한다.

**왜 필요한가?**

| 센서 | 장점 | 단점 |
| --- | --- | --- |
| Camera | 풍부한 정보, 저렴 | 조명 의존, 깊이 없음 |
| LiDAR | 정확한 3D, 조명 무관 | 비쌈, sparse |
| IMU | 고주파, 조명 무관 | 드리프트 |
| GPS | 전역 위치 | 실외 전용, 저주파 |

**퓨전 방식**:
1. **Early Fusion**: Raw 데이터 레벨에서 결합
2. **Late Fusion**: 각 센서의 결과를 결합
3. **Mid-Level Fusion**: Feature 레벨에서 결합

방식마다 트레이드오프가 있다. Early Fusion은 정보 손실이 적지만 계산 비용이 높다. Late Fusion은 각 센서를 독립적으로 처리할 수 있어 모듈화에 유리하지만 정보가 일부 날아간다. Mid-Level Fusion은 그 중간이며, 딥러닝 기반 퓨전에서 많이 쓰인다.

**대표적인 조합**:
- Camera + IMU → VIO (Visual-Inertial Odometry)
- LiDAR + IMU → LIO (LiDAR-Inertial Odometry)
- Camera + LiDAR + IMU → 멀티모달 SLAM

퓨전의 수학적 토대는 §2.7 측정 모델의 확률적 정형화에서 다룬다.

> **추천 자료**
> - [State Estimation for Robotics (Tim Barfoot) — 무료 PDF](http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17.pdf) — 센서 퓨전의 수학적 기초를 다루는 대표적인 교재. Kalman Filter, Factor Graph 기반 추정을 모두 커버한다.
> - [Cyrill Stachniss — Kalman Filter & EKF](https://www.youtube.com/watch?v=E-6paM_Iwfc) — 센서 퓨전의 핵심인 칼만 필터와 EKF를 설명
> - [Qin et al. — VINS-Mono (TRO 2018)](https://arxiv.org/abs/1708.03852) — Visual-Inertial 퓨전의 대표 논문. 실제 VIO 시스템이 어떻게 구현되는지 보여준다.

---

## 2.7 심화: 측정 모델 — 확률적 정형화 (Probabilistic Measurement Models)

베이즈 필터·SLAM·MCL은 센서 데이터를 알고리즘에 공급할 때 "측정값이 얼마나 믿을 만한가"를 수치로 표현해야 한다. 그 표현이 측정 모델 $p(z_t \mid x_t, m)$이다.

### 2.7.1 센서가 만드는 분포

레이저 거리 센서를 같은 자세로 같은 벽을 향해 100번 쏘면, 100개의 측정값이 모두 다르다. 반사면 거울각, 지나가는 사람, 다중 반사가 원인이고, 측정값의 분산 구조도 원인마다 다르다. 이 분산 구조를 하나의 확률 분포로 나타낸 것이 $p(z_t^k \mid x_t, m)$이다. $z_t^k$는 시각 $t$의 $k$번째 빔 측정값, $x_t$는 로봇 포즈, $m$은 환경 지도다.

하나의 스캔에는 수십~수백 개의 빔이 있다. PR §6.2는 각 빔의 오차가 독립적으로 발생한다는 조건부 독립 가정을 도입한다. 이 가정 아래에서 전체 스캔의 likelihood는 각 빔 likelihood의 곱이 된다:

$$p(z_t \mid x_t, m) = \prod_{k=1}^{K} p(z_t^k \mid x_t, m)$$

이 조건부 독립 가정은 현실에서 완전히 성립하지 않는다. 같은 벽을 보는 인접 빔들은 상관되어 있고, 그 상관을 무시하면 likelihood가 특정 포즈에 과도하게 몰린다. 이 문제는 §2.7.8에서 다룬다.

지도 $m$의 형태도 두 가지다. feature-based 지도는 랜드마크 목록으로 구성되며 각 요소를 ID로 참조한다. location-based 지도는 격자 셀의 점유 확률 배열이며 셀을 좌표로 참조한다. 측정 모델 4가족은 이 두 지도 형태 중 하나에 의존한다.

측정 모델 4가족:
- 빔 모델 (beam model): 측정값이 생긴 원인을 근사하는 혼합 모델. location-based 지도.
- likelihood field: 빔 끝점→nearest obstacle 거리. location-based 지도.
- 상관 기반 (map matching): local map과 global map의 정규화 상관계수.
- 특징 기반 (landmark model): 추출된 특징을 (range, bearing, signature)로 모델링. feature-based 지도.

### 2.7.2 빔 모델 — 4성분 혼합

거리 센서의 한 빔이 낼 수 있는 측정값을 네 가지 원인 가설로 근사한다. [Thrun et al. 2005](https://www.probabilistic-robotics.org/) (PR §6.3.1)은 각 가설에 확률 분포를 두고, 네 분포의 가중 혼합으로 최종 likelihood를 구성했다. 이 성분들은 센서 하드웨어의 물리적 채널과 무관하며, 관측 분포를 체계적으로 설명하기 위해 정의된 모델 항에 해당한다.

4성분 중 가장 빈번한 것은 hit, 즉 실제 장애물을 정확히 감지한 경우다. 예측 거리 $z_t^{k*}$를 평균으로 분산 $\sigma_{\text{hit}}^2$의 절단 가우시안으로 모델링한다. 절단은 $[0, z_{\max}]$ 범위 밖의 확률 질량을 제거한다.

$$p_{\text{hit}}(z_t^k \mid x_t, m) = \eta\, \mathcal{N}(z_t^k;\, z_t^{k*},\, \sigma_{\text{hit}}^2), \quad 0 \le z_t^k \le z_{\max}$$

**short (예상치 못한 가까운 장애물)**: 지도에 없는 장애물(지나가는 사람, 다른 로봇)이 빔을 가로막는다. 측정값이 $z_t^{k*}$보다 항상 짧다. $[0, z_t^{k*}]$ 범위에서 지수 분포를 따른다.

$$p_{\text{short}}(z_t^k \mid x_t, m) = \eta\, \lambda_{\text{short}}\, e^{-\lambda_{\text{short}} z_t^k}, \quad 0 \le z_t^k \le z_t^{k*}$$

**max (최대 사거리 실패)**: 검은 표면, 거울각, 안개 등에서 반사파가 돌아오지 않는다. 센서가 $z_{\max}$를 그대로 출력하는 경우다. $z_{\max}$에서의 점질량(Dirac delta)으로 모델링된다.

$$p_{\text{max}}(z_t^k \mid x_t, m) = \mathbf{1}[z_t^k = z_{\max}]$$

**rand (정체불명 노이즈)**: sonar crosstalk, 다중 반사 등 원인을 알 수 없는 측정값이다. $[0, z_{\max}]$에서 균등 분포로 모델링된다.

$$p_{\text{rand}}(z_t^k \mid x_t, m) = \frac{1}{z_{\max}}$$

최종 likelihood는 4성분의 가중 혼합이다 (PR 식 6.13):

$$p(z_t^k \mid x_t, m) = \begin{pmatrix} z_{\text{hit}} \\ z_{\text{short}} \\ z_{\text{max}} \\ z_{\text{rand}} \end{pmatrix}^T \cdot \begin{pmatrix} p_{\text{hit}}(z_t^k \mid x_t, m) \\ p_{\text{short}}(z_t^k \mid x_t, m) \\ p_{\text{max}}(z_t^k \mid x_t, m) \\ p_{\text{rand}}(z_t^k \mid x_t, m) \end{pmatrix}$$

가중치 합은 1이어야 한다: $z_{\text{hit}} + z_{\text{short}} + z_{\text{max}} + z_{\text{rand}} = 1$.

예측 거리 $z_t^{k*}$는 포즈 $x_t$와 지도 $m$에서 ray casting으로 계산한다. 빔의 방향을 따라 점유된 셀에 처음 닿는 거리가 $z_t^{k*}$다. ray casting은 §2.1 카메라 투영 모델·§2.2 LiDAR 빔 구조와 같은 기하 원리를 점유 격자에 적용한 것이다.

**알고리즘: beam_range_finder_model** (PR Table 6.1 의역)

```
입력: z_t = {z_t^1, ..., z_t^K}, x_t, m
출력: p(z_t | x_t, m)

1. q ← 1
2. for k = 1 to K do:
3.     z_t^{k*} ← ray_cast(x_t, k, m)   // 예측 거리
4.     p ← z_hit  * p_hit(z_t^k | z_t^{k*}, σ_hit)
         + z_short * p_short(z_t^k | z_t^{k*}, λ_short)
         + z_max   * p_max(z_t^k | z_max)
         + z_rand  * p_rand(z_t^k | z_max)
5.     q ← q * p
6. return q
```

### 2.7.3 빔 모델 — 파라미터 EM 학습

센서 종류, 환경 구성, 마운팅 위치가 바뀔 때마다 hit/short/max/rand의 비율과 분산이 달라진다. 파라미터를 손으로 정하면 특정 환경에 맞춘 값이 다른 환경에서는 어긋날 수 있다.

내재 파라미터는 6개다: $z_{\text{hit}}, z_{\text{short}}, z_{\text{max}}, z_{\text{rand}}, \sigma_{\text{hit}}, \lambda_{\text{short}}$. PR §6.3.2는 로봇이 알려진 환경을 주행하며 수집한 데이터 $\{(z_t^k, z_t^{k*})\}$로부터 EM 알고리즘으로 이 파라미터를 최대우도 추정한다.

EM 정식화에서는 correspondence variable을 도입한다. 잠재 변수 $c_i \in \{\text{hit, short, max, rand}\}$는 각 측정값 $z_t^k$을 생성한 성분을 나타낸다.

**E-step**: 현재 파라미터 추정값으로 각 측정에 대해 4성분의 사후 확률을 계산한다 (PR 식 6.15~6.32):

$$e_{\text{hit}}^i = \frac{z_{\text{hit}} \cdot p_{\text{hit}}(z^i \mid z^{i*})}{p(z^i \mid z^{i*})}, \quad e_{\text{short}}^i = \frac{z_{\text{short}} \cdot p_{\text{short}}(z^i \mid z^{i*})}{p(z^i \mid z^{i*})}, \quad \dots$$

**M-step**: E-step에서 계산된 기댓값으로 파라미터를 업데이트한다. $\sigma_{\text{hit}}$와 $\lambda_{\text{short}}$는 닫힌 해가 존재한다:

$$\sigma_{\text{hit}}^2 = \frac{\sum_i e_{\text{hit}}^i (z^i - z^{i*})^2}{\sum_i e_{\text{hit}}^i}, \qquad \lambda_{\text{short}} = \frac{\sum_i e_{\text{short}}^i}{\sum_i e_{\text{short}}^i \cdot z^i}$$

혼합 가중치 $z_{\text{hit}}, z_{\text{short}}, z_{\text{max}}, z_{\text{rand}}$는 각 성분에 속하는 측정 비율로 갱신된다.

**알고리즘: learn_intrinsic_parameters** (PR Table 6.2 압축)

```
입력: {(z^i, z^{i*})} — (측정값, 예측값) 쌍
출력: z_hit, z_short, z_max, z_rand, σ_hit, λ_short

초기화: 파라미터를 균등 또는 임의값으로 설정
repeat until convergence:
    // E-step
    for each i:
        e_hit^i, e_short^i, e_max^i, e_rand^i ← posterior(z^i, z^{i*}, params)
    // M-step
    z_hit  ← mean(e_hit^i);   z_short ← mean(e_short^i)
    z_max  ← mean(e_max^i);   z_rand  ← mean(e_rand^i)
    σ_hit² ← weighted variance of (z^i - z^{i*}) by e_hit^i
    λ_short ← sum(e_short^i) / sum(e_short^i * z^i)
return params
```

EM은 특정 sensor·map·환경에서 수집한 데이터에 맞춰 이 파라미터를 추정하는 방법이다. AMCL 구현도 `sigma_hit`, `lambda_short`와 혼합 가중치를 설정값으로 노출하지만, package 기본값을 여러 환경에서 EM이 보편적으로 수렴한 값으로 해석해서는 안 된다.

EM으로 파라미터를 얻었더라도, 실제 시스템에서 이 모델을 빠르게 동작시키려면 추가적인 고려가 필요하다.

### 2.7.4 빔 모델 실무 고려

빔 모델의 주요 계산 병목은 ray casting이다. 입자마다 스캔의 모든 빔에 ray casting을 수행하면 MCL에서 입자 수 × 빔 수의 연산이 필요하다.

우선 빔 수를 줄일 수 있다. 전체 스캔에서 균일 간격으로 일부 빔만 사용하면 계산량과 인접 빔의 중복을 함께 줄일 수 있다. 사용할 빔 수는 scan resolution, 환경 구조, particle 수에 맞춰 검증한다.

**$p^{\alpha}$ 지수화 보정**: 빔들 사이의 독립 가정이 위반될 때, likelihood를 $p(z_t \mid x_t, m)^{\alpha}$ ($0 < \alpha < 1$)로 지수화하면 각 빔의 기여를 축소하여 over-confidence를 완화한다. $\alpha$는 경험적으로 설정하거나 교차 검증으로 정한다.

**range 사전 계산**: 지도에서 미리 모든 (셀, 방향) 조합에 대해 ray casting 결과를 테이블로 저장해 두면, 실행 시에 테이블 조회($O(1)$)로 ray casting을 대체할 수 있다. 지도가 크면 메모리 사용량이 크지만, 실시간 MCL에서 실용적이다 (Ch.3 §3.11 파티클 필터 참조).

### 2.7.5 Likelihood Field

빔 모델은 실용 시스템에서 두 가지 약점을 보인다. 첫째, ray casting 비용이 높다. 둘째, 포즈 $x_t$가 조금만 바뀌어도 빔이 다른 장애물에 먼저 닿으면 $z_t^{k*}$가 갑자기 크게 변한다. likelihood가 포즈에 대해 불연속이어서 gradient 기반 정합이나 hill-climbing 최적화를 방해한다.

likelihood field는 ray casting을 버리고 다른 계산을 쓴다. 빔 끝점을 전역 좌표계로 변환한 뒤, 그 점에서 지도의 가장 가까운 점유 셀까지의 유클리드 거리 $\text{dist}$로 likelihood를 평가한다.

빔 끝점의 전역 좌표 변환 (PR 식 6.33):

$$\begin{pmatrix} x_{z_t^k} \\ y_{z_t^k} \end{pmatrix} = \begin{pmatrix} x \\ y \end{pmatrix} + \begin{pmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{pmatrix} \begin{pmatrix} x_{k,\text{sens}} \\ y_{k,\text{sens}} \end{pmatrix} + z_t^k \begin{pmatrix} \cos(\theta + \theta_{k,\text{sens}}) \\ \sin(\theta + \theta_{k,\text{sens}}) \end{pmatrix}$$

여기서 $(x_{k,\text{sens}}, y_{k,\text{sens}})$는 로봇 기준계에서 $k$번째 빔 센서의 위치, $\theta_{k,\text{sens}}$는 빔의 방향 오프셋이다.

빔 likelihood (PR 식 6.34~6.35):

$$p(z_t^k \mid x_t, m) = z_{\text{hit}} \cdot \mathcal{N}(\text{dist};\, 0,\, \sigma_{\text{hit}}^2) + z_{\text{rand}} \cdot \frac{1}{z_{\max}}$$

여기서 $\text{dist}$는 빔 끝점에서 가장 가까운 점유 셀까지 계산된 유클리드 거리이며, 수식의 가우시안 분포는 거리 측정 오차를 0-평균으로 처리한다. max-range 빔($z_t^k = z_{\max}$)은 이 모델에서 무시한다. 끝점 투영이 의미 없기 때문이다.

**알고리즘: likelihood_field_range_finder_model** (PR Table 6.3 의역)

```
입력: z_t = {z_t^1, ..., z_t^K}, x_t = (x, y, θ)^T, m
출력: p(z_t | x_t, m)

1. q ← 1
2. for each k do:
3.     if z_t^k == z_max: continue   // max-range 무시
4.     // 빔 끝점을 전역 좌표로 변환
5.     x_ep ← x + x_{k,sens}·cos(θ) - y_{k,sens}·sin(θ) + z_t^k · cos(θ + θ_{k,sens})
6.     y_ep ← y + y_{k,sens}·cos(θ) + x_{k,sens}·sin(θ) + z_t^k · sin(θ + θ_{k,sens})
7.     // nearest obstacle까지 거리 (사전계산된 거리 변환 테이블에서 조회)
8.     dist ← nearest_obstacle_distance(x_ep, y_ep, m)
9.     q ← q * (z_hit · N(dist; 0, σ_hit²) + z_rand / z_max)
10. return q
```

지도가 고정되어 있다면 거리 변환(distance transform)을 한 번만 수행해 테이블로 저장할 수 있다. 그러면 $\text{dist}$ 조회가 $O(1)$이 된다. 이 테이블은 SDF(Signed Distance Field)의 양수 영역에 해당한다. 포즈 $x_t$에 대한 likelihood의 gradient도 계산할 수 있으므로 gradient 기반 scan matching에 적합하다.

한계도 있다. short 성분이 없어 동적 장애물을 명시적으로 모델링하지 않는다. 또한 끝점 거리만 보고 빔이 지나온 경로를 따지지 않으므로, 점유 영역을 관통해 들어온 빔에도 벌점을 주지 않는다. 그래서 벽 너머까지 "볼 수" 있고, 지도 자체의 불확실성도 무시한다.

2D LiDAR 실내 내비게이션에서 AMCL은 빔 모델보다 likelihood field를 기본으로 쓴다. 계산이 빠르고 포즈에 대해 연속적이기 때문이다. 3D LiDAR와 RGB-D에서는 ICP, NDT가 이 역할을 넘겨받았다 (Ch.3 §3.10 칼만 필터 계열과의 연동은 Ch.14 §14.7 참조).

루프 클로저 검출처럼 두 맵이 같은 장소인지를 빠르게 판단해야 할 때는, 확률론적 엄밀성보다 계산 속도가 우선이다.

### 2.7.6 상관 기반 모델 (Map Matching)

상관 기반 모델은 가장 ad hoc한 방법이다. 최근 스캔 집합으로 local map $m_{\text{local}}$을 구성하고, 이것을 global map $m$과 정규화 상관계수 $\rho$로 비교한다. PR §6.5는 이 비교 결과를 그대로 likelihood로 사용한다:

$$p(m_{\text{local}} \mid x_t, m) = \max\{\rho(m_{\text{local}}, m \mid x_t),\ 0\}$$

$\rho$는 두 맵을 $x_t$로 정렬했을 때 대응하는 셀들 사이의 피어슨 상관계수다. 계산이 빠르고 구현이 단순하다. 다만 이 likelihood는 확률론적으로 정당화되지 않는다. $\rho$는 정규화되어 있고, 0보다 작은 값은 0으로 잘라버린다. SLAM 백엔드의 loop closure 검출처럼 정확한 likelihood보다 빠른 유사도 점수가 필요한 경우에 쓰인다.

마지막 모델은 raw 거리 측정값 대신 센서 데이터에서 추출한 구조적 특징을 다룬다.

### 2.7.7 특징 기반 측정 — 랜드마크 모델

랜드마크 모델은 센서 데이터에서 추출한 특징 $f(z_t)$를 다룬다. 저차원 특징으로 추론하므로 연산량이 작고, feature-based 지도와 자연스럽게 연결된다.

특징 추출의 형태는 센서마다 다르다. 거리 스캔에서는 선분, 코너, 국소 극솟값. 카메라에서는 edge, corner, SIFT/ORB 같은 국소 패턴 (§2.1.1 단안 카메라의 texture 특성, §2.6 VIO/Visual SLAM 참조). 추출된 각 특징은 $(r, \phi, s)$ 삼중항으로 표현된다: $r$은 range, $\phi$는 bearing, $s$는 signature(ID, 색상, 디스크립터 등).

지도의 $j$번째 랜드마크가 좌표 $(m_{j,x}, m_{j,y})$에 있고 signature $s_j$를 갖는다. 포즈 $x_t = (x, y, \theta)^T$에서의 예측 측정과 실제 측정 사이의 관계 (PR 식 6.41):

$$\begin{pmatrix} r_t^i \\ \phi_t^i \\ s_t^i \end{pmatrix} = \begin{pmatrix} \sqrt{(m_{j,x} - x)^2 + (m_{j,y} - y)^2} \\ \operatorname{atan2}(m_{j,y} - y,\, m_{j,x} - x) - \theta \\ s_j \end{pmatrix} + \begin{pmatrix} \varepsilon_{\sigma_r^2} \\ \varepsilon_{\sigma_\phi^2} \\ \varepsilon_{\sigma_s^2} \end{pmatrix}$$

$\varepsilon_{\sigma^2}$는 0-평균 분산 $\sigma^2$ 가우시안이다. 세 채널이 독립 가우시안 잡음을 갖는다는 가정이다. bearing 채널 $\varepsilon_{\sigma_\phi^2}$에 가우시안을 직접 더하는 이 모델은 $\pm\pi$ 근방에서 wrap-around 오류를 낼 수 있다. 실전 구현에서는 각도 차이를 $[-\pi, \pi]$로 정규화하거나 von Mises 분포로 대체한다.

대응 $c_t^i = j$($i$번째 특징이 $j$번째 랜드마크에 대응)가 알려진 경우의 likelihood는 세 채널 가우시안의 곱이다.

**알고리즘: landmark_model_known_correspondence** (PR Table 6.4 의역)

```
입력: f_t^i = (r_t^i, φ_t^i, s_t^i)^T, 대응 c_t^i = j, x_t = (x, y, θ)^T, m
출력: p(f_t^i | c_t^i = j, x_t, m)

1. j ← c_t^i
2. r̂ ← sqrt((m_{j,x} - x)² + (m_{j,y} - y)²)
3. φ̂ ← atan2(m_{j,y} - y, m_{j,x} - x) - θ
4. q ← prob(r_t^i - r̂, σ_r²)
       * prob(φ_t^i - φ̂, σ_φ²)
       * prob(s_t^i - s_j, σ_s²)
5. return q
   // prob(a, σ²) = N(a; 0, σ²) — 0-평균 가우시안 밀도
```

전체 스캔의 특징들 사이 조건부 독립을 가정하면 전체 likelihood는 $\prod_i$다.

**역방향 — 포즈 샘플링** (PR Table 6.5 압축): 측정으로부터 가능한 포즈를 샘플링하는 방향도 존재한다. 하나의 $(r, \phi)$ 측정은 포즈 공간에서 두 제약만 주므로, 가능한 포즈들은 랜드마크를 중심으로 한 원(2D) 또는 나선(3D) 위에 분포한다. 자유 파라미터 $\hat{\gamma} \sim U(0, 2\pi)$로 원 상의 위치를 샘플링한다. 랜드마크 하나를 한 번만 보면 위치를 특정할 수 없다는 사실의 기하학적 설명이기도 하다.

Visual SLAM의 reprojection residual $\| \pi(K[R|t]\, X_w) - u \|^2_\Sigma$도 pose와 landmark로 관측을 예측하고 실제 관측과 비교한다는 공통 구조를 가진다. 다만 pixel reprojection model과 range-bearing model은 서로 다른 sensor model이고, ORB/SIFT descriptor는 보통 likelihood의 연속 signature 항이라기보다 data association에 쓰인다. AprilTag·ArUco의 ID는 대응 모호성을 크게 줄이지만 false detection과 잘못 읽힌 ID까지 원천적으로 배제하지는 않는다.

### 2.7.8 실무 정리: 모델 선택

4가족 모델을 정성적으로 비교하면 다음과 같다. 정확도와 속도는 sensor, map resolution, implementation과 parameter에 따라 달라진다.

| 모델 | 정확도 | 계산 속도 | 미분 가능성 | 주요 용도 |
|------|--------|-----------|------------|-----------|
| 빔 모델 | 높음 | 느림 (ray casting) | 낮음 (불연속) | MCL high-fidelity, 진단 |
| Likelihood field | 중간 | 빠름 (DT lookup) | 높음 | AMCL 기본, gradient 정합 |
| 상관 기반 | 낮음 | 매우 빠름 | 낮음 | loop closure 검출 |
| 랜드마크 | 높음 (특징 의존) | 빠름 (저차원) | 높음 | visual SLAM, fiducial |

over-confidence도 실용적 고려사항이다. 빔들 사이의 조건부 독립 가정이 위반되면 likelihood가 특정 pose에 지나치게 좁게 모일 수 있다. §2.7.4처럼 $p(z_t \mid x_t, m)^{\alpha}$ ($\alpha < 1$)로 tempering하면 각 scan의 영향이 줄어 분포가 평탄해진다. Beam subsampling, correlation을 반영한 model, robust likelihood도 대안이며 $\alpha$는 calibration·validation data로 정해야 한다.

그렇다면 이 모델들이 실제 시스템에서 얼마나 살아남았는가.

### 2.7.9 무엇이 살아남았나

PR §6의 네 가족은 오늘날 시스템을 분류하고 설계할 때도 유용하다. 다만 최신 scan matcher나 visual SLAM을 이 모델들의 **직계 후손**으로 묶으면 계보를 과장하게 된다. 같은 관측-예측 비교 구조를 공유하더라도 목적함수와 지도 표현은 서로 다를 수 있다.

[Nav2 AMCL 문서](https://docs.nav2.org/configuration/packages/configuring-amcl.html)는 `beam`, `likelihood_field`, `likelihood_field_prob` 세 laser model을 제공하고, 기본값은 `likelihood_field`로 둔다. `max_beams`는 한 scan에서 균등 간격으로 사용할 빔 수를 정한다. 이와 달리 `beam_skip_*`는 `likelihood_field_prob`에서 많은 particle과 맞지 않는 빔을 건너뛰는 기능이다. 따라서 단순한 빔 subsampling과 같은 항목이 아니다. `sigma_hit`, `lambda_short` 같은 기본값도 구현의 초기값이지, 특정 EM 실험에서 보편적으로 수렴한 값이라고 단정할 근거는 없다.

다른 LiDAR 시스템은 별도의 정합 목적함수를 쓴다. Cartographer는 probability grid 위의 correlative scan matching과 비선형 최적화를 결합하고, `hdl_localization`은 3D point cloud에 NDT/GICP 계열 정합을 사용한다. 이들은 모두 관측을 지도와 비교한다는 넓은 원리는 공유하지만, likelihood-field distance transform을 그대로 쓴다고 볼 수는 없다. ESDF 경로 계획과 neural implicit map도 거리 또는 implicit field를 사용하지만, 자료구조가 비슷하다는 이유만으로 likelihood-field sensor model의 계승 관계를 주장할 수는 없다.

랜드마크 모델과 visual SLAM도 같은 구분이 필요하다. 식 6.41의 range-bearing 생성 모델, 카메라 reprojection model, DROID-SLAM의 dense bundle adjustment는 모두 pose와 scene structure에서 관측을 예측해 residual을 만든다. 그러나 측정 공간, association, noise model, optimization 변수가 다르므로 하나를 다른 하나의 역사적 일반화라고 단정하지 않는다. Fiducial ID는 association을 단순하게 만들지만 false detection까지 없애지는 않는다.

`hit·short·max·rand`는 센서의 물리적 하드웨어 채널이라기보다 range measurement가 생긴 원인을 근사하는 혼합 성분이다. 어떤 성분과 지도 표현을 쓸지는 센서 물리뿐 아니라 환경, outlier, 계산 예산, calibration data에 따라 정한다. 이 장에서 정리한 네 가지 모델군은 선택지를 비교하는 실질적 출발선이며, 모든 최신 시스템을 단순하게 엮는 획일적 계보도로 볼 수는 없다.

Ch.14 §14.7에서 MCL의 `beam_range_finder_model` 호출과 occupancy mapping의 `inverse_sensor_model`을 보면, 이 모델들이 어떻게 연결되는지 확인할 수 있다.

> **⚠ 센서 연결 점검**: 센서 데이터가 들어오지 않을 때는 드라이버와 함께 케이블, IP 설정, 전원, USB 대역폭도 확인한다. `dmesg`, `lsusb`, `ping` 같은 시스템 명령으로 장치와 연결 상태를 먼저 기록하면 원인 범위를 빠르게 좁힐 수 있다.

> **기술 흐름: 센서 기술**
> - **~2010**: 이동 로봇 연구에서는 2D LiDAR와 frame camera가 널리 쓰였다. Stereo의 실시간 처리 범위는 당시의 연산 자원과 장면 조건에 크게 좌우됐다.
> - **2010년대**: RGB-D camera와 소형 multi-beam 3D LiDAR가 보급되면서 실내 3D 인식과 야외 mapping의 선택지가 넓어졌다. Camera와 IMU를 결합한 VIO도 연구 prototype을 넘어 여러 로봇·AR 시스템에 쓰이기 시작했다.
> - **2010년대 후반~2020년대 초반**: 비반복 주사·MEMS·flash 등 서로 다른 방식이 `solid-state LiDAR`라는 이름 아래 등장했고, event camera와 automotive imaging radar 연구도 확대됐다. 가격과 성능의 변화 폭은 제품군마다 달라 하나의 수치로 일반화하기 어렵다.
> - **2020년대**: Spinning LiDAR와 solid-state 계열은 대체 관계 하나로 정리되지 않고 field of view, range, resolution, motion distortion, cost 조건에 따라 공존한다. Event camera와 Doppler radar의 채택 역시 고속·HDR·악천후 같은 응용 조건에 따라 달라진다.
> - **설계상의 함의**: 센서의 주사 방식과 timestamp 구조가 바뀌면 deskew, calibration, data association의 가정도 함께 바뀐다. 하드웨어 이름보다 실제 sampling geometry와 noise 특성을 먼저 확인해야 한다.

---

# Ch.3 — 수학적 기초 (Mathematical Foundations)

Spatial AI 논문을 읽고 구현하려면 수학적 기초가 필요하다. SLAM 논문의 "SE(3) 위의 최적화"나 "Jacobian을 유도하여 Gauss-Newton으로 풀었다"는 문장을 해석하지 못하면 방법론을 따라가기 어렵다. 여기서 다루는 수학은 단순한 수식 유도를 넘어 로봇 논문을 깊이 이해하고 코드로 구현하기 위한 실전 도구다. 학부 선형대수에서 배운 개념을 로보틱스에 어떻게 연결하는지에 초점을 둔다.

Differentiable Programming과 Auto-Differentiation(자동 미분)은 고전적인 수학 도구를 계산 파이프라인 안에 넣는 방식을 바꿨다. PyTorch나 JAX는 손으로 유도하던 Jacobian과 복잡한 파이프라인의 gradient를 자동으로 계산한다. 이 기능은 End-to-End 학습 기반 SLAM과 Differentiable Rendering(NeRF, 3D Gaussian Splatting)의 기반이 된다. 다만 자동 미분이 계산하는 내용을 해석하고 오류를 찾으려면 이 장에서 다루는 선형대수와 최적화가 필요하다.

## 3.1 선형대수 (Linear Algebra)

선형대수는 Spatial AI 전반에서 쓰이는 기본 도구다. 좌표 변환, 카메라 모델, 최적화, 딥러닝은 모두 행렬과 벡터로 표현된다. 이 절은 학부에서 배운 정의를 로보틱스의 계산에 연결한다.

### 3.1.1 벡터와 행렬

**벡터**: 크기와 방향을 가진 양

```
v = [v_x, v_y, v_z]^T  (열 벡터)
```

로보틱스에서 벡터는 3D 공간의 점, 힘, 속도 등을 나타낸다. "로봇이 월드 좌표계에서 (3, 2, 1)에 있다"는 것은 위치를 벡터로 표현한 것이다.

**행렬 연산**:
- 덧셈/뺄셈: 요소별 연산
- 곱셈: 행×열 내적
- 전치(Transpose): A^T
- 역행렬(Inverse): A^(-1), AA^(-1) = I

좌표 변환, 회전, 투영(projection) 전부 행렬 곱으로 표현된다. 카메라가 3D 점을 2D 이미지로 투영하는 것도, 로봇의 좌표계를 변환하는 것도 전부 행렬 곱이다.

> **추천 자료**
> - [3Blue1Brown — Essence of Linear Algebra](https://www.youtube.com/playlist?list=PLZHQObOWTQDPD3MizzM2xVFitgF8hE_ab) — 행렬 곱셈과 고유값을 기하학적으로 시각화한다. 선형대수를 계산 규칙보다 공간 변환으로 이해하는 데 유용하다.
> - [Introduction to Applied Linear Algebra (Boyd & Vandenberghe) — 무료 PDF](https://web.stanford.edu/~boyd/vmls/) — Stanford의 Boyd 교수가 쓴 응용 선형대수 교재. 실용적 관점, Python 예제 포함.
> - [다크 프로그래머 — 선형대수학 시리즈 (6편: 기본공식~PCA)](https://darkpgmr.tistory.com/103) — 주요용어, 역행렬, 고유값, SVD, 연립방정식, PCA를 한글로 정리
> - [다크 프로그래머 — 벡터 미분과 행렬 미분](https://darkpgmr.tistory.com/141) — 벡터/행렬 미분 규칙 정리. Jacobian 계산에 필요한 기초

### 3.1.2 고유값 분해 (Eigenvalue Decomposition)

```
Av = λv
```

- v: 고유벡터 (eigenvector)
- λ: 고유값 (eigenvalue)

**활용**: PCA, 공분산 행렬 분석, 안정성 분석

포인트 클라우드를 다루면 바로 쓸 일이 생긴다. PCA(Principal Component Analysis)로 포인트 클라우드의 주축을 구할 때, 공분산 행렬의 고유벡터가 바로 주축 방향이고 고유값이 그 방향의 분산이다. "이 포인트 클라우드가 평면인지 직선인지"를 판별하는 것도 고유값의 비율로 한다. Normal 벡터 추정도 가장 작은 고유값에 대응하는 고유벡터를 사용한다.

> **추천 자료**
> - [3Blue1Brown — Eigenvectors and Eigenvalues](https://www.youtube.com/watch?v=PFDu9oVAE-g) — 고유값의 기하학적 의미를 직관적으로 설명
> - [MIT 18.06 Linear Algebra — Gilbert Strang (YouTube)](https://www.youtube.com/playlist?list=PLE7DDD91010BC51F8) — MIT 18.06 공개 강의. 고유값 분해를 포함한 전체 선형대수를 깊이 있게 다룬다.

> **실습**: [PCA 3D · 차원 축소](https://alexjunholee.github.io/robotics-practice/app.html#pca_3d)
> 3D 분포에서 공분산 행렬의 고유벡터가 주축이 되는 과정을 직접 조작하고, PC1·PC2 평면(3D→2D)과 PC1 축(2D→1D)으로의 차원 축소를 동시에 시각화한다.

### 3.1.3 특이값 분해 (SVD: Singular Value Decomposition)

```
A = UΣV^T
```

- U: 좌측 특이벡터 (m×m 직교행렬)
- Σ: 특이값 대각행렬 (m×n)
- V: 우측 특이벡터 (n×n 직교행렬)

**활용**: 최소자승법 해, 행렬 근사, Fundamental Matrix 계산

SVD는 로보틱스에서 자주 등장한다. 과결정(overdetermined) 시스템의 최소자승 해를 구하는 데 수치적으로 안정적이기 때문이다. 카메라 캘리브레이션에서 Fundamental Matrix를 구할 때나 포인트 클라우드 정합에서 최적 변환을 구할 때 모두 SVD를 사용한다. "8-point algorithm"에서 8개 이상의 대응점으로 Fundamental Matrix를 구하는 마지막 단계도 SVD다.

> **추천 자료**
> - [Steve Brunton — Singular Value Decomposition (YouTube)](https://www.youtube.com/watch?v=nbBvuuNVfco) — SVD의 수학적 의미와 응용을 명쾌하게 설명하는 워싱턴 대학교 교수의 강의
> - [Linear Algebra and Its Applications (Gilbert Strang)](https://math.mit.edu/~gs/linearalgebra/ila6/indexila6.html) — 선형대수 표준 교재. SVD 챕터가 특히 잘 쓰여 있다.

## 3.2 3D 기하학 (3D Geometry)

3D 기하학은 로봇, 카메라, 물체의 위치와 방향을 같은 수학적 언어로 표현한다. SLAM은 이 표현 위에서 여러 시점의 관측을 연결한다.

### 3.2.1 좌표계 (Coordinate Frames)

Spatial AI에서는 여러 좌표계를 오가며 작업한다. World Frame(W)은 전역 고정 좌표계를 뜻하며, Camera Frame(C)은 카메라 중심, Body Frame(B)은 로봇 중심, IMU Frame(I)은 IMU 센서 기준 좌표계를 나타낸다.

카메라가 관측한 물체 위치는 camera frame에 표현된다. 로봇이 그 위치를 사용하려면 body frame이나 world frame으로 변환해야 한다. 센서마다 좌표계가 다르므로, sensor fusion은 좌표계 사이의 extrinsic calibration을 사용한다.

**좌표 변환**:

```
p_W = T_WC × p_C
```

T_WC: Camera → World 변환 행렬 (4×4)

> **추천 자료**
> - [State Estimation for Robotics, Ch.6 — Coordinate Frames (Tim Barfoot) — 무료 PDF](http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17.pdf) — 좌표계 변환을 로보틱스 상태 추정 관점에서 정리한 교재
> - [Stanford CS231A — Camera Models](https://web.stanford.edu/class/cs231a/) — Stanford의 CV 강의에서 카메라 좌표계와 투영 모델을 다루는 부분

### 3.2.2 회전 표현 (Rotation Representations)

회전 행렬, Euler angle, quaternion, axis-angle은 파라미터 수, 제약, 특이점, 보간 방식이 다르다. SLAM 코드가 표현을 고르는 이유는 이 차이와 최적화 방식에 있다.

**Rotation Matrix R**은 3×3 직교행렬(det(R) = 1, R^T = R^(-1))로, 9개 파라미터에 6개 제약이 걸려 실제 자유도는 3이다.

**Euler Angles**은 Roll(φ), Pitch(θ), Yaw(ψ) 세 각도로 회전을 표현한다. 직관적이지만 Gimbal Lock 문제가 있고, 적용 순서(ZYX, XYZ 등)에 따라 결과가 달라진다.

**Quaternion q = [w, x, y, z]**(||q|| = 1)는 4개 파라미터로 3 DoF를 표현한다. Gimbal Lock이 없고 보간(Slerp)이 용이해 로보틱스의 자세 표현에 널리 쓰인다.

**Axis-Angle**은 회전축 n과 회전각 θ를 조합한 3개 파라미터 표현이다. Rodrigues formula를 통해 Rotation Matrix로 변환된다.

ROS는 quaternion을 기본 회전 표현으로 쓰고, OpenCV는 Rodrigues vector(axis-angle)를 제공하며, Ceres와 GTSAM 같은 최적화 라이브러리는 Lie group 기반 표현을 지원한다. 인터페이스를 연결할 때는 각 표현의 좌표 순서와 정규화 조건을 확인한다.

> **추천 자료**
> - [3Blue1Brown — Quaternions and 3D Rotation](https://www.youtube.com/watch?v=zjMuIxRvygQ) — 쿼터니언의 기하학적 의미를 시각화한 영상. 4차원이 왜 3D 회전에 필요한지 직관적으로 이해할 수 있다.
> - [State Estimation for Robotics, Ch.7 — Rotation (Tim Barfoot)](http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17.pdf) — 모든 회전 표현과 그들 사이의 변환을 깔끔하게 정리한 교재
> - [Sola — Quaternion Kinematics for the Error-State Kalman Filter (Tech Report)](https://arxiv.org/abs/1711.02508) — VIO/INS 구현 시 쿼터니언 기반 에러 상태 칼만 필터의 수학적 기초를 정리한 테크니컬 리포트.
> - [3D Rotation Converter](https://www.andre-gaschler.com/rotationconverter/) — 쿼터니언, 오일러 각, 회전 행렬 간 변환을 확인할 수 있는 온라인 도구

> **실습**: [회전 표현과 Gimbal Lock](https://alexjunholee.github.io/robotics-practice/app.html#rotation_gimbal) | [6DoF 포즈 시각화](https://alexjunholee.github.io/robotics-practice/app.html#xyzrpy_6dof)
> 오일러 각의 Gimbal Lock 현상과 쿼터니언 회전을 직접 조작하며 비교하고, 6자유도 포즈(x, y, z, roll, pitch, yaw)를 인터랙티브하게 확인할 수 있다.

### 3.2.3 Homogeneous Coordinates

3D 점을 4D로 확장하여 변환을 단일 행렬로 표현:

```
[X, Y, Z, 1]^T  (3D 점)

T = | R   t |   (4×4 변환 행렬)
    | 0   1 |
```

Homogeneous Coordinates를 쓰는 이유: 회전과 이동(translation)을 하나의 행렬 곱으로 표현할 수 있기 때문이다. 일반 좌표에서는 p' = Rp + t (곱셈 + 덧셈)이지만, Homogeneous Coordinates에서는 p' = Tp (곱셈만)로 쓸 수 있다. 여러 변환을 연쇄적으로 적용할 때 행렬을 그냥 곱하면 되어, 로봇 팔의 관절 변환 같은 체인을 다룰 때 편리하다.

### 3.2.4 SE(3)와 SO(3)

**SE(3)**(Special Euclidean Group)는 3D 강체 변환(회전 + 이동) 전체의 집합으로 6 DoF를 가진다. **SO(3)**(Special Orthogonal Group)는 회전만의 집합으로 3 DoF다.

SE(3)와 SO(3)는 **Lie Group**이다. 최적화를 할 때 "회전 행렬의 제약조건(직교, 행렬식 1)을 만족하면서 업데이트"해야 하는데, Lie Group 이론이 이를 우아하게 해결한다. 대응되는 **Lie Algebra** (se(3), so(3))에서 제약 없이 최적화한 후 Exponential Map으로 다시 Lie Group으로 매핑하는 방식이다. SLAM의 Pose Graph Optimization에서 이 개념이 핵심으로 쓰인다.

> **추천 자료**
> - [State Estimation for Robotics, Ch.7 (Tim Barfoot) — 무료 PDF](http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17.pdf) — SE(3), SO(3), Lie group·algebra를 로보틱스 상태 추정 관점에서 설명한 교재
> - [Sola — A Micro Lie Theory for State Estimation in Robotics (arXiv:1812.01537)](https://arxiv.org/abs/1812.01537) — Lie Group 이론을 로보틱스 상태 추정에 필요한 만큼만 간결하게 정리한 논문. 매우 실용적.

## 3.3 확률 및 통계 (Probability & Statistics)

센서 관측치에는 필연적으로 노이즈가 섞이며, 로봇의 추정 상태 역시 언제나 불확실성을 수반한다. 이 불확실성을 수학적으로 표현하고 다루는 것이 확률과 통계다. "센서 값이 정확히 3.0 m" 대신 "3.0 m ± 0.05 m (95% 신뢰구간)"처럼 통계적 분포로 다루어야 비로소 의미가 있으며, 이 불확실성을 전파하고 업데이트하는 것이 상태 추정의 기본이다.

### 3.3.1 정규분포 (Gaussian Distribution)

```
p(x) = (1 / √(2πσ²)) × exp(-(x-μ)²/(2σ²))
```

**다변량 정규분포**:

```
p(x) = N(μ, Σ)
```

- μ: 평균 벡터
- Σ: 공분산 행렬

센서 노이즈, 위치 불확실성 모델링에 쓰인다.

정규분포가 이렇게까지 많이 쓰이는 이유는 수학적 편의성이다. 중심극한정리는 적절한 조건에서 많은 작은 독립 효과의 합이 정규분포로 근사되는 이유를 설명한다. 독립 정규 확률변수의 합은 정규분포이고, 같은 변수에 대한 정규 밀도의 곱도 정규화하면 정규 밀도가 되어 분석이 쉽다. 칼만 필터가 정규분포를 가정하는 것도 같은 이유다.

> **추천 자료**
> - [3Blue1Brown — But what is the Central Limit Theorem?](https://www.youtube.com/watch?v=zeJD6dqJ5lo) — 중심극한정리를 시각적으로 설명. 왜 정규분포가 어디에나 나타나는지 직관적으로 이해할 수 있다.
> - [Kalman Filter — How it works, in pictures](http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/) — 칼만 필터의 작동 원리를 시각적으로 설명. 수식 전에 직관을 잡기 좋다

> **실습**: [Kalman Filter](https://alexjunholee.github.io/robotics-practice/app.html#kalman_filter)
> 칼만 필터의 predict-update 사이클을 인터랙티브하게 조작하며, 정규분포 기반 상태 추정 과정을 확인할 수 있다.

**Mahalanobis Distance**

유클리드 거리는 모든 방향을 동등하게 취급한다. 하지만 센서 데이터는 방향에 따라 불확실성이 다르다. 예를 들어 GPS는 수평 방향(수 미터)보다 수직 방향(수십 미터)의 오차가 크다.

Mahalanobis 거리는 공분산(covariance)을 고려한 거리이다:

```
d_M = sqrt((x - μ)^T Σ^{-1} (x - μ))
```

Σ가 단위 행렬이면 유클리드 거리와 같다. Σ가 대각 행렬이면 각 축별로 스케일링된 거리이다. 일반적인 Σ에서는 공분산의 주축 방향으로 거리가 재정의된다.

SLAM에서의 활용: 데이터 연관(data association) 시 "이 관측이 이 랜드마크에서 왔는가?"를 판단할 때 Mahalanobis 거리를 쓴다. 유클리드로 가까워도 Mahalanobis로 멀면 (불확실성 방향과 맞지 않으면) 잘못된 연관일 가능성이 높다.

(참고: [다크 프로그래머 — 평균, 표준편차, 분산, 그리고 Mahalanobis 거리](https://darkpgmr.tistory.com/41))

### 3.3.2 베이즈 정리 (Bayes' Rule)

```
P(A|B) = P(B|A) × P(A) / P(B)
```

베이즈 정리는 센서 측정이 주어졌을 때 상태에 대한 확률을 갱신한다. 칼만 필터와 파티클 필터는 이 갱신을 서로 다른 분포 표현과 근사로 구현한다.

**재귀적 상태 추정**:

```
P(x_t | z_{1:t}) ∝ P(z_t | x_t) × P(x_t | z_{1:t-1})
```

- P(z_t | x_t): Measurement model (관측 모델) — "로봇이 이 위치에 있다면, 센서가 이 값을 출력할 확률은?"
- P(x_t | z_{1:t-1}): Prior (이전 상태 기반 예측) — "이전까지의 정보로 볼 때 로봇이 여기 있을 확률은?"

새 센서 데이터가 들어올 때마다 prior를 measurement likelihood와 결합해 posterior를 구한다. 칼만 필터의 predict-update cycle은 선형 Gaussian 조건에서 이 재귀를 구현한다.

베이즈 정리를 시간에 대해 *재귀적으로* 적용하면 §3.9의 베이즈 필터가 된다.

> **추천 자료**
> - [3Blue1Brown — Bayes' Theorem](https://www.youtube.com/watch?v=HZGCoVF3YvM) — 베이즈 정리를 시각적으로 이해하기 좋은 영상
> - [Probabilistic Robotics (Thrun, Burgard, Fox)](https://www.probabilistic-robotics.org/) — 확률적 로보틱스의 필수 교재. 베이즈 필터, 칼만 필터, 파티클 필터, SLAM까지 확률론적 관점에서 깔끔하게 정리한 교과서다.
> - [김기섭 블로그 — Bayesian Filtering 시리즈 (2편)](https://gisbi-kim.github.io/blog/2021/03/09/bayesfiltering-1.html) — 베이즈 필터의 한글 해설. 칼만 필터로 이어지는 기초

### 3.3.3 MLE와 MAP

**MLE (Maximum Likelihood Estimation)**:

```
x* = argmax P(z | x)
```

데이터가 주어졌을 때 가장 가능성 높은 파라미터를 구한다.

**MAP (Maximum A Posteriori)**:

```
x* = argmax P(x | z) = argmax P(z | x) × P(x)
```

사전 확률(prior)을 고려해 추정한다.

SLAM에서 "관측 데이터만 보고 최적 위치를 구하는 것(MLE)"과 "이전 위치 정보도 함께 고려하여 최적 위치를 구하는 것(MAP)"의 차이다. 실제 SLAM 시스템은 대부분 MAP 추정을 쓴다. Prior를 넣으면 노이즈가 심한 관측에도 안정적으로 추정할 수 있기 때문이다. 관측 노이즈가 가우시안이고 공분산이 고정되어 있으면, 음의 로그 사후확률을 최소화하는 MAP 문제는 "가중 관측 오차의 제곱합 + 음의 로그 사전확률에 따른 정규화 항"이 되어, 최적화 관점에서 Regularized Least Squares와 같은 형태가 된다.

> **추천 자료**
> - [Probabilistic Robotics, Ch.2 — Recursive State Estimation (Thrun)](https://www.probabilistic-robotics.org/) — MLE, MAP, 베이즈 필터의 관계를 로보틱스 맥락에서 설명
> - [Cyrill Stachniss — Maximum Likelihood and MAP Estimation](https://www.youtube.com/watch?v=XepXtl9YKwc) — MLE와 MAP의 차이를 예시와 함께 명쾌하게 설명

**MLE와 MAP의 직관적 차이**

둘 다 "가장 그럴듯한 파라미터를 찾는다"는 목표는 같지만 접근 방식은 다르다.

MLE(Maximum Likelihood)는 "이 데이터가 관측될 확률을 가장 높이는 파라미터는?"이라고 묻는다. 데이터만 본다. MAP(Maximum A Posteriori)는 여기에 prior를 더한다. "이 데이터가 관측되었을 때, 사전 지식까지 합쳐서 파라미터의 사후 확률을 가장 높이는 값은?"이 그 질문이다.

수식으로: MAP = MLE + prior. 가우시안 prior를 쓰면 MAP은 MLE에 L2 정규화를 추가한 것과 같다. 딥러닝에서 weight decay가 MAP의 구현이라고 볼 수 있다.

SLAM에서: odometry 측정값의 likelihood와 센서 관측의 likelihood를 곱하고, 이전 상태의 prior를 결합하여 MAP 추정을 한다. Factor graph에서 각 factor가 바로 이 likelihood/prior에 해당한다.

(참고: [다크 프로그래머 — 베이즈 정리, ML과 MAP, 그리고 영상처리](https://darkpgmr.tistory.com/62))

## 3.4 최적화 기초 (Optimization Basics)

SLAM의 bundle adjustment, 카메라 calibration, 딥러닝 학습은 모두 목적함수를 최소화한다. 잔차, Jacobian, 갱신 규칙을 구분하면 수렴 실패가 모델·초기값·solver 가운데 어디에서 생겼는지 추적할 수 있다.

### 3.4.1 Least Squares

```
x* = argmin ||Ax - b||²
```

**정규방정식의 해 (Normal Equation, `A`가 full column rank인 경우)**:

```
x* = (A^T A)^(-1) A^T b
```

최소자승법은 "노이즈가 있는 여러 측정값에서 가장 적합한 모델 파라미터를 찾는" 가장 기본적인 방법이다. 직선 피팅부터 카메라 캘리브레이션까지 추정 문제 대부분의 출발점이다.

> **추천 자료**
> - [Cyrill Stachniss — Least Squares for Robotics](https://www.youtube.com/watch?v=r2cyMQ5NB1o) — 최소자승법을 로보틱스 문제에 적용하는 방법을 구체적으로 설명
> - [김기섭 블로그 — SLAM back-end 시리즈 (3편)](https://gisbi-kim.github.io/blog/2021/03/04/slambackend-1.html) — "SLAM은 Ax=b를 푸는 문제"에서 시작하는 back-end 입문. Factor graph까지 3편 시리즈
> - [김기섭 블로그 — Iterative Optimization 1편](https://gisbi-kim.github.io/blog/2021/03/16/leastsquare-1.html) — 비선형 최적화의 직관적 한글 해설

**최소자승법의 직관**

"왜 오차의 제곱을 최소화하는가?" 단순 절댓값 대신 제곱 오차를 택하는 이유는 크게 두 가지다. 미분이 가능하고 큰 오차에 더 큰 페널티를 준다. 부수적으로 가우시안 노이즈 가정 하에서 Maximum Likelihood Estimation과 동일한 해를 준다는 성질도 있다.

over-determined system (방정식 수 > 미지수 수)에서는 Ax = b를 정확히 만족하는 x가 없을 수 있다. 대신 ||Ax - b||²를 최소화하는 x를 찾으면, normal equation `A^T A x = A^T b`가 된다. 이것이 최소자승법의 전부다.

주의: `A^T A`가 singular하면 (rank 부족) 위의 역행렬은 존재하지 않고, 최소자승 해도 유일하지 않을 수 있다. `A = U Σ V^T`를 SVD했을 때 Moore–Penrose pseudo-inverse를 `A^+ = V Σ^+ U^T`로 구하면, 최소 norm 해 `x = A^+ b`를 얻을 수 있다.

(참고: [다크 프로그래머 — 최소자승법 이해와 다양한 활용예](https://darkpgmr.tistory.com/56))

### 3.4.2 Gradient Descent

```
x_{k+1} = x_k - α × ∇f(x_k)
```

- α: Learning rate
- ∇f: Gradient (기울기)

Gradient Descent는 딥러닝에서 매일 쓰는 알고리즘이지만, 로보틱스 최적화에서도 기본이 된다. 함수의 기울기 반대 방향으로 조금씩 이동하여 최솟값을 찾는 직관적인 방법이다. 다만 learning rate 설정이 어렵고, local minimum에 빠질 수 있으며, 수렴 속도가 느리다는 한계가 있어서, 로보틱스에서는 보통 더 효율적인 방법(Gauss-Newton, LM)을 사용한다.

**Gradient, Jacobian, Hessian**

**Gradient** ∇f는 스칼라 함수 f의 1차 미분으로 n×1 벡터를 출력한다. "어느 방향으로 가야 f가 가장 빠르게 증가하는가"를 알려준다. **Jacobian** J는 이를 벡터 함수 f: R^n → R^m으로 확장한 것으로 m×n 행렬이다. 각 출력의 각 입력에 대한 편미분을 담는다. **Hessian** H는 스칼라 함수 f의 2차 미분으로 n×n 대칭 행렬이며, 곡률 정보를 담아 Newton's method에서 사용한다.

관계:
```
비용 함수 C(x) = (1/2)||r(x)||² 일 때:
  Gradient:  ∇C = J^T r           (J는 r의 Jacobian)
  Hessian:   H ≈ J^T J            (Gauss-Newton 근사: 2차 미분 항 무시)
  Update:    δx = -(J^T J)^{-1} J^T r
```

Gauss-Newton이 J^T J를 Hessian 근사로 쓰는 이유: 정확한 Hessian은 계산이 비싸고, 잔차 r이 작은 영역에서는 2차 항이 무시할 수 있을 만큼 작기 때문이다.

(참고: [다크 프로그래머 — Gradient, Jacobian 행렬, Hessian 행렬, Laplacian](https://darkpgmr.tistory.com/132))

### 3.4.3 Gauss-Newton

비선형 최소자승 문제를 반복적으로 선형화하여 해결:

```
(J^T J) Δx = -J^T r
x_{k+1} = x_k + Δx
```

- J: Jacobian 행렬
- r: Residual (잔차)

Gauss-Newton이 로보틱스에서 Gradient Descent보다 선호되는 이유는 2차 정보(Hessian의 근사인 J^T J)를 사용해 더 빨리 수렴하기 때문이다. SLAM에서 수천~수만 개의 변수를 최적화할 때 Gradient Descent는 수렴에 오래 걸리지만, Gauss-Newton은 몇 번의 반복으로 수렴할 수 있다.

### 3.4.4 Levenberg-Marquardt (LM)

Gauss-Newton과 Gradient Descent의 결합:

```
(J^T J + λI) Δx = -J^T r
```

- λ: Damping factor
- λ 작음 → Gauss-Newton (빠른 수렴)
- λ 큼 → Gradient Descent (안정적)

SLAM의 bundle adjustment와 pose graph optimization에 쓰인다.

Gauss-Newton은 초기값이 좋을 때 빠르게 수렴하지만, 그렇지 않으면 발산할 수 있다. LM은 λ를 조절해 큰 damping에서는 gradient descent에 가까운 갱신을, 작은 damping에서는 Gauss-Newton에 가까운 갱신을 만든다. Ceres Solver, g2o, GTSAM 같은 최적화 라이브러리가 이 방식을 제공한다.

> **추천 자료**
> - [Cyrill Stachniss — Gauss-Newton and Levenberg-Marquardt for SLAM](https://www.youtube.com/watch?v=hRyL5KwFLAE) — SLAM에서 Gauss-Newton과 LM이 어떻게 사용되는지 단계별로 설명
> - [State Estimation for Robotics, Ch.4 — Nonlinear Optimization (Tim Barfoot) — 무료 PDF](http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17.pdf) — 비선형 최적화를 로보틱스 상태 추정 관점에서 잘 설명한다
> - [Ceres Solver Tutorial](http://ceres-solver.org/tutorial.html) — Google의 비선형 최소자승 최적화 라이브러리. 실제로 LM 알고리즘을 코드로 어떻게 사용하는지 실습할 수 있다.
> - [다크 프로그래머 — 최적화 기법의 직관적 이해](https://darkpgmr.tistory.com/149) — Gradient Descent, Newton, LM 등의 기하학적 직관
> - [김기섭 블로그 — SLAM Back-end 공부자료 5개 추천](https://gisbi-kim.github.io/blog/2021/10/03/slam-textbooks.html) — Error-state KF, Factor Graphs, Bundle Adjustment 등 핵심 자료 큐레이션
> - [Derivative Calculator](https://www.derivative-calculator.net/) — 수식 미분을 단계별로 보여주는 온라인 도구. Jacobian 유도할 때 검산에 유용

**LM의 직관: Gauss-Newton과 Gradient Descent 사이의 스위칭**

비선형 최소자승 문제에서 Gauss-Newton은 수렴이 빠르지만 초기값이 나쁘면 발산한다. Gradient Descent는 느리지만 안정적이다. LM은 damping factor λ로 둘 사이를 자동으로 전환한다.

λ가 작으면 Gauss-Newton에 가까워 해 근처에서 빠르게 수렴하고, 크면 Gradient Descent에 가까워 해에서 먼 초기 단계에서 안정적이다. update가 비용을 줄이면 λ를 줄이고, 비용이 늘면 λ를 키운다. 이 adaptive한 전환이 LM의 핵심이다. Ceres Solver의 기본 solver가 LM인 이유이기도 하다.

(참고: [다크 프로그래머 — 함수최적화 기법 정리 (LM 방법 등)](https://darkpgmr.tistory.com/142))

## 3.5 심화: Lie Group과 Lie Algebra

로보틱스에서 가장 자주 마주치는 수학적 난관 중 하나는 "회전을 어떻게 최적화할 것인가"이다. Lie group과 Lie algebra는 회전과 강체 변환을 체계적으로 다루는 틀을 제공한다. SLAM 백엔드, Visual-Inertial Odometry, Bundle Adjustment를 이해하려면 이 내용이 필수다.

### 3.5.1 왜 Lie Group이 필요한가

3.2절에서 회전을 표현하는 여러 방법을 다뤘다. 그런데 이 표현들을 가지고 최적화를 하려고 하면 문제가 생긴다.

- **회전 행렬 R**: 3x3이므로 파라미터가 9개인데, 실제 자유도는 3이다. R^T R = I 와 det(R) = 1이라는 제약 조건이 있기 때문이다. 일반적인 unconstrained optimization을 적용하면 업데이트 후 R이 더 이상 유효한 회전 행렬이 아니게 된다.
- **쿼터니언**: 4개 파라미터에 정규화 제약(||q|| = 1)이 있다. 업데이트할 때마다 re-normalize해야 하고, 이 과정에서 수치 오류가 누적될 수 있다.
- **오일러 각**: Gimbal lock 문제가 있고, 각도 wrapping도 까다롭다.

문제는 회전이 비선형 manifold 위에 있는 반면, Gauss-Newton 및 LM 같은 최적화 알고리즘은 유클리드 공간에서 동작한다는 점이다. Lie group 이론은 이 간극을 메운다. Manifold 위의 점(회전 행렬) 근처에 접선 공간(Lie algebra)을 정의하고, 그 공간에서 유클리드 최적화를 수행한 뒤 결과를 다시 manifold 위로 올린다.

배경 지식: 여기서 말하는 "group"이란, 어떤 연산에 대해 닫힘(closure), 결합법칙(associativity), 항등원(identity), 역원(inverse)이 성립하는 집합이다. 예를 들어 invertible한 n x n 행렬의 집합은 행렬 곱에 대해 group을 이루며, 이를 general linear group GL(n)이라 한다. 그 중 det = 1인 부분군이 special linear group SL(n)이다. Orthogonal group O(n)은 내적을 보존하는 행렬의 집합이고, 여기서 det = 1인 것만 모으면 SO(n) — 즉 회전군이 된다. det = -1인 것들은 반사(reflection)를 포함하며, 이들은 군 연산에 대해 닫혀있지 않으므로 부분군을 형성하지 않는다.

### 3.5.2 SO(3): 3D 회전군

**정의:**
```
SO(3) = { R in R^{3x3} | R^T R = I, det(R) = 1 }
```

SO(3)는 group이다. 군 연산은 행렬 곱이고, 두 회전 R_1, R_2의 합성 R_1 R_2도 SO(3)의 원소다. 항등원은 단위 행렬 I, 역원은 R^T(= R^{-1})이다. 직교 행렬이므로 전치가 곧 역행렬이 된다. 행렬 곱은 결합법칙을 만족하지만 교환법칙은 성립하지 않는다(일반적으로 R_1 R_2 != R_2 R_1).

**Lie algebra so(3):**

SO(3)의 Lie algebra는 3x3 반대칭 행렬(skew-symmetric matrix)의 공간이며, 3차원이다.

**Hat operator** `[.]x` 는 3차원 벡터를 반대칭 행렬로 변환한다:

```
w = [w1, w2, w3]^T  (in R^3)

[  0   -w3   w2 ]
[w]x =  [  w3   0   -w1 ]   in so(3)
        [ -w2   w1   0  ]
```

이 행렬은 벡터 외적(cross product)에 대응한다: `[w]x v = w x v`

**Vee operator** `(.)v` 는 역변환이다: 반대칭 행렬에서 3차원 벡터를 추출한다.

직관적으로, so(3)의 원소 w는 "회전축 방향"과 "회전 크기"를 하나의 벡터로 인코딩한다. 축-각(axis-angle) 표현과 직접 대응된다.

### 3.5.3 Exponential Map과 Logarithmic Map

**Exponential map**: so(3) -> SO(3)

Lie algebra의 원소(벡터)를 Lie group의 원소(회전 행렬)로 보내는 사상이다. 이것이 어디서 나오는지 유도해 보자.

시간에 따라 연속적으로 회전하는 행렬 R(t)가 있다고 하자 (R(0) = I). R(t)는 항상 SO(3)에 있으므로 `R(t) R(t)^T = I`이다. 양변을 t로 미분하면:

```
d/dt (R R^T) = R_dot R^T + R R_dot^T = 0
→  R_dot R^T = -(R_dot R^T)^T
```

즉 `R_dot R^T`는 반대칭 행렬(skew-symmetric)이다. 이를 어떤 벡터 w(t)의 hat form으로 쓸 수 있다:

```
R_dot(t) R^T(t) = [w(t)]x
→  R_dot(t) = [w(t)]x R(t)
```

w가 상수(일정한 각속도)인 경우, 이 미분 방정식의 해는:

```
R(t) = exp([w]x * t) = sum_{n=0}^{inf} ([w]x * t)^n / n!
```

여기서 `exp([w]x)`는 축 w 방향으로 ||w|| 라디안만큼 회전시키는 행렬이 된다. 구체적으로, theta = ||w||로 두면 **Rodrigues' formula**로 닫힌 형태를 얻는다:

```
exp([w]x) = I + (sin(theta) / theta) [w]x + ((1 - cos(theta)) / theta^2) [w]x^2
```

이 공식은 `sin(t)`와 `cos(t)`의 Taylor 전개를 `[w]x`의 거듭제곱에 대입하면 유도된다. `[w]x^3 = -theta^2 [w]x`라는 성질을 이용하면 급수가 sin, cos 항으로 정리된다.

theta가 작을 때(|theta| < eps)는 sin(theta)/theta ≈ 1, (1-cos(theta))/theta^2 ≈ 1/2이므로:

```
exp([w]x) ≈ I + [w]x + (1/2)[w]x^2   (2차 항까지의 근사)
```

주의: 하나의 회전 행렬 R에 대해 `R = exp([w]x)`를 만족하는 w는 유일하지 않다. ||w|| + 2*pi*k (정수 k)에 대해 같은 R을 준다. 이것이 logarithmic map에서 주의해야 하는 부분이다.

**Logarithmic map**: SO(3) -> so(3)

역변환이다. 주어진 회전 행렬 R에서 축-각 벡터 w를 복원한다.

```
theta = arccos((tr(R) - 1) / 2)
[w]x = (theta / (2 sin(theta))) (R - R^T)
```

theta = 0 (항등 회전) 이나 theta = pi (180도 회전) 근처에서는 특별한 처리가 필요하다.

**직관**: Lie algebra는 group 위의 한 점(보통 항등원 I)에서의 접선 공간(tangent space)이다. "작은 회전"은 접선 공간의 벡터로 표현할 수 있고, exponential map이 이 벡터를 manifold 위의 실제 회전으로 매핑한다. 이것이 최적화에서 핵심이 되는 이유다: 업데이트량 dw를 접선 공간(R^3)에서 계산한 뒤, exp([dw]x)를 현재 회전에 곱해서 manifold 위에서 이동하는 것이다.

### 3.5.4 SE(3): 3D 강체 변환군

로봇의 포즈는 회전뿐 아니라 이동도 포함한다. 이를 다루는 것이 SE(3)이다.

**정의:**
```
SE(3) = { T = [ R  t ] | R in SO(3), t in R^3 }
              [ 0  1 ]
```

T는 4x4 homogeneous transformation matrix이다. SE(3)도 group이다. 군 연산은 행렬 곱(T_1 T_2)이며, 항등원은 4x4 단위 행렬, 역원은 T^{-1} = [ R^T  -R^T t ; 0  1 ]이다.

**Lie algebra se(3):**

SE(3)의 Lie algebra는 6차원이다. 원소를 **twist** 벡터라 부른다:

```
xi = [rho; w] in R^6     (rho in R^3: 이동 성분, w in R^3: 회전 성분)
```

**Hat operator**는 6차원 벡터를 4x4 행렬로 변환한다:

```
        [ [w]x  rho ]
xi^ =   [  0     0  ]   in se(3)    (4x4 행렬)
```

**Exponential map**: se(3) -> SE(3)

```
exp(xi^) = [ exp([w]x)   J rho ]   in SE(3)
           [    0           1   ]
```

여기서 J는 left Jacobian of SO(3)이다:

```
J = I + ((1 - cos(theta)) / theta^2) [w]x + ((theta - sin(theta)) / theta^3) [w]x^2
```

6-DoF 포즈(3 회전 + 3 이동)는 6차원 벡터 xi in R^6으로 매개변수화할 수 있다. 제약 조건이 없는 6차원 유클리드 공간에서 최적화한 뒤, exponential map으로 결과를 SE(3) manifold 위에 올린다. SLAM 최적화에서 Lie group을 쓰는 이유가 여기에 있다.

> **실습**: [SE(3) Pose Composition](https://alexjunholee.github.io/robotics-practice/app.html#pose_composition_3d)
> SE(3) 변환의 합성을 3D로 직접 조작하며, 회전과 이동이 결합된 강체 변환이 어떻게 연쇄되는지 확인할 수 있다.

### 3.5.5 Perturbation Model과 Jacobian

Gauss-Newton이나 LM 알고리즘으로 포즈를 최적화할 때, 현재 추정값 T에 작은 변화(perturbation) d_xi를 가하는 방법이 두 가지 있다.

**Left perturbation (global frame 기준):**
```
T' = exp(d_xi^) * T
```

**Right perturbation (body frame 기준):**
```
T' = T * exp(d_xi^)
```

어느 쪽을 쓰든 수학적으로 일관성 있게 유지하면 된다. 문헌마다 convention이 다르니 주의해야 한다. Barfoot의 교재가 left convention을 주로 채택한 데 비해, Strasdat의 Sophus 라이브러리는 right convention을 기본값으로 삼는다.

**Jacobian 계산:**

에러 함수 e(T)가 있을 때, perturbation에 대한 Jacobian은:

```
de/d(d_xi) = lim_{d_xi->0}  (e(exp(d_xi^) * T) - e(T)) / d_xi     (left perturbation의 경우)
```

이 Jacobian은 6열짜리 행렬이 된다 (에러 차원 x 6).

일반적인 최적화에서 업데이트는 `x <- x + dx` (유클리드 덧셈)이다. SE(3) 위에서는 덧셈이 정의되지 않는다. 대신:

1. 접선 공간에서 d_xi in R^6을 Gauss-Newton으로 계산한다: `d_xi = -(J^T J)^{-1} J^T e`
2. Manifold 위에서 업데이트한다: `T <- exp(d_xi^) * T`

이렇게 하면 업데이트 후에도 T가 항상 유효한 SE(3) 원소임이 보장된다. 별도의 제약 조건 처리가 필요 없다.

g2o, GTSAM, Ceres (with local parameterization / manifold)에서 내부적으로 이 방식을 쓴다. GTSAM의 `Pose3`는 SE(3)를 직접 구현하고, `Pose3::Expmap()`, `Pose3::Logmap()`을 제공한다. Ceres에서는 `LocalParameterization` (또는 최신 API의 `Manifold`)을 통해 같은 개념을 구현한다.

### 3.5.6 Adjoint Representation

twist를 다른 좌표계로 변환해야 할 때 Adjoint를 쓴다.

SE(3)의 원소 T에 대해, Adjoint 행렬 Ad_T는 6x6 행렬이다:

```
Ad_T = [ R    [t]x R ]   in R^{6x6}
       [ 0      R    ]
```

twist 변환:
```
xi_a = Ad_{T_ab} * xi_b
```

**실용적 의미**: 센서 프레임에서 표현한 twist(angular velocity, linear velocity)를 body 프레임이나 world 프레임의 twist 표현으로 바꿀 때 Adjoint를 쓴다. IMU는 선속도를 직접 측정하지 않고 각속도와 비력을 측정하며, 이 측정 벡터들의 축 변환에는 회전 행렬을 쓴다. 여러 센서를 fusion하는 VIO 시스템에서 좌표계 간 변환이 빈번하게 일어나므로, Adjoint의 의미를 이해하고 있어야 한다.

### 3.5.7 실무에서의 사용

**Sophus (C++)**: Strasdat가 만든 Lie group 라이브러리. SO(3), SE(3)와 그 exponential/logarithmic map, Adjoint 등을 구현한다. ORB-SLAM3 등 여러 SLAM 시스템이 사용한다. Kimera는 Lie group 연산을 GTSAM의 Pose3/Rot3로 처리한다.

```cpp
#include <sophus/se3.hpp>

// SE(3) 포즈 초기화 (항등 변환)
Sophus::SE3d T_world_body;

// se(3) perturbation (6-vector): [translation; rotation]
Sophus::SE3d::Tangent delta;
delta << 0.01, 0.0, 0.0, 0.0, 0.0, 0.001;  // 작은 x-이동 + 작은 z-회전

// Left perturbation update
T_world_body = Sophus::SE3d::exp(delta) * T_world_body;

// Log map: SE(3) -> se(3)
Sophus::SE3d::Tangent xi = T_world_body.log();
```

**Jaxlie (Python/JAX)**: Brent Yi가 만든 JAX 기반 Lie group 라이브러리. 자동 미분이 가능하므로, Jacobian을 손으로 유도하지 않아도 된다. 연구 프로토타이핑에 유용하다.

```python
import jaxlie
import jax.numpy as jnp

T = jaxlie.SE3.identity()
delta = jnp.array([0.01, 0.0, 0.0, 0.0, 0.0, 0.001])
T_updated = jaxlie.SE3.exp(delta) @ T
```

**GTSAM**: `gtsam::Pose3`가 내부적으로 SE(3)를 쓴다. Factor graph 최적화 시 Lie group 위에서의 perturbation을 자동으로 처리한다.

> **추천 자료**
> - [State Estimation for Robotics, Ch.7–8 (Barfoot)](http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17.pdf) — Lie group을 로보틱스 상태 추정 관점에서 다루는 핵심 레퍼런스
> - [A micro Lie theory for state estimation in robotics (Sola et al., arXiv:1812.01537)](https://arxiv.org/abs/1812.01537) — Lie group 핵심을 20페이지로 요약한 글
> - [TUM Multiple View Geometry, Ch.2 — Rigid Body Motion](https://cvg.cit.tum.de/teaching/online/mvg) — Daniel Cremers 교수의 강의. SO(3), SE(3)를 시각적으로 설명
> - [Sophus GitHub](https://github.com/strasdat/Sophus) — C++ Lie group 라이브러리. 코드를 읽으면 이해가 빨라진다
> - [정진용 블로그 — SE(3) and SO(3) transformation](https://jinyongjeong.github.io/2016/06/07/se3_so3_transformation/) — SE(3), SO(3) 변환의 한글 정리. GL(3), O(3)부터 체계적으로 설명
> - [T-Robotics: Lie Group Formulation for Robot Mechanics](http://t-robotics.blogspot.com/2015/07/lie-group-formulation-for-robot.html) — 한국어로 작성된 Lie Group 설명. 로봇 역학에서의 Lie Group 활용을 정리

## 3.6 심화: Factor Graph

Factor graph는 SLAM 문제를 체계적으로 정의하고 효율적으로 푸는 프레임워크다. 많은 현대 SLAM 시스템의 백엔드가 factor graph를 사용한다.

### 3.6.1 Factor Graph란

Factor graph는 두 종류의 노드로 구성된 이분 그래프(bipartite graph)이다:

- 변수 노드(variable nodes): 추정하고자 하는 상태. 로봇 포즈(x_1, x_2, ...), 랜드마크 위치(l_1, l_2, ...) 등.
- 팩터 노드(factor nodes): 변수들 사이의 제약 조건 또는 측정. 각 팩터는 연결된 변수들에 대한 비용 함수를 정의한다.

확률적으로, 전체 사후 분포는 팩터들의 곱으로 분해된다:

```
p(X | Z) proportional to  prod_i  f_i(X_i)
```

여기서 X_i는 팩터 f_i에 연결된 변수들의 부분 집합이다.

**MAP 추정** = 모든 팩터의 곱을 최대화 = 음의 로그를 취하면 합을 최소화 = **nonlinear least squares** 문제가 된다:

```
X* = argmin_X  sum_i  ||e_i(X_i)||^2_{Sigma_i}
```

e_i는 에러 함수, Sigma_i는 해당 측정의 공분산(불확실성 가중치)이다.

### 3.6.2 SLAM을 Factor Graph로 표현

SLAM에서 사용하는 대표적 팩터:

| 팩터 | 역할 |
|---|---|
| Prior factor | 초기 포즈에 대한 사전 정보. 예: "시작점은 원점이다" |
| Odometry factor | 두 연속 포즈 사이의 상대 변환. IMU preintegration이나 wheel odometry에서 온다 |
| Landmark observation factor | 포즈에서 랜드마크를 관측한 측정. reprojection error가 대표적 |
| Loop closure factor | 이전에 방문한 장소를 재인식했을 때 추가. 전체 궤적의 drift를 보정하는 핵심 |
| IMU preintegration factor | 두 키프레임 사이의 IMU 측정을 하나의 팩터로 요약 |

ASCII로 간략히 표현하면:

```
 [prior]---x1---[odom]---x2---[odom]---x3
                  |                      |
              [landmark]            [landmark]
                  |                      |
                  l1                     l2

x3 ---[loop closure]--- x1
```

각 팩터에는 측정값과 공분산(노이즈 모델)이 포함된다. 그래프가 구축되면 Gauss-Newton 또는 LM으로 전체 변수를 동시에 최적화한다.

### 3.6.3 풀이: Variable Elimination과 Bayes Tree

Factor graph를 최적화하려면 정규 방정식 `H d = -b`를 풀어야 한다 (H는 Hessian 근사, b는 gradient). 이 시스템의 구조를 이해하는 것이 효율적 풀이의 핵심이다.

**Variable elimination**: 변수를 하나씩 소거하는 과정. 이것은 sparse Cholesky factorization과 수학적으로 동등하다. 소거 순서에 따라 fill-in (원래 0이었던 곳이 non-zero가 되는 현상)이 달라지며, 이는 계산 비용에 직접 영향을 미친다.

**Variable ordering**: 소거 순서를 최적화하는 것이 중요하다. COLAMD (Column Approximate Minimum Degree) 같은 heuristic이 널리 쓰인다. 직관적으로, 연결이 적은 변수를 먼저 소거하면 fill-in이 적다.

**Bayes tree**: Kaess et al. (WAFR 2010)이 제안한 자료구조로, iSAM2(Kaess et al., IJRR 2012)의 핵심이다. Factor graph를 elimination하면 Bayes net이 되고, 이를 tree 구조로 재편하면 Bayes tree가 된다. 새로운 측정이 들어올 때, 영향을 받는 subtree만 re-elimination하면 된다.

실시간 SLAM에서는 매 프레임마다 새로운 팩터가 추가된다. 전체 시스템을 처음부터 다시 풀면 O(n^3)이지만, Bayes tree를 이용한 incremental update는 영향받는 부분만 갱신하므로 실시간 처리가 가능하다.

Factor graph는 §3.10의 정보 필터(Information Filter)와 깊이 연결된다 — 두 표현 모두 정보 행렬 $\Omega$의 sparse 구조를 활용한다.

> **추천 자료**
> - [Factor Graphs and GTSAM (Dellaert & Kaess)](https://gtsam.org/tutorials/intro.html) — GTSAM 공식 튜토리얼. Factor graph에서 SLAM으로의 연결을 설명
> - [Factor Graphs for Robot Perception (Dellaert & Kaess, 2017)](https://www.cs.cmu.edu/~kaess/pub/Dellaert17fnt.pdf) — 100페이지 분량의 종합 레퍼런스
> - [CMU 16-833 Lecture Notes](https://www.cs.cmu.edu/~kaess/teaching/16833/) — Michael Kaess 교수의 SLAM 강의. Factor graph와 iSAM2를 깊이 다룬다

> **실습**: [Factor Graph 시각화](https://alexjunholee.github.io/robotics-practice/app.html#factor_graph_viz)
> Factor graph의 변수 노드와 팩터 노드를 직접 구성하고, 그래프 구조가 최적화에 미치는 영향을 확인할 수 있다.

### 3.6.4 Ceres Solver로 Pose Graph 최적화 구현하기

GTSAM 외에 Google의 Ceres Solver로도 factor graph 기반 최적화를 구현할 수 있다. Ceres는 범용 nonlinear least squares 솔버라서 SLAM에 특화된 기능은 없지만, 그만큼 내부 동작을 직접 이해하기 좋다. 아래는 Ceres 공식 예제인 `pose_graph_3d`를 기반으로 한 분석이다.

**Error Term 정의:**

두 포즈 `x_a`, `x_b` 사이의 상대 변환 측정값 `T_ab_measured`가 있을 때, residual은 추정된 상대 변환과 측정값의 차이다.

```cpp
class PoseGraph3dErrorTerm {
 public:
  PoseGraph3dErrorTerm(Pose3d t_ab_measured,
                       Eigen::Matrix<double, 6, 6> sqrt_information)
      : t_ab_measured_(std::move(t_ab_measured)),
        sqrt_information_(std::move(sqrt_information)) {}

template <typename T>
  bool operator()(const T* const p_a_ptr, const T* const q_a_ptr,
                  const T* const p_b_ptr, const T* const q_b_ptr,
                  T* residuals_ptr) const {
    // 추정된 상대 변환 계산
    Eigen::Quaternion<T> q_a_inverse = q_a.conjugate();
    Eigen::Quaternion<T> q_ab_estimated = q_a_inverse * q_b;
    Eigen::Matrix<T, 3, 1> p_ab_estimated = q_a_inverse * (p_b - p_a);

// 측정값과의 차이
    Eigen::Quaternion<T> delta_q =
        t_ab_measured_.q.cast<T>() * q_ab_estimated.conjugate();

// residual = [position_error; orientation_error]
    residuals.block<3,1>(0,0) = p_ab_estimated - t_ab_measured_.p.cast<T>();
    residuals.block<3,1>(3,0) = T(2.0) * delta_q.vec();

// information matrix 적용 (covariance의 역)
    residuals.applyOnTheLeft(sqrt_information_.cast<T>());
    return true;
  }
};
```

- **template \<typename T\>**: Ceres 내부에서 residual 값이 필요하면 `T=double`, Jacobian이 필요하면 `T=Jet<double>`로 자동 전환된다. 이것이 AutoDiff의 원리다.
- **sqrt_information**: information matrix의 Cholesky decomposition. `information.llt().matrixL()`로 구한다.
- **AutoDiffCostFunction 차원**: `<PoseGraph3dErrorTerm, 6, 3, 4, 3, 4>` — residual 6차원, pos_a 3차원, quat_a 4차원, pos_b 3차원, quat_b 4차원.
- **SetManifold**: quaternion은 4차원이지만 자유도는 3이므로, `EigenQuaternionManifold`를 지정해서 manifold 위에서 최적화하도록 한다. 이전 API에서는 `LocalParameterization`이었다.

**문제 구성:**

```cpp
ceres::Problem problem;
ceres::LossFunction* loss_function = nullptr;  // robust loss 필요시 HuberLoss 등
ceres::Manifold* quaternion_manifold = new EigenQuaternionManifold;

for (const auto& constraint : constraints) {
    ceres::CostFunction* cost_function =
        PoseGraph3dErrorTerm::Create(constraint.t_be, sqrt_information);
    problem.AddResidualBlock(cost_function, loss_function,
                              pose_begin.p.data(), pose_begin.q.coeffs().data(),
                              pose_end.p.data(), pose_end.q.coeffs().data());
    problem.SetManifold(pose_begin.q.coeffs().data(), quaternion_manifold);
    problem.SetManifold(pose_end.q.coeffs().data(), quaternion_manifold);
}

// 첫 번째 포즈 고정 (gauge freedom 제거)
problem.SetParameterBlockConstant(poses.begin()->second.p.data());
problem.SetParameterBlockConstant(poses.begin()->second.q.coeffs().data());
```

**풀이:**

```cpp
ceres::Solver::Options options;
options.max_num_iterations = 200;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
```

`SPARSE_NORMAL_CHOLESKY`는 pose graph처럼 sparse한 문제에 적합하다. Bundle adjustment처럼 제거할 점 변수 블록이 분명한 문제에서는 `SPARSE_SCHUR`도 고려할 수 있다.

**GTSAM vs Ceres 비교**

| | GTSAM | Ceres |
|---|---|---|
| 특성 | SLAM 특화 | 범용 nonlinear least squares |
| 기본 제공 | `BetweenFactor`, `PriorFactor` 등 미리 정의된 팩터 | 없음. 모든 cost function 직접 정의 |
| 증분 최적화 | iSAM2로 incremental 풀이 가능 | 지원 안 함 |
| Manifold | Lie group 기본 지원 | `LocalParameterization` / `Manifold`로 직접 설정 |
| 적합한 상황 | SLAM 시스템 구축 | 유연한 구조가 필요할 때, 대규모 BA |

> **추천 자료**
> - [Ceres Solver 공식 pose_graph_3d 예제](https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/slam/pose_graph_3d/) — 위 코드의 전체 버전
> - [Ceres Solver Tutorial](http://ceres-solver.org/tutorial.html) — AutoDiff, Manifold 개념 설명
> - [정진용 블로그 — Ceres Solver Tutorial](https://jinyongjeong.github.io/2023/07/22/Ceres_tutorial/) — Ceres Solver 발표자료와 GitHub 실습 코드. 비선형 최적화 입문에 적합

## 3.7 심화: Robust Estimation

현실 세계의 데이터는 깨끗하지 않다. 잘못된 데이터 연관(false match), 동적 물체, 센서 고장이 outlier를 만들고, outlier는 최적화 결과를 심각하게 왜곡한다. Robust estimation은 이런 상황에서도 합리적인 추정을 내놓기 위한 기법이다.

### 3.7.1 왜 필요한가

Standard least squares는 에러의 제곱을 최소화한다: `rho(r) = r^2`. 이 함수는 큰 잔차(residual)에 큰 가중치를 주기 때문에, 하나의 outlier가 전체 해를 끌고 갈 수 있다.

SLAM에서의 구체적 사례:
- 잘못된 loop closure 하나가 전체 지도를 뒤틀어 버린다
- Visual feature matching에서의 false positive가 BA 결과를 망친다
- 동적 물체(사람, 차)에 붙은 feature가 정적 장면 가정을 위반한다

### 3.7.2 M-Estimator

M-estimator는 `rho(r) = r^2` 대신 다른 비용 함수 rho를 사용하여 outlier의 영향을 줄인다.

| M-Estimator | rho(r) | 특성 |
|---|---|---|
| **L2 (표준)** | r^2 | Outlier에 취약 |
| **Huber** | r^2 (abs(r) <= k), 2k*abs(r) - k^2 (abs(r) > k) | 작은 잔차는 L2, 큰 잔차는 L1. 여러 최적화 라이브러리가 지원 |
| **Cauchy** | c^2 * log(1 + (r/c)^2) | Huber보다 outlier 억제가 강함 |
| **Geman-McClure** | r^2 / (1 + r^2) | 극단적 outlier를 사실상 무시 |

Huber가 안전한 기본 선택이다. Outlier 비율이 높거나 극단적이면 Cauchy나 Geman-McClure를 고려한다. 파라미터(k 또는 c)는 잔차의 통계적 분포에 맞춰 튜닝해야 한다.

실무적으로, Ceres Solver에서는 `ceres::HuberLoss`, `ceres::CauchyLoss` 등을 cost function에 감싸서 적용한다. GTSAM에서는 `gtsam::noiseModel::mEstimator::Huber`를 쓴다.

> **실습**: [M-Estimator 비교](https://alexjunholee.github.io/robotics-practice/app.html#m_estimator)
> L2, Huber, Cauchy, Geman-McClure 등 다양한 비용 함수가 outlier에 어떻게 반응하는지 인터랙티브하게 비교할 수 있다.

### 3.7.3 RANSAC와 변종

RANSAC (Random Sample Consensus)은 outlier가 포함된 데이터에서 모델을 피팅하는 반복적 알고리즘이다. M-estimator와 달리, 데이터를 inlier/outlier로 명시적으로 분류한다.

**기본 RANSAC 알고리즘:**
1. 최소 샘플을 무작위로 선택
2. 해당 샘플로 모델을 피팅
3. 전체 데이터에서 inlier 수를 계산 (threshold 이내의 잔차를 가진 점)
4. 반복 -> 가장 많은 inlier를 가진 모델을 선택
5. 최종적으로 모든 inlier를 사용해 모델을 re-fit

**변종들:**

| 변종 | 핵심 아이디어 | 트레이드오프 |
|---|---|---|
| RANSAC (기본) | 무작위 샘플 → 반복 | 단순하고 구현이 쉽지만 threshold·반복 횟수에 민감 |
| PROSAC | matching score로 좋은 샘플을 먼저 시도 | 빠르게 수렴하지만 사전 품질 정보의 질에 의존 |
| Lo-RANSAC | 좋은 모델 발견 시 로컬 최적화 추가 | 정확도 향상, 속도 감소 |
| MAGSAC++ | 노이즈 스케일 sigma 자동 추정, soft inlier/outlier | 파라미터 프리에 가까우나 계산 비용이 높음 |

OpenCV의 `cv::findHomography`, `cv::findFundamentalMat` 등에서 `cv::USAC_MAGSAC` 플래그로 MAGSAC++를 사용할 수 있다.

> **추천 자료**
> - [State Estimation for Robotics, Ch.5 (Barfoot)](http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17.pdf) — 바이어스, 대응 문제, outlier를 다루는 실전적 챕터
> - [Hartley & Zisserman, Ch.4 — Estimation: 2D Projective Transforms](https://www.robots.ox.ac.uk/~vgg/hzbook/) — RANSAC(원 제안은 Fischler & Bolles, 1981)과 robust estimation 이론을 정리한 교재 설명
> - [다크 프로그래머 — RANSAC의 이해와 영상처리 활용](https://darkpgmr.tistory.com/61) — RANSAC의 원리, threshold 설정, 반복 횟수 계산을 한글로 설명
> - [정진용 블로그 — Bundle Adjustment의 Jacobian 계산](https://jinyongjeong.github.io/2020/03/01/Jacobian_of_BA/) — BA의 reprojection error Jacobian을 Lie algebra와 quaternion으로 유도. 손필기 수식 포함

> **실습**: [RANSAC 시각화](https://alexjunholee.github.io/robotics-practice/app.html#ransac)
> Outlier가 포함된 데이터에서 RANSAC이 inlier/outlier를 분류하고 모델을 피팅하는 과정을 단계별로 확인할 수 있다.

## 3.8 심화: 정보 이론 기초

Active SLAM, exploration, 불확실성 기반 의사결정에서 정보 이론 개념이 쓰인다.

**Shannon entropy**: 확률 변수 X의 불확실성을 측정한다.

```
H(X) = -sum  p(x) log p(x)
```

Entropy가 높을수록 불확실성이 크다. 가우시안 분포의 경우 공분산이 클수록 entropy가 높다.

**KL divergence (Kullback-Leibler divergence)**: 두 확률 분포 p와 q 사이의 "차이"를 측정한다.

```
D_KL(p || q) = sum  p(x) log(p(x) / q(x))
```

비대칭이다: D_KL(p||q) != D_KL(q||p). p를 q로 근사할 때 생기는 정보 손실로 해석할 수 있다.

**Mutual information**: Y를 관측하면 X에 대해 얼마나 알게 되는가를 측정한다.

```
I(X; Y) = H(X) - H(X|Y)
```

H(X)는 Y를 관측하기 전 X의 불확실성, H(X|Y)는 관측 후 불확실성. 그 차이가 Y가 X에 대해 제공하는 정보량이다.

**Active SLAM 응용**: 로봇이 다음에 어디로 갈지 결정할 때, "이 행동을 취하면 지도/포즈의 불확실성이 얼마나 줄어드는가?"를 mutual information으로 수치화할 수 있다. Expected information gain이 가장 큰 행동을 선택하는 것이 정보 이론 기반 탐색의 핵심이다.

```
a* = argmax_a  I(X; Z_a)  =  argmax_a  [ H(Z_a) - H(Z_a | X) ]
```

여기서 a는 행동(action), Z_a는 그 행동을 통해 얻을 관측, X는 환경 상태이다.

> **추천 자료**
> - [Elements of Information Theory (Cover & Thomas)](https://onlinelibrary.wiley.com/doi/book/10.1002/047174882X) — 정보 이론 교과서
> - Placed et al., "A Survey on Active Simultaneous Localization and Mapping: State of the Art and New Frontiers" (IEEE T-RO 2023) — Active SLAM에서 정보 이론이 쓰이는 방식을 정리한 서베이

> **기술 흐름: 로보틱스 수학 및 최적화**
> - **~2005**: 칼만 필터(EKF) 중심의 상태 추정. 선형 근사 기반, 소규모 문제에 적합. 실시간 처리가 어려워 문제 크기에 제약이 있었다.
> - **2006~2015**: Factor Graph 기반 최적화(iSAM, g2o, GTSAM) 등장. 스파스 행렬 구조를 활용해 대규모 SLAM 문제를 효율적으로 풀었다. Lie Group/Algebra가 SLAM 커뮤니티에서 표준 도구로 자리잡았다.
> - **2016~2020**: 실시간 대규모 최적화 실용화. 증분적 최적화(incremental optimization)로 매 프레임 업데이트가 가능해졌다. Ceres Solver 같은 공개 비선형 최소제곱 라이브러리가 연구와 산업 응용에 널리 쓰였다.
> - **2021~**: Differentiable Programming 시대. PyTorch/JAX의 자동 미분(Auto-Diff)을 활용한 End-to-End 최적화. NeRF, 3D Gaussian Splatting 등 미분 가능 렌더링이 등장하면서, 기존에 손으로 유도하던 Jacobian을 자동 미분으로 대체했다. Theseus(Meta) 같은 미분 가능 최적화 라이브러리도 나왔다.
> - **지금**: 고전적 수학(Lie Group, 확률, 최적화)은 여전히 필수다. Differentiable Programming이 최적화 문제 접근 방식을 바꾸고 있지만, 자동 미분이 내부에서 무엇을 하는지 이해하려면 여기서 다룬 기초가 필요하다. 도구만 쓸 줄 알면 디버깅할 수 없다.

---

## 3.9 심화: 베이즈 필터 (Bayes Filter)

원전: Thrun, Burgard, Fox (2005) *Probabilistic Robotics*, Ch.2 (Recursive State Estimation).
(국문: 『확률론적 로보틱스』, 에이콘, 2020)

§3.8 정보 이론은 불확실성을 *측정*하는 도구였다. 그렇다면 시간이 흐르면서 새 관측이 쌓일 때, 그 불확실성을 어떻게 *갱신*할 것인가. §3.3.2의 베이즈 정리를 시간 축 위에서 재귀적으로 돌리면, "로봇이 지금 어디 있는가"라는 질문에 답하는 구조가 만들어진다. §3.6 Factor Graph는 이미 연속 포즈 사이의 odometry·IMU factor로 시간 관계까지 담는다. 베이즈 필터와의 차이는 시간을 다루는지가 아니라 과거 상태를 marginalize하고 현재 믿음만 남기는지(filtering)에 있다.

"로봇이 지금 어디 있는가?"라는 물음은 정적인 단일 시점 추정을 넘어, 과거와 현재의 정보를 순차적으로 누적하는 **재귀 추정(recursive estimation)** 문제로 다루어야 한다. 베이즈 필터는 그 재귀 구조의 가장 일반적인 형태로, 이후 칼만 필터와 입자 필터가 모두 이 틀 안에서 작동한다.

### 3.9.1 상태와 Markov 가정

**상태 $x_t$** 는 로봇과 환경의 미래 예측에 필요한 모든 정보를 담은 변수 묶음이다. "완전 상태(complete state)"란 그 자체만으로 미래를 예측하기에 충분한 요약(sufficient statistic)이다. 이 성질이 곧 **Markov 성질**이다.

$$p(x_{t+1} \mid x_t,\, x_{0:t-1},\, z_{1:t},\, u_{1:t}) = p(x_{t+1} \mid x_t)$$

완전 상태 가정 아래 미래는 오직 현재 $x_t$에만 달려 있다. 과거는 무관하다.

상태 변수는 다양한 방식으로 분류된다. 시간에 따라 변하는 동적 상태(로봇 위치, 속도)와 변하지 않는 정적 상태(벽 위치, 랜드마크)로 나뉜다. 값의 형태에 따라 연속 상태(pose), 이산 상태(센서 고장 여부), 하이브리드 상태(둘의 결합) 등으로 세분화할 수 있다. 실제 시스템에서 완전 상태는 거의 불가능하므로, 필터는 항상 부분적 근사 위에서 돌아간다. Markov 가정을 위협하는 주된 요인으로는 모델의 부정확성과 미모델링 동역학을 들 수 있으며, 수치적 근사 과정 자체에서 생기는 오차 역시 무시할 수 없다.

### 3.9.2 환경 상호작용: 측정과 제어

로봇과 환경의 상호작용은 두 데이터 스트림으로 분해된다.

- **측정 데이터** $z_t$: 환경이 로봇에 주는 정보 (LiDAR 거리, 카메라 이미지). 시간 구간 $(t-1, t]$에서 로봇의 지식을 늘린다.
- **제어 데이터** $u_t$: 로봇이 환경에 가하는 행동 (모터 명령). 제어에 따른 상태 예측에서는 운동 잡음 때문에 상태 불확실성이 증가할 수 있다.

$$z_{t_1:t_2} = z_{t_1},\, z_{t_1+1},\, \ldots,\, z_{t_2} \qquad u_{t_1:t_2} = u_{t_1},\, \ldots,\, u_{t_2}$$

**odometry는 제어 데이터로 취급한다.** 휠 인코더는 물리적으로 센서지만 상태 변화 정보(로봇이 얼마나 이동했는가)를 담으므로 $u_t$로 분류된다. 정지 명령(do_nothing)도 제어로 카운트한다 — 시간 경과 자체가 상태 변화 정보이기 때문이다.

### 3.9.3 Belief의 정의

Belief는 직접 측정 불가능한 진(true) 상태 $x_t$에 대한 로봇 내부의 사후 분포다.

$$\text{bel}(x_t) = p(x_t \mid z_{1:t},\, u_{1:t})$$

측정 $z_t$를 반영하기 **전**의 예측 belief를 별도로 표기한다.

$$\overline{\text{bel}}(x_t) = p(x_t \mid z_{1:t-1},\, u_{1:t})$$

$\overline{\text{bel}} \to \text{bel}$ 전환을 **correction** 또는 **measurement update**라 부른다. GPS조차 로봇의 pose를 직접 주지 않는다 — belief는 항상 간접 추론의 결과다. 이 $\text{bel}/\overline{\text{bel}}$ 구분은 베이즈 필터의 두 단계가 각각 무엇을 뜻하는지 나누는 기반이다.

### 3.9.4 생성 법칙: 모션 모델과 측정 모델

완전 상태 가정으로 두 조건부 독립이 성립한다.

$$p(x_t \mid x_{0:t-1},\, z_{1:t-1},\, u_{1:t}) = p(x_t \mid x_{t-1},\, u_t) \quad \text{(모션 모델)}$$

$$p(z_t \mid x_{0:t},\, z_{1:t-1},\, u_{1:t}) = p(z_t \mid x_t) \quad \text{(측정 모델)}$$

$x_{t-1}$이 과거 모든 데이터의 충분 통계량이므로, 다음 상태는 직전 상태와 직전 제어에만, 측정은 현재 상태에만 의존한다. 시간 불변(time-invariant) 가정 시 $p(x' \mid u, x)$와 $p(z \mid x)$로 축약된다.

완전 생성 모델 = 모션 모델 + 측정 모델 + 초기 분포 $p(x_0)$. 이 구조가 곧 Hidden Markov Model / Dynamic Bayes Network다.

### 3.9.5 베이즈 필터 일반형

베이즈 필터는 동적 상태 추정을 재귀적으로 쓰는 일반적인 틀이다. **prediction** 단계와 **correction** 단계를 반복한다.

$$\overline{\text{bel}}(x_t) = \int p(x_t \mid u_t,\, x_{t-1})\, \text{bel}(x_{t-1})\, dx_{t-1} \tag{prediction}$$

$$\text{bel}(x_t) = \eta\, p(z_t \mid x_t)\, \overline{\text{bel}}(x_t) \tag{correction}$$

$\eta$는 정규화 상수로, 전확률 정리에서 나오는 §3.3.2의 베이즈 분모($P(B)$)의 역수에 해당한다.

```
# Algorithm Bayes_filter (Table 2.1 의역)
# 입력: bel(x_{t-1}), u_t, z_t
# 출력: bel(x_t)

for all x_t do
    # prediction: 모션 모델로 x_{t-1} 적분
    bel_bar(x_t) = ∫ p(x_t | u_t, x_{t-1}) · bel(x_{t-1}) dx_{t-1}

# correction: 측정 모델로 가중치 부여 후 정규화
    bel(x_t) = η · p(z_t | x_t) · bel_bar(x_t)
endfor
return bel(x_t)
```

이산 상태공간에서는 적분이 합(summation)이 된다. 초기 belief $\text{bel}(x_0)$가 필요하다 — 완전 정보면 점질량, 무지면 균등 분포로 설정한다. 이 일반형은 닫힌 형식 적분이 가능하거나 이산 공간이 충분히 작을 때만 직접 구현할 수 있다. §3.10의 칼만 필터, §3.11의 입자 필터는 각각 다른 방식으로 이 일반형을 근사한다.

### 3.9.6 문 추정 워킹 예제

두 상태(열림/닫힘) 문에 대해 belief가 어떻게 갱신되는지 손으로 추적한다.

**모델 설정:**

- 측정 모델: $p(\text{sense\_open} \mid \text{is\_open}) = 0.6$, $p(\text{sense\_open} \mid \text{is\_closed}) = 0.2$
- 모션 모델 push: 열려있으면 그대로(확률 1), 닫혀있으면 확률 0.8로 열림
- 모션 모델 do_nothing: 결정론적 항등 (상태 불변)
- 초기: $\text{bel}(X_0 = \text{open}) = \text{bel}(X_0 = \text{closed}) = 0.5$

**단계 1: $u_1$ = do_nothing (제어 적용)**

do_nothing은 항등 변환이므로 $\overline{\text{bel}}(X_1) = (0.5,\; 0.5)$로 변화 없다.

**단계 2: $z_1$ = sense_open (측정 반영)**

$$\overline{\text{bel}}(X_1 = \text{open}) = 0.5, \quad p(\text{sense\_open} \mid \text{open}) = 0.6$$
$$\overline{\text{bel}}(X_1 = \text{closed}) = 0.5, \quad p(\text{sense\_open} \mid \text{closed}) = 0.2$$

비정규화 사후: $(0.6 \times 0.5,\; 0.2 \times 0.5) = (0.30,\; 0.10)$. 정규화 상수 $\eta = 1/(0.30 + 0.10) = 2.5$.

$$\text{bel}(X_1 = \text{open}) = 0.75, \quad \text{bel}(X_1 = \text{closed}) = 0.25$$

**단계 3: $u_2$ = push (제어 적용)**

$$\overline{\text{bel}}(X_2 = \text{open}) = 1 \cdot 0.75 + 0.8 \cdot 0.25 = 0.95$$
$$\overline{\text{bel}}(X_2 = \text{closed}) = 0 \cdot 0.75 + 0.2 \cdot 0.25 = 0.05$$

**단계 4: $z_2$ = sense_open (측정 반영)**

비정규화: $(0.6 \times 0.95,\; 0.2 \times 0.05) = (0.570,\; 0.010)$. $\eta = 1/0.580 \approx 1.724$.

$$\text{bel}(X_2 = \text{open}) \approx 0.983, \quad \text{bel}(X_2 = \text{closed}) \approx 0.017$$

| 단계 | $\text{bel(open)}$ | $\text{bel(closed)}$ |
|------|:-----------------:|:-------------------:|
| 초기 | 0.500 | 0.500 |
| $z_1$ 반영 후 | 0.750 | 0.250 |
| $u_2$ 적용 후 | 0.950 | 0.050 |
| $z_2$ 반영 후 | 0.983 | 0.017 |

센서 잡음이 상당히 크고(60% / 20%) 제어도 비결정적이어도, 측정과 제어가 누적되면 belief는 빠르게 한 가설로 수렴한다. 다만 0.983이 자율주행의 의사결정 기준으로 충분한지는 이 예제만으로 판단할 수 없다.

### 3.9.7 수학적 유도

베이즈 필터의 두 갱신식은 세 가지 도구만으로 유도된다: Bayes 규칙, 전확률 정리, Markov(완전 상태) 가정.

**Correction step 유도:**

Bayes 규칙 적용:
$$p(x_t \mid z_{1:t},\, u_{1:t}) = \eta\, p(z_t \mid x_t,\, z_{1:t-1},\, u_{1:t})\, p(x_t \mid z_{1:t-1},\, u_{1:t})$$

완전 상태 가정으로 $p(z_t \mid x_t,\, z_{1:t-1},\, u_{1:t}) = p(z_t \mid x_t)$이므로:
$$\text{bel}(x_t) = \eta\, p(z_t \mid x_t)\, \overline{\text{bel}}(x_t)$$

**Prediction step 유도:**

전확률 정리로 $\overline{\text{bel}}$ 분해:
$$\overline{\text{bel}}(x_t) = \int p(x_t \mid x_{t-1},\, z_{1:t-1},\, u_{1:t})\, p(x_{t-1} \mid z_{1:t-1},\, u_{1:t})\, dx_{t-1}$$

완전 상태 가정으로 첫 항 $\to p(x_t \mid x_{t-1},\, u_t)$. 둘째 항에서 $u_t$는 $x_{t-1}$보다 늦게 도달하므로 제거할 수 있다:
$$\overline{\text{bel}}(x_t) = \int p(x_t \mid u_t,\, x_{t-1})\, \text{bel}(x_{t-1})\, dx_{t-1}$$

이 유도 전체가 Markov 가정에 의존한다. Markov 가정이 깨지면 식 자체가 부정확해진다.

이로써 §3.9.5의 알고리즘 두 줄이 Bayes 규칙·전확률 정리·Markov 가정 세 가지만의 귀결임을 확인했다. 가정이 어디에 쓰였는지 알면, 이 필터가 어디서 깨지는지도 정확히 예측할 수 있다.

> **추천 자료**
> - [Thrun, Burgard, Fox — Probabilistic Robotics (2005)](https://www.probabilistic-robotics.org/) — Ch.2 전체가 이 절의 원전. 알고리즘·예제·유도가 완결적으로 서술되어 있다.
> - [Cyrill Stachniss — Bayes Filter Lecture (YouTube)](https://www.youtube.com/watch?v=0lKHFJpaZkI) — 프라이부르크 대학교 강의. 베이즈 필터를 슬라이드와 함께 명쾌하게 설명

---

## 3.10 심화: 가우시안 필터 일가족 (Gaussian Filters: KF, EKF, IF)

원전: Thrun, Burgard, Fox (2005) *Probabilistic Robotics*, Ch.3 (Gaussian Filters).
(국문: 『확률론적 로보틱스』, 에이콘, 2020)

§3.9 베이즈 필터는 임의의 belief를 다루지만, 적분을 닫힌 형식으로 풀 수 없어 직접 구현이 어렵다. 가우시안 필터 일가족은 belief를 가우시안 $\mathcal{N}(\mu_t, \Sigma_t)$로 제한함으로써 이 문제를 해결한다. 칼만 필터(KF), 확장 칼만 필터(EKF), 정보 필터(IF)가 이 범주에 속하며, 세 가지 모두 §3.9의 prediction-correction 구조를 그대로 계승한다.

### 3.10.1 칼만 필터 (Kalman Filter)

#### 선형 가우시안 시스템 가정

KF가 정확한 베이즈 필터가 되려면 belief가 항상 가우시안이어야 한다. 이를 보장하는 세 가정이 필요하다.

**상태 천이(모션 모델):**
$$x_t = A_t x_{t-1} + B_t u_t + \varepsilon_t, \quad \varepsilon_t \sim \mathcal{N}(0, R_t)$$

$A_t$는 $n \times n$ 상태 천이 행렬, $B_t$는 $n \times m$ 제어 입력 행렬, $R_t$는 프로세스 노이즈 공분산이다.

**측정 모델:**
$$z_t = C_t x_t + \delta_t, \quad \delta_t \sim \mathcal{N}(0, Q_t)$$

$C_t$는 $k \times n$ 측정 행렬, $Q_t$는 측정 노이즈 공분산이다.

**초기 belief:**
$$\text{bel}(x_0) = \mathcal{N}(\mu_0, \Sigma_0)$$

이 세 가정 아래, 모든 시점의 belief는 가우시안으로 유지된다. 즉:
$$p(x_t \mid u_t, x_{t-1}) = \mathcal{N}(x_t;\; A_t x_{t-1} + B_t u_t,\; R_t)$$
$$p(z_t \mid x_t) = \mathcal{N}(z_t;\; C_t x_t,\; Q_t)$$

#### 칼만 필터 알고리즘

KF는 belief를 $(\mu_t, \Sigma_t)$ 두 개로 표현하고, 예측 2줄 + 갱신 3줄의 5단계로 한 사이클을 완료한다.

```
# Algorithm Kalman_filter (Table 3.1 의역)
# 입력: μ_{t-1}, Σ_{t-1}, u_t, z_t
# 출력: μ_t, Σ_t

# --- prediction ---
1: μ̄_t = A_t μ_{t-1} + B_t u_t          # 상태 예측: 모션 모델 적용
2: Σ̄_t = A_t Σ_{t-1} A_t^T + R_t        # 공분산 예측: 불확실성 증가

# --- correction ---
3: K_t  = Σ̄_t C_t^T (C_t Σ̄_t C_t^T + Q_t)^{-1}   # 칼만 이득
4: μ_t  = μ̄_t + K_t (z_t - C_t μ̄_t)    # 혁신(innovation)으로 평균 보정
5: Σ_t  = (I - K_t C_t) Σ̄_t             # 공분산 감소

return μ_t, Σ_t
```

라인 1~2가 prediction (제어 $u_t$ 반영, 불확실성 증가), 라인 3~5가 measurement update (관측 $z_t$ 반영, 불확실성 감소)다.

**칼만 이득 $K_t$의 의미:** $K_t$는 prediction과 측정 사이의 신뢰 균형을 결정한다. 측정 노이즈 $Q_t$가 크면 $K_t$가 작아져 측정을 덜 신뢰하고, prediction 불확실성 $\bar\Sigma_t$가 크면 $K_t$가 커져 측정을 더 신뢰한다.

**혁신(innovation):** $z_t - C_t \bar\mu_t$는 예측된 측정과 실제 측정의 차이로, 이 값이 0이면 평균 보정은 없지만 공분산은 줄어들 수 있다.

#### 1D KF 도해: 정보가 어떻게 결합되는가

1D 위치 추정에서 KF의 각 단계를 시각화하면 직관이 명확해진다.

- **Prior $\text{bel}(x_{t-1})$**: 좁은 가우시안. 이전 추정의 확신.
- **Prediction 후**: 모션이 더해지며 분산이 증가한다 ($\bar\Sigma_t = A_t \Sigma_{t-1} A_t^T + R_t$). 가우시안이 납작해진다.
- **측정 $z_t$**: 별도의 가우시안으로 표현. 센서 정밀도 $Q_t$가 이 곡선의 폭을 결정한다.
- **Correction 후**: 두 가우시안을 곱하면 분산이 둘 다보다 좁아진다 — 정보 결합 효과다. 평균은 두 가우시안의 가중 평균에 위치한다.
- 그 다음 모션: 다시 분산 증가. 그 다음 측정: 다시 분산 감소.

이 예에서 **측정은 분산을 줄이고, 모션은 분산을 키운다.** 두 과정의 반복이 상태 추정의 기본 구조를 이룬다. 같은 해석은 §3.10.2 EKF, §3.11.3 입자 필터, ch.14 §14.7 EKF localization, §14.10 IMU preintegration에도 적용된다.

#### KF의 수학적 유도 (핵심)

KF 5줄은 §3.9.5의 베이즈 필터 두 적분을 선형 가우시안 가정 아래 닫힌 형식으로 푼 결과다.

**Part 1 (Prediction).** 베이즈 필터의 prediction 적분에서, 지수 $L_t$가 $x_{t-1}$과 $x_t$ 모두에 대해 quadratic임을 확인한다. $L_t$를 $x_{t-1}$에 대한 quadratic 부분과 $x_t$에만 의존하는 부분으로 분해하면 $x_{t-1}$ 적분은 상수가 되어 정규화에 흡수된다. 남은 $x_t$ quadratic의 1차·2차 계수에서 바로 $\bar\mu_t = A_t \mu_{t-1} + B_t u_t$와 $\bar\Sigma_t = A_t \Sigma_{t-1} A_t^T + R_t$가 읽힌다.

**Part 2 (Measurement update).** Correction 적분 $\text{bel}(x_t) \propto \exp\{-J_t\}$에서, $J_t$의 1차·2차 도함수로부터 $\Sigma_t^{-1} = C_t^T Q_t^{-1} C_t + \bar\Sigma_t^{-1}$을 얻는다. 이를 직접 역행렬하면 $n \times n$ 연산이 필요하지만, **inversion lemma** (Woodbury identity)로:
$$K_t = \bar\Sigma_t C_t^T (C_t \bar\Sigma_t C_t^T + Q_t)^{-1}$$
처럼 $k \times k$ ($k$ = 측정 차원) 역행렬로 치환할 수 있다. $k \ll n$이면 계산 비용이 크게 줄어든다.

**복잡도:** 한 사이클 $O(k^{2.8} + n^2)$ ($k$: 측정 차원, $n$: 상태 차원).

이 유도 패턴 — quadratic 분해 + inversion lemma — 은 §3.10.2 EKF 유도와 §3.6 factor graph의 Gauss-Newton 갱신에서도 동일하게 반복된다.

KF 5줄이 이렇게 짧아진 배경에는 선형 가우시안 가정이 있다. 그 가정을 비선형으로 풀면 §3.10.2 EKF가 된다.

### 3.10.2 확장 칼만 필터 (Extended Kalman Filter)

#### 비선형 시스템으로의 확장

실제 로봇 시스템은 선형이 아니다. 로봇이 회전하며 이동하는 모션 모델 $g$와 거리 센서의 측정 모델 $h$는 모두 비선형이다.

$$x_t = g(u_t, x_{t-1}) + \varepsilon_t, \quad \varepsilon_t \sim \mathcal{N}(0, R_t)$$
$$z_t = h(x_t) + \delta_t, \quad \delta_t \sim \mathcal{N}(0, Q_t)$$

비선형 $g$를 통과한 가우시안은 일반적으로 가우시안이 아니다. EKF는 이 문제를 **1차 Taylor 전개**로 해결한다. 이전 사후 평균 주위에서 $g$를, 예측 평균 주위에서 $h$를 선형화하여 가우시안 근사를 유지한다.

$$g(u_t, x_{t-1}) \approx g(u_t, \mu_{t-1}) + G_t (x_{t-1} - \mu_{t-1})$$

$$G_t := \frac{\partial g(u_t, x_{t-1})}{\partial x_{t-1}}\bigg|_{\mu_{t-1}} \quad (n \times n \text{ Jacobian})$$

$$h(x_t) \approx h(\bar\mu_t) + H_t (x_t - \bar\mu_t)$$

$$H_t := \frac{\partial h(x_t)}{\partial x_t}\bigg|_{\bar\mu_t} \quad (k \times n \text{ Jacobian})$$

선형화 품질은 두 인자에 달려 있다: 함수 자체의 비선형도, 그리고 belief의 폭. 분산이 클수록 접평면 근사가 더 빨리 무너진다 — 그래서 EKF는 분산이 작을 때 잘 작동한다.

#### EKF 알고리즘

KF의 5줄에서 선형 항을 비선형 함수와 그 Jacobian으로 교체하기만 하면 EKF가 된다.

```
# Algorithm Extended_Kalman_filter (Table 3.3 의역)
# 입력: μ_{t-1}, Σ_{t-1}, u_t, z_t
# 출력: μ_t, Σ_t

# --- prediction ---
1: μ̄_t = g(u_t, μ_{t-1})                         # 비선형 모션 모델
2: Σ̄_t = G_t Σ_{t-1} G_t^T + R_t                  # Jacobian으로 선형화된 공분산 전파

# --- correction ---
3: K_t  = Σ̄_t H_t^T (H_t Σ̄_t H_t^T + Q_t)^{-1}  # 칼만 이득 (H_t로 치환)
4: μ_t  = μ̄_t + K_t (z_t - h(μ̄_t))               # 비선형 측정 예측 사용
5: Σ_t  = (I - K_t H_t) Σ̄_t

return μ_t, Σ_t
```

KF와 EKF의 차이는 두 줄이다: (라인 1) $A_t \mu_{t-1} + B_t u_t \to g(u_t, \mu_{t-1})$, (라인 4) $C_t \bar\mu_t \to h(\bar\mu_t)$. 공분산 전파에서는 $A_t \to G_t$, $C_t \to H_t$로 Jacobian이 대신 들어간다.

#### 유도 요약 및 실무 비교

유도는 §3.10.1 KF와 평행하다. 비선형 $g$와 $h$를 1차 Taylor로 치환한 뒤, 같은 quadratic 분해 + inversion lemma 절차를 돌리면 EKF 식이 나온다. 결과:
$$\bar\mu_t = g(u_t, \mu_{t-1}), \quad \bar\Sigma_t = G_t \Sigma_{t-1} G_t^T + R_t$$
$$\mu_t = \bar\mu_t + K_t (z_t - h(\bar\mu_t)), \quad \Sigma_t = (I - K_t H_t) \bar\Sigma_t, \quad K_t = \bar\Sigma_t H_t^T (H_t \bar\Sigma_t H_t^T + Q_t)^{-1}$$

**실무 비교:**

EKF는 2010년대 중반까지 SLAM·VIO·IMU 융합에서 널리 쓰였다. 현재는 몇 가지 대안이 경쟁한다.

- **UKF (Unscented KF):** Sigma point를 사용해 비선형성을 더 정확히 전파. Jacobian 수계산 불필요. 상태 차원이 낮을 때 유리.
- **IEKF (Iterated EKF):** 갱신점을 반복해서 Jacobian을 재계산. 강한 비선형에서 EKF보다 정확.
- **LIEKF (Left-Invariant EKF):** SO(3)/SE(3) 상태에서 Taylor 선형화 대신 manifold 선형화 사용. 회전 추정 정확도 향상.

ch.14 §14.7 EKF localization과 IEKF·MSCKF의 필터 구조는 이 알고리즘 박스와 연결된다. 반면 §14.10의 IMU preintegration은 키프레임 사이의 IMU 측정을 최적화 factor로 묶는 별도의 측정 구성 방법이다. 여기서 $g$, $h$, $G_t$, $H_t$가 무엇인지 이해하면, Ch.14에서 구체적인 필터와 센서 모델을 만날 때 알고리즘 골격을 재유도할 필요가 없다.

### 3.10.3 정보 필터 (Information Filter)

#### Canonical 표현: $(\Omega, \xi)$

KF와 EKF는 가우시안을 $(\mu, \Sigma)$로 표현했다. 같은 가우시안을 다른 좌표로 쓰면 prediction과 measurement update의 계산 복잡도가 뒤바뀐다. 특히 여러 로봇이나 여러 센서에서 독립적으로 얻은 측정을 합산해야 할 때, 이 좌표가 훨씬 유리하다.

가우시안을 표현하는 두 번째 방법이 있다. 평균·공분산 $(\mu, \Sigma)$ 대신 **정보 행렬(information matrix)** $\Omega$와 **정보 벡터(information vector)** $\xi$를 사용한다.

$$\Omega = \Sigma^{-1}, \quad \xi = \Sigma^{-1} \mu$$

역방향: $\Sigma = \Omega^{-1}$, $\mu = \Omega^{-1} \xi$.

이 좌표에서 가우시안의 음의 로그 우도는 상태 $x$에 대해 quadratic이다.

$$p(x) = \eta \exp\!\left\{-\tfrac{1}{2} x^T \Omega x + x^T \xi\right\}$$

$$-\log p(x) = \mathrm{const} + \tfrac{1}{2} x^T \Omega x - x^T \xi$$

최솟값은 $\Omega x = \xi$, 즉 $x = \Omega^{-1} \xi = \mu$. 이것이 §3.4 Gauss-Newton의 정규 방정식 $H \delta x = -b$와 정확히 같은 구조다.

$\Omega = 0$은 정보가 전혀 없는 상태(완전 불확실, 균등 분포)다. Moments 표현에서는 $\Sigma = \infty$로 표현이 불가능했지만, 정보 표현에서는 자연스럽게 처리된다. "확실성"을 직접 측정하는 좌표라 볼 수 있다.

#### 정보 필터 알고리즘

정보 필터는 KF의 쌍대(dual)다. KF에서 prediction이 가산형이었다면, 정보 필터에서는 **measurement update가 가산형**이 된다.

```
# Algorithm Information_filter (Table 3.4 의역)
# 입력: ξ_{t-1}, Ω_{t-1}, u_t, z_t
# 출력: ξ_t, Ω_t

# --- prediction (두 번의 역행렬 필요) ---
1: Ω̄_t = (A_t Ω_{t-1}^{-1} A_t^T + R_t)^{-1}
2: ξ̄_t = Ω̄_t (A_t Ω_{t-1}^{-1} ξ_{t-1} + B_t u_t)

# --- correction (단순 가산!) ---
3: Ω_t  = C_t^T Q_t^{-1} C_t + Ω̄_t       # 측정 한 번 = Ω에 한 항 추가
4: ξ_t  = C_t^T Q_t^{-1} z_t + ξ̄_t       # 측정 한 번 = ξ에 한 항 추가

return ξ_t, Ω_t
```

**KF와 IF의 복잡도 쌍대성:**

| 단계 | KF | IF |
|------|:---:|:---:|
| Prediction | $O(n^2)$ 가산형 | $O(n^{2.8})$ 역행렬 2회 |
| Measurement update | $O(k^{2.8})$ 역행렬 필요 | $O(n^2)$ 가산형 |

$k$: 측정 차원, $n$: 상태 차원. 측정이 부분 상태만 건드리면(sparse $C_t$) IF의 measurement update는 더 저렴해진다.

#### EIF (Extended Information Filter)

EKF와 마찬가지로, 비선형 $g, h$에 대해 Jacobian $G_t, H_t$를 사용하면 EIF가 된다. Prediction에서 $A_t \to G_t$, Correction에서 $C_t \to H_t$로 치환하면 EIF 알고리즘이 된다.

```
# Algorithm Extended_Information_filter (핵심 변경만)
# prediction  (μ_{t-1} = Ω_{t-1}^{-1} ξ_{t-1})
Ω̄_t = (G_t Ω_{t-1}^{-1} G_t^T + R_t)^{-1}
ξ̄_t = Ω̄_t · g(u_t, μ_{t-1})       # 선형 IF의 A_t μ_{t-1}+B_t u_t → g(u_t,μ_{t-1})

# correction
Ω_t  = H_t^T Q_t^{-1} H_t + Ω̄_t
ξ_t  = H_t^T Q_t^{-1} z_t + ξ̄_t - H_t^T Q_t^{-1} h(μ̄_t) + H_t^T Q_t^{-1} H_t μ̄_t
```

#### 정보형의 중요성: 가산성과 SLAM 연결

정보 필터의 measurement update $\Omega_t = \bar\Omega_t + C_t^T Q_t^{-1} C_t$는 측정마다 $\Omega$에 한 항을 더한다. 여러 로봇의 독립 측정도 $\Omega_{\text{total}} = \sum_i \Omega_i$, $\xi_{\text{total}} = \sum_i \xi_i$로 합산할 수 있다.

이 가산성은 §3.6 factor graph에서 "측정 factor 하나 = $H^T Q^{-1} H$와 $H^T Q^{-1} z$ 한 항 추가"로 일반화된다. 정보 행렬이 sparse한 이유는 가산성 자체가 아니라 각 factor가 소수의 변수에만 연결된다는 국소성이다. $H$의 비영 열이 적어 더해지는 블록이 희소해진다. sparse Cholesky가 실제로 희소성을 유지하는지는 소거 순서가 만드는 fill-in이 정한다.

EIF-SLAM과 SEIF는 이 정보형 가산성을 SLAM 표현에 사용한다. 역사적 연결은 Ch.14 §14.16에서 다룬다.

> **추천 자료**
> - [Thrun, Burgard, Fox — Probabilistic Robotics (2005)](https://www.probabilistic-robotics.org/) — Ch.3이 이 절의 원전. KF·EKF·IF 세 알고리즘의 유도가 나란히 서술되어 있다.
> - [Cyrill Stachniss — Kalman Filter and EKF Lectures](https://www.youtube.com/watch?v=PiCC-SxWlH8) — 프라이부르크 강의. 시각적 설명이 좋다.
> - [Welch & Bishop — An Introduction to the Kalman Filter (2006)](https://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf) — KF 입문의 표준 참고 자료. 수식 유도와 직관이 균형 있게 서술됨.

---

## 3.11 심화: 비모수 필터 (Nonparametric Filters)

원전: Thrun, Burgard, Fox (2005) *Probabilistic Robotics*, Ch.4 (Nonparametric Filters).
(국문: 『확률론적 로보틱스』, 에이콘, 2020)

§3.10의 가우시안 필터는 belief를 $(\mu, \Sigma)$ 두 개로 압축하는 대신 비선형성과 다봉(multi-modal) 분포를 제대로 다루지 못한다. 비모수 필터는 이 제약을 풀고 임의 분포를 표현한다. 대가는 계산 비용이다.

### 3.11.1 히스토그램 필터 / 이산 베이즈 필터

#### 유한 상태공간: 적분을 합으로

§3.9.5의 베이즈 필터 적분을 닫힌 형식으로 풀 수 없을 때 가장 직접적인 탈출구는 상태공간을 유한하게 만드는 것이다. 상태가 $K$개의 이산 값 $\{x_1, x_2, \ldots, x_K\}$만 가진다면, §3.9.5의 적분은 합이 된다.

$$\bar p_{k,t} = \sum_i p(x_k \mid u_t, x_i)\, p_{i,t-1} \quad \text{(prediction)}$$
$$p_{k,t} = \eta\, p(z_t \mid x_k)\, \bar p_{k,t} \quad \text{(correction)}$$

```
# Algorithm Discrete_Bayes_filter (Table 4.1 의역)
# 입력: {p_{k,t-1}}, u_t, z_t
# 출력: {p_{k,t}}

for all k do
    # prediction: 모든 이전 상태에서 x_k로의 전이 합산
    p̄_{k,t} = Σ_i p(X_t = x_k | u_t, X_{t-1} = x_i) · p_{i,t-1}

# correction: 측정 likelihood로 가중치 부여
    p_{k,t} = η · p(z_t | X_t = x_k) · p̄_{k,t}
endfor
return {p_{k,t}}
```

이 알고리즘은 음성인식의 HMM forward algorithm과 동일한 구조를 가진다. 상태공간이 자연스럽게 이산인 문제(문 열림/닫힘, semantic class 분류)에서는 여전히 최단 경로다.

#### 연속 상태: 히스토그램 필터

연속 상태공간을 유한개 영역 $\{\mathbf{x}_{k,t}\}$로 분할하고, 각 영역 안에서 belief가 균일(piecewise uniform)하다고 가정한다.

$$p(x_t) = \frac{p_{k,t}}{|\mathbf{x}_{k,t}|} \quad x_t \in \mathbf{x}_{k,t}$$

영역 대표값(평균 상태) $\hat x_{k,t}$로 모델을 근사한다.

$$p(z_t \mid \mathbf{x}_{k,t}) \approx p(z_t \mid \hat x_{k,t})$$
$$p(\mathbf{x}_{k,t} \mid u_t, \mathbf{x}_{i,t-1}) \approx \eta\,|\mathbf{x}_{k,t}|\, p(\hat x_{k,t} \mid u_t, \hat x_{i,t-1})$$

영역 크기가 모두 같으면 $|\mathbf{x}_{k,t}|$ 인수는 정규화에 흡수된다. 이렇게 만든 이산 베이즈 필터를 **히스토그램 필터**라 부른다.

**한계:** 차원의 저주로 5차원 이상에서 실용성이 급락한다. 6-DoF pose 추정에는 부적합하다. 분해 기법으로는 density tree(상태 밀도에 따른 불균일 분할), selective updating(변화 있는 영역만 갱신), 토폴로지컬/메트릭 혼합 표현 등이 제안된다.

ch.14 occupancy grid mapping이 이 히스토그램 필터의 직접적 후계다.

### 3.11.2 이진 베이즈 필터 (Binary Bayes Filter, Log-Odds 형식)

#### 정적 상태의 이진 추정

시간에 변하지 않는 이진 상태(예: "이 셀이 점유되었는가?")를 추정할 때, 상태 천이 모델이 없으므로 prediction 단계가 사라진다. Correction 단계만 반복하면 된다.

그런데 매 측정마다 사후 확률 $p(x \mid z_{1:t})$를 직접 계산하면, likelihood 곱셈이 누적되면서 수치 언더플로우 위험이 있다. 또 [0, 1] 구간 절단도 처리해야 한다. **Log-odds 표현**이 이를 해결한다.

$$l(x) := \log \frac{p(x)}{1 - p(x)} \in (-\infty, +\infty)$$

Log-odds는 $(-\infty, +\infty)$ 실수 전체를 값역으로 가지므로 절단 문제가 없다. 곱셈적 Bayes 갱신이 **덧셈적**이 된다.

$$l_t = l_{t-1} + \log\frac{p(x \mid z_t)}{1 - p(x \mid z_t)} - l_0$$

여기서 $l_0 = \log\frac{p(x)}{1-p(x)}$는 prior log-odds다.

Belief 복원: $\text{bel}_t(x) = 1 - \dfrac{1}{1 + \exp(l_t)}$

```
# Algorithm Binary_Bayes_filter (Table 4.2 의역)
# 입력: l_{t-1}, z_t
# 출력: l_t
# (정적 상태 가정: prediction 없음)

1: l_t = l_{t-1}
         + log( p(x|z_t) / (1 - p(x|z_t)) )   # inverse sensor model로 측정 반영
         - log( p(x) / (1 - p(x)) )            # prior 차감 (이중 계산 방지)
return l_t
```

#### Inverse sensor model

순방향 측정 모델 $p(z \mid x)$를 역방향으로 뒤집은 $p(x \mid z)$를 **inverse sensor model**이라 한다. 카메라로 "문이 열려 보이면" 셀이 비어있을 확률처럼, 역방향 모델이 순방향보다 짜기 쉬울 때가 있다. 이진 베이즈 필터는 이 inverse model을 직접 입력으로 받는다.

log-odds 갱신식은 ch.14 Occupancy Grid Mapping에서 그대로 셀별로 적용된다.

### 3.11.3 입자 필터 (Particle Filter)

#### 비모수 표현의 원리

히스토그램 필터의 격자는 차원이 늘면 지수적으로 커진다. 입자 필터는 격자 대신 표본으로 분포를 근사해 이 문제를 우회한다.

입자 필터는 belief를 $M$개의 무작위 표본(입자)으로 표현한다.

$$\mathcal{X}_t = \{x_t^{[1]},\, x_t^{[2]},\, \ldots,\, x_t^{[M]}\}$$

입자 $x_t^{[m]}$들은 belief가 높은 곳에 더 밀집한다. 가우시안 가정 없이 임의 형태의 분포 — 다봉 분포, 긴 꼬리 분포 — 를 표현할 수 있다.

#### 입자 필터 알고리즘: sampling → weighting → resampling

```
# Algorithm Particle_filter (Table 4.3 의역)
# 입력: X_{t-1}, u_t, z_t
# 출력: X_t (M개 입자)

X̄_t = X_t = ∅
for m = 1 to M do
    # Step 1: sampling — 모션 모델로 각 입자 전개
    x_t^[m] ~ p(x_t | u_t, x_{t-1}^[m])

# Step 2: weighting — 측정 likelihood로 중요도 가중치 계산
    w_t^[m] = p(z_t | x_t^[m])

X̄_t = X̄_t ∪ {x_t^[m], w_t^[m]}
endfor

for m = 1 to M do
    # Step 3: resampling — 가중치에 비례하여 M개 다시 뽑기
    draw i with probability ∝ w_t^[i] from X̄_t
    add x_t^[i] to X_t
endfor
return X_t
```

이상적으로 $M \to \infty$에서 $x_t^{[m]} \sim p(x_t \mid z_{1:t}, u_{1:t})$로 수렴한다.

#### 중요도 샘플링(Importance Sampling) 직관

목표 분포 $f$에서 직접 샘플링이 어려울 때, proposal 분포 $g$에서 뽑고 가중치 $w = f/g$로 보정한다.

$$w^{[m]} = \frac{f(x^{[m]})}{g(x^{[m]})}$$

가중 경험분포는 임의 Borel 집합 $A$에 대해:
$$\left[\sum_{m=1}^M w^{[m]}\right]^{-1} \sum_{m=1}^M \mathbf{1}(x^{[m]} \in A)\, w^{[m]} \;\longrightarrow\; \int_A f(x)\, dx$$

수렴률은 $O(1/\sqrt{M})$. Proposal과 target이 비슷할수록 상수가 작아진다.

입자 필터에서 proposal은 모션 모델 $p(x_t \mid u_t, x_{t-1})$로 각 입자를 전개하고, target은 측정까지 반영한 $\text{bel}(x_t)$다. 측정 $z_t$를 반영하지 않은 proposal과 반영한 target 사이의 "빠진 정보"가 $p(z_t \mid x_t^{[m]})$이고, 이것이 라인 Step 2의 가중치를 직관적으로 정당화한다.

이 직관이 왜 정확히 $p(z_t \mid x_t^{[m]})$으로 떨어지는지는 시퀀스 공간에서 엄밀히 보면 분명해진다.

#### 수렴 및 구현

이 직관의 엄밀한 유도는 입자를 단일 시점 $x_t^{[m]}$이 아니라 상태 시퀀스 $x_{0:t}^{[m]}$으로 볼 때 깔끔하게 나온다. Target의 시퀀스 분해(Bayes + Markov 두 번)와 proposal의 귀납 분해의 비율이 $\eta\, p(z_t \mid x_t^{[m]})$으로 정확하게 떨어진다. 단, $M \to \infty$에서만 정확하다.

구현: resampling이 없으면 가중치가 소수의 입자에 집중되는 **weight degeneracy**가 발생한다 — 이것이 Step 3가 필요한 이유다. §3.11.4에서 입자 필터의 오차 원천을 다룬다.

### 3.11.4 입자 필터의 4가지 오차 원천

입자 필터는 근사이므로 구조적 오차가 있다. Resampling은 가중치 기반 selection으로, 낮은 가중치 입자가 덜 선택되게 한다. 입자 필터의 4가지 오차 원천을 이해하면 PF 디버깅이 체계적으로 된다.

#### (1) 유한 $M$의 체계적 편향 (Systematic Bias)

$M = 1$인 극단을 상상하라. weight가 자기 자신으로 정규화되어 $w/w = 1$이 된다. 결과적으로 센서 측정 정보가 완전히 무시되는 셈이다. $M$이 유한하면 weight들이 $M-1$차원 simplex에 국한되어 무작위 오차가 누적된다. $M$이 커질수록 편향이 줄지만 계산 비용은 선형으로 증가한다.

#### (2) Resampling으로 인한 다양성 상실 (Sample Impoverishment)

정적 상태($x_t = x_{t-1}$)에서 전이 잡음이 없으면 입자는 각자의 초기 상태를 그대로 유지한다. 문제는 그다음이다. 새 다양성이 주입되지 않는 상태에서 재표집을 반복하면 표집 분산 때문에 살아남는 입자의 종류가 줄어들어(sample impoverishment) 결국 단일 상태로 붕괴된다.

완화: 로봇이 정지하면 resampling을 보류한다. 또는 weight variance가 높을 때만 resampling하고, 나머지 시간에는 weight를 곱셈적으로 누적한다.

$$w_t^{[m]} = \begin{cases} 1 & \text{(resampling 직후)} \\ p(z_t \mid x_t^{[m]})\, w_{t-1}^{[m]} & \text{(resampling 없을 때)} \end{cases}$$

#### (3) Proposal-Target 괴리 (Proposal-Target Divergence)

센서가 매우 정확하고 모션이 부정확하면, target belief는 좁고 proposal은 넓어 효율이 급락한다. 극단적 경우: 노이즈 없는 range sensor라면 $p(z \mid x)$의 지지가 저차원 다양체에 국한되어 대부분의 입자가 weight $\approx 0$을 받는다.

완화: 측정 노이즈를 일부러 키우거나(정밀도 손실 대가), measurement-aware proposal을 사용해 측정 정보를 sampling 단계에 통합한다.

#### (4) Particle Deprivation

고차원 공간에서 참 상태 근처에 입자가 하나도 없을 수 있다. Resampling의 무작위성이 참 상태 근처 입자를 한 cycle에 쓸어버릴 확률이 매 cycle마다 0보다 크다. 한 번 잃으면 복구가 어렵다.

완화: 매 cycle마다 prior에서 소량 **random injection**으로 새 입자를 주입한다. Posterior를 약간 왜곡하는 대가로 재난적 실패를 방지한다.

#### Low-Variance Sampler

Resampling의 표준 구현은 **low-variance (systematic) sampler**다. 난수 1개로 $M$개를 일정 간격으로 추출하여 $O(M)$ 복잡도를 달성한다.

```
# Algorithm Low_variance_sampler (Table 4.4 의역)
# 입력: X̄_t (가중 입자), W_t (가중치 배열)
# 출력: X̄_t (재샘플된 입자)

r = rand(0, M^{-1})    # [0, 1/M) 균등 난수 단 1개
c = w_t^[1]             # 누적 가중치
i = 1
X̄_t = ∅
for m = 1 to M do
    u = r + (m-1) · M^{-1}   # 일정 간격으로 위치 이동
    while u > c do
        i = i + 1
        c = c + w_t^[i]       # 누적 가중치 누가
    endwhile
    add x_t^[i] to X̄_t       # 해당 위치의 입자 선택
endfor
return X̄_t
```

핵심: 가중치가 모두 같으면 결과가 입력과 동일하다 — 측정 통합이 없는 step에서 입자를 잃지 않는다. 독립 샘플링 $O(M \log M)$ 대비 $O(M)$.

여기까지 오면 입자 필터가 잘 작동하는 이유와 한계가 드러난다. 4가지 오차 원천마다 완화 기법이 있으며, 상황에 맞는 트레이드오프를 고르는 일이 실전 구현의 핵심이다. ch.14 §14.7에서는 이 4가지 오차에 대응해 augmented MCL·mixture MCL에서 쓰는 완화 기법을 다룬다.

> **추천 자료**
> - [Thrun, Burgard, Fox — Probabilistic Robotics (2005)](https://www.probabilistic-robotics.org/) — Ch.4 전체가 이 절의 원전. 히스토그램, 이진 베이즈, 입자 필터의 알고리즘·분석이 완결적으로 서술되어 있다.
> - [Arulampalam et al. — A Tutorial on Particle Filters (IEEE Trans. Signal Processing 2002)](https://ieeexplore.ieee.org/document/978374) — 입자 필터의 이론과 응용을 종합한 표준 튜토리얼.
> - [Thrun — Particle Filters in Robotics (UAI 2002)](https://www.aaai.org/Papers/UAI/2002/UAI02-079.pdf) — Rao-Blackwellized PF·FastSLAM과의 연결을 설명하는 짧은 논문.
> - [ROS AMCL package](https://wiki.ros.org/amcl) — §3.11.3~3.11.4의 입자 필터 이론이 실제 구현된 패키지. augmented MCL, low-variance sampler가 그대로 적용되어 있다.

---

베이즈 필터가 가장 기초적인 확률론적 골격을 제공한다면, KF·EKF·IF는 가우시안 가정 위에서 이를 닫힌 형식으로 푼 해법이며, 히스토그램 필터와 입자 필터는 가우시안 가정을 내려놓는 대신 연산 비용을 지불하여 비모수적 유연성을 확보한 해법이다. 어떤 필터를 고를지는 상태공간 차원과 분포의 다봉 여부가 결정한다.

ch.14에서 EKF localization(§14.7)과 MCL(§14.7)을 만날 때, 알고리즘마다 새 유도를 따라갈 필요는 없다. 여기서 쌓은 필터 언어로 각 알고리즘의 $g$, $h$, proposal이 무엇인지 확인하면 골격이 바로 보인다. IMU preintegration(§14.10)은 필터 구조와 구별되는 기법으로, 관측치를 최적화 factor로 압축하는 방안으로 따로 다룬다.

---

# Ch.4 — 기구학 & 메카트로닉스 (Kinematics & Mechatronics)

로봇 팔 하나를 책상 위에 올려놓았다고 하자. 모터 6개에 각각 어떤 각도를 줘야 손끝이 커피잔에 닿는가? 이 질문에 답하는 학문이 기구학이다. 그리고 그 모터를 실제로 돌리고, 센서를 읽고, 제어 루프를 1kHz로 돌리는 현실의 문제가 메카트로닉스이다.

기구학의 수식은 하드웨어 선정과 통신 프로토콜을 거쳐 실제 로봇의 동작으로 이어진다.

---

## 4.1 왜 기구학을 배우는가

로봇 매니퓰레이터는 여러 관절(joint)과 링크(link)로 이루어진다. 우리가 원하는 것은 끝단(end-effector)의 위치와 자세(pose)이지만, 직접 제어하는 값은 각 관절의 각도(또는 변위)다.

이 둘 사이의 관계를 수학적으로 기술하는 것이 **기구학(Kinematics)**이다.

- 순기구학(Forward Kinematics, FK): 관절 각도 → 끝단 위치/자세
- 역기구학(Inverse Kinematics, IK): 끝단 위치/자세 → 관절 각도

동역학(Dynamics)과 다르다. 기구학은 힘과 질량을 고려하지 않는다. 어디에 위치하는가의 기하학적 문제를 다루며, 어떤 크기의 힘이나 토크가 필요한지의 역학적 문제는 배제한다. 동역학은 다음 장에서 다룬다.

기구학은 다음 작업에 쓰인다:
- 로봇 팔 경로 계획 (motion planning)
- 텔레오퍼레이션 (원격 조종 시 마스터-슬레이브 매핑)
- 캘리브레이션 (실제 로봇과 모델 사이 오차 보정)
- 충돌 회피 (각 링크가 공간 어디에 있는지 알아야 피한다)

---

## 4.2 순기구학 (Forward Kinematics)

### 4.2.1 동차 변환 행렬 (Homogeneous Transformation Matrix)

기구학의 기본 도구는 4×4 동차 변환 행렬이다:

```
T = | R  p |
    | 0  1 |
```

여기서 R은 3×3 회전 행렬, p는 3×1 위치 벡터이다. 동차 변환 행렬 하나로 강체의 위치와 자세를 함께 표현하고, 여러 변환을 행렬 곱으로 연쇄(chain)할 수 있다.

두 프레임 사이의 변환 T_01이 있고, 또 다른 변환 T_12가 있으면:

```
T_02 = T_01 * T_12
```
순기구학은 이 변환을 베이스에서 로봇 끝단까지 차례로 적용한다.

관절 각도가 주어졌을 때, 로봇 끝단(End-Effector)의 위치와 자세를 구하는 문제이다.

```
q = [θ_1, θ_2, ..., θ_n]^T  →  T = [R, t; 0, 1] ∈ SE(3)
```

FK는 **유일한 해**를 갖는다. 관절 각도가 정해지면 끝단 위치는 물리적으로 하나만 존재한다.

### 4.2.2 Denavit-Hartenberg (DH) 파라미터

로봇 링크 사이의 상대적 위치 관계를 4개의 파라미터로 표준화한 방법이다:

| 파라미터 | 의미 | 설명 |
|---------|------|------|
| $a_i$ | 링크 길이 (link length) | $z_{i-1}$과 $z_i$ 사이의 $x_i$ 축을 따른 거리 |
| $\alpha_i$ | 링크 꼬임각 (link twist) | $z_{i-1}$과 $z_i$ 사이의 $x_i$ 축 기준 회전각 |
| $d_i$ | 링크 오프셋 (link offset) | $x_{i-1}$과 $x_i$ 사이의 $z_{i-1}$ 축을 따른 거리 |
| $\theta_i$ | 관절각 (joint angle) | $x_{i-1}$과 $x_i$ 사이의 $z_{i-1}$ 축 기준 회전각 |

회전 관절(revolute joint)의 경우 $\theta_i$만이 자유도로 변하며, 나머지 3개 DH 파라미터는 고정된 상수로 유지된다.
직선 관절(prismatic joint)에서는 d_i가 변수이다.

각 관절의 변환 행렬:

```
T_i = Rot_z(θ_i) * Trans_z(d_i) * Trans_x(a_i) * Rot_x(α_i)

= | cos(θ)  -sin(θ)cos(α)   sin(θ)sin(α)   a*cos(θ) |
      | sin(θ)   cos(θ)cos(α)  -cos(θ)sin(α)   a*sin(θ) |
      | 0        sin(α)          cos(α)          d        |
      | 0        0               0               1        |
```

주의: DH convention에는 "standard"와 "modified (Craig convention)" 두 가지가 있다. Craig 교과서를 쓴다면 modified DH를 보게 되고, 많은 다른 교재는 standard DH를 사용한다. 둘은 프레임 부착 방식이 다르다. 혼용하면 결과가 틀리니 어떤 convention을 쓰는지 항상 명시해야 한다.

### 4.2.3 예제: 2-link Planar Arm의 FK

가장 간단한 예제부터 하자. 평면 위의 2-링크 로봇 팔이다.

```
       q1         q2
  O────────O────────O → end-effector
  (base)   L1       L2
```

DH 테이블 (standard convention):

| Link | a    | α   | d   | θ    |
|------|------|-----|-----|------|
| 1    | L1   | 0   | 0   | θ_1  |
| 2    | L2   | 0   | 0   | θ_2  |

끝단 위치는 단순히 삼각함수로 유도된다:

```
x = L1*cos(θ_1) + L2*cos(θ_1 + θ_2)
y = L1*sin(θ_1) + L2*sin(θ_1 + θ_2)
```

Python으로 구현하면:

```python
import numpy as np

def fk_2link(theta1, theta2, L1=1.0, L2=1.0):
    """2-link planar arm의 순기구학."""
    x = L1 * np.cos(theta1) + L2 * np.cos(theta1 + theta2)
    y = L1 * np.sin(theta1) + L2 * np.sin(theta1 + theta2)
    phi = theta1 + theta2  # 끝단의 절대 방향
    return x, y, phi

# θ_1=30°, θ_2=45°, 링크 길이 각각 1m
x, y, phi = fk_2link(np.radians(30), np.radians(45))
print(f"End-effector position: ({x:.3f}, {y:.3f}), orientation: {np.degrees(phi):.1f}°")
# 출력: End-effector position: (1.125, 1.466), orientation: 75.0°
```

단순해 보인다면 맞다. 실제 6축 로봇 팔의 FK도 원리는 같다. 4×4 행렬을 6번 곱하면 된다.

### 4.2.4 Product of Exponentials (PoE)

DH 파라미터의 대안으로, Lie group/Lie algebra에 기반한 PoE (Product of Exponentials) 방법이 있다. Lynch & Park의 "Modern Robotics"에서 채택한 방법이다.

PoE는 각 관절을 twist(나선 운동)로 표현하고, 행렬 지수(matrix exponential)로 변환을 계산한다.

```
T(θ) = e^{[S_1]θ_1} * e^{[S_2]θ_2} * ... * e^{[S_n]θ_n} * M
```

여기서:
- S_i는 i번째 관절의 screw axis (6×1 벡터)
- [S_i]는 S_i의 4×4 행렬 표현 (se(3) 원소이며, 왼쪽 위 3×3 회전 블록이 skew-symmetric)
- M은 모든 관절이 영 위치(home configuration)일 때의 끝단 자세
- θ_i는 관절 변수

DH vs PoE 비교:

| 항목 | DH | PoE |
|------|-----|-----|
| 프레임 부착 | 각 링크에 프레임 필요 | 기준 프레임과 끝단 프레임만 필요 |
| Convention 혼동 | standard vs modified 주의 | 없음 (space form vs body form 구분은 있음) |
| 수학적 기반 | 행렬 곱 | Lie group, 행렬 지수 |
| 특이점 분석 | 별도 처리 필요 | 자연스럽게 통합 |
| 산업계 채택 | 매우 높음 | 학계 중심, 점점 확산 |
| 교재 | Craig, Siciliano | Lynch & Park |

DH 파라미터는 교재와 산업용 로봇 매뉴얼에서 흔히 쓰인다. URDF는 같은 링크·관절 변환을 직접 기술한다. PoE는 Lie group에 기반한 정돈된 표현으로 연구에서 널리 쓰인다. 매뉴얼, 로봇 기술 파일, 수식 유도 사이를 오가려면 두 관례의 대응을 알아두는 편이 좋다.

```python
# robotics-toolbox-python으로 DH 기반 FK 예제 (Puma 560)
import roboticstoolbox as rtb

puma = rtb.models.DH.Puma560()
q = [0, -np.pi/4, np.pi/4, 0, np.pi/6, 0]  # 6개 관절 각도
T = puma.fkine(q)
print(T)  # 4x4 SE(3) 동차 변환 행렬 출력
print(f"Position: {T.t}")  # 끝단 위치
print(f"RPY angles: {T.rpy()}")  # Roll-Pitch-Yaw
```

> **추천 자료**
> - Lynch & Park, *Modern Robotics*, Chapter 4 — PoE를 중심으로 설명하며 무료 PDF와 Coursera 강의를 제공한다: https://modernrobotics.org
> - Craig, *Introduction to Robotics*, Chapter 3 — Modified DH convention을 사용하는 교재
> - Peter Corke, *Robotics, Vision and Control* — Python 코드와 함께 FK를 실습할 수 있다: https://github.com/petercorke/robotics-toolbox-python

---

## 4.3 역기구학 (Inverse Kinematics)

FK는 쉽다. 행렬 곱이면 된다. 문제는 IK이다.

"끝단을 (x, y, z)에 놓고 싶은데, 관절 각도를 각각 얼마로 해야 하는가?"

이 문제에는 네 가지 어려움이 있다. 삼각함수가 얽힌 비선형 방정식이며, 같은 끝단 위치에 도달하는 관절 각도 조합도 여러 개일 수 있다(elbow-up, elbow-down). Workspace 밖의 점에는 아예 해가 없고, 자유도가 남으면(redundant manipulator) 해가 무한히 많다.

### 4.3.1 Analytical IK (해석적 방법)

닫힌 형태(closed-form)의 해를 구하는 방법이다. 해가 존재하면 반복 최적화 없이 후보를 계산할 수 있지만, 수치 정확도와 실행 시간은 구현과 특이점 처리에 따라 달라진다.

**2-link planar arm의 IK:**

목표 위치 (x, y)가 주어졌을 때:

```
cos(θ_2) = (x² + y² - L1² - L2²) / (2 * L1 * L2)
θ_2 = atan2(±√(1 - cos²(θ_2)), cos(θ_2))

θ_1 = atan2(y, x) - atan2(L2*sin(θ_2), L1 + L2*cos(θ_2))
```

±에서 보듯이 해가 두 개다(elbow-up, elbow-down). 여러 해가 존재한다는 점이 IK를 어렵게 만든다.

```python
def ik_2link(x, y, L1=1.0, L2=1.0, elbow_up=True):
    """2-link planar arm의 역기구학. 해가 없으면 None 반환."""
    d_sq = x**2 + y**2
    # 도달 가능 여부 체크
    if d_sq > (L1 + L2)**2 or d_sq < (L1 - L2)**2:
        return None

cos_q2 = (d_sq - L1**2 - L2**2) / (2 * L1 * L2)
    cos_q2 = np.clip(cos_q2, -1.0, 1.0)  # 수치 안전

if elbow_up:
        q2 = np.arctan2(np.sqrt(1 - cos_q2**2), cos_q2)
    else:
        q2 = np.arctan2(-np.sqrt(1 - cos_q2**2), cos_q2)

q1 = np.arctan2(y, x) - np.arctan2(L2 * np.sin(q2), L1 + L2 * np.cos(q2))
    return q1, q2

# 검증: FK → IK → FK
target_x, target_y = 1.2, 0.8
result = ik_2link(target_x, target_y)
if result:
    q1, q2 = result
    x_check, y_check, _ = fk_2link(q1, q2)
    print(f"Target: ({target_x}, {target_y})")
    print(f"IK solution: q1={np.degrees(q1):.2f}°, q2={np.degrees(q2):.2f}°")
    print(f"FK check: ({x_check:.6f}, {y_check:.6f})")
    print(f"Error: {np.sqrt((x_check-target_x)**2 + (y_check-target_y)**2):.2e}")
```

**6R 매니퓰레이터의 해석적 IK:**

6축 로봇 중 Pieper의 조건을 만족하는 구조 — 마지막 3축이 한 점에서 만나는(spherical wrist) 경우 — 는 해석적으로 풀 수 있다. KUKA, ABB 등 전통적 산업용 6축 로봇이 이 구조이다. UR 팔은 손목 3축이 한 점에서 만나지 않는 오프셋 손목이라 닫힌 해가 있어도 별도 유도가 필요하다.

이 경우 위치 문제(처음 3축)와 자세 문제(마지막 3축)를 분리하여 풀 수 있다. 최대 8개의 해가 존재하며, 관절 제한(joint limits)과 이전 관절 각도에 가까운 해를 선택하는 것이 일반적이다.

### 4.3.2 Numerical IK (수치적 방법)

해석적 해가 불가능한 경우 (복잡한 구조, 7축 이상, 비표준 구조) 수치적으로 풀어야 한다. 반복적 최적화 문제이다.

**Jacobian Pseudo-Inverse 방법:**

```
Δq = J†(q) * Δx
```

여기서 J†는 자코비안의 pseudo-inverse이다. 이를 반복하여 목표에 수렴한다.

```python
def numerical_ik_2link(target_x, target_y, L1=1.0, L2=1.0,
                        max_iter=100, tol=1e-6):
    """Jacobian pseudo-inverse 기반 수치적 IK."""
    # 초기 추정값 (랜덤 또는 현재 관절 각도)
    q = np.array([0.5, 0.5])

for i in range(max_iter):
        # 현재 FK
        x = L1 * np.cos(q[0]) + L2 * np.cos(q[0] + q[1])
        y = L1 * np.sin(q[0]) + L2 * np.sin(q[0] + q[1])

# 오차
        error = np.array([target_x - x, target_y - y])
        if np.linalg.norm(error) < tol:
            print(f"수렴: {i+1}회 반복")
            return q

# 자코비안
        J = np.array([
            [-L1*np.sin(q[0]) - L2*np.sin(q[0]+q[1]), -L2*np.sin(q[0]+q[1])],
            [ L1*np.cos(q[0]) + L2*np.cos(q[0]+q[1]),  L2*np.cos(q[0]+q[1])]
        ])

# Pseudo-inverse로 관절 각도 업데이트
        dq = np.linalg.pinv(J) @ error
        q += dq

print("수렴 실패")
    return q
```

**Damped Least Squares (DLS, Levenberg-Marquardt):**

Pseudo-inverse의 문제는 특이점 근처에서 관절 속도가 폭발한다는 것이다. DLS는 damping factor λ를 추가하여 이를 완화한다:

```
Δq = J^T (J * J^T + λ²I)^{-1} * Δx
```

λ가 크면 특이점 근처에서 안정적이지만 수렴이 느리고, λ가 작으면 pseudo-inverse에 가까워진다. 적응적으로 λ를 조절하는 방법(Nakamura & Hanafusa, 1986)이 실무에서 많이 쓰인다.

### 4.3.3 특이점 (Singularity)

자코비안의 rank가 부족해지는 관절 배치를 특이점(singularity)이라 한다. 특이점에서는 특정 방향으로 끝단을 전혀 움직일 수 없고, 미소 이동에도 관절 속도가 폭발하며, IK 해가 불연속적이어서 경로 추종 시 관절이 급격히 점프한다.

2-link arm의 특이점은 간단하다: θ_2 = 0 (팔이 완전히 펴진 경우) 또는 θ_2 = π (완전히 접힌 경우). 이 순간 매니퓰레이터 끝단은 접선 방향으로만 순간적인 이동이 가능하며, 반지름 방향으로는 속도를 전혀 생성하지 못한다.

6축 로봇의 대표적 특이점:
- Wrist singularity: 축 4와 6이 정렬됨 (q5 ≈ 0)
- Shoulder singularity: 끝단이 축 1 위에 위치
- Elbow singularity: 팔이 완전히 펴짐

실무 대처법:
- 특이점 근처를 피하는 경로 계획
- DLS 방법으로 특이점 통과 시 속도 제한
- Redundancy(여분의 자유도) 활용

### 4.3.4 IK 솔버들

직접 IK를 구현할 일은 드물다. 검증된 솔버를 사용하는 것이 현명하다.

| 솔버 | 방법 | 특징 |
|------|------|------|
| KDL | Numerical (Newton-Raphson) | ROS 생태계에서 제공, 관절 한계·초기값·특이점에 민감 |
| IKFast (OpenRAVE) | Analytical (코드 생성) | 특정 구조에 대해 C++ 코드 자동 생성. 빠름 |
| TRAC-IK | KDL + SQP 듀얼 | KDL보다 성공률 높음, ROS 패키지 존재 |
| MoveIt2 IK | 위 솔버들을 통합 | ROS2 생태계, 충돌 회피 통합 |
| pinocchio | 자코비안 기반 수치 반복 (CLIK) | 강체 동역학 라이브러리, 빠름, 해석적 미분 제공 |

```python
# Beeson & Ames (2015)는 5개 로봇 모델의 도달 가능한 자세를
# 모델별 10,000개씩, 해 하나당 5ms 제한으로 비교했다.
# 그 실험에서는 TRAC-IK가 stock KDL보다 높은 solve rate를 보였지만,
# 수치는 관절 체인·초기값·허용 오차에 따라 달라진다.
```

> **추천 자료**
> - [Beeson & Ames, "TRAC-IK: An Open-Source Library for Improved Solving of Generic Inverse Kinematics" (2015)](https://doi.org/10.1109/HUMANOIDS.2015.7363472) — 모델별 조건과 solve rate는 원 논문의 표를 확인
> - MoveIt2 IK 문서: https://moveit.picknik.ai/main/doc/concepts/inverse_kinematics.html
> - Pinocchio (rigid body dynamics library): https://github.com/stack-of-tasks/pinocchio

---

## 4.4 자코비안 (Jacobian)

자코비안은 기구학에서 가장 많이 쓰이는 도구 중 하나이다. FK가 "위치"의 문제라면, 자코비안은 "속도"의 문제이다.

### 4.4.1 관절 속도 → 끝단 속도

끝단 속도(선속도 v, 각속도 ω)와 관절 속도 q̇의 관계:

```
ẋ = J(q) * q̇

여기서 ẋ = [v; ω] ∈ ℝ^6 (6축의 경우)
      q̇ ∈ ℝ^n
      J(q) ∈ ℝ^{6×n}
```

6차원 끝단 운동에 대해 n < 6이면 관절 자유도가 부족하고, n = 6이면 자유도 수가 같으며, n > 6이면 여유 자유도가 있다. 실제로 가능한 운동은 J(q)의 rank에 달려 있으며, underactuated 여부는 독립 구동 입력 수와 시스템 자유도를 비교해 판단한다.

차륜 mobile robot의 경우 관절 속도가 아닌 차체 속도 $(v, \omega)$가 주 제어이며, 노이즈가 동반된 형태는 §4.7 확률적 운동 모델 참조.

### 4.4.2 힘/토크 관계 (Duality)

자코비안의 전치(transpose)는 끝단 힘을 관절 토크로 매핑한다:

```
τ = J^T(q) * F
```

여기서 τ는 관절 토크, F는 끝단에 작용하는 힘/모멘트이다.

이것이 **정역학적 이중성(static duality)**이다. 속도와 힘은 자코비안과 그 전치를 통해 쌍대 관계를 이룬다. 파워 보존 원리에서 자연스럽게 유도된다:

```
P = F^T * ẋ = F^T * J * q̇ = (J^T * F)^T * q̇ = τ^T * q̇
```

이 관계는 힘 제어(force control)에서 핵심적이다. 끝단에 원하는 힘 F를 가하려면, 각 관절에 τ = J^T * F의 토크를 인가하면 된다.

### 4.4.3 Manipulability Ellipsoid

자코비안은 로봇이 현재 자세에서 "얼마나 잘 움직일 수 있는지"도 알려준다.

```
manipulability index = √det(J * J^T)
```

6차원 작업에 대해 J가 평소 full row rank를 갖는 로봇에서는 이 값이 0이면 특이점이다. 값이 클수록 조작성 타원체의 부피가 크지만, 모든 방향으로 고른지는 축 길이의 비를 따로 보아야 한다.

J * J^T의 고유값(eigenvalue)과 고유벡터(eigenvector)로 타원체(ellipsoid)를 그릴 수 있다. 고유값이 크면 그 방향으로 빠르게 움직일 수 있고, 작으면 느리다. 고유값이 모두 비슷하면 등방적(isotropic)이고, 차이가 크면 비등방적이다.

```python
import roboticstoolbox as rtb
import numpy as np

# Puma 560의 자코비안과 manipulability
puma = rtb.models.DH.Puma560()
q = [0, -np.pi/4, np.pi/4, 0, np.pi/6, 0]

J = puma.jacob0(q)  # 6x6 자코비안 (기저 프레임 기준)

# Manipulability index
m = np.sqrt(np.linalg.det(J @ J.T))
print(f"Manipulability index: {m:.4f}")

# 속도 타원체의 주축 (고유값 분석)
JJT = J[:3, :] @ J[:3, :].T  # 선속도 부분만
eigenvalues, eigenvectors = np.linalg.eigh(JJT)
print(f"Velocity ellipsoid semi-axes: {np.sqrt(eigenvalues)}")

# Condition number: 등방성 지표 (1에 가까울수록 좋다)
sigma = np.linalg.svd(J, compute_uv=False)
cond = sigma[0] / sigma[-1]
print(f"Condition number: {cond:.2f}")
# cond가 1이면 완벽한 등방성, 무한대면 특이점
```

### 4.4.4 실용 코드: 자코비안 기반 속도 제어

```python
import numpy as np

def jacobian_velocity_control(robot_fk, robot_jacob, q_current,
                               desired_twist, dt=0.001):
    """
    자코비안 기반 분해 속도 제어 (resolved rate control).

Args:
        robot_fk: FK 함수 (q -> SE3)
        robot_jacob: 자코비안 함수 (q -> 6xn matrix)
        q_current: 현재 관절 각도
        desired_twist: 원하는 끝단 속도 [vx, vy, vz, wx, wy, wz]
        dt: 제어 주기
    Returns:
        q_new: 새 관절 각도
    """
    J = robot_jacob(q_current)

# Damped least squares
    lambda_dls = 0.01
    n = J.shape[1]
    JJT = J @ J.T
    J_dls = J.T @ np.linalg.inv(JJT + lambda_dls**2 * np.eye(JJT.shape[0]))

q_dot = J_dls @ desired_twist

# 관절 속도 제한 (실제 로봇에서 필수)
    max_qdot = 2.0  # rad/s
    scale = np.max(np.abs(q_dot)) / max_qdot
    if scale > 1.0:
        q_dot /= scale

q_new = q_current + q_dot * dt
    return q_new
```

> **추천 자료**
> - Siciliano et al., *Robotics: Modelling, Planning and Control*, Chapter 3 — 자코비안을 기구학·동역학 맥락에서 폭넓게 설명
> - Corke, *Robotics, Vision and Control*, Chapter 8 — 코드 예제와 시각화 포함: https://petercorke.com/rvc/
> - robotics-toolbox-python 문서: https://github.com/petercorke/robotics-toolbox-python

---

## 4.5 메카트로닉스 기초

관절 각도를 정한다고 로봇이 움직이는 것은 아니다. 모터와 센서, 그 사이를 연결하는 전자 회로와 통신이 있어야 한다. 이것이 메카트로닉스다.

### 4.5.1 액추에이터

**DC 모터:**
가장 기본적인 액추에이터. 전압을 가하면 회전한다. 토크는 전류에 비례하고 (τ = K_t * i), 역기전력은 속도에 비례한다 (V_emf = K_e * ω). 제어가 쉽고 가격이 저렴하지만, 브러시 마모가 있다.

**BLDC (Brushless DC) 모터:**
브러시 없이 전자적으로 전류를 전환한다. 수명이 길고 토크 밀도와 효율이 높아 현대 로봇에서 자주 선택된다. FOC(Field-Oriented Control) 기법을 적용하여 회전 시 발생하는 토크 리플(ripple)을 효과적으로 억제한다.

**서보 모터 (Dynamixel 시리즈):**
모터 + 감속기 + 엔코더 + 컨트롤러를 일체형으로 묶은 제품이다. Robotis의 Dynamixel은 연구·교육용 플랫폼에서 널리 쓰이는 서보 제품군이다.

| 모델 | 공칭 최대 토크 예시 (Nm) | 통신 | 용도 |
|------|-----------|------|------|
| XL330 | 0.5 | TTL | 소형 그리퍼, 소형 저가 팔 |
| XM540 | 10.0 | RS-485 | 중형 로봇 팔 |
| PH54  | 44.7 | RS-485 | 대형 매니퓰레이터, 모바일 로봇 |

표의 토크는 모델과 공급 전압에 따라 달라지므로 실제 선정에는 각 e-Manual의 정격·stall 조건과 연속 운전 한계를 확인해야 한다. Dynamixel의 장점은 데이지 체인 연결, 위치/속도/전류 기반 제어 모드, PID 게인 조절이다. 단점은 제품별 통신·제어 주기와 열 한계이고, 필요한 대역폭과 제어 모드가 기본 펌웨어에서 지원되는지 먼저 확인해야 한다.

**Quasi-Direct Drive (QDD):**

MIT Mini Cheetah(2019)로 주목받은 방식으로, 감속비를 낮춘다.

일반적인 로봇 관절: 감속비 100:1 이상 (harmonic drive)
QDD: 감속비 6:1 ~ 10:1 (유성기어 또는 belt)

낮은 감속비에는 세 가지 장점이 있다. 외력이 가해졌을 때 관절이 따라가기 쉬운 백드라이버빌리티(backdrivability)가 높아지고, 충돌 대응과 힘 제어 설계가 단순해질 수 있다. 감속기 마찰을 충분히 모델링하면 모터 전류에서 관절 토크를 근사하기도 쉽다. 감속기의 마찰과 탄성이 작을수록 더 높은 토크 응답 대역폭을 설계할 여지도 커진다.

단점: 동일 크기 대비 출력 토크가 낮다. 큰 토크가 필요하면 더 큰 모터를 써야 한다.

QDD를 사용하는 최근 시스템들:
- MIT Mini Cheetah / Cheetah 3
- Unitree 로봇 시리즈

```
# QDD vs 전통적 감속기의 토크 제어 비교
#
# 전통적 (감속비 100:1, harmonic drive):
#   반사 관성 (reflected inertia) = N² × I_motor
#   → 모터 관성 0.001 kg·m² × 100² = 10 kg·m²
#   → 관절 출력 측에 반사되는 모터 관성이 매우 크다
#   → 정밀한 힘 제어가 어렵다
#
# QDD (감속비 8:1):
#   같은 모터를 비교하면 반사 관성 = 8² × 0.001 = 0.064 kg·m²
#   → 감속비만 바꾼 이 예에서는 약 156배 작다
#   → 실제 힘 제어 성능은 링크 관성·마찰·제어기에도 좌우된다
```

**감속기 종류:**

| 종류 | 감속비 | 백래시 | 효율 | 가격 | 용도 |
|------|--------|--------|------|------|------|
| Planetary | 3~100:1 | 중간 | 85-95% | 저렴 | 범용, QDD에 적합 |
| Harmonic Drive | 30~320:1 | 매우 낮음 | 65-85% | 비쌈 | 산업용 로봇, 정밀 |
| Cycloidal | 6~120:1 | 낮음 | 85-93% | 중간 | 최근 대안으로 부상 |

감속비·효율·백래시는 구조와 제품에 따라 크게 달라진다. 제시된 수치 범위는 감속기 계열별 특징을 비교하기 위한 참고 기준이며, 실제 부품을 선정할 때는 각 제조사 데이터시트의 정격 부하 조건을 면밀히 검토해야 한다.

**액추에이터 선정 기준:**

로봇 관절의 액추에이터를 선정할 때는 정적 토크(자세 유지), 동적 토크(가속), 충격 하중을 합산하고 하중 불확실성·수명·고장 결과에 맞는 여유 계수를 둔다. 아래 코드의 2배는 계산 예시이지 보편 규칙이 아니다. 필요 속도는 관절의 최대 각속도와 감속비로부터 모터 RPM으로 환산한다. 백드라이버빌리티, 크기와 무게, 연속 토크와 열 한계도 함께 검토한다. QDD와 harmonic drive 중 어느 쪽이 나은지는 토크 밀도, 투명도, 정밀도, 비용 요구에 따라 달라진다.

```python
# 간단한 액추에이터 선정 계산 예시
import numpy as np

# 목표: 1kg 물체를 팔 끝에서 들어올리기 (팔 길이 0.5m)
m_payload = 1.0  # kg
m_link = 0.5     # 링크 자체 무게
L = 0.5          # m
g = 9.81         # m/s²

# 최악의 경우 토크 (수평으로 뻗었을 때)
tau_static = (m_payload * L + m_link * L/2) * g
print(f"정적 토크: {tau_static:.2f} Nm")

# 가속 토크 (최대 각가속도 10 rad/s²)
alpha_max = 10.0  # rad/s²
I_total = m_payload * L**2 + m_link * (L/2)**2  # 관성 모멘트 (단순화)
tau_dynamic = I_total * alpha_max
print(f"동적 토크: {tau_dynamic:.2f} Nm")

# 총 필요 토크 (안전 계수 2)
tau_required = (tau_static + tau_dynamic) * 2.0
print(f"필요 토크 (안전 계수 2): {tau_required:.2f} Nm")

# 최대 각속도 → 모터 RPM
omega_max = 3.0  # rad/s (관절)
gear_ratio = 8    # QDD
motor_rpm = omega_max * gear_ratio * 60 / (2 * np.pi)
print(f"모터 필요 RPM: {motor_rpm:.0f}")
```

> **추천 자료**
> - Katz, "A Low Cost Modular Actuator for Dynamic Robots" (MIT, 2018) — QDD의 핵심 논문: https://dspace.mit.edu/handle/1721.1/118671
> - Dynamixel 제품 라인업 및 문서: https://emanual.robotis.com/
> - Seok et al., "Design Principles for Energy-Efficient Legged Locomotion and Implementation on the MIT Cheetah Robot" (2015)

### 4.5.2 센서 인터페이싱

**엔코더 (Encoder):**

관절 각도를 측정하는 가장 기본적인 센서이다.

*Incremental encoder*: A, B 두 채널의 펄스를 세어 상대적 회전량을 측정한다. 전원이 꺼지면 위치를 잊는다 (homing 필요). 가격이 저렴하고, 분해능이 높다 (10,000 PPR 이상도 흔함).

*Absolute encoder*: 현재 각도를 절대값으로 출력하는 방식이다. 전원을 켜자마자 위치를 즉각 파악할 수 있다. Multi-turn absolute encoder는 여러 바퀴를 기억한다. 가격이 상대적으로 높지만 별도의 원점 복귀(homing) 절차가 필요 없어, 재기동 후 위치 복원이 필수적인 산업용 로봇에 널리 쓰인다.

```
분해능 계산 예시:
  Incremental encoder, 4096 PPR, quadrature decoding (x4)
  → 분해능 = 360° / (4096 × 4) = 0.022° ≈ 0.38 mrad
  → 감속비 100:1 관절 → 출력 분해능 0.0038 mrad
```

**토크 센서:**

관절 토크 또는 끝단 힘을 직접 측정한다. 스트레인 게이지(strain gauge) 기반이 대부분이다.

*관절 토크 센서 (Joint Torque Sensor, JTS)*: 감속기 출력 측에 장착. KUKA LBR iiwa가 7개 관절 모두에 JTS를 장착하여 힘 제어의 기준을 세웠다.

*힘/토크 센서 (F/T Sensor)*: 끝단에 장착하여 6축(Fx, Fy, Fz, Tx, Ty, Tz)을 측정한다. ATI Industrial Automation을 비롯한 업체가 연구용 센서를 공급하며, 선정할 때는 측정 범위·분해능·과부하 한계·인터페이스와 견적을 함께 확인한다.

**관성 센서 (IMU):**

2장에서 이미 다루었으므로 간략히 언급한다. 가속도계 + 자이로스코프 + (자력계). 모바일 로봇이나 legged robot의 몸체 자세 추정에 사용. 매니퓰레이터에서는 링크별 IMU를 달아 진동 감쇠에 활용하기도 한다.

### 4.5.3 통신 프로토콜

센서와 액추에이터를 마이크로컨트롤러/PC에 연결하는 방법이다. 로봇 시스템에서 통신은 생각보다 많은 문제를 일으킨다. 지연(latency)이 크면 제어가 불안정해지고, 대역폭이 부족하면 데이터가 누락된다.

**기초 프로토콜:**

| 프로토콜 | 배선 | 속도 | 거리 | 특징 |
|---------|------|------|------|------|
| **UART** | 2선 (TX, RX) | ~1 Mbps | ~15m | 가장 단순, 1:1 통신 |
| **SPI** | 4선 (MOSI, MISO, SCK, CS) | ~50 Mbps | ~1m (PCB 내) | 빠름, 다수 슬레이브는 CS 추가 |
| **I2C** | 2선 (SDA, SCL) | 100k~3.4 Mbps | ~1m | 주소 기반, 센서 연결에 편리 |

이 셋은 마이크로컨트롤러 수준의 기초이다. 로봇 시스템에서는 더 강건한 프로토콜이 필요하다.

**CAN Bus:**

자동차 산업에서 시작했으며 로봇의 모터와 센서 네트워크에도 쓰인다. 차동 신호(differential signaling)로 노이즈에 강하고, 멀티마스터 구조에 우선순위 기반 중재(arbitration)를 지원한다.

- 속도: 최대 1 Mbps (CAN 2.0), 5 Mbps (CAN FD)
- 거리: 최대 약 1 km (50 kbps에서; 125 kbps는 약 500 m, 1 Mbps는 약 40 m)
- 토폴로지: 버스 (데이지 체인 가능)

로봇에서의 활용: 모터 드라이버와 메인 컨트롤러 사이 통신. MIT Cheetah, 많은 legged robot이 CAN을 사용한다.

```cpp
// CAN bus를 통한 모터 명령 전송 예시 (pseudo-code, STM32 HAL)
#include "can.h"

struct MotorCommand {
    float position;   // rad
    float velocity;   // rad/s
    float torque;     // Nm
    float kp;         // position gain
    float kd;         // velocity gain
};

void send_motor_command(CAN_HandleTypeDef* hcan, uint8_t motor_id,
                        MotorCommand cmd) {
    CAN_TxHeaderTypeDef header;
    header.StdId = motor_id;   // 각 모터에 고유 CAN ID
    header.DLC = 8;            // 8 bytes (CAN 2.0 기본)
    header.RTR = CAN_RTR_DATA;

// 부동소수점을 정수로 패킹 (로봇 모터 드라이버의 일반적 방식)
    uint8_t data[8];
    int16_t pos_int = (int16_t)(cmd.position / 0.001f);   // 0.001 rad 단위
    int16_t vel_int = (int16_t)(cmd.velocity / 0.01f);    // 0.01 rad/s 단위
    int16_t tau_int = (int16_t)(cmd.torque / 0.01f);      // 0.01 Nm 단위
    int16_t kp_int  = (int16_t)(cmd.kp / 0.01f);

data[0] = pos_int >> 8;  data[1] = pos_int & 0xFF;
    data[2] = vel_int >> 8;  data[3] = vel_int & 0xFF;
    data[4] = tau_int >> 8;  data[5] = tau_int & 0xFF;
    data[6] = kp_int >> 8;   data[7] = kp_int & 0xFF;

uint32_t mailbox;
    HAL_CAN_AddTxMessage(hcan, &header, data, &mailbox);
}
```

**EtherCAT:**

산업용 실시간 이더넷 프로토콜이다. 일반 이더넷 하드웨어를 사용하면서 마이크로초 단위의 결정론적(deterministic) 통신을 제공한다.

왜 로봇에서 쓰는가: 100 Mbps로 수십~수백 개 노드를 마이크로초 주기로 동기화하고, 패킷 지연이 일정하여 실시간 제어에 맞다. 마스터가 보낸 프레임을 각 슬레이브가 on-the-fly로 읽고 쓰는 방식이라 대역폭 효율이 극히 높다.

KUKA, Beckhoff, 그리고 최근의 많은 연구용 로봇 플랫폼이 EtherCAT을 사용한다.

단점: 전용 마스터 소프트웨어 필요 (SOEM, IgH EtherCAT Master 등), 설정이 복잡하다. 취미 수준에서는 과도한 선택이다.

**RS-485 / Dynamixel Protocol:**

Dynamixel 서보의 통신 방식이다. RS-485는 차동 신호 기반의 시리얼 통신으로, 짧은 거리에서는 수 Mbps 이상도 가능하며(아래 예제의 1 Mbps는 Dynamixel 설정값이다), 여러 장치를 데이지 체인으로 연결할 수 있다.

```python
# Dynamixel SDK를 이용한 서보 제어 예시
from dynamixel_sdk import *

PROTOCOL_VERSION = 2.0
BAUDRATE = 1000000
DEVICENAME = '/dev/ttyUSB0'
DXL_ID = 1

# 포트 열기
port = PortHandler(DEVICENAME)
packet = PacketHandler(PROTOCOL_VERSION)
port.openPort()
port.setBaudRate(BAUDRATE)

# 토크 활성화
ADDR_TORQUE_ENABLE = 64
packet.write1ByteTxRx(port, DXL_ID, ADDR_TORQUE_ENABLE, 1)

# 목표 위치로 이동 (단위: 0~4095, 0~360도)
ADDR_GOAL_POSITION = 116
goal_position = 2048  # 중앙 (180도)
packet.write4ByteTxRx(port, DXL_ID, ADDR_GOAL_POSITION, goal_position)

# 현재 위치 읽기
ADDR_PRESENT_POSITION = 132
pos, _, _ = packet.read4ByteTxRx(port, DXL_ID, ADDR_PRESENT_POSITION)
print(f"현재 위치: {pos} (= {pos * 360 / 4096:.1f}°)")
```

### 4.5.4 실시간 시스템

로봇 제어에서 "실시간(real-time)"이란 단순히 연산 속도가 "빠르다"는 의미를 넘어, 주어진 deadline 이내에 모든 처리가 결정론적으로 완료됨을 보장하는 특성을 뜻한다. 1kHz 제어 루프라면 매 1ms마다 센서 읽기 → 제어 계산 → 모터 명령 전송이 완료되어야 한다. 한 번이라도 지연되면 로봇이 불안정해질 수 있다.

**RTOS (Real-Time Operating System):**

| RTOS | 특징 | 용도 |
|------|------|------|
| FreeRTOS | 경량, 마이크로컨트롤러용, 무료 | STM32, ESP32 등 |
| Zephyr | 최신, 다양한 하드웨어 지원, Linux Foundation | IoT, 로봇 임베디드 |
| VxWorks | 상용, NASA도 사용 | 항공우주, 산업용 |

마이크로컨트롤러에서 직접 모터를 제어할 때는 RTOS를 쓴다. 태스크 우선순위를 설정하여 제어 루프가 다른 태스크에 밀리지 않도록 한다.

**PREEMPT_RT Linux:**

문제: ROS2는 Linux에서 돌아간다. 그런데 일반 Linux 커널은 실시간이 아니다. 스케줄러가 제어 스레드를 아무 때나 중단시킬 수 있고, 수 밀리초의 지연이 발생할 수 있다.

해결: PREEMPT_RT 패치를 적용한 Linux 커널. 커널 대부분의 코드 경로를 선점(preemptible)으로 만들어서 실시간에 가까운 성능을 제공한다.

설정 방법 (개략):
```bash
# 1. PREEMPT_RT 패치가 적용된 커널 설치 (Debian 예시)
sudo apt install linux-image-rt-amd64   # Debian 메타패키지. Ubuntu는 이 이름이 없고 Ubuntu Pro의 real-time kernel 등 별도 경로를 쓴다

# 2. GRUB에서 RT 커널로 부팅 설정

# 3. 제어 스레드에 실시간 우선순위 부여
sudo chrt -f 99 ./my_robot_controller

# 4. CPU isolation (선택적이지만 권장)
#    /etc/default/grub에 isolcpus=2,3 추가
#    → CPU 2, 3을 일반 프로세스에서 격리
#    → 제어 스레드를 이 CPU에 고정(affinity)

# 5. 성능 확인
sudo cyclictest -m -p 99 -t 1 -n
# 최대 지연을 목표 제어 주기와 여유 시간에 대조한다
```

**제어 주기를 어떻게 정하는가:**

1kHz(1ms)는 torque·impedance control에서 자주 쓰이는 설계점이지만 보편 표준은 아니다. 필요한 주기는 폐루프 대역폭, 기계 공진, 센서와 actuator 지연, solver 시간, jitter 여유로 정한다. Nyquist의 2배는 aliasing을 피하기 위한 하한일 뿐 제어 성능을 보장하지 않으므로, 실제 설계에서는 목표 폐루프 대역폭보다 충분히 빠르게 sampling하고 주파수 응답과 지연 여유를 검증한다. CAN 대역폭도 모터 수만으로 정해지지 않는다. frame 크기, arbitration, bus load와 feedback rate를 합산해 계산해야 한다.

일부 가벼운 robot, 고속 충돌 대응, tactile control은 수 kHz 주기를 사용한다. 이때도 EtherCAT이나 FPGA가 항상 필수인 것은 아니며, 필요한 결정성·대역폭·I/O 구조에 맞춰 fieldbus, MCU, FPGA를 고른다.

> **추천 자료**
> - FreeRTOS 공식 문서: https://www.freertos.org/
> - PREEMPT_RT Wiki: https://wiki.linuxfoundation.org/realtime/start
> - Dynamixel SDK: https://github.com/ROBOTIS-GIT/DynamixelSDK
> - IgH EtherCAT Master (Linux용 오픈소스): https://etherlab.org/en/ethercat/
> - SOEM (Simple Open EtherCAT Master): https://github.com/OpenEtherCATsociety/SOEM

---

## 4.6 심화: Workspace Analysis와 최적 설계

기구학은 "주어진 로봇을 어떻게 움직이나"의 문제이기도 하지만, "어떤 로봇을 설계해야 하나"의 문제이기도 하다. 이 절은 설계 최적화와 관련된 고급 주제를 다룬다.

### 4.6.1 Reachable Workspace vs Dexterous Workspace

**Reachable workspace**: 끝단이 적어도 하나의 자세(orientation)로 도달할 수 있는 모든 점의 집합. "어디까지 손이 닿는가."

**Dexterous workspace**: 끝단이 임의의 자세로 도달할 수 있는 점의 집합. "어디서 자유롭게 움직일 수 있는가." 당연히 reachable workspace의 부분집합이고, 보통 훨씬 작다.

6-DOF 로봇의 경우 dexterous workspace는 상당히 제한적일 수 있다. 이 제약이 7-DOF 로봇이 등장한 이유 중 하나이다.

Workspace 분석은 Monte Carlo 방법으로 수행할 수 있다: 관절 공간을 무작위로 샘플링하고, FK로 끝단 위치를 계산하여 점구름(point cloud)을 만든다.

```python
import numpy as np
import roboticstoolbox as rtb

# Puma 560의 workspace 시각화 (Monte Carlo)
puma = rtb.models.DH.Puma560()
n_samples = 50000
positions = []

for _ in range(n_samples):
    # 각 관절의 범위 내에서 무작위 샘플링
    q = puma.random_q()
    T = puma.fkine(q)
    positions.append(T.t)  # [x, y, z]

positions = np.array(positions)

# 시각화 (matplotlib)
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

fig = plt.figure(figsize=(10, 8))
ax = fig.add_subplot(111, projection='3d')
ax.scatter(positions[:, 0], positions[:, 1], positions[:, 2],
           s=0.1, alpha=0.1, c='blue')
ax.set_xlabel('X (m)')
ax.set_ylabel('Y (m)')
ax.set_zlabel('Z (m)')
ax.set_title('Puma 560 Reachable Workspace')
plt.savefig('workspace.png', dpi=150)
```

### 4.6.2 Condition Number와 Isotropy

자코비안의 condition number (κ)는 로봇이 특정 자세에서 얼마나 "잘" 움직일 수 있는지의 지표이다.

```
κ(J) = σ_max / σ_min
```

σ_max, σ_min은 자코비안의 최대/최소 특이값(singular value)이다.

- κ = 1: 완벽한 등방성 (isotropic). 모든 방향으로 균일하게 움직인다. 설계와 자세에 따라 실현 가능하다.
- κ → ∞: 특이점. 한 방향으로는 전혀 움직이지 못한다.

로봇 설계 시 작업 영역 전체에 걸쳐 condition number를 최소화하는 것이 목표가 될 수 있다. 이를 **kinematic optimization** 또는 **optimal design**이라 한다.

주의: 자코비안의 condition number를 계산할 때, 선속도(m/s)와 각속도(rad/s)의 단위가 다르므로 직접 비교하면 의미가 없다. 특성 길이(characteristic length)로 정규화하거나, 선속도와 각속도를 별도로 분석해야 한다. 이 문제는 로봇 기구학 최적화에서 오래된 논쟁거리이다.

### 4.6.3 Redundancy Resolution (7-DOF Arms)

7-DOF 로봇 팔 (Kinova Gen3, KUKA LBR iiwa, Franka Emika Panda 등)은 6-DOF 작업 공간에 비해 자유도가 1개 남는다. 이 여분의 자유도를 **kinematic redundancy**라 한다.

같은 끝단 자세를 유지하면서 팔 전체의 형태(configuration)를 바꿀 수 있다. 사람 팔이 주먹의 위치를 고정한 채 팔꿈치를 올리거나 내리는 것과 같다.

이 자유도를 활용하는 전략:
1. 특이점 회피: 자코비안의 manipulability를 최대화하는 방향으로 여분 자유도 사용
2. 관절 제한 회피: 관절이 한계에 가까워지면 여분 자유도로 중앙 위치 복귀
3. 장애물 회피: 팔꿈치가 장애물과 충돌하지 않도록 형태 조정
4. 에너지 최적화: 토크를 최소화하는 자세 선택

수학적으로, 여분 자유도는 자코비안의 null space에 해당한다:

```
q̇ = J† * ẋ + (I - J† * J) * q̇_0
```

첫째 항은 끝단 속도를 달성하는 최소 norm 관절 속도이다. 둘째 항 (I - J†J)은 null space projector로, 끝단 속도에 영향을 주지 않으면서 관절을 움직인다. q̇_0는 2차 목적(예: manipulability 최대화)의 그래디언트이다.

```python
def redundancy_resolution(J, x_dot, q, q_center, k_null=0.5):
    """
    7-DOF 로봇의 redundancy resolution.

Args:
        J: 6x7 자코비안
        x_dot: 6x1 원하는 끝단 속도
        q: 7x1 현재 관절 각도
        q_center: 7x1 관절 중앙값 (null space 목표)
        k_null: null space 게인
    Returns:
        q_dot: 7x1 관절 속도
    """
    # Damped pseudo-inverse
    lam = 0.01
    J_pinv = J.T @ np.linalg.inv(J @ J.T + lam**2 * np.eye(6))

# 1차 목적: 끝단 속도 추종
    q_dot_primary = J_pinv @ x_dot

# 2차 목적: 관절 중앙으로 복귀 (null space)
    null_projector = np.eye(7) - J_pinv @ J
    q_dot_null = null_projector @ (k_null * (q_center - q))

return q_dot_primary + q_dot_null
```

> **추천 자료**
> - Siciliano et al., *Robotics: Modelling, Planning and Control*, Chapter 3.9 — Redundancy resolution 상세 설명
> - Nakamura, "Advanced Robotics: Redundancy and Optimization" (1991) — 고전
> - Dietrich et al., "An Overview of Null Space Projections for Redundant, Torque-Controlled Robots" (2015)
> - Franka Emika 연구 인터페이스: https://frankaemika.github.io/docs/

여기까지는 결정론적 기구학이었다. §4.7에서는 그 위에 확률을 얹는다.

---

## 4.7 심화: 확률적 운동 모델 (Probabilistic Motion Models)

### 4.7.1 도입: 결정론에서 확률로

§4.2 순기구학과 §4.3 역기구학은 결정론적이다. 관절 각도를 넣으면 끝단 위치 하나가 나오고, 끝단 위치를 넣으면 관절 각도 집합이 나온다. 입력에 대한 출력은 점 추정이다. 다만 이것은 모델의 성질이지 플랫폼의 성질이 아니다. 매니퓰레이터도 관절 백래시, 링크 컴플라이언스, 캘리브레이션 오차를 가지며, 그래서 도입에서 모델 오차 보정을 다뤘다.

차륜 구동 모바일 로봇은 다르다. 명령한 속도대로 바퀴가 정확히 굴러가지 않는다. 슬립이 있고, 바퀴 마모로 실효 반지름이 변한다. 좌우 바퀴의 비대칭 마모가 직진 오차를 만들기도 한다. 결과적으로, 제어 명령 $u_t$를 내리더라도 다음 pose $x_t$는 하나의 점이 아니라 확률 분포이다. 이 분포를 정형화하는 것이 **확률적 운동 모델(probabilistic motion model)**이다.

상태는 평면 상의 pose, $x_t = (x, y, \theta)^T \in SE(2)$이다. 운동 모델은 이전 pose $x_{t-1}$과 제어 입력 $u_t$가 주어졌을 때 다음 pose의 조건부 확률 분포

$$p(x_t \mid u_t, x_{t-1})$$

를 정의한다. 이 분포를 표현하는 방법으로 두 가지가 있다.

**Velocity model**: 제어 입력이 선속도와 각속도 $u_t = (v, \omega)^T$로 주어진다. 계획 단계에서 사용할 수 있다. 실제 로봇의 명령 속도와 실제 속도 사이의 오차를 노이즈로 모델링한다.

**Odometry model**: 제어 입력이 휠 인코더 적분으로 얻은 두 pose pair $u_t = (\bar{x}_{t-1}, \bar{x}_t)$로 주어진다. 사후(retrospective) 정보이므로 계획에는 쓸 수 없다. 이 pose 쌍은 인코더 회전을 기구학 모델로 적분해 얻은 값이며 pose를 직접 측정한 것은 아니다. 그래도 명령 속도를 그대로 믿는 velocity model보다 실무에서 더 정확한 편이다.

두 모델 각각에 대해 **폐쇄형 밀도 평가(closed-form density evaluation)**와 **샘플링(sampling)** 두 가지 사용 방법이 있다. 폐쇄형은 "이 가설 pose $x_t$가 얼마나 그럴듯한가"를 확률밀도 수치로 돌려준다. 베이즈 필터의 적분을 직접 계산하는 격자 국지화 같은 경우에 필요하다. EKF·UKF의 prediction은 이 밀도값을 쓰지 않고 Jacobian이나 시그마점으로 평균과 공분산을 전파한다. 샘플링은 "다음 pose 하나를 생성하라"는 forward 시뮬레이션이다. Particle filter(MCL)가 이 형태를 직접 쓴다. 4개의 조합을 §4.7.2~§4.7.5에서 각각 다룬다.

### 4.7.2 Velocity Motion Model — 폐쇄형

**직관.** 노이즈가 없다면, 선속도 $v$와 각속도 $\omega$로 움직이는 로봇은 원형 호(circular arc)를 그린다. $\omega = 0$이면 직선이다. 노이즈가 있으면 실제로 그린 호는 명령값과 다르다. 폐쇄형 평가는 이 논리를 뒤집는다: 두 pose $x_{t-1}$과 $x_t$가 주어지면, 이 두 점을 잇는 원호의 회전 중심 $(x_c, y_c)$와 반지름 $r^*$를 역산하고, 그 호를 만들었을 가상의 속도 $(\hat{v}, \hat{\omega})$를 구한 다음, 명령 속도 $(v, \omega)$와의 차이를 노이즈 분포로 평가한다.

**수식.** 두 pose $x_{t-1} = (x, y, \theta)^T$와 가설 $x_t = (x', y', \theta')^T$가 주어졌을 때:

$$\mu = \frac{1}{2} \cdot \frac{(x - x')\cos\theta + (y - y')\sin\theta}{(y - y')\cos\theta - (x - x')\sin\theta}$$

$$x_c = \frac{x + x'}{2} + \mu(y - y'), \quad y_c = \frac{y + y'}{2} + \mu(x' - x)$$

$$r^* = \sqrt{(x - x_c)^2 + (y - y_c)^2}$$

$$\Delta\theta = \text{atan2}(y' - y_c,\ x' - x_c) - \text{atan2}(y - y_c,\ x - x_c)$$

$$\hat{v} = \frac{\Delta\theta \cdot r^*}{\Delta t}, \quad \hat{\omega} = \frac{\Delta\theta}{\Delta t}, \quad \hat{\gamma} = \frac{\theta' - \theta}{\Delta t} - \hat{\omega}$$

잡음 모델은 분산이 명령 크기에 비례하는 가산형이다. `prob(a, b)`의 두 번째 인자 $b$(분산)는 다음과 같이 결정된다:

$b_v$는 선속도, $b_\omega$는 각속도의 노이즈 분산이다. $\hat{\gamma}$는 "최종 방향 보정" 항이다. $(v, \omega)$ 두 노이즈 변수만으로는 3D pose 공간 안의 2D 매니폴드 위에서만 가설 pose가 생성되는 *축퇴(degeneracy)* 문제가 생긴다. $\hat{\gamma}$를 추가하면 3D 지지(support)가 확보된다.

6개 파라미터의 물리적 의미: $\alpha_1, \alpha_2$는 선속도 노이즈의 분산 가중치, $\alpha_3, \alpha_4$는 각속도 노이즈, $\alpha_5, \alpha_6$는 최종 회전 노이즈. 분산이 명령 크기에 선형 비례하므로 빠를수록 더 불확실해지는 직관과 일치한다. 로봇마다 직선·원·8자 주행 데이터로 $\alpha_i$를 calibration해야 한다.

알고리즘 박스 (PR Table 5.1: `motion_model_velocity`).

```
Algorithm motion_model_velocity(x_t, u_t, x_{t-1}):
  # 입력: x_t=(x',y',θ'), u_t=(v,ω), x_{t-1}=(x,y,θ)
  # 출력: p(x_t | u_t, x_{t-1}) 확률밀도

μ = 0.5 * ((x − x')cosθ + (y − y')sinθ) / ((y − y')cosθ − (x − x')sinθ)
  x* = (x + x')/2 + μ(y − y')
  y* = (y + y')/2 + μ(x' − x)
  r* = sqrt((x − x*)² + (y − y*)²)
  Δθ = atan2(y' − y*, x' − x*) − atan2(y − y*, x − x*)
  v̂ = Δθ·r*/Δt
  ω̂ = Δθ/Δt
  γ̂ = (θ' − θ)/Δt − ω̂

p1 = prob(v − v̂,  α₁|v| + α₂|ω|)
  p2 = prob(ω − ω̂,  α₃|v| + α₄|ω|)
  p3 = prob(γ̂,       α₅|v| + α₆|ω|)

return p1 · p2 · p3
```

`prob(a, b)`는 평균 0, 분산 $b$의 정규 분포 또는 삼각 분포의 밀도값이다.

같은 노이즈 파라미터로 pose를 직접 생성하는 것도 가능하다. 방향만 반대다.

### 4.7.3 Velocity Motion Model — 샘플링

폐쇄형은 "가설 pose가 얼마나 그럴듯한가"를 역산으로 평가했다. Sampling은 반대 방향이다. 노이즈를 먼저 뽑아 명령 속도를 perturb하고, perturbed 속도로 forward 시뮬레이션을 돌려 다음 pose 하나를 생성한다. Particle filter는 매 입자마다 이 샘플 하나가 필요하다. 구현도 폐쇄형보다 단순하다.

Perturbed 제어:

$$\hat{v} = v + \text{sample}(\alpha_1|v| + \alpha_2|\omega|)$$
$$\hat{\omega} = \omega + \text{sample}(\alpha_3|v| + \alpha_4|\omega|)$$
$$\hat{\gamma} = \text{sample}(\alpha_5|v| + \alpha_6|\omega|)$$

Forward 원호 적분:

$$x' = x - \frac{\hat{v}}{\hat{\omega}}\sin\theta + \frac{\hat{v}}{\hat{\omega}}\sin(\theta + \hat{\omega}\Delta t)$$
$$y' = y + \frac{\hat{v}}{\hat{\omega}}\cos\theta - \frac{\hat{v}}{\hat{\omega}}\cos(\theta + \hat{\omega}\Delta t)$$
$$\theta' = \theta + \hat{\omega}\Delta t + \hat{\gamma}\Delta t$$

주의: $|\hat{\omega}| < \epsilon$이면 위 식이 발산한다. 실제 구현에서는 직선 fallback $x' = x + \hat{v}\cos\theta\,\Delta t,\ y' = y + \hat{v}\sin\theta\,\Delta t$로 처리해야 한다.

`sample(b)`는 분산 $b$의 zero-mean 표본을 뽑는 함수이다. 정규 근사: $\frac{\sqrt{b}}{2}\sum_{i=1}^{12}\text{rand}(-1,1)$ (중심극한정리 기반, 12개 균등 합).

알고리즘 박스 (PR Table 5.3: `sample_motion_model_velocity`).

```
Algorithm sample_motion_model_velocity(u_t, x_{t-1}):
  # 입력: u_t=(v,ω), x_{t-1}=(x,y,θ)
  # 출력: 샘플 x_t ~ p(x_t | u_t, x_{t-1})

v̂ = v + sample(α₁|v| + α₂|ω|)
  ω̂ = ω + sample(α₃|v| + α₄|ω|)
  γ̂ = sample(α₅|v| + α₆|ω|)

if |ω̂| < ε:   # 직선 fallback
    x' = x + v̂·cosθ·Δt
    y' = y + v̂·sinθ·Δt
  else:
    x' = x − (v̂/ω̂)sinθ + (v̂/ω̂)sin(θ + ω̂Δt)
    y' = y + (v̂/ω̂)cosθ − (v̂/ω̂)cos(θ + ω̂Δt)
  θ' = θ + ω̂Δt + γ̂Δt

return (x', y', θ')ᵀ
```

**Closed-form vs Sampling 용도 차이.** Closed-form(`motion_model_velocity`)은 확률밀도 수치를 직접 계산해 반환한다. 베이즈 필터의 적분을 직접 계산하는 격자 국지화처럼 밀도값 자체가 필요한 곳에서 쓴다. 이에 비해 Sampling(`sample_motion_model_velocity`) 기법은 구체적인 pose 표본 하나를 추출하는 데 목적이 있다. Particle filter(MCL, §14.7)에서 각 입자를 시간 축에 따라 전개할 때 직접 호출되는 형태다. 두 알고리즘은 동일한 노이즈 파라미터 $\alpha_1..\alpha_6$를 공유하면서도 서로 반대되는 연산 방향을 취한다. closed-form 수식이 주어진 가설 pose의 우도를 평가하는 데 집중하는 한편, sampling 기법은 다음 시점의 새로운 pose를 순차적으로 생성해 나간다.

```python
import numpy as np

def sample_normal(b):
    """분산 b의 zero-mean 정규 근사 표본 (12개 균등 합)."""
    return (np.sqrt(b) / 2.0) * sum(np.random.uniform(-1, 1) for _ in range(12))

def sample_motion_model_velocity(v, omega, x, y, theta, dt,
                                  alpha, eps=1e-6):
    """
    Velocity motion model sampling.
    alpha: [α₁, α₂, α₃, α₄, α₅, α₆]
    """
    v_hat   = v     + sample_normal(alpha[0]*abs(v) + alpha[1]*abs(omega))
    w_hat   = omega + sample_normal(alpha[2]*abs(v) + alpha[3]*abs(omega))
    g_hat   =         sample_normal(alpha[4]*abs(v) + alpha[5]*abs(omega))

if abs(w_hat) < eps:
        x_new = x + v_hat * np.cos(theta) * dt
        y_new = y + v_hat * np.sin(theta) * dt
    else:
        r = v_hat / w_hat
        x_new = x - r * np.sin(theta) + r * np.sin(theta + w_hat * dt)
        y_new = y + r * np.cos(theta) - r * np.cos(theta + w_hat * dt)
    theta_new = theta + w_hat * dt + g_hat * dt

return x_new, y_new, theta_new
```

velocity model 두 버전은 같은 노이즈 파라미터 $\alpha_1..\alpha_6$를 공유한다. 제어 입력이 명령 속도 $(v, \omega)$라는 가정 자체는 두 버전 모두 동일하다. 이 가정을 바꾸면 두 번째 모델 계열이 나온다.

### 4.7.4 Odometry Motion Model — 폐쇄형

**직관.** Velocity model은 명령 속도로부터 모션을 추정한다. Odometry model은 반대로, 실제 바퀴 회전을 인코더로 측정한 두 pose pair $u_t = (\bar{x}_{t-1}, \bar{x}_t)$를 control처럼 다룬다. 이 두 pose의 상대 운동을 세 파라미터 $(\delta_{\text{rot1}}, \delta_{\text{trans}}, \delta_{\text{rot2}})$로 분해한다. 목적지 방향으로 먼저 회전한 다음 직진하고, 도착 후 최종 방향 보정이다. 이 분해는 임의의 평면 운동을 항상 표현할 수 있다.

실제 odometry measurement는 엄밀히는 센서 측정값이지만, 여기서는 control처럼 다룬다. 진짜 측정 모델로 취급하면 상태 공간에 속도를 추가해야 해서 차원이 커진다. 실용적 단순화이다.

**수식.** Odometry 측정 $u_t = (\bar{x}_{t-1}, \bar{x}_t)$에서 상대 운동 추출:

$$\delta_{\text{rot1}} = \text{atan2}(\bar{y}' - \bar{y},\ \bar{x}' - \bar{x}) - \bar{\theta}$$
$$\delta_{\text{trans}} = \sqrt{(\bar{x} - \bar{x}')^2 + (\bar{y} - \bar{y}')^2}$$
$$\delta_{\text{rot2}} = \bar{\theta}' - \bar{\theta} - \delta_{\text{rot1}}$$

잡음 모델 (파라미터 4개 $\alpha_1..\alpha_4$). `prob()`의 분산 인자는 가설 pose에서 역산한 $(\hat\delta_{\text{rot1}}, \hat\delta_{\text{trans}}, \hat\delta_{\text{rot2}})$에 의존한다:

$$b_{\text{rot1}} = \alpha_1|\hat\delta_{\text{rot1}}| + \alpha_2|\hat\delta_{\text{trans}}|$$
$$b_{\text{trans}} = \alpha_3|\hat\delta_{\text{trans}}| + \alpha_4(|\hat\delta_{\text{rot1}}| + |\hat\delta_{\text{rot2}}|)$$
$$b_{\text{rot2}} = \alpha_1|\hat\delta_{\text{rot2}}| + \alpha_2|\hat\delta_{\text{trans}}|$$

$\alpha_1$: 회전이 회전을 흔드는 정도(회전 슬립), $\alpha_2$: 직진이 회전을 흔드는 정도, $\alpha_3$: 직진의 자체 분산, $\alpha_4$: 회전이 직진을 흔드는 정도. Velocity model의 $\alpha_5, \alpha_6$에 해당하는 "최종 회전" trick이 필요 없다. 3개의 독립 노이즈 변수가 자연스럽게 3D 지지를 확보한다.

주의: 각도 차는 반드시 $[-\pi, \pi]$로 wrap해야 한다. 미준수 시 분포가 발산하는 흔한 버그이다.

알고리즘 박스 (PR Table 5.5: `motion_model_odometry`).

```
Algorithm motion_model_odometry(x_t, u_t, x_{t-1}):
  # 입력: x_t=(x',y',θ'), u_t=(x̄_{t-1}, x̄_t), x_{t-1}=(x,y,θ)
  # 출력: p(x_t | u_t, x_{t-1}) 확률밀도

# odometry 측정에서 (δ_rot1, δ_trans, δ_rot2) 추출
  δ_rot1  = atan2(ȳ' − ȳ, x̄' − x̄) − θ̄
  δ_trans = sqrt((x̄ − x̄')² + (ȳ − ȳ')²)
  δ_rot2  = θ̄' − θ̄ − δ_rot1

# 가설 pose 쌍에서 같은 분해 (역모델)
  δ̂_rot1  = atan2(y' − y, x' − x) − θ
  δ̂_trans = sqrt((x − x')² + (y − y')²)
  δ̂_rot2  = θ' − θ − δ̂_rot1

return p1 · p2 · p3
```

세 파라미터 $(\delta_{\text{rot1}}, \delta_{\text{trans}}, \delta_{\text{rot2}})$를 각각 독립 노이즈 변수로 다루므로 세 방향의 지지를 갖는 밀도가 얻어지고, 주어진 가설 pose의 우도를 그대로 평가할 수 있다.

### 4.7.5 Odometry Model — Particle Filter에서의 Sampling

Odometry 폐쇄형은 역모델을 써서 가설 pose를 평가했다. Sampling은 반대 방향이다. Odometry에서 추출한 $(\delta_{\text{rot1}}, \delta_{\text{trans}}, \delta_{\text{rot2}})$에 노이즈를 가산하고, perturbed 값으로 forward 합성하여 새 pose를 생성한다. Inverse 모델이 전혀 필요 없어 폐쇄형보다 구현이 훨씬 단순하다.

**수식.** Forward 합성 (PR 식 5.40):

$$\begin{pmatrix}x'\\y'\\\theta'\end{pmatrix} = \begin{pmatrix}x\\y\\\theta\end{pmatrix} + \begin{pmatrix}\hat{\delta}_{\text{trans}}\cos(\theta + \hat{\delta}_{\text{rot1}})\\\hat{\delta}_{\text{trans}}\sin(\theta + \hat{\delta}_{\text{rot1}})\\\hat{\delta}_{\text{rot1}} + \hat{\delta}_{\text{rot2}}\end{pmatrix}$$

이것은 원호가 아닌 *직선 + 두 회전*으로 운동을 근사한다. 짧은 $\Delta t$에서 원호의 1차 근사이다. Velocity sampling과 달리 $\omega \to 0$ 분기 처리가 필요 없다는 것이 장점이다.

알고리즘 박스 (PR Table 5.6: `sample_motion_model_odometry`).

```
Algorithm sample_motion_model_odometry(u_t, x_{t-1}):
  # 입력: u_t=(x̄_{t-1}, x̄_t), x_{t-1}=(x,y,θ)
  # 출력: 샘플 x_t ~ p(x_t | u_t, x_{t-1})

# odometry에서 상대 운동 추출
  δ_rot1  = atan2(ȳ' − ȳ, x̄' − x̄) − θ̄
  δ_trans = sqrt((x̄ − x̄')² + (ȳ − ȳ')²)
  δ_rot2  = θ̄' − θ̄ − δ_rot1

# forward 합성
  x' = x + δ̂_trans · cos(θ + δ̂_rot1)
  y' = y + δ̂_trans · sin(θ + δ̂_rot1)
  θ' = θ + δ̂_rot1 + δ̂_rot2

return (x', y', θ')ᵀ
```

ROS2 Nav2의 `nav2_amcl`은 이 형태를 `differential` motion model로 구현한다. 차륜 AMR localization의 직계 응용이다.

```python
import numpy as np

def sample_motion_model_odometry(bar_x_prev, bar_x_curr, x, y, theta,
                                  alpha):
    """
    Odometry motion model sampling.
    bar_x_prev, bar_x_curr: odometry pose pair (x̄,ȳ,θ̄)
    alpha: [α₁, α₂, α₃, α₄]
    """
    bx,  by,  bt  = bar_x_prev
    bx_, by_, bt_ = bar_x_curr

d_rot1  = np.arctan2(by_ - by, bx_ - bx) - bt
    d_trans = np.sqrt((bx - bx_)**2 + (by - by_)**2)
    d_rot2  = bt_ - bt - d_rot1

def sample_normal(b):
        return (np.sqrt(b) / 2.0) * sum(np.random.uniform(-1, 1) for _ in range(12))

dh_rot1  = d_rot1  - sample_normal(alpha[0]*abs(d_rot1)  + alpha[1]*abs(d_trans))
    dh_trans = d_trans - sample_normal(alpha[2]*abs(d_trans) + alpha[3]*(abs(d_rot1) + abs(d_rot2)))
    dh_rot2  = d_rot2  - sample_normal(alpha[0]*abs(d_rot2)  + alpha[1]*abs(d_trans))

x_new     = x + dh_trans * np.cos(theta + dh_rot1)
    y_new     = y + dh_trans * np.sin(theta + dh_rot1)
    theta_new = theta + dh_rot1 + dh_rot2

return x_new, y_new, theta_new
```

지금까지 네 알고리즘은 모두 지도 없이 운동만 모델링했다. localization 문제에서는 지도 $m$이 함께 있다.

### 4.7.6 Motion + Map: 지도 조건부 운동 모델

지금까지의 모델은 지도 정보를 무시했다. 그런데 localization에서는 지도 $m$이 있다. 이를 이용하면 물리적으로 불가능한 pose를 걸러낼 수 있다.

**수식.** 지도를 조건에 포함한 전이 분포를 정확히 계산하면 까다롭다. 실용적인 근사 분해:

$$p(x_t \mid u_t, x_{t-1}, m) \propto p(x_t \mid u_t, x_{t-1}) \cdot p(x_t \mid m)$$

앞 항 $p(x_t \mid u_t, x_{t-1})$은 §4.7.2~§4.7.5의 운동 모델이다. 뒷 항 $p(x_t \mid m)$은 지도 조건부 확률로, occupancy grid에서 $x_t$가 자유 공간(free cell)이면 1에 가깝고, 벽이나 점유 공간이면 0에 가깝다.

**효과.** Particle filter에서 입자가 벽 안에 놓이는 것을 막는다. 샘플링 후 새 pose를 지도에 조회하여 점유 공간이면 해당 입자의 weight를 0(또는 매우 낮게)으로 설정하는 것이 가장 단순한 구현이다. 끝점만 검사하므로 시간 간격이 크면 벽을 지나쳐 건너간 경로는 걸러지지 않는다. 정확히는 $p(x_t \mid m)$이 likelihood가 아니라 prior로 작용하는 형태이므로, 이 근사는 운동 모델과 지도 정보가 독립이라고 가정한다는 한계가 있다.

실제 구현에서 occupancy grid는 자유 공간 외에도 unknown 영역을 갖는다. Unknown 영역에 대해 $p(x_t \mid m)$을 어떻게 설정하느냐(1로 보느냐, 중간값으로 보느냐)는 localization 성능에 영향을 준다. ROS2 Nav2의 기본 설정은 unknown 영역을 free로 취급한다.

이 분해의 수학적 근거는 §3.3(베이즈 정리와 조건부 독립, Ch.3 참조)에 있다. 운동 모델과 지도 prior가 독립이라는 가정이 성립할 때만 이 곱 분해가 엄밀하다.

### 4.7.7 무엇이 살아남았나

Velocity model과 Odometry model의 sampling formulation은 차륜 로봇 particle-filter localization의 motion prior를 설명한다. ROS2 Nav2 `nav2_amcl`의 `differential` motion model은 이 가운데 odometry 기반 formulation에 해당한다. 실제 입자 수와 update rate는 지도 크기, 센서 update, CPU, beam 수와 오차 파라미터에 맞춰 측정해 정한다.

휴머노이드, 드론, legged robot은 차체 속도 $(v, \omega)$로 운동을 기술하기 어렵다. 발이 있으면 슬립 모델 자체가 다르고, 드론은 SE(2)가 아닌 SE(3) 위에서 움직인다. 이 플랫폼에서는 IMU preintegration이 motion prior를 제공한다(§14.10). 확률적 운동 모델의 형식적 프레임워크 $p(x_t \mid u_t, x_{t-1})$는 같지만, 내용이 완전히 다르다.

여기서 다룬 모델은 모두 SE(2) 한정이다. Holonomic robot(Mecanum 바퀴)이나 차량 dynamics(횡활 포함)처럼 구동 방식이 다른 경우에는 별도의 모델이 필요하다.

이 운동 모델의 직접 응용은 §14.7 Monte Carlo Localization(MCL)이다. Particle filter의 prediction 단계에서 `sample_motion_model_odometry`가 호출된다(Ch.14 참조). §14.10 IMU preintegration에서는 차륜 odometry 모델과 IMU 모델의 차이를 비교할 수 있다. $p(x_t \mid u_t, x_{t-1})$ 형식 자체는 §3.10·§3.11의 가우시안 필터·비모수 필터에서 prediction 항으로 그대로 쓰인다(Ch.3 참조).

---

결정론적 FK는 관절 각도를 하나의 자세로 보내고, IK는 목표 자세에 대응하는 관절 각도의 해 집합을 구한다. 확률적 운동 모델은 "입력 → 출력 분포"이다. 두 모델(Velocity, Odometry)과 두 사용 방식(밀도 평가, 샘플링)의 조합 4개가 실제 localization 시스템을 구성하는 기본 단위다. Odometry model은 인코더 회전을 적분해 얻은 사후 정보라 계획에 쓸 수 없지만 실무 정확도가 높고, Velocity model은 사전 계획에 쓸 수 있지만 실제 슬립을 반영하지 못한다. Sampling은 particle filter에서, 밀도 평가는 격자 국지화처럼 밀도값이 직접 필요한 곳에서 각각의 역할이 있다.

한 가지 물음을 남긴다. 여기서 다룬 모든 모델은 바퀴가 미끄러지지 않는다는 운동학적 제약 위에 노이즈를 얹는 구조다. 진흙탕이나 경사 주행처럼 그 제약 자체가 무너지는 환경에서 $\alpha_i$ calibration은 어느 정도까지 보상할 수 있을까.

---

## 4.8 추천 자료

기구학과 메카트로닉스를 진지하게 공부하려면 교재 하나를 처음부터 끝까지 풀어보는 것이 가장 효과적이다.

**교재:**

- Craig, "Introduction to Robotics: Mechanics and Control" — DH 파라미터와 기구학을 다루며 Modified DH convention을 사용한다. 선수지식과 강의 구성에 맞는지는 목차와 예제를 보고 판단한다.

- Lynch & Park, "Modern Robotics: Mechanics, Planning, and Control" — PoE 기반. 무료 PDF와 Coursera 강의를 제공하여 접근성이 뛰어나다. 수학적으로 더 깔끔하지만 처음 보면 어렵다. https://modernrobotics.org

- Corke, "Robotics, Vision and Control" — MATLAB/Python 코드와 함께 기구학을 실습할 수 있다. robotics-toolbox-python은 이 책의 동반 라이브러리이다. 3판은 Python 기반. https://petercorke.com/rvc/

- Siciliano et al., "Robotics: Modelling, Planning and Control" — 기구학, 동역학, 제어를 한 권에서 폭넓게 다루는 대학원 교과서

**온라인 강의:**

- Modern Robotics, Coursera (Northwestern University): https://www.coursera.org/specializations/modernrobotics
- Introduction to Robotics, Stanford CS223A (Khatib): https://see.stanford.edu/Course/CS223A

**소프트웨어/라이브러리:**

- robotics-toolbox-python: https://github.com/petercorke/robotics-toolbox-python
- Pinocchio (고속 동역학, 미분 가능 기구학): https://github.com/stack-of-tasks/pinocchio
- MoveIt2 (ROS2 모션 플래닝): https://moveit.picknik.ai/
- Drake (시뮬레이션 + 최적화 + 제어): https://drake.mit.edu/

---

## 기술 흐름

```
1955 ── DH 파라미터 제안 (Denavit & Hartenberg)
1969 ── Stanford Arm (초기의 전기식 컴퓨터 제어 로봇 팔)
1970~80년대 ── Harmonic Drive가 산업용 다관절 로봇 관절의 표준 감속기로 정착
1985 ── Product of Exponentials 정형화
2019 ── MIT Mini Cheetah: QDD 액추에이터
2019 ── MoveIt2 (ROS2 기반 모션 플래닝 프레임워크)
2023 ── ALOHA: 저비용 양팔 텔레오퍼레이션 플랫폼
2024 ── SO-ARM100: 공개 BOM과 조립 문서를 제공한 오픈소스 5축 로봇 팔
```

---

*기구학 위에 힘과 질량을 얹으면 동역학(Dynamics)과 제어(Control)로 넘어간다. 관절 각도를 "원하는 값으로 보내는" 것이 아니라, "원하는 토크를 가하는" 관점으로 바뀐다.*

---

# Ch.5 — 강체 역학 & 동역학 (Rigid Body Dynamics)

---

## 5.1 왜 동역학을 배우는가

기구학(kinematics)이 "로봇이 *어디로* 움직이는가"를 다룬다면, 동역학(dynamics)은 "*어떤 힘으로* 움직이는가"를 다룬다. 기구학만으로 로봇을 제어할 수 있는 경우도 있다. 느리게 움직이는 산업용 매니퓰레이터가 그렇다. 관절 속도가 충분히 낮으면 관성력과 코리올리 힘이 무시할 만하고, PID 제어기가 나머지 오차를 보정한다.

그런데 다음과 같은 상황에서는 동역학 없이 버틸 수 없다:

- **고속 매니퓰레이션**: 산업 현장에서 cycle time을 줄이려면 로봇을 빠르게 움직여야 한다. 빠르게 움직이면 관성력, 원심력, 코리올리 힘이 커진다. 이걸 무시하면 경로 추종 오차가 커지고, 최악의 경우 관절 모터가 포화(saturation)된다.
- **보행 로봇 (legged robots)**: 두 발이든 네 발이든, 지면과의 접촉력을 관리하면서 넘어지지 않아야 한다. 이건 순수하게 동역학 문제다.
- **시뮬레이션**: 물리 시뮬레이터는 힘/토크를 받아서 가속도를 계산하고, 이를 적분하여 다음 상태를 구한다. 동역학 모델이 곧 시뮬레이터의 핵심이다.
- **최적 제어 (optimal control)**: 에너지를 최소화하거나 시간을 최소화하는 궤적을 찾으려면 동역학 모델이 constraints로 들어간다.
- **충돌/접촉 처리**: 물체를 잡거나(grasp), 밀거나(push), 던지는(throw) 작업은 접촉 역학 없이 불가능하다.

기구학은 로봇의 "기하학"이고, 동역학은 로봇의 "물리학"이다. 기하학만으로는 세상이 움직이지 않는다.

> **추천 자료**
> - Featherstone, *Rigid Body Dynamics Algorithms*, Chapter 1 — 동역학이 왜 필요한지를 간결하게 설명한다.
> - Russ Tedrake, *Underactuated Robotics* Ch.1 (https://underactuated.csail.mit.edu/) — 동역학 기반 제어가 왜 기구학 기반보다 강력한지 직관적으로 보여준다.

---

## 5.2 뉴턴-오일러 역학 (Newton-Euler Formulation)

### 기본 원리

뉴턴 역학은 병진 운동과 회전 운동을 따로 기술한다.

**병진 운동 (translational motion):**
```
F = ma
```
물체에 작용하는 합력 F는 질량 m과 질량중심(CoM) 가속도 a의 곱이다.

**회전 운동 (rotational motion):**
```
τ = Iα + ω × (Iω)
```
물체에 작용하는 합토크 τ는 관성 텐서 I와 각가속도 α의 곱에 자이로스코픽 항 ω × (Iω)를 더한 것이다. 2D에서는 뒤의 항이 사라져 τ = Iα로 단순해지지만, 3D에서는 이 항을 포함해야 한다. 빠뜨리면 시뮬레이션에서 비현실적인 회전 거동이 나타난다.

### Recursive Newton-Euler Algorithm (RNEA)

RNEA는 직렬 매니퓰레이터(serial manipulator)의 역동역학(inverse dynamics)을 푸는 가장 효율적인 방법이다. 두 pass로 구성된다.

**Forward pass (base → end-effector):** 각 링크의 속도와 가속도를 순방향으로 전파한다. 링크 i의 속도는 링크 i-1의 속도에 관절 i의 기여분을 더한 것이다.

**Backward pass (end-effector → base):** 각 링크에 작용하는 힘과 토크를 역방향으로 전파한다. 뉴턴-오일러 방정식으로 링크 i에 필요한 합력/합토크를 구하고, 이를 관절 i의 토크로 변환한다.

왜 재귀적(recursive)으로 푸는가? 단일 강체의 동역학은 O(1)이다. n개의 링크를 순차적으로 처리하면 O(n)이다. 라그랑주 방정식을 기계적으로 직접 전개하면 M(q) 행렬 계산만으로도 O(n³) 이상의 비용이 든다. 6자유도 팔이라면 O(n)과 O(n³)의 차이가 제어 루프의 실시간성을 가른다.

RNEA의 의사 코드를 정리하면 다음과 같다:

```
RNEA(model, q, q̇, q̈):
    # Forward pass: i = 1, 2, ..., n
    for i = 1 to n:
        v[i] = v[i-1] + S[i] * q̇[i]        # 관절축 방향 속도 추가
        a[i] = a[i-1] + S[i] * q̈[i] + v[i] × (S[i] * q̇[i])
        f[i] = I[i] * a[i] + v[i] × (I[i] * v[i])  # Newton-Euler

# Backward pass: i = n, n-1, ..., 1
    for i = n downto 1:
        τ[i] = S[i]^T * f[i]               # 관절 토크 추출
        f[parent(i)] += f[i]                # 부모 링크로 전파

return τ
```

여기서 S[i]는 관절 i의 motion subspace matrix (관절 축 방향), v[i]는 링크 i의 공간 속도, I[i]는 링크 i의 공간 관성이다. Featherstone의 spatial vector 표기를 따랐다. 5.7절에서 더 자세히 다룬다.

### 실제 코드: Pinocchio

Pinocchio는 RNEA를 포함한 다양한 동역학 알고리즘을 구현한 C++/Python 라이브러리이다. 다음은 RNEA로 역동역학을 계산하는 예시다:

```python
import pinocchio as pin
import numpy as np

# URDF에서 모델 로드
model = pin.buildModelFromUrdf("robot.urdf")
data = model.createData()

# 현재 상태 설정
q = pin.randomConfiguration(model)     # 관절 위치
v = np.random.randn(model.nv)          # 관절 속도
a = np.random.randn(model.nv)          # 관절 가속도

# RNEA: (q, v, a) → τ
tau = pin.rnea(model, data, q, v, a)
print("Joint torques:", tau)

# 중력 토크만 계산 (v=0, a=0)
tau_g = pin.rnea(model, data, q, np.zeros(model.nv), np.zeros(model.nv))
print("Gravity compensation torques:", tau_g)
```

중력 보상(gravity compensation)은 RNEA에서 관절 속도·가속도를 0으로 두고 베이스 가속도를 a[0] = -g로 초기화하면 바로 나온다. 위 의사 코드는 이 초기화를 생략했다. 이것만으로도 로봇이 중력에 처지지 않는다. 로봇 팔을 처음 세울 때 첫 번째로 구현하는 제어기다.

Drake에서의 동일한 계산:

```python
from pydrake.multibody.plant import MultibodyPlant
from pydrake.multibody.parsing import Parser
from pydrake.multibody.tree import MultibodyForces
import numpy as np

plant = MultibodyPlant(time_step=0.0)
Parser(plant).AddModels("robot.urdf")
plant.Finalize()

context = plant.CreateDefaultContext()

q = np.random.randn(plant.num_positions())
v = np.random.randn(plant.num_velocities())
vdot = np.random.randn(plant.num_velocities())

plant.SetPositions(context, q)
plant.SetVelocities(context, v)

# Inverse dynamics: vdot → τ
tau = plant.CalcInverseDynamics(context, vdot, MultibodyForces(plant))
```

> **추천 자료**
> - Featherstone, *Rigid Body Dynamics Algorithms*, Chapter 5 — RNEA의 원본 설명
> - Pinocchio documentation (https://github.com/stack-of-tasks/pinocchio) — 실무에서 RNEA를 가장 쉽게 써볼 수 있는 라이브러리
> - Luh, Walker, Paul (1980), "On-Line Computational Scheme for Mechanical Manipulators" — RNEA의 원 논문

---

## 5.3 라그랑주 역학 (Lagrangian Mechanics)

### Lagrangian이란

라그랑주 역학은 힘과 토크를 직접 다루는 대신, 에너지를 통해 운동 방정식을 유도한다.

**Lagrangian** L은 다음과 같이 정의된다:

```
L(q, q̇) = T(q, q̇) - V(q)
```

여기서 T는 시스템의 운동에너지(kinetic energy), V는 위치에너지(potential energy)이다.

**Euler-Lagrange 방정식:**

```
d/dt (∂L/∂q̇_i) - ∂L/∂q_i = τ_i
```

각 일반화 좌표(generalized coordinate) q_i에 대해 이 방정식을 세우면, 시스템의 운동 방정식이 나온다.

좌표계를 자유롭게 선택할 수 있다. 뉴턴-오일러 방식은 링크마다 힘과 모멘트를 주고받으며 풀기 때문에 어떤 프레임에서 표현할지와 구속을 어떻게 다룰지를 정해야 한다. 라그랑주 역학에서 관절 각도를 일반화 좌표로 잡으면 구속 조건이 식에서 사라지는데, 이는 홀로노믹 구속과 독립 일반화 좌표가 존재하는 개방 사슬에 한한 이야기다. 폐쇄 사슬이나 비홀로노믹 구속에서는 Lagrange 승수가 남는다.

### Manipulator Equation

n-DOF 직렬 매니퓰레이터의 Euler-Lagrange 방정식을 정리하면 다음과 같은 표준 형태가 나온다:

```
M(q)q̈ + C(q, q̇)q̇ + g(q) = τ
```

각 항의 의미:

- **M(q)** — 질량/관성 행렬(mass/inertia matrix). n×n 대칭 양정치(symmetric positive definite) 행렬이다. 로봇의 자세 q에 따라 달라진다 — 팔을 쭉 펴면 관성이 커지고, 접으면 작아지는 것과 같은 원리다.
- **C(q, q̇)q̇** — 코리올리 및 원심력 항(Coriolis and centrifugal terms). 관절들이 동시에 움직일 때 발생하는 관성 커플링이다. 느리게 움직이면 무시해도 되지만, 빠르게 움직이면 이 항이 크다.
- **g(q)** — 중력 벡터(gravity vector). 로봇이 중력장에 있을 때 각 관절에 작용하는 중력 토크이다.
- τ — 관절 토크 벡터. 모터가 내는 힘이다. 마찰(friction)은 보통 별도로 모델링하여 더한다.

제어, 시뮬레이션, 궤적 최적화 전부 이 방정식에서 출발한다.

### 2-Link Planar Arm 예제

2-link planar arm은 동역학 입문에서 빠지지 않는 예제이다. 노트에 직접 유도해보는 것을 강력히 권한다 — 한 번 해보면 n-DOF 경우의 구조가 명확해진다.

설정:
- 링크 길이: l_1, l_2
- 링크 질량: m_1, m_2 (질량이 링크 끝에 집중된다고 가정 — point mass)
- 관절 각도: q_1, q_2 (base에서부터)
- 중력: g (아래 방향)

**운동에너지 T:**

링크 1 끝점의 위치:
```
x_1 = l_1 cos(q_1)
y_1 = l_1 sin(q_1)
```

링크 2 끝점의 위치:
```
x_2 = l_1 cos(q_1) + l_2 cos(q_1 + q_2)
y_2 = l_1 sin(q_1) + l_2 sin(q_1 + q_2)
```

각 질량의 속도를 구하고 T = (1/2)m_1 v_1^2 + (1/2)m_2 v_2^2 을 전개하면:

```
T = (1/2)(m_1 + m_2) l_1^2 q̇_1^2
  + (1/2) m_2 l_2^2 (q̇_1 + q̇_2)^2
  + m_2 l_1 l_2 cos(q_2) q̇_1 (q̇_1 + q̇_2)
```

**위치에너지 V:**

```
V = m_1 g l_1 sin(q_1) + m_2 g [l_1 sin(q_1) + l_2 sin(q_1 + q_2)]
```

**M(q) 행렬:**

```
M(q) = [ (m_1+m_2)l_1^2 + m_2 l_2^2 + 2 m_2 l_1 l_2 cos(q_2)    m_2 l_2^2 + m_2 l_1 l_2 cos(q_2) ]
        [ m_2 l_2^2 + m_2 l_1 l_2 cos(q_2)                          m_2 l_2^2                          ]
```

M(q)는 q_2에 따라 달라진다. base 관절에 대한 유효 관성인 M_{11}을 보면, q_2 = 0으로 팔이 완전히 펴졌을 때 최댓값이고 q_2 = \pi로 접혔을 때 최솟값이다.

**C(q, q̇) 행렬:**

```
C(q, q̇) = [ -m_2 l_1 l_2 sin(q_2) q̇_2    -m_2 l_1 l_2 sin(q_2)(q̇_1 + q̇_2) ]
            [  m_2 l_1 l_2 sin(q_2) q̇_1     0                                     ]
```

C 행렬의 유도 방법은 여러 가지가 있다(Christoffel symbols 등). 가장 체계적인 방법은 Christoffel symbols를 쓰는 것이지만, 2-link의 경우 Euler-Lagrange 방정식에서 직접 항을 정리하는 편이 빠르다.

**g(q) 벡터:**

```
g(q) = [ (m_1 + m_2) g l_1 cos(q_1) + m_2 g l_2 cos(q_1 + q_2) ]
        [ m_2 g l_2 cos(q_1 + q_2)                                 ]
```

이것을 SymPy로 검증하는 코드:

```python
import sympy as sp

q1, q2, dq1, dq2, ddq1, ddq2 = sp.symbols('q1 q2 dq1 dq2 ddq1 ddq2')
m1, m2, l1, l2, g = sp.symbols('m1 m2 l1 l2 g', positive=True)

# 위치
x1 = l1 * sp.cos(q1)
y1 = l1 * sp.sin(q1)
x2 = x1 + l2 * sp.cos(q1 + q2)
y2 = y1 + l2 * sp.sin(q1 + q2)

# 속도 (chain rule)
vx1 = sp.diff(x1, q1) * dq1
vy1 = sp.diff(y1, q1) * dq1
vx2 = sp.diff(x2, q1) * dq1 + sp.diff(x2, q2) * dq2
vy2 = sp.diff(y2, q1) * dq1 + sp.diff(y2, q2) * dq2

# 운동에너지
T = sp.Rational(1,2)*m1*(vx1**2 + vy1**2) + sp.Rational(1,2)*m2*(vx2**2 + vy2**2)
T = sp.trigsimp(sp.expand(T))

# 위치에너지
V = m1*g*y1 + m2*g*y2

# Lagrangian
L = T - V

# Euler-Lagrange equations
# d/dt(∂L/∂q̇_i) - ∂L/∂q_i = τ_i
# 여기서 d/dt는 q1, q2에 대한 시간 미분을 포함해야 하므로 치환이 필요하다.
# 간단하게 M, C, g를 추출하는 것은 교재를 참고하라.

print("T =", T)
print("V =", V)
```

이 코드는 운동에너지 T와 위치에너지 V를 출력하므로, 위에서 유도한 에너지 식과 비교할 수 있다. SymPy가 trigsimp을 적용하면 깔끔한 형태가 나온다.

> **추천 자료**
> - Murray, Li, Sastry, *A Mathematical Introduction to Robotic Manipulation*, Ch. 4 (https://www.cds.caltech.edu/~murray/mlswiki/) — 라그랑주 역학을 로보틱스 맥락에서 가장 엄밀하게 다룬 교재. 무료 PDF 제공.
> - Spong, Hutchinson, Vidyasagar, *Robot Modeling and Control*, Ch. 6-7 — 학부 수준에서 가장 접근하기 쉬운 설명
> - Craig, *Introduction to Robotics*, Ch. 6 — 2-link arm 예제가 상세히 나와 있다

---

## 5.4 뉴턴-오일러 vs 라그랑주

이 둘은 같은 물리를 다른 관점에서 보는 것이다. 최종 결과(운동 방정식)는 동일하다. 차이는 유도 과정과 계산 효율에 있다.

| 항목 | 뉴턴-오일러 (RNEA) | 라그랑주 |
|------|-------------------|---------|
| 관점 | 힘/토크 (force-based) | 에너지 (energy-based) |
| 계산 복잡도 | O(n) | O(n^3) (M 행렬 직접 계산 시) |
| 유도 난이도 | 재귀적이라 n이 커져도 같은 패턴 | n이 커지면 편미분이 폭발 |
| 물리적 직관 | 각 링크의 힘/토크를 직접 볼 수 있음 | 에너지 보존/변환을 볼 수 있음 |
| 주 용도 | 실시간 제어, 시뮬레이션 | 모델 유도, 에너지 기반 분석, Lyapunov 안정성 |
| 구속력 | 명시적으로 계산 가능 | 일반화 좌표 사용 시 자동으로 소거 |

실무 워크플로우는 대체로 이렇다:

1. 라그랑주 역학으로 manipulator equation의 구조를 이해한다 (모델 유도).
2. RNEA(또는 ABA)로 실시간 계산한다 (수치 계산).
3. manipulator equation의 구조(M, C, g)를 이용한 computed torque control, passivity-based control 등을 설계한다 (제어기 설계).
4. Pinocchio나 Drake가 내부적으로 RNEA/ABA를 사용하므로, 라이브러리를 호출하면 된다 (코드 구현).

두 formulation은 서로 다른 역할을 맡는다. 라그랑주 역학이 제어계 설계에 필요한 동역학적 대칭 구조를 투명하게 보여준다면, 뉴턴-오일러 역학은 실시간 루프에서 고속 연산을 수행하기에 이상적이다.

> **추천 자료**
> - Featherstone, *Rigid Body Dynamics Algorithms*, Ch. 3 — 두 formulation의 관계를 명확히 설명
> - Siciliano et al., *Robotics: Modelling, Planning and Control*, Ch. 7 — 두 방법으로 같은 로봇의 동역학을 유도하는 비교 예제

---

## 5.5 Forward Dynamics vs Inverse Dynamics

동역학에는 두 가지 "방향"이 있다:

**Inverse Dynamics (역동역학):**
```
주어진 것: q, q̇, q̈
구하는 것: τ
```
"이 궤적을 따라가려면 모터가 얼마의 토크를 내야 하는가?"에 답한다. 제어에서 주로 사용한다. Computed torque control의 핵심이다.

**Forward Dynamics (순동역학):**
```
주어진 것: q, q̇, τ
구하는 것: q̈
```
"이 토크를 가하면 로봇이 어떻게 가속하는가?"에 답하며, 시뮬레이터는 매 time step마다 이 계산을 반복한다.

수학적으로 forward dynamics는 manipulator equation에서 q̈을 풀어내는 것이다:

```
q̈ = M(q)^{-1} [τ - C(q, q̇)q̇ - g(q)]
```

단순히 M(q)의 역행렬을 구하면 O(n^3)이다. 이건 관절 수가 많으면 느리다.

### Articulated Body Algorithm (ABA)

Featherstone이 제안한 ABA는 forward dynamics를 O(n)에 계산한다. RNEA가 inverse dynamics의 O(n) 알고리즘이듯, ABA는 forward dynamics의 O(n) 알고리즘이다.

ABA는 각 링크를 "articulated body"로 보고, 해당 서브트리의 관성을 재귀적으로 합산한다. M 행렬을 명시적으로 구성하지 않고도 q̈을 직접 계산할 수 있다.

```
ABA(model, q, q̇, τ):
    # Pass 1 (forward): 속도 전파
    for i = 1 to n:
        v[i] = v[parent(i)] + S[i] * q̇[i]
        c[i] = v[i] × (S[i] * q̇[i])  # Coriolis acceleration

# Pass 2 (backward): articulated body inertia 계산
    for i = n downto 1:
        I_A[i] = I[i]  # spatial inertia
        p_A[i] = v[i] × (I[i] * v[i]) - f_ext[i]  # bias force
        # 자식 링크들의 기여를 합산 (생략)
        # 관절 가속도의 중간값 계산

# Pass 3 (forward): 가속도 전파
    for i = 1 to n:
        q̈[i] = ...  # articulated body inertia를 이용해 계산
        a[i] = a[parent(i)] + S[i] * q̈[i] + c[i]

return q̈
```

실제 구현은 상당히 복잡하므로 Pinocchio나 Drake 같은 검증된 라이브러리를 쓰는 편이 낫다.

### 시뮬레이터에서의 역할

시뮬레이터마다 사용하는 알고리즘이 다르다. **MuJoCo**는 forward dynamics에 자체 알고리즘을 쓴다. 접촉까지 포함한 통합 solver가 특징이고, 내부적으로 sparse factorization을 활용하며 분기형(branching) 구조에 특화되어 있다. **Drake**는 MultibodyPlant에서 ABA를 쓰고, 접촉은 별도의 solver(time-stepping, hydroelastic 등)로 처리한다. **Bullet(PyBullet)**은 Featherstone ABA를 기반으로 하되, 접촉은 sequential impulse solver를 사용한다.

코드로 보면:

```python
# Pinocchio: forward dynamics (ABA)
import pinocchio as pin
import numpy as np

model = pin.buildModelFromUrdf("robot.urdf")
data = model.createData()

q = pin.randomConfiguration(model)
v = np.random.randn(model.nv)
tau = np.random.randn(model.nv)

# ABA: (q, v, τ) → q̈
qdd = pin.aba(model, data, q, v, tau)
print("Joint accelerations:", qdd)

# 검증: RNEA로 역계산
tau_check = pin.rnea(model, data, q, v, qdd)
print("Torque error:", np.linalg.norm(tau - tau_check))  # ≈ 0
```

RNEA와 ABA는 서로 역연산 관계이다. RNEA(q, v, ABA(q, v, τ)) ≈ τ 가 성립한다 (부동소수점 오차 이내).

> **추천 자료**
> - Featherstone, *Rigid Body Dynamics Algorithms*, Ch. 7 — ABA의 원본 설명
> - MuJoCo documentation: Computation (https://mujoco.readthedocs.io/en/latest/computation/) — MuJoCo의 동역학 파이프라인 설명
> - Drake MultibodyPlant tutorial (https://drake.mit.edu/doxygen_cxx/classdrake_1_1multibody_1_1_multibody_plant.html) — Drake에서의 동역학 계산 API

---

## 5.6 접촉 역학 (Contact Dynamics)

로봇이 환경과 접촉하는 순간, 동역학은 한 단계 더 복잡해진다. 자유 공간에서의 동역학은 ODE(ordinary differential equation)로 깔끔하게 표현되지만, 접촉이 들어가면 부등식 구속 조건과 불연속성이 생긴다.

### Rigid Contact vs Compliant Contact

접촉을 모델링하는 두 가지 큰 틀이 있다:

**Rigid contact (경성 접촉):**
- 물체가 서로 관통하지 않는다는 구속 조건을 직접 부과한다.
- 접촉력은 구속 조건의 Lagrange multiplier로 나온다.
- 수학적으로 깔끔하지만, 수치적으로 어렵다 — 접촉/비접촉 전환 시 불연속성이 생기고, 이를 처리하기 위해 LCP(Linear Complementarity Problem)나 NCP(Nonlinear Complementarity Problem)를 풀어야 한다.
- Stewart-Trinkle 계열의 LCP time-stepping 방법이 이 계열이다.

**Compliant contact (연성 접촉):**
- 접촉면에 가상의 스프링-댐퍼를 놓는다. 관통 깊이에 비례하는 반발력을 생성한다.
- 수치적으로 안정적이고 구현이 쉽다.
- 스프링 강성(stiffness)을 높이면 rigid contact에 가까워지지만, 수치 적분의 time step을 줄여야 한다 (stiff ODE).
- MuJoCo의 기본 접촉 모델이 이 계열이다.

### Coulomb Friction Model

접촉이 있으면 마찰(friction)이 따라온다. 가장 기본적인 마찰 모델은 Coulomb friction이다:

```
|f_t| ≤ μ f_n          (static friction: 정지 마찰)
|f_t| = μ f_n, f_t ∥ -v_t  (sliding friction: 운동 마찰)
```

여기서 f_t는 접선 방향 마찰력, f_n은 법선 방향 수직항력, μ는 마찰 계수, v_t는 접선 방향 상대 속도이다.

다만 한계가 있다. 정지 마찰에서 운동 마찰로의 전환이 불연속이고, 3D 마찰 원뿔(friction cone)은 비선형이라 선형화하면(friction pyramid) 정확도가 떨어진다. 더 심각하게는, 특정 조건에서 rigid contact + Coulomb friction 조합의 해가 존재하지 않거나 유일하지 않다 — Painleve's paradox다.

### Contact-Rich Manipulation이 어려운 이유

물체를 잡고, 돌리고, 끼우는 작업(peg-in-hole, in-hand manipulation 등)은 왜 그렇게 어려운가?

첫째, 접촉 모드가 수시로 바뀐다(contact/no-contact, stick/slip). 각 모드마다 동역학이 다르고, 모드 전환 시점을 예측하기 어렵다. 둘째, 전환 순간에 상태가 불연속적으로 변할 수 있다(충격, impact). 셋째, 마찰 계수나 접촉 강성의 정확한 값을 모르면 sim-to-real gap이 커진다. 접촉점 수가 늘면 contact/separation과 stick/slip 조합이 지수적으로 늘어나는 조합론적 복잡성도 겹친다. 정확한 모드 수는 마찰 모델과 접선 방향의 이산화 방식에 따라 달라진다.

### 시뮬레이터마다 접촉 처리가 다른 이유

접촉을 수치적으로 근사하는 방식이 여러 가지이기 때문이다. 각 시뮬레이터는 정확도, 속도, 안정성 사이의 트레이드오프를 다르게 선택한다.

**MuJoCo**는 compliant contact + convex optimization 방식을 쓴다. 빠르고 안정적이지만 물리적으로 완벽하지는 않다. 특히 관통이 허용되는데, 이를 "soft contact"의 일부로 받아들인다. RL 환경으로 인기 많은 이유 중 하나가 이 안정성이다. **Drake**는 compliant point contact(TAMSI/SAP 이산 솔버)과 hydroelastic contact을 지원하며, 둘 다 compliant 계열이다. 물리적으로 더 엄밀하지만 계산 비용이 높을 수 있다. Hydroelastic contact은 접촉면의 압력 분포까지 계산한다.

**Bullet**은 velocity-level LCP + sequential impulse 방식으로, 게임/VR에서 출발한 엔진이라 속도에 최적화되어 있다. 접촉 정밀도는 설정과 작업에 따라 갈리므로 MuJoCo·Drake와의 우열은 목표 작업에서 직접 비교한다. **DART**는 LCP 기반 rigid contact으로 학술적으로 엄밀한 구현이지만, MuJoCo나 Drake에 비해 사용자 기반이 작다.

어떤 시뮬레이터를 쓸지는 연구 목적에 따라 달라진다. MuJoCo가 locomotion 분야의 강화학습에서 주도적으로 활용된다면, contact-rich manipulation 영역에서는 Drake와 MuJoCo가 모두 강력한 해석 능력을 제공한다. 필요한 접촉 모델, gradient, 처리 속도, 재현할 hardware와 검증 사례를 기준으로 고른다.

```python
# MuJoCo에서 접촉 정보 접근
import mujoco
import numpy as np

model = mujoco.MjModel.from_xml_path("scene.xml")
data = mujoco.MjData(model)

mujoco.mj_step(model, data)

# 접촉점 개수
n_contacts = data.ncon
print(f"Number of contacts: {n_contacts}")

# 각 접촉의 정보
for i in range(n_contacts):
    contact = data.contact[i]
    print(f"Contact {i}:")
    print(f"  Position: {contact.pos}")
    print(f"  Normal: {contact.frame[:3]}")  # 접촉 법선
    print(f"  Signed distance (음수면 관통): {contact.dist}")
    print(f"  Geom pair: ({contact.geom1}, {contact.geom2})")
```

> **추천 자료**
> - Stewart, "Rigid-Body Dynamics with Friction and Impact", SIAM Review 2000 — 접촉 역학의 수학적 기초
> - Todorov, "Convex and analytically-invertible dynamics with contacts and constraints", ICRA 2014 — MuJoCo의 접촉 모델 논문
> - [Todorov et al., "MuJoCo: A Physics Engine for Model-Based Control" (IROS 2012)](https://ieeexplore.ieee.org/document/6386109) — MuJoCo의 convex contact formulation과 velocity stepping을 설명한 원 논문
> - Drake의 접촉 모델 documentation (https://drake.mit.edu/doxygen_cxx/group__hydroelastic__user__guide.html) — Hydroelastic contact 설명
> - Russ Tedrake, *Underactuated Robotics*, Ch. "Contact" (https://underactuated.csail.mit.edu/) — 접촉 역학 개론

---

## 5.7 심화: Featherstone 알고리즘과 Spatial Algebra

이하는 대학원 심화 과정 수준에 해당한다. Featherstone의 spatial vector algebra는 동역학 알고리즘을 간결하고 효율적으로 표현하기 위한 수학적 틀이다.

### Spatial Vectors (6D Vectors)

3D 공간에서 강체의 운동은 병진(3 DOF) + 회전(3 DOF) = 6 DOF이다. 이를 하나의 6D 벡터로 통합한 것이 spatial vector이다.

**Motion vector (spatial velocity, twist):**
```
v = [ω; v_O]
```
위 3개는 각속도(ω), 아래 3개는 기준점 O에서의 선속도(v_O)이다.

**Force vector (spatial force, wrench):**
```
f = [n_O; f]
```
위 3개는 기준점 O 주위의 모멘트(n_O), 아래 3개는 힘(f)이다.

spatial velocity와 spatial force의 내적이 곧 power(일률)이다.
```
P = f^T v = n_O · ω + f · v_O
```

Featherstone은 이 성질이 성립하도록 spatial vector를 정의했다.

### Spatial Inertia

6×6 spatial inertia matrix는 질량, 질량중심 위치, 회전 관성을 하나의 행렬에 통합한다:

```
I_sp = [ I_cm + m·[c]×[c]×^T    m·[c]× ]
       [ m·[c]×^T                 m·1    ]
```

여기서 m은 질량, c는 질량중심까지의 벡터, I_cm은 질량중심 주위의 회전 관성, [c]×는 c의 skew-symmetric matrix이다.

여러 강체의 관성을 같은 좌표계와 기준점으로 표현하면 더할 수 있다(I_composite = I_1 + I_2 + ...). 좌표 변환은 congruence transform 하나로 끝난다(I_B = X^T I_A X).

### RNEA와 ABA의 Spatial Vector 표현

5.2절과 5.5절에서 보인 의사 코드가 사실 spatial vector 표기였다. S[i]는 관절 i의 motion subspace(revolute 관절이면 [e_z; 0], prismatic이면 [0; e_z]), v[i]는 spatial velocity, f[i]는 spatial force이다.

Spatial vector를 쓰면 회전 관절이든 직동 관절이든 같은 코드로 처리할 수 있다. Pinocchio와 Drake가 내부적으로 spatial algebra를 사용하는 이유다.

### Pinocchio에서 Spatial Quantities 접근

```python
import pinocchio as pin
import numpy as np

model = pin.buildModelFromUrdf("robot.urdf")
data = model.createData()

q = pin.randomConfiguration(model)
v = np.random.randn(model.nv)

# 순기구학 + 속도 계산
pin.forwardKinematics(model, data, q, v)

# 각 프레임의 spatial velocity
for i in range(model.njoints):
    # 월드 프레임 기준 spatial velocity
    v_world = pin.getVelocity(model, data, i, pin.ReferenceFrame.WORLD)
    print(f"Joint {i} spatial velocity (world): {v_world}")

# Composite Rigid Body Algorithm (CRBA): M(q) 계산
M = pin.crba(model, data, q)
print("Mass matrix M(q):\n", data.M)

# Centroidal momentum matrix
pin.computeCentroidalMap(model, data, q)
Ag = data.Ag  # 6 x nv matrix
# h = Ag @ v 가 centroidal momentum (선운동량 + 각운동량)
```

C++에서 Pinocchio를 사용할 때:

```cpp
#include <pinocchio/algorithm/rnea.hpp>
#include <pinocchio/algorithm/aba.hpp>
#include <pinocchio/parsers/urdf.hpp>

pinocchio::Model model;
pinocchio::urdf::buildModel("robot.urdf", model);
pinocchio::Data data(model);

Eigen::VectorXd q = pinocchio::randomConfiguration(model);
Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv);
Eigen::VectorXd tau = Eigen::VectorXd::Random(model.nv);

// Inverse dynamics (RNEA)
Eigen::VectorXd tau_id = pinocchio::rnea(model, data, q, v, Eigen::VectorXd::Zero(model.nv));

// Forward dynamics (ABA)
Eigen::VectorXd qdd = pinocchio::aba(model, data, q, v, tau);
```

Pinocchio의 C++ API는 Eigen 기반이며, Python API와 거의 동일한 인터페이스를 제공한다. kHz급 실시간 제어에서는 Python 오버헤드를 피하려고 C++ API를 사용한다.

> **추천 자료**
> - Featherstone, *Rigid Body Dynamics Algorithms*, Ch. 2 — Spatial vector algebra의 원본 설명
> - Featherstone, "A Beginner's Guide to 6-D Vectors" (IEEE Robotics & Automation Magazine, 2010) — 교과서보다 접근하기 쉬운 소개 논문
> - Pinocchio GitHub (https://github.com/stack-of-tasks/pinocchio) — 소스 코드 자체가 spatial algebra의 좋은 구현 예시이다

---

## 5.8 심화: 부유 베이스 시스템 (Floating Base)

산업용 매니퓰레이터는 base가 바닥에 볼트로 고정되어 있다. 반면 보행 로봇, 드론, 수중 로봇은 base 자체가 움직인다. 이 경우 base의 위치와 자세가 자유도에 추가되면서 동역학의 구조도 달라진다.

### 부유 베이스의 Configuration

고정 base 로봇의 configuration은 q ∈ R^n이다. 부유 base 로봇의 configuration은:

```
q = [q_base; q_joints]
```

q_base는 SE(3)의 원소이다 — 위치(3) + 자세(3, 또는 quaternion으로 4). Pinocchio에서는 q의 차원(nq)과 v의 차원(nv)이 다를 수 있다(quaternion을 쓰면 nq = nv + 1). q와 v가 같은 벡터 공간에 살지 않으므로, 적분이나 차분을 할 때 단순히 `q += v*dt`를 하면 안 된다. `pin.integrate(model, q, v*dt)`를 써야 한다.

### Underactuated Systems

부유 base 시스템은 대개 **underactuated**다. 보행 로봇은 base에 직접 구동기가 없어 발이 지면을 밀어야 base가 움직이고, 쿼드로터는 base에 로터가 붙어 있지만 4개 입력으로 6자유도를 다뤄야 한다. underactuation은 부유 base라는 사실이 아니라 독립 입력 수와 접촉 조건이 정한다. 추력기를 6방향으로 갖춘 자유 비행체나 전방향 멀티로터는 완전 구동이고, 보행 로봇도 접촉이 충분하면 base 6자유도를 제어할 수 있다.

Manipulator equation을 base와 joints로 나누면:

```
[ M_bb  M_bj ] [ a_base  ]   [ C_b ]   [ g_b ]   [  0  ]   [ J_{c,b}^T ]
[ M_jb  M_jj ] [ q̈_joints] + [ C_j ] + [ g_j ] = [ τ_j ] + [ J_{c,j}^T ] λ
```

등호 오른쪽 입력 벡터의 위 성분 `0`은 base에 관절 토크가 없음을 나타낸다. λ는 접촉력이고, J_{c,b}(6×k)와 J_{c,j}(n×k)는 접촉 Jacobian J_c를 base 열과 관절 열로 나눈 블록이다. Base는 접촉력과 중력을 통해 가속한다.

이 구속 조건이 locomotion 제어를 어렵게 만든다. 고정 base 매니퓰레이터는 원하는 관절 토크를 그냥 모터에 명령하면 되지만, 보행 로봇은 적절한 접촉력을 만들어내야 base를 원하는 대로 움직일 수 있다.

### Centroidal Dynamics

전체 시스템의 운동량(momentum)을 질량중심(center of mass, CoM)에서 표현한 것이 centroidal dynamics이다:

**선운동량 (linear momentum):**
```
p = m v_CoM = Σ m_i v_i
```

**각운동량 (angular momentum about CoM):**
```
L = Σ (r_i - r_CoM) × (m_i v_i) + I_i ω_i
```

**Centroidal momentum의 시간 미분:**
```
ṗ = m g + Σ f_contact
L̇ = Σ (r_contact - r_CoM) × f_contact
```

CoM 역학이 balance를 결정한다. 정지 상태에서는 CoM의 수직 투영이 지지 영역(support polygon) 안에 있어야 한다(정적 안정 조건). 걷는 동안에는 ZMP(= 접촉면의 압력 중심)가 지지 영역 안에 있어야 한다(ZMP 조건). 더 일반적으로는 centroidal momentum이 적절히 조절되어야 한다.

차원 축소의 효과도 있다. n-DOF 보행 로봇의 전체 동역학은 n차원이지만, centroidal dynamics는 6차원(선운동량 3 + 각운동량 3)이다. 이 6차원 공간에서 원하는 운동량 궤적을 먼저 계획하고, 그다음 전체 관절 수준으로 분해하는 것이 일반적인 접근이다.

Centroidal momentum의 변화율은 외력(접촉력 + 중력)으로 결정된다. 따라서 locomotion에서는 원하는 운동량 궤적을 만드는 접촉력 패턴을 계획해야 한다.

```python
# Pinocchio에서 centroidal dynamics 계산
import pinocchio as pin
import numpy as np

model = pin.buildModelFromUrdf("humanoid.urdf", pin.JointModelFreeFlyer())
data = model.createData()

q = pin.randomConfiguration(model)
v = np.random.randn(model.nv)

# Centroidal momentum
pin.computeCentroidalMomentum(model, data, q, v)
h = data.hg  # 6D centroidal momentum, Pinocchio 순서는 [linear; angular]
print("Angular momentum:", h.angular)
print("Linear momentum:", h.linear)

# Centroidal momentum matrix: h = A_g(q) * v
pin.computeCentroidalMap(model, data, q)
Ag = data.Ag  # 6 x nv
h_check = Ag @ v
print("Centroidal momentum (via Ag):", h_check)

# CoM 위치 및 속도
pin.centerOfMass(model, data, q, v)
print("CoM position:", data.com[0])
print("CoM velocity:", data.vcom[0])
```

### Centroidal Dynamics 기반 Locomotion 제어의 구조

현대적인 보행 로봇 제어 파이프라인의 전형적인 구조는 다음과 같다:

```
[Contact Schedule] → [Centroidal Trajectory Optimization] → [Whole-Body Control] → [Joint Torques]

1단계: 언제 어떤 발이 땅에 닿는지 결정 (gait pattern)
2단계: Centroidal dynamics를 만족하는 CoM 궤적 + 접촉력 계획
3단계: Centroidal 목표를 달성하면서 관절 수준의 여러 제약을 만족하는 토크 계산
4단계: 모터에 토크 명령
```

부유 base를 가진 시스템에는 이 구조를 적용할 수 있다. 드론의 trajectory optimization과 수중 로봇 제어에도 유사한 구조를 사용한다.

> **추천 자료**
> - Orin et al., "Centroidal Dynamics of a Humanoid Robot", Autonomous Robots 2013 — Humanoid의 centroidal dynamics를 분석한 논문
> - Wensing et al., "Optimization-Based Control for Dynamic Legged Robots", IEEE T-RO 2024 (arXiv 2022) — Locomotion 제어 survey
> - Russ Tedrake, *Underactuated Robotics*, Ch. "Walking" (https://underactuated.csail.mit.edu/) — Underactuated 시스템과 보행의 관계
> - Carpentier, Mansard, "Pinocchio: fast forward and inverse dynamics for poly-articulated systems" (https://github.com/stack-of-tasks/pinocchio) — Pinocchio의 centroidal dynamics 구현

---

## 5.9 추천 자료

추천 순서는 배경지식에 따라 다르다.

**학부 3-4학년, 동역학 입문:**

> - Spong, Hutchinson, Vidyasagar, *Robot Modeling and Control* — 학부 수준의 입문 교재. Manipulator equation 유도가 상세하다.
> - Craig, *Introduction to Robotics: Mechanics and Control* — 산업 현장 관점. 실용적이지만 수학적 깊이는 좀 얕다.

**대학원, 수학적으로 엄밀한 이해가 필요할 때:**

> - Murray, Li, Sastry, *A Mathematical Introduction to Robotic Manipulation* (https://www.cds.caltech.edu/~murray/mlswiki/) — Lie group/algebra 관점에서 동역학을 엄밀하게 다룬다. 무료 PDF를 제공하며, 입문자가 바로 읽기에는 어렵다.
> - Featherstone, *Rigid Body Dynamics Algorithms* — Spatial vector algebra, RNEA, ABA, composite rigid body algorithm을 체계적으로 다루는 대학원 수준의 참고서.

**동역학 + 제어 통합:**

> - Russ Tedrake, *Underactuated Robotics* (https://underactuated.csail.mit.edu/) — 동역학 모델을 제어와 최적화에 어떻게 활용하는지를 다룬다. MIT OCW에 강의 영상도 있다. 무료.

**라이브러리 & 도구:**

> - Pinocchio (https://github.com/stack-of-tasks/pinocchio) — 순수 동역학 계산용 C++/Python 라이브러리. RNEA, ABA, CRBA, centroidal dynamics, analytical derivatives 등을 지원한다. CasADi·CppAD를 통한 autodiff도 가능하다 (Pinocchio 3.x).
> - Drake (https://drake.mit.edu/) — 시뮬레이션 + 최적화 + 제어를 통합한 프레임워크. MultibodyPlant가 동역학 엔진이다. Mathematical programming 인터페이스가 강력하여 trajectory optimization에 특히 유용하다.
> - MuJoCo (https://mujoco.org/) — DeepMind가 관리하는 물리 시뮬레이터. 접촉을 포함한 로봇 학습 연구에 널리 쓰인다.
> - PyBullet (https://pybullet.org/) — Bullet Physics의 Python 인터페이스. 진입 장벽이 낮아 교육용으로 적합하지만, 접촉 물리의 정확도와 속도는 관절체(articulated body) 구성, 접촉 설정, 적분 시간 간격에 따라 갈리므로 목표 작업에서 직접 비교해야 한다.

---

## 기술 흐름

```
1687 ── Newton의 운동 법칙 (Principia Mathematica)
1788 ── Lagrange의 해석역학 (Mécanique Analytique)
1965 ── Uicker의 동역학 방정식 (symbolic, 비효율적)
1980 ── Luh, Walker, Paul의 Newton-Euler 재귀 알고리즘 (RNEA, O(n))
1983 ── Featherstone의 Articulated Body Algorithm (ABA, O(n) forward dynamics)
1987 ── Featherstone의 Spatial Vector Algebra 체계 정립
2000 ── Stewart의 rigid contact dynamics 수학적 정리 (SIAM Review)
2004 ── ODE (Open Dynamics Engine) — 초기 오픈소스 물리 엔진
2012 ── MuJoCo 공개 (Todorov, Erez, Tassa)
2015 ── Bullet Physics 2.x → PyBullet 인터페이스
2016 ── Pinocchio 1.0 공개 (LAAS-CNRS)
2021 ── DeepMind가 MuJoCo 인수, 무료 공개 (소스 공개는 2022년 5월 Apache 2.0)
2022 ── Drake 1.0 (MIT → Toyota Research Institute)
2022 ── MuJoCo 2.3 릴리스 (implicitfast 적분기는 2023년 2.3.x에서 추가, elliptic friction cone은 이전부터 제공)
2023 ── MuJoCo 3.0: MJX (JAX backend for GPU parallelism)
2024 ── Pinocchio 3.0 정식 릴리스: CasADi·CppAD autodiff 지원
```

---

## 정리

실무 요점:

1. Manipulator equation `M(q)q̈ + C(q,q̇)q̇ + g(q) = τ`가 동역학 계산의 표준 형태다.
2. 역동역학(τ 계산)에는 RNEA, 순동역학(q̈ 계산)에는 ABA를 쓴다. 둘 다 O(n)이다.
3. 접촉은 제약과 불연속성을 추가하므로, 접촉 모델과 시뮬레이터의 선택을 함께 검토한다.
4. 부유 base 시스템에서는 centroidal dynamics가 핵심 도구다.
5. 실무에서는 Pinocchio나 Drake를 활용하되, 라이브러리가 내부에서 계산하는 양과 가정을 확인한다.

이 동역학 모델 위에서 computed torque control, operational space control, whole-body control 같은 제어 기법이 만들어진다.

---

# Ch.6 — 제어 이론 (Control Theory)

인식은 로봇의 상태와 환경을 추정하고, 제어는 그 정보를 바탕으로 원하는 동작을 만든다. 제어 입력을 잘못 계산하면 로봇이 불안정해지거나 충돌할 수 있으므로 정확한 모델과 feedback이 필요하다.

---

## 6.1 왜 제어를 배우는가

인식(Perception)이 환경과 상태를 추정한다면, 제어(Control)는 목표 상태에 도달하도록 actuator의 입력을 계산한다.

제어가 필요한 이유는 다음과 같다.

- **정확한 위치 추종**: feedback 없이 큰 전류를 인가하면 관절이 목표 각도를 지나쳐 진동할 수 있다. 제어기는 오차를 줄이면서 목표 위치에 도달하도록 입력을 조절한다.
- **외란 대응**: 미끄러운 바닥, 바람, 예상과 다른 payload처럼 모델에 포함되지 않은 변화가 생긴다. 센서-actuator loop를 닫아 이런 불확실성에 대응한다.
- **안전한 접촉**: 사람과 함께 일하는 산업용 로봇은 위치뿐 아니라 접촉력도 제한해야 한다.

기본 흐름은 PID에서 출발해 상태공간 제어, MPC, 임피던스 제어, Whole-Body Control로 이어진다.
 
행렬 연산, 고유값(eigenvalue), 미분방정식을 바탕에 둔다.

---

## 6.2 PID 제어

PID(Proportional-Integral-Derivative)는 1922년 Minorsky가 선박 조타 시스템을 위해 제안한 뒤 산업 현장에 널리 쓰여 왔다. 구조가 단순하고 각 항의 역할이 분명해 제어 입문에서 먼저 다루는 제어기이기도 하다.

### 기본 구조

오차 e(t) = r(t) - y(t)로 정의한다. r(t)는 목표값(reference), y(t)는 현재 출력이다.

```
u(t) = Kp * e(t) + Ki * integral(e(τ)dτ, 0, t) + Kd * de(t)/dt
```

각 항의 역할:

- P (Proportional): 현재 오차에 비례하여 제어 입력을 생성한다. Kp가 크면 반응이 빠르지만, 오버슈트가 커지고 진동이 발생한다. 적분기가 없는 플랜트에 계단 입력을 주면 P 항만으로는 정상상태 오차(steady-state error)가 남는다. 목표값 근처에서 오차가 작아지면 제어 입력도 작아지기 때문이다. 플랜트에 자유 적분기가 있으면 계단 추종의 정상 오차가 0이 될 수 있고, 중력 같은 정상 외란은 별개의 오차 원인이다.

- I (Integral): 오차의 누적값에 비례한다. 정상상태 오차를 제거하는 역할을 한다. 중력이나 마찰 같은 상수 외란이 있을 때 필수적이다. 다만 과도하면 wind-up 현상이 발생한다. 오차가 오랫동안 누적되어 제어 입력이 포화(saturation)된 상태에서, 목표에 도달한 후에도 누적된 적분값 때문에 큰 오버슈트가 생기는 문제다. 실무에서는 anti-windup 로직을 반드시 구현해야 한다.

- D (Derivative): 오차의 변화율에 비례한다. 오차가 빠르게 줄어들고 있으면 제어 입력을 줄여서 오버슈트를 억제한다. 일종의 "브레이크" 역할이다. 문제는 미분이 노이즈에 극도로 민감하다는 것이다. 센서 노이즈가 있는 실제 시스템에서는 D 항에 저역통과 필터(low-pass filter)를 걸어야 한다. D 항을 아예 안 쓰고 PI 제어만 하는 현장도 많다.

### Python 구현

```python
class PIDController:
    """이산시간 PID 제어기. Anti-windup 포함."""

def __init__(self, kp: float, ki: float, kd: float, dt: float,
                 output_limit: tuple[float, float] = (-float('inf'), float('inf')),
                 d_filter_coeff: float = 0.1):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.dt = dt
        self.output_limit = output_limit
        self.d_filter_coeff = d_filter_coeff  # D항 저역통과 필터 계수

self.integral = 0.0
        self.prev_error = 0.0
        self.prev_d_filtered = 0.0

def compute(self, error: float) -> float:
        # Proportional
        p_term = self.kp * error

# Integral (trapezoidal integration)
        self.integral += 0.5 * (error + self.prev_error) * self.dt
        i_term = self.ki * self.integral

# Derivative (with low-pass filter)
        d_raw = (error - self.prev_error) / self.dt
        d_filtered = (self.d_filter_coeff * d_raw
                      + (1.0 - self.d_filter_coeff) * self.prev_d_filtered)
        d_term = self.kd * d_filtered

# 제어 출력
        output = p_term + i_term + d_term

# Output saturation + anti-windup (clamping)
        lo, hi = self.output_limit
        if output > hi:
            output = hi
            # Anti-windup: 포화 시 적분값 역산
            self.integral -= 0.5 * (error + self.prev_error) * self.dt
        elif output < lo:
            output = lo
            self.integral -= 0.5 * (error + self.prev_error) * self.dt

self.prev_error = error
        self.prev_d_filtered = d_filtered
        return output

# 사용 예시: 1-DOF 위치 제어
import numpy as np

dt = 0.001  # 1kHz 제어 주기
pid = PIDController(kp=100.0, ki=10.0, kd=5.0, dt=dt,
                    output_limit=(-50.0, 50.0))

position = 0.0
velocity = 0.0
mass = 1.0
target = 1.0

positions = []
for step in range(5000):
    error = target - position
    force = pid.compute(error)

# 간단한 2차 위치 동역학: F = ma, 감쇠 포함
    acceleration = (force - 0.5 * velocity) / mass
    velocity += acceleration * dt
    position += velocity * dt
    positions.append(position)
```

### 튜닝 방법

**Ziegler-Nichols 방법**: 고전적 튜닝법이다. Ki = 0, Kd = 0으로 놓고 Kp를 올려가면서 시스템이 지속 진동(sustained oscillation)하는 임계 이득 Ku와 진동 주기 Tu를 구한다. 그리고 다음 표에 따라 게인을 설정한다.

```
PID:  Kp = 0.6 * Ku,  Ki = 2 * Kp / Tu,  Kd = Kp * Tu / 8
PI:   Kp = 0.45 * Ku,  Ki = 1.2 * Kp / Tu
P:    Kp = 0.5 * Ku
```

Ziegler-Nichols 튜닝은 오버슈트가 크게 나올 수 있다. 따라서 초기 게인을 정하는 데 쓰고, 응답을 보며 다시 조정한다.

**실무에서의 경험적 튜닝**: 다음은 경험적으로 게인을 조정하는 순서다.

1. D, I를 0으로 설정한다.
2. P를 높여 시스템이 빠르게 반응하되 진동하지 않는 지점을 찾는다.
3. 정상상태 오차가 남으면 I를 점진적으로 올린다. 적분 누적(wind-up) 방지가 필수적이다.
4. 오버슈트가 크면 D를 소량 추가하고 노이즈 필터를 함께 확인한다.

시스템 모델을 쓸 수 있다면 시뮬레이션에서 먼저 게인을 조정한 뒤 실기에 적용한다.

### PID의 한계

PID는 분명한 한계가 있다:

- SISO(Single-Input Single-Output) 전용이다. 6축 로봇 팔처럼 관절 간 커플링이 있는 시스템에서는 각 관절에 독립적으로 PID를 걸면 성능이 떨어진다. 한 관절의 움직임이 다른 관절에 외란으로 작용하기 때문이다.
- 비선형 시스템에 약하다. PID는 선형 제어기다. 로봇 동역학은 비선형이므로 작동점(operating point) 근처에서만 잘 동작한다.
- 제약 조건을 처리할 수 없다. 토크 제한, 관절 각도 제한, 속도 제한 같은 물리적 제약을 PID 구조 안에서 명시적으로 다룰 방법이 없다.
- 미래를 예측하지 않는다. 현재 오차와 누적 오차, 오차의 변화율에 반응한다. Feedforward가 없으면 추종 성능이 제한된다.

PID는 구현과 해석이 비교적 단순하다. 제어 대상의 결합이 약하고 성능 요구를 만족한다면 PID로 충분할 수 있으며, 산업용 로봇의 관절 서보에도 PID 계열 제어기가 쓰인다.

---

## 6.3 상태공간 표현 (State-Space Representation)

PID는 입력-출력 관계만 본다. 시스템 "내부"에서 무슨 일이 일어나는지는 모른다. 상태공간 표현은 시스템의 내부 상태를 명시적으로 기술하는 방법이다.

### 기본 형태

연속시간 선형 시스템:

```
x_dot(t) = A * x(t) + B * u(t)    (상태 방정식)
y(t)     = C * x(t) + D * u(t)    (출력 방정식)
```

- x(t): 상태 벡터 (n x 1). 시스템을 완전히 기술하는 데 필요한 최소 변수 집합.
- u(t): 입력 벡터 (m x 1). 제어 입력.
- y(t): 출력 벡터 (p x 1). 측정 가능한 출력.
- A: 시스템 행렬 (n x n). 시스템의 고유 동특성을 결정한다.
- B: 입력 행렬 (n x m). 입력이 상태에 미치는 영향.
- C: 출력 행렬 (p x n). 상태에서 출력으로의 매핑.
- D: 직접 전달 행렬 (p x m). 대부분의 물리 시스템에서 0이다.

예를 들어, 질량-스프링-댐퍼 시스템 (m * x_ddot + c * x_dot + k * x = F)에서 상태를 x1 = 위치, x2 = 속도로 잡으면:

```
A = [[0, 1], [-k/m, -c/m]]
B = [[0], [1/m]]
C = [[1, 0]]   (위치만 측정)
D = [[0]]
```

### 전달함수와의 관계

전달함수 G(s) = C * (sI - A)^(-1) * B + D 이다. 전달함수는 SISO 시스템 분석에 유용하며, 다입출력 로봇 제어에서는 상태공간 표현이 변수 사이의 결합 관계를 직관적으로 드러낸다. 어느 표현을 쓸지는 시스템의 입출력 구조와 분석 목적에 따라 정한다.

### 가제어성 (Controllability)

시스템이 가제어(controllable)하다는 것은, 임의의 초기 상태에서 임의의 최종 상태로 유한 시간 내에 이동할 수 있다는 것이다. 가제어성 행렬:

```
C_ctrl = [B, A*B, A^2*B, ..., A^(n-1)*B]
```

이 행렬의 rank가 n이면 가제어이다. rank가 n보다 작으면, 제어 입력으로 도달할 수 없는 상태가 존재한다는 뜻이다. 무한시간 LQR의 안정화 해에는 완전한 가제어성 대신 안정화 가능성(stabilizability)이 필요하다. 또한 상태 비용의 검출 가능성(detectability)과 양의 정부호 입력 비용 같은 조건을 확인해야 한다.

### 가관측성 (Observability)

시스템이 가관측(observable)하다는 것은, 출력 y(t)를 관찰하여 초기 상태 x(0)를 유일하게 결정할 수 있다는 것이다. 가관측성 행렬:

```
O = [C; C*A; C*A^2; ...; C*A^(n-1)]
```

rank가 n이면 가관측이다. 가관측하지 않으면 상태 추정(observer, Kalman filter)이 제대로 동작하지 않는다.

### 왜 PID에서 상태공간으로 넘어가야 하는가

PID로 각 관절을 독립적으로 제어하면, 관절 간 동적 커플링을 무시하게 된다. 2-DOF 로봇 팔만 해도 한 관절이 빠르게 움직이면 다른 관절에 원심력과 코리올리 힘이 작용한다. 이걸 외란으로 처리하면 PID의 I 항이 열심히 보상하겠지만, 응답이 느리고 성능이 나쁘다.

상태공간에서는 시스템 전체를 하나의 모델로 기술하고, 모든 상태 변수를 동시에 고려하여 제어 입력을 계산한다. 이 방식이 다음 절의 LQR과 MPC의 기반이 된다.

```python
import numpy as np
from scipy import signal
import control  # pip install control

# 도립진자(inverted pendulum) 상태공간 모델
# 상태: [x, x_dot, theta, theta_dot]
# x: 카트 위치, theta: 진자 각도 (수직에서)
M = 1.0    # 카트 질량 (kg)
m = 0.1    # 진자 질량 (kg)
l = 0.5    # 진자 길이 (m)
g = 9.81   # 중력 (m/s^2)

# 선형화된 상태공간 행렬 (theta ≈ 0 근처)
A = np.array([
    [0, 1, 0, 0],
    [0, 0, -m * g / M, 0],
    [0, 0, 0, 1],
    [0, 0, (M + m) * g / (M * l), 0]
])
B = np.array([[0], [1 / M], [0], [-1 / (M * l)]])
C = np.array([[1, 0, 0, 0],
              [0, 0, 1, 0]])  # 카트 위치와 진자 각도 측정
D = np.zeros((2, 1))

# 가제어성 확인
ctrb_matrix = control.ctrb(A, B)
print(f"가제어성 행렬 rank: {np.linalg.matrix_rank(ctrb_matrix)}")  # 4 = 가제어

# 가관측성 확인
obsv_matrix = control.obsv(A, C)
print(f"가관측성 행렬 rank: {np.linalg.matrix_rank(obsv_matrix)}")  # 4 = 가관측

# 시스템 극점 (eigenvalues of A)
eigenvalues = np.linalg.eigvals(A)
print(f"시스템 극점: {eigenvalues}")
# 양의 실수부를 가진 극점이 있으면 → 불안정 시스템 (도립진자가 그렇다)
```

---

## 6.4 LQR (Linear Quadratic Regulator)

PID가 경험과 튜닝에 의존한다면, LQR은 최적화에 기반한 제어기이다. 주어진 비용 함수를 최소화하는 제어 입력을 해석적으로 구할 수 있다.

### 비용 함수

```
J = integral_0^inf (x(t)^T * Q * x(t) + u(t)^T * R * u(t)) dt
```

- Q (n×n, 양의 반정치): 상태 오차에 대한 페널티. "상태가 0에서 벗어나는 것이 얼마나 싫은가."
- R (m×m, 양정치): 제어 입력에 대한 페널티. "제어 에너지를 얼마나 아끼고 싶은가."

Q를 크게 하면 상태가 빠르게 0으로 수렴하지만 제어 입력은 커진다. R을 크게 하면 제어 입력이 작아지는 대신 상태 수렴이 느려진다. LQR은 이 둘 사이의 균형을 조절한다.

### Q, R 행렬 튜닝

실용적인 방법: Q와 R을 대각 행렬로 놓고, 각 대각 원소를 해당 상태/입력의 허용 범위의 역수 제곱으로 설정한다.

```
Q_ii = 1 / (허용 가능한 x_i의 최대값)^2
R_jj = 1 / (허용 가능한 u_j의 최대값)^2
```

예: 카트 위치가 0.5m 이내, 진자 각도가 0.1rad 이내, 힘이 20N 이내를 원한다면:

```
Q = diag(1/0.5^2, 0, 1/0.1^2, 0) = diag(4, 0, 100, 0)
R = [1/20^2] = [0.0025]
```

이것은 출발점일 뿐이다. 이후 시뮬레이션을 돌려가며 조정한다.

### Algebraic Riccati Equation (ARE)

LQR의 최적 게인 K는 다음 Algebraic Riccati Equation의 해 P로부터 구한다:

```
A^T * P + P * A - P * B * R^(-1) * B^T * P + Q = 0
```

최적 상태 피드백 게인: K = R^(-1) * B^T * P

제어 법칙: u(t) = -K * x(t)

폐루프 시스템 (A - BK)의 모든 고유값은 좌반면에 놓인다. 따라서 시스템의 안정성을 수학적으로 보장할 수 있다.

### Python 구현

```python
import numpy as np
from scipy.linalg import solve_continuous_are

# 앞 절의 도립진자 모델 사용
M, m, l, g = 1.0, 0.1, 0.5, 9.81

A = np.array([
    [0, 1, 0, 0],
    [0, 0, -m * g / M, 0],
    [0, 0, 0, 1],
    [0, 0, (M + m) * g / (M * l), 0]
])
B = np.array([[0], [1 / M], [0], [-1 / (M * l)]])

# 비용 함수 가중치
Q = np.diag([4.0, 0.0, 100.0, 0.0])  # 위치, 속도, 각도, 각속도
R = np.array([[0.0025]])

# ARE 풀기
P = solve_continuous_are(A, B, Q, R)

# 최적 게인 계산
K = np.linalg.inv(R) @ B.T @ P
print(f"LQR 게인 K: {K}")

# 폐루프 극점 확인
A_cl = A - B @ K
eigenvalues_cl = np.linalg.eigvals(A_cl)
print(f"폐루프 극점: {eigenvalues_cl}")
# 모든 실수부가 음수 → 안정

def simulate_lqr(A, B, K, x0, dt=0.001, t_final=5.0):
    """LQR 폐루프 시뮬레이션 (Euler 적분)."""
    n_steps = int(t_final / dt)
    n = A.shape[0]
    x_history = np.zeros((n_steps, n))
    u_history = np.zeros((n_steps, 1))

x = x0.copy()
    for i in range(n_steps):
        u = -K @ x
        x_history[i] = x.flatten()
        u_history[i] = u.flatten()
        x_dot = A @ x + B @ u
        x = x + x_dot * dt

return x_history, u_history

# 초기 조건: 진자가 10도 기울어진 상태
x0 = np.array([[0.0], [0.0], [np.radians(10)], [0.0]])
x_hist, u_hist = simulate_lqr(A, B, K, x0)

# x_hist[:, 2]가 0으로 수렴하면 성공
print(f"최종 진자 각도: {np.degrees(x_hist[-1, 2]):.4f} deg")
```

### LQR의 한계

- 선형 모델이 필요하다. 비선형 시스템은 작동점 근처에서 선형화해야 한다. 작동점에서 멀어지면 성능이 급격히 떨어진다.
- 제약 조건을 명시적으로 처리할 수 없다. 토크 제한, 속도 제한 같은 물리적 제약을 비용 함수에 넣을 수 없다. 제어 입력이 포화되면 최적성이 깨진다.
- 전체 상태를 알아야 한다. u = -Kx이므로 모든 상태 변수를 측정하거나 추정(observer)해야 한다.
- 추종(tracking) 문제에 그대로 적용할 수 없다. 기본 LQR은 regulator, 즉 상태를 0으로 보내는 문제만 풀 수 있다. 시변 목표를 추종하려면 확장이 필요하다.

이런 한계를 극복하기 위해 MPC가 등장한다.

---

## 6.5 MPC (Model Predictive Control)

MPC(Model Predictive Control)는 매 제어 주기마다 유한 구간(finite horizon) 최적화 문제를 풀어 제어 입력을 계산하는 방법이다. 제약을 최적화 문제에 직접 넣을 수 있어 로봇 제어에서 널리 연구·적용된다.

### 기본 개념

매 time step k에서 다음 절차를 거친다:

1. 현재 상태 x(k)를 센서로 측정하거나 필터로 추정한다.
2. 시스템 모델을 바탕으로 N step 앞까지 미래 거동을 예측한다.
3. 제약 조건을 명시적으로 반영하며 비용 함수를 최소화하는 입력 시퀀스 {u(k), u(k+1), ..., u(k+N-1)}을 구하는 최적화 단계다.
4. 산출된 수열 가운데 첫 번째 입력 u(k)만 실제로 인가하고 나머지는 폐기한다.
5. 다음 time step에서 1단계로 순환한다.

이것을 "receding horizon" 전략이라 한다. 매번 최적화를 새로 풀기 때문에, 모델 오차나 외란에 대한 피드백 효과가 자연스럽게 생긴다.

### 로보틱스에서 MPC가 유용한 이유

- 제약 조건 처리: 토크 제한, 관절 각도 제한, 속도 제한, 충돌 회피 등을 최적화 문제의 제약 조건으로 직접 넣을 수 있다. 기본 PID나 LQR에는 이런 제약 표현이 내장되어 있지 않지만 saturation, reference governor, constrained-LQR 같은 확장을 사용할 수 있다.
- 비선형 모델 사용 가능: Nonlinear MPC에서는 비선형 동역학 모델을 그대로 쓸 수 있다.
- 미래 예측: 현재 오차에 반응하는 데 머물지 않고 미래 궤적을 예측하여 능동적으로 대응한다. 보행 로봇이 다음 발을 내딛기 전에 미리 무게 중심을 이동시키는 것이 이 원리다.
- 다목적 최적화: 비용 함수에 여러 목표를 동시에 넣을 수 있다. "목표 궤적을 추종하면서 에너지를 아끼고 토크 제한을 지켜라."

### Linear MPC vs Nonlinear MPC

Linear MPC: 선형 모델(x(k+1) = A*x(k) + B*u(k))을 사용하고, 비용 함수가 이차(quadratic), 제약이 선형이면 문제가 QP(Quadratic Program)가 된다. feasible한 convex QP는 전역 최적해 계산이 보장된다. 다만 실제 연산 시간은 문제 크기, 희소성 구조, solver와 실행 하드웨어에 좌우된다.

Nonlinear MPC (NMPC): 비선형 동역학 모델을 사용한다. 문제가 비볼록(non-convex)이 되어 풀기 어렵고, 전역 최적해를 보장하지 못한다. CasADi + IPOPT는 자동 미분과 NLP solver를 연결하는 한 가지 널리 쓰이는 조합이며, acados·FORCESPRO·SNOPT 등 다른 선택지도 있다.

실무에서의 선택: 시스템이 충분히 선형에 가깝거나 제어 주기가 매우 짧아야 하면 Linear MPC를 쓰고, 비선형성이 크고 제어 주기에 여유가 있으면 NMPC를 쓴다.

### 실시간성 문제

MPC의 핵심 난관은 제한된 시간 안에 최적화를 반복해서 풀어야 한다는 것이다. MPC를 1kHz inner loop와 같은 주기로 동기 실행한다면 1ms 안에 상태 처리와 QP 풀이를 끝내야 한다. 실제 시스템은 MPC를 더 느린 outer loop로 돌리고 빠른 tracking controller와 결합하기도 한다.

주요 QP solver:
- OSQP (https://osqp.org/): operator splitting 기반, sparse QP에 강하다. Linear MPC에서 가장 먼저 고려할 선택.
- qpOASES: active-set 기반, warm-starting이 가능하여 연속적인 QP 풀이에 효율적.
- ECOS/Clarabel: second-order cone programming까지 처리 가능.

NMPC는:
- CasADi + IPOPT: 자동 미분 모델과 범용 interior-point NLP solver의 조합.
- acados (https://docs.acados.org/): CasADi 기반이지만 실시간성에 최적화됨. C 코드 생성 가능.

동기식 단일 solve가 5ms 걸리면 그 solve를 매번 완료하는 nominal update rate는 200Hz를 넘을 수 없다. 다만 전체 제어 주기는 전처리·통신·jitter를 포함하고, asynchronous solve나 별도 inner loop를 쓰면 구조가 달라진다.

### Linear MPC Python 예시

```python
import numpy as np
from scipy import sparse
import osqp

def linear_mpc(A, B, Q, R, Q_f, x0, N, x_min, x_max, u_min, u_max):
    """
    Linear MPC: QP로 변환하여 OSQP로 풀기.

A, B: 이산시간 시스템 행렬
    Q: 상태 비용 (stage)
    R: 입력 비용
    Q_f: 종단 비용 (terminal)
    x0: 현재 상태
    N: 예측 구간 (horizon)
    x_min, x_max: 상태 제약
    u_min, u_max: 입력 제약
    """
    n = A.shape[0]  # 상태 차원
    m = B.shape[1]  # 입력 차원

# 결정 변수: z = [x(0), x(1), ..., x(N), u(0), ..., u(N-1)]
    n_var = (N + 1) * n + N * m

# --- 비용 함수 행렬 (P, q) ---
    # min 0.5 * z^T P z + q^T z
    P_blocks = [sparse.kron(sparse.eye(N), Q)]      # x(0) ~ x(N-1)
    P_blocks.append(Q_f)                              # x(N) terminal cost
    P_blocks.append(sparse.kron(sparse.eye(N), R))   # u(0) ~ u(N-1)
    P = sparse.block_diag(P_blocks, format='csc')
    q = np.zeros(n_var)

# --- 등식 제약: 동역학 ---
    # x(k+1) = A*x(k) + B*u(k)
    # → A*x(k) + B*u(k) - x(k+1) = 0
    Ax_eq = sparse.kron(sparse.eye(N + 1), -sparse.eye(n), format='lil')  # 슬라이스 대입이 가능한 포맷
    Au_shift = sparse.kron(sparse.eye(N, N + 1, 1), sparse.eye(n))
    # 수정: 좌하단에 A 추가
    for i in range(N):
        row_start = (i + 1) * n
        col_start = i * n
        Ax_eq[row_start:row_start + n, col_start:col_start + n] = A

Bu_eq = sparse.lil_matrix(((N + 1) * n, N * m))
    for i in range(N):
        Bu_eq[(i + 1) * n:(i + 2) * n, i * m:(i + 1) * m] = B
    Bu_eq = sparse.csc_matrix(Bu_eq)

A_eq = sparse.hstack([Ax_eq, Bu_eq], format='csc')
    l_eq = np.zeros((N + 1) * n)
    l_eq[:n] = -x0.flatten()  # 초기 조건
    u_eq = l_eq.copy()

# --- 부등식 제약: 상태 및 입력 범위 ---
    A_ineq = sparse.eye(n_var, format='csc')
    l_ineq = np.concatenate([
        np.tile(x_min, N + 1),
        np.tile(u_min, N)
    ])
    u_ineq = np.concatenate([
        np.tile(x_max, N + 1),
        np.tile(u_max, N)
    ])

# --- 전체 제약 결합 ---
    A_total = sparse.vstack([A_eq, A_ineq], format='csc')
    l_total = np.concatenate([l_eq, l_ineq])
    u_total = np.concatenate([u_eq, u_ineq])

# --- OSQP 풀기 ---
    solver = osqp.OSQP()
    solver.setup(P, q, A_total, l_total, u_total,
                 warm_starting=True, verbose=False,
                 eps_abs=1e-6, eps_rel=1e-6)
    result = solver.solve()

if result.info.status != 'solved':
        print(f"MPC 풀이 실패: {result.info.status}")
        return None, None

# 첫 번째 입력만 반환
    u_opt = result.x[(N + 1) * n:(N + 1) * n + m]
    x_pred = result.x[:(N + 1) * n].reshape(N + 1, n)
    return u_opt, x_pred

# 사용 예시: 2차원 더블 인티그레이터
dt = 0.1
A_d = np.array([[1, dt], [0, 1]])   # 이산시간
B_d = np.array([[0.5 * dt**2], [dt]])
n, m_ctrl = 2, 1

Q_mpc = sparse.diags([10.0, 1.0])
R_mpc = sparse.diags([0.1])
Q_f_mpc = sparse.diags([100.0, 10.0])  # terminal cost 크게

x0 = np.array([5.0, 0.0])  # 초기 위치 5m, 속도 0
N_horizon = 20

x_min_val = np.array([-10.0, -5.0])
x_max_val = np.array([10.0, 5.0])
u_min_val = np.array([-1.0])   # 힘 제한
u_max_val = np.array([1.0])

u_opt, x_pred = linear_mpc(
    A_d, B_d,
    Q_mpc, R_mpc, Q_f_mpc,
    x0, N_horizon,
    x_min_val, x_max_val,
    u_min_val, u_max_val
)
print(f"최적 제어 입력: {u_opt}")
print(f"예측 궤적 (위치): {x_pred[:5, 0]}")
```

### 산업 사례

- **Boston Dynamics Atlas**: 제조사의 공개 설명에 따르면 Atlas의 컨트롤러 중심에는 MPC가 있다. 물체를 들고 던지는 실험에서는 전신 관절의 운동, 각 링크의 운동량, 로봇과 물체 사이의 힘을 모델에 포함했다.

---

## 6.6 임피던스/어드미턴스 제어 (Impedance/Admittance Control)

지금까지 다룬 제어 기법들은 주로 "위치를 원하는 곳에 보내는 것"에 집중했다. 하지만 로봇이 환경과 물리적으로 접촉하는 순간, 위치 제어만으로는 부족해진다.

### 위치 제어 vs 힘 제어 vs 임피던스 제어

- 위치 제어(Position Control): 목표 위치를 추종한다. 자유 공간이나 컴플라이언트한 환경에서 적합하다. 강성 환경에서는 접촉력이 환경 강성과 위치 오차의 곱으로 커지므로 오히려 위험하다. 로봇 팔이 테이블 위의 컵을 집으려는데 테이블 높이가 1mm만 달라도 위치 제어기는 이를 모른 채 계속 밀어 넣으려 하고, 과도한 힘이 발생한다.

- 힘 제어(Force Control): 목표 힘을 추종한다. 연마, 조립 같은 접촉 태스크에서 필요하다. 그러나 순수 힘 제어는 비접촉 상태에서 불안정하다. 힘 센서 노이즈에도 민감하다.

- 임피던스 제어(Impedance Control): 위치와 힘의 관계를 제어한다. 로봇이 가상의 스프링-댐퍼 시스템처럼 행동하도록 만든다. 환경과 접촉하면 자연스럽게 힘이 발생하고, 비접촉 상태에서는 위치 제어처럼 동작한다.

### 가상 스프링-댐퍼 모델

임피던스 제어는 목표로 하는 질량-스프링-댐퍼 관계를 다음 식으로 나타낸다.

```
F = M_d * (x_ddot_d - x_ddot) + D_d * (x_dot_d - x_dot) + K_d * (x_d - x)
```

또는 관성 항을 무시한 간소화 버전:

```
F = K_d * (x_d - x) + D_d * (x_dot_d - x_dot)
```

- K_d: 가상 강성(virtual stiffness). 크면 위치 추종이 정확하지만, 접촉 시 힘이 크다.
- D_d: 가상 감쇠(virtual damping). 진동을 억제한다.
- M_d: 가상 관성(virtual inertia). 보통 조정하기 어려워서 관성 항은 생략하는 경우가 많다.

K_d와 D_d는 태스크에 맞게 조정한다:
- 유리잔을 집을 때: K_d 낮게 (부드럽게), D_d 높게 (안정적으로)
- 볼트를 조일 때: K_d 높게 (정밀하게)
- 사람과 협업할 때: K_d 매우 낮게 (안전하게)

```python
import numpy as np

class ImpedanceController:
    """카르테시안 공간 임피던스 제어기 (1-DOF 간소화)."""

def __init__(self, k_d: float, d_d: float, m_d: float = 0.0):
        self.k_d = k_d   # 가상 강성 (N/m)
        self.d_d = d_d   # 가상 감쇠 (N*s/m)
        self.m_d = m_d   # 가상 관성 (kg)

def compute_force(self, x_d, x, x_dot_d, x_dot,
                      x_ddot_d=0.0, x_ddot=0.0) -> float:
        """목표 임피던스 관계에 따른 힘 계산."""
        f = (self.k_d * (x_d - x)
             + self.d_d * (x_dot_d - x_dot)
             + self.m_d * (x_ddot_d - x_ddot))
        return f

# 시뮬레이션: 로봇이 벽에 접근하여 접촉
dt = 0.001
controller = ImpedanceController(k_d=500.0, d_d=50.0)

# 로봇 + 환경
robot_mass = 2.0
position = 0.0
velocity = 0.0
target_position = 0.15  # 목표 위치
wall_position = 0.10    # 벽 위치 (목표보다 가까움)
wall_stiffness = 10000.0  # 벽의 강성

positions = []
forces = []
contact_forces = []

for step in range(10000):
    # 환경 접촉력
    if position > wall_position:
        f_env = -wall_stiffness * (position - wall_position)
    else:
        f_env = 0.0

# 임피던스 제어 출력
    f_ctrl = controller.compute_force(
        x_d=target_position, x=position,
        x_dot_d=0.0, x_dot=velocity
    )

# 동역학
    acceleration = (f_ctrl + f_env) / robot_mass
    velocity += acceleration * dt
    position += velocity * dt

positions.append(position)
    forces.append(f_ctrl)
    contact_forces.append(-f_env)

# 결과: position은 wall_position 근처에서 안정화
# 벽을 부수지 않고, 적절한 접촉력으로 밀고 있다
print(f"최종 위치: {positions[-1]:.4f} m (벽: {wall_position} m)")
print(f"최종 접촉력: {contact_forces[-1]:.2f} N")
# 순수 위치 제어였으면 벽에 10000 N/m * 0.05 m = 500 N을 때렸을 것이다
```

### Admittance Control

임피던스 제어가 "위치 편차 → 힘 출력"이라면, 어드미턴스 제어는 반대다: "힘 입력 → 위치 출력."

```
x_d_new = x_d + (1 / K_d) * F_ext      # 강성 항: 힘 → 위치 오프셋
x_dot_d = (1 / D_d) * F_ext             # 감쇠 항: 힘 → 목표 속도 (힘의 미분이 아니다)
```

좀 더 정확히, 외력 F_ext가 측정되면 이를 가상 임피던스 모델에 넣어서 목표 위치를 수정하고, 그 수정된 목표를 기존 (강성이 높은) 위치 제어기에 전달한다.

산업용 로봇에서 어드미턴스 제어가 많이 쓰이는 이유: 산업용 로봇은 이미 매우 정밀한 위치 제어기가 내장되어 있고, 대부분 외부에서 토크 명령을 직접 줄 수 없다. 그래서 힘 센서(F/T sensor)로 외력을 측정하고, 위치 명령을 수정하는 어드미턴스 방식이 더 실용적이다.

외부 토크 제어가 개방된 Franka Emika Panda 같은 연구용 로봇에서는 임피던스 제어가 한층 자연스럽게 맞아떨어진다.

---

## 6.7 심화: Whole-Body Control

휴머노이드 로봇이나 사족 보행 로봇은 관절이 수십 개이고, 여러 개의 접촉점(발, 손)을 동시에 관리해야 하며, 균형도 유지해야 한다. 이런 시스템에서 "각 관절에 PID를 걸어라"는 것은 사실상 의미가 없다. 전신(whole-body) 레벨에서 통합적으로 제어해야 한다.

### Task-space vs Joint-space

- Joint-space control: 관절 각도 q를 직접 제어한다. 간단하지만 태스크 수준의 목표(end-effector 위치, 무게중심 위치)를 달성하려면 역기구학(inverse kinematics)을 먼저 풀어야 한다.

- Task-space control: 태스크 좌표(카르테시안 위치, 방향)에서 직접 제어한다. 태스크 목표를 자연스럽게 기술할 수 있다. 관절 공간으로의 매핑은 제어기 내부에서 처리한다.

### Operational Space Control (Khatib, 1987)

Khatib의 Operational Space Framework는 task-space 제어의 기초다. 이 방법은 태스크 공간에서 동역학을 직접 유도한다.

조인트 공간 동역학:

```
M(q) * q_ddot + C(q, q_dot) * q_dot + g(q) = tau + J^T * F_ext
```

태스크 공간으로 변환:

```
Lambda(q) * x_ddot + mu(q, q_dot) * x_dot + p(q) = F + F_ext
```

여기서 Lambda = (J * M^(-1) * J^T)^(-1)은 태스크 공간 관성 행렬이다.

태스크 공간에서 원하는 가속도 x_ddot_d를 달성하기 위한 관절 토크:

```
tau = J^T * Lambda * (x_ddot_d - J_dot * q_dot) + C * q_dot + g(q)
```

이 프레임워크 위에 임피던스 제어를 결합하면, 태스크 공간에서 원하는 동적 행동(impedance)을 구현할 수 있다.

### QP 기반 Whole-Body Control

현대적 WBC는 매 제어 주기에 QP(Quadratic Program)를 풀어 여러 태스크를 동시에 처리한다.

기본 구조:

```
minimize    || J_task * q_ddot - x_ddot_d ||^2  (태스크 추종)
subject to  M(q)*q_ddot + h(q,q_dot) = S^T*tau + J_c^T*F_c  (동역학)
            F_c ∈ friction cone                   (접촉력 제약)
            tau_min ≤ tau ≤ tau_max               (토크 제한)
```

여기서:
- J_task: 태스크 자코비안
- J_c: 접촉 자코비안
- F_c: 접촉력
- S: selection matrix (underactuated 자유도 제거)

**다중 태스크 우선순위**: 실제 로봇에서는 여러 태스크가 충돌한다. "오른손을 목표 위치에 보내라", "균형을 유지하라", "관절 한계를 지켜라"가 동시에 걸린다. 이때 태스크에 우선순위를 부여한다:

1. 최고 우선순위: 접촉 제약 (발이 바닥에 붙어 있어야 한다), 관절 한계
2. 높은 우선순위: 균형 유지 (CoM 제어)
3. 중간 우선순위: end-effector 위치 제어
4. 낮은 우선순위: 자세 유지 (null-space)

이것을 strict hierarchy로 구현하려면 null-space projection을 쓰거나, 각 우선순위 레벨의 QP를 순차적으로 푼다 (hierarchical QP). 또는 soft priority로 가중치를 다르게 두어 하나의 QP로 합칠 수도 있다.

### Contact-Consistent Control

보행 로봇에서 접촉력은 물리적으로 타당해야 한다:

- 단방향 접촉(unilateral contact): 발이 바닥을 당길 수 없다. F_z >= 0.
- 마찰 원뿔(friction cone): 접선력이 수직력 x 마찰계수보다 작아야 한다. sqrt(F_x^2 + F_y^2) <= mu * F_z.
- ZMP/CoP 제약: 압력 중심(Center of Pressure)이 지지 다각형(support polygon) 안에 있어야 넘어지지 않는다.

이 모든 제약을 QP에 넣으면, 물리적으로 실현 가능한 제어 입력을 얻을 수 있다. 마찰 원뿔은 원래 비선형(second-order cone)이지만, 다면체로 근사(linearized friction cone)하면 QP로 풀 수 있다.

```python
import numpy as np

def linearized_friction_cone(mu, n_edges=8):
    """
    마찰 원뿔의 다면체 근사.
    반환: A_cone * F <= 0 형태의 제약 행렬.
    F = [fx, fy, fz]^T
    """
    A_rows = []
    for i in range(n_edges):
        theta = 2 * np.pi * i / n_edges
        # mu * fz >= cos(theta)*fx + sin(theta)*fy
        # → cos(theta)*fx + sin(theta)*fy - mu*fz <= 0
        row = [np.cos(theta), np.sin(theta), -mu]
        A_rows.append(row)
    # fz >= 0 → -fz <= 0
    A_rows.append([0, 0, -1])
    return np.array(A_rows)

# 마찰계수 0.7, 8각형 근사
A_friction = linearized_friction_cone(mu=0.7)
print(f"마찰 원뿔 제약 행렬 shape: {A_friction.shape}")
# (9, 3) → 9개의 선형 부등식으로 3D 마찰 원뿔을 근사
```

---

## 6.8 심화: Lyapunov 안정성과 적응 제어

제어기를 설계했으면, "이 제어기가 시스템을 정말 안정하게 만드는가?"를 증명해야 한다. 시뮬레이션에서 잘 되는 것과 수학적으로 안정성이 보장되는 것은 전혀 다른 문제다. Lyapunov 이론은 이 증명의 핵심 도구다.

### Lyapunov 안정성

비선형 시스템 x_dot = f(x)에서 원점이 평형점이라 하자 (f(0) = 0).

Lyapunov의 직접 방법(direct method): 함수 V(x)가 다음을 만족하면 원점은 안정하다.

1. V(0) = 0
2. V(x) > 0 for all x != 0 (양정치)
3. V_dot(x) = dV/dx * f(x) <= 0 (비증가)

V_dot(x) < 0이면 점근적 안정(asymptotically stable), 즉 시간이 지남에 따라 상태가 원점으로 수렴한다.

물리적 직관: V(x)를 에너지로 생각하면 된다. 에너지가 항상 양수이고, 시간에 따라 감소하면, 시스템은 에너지가 최소인 평형점으로 수렴한다.

어려운 점: V(x)를 찾는 것이다. 일반적인 방법론이 없다. 기계 시스템에서는 역학적 에너지(운동에너지 + 위치에너지)가 자연스러운 Lyapunov 함수 후보이다. 선형 시스템에서는 V(x) = x^T * P * x (P는 ARE의 해)가 Lyapunov 함수가 된다. LQR의 안정성 증명이 여기서 나온다.

### 적응 제어 (Adaptive Control)

모델 파라미터가 정확히 알려져 있지 않을 때 쓴다. 예를 들어 로봇 팔에 실린 페이로드의 무게를 모른다거나, 마찰 계수가 시간에 따라 변한다거나.

제어기 내에 파라미터 추정기(estimator)를 내장하고, 제어와 추정을 동시에 수행한다.

로봇 동역학은 다음과 같이 파라미터에 대해 선형인 형태로 쓸 수 있다:

```
M(q)*q_ddot + C(q,q_dot)*q_dot + g(q) = Y(q, q_dot, q_ddot) * theta
```

여기서 Y는 regressor matrix이며, theta는 질량·관성·마찰 등을 묶은 동역학 파라미터 벡터다.

적응 제어 법칙:

```
tau = Y * theta_hat - K_d * s
theta_hat_dot = -Gamma * Y^T * s
```

여기서 s는 sliding variable, theta_hat은 파라미터 추정값, Gamma는 적응 게인 행렬이다.

Lyapunov 함수를 적절히 잡으면 (V = 0.5*s^T*M*s + 0.5*theta_tilde^T*Gamma^(-1)*theta_tilde), V_dot <= 0을 보일 수 있고, 추종 오차가 0으로 수렴함을 증명할 수 있다. 단, theta_hat이 실제 theta로 수렴하는 것은 보장되지 않는다. 수렴하는 것은 추종 오차뿐이다.

### Robust Control

모델 불확실성이 있지만 그 범위(bound)는 아는 경우에 쓴다.

- H-infinity control: 최악의 외란에 대한 성능을 최적화한다. 보장의 형태는 유도 L2 노름의 상한으로, 외란에서 오차로 가는 에너지 이득이 $\gamma$ 이하라는 뜻이다. 외란의 에너지가 커지면 오차도 그에 비례해 커질 수 있으므로 절대 상한이 아니다. 수학이 무겁고 (Riccati 부등식, LMI), 보수적인 경향이 있다.

- Sliding Mode Control: 상태를 슬라이딩 면(sliding surface)으로 유한 시간 내에 끌어온 뒤, 슬라이딩 면 위에서 원하는 동특성을 따르게 한다. 모델 불확실성에 매우 강건하다. 문제는 chattering: 슬라이딩 면 근처에서 고주파 스위칭이 발생하여 액추에이터에 부담을 준다. Boundary layer approach나 higher-order sliding mode로 완화한다.

### 언제 쓰는가, 언제 안 쓰는가

| 상황 | 추천 | 비추천 |
|------|------|--------|
| 모델이 정확하고 선형성 충분 | LQR, MPC | 적응 제어 (과설계) |
| 파라미터 불확실성이 큼 | 적응 제어 | PID만으로 버티기 |
| 불확실성 범위를 알고 있음 | Robust control (H-inf) | 적응 제어 (불필요) |
| 안전 인증이 필요함 | Lyapunov 기반 증명 | "시뮬레이션에서 됐으니까 OK" |
| 빠르게 프로토타입 | PID + feedforward | 처음부터 H-infinity |

적응 제어와 sliding mode는 불확실성의 구조와 요구되는 보장에 따라 선택한다. 제어기 안정성을 체계적으로 입증하려면 Lyapunov 함수나 동등한 수학적 분석이 뒷받침되어야 한다. 기능안전 규격은 위험 분석과 안전 등급 결정, 검증·확인 증거의 체계를 요구하며 특정 증명 기법을 지정하지는 않는다. 이런 해석 결과는 그 증거를 구성하는 유력한 수단이다.

---

## 6.9 추천 자료

> **Åström & Murray, "Feedback Systems: An Introduction for Scientists and Engineers"**
> https://fbswiki.org/
> 무료 PDF. PID, 상태공간, 주파수 응답을 한 흐름으로 설명하는 입문서다.

> **Steve Brunton, "Control Bootcamp" (YouTube)**
> https://www.youtube.com/playlist?list=PLMrJAkhIeNNR20Mz-VpzgfQs5zrYi085m
> 상태공간, 가제어성, 가관측성, LQR을 15분 안팎의 영상으로 설명한다. 교재의 수식 유도에 들어가기 전 개념을 정리하는 데 유용하다.

> **Slotine & Li, "Applied Nonlinear Control"**
> 비선형 제어, Lyapunov 안정성, 적응 제어를 다루는 교재다. 6.8절의 내용을 더 깊이 공부할 때 참고할 수 있다. 현재는 절판되었다.

> **Russ Tedrake, "Underactuated Robotics" (MIT OCW)**
> https://underactuated.csail.mit.edu/
> 무료 온라인 교재 + 강의. MPC, trajectory optimization, 그리고 제어와 계획(planning)의 연결을 깊이 있게 본다. Drake 라이브러리의 이론적 배경이기도 하다.

> **python-control library**
> https://python-control.readthedocs.io/
> Python으로 제어 시스템을 분석하고 설계하는 라이브러리. MATLAB Control System Toolbox의 Python 대안. Bode plot, root locus, state-space 분석 등을 지원한다.

> **CasADi**
> https://web.casadi.org/
> 자동 미분과 다양한 NLP solver(IPOPT, SNOPT 등)를 연결하는 널리 쓰이는 프레임워크. Python, MATLAB, C++ 인터페이스를 제공한다.

> **OSQP (Operator Splitting Quadratic Program)**
> https://osqp.org/
> Linear MPC용 QP solver. 빠르고, robust하고, 코드 생성(code generation)이 가능하여 임베디드 시스템에 배포할 수 있다. C 구현 기반으로 Python, MATLAB, Julia 등 다양한 바인딩을 제공한다.

> **주요 논문**
> - [Hogan, "Impedance Control: An Approach to Manipulation" (ASME JDSMC 1985)](https://doi.org/10.1115/1.3140702) — 임피던스 제어의 원논문. 위치 제어와 힘 제어를 통합하는 프레임워크 제시
> - [Khatib, "A Unified Approach for Motion and Force Control of Robot Manipulators: The Operational Space Formulation" (IEEE RA 1987)](https://doi.org/10.1109/JRA.1987.1087068) — Operational Space Control의 원논문. Task-space 동역학 유도와 제어의 기초
> - [Khazoom et al., "Tailoring Solution Accuracy for Fast Whole-Body MPC" (RA-L 2024, arXiv:2407.10789)](https://arxiv.org/abs/2407.10789) — 실시간 whole-body MPC의 최신 접근

---

## 기술 흐름

```
1922 ── PID 제어 개념 정립 (Minorsky)
1960 ── 상태공간 이론 (Kalman)
1960 ── LQR (Kalman)
1985 ── Impedance Control 개념 (Hogan)
1987 ── Operational Space Control (Khatib)
1990s ─ Robust control (H-infinity) 이론 정립과 적용 시도
2004 ── 실시간 MPC 실용화 시작
2020s ─ Boston Dynamics Atlas: 전신 동작과 물체에 가하는 힘을 포함한 MPC 공개
```

---

각 기법의 수학적 세부사항은 추천 자료에서 이어진다. 이 장의 코드를 실행하고 파라미터를 바꾸면, 시스템 응답이 어떻게 달라지는지 관찰할 수 있다.

---

# Ch.7 — 모션 플래닝 & 궤적 최적화 (Motion Planning & Trajectory Optimization)

로봇이 A에서 B로 가려면 장애물·관절 한계·동역학 제약을 모두 만족하는 경로가 필요하다. 모션 플래닝은 충돌 없는 기하학적 경로를 탐색하며, 궤적 최적화는 동역학 한계와 목적 함수를 반영하여 시간 축에 정렬된 구체적 궤적을 산출한다.

---

## 7.1 왜 모션 플래닝을 배우는가

6축 로봇 팔에게 "저 컵을 집어라"라고 명령했다고 하자. IK로 목표 관절 각도를 구했다. 그런데 현재 자세에서 목표 자세로 관절을 직선으로 보간(interpolation)하면, 팔이 테이블을 관통하거나 자기 몸체에 충돌할 수 있다. 관절 공간에서의 직선이 작업 공간에서의 직선이 아니기 때문이다.

모션 플래닝은 다음 질문에 답한다:
- 충돌 없이 목표에 도달하는 경로가 존재하는가?
- 존재한다면, 가장 짧은/빠른/부드러운 경로는 무엇인가?
- 동역학 제약(토크 한계, 속도 한계)을 만족하면서 그 경로를 따라갈 수 있는가?

---

## 7.2 Configuration Space (C-space)

로봇의 모든 가능한 상태를 하나의 공간으로 표현한다.

**Joint space = Configuration space**: n-DOF 로봇의 configuration은 q = (q1, q2, ..., qn)이다. 이 q가 살고 있는 n차원 공간이 C-space이다.

**C-space obstacle**: 작업 공간(3D)의 장애물을 C-space로 변환한 것이다. C-space에서 장애물 영역에 속하는 configuration은 충돌 상태이다.

왜 C-space에서 생각해야 하는가? 로봇은 점이 아니다. 3D 공간에서 모든 링크가 장애물과 충돌하지 않는지 확인하려면 각 configuration에서 FK를 계산하고 충돌 검사를 해야 한다. C-space에서는 로봇을 "점"으로 취급할 수 있어, 장애물 회피가 점의 경로를 찾는 문제로 환원된다.

문제는 C-space obstacle의 정확한 형태를 계산하기가 어렵다는 데 있다. 실무에서는 C-space obstacle을 명시적으로 구하지 않고, 특정 configuration에서의 충돌 여부를 검사하는 collision checker를 사용한다.

---

## 7.3 그래프 탐색 기반 플래닝

C-space를 이산화(discretize)하고 그래프 탐색 알고리즘으로 경로를 찾는, 가장 고전적인 접근이다.

### Dijkstra 알고리즘

음이 아닌 간선 가중치를 가진 그래프에서 최단 경로를 찾는다. binary heap과 adjacency list를 쓰면 시간 복잡도는 $O((V+E)\log V)$이며, 목표 노드가 확정되면 전체 간선을 모두 처리하기 전에 멈출 수 있다.

### A* 알고리즘

Dijkstra에 휴리스틱을 추가한 것이다. 목표까지의 추정 거리(heuristic)를 이용하여 탐색 방향을 유도한다. graph-search A*는 admissible하면서 consistent한 휴리스틱에서 최적해를 찾는다. 좋은 휴리스틱은 탐색 노드를 줄일 수 있지만, 실행 시간이 항상 Dijkstra보다 짧은 것은 아니다.

```python
import heapq
import numpy as np

def astar_2d(grid, start, goal):
    """2D 격자에서의 A* 경로 탐색.
    grid: 0=free, 1=obstacle
    """
    rows, cols = grid.shape
    open_set = [(0, start)]  # (f_score, node)
    came_from = {}
    g_score = {start: 0}

def heuristic(a, b):
        return np.hypot(a[0] - b[0], a[1] - b[1])  # 대각선 이동 비용과 일치하는 유클리드 거리

neighbors = [(-1,0), (1,0), (0,-1), (0,1), (-1,-1), (-1,1), (1,-1), (1,1)]

while open_set:
        f, current = heapq.heappop(open_set)
        if current == goal:
            # 경로 복원
            path = [current]
            while current in came_from:
                current = came_from[current]
                path.append(current)
            return path[::-1]

for dx, dy in neighbors:
            neighbor = (current[0] + dx, current[1] + dy)
            if (0 <= neighbor[0] < rows and 0 <= neighbor[1] < cols
                    and grid[neighbor] == 0):
                cost = np.sqrt(dx**2 + dy**2)
                tentative_g = g_score[current] + cost
                if tentative_g < g_score.get(neighbor, float('inf')):
                    came_from[neighbor] = current
                    g_score[neighbor] = tentative_g
                    f_score = tentative_g + heuristic(neighbor, goal)
                    heapq.heappush(open_set, (f_score, neighbor))

return None  # 경로 없음
```

### 장단점

유한 격자에서 모든 도달 가능한 셀을 탐색하는 알고리즘은 그 **이산 문제**에 대해 완전하다. 그러나 격자 해상도 때문에 연속 공간의 좁은 통로를 놓칠 수 있고, **차원의 저주(curse of dimensionality)**에도 시달린다. 6-DOF 로봇 팔의 C-space를 각 축 100개로 이산화하면 100^6 = 10^12개의 셀이 된다. 이 한계 때문에 샘플링 기반 플래너가 등장했다.

---

## 7.4 샘플링 기반 플래너

C-space를 균일한 고정 격자로 모두 나누지 않고 표본을 뽑아 경로를 탐색한다. 고차원 C-space에서 중요한 선택지이며, optimization-based planning이나 search와 결합하기도 한다.

### RRT (Rapidly-exploring Random Tree)

LaValle (1998)이 제안한 알고리즘이다. 아이디어는 단순하다:

```
1. 시작점에서 트리를 초기화한다.
2. C-space에서 무작위 점 q_rand를 샘플링한다.
3. 트리에서 q_rand에 가장 가까운 노드 q_near를 찾는다.
4. q_near에서 q_rand 방향으로 step_size만큼 확장하여 q_new를 만든다.
5. q_near → q_new 경로가 충돌하지 않으면 트리에 추가한다.
6. q_new가 목표 근처면 종료. 아니면 2로 돌아간다.
```

```python
import numpy as np

class RRT:
    def __init__(self, start, goal, obstacle_fn, bounds, step_size=0.3, max_iter=5000):
        self.start = np.array(start)
        self.goal = np.array(goal)
        self.obstacle_fn = obstacle_fn  # config → bool (충돌이면 True)
        self.bounds = np.array(bounds)  # [[min_q1, max_q1], ...]
        self.step_size = step_size
        self.max_iter = max_iter
        self.nodes = [self.start]
        self.parents = {0: -1}

def sample_random(self):
        # 10% 확률로 goal을 샘플링 (goal bias)
        if np.random.random() < 0.1:
            return self.goal
        return np.random.uniform(self.bounds[:, 0], self.bounds[:, 1])

def nearest(self, q):
        dists = [np.linalg.norm(node - q) for node in self.nodes]
        return np.argmin(dists)

def steer(self, q_near, q_rand):
        direction = q_rand - q_near
        dist = np.linalg.norm(direction)
        if dist < self.step_size:
            return q_rand
        return q_near + (direction / dist) * self.step_size

def collision_free(self, q1, q2, n_checks=10):
        for t in np.linspace(0, 1, n_checks):
            q = q1 + t * (q2 - q1)
            if self.obstacle_fn(q):
                return False
        return True

def plan(self):
        for i in range(self.max_iter):
            q_rand = self.sample_random()
            idx_near = self.nearest(q_rand)
            q_near = self.nodes[idx_near]
            q_new = self.steer(q_near, q_rand)

if self.collision_free(q_near, q_new):
                idx_new = len(self.nodes)
                self.nodes.append(q_new)
                self.parents[idx_new] = idx_near

if np.linalg.norm(q_new - self.goal) < self.step_size:
                    # 경로 복원
                    path = [q_new]
                    idx = idx_new
                    while self.parents[idx] != -1:
                        idx = self.parents[idx]
                        path.append(self.nodes[idx])
                    return path[::-1]

return None  # 실패
```

### RRT* (Optimal RRT)

Karaman & Frazzoli (2011). RRT는 해를 찾지만 최적이 아니다. RRT*는 새 노드 추가 시 근처 노드들과 re-wiring을 수행하여 점근적 최적성(asymptotic optimality)을 보장한다. 샘플 수가 무한대로 가면 최적 경로에 수렴한다.

실무적으로 RRT*는 RRT보다 좋은 경로를 찾지만, 수렴이 느리다. 시간 제한이 있는 실시간 상황에서는 RRT-Connect가 더 실용적인 경우가 많다.

### PRM (Probabilistic Roadmap)

Kavraki et al. (1996). RRT가 single-query(한 번에 하나의 start-goal 쌍)인 반면, PRM은 multi-query에 적합하다.

1단계 (offline): C-space에 많은 점을 샘플링하고, 가까운 점들을 충돌 없는 간선으로 연결하여 로드맵(graph)을 구축한다.
2단계 (online): start와 goal을 로드맵에 연결하고, 그래프 탐색(A* 등)으로 경로를 찾는다.

같은 환경에서 여러 경로 쿼리가 필요한 경우(예: 산업용 로봇 셀) PRM이 효율적이다.

### RRT-Connect

Kuffner & LaValle (2000). 시작점과 목표점에서 동시에 트리를 성장시키고, 두 트리가 만나면 경로를 연결한다. 빠르게 초기 경로를 찾는 용도로 널리 쓰이며, MoveIt2의 여러 OMPL 설정 예시에서도 `RRTConnect`를 기본 planner config로 지정한다. 실제 기본값은 배포판과 사용자 설정에 따라 달라진다.

### OMPL 라이브러리

Open Motion Planning Library (https://ompl.kavrakilab.org/). Kavraki Lab (Rice University)에서 개발한 C++ 라이브러리로, RRT, RRT*, RRT-Connect, PRM, EST, KPIECE 등 수십 가지 샘플링 기반 플래너를 제공한다.

OMPL 자체는 충돌 검사를 하지 않는다. State validity checker를 사용자가 제공해야 한다. MoveIt2는 OMPL + FCL(Flexible Collision Library)를 결합하여 완전한 모션 플래닝 파이프라인을 구성한다.

```python
# MoveIt2에서 OMPL 기반 모션 플래닝 (ROS2 Python API, 간략화)
from moveit.planning import MoveItPy

moveit = MoveItPy(node_name="motion_planner")
arm = moveit.get_planning_component("manipulator")

# 목표 설정
arm.set_goal_state(configuration_name="home")

# 플래닝 (OMPL RRT-Connect가 기본)
plan_result = arm.plan()

if plan_result:
    # 실행
    arm.execute()
```

> **추천 자료**
> - [LaValle, "Planning Algorithms"](http://lavalle.pl/planning/) — 무료 온라인 교재. 모션 플래닝의 표준 교재
> - [OMPL](https://ompl.kavrakilab.org/) — 오픈소스 모션 플래닝 라이브러리
> - [MoveIt2 Tutorials](https://moveit.picknik.ai/) — ROS2 기반 실전 모션 플래닝 가이드

---

## 7.5 궤적 최적화 (Trajectory Optimization)

샘플링 기반 플래너는 "충돌 없는 경로"를 찾아준다. 하지만 그 경로는:
- 울퉁불퉁하다 (random sampling이므로)
- 동역학을 무시한다 (기구학적 경로만 제공)
- 시간 정보가 없다 (어떤 속도로 따라가야 하는지 모른다)

궤적 최적화는 이 한계를 보완한다. 비용 함수(시간, 에너지, 부드러움)를 최소화하면서, 동역학 제약, 충돌 회피, 관절 한계를 모두 만족하는 궤적을 찾는다.

### Direct Collocation

궤적을 시간 구간으로 나누고, 각 구간의 상태와 입력을 결정 변수(decision variable)로 둔다. 동역학 방정식은 등식 제약(equality constraint)으로 처리한다.

```
minimize    Σ_k L(x_k, u_k) * dt                    (비용)
subject to  x_{k+1} = f(x_k, u_k)   for all k       (동역학)
            g(x_k) <= 0              for all k       (부등식 제약: 충돌, 관절 한계)
            x_0 = x_init                             (초기 조건)
            x_N = x_goal                             (종단 조건)
```

이것을 하나의 큰 nonlinear program(NLP)으로 만들고, IPOPT 같은 솔버로 푼다.

장점: 동역학과 제약을 동시에 처리, 부드러운 궤적
단점: 초기 추측(initial guess)에 민감, 비볼록이므로 지역 최적해에 빠질 수 있음

### Direct Shooting

상태를 결정 변수에서 제거하고, 입력 시퀀스 {u_0, u_1, ..., u_{N-1}}만을 결정 변수로 둔다. 상태는 동역학 시뮬레이션으로 계산한다.

collocation보다 결정 변수가 적지만, 시뮬레이션이 불안정하면 (예: 도립진자) 최적화도 불안정해진다.

### CHOMP (Covariant Hamiltonian Optimization for Motion Planning)

Ratliff et al. (2009). 초기 궤적(보통 직선 보간)에서 시작하여, 충돌 비용 + 부드러움 비용의 gradient를 따라 궤적을 반복적으로 개선한다. 공변 gradient(covariant gradient)를 사용하여 업데이트가 부드럽다.

장점: 직관적, 기존 궤적을 점진적으로 개선
단점: 좁은 통로(narrow passage)를 통과하기 어려움, 지역 최적해

### TrajOpt

Schulman et al. (2014). Sequential convex optimization 기반으로, 매 반복에서 비선형 문제를 선형/이차 근사로 바꿔서 풀고 trust region으로 근사의 유효 범위를 제한한다. 비볼록 문제이므로 전역 최적해는 보장하지 않는다. 충돌 회피를 signed distance 기반 비용으로 다뤄 gradient를 사용한다.

### CasADi를 이용한 Trajectory Optimization

CasADi는 symbolic computation, automatic differentiation, NLP solver 연결을 제공하는 널리 쓰이는 프레임워크다. Trajectory optimization에서는 Drake, direct solver API, JAX 기반 구현 등과 함께 선택지 중 하나다.

```python
import casadi as ca
import numpy as np

# 간단한 예: 고정 시간 동안 1D 더블 인티그레이터의 제어 입력 제곱 적분 최소화
# x = [position, velocity], u = force
# x_dot = [velocity, force/mass]

N = 50           # 구간 수
dt = 0.1         # 시간 간격
mass = 1.0

opti = ca.Opti()

# 결정 변수
X = opti.variable(2, N + 1)  # 상태 궤적
U = opti.variable(1, N)      # 입력 궤적

# 비용: 고정 시간 동안 제어 입력 제곱 적분 최소화
cost = 0
for k in range(N):
    cost += U[0, k]**2 * dt  # 제어 입력 제곱 적분
opti.minimize(cost)

# 동역학 제약 (Euler integration)
for k in range(N):
    x_next = X[:, k] + ca.vertcat(X[1, k], U[0, k] / mass) * dt
    opti.subject_to(X[:, k + 1] == x_next)

# 경계 조건
opti.subject_to(X[:, 0] == ca.vertcat(0, 0))    # 시작: 위치 0, 속도 0
opti.subject_to(X[:, N] == ca.vertcat(1, 0))     # 종료: 위치 1, 속도 0

# 입력 제약
opti.subject_to(opti.bounded(-5.0, U, 5.0))

# 상태 제약 (속도 제한)
opti.subject_to(opti.bounded(-2.0, X[1, :], 2.0))

# 솔버 설정
opti.solver('ipopt', {'print_time': False}, {'print_level': 0})
sol = opti.solve()

x_opt = sol.value(X)
u_opt = sol.value(U)
print(f"최적 궤적 - 최종 위치: {x_opt[0, -1]:.4f}")
print(f"최대 힘: {np.max(np.abs(u_opt)):.4f} N")
```

> **추천 자료**
> - [Matthew Kelly, "An Introduction to Trajectory Optimization" (SIAM Review 2017)](https://www.matthewpeterkelly.com/research/MatthewKelly_IntroTrajectoryOptimization_SIAM_Review_2017.pdf) — collocation과 shooting을 비교하는 좋은 튜토리얼
> - [CasADi](https://web.casadi.org/) — automatic differentiation과 NLP solver 연결 도구
> - [Drake Trajectory Optimization](https://drake.mit.edu/) — direct collocation 예제 포함

---

## 7.6 MoveIt2: 실전 모션 플래닝

MoveIt2는 ROS2 기반의 공개 모션 플래닝 프레임워크로, 로봇 팔 연구와 응용에서 널리 쓰인다.

**아키텍처:**
- **Planning Scene**: 로봇 + 환경(장애물)의 3D 모델 관리. 충돌 검사의 기반.
- **Planning Pipeline**: OMPL 등 플래너 호출 → 경로 검증 → 시간 매개변수화(time parameterization)
- **Move Group Interface**: 사용자 API. 목표 설정, 플래닝, 실행을 추상화.

**OMPL 통합**: OMPL은 MoveIt2에서 사용할 수 있는 대표적인 planning pipeline plugin이다. `ompl_planning.yaml`에서 플래너 종류와 파라미터를 설정하며, 다른 pipeline도 구성할 수 있다.

```yaml
# ompl_planning.yaml 예시
manipulator:
  planner_configs:
    - RRTConnectkConfigDefault
    - RRTstarkConfigDefault
    - PRMkConfigDefault
  default_planner_config: RRTConnectkConfigDefault
  projection_evaluator: joints(joint1, joint2)
  longest_valid_segment_fraction: 0.01
```

**Pick-and-Place 파이프라인:**
1. 물체 인식 (Perception) → 물체의 6-DoF 포즈 추정
2. Grasp planning → 잡을 위치/자세 결정
3. Approach trajectory → 물체 위 접근점까지 모션 플래닝
4. Grasp → 그리퍼 닫기
5. Retreat trajectory → 물체를 들어올림
6. Place trajectory → 놓을 위치까지 모션 플래닝
7. Release → 그리퍼 열기

각 단계에서 MoveIt2가 충돌 회피와 관절 한계를 자동으로 처리한다.

---

## 7.7 심화: Optimization-Based Planning

### Constrained Nonlinear Optimization

실제 로봇의 궤적 최적화는 대부분 constrained NLP이다:

```
minimize    Σ L(x_k, u_k) + Φ(x_N)
subject to  x_{k+1} = f(x_k, u_k)           (동역학)
            h(x_k, u_k) = 0                  (등식 제약)
            g(x_k, u_k) <= 0                 (부등식 제약: 충돌, 토크 한계 등)
```

IPOPT(Interior Point Optimizer)가 이 문제를 푸는 표준 솔버다. CasADi에서 IPOPT를 기본으로 사용한다.

### Contact-Implicit Trajectory Optimization

접촉 모드(어디가 닿아 있고 어디가 떨어져 있는지)를 미리 지정하지 않고, 최적화가 자동으로 결정하게 하는 방법이다. 걷기, 잡기 같은 접촉 전환이 필요한 태스크에서 유용하다.

접촉력을 결정 변수에 포함하고, 상보성 조건(complementarity constraint)을 추가한다:

```
F_n >= 0                   (접촉력은 당기지 못함)
d >= 0                     (물체가 바닥 아래로 못 감)
F_n * d = 0                (떨어져 있으면 힘 0, 닿아 있으면 거리 0)
```

이 문제는 수학적으로 MPCC(Mathematical Program with Complementarity Constraints)이고, 풀기 어렵다. Relaxation 기법이나 smoothed contact model을 쓴다.

Drake에는 이 방법을 그대로 감싼 클래스가 없으며, `MathematicalProgram`에 접촉 상보성 제약을 직접 추가해 구성한다.

### 실시간 Re-planning과 MPC의 연결

정적 환경에서 한 번 계획하면 끝이지만, 동적 환경에서는 실시간으로 재계획(re-plan)해야 한다. 궤적 최적화와 MPC가 여기서 만난다. MPC를 짧은 horizon의 trajectory optimization으로 볼 수 있다. 매 제어 주기마다 짧은 구간의 궤적을 최적화하고, 첫 입력만 적용한 뒤 다시 최적화한다. 이전 장의 MPC가 정확히 이것이다.

차이점: 모션 플래닝의 trajectory optimization이 오프라인 환경에서 전체 궤적 일괄 산출에 무게를 둔다면, MPC는 실시간 온라인 루프 안에서 짧은 구간을 끊임없이 갱신해 나간다.

---

## 7.8 심화: Task and Motion Planning (TAMP)

"컵을 선반 위에 놓아라"라는 명령을 수행하려면:

1. 컵이 어디 있는지 인식
2. 컵을 잡을 수 있는 grasp pose 결정
3. 접근 → 잡기 → 들기 → 이동 → 놓기 순서 계획
4. 각 단계의 모션 플래닝

1은 인식, 2는 연속 기하 결정(어느 grasp pose를 잡을지), 3은 **symbolic planning**(어떤 순서로 어떤 action을 할지), 4는 **motion planning**(구체적으로 어떤 궤적으로 움직일지)이다. TAMP는 기호 계획과 운동 계획을 결합하며, 2번처럼 연속 파라미터를 고르는 자리를 아래 PDDLStream은 stream으로 분리해 다룬다.

### PDDLStream

MIT에서 개발한 TAMP 프레임워크. PDDL(Planning Domain Definition Language)로 symbolic action을 정의하고, stream을 통해 연속적 파라미터(grasp pose, placement pose)를 생성한다.

### LLM 기반 Task Planning

최근에는 LLM이 symbolic planner를 대체하는 시도가 활발하다:

- **SayCan** (Google, 2022): LLM이 가능한 action들의 자연어 설명을 평가하고, affordance model이 현재 상태에서 실행 가능한 action을 필터링한다. 둘의 곱으로 다음 action을 선택한다.
- **Code as Policies** (Google, 2023): LLM이 직접 로봇 제어 코드를 생성한다. 자연어 명령 → Python 코드 → 로봇 실행.
- **Inner Monologue** (Google, 2022): LLM + 환경 피드백의 반복적 대화로 태스크를 완수한다.

LLM 기반 TAMP는 아직 실험 단계이다. 복잡한 기하학적 제약(좁은 공간에서의 조작, 정밀 조립)은 LLM이 처리하기 어렵고, 결국 전통적 motion planner가 필요하다. LLM은 high-level 계획, motion planner는 low-level 실행이라는 역할 분담이 현실적이다.

TAMP는 환경과 행동이 결정론적이라고 가정한다. 환경 동역학과 관측이 확률적이라면 §7.9 심화: POMDP를 본다.

---

## 7.9 심화: 불확실성하 의사결정 (POMDP와 belief space planning)

§7.1~§7.8은 로봇이 자신의 상태와 환경을 정확히 안다고 가정했다. 하지만 실제 로봇은 노이즈 있는 센서로 부분적인 정보만 관측한다. 대칭 복도에서 어느 쪽에 있는지 모르는 로봇, 문이 열려 있는지 닫혀 있는지 불확실한 상황 — 이럴 때 "현재 최선 추정 상태"에서 계획하면 틀린다. belief(사후 분포) 위에서 직접 계획해야 한다. 이 절의 내용은 Thrun, Burgard, Fox의 *Probabilistic Robotics* §15.2·§16을 기반으로 한다.

### 7.9.1 도입: 세 패러다임

같은 환경에서 세 가지 플래너가 다른 답을 낸다. Goal·Pit·Robot이 놓인 좌우 대칭 복도를 예로 들자.

Classical planning은 상태를 완전히 알고 행동도 결정론적이다. §7.3의 A*가 이 범주로, 최단 경로를 한 번 계산하면 실행 중 센싱이 필요 없다.

**MDP(Markov Decision Process)**: 상태는 완전히 관측되고 행동은 확률적이다. 정책 $\pi: s \to a$로 모든 상태에 행동을 매핑한다. 좁은 길에서 벽과 충돌 위험을 고려해 더 넓은 경로를 택할 수 있다. ch.8 §8.2가 이 범주이다.

**POMDP**: 행동·관측 모두 확률적. belief $b$ 위에 정책 $\pi: b \to a$를 정의한다. 대칭 복도에서 처음엔 위치를 모르기 때문에, 일부러 비대칭 영역으로 우회해 정보를 수집한 뒤 목표로 향한다. 이것이 **능동적 정보 수집(active information gathering)**이다.

세 패러다임은 classical $\subset$ MDP $\subset$ POMDP 순으로 포함된다. 불확실성의 축은 두 가지다. 행동 불확실성(어디로 가려 했는데 실제로 어디로 갔나)과 지각 불확실성(실제로 어디 있는데 센서가 뭐라 읽었나)이 그것이다. MDP는 전자만, POMDP는 둘 다 다룬다.

ch.3의 필터들이 belief를 *추적*했다면, 이 절은 추적된 belief로 *무엇을 할 것인가*를 본다.

### 7.9.2 belief 위 가치 반복

세 패러다임의 수식을 비교하면 POMDP가 어디서 어려워지는지 바로 보인다. MDP 가치 반복의 핵심 식은 다음과 같다 (Bellman 방정식):

$$C^T(s) = \max_a \int \left[ c(s') + C^{T-1}(s') \right] P(s' \mid a, s)\, ds'$$

상태 $s$를 belief $b$로 바꾸면 POMDP의 가치 반복이 된다:

$$C^T(b) = \max_a \int \left[ c(b') + C^{T-1}(b') \right] P(b' \mid a, b)\, db' \tag{16.2}$$

정책은:

$$\pi^T(b) = \arg\max_a \int \left[ c(b') + C^{T-1}(b') \right] P(b' \mid a, b)\, db' \tag{16.3}$$

$b$와 $b'$는 모두 상태 공간 $\mathcal{S}$ 위의 확률 분포다. 분포들의 공간 위에 정의되는 것은 belief 전이 분포 $P(b' \mid a,b)$이며, 위 식은 이 belief 공간에 걸쳐 적분한다. 유한 상태 공간에서는 차원이 $|\mathcal{S}|-1$이고, 연속 상태 공간의 일반적인 belief는 무한차원이다.

무한 horizon 극한에서 이 재귀가 수렴하면 표준 Bellman 방정식을 얻는다:

$$V(b) = \max_a \left[ r(b, a) + \gamma \sum_{o'} P(o' \mid b, a)\, V(B(b, a, o')) \right]$$

여기서 $r(b,a) = \sum_s b(s)\, c(s,a)$는 belief에 대한 기대 즉시 보상이다. 유한 horizon 재귀 형태로 쓰면 식 (16.2)가 된다.

관측 $o'$가 결정되면 사후 belief $B(b, a, o')$가 Bayes 필터로 *유일하게* 결정된다. 이 점을 이용하면 belief 공간 전체 적분을 관측 공간 위 적분으로 재구성할 수 있다:

$$C^T(b) = \max_a \int \left[ c(B(b, a, o')) + C^{T-1}(B(b, a, o')) \right] P(o' \mid a, b)\, do' \tag{16.34}$$

belief update operator는:

$$B(b, a, o')(s') = \frac{1}{P(o' \mid a, b)}\, P(o' \mid s') \int P(s' \mid a, s)\, b(s)\, ds$$

이산 상태·관측 공간에서는 적분이 합으로 대체된다. 이 재구성이 모든 현대 POMDP solver의 출발점이다.

### 7.9.3 4상태 toy 예시

PWLC(piecewise-linear convex) 구조를 직접 보려면 작은 예제가 필요하다. 4상태·2행동·2관측 문제를 손으로 계산해 보자.

**설정:**
- 상태 $s_1, s_2, s_3, s_4$. 초기에 $(s_1, s_2)$ 중 하나.
- 행동 $a_1$: 정보 수집. $s_1 \leftrightarrow s_2$를 0.9 확률로 교환.
- 행동 $a_2$: 종결. $s_3$(보상 +80) 또는 $s_4$(보상 -80)로 이동.
- 관측 $o_1, o_2$: $s_1$에서 확률 (0.7, 0.3), $s_2$에서 확률 (0.4, 0.6).
- belief는 $b = (p_1, p_2)$이고 $p_1 + p_2 = 1$이므로 1차원.

**horizon 1 계산:**

즉시 보상은 belief에 대해 선형이다: $c(b) = \sum_i c(s_i) p_i$.

$a_2$를 택하면 $T=1$ 가치 ($\gamma = 0.9$):
$$C^1(b, a_2) = \gamma(80 p_1 - 80 p_2) = 72 p_1 - 72 p_2$$

$a_1$을 택하면 종결 없으므로 즉시 보상만: $C^1(b, a_1) = 0$에 가깝다.

따라서:
$$C^1(b) = \max\{ 0,\; 72p_1 - 72p_2 \}$$

$C^1(b)$는 두 선형 함수의 max다. $p_1 = 0.5$에서 꺾인다. $p_1 > 0.5$이면 $a_2$, 아니면 $a_1$.

**horizon 2 계산:**

$a_1$ 후 관측 $o_1, o_2$가 올 확률을 적분하면:
$$C^2(b, a_1) \approx \max\{0,\; -33.05 p_1 + 13.61 p_2\}$$

(계수는 관측 확률과 belief update를 통해 계산.)

$T=2$ 전체:
$$C^2(b) = \max\{ 0,\; -33.05 p_1 + 13.61 p_2,\; 72 p_1 - 72 p_2 \}$$

세 선형 조각의 max. horizon이 늘수록 조각이 추가된다.

가치 함수는 belief 공간에서 볼록(convex)이다: $\beta C(b) + (1-\beta) C(b') \geq C(\beta b + (1-\beta) b')$. 따라서 두 belief의 혼합에서 가치는 두 가치의 가중평균보다 크지 않다.

### 7.9.4 PWLC 구조와 alpha-vectors

4상태 예제에서 가치 함수가 *선형 조각의 max* 형태임을 보았다. 이것이 우연이 아님을 귀납으로 보인다.

베이스 케이스 ($T=1$): 즉시 보상 $c(b) = \sum_i c(s_i) p_i$는 belief에 대해 선형이다. 따라서 $C^1(b) = \max_a \sum_i C^1_{a,i}\, p_i$이고, 각 행동에 대해 선형 함수 하나씩이 나온다.

귀납 단계: $C^{T-1}(b)$가 PWLC라고 하자. 식 (16.34)에서 $C^{T-1}(B(b,a,o'))$를 $b$의 함수로 전개하면: belief update의 비선형 정규화 인자 $1/P(o'\mid a, b)$가 식 (16.34)의 가중치 $P(o'\mid a, b)$와 상쇄되어, 각 alpha-vector와의 내적 $\langle \phi, B(b,a,o') \rangle \cdot P(o'\mid a, b)$이 $b$의 선형 함수로 정리된다. 선형 함수들의 max의 max는 여전히 선형 함수들의 max다. 따라서 $C^T(b)$도 PWLC.

각 선형 조각의 계수 벡터를 **alpha-vector** $\phi$라 한다. 가치 함수는:

$$V(b) = \max_\phi \langle \phi, b \rangle$$

$\Phi$가 alpha-vector 집합이면 $V(b) = \max_{\phi \in \Phi} \sum_i \phi_i\, p_i$.

각 alpha-vector는 하나의 *조건부 정책*(현재 행동 + 관측에 따른 후속 정책)에 대응한다. 가지치기 전 후보 수는 $|\Phi^T| = |A| \cdot |\Phi^{T-1}|^{|\mathcal{O}|}$으로 이중 지수적으로 증가한다. 행동과 관측이 각각 2개이고 $|\Phi^0| = 1$이면, $|\Phi^1| = 2$, $|\Phi^2| = 2 \cdot 2^2 = 8$, $|\Phi^3| = 2 \cdot 8^2 = 128$, $|\Phi^4| = 2 \cdot 128^2 = 32768$이다. 이 빠른 증가 때문에 긴 horizon의 정확 해법은 비실용적이다.

### 7.9.5 LP 해법

alpha-vector 수가 이중지수적으로 증가한다는 것이 문제라면, 그 max·sum·max 구조를 LP(linear program)로 환원하여 지배되는 alpha-vector를 판별·가지치기하며 정확 해를 구하는 방법이 있다.

**변환 원리**: $C = \max_a x(a)$는 $\{C \geq x(a) \;\forall a\}$ 제약에서 $\min C$로 풀린다. $C = \sum_i \max_a x(a,i)$는 각 $i$마다 행동을 선택하는 함수 $a(\cdot)$의 모든 조합에 대해 $\{C \geq \sum_i x(a(i),i)\}$ 제약을 만들면 된다. 제약 수는 $|A|^{|\mathcal{S}|}$.

POMDP horizon $T$의 제약 (식 16.67):

$$\bigcup_a \bigcup_{k(o'):1 \leq k(o') \leq |\Phi^{T-1}|} \left\{ C^T(b) \geq \gamma \sum_{o'} \sum_i \left(c_i + C^{T-1}_{k(o'),i}\right) P(o' \mid s_i') \sum_j P(s_i' \mid a, s_j)\, p_j \right\}$$

가지치기 전 제약 수는 $|\Phi^T| = |A| \cdot |\Phi^{T-1}|^{|\mathcal{O}|}$.

---

**알고리즘: finite_world_POMDP** (Thrun et al., Table 16.1 의역)

```
Algorithm finite_world_POMDP(T):
  Φ¹ = { φ : C¹(b) = γ Σᵢ c(sᵢ) pᵢ }     # horizon 1 단일 alpha-vector

for t = 2 to T:
    Φᵗ = ∅
    for each action a:
      for each assignment k(o') ∈ {1, …, |Φᵗ⁻¹|} for each o':
        # 새 alpha-vector 계산
        for each state sⱼ:
          φⱼ = γ Σₒ' Σᵢ (cᵢ + Φᵗ⁻¹[k(o'), i]) · P(o'|sᵢ') · P(sᵢ'|a, sⱼ)
        Φᵗ = Φᵗ ∪ { ⟨a, φ⟩ }

# dominated alpha-vectors 제거 (pruning)
  Φᵀ = prune(Φᵀ)
  return Φᵀ
```

---

$|\Phi^T|$는 이중지수적으로 증가한다. horizon 3, 행동 3개, 관측 5개면 가지치기 전 alpha-vector가 이미 $10^{14}$개 규모이고, pruning을 거치더라도 현실적인 도메인에서는 계산 한계를 넘는다. 정확 해법은 개념 증명 차원에 머물며, 실무에서는 근사 기법이 불가피하다.

### 7.9.6 일반 POMDP

이산 finite-state 문제에서 LP 해법이 이미 비실용적이라면, 연속 상태 공간에서는 어떤 일이 벌어지는가.

상태 공간이 연속이면 alpha-vector 표현도 연속 함수가 된다. 식 (16.34)는 원칙적으로 여전히 성립하지만, $\Phi^{T-1}$가 함수들의 집합으로 무한차원이 된다.

---

**알고리즘: POMDP(T)** (Thrun et al., Table 16.2 의역, 압축)

```
Algorithm POMDP(T):
  초기화: Φ¹ ← horizon 1 가치 함수 (연속)

for t = 2 to T:
    for each action a:
      for each "conditional plan" k(·) mapping observations to Φᵗ⁻¹ elements:
        새 함수 φ(b) = γ ∫ₒ' [ c(B(b,a,o')) + Φᵗ⁻¹[k(o')](B(b,a,o')) ] P(o'|a,b) do'
        Φᵗ ← Φᵗ ∪ { φ }

return Φᵀ
```

---

연속 공간에서는 함수들의 집합을 저장·비교하는 것 자체가 비실용적이다. 이 알고리즘은 in-principle 해법이고, 실용 알고리즘(MC-POMDP, AMDP)이 대안으로 등장한다.

### 7.9.7 MC-POMDP

정확 해법이 막혔으니, belief를 표본으로 근사하여 계산을 현실적인 수준으로 낮추는 방향을 택한다.

particle filter로 belief를 표현하고 가치 반복 갱신을 표본 기반으로 근사한다. ch.3 §3.11의 파티클 필터가 추정용으로 쓰였다면, 여기서는 *플래닝용*으로 쓰인다.

belief $\theta$는 가중 입자 집합 $\langle s^{(i)}, w^{(i)} \rangle$이다. belief update $B(b, a, o')$를 입자 형태로 구현한다:

```
Algorithm particle_filter_belief_update(θ, a, o'):
  θ' = ∅
  for i = 1 to N:
    s ~ θ                         # 입자 샘플링
    s' ~ P(s'|a, s)               # 운동 모델 (motion model)
    w' = P(o'|s')                 # 측정 모델 (measurement model)
    θ' ← θ' ∪ { ⟨s', w'⟩ }
  normalize weights in θ'
  return θ'
```

가치 반복 갱신은 belief $\theta$마다 행동 $a$별 Q값 $Q(\theta, a)$를 학습한다. 각 belief에서 $N$번 샘플링하고, 다음 belief에서 max Q를 가져와 평균낸다.

---

**알고리즘: MC-POMDP** (Thrun et al., Table 16.3 골격 의역)

```
Algorithm MCPOMDP(belief_database):
  for each belief θ in database:
    V(θ) = −∞
    for each action a:
      Q(θ, a) = 0
      for i = 1 to N:
        s ~ θ
        s' ~ P(s'|a, s)
        o' ~ P(o'|s')
        θ' = particle_filter_belief_update(θ, a, o')
        Q(θ, a) += (1/N) · γ · [V(θ') + c(s')]
      if Q(θ, a) > V(θ):
        V(θ) = Q(θ, a)

return V, policy σ(θ) = argmax_a Q(θ, a)
```

---

Q함수 갱신 (식 16.78):

$$Q(\theta_t, a_t) \leftarrow \mathbb{E}\left[ R(o_{t+1}) + \gamma \max_{\bar{a}} Q(\theta_{t+1}, \bar{a}) \right]$$

정책 (식 16.79):

$$\sigma^Q(\theta) = \arg\max_{\bar{a}} Q(\theta, \bar{a})$$

Q값 함수 근사는 nearest-neighbor 방식을 쓴다. belief $\theta$가 순서가 없는 입자 집합이라, 고정 순서의 벡터를 받는 당시의 feedforward 네트워크에 그대로 넣을 수 없었다. Thrun et al.은 $\langle \theta, a, Q \rangle$ 데이터베이스를 유지하고, 새 belief $\theta'$가 들어오면 KL divergence로 $k$-nearest neighbor를 찾아 Q값 평균을 쓴다. 오늘날에는 집합을 다루는 인코더로 신경망 입력을 만들 수도 있다.

두 belief 사이의 KL divergence는 Gaussian KDE로 근사한다. KL 기반 kNN이 함수 근사기 역할을 한다. 현대에서는 neural function approximation으로 대체되었지만 알고리즘 골격은 동일하다.

Outer loop는 belief 데이터베이스를 정적으로 유지하거나, $\varepsilon$-greedy 시뮬레이션 trial로 belief를 자연스럽게 방문하며 생성한다. 후자가 실제 로봇 궤적에서 마주칠 belief에 집중하기 때문에 계산 예산을 아낄 수 있다.

### 7.9.8 실험: heaven/hell과 find-and-fetch

**Heaven/Hell 문제**: T자 복도에서 한쪽 끝은 천국(+1), 반대쪽은 지옥(-1)이다. 오직 입구 근처 priest만이 어느 쪽이 천국인지 안다. 로봇은 먼저 priest에게 물어보고(정보 수집) 올바른 방향으로 가야 한다. POMDP planner는 priest로 우회하는 정책을 자동으로 학습한다. 직접 최단 경로로 가면 50%의 확률로 지옥에 도달하지만, priest를 거치면 올바른 방향으로 갈 수 있다.

**Find-and-Fetch (단안 카메라)**: 로봇이 단안 카메라로 목표 물체를 찾고 가져오는 태스크다. 카메라로는 물체의 방향은 알지만 거리를 정확히 모른다. MC-POMDP는 능동적으로 시점을 바꿔 거리 불확실성을 줄이는 정책을 학습한다. 물체를 여러 각도에서 관찰해 위치를 좁힌 뒤 접근한다.

두 실험 모두 belief를 추적하며 *정보 수집 행동*을 계획에 포함시킨다. 상태 추정 후 greedy action selection만 하면 이런 우회 경로는 나오지 않는다.

### 7.9.9 AMDP — belief 통계로 차원 축소

MC-POMDP가 입자 집합으로 belief를 직접 추적한다면, 같은 불확실성을 훨씬 적은 수의 통계량으로 요약할 수 있다. 이 발상이 AMDP(Augmented MDP)의 출발점이다. 복잡도 스펙트럼에서 MDP($|S|$에 polynomial)와 정확 POMDP(이중지수)가 양극단에 놓인다면, AMDP는 실용성을 추구하며 그 중간 지점을 절충한다.

아이디어: 실제 로봇 궤적에서 belief는 belief 공간 전체를 채우지 않고 좁은 manifold만 점유한다. 그 manifold를 *저차원 통계* $\bar{b} = f(b)$로 요약하고, $\bar{b}$ 위에서 표준 MDP 가치 반복을 적용한다.

**표준 통계 선택** (식 16.80):

$$\bar{b} = \langle \arg\max_s b(s),\; H[b] \rangle$$

최대 가능 상태 + belief 엔트로피. 엔트로피는:

$$H[b] = -\int b(s) \ln b(s)\, ds \tag{16.81}$$

무한 차원 belief를 최빈 상태와 엔트로피의 쌍으로 요약한다. 이것이 *충분 통계*인지는 보장되지 않지만 ("충분 통계라는 가정이 거의 성립하지 않는다"고 Thrun et al.이 명시한다), coastal navigation 실험에서 합리적 행동 선택에 충분함이 확인된다.

$\arg\max_s b(s)$만 쓰면 표준 MDP 그대로다. 거기에 엔트로피를 더해 "내가 얼마나 모르는가"를 상태에 포함시켰다.

---

**알고리즘: Augmented_MDP_value_iteration** (Thrun et al., Table 16.4 의역)

```
Algorithm Augmented_MDP_value_iteration():
  for all b̄:
    Ĉ(b̄) = 0

repeat until convergence:
    for all b̄:
      Ĉ(b̄) ← max_a ∫ [c(b̄') + Ĉ(b̄')] P(b̄'|a, b̄) db̄'

return Ĉ
  policy: π(b̄) = argmax_a ∫ [c(b̄') + Ĉ(b̄')] P(b̄'|a, b̄) db̄'
```

---

MDP_value_iteration (원전 Probabilistic Robotics §15.3.3)과 외형이 동일하다. 상태가 $s$ 대신 $\bar{b}$라는 점만 다르다.

전이 확률 $P(\bar{b}' \mid a, \bar{b})$ 계산 (식 16.85):

$$P(\bar{b}' \mid a, \bar{b}) = \int\!\!\int\!\!\int I_{f(b)=\bar{b}}\, I_{f(B(o',a,b))=\bar{b}'}\, P(o' \mid s') P(s' \mid a, s) P(s \mid b)\, ds\, ds'\, do'\, db$$

실용에서는 시뮬레이션 + lookup table 캐시로 근사한다. 여러 랜덤 시도로 전이를 통계적으로 추정한다.

### 7.9.10 Coastal Navigation 예시

coastal navigation은 AMDP가 낳는 emergent 행동 중 설명이 가장 쉬운 사례다.

동기: 넓은 open space를 가로지를 때 conventional MDP planner는 직선 경로를 택한다. 거리가 짧아서다. 하지만 open space에서는 라이더나 카메라에 특징이 없는 벽만 보이므로 위치 belief의 엔트로피가 크게 증가한다. 목적지에 도착해도 어디 있는지 모른다.

AMDP planner는 같은 환경에서 벽을 따라 도는 곡선 경로를 선택한다. 벽 근처에서 라이더 측정이 위치를 잘 제약하여 엔트로피가 낮게 유지된다. 비용 함수에 엔트로피가 포함되어 있으므로, 정보가 많은 경로를 선호하는 동작이 자동으로 나온다.

비유: 선박이 GPS 없이 항해할 때 해안선을 따라간다(coast = 해안). 랜드마크가 많은 경로가 위치 유지에 유리하기 때문이다.

Thrun et al.의 그림 16.5에서 센서 range를 줄일수록 conventional planner의 도착 엔트로피는 급격히 커지지만, coastal planner의 도착 엔트로피는 거의 변화가 없다. 정보를 고려한 경로 계획의 강건성이 여기서 드러난다.

Active SLAM에서 위치 불확실성을 줄이도록 경로를 선택하는 것, next-best-view planning에서 정보량이 큰 시점으로 이동하는 것이 모두 coastal navigation의 현대적 형태다.

### 7.9.11 무엇이 살아남았나

coastal navigation은 비용 함수에 엔트로피를 포함했을 때 계획기가 자동으로 도달하는 결론이다. §7.9.2의 belief 위 가치 반복이 실제 경로 선택에서 어떻게 드러나는지 보여주는 가장 직관적인 사례이기도 하다.

정확 해법(§7.9.5·§7.9.6)은 비실용적이지만 개념 도구로 살아 있고, 현대 POMDP solver는 이를 기반으로 세 방향으로 발전했다.

Point-based value iteration(SARSOP, HSVI, PBVI)은 belief 공간 전체가 아니라 샘플된 belief 점에서만 alpha-vector backup을 수행한다. §7.9.4의 alpha-vector 구조는 그대로이고, 탐색 범위를 제한하여 폭발을 막는다.

**MCTS 계열**: POMCP(Silver & Veness, 2010), DESPOT. rollout으로 Q값을 추정하고 belief tree를 MCTS로 탐색한다. §7.9.7 MC-POMDP의 Q 추정 구조를 tree search에 결합했다.

**Deep POMDP**: DRQN(Recurrent Q-network), DVRL(Igl et al.). RNN의 hidden state가 implicit belief 역할을 한다. MC-POMDP의 nearest-neighbor 함수 근사가 neural function approximation으로 대체된 형태다.

AMDP에 담긴 원리는 여러 후속 연구에서도 확인된다. Bayes-adaptive MDP(BAMDP)의 경우 모델 파라미터의 사후 분포를 증강 상태에 직접 반영하며, Active SLAM은 위치 belief의 분산 지표를 비용 함수에 포함한다. NeRF 기반 능동 인식에서도 entropy-augmented planning을 사용한다.

ch.8 §8.3의 PPO·SAC 등 deep RL은 경험에서 학습하고, 모델(전이 확률, 관측 모델)을 알 필요가 없다. POMDP planning은 모델을 알 때 최적 정책을 계산한다. 모델이 없으면 MC-POMDP도 돌아가지 않는다. MC-POMDP는 그 교집합에 있다 — belief는 모델로 추적하고, Q값은 경험에서 학습한다.

belief 위의 가치 반복은 horizon과 관측 수에 대해 이중지수적으로 폭발하기 때문에 근사 solver가 등장했다. MC-POMDP는 particle filter로 belief를 표현하고 Q값을 표본으로 학습한다. AMDP는 belief를 (최대 가능 상태, 엔트로피) 쌍으로 요약해 표준 MDP로 환원한다. 방법은 다르지만 목적은 같다 — 능동적 정보 수집. 그 현대적 형태가 MCTS 기반 POMCP, Deep POMDP, Active SLAM의 entropy-augmented planning이다.

---

## 7.10 추천 자료

> **LaValle, "Planning Algorithms"**
> http://lavalle.pl/planning/
> 무료 온라인. 모션 플래닝의 가장 포괄적인 교과서. RRT의 원저자가 쓴 책이니 당연히 좋다.

> **Russ Tedrake, "Underactuated Robotics" Ch.10: Trajectory Optimization**
> https://underactuated.csail.mit.edu/trajopt.html
> Drake를 이용한 trajectory optimization 실습. 코드와 이론이 함께 제공된다.

> **Matthew Kelly, "An Introduction to Trajectory Optimization" (SIAM Review 2017)**
> https://www.matthewpeterkelly.com/research/MatthewKelly_IntroTrajectoryOptimization_SIAM_Review_2017.pdf
> Direct collocation과 shooting을 비교하는 좋은 튜토리얼. 예제 코드도 제공.

> **OMPL**
> https://ompl.kavrakilab.org/
> 오픈소스 모션 플래닝 라이브러리. RRT, RRT*, PRM 등 수십 가지 알고리즘 구현.

> **MoveIt2 Tutorials**
> https://moveit.picknik.ai/
> ROS2 기반 실전 모션 플래닝. Pick-and-place부터 고급 설정까지.

> **Drake**
> https://drake.mit.edu/
> Trajectory optimization + 시뮬레이션 통합 프레임워크. Contact-implicit 지원.

> **CasADi**
> https://web.casadi.org/
> Nonlinear trajectory optimization 구현 표준 도구.

> **추가 논문**
> - [Garrett et al., "Integrated Task and Motion Planning" (2021, arXiv:2010.01083)](https://arxiv.org/abs/2010.01083) — TAMP의 표준 서베이 논문
> - [Janner et al., "Planning with Diffusion for Flexible Behavior Synthesis" (ICML 2022, arXiv:2205.09991)](https://arxiv.org/abs/2205.09991) — trajectory-level diffusion 기반 planning의 시작

---

## 기술 흐름

```
1979 ── Visibility graph 기반 path planning
1996 ── PRM (Kavraki et al.) — 샘플링 기반 플래닝의 시작
1998 ── RRT (LaValle) — 영향력 큰 single-query sampling planner
2000 ── RRT-Connect (Kuffner & LaValle) — 실무 motion-planning 라이브러리에서 널리 제공되는 변종
2009 ── CHOMP (Ratliff et al.) — gradient 기반 궤적 최적화
2011 ── RRT* (Karaman & Frazzoli) — 점근적 최적성 보장
2012 ── OMPL 소개 논문 (IEEE RAM) — 샘플링 기반 플래너 통합 라이브러리 (1.0 릴리스는 2014)
2014 ── TrajOpt (Schulman et al.) — sequential convex optimization
2019 ── MoveIt2 (ROS2) — 산업·연구에서 쓰이는 공개 motion-planning framework
2022 ── SayCan (Google) — LLM + motion planning
2023 ── Contact-implicit trajectory optimization 실용화
2024 ── LLM 기반 TAMP 연구 확산
```

---

# Ch.8 — 로봇 러닝 (Robot Learning)

로봇 러닝은 로봇이 명시적 프로그래밍 대신 데이터와 경험에서 행동을 학습하는 분야다. 전통적 제어가 한계에 부딪히는 지점에서 시작해 sim-to-real transfer, 모방학습, 최근 foundation model 기반 접근으로 이어진다.

---

## 8.1 왜 로봇 러닝을 배우는가

**전통적 방법이 잘 되는 영역**

PID 제어, MPC, RRT 같은 전통적 제어/플래닝은 dynamics 모델이 정확하고 환경이 정형화되어 있을 때 잘 동작한다. 산업용 로봇 팔이 정해진 위치의 부품을 집어 조립하는 작업이 대표적이다. 안정성이나 최적성의 수학적 보장은 각 방법의 모델·제약·해법 가정 안에서 성립하며, 그 조건을 만족하는 작업에서는 학습 기반 방법의 이점을 별도로 입증해야 한다.

**전통적 방법이 힘든 영역**

문제는 현실 세계가 깔끔하지 않다는 점이다.

- **모델링이 어려운 dynamics**: 천, 로프, 유체 같은 deformable object의 물리 모델을 정확히 세우는 건 현실적으로 불가능에 가깝다.
- **복잡한 접촉(contact)**: 물체를 손으로 돌리거나 끼워 맞추는 작업은 접촉 모드가 수시로 바뀐다. 접촉 역학을 정확히 모델링하는 것은 아직 열린 문제다.
- **비정형 환경**: 가정집 부엌, 재난 현장 등 미리 모델링할 수 없는 환경에서의 동작. 어떤 물체가 어디에 있을지 알 수 없다.

이런 상황에서 학습 기반 접근은 데이터에서 직접 입력-출력 관계를 근사하므로, 명시적 모델 없이도 동작할 수 있다.

**학습 기반 방법의 한계**

- **데이터 효율(sample efficiency)**: 강화학습은 수백만 스텝의 상호작용이 필요한 경우가 많다. 실제 로봇에서 이 데이터를 모으는 건 시간과 비용 면에서 비현실적이다.
- **안전성(safety)**: 학습 중 로봇이 자기 자신이나 주변 환경을 파손할 수 있다. 탐색(exploration) 과정 자체에 위험이 따른다.
- **일반화(generalization)**: 학습한 조건과 조금만 달라져도 성능이 급락하는 경우가 흔하다.

전통적 방법으로 풀 수 있으면 전통적 방법을 쓰는 편이 낫다. 학습은 전통적 방법이 한계에 부딪히는 문제에 적용하는 도구다. 실무에서는 둘을 적절히 조합한다.

그렇다면 전통적 방법이 부딪히는 한계는 구체적으로 어디에 있는가. Probabilistic Robotics(Thrun·Burgard·Fox) 15장은 매니퓰레이터, 수중 차량, 헬리콥터, 행성 탐사 로봇 팀이라는 네 도메인을 들어 불확실성 아래의 행동 선택이 왜 필요한지 보여준다. 그 예시를 학습의 관점에서 다시 읽으면 이렇다. 산업용 매니퓰레이터는 작업 공간이 제어되면 전통적 제어로 충분하지만, 수중 탐사 차량은 조류·시야·부력 변화 때문에 사전 모델을 세우기 어렵다. 헬리콥터는 돌풍 같은 외란이 커서 장애물과의 여유를 얼마로 둘지가 문제이고, 행성 탐사 로봇은 지형과 서로의 상대 위치를 사전에 알 수 없다. 어느 도메인에서 학습이 더 절실한지는 모델 불확실성의 크기와 직결된다.

---

## 8.2 강화학습 기초 (RL Basics)

### MDP (Markov Decision Process)

강화학습의 수학적 프레임워크는 MDP다. 구성 요소는 다음과 같다.

- **State (s)**: 환경의 현재 상태. 로봇의 관절 각도, 속도, 물체 위치 등.
- **Action (a)**: 에이전트가 취하는 행동. 관절 토크, 목표 관절 각도 등.
- **Reward (r)**: 행동의 결과로 받는 스칼라 보상 신호. r = R(s, a).
- **Transition (T)**: 상태 전이 확률. T(s'|s, a). 현재 상태에서 행동을 취했을 때 다음 상태의 분포.
- **Discount factor (γ)**: 미래 보상의 할인율. 0 < γ ≤ 1. 로봇 RL에서는 γ = 0.99가 흔한 초기값이다.

목표는 cumulative discounted reward를 최대화하는 policy π(a|s)를 찾는 것이다.

```
J(π) = E[ Σ_{t=0}^{∞} γ^t · r_t ]
```

Markov property는 "다음 상태는 현재 상태와 행동에만 의존한다"는 가정이다. 이전 히스토리 전체를 볼 필요가 없다는 뜻이다. 실제 로봇에서 깨지는 것은 상태 전이의 Markov성이 아니라 관측이 곧 상태라는 가정이다(부분 관측, 즉 POMDP 상황). 잠재 상태의 전이는 여전히 Markov지만 관측만 보고 세운 정책은 그렇지 않다. 이 경우 observation history나 belief를 state로 사용하거나 recurrent policy를 쓴다.

MDP 정의는 "무엇이 최적인가"의 기준을 세운다. 환경 모델 $p(x'|x,a)$와 보상 $r$이 알려져 있을 때 그 최적 정책을 *어떻게* 계산하는가는 다음 절에서 다룬다.

### MDP Value Iteration

환경 모델 $p(x'|x,a)$와 보상 $r$이 **알려진** 경우 dynamic programming으로 직접 최적 정책을 계산할 수 있다. 여기서는 PR(Thrun·Burgard·Fox) 표기를 따라 상태를 $x$로 쓴다(앞 절의 $s$와 동일한 개념이다).

#### Payoff와 Horizon

보상 $r(x, a)$를 payoff 함수라 부른다. 상태 $x$에서 행동 $a$를 취했을 때 즉시 받는 스칼라 값이다. 정책 $\pi: x \mapsto a$는 모든 상태를 행동으로 보내는 함수다. 정책의 품질은 누적 할인 보상의 기댓값으로 측정한다.

$$V^\pi(x_0) = \mathbb{E}\left[\sum_{t=0}^{T} \gamma^t \, r(x_t, \pi(x_t))\right]$$

Horizon $T$에 따라 세 경우로 나뉜다.

- **T=1 (greedy)**: 다음 한 스텝의 보상만 최대화한다. 단순하지만 장기 결과를 무시한다.
- **유한 horizon**: 정해진 $T$ 스텝까지 최적화한다. 시간 $t$에 따라 정책이 달라져야 하므로(time-dependent policy) 표현이 복잡해진다.
- **무한 horizon (T=∞)**: $\gamma < 1$이면 $V^\pi$가 유한하다($|V^\pi| \leq r_{\max}/(1-\gamma)$). 따라서 시간에 무관한 정상 정책(stationary policy)이 존재한다. 로봇 RL에서는 무한 horizon + 할인이 기본 설정이다.

할인 인자 $\gamma$의 직관: 1 스텝 뒤의 보상은 $\gamma$배, 2 스텝 뒤는 $\gamma^2$배로 감가된다. $\gamma$가 낮을수록 로봇이 근시안적으로, 높을수록 장기적으로 행동한다. 로봇 RL에서 $\gamma = 0.99$가 흔한 초기값인 이유는, 충분히 먼 미래까지 고려하면서도 수렴을 보장하기 위해서다.

#### Bellman 방정식

무한 horizon에서 최적 가치 함수 $V^*(x)$는 다음 Bellman 방정식을 만족한다.

$$V^*(x) = \max_a \left[ r(x, a) + \gamma \sum_{x'} p(x' \mid x, a) \, V^*(x') \right]$$

이 식의 의미: 상태 $x$에서 최적 가치는, 행동 $a$를 취하고 얻는 즉시 보상 $r(x,a)$와 그 다음 상태들의 최적 가치를 할인해 더한 것 중 최대값이다. 재귀 구조다.

최적 정책은 이 식에서 그리디하게 추출된다.

$$\pi^*(x) = \arg\max_a \left[ r(x, a) + \gamma \sum_{x'} p(x' \mid x, a) \, V^*(x') \right]$$

$T=1$ 최적 → $T=2$ 재귀 → $T \to \infty$ 극한을 취하면 위 Bellman 방정식이 유도된다. 각 단계에서 "지금 최적 + 나머지 최적"을 결합하는 Dynamic Programming의 표준 구조다.

#### Value Iteration 알고리즘

Bellman 방정식은 $V^*$에 대한 고정점 방정식(fixed-point equation)이다. $V^*$를 직접 풀기 어렵지만, 오른쪽에 현재 추정값 $V_k$를 대입하여 $V_{k+1}$을 구하고 반복하면 수렴한다. 이것이 Value Iteration이다.

```
Algorithm MDP_value_iteration():
  모든 상태 x에 대해:
    V_0(x) ← 0

수렴할 때까지 반복:           ε: 허용 오차 (예: 1e-6)
    모든 상태 x에 대해:
      V_{k+1}(x) ← max_a [ r(x, a) + γ · Σ_{x'} p(x'|x, a) · V_k(x') ]
    if max_x |V_{k+1}(x) - V_k(x)| < ε: break

최적 정책 추출:
    π*(x) ← argmax_a [ r(x, a) + γ · Σ_{x'} p(x'|x, a) · V_k(x') ]

return V_k, π*
```

변수 정리: $V_k$는 k번째 반복의 가치 추정값, $V^*$는 수렴 후 최적 가치, $\pi^*$는 최적 정책, $r$은 보상, $\gamma$는 할인 인자, $p(x'|x,a)$는 전이 확률.

갱신 순서는 임의여도 된다. Sutton & Barto(2018) §4.5에 따르면 각 상태가 무한히 자주 갱신되기만 하면 수렴이 보장된다. 위 수식은 이산 상태 공간의 합(Σ)으로 썼으며, 연속 상태 공간에서는 적분으로 표현한다.

수렴 보장: Bellman 갱신 연산자는 $\gamma$를 수축률(contraction rate)로 하는 수축 사상(contraction mapping)이다. 따라서 반복은 반드시 유일한 고정점 $V^*$로 수렴한다. $k$번 반복 후 오차는 초기 오차의 $\gamma^k$배 이하로 감소한다: $\|V_k - V^*\|_\infty \leq \gamma^k \|V_0 - V^*\|_\infty$.

#### 2D Grid World 예시

$5 \times 5$ 격자 공간을 상상한다. 각 셀이 상태 $x$이고, 행동은 상하좌우 4방향이다. 목표 셀에 도달하면 $r = +100$, 장애물 셀에 부딪히면 $r = -10$, 그 외는 $r = -1$ (이동 비용). 전이는 의도한 방향으로 0.8 확률, 직교 방향으로 각 0.1 확률 (확률적 전이).

Value Iteration을 실행하면 가치 함수가 목표 셀 주변에서 높고 장애물 주변에서 낮은 등고선 형태를 띤다. 초기에는 목표 인접 상태만 양수 가치를 보이지만, 반복 계산이 거듭되며 보상 파급(propagation)이 공간 전체로 확장되는 구조다. 수렴에 도달한 뒤 각 셀에서 즉시 보상과 후속 상태 가치의 기대값을 합한 값이 가장 큰 행동을 고르면 최적 정책이 얻어지고, 장애물을 우회하는 거동 역시 자연스럽게 형성된다. 정책은 모든 상태에 행동을 주는 사상이며, 확률적 전이에서는 실제 궤적이 실행마다 달라진다.

이렇게 얻은 상태별 행동 사상이 $\pi^*$다. 명시적으로 경로를 탐색(RRT, A*)하지 않아도 가치 함수만으로 행동이 정해진다.

MDP는 환경 모델 $p(x'|x,a)$와 $r$이 *주어졌을 때* 적용된다. 모델을 *모르고* 경험으로 학습하는 RL은 §8.3에서, *부분 관측*하의 플래닝은 ch.7 §7.9 심화 POMDP에서 다룬다.

실제 로봇에서는 전이 확률 $p(x'|x,a)$를 미리 아는 경우가 드물다. 모델 없이 직접 policy를 개선하는 방법이 필요하다.

### Policy Gradient 직관

Policy gradient는 다음 세 단계로 policy를 개선한다.

1. 현재 policy로 여러 trajectory를 수집한다.
2. 높은 return을 받은 trajectory에서의 action 확률을 올린다.
3. 낮은 return을 받은 trajectory에서의 action 확률을 내린다.

수식으로 쓰면:

```
∇J(θ) = E[ Σ_t ∇log π_θ(a_t|s_t) · A_t ]
```

A_t는 advantage function으로, 해당 action이 평균 대비 얼마나 좋았는지를 나타낸다. 이 gradient를 따라 파라미터 θ를 업데이트한다.

직관적으로, `log π(a|s)`의 gradient는 action a의 확률을 올리는 방향이고, 여기에 advantage를 곱해서 좋은 action은 더 자주, 나쁜 action은 덜 자주 선택하도록 만든다.

### Value Function, Q-function

- **Value function V^π(s)**: state s에서 policy π를 따랐을 때 기대되는 cumulative reward.
- **Q-function Q^π(s, a)**: state s에서 action a를 취하고 이후 π를 따랐을 때의 기대 cumulative reward.
- **Advantage A^π(s, a) = Q^π(s, a) - V^π(s)**: action a가 평균 대비 얼마나 좋은지.

Value function을 따로 학습해두면 variance를 줄일 수 있다. 대부분의 현대 RL 알고리즘은 policy network와 value network를 함께 학습하는 actor-critic 구조를 사용한다.

### On-policy vs Off-policy

- **On-policy**: 현재 policy가 수집한 데이터로만 학습한다. 현재 rollout을 여러 epoch에 사용할 수 있지만, 오래된 정책의 데이터를 replay buffer에 쌓아 계속 재사용하지는 않는다. PPO가 대표적이다. 안정적이지만 sample efficiency가 낮다.
- **Off-policy**: 과거 policy가 수집한 데이터도 재사용한다 (replay buffer). SAC, TD3가 대표적이다. sample efficient하지만 학습이 불안정할 수 있다.

로봇에서는 데이터 수집 비용이 크기 때문에 off-policy 방법의 sample efficiency가 매력적이다. 하지만 시뮬레이션에서 대규모 병렬 환경을 돌릴 수 있다면 on-policy(PPO)도 충분히 경쟁력이 있다.

---

## 8.3 주요 RL 알고리즘

### PPO (Proximal Policy Optimization)

PPO는 Schulman et al. (2017)이 제안한 on-policy 알고리즘이다. surrogate 목적함수 안의 확률 비율을 clipping하여, 이전 policy에서 크게 벗어나는 변화에 이득이 생기지 않도록 만든다. 정책 변화 자체를 강제로 묶는 신뢰 영역 제약은 TRPO 쪽이고, PPO는 그 역할을 목적함수 설계로 대신한다.

```
L_CLIP(θ) = E[ min( r_t(θ) · A_t, clip(r_t(θ), 1-ε, 1+ε) · A_t ) ]
```

여기서 r_t(θ) = π_θ(a_t|s_t) / π_θ_old(a_t|s_t)는 probability ratio, ε의 원 논문 기본값은 0.2다.

PPO가 널리 쓰이는 이유는 구현이 간단하고 기본값이 비교적 넓은 범위에서 무난하게 동작한다는 데 있다. 다만 advantage 정규화·value clipping 같은 구현 세부에 성능이 크게 좌우된다는 재현 연구도 있다. 안정적이기도 하다. NVIDIA Isaac Lab 등 대규모 병렬 시뮬레이션과 결합하면 수천 개 환경에서 동시에 데이터를 수집할 수 있어서 sample efficiency 문제를 물량으로 해결할 수 있다.

### SAC (Soft Actor-Critic)

SAC는 off-policy 알고리즘으로, entropy regularization을 추가한 것이 특징이다. 보상을 최대화하면서 동시에 policy의 entropy를 최대화한다. 즉, 가능한 한 다양한 action을 시도하도록 유도한다.

```
J(π) = E[ Σ_t γ^t ( r_t + α · H(π(·|s_t)) ) ]
```

α는 temperature parameter로, entropy와 reward 사이의 균형을 조절한다. 자동으로 α를 조절하는 방법도 있다.

연속 action space에서 sample efficient하다. Replay buffer를 써서 수집한 데이터를 여러 번 재사용할 수 있기 때문이다. 실제 로봇에서 데이터를 직접 수집할 때 off-policy인 SAC가 on-policy PPO보다 데이터 효율 면에서 유리하다.

### TD3 (Twin Delayed DDPG)

TD3는 DDPG의 개선 버전으로, SAC와 비슷한 off-policy 알고리즘이다. 세 가지를 바꿨다.

1. **Twin Q-networks**: Q-function 두 개를 학습하고 작은 값을 사용하여 overestimation bias를 줄인다.
2. **Delayed policy update**: critic을 여러 번 업데이트한 후에 policy를 한번 업데이트한다.
3. **Target policy smoothing**: target action에 노이즈를 추가한다.

SAC와 성능이 비슷하지만, entropy tuning이 필요 없어서 하이퍼파라미터가 약간 적다. 다만 탐색(exploration)이 SAC보다 약할 수 있다.

### 알고리즘 선택 가이드

| 상황 | 추천 알고리즘 | 이유 |
|------|-------------|------|
| 시뮬레이션, GPU 병렬화 가능 | PPO | 병렬 환경으로 sample efficiency 보상 가능 |
| 실제 로봇, 데이터 적음 | SAC | off-policy, sample efficient |
| 연속 action space, 안정성 중시 | SAC 또는 TD3 | 둘 다 연속 공간에 강함 |
| 이산 action space | PPO 또는 DQN | 원 SAC는 연속 행동용이며 이산 변형은 별도 구현이 필요 |
| 처음 시작하는 프로젝트 | PPO | 튜닝이 쉽고, 디버깅이 용이 |

### Stable-Baselines3 코드 예시

PPO로 MuJoCo Ant 환경을 학습하는 기본 코드다.

```python
import gymnasium as gym
from stable_baselines3 import PPO
from stable_baselines3.common.env_util import make_vec_env
from stable_baselines3.common.evaluation import evaluate_policy

# 병렬 환경 생성 (8개)
vec_env = make_vec_env("Ant-v4", n_envs=8)

# PPO 에이전트 생성
model = PPO(
    "MlpPolicy",
    vec_env,
    learning_rate=3e-4,
    n_steps=2048,        # 한 번의 rollout에서 수집할 스텝 수
    batch_size=64,
    n_epochs=10,         # 수집한 데이터로 몇 epoch 학습할지
    gamma=0.99,
    gae_lambda=0.95,     # GAE (Generalized Advantage Estimation)
    clip_range=0.2,
    verbose=1,
    tensorboard_log="./ppo_ant_tb/",
)

# 학습 (총 2M 스텝)
model.learn(total_timesteps=2_000_000)

# 평가
eval_env = gym.make("Ant-v4")
mean_reward, std_reward = evaluate_policy(model, eval_env, n_eval_episodes=20)
print(f"Mean reward: {mean_reward:.1f} +/- {std_reward:.1f}")

# 모델 저장/로드
model.save("ppo_ant")
loaded_model = PPO.load("ppo_ant")
```

SAC 예시도 구조는 비슷하다.

```python
from stable_baselines3 import SAC

model = SAC(
    "MlpPolicy",
    "Ant-v4",
    learning_rate=3e-4,
    buffer_size=1_000_000,    # replay buffer 크기
    learning_starts=10_000,   # 이 스텝 이후부터 학습 시작
    batch_size=256,
    tau=0.005,                # target network soft update rate
    gamma=0.99,
    verbose=1,
)

model.learn(total_timesteps=1_000_000)
```

> Stable-Baselines3는 빠르게 프로토타이핑하기에 좋다. 알고리즘 내부를 이해하고 싶으면 CleanRL을 권장한다. 모든 알고리즘이 단일 파일로 구현되어 있어서 코드를 따라 읽기에 좋다.

---

## 8.4 시뮬레이션 환경

로봇 RL은 실제 하드웨어에서 수백만 스텝을 수집하는 데 비용과 위험이 따르므로 시뮬레이션을 이용한다. 시뮬레이터마다 접촉 모델, 병렬화 방식, 로봇 자산이 다르다.

### MuJoCo (Multi-Joint dynamics with Contact)

DeepMind가 인수해 2021년에 무료로 공개했고, 2022년에 전체 코드베이스의 오픈소스화를 완료했다. 접촉 시뮬레이션 품질과 안정적인 수치 적분 덕분에 RL 연구의 표준 벤치마크 환경으로 자리잡았다. 기본 엔진은 CPU 기반이고, MuJoCo 3.0+에서 MJX (JAX backend)로 GPU 병렬화가 가능하지만 Isaac Lab 대비 생태계가 작다. 알고리즘 벤치마크와 소규모 실험에 적합하다.

### Isaac Lab (NVIDIA)

NVIDIA Isaac Sim 위에 구축된 로봇 학습 프레임워크다. GPU 병렬 시뮬레이션으로 수천~수만 개 환경을 동시에 실행할 수 있고, 사실적 렌더링과 sensor 시뮬레이션을 지원한다. NVIDIA GPU가 필수이고 설치·설정이 복잡하다. 대규모 locomotion 학습과 sim-to-real 파이프라인에 쓴다.

### PyBullet

입문용으로 적합한 오픈소스 물리 엔진이다. `pip install` 한 줄로 설치된다. 물리 정확도와 속도는 MuJoCo보다 낮지만, 처음 RL 코드를 돌려보거나 빠르게 아이디어를 검증할 때는 충분하다.

### Brax

Google에서 개발한 JAX 기반 물리 엔진이다. JAX의 JIT 컴파일과 자동 미분 덕분에 GPU/TPU에서 초고속으로 실행되고, differentiable physics 연구에 쓸 수 있다. 다만 물리 정확도가 제한적이고 복잡한 접촉 시나리오에 약하다.

### 환경 비교표

| 시뮬레이터 | 물리 정확도 | 속도 | GPU 병렬화 | 설치 난이도 | 주 용도 |
|-----------|-----------|------|-----------|-----------|---------|
| MuJoCo | 높음 | 보통 | MJX로 가능 | 쉬움 | 알고리즘 벤치마크 |
| Isaac Lab | 높음 | 매우 빠름 | 수천~만 | 어려움 | 대규모 로봇 학습 |
| PyBullet | 보통 | 느림 | 불가 | 매우 쉬움 | 입문/교육 |
| Brax | 낮음 | 매우 빠름 | 가능 | 보통 | 빠른 반복 실험 |

시작하는 단계라면 MuJoCo + Gymnasium 조합을 권장한다. 대규모 실험이 필요해지면 Isaac Lab으로 넘어간다.

---

## 8.5 Sim-to-Real Transfer

시뮬레이션에서 학습한 policy를 실제 로봇에 그대로 올리는 것을 sim-to-real transfer라 한다. 시뮬레이션에서 충분히 학습한 뒤 배포하면 될 것 같지만, 실제로는 그렇게 되지 않는다.

### Reality Gap

시뮬레이션과 현실 사이에는 차이(gap)가 존재한다.

- **물리 파라미터 차이**: 마찰 계수, 질량, 관성 모멘트 등이 시뮬레이션과 다르다.
- **센서 노이즈**: 실제 센서는 노이즈, 지연, 드리프트가 있다.
- **액추에이터 모델링 오차**: 모터의 비선형성, 기어 백래시, 컴플라이언스 등.
- **접촉 모델 차이**: 시뮬레이션의 접촉 모델은 현실의 근사에 불과하다.

시뮬레이션에서 reward 10,000을 찍어도 실제 로봇에서 쓰러지는 건 흔한 일이다.

### Domain Randomization

아이디어는 시뮬레이션의 물리 파라미터를 랜덤하게 변화시켜서, policy가 특정 파라미터에 의존하지 않고 robust하게 학습되도록 하는 것이다. OpenAI의 Dactyl(2018)이 수백 개 물리 파라미터를 동시에 랜덤화해 sim-to-real을 성공시키면서 이 접근의 가능성을 보여줬다.

랜덤화하는 대표적인 파라미터들:

- 마찰 계수: 0.5 ~ 1.5 사이에서 uniform 샘플링
- 물체 질량: 기본값의 0.8 ~ 1.2배
- 관절 damping: 기본값의 0.5 ~ 2.0배
- 센서 노이즈: Gaussian noise 추가
- 액추에이터 강도(strength): 기본값의 0.8 ~ 1.2배
- 통신 지연: 0 ~ 2 스텝 랜덤 지연

```python
# Isaac Lab 스타일의 domain randomization 설정 예시 (pseudo-code)
class RandomizationConfig:
    # 에피소드 시작마다 랜덤화
    friction_range = (0.5, 1.5)
    mass_scale_range = (0.8, 1.2)
    joint_damping_scale_range = (0.5, 2.0)

# 매 스텝마다 적용
    obs_noise_std = 0.05           # observation에 Gaussian noise
    action_delay_steps = (0, 2)    # action 적용 지연
    push_force_range = (-5.0, 5.0) # 외부 교란 (N)

def randomize_env(env, config):
    """에피소드 시작 시 호출."""
    import numpy as np
    friction = np.random.uniform(*config.friction_range)
    mass_scale = np.random.uniform(*config.mass_scale_range)
    damping_scale = np.random.uniform(*config.joint_damping_scale_range)
    env.set_friction(friction)
    env.scale_mass(mass_scale)
    env.scale_joint_damping(damping_scale)

def add_obs_noise(obs, config):
    """매 스텝 observation에 노이즈 추가."""
    import numpy as np
    noise = np.random.normal(0, config.obs_noise_std, size=obs.shape)
    return obs + noise
```

충분히 넓은 범위로 랜덤화하면, 현실은 그 범위 안에 포함될 가능성이 높다. 대신 특정 파라미터 설정에서 재면 성능이 낮아진다. 실제 시스템에서는 그 파라미터를 정확히 동정하기 어려우므로, 무작위화 정책이 특화 정책보다 나은 경우도 있다.

### System Identification (Sys-ID)

Domain randomization이 불확실성을 폭넓게 덮는 방향이라면, Sys-ID는 반대로 실제 로봇의 물리 파라미터를 최대한 정확하게 측정해서 시뮬레이션에 반영한다.

방법:
- 직접 측정: 전자저울로 질량 측정, 마찰 계수 실험 측정
- 파라미터 최적화: 실제 로봇의 trajectory와 시뮬레이션 trajectory의 차이를 최소화하는 파라미터를 찾음
- 온라인 적응: 실제 운용 중에 파라미터를 지속적으로 추정/업데이트

Sys-ID는 domain randomization과 같이 쓰는 경우가 많다. Sys-ID로 대략적인 파라미터를 잡고, 나머지 불확실성은 domain randomization으로 커버하는 방식이다.

### Teacher-Student 구조

시뮬레이션에서는 접근할 수 있지만 현실에서는 접근할 수 없는 정보(privileged information)를 활용하는 방법이다. 2단계로 학습한다.

1. **Teacher 학습**: 시뮬레이션에서 privileged information (정확한 지형 높이, 정확한 마찰 계수, 물체의 정확한 위치 등)을 state에 포함하여 policy를 학습한다. 정보가 많으므로 학습이 쉽다.
2. **Student 학습**: 실제 로봇에서 사용 가능한 observation (IMU, 관절 encoder, 카메라 등)만으로 teacher의 행동을 모방하도록 학습한다.

이 방식은 ANYmal 사족보행 로봇의 locomotion 연구에서 큰 성공을 거뒀다. Teacher는 정확한 지형 높이맵을 알지만, student는 proprioception history만으로 teacher와 비슷한 행동을 학습한다.

### 실제 사례

**ANYmal 보행 (ETH Zurich / Robotic Systems Lab)**
- PPO + domain randomization + teacher-student로 사족보행 학습
- 시뮬레이션에서 수십억 스텝 학습 후 실제 로봇에 zero-shot transfer
- 계단, 자갈, 경사면 등 다양한 지형에서 robust하게 보행
- 핵심: 대규모 domain randomization + privileged learning + proprioception history

**Dexterous Hand Manipulation (OpenAI, NVIDIA 등)**
- Rubik's cube를 Shadow Hand로 풀기 (OpenAI, 2019)
- 대규모 domain randomization이 핵심: 수백 개의 물리 파라미터를 동시에 랜덤화
- 시뮬레이션에서 약 13,000년 분량의 경험으로 학습
- 현실에서의 성공률은 시뮬레이션 대비 상당히 낮았지만, 학습 기반 접근의 가능성을 보여줌

---

## 8.6 모방 학습 (Imitation Learning)

강화학습은 reward 함수를 설계해야 하고 학습에 많은 데이터가 필요하다. 모방 학습은 전문가(사람)의 시연 데이터에서 직접 policy를 학습한다. 보고 배우는 방식이다.

### Behavioral Cloning (BC)

가장 단순한 모방 학습이다. 전문가의 (observation, action) 쌍을 수집하고, 지도학습(supervised learning)으로 policy를 학습한다.

```python
import torch
import torch.nn as nn
from torch.utils.data import DataLoader, TensorDataset

class BCPolicy(nn.Module):
    def __init__(self, obs_dim, act_dim, hidden_dim=256):
        super().__init__()
        self.net = nn.Sequential(
            nn.Linear(obs_dim, hidden_dim),
            nn.ReLU(),
            nn.Linear(hidden_dim, hidden_dim),
            nn.ReLU(),
            nn.Linear(hidden_dim, act_dim),
        )

def forward(self, obs):
        return self.net(obs)

# 전문가 데이터 로드 (NumPy -> Tensor)
# expert_obs: (N, obs_dim), expert_act: (N, act_dim)
dataset = TensorDataset(
    torch.FloatTensor(expert_obs),
    torch.FloatTensor(expert_act),
)
loader = DataLoader(dataset, batch_size=256, shuffle=True)

policy = BCPolicy(obs_dim=48, act_dim=7)
optimizer = torch.optim.Adam(policy.parameters(), lr=1e-3)
loss_fn = nn.MSELoss()

# 학습
for epoch in range(100):
    total_loss = 0.0
    for obs_batch, act_batch in loader:
        pred_act = policy(obs_batch)
        loss = loss_fn(pred_act, act_batch)
        optimizer.zero_grad()
        loss.backward()
        optimizer.step()
        total_loss += loss.item()
    if (epoch + 1) % 10 == 0:
        print(f"Epoch {epoch+1}, Loss: {total_loss/len(loader):.4f}")
```

**Compounding error**: BC의 구조적 한계다. 학습된 policy가 조금이라도 전문가 trajectory에서 벗어나면 학습 데이터에 없는 상태에 도달하고, 거기서의 행동은 예측 불가능해진다. 더 벗어나고, 에러가 누적되는 과정이 반복되면서 horizon $T$에 대해 오차가 $O(\epsilon T^2)$로 커질 수 있다(Ross & Bagnell, 2010).

### DAgger (Dataset Aggregation)

DAgger는 compounding error를 해결하기 위한 방법이다.

1. 초기 전문가 시연 데이터로 기본 BC policy를 학습한다.
2. 학습된 policy를 롤아웃하여 새로운 trajectory 집합을 확보하는 단계다.
3. 확보된 궤적의 각 상태마다 전문가가 직접 개입해 취할 행동을 레이블링한다.
4. 수집된 신규 상태-행동 쌍을 기존 데이터셋에 병합해 정책을 재학습시킨다.
5. 오차가 수렴할 때까지 2단계에서 4단계를 반복한다.

DAgger는 policy가 실제로 방문한 state에서 전문가가 선택할 action을 다시 받아 학습 데이터에 포함한다. 원 논문의 no-regret bound는 각 반복의 online learner와 expert-query 가정 아래 성립한다.

단점은 전문가가 반복적으로 레이블링해야 한다는 점이다. 사람이 일일이 correction을 해줘야 하므로 노동 집약적이다.

### ACT (Action Chunking with Transformers)

Stanford의 ALOHA 프로젝트에서 제안한 방법으로, 두 가지 장치를 결합한다.

1. **Action chunking**: 한 번에 하나의 action을 예측하는 대신, 미래 k 스텝의 action sequence를 한 번에 예측한다. 이렇게 하면 temporal correlation을 잡을 수 있고, compounding error를 줄인다.
2. **CVAE (Conditional Variational Autoencoder)**: action의 다봉(multimodal) 분포를 모델링한다. 같은 상황에서도 여러 유효한 행동이 있을 수 있는데, 단순 MSE loss로는 이걸 평균내버려서 어중간한 action이 나온다.

구조는 Transformer encoder-decoder를 사용하며, 입력으로 joint position과 카메라 이미지를 받는다.

### Diffusion Policy

Chi et al. (2023)이 제안한 방법으로, diffusion model을 action 생성에 적용한다.

Diffusion policy는 denoising 과정을 통해 임의의 복잡한 action 분포를 표현할 수 있다. MSE 회귀로 구현한 BC는 사실상 단봉 Gaussian을 가정하는데, diffusion policy는 다봉 분포도 자연스럽게 다룬다. BC는 학습 방식의 이름이므로 에너지 기반이나 혼합 밀도로 구현하면 다봉을 표현할 수 있다.

```python
# Diffusion Policy의 action 생성 과정 (pseudo-code)
# 1. 순수 noise에서 시작
action = torch.randn(batch_size, horizon, action_dim)

# 2. K번의 denoising step
for k in reversed(range(K)):
    # 현재 observation 조건 하에 noise 예측
    predicted_noise = noise_pred_net(action, k, obs_encoding)
    # noise 제거 (DDPM 또는 DDIM scheduler 사용)
    action = scheduler.step(predicted_noise, k, action)

# 3. 최종 action sequence 출력
```

Diffusion policy와 ACT는 2023년 이후 manipulation 모방학습의 주요 베이스라인으로 자리잡았다. LeRobot(HuggingFace) 등 공개 프레임워크에도 둘 다 구현이 포함되어 있다.

### 데이터 수집 방법

모방 학습의 성능은 데이터 품질에 크게 좌우된다. 주요 데이터 수집 방법은 다음과 같다.

- **Teleoperation**: 사람이 원격으로 로봇을 조종한다. ALOHA는 leader-follower 구조를 사용했고, 비교적 저렴하게 양팔 조작 데이터를 수집할 수 있다.
- **VR controller**: VR 컨트롤러로 end-effector 위치/자세를 지정한다. 직관적이지만 contact-rich 작업에서는 힘 피드백이 부족할 수 있다.
- **Kinesthetic teaching**: 로봇 팔을 직접 잡고 움직인다. 가장 직관적이지만, 로봇 크기가 크거나 무거우면 어렵다.
- **Space mouse**: 6-DoF 입력 장치. 한 손으로 조작 가능. 정밀 작업에 유용하다.

데이터 양은 태스크와 방법에 따라 다르다. Chi et al. (2023) Diffusion Policy 논문에서는 약 100~200개의 시연으로 유의미한 성능을 보였다. 더 많을수록 좋지만, 수집 비용과의 trade-off가 있다.

---

## 8.7 심화: Foundation Models for Robot Control

LLM과 VLM의 성공에 영감을 받아, 로봇 분야에서도 대규모 사전학습 모델(foundation model)을 만들려는 시도가 이어지고 있다. 대량의 로봇 데이터로 범용 정책(generalist policy)을 학습한 뒤 새로운 로봇이나 태스크에 적응시킨다.

### RT-1, RT-2 (Google DeepMind)

**RT-1 (2022)**: 13만 개의 로봇 시연 데이터(약 17개월 수집)로 학습한 Transformer 기반 policy다. 이미지와 자연어 명령을 입력으로 받아 action을 출력하며, 700개 이상의 태스크를 하나의 모델로 수행했다.

**RT-2 (2023)**: VLM(Vision-Language Model)을 직접 action 출력으로 fine-tuning했다. PaLM-E나 PaLI-X를 base model로 사용했다. 웹 스케일 사전학습 지식이 로봇 제어에도 전이된다는 것을 보여줬고, 학습 데이터에 없던 물체에 대해서도 어느 정도 일반화됐다.

### Octo

UC Berkeley 등에서 개발한 오픈소스 범용 로봇 정책이다. Open X-Embodiment 데이터셋(다양한 로봇, 다양한 기관에서 수집한 데이터)으로 학습했다. Diffusion 기반 action head를 사용하며, 새로운 로봇에 fine-tuning할 수 있도록 설계했다.

### π0 (Physical Intelligence)

[π0 기술 보고서(2024)](https://arxiv.org/abs/2410.24164)가 제안한 범용 로봇 정책이다. 사전학습 VLM 위에 flow-matching action expert를 결합한다. 저자들은 single-arm·dual-arm·mobile manipulator 데이터로 학습한 뒤 zero-shot, 언어 지시, fine-tuning 조건을 평가했으며, 빨래 접기·테이블 닦기·상자 조립 등을 시연했다. 성능 평가는 보고서의 로봇·태스크·baseline 범위로 한정해 읽어야 한다.

### OpenVLA

오픈소스 VLA (Vision-Language-Action) 모델이다. 7B 파라미터의 VLM을 fine-tuning하여 action token을 출력하도록 학습했다. 누구나 접근할 수 있는 오픈소스 공개가 핵심 기여다.

### 현실적 평가

로보틱스 분야의 파운데이션 모델은 여전히 기술 발전의 초입에 서 있다. 단일 도메인의 특정 태스크를 다룰 때는 전통적 제어 기법이나 태스크 특화 학습 모델이 더 높은 완성도를 보여주기도 한다. 물리 환경에서의 대규모 로봇 데이터 수집 비용 역시 만만치 않으며, 인터넷 텍스트나 웹 이미지와는 수집 단가 자체가 판이하다. 안전성 보장이 없고, 행동을 예측하기 어렵다는 점도 실사용의 장벽이다. 추론 지연(inference latency)이 실시간 제어의 허용 시간을 넘을 수 있다.

### 연구 방향

- **Data scaling**: Open X-Embodiment처럼 여러 기관의 데이터를 합치는 시도. 데이터가 많을수록 일반화가 좋아지는지 검증 중.
- **Cross-embodiment transfer**: 한 로봇에서 학습한 정책을 다른 로봇에 전이하는 연구. 서로 다른 action space를 어떻게 통일할 것인가가 핵심 문제.
- **Efficient fine-tuning**: LoRA 같은 parameter-efficient fine-tuning으로 새 태스크에 빠르게 적응.
- **Action representation**: action을 어떻게 토큰화/표현할 것인가. 이산화, 연속 분포, diffusion 등 다양한 접근이 경쟁 중.

---

## 8.8 심화: Reward Design과 Safe RL

reward 함수를 잘못 설계하면 RL은 엉뚱한 방향으로 수렴한다. 실제 로봇에 RL을 적용할 때는 안전성 문제도 함께 다뤄야 한다.

### Reward Shaping

**Sparse reward의 문제**: "목표에 도달하면 +1, 아니면 0" 같은 sparse reward는 정의하기 쉽지만, agent가 우연히 보상을 받기까지 무작위 탐색을 해야 한다. State-action 공간이 크면 학습 신호를 얻기 어렵다.

**Dense reward**: 중간 과정에 대한 보상을 추가한다. 예를 들어 물체 잡기 태스크에서:

```python
def compute_reward(gripper_pos, object_pos, target_pos, is_grasped):
    # 1. 그리퍼를 물체에 가까이 가져가기
    dist_to_object = np.linalg.norm(gripper_pos - object_pos)
    reaching_reward = -1.0 * dist_to_object

# 2. 물체를 잡았으면 보너스
    grasp_reward = 5.0 if is_grasped else 0.0

# 3. 물체를 목표 위치에 가까이
    if is_grasped:
        dist_to_target = np.linalg.norm(object_pos - target_pos)
        place_reward = -1.0 * dist_to_target
    else:
        place_reward = 0.0

# 4. 목표 도달 보너스
    success_reward = 10.0 if (is_grasped and
        np.linalg.norm(object_pos - target_pos) < 0.05) else 0.0

return reaching_reward + grasp_reward + place_reward + success_reward
```

**Curriculum learning**: 쉬운 태스크에서 시작해 점차 어려운 태스크로 넘어가는 방법이다. 예를 들어 locomotion에서 처음에는 평지에서 걷기, 다음에 작은 장애물, 그 다음 계단 순으로 난이도를 올린다. 이렇게 하면 sparse reward 상황에서도 학습 초기에 agent가 성공 경험을 쌓을 수 있다.

### Reward Hacking

Agent가 reward를 최대화하되, 설계자가 의도하지 않은 방식으로 행동하는 현상이다.

대표적인 예시:
- 로봇 팔이 물체를 "옮기라"고 했는데 물체를 밀어서 목표 위치로 보내기 (잡지 않음)
- 보행 로봇이 "빠르게 이동하라"고 했는데 넘어지면서 미끄러지기
- 점프를 학습하라고 했는데 비정상적으로 긴 형태로 진화 (형태 최적화와 결합 시)

대응 방법:
- 학습된 행동을 확인하면서 reward 함수를 반복해서 조정한다.
- 원치 않는 행동에 대한 penalty term을 추가한다.
- 비디오를 보면서 정성적으로 검토한다. 자동화하기 어려운 부분이다.

### Constrained RL

Safety constraint를 명시적으로 다루는 RL이다. 일반 RL과 달리, constrained RL은 reward를 최대화하면서 동시에 cost를 일정 한도 이하로 유지한다는 조건을 건다.

```
max_π E[ Σ γ^t r_t ]   subject to   E[ Σ γ^t c_t ] ≤ d
```

c_t는 cost (예: 관절 토크 한계 초과, 장애물 충돌), d는 허용 한도다.

대표적인 알고리즘으로 CPO (Constrained Policy Optimization), PCPO, Lagrangian relaxation 기반 방법 등이 있다. 실제 로봇에서는 하드웨어 보호를 위해 토크 제한, 관절 각도 제한 등을 constraint로 넣는 것이 현실적이다.

### Human-in-the-loop RL

사람의 피드백을 reward 신호로 사용하는 접근이다. LLM의 RLHF (RL from Human Feedback)와 같은 아이디어를 로보틱스에 적용한 것이다.

방법:
1. 로봇의 행동 쌍을 보여주고 사람이 선호도를 표시한다 (A가 B보다 나음).
2. 선호도 데이터로 reward model을 학습한다.
3. 학습된 reward model로 RL을 수행한다.

수치로 reward를 정의하기 어려운 태스크, 예컨대 걸음걸이의 자연스러움이나 물건을 놓을 때의 조심성 같은 것을 다룰 때 유용하다. 단점은 사람의 시간이 많이 든다는 점과, reward model이 부정확할 수 있다는 점이다.

---

## 8.9 추천 자료

> **Sutton & Barto, "Reinforcement Learning: An Introduction" (2nd edition)**
> http://incompleteideas.net/book/the-book-2nd.html
> MDP부터 policy gradient까지 다루는 표준 교재이며 무료 PDF가 제공된다. 기초를 먼저 볼 때는 Ch.1-6과 Ch.13부터 읽을 수 있다.

> **Sergey Levine, CS285: Deep Reinforcement Learning**
> https://rail.eecs.berkeley.edu/deeprlcourse/
> 로봇 RL에 초점을 맞춘 대학원 수준 강의. 강의 영상과 슬라이드 모두 공개되어 있다. 이 챕터의 대부분의 주제를 더 깊이 다룬다.

> **Stable-Baselines3**
> https://stable-baselines3.readthedocs.io/
> PyTorch 기반 RL 알고리즘 라이브러리. PPO, SAC, TD3 등 주요 알고리즘이 구현되어 있다. 빠른 프로토타이핑에 적합.

> **CleanRL**
> https://github.com/vwxyzjn/cleanrl
> 단일 파일 RL 구현 모음. 한 파일에 알고리즘 전체가 들어 있어서 코드를 따라가며 공부하기에 좋다. 알고리즘 내부를 이해하고 싶으면 SB3보다 이쪽을 권장한다.

> **Isaac Lab**
> https://isaac-sim.github.io/IsaacLab/
> NVIDIA의 GPU 병렬 로봇 시뮬레이션 프레임워크. ANYmal locomotion, dexterous manipulation 등 대규모 학습 프로젝트에서 폭넓게 채택되고 있다.

> **LeRobot (HuggingFace)**
> https://github.com/huggingface/lerobot
> 모방학습과 로봇 학습을 위한 프레임워크. ACT, Diffusion Policy 등의 구현이 포함되어 있다. 데이터셋도 함께 제공한다.

> **robomimic**
> https://robomimic.github.io/
> 모방학습 알고리즘 벤치마크. BC, BC-RNN, HBC 등 다양한 모방학습 방법을 동일 조건에서 비교할 수 있다.

> **추가 논문**
> - [Andrychowicz et al., "Hindsight Experience Replay" (NeurIPS 2017, arXiv:1707.01495)](https://arxiv.org/abs/1707.01495) — sparse reward 문제 해결의 핵심. 실패한 trajectory를 성공으로 재레이블링
> - [Hafner et al., "Mastering Diverse Domains through World Models" (DreamerV3, arXiv:2301.04104)](https://arxiv.org/abs/2301.04104) — 고정된 하이퍼파라미터 설정 하나로 150개 이상의 태스크 학습. World model 기반 RL의 현 시점 대표적 성과
> - [Chi et al., "Universal Manipulation Interface" (UMI, RSS 2024, arXiv:2402.10329)](https://arxiv.org/abs/2402.10329) — 핸드헬드 그리퍼로 데이터 수집, 다양한 로봇에 zero-shot 배포
> - [Fu et al., "Mobile ALOHA" (CoRL 2024, arXiv:2401.02117)](https://arxiv.org/abs/2401.02117) — 모바일 베이스 + 양팔 텔레오퍼레이션. co-training으로 성공률 대폭 향상

---

## 기술 흐름

```
1992 ── REINFORCE algorithm (Williams)
         초기의 영향력 큰 Monte Carlo policy-gradient 방법. score-function 추정량의 variance가 높다.

2013 ── DQN (Mnih et al., Atari)
         Deep RL의 시작. replay buffer로 상관된 표본 문제를 완화했다.
         고정 target network는 2015년 Nature 판에서 더해졌다.

2015 ── TRPO (Schulman et al.)
         trust region 제약으로 안정적 policy update. 이론은 좋으나 구현이 복잡.

2017 ── PPO (Schulman et al.)
         TRPO의 실용적 대안. clipping으로 간단하게 구현. 로봇 RL 실험의 기본 베이스라인.

2018 ── SAC (Haarnoja et al.)
         entropy regularization + off-policy. 연속 공간에서 sample efficient.

2019 ── ANYmal: sim-to-real locomotion (ETH Zurich)
         실제 로봇 데이터로 학습한 actuator network로 구동계 동역학을 모사해 사족보행 sim-to-real 성공.

2020 ── DAgger 실전 적용 확산
         모방학습의 실용성 입증. 다양한 로봇 플랫폼에 적용.

2022 ── RT-1 (Google)
         대규모 로봇 데이터 + Transformer. 다중 태스크 범용 정책.

2023 ── ACT/ALOHA (Stanford), Diffusion Policy (Columbia·TRI·MIT)
         모방학습의 새로운 표준. action chunking과 diffusion으로 성능 향상.

2023 ── RT-2 (Google)
         VLM을 직접 action 생성에 사용. 웹 지식의 로봇 전이.

2024 ── Octo, OpenVLA, pi0
         오픈소스 범용 정책 모델 등장. Cross-embodiment 학습 시작.

2025 ── Cross-embodiment learning 연구 확산
         서로 다른 로봇 간 정책 전이. 데이터 스케일링 법칙 검증 중.
```

---

모델이 주어지면 MDP Value Iteration처럼 dynamic programming으로 최적 정책을 계산할 수 있다. 모델을 모를 때는 PPO·SAC 같은 model-free RL로 경험에서 직접 학습한다. 시연 데이터가 있으면 BC·ACT·Diffusion Policy로 모방한다. 시뮬레이션에서 학습한 모델을 실제 로봇에 적용할 때는 어느 접근이든 sim-to-real gap을 따로 평가해야 한다. OpenAI의 루빅스 큐브 연구(2019)는 이를 줄이기 위해 시뮬레이션에서 약 13,000년 분량의 경험을 생성했다.

로봇이 학습하려면 먼저 환경을 지각해야 한다. Ch.9에서는 카메라 이미지에서 정보를 뽑아내는 컴퓨터 비전 기초를 다룬다.

---

# Ch.9 — 컴퓨터 비전 기초 (Computer Vision Fundamentals)

컴퓨터 비전은 카메라의 원시 데이터를 로봇이 활용할 수 있는 정보로 바꾼다. 이미지 처리와 카메라 기하를 이해해야 SLAM이나 물체 조작 파이프라인에서 생기는 오류를 추적할 수 있다.

---

## 9.1 이미지 처리 기초 (Image Processing)

카메라의 raw 이미지에는 노이즈와 불필요한 변화가 섞여 있다. 필터링, 에지 검출, 형태학적 연산으로 입력을 정리하면 후속 파이프라인의 이상이 전처리에서 생겼는지 구분할 수 있다.

### 9.1.1 OpenCV 소개

OpenCV(Open Source Computer Vision Library)는 널리 쓰이는 공개 CV 라이브러리이다.

OpenCV는 C++와 Python 바인딩을 제공하며, 영상 입출력부터 필터링·특징 추출·기하 계산까지 자주 쓰는 연산을 묶어 둔다.

설치:

```bash
pip install opencv-python opencv-contrib-python
```

기본 사용:

```python
import cv2
import numpy as np

# 이미지 읽기
img = cv2.imread('image.jpg')

# 그레이스케일 변환
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

# 이미지 표시
cv2.imshow('Image', img)
cv2.waitKey(0)
cv2.destroyAllWindows()
```

주의: OpenCV는 RGB가 아니라 BGR 순서를 사용한다. Matplotlib처럼 RGB를 쓰는 라이브러리에 이미지를 넘길 때는 `cv2.cvtColor(img, cv2.COLOR_BGR2RGB)`로 채널 순서를 바꾼다.

> 추천 자료
> - [OpenCV 공식 튜토리얼](https://docs.opencv.org/4.x/d9/df8/tutorial_root.html) — Python/C++ 예제가 잘 정리되어 있다
> - [First Principles of Computer Vision](https://www.youtube.com/channel/UCf0WB91t8Ky6AuYcQV0CcLw) — Columbia의 Shree Nayar 교수의 채널. 이미지 처리 원리를 직관적으로 설명한다
> - [Szeliski, "Computer Vision: Algorithms and Applications"](https://szeliski.org/Book/) — 무료 PDF 제공. CV 분야의 표준 교과서
> - [Stanford CS131 — Computer Vision: Foundations and Applications](http://vision.stanford.edu/teaching/cs131_fall1415/schedule.html) — CS231n보다 기초적인 CV 강의. 이미지 처리부터 시작하고 싶다면 여기서

### 9.1.2 필터링 (Filtering)

필터링은 이미지에서 원하는 정보를 남기고 노이즈를 줄인다. 에지 검출이나 segmentation에 앞서 blur를 적용하는 이유도 입력의 고주파 변동을 줄이기 위해서다.

Blur (흐림):

```python
# Gaussian Blur
blurred = cv2.GaussianBlur(img, (5, 5), 0)

# Median Blur (노이즈 제거에 효과적)
median = cv2.medianBlur(img, 5)
```

Edge Detection (에지 검출):

```python
# Canny Edge Detection
edges = cv2.Canny(gray, threshold1=50, threshold2=150)

# Sobel Operator
sobel_x = cv2.Sobel(gray, cv2.CV_64F, 1, 0, ksize=3)
sobel_y = cv2.Sobel(gray, cv2.CV_64F, 0, 1, ksize=3)
```

에지는 물체의 윤곽과 밝기 변화가 큰 경계를 드러낸다. Canny 검출 결과는 threshold 값에 민감하므로, 같은 이미지에서 값을 바꾸며 누락되는 경계와 남는 노이즈를 비교한다.

> 추천 자료
> - [First Principles of Computer Vision — Edge Detection](https://www.youtube.com/playlist?list=PL2zRqk16wsdoCCLpouGuRbcJFBVVJlvgr) — 에지 검출의 수학적 원리를 시각적으로 설명
> - [OpenCV 필터링 튜토리얼](https://docs.opencv.org/4.x/d4/d13/tutorial_py_filtering.html) — 코드와 함께 바로 따라할 수 있다
> - [Papers With Code — Edge Detection](https://paperswithcode.com/task/edge-detection) — 에지 검출 최신 벤치마크와 논문 모음

> 실습: [Canny Edge Detection](https://alexjunholee.github.io/robotics-practice/app.html#canny_edge)
> Canny 에지 검출기의 threshold 파라미터를 실시간으로 조절하며 결과 변화를 확인할 수 있다.

> 실습: [Convolution 시각화](https://alexjunholee.github.io/robotics-practice/app.html#convolution)
> 다양한 커널을 이미지에 적용하며 convolution 연산이 어떻게 필터링을 수행하는지 직관적으로 이해할 수 있다.

### 9.1.3 Morphology

Morphology는 이진 이미지(binary image)의 형태를 다듬는다. Segmentation 결과에서 작은 노이즈 점을 제거하거나 끊어진 영역을 잇는 데 쓴다.

```python
kernel = np.ones((5, 5), np.uint8)

# Erosion (침식)
eroded = cv2.erode(binary_img, kernel, iterations=1)

# Dilation (팽창)
dilated = cv2.dilate(binary_img, kernel, iterations=1)

# Opening (침식 → 팽창): 노이즈 제거
opening = cv2.morphologyEx(binary_img, cv2.MORPH_OPEN, kernel)

# Closing (팽창 → 침식): 구멍 채우기
closing = cv2.morphologyEx(binary_img, cv2.MORPH_CLOSE, kernel)
```

Opening은 먼저 깎고(erosion) 다시 키우므로(dilation) 작은 돌기와 노이즈를 없앤다. Closing은 먼저 키우고 다시 깎아 작은 구멍을 메운다.

> 추천 자료
> - [OpenCV Morphological Operations](https://docs.opencv.org/4.x/d9/d61/tutorial_py_morphological_ops.html) — 시각적 예제와 함께 설명
> - [First Principles of Computer Vision — Binary Image Processing](https://www.youtube.com/watch?v=IcBzsP-fvPo) — 형태학적 연산의 원리

---

## 9.2 카메라 모델 (Camera Model)

카메라가 세상을 어떻게 읽는지 모르면, 2D 이미지에서 3D를 복원하는 건 불가능하다. SLAM과 3D reconstruction의 출발점이 카메라 모델이다. 선형대수를 배웠다면 여기서 행렬이 어떻게 쓰이는지 체감할 수 있다.

### 9.2.1 Pinhole Model

이상적인 카메라 모델로, 3D 점을 2D 이미지로 투영한다.

픽셀 좌표 (u, v)에서 실제 세상의 3D 위치를 역으로 계산하려면 이 투영 관계를 정확히 알아야 한다. 이 관계를 수식으로 표현한 것이 Pinhole Model이다.

투영 방정식:

```
[u]   [f_x  0   c_x] [X/Z]
[v] = [0   f_y  c_y] [Y/Z]
[1]   [0    0    1 ] [ 1 ]
```

Intrinsic Parameters (내부 파라미터):
- f_x, f_y: Focal length (픽셀 단위)
- c_x, c_y: Principal point (광축이 영상면과 만나는 점이며, 이미지 중심과 다를 수 있음)
- Intrinsic Matrix K (3×3)

Extrinsic Parameters (외부 파라미터):
- R: 회전 행렬 (3×3)
- t: 이동 벡터 (3×1)
- World → Camera 변환

K는 카메라의 렌즈 특성을, [R|t]는 카메라가 세상 어디에 어떤 방향으로 놓여 있는지를 나타낸다. 이 둘을 곱하면 3D 점이 2D 픽셀로 매핑된다.

> 추천 자료
> - [Stanford CS231A — Camera Models](https://web.stanford.edu/class/cs231a/) — 기하 기반 CV의 핵심 강의
> - [First Principles of CV — Camera and Imaging](https://www.youtube.com/playlist?list=PL2zRqk16wsdoYzrWStQ2SQHXXS2K6ofd4) — Pinhole부터 실제 렌즈까지 차근차근 설명
> - [Szeliski Ch.2 — Image Formation](https://szeliski.org/Book/) — 카메라 모델의 수학적 기초
> - [정진용 블로그 — Camera Models and Distortion (Perspective, Fisheye, Omni)](https://jinyongjeong.github.io/2020/06/15/Camera_and_distortion_model/) — Perspective, Equidistant, Omni 카메라 모델 비교 정리
> - [정진용 블로그 — OpenCV Camera model 정리](https://jinyongjeong.github.io/2020/06/19/SLAM-Opencv-Camera-model-%EC%A0%95%EB%A6%AC/) — OpenCV의 핀홀/어안 카메라 모델 구현 기준 정리

> 실습: [Camera Projection](https://alexjunholee.github.io/robotics-practice/app.html#camera_projection)
> 3D 공간의 점이 카메라 내부/외부 파라미터를 통해 2D 이미지로 투영되는 과정을 인터랙티브하게 확인할 수 있다.

### 9.2.2 Distortion Models

실제 렌즈에서는 왜곡이 발생한다.

실제 카메라로 찍은 이미지는 Pinhole Model이 가정하는 것처럼 깔끔하지 않다. 특히 광각 렌즈나 fisheye 렌즈를 쓰면 직선이 곡선으로 보이는 왜곡이 심하다. 왜곡 보정을 빠뜨리면 SLAM 정확도가 뚝 떨어지고 3D reconstruction 결과가 찌그러진다.

카메라 렌즈는 완벽한 핀홀이 아니다. 렌즈를 통과하면서 빛이 휘어지고, 이 휘어짐이 이미지에 왜곡으로 나타난다.

Radial distortion (방사 왜곡): 이미지 중심에서 멀어질수록 심해진다. 파라미터 k1, k2, k3로 모델링한다. k1 < 0이면 barrel distortion (직선이 바깥으로 볼록), k1 > 0이면 pincushion distortion (직선이 안쪽으로 오목). 대부분의 렌즈는 barrel distortion을 가진다. 광각 렌즈일수록 심하다.

Tangential distortion (접선 왜곡): 렌즈가 이미지 센서와 완벽하게 평행하지 않을 때 발생한다. 파라미터 p1, p2. 보통 radial보다 영향이 작지만, 렌즈와 센서의 정렬 공차가 큰 모듈에서는 무시할 수 없다.

왜곡 보정:

```python
# 단순 보정 (매 프레임 계산 — 느림)
undistorted = cv2.undistort(distorted, K, dist_coeffs)

# 보정 맵 미리 계산 후 재사용 (SLAM 파이프라인의 일반적 패턴)
map1, map2 = cv2.initUndistortRectifyMap(K, dist_coeffs, None, K, (w, h), cv2.CV_32FC1)
undistorted = cv2.remap(distorted, map1, map2, cv2.INTER_LINEAR)
```

고정된 카메라 파라미터라면 매 프레임 `cv2.undistort()`를 호출하는 대신 `initUndistortRectifyMap()`으로 맵을 미리 계산하고 `cv2.remap()`으로 재사용할 수 있다. 실시간 파이프라인에서 계산을 줄이는 일반적인 구현 패턴이다.

**어안 렌즈 (Fisheye)**: 저차 radial-tangential 핀홀 모델은 광각 렌즈의 투영을 충분히 표현하지 못할 수 있다. 어안 모델은 빛의 입사각 θ에 대한 투영을 사용하며, equidistant ideal은 r = f·θ다. OpenCV는 이에 맞는 `cv2.fisheye` routine을 제공한다. FoV의 단일 경계값이 아니라 렌즈 투영과 별도 검증 영상의 재투영 잔차로 모델을 고른다.

(참고: [다크 프로그래머 — 카메라 왜곡보정](https://darkpgmr.tistory.com/31), [정진용 블로그 — Camera Models and Distortion](https://jinyongjeong.github.io/2020/06/15/Camera_and_distortion_model/))

> 추천 자료
> - [OpenCV Camera Calibration and 3D Reconstruction](https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html) — 왜곡 모델의 수식이 잘 정리되어 있다
> - [First Principles of CV — Lens Related Issues](https://www.youtube.com/watch?v=hzOeqCb2Fg4) — 렌즈 왜곡이 왜 생기는지 물리적 직관 설명

> 실습: [Lens Distortion 시각화](https://alexjunholee.github.io/robotics-practice/app.html#lens_distortion)
> Radial/Tangential 왜곡 파라미터를 조절하며 이미지가 어떻게 변형되는지 직접 확인할 수 있다.

### 9.2.3 캘리브레이션 (Calibration)

카메라의 내부/외부 파라미터를 추정하는 과정이다.

K (intrinsic matrix)와 왜곡 계수를 실제로 알아내야 카메라 모델을 쓸 수 있다. 캘리브레이션이 부정확하면 그 위에 쌓는 모든 것 — SLAM, 스테레오 깊이 추정, hand-eye calibration — 전부 정확도가 떨어진다. "garbage in, garbage out"의 대표적인 사례다.

체커보드 방식:

```python
# 체커보드 코너 검출
ret, corners = cv2.findChessboardCorners(gray, (9, 6), None)

# 코너 정밀화
corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)

# 캘리브레이션
ret, K, dist, rvecs, tvecs = cv2.calibrateCamera(
    object_points, image_points, gray.shape[::-1], None, None
)
```

캘리브레이션 영상은 장수보다 관측 가능성으로 평가한다. 타겟이 이미지 중앙과 가장자리를 덮고, 거리와 여러 축의 기울기가 달라지도록 촬영한다. 전체 RMS 하나뿐 아니라 이미지별·위치별 잔차와 별도 검증 영상의 오차를 확인한다.

카메라 캘리브레이션은 결국 "이 카메라가 3D 세상을 2D 이미지로 어떻게 변환하는지"의 파라미터를 알아내는 것이다. 체커보드 패턴을 여러 각도에서 촬영하면, 체커보드의 3D 좌표(알고 있음)와 이미지의 2D 좌표(검출함)의 대응 쌍이 수십~수백 개 생긴다. 이 대응 쌍으로부터:

1. Intrinsic parameters (fx, fy, cx, cy): 초점거리와 주점. 초점·줌·해상도·crop, 온도와 기계적 조립이 바뀌면 다시 확인한다.
2. Distortion coefficients (k1, k2, p1, p2, k3): 선택한 투영 모델에서 렌즈와 조립의 왜곡을 근사하는 계수다. 가격만으로 크기를 예측할 수 없다.
3. Extrinsic parameters (R, t): 각 촬영 위치에서의 카메라 자세. 캘리브레이션 자체에서는 부산물이지만, hand-eye calibration 등에서 별도로 쓰인다.

체커보드를 다양한 각도와 거리에서 촬영하고 이미지 전체에 관측을 분포시킨다. 필요한 장수는 parameter uncertainty와 conditioning, 검출 품질에 따라 달라진다. Reprojection error의 기대 범위도 해상도, 렌즈 모델, 타겟, corner detector에 의존하므로 고정된 pixel 등급표를 쓰지 않는다. 큰 residual 이미지는 자동 삭제하기 전에 blur·반사·검출 실패를 확인하고, 제외 전후의 parameter 안정성과 held-out error를 비교한다.

(참고: [다크 프로그래머 — 카메라 캘리브레이션](https://darkpgmr.tistory.com/32))

Kalibr: 멀티 카메라, Camera-IMU 캘리브레이션 도구
- ROS 기반
- AprilTag 보드 사용
- 시간 오프셋까지 추정

> 추천 자료
> - [OpenCV 카메라 캘리브레이션 튜토리얼](https://docs.opencv.org/4.x/dc/dbb/tutorial_py_calibration.html) — 체커보드 캘리브레이션 step-by-step
> - [Kalibr 공식 Wiki](https://github.com/ethz-asl/kalibr/wiki) — 널리 쓰이는 공개 Camera-IMU 캘리브레이션 도구
> - [Zhang, "A Flexible New Technique for Camera Calibration" (2000)](https://www.microsoft.com/en-us/research/publication/a-flexible-new-technique-for-camera-calibration/) — 현재 OpenCV 캘리브레이션의 기반이 되는 논문
> - [Tangram Vision Blog](https://www.tangramvision.com/blog) — 카메라 캘리브레이션, 센서 퓨전 등 실전 엔지니어링 글 모음

---

## 9.3 특징점 (Features)

이미지에서 구별 가능한 점(keypoint)과 그 주변을 설명하는 벡터(descriptor)이다.

SLAM과 Visual Odometry는 카메라가 움직이는 동안 이미지 사이에서 같은 점을 찾아야 한다. 특징점은 그 대응점을 안정적으로 찾는 수단이다.

### 9.3.1 Keypoint Detection

Harris Corner:
- 코너 검출의 고전적 방법
- 속도 느림, 스케일 변화에 취약

FAST (Features from Accelerated Segment Test):
- 매우 빠른 코너 검출
- 실시간 시스템에 적합
- 스케일 불변 아님

ORB (Oriented FAST and Rotated BRIEF):
- FAST 검출 + BRIEF 디스크립터 + 방향 정보
- 특허 무료
- 실시간 SLAM에서 널리 사용

ORB-SLAM 시리즈는 ORB를 사용한다. ORB는 특허 제약이 없고 계산이 빨라 실시간 시스템에 적합하다.

SIFT (Scale-Invariant Feature Transform):
- 스케일, 회전 불변
- 높은 반복성
- 계산 비용 높음 (과거 특허 문제, 현재 해제)

Lowe는 2004년에 SIFT를 발표했다. 스케일과 회전에 불변하는 특징점 추출 원리는 이후 나온 SURF와 ORB를 비교하는 기준이 된다.

SuperPoint (딥러닝 기반):
- Self-supervised 학습
- 높은 반복성과 정확도
- GPU 필요

> 추천 자료
> - [Lowe, "Distinctive Image Features from Scale-Invariant Keypoints" (2004)](https://www.cs.ubc.ca/~lowe/papers/ijcv04.pdf) — SIFT 원논문
> - [Rublee et al., "ORB: An efficient alternative to SIFT or SURF" (2011)](https://ieeexplore.ieee.org/document/6126544) — ORB 원논문
> - [First Principles of CV — Feature Detection](https://www.youtube.com/playlist?list=PL2zRqk16wsdqXEMpHrc4Qnb5rA1Cylrhx) — 특징점 검출의 원리를 시각적으로
> - [DeTone et al., "SuperPoint: Self-Supervised Interest Point Detection and Description" (2018)](https://arxiv.org/abs/1712.07629) — 딥러닝 기반 특징점의 시작
> - [다크 프로그래머 — 영상 특징점(keypoint) 추출방법](https://darkpgmr.tistory.com/131) — SIFT, HOG, Haar, Ferns, LBP, MCT 등 특징점 비교 정리

### 9.3.2 Descriptor

Keypoint를 찾았으면, 그 주변을 "어떻게 설명할 것인가"가 descriptor이다. 두 이미지에서 같은 물리적 점을 찾으려면, 그 점 주변의 패턴을 숫자로 표현해서 비교해야 한다.

BRIEF (Binary Robust Independent Elementary Features):
- 이진 디스크립터 (0 or 1)
- 빠른 매칭 (Hamming distance)
- 회전 불변 아님

ORB Descriptor:
- BRIEF + 방향 정보
- 256비트 이진 벡터

이진 디스크립터의 장점은 매칭 속도이다. 두 디스크립터 간 거리를 Hamming distance (XOR 연산)로 계산하기 때문에 SIFT의 유클리드 거리 비교보다 훨씬 빠르다. 임베디드 시스템에서 이 차이는 크다.

SuperGlue (딥러닝 기반):
- Graph Neural Network 기반 매칭
- 반복 패턴, 적은 텍스처에서도 강건
- LightGlue: 경량화 버전

> 추천 자료
> - [Sarlin et al., "SuperGlue: Learning Feature Matching with Graph Neural Networks" (2020)](https://arxiv.org/abs/1911.11763) — 딥러닝 기반 매칭의 대표작
> - [OpenCV Feature Matching 튜토리얼](https://docs.opencv.org/4.x/dc/dc3/tutorial_py_matcher.html) — BFMatcher, FLANN 사용법

### 9.3.3 Feature Matching

SLAM은 카메라가 움직일 때 이전 프레임과 현재 프레임에서 같은 점을 찾아야 한다. Feature matching 결과를 보면 `tracking lost`가 대응점 부족에서 시작됐는지, 잘못된 대응에서 시작됐는지 구분할 수 있다.

```python
# ORB 특징점 및 디스크립터 추출
orb = cv2.ORB_create()
kp1, des1 = orb.detectAndCompute(img1, None)
kp2, des2 = orb.detectAndCompute(img2, None)

# BFMatcher (Brute-Force)
bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
matches = bf.match(des1, des2)

# Ratio Test (Lowe's ratio)
bf = cv2.BFMatcher(cv2.NORM_HAMMING)
matches = bf.knnMatch(des1, des2, k=2)
good = [m for m, n in matches if m.distance < 0.75 * n.distance]
```

Lowe's ratio test는 kNN으로 가장 가까운 매치 두 개를 찾고, 첫째와 둘째 거리의 비가 threshold 이하인 것만 남긴다. 두 거리가 비슷한 애매한 매치를 이 과정에서 걸러낸다. Lowe의 원논문은 0.8을 사용했고 위 예제는 0.75를 사용하며, 상황에 따라 0.6~0.8 사이에서 조절할 수 있다.

> 추천 자료
> - [OpenCV Feature Matching](https://docs.opencv.org/4.x/dc/dc3/tutorial_py_matcher.html) — BFMatcher, FLANN, Ratio Test 예제
> - [Computerphile — SIFT Features](https://www.youtube.com/watch?v=ram-jbLJjFg) — 특징점 매칭의 직관적 설명

> 실습: [Feature Matching](https://alexjunholee.github.io/robotics-practice/app.html#feature_matching)
> 두 이미지 간 특징점 매칭과 Lowe's ratio test 적용 과정을 인터랙티브하게 실험할 수 있다.

---

## 9.4 에피폴라 기하학 (Epipolar Geometry)

두 카메라 시점 사이의 기하학적 관계를 다룬다.

두 장의 사진에서 같은 물체를 봤을 때, 카메라가 어떻게 움직였는지(상대 자세)를 알아내고 나아가 3D 구조를 복원하는 것이 목표다. Visual Odometry와 SfM(Structure from Motion)의 수학적 기반이 에피폴라 기하학이다. 선형대수에서 배운 SVD, eigenvalue 분해 등이 직접 쓰이는 부분이기도 하다.

### 9.4.1 Essential Matrix (E)

정의: 캘리브레이션된 카메라 쌍의 상대 자세를 인코딩

```
x2^T E x1 = 0
```

- x1, x2: 정규화된 이미지 좌표
- E = [t]_× R (t의 skew-symmetric 행렬 × R)

5-point 알고리즘: 최소 5쌍의 대응점으로 E 추정 (RANSAC과 함께 사용)

Essential Matrix에서 R과 t를 분해(decompose)하면 양의 깊이 조건으로 후보를 고른 뒤 두 카메라 간의 상대 회전과 이동 방향을 알 수 있다. 이동의 크기는 별도의 척도 정보가 필요하다. Visual Odometry의 핵심 원리다.

> 실습: [Epipolar Geometry 시각화](https://alexjunholee.github.io/robotics-practice/app.html#epipolar)
> 두 카메라 시점 간의 에피폴라 선과 에피폴을 인터랙티브하게 확인하며, Essential/Fundamental Matrix의 기하학적 의미를 이해할 수 있다.

### 9.4.2 Fundamental Matrix (F)

정의: 캘리브레이션되지 않은 카메라 쌍의 관계

```
p2^T F p1 = 0
```

- p1, p2: 픽셀 좌표
- F = K2^(-T) E K1^(-1)

8-point 알고리즘: 최소 8쌍의 대응점으로 F 추정

E와 F의 관계를 정리하면: F는 "픽셀 좌표에서 바로 쓸 수 있는" 버전이고, E는 "카메라 내부 파라미터를 이미 알고 있을 때 쓰는" 버전이다. 캘리브레이션을 했다면 E를, 안 했다면 F를 쓴다.

### 9.4.3 Triangulation

두 시점에서 동일 점을 관측했을 때, 3D 위치를 계산한다.

두 눈으로 깊이를 느끼는 것과 같은 원리다. 두 카메라(또는 움직인 하나의 카메라)에서 같은 점을 관측하면 기하학적으로 그 점의 3D 위치를 계산할 수 있다.

```python
# OpenCV triangulation
points_4d = cv2.triangulatePoints(P1, P2, pts1, pts2)
points_3d = points_4d[:3] / points_4d[3]  # Homogeneous → Cartesian
```

주의: baseline (두 카메라 간 거리)이 너무 작으면 삼각측량 정확도가 떨어지고, 너무 크면 같은 점을 양쪽에서 동시에 관측하기 어려워진다. 이 trade-off를 잘 이해해야 한다.

> 추천 자료
> - [Stanford CS231A — Epipolar Geometry](https://web.stanford.edu/class/cs231a/) — 수학적 유도가 잘 정리된 강의 자료
> - [Hartley & Zisserman, "Multiple View Geometry in Computer Vision"](https://www.robots.ox.ac.uk/~vgg/hzbook/) — 다중 시점 기하학의 핵심 교재. 깊이 들어가려면 필독
> - [First Principles of CV — Stereo Vision](https://www.youtube.com/playlist?list=PL2zRqk16wsdoYzrWStQ2SQHXXS2K6ofd4) — Epipolar geometry를 직관적으로 설명
> - [다크 프로그래머 — 영상 Geometry 시리즈 (7편: 좌표계~Epipolar)](https://darkpgmr.tistory.com/77) — 좌표계, Homogeneous, 2D/3D 변환, Homography, Imaging, Epipolar Geometry를 한글로 체계적 정리

> 실습: [Homography 시각화](https://alexjunholee.github.io/robotics-practice/app.html#homography)
> 평면 간 호모그래피 변환을 인터랙티브하게 조작하며, 4개의 대응점으로 투영 변환이 어떻게 결정되는지 확인할 수 있다.

---

## 9.5 광학 흐름 (Optical Flow)

연속 프레임 간 픽셀 이동을 추정한다.

로봇이 카메라로 세상을 보면서 움직일 때, 각 픽셀이 다음 프레임에서 어디로 갔는지 아는 것은 유용하다. Visual Odometry 자세 추정과 동적 물체 감지에 직접 쓰인다. Feature matching이 sparse한 점만 다루는 반면, dense optical flow는 모든 픽셀의 움직임을 추정한다.

### 9.5.1 Lucas-Kanade Method

- Sparse optical flow (특정 점들만)
- 밝기 불변 가정
- 작은 움직임 가정

```python
# 광류 계산
p1, status, err = cv2.calcOpticalFlowPyrLK(
    prev_gray, curr_gray, p0, None, **lk_params
)
```

"PyrLK"에서 "Pyr"는 Pyramid를 뜻한다. 이미지 피라미드를 사용해서 큰 움직임도 잡을 수 있게 한 것이다. Lucas-Kanade의 "작은 움직임 가정"을 극복하기 위한 기법이다.

### 9.5.2 Dense Optical Flow

- 모든 픽셀의 움직임 계산
- Farneback, RAFT (딥러닝)

```python
# Farneback dense flow
flow = cv2.calcOpticalFlowFarneback(prev_gray, curr_gray, None, 0.5, 3, 15, 3, 5, 1.2, 0)
```

최근에는 RAFT(Recurrent All-Pairs Field Transforms)가 dense optical flow의 기준점이 됐다. 딥러닝 기반이지만 정확도가 크게 높아서 품질이 중요한 경우에 쓰인다.

> 추천 자료
> - [First Principles of CV — Optical Flow](https://www.youtube.com/playlist?list=PL2zRqk16wsdp8KbDfHKvPYNGF2L-zQASc) — 광류의 수학적 원리
> - [Teed & Deng, "RAFT: Recurrent All-Pairs Field Transforms for Optical Flow" (2020)](https://arxiv.org/abs/2003.12039) — 딥러닝 기반 optical flow의 대표작
> - [Huang et al., "FlowFormer: A Transformer Architecture for Optical Flow" (ECCV 2022, arXiv:2203.16194)](https://arxiv.org/abs/2203.16194) — Transformer 기반 optical flow
> - [OpenCV Optical Flow 튜토리얼](https://docs.opencv.org/4.x/d4/dee/tutorial_optical_flow.html) — Lucas-Kanade, Farneback 코드 예제

> 실습: [Optical Flow 시각화](https://alexjunholee.github.io/robotics-practice/app.html#optical_flow)
> Lucas-Kanade와 Dense Optical Flow 알고리즘의 동작을 인터랙티브하게 비교하며 픽셀 이동 추정 과정을 확인할 수 있다.

---

## 9.6 심화: PnP 문제

**Perspective-n-Point (PnP)**은 3D 공간의 점과 2D 이미지의 대응점이 주어졌을 때 카메라 포즈(회전 R + 이동 t)를 추정하는 문제다. SLAM에서 매 프레임 카메라 tracking이 곧 PnP 문제이며, AR에서 마커 기반 위치 추정도 PnP로 푼다.

문제 정의: n개의 3D-2D 대응 {(X_i, x_i)}가 주어졌을 때, 카메라 외부 파라미터 [R|t]를 추정한다.

$$x_i = K [R | t] X_i$$

여기서 K는 카메라 내부 파라미터(intrinsics)이다.

P3P (3-Point Problem):
- 최소 3개의 대응점으로 풀 수 있다.
- 3점으로 최대 4개의 해가 나온다. 4번째 점을 사용해 disambiguation한다.
- RANSAC과 결합하여 outlier에 robust하게 풀 수 있다.

EPnP (Efficient PnP):
- O(n) 복잡도로, 대응점이 많을 때 효율적이다.
- 3D 점들을 4개의 가상 제어점(virtual control points)으로 표현하고, 이 제어점의 카메라 좌표를 추정하는 방식이다.
- 많은 점이 있는 경우 P3P+RANSAC보다 빠르고 안정적이다.

실무 사용법:

```python
import cv2
import numpy as np

# 3D 월드 좌표 (n x 3)
object_points = np.array([...], dtype=np.float64)
# 대응하는 2D 이미지 좌표 (n x 2)
image_points = np.array([...], dtype=np.float64)
# 카메라 내부 파라미터
camera_matrix = np.array([[fx, 0, cx],
                          [0, fy, cy],
                          [0,  0,  1]], dtype=np.float64)
dist_coeffs = np.zeros(4)

# EPnP: 비반복 폐쇄형 해법이라 초기값이 필요 없다
success, rvec, tvec = cv2.solvePnP(
    object_points, image_points, camera_matrix, dist_coeffs,
    flags=cv2.SOLVEPNP_EPNP
)

# RANSAC 버전 — outlier가 있을 때 필수
success, rvec, tvec, inliers = cv2.solvePnPRansac(
    object_points, image_points, camera_matrix, dist_coeffs,
    iterationsCount=1000, reprojectionError=3.0
)
```

SLAM과의 연결: Visual SLAM에서 매 프레임마다 수행하는 과정은 다음과 같다.
1. 이전 프레임에서 삼각측량(triangulation)으로 3D 맵 포인트를 만든다.
2. 새 프레임에서 해당 맵 포인트의 2D 재투영(reprojection)을 예측한다.
3. 실제 관측된 2D 키포인트와 매칭한다.
4. 이 3D-2D 대응으로 PnP를 풀어 새 프레임의 카메라 포즈를 구한다.

ORB-SLAM3의 Tracking도 대체로 이 흐름을 따른다. 다만 단안 모드에서 키프레임 사이의 삼각측량으로 새 맵 포인트를 만드는 일은 주로 Local Mapping이 담당한다(스테레오·RGB-D는 키프레임 삽입 시 Tracking이 근거리 점을 바로 만든다).

> 추천 자료
> - [Lepetit et al., "EPnP: An Accurate O(n) Solution to the PnP Problem" (2009)](https://doi.org/10.1007/s11263-008-0152-6) — EPnP 원논문
> - [OpenCV solvePnP 문서](https://docs.opencv.org/4.x/d5/d1f/calib3d_solvePnP.html) — 다양한 PnP 알고리즘 flag 설명
> - [Multiple View Geometry — Ch. 7](https://www.robots.ox.ac.uk/~vgg/hzbook/) — PnP 문제의 수학적 배경

solvePnP 실전 팁

OpenCV의 `cv2.solvePnP()`는 3D-2D 대응점으로 카메라 포즈를 추정한다. 반환하는 `rvec`은 Rodrigues 벡터(축-각 표현)이다.

```python
# rvec → 회전 행렬 변환
R, _ = cv2.Rodrigues(rvec)

# 카메라의 월드 좌표 위치
camera_position = -R.T @ tvec
```

주의할 점:
- `solvePnP`의 결과는 **세계→카메라 변환**이다. 카메라의 세계 좌표 위치를 구하려면 역변환을 해야 한다.
- 최소 4점이 필요하지만, 점이 많을수록 노이즈에 강건하다. RANSAC 버전인 `cv2.solvePnPRansac()`을 쓰면 outlier를 자동으로 걸러준다.
- `flags` 파라미터로 알고리즘을 선택할 수 있다: `cv2.SOLVEPNP_ITERATIVE` (기본, LM), `cv2.SOLVEPNP_P3P` (`solvePnP`에서는 정확히 4점), `cv2.SOLVEPNP_EPNP` (빠르고 안정적, 많은 점에 적합).

Rodrigues 벡터의 방향이 회전축, 크기(norm)가 회전 각도다. 3장의 Lie algebra so(3)에서 다룬 축-각 표현과 정확히 같다. `cv2.Rodrigues()`는 exp/log map의 구현이다.

(참고: [다크 프로그래머 — solvePnP 함수 사용법과 Rodrigues 표현법](https://darkpgmr.tistory.com/99))

---

## 9.7 심화: RANSAC 변종

3장의 robust estimation에서 RANSAC을 소개했다. 실제 연구에서는 vanilla RANSAC을 그대로 쓰는 경우가 드물다. 수렴 속도와 정확도를 개선한 여러 변종이 존재하며, 어떤 것을 쓰느냐에 따라 결과가 크게 달라질 수 있다.

주요 변종:

| 방법 | 핵심 아이디어 | 특징 |
|------|-------------|------|
| Lo-RANSAC | inlier로 로컬 최적화 수행 | vanilla보다 적은 iteration으로 수렴 |
| PROSAC | 매칭 신뢰도 순으로 샘플링 | 좋은 매칭부터 먼저 시도하여 수렴 가속 |
| MAGSAC++ | σ(inlier threshold)를 marginalization | threshold 설정에 덜 민감하지만 데이터별 검증 필요 |

Lo-RANSAC (Locally Optimized RANSAC):
- 좋은 모델을 찾으면, 그 모델의 inlier들로 다시 모델을 추정(local optimization)한다.
- 찾은 모델을 inlier로 다시 추정하므로, 특히 inlier ratio가 낮을 때 유용하다.

PROSAC (Progressive Sample Consensus):
- 매칭 스코어가 높은 대응점부터 우선적으로 샘플링한다.
- 좋은 매칭이 앞에 많으면 초기 iteration에서 바로 좋은 모델을 찾는다.

MAGSAC++ (Marginalizing Sample Consensus):
- 가장 골치 아픈 하이퍼파라미터인 inlier threshold σ를 marginalize한다.
- Threshold를 고정하지 않고 여러 σ 값에 대해 적분하므로 임계값 선택에 덜 민감하다. σ의 상한은 여전히 지정해야 한다.
- OpenCV에서는 `USAC_MAGSAC` 옵션으로 사용할 수 있다.

OpenCV에서 MAGSAC++ 사용:

```python
import cv2

# Fundamental matrix 추정에 MAGSAC++ 사용
F, mask = cv2.findFundamentalMat(
    pts1, pts2,
    method=cv2.USAC_MAGSAC,
    ransacReprojThreshold=1.0,
    confidence=0.999,
    maxIters=10000
)

# Homography 추정에도 동일하게 적용 가능
H, mask = cv2.findHomography(
    src_pts, dst_pts,
    method=cv2.USAC_MAGSAC,
    ransacReprojThreshold=3.0
)
```

실무 팁:
- Iteration 수: `confidence` 파라미터로 제어한다. 0.999는 주어진 inlier 비율에서 "전부 inlier인 최소 표본을 적어도 한 번 뽑을 확률이 99.9%"가 되도록 반복 수를 정한다는 뜻이며, 최종 채택 모델이 옳을 확률은 아니다. inlier ratio가 낮을수록 필요한 iteration이 기하급수적으로 증가한다.
- Threshold: MAGSAC++를 쓰면 threshold에 덜 민감하지만, 초기값은 여전히 줘야 한다. 예시 코드의 값은 시작점일 뿐이며 영상 해상도와 노이즈에 맞춰 검증해야 한다.
- PROSAC과 MAGSAC++의 속도·정확도 관계는 매칭 점수의 품질, outlier 비율, 구현에 따라 달라진다. 같은 데이터와 시간 예산으로 비교해 고른다.

> 추천 자료
> - [Barath et al., "MAGSAC++, a Fast, Reliable and Accurate Robust Estimator" (2020)](https://arxiv.org/abs/1912.05909) — MAGSAC++ 원논문
> - [OpenCV USAC 문서](https://docs.opencv.org/4.x/d1/df1/md__build_4rdparty_ippicv_ippicv_lnx_doc_USAC.html) — OpenCV의 universal RANSAC 프레임워크
> - [Chum & Matas, "Matching with PROSAC" (2005)](https://doi.org/10.1109/CVPR.2005.221) — PROSAC 원논문

---

## 9.8 심화: 학습 기반 특징 매칭

ORB, SIFT 같은 hand-crafted feature는 수십 년간 잘 작동해왔지만 반복 패턴, 텍스처 부족, 극단적 조명 변화 등에서 실패한다. 2018년 이후 딥러닝 기반 특징 추출과 매칭이 고전 방법을 넘어서기 시작했다.

파이프라인 발전 과정:

```
SuperPoint (2018) → SuperGlue (2020) → LightGlue (2023)
  [키포인트 검출+기술]    [그래프 신경망 매칭]     [경량화된 매칭]
```

SuperPoint:
- 자기지도 학습으로 키포인트 검출기와 디스크립터를 동시에 학습한다.
- Homographic adaptation: 합성 변환을 적용하고 역변환해 pseudo ground truth를 생성한다.
- 원 논문의 평가 조건에서는 여러 고전 방법보다 높은 repeatability를 보였다.

SuperGlue:
- 두 이미지의 키포인트를 그래프로 보고, attention mechanism으로 매칭한다.
- Self-attention으로 같은 이미지 내 키포인트 관계를 학습하고, cross-attention으로 두 이미지 간 매칭을 수행한다.
- Sinkhorn algorithm으로 최적 할당(optimal assignment) 문제를 푼다.
- 원 논문의 평가에서는 강한 매칭 성능을 보였지만, 연산량과 지연은 입력 특징 수와 하드웨어에 따라 달라진다.

LightGlue:
- SuperGlue의 경량 버전. Adaptive early stopping으로 쉬운 이미지 쌍은 빨리, 어려운 쌍은 더 많은 layer를 통과시킨다.
- 원 논문의 평가에서는 adaptive depth·point pruning을 사용해 SuperGlue보다 낮은 지연으로 유사한 정확도를 보였다. 배수는 하드웨어와 설정에 따라 달라진다.

LoFTR (Detector-Free Local Feature Matching):
- 키포인트 검출 단계 자체를 제거한다. 이미지 전체에서 dense matching을 수행한다.
- Transformer 기반으로 coarse-to-fine 매칭을 한다.
- 텍스처가 부족한 영역에서도 매칭이 가능하다는 것이 가장 큰 장점이다.
- 단점: 속도가 느리고 GPU 메모리를 많이 사용한다.

고전 방법 vs 학습 기반 비교:

| 항목 | ORB/SIFT | SuperPoint+LightGlue | LoFTR |
|------|----------|---------------------|-------|
| 속도 (CPU) | 빠름 | 느림 | 매우 느림 |
| 속도 (GPU) | 해당 없음 | 보통 | 느림 |
| 텍스처 부족 영역 | 실패 | 보통 | 강함 |
| 반복 패턴 | 약함 | 강함 | 강함 |
| GPU 의존성 | 없음 | 높음 | 매우 높음 |
| 실시간 로봇 적용 | 용이 | 조건부 가능 | 어려움 |

코드 예시 — LightGlue (kornia 사용):

```python
import torch
import kornia
from kornia.feature import LightGlueMatcher, KeyNetAffNetHardNet

# 특징 추출기 + 매칭기 구성
extractor = KeyNetAffNetHardNet(num_features=2048).eval()
matcher = LightGlueMatcher("keynet_affnet_hardnet").eval()

# GPU로 이동
device = torch.device("cuda" if torch.cuda.is_available() else "cpu")
extractor = extractor.to(device)
matcher = matcher.to(device)

# 이미지 로드 (kornia 형식: B x C x H x W, 0-1 범위; KeyNet은 1채널 grayscale 입력)
img0 = kornia.io.load_image(path0, kornia.io.ImageLoadType.GRAY32).unsqueeze(0).to(device)
img1 = kornia.io.load_image(path1, kornia.io.ImageLoadType.GRAY32).unsqueeze(0).to(device)

# 특징 추출
with torch.no_grad():
    lafs0, resp0, desc0 = extractor(img0)   # (lafs, responses, descriptors) 튜플
    lafs1, resp1, desc1 = extractor(img1)

# 매칭 — LightGlue는 디스크립터와 함께 키포인트 기하(lafs)를 받는다
    dists, match_idxs = matcher(desc0[0], desc1[0], lafs0, lafs1)
```

LightGlue 계열(SuperPoint+LightGlue, 위 예제의 KeyNetAffNetHardNet+LightGlue)은 GPU를 사용할 수 있고 학습 기반 매칭의 강건성이 필요한 경우 검토할 수 있다. GPU가 없거나 지연·전력 예산이 작다면 ORB 같은 고전 특징이 더 단순한 기준선이 된다. 최종 선택은 목표 데이터에서 정확도, 지연, 메모리를 함께 측정해 결정한다.

> 추천 자료
> - [DeTone et al., "SuperPoint: Self-Supervised Interest Point Detection and Description" (2018)](https://arxiv.org/abs/1712.07629) — SuperPoint 원논문
> - [Lindenberger et al., "LightGlue: Local Feature Matching at Light Speed" (2023)](https://arxiv.org/abs/2306.13643) — LightGlue 원논문
> - [Sun et al., "LoFTR: Detector-Free Local Feature Matching with Transformers" (2021)](https://arxiv.org/abs/2104.00680) — LoFTR 원논문

---

> 기술 흐름: 컴퓨터 비전 기초 (Classical Methods)
> - **~2004**: 고전적 특징점의 시대. Harris Corner (1988), SIFT (2004) 등 hand-crafted feature가 주류. 수학적으로 정교하지만 계산이 무거움
> - **2006~2011**: 실시간을 위한 경량화. SURF (2006), FAST (2006), BRIEF (2010), ORB (2011) 등이 등장. 속도 문제를 풀고, ORB 계열이 SIFT·SURF의 특허 제약까지 벗어나면서 실시간 SLAM이 가능해짐
> - **2015~2019**: 딥러닝의 침투. SuperPoint (2018) 등 학습 기반 특징점이 고전 방법의 성능을 넘어서기 시작
> - **2020~**: Geometry + Learning의 융합. SuperGlue (2020) 같은 학습 기반 매처가 등장. LoFTR (2021) 같은 detector-free matching, LightGlue (2023) 같은 경량 학습 매칭이 등장. 고전 기하학은 여전히 SLAM/VO의 백엔드에서 핵심
> - **최근 흐름**: 학습 기반 특징점과 매칭이 front-end에 도입되고 있지만, back-end에서는 여전히 epipolar geometry와 triangulation을 사용한다. 두 계열은 한 시스템 안에서 함께 쓰인다.

---

# Ch.10 — 딥러닝 기반 인식 (Deep Learning for Perception)

고전 CV가 영상 처리와 기하 관계를 직접 설계했다면, 학습 기반 인식은 데이터에서 표현을 학습해 물체의 클래스, 위치, 영역을 예측한다. 분류, 물체 탐지, 분할은 로봇이 장면 속 대상을 찾아 조작하는 데 필요한 서로 다른 출력을 만든다.

---

## 10.1 프레임워크 선택

프레임워크 선택은 기존 연구 코드와 사전학습 모델을 실행할 수 있는지에 영향을 준다. 하나의 프레임워크에서 모델 정의, 학습, 추론, 디버깅 흐름을 익히면 다른 코드베이스도 비교하기 쉽다.

### 10.1.1 PyTorch (권장)

**장점**:
- 직관적인 동적 그래프 (eager execution)
- 디버깅 용이
- 연구 코드와 사전학습 모델 생태계가 큼
- 풍부한 사전학습 모델 (torchvision, timm)

실용적인 이유도 있다. 많은 공개 연구 코드와 사전학습 모델이 PyTorch를 제공하므로, PyTorch에 익숙하면 최신 논문의 구현을 실행하고 수정하기 쉽다. 다만 특정 학회 코드의 사용 비율은 집계 방식에 따라 달라지므로 고정된 수치로 일반화하지 않는다.

**설치**:

```bash
# CUDA 12.1 버전
pip install torch torchvision torchaudio --index-url https://download.pytorch.org/whl/cu121
```

**기본 사용**:

```python
import torch
import torch.nn as nn

# 텐서 생성
x = torch.randn(32, 3, 224, 224)  # (batch, channel, height, width)

# GPU 사용
device = torch.device('cuda' if torch.cuda.is_available() else 'cpu')
x = x.to(device)
```

> **추천 자료**
> - [PyTorch 공식 튜토리얼](https://pytorch.org/tutorials/) — 입문부터 고급까지 체계적으로 정리되어 있다
> - [d2l.ai (Dive into Deep Learning)](https://d2l.ai/) — 인터랙티브 교과서. PyTorch 코드와 수학이 함께 나온다
> - [Andrej Karpathy — Neural Networks: Zero to Hero](https://www.youtube.com/playlist?list=PLAqhIrjkxbuWI23v9cThsA9GvCAUhRvKZ) — 전 Tesla AI Director가 신경망을 밑바닥부터 설명
> - [Jaejun Yoo's Playground](http://jaejunyoo.blogspot.com/search/label/kr) — 한국어로 GAN, VAE 등 생성 모델을 잘 설명한 블로그

### 10.1.2 TensorFlow / JAX

**TensorFlow**: 프로덕션 배포에 강점, TF Lite 모바일 지원
**JAX**: 고성능 연산, 함수형 프로그래밍, 연구용

대부분의 최신 연구 코드가 PyTorch로 공개되므로, PyTorch를 먼저 배우길 권장한다.

단, TensorFlow Lite는 로봇의 엣지 디바이스(Jetson, 라즈베리파이 등)에 모델을 배포할 때 여전히 많이 쓰이고, JAX는 Google DeepMind 계열 연구에서 많이 사용되므로 존재는 알아두자.

> **추천 자료**
> - [TensorFlow 공식 가이드](https://www.tensorflow.org/guide) — TFLite 변환까지 커버
> - [JAX 공식 문서](https://jax.readthedocs.io/) — 함수형 딥러닝 프레임워크

---

## 10.2 딥러닝 기초 개념

CNN의 합성곱 구조는 ResNet의 배경이 되고, Transformer의 attention 구조는 ViT와 DETR이 기존 convolution 기반 접근과 어떻게 다른지 설명한다.

### 10.2.1 CNN (Convolutional Neural Network)

CNN은 학습된 필터를 영상 전체에 적용해 공간 특징을 추출한다.

CNN은 에지, 코너, 텍스처 같은 지역 패턴을 데이터에서 학습한다. 앞서 본 SIFT와 ORB가 사람이 설계한 특징이라면, CNN의 필터는 학습 목적함수와 함께 최적화된다.

**주요 구성 요소**:
- Convolution Layer: 필터로 특징 추출
- Pooling Layer: 공간 크기 축소 (Max, Average)
- Activation: nonlinearity 도입 (ReLU, GELU)
- Batch Normalization: 학습 안정화

```python
# 간단한 CNN 블록
class ConvBlock(nn.Module):
    def __init__(self, in_ch, out_ch):
        super().__init__()
        self.conv = nn.Conv2d(in_ch, out_ch, kernel_size=3, padding=1)
        self.bn = nn.BatchNorm2d(out_ch)
        self.relu = nn.ReLU(inplace=True)

def forward(self, x):
        return self.relu(self.bn(self.conv(x)))
```

Convolution은 선형대수적으로 보면 "필터(kernel)와 이미지 패치의 내적"이다. 수업에서 배운 행렬 곱이 여기서 바로 쓰인다. `kernel_size=3, padding=1`이면 출력 크기가 입력과 같게 유지된다 — 이 패턴은 매우 자주 나온다.

> **추천 자료**
> - [Stanford CS231n — Convolutional Neural Networks for Visual Recognition](https://www.youtube.com/playlist?list=PLoROMvodv4rMFqRtEuo6SGjY4XbRIVRd4) — CNN을 이해하기 위한 대표 강의. 보는 걸 권한다
> - [d2l.ai — CNN 챕터](https://d2l.ai/chapter_convolutional-neural-networks/index.html) — 코드와 수학 설명이 동시에
> - [3Blue1Brown — But what is a Neural Network?](https://www.youtube.com/watch?v=aircAruvnKk) — 신경망의 직관적 이해

### 10.2.2 Attention & Transformer

**Self-Attention**: 시퀀스 내 모든 위치 간의 관계를 학습

```
Attention(Q, K, V) = softmax(QK^T / √d_k) V
```

CNN의 convolution은 한 층에서 국소 이웃을 모으고, 층을 쌓으며 receptive field를 넓힌다. Transformer의 self-attention은 한 층에서 떨어진 위치 사이의 관계도 계산한다. 2020년 이후 비전 모델은 pure Transformer와 CNN-Transformer hybrid를 분류·탐지·분할에 함께 사용해 왔다.

**Vision Transformer (ViT)**는 이미지를 16×16 같은 고정 크기 패치로 나누고, 각 패치를 NLP의 token처럼 취급해 Transformer encoder에 넣는다. 원 논문은 대규모 사전학습 조건에서 비교 CNN보다 높은 image-classification 성능을 보고했고, 이후 Transformer 계열은 여러 비전 태스크의 주요 선택지가 되었다.

> **추천 자료**
> - [Vaswani et al., "Attention Is All You Need" (2017)](https://arxiv.org/abs/1706.03762) — Transformer 원논문
> - [Dosovitskiy et al., "An Image is Worth 16x16 Words" (2020)](https://arxiv.org/abs/2010.11929) — ViT 원논문
> - [Yannic Kilcher — Vision Transformer 설명](https://www.youtube.com/watch?v=TrdevFK_am4) — 논문을 쉽게 풀어서 설명
> - [Andrej Karpathy — Let's build GPT from scratch](https://www.youtube.com/watch?v=kCc8FmEb1nY) — Transformer 구현을 밑바닥부터. NLP지만 ViT 이해에 직결된다

---

## 10.3 Image Classification

분류(classification)는 이미지에 무엇이 있는지 묻는 기본 태스크다. Detection과 segmentation 모델도 내부에 분류기를 포함한다. 사전학습된 분류 모델의 backbone(ResNet, ViT 등)은 다른 태스크의 feature extractor로 쓰인다.

**대표 모델**:

| 모델 | 특징 | 용도 |
| --- | --- | --- |
| ResNet | Residual connection, 안정적 학습 | 백본 네트워크 |
| EfficientNet | Compound scaling, 효율적 | 모바일, 효율성 중시 |
| ViT | Transformer 기반 | 대규모 데이터, 고성능 |
| ConvNeXt | CNN의 현대화 | ViT와 경쟁 |

ResNet의 residual connection은 블록의 입력을 출력에 더해 깊은 네트워크의 학습을 안정시킨다. 2015년 발표 이후 여러 딥러닝 아키텍처가 이 구조를 채택했다.

**사전학습 모델 사용**:

```python
import torchvision.models as models

# Pretrained ResNet50
model = models.resnet50(weights='IMAGENET1K_V2')

# Feature extractor로 사용
model.fc = nn.Identity()  # 마지막 FC 제거
features = model(x)  # (batch, 2048)
```

ImageNet으로 사전학습된 모델의 마지막 분류 레이어를 제거하고, 앞단의 출력을 feature로 재사용할 수 있다. 이 transfer learning 패턴은 새 데이터가 제한된 인식 문제에서 초기 표현을 제공한다.

> **추천 자료**
> - [He et al., "Deep Residual Learning for Image Recognition" (2015)](https://arxiv.org/abs/1512.03385) — ResNet 원논문이자 후속 비전 모델에 큰 영향을 준 연구
> - [Papers With Code — Image Classification](https://paperswithcode.com/task/image-classification) — 공개 구현과 과거 leaderboard를 찾는 출발점. 최신 수치와 protocol은 벤치마크 공식 페이지와 원 논문에서 다시 확인
> - [timm (PyTorch Image Models) 라이브러리](https://github.com/huggingface/pytorch-image-models) — 수백 개의 사전학습 모델을 한 줄로 로드
> - [Stanford CS231n — Training Neural Networks](https://www.youtube.com/playlist?list=PLoROMvodv4rMFqRtEuo6SGjY4XbRIVRd4) — 학습 기법과 트릭

---

## 10.4 Object Detection

분류가 컵의 존재를 알려준다면, manipulation에는 컵의 위치도 필요하다. Object detection은 물체의 클래스와 bounding box를 함께 예측하며, 로봇 manipulation과 자율주행 등에 쓰인다.

### 10.4.1 Two-Stage Detectors

**Faster R-CNN**:
1. Region Proposal Network (RPN): 후보 영역 제안
2. ROI Pooling: 각 영역에서 특징 추출
3. Classification + Bounding Box Regression

장점: 높은 정확도
단점: 느린 속도

Faster R-CNN은 two-stage detection의 대표작이고, 정확도가 중요한 경우(예: 산업용 검사)에서 여전히 쓰인다. "먼저 후보를 뽑고, 그 후보를 정밀 분석한다"는 구조는 직관적이고, 이후 Mask R-CNN 등으로 이어졌다.

### 10.4.2 One-Stage Detectors

**YOLO (You Only Look Once)**:
- 이미지를 그리드로 나누고 한 번에 예측
- 실시간 처리 가능 (30+ FPS)
- 버전: YOLOv5, YOLOv8, YOLOv11 (Ultralytics)

YOLO는 two-stage 방식처럼 후보 영역을 먼저 만들지 않고 이미지 전체를 한 번에 처리한다. 실시간 처리가 필요한 로봇 시스템에서 사용할 수 있다. Ultralytics의 YOLOv8/v11은 설치와 사용 절차가 짧아 프로토타이핑에 적합하다.

```python
from ultralytics import YOLO

# 모델 로드 및 추론
model = YOLO('yolov8n.pt')  # nano 모델
results = model('image.jpg')

# 결과 시각화
results[0].show()
```

**SSD (Single Shot Detector)**:
- 다양한 스케일의 feature map에서 예측
- 작은 객체는 SSD의 약점으로 알려져 있고, FPN을 도입한 YOLOv3 이후 세대가 이 부분에서 앞선다

> **추천 자료**
> - [Redmon et al., "You Only Look Once: Unified, Real-Time Object Detection" (2016)](https://arxiv.org/abs/1506.02640) — YOLO 원논문. 간결하고 읽기 좋다
> - [Ultralytics YOLOv8 문서](https://docs.ultralytics.com/) — 설치부터 커스텀 학습까지 잘 정리
> - [Papers With Code — Object Detection](https://paperswithcode.com/task/object-detection) — 공개 구현과 과거 leaderboard를 찾는 출발점. 최신 수치와 protocol은 벤치마크 공식 페이지와 원 논문에서 다시 확인
> - [다크 프로그래머 — precision, recall의 이해](https://darkpgmr.tistory.com/162) — detection 평가 지표를 직관적으로 설명

### 10.4.3 Transformer-based

**DETR (Detection Transformer)**은 detection을 "집합 예측 문제"로 재정의했다. Object Query라는 고정 개수의 학습 가능한 벡터가 각 물체에 대응하고, NMS 없이 end-to-end로 학습한다. 기존 방법들이 수천 개의 anchor box와 NMS 후처리를 요구했던 것에 비하면 파이프라인이 훨씬 단순하다. 초기 학습이 느리다는 단점이 있었지만, 구조가 깔끔해서 Deformable DETR, DINO, Co-DETR 등 많은 후속 연구로 이어졌다.

> **추천 자료**
> - [Carion et al., "End-to-End Object Detection with Transformers" (2020)](https://arxiv.org/abs/2005.12872) — DETR 원논문
> - [Yannic Kilcher — DETR 설명](https://www.youtube.com/watch?v=T35ba_VXkMY) — 논문을 잘 풀어서 설명
> - [HuggingFace — Object Detection 가이드](https://huggingface.co/docs/transformers/tasks/object_detection) — Transformers 라이브러리로 DETR 사용하기
> - [Zhao et al., "DETRs Beat YOLOs on Real-time Object Detection" (RT-DETR, CVPR 2024, arXiv:2304.08069)](https://arxiv.org/abs/2304.08069) — 논문의 비교 설정에서 실시간 DETR의 속도·정확도 개선을 보고
> - [Cheng et al., "YOLO-World: Real-Time Open-Vocabulary Object Detection" (CVPR 2024, arXiv:2401.17270)](https://arxiv.org/abs/2401.17270) — YOLO에 텍스트 프롬프트 기반 open-vocabulary detection 추가. 로보틱스에서 임의 물체 탐지에 실용적

---

## 10.5 Semantic Segmentation

픽셀 단위로 클래스를 예측하는 태스크이다.

로봇 manipulation을 생각하면 차이가 바로 보인다. detection은 bounding box를 예측하지만, semantic segmentation은 각 픽셀의 class를 예측해 물체와 배경의 경계를 더 세밀하게 표현한다. 예측 경계가 실제 윤곽과 정확히 일치하는 것은 아니므로 grasping에는 depth·instance 분리·불확실성도 함께 확인해야 한다. 자율주행의 도로·인도·차선 구분에도 픽셀 단위 분류가 쓰인다.

**대표 모델**:

| 모델 | 특징 |
| --- | --- |
| FCN | fully convolutional end-to-end segmentation의 초기 대표 모델 |
| U-Net | Encoder-Decoder 구조, 의료 영상에서 시작 |
| DeepLab v3+ | Atrous convolution, 다중 스케일 |
| SegFormer | Transformer 기반, 경량 디코더 |

U-Net의 Encoder-Decoder + Skip Connection 구조는 segmentation의 기본 패턴이 되었다. Encoder에서 특징을 추출하면서 해상도를 줄이고, Decoder에서 다시 해상도를 복원하면서 Skip Connection으로 세부 정보를 보충한다. 이 패턴은 depth estimation, image generation 등 다른 태스크에서도 널리 쓰인다.

```python
# Segmentation 모델 사용 (transformers 라이브러리)
from transformers import SegformerForSemanticSegmentation

model = SegformerForSemanticSegmentation.from_pretrained(
    "nvidia/segformer-b0-finetuned-ade-512-512"
)
```

> **추천 자료**
> - [Papers With Code — Semantic Segmentation](https://paperswithcode.com/task/semantic-segmentation) — 최신 벤치마크
> - [HuggingFace — Image Segmentation](https://huggingface.co/docs/transformers/tasks/semantic_segmentation) — SegFormer 등 사용법
> - [Two Minute Papers — Semantic Segmentation 관련 영상들](https://www.youtube.com/@TwoMinutePapers) — 최신 연구를 2분으로 요약

---

## 10.6 Instance & Panoptic Segmentation

**Instance Segmentation**: 각 객체 인스턴스를 구분
- Mask R-CNN: Faster R-CNN + Mask 브랜치

**Panoptic Segmentation**: Semantic + Instance 통합
- "Things" (객체): 인스턴스 구분
- "Stuff" (배경): 인스턴스 구분 없음

한 단계 더 생각해 보면, semantic segmentation은 "여기가 의자 영역"이라고만 알려주지, "의자가 3개 있는데 각각 어디까지인지"는 구분하지 못한다. 로봇이 "왼쪽 의자를 집어"라는 명령을 수행하려면, instance segmentation이 필요하다. Panoptic segmentation은 이 둘을 통합한 것으로, 장면 전체를 완전하게 이해하는 데 쓰인다.

> **추천 자료**
> - [He et al., "Mask R-CNN" (2017)](https://arxiv.org/abs/1703.06870) — Instance segmentation의 대표작
> - [Detectron2](https://github.com/facebookresearch/detectron2) — Meta의 detection/segmentation 프레임워크. Mask R-CNN 등을 쉽게 사용 가능

---

## 10.7 Depth Estimation

단일 이미지에서 깊이를 예측하는 태스크이다.

스테레오 카메라나 LiDAR 없이 단안(monocular) 카메라 하나로 깊이 정보를 얻을 수 있다면, 하드웨어 비용과 무게를 크게 줄일 수 있다. 특히 드론이나 소형 로봇처럼 payload가 제한적인 시스템에서 매우 유용하다. 최근 foundation model 수준의 일반화 성능을 보여주는 모델들이 나왔다.

**대표 모델**:
- MiDaS: 다양한 데이터셋 학습, 범용성
- Depth Anything: Foundation model 수준의 일반화
- ZoeDepth: 메트릭 깊이 추정

```python
# Depth Anything 사용
from transformers import pipeline

pipe = pipeline("depth-estimation", model="LiheYoung/depth-anything-base-hf")
result = pipe("image.jpg")
depth = result['depth']
```

주의할 점: MiDaS와 Depth Anything은 기본적으로 상대적 깊이(relative depth)를 추정한다. "A가 B보다 가깝다"는 알 수 있지만, "A까지 정확히 몇 미터"인지는 알 수 없다. 메트릭 깊이가 필요하면 ZoeDepth나 Depth Anything V2의 metric 버전을 사용해야 한다.

> **추천 자료**
> - [Yang et al., "Depth Anything: Unleashing the Power of Large-Scale Unlabeled Data" (2024)](https://arxiv.org/abs/2401.10891) — Depth Anything 원논문
> - [Godard et al., "Digging Into Self-Supervised Monocular Depth Estimation" (Monodepth2, ICCV 2019, arXiv:1806.01260)](https://arxiv.org/abs/1806.01260) — self-supervised depth의 기준선
> - [HuggingFace — Monocular Depth Estimation](https://huggingface.co/docs/transformers/tasks/monocular_depth_estimation) — 바로 돌려볼 수 있는 코드
> - [Papers With Code — Monocular Depth Estimation](https://paperswithcode.com/task/monocular-depth-estimation) — 최신 벤치마크 확인

---

## 10.8 심화: 학습 레시피

모델 아키텍처만으로는 재현 가능한 결과를 얻기 어렵다. 같은 모델도 learning rate와 augmentation 설정에 따라 수렴과 성능이 달라진다. 아래는 실무에서 반복적으로 쓰이는 학습 기법이다.

**Learning Rate Schedule**:

- Cosine Annealing with Warm-up: 자주 쓰이는 선택지 중 하나다. 초기 몇 epoch 동안 learning rate를 0에서 목표값까지 선형으로 올리고(warm-up), 이후 cosine 곡선으로 감쇠한다.

$$\eta_t = \eta_{min} + \frac{1}{2}(\eta_{max} - \eta_{min})\left(1 + \cos\left(\frac{t \cdot \pi}{T}\right)\right)$$

- OneCycleLR: learning rate를 한 번 올렸다가 내리는 정책. Super-convergence를 달성할 수 있어서 적은 epoch으로 빠르게 수렴한다.

```python
import torch.optim as optim

# Cosine Annealing (warm-up 없음). warm-up이 필요하면 아래 OneCycleLR의 pct_start처럼 넣는다
optimizer = optim.AdamW(model.parameters(), lr=1e-4, weight_decay=0.05)
scheduler = optim.lr_scheduler.CosineAnnealingLR(optimizer, T_max=100)

# OneCycleLR
scheduler = optim.lr_scheduler.OneCycleLR(
    optimizer, max_lr=1e-3, total_steps=len(dataloader) * num_epochs,
    pct_start=0.1  # 처음 10%를 warm-up에 사용
)
```

**Data Augmentation**:

| 기법 | 설명 | 주 용도 |
|------|------|---------|
| **RandAugment** | N개의 변환을 magnitude M으로 랜덤 적용 | 분류 전반 |
| **CutMix** | 이미지 영역을 다른 이미지로 대체, 라벨도 비율 혼합 | 분류 |
| **MixUp** | 두 이미지와 라벨을 선형 보간 | 분류 |
| **Mosaic** | 4개 이미지를 하나로 합성 | Detection (YOLO 계열) |

```python
import torchvision.transforms.v2 as T

# RandAugment
transform = T.Compose([
    T.RandAugment(num_ops=2, magnitude=9),
    T.ToTensor(),
    T.Normalize(mean=[0.485, 0.456, 0.406], std=[0.229, 0.224, 0.225]),
])
```

**Regularization**:

- Label Smoothing: hard label (0 또는 1) 대신 soft label (0.1, 0.9 등)을 사용. overconfidence를 방지한다. `nn.CrossEntropyLoss(label_smoothing=0.1)`
- Stochastic Depth: 학습 시 일부 layer를 랜덤으로 건너뛴다. ResNet 계열에서 overfitting 방지에 효과적이다.
- Weight Decay: optimizer에서 `weight_decay=0.01~0.05` 설정. AdamW에서는 decoupled weight decay를 사용한다.

**Gradient Clipping**: gradient가 폭발하는 것을 방지한다. Transformer 학습에서 안정화 도구로 자주 쓴다.

```python
torch.nn.utils.clip_grad_norm_(model.parameters(), max_norm=1.0)
```

**Loss Curve로 문제 진단**:

| 패턴 | 진단 | 대응 |
|------|------|------|
| train loss 감소, val loss 증가 | Overfitting | augmentation 추가, dropout/weight decay 증가, 데이터 확보 |
| train loss 높은 상태 정체 | Underfitting | 모델 크기 증가, learning rate 조정, augmentation 감소 |
| train loss 진동이 심함 | Learning rate 과다 | learning rate 감소 |
| train loss NaN 발생 | Gradient explosion | gradient clipping, learning rate 대폭 감소, 데이터 검증 |
| val loss 초반에 급감 후 완전 정체 | Learning rate 부족 또는 스케줄 문제 | warm-up 추가, cosine schedule 적용 |

**Distributed Training — PyTorch DDP 기본**:

모델이 커지면 GPU 1개로는 시간이 부족하다. DistributedDataParallel (DDP)은 여러 GPU에 모델을 복제하고 gradient를 동기화하는 가장 기본적인 병렬 학습 방법이다.

```python
# DDP 최소 구조 (torchrun으로 실행)
import os
import torch
import torch.distributed as dist
from torch.nn.parallel import DistributedDataParallel as DDP

dist.init_process_group("nccl")
local_rank = int(os.environ["LOCAL_RANK"])
torch.cuda.set_device(local_rank)
model = model.to(local_rank)
model = DDP(model, device_ids=[local_rank])

# 실행: torchrun --nproc_per_node=4 train.py
```

> **추천 자료**
> - [Goyal et al., "Accurate, Large Minibatch SGD" (2017)](https://arxiv.org/abs/1706.02677) — 대규모 학습의 learning rate scaling rule
> - [PyTorch DDP 튜토리얼](https://pytorch.org/tutorials/intermediate/ddp_tutorial.html) — 분산 학습 공식 가이드
> - [Wightman et al., "ResNet strikes back" (2021)](https://arxiv.org/abs/2110.00476) — 학습 레시피의 중요성을 보여주는 논문. 같은 ResNet으로 학습 기법만 바꿔 정확도를 크게 향상

> **실습**: [Data Augmentation 시각화](https://alexjunholee.github.io/robotics-practice/app.html#data_augmentation)
> RandAugment, CutMix, MixUp 등 다양한 augmentation 기법이 이미지를 어떻게 변형하는지 인터랙티브하게 확인할 수 있다.

> **실습**: [Learning Rate Schedule 시각화](https://alexjunholee.github.io/robotics-practice/app.html#lr_schedule)
> Cosine Annealing, OneCycleLR 등 다양한 learning rate schedule의 곡선을 비교하며 하이퍼파라미터의 영향을 확인할 수 있다.

---

## 10.9 심화: 자기지도 학습과 대조 학습

로보틱스 데이터는 라벨이 부족하다. 로봇이 수집하는 이미지는 수천, 수만 장이지만, 이것에 일일이 바운딩 박스나 세그멘테이션 마스크를 다는 것은 비현실적이다. 자기지도 학습(self-supervised learning)은 라벨 없이 데이터 자체에서 학습 신호를 만들어내는 방법이다.

**Contrastive Learning**:

같은 이미지의 다른 augmentation은 가깝게(positive pair), 다른 이미지는 멀게(negative pair) 임베딩 공간에 배치한다.

- SimCLR: 같은 이미지에 서로 다른 augmentation을 적용하여 positive pair를 만든다. 배치 내 다른 이미지들이 negative pair. 큰 배치 사이즈가 필요하다.
- MoCo (Momentum Contrast): momentum encoder와 queue를 사용해서 큰 배치 없이도 많은 negative를 확보한다.

**InfoNCE Loss**:

$$\mathcal{L} = -\log \frac{\exp(\text{sim}(z_i, z_j) / \tau)}{\sum_{k=1}^{2N} \mathbb{1}_{[k \neq i]} \exp(\text{sim}(z_i, z_k) / \tau)}$$

여기서 sim은 코사인 유사도, τ는 temperature이다. 분자는 positive pair의 유사도를 크게 하고, 분모는 negative pair와 구분하도록 학습한다.

**Masked Image Modeling — MAE**:

ViT 기반으로, 이미지 패치의 75%를 랜덤 마스킹하고 나머지 25%에서 마스킹된 부분을 복원하는 방식이다. NLP의 BERT가 단어를 마스킹하고 복원하는 것과 같은 원리이다.

- 마스킹 비율이 75%나 되는 이유: 이미지는 텍스트보다 redundancy가 크기 때문에, 높은 마스킹 비율이 더 어려운 과제를 만들어 좋은 표현을 학습시킨다.
- 패치의 75%를 가려 인코더가 25%만 처리하므로 학습이 효율적이다. 원 논문은 사전학습 시간이 3배 이상 줄었다고 보고한다. attention 비용이 토큰 수에 초선형이고 decoder 비용이 더해지므로 마스킹 비율과 연산량 감소율은 같지 않다.

**DINOv2와의 연결**:

DINOv2는 self-distillation 방식으로 학습한다. Teacher-student 구조이되, teacher는 student의 EMA(exponential moving average)이다.

- Self-distillation: student와 teacher가 같은 구조. teacher의 weight는 student weight의 EMA.
- Sharpening + 배치 정규화: teacher output에 sharpening(낮은 temperature)을 적용하고, DINO v1의 centering(평균 빼기) 대신 SwAV의 Sinkhorn-Knopp 배치 정규화로 mode collapse를 방지한다.
- 결과물인 DINOv2 feature는 backbone을 고정한 채로도 supervised 방법에 필적한다. ImageNet 분류는 학습 없는 k-NN으로 평가하고, ADE20K 의미 분할은 선형 head를 학습하는 linear probe에 mIoU로 평가한다. 두 평가는 학습 범위와 지표가 다르므로 모델 크기와 함께 원 논문의 수치를 확인해 인용한다.

**실무 — HuggingFace에서 self-supervised backbone fine-tune**:

```python
from transformers import AutoModel, AutoImageProcessor
import torch.nn as nn

# DINOv2 backbone 로드
processor = AutoImageProcessor.from_pretrained("facebook/dinov2-base")
backbone = AutoModel.from_pretrained("facebook/dinov2-base")

# Backbone freeze 후 classification head만 학습
for param in backbone.parameters():
    param.requires_grad = False

class MyClassifier(nn.Module):
    def __init__(self, backbone, num_classes):
        super().__init__()
        self.backbone = backbone
        self.head = nn.Linear(768, num_classes)  # DINOv2-base dim = 768

def forward(self, pixel_values):
        features = self.backbone(pixel_values).last_hidden_state[:, 0]  # CLS token
        return self.head(features)
```

> **추천 자료**
> - [Chen et al., "A Simple Framework for Contrastive Learning of Visual Representations (SimCLR)" (2020)](https://arxiv.org/abs/2002.05709) — Contrastive learning의 대표작
> - [He et al., "Masked Autoencoders Are Scalable Vision Learners" (2022)](https://arxiv.org/abs/2111.06377) — MAE 원논문
> - [Oquab et al., "DINOv2: Learning Robust Visual Features without Supervision" (2024)](https://arxiv.org/abs/2304.07193) — DINOv2 원논문

---

## 10.10 심화: Knowledge Distillation

큰 모델(teacher)의 "지식"을 작은 모델(student)에 전달하는 기법이다. 로보틱스에서는 VFM 같은 거대 모델을 edge device에서 실행하려고 distillation을 쓴다. SAM을 Jetson에서 실시간으로 실행할 때도 distillation을 적용한다.

**Teacher-Student 구조**:

Teacher 모델(큰 모델, 이미 학습됨)의 출력을 student 모델(작은 모델)이 모방하도록 학습한다. Teacher의 soft prediction은 hard label(정답)보다 더 많은 정보를 담는다.

예를 들어, "고양이" 이미지에 대해 hard label은 [1, 0, 0]이지만, teacher의 soft prediction은 [0.85, 0.10, 0.05]일 수 있다. 이 soft prediction에는 "고양이와 개가 어느 정도 유사하다"는 정보가 담겨 있고, student는 이 정보까지 학습한다.

**Soft Targets와 Temperature Scaling**:

$$\mathcal{L}_{KD} = \text{KL}\left(\sigma\left(\frac{z_t}{\tau}\right) \| \sigma\left(\frac{z_s}{\tau}\right)\right)$$

여기서 z_t, z_s는 각각 teacher, student의 logits이고, τ는 temperature이다. τ > 1이면 probability distribution이 더 "부드러워"져서 클래스 간 관계 정보가 더 많이 전달된다. τ = 3~5를 사용한다.

전체 loss는 hard label loss와 distillation loss의 가중 합이다:

$$\mathcal{L} = \alpha \cdot \mathcal{L}_{CE}(y, \sigma(z_s)) + (1 - \alpha) \cdot \tau^2 \cdot \mathcal{L}_{KD}$$

τ^2를 곱하는 이유: temperature scaling 때문에 gradient 크기가 1/τ^2로 줄어드는 것을 보상한다.

**Feature-based Distillation (FitNets)**:

Logit뿐 아니라 중간 layer의 feature map도 teacher와 유사하게 만든다.

$$\mathcal{L}_{feat} = \|f_t(x) - r(f_s(x))\|^2$$

여기서 r은 student feature 차원을 teacher 차원에 맞추는 projection layer이다. Logit distillation만으로는 전달하기 어려운 중간 표현까지 학습시킬 수 있다.

**VFM 경량화 응용**:

| Teacher | Student | 방법 |
|---------|---------|------|
| SAM (ViT-H) | MobileSAM | Image encoder를 경량 ViT로 교체, distillation |
| SAM (ViT-H) | FastSAM | YOLO 아키텍처로 전체 파이프라인 대체 |
| DINOv2-giant | DINOv2-small | 같은 구조의 작은 버전으로 distillation |

```python
import torch
import torch.nn.functional as F

def distillation_loss(student_logits, teacher_logits, labels,
                      temperature=4.0, alpha=0.5):
    # Soft target loss (KL divergence)
    soft_loss = F.kl_div(
        F.log_softmax(student_logits / temperature, dim=-1),
        F.softmax(teacher_logits / temperature, dim=-1),
        reduction="batchmean"
    ) * (temperature ** 2)

# Hard target loss
    hard_loss = F.cross_entropy(student_logits, labels)

return alpha * hard_loss + (1 - alpha) * soft_loss
```

> **추천 자료**
> - [Hinton et al., "Distilling the Knowledge in a Neural Network" (2015)](https://arxiv.org/abs/1503.02531) — Knowledge distillation 원논문
> - [Zhang et al., "Faster Segment Anything (MobileSAM)" (2023)](https://arxiv.org/abs/2306.14289) — SAM distillation 사례
> - [Romero et al., "FitNets: Hints for Thin Deep Nets" (2015)](https://arxiv.org/abs/1412.6550) — Feature-based distillation 원논문

---

## 10.11 심화: 도메인 적응

시뮬레이션에서 학습한 모델을 실제 로봇에 배포하면, 성능이 크게 떨어진다. 실내 데이터로 학습해서 실외에 배포해도 마찬가지이다. 이 문제를 **domain shift**라 하고, 이를 해결하는 연구가 domain adaptation이다. 로보틱스에서는 sim-to-real gap 문제와 직결된다.

**문제 정의**:
- Source domain D_s (라벨 있음): 시뮬레이션 데이터 또는 기존 데이터셋
- Target domain D_t (라벨 없음 또는 소량): 실제 배포 환경
- 목표: D_s에서 학습한 모델이 D_t에서도 잘 작동하도록 한다.

**Domain Randomization**:

가장 단순하지만 효과적인 접근법이다. 시뮬레이터에서 학습 데이터를 생성할 때, 환경 파라미터를 극단적으로 랜덤화한다.

- 텍스처: 벽, 바닥, 물체의 텍스처를 매 에피소드마다 랜덤 변경
- 조명: 위치, 색상, 강도를 랜덤 변경
- 카메라 파라미터: focal length, 위치, 각도에 noise 추가
- 물리 파라미터: 마찰 계수, 질량, 관성 등을 범위 내에서 랜덤 설정

충분히 다양한 시뮬레이션 환경을 보면, 실제 환경도 "또 하나의 변종"으로 처리될 수 있다.

**Adversarial Domain Adaptation**:

Domain discriminator를 도입하여, feature extractor가 source와 target을 구분할 수 없는 domain-invariant feature를 학습하도록 만든다.

```
Input --> Feature Extractor --> [Task Classifier]      --> Task Loss
                              \--> [Domain Discriminator] --> Domain Loss (GRL)
```

- Gradient Reversal Layer (GRL): domain discriminator의 gradient를 반전시켜서, domain discriminator가 source와 target을 구분하지 못하는 방향으로 feature extractor를 학습시킨다.
- Task classifier는 source domain에서 정상적으로 학습한다.
- feature extractor는 task에 유용하면서도 domain에 불변인 표현을 학습한다.

$$\mathcal{L} = \mathcal{L}_{task}(D_s) - \lambda \cdot \mathcal{L}_{domain}(D_s, D_t)$$

마이너스 부호가 핵심이다. Domain loss를 "최대화"하는 방향으로 feature extractor를 학습시킨다 (GAN과 유사한 적대적 학습).

**Test-Time Adaptation (TTA)**:

배포 후에도 모델이 새로운 환경에 적응하는 방법이다. 학습 데이터에 접근하지 않고, 추론 시 들어오는 데이터만으로 모델을 조정한다.

- TENT: batch normalization의 affine parameter를 entropy minimization으로 조정한다.
- CoTTA: continual TTA. 시간에 따라 distribution이 변하는 경우에도 적응한다.

```python
# TENT 업데이트 과정 (간략화)
model.eval()
model.requires_grad_(False)          # 먼저 전체를 얼린다
params = []
for m in model.modules():
    if isinstance(m, nn.BatchNorm2d):
        m.requires_grad_(True)       # BN의 affine parameter만 다시 켠다
        m.track_running_stats = False
        m.running_mean = None        # eval 모드에서 batch statistics를 쓰려면 running 버퍼를 비운다
        m.running_var = None
        params += [m.weight, m.bias]

optimizer = optim.SGD(params, lr=1e-4)  # BN 파라미터만 최적화

# 추론 시 adaptation
for batch in test_loader:
    output = model(batch)
    loss = entropy(output)  # prediction entropy 최소화
    loss.backward()
    optimizer.step()
    optimizer.zero_grad()
```

**Sim-to-Real Gap과의 연결**:

실제 로보틱스에서는 목표와 가용 데이터에 따라 이 기법들을 조합할 수 있다:
1. 시뮬레이터에서 domain randomization으로 다양한 데이터를 생성한다.
2. 실제 환경의 소량 데이터로 adversarial adaptation을 수행한다.
3. 배포 후 TTA로 환경 변화에 지속 적응한다.

이 세 단계를 항상 함께 쓰는 것이 표준은 아니다. Domain randomization만 쓰거나, 실제 데이터 fine-tuning을 추가하거나, 배포 중 adaptation을 금지하는 등 안전성과 계산 예산에 맞춰 조합을 정한다.

> **추천 자료**
> - [Tobin et al., "Domain Randomization for Transferring Deep Neural Networks from Simulation to the Real World" (2017)](https://arxiv.org/abs/1703.06907) — Domain randomization 원논문
> - [Ganin et al., "Domain-Adversarial Training of Neural Networks" (2016)](https://arxiv.org/abs/1505.07818) — Adversarial domain adaptation 원논문 (GRL 제안)
> - [Wang et al., "TENT: Fully Test-Time Adaptation by Entropy Minimization" (2021)](https://arxiv.org/abs/2006.10726) — TTA 대표작
> - [Wen et al., "FoundationPose: Unified 6D Pose Estimation and Tracking of Novel Objects" (CVPR 2024, arXiv:2312.08344)](https://arxiv.org/abs/2312.08344) — 새로운 물체의 6D pose 추정. CAD 모델 또는 참조 이미지 몇 장으로 동작

---

> **기술 흐름: 딥러닝 기반 인식 (Deep Learning for Perception)**
> - 2012: AlexNet이 ImageNet 대회에서 기존 방법을 큰 차이로 이기며 우승. hand-crafted feature 시대의 전환점
> - 2014~2016: VGGNet, GoogLeNet, ResNet 등장. ResNet (2015)의 residual connection은 수백 층 네트워크의 학습을 가능하게 만들었다. Faster R-CNN (2015), YOLO (2016)로 실시간 object detection이 가능해짐
> - 2017: "Attention Is All You Need" — Transformer 발표. 원래 NLP용이었지만, 이후 비전까지 확장
> - 2020~2021: ViT (Vision Transformer) 등장. 이미지를 패치 시퀀스로 처리하는 접근이 확산됐고 DETR은 detection에 Transformer를 적용했다. Swin Transformer 원 논문은 당시 여러 공개 비전 benchmark에서 비교 모델보다 높은 점수를 보고했다.
> - 2022~: ConvNeXt가 CNN 기반 모델도 여전히 경쟁력이 있음을 보였다. Segment Anything(SAM)이 segmentation을 foundation model로 끌어올림. 이후 depth estimation, 3D scene understanding이 딥러닝의 다음 영역으로 부상
> - **최근 흐름**: 하나의 foundation model 표현을 detection, segmentation, depth estimation에 함께 사용하는 파이프라인이 연구되고 있다. DINOv2 특징을 여러 downstream task에 재사용하는 방식이 한 예다.

---

# Ch.11 — Vision Foundation Models (VFM)

앞 장의 모델들은 주로 특정 데이터셋과 태스크에 맞춰 학습됐다. Vision Foundation Model은 더 큰 데이터에서 사전학습한 표현을 여러 태스크에 옮겨 쓰는 접근이다. 2023년 이후 ICRA와 IROS에서는 이 표현을 로봇 인식에 적용한 연구가 늘었다.

---

## 11.1 Foundation Model이란?

**Foundation Model**은 대규모 데이터로 사전학습되어 다양한 downstream task에 적용 가능한 모델이다.

태스크별 모델은 환경이나 인식 대상이 바뀔 때 데이터를 다시 모으고 라벨링해 학습하는 경우가 많다. Foundation model은 대규모 사전학습 표현을 재사용해 이 비용을 줄이려 한다. 본 적 없는 물체나 환경으로 전이되는 정도는 모델과 대상 도메인에 따라 달라지므로, zero-shot 평가와 추가 학습 결과를 구분해 봐야 한다.

특징:
- Scale: 수억~수십억 파라미터
- Pretraining: 대규모 데이터 (수억 이미지)
- Zero-shot / Few-shot: 학습 없이 또는 적은 예제로 새 태스크 수행
- Transfer: 다양한 도메인으로 전이

사전학습 표현을 재사용하면 새 환경마다 처음부터 라벨을 구축하는 비용을 줄일 수 있다. 다만 실제 일반화 성능은 대상 도메인과 추가 학습 여부를 구분해 평가해야 한다.

Scaling law는 성능과 모델 크기·데이터·연산량 사이에서 관찰되는 power-law 관계를 가리킨다(Kaplan et al., 2020; Zhai et al., 2022 for ViT). GPT, CLIP, SAM 같은 대형 모델의 규모를 정할 때 이 경험적 관계를 참고한다. Hoffmann et al.(2022)은 모델 크기만 늘리기보다 데이터와 연산량을 함께 배분해야 함을 보였다.

> **추천 자료**
> - [Bommasani et al., "On the Opportunities and Risks of Foundation Models" (2021)](https://arxiv.org/abs/2108.07258) — Foundation Model이라는 용어를 정의한 Stanford 보고서
> - [Two Minute Papers — Foundation Models 관련 영상들](https://www.youtube.com/@TwoMinutePapers) — 최신 VFM 연구를 빠르게 따라잡기
> - [HuggingFace Model Hub](https://huggingface.co/models) — 수천 개의 사전학습 모델을 바로 사용 가능

---

## 11.2 주요 VFM

DINOv2, SAM, CLIP, Depth Anything, GroundingDINO를 각각 표현 학습, 분할, 이미지-텍스트 정렬, 단안 깊이 추정, open-vocabulary 검출의 관점에서 비교한다.

### 11.2.1 DINOv2

**Self-Supervised Vision Transformer**로, 라벨 없이 이미지에서 풍부한 특징을 학습한다.

DINOv2는 라벨 없이도 범용적인 시각 특징을 학습한다. 이 특징은 분류, 분할, 매칭 등 다양한 태스크에 그대로 쓸 수 있다. 특히 로보틱스에서는 DINOv2의 dense feature가 텍스처 없는 영역에서도 안정적인 매칭을 제공하여, SLAM이나 Visual Odometry에서 textureless 환경의 tracking failure rate를 줄이는 데 쓰인다.

특징:
- Self-distillation (DINO) + masked image modeling (iBOT) + KoLeo 정규화
- 다양한 태스크에서 우수한 전이 성능
- Dense visual features 제공

활용:
- Image retrieval
- Semantic segmentation (linear probe)
- Feature matching for SLAM/VO
- 3D reconstruction의 feature backbone

```python
import torch
from transformers import AutoModel, AutoImageProcessor

processor = AutoImageProcessor.from_pretrained('facebook/dinov2-base')
model = AutoModel.from_pretrained('facebook/dinov2-base')

inputs = processor(images=image, return_tensors="pt")
outputs = model(**inputs)
features = outputs.last_hidden_state  # (1, num_patches+1, 768): CLS + 패치 feature
```

`last_hidden_state`에서 인덱스 0의 [CLS] 토큰은 이미지 전체 요약 벡터 역할을 하며, 이어지는 토큰들은 패치별 국소 feature를 담는다. 전자는 분류 태스크에 적합하고, 후자는 세그멘테이션이나 feature 매칭 같은 dense prediction에 활용된다.

> **추천 자료**
> - [Oquab et al., "DINOv2: Learning Robust Visual Features without Supervision" (2023)](https://arxiv.org/abs/2304.07193) — DINOv2 원논문
> - [DINOv2 GitHub](https://github.com/facebookresearch/dinov2) — 공식 코드 및 사전학습 모델
> - [HuggingFace — DINOv2](https://huggingface.co/docs/transformers/model_doc/dinov2) — HuggingFace에서 바로 사용
> - [Yannic Kilcher — DINO 설명](https://www.youtube.com/watch?v=h3ij3F3cPIk) — DINOv1의 self-distillation 원리를 설명하며 DINOv2를 이해하는 데도 도움이 된다.

### 11.2.2 SAM (Segment Anything Model)

**Promptable Segmentation**: 점, 박스, 마스크 프롬프트로 어떤 객체든 분할한다. 텍스트 프롬프트는 원 논문의 탐색적 실험이고 공개 checkpoint와 API는 지원하지 않아, 뒤에 나오는 GroundingDINO가 텍스트를 박스로 바꿔 주는 결합으로 다룬다.

기존 segmentation 모델은 학습에 사용된 클래스만 분할할 수 있었다. "의자, 테이블, 사람"으로 학습하면 "컵"은 분할하지 못한다. SAM은 1.1B개의 마스크로 학습되어, 학습 클래스에 묶이지 않고 임의의 물체를 분할하려 한다. 의료·위성·수중처럼 분포가 다른 도메인에서는 성능이 떨어진다(§11.5). 로봇이 새로운 환경에서 처음 보는 물체를 조작해야 할 때, SAM 이후로 segmentation 접근 방식이 달라졌다.

구성:
- Image Encoder: ViT로 이미지 임베딩
- Prompt Encoder: 포인트, 박스, 마스크 등
- Mask Decoder: 경량 디코더로 마스크 생성

SAM2: 비디오 지원, 더 빠른 속도

SAM2는 단일 이미지뿐 아니라 비디오에서도 동작한다. 첫 프레임에서 포인트/박스로 물체를 지정하면, 이후 프레임에서 자동으로 추적하며 분할한다. 이 기능은 로봇이 실시간으로 물체를 추적하며 조작하는 시나리오에 직접 활용할 수 있다.

```python
from segment_anything import sam_model_registry, SamPredictor

sam = sam_model_registry["vit_h"](checkpoint="sam_vit_h.pth")
predictor = SamPredictor(sam)

predictor.set_image(image)
masks, scores, logits = predictor.predict(
    point_coords=np.array([[500, 375]]),
    point_labels=np.array([1]),  # 1: 전경(foreground)
    multimask_output=True,
)
```

`multimask_output=True`로 하면 3개의 마스크 후보가 나온다(전체 물체, 부분, 더 작은 부분). `scores`는 예측된 마스크 품질(IoU) 추정치이므로, 원하는 범위를 고르려면 프롬프트를 바꾸거나 세 후보를 직접 비교해야 한다.

> **추천 자료**
> - [Kirillov et al., "Segment Anything" (2023)](https://arxiv.org/abs/2304.02643) — SAM 원논문
> - [Ravi et al., "SAM 2: Segment Anything in Images and Videos" (2024)](https://arxiv.org/abs/2408.00714) — SAM2 원논문. 비디오 segmentation으로 확장
> - [Segment Anything GitHub](https://github.com/facebookresearch/segment-anything) — 공식 코드
> - [Segment Anything Explained](https://www.youtube.com/watch?v=KRAJd4_rNrc) — SAM의 구조와 임팩트를 이해
> - [HuggingFace — SAM](https://huggingface.co/docs/transformers/model_doc/sam) — HuggingFace에서 바로 사용

> **실습**: [SAM2 Interactive Segmentation](https://alexjunholee.github.io/robotics-practice/app.html#hf_sam)
> SAM2 모델을 직접 사용하여 이미지에서 프롬프트 기반 세그멘테이션을 체험할 수 있다 (HuggingFace Space).

### 11.2.3 CLIP

**Vision-Language Model**: 이미지와 텍스트를 공유 임베딩 공간에 매핑한다.

CLIP 이전에는 이미지를 분류하려면 미리 정한 클래스 목록이 필요했다. CLIP은 이미지와 텍스트를 같은 공간에 놓으므로, 임의의 텍스트로 이미지를 검색하거나 분류할 수 있다. "red mug on a wooden table" 같은 자연어로 로봇에게 목표 물체를 지시할 수 있게 된 것이다. CLIP이 연 open-vocabulary 패러다임은 로봇이 사용자의 자연어 지시를 공간 내 객체와 연결하는 든든한 기술적 기반이 되었다.

학습: 4억 쌍의 이미지-텍스트 데이터로 contrastive learning

활용:
- Zero-shot image classification
- Image-text retrieval
- Open-vocabulary detection의 기반

```python
import clip
import torch

model, preprocess = clip.load("ViT-B/32", device="cuda")

image = preprocess(Image.open("image.jpg")).unsqueeze(0).to("cuda")
text = clip.tokenize(["a dog", "a cat", "a car"]).to("cuda")

with torch.no_grad():
    image_features = model.encode_image(image)
    text_features = model.encode_text(text)

# CLIP은 L2 정규화 후 내적해야 코사인 유사도가 된다
    image_features /= image_features.norm(dim=-1, keepdim=True)
    text_features /= text_features.norm(dim=-1, keepdim=True)
    similarity = (100.0 * image_features @ text_features.T).softmax(dim=-1)  # 100.0 ≈ model.logit_scale.exp(), 학습된 온도 스케일
    print(similarity)  # 각 텍스트와 이미지 간 유사도
```

`@`는 행렬 곱이다. CLIP은 두 feature를 L2 정규화한 뒤 내적하므로 그 값이 코사인 유사도가 되고, 여기에 학습된 스케일을 곱해 softmax를 취한다. 정규화를 건너뛰면 코사인이 아니라 단순 내적이 된다. 이것이 zero-shot classification의 원리이다.

> **추천 자료**
> - [Radford et al., "Learning Transferable Visual Models From Natural Language Supervision" (2021)](https://arxiv.org/abs/2103.00020) — CLIP 원논문
> - [OpenAI CLIP GitHub](https://github.com/openai/CLIP) — 공식 코드 및 사전학습 모델
> - [Yannic Kilcher — CLIP 설명](https://www.youtube.com/watch?v=T9XSU0pKX2E) — CLIP의 아이디어를 잘 풀어서 설명
> - [HuggingFace — CLIP](https://huggingface.co/docs/transformers/model_doc/clip) — HuggingFace에서 다양한 CLIP 변형 사용

### 11.2.4 Depth Anything

**Monocular Depth Foundation Model**: 단일 이미지에서 상대적 깊이를 추정한다.

Depth Anything은 1.5M 라벨 데이터와 62M 비라벨 데이터를 활용한 단안 깊이 모델이다. 실내(NYU), 실외(KITTI), zero-shot 도메인에서 평가됐지만, 학습 데이터와 크게 다른 내시경·수중 영상 등에서는 정확도가 떨어질 수 있다. 따라서 추가 학습 없이 얻은 결과와 대상 환경에 맞춰 조정한 결과를 나누어 본다.

특징:
- 1.5M 라벨 데이터 + 62M 비라벨 데이터 학습
- 다양한 도메인에서 강건
- V1·V2 모두 metric depth 모델 제공 (V2의 차별점은 합성 라벨 기반 재학습으로 개선된 상대 깊이 품질)

Depth Anything V2는 metric depth(절대 깊이)를 추정하는 모델도 제공한다. 물리 단위의 거리가 필요한 로봇 시스템에서는 상대적 깊이 순서와 metric depth를 구분해 사용한다.

> **추천 자료**
> - [Yang et al., "Depth Anything: Unleashing the Power of Large-Scale Unlabeled Data" (2024)](https://arxiv.org/abs/2401.10891) — Depth Anything 원논문
> - [Yang et al., "Depth Anything V2" (2024)](https://arxiv.org/abs/2406.09414) — V2 원논문. Metric depth 지원
> - [Depth Anything GitHub](https://github.com/LiheYoung/Depth-Anything) — 공식 코드
> - [HuggingFace — Depth Anything](https://huggingface.co/docs/transformers/model_doc/depth_anything) — HuggingFace에서 바로 사용

> **실습**: [Depth Anything V2](https://alexjunholee.github.io/robotics-practice/app.html#hf_depth)
> Depth Anything V2 모델로 이미지에서 깊이를 추정하는 과정을 직접 체험할 수 있다 (HuggingFace Space).

### 11.2.5 GroundingDINO

**Open-Vocabulary Object Detection**: 텍스트 프롬프트로 임의의 객체를 탐지한다.

표준 YOLO나 Faster R-CNN 같은 closed-set detector는 고정된 클래스 집합을 예측한다. GroundingDINO는 텍스트 질의를 받아 그 표현과 대응하는 이미지 영역의 bounding box를 반환한다. 따라서 "red cup" 같은 표현을 새 클래스별 출력 head를 학습하지 않고 탐지 질의로 쓸 수 있다.

```
입력: 이미지 + "person. car. traffic light."
출력: 해당 객체들의 bounding box
```

Grounded-SAM: GroundingDINO + SAM 결합
→ 텍스트 프롬프트로 객체 탐지 + 세그멘테이션

Grounded-SAM에서는 GroundingDINO가 "red cup" 같은 질의에 대응하는 bounding box를 찾고, SAM이 그 상자 안의 mask를 만든다. 이렇게 얻은 텍스트 조건 영역과 mask를 manipulation pipeline의 입력으로 사용할 수 있다.

> **추천 자료**
> - [Liu et al., "Grounding DINO: Marrying DINO with Grounded Pre-Training for Open-Set Object Detection" (2023)](https://arxiv.org/abs/2303.05499) — GroundingDINO 원논문
> - [Grounded-SAM GitHub](https://github.com/IDEA-Research/Grounded-Segment-Anything) — 텍스트 기반 탐지+분할 파이프라인
> - [HuggingFace — Grounding DINO](https://huggingface.co/docs/transformers/model_doc/grounding-dino) — HuggingFace에서 사용

> **실습**: [Grounding DINO Demo](https://alexjunholee.github.io/robotics-practice/app.html#hf_grounding_dino)
> 텍스트 프롬프트로 이미지에서 임의의 객체를 탐지하는 Open-Vocabulary Detection을 직접 체험할 수 있다 (HuggingFace Space).

---

## 11.3 VFM의 Spatial AI 응용

앞서 배운 VFM들이 실제 로보틱스 시스템에서 어떻게 결합되어 쓰이는지를 본다. 개별 모델 하나하나보다 이것들을 조합해서 공간을 이해하는 AI를 만드는 것이 로보틱스의 목표이다.

Open-vocabulary Scene Understanding:
- 사전 정의된 클래스 없이 장면 이해
- "navigate to the red chair" 같은 자연어 명령 처리

로봇이 실제 환경에서 동작하려면 미리 정해둔 물체 목록에 의존하면 안 된다. 사람의 자연어 명령을 이해하고 그에 해당하는 물체를 찾아서 행동해야 한다. CLIP + SAM + GroundingDINO 조합으로 이 파이프라인을 구현할 수 있다.

Zero-shot Semantic Segmentation:
- 새로운 환경에서 라벨링 없이 segmentation
- CLIP + SAM 조합으로 구현

Dense Feature for SLAM:
- DINOv2 features를 특징점 대신 사용
- 텍스처 없는 영역에서도 매칭 가능
- 최근 연구: DROID-SLAM + DINOv2

고전 SLAM은 ORB, SIFT 같은 특징점에 의존하므로 텍스처가 없는 벽면이나 바닥에서 특징점을 얻기 어렵다. DINOv2의 dense feature는 시맨틱 정보를 포함해 이런 영역에서도 위치별 특징을 구분할 수 있다. SLAM은 이 정보를 매칭에 활용해 textureless 환경의 tracking failure를 줄인다.

3D Scene Understanding:
- 2D VFM features를 3D로 리프팅
- Semantic NeRF, Feature 3DGS

2D에서 추출한 VFM feature를 3D 표현(NeRF, 3D Gaussian Splatting)에 심어넣으면 3D 공간 자체에 시맨틱 정보를 담을 수 있다. "이 3D 맵에서 의자는 어디에 있지?"라는 질문에 텍스트 쿼리로 답할 수 있게 된다. 이 방향의 연구가 Spatial AI에서 늘고 있다(LERF, LangSplat, ConceptGraphs 등).

> **추천 자료**
> - [Kerr et al., "LERF: Language Embedded Radiance Fields" (2023)](https://arxiv.org/abs/2303.09553) — CLIP feature를 NeRF에 심는 연구. Spatial AI의 대표적 예
> - [Tschernezki et al., "Neural Feature Fusion Fields: 3D Distillation of Self-Supervised 2D Image Representations" (2022)](https://arxiv.org/abs/2209.03494) — 2D feature를 3D로 올리는 초기 연구
> - [Papers With Code — 3D Scene Understanding](https://paperswithcode.com/task/3d-scene-understanding) — 최신 연구 동향

---

## 11.4 경량화 및 Edge 배포

로봇의 Local Module에서 사용하려면 경량화가 필요하다.

VFM은 파라미터가 수억 개에 이르러 연산과 메모리 요구가 크다. ViT-B급 모델은 데스크톱 GPU에서 돌지만, 온보드 연산 장치에서 다른 모듈과 자원을 나눠 쓰며 실시간을 맞추기는 어렵다. 반면 현장 로봇은 온보드 연산 장치에서 돌려야 하고, 필요한 처리율은 제어 루프와 작업이 정한다. 검색·지도 갱신·조작은 각각 다른 지연 예산을 갖는다. 이 간극을 메우는 것이 경량화와 Edge 배포 기술이다. 아무리 좋은 모델도 로봇에서 실시간으로 돌릴 수 없으면 논문 안에서만 빛난다.

경량화 기법:
| 기법 | 설명 |
|------|------|
| **Distillation** | 큰 모델의 지식을 작은 모델로 전이 |
| **Quantization** | FP32 → INT8/INT4로 precision 감소 |
| **Pruning** | 중요하지 않은 weight 제거 |

각 기법의 trade-off를 고려해야 한다. Quantization은 기존 네트워크 구조를 유지한 채 정밀도만 낮추므로 도입 난도가 낮다. Pruning 기법은 실제 FLOPs 절감 효과가 큰 대신 정밀한 가지치기 기준이 필요하며, Distillation은 소형 학생 모델을 새로 학습시켜 경량화 효과를 극대화하지만 추가 학습 비용을 감수해야 한다.

경량 VFM:
- FastSAM: SAM의 경량 버전 (YOLO 기반)
- MobileSAM: 모바일용 SAM
- EfficientViT-SAM: 효율적인 ViT 백본

Edge 배포 도구:
- TensorRT: NVIDIA GPU 최적화
- ONNX Runtime: 크로스 플랫폼
- TFLite: 모바일/임베디드

```python
# TensorRT 변환 예시 (PyTorch → ONNX → TensorRT)
import torch

# 1. ONNX 내보내기
torch.onnx.export(model, dummy_input, "model.onnx")

# 2. TensorRT 변환 (trtexec 사용)
# trtexec --onnx=model.onnx --saveEngine=model.trt --fp16
```

NVIDIA Jetson에서는 TensorRT가 선택 가능한 inference backend 중 하나다. FP16·INT8의 latency, memory와 task metric 변화는 model graph, input size, batch, Jetson power mode, TensorRT·CUDA version에 따라 달라진다. 따라서 target 장치에서 FP32 baseline과 같은 validation set으로 benchmark하고, INT8은 representative calibration data로 scale을 정한 뒤 정확도를 다시 측정한다.

> **추천 자료**
> - [NVIDIA TensorRT 문서](https://docs.nvidia.com/deeplearning/tensorrt/) — TensorRT 사용법과 최적화 가이드
> - [ONNX Runtime](https://onnxruntime.ai/) — 크로스 플랫폼 추론 최적화
> - [MobileSAM GitHub](https://github.com/ChaoningZhang/MobileSAM) — SAM의 모바일 경량화 버전
> - [FastSAM GitHub](https://github.com/CASIA-IVA-Lab/FastSAM) — YOLO 기반 SAM 경량화
> - [NVIDIA Jetson AI Courses](https://developer.nvidia.com/embedded/learn/jetson-ai-certification-programs) — 엣지 배포 실습

---

## 11.5 심화: VFM Fine-tuning과 Adaptation

VFM을 그대로 쓰면 zero-shot 성능이 나오지만, 특정 도메인(의료, 위성, 수중 등)에서는 성능이 떨어진다. fine-tuning이 필요한데, 수억 개의 파라미터를 전부 학습시키는 것은 비용이 크다. Parameter-efficient fine-tuning(PEFT)은 모델의 극소수 파라미터만 학습하면서도 full fine-tuning에 근접한 성능을 얻는 방법이다.

Fine-tuning 전략 비교:

| 전략 | 학습 파라미터 비율 | 성능 | GPU 메모리 | 적용 난이도 |
|------|-------------------|------|-----------|------------|
| **Full fine-tuning** | 100% | 최고 (데이터 충분 시) | 매우 높음 | 낮음 |
| **Linear probing** | <1% (head만) | 낮음 | 낮음 | 매우 낮음 |
| **LoRA** | 0.1~1% | 높음 | 낮음 | 보통 |
| **Adapter** | 1~5% | 높음 | 보통 | 보통 |
| **Prompt tuning** | <0.1% | 보통 | 낮음 | 높음 |

LoRA (Low-Rank Adaptation):

LoRA는 사전학습된 weight matrix $\mathbf{W}$에 low-rank update를 추가한다.

$$\mathbf{W}' = \mathbf{W} + \Delta\mathbf{W} = \mathbf{W} + \mathbf{B}\mathbf{A}$$

여기서 $\mathbf{W} \in \mathbb{R}^{d \times d}$이며, 분해 행렬의 차원은 각각 $\mathbf{B} \in \mathbb{R}^{d \times r}$, $\mathbf{A} \in \mathbb{R}^{r \times d}$로 정의된다 ($r \ll d$). 원래 $\mathbf{W}$의 파라미터 수 $d^2$ 대신 $2dr$개만 학습한다.

예를 들어 $d = 768$, $r = 8$이면, 원래 589,824개의 파라미터 대신 12,288개만 학습한다(약 2%).

```python
from peft import LoraConfig, get_peft_model
from transformers import AutoModelForImageClassification

# 기본 모델 로드
model = AutoModelForImageClassification.from_pretrained(
    "facebook/dinov2-base",
    num_labels=10
)

# LoRA 설정
lora_config = LoraConfig(
    r=16,                      # rank (저랭크 행렬 차원)
    lora_alpha=32,             # scaling factor
    target_modules=["query", "value"],  # attention Q, V에만 적용
    lora_dropout=0.1,
    bias="none",
)

# PEFT 모델 생성
model = get_peft_model(model, lora_config)
model.print_trainable_parameters()
# 출력 예: trainable params: 589,824 || all params: 86,567,178 || trainable%: 0.68%
```

Adapter:

Transformer block 사이에 작은 bottleneck layer를 삽입한다. 원래 weight는 고정하고 adapter layer만 학습한다.

```
Input → [Frozen Attention] → [Adapter: down_proj → ReLU → up_proj] → [Frozen FFN] → Output
```

LoRA는 훈련 후 저차원 행렬을 기존 가중치에 직접 병합하므로 추론 단계의 연산 오버헤드가 발생하지 않는다. 이와 달리 추가 레이어를 거쳐야 하는 어댑터 방식은 미세한 추론 지연을 동반한다.

Prompt Tuning:

입력에 학습 가능한 가상 토큰을 추가한다. 모델 자체는 전혀 건드리지 않고 입력만 조작한다.

- Visual Prompt Tuning(VPT): ViT의 각 layer 입력에 학습 가능한 토큰을 추가한다.
- 파라미터 효율이 가장 높지만, 성능은 LoRA보다 약간 낮은 경향이 있다.

SAM을 특정 도메인에 맞추기:

SAM의 도메인 적응에는 prompt 구성과 생성기를 조정하거나 image encoder를 미세 조정하는 전략을 쓴다.

1. Grid prompt: 이미지를 NxN 그리드로 나누고 각 교차점을 point prompt로 사용한다.
2. 학습된 prompt generator: 이미지를 입력받아 자동으로 point/box prompt를 생성하는 경량 네트워크를 학습한다.
3. LoRA + SAM: image encoder에 LoRA를 적용하여 도메인 특화 feature를 학습한다.

```python
# SAM + LoRA 적용 예시 (개념적)
from segment_anything import sam_model_registry
from peft import LoraConfig, get_peft_model

sam = sam_model_registry["vit_b"](checkpoint="sam_vit_b.pth")

# Image encoder에만 LoRA 적용
lora_config = LoraConfig(
    r=4,
    lora_alpha=8,
    target_modules=["qkv"],  # SAM attention qkv projection
)

sam.image_encoder = get_peft_model(sam.image_encoder, lora_config)
# mask decoder는 full fine-tuning (파라미터가 적으므로)
for param in sam.mask_decoder.parameters():
    param.requires_grad = True
```

평가 방법론:

VFM adaptation을 비교할 때는 다음과 같은 protocol matrix를 사용할 수 있다.

| 프로토콜 | 설명 | 비교 목적 |
|---------|------|----------|
| **Zero-shot** | 학습 없이 바로 평가 | VFM의 기본 범용성 확인 |
| **Few-shot (1/5/10-shot)** | 클래스당 소수 샘플로 학습 | 데이터 효율성 비교 |
| **Full fine-tune** | 전체 학습 데이터 사용 | 상한선 확인 |
| **PEFT (LoRA 등)** | 소수 파라미터로 학습 | 효율성-성능 trade-off |

비교할 때는 동일 backbone, 동일 데이터 split, 동일 augmentation을 써야 공정하다. Few-shot에서는 seed에 따라 결과 분산이 크므로 3~5회 반복 후 평균과 표준편차를 보고해야 한다.

> **추천 자료**
> - [Hu et al., "LoRA: Low-Rank Adaptation of Large Language Models" (2022)](https://arxiv.org/abs/2106.09685) — LoRA 원논문 (LLM용이지만 ViT에도 그대로 적용 가능)
> - [HuggingFace PEFT 라이브러리](https://github.com/huggingface/peft) — LoRA, Adapter 등 PEFT 구현체
> - [Chen et al., "SAM Fails to Segment Anything? — SAM-Adapter" (2023)](https://arxiv.org/abs/2304.09148) — SAM의 도메인 adaptation 사례

---

> **기술 흐름: Vision Foundation Models**
> - **2021**: CLIP(OpenAI) 발표. 4억 쌍의 이미지-텍스트 데이터로 공유 임베딩을 학습하고 zero-shot 분류를 시연. DINO(ICCV 2021)가 self-supervised ViT의 가능성을 보여줌
> - **2022**: Masked Autoencoders (MAE, CVPR 2022) 등 self-supervised 사전학습 방법이 주목받기 시작
> - **2023**: SAM (Segment Anything Model) 발표. 11M 이미지, 1.1B 마스크로 학습. "어떤 물체든 분할"이라는 foundation model 수준의 범용성 달성. 같은 해 DINOv2 발표 — self-supervised 비전 feature의 새 기준
> - **2024**: SAM2 (비디오 segmentation 확장), Depth Anything V2 (metric depth 지원), Florence-2 (통합 비전 모델) 등 VFM이 빠르게 진화. 모델들의 경량화와 edge 배포가 활발해짐
> - **2025~**: VFM들의 3D 확장과 멀티모달 통합이 가속. 하나의 foundation model이 detection, segmentation, depth, tracking을 통합 처리하는 방향. 로보틱스에서는 VFM이 perception의 표준 백본으로 자리잡는 추세
> - **최근 흐름**: Zero-shot 전이는 foundation model을 로봇 인식에 적용하는 주요 이유 가운데 하나다. NLMap과 ConceptGraphs 등은 CLIP, SAM, DINOv2의 표현을 open-vocabulary 인식에 조합한다. 실제 로봇에서는 FastSAM, MobileSAM 같은 경량 모델의 지연 시간과 정확도를 함께 평가한다.

---

# Ch.12 — Vision-Language-Action (VLA) & Embodied AI

VLA의 목표는 로봇이 "빨간 컵을 집어서 테이블 위에 놓아줘" 같은 자연어 명령을 받아 실행하는 것이다. 비전, 언어 모델, 제어를 하나로 묶는 분야이며, "왜 ChatGPT가 로봇을 움직이지 못하는지", "왜 시뮬레이션에서 잘 되던 정책이 실제 로봇에서 실패하는지"를 이해하려면 이 장의 개념이 필요하다.

## 12.1 VLA 개념

기존 로봇 시스템은 시각 인식, 언어 이해, 행동 생성이 각각 독립 파이프라인으로 분리되어 있었다.

**VLA (Vision-Language-Action)**는 이 세 역할을 단일 모델로 처리한다.

```
입력: 이미지 + 자연어 명령 ("pick up the red cup")
출력: 로봇 행동 (관절 각도, gripper 명령 등)
```

**Embodied AI**: 물리적 환경에서 상호작용하며 학습하는 AI
- 단순 인식을 넘어 행동까지 포함
- 시뮬레이션과 실제 환경의 간극 (Sim-to-Real)

Embodied AI는 시각적 객체 분류를 넘어 실제 환경에서 컵을 집어 올리는 물리적 상호작용까지 포괄한다. 중력, 마찰, 충돌 같은 물리 법칙이 직접 작용하므로 단순 인식 태스크보다 고려할 변수가 훨씬 복잡하다.

> **추천 자료**
> - [Google DeepMind Robotics Blog](https://deepmind.google/discover/blog/) — RT-1, RT-2, PaLM-E 등의 공식 블로그 포스트
> - [Brohan et al., "RT-2: Vision-Language-Action Models" (2023)](https://arxiv.org/abs/2307.15818) — VLA의 핵심 논문

## 12.2 주요 모델 및 연구

### 12.2.1 RT-1, RT-2 (Google DeepMind)

RT-1과 RT-2는 대규모 로봇 데이터와 웹 규모의 시각·언어 지식을 하나의 정책에 결합한 사례다. RT-1은 하나의 모델이 수백 가지 태스크를 수행할 수 있음을 논문의 실험 환경에서 보였다.

RT-1(Robotics Transformer 1)은 대규모 로봇 데모 데이터, 즉 130K 에피소드와 700개 이상의 태스크로 학습했다. 출력은 tokenized action이다.

RT-2(Robotics Transformer 2)는 PaLI-X와 PaLM-E 같은 VLM을 로봇 행동 출력에 맞게 파인튜닝한다. 원 논문은 웹 규모 데이터에서 얻은 지식의 전이와 별도의 robot-chain-of-thought 실험을 보고한다.

RT-2는 인터넷에서 학습한 거대 언어·비전 모델의 "세상에 대한 지식"을 로봇 행동에 맞춰 파인튜닝한다. 원 논문의 실험에서는 이 구조가 제로샷(zero-shot)으로 새로운 물체나 상황에 대응할 수 있음을 보였다. RT-2는 학습 데이터에 없던 물체도 언어 지식을 활용해 집어 올리는 식이다.

> **추천 자료**
> - [Google DeepMind — RT-2 Demo Video](https://deepmind.google/discover/blog/rt-2-new-model-translates-vision-and-language-into-action/) — RT-2의 실제 동작 영상과 설명
> - [Brohan et al., "RT-1: Robotics Transformer" (2022)](https://arxiv.org/abs/2212.06817) — RT-1 원 논문
> - [Brohan et al., "RT-2" (2023)](https://arxiv.org/abs/2307.15818) — RT-2 원 논문

### 12.2.2 PaLM-E

PaLM-E는 Embodied Multimodal Language Model로, PaLM(언어)과 ViT(비전)에 로봇 상태를 결합한 562B 파라미터 모델이다. 다양한 로봇 태스크를 단일 모델로 처리한다.

PaLM-E는 "positive transfer"를 보였다는 점에서 주목할 만하다. 로봇 데이터, 웹 이미지, 텍스트를 함께 학습하면 각각을 따로 학습했을 때보다 로봇 태스크 성능이 올라간다. 범용 지식이 로봇 행동에도 도움이 될 수 있음을 실증한 사례다.

> **추천 자료**
> - [Driess et al., "PaLM-E: An Embodied Multimodal Language Model" (2023)](https://arxiv.org/abs/2303.03378) — PaLM-E 원 논문

### 12.2.3 OpenVLA

RT-2와 PaLM-E의 전체 가중치는 공개되지 않았다. OpenVLA는 코드와 가중치가 공개되어, 필요한 연산 자원이 있는 연구실에서 다운로드해 파인튜닝하거나 로봇에 배포할 수 있다.

OpenVLA는 7B 파라미터(Llama 2 기반)로, 970K 에피소드로 학습됐다. 다양한 로봇 embodiment에 적용할 수 있다.

```python
# OpenVLA 사용 예시 (개념)
from openvla import OpenVLAModel

model = OpenVLAModel.from_pretrained("openvla/openvla-7b")

action = model.predict(
    image=current_image,
    instruction="pick up the blue block and place it on the red target"
)
```

RT-X 프로젝트도 OpenVLA와 함께 살펴볼 만하다. 여러 연구 기관이 수집한 로봇 데이터를 모은 Open X-Embodiment 데이터셋으로 범용 로봇 정책을 학습한다. 이 데이터셋에는 21개 기관이 모은 22개 로봇 유형의 데이터가 포함된다.

**Octo**: RT-X 데이터로 학습한 또 다른 오픈소스 모델이다. OpenVLA보다 작은 93M 파라미터 규모라 더 가볍게 활용할 수 있으며, 다양한 로봇 플랫폼에 빠르게 파인튜닝하도록 설계됐다.

> **추천 자료**
> - [OpenVLA GitHub](https://github.com/openvla/openvla) — 코드와 모델 가중치 공개
> - [Kim et al., "OpenVLA" (2024)](https://arxiv.org/abs/2406.09246) — OpenVLA 논문
> - [Open X-Embodiment Collaboration, "Open X-Embodiment" (2023)](https://arxiv.org/abs/2310.08864) — RT-X 데이터셋 논문
> - [Octo GitHub](https://github.com/octo-models/octo) — 경량 오픈소스 로봇 정책 모델

### 12.2.4 Navigation 관련

물체 조작(manipulation)뿐 아니라 환경 안에서의 이동(navigation)도 Embodied AI의 핵심 과제다. 아래 연구들은 LLM의 언어 이해 능력을 navigation에 활용한다.

LINGO는 Wayve가 공개한 주행 도메인 VLA 계열이다. LINGO-1은 주행 장면을 언어로 해설하는 개방 루프 모델이고, LINGO-2는 언어 지시와 주행 제어를 함께 다루는 폐쇄 루프 모델이다.

**SayCan**: LLM이 "할 수 있는 것"과 "해야 하는 것"을 분리
- Affordance function: 로봇이 현재 할 수 있는 행동
- LLM: 목표 달성을 위해 해야 하는 행동

예를 들어 LLM은 "커피 만들어줘"라는 요청에 "1. 컵을 잡아 2. 커피 머신으로 가 3. 버튼을 눌러..."처럼 계획을 세울 수 있다. 그러나 로봇이 컵 근처에 없다면 "컵을 잡아"는 실행할 수 없다. SayCan은 LLM의 계획(해야 하는 것)과 로봇이 현재 할 수 있는 행동을 함께 고려해, 실행 가능하면서 목표에 가까운 행동을 고른다.

> **추천 자료**
> - [Ahn et al., "Do As I Can, Not As I Say: Grounding Language in Robotic Affordances" (2022)](https://arxiv.org/abs/2204.01691) — SayCan 논문
> - [SayCan project page](https://say-can.github.io/) — 데모 영상 포함

## 12.3 World Models

실제 로봇에서 시행착오를 거듭하면 시간과 장비 비용이 커진다. **World Model**은 현재 상태와 행동으로부터 다음 상태나 관측을 예측한다. 이 예측으로 실제 장비에서 모든 후보 행동을 실행하지 않고 model-based policy를 평가할 수 있다.

World model은 model-based RL의 rollout과 위험한 행동의 사전 평가에 사용할 수 있다.

자율주행에서는 GAIA-1(Wayve)이 행동 조건부 주행 영상을 예측하고, DriveDreamer가 텍스트 조건의 주행 장면을 생성하며, MILE이 implicit world model에서 미래 상태와 주행 정책을 함께 학습한다.

구조는 상태 공간 모델과 비슷하다.

```
z_{t+1} = f(z_t, a_t)     # Dynamics model (현재 상태 + 행동 → 다음 상태)
o_t = g(z_t)              # Observation model (잠재 상태 → 관측)
r_t = h(z_t, a_t)         # Reward model (보상 예측)
```

선형대수 시간에 배운 x_{t+1} = Ax_t + Bu_t 가 비선형 신경망 버전으로 확장된 것이라고 보면 된다.

> **추천 자료**
> - [Hu et al., "GAIA-1: A Generative World Model for Autonomous Driving" (2023)](https://arxiv.org/abs/2309.17080) — Wayve의 World Model 논문
> - [Wang et al., "DriveDreamer" (2023)](https://arxiv.org/abs/2309.09777) — 주행 시나리오 생성 논문
> - [Yannic Kilcher — World Models Explained](https://www.youtube.com/watch?v=dPsXxLyqpfs) — World Model 개념 설명 영상

## 12.4 End-to-End vs Modular

End-to-end와 modular architecture는 인식, 계획, 제어를 어디에서 분리할지에 대한 두 가지 설계 방식이다.

End-to-End:

```
센서 입력 → [단일 신경망] → 행동 출력
```

- 장점: 간단한 파이프라인, 중간 표현의 bottleneck 없음
- 단점: Interpretability 부족, 대규모 데이터 필요
- 예시: NVIDIA PilotNet, Tesla FSD (추정)

자율주행에서는 End-to-End 방식이 여러 형태로 발전했다. UniAD(Unified Autonomous Driving, 2023)는 End-to-End이면서도 내부에 detection, tracking, mapping, prediction, planning 모듈을 두어 해석 가능성을 유지한다. CVPR 2023 Best Paper. VAD(2023)는 장면을 벡터화된 기하 표현으로 변환하여 효율적인 주행 정책을 유도하며, GenAD(2024)는 생성형 파이프라인을 바탕으로 다채로운 도로 시나리오에 걸친 일반화 성능을 확보한다.

Modular:

```
센서 → [인식] → [예측] → [계획] → [제어] → 행동
```

- 장점: 각 모듈 독립적 개발/디버깅, 해석 가능
- 단점: 모듈 간 정보 손실, 최적화 어려움
- 예시: Apollo, Autoware

최근 트렌드는 하이브리드다. 인식은 학습 기반, 계획·제어는 모델 기반으로 안전성을 확보하되, UniAD처럼 End-to-End 프레임워크 안에 명시적 모듈을 배치한다.

> **추천 자료**
> - [Hu et al., "Planning-oriented Autonomous Driving (UniAD)" (2023)](https://arxiv.org/abs/2212.10156) — CVPR 2023 Best Paper
> - [Jiang et al., "VAD" (2023)](https://arxiv.org/abs/2303.12077) — 벡터화 기반 자율주행
> - [Andrej Karpathy — Tesla AI Day 2022 Presentation](https://www.youtube.com/watch?v=ODSJsviD_SU) — End-to-End 자율주행 실무 관점

### End-to-End vs Modular: 2026년 현실

End-to-end와 모듈형 중 어느 쪽이 우세한지는 응용과 검증 조건에 따라 달라진다. 실제 시스템에서 모듈형이나 하이브리드 구성을 선택하는 이유는 다음과 같다.

- 디버깅: end-to-end 모델이 실패했을 때 원인을 찾기 어렵다. "왜 로봇이 컵을 놓쳤는가?"에 대해, 모듈형이면 "depth estimation이 틀렸다" 또는 "grasp planning이 잘못됐다"로 좁힐 수 있지만, end-to-end에서는 어디서 틀렸는지 모른다.
- 안전 보장: 모듈형은 각 모듈에 safety check를 넣을 수 있다 (속도 제한, 충돌 감지 등). End-to-end에서 이런 보장을 넣기 어렵다.
- 부분 업데이트: perception 모듈만 개선하고 싶을 때, 모듈형이면 해당 모듈만 교체할 수 있다. End-to-end에서는 변경 범위에 따라 여러 구성요소를 함께 재학습하거나 재검증해야 할 수 있다.
- 데이터 효율: 이 장에서 다룬 범용 정책은 대규모 데이터를 사용했다. RT-1은 약 13만 에피소드의 시연 데이터를 활용했으며, OpenVLA에 이르러서는 97만 에피소드 규모의 방대한 데이터셋으로 학습이 이뤄졌다. 대부분의 연구실이 같은 규모의 데이터를 직접 수집하기는 어렵다.

현실적인 선택지 가운데 하나는 하이브리드 구성이다. 인식에는 VFM을 쓰되 계획·제어에는 명시적 모듈과 안전 검사를 둔다. 연구실의 Local/Global Module 설계(18장)도 이 방향에 속한다.

end-to-end가 모듈형을 넓게 대체하려면 디버깅 가능성, 소규모 데이터에서의 학습 효율, safety guarantee를 응용별 검증 기준으로 입증해야 한다. 현재 공개 시스템은 이 조건들을 서로 다른 범위에서 다루며, 하나의 구조가 모두 해결했다고 보기는 어렵다.

## 12.5 Spatial AI + VLA 통합

VLA가 "커피 가져와" 같은 고수준 장기 작업을 의미론적으로 분해한다면, 로컬 제어기는 실시간 동적 장애물 회피와 차체 자세 안정화를 전담한다. 실제 시스템에서는 서로 다른 시간척도의 두 출력을 연결해야 한다.

연구실의 2-Module Architecture와 연결:

Local(Fast) Perception은 geometric understanding을 담당한다. 깊이, 장애물, 자세를 실시간(10–100 Hz)으로 처리하며, classical 또는 경량 학습 모델을 쓴다.

Global(Heavy) Understanding은 semantic understanding 담당이다. 객체, 관계, 맥락을 VFM/VLA 기반으로 처리하고, 서버 또는 클라우드에서 1–10 Hz로 실행된다.

통합 시나리오:

```
1. Local: 실시간 obstacle avoidance, odometry
2. Global: "kitchen에서 cup을 찾아서 table로 가져와"
   - VLM으로 cup 인식
   - Semantic map에서 경로 계획
3. Local이 Global의 waypoint를 받아 실제 이동 수행
```

## 12.6 Sim-to-Real & Simulation Platforms

실제 로봇으로 데이터를 모으는 일은 시간과 비용이 많이 들고 위험하다. 그래서 시뮬레이션에서 먼저 학습한 뒤 실제 로봇으로 옮기는 Sim-to-Real을 많이 쓴다. 다만 시뮬레이션과 현실 사이에는 "Reality Gap"이 있다. 다음 기법들은 이 간극을 줄이는 데 쓰인다.

**Domain Randomization**: 시뮬레이션에서 텍스처, 조명, 물리 파라미터 등을 무작위로 변경하여 학습한다. 모델이 다양한 조건에 노출되면, 현실 환경도 그 중 하나의 변형(variation)으로 처리할 수 있게 된다.

대표적인 시뮬레이션 플랫폼은 네 가지다. NVIDIA Isaac Sim/Lab은 GPU 가속 물리 시뮬레이션으로 수천 개의 환경을 병렬로 돌릴 수 있어 대규모 강화학습에 맞는다. Isaac Lab은 로봇 학습 연구를 위한 통합 프레임워크다. AI2-THOR(Allen Institute)는 주방·거실 같은 가정 환경에서 물체 조작(manipulation)을 연습할 수 있는 실내 시뮬레이터다. Habitat(Meta)는 Matterport3D·Gibson 같은 대규모 3D 스캔 환경에서 내비게이션을 학습하며, Habitat Challenge를 통해 매년 벤치마크를 제공한다. MuJoCo는 접촉(contact) 물리에 강점이 있어 로봇 팔 조작과 보행 학습에 널리 쓰이며, DeepMind가 인수한 뒤 오픈소스로 전환됐다.

> **추천 자료**
> - [NVIDIA Isaac Lab Documentation](https://isaac-sim.github.io/IsaacLab/) — 로봇 학습을 위한 시뮬레이션 프레임워크
> - [AI2-THOR Documentation](https://ai2thor.allenai.org/) — 실내 환경 시뮬레이터
> - [Habitat Documentation](https://aihabitat.org/) — Meta의 Embodied AI 플랫폼
> - [Tobin et al., "Domain Randomization for Transferring Deep Neural Networks from Simulation to the Real World" (2017)](https://arxiv.org/abs/1703.06907) — Domain Randomization 원 논문

## 12.7 심화: Imitation Learning

VLA와 Embodied AI의 정책(policy) 학습은 크게 강화학습(RL)과 모방학습(IL)으로 나뉜다. 두 접근은 데이터 수집 방식, 보상 설계, 실제 로봇에서의 탐색 비용이 다르다.

**Behavioral Cloning (BC)**

가장 단순한 IL 방법이다. 전문가(사람 또는 사전 설계된 스크립트) 시연 데이터 `{(s_t, a_t)}`를 확보한 뒤, 임의 상태 `s_t`에 대응하는 행동 `a_t`를 예측하도록 지도학습(supervised learning)을 수행한다.

```
Loss = E[ || π_θ(s_t) - a_t ||^2 ]
```

간단하고 구현하기 쉽지만 **distribution shift**라는 한계가 있다. 학습 때는 전문가의 상태 분포를 따르지만, 추론 때는 정책의 불완전한 행동이 다음 상태를 결정한다. 작은 오차가 누적되면 전문가가 방문하지 않은 상태에 이르고, 정책은 그 상태에서 적절한 행동을 고르지 못한다.

**DAgger (Dataset Aggregation)**

DAgger는 distribution shift를 완화하는 대표적 방법이다. 학습된 정책으로 데이터를 수집하면서 전문가에게 라벨을 받아 데이터셋에 추가한다.

```
1. 초기 데이터 D = {전문가 시연}으로 정책 π_1 학습
2. for i = 1, 2, ...
     π_i로 rollout 수행 → 방문한 상태 {s_t} 수집
     전문가에게 {s_t}에서의 행동 {a_t^*}를 질의
     D = D ∪ {(s_t, a_t^*)}
     D로 π_{i+1} 학습
```

전문가에게 매번 질의하는 건 비싸기 때문에, human-in-the-loop 변형이나 DAgger의 근사 버전(HG-DAgger, ThriftyDAgger 등)이 쓰인다.

두 접근인 RL과 IL의 전형적 차이는 다음과 같다. 실제 표본 수와 안전성은 알고리즘, 시뮬레이터, 전문가 데이터의 품질에 따라 달라진다.

| 기준 | RL | IL |
|------|----|----|
| Sample efficiency | 환경 상호작용이 많이 필요할 수 있음 | 전문가 시연 수와 다양성에 좌우됨 |
| 보상 함수 | 직접 설계해야 함 (reward engineering) | 불필요 |
| 안전성 | 탐색(exploration) 중 위험한 행동 가능 | 전문가 행동 모방이므로 상대적으로 안전 |
| Sim-to-Real | 보상 함수의 sim-real gap도 문제 | 실제 시연 데이터를 쓰면 gap 감소 |

로보틱스에서 보상 함수를 제대로 설계하는 것은 매우 어렵다. "컵을 잡아라"의 보상을 어떻게 정의할 것인가? 컵과 그리퍼 사이의 거리? 그러면 로봇이 컵 옆에만 가서 멈출 수 있다. 잡았는지 여부? 그러면 sparse reward 문제가 생긴다. IL은 이 문제를 우회한다.

> **추천 자료**
> - [Ross et al., "A Reduction of Imitation Learning and Structured Prediction to No-Regret Online Learning" (2011)](https://arxiv.org/abs/1011.0686) — DAgger 원 논문
> - [Florence et al., "Implicit Behavioral Cloning" (CoRL 2021)](https://arxiv.org/abs/2109.00137) — BC의 한계를 극복하기 위한 implicit 접근
> - Zare et al., "A Survey of Imitation Learning: Algorithms, Recent Developments, and Challenges" (IEEE Trans. Cybernetics, 2024) — IL 전반 서베이

## 12.8 심화: Diffusion Policy

Chi et al.(RSS 2023)이 제안한 Diffusion Policy는 로봇 조작(manipulation) 분야에서 BC 계열 방법의 대안으로 쓰이는 정책 표현 방식이다. 행동 시퀀스(action trajectory)를 denoising diffusion 과정으로 생성한다.

기존 BC는 `π_θ(s) → a`로 단일 action을 결정론적으로 예측한다. 그러나 현실에서는 같은 상태에서도 여러 행동이 가능하다(multi-modality). 테이블 위의 컵은 왼쪽에서도, 오른쪽에서도 잡을 수 있다. 결정론적 BC는 이 두 행동의 평균을 출력해 어느 쪽도 성공하지 못할 수 있다. Gaussian Mixture Model 같은 방법도 있지만 모드 수를 미리 정해야 한다.

Diffusion Policy는 denoising 과정을 이용해 이 multi-modal 분포를 표현한다.

동작은 다음 순서를 따른다.

```
1. 랜덤 노이즈 a_T ~ N(0, I)에서 시작 (T = diffusion steps)
2. 현재 관측 s를 조건으로 반복적으로 denoising:
   a_{t-1} = denoise_θ(a_t, s, t)    for t = T, T-1, ..., 1
3. 최종 a_0가 실행할 action trajectory
```

Action trajectory는 단일 action이 아니라 미래 수 스텝의 action 시퀀스 `[a(0), a(1), ..., a(H)]`이다. 괄호 안 숫자는 실행 시간 인덱스이며, 앞 의사코드의 아래첨자(diffusion step)와는 다른 축이다. 이 중 처음 몇 스텝만 실행하고(receding horizon), 다시 새 관측으로 다음 trajectory를 생성한다.

이 방식은 multi-modal action distribution의 모드 수를 미리 정할 필요가 없고, action sequence를 한 번에 생성해 시간적으로 이어진 행동을 만들 수 있다. 학습에는 denoising score matching을 쓴다.

단점은 속도다. Inference 때는 denoising 과정을 10~100 steps 반복해야 하므로 실시간 제어(>100 Hz)에는 맞지 않을 수 있다. DDIM 같은 가속 기법이나 consistency distillation으로 이를 완화할 수 있다.

Chi et al.의 원 논문 프로젝트 페이지는 4개 벤치마크의 12개 태스크에서 기존 로봇 학습 방법보다 평균 46.9% 향상됐다고 보고한다. 이 수치는 논문이 사용한 태스크·평가지표·baseline에 한정해 해석해야 한다.

> **추천 자료**
> - [Chi et al., "Diffusion Policy: Visuomotor Policy Learning via Action Diffusion" (RSS 2023)](https://arxiv.org/abs/2303.04137) — Diffusion Policy 원 논문
> - [Diffusion Policy 프로젝트 페이지](https://diffusion-policy.cs.columbia.edu/) — 코드, 데모, 영상
> - [Ho et al., "Denoising Diffusion Probabilistic Models" (NeurIPS 2020)](https://arxiv.org/abs/2006.11239) — Diffusion model 기초 논문

## 12.9 심화: Sim-to-Real Transfer

12.6에서 시뮬레이션 플랫폼과 Domain Randomization을 간략히 다뤘다. 여기서는 Sim-to-Real transfer의 구체적 기법들을 살펴본다.

**1. Domain Randomization (DR)**

시뮬레이션 환경의 파라미터를 학습 시마다 무작위로 변경한다. 모델이 충분히 다양한 조건에서 학습하면, 현실 환경이 그 변형 중 하나에 포함될 것이라는 가정이다.

랜덤화 대상:
- **시각적(Visual)**: 텍스처, 조명 방향/세기, 카메라 위치/시야각, 배경
- **물리적(Physical)**: 마찰 계수, 관성 모멘트, 링크 질량, 관절 감쇠(damping)
- **동역학(Dynamics)**: actuator 지연, 센서 노이즈, 제어 주기

DR의 한계: 랜덤화 범위를 너무 넓히면 학습 자체가 어려워지고, 너무 좁히면 현실을 커버하지 못한다. 적절한 범위를 찾는 것이 중요하다.

**2. System Identification (SysID)**

실제 시스템의 물리 파라미터를 측정하거나 추정해 시뮬레이터를 보정한다.

```
1. 실제 로봇에서 특정 trajectory를 실행하여 데이터 수집
2. 시뮬레이터의 파라미터 φ를 최적화:
   φ* = argmin_φ || f_sim(φ) - f_real ||^2
3. 보정된 시뮬레이터에서 정책 학습
```

전통적이고 효과적이지만, 모든 파라미터를 정확히 추정하기는 어렵고, 시뮬레이터가 모델링하지 않는 현상(케이블의 유연함, 접촉면의 미세 변형 등)에는 무력하다.

**3. Real-to-Sim-to-Real (R2S2R)**

실제 데이터로 시뮬레이터를 교정한 뒤 그 시뮬레이터에서 정책을 학습해 현실로 되돌리는 접근이다. 무작위화를 함께 쓰려면 어느 파라미터를 어느 범위로 흔들지 단계에 명시해야 한다.

```
1. 실제 데이터를 소량 수집
2. 실제 데이터로 시뮬레이터를 교정 (SysID) 또는 차이를 모델링
3. 교정된 시뮬레이터에서 정책 학습
4. 학습된 정책을 실제 로봇에 적용
5. (반복) 실제 결과로 시뮬레이터를 다시 교정
```

**4. Transfer 성공 여부 판단**

정량적 판단 기준은 sim과 real에서 동일 태스크의 성공률 비교다.

- Sim 성공률 ≈ Real 성공률: 두 값이 모두 충분히 높고 실패 양상도 비슷하면 transfer가 된 것으로 본다. 둘 다 낮은 경우에는 근접 자체가 근거가 되지 않으므로 절대 성능과 시행 수를 함께 본다.
- Sim >> Real: reality gap이 큼. DR 범위 확장 또는 SysID 보정 필요.
- Sim < Real: 드물지만 발생. 시뮬레이터가 오히려 더 어려운 조건(보수적)으로 설정된 경우.

추가 지표로 행동 궤적(trajectory)의 유사도, 접촉력(contact force) 비교 등을 사용하기도 한다.

> **추천 자료**
> - [Tobin et al., "Domain Randomization for Transferring Deep Neural Networks from Simulation to the Real World" (2017)](https://arxiv.org/abs/1703.06907) — DR 원 논문
> - Muratore et al., "Robot Learning from Randomized Simulations: A Review" (Frontiers in Robotics and AI, 2022) — DR 체계적 정리
> - [Hanna & Stone, "Grounded Action Transformation for Robot Learning in Simulation" (AAAI 2017)](https://ojs.aaai.org/index.php/AAAI/article/view/11044) — transfer 방법론
> - [NVIDIA Isaac Lab Tutorials](https://isaac-sim.github.io/IsaacLab/) — 실습용 DR/SysID 파이프라인

> **추가 논문 (3D/Spatial 이해 + 벤치마크)**
> - [Hong et al., "3D-LLM: Injecting the 3D World into Large Language Models" (NeurIPS 2023, arXiv:2307.12981)](https://arxiv.org/abs/2307.12981) — LLM에 3D 공간 이해 능력을 부여. 3D captioning, QA, navigation
> - [Chen et al., "SpatialVLM: Endowing Vision-Language Models with Spatial Reasoning" (CVPR 2024, arXiv:2401.12168)](https://arxiv.org/abs/2401.12168) — VLM에 거리/크기 등 공간 추론 능력 추가
> - [Nasiriany et al., "RoboCasa: Large-Scale Simulation of Everyday Tasks for Generalist Robots" (RSS 2024, arXiv:2406.02523)](https://arxiv.org/abs/2406.02523) — 100개 주방 태스크, 150+ 물체 카테고리. 가정용 로봇 벤치마크
> - [Puig et al., "Habitat 3.0: A Co-Habitat for Humans, Avatars, and Robots" (ICLR 2024, arXiv:2310.13724)](https://arxiv.org/abs/2310.13724) — 사람-로봇 공존 시뮬레이션. Social navigation, 협업 태스크

> **기술 흐름: VLA & Embodied AI**
> - **~2015**: 개별 태스크별 모방학습(imitation learning), 단일 물체 grasping 연구 중심
> - **2017~**: Domain Randomization을 통한 Sim-to-Real 전이 본격화, MuJoCo/PyBullet 기반 연구
> - **2020~**: 대규모 언어 모델(LLM)과 비전의 결합 시도. CLIPort, SayCan 등 언어 기반 로봇 제어 등장
> - **2022~**: RT-1, RT-2, PaLM-E 등 Foundation Model 기반 로봇 정책 등장. Open X-Embodiment 데이터셋 구축
> - **2024~**: OpenVLA, Octo 등 오픈소스 VLA 모델 공개. World Model 기반 계획(planning), End-to-End 자율주행(UniAD, VAD, GenAD), modular·hybrid 설계가 함께 연구됨
> - **최근 흐름**: 2023년 이후 RT-2, OpenVLA, Octo, pi0 등 Foundation Model 기반 로봇 정책이 발표됐다. 이 가운데 OpenVLA와 Octo는 공개된 코드와 가중치로 추가 학습을 실험할 수 있다.

---

# Ch.13 — 3D 비전 (3D Vision)

로봇은 2D 이미지만으로 "저 물체가 얼마나 멀리 있는지", "저 벽 뒤에 무엇이 있는지"를 알기 어렵다. 3D 비전은 로봇에게 공간 감각을 부여하는 분야다. 포인트 클라우드 처리, 3D 물체 감지, 장면 복원이 여기에 속한다. SLAM과 로봇 조작(manipulation)도 이 내용 없이는 온전히 이해하기 어렵다.

## 13.1 Point Cloud 기초

LiDAR나 깊이 카메라에서 나오는 데이터가 바로 포인트 클라우드이다. 이미지는 픽셀 격자에 정렬된 2D 데이터인 반면, 포인트 클라우드는 3D 공간에 불규칙하게 흩어진 점들이다. 이 비정형 데이터를 어떻게 다루는지가 3D 비전의 출발점이다.

**Point Cloud (포인트 클라우드)**는 3D 공간에 있는 점들의 집합이다. 각 점은 최소한 (x, y, z) 좌표를 가지며, 추가로 색상(RGB), 반사도(intensity), 법선(normal) 등의 속성을 가질 수 있다.

### 13.1.1 데이터 구조 및 포맷

**일반적인 구조**:

```
Point: [x, y, z, r, g, b, intensity, ...]
Point Cloud: N × D 행렬 (N개 점, D차원 속성)
```

선형대수 관점에서 포인트 클라우드는 N×D 행렬로 볼 수 있다. N은 수만~수백만 개의 점, D는 각 점의 속성 차원이다. 회전과 이동은 각 점의 위치 좌표를 동차 형태로 만들어 4×4 변환 행렬을 곱하는 연산이다. 색상이나 반사도 같은 속성은 이 변환의 대상이 아니고, 법선은 회전만 적용한다.

**주요 파일 포맷**:
| 포맷 | 특징 |
|---|---|
| **PCD** | PCL 표준, ASCII/Binary |
| **PLY** | 다목적, mesh도 지원 |
| **LAS/LAZ** | 지리정보 표준, LAZ는 압축 |
| **XYZ** | 단순 텍스트, 좌표만 |
| **BIN** | KITTI 등에서 사용, 바이너리 |

> **추천 자료**
> - [Open3D Documentation](http://www.open3d.org/docs/release/) — 포인트 클라우드 처리의 현대적 라이브러리
> - [PCL (Point Cloud Library) Tutorials](https://pcl.readthedocs.io/projects/tutorials/en/latest/) — 포인트 클라우드 처리의 고전 라이브러리

### 13.1.2 라이브러리

**PCL (Point Cloud Library)**:
- C++ 기반, 가장 포괄적
- ROS와 통합
- 필터링, 세그멘테이션, 정합 등

```cpp
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPCDFile<pcl::PointXYZ>("cloud.pcd", *cloud);
```

**Open3D**:
- Python/C++, 현대적 API
- 시각화 강점
- 딥러닝 친화적

```python
import open3d as o3d

# 포인트 클라우드 읽기
pcd = o3d.io.read_point_cloud("cloud.pcd")

# 시각화
o3d.visualization.draw_geometries([pcd])

# NumPy 변환
points = np.asarray(pcd.points)  # (N, 3)
```

Open3D는 직관적인 파이썬 바인딩 덕분에 초기 알고리즘 프로토타이핑에 매우 적합하다. 한편 실시간 C++ ROS 파이프라인 구축에는 전통적인 PCL이 여전히 폭넓게 활용된다. 처음 입문한다면 Open3D부터 시작하는 경로를 추천한다.

> **추천 자료**
> - [Open3D Getting Started](http://www.open3d.org/docs/release/getting_started.html) — Python으로 포인트 클라우드 다루기 입문
> - [PCL Tutorials — Basic Usage](https://pcl.readthedocs.io/projects/tutorials/en/latest/#basic-usage) — C++ 기반 포인트 클라우드 처리
> - [Open3D YouTube Channel](https://www.youtube.com/@Open3D) — 시각화 및 처리 튜토리얼

## 13.2 Point Cloud 처리

### 13.2.1 필터링 (Filtering)

원시 포인트 클라우드에는 노이즈가 많고 점의 밀도도 불균일하다. 그대로 쓰면 정합이나 세그멘테이션 같은 후속 알고리즘이 느려지거나 결과가 나빠진다. 그래서 필터링은 대부분의 포인트 클라우드 파이프라인에서 첫 단계가 된다.

**Voxel Grid Downsampling**:
공간을 격자로 나누고 각 격자 내 점들을 하나로 축소한다.

```python
# Open3D
voxel_pcd = pcd.voxel_down_sample(voxel_size=0.05)  # 5cm 격자
```

**Statistical Outlier Removal**:
이웃과의 거리 통계를 기반으로 이상치를 제거한다.

```python
# 이상치 제거
cl, ind = pcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0)
filtered_pcd = pcd.select_by_index(ind)
```

**Radius Outlier Removal**:
주어진 반경 내 이웃 수가 부족한 점을 제거한다.

> **추천 자료**
> - [Open3D — Point Cloud Filtering Tutorial](http://www.open3d.org/docs/release/tutorial/geometry/pointcloud.html) — Voxel downsampling, outlier removal 예제
> - [PCL — Filtering Tutorial](https://pcl.readthedocs.io/projects/tutorials/en/latest/passthrough.html) — PCL 기반 필터링

### 13.2.2 Normal Estimation

3D 처리 파이프라인에서 각 점의 표면 법선 벡터를 추정하는 전처리 단계다.

표면 재구성(reconstruction), 조명 계산, 그리고 point-to-plane ICP가 법선 벡터를 요구한다. point-to-point ICP는 법선 없이 동작한다. 법선이 없으면 "이 점이 평면의 일부인지 모서리의 일부인지"를 알 수 없다.

```python
# 법선 추정
pcd.estimate_normals(
    search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30)
)
```

내부적으로는 각 점 주변의 이웃 $k$개 점들로 공분산 행렬을 구성한 뒤, 최소 고유값에 대응하는 고유벡터를 산출하여 법선 방향으로 채택한다. 이는 선형대수의 주성분 분석(PCA)과 동일한 기하학적 원리다.

### 13.2.3 Registration (정합)

두 포인트 클라우드를 정렬하는 과정이다. SLAM에서 연속 프레임을 이어 붙이거나, 여러 뷰에서 스캔한 데이터를 합칠 때 필요하다.

**ICP (Iterative Closest Point)**:
1. 가장 가까운 점 쌍 찾기
2. 변환 계산 (최소자승법)
3. 변환 적용
4. 수렴할 때까지 반복

```python
# Point-to-Point ICP
reg = o3d.pipelines.registration.registration_icp(
    source, target, max_correspondence_distance=0.05,
    estimation_method=o3d.pipelines.registration.TransformationEstimationPointToPoint()
)
transformation = reg.transformation
```

ICP는 두 포인트 클라우드에서 가장 가까운 점 쌍을 찾고, 그 쌍이 최대한 겹치도록 회전·이동 변환을 구하는 과정을 반복한다. 선형대수적으로는 SVD(특이값 분해)로 최적의 회전 행렬 R과 이동 벡터 t를 구한다.

**Point-to-Plane ICP**: 점과 평면 거리 최소화 (더 정확)

**GICP (Generalized ICP)**: 점 분포 고려

**NDT (Normal Distributions Transform)**: 공간을 셀로 나누고 각 셀의 정규분포 매칭

**Feature-based Registration**:
- FPFH, SHOT 등 특징 추출
- RANSAC으로 초기 정합
- ICP로 정밀화

> **추천 자료**
> - [Open3D — ICP Registration Tutorial](http://www.open3d.org/docs/release/tutorial/pipelines/icp_registration.html) — ICP 실습 코드
> - [Open3D — Global Registration (RANSAC + Feature)](http://www.open3d.org/docs/release/tutorial/pipelines/global_registration.html) — Feature 기반 정합
> - [Cyrill Stachniss — ICP & Point Cloud Registration](https://www.youtube.com/watch?v=dhzLQfDBx2Q) — ICP 알고리즘의 직관적 설명

> **실습**: [ICP 2D 단계별 시각화](https://alexjunholee.github.io/robotics-practice/app.html#icp_steps) | [ICP 3D](https://alexjunholee.github.io/robotics-practice/app.html#icp_3d)
> ICP 알고리즘이 두 포인트 클라우드를 정합하는 과정을 iteration별로 확인하고, 2D와 3D 환경에서 수렴 과정을 비교할 수 있다.

## 13.3 3D Object Detection

포인트 클라우드에서 3D bounding box를 예측하는 분야다. 자율주행에서 "저 차가 어디에 있고 얼마나 큰지"를 파악하는 핵심 기술이다.

### 13.3.1 Point-based Methods

PointNet은 포인트 클라우드를 복셀이나 이미지로 변환하지 않고 raw 포인트에 직접 신경망을 적용한다. 격자 구조를 전제로 하는 CNN과 달리, 불규칙한 점 집합을 그대로 입력으로 받는다.

**PointNet (2017)**:
- Raw 포인트에 직접 적용
- Permutation invariant (점 순서 무관)
- Max pooling으로 global feature

**PointNet++ (2017)**:
- Hierarchical feature learning
- Set Abstraction: 영역별 특징 추출
- 로컬 패턴 학습 가능

```python
# PointNet++ 개념 구조
# 1. Sampling: FPS로 중심점 선택
# 2. Grouping: Ball query로 이웃 수집
# 3. PointNet: 각 그룹에서 특징 추출
```

PointNet의 출력은 포인트 클라우드의 점 순서가 바뀌어도 같아야 한다(permutation invariance). 이를 위해 각 점을 독립적으로 MLP에 통과시킨 뒤 max pooling으로 집계한다. 수학적으로 f({x1, ..., xn}) = g(MAX(h(x1), ..., h(xn))) 형태이다.

### 13.3.2 Voxel-based Methods

**VoxelNet (2018)**:
- 포인트 클라우드를 3D 복셀로 변환
- Voxel Feature Encoding
- 3D CNN으로 처리

**SECOND (Sparsely Embedded Convolutional Detection)**:
- Sparse Convolution 사용
- VoxelNet 대비 훨씬 빠름
- 널리 사용되는 베이스라인

**PointPillars (2019)**:
- Pillar (수직 기둥) 단위 처리
- 2D CNN으로 변환하여 빠른 속도
- 실시간 가능

PointPillars는 3D 공간을 수직 기둥(pillar)으로 나누고, 각 pillar 안의 점을 하나의 특징 벡터로 압축해 2D 이미지처럼 배열한다. 이렇게 하면 2D CNN을 그대로 활용할 수 있어 3D CNN보다 계산이 빠르다.

### 13.3.3 Multi-modal Methods

카메라가 풍부한 색상과 텍스처 정보를 제공하는 데 반해 정밀 깊이 정보를 직접 주지 못한다면, 라이다는 정확한 3차원 기하 좌표를 측정하되 표면 텍스처가 결여된다. 따라서 두 센서의 상보적 강점을 어떻게 유기적으로 결합할지가 핵심이다.

**BEVFusion**:
- Camera + LiDAR 융합
- 조감도(Bird's Eye View, BEV) 공간에서 통합

**TransFusion**:
- Transformer 기반 융합
- Query 기반 detection

> **추천 자료**
> - [Qi et al., "PointNet: Deep Learning on Point Sets" (2017)](https://arxiv.org/abs/1612.00593) — 3D 딥러닝의 시작점
> - [Lang et al., "PointPillars" (2019)](https://arxiv.org/abs/1812.05784) — 실시간 3D 감지
> - [Liu et al., "BEVFusion" (2023)](https://arxiv.org/abs/2205.13542) — 멀티모달 융합의 대표작
> - [MMDetection3D GitHub](https://github.com/open-mmlab/mmdetection3d) — 3D Object Detection 통합 프레임워크

> **실습**: [BEV Projection 시각화](https://alexjunholee.github.io/robotics-practice/app.html#bev_projection)
> 카메라 이미지를 BEV로 변환하는 과정을 인터랙티브하게 확인하며, BEV 기반 3D 감지의 원리를 이해할 수 있다.

## 13.4 3D Reconstruction

여러 뷰 또는 깊이 정보로부터 3D 모델을 생성한다. 로봇이 환경을 3D로 "기억"하려면 이 기술이 필요하다.

### 13.4.1 Structure from Motion (SfM)

여러 장의 2D 사진만으로 3D 구조를 복원할 수 있다. 스마트폰 사진 몇 장으로 건물의 3D 모델을 만드는 방식이며, NeRF나 3D Gaussian Splatting에 필요한 카메라 포즈를 만드는 전처리 단계이기도 하다.

여러 이미지에서 카메라 포즈와 3D 구조를 동시에 복원한다.

**파이프라인**:
1. 특징점 추출 및 매칭
2. 초기 두 뷰로 삼각측량
3. 점진적 카메라 추가
4. Bundle Adjustment (BA)

Bundle Adjustment는 모든 카메라 포즈와 3D 점 위치를 동시에 최적화한다. 비선형 최소자승법(Levenberg-Marquardt 등)을 쓰며, 변수는 수만~수십만 개가 될 수 있다. 선형대수의 최소자승법을 대규모 비선형 문제로 확장한 형태다.

**도구**:
- **COLMAP**: 널리 쓰이는 공개 SfM/MVS 참조 구현, GUI/CLI
- **OpenMVG**: 라이브러리 형태

```bash
# COLMAP 사용
colmap feature_extractor --database_path db.db --image_path ./images
colmap exhaustive_matcher --database_path db.db
mkdir -p sparse
colmap mapper --database_path db.db --image_path ./images --output_path ./sparse
```

> **추천 자료**
> - [COLMAP Documentation](https://colmap.github.io/) — 널리 쓰이는 공개 SfM/MVS 참조 구현
> - [Daniel Cremers — Multiple View Geometry (TUM)](https://www.youtube.com/playlist?list=PLTBdjV_4f-EJn6udZ34tht9EVIW7lbeo4) — 다중 뷰 기하학 핵심 강의
> - [Schönberger & Frahm, "Structure-from-Motion Revisited" (2016)](https://openaccess.thecvf.com/content_cvpr_2016/papers/Schonberger_Structure-From-Motion_Revisited_CVPR_2016_paper.pdf) — COLMAP 논문

### 13.4.2 Multi-View Stereo (MVS)

SfM 결과를 기반으로 dense 포인트 클라우드를 생성한다.

SfM이 "카메라가 어디 있었는지"와 "sparse한 3D 점들"을 복원한다면, MVS는 그 카메라 포즈를 이용해 조밀한(dense) 3D 포인트 클라우드를 만든다. SfM → MVS → Mesh 생성이 전형적인 3D 복원 파이프라인이다.

**도구**: COLMAP (dense reconstruction), OpenMVS

### 13.4.3 Volumetric Reconstruction

**TSDF (Truncated Signed Distance Function)**:
- 공간을 복셀로 나누고 각 복셀에 표면까지의 거리 저장
- 여러 뷰 통합
- Marching Cubes로 mesh 추출

TSDF는 각 복셀에 가장 가까운 표면까지의 부호 있는 거리(signed distance)를 저장한다. 양수는 표면 바깥, 음수는 표면 안쪽을 뜻한다. 여러 뷰에서 관측한 깊이를 가중 평균하면 노이즈가 줄어들고, 부호가 바뀌는 지점에서 표면을 추출할 수 있다.

```python
# Open3D TSDF Integration
volume = o3d.pipelines.integration.ScalableTSDFVolume(
    voxel_length=0.01,
    sdf_trunc=0.04,
    color_type=o3d.pipelines.integration.TSDFVolumeColorType.RGB8
)

for i, (color, depth, pose) in enumerate(frames):
    rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(
        color, depth, depth_trunc=4.0, convert_rgb_to_intensity=False)
    volume.integrate(rgbd, intrinsic, np.linalg.inv(pose))

mesh = volume.extract_triangle_mesh()
```

> **추천 자료**
> - [Open3D — TSDF Integration Tutorial](http://www.open3d.org/docs/release/tutorial/pipelines/rgbd_integration.html) — TSDF 실습 코드
> - [Curless & Levoy, "A Volumetric Method for Building Complex Models from Range Images" (1996)](https://graphics.stanford.edu/papers/volrange/volrange.pdf) — TSDF 원 논문 (고전이지만 읽어볼 가치가 있다)

## 13.5 Neural Rendering

딥러닝으로 3D 장면을 표현하고 렌더링하는 방식이다. 기존 방법(mesh, point cloud)은 반사·투명체·가는 구조가 있는 복잡한 장면을 표현하는 데 한계가 있었다. Neural Rendering은 장면을 학습 가능한 함수로 표현해 이런 효과를 다룬다. 최근에는 SLAM과 결합해 온라인 매핑에도 활용된다.

### 13.5.1 NeRF (Neural Radiance Fields)

**개념**: 3D 장면을 continuous 함수로 표현

```
F: (x, y, z, θ, φ) → (r, g, b, σ)
- 위치 (x, y, z)와 시점 방향 (θ, φ)
- 색상 (r, g, b)과 밀도 (σ) 출력
```

직관적으로 설명하면: NeRF는 "3D 공간의 모든 점에 대해, 어떤 방향에서 보면 어떤 색과 밀도를 가지는지"를 신경망으로 학습하는 것이다. 학습이 끝나면 어떤 카메라 위치에서든 새로운 뷰를 합성(novel view synthesis)할 수 있다.

**렌더링**: 광선을 따라 색상과 밀도를 적분 (volume rendering)

**장점**:
- 사실적인 novel view synthesis
- 반사, 투명 등 복잡한 효과 처리

**단점**:
- 학습 시간 오래 걸림
- 동적 장면 어려움

**발전**:
- Instant-NGP: 해시 인코딩으로 빠른 학습 (수 분)
- Mip-NeRF: 안티앨리어싱
- Block-NeRF: 대규모 장면

> **추천 자료**
> - [Mildenhall et al., "NeRF: Representing Scenes as Neural Radiance Fields" (2020)](https://arxiv.org/abs/2003.08934) — NeRF 원 논문
> - [NeRFStudio Documentation](https://docs.nerf.studio/) — NeRF 실험을 쉽게 할 수 있는 통합 프레임워크. NeRF를 직접 돌려보고 싶다면 여기서 시작하자.
> - [Yannic Kilcher — NeRF Explained](https://www.youtube.com/watch?v=CRlN-cYFxTk) — NeRF의 표현 방식을 직관적으로 설명
> - [Jon Barron — Understanding NeRF (ECCV 2022 Tutorial)](https://www.youtube.com/watch?v=HfJpQCBTqZs) — NeRF 저자 직강

### 13.5.2 3D Gaussian Splatting (3DGS)

3DGS는 NeRF에서 오래 걸리던 렌더링 시간을 크게 줄이면서 빠르게 채택됐다. NeRF는 한 프레임을 렌더링하는 데 수 초가 걸릴 수 있지만, 3DGS는 100 FPS 이상의 실시간 렌더링 결과를 보였다. 이 차이로 SLAM과 온라인 매핑 같은 로봇 응용에서도 활용할 수 있게 됐다.

**개념**: 장면을 수백만 개의 3D Gaussian으로 표현

각 Gaussian:
- 위치 (mean)
- 공분산 (모양/크기/방향)
- 색상 (Spherical Harmonics)
- 불투명도

선형대수에서 배운 공분산 행렬을 떠올려보자. 3×3 공분산 행렬의 고유벡터가 타원체의 축 방향을, 고유값이 축의 길이를 결정한다. 3DGS는 이 개념을 그대로 활용해서, 각 Gaussian의 모양과 크기를 표현한다.

**렌더링**: Gaussian을 이미지에 투영 (splatting)

**장점**:
- NeRF 대비 실시간 렌더링 (100+ FPS)
- 빠른 학습 (수 분)
- 명시적 표현 (편집 용이)

**응용**:
- SLAM: SplaTAM, Gaussian Splatting SLAM
- Mapping: 대규모 환경 표현
- Dynamic scenes: 동적 장면 확장

```python
# 3DGS 기본 개념 (pseudo-code)
# 각 Gaussian: position, covariance, color, opacity
# 렌더링: 카메라 뷰로 투영하여 이미지 생성
```

**3DGS + SLAM (최신 트렌드)**:

3D Gaussian Splatting이 SLAM과 결합되면서 Neural SLAM의 새로운 방향이 열리고 있다. 기존 SLAM이 sparse한 포인트 맵이나 복셀 맵을 만들었다면, 3DGS-SLAM은 포토리얼리스틱한 3D 맵을 만든다. 실시간에 해당하는 100 FPS급 수치는 학습이 끝난 장면의 렌더링 처리율이고, tracking과 mapping 최적화를 포함한 종단 처리율은 그보다 훨씬 낮게 보고된다.

- **SplaTAM (2024)**: RGB-D 카메라 입력으로 3DGS 기반 dense SLAM을 수행한다. Tracking(카메라 포즈 추정)과 Mapping(Gaussian 추가/업데이트)을 번갈아 수행하며, 기존 Neural SLAM 대비 렌더링 품질과 속도를 크게 높인다.
- **MonoGS (2024)**: 단안(monocular) 카메라만으로 3DGS 기반 SLAM을 수행한다. 깊이 센서 없이도 dense한 3D 맵을 구축할 수 있다.
- **Gaussian-SLAM (2024)**: 서브맵(sub-map) 기반으로 대규모 환경에서도 3DGS SLAM을 돌릴 수 있다.

로봇이 돌아다니면서 포토리얼리스틱한 3D 맵을 실시간으로 만들 수 있다면, 디지털 트윈이나 AR/VR 콘텐츠 생성 같은 응용이 열린다.

> **추천 자료**
> - [Kerbl et al., "3D Gaussian Splatting for Real-Time Radiance Field Rendering" (2023)](https://arxiv.org/abs/2308.04079) — 3DGS 원 논문
> - [Huang et al., "2D Gaussian Splatting for Geometrically Accurate Radiance Fields" (SIGGRAPH 2024, arXiv:2403.17888)](https://arxiv.org/abs/2403.17888) — 2D Gaussian으로 표면 복원 품질 향상
> - [Keetha et al., "SplaTAM: Splat, Track & Map 3D Gaussians for Dense RGB-D SLAM" (2024)](https://arxiv.org/abs/2312.02126) — 3DGS + SLAM의 대표작
> - [Matsuki et al., "Gaussian Splatting SLAM" (2024)](https://arxiv.org/abs/2312.06741) — MonoGS 논문
> - [Wang et al., "DUSt3R: Geometric 3D Vision Made Easy" (CVPR 2024, arXiv:2312.14132)](https://arxiv.org/abs/2312.14132) — 카메라 내부/외부 파라미터 없이 이미지 쌍에서 dense 3D를 복원한다.
> - [Leroy et al., "Grounding Image Matching in 3D with MASt3R" (ECCV 2024, arXiv:2406.09756)](https://arxiv.org/abs/2406.09756) — DUSt3R에 local feature matching 추가. 복원 + 정밀 대응점 동시 제공
> - [NeRFStudio Documentation](https://docs.nerf.studio/) — NeRF/3DGS 실험 통합 프레임워크
> - [3DGS Original Implementation (GitHub)](https://github.com/graphdeco-inria/gaussian-splatting) — 공식 코드

> **실습**: [3D Gaussian Splatting 시각화](https://alexjunholee.github.io/robotics-practice/app.html#gaussian_splatting)
> 3D Gaussian의 위치, 공분산, 색상을 조작하며 splatting 렌더링 과정을 인터랙티브하게 이해할 수 있다.

## 13.6 심화: Neural Implicit Representations

13.5에서 NeRF와 3DGS를 다뤘다. NeRF는 density field를 사용하여 volume rendering을 수행하지만, density에서 명확한 surface를 추출하기 어렵다는 한계가 있다. 로보틱스에서 물체를 잡거나 충돌을 판단하려면 정확한 surface가 필요하다. 여기서 부호 있는 거리 함수(Signed Distance Function, SDF) 기반 접근이 등장한다.

**SDF (Signed Distance Function)**

공간의 각 점 `x`에서 가장 가까운 표면까지 부호 있는 거리를 반환하는 함수다.

```
f(x) > 0  : 표면 바깥
f(x) < 0  : 표면 안쪽
f(x) = 0  : 표면 위 (zero level set)
```

SDF의 핵심 성질: 미분 가능한 점에서 gradient의 크기가 1이다 (Eikonal equation).

```
||∇f(x)|| = 1
```

이 조건을 만족하는 함수만이 올바른 거리 함수이다. Neural network로 SDF를 학습할 때 이 조건을 정규화 항(regularization)으로 추가하는데, 이를 **Eikonal loss**라 한다.

```
L_eikonal = E_x[ (||∇f_θ(x)|| - 1)^2 ]
```

**DeepSDF**

SDF를 신경망으로 학습하는 초기 대표 연구다. Decoder-only architecture를 사용하며, 각 물체의 형상을 latent code `z`로 표현한다.

```
f_θ(z, x) → SDF value
```

새로운 물체에 대해서는 test-time optimization으로 `z`를 추정한다.

**NeuS**

NeRF의 volume rendering 품질과 SDF의 깨끗한 surface를 결합한 연구다. SDF 값을 density로 변환하는 함수를 도입하여, volume rendering framework 안에서 SDF를 학습한다.

```
density ρ(t) = max(-dΦ_s(f(r(t)))/dt, 0) / Φ_s(f(r(t))),   r(t) = o + t·d 는 광선 위의 점
```

여기서 `Φ_s`는 learnable parameter `s`가 제어하는 시그모이드다. 그 도함수로 정의되는 S-density의 표준편차가 `1/s`이므로, 학습이 수렴하면서 `s`가 커지고 density가 surface 근처로 좁아진다.

**VolSDF**

유사한 접근이지만, density를 SDF의 Laplace 분포 CDF로 정의한다.

```
σ(x) = (1/β) · Ψ_β(-f(x))
```

`β`가 줄어들수록 density가 surface에 집중된다.

**Surface 추출**

학습된 SDF에서 `f(x) = 0`인 iso-surface를 mesh로 변환하는 표준 방법이 **Marching Cubes** 알고리즘이다. 공간을 격자로 나누고, 각 격자 꼭짓점에서 SDF 부호를 확인해 surface가 지나가는 위치를 보간으로 결정한다.

**비교표**

| 표현 | 장점 | 단점 | 예시 |
|------|------|------|------|
| NeRF (density) | 렌더링 품질 높음 | surface 추출 어려움 | Instant-NGP |
| SDF (neural) | 깨끗한 surface | 학습 어려움 | NeuS, VolSDF |
| 3DGS (explicit) | 실시간 렌더링 | 메모리 사용량 | Gaussian Splatting |
| Occupancy | 이진 분류로 단순 | 표면 디테일 한계 | ConvONet |

> **추천 자료**
> - [Wang et al., "NeuS: Learning Neural Implicit Surfaces by Volume Rendering" (NeurIPS 2021)](https://arxiv.org/abs/2106.10689) — NeuS 원 논문
> - [Yariv et al., "Volume Rendering of Neural Implicit Surfaces" (NeurIPS 2021)](https://arxiv.org/abs/2106.12052) — VolSDF 논문
> - [Park et al., "DeepSDF: Learning Continuous Signed Distance Functions for Shape Representation" (CVPR 2019)](https://arxiv.org/abs/1901.05103) — DeepSDF 원 논문
> - [Mescheder et al., "Occupancy Networks" (CVPR 2019)](https://arxiv.org/abs/1812.03828) — Occupancy 기반 접근의 대표작

## 13.7 심화: Differentiable Rendering

NeRF, 3DGS, NeuS는 렌더링 과정을 미분 가능하게 만들고, 렌더링 결과와 실제 이미지의 차이로 3D 표현을 최적화한다. 관측 이미지를 가장 잘 재현하는 장면 표현을 찾는 이 방식을 analysis-by-synthesis라 한다.

**Volume Rendering Equation**

NeRF 계열에서 사용하는 기본 렌더링 공식이다. 카메라에서 발사한 ray `r(t) = o + td` 위의 색상을 적분한다.

```
C(r) = ∫ T(t) · σ(t) · c(t) dt

where T(t) = exp( -∫_{t_n}^{t} σ(s) ds )
```

- `σ(t)`: 위치 `t`에서의 density (부피 밀도, 길이의 역수 차원)
- `c(t)`: 위치 `t`에서의 색상 (RGB)
- `T(t)`: 누적 투과도 (ray가 `t`까지 도달할 확률)

실제로는 이 연속 적분을 discretize하여 ray 위의 N개 샘플 점에서 근사한다 (ray marching).

```
C(r) ≈ Σ_i T_i · α_i · c_i
where α_i = 1 - exp(-σ_i · δ_i),  T_i = Π_{j<i} (1 - α_j)
```

**3DGS의 Rasterization 기반 렌더링**

3DGS는 위의 ray marching 대신 각 Gaussian을 이미지 평면에 투영(splatting)하는 rasterization 방식을 쓴다. 각 픽셀의 색상은 해당 픽셀에 영향을 주는 Gaussian들의 가중 합이다.

```
C(p) = Σ_i c_i · α_i · Π_{j<i} (1 - α_j)
```

수식 구조는 volume rendering과 유사하지만, ray를 따라 샘플링하는 대신 Gaussian을 정렬(depth sorting)하여 순서대로 합성한다. 이것이 GPU rasterization pipeline과 호환되어 실시간 렌더링이 가능한 이유이다.

**Differentiable Rasterization 라이브러리**

mesh 기반 3D 표현을 미분 가능하게 렌더링하는 도구들이다.

- **PyTorch3D** (Meta): differentiable mesh renderer, point cloud renderer 제공
- **nvdiffrast** (NVIDIA): CUDA 기반 고성능 differentiable rasterizer
- **Kaolin** (NVIDIA): 3D 딥러닝 전반을 위한 라이브러리

**Analysis-by-Synthesis 파이프라인**

```
1. 가설적 3D 장면을 설정 (NeRF, 3DGS, mesh 등)
2. 해당 장면을 카메라 시점에서 렌더링 → 예측 이미지
3. 실제 관측 이미지와 비교 → loss 계산
4. loss를 역전파하여 3D 장면 파라미터 업데이트
5. 반복
```

이 패러다임은 3D 표현 형태에 관계없이 "이미지"라는 공통 supervision을 쓸 수 있다. 별도의 3D ground truth가 필요 없다.

**SLAM과의 연결**

Differentiable rendering을 SLAM에 적용하면, tracking과 mapping을 모두 렌더링 loss로 최적화할 수 있다.

- **NeRF-SLAM**: NeRF를 map representation으로 사용. 새 프레임이 들어오면 현재 맵을 렌더링하여 포즈를 추정(tracking)하고, 맵을 업데이트(mapping).
- **3DGS-SLAM** (SplaTAM, MonoGS 등): 3DGS를 맵으로 사용. 실시간 렌더링이 가능하므로 NeRF-SLAM보다 빠르다.

```
[새 프레임] → [현재 맵 렌더링] → [렌더링 vs 실제 비교]
                                      ↓
                              [포즈 업데이트 (tracking)]
                              [맵 업데이트 (mapping)]
```

> **추천 자료**
> - [Tewari et al., "Advances in Neural Rendering" (EUROGRAPHICS 2022 STAR)](https://arxiv.org/abs/2111.05849) — Differentiable rendering 서베이
> - [Ravi et al., "Accelerating 3D Deep Learning with PyTorch3D" (2020)](https://arxiv.org/abs/2007.08501) — PyTorch3D 논문
> - [Laine et al., "Modular Primitives for High-Performance Differentiable Rendering" (2020)](https://arxiv.org/abs/2011.03277) — nvdiffrast 논문

## 13.8 심화: 3D Scene Graph

로봇에게 "주방에 있는 빨간 컵을 가져와"라고 명령하면, 포인트 클라우드나 mesh만으로는 이 명령을 수행하기 어렵다. "주방"이 어디인지, "빨간 컵"이 어떤 물체인지, 그것이 주방 "안에 있다"는 관계를 이해해야 한다. 3D Scene Graph는 환경을 기하학적 표현을 넘어 의미론적 관계 그래프로 표현하는 방법이다.

**구조**

- **노드(Node)**: 물체, 방, 건물 등 — 계층적(hierarchical) 구조
  - 건물 → 층 → 방 → 물체
  - 각 노드는 3D 위치, 바운딩 박스, 의미론적 라벨을 가짐
- **엣지(Edge)**: 노드 간 관계
  - "위에 있다" (on), "안에 있다" (in), "가까이 있다" (near), "지지한다" (support) 등

```
[Building]
  └── [Floor 1]
        ├── [Kitchen]
        │     ├── [Table]  ──(on)──  [Red Cup]
        │     ├── [Sink]
        │     └── [Chair]
        └── [Living Room]
              ├── [Sofa]
              └── [TV]
```

**Hydra**

MIT에서 개발한 실시간 3D scene graph 구축 시스템이다. RGB-D 또는 LiDAR 입력을 받아, 로봇이 이동하면서 계층적 scene graph를 점진적으로 구축한다.

파이프라인:
1. Metric-semantic mesh 구축 (TSDF + semantic segmentation)
2. 방(room) 단위 분할 (free-space clustering)
3. 물체 노드 추출 및 관계 설정
4. 계층 구조 연결

Hydra는 이 모든 과정을 실시간(online)으로 수행한다. 로봇이 탐색하면서 동시에 scene graph가 갱신된다.

**ConceptGraphs**

Foundation model(CLIP, LLM)을 활용하여 open-vocabulary scene graph를 구축하는 연구다.

기존 scene graph는 미리 정의된 카테고리(의자, 테이블 등)에 의존했다. ConceptGraphs는 CLIP으로 임의의 자연어 쿼리에 대응하는 물체를 찾고, LLM으로 물체 간 관계를 추론한다.

```
1. RGB-D 프레임에서 open-vocabulary detector로 물체 감지
2. CLIP feature로 물체 임베딩 추출
3. 3D 공간에서 동일 물체 병합 (multi-view association)
4. LLM으로 물체 간 관계 추론
5. Scene graph 구축
```

훈련 시 본 적 없는 "빨간 컵" 같은 쿼리도 처리할 수 있다.

**왜 필요한가?**

| 표현 | "주방의 빨간 컵을 가져와" 수행 가능? | 이유 |
|------|------|------|
| Point Cloud | 불가 | 의미 정보 없음 |
| Semantic Map | 부분적 | "컵"은 찾지만 "주방에 있는"이라는 관계 처리 어려움 |
| 3D Scene Graph | 가능 | 물체 + 관계 + 계층 모두 표현 |

task planning이나 자연어 기반 내비게이션에서 scene graph는 3D 표현과 고수준 추론을 연결하는 다리 역할을 한다.

> **추천 자료**
> - [Hughes et al., "Hydra: A Real-time Spatial Perception System for 3D Scene Graph Construction and Optimization" (RSS 2022)](https://arxiv.org/abs/2201.13360) — Hydra 원 논문
> - [Gu et al., "ConceptGraphs: Open-Vocabulary 3D Scene Graphs for Perception and Planning" (2023)](https://arxiv.org/abs/2309.16650) — ConceptGraphs 논문
> - [Rosinol et al., "3D Dynamic Scene Graphs: Actionable Spatial Perception with Places, Objects, and Humans" (RSS 2020)](https://arxiv.org/abs/2002.06289) — Dynamic Scene Graph 개념 제시
> - [Armeni et al., "3D Scene Graph: A Structure for Unified Semantics, 3D Space, and Camera" (ICCV 2019)](https://arxiv.org/abs/1910.02527) — 3D Scene Graph 초기 연구

> **기술 흐름: 3D Vision**
> - **~2010**: 포인트 클라우드 처리 고전기. PCL 라이브러리, ICP 정합, TSDF 기반 볼류메트릭 복원이 주류.
> - **2010~**: Kinect 출시(2010)와 KinectFusion(2011, TSDF + ICP)이 실시간 RGB-D 3D 복원의 문을 열고 대중화를 이끌었다.
> - **2017~**: PointNet/PointNet++로 포인트 클라우드 딥러닝이 시작되었다. VoxelNet, PointPillars 등 3D Object Detection 연구도 이 시기에 급증했다.
> - **2020~**: NeRF 등장으로 Neural Rendering이 주목받았다. 사진 몇 장으로 포토리얼리스틱한 3D 장면을 만들 수 있게 되었고, Instant-NGP, Mip-NeRF 등 후속 연구가 빠르게 이어졌다.
> - **2023~**: 3D Gaussian Splatting이 NeRF의 속도 한계를 극복했다. 실시간 렌더링과 명시적 표현의 장점을 동시에 갖추었고, BEVFusion 등 멀티모달 3D 감지가 자율주행에서 기준점이 되었다.
> - **2024~**: 3DGS + SLAM 결합(SplaTAM, MonoGS, Gaussian-SLAM)으로 Neural SLAM의 새 방향이 열리고 있다. 로봇이 이동하면서 포토리얼리스틱 3D 맵을 구축하는 방식이며, 렌더링은 실시간이지만 tracking·mapping을 포함한 종단 처리율은 아직 그에 못 미친다.
> - **최근 흐름**: SLAM과 로보틱스에서 3DGS 기반 장면 표현을 사용하는 연구가 늘고 있다. NeRFStudio에서는 NeRF와 3DGS의 표현과 렌더링 속도를 같은 데이터로 비교할 수 있다.

---

# Ch.14 — SLAM & Odometry

SLAM은 낯선 환경에서 로봇의 위치를 추정하는 동시에 주변 지도를 만드는 문제다. 실내나 지하처럼 GPS를 사용할 수 없는 공간에서 자율 이동의 기반이 된다.

---

## Part 1. 기초와 시스템

### 14.1 개념 소개

내비게이션과 경로 계획에는 로봇의 현재 위치와 주변 환경 정보가 필요하다. SLAM은 두 정보를 함께 추정한다.

**SLAM (Simultaneous Localization and Mapping)**:
자신의 위치를 추정하면서 동시에 주변 환경의 지도를 작성하는 문제이다.

닭과 달걀 문제:
- 지도가 있어야 위치를 알 수 있음
- 위치를 알아야 지도를 만들 수 있음
→ 동시에 해결

센서는 항상 노이즈가 있다. 바퀴가 미끄러지기도 하고, 카메라 이미지가 흔들리기도 한다. 이런 불확실성이 시간이 지날수록 누적되어 위치 추정이 점점 틀어진다(drift). SLAM의 핵심 과제는 이 drift를 보정하면서 일관된 지도를 만드는 것이다.

**Odometry vs SLAM**:
| 특징 | Odometry | SLAM |
|---|---|---|
| 출력 | 상대적 이동 | 위치 + 지도 |
| Loop Closure | 없음 | 있음 |
| Drift | 누적 | 보정 가능 |
| 계산량 | 적음 | 많음 |

> **추천 자료**
> - [Cyrill Stachniss — SLAM Course (University of Bonn)](https://www.youtube.com/playlist?list=PLgnQpQtFTOGQrZ4O5QzbIHgl3b1JHimN_) — Bayes filter부터 graph-based SLAM까지 이어지는 공개 강의 시리즈
> - [Thrun, Burgard, Fox, "Probabilistic Robotics" (Textbook)](https://mitpress.mit.edu/9780262201629/probabilistic-robotics/) — SLAM의 수학적 기반을 다루는 교과서. 칼만 필터, 파티클 필터, EKF-SLAM 등
> - [Barfoot, "State Estimation for Robotics" (Free PDF)](http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17.pdf) — 상태 추정의 수학을 깊이 있게 다루는 교재. 무료 PDF 제공
> - [Awesome-SLAM GitHub](https://github.com/SilenceOverflow/Awesome-SLAM) — SLAM 관련 논문, 라이브러리, 데이터셋을 모아놓은 목록
> - [정진용 블로그 — SLAM 강의 시리즈 (Freiburg Robot Mapping 기반)](https://jinyongjeong.github.io/2017/02/13/lec01_SLAM_bayes_filter/) — Bayes filter부터 EKF/UKF/Particle filter, Graph SLAM, Robust SLAM까지 잇는 15편의 한국어 시리즈
> - [김기섭 블로그 — SLAM Back-end 공부자료 5개 추천](https://gisbi-kim.github.io/blog/2021/10/03/slam-textbooks.html) — Error-state KF, Factor Graphs, Bundle Adjustment 등 핵심 자료 큐레이션
> - [Robot Mapping Course (Uni Freiburg, Cyrill Stachniss)](http://ais.informatik.uni-freiburg.de/teaching/ws13/mapping/) — SLAM 강의 슬라이드와 과제 자료. 영상과 함께 보면 좋다
> - [EKF-SLAM 슬라이드 (Freiburg)](http://ais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/slam04-ekf-slam.pdf) — 위 강의 중 EKF-SLAM 파트. 수식 전개가 깔끔하게 정리되어 있다

> **실습**: [SE(2) Odometry](https://alexjunholee.github.io/robotics-practice/app.html#se2_odometry)
> 2D 평면에서의 odometry 누적 과정을 직접 조작하며, drift가 어떻게 발생하는지 확인할 수 있다.

### 14.2 Visual Odometry (VO)

카메라만으로 상대적 이동을 추정한다. SLAM의 "front-end"에 해당하며, 이 단계의 이동 추정이 부정확하면 SLAM 전체의 추정 정확도가 떨어진다.

#### 14.2.1 Feature-based vs Direct Method

두 방식은 장단점이 뚜렷하다. 운용 환경에 따라 선택이 달라진다.

**Feature-based** (ORB-SLAM 계열)는 이미지에서 변하지 않는 특징적인 점(코너, 블롭 등)을 추출하고, 프레임 간 매칭으로 카메라 움직임을 역추정한다. 선형대수적으로는 두 단계로 갈린다. 맵이 없는 초기화에서는 2D-2D 대응으로 Essential 또는 Fundamental Matrix(평면에서는 Homography)를 구하고, 맵이 생긴 뒤의 추적에서는 3D 맵 포인트와 2D 관측의 재투영 오차를 최소화한다. 조명 변화에 강건하고 방법론이 검증되어 있지만, 흰 벽·텍스처 없는 바닥처럼 특징점을 뽑기 어려운 환경에서는 한계가 있다.

```
이미지 → 특징점 추출 → 매칭 → 움직임 추정
```

**Direct Method** (DSO, LSD-SLAM 계열)는 픽셀 밝기를 직접 비교한다. "연속 프레임에서 같은 3D 점을 관측하면 밝기가 같아야 한다"는 가정(brightness constancy)을 이용하므로, 특징점을 뽑을 필요가 없어 텍스처가 적은 환경에서도 작동할 수 있다. 대신 조명 변화에 민감하다.

```
이미지 → 픽셀 밝기 직접 비교 → 움직임 추정
```

#### 14.2.2 Mono vs Stereo vs RGB-D

각 구성의 트레이드오프를 알아야 실제 로봇에 맞는 센서를 선택할 수 있다.

| 구성 | Scale | 특징 | 적합 환경 |
|---|---|---|---|
| **Monocular** | 불가 (ambiguity) | 경량·단순, IMU 없이 스케일 복원 불가 | 저비용 드론, 모바일 |
| **Stereo** | 가능 | 베이스라인이 측정 범위를 제한 | 일반 실내·실외 |
| **RGB-D** | 가능 | 깊이 직접 측정, 실외·직사광선에 취약 | 실내 구조화 환경 |

Scale ambiguity는 단안 카메라가 "가까이 있는 작은 물체"와 "멀리 있는 큰 물체"를 구분하지 못하는 데서 생긴다. 모노 SLAM의 지도는 임의의 스케일로 나오므로 IMU나 다른 센서로 복원해야 한다. 초기화에 충분한 이동이 필요한 이유는 여기서 갈라진다. 단안 구조 초기화가 요구하는 것은 parallax를 만드는 translation이며 순수 회전에서는 two-view 기하가 퇴화한다. scale과 IMU bias, 중력 방향의 관측 가능성을 위해 운동 여기가 필요한 것은 visual-inertial 초기화 쪽이다. 순수 단안에서는 이동을 늘려도 절대 scale이 관측 가능해지지 않는다.

> **추천 자료**
> - [Daniel Cremers — Multiple View Geometry (TUM)](https://www.youtube.com/playlist?list=PLTBdjV_4f-EJn6udZ34tht9EVIW7lbeo4) — Visual Odometry에 필요한 다중 뷰 기하를 다루는 공개 강의
> - [EuRoC MAV Dataset](https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets) — Visual(-Inertial) Odometry 벤치마크 데이터셋
> - [TUM RGB-D Benchmark](https://cvg.cit.tum.de/data/datasets/rgbd-dataset) — RGB-D SLAM/VO에서 널리 쓰이는 실내 데이터셋과 평가 도구

### 14.3 Visual SLAM

#### 14.3.1 ORB-SLAM2/3

ORB-SLAM 계열은 Visual SLAM 논문에서 자주 쓰이는 공개 baseline 중 하나다. 코드가 공개되어 있어 직접 빌드하고 입력 조건과 실패 사례를 확인할 수 있다.

**구성**:
1. **Tracking**: 현재 프레임에서 포즈 추정
2. **Local Mapping**: 키프레임 기반 지역 지도 관리
3. **Loop Closing**: 루프 감지 및 전역 최적화

이 세 스레드 구조가 ORB-SLAM의 핵심 설계다. Tracking은 매 프레임 실시간으로, Local Mapping은 키프레임이 들어올 때, Loop Closing은 루프가 감지될 때 동작한다. 각기 다른 주기로 병렬 실행하므로 실시간 성능을 유지하면서 전역 일관성을 확보할 수 있다.

**ORB-SLAM3 특징**:
- Visual-Inertial 모드 지원
- 멀티맵 지원
- Fish-eye 카메라 지원

ORB-SLAM의 역사적 맥락:
- **MonoSLAM (2007)**: 실시간 단안 SLAM을 대표한 초기 시스템. EKF 기반으로 작동했으나, 맵 크기가 커지면 계산량이 급증하는 한계가 있었다.
- **PTAM (Parallel Tracking and Mapping, 2007)**: Tracking과 Mapping을 병렬 thread로 분리한 영향력 있는 초기 시스템. 이 아키텍처가 이후 ORB-SLAM에 큰 영향을 미쳤다.
- **ORB-SLAM (2015)**: PTAM의 설계를 계승하면서 ORB 특징점, Loop Closure, 재위치 추정(relocalization)을 추가한 완전한 SLAM 시스템.
- **ORB-SLAM2 (2017)**: Stereo, RGB-D 지원 추가.
- **ORB-SLAM3 (2021)**: Visual-Inertial, 멀티맵 등 추가.

```bash
# ORB-SLAM3 실행 예시
./Examples/Monocular/mono_euroc \
    Vocabulary/ORBvoc.txt \
    Examples/Monocular/EuRoC.yaml \
    ~/Datasets/EuRoC/MH01
```

> **추천 자료**
> - [Campos et al., "ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM" (2021)](https://arxiv.org/abs/2007.11898) — ORB-SLAM3 논문
> - [ORB-SLAM3 GitHub](https://github.com/UZ-SLAMLab/ORB_SLAM3) — 공식 코드
> - [EuRoC MAV Dataset](https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets) — ORB-SLAM3 테스트용 표준 데이터셋
> - [정진용 블로그 — Visual SLAM 비교 실험 (KAIST Urban Dataset)](https://jinyongjeong.github.io/2019/10/22/visual_slam_compare/) — ORB-SLAM2 vs VINS-Fusion 실전 비교. 실제 데이터셋에서의 성능 차이 분석

#### 14.3.2 DSO (Direct Sparse Odometry)

**Direct Method** + **Sparse Points**

Direct Method는 대개 dense(모든 픽셀)하게 쓰이고 Sparse는 Feature-based에서 쓰이는 방식이지만, DSO는 "Direct이면서 Sparse"인 조합을 사용한다. 선별한 고품질 점만 사용하면서 광도(photometric) 오차를 최소화한다.

- 특징점 추출 없이 픽셀 밝기 직접 사용
- 선별된 점들만 사용 (Sparse)
- Photometric bundle adjustment

> **추천 자료**
> - [Engel et al., "Direct Sparse Odometry" (2018)](https://arxiv.org/abs/1607.02565) — DSO 논문

#### 14.3.3 VINS-Mono/Fusion

단안 카메라 추적은 빠른 움직임이나 텍스처가 부족한 환경에서 실패할 수 있다. IMU는 영상 프레임 사이의 고주파 운동 제약을 보완한다. VINS-Mono는 카메라와 IMU를 결합한 드론·모바일 로봇용 Visual-Inertial SLAM 시스템이다.

**Visual-Inertial Navigation System**

- Camera + IMU tight coupling
- Sliding window optimization
- Loop closure 지원
- 모바일/드론에서 널리 사용

```
센서 입력 → IMU Preintegration →
Visual Feature Tracking →
Sliding Window Optimization →
Loop Closure (optional)
```

VINS-Mono는 IMU preintegration을 사용해 두 키프레임 사이의 여러 IMU 측정을 하나의 상대 운동 제약으로 묶는다. 최적화에서는 각 원시 측정을 다시 적분하는 대신 이 제약을 사용한다.

> **추천 자료**
> - [Qin et al., "VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator" (2018)](https://arxiv.org/abs/1708.03852) — VINS-Mono 논문
> - [VINS-Mono GitHub](https://github.com/HKUST-Aerial-Robotics/VINS-Mono) — 공식 코드, ROS 지원

### 14.4 LiDAR Odometry & SLAM

카메라 기반 방법은 조명과 텍스처의 영향을 받는다. LiDAR는 3D 거리를 직접 측정하므로 영상의 밝기나 텍스처에 같은 방식으로 의존하지 않는다. 이런 차이 때문에 자율주행과 실외 로봇에서 LiDAR SLAM을 쓴다.

#### 14.4.1 LOAM (Lidar Odometry and Mapping)

LOAM의 edge·planar feature 분리와 odometry-mapping 이중 주기 구조는 LeGO-LOAM, LIO-SAM 등 후속 LiDAR SLAM에 영향을 주었다.

- Edge points와 Planar points 분류
- Point-to-edge, point-to-plane 거리 최소화
- Odometry와 Mapping 분리 (주파수 다르게)

포인트 클라우드에서 모서리와 평면에 해당하는 점을 추려 사용한다. 모든 점을 매칭하는 대신 edge·planar 점으로 제약을 구성해 계산량을 줄인다.

#### 14.4.2 LeGO-LOAM

**Lightweight and Ground-Optimized LOAM**:
- 지면 분리로 계산량 감소
- 지면을 기반으로 초기 추정
- 모바일 로봇에 적합

#### 14.4.3 LIO-SAM

LIO-SAM은 Factor Graph 기반 최적화를 LiDAR-Inertial SLAM에 적용한 대표적인 방법이다. Factor Graph에서는 새 센서의 측정을 factor로 추가할 수 있어 시스템을 확장하기 쉽다.

**LiDAR-Inertial Odometry via Smoothing and Mapping**:
- Factor graph 기반
- Tight IMU-LiDAR coupling
- GPS, Loop closure 통합

```
                    ┌──────────────┐
IMU ──────────────→ │              │
                    │ Factor Graph │ ──→ Pose
LiDAR ────────────→ │              │
                    │  iSAM2       │
GPS (optional) ───→ │              │
                    └──────────────┘
```

Factor Graph가 뭐냐면: 변수(로봇 포즈, 랜드마크 위치)와 제약(센서 측정)의 관계를 그래프로 표현하는 것이다. IMU 측정이 factor 하나, LiDAR 매칭이 factor 하나, GPS가 factor 하나, Loop Closure가 factor 하나... 이런 식으로 센서를 추가하려면 해당 factor만 추가하면 된다. GTSAM 라이브러리가 이 최적화를 효율적으로 수행한다.

> **추천 자료**
> - [Shan et al., "LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping" (2020)](https://arxiv.org/abs/2007.00258) — LIO-SAM 논문
> - [Vizzo et al., "KISS-ICP: In Defense of Point-to-Point ICP" (RA-L 2023, arXiv:2209.15397)](https://arxiv.org/abs/2209.15397) — 잘 만든 vanilla ICP가 복잡한 LiDAR odometry와 동등한 성능. 단순함의 힘
> - [LIO-SAM GitHub](https://github.com/TixiaoShan/LIO-SAM) — 공식 코드, ROS 지원
> - [GTSAM Documentation](https://gtsam.org/) — Factor Graph 최적화 라이브러리. LIO-SAM 등의 SLAM 시스템의 백엔드로 사용된다
> - [Frank Dellaert — Factor Graphs for Perception and Action (MIT Robotics)](https://www.youtube.com/watch?v=-yCC7mpgL4w) — GTSAM 개발자가 직접 설명하는 Factor Graph
> - [김기섭 블로그 — Scan Context-based LiDAR Pose-graph SLAM 구현](https://gisbi-kim.github.io/blog/2021/05/17/sclidarslam.html) — Scan Context를 LiDAR SLAM에 통합한 구현 해설

#### 14.4.4 FAST-LIO / FAST-LIO2

**Fast LiDAR-Inertial Odometry**:
- Iterated Kalman Filter 기반 (Gauss-Newton과 동등한 반복 갱신)
- ikd-Tree: 동적 KD-트리로 빠른 매핑
- 실시간 성능

FAST-LIO가 고속으로 동작하는 이유: LIO-SAM이 비선형 최소자승 기반의 Factor Graph 최적화를 채택했다면, FAST-LIO는 반복 확장 칼만 필터(Iterated EKF, IEKF)를 적용한다. IEKF의 반복 갱신은 MAP 목적함수에 대한 Gauss-Newton과 동등하므로 최적화를 안 푸는 것이 아니다. 속도의 실제 근거는 Kalman gain 계산을 측정 차원이 아니라 상태 차원에 의존하도록 바꾼 정식화이며, 상태를 슬라이딩 윈도로 쌓지 않는 구조도 함께 작용한다. 후속작 FAST-LIO2는 여기에 증분적 KD-트리 ikd-Tree를 더해 맵에 새 점을 추가하는 비용도 줄였다.

> **추천 자료**
> - [Xu & Zhang, "FAST-LIO: A Fast, Robust LiDAR-Inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter" (2021)](https://arxiv.org/abs/2010.08196) — FAST-LIO 논문
> - [Xu et al., "FAST-LIO2: Fast Direct LiDAR-Inertial Odometry" (2022)](https://arxiv.org/abs/2107.06829) — FAST-LIO2 논문
> - [FAST-LIO2 GitHub](https://github.com/hku-mars/FAST_LIO) — 공식 코드

### 14.5 Multi-sensor Fusion

단일 센서로 모든 상황을 커버하기는 어렵다. 카메라는 어두우면 안 되고, LiDAR는 비가 오면 힘들고, IMU만으로는 drift가 커진다. 센서를 결합(fusion)하면 각 센서의 약점을 다른 센서가 보완한다.

#### 14.5.1 Camera + IMU (VIO)

Visual과 Inertial을 결합하는 방식에는 두 가지 전략이 있다. **Loosely-coupled**는 카메라와 IMU가 각자 상태를 추정한 뒤 결과를 covariance 기반으로 합친다. 구현이 단순하지만 정보를 충분히 활용하지 못한다. **Tightly-coupled**는 카메라 특징점의 재투영 오차와 IMU 측정이 하나의 공유 상태를 직접 제약하는 결합 수준을 말한다. 그 상태를 어떻게 푸는지는 별개다. VINS-Mono는 단일 비용 함수를 최적화하고, MSCKF는 sliding window EKF에 feature null-space marginalization을 쓰는 필터다. 더 정확하지만 구현이 복잡하다.

**IMU Preintegration**:
두 키프레임 사이의 IMU 측정을 사전 적분하여 상대 변환 계산. 최적화 반복마다 원시 측정을 다시 적분하지 않아도 된다.

#### 14.5.2 LiDAR + IMU (LIO)

회전형 LiDAR와 IMU의 rate는 제품마다 다르지만 LiDAR scan 주기 안에 platform이 움직이면 motion distortion이 생긴다. LIO는 더 높은 rate의 IMU와 timestamp를 이용해 scan 안의 움직임을 보정(de-skewing)하고 LiDAR 관측과 함께 상태를 추정한다. 보정의 이득은 동기화, IMU bias, motion과 scan pattern에 달려 있다.

#### 14.5.3 Camera + LiDAR + IMU

**최신 트렌드**: 모든 센서 통합
- 예시: R3LIVE, LVI-SAM
- 각 센서의 장점 활용

R3LIVE는 LiDAR(기하 정보) + Camera(텍스처/색상 정보) + IMU(고속 움직임 보상)를 모두 결합한다. 정확한 위치 추정뿐 아니라 색상이 입혀진(colored) 고밀도 3D 맵까지 실시간으로 생성할 수 있다.

> **추천 자료**
> - [KITTI Odometry Benchmark](https://www.cvlibs.net/datasets/kitti/eval_odometry.php) — LiDAR/Visual Odometry 벤치마크의 표준
> - [EuRoC MAV Dataset](https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets) — VIO 벤치마크 데이터셋
> - [Lin & Zhang, "R3LIVE: A Robust, Real-time, RGB-colored, LiDAR-Inertial-Visual tightly-coupled state Estimation and mapping package" (2022)](https://arxiv.org/abs/2109.07982) — 3센서 융합의 대표작
> - [김기섭 블로그 — Filter-based VIO: MSCKF 계열 history 정리](https://gisbi-kim.github.io/blog/2021/04/27/msckf-history.html) — MSCKF 원본부터 stereo 확장까지 계보 정리

### 14.6 Loop Closure & Global Optimization

SLAM을 오래 실행하면 지도가 점차 뒤틀린다. 로봇이 큰 원을 그리며 출발점으로 돌아와도 지도에서는 시작점과 끝점이 어긋날 수 있다. Loop Closure는 이전에 방문한 장소를 다시 인식해 이 오차를 교정한다. 대규모 환경에서 누적 drift를 억제하려면 이 과정이 필요하다.

#### 14.6.1 Place Recognition

이전에 방문한 장소를 인식하여 drift를 보정한다.

같은 장소라도 시간, 조명, 계절이 바뀌면 아예 다르게 보인다. 비슷하게 생긴 서로 다른 장소를 동일 장소로 오인(false positive)할 경우 지도 전역에 심각한 왜곡이 유발된다. Place Recognition의 정밀도(precision)가 매우 높아야 하는 이유다.

**Bag of Words (BoW)**: visual vocabulary를 기반으로 이미지 간 유사도를 계산한다. DBoW2 라이브러리가 대표적이며 ORB-SLAM에서 사용된다. 빠르고 검증되어 있지만 조명·시점 변화에 취약하다.

**NetVLAD**: 딥러닝 기반 end-to-end 학습으로 조명·날씨 변화에 강건한 글로벌 디스크립터를 생성한다. (14.14절 참조)

**LiDAR Place Recognition**: Scan Context는 3D 포인트 클라우드를 조감도(bird's-eye view) 형태의 2D 기술자로 압축 투영한다. 이와 달리 PointNetVLAD는 신경망 구조를 통해 원시 포인트 집합으로부터 직접 전역 임베딩을 학습한다.

#### 14.6.2 Pose Graph Optimization

루프가 감지되면 전체 경로를 보정한다.

```
노드: 로봇 포즈
에지: 상대 변환 (odometry, loop closure)

목표: 모든 에지 제약을 만족하는 노드 위치 찾기
```

직관적으로 설명하면: Odometry가 만든 경로는 "각 구간은 대충 맞지만, 전체적으로는 뒤틀린" 상태이다. Loop Closure가 "이 위치와 저 위치가 같은 곳이다"라는 제약을 추가하면, Pose Graph Optimization이 "모든 제약을 최대한 만족하도록" 전체 경로를 부드럽게 조정한다. 이것은 비선형 최소자승법 문제이다.

주요 도구로는 경량 pose graph/BA 전용인 **g2o** (ORB-SLAM), factor graph와 iSAM2 기반의 **GTSAM** (LIO-SAM), Google이 개발한 범용 비선형 최소자승 라이브러리 **Ceres Solver**가 있다. 선택 기준은 14.9.4절의 비교표를 참고하라.

> **추천 자료**
> - [GTSAM Documentation & Tutorials](https://gtsam.org/) — Factor Graph 기반 최적화 라이브러리. Pose Graph Optimization 예제 포함
> - [Cyrill Stachniss — Graph-based SLAM](https://www.youtube.com/watch?v=uHbRKvD8TWg) — Pose Graph Optimization의 직관적 설명
> - [g2o GitHub](https://github.com/RainerKuemmerle/g2o) — Graph Optimization 프레임워크
> - [정진용 블로그 — Robust Graph SLAM](https://jinyongjeong.github.io/2017/03/04/lec15_Robust_Graph_SLAM/) — M-estimator, Max-mixture, DCS 등 robust SLAM 기법 한글 해설

> **실습**: [Pose Graph Optimization](https://alexjunholee.github.io/robotics-practice/app.html#pose_graph)
> Pose Graph의 노드(포즈)와 에지(제약)를 조작하고, loop closure 추가 시 전체 경로가 어떻게 보정되는지 확인할 수 있다.

### 14.7 Localization

사전 지도 기반으로 현재 위치를 추정한다. SLAM이 "지도를 만들면서 위치를 추정"하는 것이라면, Localization은 "이미 만들어진 지도에서 위치만 추정"하는 것이다. 실제 현장의 서비스 로봇은 사전에 오프라인 SLAM으로 정밀 지도를 구축해 둔 뒤, 일상 운용 단계에서는 고정 지도 기반의 Localization에 집중하는 경우가 일반적이다.

MCL 알고리즘의 *원리·유도*는 §3.11 비모수 필터 (Ch.3 참조). EKF의 수학적 기반은 §3.10 (Ch.3 참조), IMU와 결합한 위치추정 확장은 §14.10 참조. 아래에서는 위치추정 시나리오 분류와 알고리즘 변형을 본다.

Map-based Localization은 미리 만들어진 지도를 사용하므로 SLAM보다 계산이 가볍지만, 환경이 바뀌면 지도 업데이트가 필요하다.

**Monte Carlo Localization (MCL)**:
- 파티클 필터 기반
- 2D LiDAR + 점유 격자 지도
- ROS AMCL 패키지

MCL의 직관: 수천 개의 "가상 로봇(파티클)"을 지도 위에 뿌린다. 각 파티클은 "나는 여기에 이런 방향으로 있다"라는 가설이다. 실제 센서 측정과 비교해서, 측정과 잘 맞는 파티클은 살아남고 안 맞는 파티클은 사라진다. 시간이 지나면 파티클들이 실제 위치 주변에 모이게 된다.

**LiDAR Localization**: 포인트 클라우드 맵에 ICP 또는 NDT 매칭으로 정밀하게 위치를 추정한다.

> **추천 자료**
> - [Cyrill Stachniss — Monte Carlo Localization](https://www.youtube.com/watch?v=MsYlueVDLI0) — MCL/파티클 필터의 직관적 설명
> - [ROS Navigation Stack — AMCL](http://wiki.ros.org/amcl) — ROS에서 MCL 사용하기

> **실습**: [Particle Filter](https://alexjunholee.github.io/robotics-practice/app.html#particle_filter)
> 파티클 필터 기반 로봇 위치 추정 과정을 시각화하며, 파티클의 수렴 과정을 인터랙티브하게 확인할 수 있다.

> **실습**: [Occupancy Grid](https://alexjunholee.github.io/robotics-practice/app.html#occupancy_grid)
> 2D 점유 격자 지도를 구축하는 과정을 시각화하며, 센서 측정이 어떻게 확률적 지도로 변환되는지 확인할 수 있다.

#### 14.7.1 위치추정 문제 분류

위치추정의 난이도는 단일 수치로 표현되지 않는다. 네 축이 교차하며 알고리즘 선택을 결정한다.

| 축 | 옵션 | 비고 |
|---|---|---|
| 사전지식 | position tracking → global localization → kidnapped robot | 난이도 상승 |
| 환경 | static (로봇만 이동) → dynamic (사람·문·조명) | 동적일수록 어려움 |
| 능동성 | passive (관찰만) → active (탐색 행동 선택) | active가 더 빠른 수렴 |
| 로봇 수 | single → multi (상호 관측으로 belief 공유) | multi는 정보 풍부 |

대표 시나리오는 position tracking과 global localization, 여기에 kidnapped robot이 확장으로 더해진다.

**Position tracking**: 초기 자세가 알려져 있고, belief가 좁은 단봉 Gaussian으로 유지된다. EKF Localization이 적합하다.

**Global localization**: 초기 자세를 모른다. 균등 분포에서 시작하여 측정이 쌓이면서 belief가 수렴해야 한다. 다봉(multi-modal) belief 표현이 필요하므로 Grid Localization 또는 MCL이 적합하다.

**Kidnapped robot**: 운용 중 로봇이 강제로 다른 위치로 옮겨진다. 로봇이 그 사실을 스스로 알아채지 못한다는 점에서 global localization보다 어렵다. 어떤 알고리즘도 언젠가 이 상황을 만나므로, 복구 능력 자체가 로봇 자율성의 척도가 된다.

ROS Nav2의 `recovery_alpha_slow/fast` 파라미터는 kidnapped 시나리오 대비 설계다. warehouse AGV·청소로봇의 부팅은 global localization에, 운용 중은 tracking에 해당한다.

#### 14.7.2 Markov Localization

Markov localization은 알고리즘이 아니라 **베이즈 필터를 위치추정 문제에 그대로 적용한 것**의 이름이다. EKF Localization·Grid Localization·MCL은 모두 이 베이즈 필터의 belief 표현 방식에서 갈라진다.

베이즈 필터(Ch.3 §3.9)와의 차이는 단 하나다: 운동 모델과 관측 모델에 **지도 m**이 추가 입력으로 들어간다.

```
Markov_localization(bel(x_{t-1}), u_t, z_t, m):
  for all x_t do
    bel̄(x_t) = ∫ p(x_t | u_t, x_{t-1}, m) bel(x_{t-1}) dx_{t-1}   // motion update
    bel(x_t) = η p(z_t | x_t, m) bel̄(x_t)                          // measurement update
  endfor
  return bel(x_t)
```

초기 belief bel(x_0)은 시나리오마다 다르게 초기화한다:
- Position tracking: $\text{bel}(x_0) = \mathcal{N}(x_0;\, \bar{x}_0, \Sigma)$ — 좁은 Gaussian
- Global localization: $\text{bel}(x_0) = 1/|X|$ — 모든 합법 자세에 균등
- Partial knowledge: 알려진 구역 근방에만 균등, 그 외 0

§14.7.3~§14.7.7의 알고리즘은 "위 박스의 bel 표현을 무엇으로 구현하는가"의 변주다.

#### 14.7.3 EKF Localization

EKF Localization은 Markov localization의 특수 케이스로 belief를 $(\mu_t, \Sigma_t)$ 가우시안으로 표현한다. **단봉(unimodal) 가정 → position tracking 전용**이다. Global localization과 kidnapped 문제는 다봉 belief를 요구하므로 EKF로 풀 수 없다.

§3.10.2의 EKF를 위치추정에 적용한 것이므로 (Ch.3 참조), 여기서는 feature 기반 지도 + 알려진 랜드마크 대응이라는 가정 구조와 구체 알고리즘을 본다.

**가정**: 지도 m이 feature 기반 (점 랜드마크 집합). 각 측정 $z_t^i = (r, \phi, s)^T$ (range, bearing, signature). Correspondence $c_t^i$는 알려짐 (ARTag·QR 코드·Eiffel Tower 같은 식별 가능 랜드마크).

```
EKF_localization_known_correspondences(μ_{t-1}, Σ_{t-1}, u_t, z_t, c_t, m):
  // Motion update (velocity model 선형화)
  μ̄_t = μ_{t-1} + [velocity model 변위]
  G_t = ∂g/∂x |_{μ_{t-1}, u_t}         // 3×3 Jacobian
  Σ̄_t = G_t Σ_{t-1} G_t^T + R_t

// Measurement update (랜드마크별 순차 갱신)
  μ_t = μ̄_t
  Σ_t = Σ̄_t
  for each observed z_t^i = (r, φ, s)^T do
    j = c_t^i
    δ = (m_{j,x} − μ_{t,x},  m_{j,y} − μ_{t,y})^T,   q = δ^T δ
    ẑ_t^i = (√q,  atan2(δ_y, δ_x) − μ_{t,θ},  m_{j,s})^T
    H_t^i = Jacobian (3×3, 마지막 행은 0 — signature는 pose와 무관)
    K_t^i = Σ_t H_t^{i,T} (H_t^i Σ_t H_t^{i,T} + Q_t)^{-1}
    μ_t = μ_t + K_t^i (z_t^i − ẑ_t^i)
    Σ_t = (I − K_t^i H_t^i) Σ_t
  endfor
  return μ_t, Σ_t
```

**조건부 독립 가정** $p(z_t | x_t, m) = \prod_i p(z_t^i | x_t, m)$ 아래에서는 측정을 쌓아 한 번에 갱신하거나 순차적으로 conditioning할 수 있다. 위 코드는 각 측정 뒤의 $(\mu_t, \Sigma_t)$를 다음 측정에 사용하는 순차형이다. 비선형 모델에서는 재선형화 여부에 따라 stacked update와 작은 차이가 날 수 있다.

실용적 한계 — *Probabilistic Robotics*의 예시는 heading 불확실도가 약 ±20°를 넘는 경우를 선형화가 위험해지는 경험적 구간으로 든다. 이는 보편 임계값이 아니다. 관측 기하, motion, noise에 따라 NIS·NEES나 Monte Carlo consistency를 확인해야 한다. EKF localization 구조는 식별 가능한 ARTag·AprilTag landmark나 GNSS+IMU fusion처럼 belief가 단봉으로 유지되는 문제에 계속 적용할 수 있다.

**미지 대응(unknown correspondences)**: 실전에서 $c_t^i$는 보통 모른다. ML(maximum likelihood) data association은 마할라노비스 거리가 최소인 지도 랜드마크를 선택하는 방법이다.

$$j(i) = \arg\min_k (z_t^i - \hat{z}_t^k)^T \Psi_k^{-1} (z_t^i - \hat{z}_t^k), \quad \Psi_k = H_t^k \bar\Sigma_t H_t^{k,T} + Q_t$$

마할라노비스 거리 최소화는 공분산 determinant와 prior가 후보마다 같다는 조건에서 Gaussian log-likelihood 최대화와 대응한다. 실전에서는 (1) 측정 차원에 맞는 $\chi^2$ gate, (2) 한 프레임의 여러 측정에 대한 one-to-one assignment 같은 제약을 더한다. ORB-SLAM의 descriptor matching과 geometric verification도 후보 생성 뒤 outlier를 거른다는 점에서는 비교할 수 있지만, 이 EKF의 ML association을 그대로 구현한 것은 아니다.

#### 14.7.4 Multi-Hypothesis Tracking (MHT)

EKF는 단봉 Gaussian이라 데이터 연관 모호성을 표현하지 못한다. MHT는 belief를 **가우시안 혼합(Gaussian mixture)**으로 표현하여 여러 가설을 동시에 유지한다.

각 가설 $h$는 독립적인 EKF를 구동한다. 측정이 들어오면 각 가설을 확장하고, 가중치(사후확률)가 임계값 $\psi_{\min}$ 아래로 떨어진 가설은 가지치기한다. 가설 수가 폭발하는 것을 막으려면 가지치기 정책이 필수다.

자율주행 multi-object tracking(MOT)에서도 Mahalanobis gating과 Hungarian assignment를 자주 결합한다. 이는 단일 최적 매칭을 확정하는 결정론적 방식에 해당하므로, 다중 가설을 시간 축 상에서 유지하는 MHT와는 명확히 구분된다.

#### 14.7.5 Grid Localization

MHT가 가우시안 혼합으로 belief를 표현한다면, Grid Localization은 포즈 공간 전체를 격자로 나누고 셀마다 확률을 누산하는 더 직접적인 방법이다.

포즈 공간을 셀로 이산화한 **히스토그램 필터**다. EKF가 풀지 못하는 global·multi-modal belief를 표현할 수 있지만, 셀 수 $K$에 비례하는 계산 비용이 단점이다.

```
Grid_localization({p_{k,t-1}}, u_t, z_t, m):
  for all k do
    p̄_{k,t} = Σ_i p_{i,t-1} · motion_model(mean(x_k), u_t, mean(x_i))
    p_{k,t}  = η · measurement_model(z_t, mean(x_k), m) · p̄_{k,t}
  endfor
  return {p_{k,t}}
```

$\text{bel}(x_t) = \{p_{k,t}\}$이며 각 셀 $x_k$에 확률 하나, 합은 1이다.

**해상도 트레이드오프**: 격자가 촘촘할수록 위치 추정의 양자화 오차가 줄어든다. 다만 격자가 세밀해질수록 global localization에 요구되는 CPU 연산량 역시 급격히 늘어난다. 실시간 트릭으로는 raycast 결과 캐싱, 스캔 서브샘플링, 선택적 업데이트(임계 이상 셀만)가 있다.

이산 격자로 global을 표현하면서도, "왜 파티클 필터가 더 나은가"를 이해하는 교육적 다리 역할을 한다. ROS `amcl` 노드는 Grid Localization의 격자를 파티클로 교체한 것이다.

#### 14.7.6 MCL 알고리즘 (보강)

MCL의 원리와 유도는 §3.11 (Ch.3 참조). 여기서는 위치추정 알고리즘으로서의 전체 골격을 명시한다.

```
MCL(X_{t-1}, u_t, z_t, m):
  X̄_t = X_t = ∅
  for k = 1 to M do
    x_t^[k] = sample_motion_model(u_t, x_{t-1}^[k])    // motion proposal
    w_t^[k] = measurement_model(z_t, x_t^[k], m)        // likelihood weight
    X̄_t += ⟨x_t^[k], w_t^[k]⟩
  endfor
  for k = 1 to M do
    i ~ Categorical(w_t^[1], ..., w_t^[M])              // 중요도 비례 재샘플
    X_t += x_t^[i]
  endfor
  return X_t
```

세 단계: **predict (sample) → weight → resample**. 초기화는 시나리오에 따라 다르다: global localization이면 자유공간 균등 분포에서 $M$개 샘플, position tracking이면 좁은 Gaussian에서 샘플한다.

**계산 자원 적응성**: $M$을 고정하지 않고 "다음 측정이 도착할 때까지 가능한 한 많이 샘플"하면 CPU가 빠를수록 $M$ 증가, 정확도 자동 향상된다.

proposal이 motion model이므로, perfect sensor(측정 우도가 극도로 좁은) 환경에서는 거의 모든 입자의 가중치가 0에 가까워진다. Mixture MCL(§14.7.8)이 이 문제를 해결한다.

ROS2 Nav2의 `nav2_amcl`이 이 구조를 그대로 구현한다.

#### 14.7.7 Augmented MCL — 납치 복구

표준 MCL은 납치에 취약하다. 입자들이 하나의 자세로 수렴한 뒤 로봇이 강제로 옮겨지면, 어떤 입자도 새 위치 근방에 없어 복구 경로가 없다. Augmented MCL은 측정 우도의 **단기 평균이 장기 평균에 비해 갑자기 떨어지면 무작위 입자를 주입**한다. "센서가 갑자기 지도와 안 맞기 시작했다 = 길 잃었다"는 직관을 두 지수이동평균의 비율로 수치화한다.

```
Augmented_MCL(X_{t-1}, u_t, z_t, m):
  static w_slow, w_fast
  X̄_t = X_t = ∅,  w_avg = 0
  for k = 1 to M do
    x_t^[k] = sample_motion_model(u_t, x_{t-1}^[k])
    w_t^[k] = measurement_model(z_t, x_t^[k], m)
    X̄_t += ⟨x_t^[k], w_t^[k]⟩
    w_avg += w_t^[k] / M
  endfor
  w_slow += α_slow (w_avg − w_slow)    // 장기 평균 (천천히 변함)
  w_fast += α_fast (w_avg − w_fast)    // 단기 평균 (빨리 변함)
  for k = 1 to M do
    with probability max(0, 1 − w_fast/w_slow) do
      X_t += random pose from bel(x_0)  // 무작위 입자 주입
    else
      i ~ Categorical(w_t^[1], ..., w_t^[M])
      X_t += x_t^[i]
  endfor
  return X_t
```

요건: $0 \le \alpha_{\text{slow}} \ll \alpha_{\text{fast}}$ (예: $\alpha_{\text{slow}} = 0.001$, $\alpha_{\text{fast}} = 0.1$).

$$p_{\text{inject}} = \max\!\left(0,\, 1 - \frac{w_{\text{fast}}}{w_{\text{slow}}}\right)$$

평소에는 $w_{\text{fast}} \approx w_{\text{slow}}$ → 비율 $\approx 1$ → 주입 확률 $\approx 0$ → 표준 MCL과 동일. 납치 직후에는 측정이 어디에도 안 맞음 → $w_{\text{fast}}$ 급락 → 주입 확률 상승. 장기 평균이 따라잡으면 비율 다시 1 → 주입 멎음.

일시적인 노이즈 스파이크도 $w_{\text{fast}}$를 떨어뜨려 주입 확률을 올린다. $w_{\text{slow}}$가 느리게 움직이는 것은 그 비율을 만들어 주는 조건이므로, 오탐을 줄이려면 $w_{\text{fast}}$의 평활 계수($\alpha_{\text{fast}}$)를 조절한다.

ROS `amcl`의 `recovery_alpha_slow`·`recovery_alpha_fast` 파라미터가 이 적응형 random-pose 주입을 제어한다. 연구 시스템에서는 NetVLAD류 place recognition과 PnP가 낸 pose 후보를 particle proposal로 섞기도 하지만, 이는 기본 AMCL 동작이 아니며 목표 환경에서 별도로 검증해야 한다.

#### 14.7.8 Mixture MCL

Augmented MCL이 무작위 포즈를 주입하는 것과 달리, Mixture MCL은 **proposal 분포 자체를 바꾼다**. 일부 입자를 motion model이 아니라 **측정 모델**에서 직접 샘플한다.

$$x_t^{[k]} \sim \begin{cases} p(z_t | x_t, m) & \text{확률 } \rho \\ \text{sample\_motion\_model}(u_t, x_{t-1}^{[k]}) & \text{확률 } 1 - \rho \end{cases}$$

측정에서 바로 샘플한 입자는 센서 정보가 강한 곳에 집중되므로, 저잡음 센서 환경에서 기본 MCL의 proposal 비효율을 해결한다. 납치 복구와 low-noise 센서 실패 모두를 다룬다는 점이 Augmented MCL과 다른 강점이다. 단, $p(z_t | x_t, m)$에서 직접 샘플하려면 역방향 센서 모델이 필요하다는 구현 부담이 있다.

#### 14.7.9 동적 환경 필터링

동적 물체(사람, 차량)가 있는 환경에서는 빔의 일부가 지도에 없는 장애물을 관측한다. 빔 센서 모델의 short hit 성분 $p_{\text{short}}(z | x, m)$의 사후확률을 이용해 의심스러운 빔을 위치추정에서 제외한다.

각 빔 $z_t^k$에 대해 네 성분 혼합 모델(§2.7, Ch.2 참조)을 평가하고, short 성분의 사후확률이 높은 빔은 가중치 계산에서 배제한다. 이 필터링이 없으면 복도에 사람이 많을 때 MCL이 흔들린다.

#### 14.7.10 필터 비교 정리

| 알고리즘 | Belief 표현 | Position tracking | Global loc | Kidnapped | 계산 비용 |
|---|---|---|---|---|---|
| EKF Loc | Gaussian (μ, Σ) | 좋음 | 불가 | 불가 | O(N) |
| MHT | Gaussian 혼합 | 좋음 | 제한 | 제한 | O(H·N) |
| Grid Loc | 히스토그램 | 좋음 | 가능 | 가능 | O(K) |
| MCL | 파티클 집합 | 좋음 | 가능 | Augmented MCL로 가능 | O(M) |

N은 랜드마크 수, H는 가설 수, K는 격자 셀 수, M은 파티클 수다. EKF는 Gaussian 단봉 가정 때문에 global/kidnapped 문제를 다룰 수 없다. Grid와 MCL은 계산 자원을 조절해 정확도와 속도의 균형을 선택할 수 있다.

#### 14.7.11 실무 고려: 랜드마크 효율·Negative Information

실제 EKF Localization을 구현할 때 자주 부딪히는 문제들이 있다.

**효율적 랜드마크 검색**: 지도에 N개 landmark가 있을 때 매 관측마다 전수 검색은 O(N)이다. 저차원에서 균형을 유지하는 KD-tree의 평균 탐색 비용은 O(log N) 수준이지만 최악의 경우 O(N)까지 치솟을 수 있다. 한편 격자 인덱싱 기법의 연산량은 셀 점유 밀도와 탐색 반경 설정에 좌우된다.

**Mutual exclusion**: 한 프레임에서 두 측정이 동일 랜드마크에 대응될 수 없다. ML data association은 component-wise 최적화라 이 제약을 자동으로 강제하지 않는다. 충돌 쌍이 생기면 마할라노비스 거리가 더 작은 측정을 선택하고 나머지를 버리는 repair 단계가 필요하다.

**Outlier rejection**은 마할라노비스 거리가 $\chi^2_{95\%}$ 임계값을 초과하는 측정을 제거한다. 이 한 줄이 EKF의 brittleness를 크게 줄인다.

**Negative information**: "이 각도 범위에서 랜드마크가 관측되지 않았다"는 정보도 위치추정에 유용할 수 있지만, 정확한 확률 처리가 복잡하고 구현 부담이 크다. 대부분의 실용 시스템에서 negative information은 무시된다.

---

### 14.7B Occupancy Grid Mapping

§14.7은 지도가 *주어졌을 때* 위치를 추정했다. 여기서는 반대 방향, 즉 위치가 알려진 상태에서 지도를 *만드는* 문제인 Occupancy Grid Mapping을 다룬다. Occupancy Grid Mapping은 **위치가 알려진 상태에서 셀별 점유 확률을 추정**하는 기법이다. 이는 pose graph 최적화로 정렬된 자세 궤적을 토대로 최종 점유 지도를 생성하는 후처리 단계에서 중추적인 역할을 수행한다. 이 후처리 구성에서는 두 단계가 순서대로 작동한다: pose graph 최적화로 자세 궤적을 확정한 뒤, 이 절의 알고리즘으로 최종 지도를 완성한다. binary Bayes 필터의 기반은 §3.11.2 (Ch.3 참조).

#### 14.7B.1 도입: 지도 작성의 어려움

매핑이 위치추정보다 더 어렵다는 말이 있다. 위치는 연속적 $x_t \in \mathbb{R}^3$이지만, 지도 m은 수만~수백만 개의 셀로 이루어진 고차원 불연속 변수다. 가능한 지도의 수가 $2^{|m|}$이므로 직접 탐색은 불가능하다.

이 조합 폭발을 피하는 핵심 가정 둘: (1) **자세를 안다**(알려진 $x_{1:t}$), (2) **셀들은 조건부 독립**이다. 두 번째 가정 덕에 지도 사후확률을 셀별 marginal의 곱으로 분해하여, 전체 문제를 셀마다 독립적인 binary Bayes 필터로 쪼갤 수 있다.

$$p(m \mid z_{1:t}, x_{1:t}) = \prod_i p(m_i \mid z_{1:t}, x_{1:t})$$

추가 어려움: 센서 잡음·지각 모호성(같은 위치에서 다른 측정)·환경 동적 변화·닫힌 루프에서의 오차 누적이 있다.

#### 14.7B.2 표준 알고리즘: Log-Odds 누산

각 셀의 occupancy 사후확률을 **log-odds** 형태로 누산한다.

$$l_{t,i} = \log \frac{p(m_i \mid z_{1:t}, x_{1:t})}{1 - p(m_i \mid z_{1:t}, x_{1:t})}$$

prior log-odds: $l_0 = \log[p(m_i) / (1 - p(m_i))]$. binary Bayes 필터(§3.11.2, Ch.3 참조)의 유도로부터 갱신식은:

$$l_{t,i} = l_{t-1,i} + \text{inverse\_sensor\_model}(m_i, x_t, z_t) - l_0$$

직관: 새 측정이 셀 $m_i$에 hit 증거를 주면 log-odds가 오르고, free 증거를 주면 내린다. $-l_0$ 항이 prior의 이중 계상을 막는다.

```
occupancy_grid_mapping({l_{t-1,i}}, x_t, z_t):
  for all cells m_i do
    if m_i is in perceptual field of z_t then
      l_{t,i} = l_{t-1,i} + inverse_sensor_model(m_i, x_t, z_t) − l_0
    else
      l_{t,i} = l_{t-1,i}    // 관측 범위 밖 — 변화 없음
  endfor
  return {l_{t,i}}
```

확률로 복원: $p(m_i | z_{1:t}, x_{1:t}) = 1 - 1/(1 + \exp\{l_{t,i}\})$.

**inverse_sensor_model** (range finder용 단순 예시):
```
inverse_range_sensor_model(m_i, x_t, z_t):
  셀 중심까지 거리 r, 방위각 φ 계산
  가장 가까운 빔 인덱스 k = argmin_j |φ − θ_{j,sens}|
  if 빔 밖이거나 z_t^k + α/2 너머:
    return l_0                   // 정보 없음
  if |r − z_t^k| < α/2:
    return l_occ                 // hit (> l_0)
  if r ≤ z_t^k:
    return l_free                // free (< l_0)
```

$\alpha$는 장애물 두께 파라미터, $\beta$는 빔 개구각. Cartographer의 submap probability grid가 이 누산을 odds 곱 형태로 구현한다(lookup table 기반). ROS Nav2의 `costmap_2d`는 0~254 cost 값에 raytracing 기반 marking/clearing을 쓰고, SLAM Toolbox(Karto 계열)는 셀별 hit/visit 카운트 비율에 임계를 적용하므로 표현과 갱신 방식이 다르다.

#### 14.7B.3 다중 센서 융합

카메라·LiDAR·소나·적외선이 서로 다른 inverse_sensor_model을 가진다. 융합 전략 중 가장 단순한 것은 **셀별 최대값(conservative max)**이다: 어떤 센서라도 hit을 보고하면 그 셀은 occupied로 분류한다. 이 보수적 정책은 충돌 회피에서 안전하지만, 자유 공간을 과소평가하는 경향이 있다.

각 센서의 log-odds 업데이트를 독립적으로 누산한 후 셀별 합계를 구하는 방법도 있다. 이 경우 센서마다 정보량이 다른 경우 가중 합산이 필요하다.

#### 14.7B.4 inverse_sensor_model 학습

수작업으로 설계한 inverse_sensor_model은 간단한 기하학적 모델이다. **forward 모델 $p(z | x, m)$을 이미 갖고 있다면 역방향을 학습으로 도출**할 수 있다.

절차: (자세, 측정, 점유) 삼중쌍 $\{(x^{(k)}, z^{(k)}, m_i^{(k)})\}$를 시뮬레이션으로 생성한 후, cross-entropy 손실로 함수 근사기를 학습한다.

$$\mathcal{L} = -\sum_k \left[m_i^{(k)} \log \hat{p}_i + (1 - m_i^{(k)}) \log(1 - \hat{p}_i)\right]$$

입력 $(x, z)$, 출력 $\hat{p}_i = p(m_i | x, z)$인 신경망이 inverse_sensor_model의 역할을 대체한다. 복잡한 센서 기하(sonar의 반사 패턴, 유리에서의 LiDAR 특성)를 사람이 명시적으로 모델링하기 어려울 때 유용하다.

#### 14.7B.5 MAP Occupancy Mapping (심화)

표준 알고리즘의 셀 독립 가정은 한 가지 모순을 만든다: 같은 빔 cone 안에 있는 인접 셀들이 실제로는 서로 연관된 증거를 공유하지만, 독립 가정 때문에 이 연관성이 무시된다. 소나처럼 빔 폭이 넓은 센서에서 이 문제가 두드러진다.

MAP Occupancy Mapping은 지도 사후확률의 mode를 직접 최대화한다.

$$m^* = \arg\max_m \left[\sum_t \log p(z_t \mid x_t, m) + \log p(m)\right]$$

inverse model 대신 **forward 모델** $p(z_t | x_t, m)$을 그대로 쓴다. 모든-free 지도에서 출발해 셀 하나씩 occupancy를 뒤집으면서 log-likelihood가 증가하는 방향으로 반복하는 hill-climbing이다.

```
MAP_occupancy_grid_mapping(x_{1:t}, z_{1:t}):
  m ← 모든 셀 free로 초기화
  repeat until convergence:
    for all cells m_i do
      m_i ← argmax_{k ∈ {0,1}} [k·l_0 + Σ_t log measurement_model(z_t, x_t, m | m_i=k)]
  return m
```

실용적 한계: batch라 incremental SLAM에 맞지 않고, hill-climbing이 local maximum에 갇힌다. 사후 불확실성도 사라진다. 그러나 **"셀 독립 가정을 깨야 한다"는 통찰은 이후에 이어진다**.

#### 14.7B.6 다른 공간 표현과의 비교: OctoMap·Voxblox·NeRF·3DGS

**OctoMap**은 3D occupancy를 octree에 저장하므로 occupancy grid의 직접적인 3D 확장으로 볼 수 있다. **Voxblox**와 **nvblox**는 별도의 거리장 계열로, TSDF(Truncated Signed Distance Function)에 표면까지의 부호 있는 거리를 저장한다. **NeRF**의 density field와 **3D Gaussian Splatting**의 opacity도 ray를 따라 투명도와 색을 합성하지만, 이들은 binary occupancy Bayes filter의 직계 후예가 아니라 novel-view rendering을 학습하는 서로 다른 장면 표현이다. 따라서 forward sensor model과 ray integration이라는 수학적 공통점을 비교할 수는 있어도 계보를 동일시해서는 안 된다. Occupancy grid는 SLAM Toolbox와 Nav2 costmap 같은 navigation 구성에서 계속 쓰인다.

> **실습**: [Occupancy Grid](https://alexjunholee.github.io/robotics-practice/app.html#occupancy_grid)
> Log-odds 누산 과정을 셀별로 시각화하며, inverse_sensor_model의 hit/free 영역이 어떻게 지도로 쌓이는지 확인할 수 있다.

---

## Part 2. 최신 트렌드

### 14.8 Learning-based & Neural SLAM

전통적인 SLAM은 수작업으로 설계된 특징점, 매칭 알고리즘, 최적화 파이프라인을 사용한다. 최근에는 이 과정 일부 또는 전체를 딥러닝으로 대체하는 연구가 이어지고 있다.

**DROID-SLAM (2021)**:
- Dense Recurrent Optical-flow 기반 SLAM
- 특징점 추출/매칭 없이, Dense optical flow를 반복적으로 정제하여 카메라 포즈와 깊이를 동시에 추정
- 텍스처 없는 환경·조명 변화 등 기존 방법이 실패하는 상황에서 robustness 향상
- Differentiable한 Dense Bundle Adjustment(DBA) 레이어를 사용하여 end-to-end 학습

DROID-SLAM이 주목받은 이유: 전통적 특징점 기반 SLAM(ORB-SLAM)은 텍스처가 부족한 환경에서 성능이 급감하며, 직접법(DSO)은 조명 변화에 취약성을 보인다. DROID-SLAM은 학습된 representation을 사용하기 때문에 이런 한계를 상당 부분 극복한다. 다만 GPU가 필수이고 실시간 성능은 아직 기존 방법에 미치지 못하는 경우가 있다.

**3DGS-SLAM 융합**:
13.5.2에서 다룬 3D Gaussian Splatting을 SLAM의 맵 표현(map representation)으로도 쓴다. SplaTAM, MonoGS 등이 대표적이며, 기존 SLAM의 sparse/dense 포인트 맵 대신 3D Gaussian으로 환경을 표현한다. 장면의 시각적 충실도가 높아지고, 렌더링 기반의 새로운 응용(가상 뷰 생성, AR 오버레이 등)이 가능해진다.

> **추천 자료**
> - [Teed & Deng, "DROID-SLAM: Deep Visual SLAM for Monocular, Stereo, and RGB-D Cameras" (2021)](https://arxiv.org/abs/2108.10869) — DROID-SLAM 논문
> - [Keetha et al., "SplaTAM" (2024)](https://arxiv.org/abs/2312.02126) — 3DGS 기반 Dense SLAM
> - [Awesome-SLAM GitHub](https://github.com/SilenceOverflow/Awesome-SLAM) — 최신 SLAM 논문/프로젝트 모음

---

## Part 3. 심화

### 14.9 심화: SLAM 백엔드 최적화

factor graph 이전 시대의 정보형 SLAM (EKF-SLAM·EIF·SEIF·EM)은 §14.16 심화: 정보형 SLAM의 역사 참조.

SLAM 프론트엔드가 센서 데이터를 처리해서 제약 조건(constraint)을 만들어내면, 백엔드는 이 제약 조건들을 동시에 만족하는 최적의 상태(포즈, 랜드마크)를 찾는다. 이 과정은 비선형 최소제곱(nonlinear least squares) 문제다. 여기서 다루는 내용은 g2o, GTSAM, Ceres 같은 라이브러리를 "왜 그렇게 설정하는지" 이해하기 위한 수학적 배경이다.

**SLAM 백엔드가 푸는 문제의 직관**

SLAM 백엔드는 결국 **Ax = b를 푸는 문제**이다.

로봇이 주행하면서 얻는 데이터는 두 종류다:
1. **오도메트리**: "나는 1 m 앞으로 갔다" (상대적 이동)
2. **관측**: "저 랜드마크가 3 m 거리에 보인다"

이 측정값들을 모두 만족시키는 포즈와 랜드마크 위치를 찾고 싶지만, 센서 노이즈 때문에 완벽히 만족시키는 해는 없다. 대신 "모든 측정값과의 오차 제곱합을 최소화"하는 해를 찾는다. 이것이 nonlinear least squares 문제이고, 이걸 효율적으로 푸는 것이 SLAM 백엔드의 역할이다.

비선형이라 한 번에 못 풀고, 현재 추정값 근처에서 선형화(linearize)해서 반복적으로 갱신한다. 이 "선형화 → Ax=b 풀기 → 업데이트 → 반복"이 Gauss-Newton이다.

(참고: [김기섭 블로그 — SLAM back-end 시리즈](https://gisbi-kim.github.io/blog/2021/03/04/slambackend-1.html))

#### 14.9.1 Manifold 위의 Gauss-Newton

SLAM의 상태 변수(포즈)는 SE(3) 위에 있다. SE(3)는 유클리드 공간이 아니라 Lie group이므로, 일반적인 Gauss-Newton update `x ← x + δx`를 그대로 쓸 수 없다. 회전 행렬에 벡터를 더하면 더 이상 회전 행렬이 아니게 된다.

해법은 Lie algebra se(3) 위에서 perturbation을 정의하는 것이다.

**Update step (left perturbation)**:
```
T ← exp(δξ^) · T
```
여기서 `δξ ∈ R^6`는 se(3) 리 대수 위의 미소 섭동(perturbation) 벡터이며, `exp(·)`는 지수 사상(exponential map)을, `^`(hat 연산자)는 6차원 벡터를 4x4 행렬로 사상하는 연산자다.

**Jacobian 계산**: 오차 함수 `e(T)`의 Jacobian을 `δξ`에 대해 계산한다.
```
J = ∂e / ∂δξ
```
이것은 chain rule로 `∂e/∂(Tp) · ∂(exp(δξ^)Tp)/∂δξ`가 되는데, 뒤 항은 좌섭동에서 오는 group action의 미분으로 3차원 점에 대해 (δξ를 병진·회전 순으로 둘 때) `[I, -(Tp)^]` 형태다. SE(3)의 left Jacobian은 BCH 관계에서 Lie 대수 섭동을 잇는 6×6 행렬이므로 이 항과 다른 대상이다.

**Normal equation**:
```
(J^T Σ^{-1} J) δξ* = -J^T Σ^{-1} e
```
- `Σ`는 측정 노이즈 공분산
- `H = J^T Σ^{-1} J`가 Hessian의 Gauss-Newton 근사이고, 이것이 **information matrix**
- 여러 제약 조건이 있으면 각각의 `J^T Σ^{-1} J`를 합산한다 (additive property)

이 과정을 수렴할 때까지 반복한다. 매 iteration마다 현재 추정치에서 Jacobian을 재계산하고, update를 적용한다.

#### 14.9.2 Schur Complement (Marginalization)

Bundle Adjustment(BA)에서 상태 변수는 카메라 포즈(p)와 랜드마크(l) 두 종류이다. Normal equation의 Hessian `H`는 다음 block 구조를 갖는다:

```
[H_pp  H_pl] [δp]   [b_p]
[H_lp  H_ll] [δl] = [b_l]
```

포즈 수를 `m`, 랜드마크 수를 `n`이라 하면, 보통 `n >> m`이다. 이 큰 시스템을 직접 풀면 비싸다.

**Schur complement**로 랜드마크를 marginalize한다:

```
(H_pp - H_pl · H_ll^{-1} · H_lp) δp = b_p - H_pl · H_ll^{-1} · b_l
```

**`H_ll`은 block diagonal**이므로 이것이 가능하다. 각 랜드마크는 다른 랜드마크와 직접 연결되지 않으므로(랜드마크끼리는 공통 factor가 없다), `H_ll`의 역행렬은 각 block을 독립적으로 역산하면 된다. 계산 비용이 `O(n)`으로 싸다.

Schur complement 뒤 reduced camera system의 차원은 pose 수 `m`으로 정해지지만, landmark elimination과 back-substitution 비용은 여전히 관측 수와 landmark 수 `n`에 의존한다. 이 block sparsity 덕분에 큰 BA 문제를 효율적으로 풀 수 있지만 처리 속도는 graph 구조, solver, hardware에 달려 있다.

`δp`를 구한 뒤, `δl`은 back-substitution으로 복원한다:
```
δl = H_ll^{-1} (b_l - H_lp · δp)
```

#### 14.9.3 희소성과 Variable Ordering

Pose graph optimization의 `H`는 각 factor가 일부 pose만 연결하므로 보통 **sparse**하다. Odometry factor는 인접 pose를, loop factor는 떨어진 pose를 연결한다. 노드 차수는 dataset과 closure 수에 따라 달라지며 dense한 loop proposal이나 elimination ordering은 fill-in을 늘릴 수 있다.

sparse linear system을 풀 때 Cholesky factorization(`H = L L^T`)을 사용하는데, 여기서 **fill-in** 문제가 발생한다. 원래 0이었던 위치가 factorization 과정에서 non-zero가 되는 현상이다. Fill-in이 많으면 메모리와 계산 비용이 급증한다.

Fill-in을 최소화하려면 변수의 순서(variable ordering)를 잘 정해야 한다:
- **COLAMD** (Column Approximate Minimum Degree): sparse least-squares에서 흔히 쓰이는 heuristic. 예상 fill-in이 작도록 column ordering을 근사
- **AMD** (Approximate Minimum Degree): COLAMD와 유사하지만 symmetric 행렬에 특화
- **Nested dissection**: 그래프를 재귀적으로 분할하여 ordering을 결정. 대규모 문제에서 효과적

g2o, GTSAM, Ceres 같은 라이브러리에서 solver를 설정할 때 linear solver type(DENSE_SCHUR, SPARSE_NORMAL_CHOLESKY 등)과 ordering strategy를 함께 살펴야 한다. Ordering이 fill-in과 실행 시간에 미치는 영향은 graph structure에 따라 달라지므로, 대표 dataset에서 memory와 latency를 측정해 선택한다.

```python
# Ceres Solver에서 ordering 설정 예시 (Python binding)
options = ceres.SolverOptions()
options.linear_solver_type = ceres.LinearSolverType.SPARSE_NORMAL_CHOLESKY
options.sparse_linear_algebra_library_type = ceres.SparseLinearAlgebraLibraryType.SUITE_SPARSE
# ordering은 보통 자동으로 COLAMD를 사용하지만, 수동 설정도 가능
```

#### 14.9.4 최적화 라이브러리 비교

| 라이브러리 | 특징 | 주요 사용처 |
|---|---|---|
| **g2o** | Pose graph / BA 전용, 가벼움, C++ only | ORB-SLAM2/3, LSD-SLAM |
| **GTSAM** | Factor graph 기반, Bayes tree(iSAM2) 지원, incremental 최적화에 강함 | LIO-SAM, 연구용 |
| **Ceres Solver** | 범용 nonlinear least squares, auto-diff 지원, Google 개발 | Cartographer, VINS-Fusion, 다양한 프로젝트 |

선택 기준:
- SLAM 전용이고 가볍게 쓰고 싶다 → g2o
- Factor graph 모델링이 필요하고, incremental update(키프레임이 추가될 때마다 점진적으로 최적화)가 중요하다 → GTSAM (iSAM2)
- SLAM 이외의 범용 최적화도 해야 하고, Jacobian을 직접 유도하기 싫다 → Ceres (auto-diff)

> **추천 자료**
> - Barfoot, "State Estimation for Robotics" Ch.4 (Nonlinear Estimation) — Manifold 위의 최적화를 체계적으로 설명
> - [Dellaert & Kaess, "Factor Graphs for Robot Perception" (Foundations and Trends in Robotics, 2017)](https://www.cs.cmu.edu/~kaess/pub/Dellaert17fnt.pdf) — Factor graph와 SLAM 백엔드 이론
> - [g2o Tutorial](https://github.com/RainerKuemmerle/g2o) / [GTSAM Tutorial](https://gtsam.org/tutorials/intro.html) — 라이브러리별 실습
> - [김기섭 블로그 — Gauss-Newton Opt == IEKF update?](https://gisbi-kim.github.io/blog/2022/03/05/gn-iekf-same.html) — GN 최적화와 반복 칼만 필터의 수학적 동치성 설명. 필터 vs 최적화 논쟁에 대한 정리

> **실습**: [Bundle Adjustment 시각화](https://alexjunholee.github.io/robotics-practice/app.html#bundle_adjustment)
> 카메라 포즈와 3D 포인트를 동시에 최적화하는 Bundle Adjustment 과정을 인터랙티브하게 확인할 수 있다.

### 14.10 심화: IMU Preintegration

IEKF·MSCKF의 EKF 기반 구조는 §3.10.2 EKF (Ch.3 참조).

14.3.3에서 VINS-Mono를 소개할 때 IMU preintegration을 간단히 언급했다. 여기서는 그 수학적 배경을 본다.

**문제 정의**: IMU 센서는 통상 200~1000 Hz의 높은 주기로 가속도와 각속도를 연속 출력한다. 이와 대조적으로 비전 SLAM 백엔드 최적화는 수 Hz에서 수십 Hz 주기의 키프레임 단위로 실행된다. 키프레임 사이에 수백 개의 IMU 측정이 존재한다. IMU 측정 자체는 factor의 관측이지만, 그 시점마다 상태(포즈·속도·bias)를 추가하면 문제 크기가 폭발한다.

**Preintegration의 아이디어**: 두 키프레임 `i`와 `j` 사이의 IMU 측정값들을 하나의 "상대 운동 측정(relative motion measurement)"으로 압축한다. 이 압축된 측정값이 최적화의 factor로 들어간다.

**Preintegrated measurements**: 키프레임 `i`에서 `j`까지의 상대 변화량 세 가지를 계산한다.

```
ΔR_ij = Π_{k=i}^{j-1} Exp((ω_k - b_g) · Δt)          # 상대 회전
Δv_ij = Σ_{k=i}^{j-1} ΔR_ik · (a_k - b_a) · Δt        # 상대 속도
Δp_ij = Σ_{k=i}^{j-1} (Δv_ik · Δt + 0.5 · ΔR_ik · (a_k - b_a) · Δt^2)  # 상대 위치
```

여기서 `ω_k`, `a_k`는 IMU 측정값, `b_g`, `b_a`는 gyroscope/accelerometer bias, `Δt`는 IMU 샘플링 간격이다.

이 preintegrated measurement들은 **키프레임 `i`의 좌표계를 기준으로** 계산된다. 따라서 키프레임 `i`의 절대 포즈가 최적화 과정에서 바뀌더라도, preintegrated measurement를 재계산할 필요가 없다.

**공분산 전파**: IMU 측정 노이즈가 preintegrated measurement에 어떻게 전파되는지 계산한다. Discrete-time propagation으로 각 IMU 측정마다 공분산을 업데이트한다.

```
Σ_{k+1} = A_k · Σ_k · A_k^T + B_k · Q · B_k^T
```
- `A_k`: 상태 전이 행렬 (현재 상태에서의 Jacobian)
- `B_k`: 노이즈 입력 행렬
- `Q`: IMU 노이즈 공분산 (데이터시트에서 확인)

이 공분산이 최적화에서 해당 factor의 information matrix(`Σ^{-1}`)로 사용된다.

**Bias 변화 시 보정**: 최적화 과정에서 IMU bias 추정치가 바뀔 수 있다. Bias가 바뀌면 원칙적으로 preintegration을 처음부터 다시 해야 한다. 하지만 이것은 비싸다. 대신 **first-order approximation**으로 보정한다:

```
ΔR_ij ≈ ΔR_ij^0 · Exp(∂ΔR/∂b_g · δb_g)
Δv_ij ≈ Δv_ij^0 + ∂Δv/∂b_g · δb_g + ∂Δv/∂b_a · δb_a
Δp_ij ≈ Δp_ij^0 + ∂Δp/∂b_g · δb_g + ∂Δp/∂b_a · δb_a
```

`^0`은 이전 bias 추정치로 계산한 값, `δb`는 bias 변화량, 편미분들은 preintegration 과정에서 함께 축적해둔다. Bias 변화가 크지 않은 한(보통 그렇다) 이 근사는 충분히 정확하다.

**왜 manifold에서 preintegration하는가**: 사전 적분 자체는 Lupton과 Sukkarieh가 먼저 제시했고, 그 정식화는 회전을 Euler angle로 매개화했다. Forster et al.이 이를 SO(3) 위에서 다시 세웠다. Lie group 위에서 미리 integration해두면, 1) 회전이 군 구조를 유지하고 Euler 각의 특이점을 피하며, 2) 결과가 relative motion measurement로서 factor graph에 바로 들어갈 수 있다. 이산 시간 적분 오차와 측정 잡음은 그대로 남는다. Forster et al. (2015 RSS, 2017 TRO)의 핵심 기여가 이 부분이다.

**Tightly-coupled vs Loosely-coupled**: LIO-SAM을 예로 설명하면:
- **Loosely-coupled**: 두 갈래가 있다. IMU를 포인트 클라우드 de-skew와 scan matching 초기값으로만 쓰는 방식과, LiDAR odometry와 IMU가 각자 상태를 추정한 뒤 covariance로 합치는 방식이다. LeGO-LOAM은 앞쪽에 해당한다.
- **Tightly-coupled**: IMU preintegration factor를 LiDAR odometry factor와 같은 factor graph 안에서 동시에 최적화한다. IMU가 단순 초기값이 아니라, 키프레임 사이의 상대 포즈에 대한 독립적 관측으로 작용한다. LIO-SAM이 이 방식이다.

Tightly-coupled의 장점은 모션이 심한 상황(빠른 회전, 급격한 가감속)에서 드러난다. LiDAR scan matching만으로는 잡기 어려운 빠른 변화를 IMU factor가 잡아주기 때문이다. Factor graph 형식이므로 GPS factor, loop closure factor 등을 모듈처럼 추가할 수 있다는 것도 실용적 장점이다.

**LIO-SAM의 구조**: GTSAM 기반으로, IMU preintegration factor + LiDAR odometry factor + GPS factor + loop closure factor를 하나의 그래프에서 최적화한다. LiDAR odometry는 edge feature와 planar feature를 따로 추출하고, 각각 다른 resolution의 voxel map으로 관리한다. Scan matching 시 planar feature는 point-to-plane distance, edge feature는 point-to-line distance를 최소화하는 상대 변환을 구한다.

VINS-Mono, ORB-SLAM3 (Visual-Inertial mode), LIO-SAM 등 현대 VIO/LIO 시스템이 이 기법을 IMU factor 구현에 그대로 쓴다.

> **추천 자료**
> - [Forster et al., "On-Manifold Preintegration for Real-Time Visual-Inertial Odometry" (TRO 2017, arXiv:1512.02363)](https://arxiv.org/abs/1512.02363) — on-manifold preintegration의 원본 논문(사전 적분 개념 자체는 Lupton & Sukkarieh 2012). 수식이 많지만 이 분야의 필수 논문
> - [Forster et al., "IMU Preintegration on Manifold for Efficient VIO" (2015 RSS)](https://rpg.ifi.uzh.ch/docs/RSS15_Forster.pdf) — 위 논문의 초기 버전으로, 방법을 더 간결하게 설명한다.
> - [Shan et al., "LIO-SAM" (IROS 2020)](https://github.com/TixiaoShan/LIO-SAM) — Tightly-coupled LIO의 레퍼런스 구현. 코드와 논문 모두 읽을 것
> - [Sola et al., "A micro Lie theory for state estimation in robotics" (arXiv:1812.01537)](https://arxiv.org/abs/1812.01537) — Lie group/algebra의 실용적 정리. Preintegration 읽기 전에 이것부터 보면 좋다
> - GTSAM의 `PreintegratedImuMeasurements` 클래스 소스코드 — 이론이 코드로 어떻게 구현되는지 확인
> - [IMU Preintegration MATLAB 구현](https://github.com/GentleDell/imu_preintegration_matlab) — KITTI에서 테스트한 MATLAB 코드. 수식과 코드를 대조하며 공부하기 좋다

### 14.11 심화: 관측가능성 분석 (Observability)

SLAM/VIO 시스템을 돌려보면 "왜 이 상황에서 drift가 심한가?", "왜 가만히 서있으면 위치가 흔들리는가?" 같은 현상을 겪게 된다. 이런 현상의 상당수는 시스템의 **관측가능성(observability)** 한계에서 비롯된다.

**Visual-Inertial 시스템의 관측 불가능한 상태**: Hesch et al. (2014)이 임의의 운동을 하며 하나의 점 특징을 관측하는 이상적 선형화 VINS 모델을 분석한 결과, 다음 4개 방향이 관측 불가능했다:

1. **Global position (3 DoF)** — 절대 위치를 알 수 없다. GPS 같은 절대 기준이 없으면 시작점을 원점으로 잡을 수밖에 없다
2. **Global yaw (1 DoF)** — 중력 방향 축 기준의 회전(heading). 나침반 없이는 "어느 쪽이 북쪽인지" 알 수 없다

이 결과는 명시한 모델과 운동 조건에 대한 것이다. Roll/pitch, metric scale, IMU bias와 calibration 파라미터의 추정 가능성은 센서 구성과 운동 여기(excitation)에 따라 달라진다. Stereo baseline은 metric 기준을 주지만, monocular 영상만으로는 scale을 정할 수 없다. Monocular VIO는 정보가 충분한 운동에서 IMU 측정을 함께 써 scale을 추정한다.

**Degenerate motion** — 특정 움직임 패턴에서 추가적인 상태가 관측 불가능해진다:

- **순수 회전(pure rotation)**: 병진 parallax가 없으므로 monocular 영상만으로는 translation을 복원할 수 없다. 회전 자체의 추정과 트래킹 가능성은 별개이다.
- **초기화 구간의 부족한 운동**: 단안 VIO 초기화에서 등속 운동이나 정지 상태만 관측하면 IMU bias에 대한 복수의 해가 남을 수 있다. 이때 scale·중력·bias 초기화가 약해질 수 있으며, 정확한 degeneracy는 상태와 측정 모델에 따라 다르다.

**EKF 기반 시스템에서의 문제**: 표준 EKF를 VIO에 적용하면, linearization 오차로 인해 이론적으로 관측 불가능한 방향에서도 공분산이 줄어드는(uncertainty가 인위적으로 감소하는) 현상이 발생한다. 이것은 inconsistency의 주요 원인이다.

**OC-EKF (Observability-Constrained EKF)**: 이 문제를 해결하기 위해, EKF의 Jacobian을 수정하여 관측 불가능한 방향의 null space를 보존한다. 추정기가 "모르는 것은 모른다고 유지"하도록 강제하는 것이다.

실무적 함의:
- VIO 초기화에서는 사용하는 구현의 조건에 맞는 충분한 회전·병진 운동을 제공하고, scale·중력·bias의 수렴 지표를 확인한다
- Loop closure는 재방문 제약으로 누적 drift를 줄일 수 있지만, 그 자체가 절대 global position이나 yaw 기준을 새로 만들지는 않는다
- Monocular VIO의 metric scale은 영상만으로는 정해지지 않으므로, 초기화 구간의 IMU 측정과 운동이 충분한 정보를 주는지 확인한다

> **추천 자료**
> - [Hesch et al., "Consistency Analysis and Improvement of Vision-aided Inertial Navigation" (TRO 2014)](https://ieeexplore.ieee.org/document/6672119) — OC-EKF/OC-VINS의 원본 논문
> - [Hong & Lim, "Visual-Inertial Odometry with Robust Initialization and Online Scale Estimation" (Sensors 2018)](https://pmc.ncbi.nlm.nih.gov/articles/PMC6308559/) — 등속·정지 구간에서 초기화가 약해지는 조건을 다루는 구현 예
> - Barfoot, "State Estimation for Robotics" Ch.9 — 관측가능성 분석의 이론적 토대
> - [Huang & Dissanayake, "A critique of current developments in Simultaneous Localization and Mapping" (IJRR 2016)](https://journals.sagepub.com/doi/10.1177/0278364916643566) — SLAM의 관측가능성/일관성 문제를 비판적으로 정리

> **실습**: [Odometry Uncertainty 시각화](https://alexjunholee.github.io/robotics-practice/app.html#odom_uncertainty)
> Odometry의 불확실성이 시간에 따라 어떻게 누적되는지, 공분산 타원이 어떻게 커지는지 인터랙티브하게 확인할 수 있다.

#### 14.11.1 필터 기반 vs 최적화 기반: 뭐가 더 나은가?

필터와 최적화 중 어느 쪽이 나은지는 SLAM/VIO 분야의 오래된 논쟁이다. 수학적으로 Gauss-Newton 최적화와 Iterated EKF (IEKF)는 같은 문제를 서로 다른 형태로 푼다.

- **필터 (EKF, MSCKF 등)**: 새 측정이 들어올 때마다 상태와 covariance를 갱신한다. 단순 EKF odometry는 현재 state만 둘 수 있지만, MSCKF는 일정 수의 과거 camera clone을 state에 유지한다. 보관 범위를 제한하므로 full smoothing보다 memory를 작게 만들 수 있다.
- **최적화 (BA, factor graph)**: 과거 상태를 전부 유지하고 한꺼번에 최적화한다. 과거 데이터를 relinearize할 수 있으므로 정확도가 높다. 하지만 상태 수가 늘어나면 계산량이 커진다 (sliding window나 iSAM2로 완화).

VINS-Mono(최적화)와 MSCKF(필터)의 성능 차이는 solver보다 **시스템 구조**에서 온다. 어떤 상태를 유지하고 어떤 측정을 사용하는지가 다르기 때문이다. 최적화 기반 시스템은 relinearization으로 과거의 linearization error를 줄일 수 있다는 이점이 있다.

실무적 선택:
- IMU 중심 + 경량 → 필터 (MSCKF, FAST-LIO2의 IEKF)
- 카메라 중심 + 정확도 → 최적화 (VINS-Mono, ORB-SLAM3)
- 둘 다 필요 → 하이브리드 (LIO-SAM: IMU preintegration을 factor로 넣은 최적화)

(참고: [김기섭 블로그 — Gauss-Newton Opt == IEKF update?](https://gisbi-kim.github.io/blog/2022/03/05/gn-iekf-same.html))

### 14.12 심화: Semantic SLAM

전통적 SLAM 파이프라인은 순수하게 기하학적(geometric) 형상을 복원하는 데 집중한다. 포인트 클라우드, 메쉬, 점유 격자처럼 공간의 외형적 구조를 기록하는 형태다. 장애물이 특정 위치에 존재한다는 사실은 파악하지만 그것이 벽인지, 출입문인지, 책장인지와 같은 기능적 정체는 구분하지 못한다. 이에 반해 Semantic SLAM은 기하학적 좌표계 위에 의미론적(semantic) 맥락을 결합한다.

접근 방식은 랜드마크 표현 방식에 따라 나뉜다. **Object-level SLAM** (CubeSLAM, QuadricSLAM)은 점(point) 대신 3D cuboid·dual quadric 같은 물체 단위를 랜드마크로 추정한다. 물체 검출기에 의존하지만, data association이 점 기반보다 견고하고 물체 수준의 추론이 가능하다. **Panoptic SLAM**은 panoptic segmentation 결과를 3D로 융합하여 모든 픽셀에 semantic label이 붙은 지도를 만든다. 로봇이 "이 방에 의자가 3개"를 지도에서 바로 쿼리할 수 있다. **Open-vocabulary SLAM** (ConceptGraphs)은 CLIP 같은 vision-language model의 feature를 지도에 저장하여 자연어로 장소를 검색할 수 있게 한다. 13장에서 다룬 3D Scene Graph (Hydra 등)와 직접 연결되는 주제다.

**동적 물체 처리**: Semantic label은 동적 환경에서 SLAM의 robustness를 높이는 데도 쓰인다. "사람", "차" 등 동적일 가능성이 높은 클래스의 feature를 tracking/mapping에서 빼면, 정적 환경만으로 깨끗한 SLAM이 가능하다.
- DynaSLAM: ORB-SLAM2 + Mask R-CNN으로 동적 물체 마스킹
- DS-SLAM: semantic segmentation으로 동적 영역 필터링

```python
# 동적 물체 필터링의 의사코드
dynamic_labels = {'person', 'car', 'bicycle', 'dog'}
for feature in detected_features:
    pixel = feature.pixel_coords
    label = semantic_map[pixel.y, pixel.x]
    if label in dynamic_labels:
        feature.ignore = True  # SLAM에서 제외
```

> **추천 자료**
> - [Nicholson et al., "QuadricSLAM: Dual Quadrics from Object Detections as Landmarks in Object-Oriented SLAM" (RA-L 2019)](https://arxiv.org/abs/1804.04011) — Object-level SLAM의 대표 논문
> - [ConceptGraphs (arXiv:2309.16650)](https://arxiv.org/abs/2309.16650) — Open-vocabulary 3D scene graph. 13장과 연계해서 읽을 것
> - [Bescos et al., "DynaSLAM: Tracking, Mapping and Inpainting in Dynamic Scenes" (RA-L 2018)](https://arxiv.org/abs/1806.05620) — 동적 환경 SLAM

### 14.13 심화: Multi-Robot SLAM

한 대의 로봇이 넓은 환경을 탐색하려면 시간이 오래 걸린다. 여러 로봇이 동시에 나눠서 탐색하면 시간을 줄일 수 있지만, 각 로봇이 만든 부분 지도(submap)를 하나의 일관된 글로벌 지도로 합치는 것은 단순하지 않다.

**Centralized 접근**은 여러 로봇의 센서 데이터나 local map을 중앙 server로 보내 공동 최적화를 수행한다. global information에 접근하기 쉬운 대신 통신량과 server 계산량이 커질 수 있고, server가 단일 장애점이 된다. 비볼록 SLAM에서 중앙화 자체가 전역 최적해를 보장하지는 않는다.

**Distributed 접근**은 각 로봇이 local SLAM을 수행하고 rendezvous나 inter-robot loop closure에서 relative pose constraint를 만든다. 원본 data 대신 descriptor나 submap을 교환하면 통신량을 줄일 수 있지만 검증에 필요한 정보도 줄어든다. 각 robot이 어떤 변수와 factor를 보유·교환하는지, 그리고 asynchronous optimization이 어떤 조건에서 수렴하는지는 algorithm마다 다르다.

분산 시스템에서는 **inter-robot loop closure**, **좌표계 정렬**, **outlier rejection**을 함께 다뤄야 한다. Relative SE(3) 변환은 사전에 검증된 6자유도 포즈 제약으로 직접 주어질 수 있으며, 3차원 점 대응쌍으로 계산할 경우 퇴화(degenerate)하지 않는 최소 세 점과 충분한 인라이어 집합이 요구된다. PCM(Pairwise Consistency Maximization), GNC(Graduated Non-Convexity), distributed Gauss-Seidel, ADMM 등은 각기 다른 가정과 통신 비용을 가지므로 system 조건에 맞춰 선택한다.

**대표 시스템**:
| 시스템 | 특징 |
|---|---|
| **Kimera-Multi** | Distributed, 3D mesh + semantic, Kimera 기반 |
| **DOOR-SLAM** | Distributed, outlier-robust, DGS 최적화 |
| **Swarm-SLAM** | ROS2 기반, 다양한 센서 지원, 경량 |

> **추천 자료**
> - [Lajoie et al., "DOOR-SLAM: Distributed, Online, and Outlier Resilient SLAM for Robotic Teams" (RA-L 2020)](https://arxiv.org/abs/1909.12198) — Distributed SLAM + robust optimization
> - [Tian et al., "Kimera-Multi: Robust, Distributed, Dense Metric-Semantic SLAM for Multi-Robot Systems" (T-RO 2022)](https://arxiv.org/abs/2106.14386) — Multi-robot semantic SLAM
> - [Cieslewski et al., "Data-Efficient Decentralized Visual SLAM" (ICRA 2018)](https://arxiv.org/abs/1710.05772) — 통신 효율적인 분산 SLAM의 초기 연구

### 14.14 심화: Place Recognition

Loop closure의 핵심 문제는 "지금 보는 장면을 이전에 본 적 있는가?"이다. 이 질문은 이미지 검색(image retrieval) 문제다. 현재 프레임의 descriptor를 과거 모든 키프레임의 descriptor와 비교해서 가장 유사한 것을 찾는다. SLAM의 정확도는 loop closure에, loop closure는 place recognition에 달려 있다.

**전통적 방법: Bag of Visual Words (BoVW)**

DBoW2 라이브러리가 대표적이며 ORB-SLAM2/3에서 사용된다.
1. 대규모 이미지에서 local feature(ORB 등)를 추출
2. k-means clustering으로 visual vocabulary(단어 사전) 구축
3. 각 이미지를 "어떤 visual word가 몇 번 나타났는지"의 histogram(BoW vector)으로 표현
4. BoW vector 간의 유사도(L1-score 등)로 이미지 비교

장점: 빠르다 (inverted index 사용), 검증된 방법. 단점: 시점/조명 변화에 취약, vocabulary 학습이 필요.

**학습 기반 방법: Global Descriptor**

이미지 전체를 하나의 compact vector로 압축하는 방식이다. **NetVLAD** (2016)는 합성곱 신경망 특징과 VLAD 집계 기법을 통합 설계한 모델로, 도시 규모 벤치마크 평가에서 기존 비교 대상 대비 향상된 재현율(recall)을 입증했다. **CosPlace** (2022)와 **MixVPR** (2023)은 각 논문의 dataset·protocol에서 descriptor 학습과 aggregation의 개선을 평가했다. **AnyLoc** (2023)은 DINOv2 feature를 활용해 여러 indoor·outdoor·aerial dataset에서 별도 place-recognition fine-tuning 없는 결과를 보고했다. 이 결과는 모든 환경에서 BoVW보다 robust하다는 보장이 아니므로 target domain에서 recall과 false positive를 다시 측정해야 한다.

**LiDAR 기반 Place Recognition**:

시각 정보 없이 3D 구조만으로 장소를 인식하므로 영상 밝기 변화에 직접 의존하지 않는다. 다만 날씨와 동적 물체가 point return을 바꿀 수 있고, 구조적으로 유사한 환경(예: 긴 복도)에서는 혼동될 수 있다.

- **Scan Context** (IROS 2018): 3D 포인트 클라우드를 bird-eye view로 투영한 뒤, 거리/높이 기반의 2D descriptor 생성. Rotation-invariant한 매칭 가능
- **OverlapTransformer** (2022): Transformer 기반으로 LiDAR range image에서 global descriptor를 학습

**Cross-modal Place Recognition**: 카메라 이미지로 query하고 LiDAR 지도에서 검색하거나, 그 반대. 센서가 다른 로봇 간의 multi-robot SLAM에서 중요하다.

**Sequence Matching**: 단일 이미지 매칭의 한계를 극복하기 위해, 연속된 프레임의 시퀀스를 함께 매칭한다.
- **SeqSLAM** (2012): 이미지 개별 유사도는 낮아도, 시퀀스 패턴이 일치하면 같은 장소로 판단. 극적인 외관 변화(주간 vs 야간)에서도 동작
- 최근 방법: sequence descriptor를 학습하여 더 효율적으로 시퀀스 매칭

실무 팁: DBoW2는 ORB-SLAM 계열에서 쓰이는 공개 baseline이다. 조명·계절 변화가 크다면 NetVLAD 이후의 학습 기반 descriptor도 같은 데이터에서 비교하라. AnyLoc은 별도 fine-tuning 없이 feature를 구성하지만 backbone 연산량, descriptor memory, target-domain recall을 확인해야 한다.

§14.16.5의 cycle posterior도 place 일치 신호 뒤 과거 trajectory를 보정한다는 기능적 구조를 갖는다. 현대 loop closure가 그 알고리즘에서 직접 유래했다고 단정하지는 않는다.

> **추천 자료**
> - [Arandjelovic et al., "NetVLAD: CNN architecture for weakly supervised place recognition" (arXiv:1511.07247)](https://arxiv.org/abs/1511.07247) — 학습 기반 place recognition의 시작점
> - [Keetha et al., "AnyLoc: Towards Universal Visual Place Recognition" (arXiv:2308.00688)](https://arxiv.org/abs/2308.00688) — Foundation model 기반 zero-shot place recognition
> - [Kim & Kim, "Scan Context: Egocentric Spatial Descriptor for Place Recognition within 3D Point Cloud Map" (IROS 2018)](https://ieeexplore.ieee.org/document/8593953) — LiDAR place recognition의 대표 방법
> - [김기섭 블로그 — Scan Context-based LiDAR Pose-graph SLAM 구현](https://gisbi-kim.github.io/blog/2021/05/17/sclidarslam.html) — Scan Context를 LiDAR SLAM에 통합한 구현 해설
> - [다크 프로그래머 — Bag of Words 기법](https://darkpgmr.tistory.com/125) — BoW의 원리를 이미지 검색과 연결하여 설명

> **기술 흐름: SLAM & Odometry**
> - **~2007**: 고전기. EKF-SLAM, FastSLAM(파티클 필터 기반)이 주류. MonoSLAM(2007)이 실시간 단안 SLAM의 시작을 알림. PTAM(2007)이 Tracking/Mapping 분리 아키텍처를 제안
> - **2010~2015**: LSD-SLAM, SVO 등 direct method 등장. LOAM(2014)이 LiDAR odometry와 mapping의 영향력 있는 구조를 제시했고, ORB-SLAM(2015)이 공개 feature-based visual SLAM baseline을 제공
> - **2015~2020**: VINS-Mono, MSCKF 계열 등 VIO system의 공개 구현과 적용 확대. DSO가 direct sparse 방식을 제시했고, LIO-SAM 등 LiDAR-inertial system 등장
> - **2020~2023**: FAST-LIO/FAST-LIO2, ORB-SLAM3, DROID-SLAM, R3LIVE 등 filter·optimization·learning을 서로 다르게 조합한 공개 system 등장
> - **2024~**: 3DGS 기반 SLAM(SplaTAM, MonoGS, Gaussian-SLAM)이 Neural SLAM의 방향을 바꾸고 있다. Foundation Model과 SLAM의 결합(예: 자연어로 장소를 설명하여 위치를 찾는 등) 연구도 시작
> - **최근 흐름**: geometric SLAM은 ORB-SLAM3, LIO-SAM, FAST-LIO2처럼 공개 구현과 벤치마크가 축적된 방법을 중심으로 발전해 왔다. 한편 3DGS-SLAM과 learning-based 방법은 장면 표현과 front-end의 선택지를 넓히고 있다. KITTI, EuRoC, TUM RGB-D에서 두 계열의 입력 조건과 실패 사례를 비교할 수 있다.

### 14.15 심화: Long-term Mapping

실제 환경에서 로봇을 운용하면 "지도를 한 번 만들고 끝"이 아니다. 같은 장소를 여러 번 방문하면서 지도를 업데이트하고, 동적 물체(사람, 차량)를 제거하고, 여러 세션의 데이터를 통합해야 한다. 이것이 long-term mapping이고, 실용적인 로봇 시스템에서 피할 수 없는 문제다.

#### 14.15.1 Incremental Smoothing: iSAM에서 iSAM2까지

Filter-based SLAM(EKF 등)은 state 수가 늘어나면 Jacobian 행렬이 커져서 실시간 처리가 어렵다. iSAM(Kaess et al., TRO 2008)은 QR factorization의 R matrix를 Givens rotation으로 incremental하게 업데이트할 수 있음을 보여줬다. 새로운 measurement가 추가될 때 변경된 부분만 갱신하면 된다. 다만 non-zero element가 누적되면 주기적으로 re-ordering이 필요하다.

iSAM2(Kaess et al., IJRR 2012)는 Bayes tree 구조를 도입하여 이 한계를 극복했다. 영향받는 subtree만 re-elimination하므로, 대규모 문제에서도 일관된 성능을 보인다. GTSAM의 핵심 엔진이 바로 iSAM2다.

#### 14.15.2 Dynamic Object Removal

지도에서 동적 물체를 제거하는 것은 long-term mapping의 필수 과제다.

**Removert** (Kim et al., 2020): multi-resolution range image를 이용해 static/dynamic을 분류한다. Point cloud를 range image로 투영한 뒤, 다른 시점에서 관측한 range와 비교하여 동적 여부를 판단한다. 먼저 static point를 보수적으로 확보하고, 잘못 제거한 point를 복원하는 two-stage 방식을 사용한다. 여러 confidence level을 두어 이 두 단계의 trade-off를 조절할 수 있다.

기존 동적 물체 제거 기법들과 대비되는 지점은 다음과 같다. 복셀 레이캐스팅은 정밀한 대신 연산 비용이 과다하며, 가시성(visibility) 기반 방식은 후면의 정적 포인트 보존이라는 제약 가정을 둔다. 또한 시맨틱 세분화 기반 방식은 미등록 레이블에 취약하고 스캔과 지도 간 기하학적 정합 관계를 충분히 살리지 못한다. Removert는 이 세 방법의 단점을 multi-resolution range image 비교로 보완한다.

**SuMa++** (Chen et al., IROS 2019): surfel-based mapping에 semantic label을 추가한 시스템이다. LiDAR point에 normal과 semantic 정보를 더하고, semantic과 motion 양쪽에서 dynamic으로 판정된 surfel만 제거한다. Motion-degenerate 환경에서는 움직이는 point도 geometric constraint로 유용할 수 있기 때문이다.

#### 14.15.3 Multi-Session SLAM

같은 환경을 여러 날에 걸쳐 매핑하면, 각 세션의 trajectory를 하나로 합쳐야 한다. 문제는 gauge freedom — 각 세션의 좌표계가 다르므로 단순히 합치면 안 맞는다.

**LT-mapper** (Kim et al., 2021): Scan Context 기반 anchor node로 multi-session을 정렬하고, positive/negative change detection으로 지도를 업데이트한다. 변화를 high dynamic과 low dynamic으로 나누고, low dynamic을 다시 positive difference(새로 생긴 점)와 negative difference(사라진 점)로 구분하여 delta map을 관리한다.

**Continuous-Time Estimation** (Furgale et al., ICRA 2012): discrete-time 대신 B-spline basis function으로 trajectory를 표현하면, 다른 Hz의 센서들을 더 적은 변수로 통합할 수 있다. 빠른 센서(IMU)와 느린 센서(LiDAR, 카메라) 사이의 self-calibration에도 활용 가능하다.

> **추천 자료**
> - [Kaess et al., "iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree" (IJRR 2012)](https://www.cs.cmu.edu/~kaess/pub/Kaess12ijrr.pdf) — Bayes tree 기반 incremental SLAM의 원본 논문
> - [Kim et al., "Remove, then Revert: Static Point Cloud Map Construction using Multiresolution Range Images" (IROS 2020)](https://github.com/irapkaist/removert) — Dynamic point 제거의 실용적 방법. 코드 공개
> - [Kim et al., "LT-mapper: A Modular Framework for LiDAR-based Lifelong Mapping" (ICRA 2022)](https://github.com/gisbi-kim/lt-mapper) — Multi-session SLAM 프레임워크
> - [Chen et al., "SuMa++: Efficient LiDAR-based Semantic SLAM" (IROS 2019)](https://github.com/PRBonn/semantic_suma) — Semantic 정보를 활용한 LiDAR SLAM

---

### 14.16 심화: 정보형 SLAM의 역사

*§14.9 factor graph 최적화(양방향 참조).*

2005년 *Probabilistic Robotics*가 출간될 무렵에는 SLAM의 정보 표현을 두고 여러 접근이 경쟁했다. EKF-SLAM이 결합 상태 벡터와 공분산 행렬을 추적했다면, EIF/SEIF는 정보 행렬의 가산적 특성을 바탕으로 계산 효율을 도모했고 EM mapping은 미지 데이터 연관 문제를 기댓값 최대화로 풀어냈다. 2010년대의 factor graph와 GTSAM/iSAM2는 이 계보에서 정보형의 가산성, 변수 소거, 증분 갱신을 이어받아 현대적인 최적화 구조로 정리했다.

#### 14.16.1 EKF-SLAM (PR §10)

**Smith, Self, Cheeseman (1986/1990)** "Estimating Uncertain Spatial Relationships in Robotics"에서 시작된 계보다. 이들이 제안한 'stochastic map' — 로봇 포즈와 랜드마크를 하나의 확률 변수로 묶는다는 발상 — 이 EKF-SLAM의 원형이다. 1990년대 Leonard·Durrant-Whyte, 그리고 Dissanayake et al. (2001, IEEE T-RA)이 정식화를 완성했다.

**알고리즘 골격**: 포즈 $x_t = (x, y, \theta)$와 N개 랜드마크 $(m_{j,x}, m_{j,y}, s_j)$를 $(3N+3)$차원 상태 벡터 $y_t$로 묶고 EKF를 굴린다.

```
EKF_SLAM_known_correspondences(μ_{t-1}, Σ_{t-1}, u_t, z_t, c_t):
  // Motion: F_x로 3D motion을 (3N+3)D로 lift
  // F_x = [I_3 | 0_{3×3N}] — (3×(3N+3)) 프로젝션, F_x^T 는 (3N+3)×3
  // G_t = I_{3N+3} + F_x^T G_t^{pose} F_x — (3N+3)×(3N+3), G_t^{pose}는 포즈 3×3 Jacobian
  μ̄_t = μ_{t-1} + F_x^T · g(u_t, μ_{t-1}[포즈 부분])
  Σ̄_t = G_t Σ_{t-1} G_t^T + F_x^T R_t F_x

// Measurement loop
  for each observation z_t^i with j = c_t^i do
    if j is new landmark:
      μ̄_{j} ← range-bearing 역변환으로 초기화
    ẑ_t^i = h(μ̄_t, j),   H_t^i = Jacobian  // H_t^i: 3×(3N+3)
    K_t^i = Σ̄_t H_t^{i,T} (H_t^i Σ̄_t H_t^{i,T} + Q_t)^{-1}
  endfor

// Update
  μ_t = μ̄_t + Σ_i K_t^i (z_t^i − ẑ_t^i)
  Σ_t = (I − Σ_i K_t^i H_t^i) Σ̄_t
  return μ_t, Σ_t
```

**Kalman gain $K_t^i$는 $(3N+3) \times 3$ 행렬**이다 — 단일 랜드마크 관측이 전체 상태를 갱신한다. 한 관측이 공분산 off-diagonal을 통해 다른 랜드마크 추정을 개선하지만, 업데이트 비용이 $O(N^2)$로 늘어난다는 대가가 따른다.

미지 대응(unknown correspondence)이면 가상의 $(N_t+1)$번째 랜드마크를 맵 끝에 잠시 추가하고, 모든 후보에 대해 마할라노비스 거리를 계산하여 ML 대응을 고른다. 임계값 $\alpha$ 초과면 신규 랜드마크로 등록한다. 이 greedy ML 결정이 한번 틀리면 회복할 수 없다는 것이 ML data association의 근본 약점이다.

EKF-SLAM의 한계는 세 방향에서 드러났다. 공분산 행렬 $\Sigma \in \mathbb{R}^{(3N+3) \times (3N+3)}$의 메모리는 $N^2$에 비례한다 — 100개 랜드마크면 303×303, 1000개면 3003×3003. 랜드마크가 추가될수록 과거의 선형화 오차가 쌓여 추정이 inconsistent해진다 (Bailey et al. 2006). 과거 포즈를 marginalize한 상태를 유지하므로 과거 포즈는 다시 볼 수 없어 full posterior 최적화가 불가능하다.

대규모 landmark map에서는 dense covariance와 누적 선형화 오차 때문에 고전적 full-state EKF-SLAM보다 smoothing·factor-graph 방식이 흔하다. 그렇다고 EKF-SLAM이 특정 landmark 수 아래에서만 유효하다는 보편 경계가 있는 것은 아니다. 계산 예산, 관측 sparsity, consistency 요구에 따라 소규모 landmark system이나 fiducial mapping에서 사용할 수 있다. MSCKF류 sliding-window filter는 EKF 선형화와 covariance propagation을 쓰지만 landmark를 상태에 영구 보관하지 않는 별도 정식화다. JCBB는 ML data association의 대안이다. GTSAM/iSAM2는 같은 SLAM 문제를 smoothing과 factor graph로 푸는 후속 세대이지만 EKF-SLAM의 직계 알고리즘 후예는 아니다.

#### 14.16.2 GraphSLAM — 정보형 배치 SLAM (PR §11)

EKF-SLAM에서 $\Sigma$는 측정마다 dense한 상관관계를 갱신한다. 정보형 $\Omega = \Sigma^{-1}$에서는 독립 factor의 기여를 국소적으로 더할 수 있어 SLAM graph의 sparsity를 드러내기 쉽다. 다만 motion update와 marginalization은 fill-in을 만들 수 있으므로 정보 행렬이 자동으로 계속 sparse한 것은 아니다. 이것이 EIF와 sparse graph formulation을 살펴보는 동기다.

##### 정보형의 직관: 스프링-매스 비유

EIF SLAM의 핵심 발상은 **정보는 가산량이다**. 공분산 $\Sigma$ 대신 정보 행렬 $\Omega = \Sigma^{-1}$과 정보 벡터 $\xi = \Omega \mu$를 사용한다.

이것을 스프링-매스 시스템으로 보면: 각 변수(포즈, 랜드마크)는 노드, $\Omega$의 비대각 원소는 두 노드를 잇는 스프링이다.
- Control $u_t$: $x_{t-1}$과 $x_t$ 사이 스프링. Stiffness = $R_t^{-1}$ (motion noise가 작을수록 강한 결속).
- Measurement $z_t^i$: 포즈 $x_t$와 랜드마크 $m_j$ 사이 스프링. Stiffness = $Q_t^{-1}$.
- 두 다른 랜드마크 사이 직접 스프링은 없다 — 랜드마크끼리 직접 측정한 적이 없으니.

**정보형 갱신식**:
$$\Omega \leftarrow \Omega + H_t^{iT} Q_t^{-1} H_t^i, \qquad \xi \leftarrow \xi + H_t^{iT} Q_t^{-1}[z_t^i - h(\mu_t) + H_t^i \mu_t]$$

Measurement information은 **국소 덧셈**으로 추가할 수 있다. 다만 motion update와 marginalization에는 elimination과 fill-in이 생길 수 있으므로 모든 연산이 국소적인 것은 아니다. Factor graph도 각 factor가 일부 변수만 연결한다는 유사한 spring 직관을 쓸 수 있지만, factor graph의 역사나 정식화가 SEIF에서 시작한 것은 아니다.

##### 4단계 파이프라인

GraphSLAM은 full posterior $p(x_{0:t}, m | z_{1:t}, u_{1:t})$를 정보형으로 배치(offline) 처리한다. 같은 정보형이지만 EIF·SEIF는 과거 상태를 marginalize하는 온라인 필터이므로 목표와 상태 보관이 다르다.

```
GraphSLAM_known_correspondence(u_{1:t}, z_{1:t}, c_{1:t}):
  1. Initialize:  μ_{0:t} ← motion model만으로 초기 추정 (관측 무시)
  2. Construct:   Ω = 0, ξ = 0에서 출발,
                  prior·controls·measurements를 국소 덧셈으로 누적
  3. Reduce:      각 랜드마크 j에 대해 Schur complement로 소거
                  Ω̄ ← Ω̄ − Ω_{τ(j),j} Ω_{j,j}^{-1} Ω_{j,τ(j)}
                  ξ̄ ← ξ̄ − Ω_{τ(j),j} Ω_{j,j}^{-1} ξ_j
                  → 포즈만 남은 reduced Ω̄, ξ̄
  4. Solve:       Σ_{0:t} = Ω̄^{-1},  μ_{0:t} = Σ_{0:t} ξ̄
                  각 랜드마크: μ_j = Ω_{j,j}^{-1}(ξ_j − Ω_{j,τ(j)} μ_{τ(j)})
  전체 2-3회 반복 (linearization 개선)
  return μ_{0:t}, {μ_j}
```

$\tau(j)$는 랜드마크 $j$를 본 모든 포즈 시점이다. Reduce 단계에서는 *각 랜드마크에 인접한 포즈끼리 새 스프링을 만들고 랜드마크 노드를 떼어낸다*. 이는 Bundle Adjustment의 **block diagonal Schur complement** 트릭과 수학적으로 동일하다 (§14.9.2 참조).

**Marginalization Lemma**: 선형 Gaussian 정보형의 marginal은 Schur complement로 표현된다. 이 연산은 남은 변수 사이에 fill-in을 만들 수 있다.
$$\bar\Omega_{xx} = \Omega_{xx} - \Omega_{xy} \Omega_{yy}^{-1} \Omega_{yx}$$

Thrun & Montemerlo (2006, IJRR)의 GraphSLAM은 전체 trajectory와 map posterior를 sparse information graph로 batch 최적화한다. EIF와 정보형 가산성을 공유하지만 online filtering과 batch smoothing은 구분해야 한다. Lu & Milios (1997)는 pose relation을 전역 최적화하는 앞선 연구다.

GraphSLAM의 한 unknown-correspondence 절차는 feature 쌍 $(m_j, m_k)$의 동일성 후보를 평가하고 선택한 제약을 graph에 추가한 뒤 다시 최적화한다. Factor를 명시적으로 보관하면 선택한 제약을 제거하고 재최적화할 수 있다. Switchable constraints (Sünderhauf & Protzel 2012)의 경우 loop factor의 활성도를 연속 최적화 변수로 직접 모델링하는 독립된 견고성(robustness) 정식화다. 따라서 단순한 역사적 파생 관계로 동일시하는 해석은 피해야 한다.

Initialize → factor 구성 → variable elimination → solve라는 단계는 현대 batch SLAM과 비교할 수 있다. iSAM/iSAM2는 새로운 factor 유입 시 선형화와 소거 영향권을 국소 갱신하는 증분 평활화(incremental smoothing) 기법을 기반으로 삼는다. 이때 Bayes tree는 이러한 행렬 분해 구조를 트리 형태로 체계화하여 관리하는 핵심 자료구조다. GraphSLAM이 단순히 이름만 바뀌어 진화한 것으로 보아서는 안 된다.

#### 14.16.3 SEIF — Sparse Extended Information Filter (PR §12)

앞 절의 GraphSLAM은 batch smoothing formulation이다. 일반적인 EIF는 online filter로도 쓸 수 있지만, 정확한 motion update가 information matrix를 dense하게 만들 수 있다. **SEIF**는 active feature 수를 제한하고 sparsification 근사를 도입해 map-size-independent update를 목표로 한 online filter다. Thrun et al. (2004, IJRR)은 Victoria Park 3.5 km 실험의 해당 구현에서 EKF-SLAM 대비 약 절반의 시간과 4분의 1의 메모리로 유사한 오차를 보고했다.

##### 4단계 업데이트

```
SEIF_SLAM_known_correspondences(ξ_{t-1}, Ω_{t-1}, μ_{t-1}, u_t, z_t, c_t):
  1. Motion update:       ξ̄_t, Ω̄_t, μ̄_t ← u_t로 정보형 갱신
                          (active feature + robot pose만 변경, sparse 유지)
  2. Measurement update:  Ω_t ← Ω̄_t + Σ_i H_t^{iT} Q_t^{-1} H_t^i  [가산]
                          ξ_t ← ξ̄_t + 해당 항 가산
  3. Sparsification:      일부 active feature를 passive로 강제
                          — robot과의 link를 끊고 정보를 인접 노드에 재분배
  4. State estimate:      amortized coordinate descent로
                          active feature 추정만 incremental 갱신
  return ξ_t, Ω_t, μ_t
```

##### Sparsification

SEIF의 핵심 메커니즘이다. 변수 $a, b$ 사이의 직접 의존성을 두 marginal의 곱으로 근사하여 $\Omega$에서 0 원소를 만든다.

$$\tilde p(a,b,c) = \frac{p(a,c)\, p(b,c)}{p(c)} \quad \Longrightarrow \quad \Omega_{a,b} = 0$$

이 근사는 $a \perp b | c$를 강제하는 KL projection으로 설명할 수 있다. 그러나 sparsification과 반복 선형화가 filter consistency에 미치는 영향은 별도로 평가해야 하며, 단순히 "분산이 절대 줄지 않는다"고 일반화할 수 없다.

Active feature 수 $K$를 상수로 고정하면 local motion·measurement update의 핵심 행렬 크기를 $(2K+3) \times (2K+3)$로 제한할 수 있다. 이는 map size에 대한 update 복잡도를 상수로 만드는 근거이지만, 전체 map state recovery나 data association 비용까지 자동으로 O(1)이 되는 것은 아니다.

*Probabilistic Robotics*의 예시는 약 6개 active feature를 사용한다. 이는 보편 권장값이 아니며 sensor geometry와 map에 맞춰 consistency와 계산량을 함께 확인해야 한다. Eustice et al. (2006)의 Exactly Sparse EIF는 특정 구조에서 sparsification approximation을 피하는 접근을 다룬다. PR Figure 12.3은 measurement, motion, sparsification 단계의 link 변화를 보여준다.

##### 트리 기반 데이터 연관

정보형의 가산성은 data association에서도 특별한 능력을 준다: **소프트 대응 제약을 더하거나 뺄 수 있다**. 두 feature $m_i, m_j$가 동일하다는 soft constraint를

$$\Omega \leftarrow \Omega + F_{m_i - m_j}^T C\, F_{m_i - m_j}$$

로 추가하고, 그 factor를 별도로 보관했다면 제거할 수 있다. 이 구조로 data association tree를 A*류 frontier search로 탐색할 수 있지만 worst-case 가설 수는 지수적으로 늘어난다. Switchable constraints (Sünderhauf & Protzel 2012)와 Max-mixtures (Olson & Agarwal 2013)도 잘못된 loop closure에 robust하도록 설계됐다는 문제의식을 공유하지만, 각각 switch variable과 mixture factor를 사용하는 별도 정식화다.

##### 다중 로봇 맵 융합

정보형에서는 서로 독립적인 관측 factor의 기여를 더할 수 있어 multi-robot fusion을 표현하기 편하다. 두 로봇 $j, k$의 상태가 같은 좌표계에 있고 두 estimate 사이에 중복된 prior·관측 정보가 없다는 조건에서 정보 항을 더할 수 있다.

$$\Omega^{\text{fused}} = \Omega^{j \leftarrow k\text{-aligned}} + \Omega^k, \qquad \xi^{\text{fused}} = \xi^{j \leftarrow k\text{-aligned}} + \xi^k$$

공통 정보를 추적하지 않고 더하면 같은 관측을 두 번 세어 over-confidence가 생긴다. 공분산형도 단순 덧셈은 할 수 없지만 covariance intersection 같은 보수적 fusion 방법이 있다. Nettleton et al. (2003)은 분산 정보 fusion을 다뤘고, DDF-SAM, Kimera-Multi, Swarm-SLAM도 multi-robot estimation을 다루지만 통신 모델과 중복 정보 처리 방식은 서로 다르다(§14.13 참조).

SEIF와 iSAM2는 각각 approximate information filtering과 incremental smoothing이라는 다른 문제 설정이다. iSAM2도 비선형 문제에서는 linearization point와 relinearization 정책의 영향을 받는다. ESEIF는 SEIF의 sparsity를 더 정확히 다루는 계열이지만, VINS-Mono·OKVIS의 sliding-window marginalization과 MSCKF의 feature elimination은 제한된 상태를 유지하기 위한 별도 기법이다. 공통점은 sparsity와 계산량을 관리한다는 데 있다(§14.9.2 참조).

#### 14.16.4 EM Mapping

정보형 가산성에 sparsification을 더한 것이 SEIF다. 모호한 data association을 통계적으로 다루는 계열은 이보다 앞서 있었다. MHT와 JPDA가 다중 가설과 확률적 연관을 세웠고 FastSLAM은 입자마다 연관을 두었다. EM Mapping은 그중 지도 추정과 연관을 번갈아 푸는 접근이다.

EKF-SLAM, EIF SLAM, SEIF는 모두 data association이 알려졌거나, ML greedy하게 결정한다고 가정했다. EM Mapping은 **unknown data association을 EM의 latent variable로 다뤄서 ambiguous한 데이터까지 활용**한다. Thrun, Burgard, Fox(1998–2000, AAAI/JAIR)의 원형은 RHINO 박물관 가이드 로봇(Burgard et al. 1999)에 변형되어 쓰였다.

##### E-step / M-step 골격

```
EM_mapping(d):
  m ← uniform map 초기화
  repeat until satisfied:
    // E-step (forward α)
    α^(0) = δ(⟨0,0,0⟩)
    for t = 1 to T:
      α^(t) = η P(o^(t)|s^(t),m) ∫ P(s^(t)|a^(t-1),s^(t-1)) α^(t-1) ds^(t-1)

// E-step (backward β)
    β^(T) = uniform
    for t = T-1 downto 0:
      β^(t) = ∫ P(o^(t+1)|s^(t+1),m) P(s^(t+1)|a^(t),s^(t)) β^(t+1) ds^(t+1)

// E-step (combine)
    Bel(s^(t)) = α^(t) · β^(t)   [정규화]

// M-step
    for each cell ⟨x,y⟩, property l:
      m_{⟨x,y⟩=l} ∝ Σ_t ∫ P(o^(t)|s^(t),m_{⟨x,y⟩}=l) · I_{⟨x,y⟩ ∈ range} · Bel(s^(t)) ds^(t)
    정규화
  return m
```

$\alpha$는 forward localization (Markov localization), $\beta$는 backward (미래 데이터로 과거 belief 보정). $\beta$ 덕분에 루프를 닫는 시점에 시간 역방향으로 과거 belief가 소급 수정된다 — 이것이 EM mapping의 통계적 핵심이다. forward-backward 구조는 HMM의 Baum-Welch와 동일하다.

M-step은 frequentist count: "셀이 property l로 관측된 횟수 / 무엇이든 관측된 횟수", belief로 가중. 3~5회 반복이면 보통 수렴한다.

##### Layered EM Mapping

기본 EM_mapping의 M-step이 sensor cone에서 기하학적 일관성을 깨뜨리는 문제를 해결한 변종이다. 짧은 motion segment마다 **로컬 occupancy grid**를 먼저 만들고, EM은 그 로컬 맵의 *위치*만 최적화한다. **Deterministic annealing** ($\sigma: 1.0 \to 0$으로 냉각)으로 EM의 local maxima 함정을 회피한다.

```
layered_EM_mapping(d):
  1. 각 t: m^(t) = occupancy_grid(o^(t))  [로컬 맵 생성]
     Bel(s^(t)) ← uniform 초기화
  2. repeat until satisfied  [σ = 1.0 → 0]:
     E-step (α, β)  [layered perceptual model 사용]
     M-step (annealed): Bel(s^(t)) = η (α^(t) β^(t))^{1/σ}
     σ ← 0.9σ
  3. 각 로컬 맵의 ML 포즈 추출 → occupancy_grid()로 글로벌 합성
  return m_global
```

Deterministic annealing과 GNC (Yang et al. 2020)·robust kernel scheduling은 목적 함수의 난도를 단계적으로 높여 나쁜 국소해를 피한다는 설계 원리를 공유한다.

##### EM Mapping이 주류에서 벗어난 이유

EM_mapping과 layered_EM_mapping은 현대 SLAM 시스템의 주류가 아니다. 이유:
- pose와 map을 E/M-step으로 분리하려는 시도는 factor graph의 joint optimization이 대체하면서 설 자리를 잃었다.
- batch·offline 성격이 실시간 SLAM과 맞지 않는다.
- Cartographer (Hess et al. 2016)는 EM 없이 scan matching과 pose graph 최적화로 직접 풀었다. GMapping (Grisetti et al. 2007)도 EM을 쓰지 않지만 구조가 다르다. scan matching으로 제안 분포를 개선한 Rao-Blackwellized particle filter이며 pose graph를 쓰지 않는다.

다만 layered EM의 **submap + global alignment**와 Cartographer의 local/global SLAM은, 로컬 지도를 먼저 만들고 전역적으로 정렬한다는 구조적 공통점이 있다. 이것은 직접적인 계보를 뜻하기보다 같은 계산 문제를 나누는 두 설계로 이해하는 편이 정확하다.

#### 14.16.5 Cycle Posterior

stepwise ML mapper는 두 가지 한계를 가진다: (1) 큰 odometry error를 견디지 못하고, (2) 과거 자세를 시간 역방향으로 보정할 수 없다. cycle posterior 접근은 ML mapper와 *동시에* 자세 사후분포 추정기를 병행 구동하여 두 결함을 해결한다.

**알고리즘 골격**:
```
Incremental Mapping with Posterior Estimation:
  1. incremental_ML_mapping(o, a, s, m)  → ⟨m', s'⟩    [ML 갱신]
  2. Bel(s') = P(o,s') ∫ P(s'|a,s) Bel(s) ds            [사후분포 한 스텝]
  3. s'' = argmax Bel(s')                                 [사후 mode]
  4. s'' ≠ s'  →  cycle closure 검출
                   s'' − s'를 cycle 경로 따라 선형 분배
  5. incremental_ML_mapping을 시간 역방향으로 재실행       [nested ML refinement]
```

사후분포가 급격히 좁아지는 사건을 cycle closure로 보고, 좁아진 mode와 ML 추정값의 차이를 보정 신호로 쓴다. ML mapper와 posterior estimator를 함께 구동하므로 MCL 기반 구현과 잘 맞으며, odometry 없이도 동작하도록 설계됐다. 명시적인 cycle detection과 backwards correction을 한 프레임워크에 묶은 초기 사례라는 점에서 이후의 online loop-closure 시스템과 비교해 볼 수 있다. 다만 현대의 place recognition과 incremental graph optimization이 이 알고리즘에서 직접 계승되었다고 단정할 수는 없다.

MCL, linear distribution, nested ML을 그대로 결합한 구현은 현재 널리 쓰이지 않는다. 그러나 detector(place recognition)와 corrector(graph optimization)를 나누고, closure가 확인되면 graph 전체의 상태를 다시 최적화하는 구조는 현대 SLAM에서도 볼 수 있다. iSAM (Kaess et al. 2008)·iSAM2 (2012)·GTSAM의 incremental smoothing은 변화의 영향을 받는 부분을 선택적으로 갱신한다.

#### 14.16.6 정리: 무엇이 살아남았나

*Probabilistic Robotics*(2005)의 정보형 SLAM에서 현재 시스템으로 이어진 요소를 정리하면 다음과 같다.

| PR 알고리즘 | 핵심 기여 | 비교할 현대 구조 | 관계의 범위 |
|---|---|---|---|
| EKF-SLAM | 통합 landmark state, off-diagonal covariance | MSCKF류 filter, fiducial mapping | EKF를 공유하지만 state 구성은 다름 |
| GraphSLAM | 정보형 factor 가산, variable elimination, full trajectory | GTSAM, g2o, Ceres, iSAM2 | sparse least squares와 smoothing 관점이 이어짐 |
| SEIF | bounded active set, sparsification | ESEIF, bounded-state estimator | SEIF 계열과 다른 sparse estimator를 구분해야 함 |
| EM Mapping | latent data association, forward-backward localization, submap | EM 기반 mapping, submap system | 통계 기법·구조의 비교이며 직접 계보는 아님 |
| Cycle Posterior | online closure detection과 correction 결합 | place recognition + graph optimization | 기능 분할이 유사함 |

정보형의 **가산성**은 현대 factor cost의 합과 직접 비교할 수 있다. 이에 비해 SEIF의 **sparsification**, sliding-window marginalization, Bayes-tree elimination은 연산 부담 완화라는 공통 목적을 공유할 뿐 엄연히 구별되는 연산 절차다. Cycle posterior의 detector/corrector 구분과 EM mapping의 submap은 현대 시스템과 구조적으로 비교할 수 있으나, 이를 특정 구현의 직접 계보로 단정하지 않는다(§14.9 참조).

> **참고 자료**
> - [Thrun et al., "Simultaneous Localization and Mapping with Sparse Extended Information Filters" (IJRR 2004)](https://journals.sagepub.com/doi/10.1177/0278364904045026) — SEIF 원본 논문
> - [Thrun & Montemerlo, "The GraphSLAM Algorithm with Applications to Large-Scale Mapping of Urban Structures" (IJRR 2006)](https://journals.sagepub.com/doi/10.1177/0278364906065390) — EIF/GraphSLAM 정식화
> - [Dissanayake et al., "A Solution to the Simultaneous Localization and Map Building (SLAM) Problem" (IEEE T-RA 2001)](https://ieeexplore.ieee.org/document/938381) — EKF-SLAM 고전 정식화
> - [Kaess et al., "iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree" (IJRR 2012)](https://www.cs.cmu.edu/~kaess/pub/Kaess12ijrr.pdf) — incremental smoothing과 Bayes tree의 원 논문

---

서비스 로봇의 localization은 belief 표현 체계에 따라 접근법이 갈린다. 또한 pose graph 최적화 결과는 별도의 지도 생성(map generation) 절차를 거쳐 최종 내비게이션 지도로 완성된다. Factor graph가 널리 쓰이게 된 역사도 이 흐름 안에서 이해할 수 있다. 천장 마커나 계산 자원이 제한된 시스템에서는 EKF localization의 골격도 여전히 선택할 수 있다. 두 방식의 공존은 한 구조로의 수렴이 모든 운용 조건의 답은 아니라는 점을 보여준다.

---

# Ch.15 — 로봇 프레임워크 (Robot Frameworks)

로봇 소프트웨어는 센서 드라이버, 경로 계획, 모터 제어를 동시에 실행하면서 그 사이의 통신도 관리해야 한다. 프레임워크는 스레드 관리, 메시지 직렬화, 좌표 변환처럼 여러 모듈이 함께 쓰는 기능을 제공한다. ROS의 통신 구조와 패키지 체계를 중심으로 시뮬레이터와 주변 도구를 차례로 짚는다.

## 15.1 ROS (Robot Operating System)

ROS는 로봇 소프트웨어 개발을 위한 오픈소스 프레임워크다. 독립된 운영체제라기보다 프로세스 간 통신, 패키지 관리, 빌드 도구를 제공하는 **미들웨어 계층**에 해당한다.

로봇에서는 카메라·LiDAR·모터·제어기 같은 모듈이 동시에 실행된다. ROS는 이 모듈의 통신 방식과 메시지 형식을 통일해 각 기능을 별도 노드로 나누어 개발할 수 있게 한다.

### 15.1.1 ROS1 vs ROS2

| 특징 | ROS1 | ROS2 |
| --- | --- | --- |
| 첫 공개 릴리스 | 2010 (ROS 1.0 Box Turtle; 개발 시작 2007) | 2017 (Ardent Apalone) |
| 통신 | Custom (TCPROS) | DDS 기반 |
| 실시간 | 미지원 | 지원 |
| 보안 | 없음 | SROS2 |
| Multi-robot | 어려움 | 쉬움 |
| Master | 필요 (roscore) | 불필요 |
| Python | 2/3 | 3 only |

현재 권장: 새 장기 프로젝트는 ROS2 Jazzy LTS를 우선 검토한다. Ubuntu 22.04나 기존 package 제약 때문에 Humble을 유지하는 프로젝트도 있다.

다만 사용하는 robot driver와 package의 지원 distribution, Ubuntu 버전, EOL 날짜를 함께 확인해 선택한다. ROS1 Noetic은 2025년 5월에 공식 지원이 끝났다.

새 프로젝트라면 특별한 이유가 없는 한 ROS2로 시작한다. 기존 ROS1 package가 필요하면 지원되는 조합에서 `ros1_bridge`를 검토할 수 있지만, ROS1·ROS2·Ubuntu 버전 제약을 먼저 확인해야 한다. Nav2와 MoveIt 2는 ROS2용으로 제공된다.

> **추천 자료**
> - [ROS2 공식 튜토리얼](https://docs.ros.org/en/jazzy/Tutorials.html) — ROS2 Jazzy LTS 기준 공식 단계별 가이드. 처음이라면 "Beginner: CLI tools"부터 시작
> - [The Construct - ROS2 Basics](https://www.youtube.com/@TheConstruct) — ROS 전문 교육 채널. 시뮬레이터 내에서 실습 가능
> - [ROS1 to ROS2 Migration Guide](https://docs.ros.org/en/jazzy/How-To-Guides/Migrating-from-ROS1.html) — 기존 ROS1 코드 이전 공식 가이드

### 15.1.2 핵심 개념

Topic, Service, Action은 통신 시점과 응답 방식이 다르다. 센서 스트림, 짧은 요청-응답, 오래 걸리며 중간 상태가 필요한 작업을 각각 다른 인터페이스로 표현한다.

**Node (노드)**:
- 그래프에 참여하는 기능 단위. ROS 1에서는 사실상 프로세스 하나였고, ROS 2에서는 component로 묶어 한 프로세스에 여러 노드를 담을 수 있다
- 단일 목적 (sensor driver, controller 등)

**Topic (토픽)**:
- 비동기 메시지 스트림
- Publisher/Subscriber 패턴
- 예: 센서 데이터, 명령

```python
# ROS2 Publisher 예시
import rclpy
from rclpy.node import Node
from std_msgs.msg import String

class MinimalPublisher(Node):
    def __init__(self):
        super().__init__('minimal_publisher')
        self.publisher_ = self.create_publisher(String, 'topic', 10)
        self.timer = self.create_timer(0.5, self.timer_callback)

def timer_callback(self):
        msg = String()
        msg.data = 'Hello, World!'
        self.publisher_.publish(msg)
```

**Service (서비스)**:
- 요청/응답 인터페이스. ROS 2 클라이언트 API는 비동기 호출이 기본이며, executor를 돌리는 스레드(콜백 안)에서 동기로 기다리면 교착이 생긴다
- 일회성 작업에 적합
- 예: 설정 변경, 상태 조회

**Action (액션)**:
- 비동기 목표 지향 작업
- Feedback 제공
- 취소 가능
- 예: 네비게이션, 조작

Topic은 카메라 영상처럼 계속 흐르는 데이터에 쓴다. Service는 "지금 배터리 잔량 알려줘"처럼 한 번 요청해 응답을 받는 상황에, Action은 "저기까지 가"처럼 시간이 걸리는 작업에 알맞다. 세 인터페이스의 차이를 구분해야 통신 구조를 제대로 설계할 수 있다.

**Parameter (파라미터)**:
- 노드 설정값
- 런타임 변경 가능

> **⚠ 생성된 코드 점검**: ROS2 코드를 요청할 때는 센서 토픽의 QoS를 함께 제공한다. 생성된 subscriber의 reliability와 durability가 publisher와 호환되는지 확인해야, 오류 메시지 없이 데이터가 끊기는 상황을 피할 수 있다. 조건은 동일이 아니라 호환이다. RELIABLE publisher는 BEST_EFFORT subscriber와 연결되지만 그 반대는 연결되지 않고, durability도 TRANSIENT_LOCAL publisher와 VOLATILE subscriber는 되지만 반대는 안 된다.

> **추천 자료**
> - [ROS2 Concepts — Understanding nodes, topics, services, actions](https://docs.ros.org/en/humble/Concepts.html) — 공식 개념 문서
> - [The Construct - ROS2 Topics vs Services vs Actions](https://www.youtube.com/@TheConstruct) — 세 가지 통신 방식 비교 영상

### 15.1.3 도구

로봇 개발에서 "일단 코드를 짜고 로봇에 올려 보자"는 접근은 위험하다. 센서 데이터가 제대로 들어오는지, 좌표계가 맞는지를 눈으로 확인해야 디버깅 시간을 줄일 수 있다. 아래 도구들은 ROS 개발에서 자주 쓰는 기본 유틸리티다.

**rviz / rviz2**:
- 3D 시각화 도구
- 센서 데이터, TF, 경로 등 표시

**rqt**:
- Qt 기반 GUI 도구 모음
- rqt_graph: 노드/토픽 관계 시각화
- rqt_plot: 데이터 그래프

**rosbag / ros2 bag**:
- 데이터 녹화/재생
- 디버깅, 알고리즘 개발에 필수

실제 로봇을 매번 움직여 알고리즘을 테스트하면 시간과 비용이 많이 든다. rosbag으로 한 번 녹화해 두면 같은 데이터로 몇 번이고 반복 실험할 수 있어 재현성에도 필수다.

```bash
# ROS2 bag 녹화
ros2 bag record -a -o my_bag

# 재생
ros2 bag play my_bag
```

**tf2 (Transform Library)**:
- 좌표계 변환 관리
- 시간에 따른 변환 추적

로봇에는 카메라 좌표계, LiDAR 좌표계, 베이스 좌표계, 월드 좌표계 등이 동시에 존재한다. "카메라에서 본 점이 로봇 기준으로 어디인가?"를 계산하려면 좌표 변환이 필요하며, tf2가 이를 관리한다. 선형대수의 SE(3) 변환 행렬로 이해할 수 있다.

```python
# tf2 리스너 예시
import rclpy
from rclpy.node import Node
from tf2_ros import Buffer, TransformListener, LookupException, ExtrapolationException

class TfDemo(Node):
    def __init__(self):
        super().__init__('tf_demo')
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)
        # 리스너가 버퍼를 채울 시간을 주기 위해 타이머 안에서 조회한다
        self.create_timer(0.1, self.lookup)

def lookup(self):
        try:
            # camera_link 좌표를 base_link 좌표로 변환하는 변환 조회
            t = self.tf_buffer.lookup_transform('base_link', 'camera_link', rclpy.time.Time())
        except (LookupException, ExtrapolationException) as e:
            self.get_logger().warn(f'tf not ready: {e}')
```

> **추천 자료**
> - [ROS2 tf2 Tutorials](https://docs.ros.org/en/humble/Tutorials/Intermediate/Tf2/Tf2-Main.html) — 좌표 변환 공식 튜토리얼
> - [The Construct 채널](https://www.youtube.com/@TheConstruct) — rviz2 활용 영상을 포함한 ROS2 강의 채널
> - [ros2 bag CLI 문서](https://docs.ros.org/en/humble/Tutorials/Beginner-CLI-Tools/Recording-And-Playing-Back-Data/Recording-And-Playing-Back-Data.html) — 데이터 녹화/재생 공식 가이드

### 15.1.4 주요 패키지

아래 패키지는 표준 메시지, 영상·포인트 클라우드 변환, 내비게이션 기능을 제공한다.

| 패키지 | 용도 |
| --- | --- |
| sensor_msgs | 센서 메시지 타입 |
| geometry_msgs | 기하학 메시지 (Pose, Twist 등) |
| cv_bridge | OpenCV ↔︎ ROS 이미지 변환 |
| image_transport | 이미지 압축 전송 |
| pcl_ros | PCL ↔︎ ROS 포인트 클라우드 |
| nav2 | Navigation 스택 (ROS2) |

> **추천 자료**
> - [Nav2 Documentation](https://docs.nav2.org/) — ROS2 Navigation 스택 공식 문서
> - [ROS2 Package Index](https://index.ros.org/packages/) — ROS2 패키지 검색

## 15.2 시뮬레이션

실물 로봇에서 바로 실험하면 하드웨어가 손상되거나 사람이 다칠 수 있다. 시뮬레이터는 하드웨어 동작 범위와 실패 조건을 사전에 검증할 수 있게 돕는다. 아울러 강화학습에 필요한 수많은 시행착오 에피소드를 가상 공간에서 빠르게 반복 생성하는 환경을 지원한다.

**Embodied AI** 연구가 늘면서 로봇이 가상 환경에서 자율적으로 학습하는 시뮬레이터의 역할도 커졌다. NVIDIA Isaac Sim, AI2-THOR, Habitat 등의 플랫폼이 널리 쓰인다. 특히 가상 환경에서 학습된 정책을 물리 로봇으로 성공적으로 전이하는 Sim-to-Real Transfer는 활발히 연구되는 핵심 주제다.

### 15.2.1 Gazebo

Gazebo는 ROS와 연동되는 시뮬레이터다. 여러 ROS 패키지가 Gazebo용 시뮬레이션 데모와 로봇 모델을 제공한다.

구성 요소:
- **SDF (Simulation Description Format)**: 환경 정의
- **URDF (Unified Robot Description Format)**: 로봇 모델

Gazebo Classic vs Gazebo Sim (Ignition):
- Gazebo Sim: 새 버전, ROS2 권장
- 모듈화 구조, 더 나은 확장성

```xml

<robot name="my_robot">
  <link name="base_link">
    <visual>
      <geometry>
        <box size="0.5 0.3 0.1"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <box size="0.5 0.3 0.1"/>
      </geometry>
    </collision>
  </link>
</robot>
```

> **추천 자료**
> - [Gazebo Sim 공식 튜토리얼](https://gazebosim.org/docs) — Gazebo Sim(구 Ignition) 공식 가이드
> - [URDF Tutorial (ROS2)](https://docs.ros.org/en/humble/Tutorials/Intermediate/URDF/URDF-Main.html) — 로봇 모델링 기초
> - [The Construct - Gazebo Sim with ROS2](https://www.youtube.com/@TheConstruct) — Gazebo + ROS2 실습 영상

### 15.2.2 NVIDIA Isaac Sim

Embodied AI 연구에서 대규모 합성 데이터 생성과 Sim-to-Real 학습에 많이 쓰인다. RTX 렌더링과 PhysX 5 물리 엔진을 결합하고, Domain Randomization으로 합성 데이터를 생성한다. ROS2와 통합되며 Manipulation 연구에 주로 쓰인다.

Isaac Sim 외에도 Embodied AI 연구에 많이 쓰이는 시뮬레이터들이 있다.

| 시뮬레이터 | 주 용도 | 특징 |
| --- | --- | --- |
| NVIDIA Isaac Sim | 범용 (Manipulation, Navigation) | RTX 렌더링, PhysX 5, 대규모 합성 데이터 |
| AI2-THOR | 실내 내비게이션, 물체 상호작용 | 120+ 실내 장면, 사실적 상호작용 |
| Habitat (Meta) | 시각 내비게이션, Embodied QA | 초고속 렌더링 (수천 FPS), 대규모 학습 |
| iGibson | 실내 로봇 작업 | 물리 기반 렌더링, 가정환경 |
| MuJoCo | 로봇 제어, 강화학습 | 정확한 접촉 역학, 빠른 시뮬레이션 |

> **추천 자료**
> - [NVIDIA Isaac Sim 공식 문서](https://docs.omniverse.nvidia.com/isaacsim/latest/index.html) — 설치부터 고급 활용까지
> - [AI2-THOR Documentation](https://ai2thor.allenai.org/ithor/documentation) — Embodied AI 연구용 실내 시뮬레이터
> - [Habitat Documentation](https://aihabitat.org/docs/habitat2/) — Meta의 Embodied AI 플랫폼

### 15.2.3 CARLA

자율주행 연구용 시뮬레이터로, 논문 실험 환경으로 자주 쓰인다. 자율주행 쪽 연구를 한다면 익혀 두면 좋다.

도시 환경과 다양한 날씨·시간대를 지원하고, 카메라·LiDAR·Radar 센서 시뮬레이션과 ROS 브릿지를 제공한다.

> **추천 자료**
> - [CARLA Documentation](https://carla.readthedocs.io/) — 공식 문서 및 Python API 레퍼런스
> - [CARLA Simulator YouTube](https://www.youtube.com/channel/UC1llP9ekCwt8nEJzMJBQekg) — 시뮬레이터 활용 데모 영상

## 15.3 기타 프레임워크

ROS와 시뮬레이터 외에도 특정 용도에 맞춘 프레임워크와 라이브러리가 있다. 필요한 기능을 직접 만들지 않고 가져다 쓸 수 있다는 점이 로보틱스 생태계의 장점이다.

**Isaac ROS**:
- NVIDIA GPU 가속 ROS 패키지
- DNN 추론, Visual SLAM, 3D 인식
- Jetson 최적화

**Autoware**:
- 완전한 자율주행 스택
- 인식, 계획, 제어 포함
- ROS2 기반 (Autoware.Universe)

**ROS 외 도구**:
- **OpenCV**: 컴퓨터 비전
- **Open3D**: 3D 처리/시각화
- **Eigen**: 선형대수 (C++)
- **Sophus**: SE(3), SO(3) 연산

Eigen과 Sophus는 로보틱스에서 좌표 변환을 다룰 때 핵심적으로 쓰이는 라이브러리다. Eigen이 C++ 기반의 범용 선형대수 행렬 연산을 전담한다면, Sophus는 3차원 회전군 SO(3)과 특수 유클리드군 SE(3) 매니폴드 연산을 특화 지원한다.

> **추천 자료**
> - [OpenCV Tutorials](https://docs.opencv.org/4.x/d9/df8/tutorial_root.html) — 컴퓨터 비전 기초부터 고급까지
> - [Open3D Documentation](http://www.open3d.org/docs/) — 3D 데이터 처리 라이브러리 공식 문서
> - [Eigen Getting Started](https://eigen.tuxfamily.org/dox/GettingStarted.html) — C++ 선형대수 라이브러리 입문

## 15.4 심화: 시스템 설계

15.4.1 Latency Budgeting
- 전체 파이프라인의 latency를 구간별로 할당
- 예시: 자율주행 — 센서 입력(10 ms) → 인식(50 ms) → 계획(30 ms) → 제어(10 ms) = 100 ms total
- 구간별 값은 종단 deadline을 나눠 배분한 설계 목표다. 구간별로 강제 deadline을 두지 않았다면 한 구간의 초과를 다른 구간의 여유가 흡수할 수 있다. 처리율을 볼 때는 가장 느린 구간이 bottleneck이다
- profiling 방법: ROS2 callback duration, `ros2 topic delay`, tracing (ros2_tracing)

15.4.2 Behavior Tree
- 유한 상태 기계(FSM)보다 확장성이 좋은 로봇 행동 설계 방법
- 노드 유형: Sequence (순차), Fallback (대안), Action (실행), Condition (조건)
- 장점: 모듈적 — 하위 트리를 독립적으로 테스트/재사용 가능
- ROS2에서: BehaviorTree.CPP, Nav2에서 사용
- FSM은 상태가 늘어나면 전이가 기하급수적으로 복잡해진다. BT는 트리 구조로 복잡도를 관리

15.4.3 Safety와 Failsafe
- Watchdog timer: 특정 시간 내에 heartbeat 없으면 safe stop
- E-stop (Emergency Stop): 하드웨어 레벨의 전원 차단
- Software safety: 속도 제한, workspace 제한, collision check
- ISO 13482: 서비스 로봇 안전 표준 (개요만)
- 실무: 새 알고리즘을 올릴 때는 safety wrapper를 먼저 만들고, 그 안에서 실험

15.4.4 배포와 필드 테스트
- CI/CD: colcon build + test 자동화, Docker 이미지 빌드
- 로그 재생 테스트: 기록한 센서 데이터를 재생하면서 새 코드 테스트
- Hardware-in-the-Loop (HIL): 실제 하드웨어를 폐루프에 넣고 플랜트 쪽을 실시간 시뮬레이션하며 테스트
- 필드 테스트 프로토콜: 통제된 환경 → 반통제 → 실제 환경, 단계적으로
- 로그 수집: rosbag + 시스템 로그 (journalctl) + 센서 상태 모니터링

> **추천 자료**
> - [BehaviorTree.CPP Documentation](https://www.behaviortree.dev/) — BT 설계 패턴과 튜토리얼
> - [Nav2 Documentation](https://docs.nav2.org/) — ROS2 Navigation2 스택. BT 기반 설계의 실전 예시

## 기술 흐름: 로봇 프레임워크의 과거 → 현재 → 미래

```
2007 ─── ROS 개발 시작 (Stanford → Willow Garage; 1.0 릴리스는 2010)
  │       로봇 미들웨어로 널리 사용됨
  │
2012 ─── Gazebo Classic 독립 프로젝트화
  │       시뮬레이션이 로봇 개발의 필수 단계로 자리잡음
  │
2017 ─── ROS2 첫 릴리즈
  │       DDS 기반 통신, 실시간 지원, 보안 추가
  │
2019 ─── NVIDIA Isaac Sim 공개
  │       RTX 기반 고품질 렌더링 + 합성 데이터 생성
  │
2020 ─── Habitat, AI2-THOR 등 Embodied AI 시뮬레이터 부상
  │       대규모 학습 기반 로봇 정책 연구 본격화
  │
2022 ─── ROS2 Humble LTS 출시
  │       산업계 채택 가속화, Nav2/MoveIt2 안정화
  │
2025 ─── ROS1 Noetic EOL (지원 종료)
  │       ROS2 전환의 사실상 마감 시점
  │
2025+ ── Embodied AI + Foundation Models 시대
          시뮬레이터에서 대규모 사전학습 → Sim-to-Real 전이
          언어 명령 기반 로봇 조작 (VLA)
          NVIDIA Isaac Lab 등에서 시뮬레이터→학습→실제 로봇 배포를 일관된 파이프라인으로 제공 시작
```

---

# Ch.16 — 개발 환경 & 도구

로봇 연구 코드는 CUDA, Python, ROS, 시스템 라이브러리의 버전 조합에 민감하다. 안정적인 개발을 위해서는 실행 환경을 분리하고 재현하는 언어 도구, 패키지 환경, Docker가 기본으로 뒷받침되어야 한다.

## 16.1 프로그래밍 언어

AI 코딩 에이전트가 코드 작성의 상당 부분을 돕더라도 기존 코드를 읽고 이해하는 능력은 필요하다. 다른 사람의 연구 코드를 클론해 구조를 파악하고, AI가 생성한 코드를 검증하며, 문제가 생겼을 때 어디를 고칠지 판단할 수 있어야 한다.

### 16.1.1 C++

**용도**: 실시간 시스템, ROS 노드, SLAM, 성능 중요 모듈

연구실에서 다루는 핵심 코드는 대부분 C++이다. SLAM, 실시간 제어, ROS 패키지의 핵심 로직이 C++로 작성되어 있어 읽고 수정할 일이 많다. ORB-SLAM3, LOAM, VINS-Mono 같은 코드를 이해하려면 C++에 익숙해야 한다.

**장점**:
- 빠른 실행 속도
- 메모리 직접 제어
- ROS/SLAM 코드 대부분 C++

**단점**:
- 배우기 어려움
- 개발 속도 느림
- 메모리 관리 실수

**modern C++ (C++17/20)**:

```cpp
// 스마트 포인터
auto ptr = std::make_shared<MyClass>();

// Range-based for
for (const auto& item : container) { ... }

// Lambda
auto func = [&](int x) { return x * 2; };
```

> **추천 자료**
> - [C++ Core Guidelines](https://isocpp.github.io/CppCoreGuidelines/CppCoreGuidelines) — 모던 C++ 코딩 가이드 (Bjarne Stroustrup, Herb Sutter)
> - [The Cherno - C++ Playlist](https://www.youtube.com/playlist?list=PLlrATfBNZ98dudnM48yfGUldqGD0S4FFb) — C++ 기초부터 고급까지 영상 시리즈
> - [Modernes C++](https://www.modernescpp.com/index.php) — 모던 C++ (C++17/20/23) 기능을 체계적으로 정리한 블로그

> **⚠ 타겟 환경 점검**: C++ 코드를 요청할 때 x86인지 Jetson(ARM)인지, 크로스 컴파일을 사용하는지 함께 적는다. 생성된 의존성과 build flag가 타겟 아키텍처를 지원하는지도 확인한다.

### 16.1.2 Python

**용도**: 프로토타이핑, 딥러닝 학습/추론, 데이터 분석, 시각화

PyTorch 학습 스크립트와 데이터 전처리에 널리 쓴다. 에이전트로 초안을 만들기에는 좋지만, 생성된 코드를 읽고 실행 결과와 성능 병목을 직접 확인할 수 있어야 한다.

**자주 쓰는 라이브러리**:

```bash
pip install numpy scipy matplotlib
pip install opencv-python open3d
pip install torch torchvision
pip install transformers  # HuggingFace
```

> **추천 자료**
> - [Real Python](https://realpython.com/) — Python 기초부터 고급까지 체계적 튜토리얼
> - [Fireship - Python in 100 Seconds](https://www.youtube.com/watch?v=x7X9w_GIm1s) — Python 전체를 빠르게 훑어보는 영상

## 16.2 개발 환경 설정

### 16.2.1 Ubuntu

Ubuntu는 ROS의 1차 지원 플랫폼이며 GPU 드라이버, CUDA, cuDNN 조합에 관한 문서와 사례도 많다. macOS와 Windows에서도 일부 개발이 가능하지만, ROS 배포판과 실제 로봇의 운영체제에 맞춰 개발 환경을 선택해야 한다.

**권장 버전**:
- Ubuntu 22.04 LTS (ROS2 Humble)
- Ubuntu 24.04 LTS (ROS2 Jazzy)

**초기 설정**:

```bash
# 기본 도구
sudo apt update && sudo apt upgrade -y
sudo apt install -y build-essential cmake git curl wget

# Python 관련
sudo apt install -y python3-pip python3-venv

# 개발 도구
sudo apt install -y vim tmux htop
```

> **추천 자료**
> - [The Missing Semester of Your CS Education (MIT)](https://missing.csail.mit.edu/) — 셸, vim, tmux, Git 등 "수업에서는 안 가르치지만 매일 쓰는" 개발 도구를 체계적으로 정리한 MIT 강의
> - [Fireship - Linux in 100 Seconds](https://www.youtube.com/watch?v=rrB13utjYV4) — Linux가 뭔지 빠르게 감 잡기

### 16.2.2 CUDA / cuDNN

딥러닝 모델 학습에는 대개 GPU 가속을 쓴다. NVIDIA GPU를 쓰는 환경에서는 드라이버가 PyTorch에 번들된 CUDA 런타임을 지원하지 않으면 GPU를 잡지 못한다.

**설치 확인**:

```bash
nvidia-smi          # GPU 상태
nvcc --version      # CUDA 버전
```

**버전 선택**: 사용할 PyTorch/TensorFlow 릴리스의 공식 설치 표에서 지원하는 CUDA·cuDNN 조합을 고른다.

> **추천 자료**
> - [PyTorch - Previous Versions](https://pytorch.org/get-started/previous-versions/) — PyTorch와 CUDA 버전 매칭 확인. 설치 전 확인할 것
> - [NVIDIA CUDA Toolkit Documentation](https://docs.nvidia.com/cuda/) — CUDA 공식 문서

**NVIDIA 드라이버 설치 트러블슈팅**

Ubuntu에서 NVIDIA 드라이버 설치 시 가장 흔한 문제는 `nouveau` (오픈소스 드라이버)와의 충돌이다.

```bash
# nouveau 비활성화
sudo bash -c "echo blacklist nouveau > /etc/modprobe.d/blacklist-nvidia-nouveau.conf"
sudo bash -c "echo options nouveau modeset=0 >> /etc/modprobe.d/blacklist-nvidia-nouveau.conf"
sudo update-initramfs -u
sudo reboot

# 드라이버 설치 (권장: apt 사용)
sudo apt install nvidia-driver-535  # 버전은 GPU에 맞게 조정
sudo reboot

# 확인
nvidia-smi
```

설치 후 `nvidia-smi`에서 GPU가 안 보이면: (1) `sudo dkms status`로 모듈이 빌드·설치됐는지, `lsmod | grep nvidia`로 실제 로드됐는지 확인, (2) Secure Boot가 켜져 있으면 서명되지 않은 모듈이 로드되지 않으므로 MOK 키를 등록하거나 배포판 서명 패키지를 쓰거나 Secure Boot를 끈다. (참고: [정진용 블로그](https://jinyongjeong.github.io/2016/11/22/ubuntu_graphic_driver_install/))

**CUDA/cuDNN 버전 호환성**

PyTorch 공식 wheel은 자체 CUDA 런타임을 포함하므로 호스트의 CUDA toolkit(nvcc) 버전과 같아야 하는 것은 아니다. 실제 조건은 드라이버가 wheel의 CUDA 런타임을 지원하는지다. 이 조건이 깨지면 `import torch`는 되지만 `torch.cuda.is_available()`이 False가 되거나 첫 CUDA 호출에서 오류가 난다. 확인 순서:

```bash
# 1. GPU 확인
nvidia-smi  # 오른쪽 상단의 CUDA Version은 "드라이버가 지원하는 최대 버전"

# 2. 설치된 CUDA toolkit 확인
nvcc --version

# 3. PyTorch가 사용하는 CUDA 확인
python -c "import torch; print(torch.version.cuda)"

# wheel을 쓰면 조건은 하나다: 드라이버(1)가 PyTorch의 CUDA 런타임(3)을 지원하는가.
# nvcc(2)는 소스 빌드나 커스텀 CUDA 확장을 컴파일할 때만 관여한다. 조합은 PyTorch 공식 사이트에서 확인:
# https://pytorch.org/get-started/locally/
```

(참고: [정진용 블로그](https://jinyongjeong.github.io/2016/09/19/cuda_setting/))

**OpenCV + CUDA 직접 빌드**

apt로 설치하는 `python3-opencv`나 `pip install opencv-python`은 CUDA 가속이 안 된다. GPU 가속이 필요하면 (DNN 모듈, optical flow 등) 소스에서 직접 빌드해야 한다.

```bash
# 의존성 설치
sudo apt install -y build-essential cmake git libgtk2.0-dev pkg-config \
    libavcodec-dev libavformat-dev libswscale-dev \
    libtbb-dev libjpeg-dev libpng-dev \
    python3-dev python3-numpy   # BUILD_opencv_python3=ON에 필요. cmake 요약의 Python 3 항목을 확인한다

# OpenCV + contrib 소스
git clone https://github.com/opencv/opencv.git
git clone https://github.com/opencv/opencv_contrib.git

# 빌드 (CUDA 활성화)
cd opencv && mkdir build && cd build
# CUDA_ARCH_BIN은 GPU에 맞게 조정 (RTX 3090=8.6, RTX 4090=8.9). 줄 연속(\) 뒤에는 주석을 둘 수 없다
cmake -D CMAKE_BUILD_TYPE=Release \
      -D CMAKE_INSTALL_PREFIX=/usr/local \
      -D OPENCV_EXTRA_MODULES_PATH=../../opencv_contrib/modules \
      -D WITH_CUDA=ON \
      -D CUDA_ARCH_BIN="8.6" \
      -D WITH_CUDNN=ON \
      -D OPENCV_DNN_CUDA=ON \
      -D BUILD_opencv_python3=ON \
      ..
make -j$(nproc)
sudo make install
```

`CUDA_ARCH_BIN`은 자기 GPU에 맞춰야 한다. 틀리면 빌드는 되지만 런타임에 느리거나 에러가 난다. [NVIDIA GPU Compute Capability](https://developer.nvidia.com/cuda-gpus)에서 확인.

주의: ROS 환경에서 pip opencv와 apt cv_bridge가 충돌하는 문제가 있다 (21장 부록 C.4 트러블슈팅 참고). CUDA OpenCV를 직접 빌드하면 이 문제가 더 복잡해질 수 있으니, Docker로 격리하는 것을 권장한다.

(참고: [다크 프로그래머 — OpenCV + CUDA 직접 빌드하기](https://darkpgmr.tistory.com/184))

### 16.2.3 환경 관리

프로젝트마다 필요한 Python 버전과 라이브러리 버전이 다르다. 환경 관리 도구 없이 `pip install`을 전역으로 하면 프로젝트 A의 라이브러리가 프로젝트 B와 충돌하는 "의존성 지옥(dependency hell)"에 빠질 수 있다. Conda나 venv로 프로젝트별 독립 환경을 만드는 것이 기본이다.

**Conda** (권장):

```bash
# Miniconda 설치
wget https://repo.anaconda.com/miniconda/Miniconda3-latest-Linux-x86_64.sh
bash Miniconda3-latest-Linux-x86_64.sh

# 환경 생성
conda create -n myenv python=3.10
conda activate myenv

# 패키지 설치
# PyTorch는 2.6부터 pytorch Anaconda 채널 게시를 중단했다. conda 환경 안에서 pip 인덱스를 쓴다
pip install torch torchvision --index-url https://download.pytorch.org/whl/cu121
```

**venv** (가벼움):

```bash
python3 -m venv myenv
source myenv/bin/activate
pip install -r requirements.txt
```

가령 SLAM 연구에는 Python 3.8이, 최신 트랜스포머 모델에는 Python 3.10이 필요할 수 있다. Conda에서는 `conda activate slam_env`, `conda activate transformer_env`로 환경을 전환하면 된다.

> **추천 자료**
> - [Conda Documentation - Managing Environments](https://docs.conda.io/projects/conda/en/latest/user-guide/tasks/manage-environments.html) — Conda 환경 관리 공식 가이드
> - [Python venv Documentation](https://docs.python.org/3/library/venv.html) — 파이썬 공식 가상환경 문서

## 16.3 Docker

### 16.3.1 왜 Docker인가?

Docker는 OS 사용자 공간, 라이브러리, 환경 설정을 이미지로 묶는다. 같은 이미지를 쓰면 개발자 사이의 의존성 차이를 줄일 수 있고, 논문 코드의 실행 환경도 함께 전달할 수 있다.

### 16.3.2 기본 사용법

```bash
# 이미지 다운로드
docker pull nvidia/cuda:12.1.0-devel-ubuntu22.04

# 컨테이너 실행
docker run -it --rm \
    --gpus all \
    -v $(pwd):/workspace \
    nvidia/cuda:12.1.0-devel-ubuntu22.04 bash

# Dockerfile 빌드
docker build -t my_image .
```

**Dockerfile 예시**:

```dockerfile
FROM nvidia/cuda:12.1.0-devel-ubuntu22.04

RUN apt-get update && apt-get install -y \
    python3-pip git

COPY requirements.txt /tmp/
RUN pip3 install -r /tmp/requirements.txt

WORKDIR /workspace
```

> **추천 자료**
> - [Docker 공식 Getting Started Guide](https://docs.docker.com/get-started/) — Docker 처음이라면 여기부터. 컨테이너, 이미지, 볼륨 개념을 잘 설명
> - [NetworkChuck - Docker Tutorial](https://www.youtube.com/watch?v=eGz9DS-aIeY) — Docker를 재미있고 쉽게 설명하는 영상. 입문용으로 적합
> - [Fireship - Docker in 100 Seconds](https://www.youtube.com/watch?v=Gjnup-PuquQ) — Docker 핵심 개념을 빠르게 훑어보기

### 16.3.3 NVIDIA Container Toolkit

일반 Docker 컨테이너 안에서는 GPU가 보이지 않는다. 딥러닝 학습이나 CUDA 기반 연산에는 nvidia-container-toolkit과 `--gpus all` 플래그가 필요하다.

참고: NVIDIA의 현재 문서는 `nvidia-container-toolkit`을 사용한다. 현대 Docker에서는 과거의 `--runtime=nvidia` 방식 대신 `--gpus all`로 GPU 접근을 요청한다.

```bash
# nvidia-container-toolkit 설치 (Ubuntu 22.04/24.04)
curl -fsSL https://nvidia.github.io/libnvidia-container/gpgkey | \
    sudo gpg --dearmor -o /usr/share/keyrings/nvidia-container-toolkit-keyring.gpg
curl -s -L https://nvidia.github.io/libnvidia-container/stable/deb/nvidia-container-toolkit.list | \
    sed 's#deb https://#deb [signed-by=/usr/share/keyrings/nvidia-container-toolkit-keyring.gpg] https://#g' | \
    sudo tee /etc/apt/sources.list.d/nvidia-container-toolkit.list
sudo apt-get update && sudo apt-get install -y nvidia-container-toolkit

# Docker 데몬에 NVIDIA 런타임 등록
sudo nvidia-ctk runtime configure --runtime=docker
sudo systemctl restart docker

# 테스트
docker run --rm --gpus all nvidia/cuda:12.4.1-base-ubuntu22.04 nvidia-smi
```

### 16.3.4 실전 레시피: ROS2 + GPU + GUI + 센서

로보틱스용 컨테이너는 GPU, GUI(RViz/Gazebo), USB 센서 접근을 함께 요구할 수 있다. 아래 레시피는 세 권한과 실행 환경을 한곳에서 설정한다.

아래 레시피는 [turlucode/ros-docker-gui](https://github.com/turlucode/ros-docker-gui)의 구조를 nvidia-container-toolkit과 ROS2 Humble 환경에 맞춘 것이다.

**1단계: X11 포워딩 준비 (호스트)**

```bash
# 호스트에서 한 번만 실행
sudo apt-get install -y xauth
xhost +local:docker
```

`xhost +local:docker`는 로컬 연결을 허용하는 항목을 추가한다. xhost는 호스트 기반 접근 제어라 컨테이너 신원을 구별하지 못하므로 Docker에만 열리는 것은 아니다. 컨테이너를 지정해 제한하려면 `xauth` 쿠키 기반 인증을 쓴다.

**2단계: 실행 스크립트**

```bash
#!/bin/bash
# run_ros2_docker.sh — GPU + GUI + USB 센서 풀 세팅

docker run --rm -it \
    --gpus all \
    --privileged \
    --net=host \
    --ipc=host \
    -e DISPLAY=$DISPLAY \
    -e QT_X11_NO_MITSHM=1 \
    -e ROS_DOMAIN_ID=42 \
    -v /tmp/.X11-unix:/tmp/.X11-unix:rw \
    -v $HOME/.Xauthority:/root/.Xauthority:ro \
    -v /dev:/dev \
    -v $HOME/catkin_ws:/root/catkin_ws \
    --name ros2_dev \
    osrf/ros:humble-desktop \
    bash
```

각 플래그가 뭘 하는지:

| 플래그 | 역할 |
|--------|------|
| `--gpus all` | GPU 패스스루 (nvidia-container-toolkit) |
| `--privileged` | USB/시리얼 디바이스 전체 접근. 프로덕션에서는 `--device`로 개별 매핑할 것 |
| `--net=host` | DDS multicast 설정을 간단히 하려고 호스트 네트워크 공유. 통신 구성에 따라 선택 |
| `--ipc=host` | 공유 메모리. Rviz 등 GUI 도구에서 필요 |
| `-e QT_X11_NO_MITSHM=1` | MIT-SHM 호환성 문제로 RViz 등이 종료될 때 사용하는 우회 설정 |
| `-e ROS_DOMAIN_ID=42` | 같은 네트워크의 다른 ROS2 시스템과 격리. 여러 시스템이 함께 있을 때 도메인 값을 분리 |
| `-v /dev:/dev` | 센서 USB가 언제 꽂힐지 모르므로 /dev 전체 마운트. `--privileged`와 세트 |
| `-v .Xauthority` | X11 인증. 1단계의 xhost 설정을 쓰지 않을 때 이것이 없으면 `cannot open display` |

**3단계: 작업 후 컨테이너 저장** — 2단계를 `--rm`으로 실행하면 셸을 나가는 순간 컨테이너가 삭제되므로 commit할 대상이 남지 않는다. 컨테이너가 살아 있는 동안 다른 셸에서 commit하거나, `--rm` 없이 실행한다.

```bash
# 컨테이너 안에서 패키지 설치 등 작업을 했으면 커밋
docker commit ros2_dev my_ros2_workspace:v1

# 다음번에는 저장된 이미지로 실행
# run_ros2_docker.sh에서 이미지 이름만 바꾸면 된다
```

**Dockerfile로 관리하는 방식** (더 권장):

```dockerfile
FROM osrf/ros:humble-desktop

# 기본 도구
RUN apt-get update && apt-get install -y \
    python3-pip git wget curl vim \
    ros-humble-rviz2 \
    ros-humble-rqt* \
    && rm -rf /var/lib/apt/lists/*

# Python 패키지
RUN pip3 install torch torchvision numpy opencv-python-headless

# ROS2 워크스페이스
RUN mkdir -p /root/ros2_ws/src
WORKDIR /root/ros2_ws

# 소스 빌드가 필요한 패키지가 있으면 여기서
# RUN cd src && git clone https://github.com/...
# RUN . /opt/ros/humble/setup.sh && colcon build

ENTRYPOINT ["/ros_entrypoint.sh"]
CMD ["bash"]
```

`docker commit`보다 Dockerfile이 나은 이유: 나중에 "이 이미지에 뭐가 깔려있지?"를 추적할 수 있다. commit으로 만든 이미지도 layer history는 남지만, 구성 절차가 Dockerfile로 기록되지 않아 다시 만들기 어렵다.

> **추천 자료**
> - [turlucode/ros-docker-gui](https://github.com/turlucode/ros-docker-gui) — ROS + NVIDIA + GUI Docker 설정 레퍼런스. Melodic부터 Humble까지 지원
> - [NVIDIA Container Toolkit Documentation](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/index.html) — 공식 설치 가이드
> - [OSRF Docker Images](https://hub.docker.com/r/osrf/ros) — ROS 공식 Docker 이미지. `humble-desktop`이 GUI 포함 버전

> **Docker 요구사항 점검**: 설정을 요청할 때 ROS2, GPU, USB 센서, GUI 시각화를 함께 쓸지 한 번에 적는다. 따로 만든 설정을 합치면 권한과 네트워크 옵션이 충돌할 수 있다. 완성된 명령에는 `QT_X11_NO_MITSHM`, `ROS_DOMAIN_ID`, device mapping이 필요한지 확인한다.

## 16.4 원격 관리: Git, SSH, 파일 전송

원격 실험 환경은 SSH 접속, Git 변경 이력, 서버 간 파일 전송으로 관리한다.

### 16.4.1 Git/GitHub

### 16.4.1.1 기본 워크플로우

Git은 코드 변경과 실험 시점의 상태를 기록한다. 실행에 사용한 commit을 결과와 함께 남기면 이전 상태를 재현하거나 변경 원인을 비교할 수 있다.

```bash
# 저장소 클론
git clone https://github.com/user/repo.git

# 변경 사항 확인
git status
git diff

# 커밋
git add .
git commit -m "feat: add new feature"

# 푸시
git push origin main
```

> **추천 자료**
> - [GitHub's Git Handbook](https://docs.github.com/en/get-started/using-git/about-git) — Git 핵심 개념을 깔끔하게 정리한 공식 가이드
> - [The Missing Semester - Version Control (Git)](https://missing.csail.mit.edu/2020/version-control/) — MIT 강의. Git의 내부 모델(DAG)까지 설명해 줘서 깊이 있게 이해 가능

### 16.4.1.2 브랜치 전략

**Git Flow**:
- `main`: 안정 버전
- `develop`: 개발 버전
- `feature/*`: 기능 개발
- `hotfix/*`: 긴급 수정

**GitHub Flow** (간단):
- `main`: 항상 배포 가능
- `feature-branch`: 기능별 브랜치 → PR → Merge

여러 사람이 같은 코드를 수정할 때는 기능별 branch에서 변경을 분리하고 PR에서 검토한 뒤 `main`에 합칠 수 있다. 단순한 GitHub Flow만으로도 충돌 범위와 변경 목적을 구분하기 쉽다.

### 16.4.1.3 협업

**Pull Request (PR)**:
1. Fork 또는 브랜치 생성
2. 변경 사항 커밋
3. PR 생성 및 리뷰 요청
4. 코드 리뷰 후 머지

**커밋 메시지 규칙** (Conventional Commits):

```
feat: 새로운 기능
fix: 버그 수정
docs: 문서 변경
refactor: 리팩토링
test: 테스트 추가/수정
chore: 빌드/설정 변경
```

> **추천 자료**
> - [GitHub's Git Handbook](https://docs.github.com/en/get-started/using-git/about-git) — GitHub에서 직접 만든 Git 입문 가이드
> - [Conventional Commits Specification](https://www.conventionalcommits.org/) — 커밋 메시지 규칙 공식 스펙

### 16.4.2 SSH

연구실 GPU 서버에 접속하는 기본 도구이다. 비밀번호 대신 키 인증을 쓰면 편하고 안전하다.

```bash
# 키 생성 (처음 한 번)
ssh-keygen -t ed25519

# 서버에 공개키 등록
ssh-copy-id user@server_ip

# 접속
ssh user@server_ip

# 포트 포워딩 (서버의 Jupyter/TensorBoard를 로컬에서 보기)
ssh -L 8888:localhost:8888 user@server_ip
```

**~/.ssh/config**를 설정해 두면 매번 IP와 사용자명을 입력하지 않아도 된다:

```
Host lab-server
    HostName 192.168.1.100
    User junholee
    IdentityFile ~/.ssh/id_ed25519
```

이후 `ssh lab-server`로 바로 접속 가능. VS Code Remote-SSH도 이 설정을 읽는다.

### 16.4.3 SCP & rsync

서버와 로컬 간 파일 전송 도구다. **SCP**는 단순 파일 복사, **rsync**는 변경된 부분만 전송한다.

**SCP**:

```bash
# 로컬 → 서버
scp model.pth user@server:/home/user/weights/

# 서버 → 로컬
scp user@server:/home/user/results/log.txt ./

# 디렉토리 복사
scp -r dataset/ user@server:/data/
```

**rsync** — 대용량 데이터셋이나 반복 전송에 유리하다:

```bash
# 로컬 → 서버 (변경분만 전송, 진행률 표시)
rsync -avz --progress dataset/ user@server:/data/dataset/

# 서버 → 로컬
rsync -avz user@server:/home/user/results/ ./results/

# 삭제된 파일도 동기화 (미러링)
rsync -avz --delete source/ user@server:/data/source/
```

`scp`는 매번 전체를 복사하지만, `rsync`는 diff만 보내므로 수십 GB 데이터셋을 동기화할 때 차이가 크다.

### 16.4.4 Tailscale

연구실 서버가 NAT/방화벽 뒤에 있으면 외부에서 SSH 접속이 안 된다. Tailscale은 WireGuard 기반 VPN으로, 설치만 하면 어디서든 연구실 서버에 직접 접속할 수 있게 해 준다.

```bash
# 설치 (서버와 로컬 양쪽에)
curl -fsSL https://tailscale.com/install.sh | sh
sudo tailscale up

# 상태 확인 — 연결된 기기 목록과 IP 확인
tailscale status

# 이후 Tailscale IP로 SSH
ssh user@100.x.y.z
```

포트 포워딩이나 공유기 설정이 필요 없고, 카페·집·학교 어디서든 같은 IP로 서버에 접속할 수 있다. Tailscale SSH를 쓰면 SSH 키 관리도 자동화된다.

**~/.ssh/config**에 Tailscale IP를 등록해 두면 편하다:

```
Host lab-gpu
    HostName 100.x.y.z
    User junholee
```

> **추천 자료**
> - [Tailscale 공식 문서](https://tailscale.com/kb/) — 설치부터 ACL 설정까지
> - [The Missing Semester - Remote Machines](https://missing.csail.mit.edu/2020/command-line/#remote-machines) — SSH, 포트 포워딩, tmux 등 원격 작업 기초

## 16.5 실험 관리

### 16.5.1 Weights & Biases (wandb)

딥러닝 실험을 하다 보면 "어제 돌린 모델의 하이퍼파라미터가 뭐였지?"라는 상황이 매일 발생한다. 실험 관리 도구 없이 엑셀이나 노트로 기록하면 금방 한계에 부딪힌다. wandb는 학습 과정을 자동으로 로깅하고 시각화해 주고, 하이퍼파라미터와 모델 버전을 추적하며, 팀원과 결과를 공유하기도 쉽다.

```python
import wandb

# 초기화
wandb.init(project="my-project", config={
    "learning_rate": 0.001,
    "epochs": 100
})

# 로깅
for epoch in range(epochs):
    loss = train_one_epoch()
    wandb.log({"loss": loss, "epoch": epoch})

# 완료
wandb.finish()
```

> **추천 자료**
> - [Weights & Biases 공식 문서 및 Quickstart](https://docs.wandb.ai/quickstart) — wandb 시작 가이드. 5분이면 첫 실험 로깅 가능
> - [Weights & Biases YouTube](https://www.youtube.com/@WeightsBiases) — 사용법 튜토리얼 및 MLOps 관련 강연

### 16.5.2 MLflow

wandb가 클라우드 기반 서비스라면, MLflow는 자체 서버에서 운영할 수 있는 오픈소스 대안이다. 데이터 보안이 중요한 환경에서 유용하다.

```python
import mlflow

mlflow.set_experiment("my-experiment")

with mlflow.start_run():
    mlflow.log_param("lr", 0.001)
    mlflow.log_metric("accuracy", 0.95)
    mlflow.pytorch.log_model(model, "model")
```

### 16.5.3 TensorBoard

```python
from torch.utils.tensorboard import SummaryWriter

writer = SummaryWriter("runs/experiment1")
writer.add_scalar("Loss/train", loss, epoch)
writer.add_image("Sample", image, epoch)
writer.close()
```

```bash
tensorboard --logdir runs
```

TensorBoard는 PyTorch에서도 쓸 수 있고, 별도의 계정 없이 로컬에서 바로 시각화된다. 간단한 실험이라면 wandb 대신 TensorBoard만으로도 충분하다.

> **추천 자료**
> - [PyTorch TensorBoard Tutorial](https://pytorch.org/tutorials/recipes/recipes/tensorboard_with_pytorch.html) — PyTorch에서 TensorBoard 사용하는 공식 가이드
> - [MLflow Documentation](https://mlflow.org/docs/latest/index.html) — MLflow 공식 문서

## 16.6 코드 포매팅

### 16.6.1 Linting & Formatting

코드 스타일이 사람마다 다르면 코드 리뷰에서 로직보다 스타일 논쟁에 시간을 더 쓰게 된다. 자동 포매터를 쓰면 이 문제가 사라진다. 혼자 연구할 때도 일관된 코드 스타일은 나중에 자기 코드를 다시 읽을 때 큰 도움이 된다.

**Python**:

```bash
# Ruff (빠른 린터, Black 호환 포맷터)
pip install ruff
ruff check .
ruff format .

# Black (포매터)
pip install black
black .

# 타입 체크
pip install mypy
mypy .
```

**C++**:

```bash
# clang-format
clang-format -i src/*.cpp
```

### 16.6.2 Testing

테스트를 작성하는 습관은 연구 코드에서도 중요하다. "모델 forward pass가 제대로 되는지", "데이터 전처리 결과가 예상과 같은지" 같은 기본적인 테스트만 있어도 리팩토링할 때 훨씬 안심이 된다.

**Python (pytest)**:

```python
# test_module.py
def test_addition():
    assert 1 + 1 == 2

def test_function():
    result = my_function(input)
    assert result == expected
```

```bash
pytest tests/ -v
```

**C++ (gtest)**:

```cpp
#include <gtest/gtest.h>

TEST(MyTest, BasicTest) {
    EXPECT_EQ(1 + 1, 2);
}
```

> **추천 자료**
> - [Real Python - Python Testing with pytest](https://realpython.com/pytest-python-testing/) — pytest 사용법 상세 튜토리얼
> - [The Missing Semester (MIT)](https://missing.csail.mit.edu/) — 셸, 에디터, 디버깅, 프로파일링 등 개발 도구 전반. 연구실 입학 전에 한 번 쭉 보면 좋다
> - [Fireship YouTube](https://www.youtube.com/@Fireship) — 각종 개발 도구를 "100 Seconds" 시리즈로 빠르게 훑어볼 수 있다
> - [정진용 블로그 — 로봇 소프트웨어 개발 문화](https://jinyongjeong.github.io/2025/02/14/developmen_culture/) — 코드 리뷰, CI/CD, 스타일 가이드 등 로봇 개발팀의 개발 문화 정착 방법
> - [정진용 블로그 — 로봇 개발과 테스트 코드](https://jinyongjeong.github.io/2025/02/19/test_code/) — 로봇 소프트웨어에서 테스트 코드가 필수인 6가지 이유

## 기술 흐름: 개발 환경 & 도구의 과거 → 현재 → 미래

```
2005 ─── Git 탄생 (Linus Torvalds)
  │       분산 버전 관리의 시작
  │
2008 ─── GitHub 출시
  │       오픈소스 협업의 중심지가 됨
  │
2012 ─── Conda (Anaconda) 등장
  │       데이터 과학에서 널리 쓰이는 Python 환경 관리 도구가 됨
  │
2013 ─── Docker 공개
  │       컨테이너 이미지로 사용자 공간 의존성 재현이 쉬워짐
  │
2015 ─── TensorBoard (TensorFlow와 함께 공개)
  │       딥러닝 학습 시각화의 시작
  │
2017 ─── nvidia-docker 공개
  │       Docker 컨테이너에서 GPU 사용 가능
  │
2018 ─── Weights & Biases 출시
  │       실험 추적·시각화·팀 협업을 클라우드로
  │
2020~2022 ─── Black 대중화, Ruff 등장 (2022)
  │       Python 코드 품질 도구의 고속화
  │
2023+ ── AI-assisted 개발 도구 확산
          Copilot, Cursor 등 AI 코딩 보조 도구
          Dev Container 표준화 (VS Code Remote)
          재현 가능한 연구를 위한 Docker + wandb 조합 보편화
```

---

# Ch.17 — 데이터셋 & 벤치마크

데이터셋의 센서 구성, 수집 조건, split, annotation은 학습 결과와 비교가 성립하는 범위를 정한다. 연구에 널리 쓰이는 주요 벤치마크의 구조를 비교하고 자체 데이터를 수집·관리하는 절차를 차례로 짚는다.

최근 **합성 데이터(Synthetic Data)**의 비중이 커지고 있다. 실제 데이터 수집과 라벨링에는 비용과 시간이 많이 들기 때문에, 시뮬레이터가 자동 생성한 합성 데이터로 사전학습한 뒤 소량의 실제 데이터로 미세조정(fine-tuning)하는 방식을 쓴다. NVIDIA Isaac Sim의 Domain Randomization과 Habitat의 대규모 장면 생성이 대표적이다. 시뮬레이터 데이터와 대응하는 실제 데이터를 쌍으로 제공하는 **Sim-to-Real 데이터셋**도 활발히 구축되고 있다.

## 17.1 자율주행/로봇 데이터셋

### 17.1.1 KITTI / KITTI360

자율주행 연구의 시작점이 된 오래된 데이터셋이다.

KITTI는 2012년 공개 뒤 자율주행과 3D 비전의 공통 비교 기준으로 쓰여 왔다. 더 크고 다양한 데이터셋이 나온 뒤에도 VO·SLAM, stereo depth estimation 결과를 과거 연구와 비교할 때 자주 사용된다.

구성:
- 스테레오 카메라
- 3D LiDAR (Velodyne HDL-64E)
- GPS/IMU
- 2D/3D 라벨

태스크:
- Stereo depth estimation
- Optical flow
- Visual odometry / SLAM
- 3D object detection
- Semantic segmentation

다운로드: https://www.cvlibs.net/datasets/kitti/

> **추천 자료**
> - [KITTI Benchmark 공식 사이트](https://www.cvlibs.net/datasets/kitti/) — 데이터셋 다운로드 및 각 태스크별 리더보드 확인
> - [KITTI-360 사이트](https://www.cvlibs.net/datasets/kitti-360/) — 더 넓은 범위의 360도 데이터셋
> - [다크 프로그래머 — KITTI 데이터 사용하기 (LiDAR-카메라 변환)](https://darkpgmr.tistory.com/190) — KITTI 데이터의 좌표계 변환과 LiDAR-카메라 매핑 실습

### 17.1.2 nuScenes

대규모 자율주행 데이터셋이다.

KITTI보다 센서 구성이 풍부하고(360도 카메라, Radar 포함) 데이터 규모도 크다. 3D Object Detection과 BEV(Bird's Eye View) 기반 인식 연구에서 KITTI와 함께 표준 평가셋으로 자리 잡았다.

구성:
- 6개 카메라 (360° 커버)
- 5개 Radar
- 1개 LiDAR
- 1000 장면, 40K 키프레임

특징:
- 23개 객체 클래스
- 풍부한 Annotation (속성, 가시성)
- 밤, 비 등 다양한 조건

평가 메트릭: mAP, NDS

> **추천 자료**
> - [nuScenes devkit Documentation](https://www.nuscenes.org/nuscenes) — 데이터셋 사용법, devkit API, 튜토리얼 노트북
> - [nuScenes devkit GitHub](https://github.com/nutonomy/nuscenes-devkit) — Python devkit 코드 및 예제

### 17.1.3 Waymo Open Dataset

Waymo(Alphabet 산하)가 공개한 대규모 자율주행 데이터셋이다.

nuScenes와 함께 최신 자율주행 연구의 대표 벤치마크다. 데이터 품질이 높고 규모가 크며, 매년 챌린지를 통해 최신 기술 동향을 확인할 수 있다.

규모:
- 1,150 장면 (20초)
- 12M LiDAR 라벨
- 12M 카메라 라벨

특징:
- 높은 품질의 센서
- 다양한 환경 (도시, 교외, 밤)
- 연간 챌린지 개최

> **추천 자료**
> - [Waymo Open Dataset 공식 사이트](https://waymo.com/open/) — 데이터셋 다운로드 및 챌린지 참가
> - [Waymo Open Dataset GitHub](https://github.com/waymo-research/waymo-open-dataset) — 공식 도구 및 예제 코드

### 17.1.4 RGB-D SLAM 및 VIO / VINS용 데이터셋

TUM RGB-D와 EuRoC MAV는 각각 실내 RGB-D SLAM과 드론 VIO 평가에 쓰이는 데이터셋이다.

**TUM RGB-D**:
- RGB-D 카메라 시퀀스
- 정밀 ground truth (모션 캡처)
- 실내 환경
- Visual SLAM 평가 표준

**EuRoC MAV**:
- 드론 비행 데이터
- 스테레오 + IMU
- VIO 평가 표준
- 다양한 난이도

> **추천 자료**
> - [TUM RGB-D Benchmark](https://cvg.cit.tum.de/data/datasets/rgbd-dataset) — Visual SLAM 평가 표준 데이터셋 및 평가 도구
> - [EuRoC MAV Dataset](https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets) — VIO 평가 표준 데이터셋

## 17.2 컴퓨터 비전 데이터셋

### 17.2.1 ImageNet

이미지 분류의 표준 벤치마크이다.

딥러닝 전환기의 출발점이 된 데이터셋이다. 2012년 AlexNet이 ImageNet에서 큰 성능 향상을 보인 뒤, ImageNet 사전학습(pretrained) 가중치는 여러 비전 모델의 표준적인 출발점이 됐다. 로보틱스에서도 카메라 기반 인식 모듈의 백본(backbone)에 ImageNet 사전학습 모델을 널리 쓴다.

- 1000 클래스
- 120만 학습 이미지
- 사전학습(pretraining) 표준

### 17.2.2 COCO

COCO는 객체 탐지와 instance segmentation을 함께 평가한다.

COCO mAP는 여러 IoU threshold에서 AP를 계산해 평균한다. 단일 IoU threshold를 쓰는 PASCAL VOC 방식과 구분해야 한다.

특징:
- 80 객체 카테고리
- 33만 이미지, 150만 객체 인스턴스
- Dense annotation (bounding box, segmentation mask)

태스크:
- Object detection
- Instance segmentation
- Keypoint detection
- Captioning

### 17.2.3 ScanNet / NYU Depth V2

**ScanNet**:
- 1513개 실내 장면
- RGB-D 시퀀스
- 3D semantic segmentation
- 카메라 포즈, 메시 제공

**NYU Depth V2**:
- 실내 RGB-D
- Depth estimation 벤치마크
- 464 장면, 407K 프레임

실내 로봇(가정용, 서비스 로봇 등)을 다룬다면 ScanNet과 NYU Depth V2는 핵심 벤치마크다. 특히 ScanNet은 3D 장면 이해(Scene Understanding) 연구에서 빠지지 않는다.

> **추천 자료**
> - [COCO Dataset](https://cocodataset.org/) — 공식 사이트, 데이터셋 다운로드 및 evaluation 도구
> - [ScanNet Benchmark](http://www.scan-net.org/) — 3D Scene Understanding 벤치마크
> - [Papers With Code - Datasets](https://paperswithcode.com/datasets) — 태스크별 데이터셋 검색 및 리더보드 통합 사이트

## 17.3 데이터셋 활용법

### 17.3.1 다운로드 및 포맷 이해

각 데이터셋마다 고유한 디렉토리 구조와 포맷이 있다.

데이터셋을 내려받고도 디렉터리 구조와 라벨 포맷을 이해하지 못하면 데이터 로더 작성에만 며칠이 걸릴 수 있다. 특히 3D 라벨은 데이터셋마다 좌표계(coordinate system)가 다르므로(카메라 좌표계 vs LiDAR 좌표계, y-up vs z-up 등) 문서를 꼼꼼히 읽어야 한다.

예시 — KITTI Object Detection:

```
kitti/
├── training/
│   ├── image_2/       # Left RGB images
│   ├── velodyne/      # LiDAR point clouds (.bin)
│   ├── calib/         # Calibration files
│   └── label_2/       # 2D/3D annotations
└── testing/
    └── ...
```

라벨 파일 읽기 예시:

```python
# KITTI label format: type truncated occluded alpha bbox(4) dimensions(3) location(3) rotation_y
with open('label.txt', 'r') as f:
    for line in f:
        parts = line.strip().split()
        obj_type = parts[0]
        bbox = [float(x) for x in parts[4:8]]  # left, top, right, bottom
        dimensions = [float(x) for x in parts[8:11]]  # height, width, length
        location = [float(x) for x in parts[11:14]]  # x, y, z
```

> **추천 자료**
> - [KITTI Benchmark 공식 사이트 - Object Detection DevKit](https://www.cvlibs.net/datasets/kitti/eval_object.php?obj_benchmark=3d) — 라벨 포맷 설명 및 평가 코드
> - [nuScenes devkit Tutorial Notebooks](https://github.com/nutonomy/nuscenes-devkit/tree/master/python-sdk/tutorials) — Jupyter 노트북으로 데이터 구조 이해

### 17.3.2 DataLoader 구현

PyTorch에서 데이터 로딩을 위한 표준 패턴이다.

PyTorch의 `Dataset`은 샘플 읽기와 전처리를 정의하고, `DataLoader`는 batch 구성과 병렬 로딩을 맡는다. `__getitem__`의 전처리 비용과 `num_workers` 설정은 학습 처리량에 영향을 준다.

```python
from torch.utils.data import Dataset, DataLoader

class MyDataset(Dataset):
    def __init__(self, root_dir, transform=None):
        self.root_dir = root_dir
        self.transform = transform
        self.samples = self._load_samples()

def _load_samples(self):
        # 파일 목록 로드
        return list_of_samples

def __len__(self):
        return len(self.samples)

def __getitem__(self, idx):
        sample = self.samples[idx]
        image = load_image(sample['image_path'])
        label = sample['label']

if self.transform:
            image = self.transform(image)

return image, label

# 사용
dataset = MyDataset(root_dir='./data')
dataloader = DataLoader(dataset, batch_size=32, shuffle=True, num_workers=4)
```

> **추천 자료**
> - [PyTorch Data Loading Tutorial](https://pytorch.org/tutorials/beginner/data_loading_tutorial.html) — 커스텀 Dataset 작성 공식 가이드
> - [Real Python - PyTorch DataLoader](https://realpython.com/python-data-loading/) — DataLoader 활용법 상세 설명

## 17.4 자체 데이터 수집

공개 데이터셋만으로는 자기 연구에 딱 맞는 데이터를 구하기 어렵다. 자체 로봇에 맞는 센서 구성, 특수한 환경 조건을 위해 직접 데이터를 수집해야 할 때가 있다. 이때 센서 동기화, 캘리브레이션, 라벨링 과정을 체계적으로 해 두지 않으면 나중에 데이터를 쓸 수 없게 된다.

### 17.4.1 센서 동기화

여러 센서의 데이터를 시간 동기화하지 않으면 퓨전 자체가 의미가 없다.

카메라와 LiDAR의 타임스탬프가 10 ms만 어긋나도 고속 주행 시 수십 cm의 위치 오차가 생긴다. 센서 퓨전의 기본 전제가 "같은 시점의 데이터"인데, 동기화가 안 되면 그 전제가 무너진다.

하드웨어 동기화:
- 트리거 신호로 동시 촬영
- PPS (Pulse Per Second) 신호

소프트웨어 동기화:
- 타임스탬프 기반 근사 동기화
- 보간(interpolation) 사용

ROS의 `message_filters`는 메시지 header의 stamp를 기준으로 동기화한다. header가 없는 메시지에 `allow_headerless`를 지정할 때만 도착 시점을 쓴다:

```python
import message_filters

# Approximate Time Synchronizer
image_sub = message_filters.Subscriber(self, Image, '/camera/image')
lidar_sub = message_filters.Subscriber(self, PointCloud2, '/lidar/points')

sync = message_filters.ApproximateTimeSynchronizer(
    [image_sub, lidar_sub], queue_size=10, slop=0.1
)
sync.registerCallback(self.callback)
```

### 17.4.2 캘리브레이션

Camera Intrinsic: 체커보드 사용 (OpenCV calibrateCamera)

Camera-LiDAR Extrinsic:
- 체커보드 기반 (평면 맞춤)
- Target-based (특수 타겟 사용)
- Target-less (자동 특징점 매칭)

Camera-IMU: Kalibr 사용 권장

캘리브레이션이 부정확하면 카메라에서 본 객체 위치와 LiDAR에서 본 객체 위치가 일치하지 않는다. 센서 퓨전 정확도는 캘리브레이션 품질에 달려 있다. 선형대수 기준으로 보면, intrinsic은 3×3 카메라 행렬 K다. extrinsic은 카메라 투영에 들어갈 때는 3×4 블록 [R|t]이고, 센서 간 좌표 변환으로 쓸 때는 아래에 [0 0 0 1] 행을 붙인 4×4 동차 변환이다.

> **추천 자료**
> - [OpenCV Camera Calibration Tutorial](https://docs.opencv.org/4.x/dc/dbb/tutorial_py_calibration.html) — 체커보드 기반 카메라 캘리브레이션
> - [Kalibr GitHub](https://github.com/ethz-asl/kalibr) — Camera-IMU 캘리브레이션 표준 도구

### 17.4.3 라벨링 도구

데이터를 수집했으면 라벨링(annotation)을 해야 한다. 라벨링은 연구에서 가장 시간이 많이 드는 작업 중 하나이며, 라벨 품질이 모델 성능을 좌우한다. SAM(Segment Anything Model) 같은 기초 모델을 활용한 반자동 라벨링도 2023년 이후 연구 환경에서 쓰이기 시작했다.

**CVAT (Computer Vision Annotation Tool)**:
- 웹 기반, 무료
- 이미지, 비디오 annotation
- 다양한 태스크 지원 (bbox, polygon, points)

**Labelbox**:
- 클라우드 기반
- 팀 협업 기능
- 3D annotation 지원

**3D Labeling**:
- SUSTechPOINTS: LiDAR 포인트 클라우드
- KITTI-360 labeling tool

합성 데이터를 통한 자동 라벨링: 시뮬레이터(NVIDIA Isaac Sim, AI2-THOR 등)에서 데이터를 생성하면 라벨도 함께 만들어지므로 수동 라벨링을 줄일 수 있다. Domain Randomization으로 텍스처, 조명, 배경을 무작위 변형하면 모델의 일반화 성능도 높일 수 있다. 실제 데이터 수집·수동 주석 비용을 줄일 수 있지만, 시뮬레이터 구축·렌더링·검수 비용은 남는다.

> **추천 자료**
> - [CVAT Documentation](https://docs.cvat.ai/) — 오픈소스 라벨링 도구 공식 문서
> - [Roboflow](https://roboflow.com/) — 라벨링, 데이터 증강, 모델 학습을 통합 제공하는 플랫폼
> - [NVIDIA Isaac Sim - Synthetic Data Generation](https://docs.omniverse.nvidia.com/isaacsim/latest/replicator_tutorials/index.html) — 합성 데이터 생성 가이드

## 기술 흐름: 데이터셋 & 벤치마크의 과거 → 현재 → 미래

```
2009 ─── ImageNet 공개
  │       대규모 이미지 분류 벤치마크의 시작
  │
2012 ─── KITTI 공개 / AlexNet의 ImageNet 제패
  │       자율주행 벤치마크의 탄생, 딥러닝 혁명 시작
  │
2014 ─── COCO 공개
  │       Object Detection, Segmentation 표준 벤치마크
  │
2017 ─── ScanNet 공개
  │       실내 3D Scene Understanding 연구 활성화
  │
2019 ─── nuScenes, Waymo Open Dataset 공개
  │       대규모·고품질 자율주행 데이터셋 시대
  │
2020 ─── 합성 데이터 연구 본격화
  │       Domain Randomization, Sim-to-Real Transfer
  │       NVIDIA Isaac Sim 기반 대규모 합성 데이터 생성
  │
2023 ─── Foundation Model 시대의 데이터셋
  │       SA-1B (SAM 학습용, 10억 마스크)
  │       Open X-Embodiment (로봇 조작 데이터 통합)
  │
2024+ ── 데이터셋의 미래 트렌드
          합성 데이터 + 실제 데이터 혼합 학습 보편화
          Sim-to-Real 데이터셋 (시뮬·실제 쌍 데이터)
          자동 라벨링 (Foundation Model 기반)
          로봇 조작 데이터의 대규모 수집·공유 (Open X-Embodiment)
          멀티모달 데이터셋 (비전 + 언어 + 촉각 + 힘/토크)
```

---

# Ch.18 — 연구실 연구 방향

우리 연구실은 Spatial AI 시스템을 두 모듈로 나눠 설계한다. 이 구조는 물리적 제약과 실시간 요구사항에서 비롯되며, 앞선 장들에서 다룬 개념들이 여기서 맞물린다.

## 18.1 개요

Spatial AI 시스템을 **두 개의 모듈**로 구분하여 설계한다.

```
┌──────────────────────────────────────────────────────────────┐
│                     Spatial AI System                         │
├──────────────────────────────────────────────────────────────┤
│  ┌─────────────────────┐     ┌─────────────────────────────┐ │
│  │   Local Module      │     │      Global Module          │ │
│  │   (경량, 온보드)     │ ←→  │   (중량, 서버/클라우드)      │ │
│  │                     │     │                             │ │
│  │ • 실시간 Geometry    │     │ • VFM 기반 이해             │ │
│  │ • Odometry          │     │ • Semantic Scene Graph      │ │
│  │ • Local Obstacle    │     │ • Long-term Memory          │ │
│  │ • 제어 예산 기반 rate│     │ • 태스크 예산 기반 rate      │ │
│  └─────────────────────┘     └─────────────────────────────┘ │
└──────────────────────────────────────────────────────────────┘
```

### 두 모듈로 나누는 이유

온보드 컴퓨터 하나로 모든 기능을 처리하기에는 무게, 전력, 응답 시간의 제약이 크다.

**물리적 제약**부터 보자. NVIDIA A100 GPU 서버는 무게가 수십 kg이고 전력도 수백 와트를 사용하므로 배터리로 움직이는 드론에 실을 수 없다. 로봇에는 대개 Jetson Orin 같은 임베디드 보드를 탑재하지만, 이 장치에서 DINOv2나 SAM 같은 대형 모델을 실시간으로 실행하기는 어렵다.

**시간 제약**도 다르다. 장애물 회피는 수십 ms 안에 반응해야 하지만, 물체의 의미를 해석하는 작업은 그보다 느리게 처리해도 된다. 전자는 서버 응답을 기다릴 수 없고, 후자는 더 큰 모델을 사용할 여지가 있다.

두 모듈은 이 차이에 맞춰 역할을 나눈다.

1. **계산 자원의 현실**: 로봇 온보드 컴퓨터(Jetson 등)는 모델 크기와 전력·응답 시간에 따라 대형 모델의 실시간 실행이 제한될 수 있다.
2. **실시간 요구사항**: 장애물 회피에는 수십 ms 단위의 응답이 필요하다.
3. **의미 이해**: VFM/VLA로 "깨진 유리컵"처럼 물체의 종류와 상태를 구분한다.
4. **상호 보완**: Local의 기하학적 정밀도와 Global의 의미론적 이해를 결합한다.

> 인체에 빗대면 Local Module은 즉각적인 **반사 신경**에 해당하며, Global Module은 고차원 판단을 내리는 **대뇌 피질**의 역할을 맡는다. 뜨거운 물체에 닿았을 때 손을 무조건반사로 먼저 뗀 뒤 뒤이어 원인을 인지하듯, 로봇 역시 반사적 기하 회피와 인지적 의미 이해가 분리되어 맞물린다.

## 18.2 Local Module: Lightweight Geometry

Local Module은 로봇에 직접 탑재되어 실시간으로 동작하며, 안전한 이동에 필요한 정보를 처리한다.

### 18.2.1 목표

- **Odometry**: 자신의 움직임 추정 — "나는 지금 어디에 있는가?"
- **Obstacle Detection**: 즉각적인 장애물 감지 — "앞에 뭔가 있다, 피해!"
- **Local Mapping**: 주변 환경의 기하학적 지도 — "내 주변 3 m 이내는 이렇게 생겼다"

**운영 예**: 아파트 복도를 지나던 배달 로봇 앞에 아이가 갑자기 뛰어나오면, Local Module은 depth 센서로 장애물을 감지하고 odometry로 위치를 추정해 제어·안전 분석에서 정한 deadline 안에 회피 경로를 계산한다. 이 단계에서는 장애물의 종류보다 충돌 가능성을 먼저 판단한다. 물체의 의미는 Global Module이 별도로 해석한다.

### 18.2.2 특징

Local Module의 update rate와 latency deadline은 플랫폼 속도, braking distance, control bandwidth와 sensor rate에서 유도한다. 전력 한도도 선택한 embedded module과 power mode, 냉각 조건으로 정한다. 평균 FPS뿐 아니라 worst-case latency, jitter와 deadline miss를 target hardware에서 측정해야 한다.

### 18.2.3 기술 스택

**Classical 방법**:
- ORB-SLAM3: Feature-based Visual SLAM — 카메라 하나로 위치 추정 (→ 9장, 14장 참고)
- VINS-Mono: Visual-Inertial Odometry — 카메라+IMU 융합 (→ 14장 참고)
- FAST-LIO2: LiDAR-Inertial Odometry — LiDAR+IMU 융합 (→ 2장, 14장 참고)

**경량 학습 모델**:
- 경량 depth estimation — MobileNet 기반으로 압축 (→ 10장 참고)
- 압축된 segmentation 모델 — knowledge distillation 적용 (→ 10장, 11장 참고)
- TensorRT 최적화 — NVIDIA GPU용 graph·kernel·precision 최적화 후보

**Edge 배포**:

```bash
# TensorRT 최적화 예시
trtexec --onnx=model.onnx --saveEngine=model.trt --fp16 --memPoolSize=workspace:4096
```

> TensorRT는 NVIDIA GPU용 inference engine을 만든다. FP16이 memory와 latency를 줄일 수 있지만 이득과 task metric 변화는 model, input, batch, power mode와 software version에 따라 달라진다. Target Jetson에서 end-to-end latency와 validation metric을 함께 비교해 채택 여부를 정한다.

### 18.2.4 예시 구현

```python
# Local Module 개념 코드
class LocalModule:
    def __init__(self):
        self.odometry = FastLIO2()
        self.obstacle_detector = LightweightObstacleNet()  # TensorRT

def process(self, sensor_data):
        # 1. Odometry 업데이트 (IMU 입력 100 Hz)
        pose = self.odometry.update(sensor_data.imu, sensor_data.lidar)

# 2. 장애물 감지 (카메라 입력 30 Hz)
        obstacles = self.obstacle_detector(sensor_data.image)

# 3. Global Module로 키프레임 전송
        if self.is_keyframe(pose):
            self.send_to_global(sensor_data, pose)

return pose, obstacles
```

**실행 시나리오**: 위 코드에서 `process()`는 융합 tick마다 호출된다고 보자. IMU 데이터는 100 Hz(초당 100번), 카메라 이미지는 30 Hz로 들어오므로, 센서별 콜백이 최신 측정을 버퍼에 넣고 tick이 그것을 함께 소비하는 구조가 된다. 매 tick마다 "나 지금 어디?"(odometry)와 "앞에 뭐 있어?"(obstacle)를 계산하고, 중요한 순간의 키프레임만 Global Module에 보낸다. 모든 프레임을 전송하면 네트워크 대역폭을 넘길 수 있기 때문이다.

## 18.3 Global Module: VFM-based Understanding

Global Module은 서버나 cloud에서 동작하며 물체의 종류와 관계를 해석한다. Local Module이 앞의 장애물을 감지하면 Global Module은 이를 깨진 유리잔으로 분류하고 scene graph의 위치 정보와 연결할 수 있다.

### 18.3.1 목표

- **전체 지도 이해**: 공간 구조와 의미 파악 — "여기는 주방이고, 저기는 거실이다"
- **Semantic Scene Graph**: 객체 간 관계 표현 — "컵이 테이블 위에 있다"
- **Long-term Memory**: 환경 변화 추적 — "어제는 여기에 의자가 없었는데 오늘은 있다"

**실제 시나리오**: 가정용 서비스 로봇이 매일 집 안을 다니며 환경을 학습한다. Global Module은 "거실에 소파·TV·테이블이 배치되어 있으며 주방에는 냉장고·싱크대가 위치한다"는 고수준 시맨틱 지도를 유지한다. 사용자가 "거실 테이블에 있는 리모컨 가져와"라고 하면 Scene Graph에서 리모컨의 위치를 찾아 Local Module에 waypoint를 전달한다.

### 18.3.2 특징

DINOv2와 SAM2에는 크기가 다른 model variant가 있고 모두 수십억 parameter인 것은 아니다. Global Module의 hardware와 update period는 variant, input resolution, precision, scene 수와 허용 응답 시간으로 정한다. Local control deadline과 분리할 수 있는 task도 있지만, 사용자 상호작용이나 변화 감지처럼 end-to-end latency 요구가 있는 경우에는 별도 budget이 필요하다.

### 18.3.3 기술 스택

**Vision Foundation Models** (→ 11장 참고):
- DINOv2: Dense feature extraction — 이미지를 패치 격자로 나눠 각 패치의 feature 벡터를 생성 (픽셀 단위로 쓰려면 보간·업샘플링이 필요)
- SAM2: promptable image/video segmentation — point·box·mask prompt로 대상 mask를 추적
- GroundingDINO: Text-guided detection — "빨간 컵"이라고 말하면 찾아줌

**3D Understanding** (→ 11장, 13장 참고):
- Gaussian Splatting with semantic features — 예쁘고 빠른 3D 재구성 + 의미 정보
- 3D Scene Graph 구축 — 객체들의 관계를 그래프로 표현
- VFM features의 3D lifting — 2D 이미지에서 뽑은 feature를 3D 공간에 올리기

**Language Integration** (→ 11장, 12장 참고):
- CLIP features for open-vocabulary — "본 적 없는 물체"도 텍스트로 검색 가능
- LLM for scene reasoning — "이 방은 어떤 용도일까?" 추론
- VLA for action planning — "컵을 집으려면 어떻게 팔을 움직여야 할까?"

### 18.3.4 예시 구현

```python
# Global Module 개념 코드
class GlobalModule:
    def __init__(self):
        self.dinov2 = load_dinov2()
        self.sam = load_sam2()
        self.scene_graph = SemanticSceneGraph()
        self.gaussian_map = GaussianSplatMap()

def process_keyframe(self, image, depth, pose):
        # 1. VFM feature 추출
        features = self.dinov2.extract(image)

# 2. Promptable segmentation (텍스트 어휘 기반 프롬프트는 GroundingDINO/CLIP이 만들어 준다)
        masks = self.sam.segment(image, prompts=self.get_prompts())

# 3. 3D Scene Graph 업데이트
        self.scene_graph.update(masks, depth, pose, features)

# 4. Gaussian Map 업데이트
        self.gaussian_map.add_keyframe(image, depth, pose, features)

def query(self, text_prompt):
        # "Where is the red cup?" → 위치 반환
        return self.scene_graph.find(text_prompt)
```

**시나리오로 읽기**: Local Module에서 키프레임이 올 때마다 `process_keyframe()`이 호출된다. DINOv2로 이미지에서 풍부한 feature를 뽑고 SAM으로 물체를 분리한 뒤, 그 결과를 3D Scene Graph와 Gaussian Map에 누적한다. 나중에 사용자가 "빨간 컵 어디 있어?"라고 물으면 `query()`로 검색한다. 이 과정은 1초 정도 걸려도 괜찮다. 실시간 안전은 Local Module이 맡기 때문이다.

## 18.4 두 모듈의 협업

두 모듈은 독립적으로 동작하면서 정보를 주고받는다. 눈앞의 도로를 보고 운전하는 드라이버(Local)와 전체 경로를 안내하는 내비게이션 앱(Global)의 관계에 가깝다.

### 18.4.1 Local → Global

**전송 내용**:
- 키프레임 이미지/포인트 클라우드
- 로컬 포즈
- 센서 메타데이터

**키프레임 선택 기준**:
- 이동 거리/회전량 threshold — "1 m 이동하거나 30도 회전하면 보내기"
- 장면 변화 감지 — "새로운 방에 들어갔다"
- 정보량 (특징점 수, 커버리지) — "이 프레임에 새로운 정보가 많다"

### 18.4.2 Global → Local

**전송 내용**:
- 사전 지도 (필요 영역) — "주방 근처의 장애물 정보"
- Semantic 정보 (객체 위치, 클래스) — "테이블은 여기, 의자는 저기"
- 네비게이션 waypoints — "이 경로를 따라가라"

**예시 시나리오**:

```
1. 사용자: "Go to the kitchen and bring the cup"

2. Global:
   - VLM으로 명령 이해
   - Scene Graph에서 kitchen, cup 위치 찾기
   - 경로 계획

3. Global → Local:
   - Waypoints: [현재 → 복도 → 주방 → 컵 앞]
   - 주방 영역의 local map
   - 컵의 예상 위치

4. Local:
   - Waypoints 따라 이동
   - 실시간 장애물 회피
   - 컵 근처에서 정밀 접근
```

**다른 시나리오 — 통신 불안정 상황**: 로봇이 지하 주차장에서 작업 중인데 WiFi가 끊어졌다. 이 경우 Local Module만으로 동작해야 한다. Odometry로 위치를 추정하고, 장애물을 피하면서 마지막으로 받은 waypoint까지 이동한다. WiFi가 복구되면 그동안의 데이터를 Global에 한꺼번에 보내고, 업데이트된 계획을 받는다. 실제 로봇은 이런 **graceful degradation**이 필요하다.

### 18.4.3 통신 및 동기화

**통신 방식**:
- ROS2 DDS: 로컬 네트워크 (같은 건물 안)
- WebSocket: 클라우드 연결 (원격 서버)
- 5G/WiFi: 모바일 로봇 (실외 환경)

**동기화 전략**: 연속 스트리밍 대신 키프레임 단위로 보내 대역폭을 줄이고, Global 응답을 기다리지 않고 비동기로 처리해 Local이 멈추지 않도록 한다. 자주 방문하는 영역은 캐싱해 중복 전송을 피한다.

## 18.5 연구 과제 예시

아래 연구 과제들은 실제로 우리 연구실에서 진행하거나 진행할 수 있는 주제들이다. 각 과제마다 선행 학습이 필요한 챕터를 표시해두었으니, 관심 있는 주제가 있으면 해당 챕터부터 공부하자.

### Local Module 연구

1. **더 가벼운 SLAM**
    - 신경망 기반 경량 VO — 기존 VO를 대체하되 Jetson에서 동작하도록 설계
    - 이벤트 카메라 활용 — 저전력·초고속 센서로 극한 환경의 SLAM을 구성
    - 하드웨어 가속(FPGA) — SLAM의 핵심 연산을 전용 하드웨어로 구현
    - 선행 학습: 9장(카메라 모델), 14장(Visual Odometry, SLAM) 필수, 3장(최적화) 권장
2. **효율적 장애물 인식**
    - Depth-only obstacle detection — RGB 없이 depth 정보만으로 장애물 감지
    - 시간적 일관성 — 물체가 프레임마다 나타났다 사라지는 깜빡임을 억제
    - Uncertainty-aware — "이게 장애물인지 확실하지 않다"는 불확실성 정보도 회피 결정에 반영
    - 선행 학습: 10장(Depth Estimation, Object Detection) 필수, 3장(좌표 변환) 중요
3. **센서 융합 최적화**
    - Tight coupling 경량화 — IMU+Camera+LiDAR를 촘촘하게 결합하되 가볍게
    - 센서 드롭아웃 대응 — 센서 하나가 고장나도 계속 동작
    - 선행 학습: 2장(센서), 14장(Visual Odometry), 3장(최적화) 필수

### Global Module 연구

1. **VFM의 3D 확장**
    - DINOv2 features in 3D — 2D feature를 3D 공간에 올려서 활용
    - Semantic Gaussian Splatting — 3D 재구성에 의미 정보를 같이 넣기
    - 3D scene understanding — "이 공간이 어떤 구조인지" 이해하기
    - 선행 학습: 10장(Depth), 13장(3D 표현), 11장(VFM) 필수, 9장(카메라 모델) 기본
2. **VLA 통합**
    - Open-vocabulary manipulation — "저 빨간 거 집어" 같은 자연어 명령으로 로봇팔 제어
    - 언어 기반 내비게이션 — 자연어 명령에 따라 이동
    - 상황 인식 행동 — "아이가 있으니 천천히"처럼 맥락을 행동 제약으로 변환
    - 선행 학습: 11장(VFM 활용), 12장(VLA) 필수, 10장(Detection)도 알면 좋다
3. **Scalability**: 아파트 단지·캠퍼스 전체를 단일 지도로 표현하고, 수 GB짜리 지도를 압축·갱신하며, 여러 로봇이 함께 만든 지도를 공유하는 문제다. 선행 학습: 14장(SLAM), 3장(최적화), 11장(VFM) 필수.

### Integration 연구

1. **효율적 통신**: 무엇을 언제 보낼지 정해야 한다. 모든 프레임을 보내면 대역폭을 소진하고, 키프레임만 보내면 Global이 환경 변화를 놓칠 수 있다. 5G가 끊기거나 WiFi가 느릴 때의 열화 전략도 함께 설계한다. 선행 학습: 14장(SLAM, 키프레임 선택), Local/Global 모듈 이해.
2. **Fallback 전략**
    - 통신 끊김 시 Local-only 동작 — 서버 연결 없이도 기본 임무 수행
    - Graceful degradation — 기능이 점진적으로 줄어들되, 갑자기 멈추지는 않기
    - 선행 학습: 시스템 전체 이해 필요. 최소 3~14장은 읽고 오자
3. **일관성 유지**: Local이 "여기 빈 공간"이라 보고, Global이 "거기 의자 있음"이라 기억하면 로봇은 어느 쪽을 믿어야 할지 모른다. 두 지도를 동기화하고, 의미 정보의 근거 없는 변동("저건 의자"가 근거 없이 "테이블"로 바뀌는 일)을 억제하되 새 관측에 따른 갱신은 허용하는 문제다. 선행 학습: 3장(최적화), 14장(SLAM, 맵 관리) 필수.

## 18.6 Motivation과 Novelty를 가르는 질문

Motivation이 문제 해결의 당위성을 제시한다면, novelty는 기존 방법론 대비 어떤 기술적 지점을 구체적으로 변경했는지 입증한다. 둘은 연구 방향을 잡거나 첫 논문을 쓸 때 자주 뒤섞인다.

"기존 방법이 X를 하지 못하므로 모듈을 붙였다"는 문장만으로는 motivation을 넘어가기 어렵다. 그 모듈이 왜 필요한지, 왜 그 형태여야 하는지까지 설명해야 novelty가 구체화된다.

아래 세 논문은 문제 제기와 설계 기여의 차이를 보여준다.

### Case 1 — ORB-SLAM2 (Mur-Artal & Tardós 2017)

- **Motivation**: 단안용 ORB-SLAM의 map reuse·loop closing·relocalization 구조를 stereo와 RGB-D 입력에도 적용한다.
- **직접적 확장**: 입력 modality마다 별도의 SLAM 시스템을 만든다.
- **논문의 설계**: 세 modality가 tracking·local mapping·loop closing의 시스템 구조와 ORB 특징을 공유하되, stereo는 disparity에서 depth를 얻고 RGB-D는 측정 depth로 가상 오른쪽 좌표를 만들어 둘 다 stereo 관측 형태로 통일한 뒤 metric-scale bundle adjustment에 반영한다.
- **설계 원리**: 공통 시스템 구조는 유지하고, modality별 차이를 관측 생성과 bundle-adjustment 잔차에 둔다.

원 논문은 [*ORB-SLAM2: An Open-Source SLAM System for Monocular, Stereo and RGB-D Cameras*](https://doi.org/10.1109/TRO.2017.2705103)다. 2015년 ORB-SLAM은 단안 시스템이므로 이 사례의 세 modality 통합 근거로 사용할 수 없다.

### Case 2 — 3D Gaussian Splatting (Kerbl et al. 2023)

- **Motivation**: NeRF가 너무 느리다 → 빠르게 만들어야 한다
- **직접적 확장**: NeRF 위에 sparse sampling · pruning · distillation 같은 가속 모듈을 추가
- **논문의 설계**: ray-marching의 탐색 비용을 병목으로 보고, 명시적 primitive인 3D Gaussian으로 표현을 교체. Primitive는 직접 rasterization할 수 있게 설계
- **설계 원리**: 연산 속도 저하의 근본 원인을 개별 커널 연산을 넘어 공간 표현과 래스터화 렌더링 방식의 구조적 결합에서 찾는다.

### Case 3 — DUSt3R (Wang et al. 2024)

기존 SfM은 camera intrinsics가 필요하고 단계별 오차에 민감하다. Matching이나 triangulation 한 단계만 신경망으로 교체할 수도 있지만, Wang et al.은 출력 형식 자체를 바꿨다. 두 view를 입력받아 공통 좌표계의 pointmap을 직접 예측하면 intrinsics, correspondence, structure를 함께 얻을 수 있다. Camera intrinsics는 선행 주입해야 하는 고정 입력값을 벗어나 pointmap 예측 결과로부터 자연스럽게 유도되는 산출물로 바뀐다. DUSt3R의 기여는 이처럼 SfM의 단계별 분해를 pointmap 예측 문제로 다시 정식화한 데 있다.

### 세 논문이 공유하는 설계 질문

세 논문은 모두 *왜 이 모듈이 이 형태여야 하는가*를 묻는다. 답은 모듈의 수보다 인터페이스, 표현, 출력 형식을 어떻게 정했는지에 있다.

> Contribution 절에는 *왜 이 모듈이어야 하는가*에 대한 답이 있어야 한다. 문제의 필요성만 설명한다면 motivation에 머물고, 설계 선택의 근거까지 제시해야 novelty가 드러난다.

논문에서 motivation과 method를 전개하는 방법은 [「연구노트」 Ch.23 Introduction](../research-notes/guide.html#chapter-23)과 [Ch.25 Method](../research-notes/guide.html#chapter-25)에서 자세히 다룬다.

---

# Ch.19 — AI 코딩 에이전트 활용하기

## 19.1 로봇 앞에서 에이전트를 쓰는 순서

AI 코딩 에이전트는 작은 ROS2 노드, launch 파일, 로그 요약, 실험 스크립트 정리에 쓸 만하다. 로봇 실험에서는 현재 상태를 먼저 출력으로 남겨야 한다. 센서가 보이는지, topic이 떠 있는지, QoS가 맞는지, container 안에 device가 들어왔는지부터 확인한다.

자세한 운영 절차는 [`AI와 로봇 연구하기` 11장 — 로봇 실험의 조건을 기록한다](../ai-research-practice/guide.html#part-03-rules-11장-로봇-실험의-조건을-기록한다)에 모아 두었다. 여기에는 `robotics-practice`를 읽는 동안 바로 붙여 쓸 런타임 관측값만 둔다.

## 19.2 먼저 남길 런타임 신호

로봇의 현재 상태는 코드만으로 드러나지 않는다. ROS2 topic, DDS QoS, `/clock`, TF buffer, Docker device mapping, USB bus, LiDAR IP, camera exposure, Jetson architecture는 명령 출력으로 확인한다.

문제가 생기면 먼저 다음 신호를 본다.

```bash
ros2 topic list
ros2 topic info /camera/image_raw --verbose
ros2 node list
ros2 topic hz /cmd_vel
dmesg | tail -30
lsusb -t
```

LiDAR나 네트워크 센서는 packet부터 본다.

```bash
ping 192.168.1.201
sudo tcpdump -i eth0 udp port 2368 -c 10
```

아래 명령으로 카메라 device와 지원 영상 포맷을 본다.

```bash
v4l2-ctl --list-devices
v4l2-ctl -d /dev/video0 --list-formats-ext
```

이 출력이 없으면 원인 판단을 보류한다. 코드가 맞아도 필수 device가 container 안에 없거나, QoS가 호환되지 않는 조합이거나, 센서가 같은 USB controller에 몰려 대역폭·전력이 부족하면 로봇은 움직이지 않는다.

## 19.3 질문에 붙일 정보

로봇 runtime 문제를 다룰 때는 최소한 다음 묶음을 붙인다.

```text
OS / ROS version:
hardware platform:
sensor model:
full error message:
ros2 topic list:
ros2 topic info --verbose:
ros2 node list:
docker run command:
network / IP range:
dmesg or device log:
what changed since last working run:
```

이 묶음은 답을 검증하는 기준이다. 실행 전에는 package 지원 범위, 현재 설정, device 권한, target architecture, metric 조건을 다시 확인한다.

## 19.4 다음으로 읽을 곳

로봇 runtime과 AI 에이전트 협업의 상세 체크리스트는 [`ai-research-practice` 11장과 부록 F](../ai-research-practice/guide.html#part-03-rules-11장-로봇-실험의-조건을-기록한다)에 있다.

AI 답변을 연구 행동으로 옮기는 일반 원칙은 같은 가이드의 1–10장과 12–13장에서 다룬다. 논문과 코드를 맞춰 보는 법, 실험 숫자의 조건, reviewer 답변의 주장과 근거까지 함께 읽으면 runtime 체크리스트의 쓰임이 분명해진다.

---

# Ch.20 — 추천 자료

교과서, 강의, 논문을 주제별로 묶고, 맨 아래 **학습 경로**에서 선수지식에 따른 읽기 순서를 제시한다.

## 20.1 교과서

### Computer Vision

**Multiple View Geometry in Computer Vision** (Hartley & Zisserman)
- 다시점 기하학의 핵심 교재
- 카메라 모델, Epipolar Geometry, 3D 복원
- 수학적으로 엄밀하며, 필요한 주제의 장을 골라 참고하기 좋음
- 링크: [Cambridge University Press](https://www.cambridge.org/core/books/multiple-view-geometry-in-computer-vision/0B6F289C78B2B23F596CAA76D3D43F7A)
- 저자 홈페이지에서 일부 챕터 PDF 제공: https://www.robots.ox.ac.uk/~vgg/hzbook/

**Computer Vision: Algorithms and Applications** (Szeliski)
- 포괄적인 CV 교과서
- 최신 버전 (2022)에 딥러닝 포함
- 무료 PDF 제공
- 무료 PDF: https://szeliski.org/Book/

### Robotics

**Probabilistic Robotics** (Thrun, Burgard, Fox)
- 확률적 로보틱스의 표준 교재
- Kalman Filter, Particle Filter, SLAM
- SLAM을 연구할 때 읽어야 할 교재
- 링크: [MIT Press](https://mitpress.mit.edu/9780262201629/probabilistic-robotics/)
- PDF는 공식적으로 무료가 아니지만, 저자의 강의 슬라이드가 대부분의 내용을 커버한다

**State Estimation for Robotics** (Tim Barfoot)
- 상태 추정 심화
- Lie Groups, Factor Graph — 수학적으로 깊지만 설명이 친절하다
- 무료 PDF 제공
- 무료 PDF: http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17.pdf

### Deep Learning

**Deep Learning** (Goodfellow, Bengio, Courville)
- 딥러닝 이론 표준 교과서
- 무료 온라인 제공
- 무료 온라인(장별 HTML): https://www.deeplearningbook.org/

**Dive into Deep Learning** (d2l.ai)
- 실습 중심 — 코드와 함께 배운다
- 무료, 인터랙티브
- 링크: https://d2l.ai/
- PyTorch, TensorFlow, JAX 버전 모두 지원

### 수학 보충

**Introduction to Linear Algebra** (Gilbert Strang)
- 선형대수를 직관적으로 설명하는 명저
- MIT OCW 강의와 함께 보면 이해에 도움이 됨
- 링크: https://math.mit.edu/~gs/linearalgebra/ila6/indexila6.html

**Convex Optimization** (Boyd & Vandenberghe)
- 최적화 이론의 표준 교재
- 무료 PDF 제공
- 무료 PDF: https://web.stanford.edu/~boyd/cvxbook/

## 20.2 온라인 강의

### Computer Vision

**CS231n: Convolutional Neural Networks for Visual Recognition** (Stanford)
- 딥러닝 비전의 기초를 다루는 강의
- 무료 강의 자료, 영상
- 강의 영상: https://www.youtube.com/playlist?list=PL3FW7Lu3i5JvHM8ljYj-zLfQRF3EO8sYv
- 강의 노트: https://cs231n.github.io/

**CS231A: Computer Vision, From 3D Reconstruction to Recognition** (Stanford)
- 3D Vision 중심
- 기하학 기반
- 강의 자료: https://web.stanford.edu/class/cs231a/

### SLAM

**Cyrill Stachniss SLAM Course** (YouTube)
- SLAM 이론 강의 — 독일어 억양이 있지만 설명이 명확함
- SLAM 입문자에게 먼저 권할 만한 강의
- YouTube: https://www.youtube.com/playlist?list=PLgnQpQtFTOGQrZ4O5QzbIHgl3b1JHimN_

**Multiple View Geometry** (TUM, Daniel Cremers 교수)
- YouTube 공개 — 수학적으로 탄탄한 강의
- YouTube: https://www.youtube.com/playlist?list=PLTBdjV_4f-EJn6udZ34tht9EVIW7lbeo4

**SLAM 입문 (한국어)**:
- SLAM KR 커뮤니티(국내 SLAM 연구자 모임)의 스터디 자료

### ROS

**ROS2 공식 튜토리얼**
- 가장 최신 정보
- 링크: https://docs.ros.org/en/humble/Tutorials.html (Humble 기준)
- ROS2 Iron/Jazzy 등 다른 버전은 상단 드롭다운에서 변경

**The Construct** (온라인 플랫폼)
- ROS 전문 강의
- 일부 무료
- 링크: https://www.theconstructsim.com/

### 딥러닝 기초

**CS229: Machine Learning** (Stanford, Andrew Ng)
- ML 기초 — 딥러닝 전에 살펴보기 좋음
- YouTube: https://www.youtube.com/playlist?list=PLoROMvodv4rMiGQp3WXShtMGgzqpfVfbU

**Neural Networks: Zero to Hero** (Andrej Karpathy)
- 신경망을 밑바닥부터 구현하면서 배우기
- 설명과 코드를 함께 진행
- YouTube: https://www.youtube.com/playlist?list=PLAqhIrjkxbuWI23v9cThsA9GvCAUhRvKZ

## 20.3 유튜브 채널 추천

교과서나 전체 강의보다 짧은 단위로 볼 수 있는 유튜브 채널들이다.

| 채널 | 주제 | 특징 |
| --- | --- | --- |
| **Cyrill Stachniss** | SLAM, Robotics | 학부 수업 수준으로 SLAM을 체계적으로 설명 |
| **First Principles of Computer Vision** (Shree Nayar) | Computer Vision | CV 기초를 개념별로 설명 |
| **Andrej Karpathy** | Deep Learning, AI | Tesla AI Director 출신. Neural Net을 밑바닥부터 구현 |
| **Yannic Kilcher** | 논문 리뷰 | 최신 ML/AI 논문을 매주 리뷰. 논문 읽는 법을 배울 수 있다 |
| **Two Minute Papers** | AI 연구 트렌드 | 최신 연구를 2~3분 영상으로 소개. "What a time to be alive!" |
| **3Blue1Brown** | 수학 시각화 | 선형대수, 미적분을 시각적으로 설명. 수학이 막힐 때 |
| **Computerphile** | CS 전반 | 컴퓨터과학의 다양한 주제를 쉽게 설명 |
| **sentdex** | Python, ML | Python으로 ML/로보틱스 실습. 코드 중심 |
| **The Coding Train** | 알고리즘 시각화 | 알고리즘을 시각적으로 이해. 에너지 넘치는 진행 |

링크:
- Cyrill Stachniss: https://www.youtube.com/@CyrillStachniss
- First Principles of Computer Vision: https://www.youtube.com/@firstprinciplesofcomputerv3258
- Andrej Karpathy: https://www.youtube.com/@AndrejKarpathy
- Yannic Kilcher: https://www.youtube.com/@YannicKilcher
- Two Minute Papers: https://www.youtube.com/@TwoMinutePapers
- 3Blue1Brown: https://www.youtube.com/@3blue1brown
- Computerphile: https://www.youtube.com/@Computerphile
- sentdex: https://www.youtube.com/@sentdex
- The Coding Train: https://www.youtube.com/@TheCodingTrain

## 20.4 논문 읽기

### 어떻게 읽을 것인가?

논문을 고르는 기준과 Keshav의 3-pass, 5 Cs, reviewer 관점, CCC 렌즈는 [「연구노트」 Ch.6–15 — 읽기](../research-notes/guide.html#chapter-6)에서 자세히 다룬다.

### 필독 논문 리스트

**Classical CV/SLAM**:
- ORB-SLAM: Mur-Artal et al., 2015 — [arXiv:1502.00956](https://arxiv.org/abs/1502.00956)
- LOAM: Zhang & Singh, 2014 — [RSS 2014](https://www.ri.cmu.edu/pub_files/2014/7/Ji_LidarMapping_RSS2014_v8.pdf)
- VINS-Mono: Qin et al., 2018 — [arXiv:1708.03852](https://arxiv.org/abs/1708.03852)

**Deep Learning 기초**:
- ResNet: He et al., 2015 — [arXiv:1512.03385](https://arxiv.org/abs/1512.03385)
- Transformer (Attention Is All You Need): Vaswani et al., 2017 — [arXiv:1706.03762](https://arxiv.org/abs/1706.03762)
- ViT: Dosovitskiy et al., 2020 — [arXiv:2010.11929](https://arxiv.org/abs/2010.11929)

**Object Detection**:
- Faster R-CNN: Ren et al., 2015 — [arXiv:1506.01497](https://arxiv.org/abs/1506.01497)
- YOLO (original): Redmon et al., 2015 — [arXiv:1506.02640](https://arxiv.org/abs/1506.02640)
- DETR: Carion et al., 2020 — [arXiv:2005.12872](https://arxiv.org/abs/2005.12872)

**Foundation Models**:
- CLIP: Radford et al., 2021 — [arXiv:2103.00020](https://arxiv.org/abs/2103.00020)
- SAM (Segment Anything): Kirillov et al., 2023 — [arXiv:2304.02643](https://arxiv.org/abs/2304.02643)
- DINOv2: Oquab et al., 2023 — [arXiv:2304.07193](https://arxiv.org/abs/2304.07193)

**최신 트렌드**:
- RT-2: Brohan et al., 2023 — [arXiv:2307.15818](https://arxiv.org/abs/2307.15818)
- 3D Gaussian Splatting: Kerbl et al., 2023 — [arXiv:2308.04079](https://arxiv.org/abs/2308.04079)
- Depth Anything: Yang et al., 2024 — [arXiv:2401.10891](https://arxiv.org/abs/2401.10891)

> 논문 검색에는 [Google Scholar](https://scholar.google.com/), [Semantic Scholar](https://www.semanticscholar.org/), [arXiv](https://arxiv.org/)를 활용할 수 있다. 벤치마크 순위와 코드 링크를 함께 보여 주던 Papers With Code는 2025년 서비스를 종료해 현재 도메인은 Hugging Face로 넘어간다.

### 논문 작성 도구

> **추천 자료**
> - [Overleaf](https://www.overleaf.com/) — 공동 작성 기능을 제공하는 온라인 LaTeX 에디터
> - [Mathpix](https://mathpix.com/) — 수식 스크린샷을 LaTeX 코드로 변환
> - [Detexify](http://detexify.kirelabs.org/classify.html) — 손으로 그려서 LaTeX 기호를 검색
> - [Tables Generator](https://www.tablesgenerator.com/) — LaTeX/HTML 테이블 생성기
> - [QuillBot](https://quillbot.com/) — 영어 문장 paraphrasing 도구. 논문 영작에 유용
> - [Ludwig](https://ludwig.guru/) — 영어 표현 검색 엔진. 원어민이 실제로 쓰는 표현을 확인
> - [DL Monitor (deeplearn.org)](https://deeplearn.org/) — 주요 학회/arXiv의 딥러닝 논문을 자동 추적

## 20.5 주요 학회

CV·로보틱스·자율주행 학회의 일정과 성격은 [「연구노트」 Ch.34 — 학회 2~3주 전 체크리스트](../research-notes/guide.html#chapter-34)의 *분야별 학회 reference* 표에 정리되어 있다.

학회에 참석하는 목적과 발표를 시작하는 방법도 같은 장에서 확인할 수 있다.

## 20.6 유용한 GitHub 저장소

### SLAM

```
# ORB-SLAM3 — Visual(-Inertial) SLAM의 레퍼런스
https://github.com/UZ-SLAMLab/ORB_SLAM3

# VINS-Fusion — 다중 카메라+IMU 융합
https://github.com/HKUST-Aerial-Robotics/VINS-Fusion

# LIO-SAM — LiDAR-Inertial SLAM (factor graph 기반)
https://github.com/TixiaoShan/LIO-SAM

# FAST-LIO2 — 빠른 LiDAR-Inertial Odometry
https://github.com/hku-mars/FAST_LIO

# RTAB-Map — RGB-D SLAM, 대규모 환경 지원
https://github.com/introlab/rtabmap

# SplaTAM — 3D Gaussian Splatting 기반 SLAM
https://github.com/spla-tam/SplaTAM
```

### Deep Learning

```
# Ultralytics YOLO — YOLOv8/v11, 가장 쓰기 쉬운 detection 프레임워크
https://github.com/ultralytics/ultralytics

# HuggingFace Transformers — NLP/Vision 모델 허브
https://github.com/huggingface/transformers

# OpenMMLab — Detection, Segmentation, 3D 등 종합 프레임워크
https://github.com/open-mmlab

# PyTorch Lightning — 학습 코드 구조화
https://github.com/Lightning-AI/pytorch-lightning

# timm (PyTorch Image Models) — 사전학습된 Vision 모델 모음
https://github.com/huggingface/pytorch-image-models
```

### 3D Vision

```
# Open3D — 포인트 클라우드, 메쉬 처리
https://github.com/isl-org/Open3D

# 3D Gaussian Splatting — 원본 구현
https://github.com/graphdeco-inria/gaussian-splatting

# NeRF Studio — NeRF/3DGS 통합 프레임워크
https://github.com/nerfstudio-project/nerfstudio

# Depth Anything V2 — 범용 depth estimation
https://github.com/DepthAnything/Depth-Anything-V2

# COLMAP — Structure from Motion 파이프라인
https://github.com/colmap/colmap
```

### VFM/VLA

```
# Segment Anything (SAM) — Meta의 범용 세그멘테이션
https://github.com/facebookresearch/segment-anything

# SAM 2 — 비디오까지 확장
https://github.com/facebookresearch/sam2

# DINOv2 — Self-supervised vision features
https://github.com/facebookresearch/dinov2

# Grounded-SAM — 텍스트로 물체 찾기 + 세그멘테이션
https://github.com/IDEA-Research/Grounded-Segment-Anything

# OpenVLA — 오픈소스 Vision-Language-Action 모델
https://github.com/openvla/openvla
```

### ROS / 로봇 개발

```
# ROS2 공식 저장소
https://github.com/ros2

# Nav2 — ROS2 네비게이션 스택
https://github.com/ros-navigation/navigation2

# MoveIt2 — 로봇팔 모션 플래닝
https://github.com/moveit/moveit2

# micro-ROS — 마이크로컨트롤러용 ROS
https://github.com/micro-ROS
```

### 유용한 Awesome 리스트

```
# Awesome SLAM — SLAM 자료 종합
https://github.com/SilenceOverflow/Awesome-SLAM

# Awesome Robotics — 로보틱스 자료 종합
https://github.com/kiloreux/awesome-robotics

# Awesome 3D Gaussian Splatting — 3DGS 논문/코드 모음
https://github.com/MrNeRF/awesome-3D-gaussian-splatting
```

## 20.7 추천 학습 경로

아래는 기존 1.4절에 있던 학습 경로를 확장한 것이다. 각 단계별로 구체적인 자료와 링크를 달았으니, 자기 수준에 맞는 단계부터 시작하면 된다.

### 입문 단계 (1~3개월)

**목표**: 기초 도구 습득 — 일단 뭔가 돌려볼 수 있는 상태

| 주제 | 학습 내용 | 추천 자료 |
| --- | --- | --- |
| Python 숙달 | 문법, 클래스, 파일 I/O | [점프 투 파이썬](https://wikidocs.net/book/1) (무료, 한국어) |
| NumPy, OpenCV 기초 | 배열 연산, 이미지 읽기/처리 | [OpenCV 공식 튜토리얼](https://docs.opencv.org/4.x/d6/d00/tutorial_py_root.html) |
| 선형대수 복습 | 행렬, 고유값, SVD | [3Blue1Brown: Essence of Linear Algebra](https://www.youtube.com/playlist?list=PLZHQObOWTQDPD3MizzM2xVFitgF8hE_ab) |
| 확률/통계 복습 | 베이즈 정리, 가우시안 | [StatQuest](https://www.youtube.com/@statquest) |
| ROS2 기본 | 노드, 토픽, 서비스 | [ROS2 공식 튜토리얼](https://docs.ros.org/en/humble/Tutorials.html) |
| Git 사용법 | commit, branch, PR | [Git 입문](https://backlog.com/git-tutorial/kr/) (한국어) |

**실습 과제**:
- OpenCV로 이미지 처리 (grayscale 변환, edge detection, feature 추출)
- 간단한 ROS2 노드 작성 (publisher/subscriber)
- 카메라 캘리브레이션 수행 — 본 문서 9장 참고
- 본 문서의 **3장, 9장**을 읽으면서 좌표 변환과 카메라 모델을 이해한다

**마일스톤**: Python으로 이미지를 읽어서 특징점을 추출하고, 두 이미지 간 매칭을 시각화할 수 있으면 입문 단계 졸업이다.

### 중급 단계 (3~6개월)

**목표**: 핵심 기술 이해 — 논문을 읽고 코드를 돌려볼 수 있는 상태

| 주제 | 학습 내용 | 추천 자료 |
| --- | --- | --- |
| 딥러닝 기초 (PyTorch) | CNN, 학습, 역전파 | [CS231n](https://www.youtube.com/playlist?list=PL3FW7Lu3i5JvHM8ljYj-zLfQRF3EO8sYv) + [PyTorch 공식 튜토리얼](https://pytorch.org/tutorials/) |
| Object Detection | YOLO, Faster R-CNN | [Ultralytics 문서](https://docs.ultralytics.com/) + 본 문서 10장 |
| Visual SLAM 이해 | ORB-SLAM3 분석 | [Cyrill Stachniss SLAM 강의](https://www.youtube.com/playlist?list=PLgnQpQtFTOGQrZ4O5QzbIHgl3b1JHimN_) + 본 문서 14장 |
| 포인트 클라우드 처리 | Open3D 사용법 | [Open3D 튜토리얼](http://www.open3d.org/docs/release/tutorial/) + 본 문서 13장 |
| Depth Estimation | 단안 카메라 depth 추정 | 본 문서 10장 + [Depth Anything 코드](https://github.com/DepthAnything/Depth-Anything-V2) |

**실습 과제**:
- KITTI 데이터셋 다루기 — [KITTI 홈페이지](https://www.cvlibs.net/datasets/kitti/)
- YOLOv8 파인튜닝 — 커스텀 데이터셋으로 fine-tuning
- ORB-SLAM3 실행 및 분석 — TUM RGB-D 데이터셋으로 평가
- TUM RGB-D 벤치마크 — ATE, RPE 계산해보기
- 본 문서의 **9~14장**을 읽으면서 이론적 배경을 다진다

**마일스톤**: ORB-SLAM3를 직접 빌드하고 데이터셋으로 돌려서, trajectory를 ground truth와 비교할 수 있으면 중급 단계 졸업이다.

### 고급 단계 (6개월+)

**목표**: 연구 능력 개발 — 새로운 아이디어를 내고 실험할 수 있는 상태

| 주제 | 학습 내용 | 추천 자료 |
| --- | --- | --- |
| VFM 심화 (1.4절 중급 항목의 확장) | DINOv2, SAM, CLIP | 본 문서 10~11장 + 논문 직접 읽기 |
| 3D 재구성 심화 | NeRF, 3D Gaussian Splatting | [NeRF Studio](https://github.com/nerfstudio-project/nerfstudio) + 본 문서 13장 |
| 논문 읽기 및 구현 | 최신 논문 분석 | [Hugging Face Papers](https://huggingface.co/papers/trending) + [Yannic Kilcher 채널](https://www.youtube.com/@YannicKilcher) — *본격은 [「연구노트」 Ch.6–15 — 읽기](../research-notes/guide.html#chapter-6)* |
| 새로운 아이디어 실험 | 가설 수립, 실험 설계 | 연구실 세미나 + 학회 워크숍 참여 — *본격은 [「연구노트」 Ch.1–5 — 시작하기](../research-notes/guide.html#chapter-1)* |
| 벤치마크 평가 | 정량적 비교 | 각 분야별 표준 벤치마크 (KITTI, ScanNet, Replica 등) — *결과 해석 frame은 [「연구노트」 Ch.32 — Revision/Rebuttal](../research-notes/guide.html#chapter-32)* |

**실습 과제**:
- 최신 논문 코드 분석 — GitHub에서 코드를 받아 직접 돌려보기
- 자체 개선 아이디어 실험 — 이 부분을 바꾸면 어떻게 될지 직접 시도
- 논문 작성 시도 — *본격 frame은 [「연구노트」 Part 2 — 쓰기](../research-notes/guide.html#chapter-16)* 참고
- 본 문서의 **10~13장**을 읽으면서 최신 연구 방향을 파악한다

**마일스톤**: 기존 논문의 방법을 수정/개선한 실험을 하고, 그 결과를 정량적으로 비교할 수 있으면 고급 단계에 진입한 것이다. 학회 워크숍에 제출할 수 있는 수준이 되는 것을 목표로 하자.

### 학습 순서 요약

```
입문 (1-3개월)                    중급 (3-6개월)                     고급 (6개월+)
─────────────                   ─────────────                    ─────────────
Python + NumPy                  PyTorch + CNN                    VFM (DINOv2, SAM)
OpenCV 기초                     YOLO 파인튜닝                     3DGS / NeRF
선형대수/확률 복습               ORB-SLAM3 분석                    논문 구현
ROS2 기초                       KITTI/TUM 벤치마크                 아이디어 실험
Git 사용법                      포인트 클라우드 (Open3D)           논문 작성
                                Depth Estimation
     ↓                                ↓                                ↓
 "코드를 돌릴 수 있다"          "논문을 읽고 재현한다"           "새 아이디어를 실험한다"
```

## 20.8 연구 실전

*대학원 수준.* 논문 읽기·쓰기, 실험 설계, 학회 발표, peer review는 [research-notes](../research-notes/guide.html)에서, 박사과정의 장기 운영은 [grad-notes](../grad-notes/guide.html)에서 자세히 다룬다. 여기서는 SLAM·CV·로보틱스에 직접 적용되는 항목을 연결한다.

연구를 운영할 때는 방향, 지속 가능한 작업 방식, 도구를 함께 살펴야 한다.

### 20.8.0 연구자 마인드셋

- 방향·엔진·도구 세 layer + 통합 frame → [「대학원노트」 Ch.17 — 연구가 삶이 되는 조건](../grad-notes/guide.html#chapter-17) § 5
- 자율성의 무게 + Hyun *모든 것이 optimization* + optimization horizon = 장기전 → [「대학원노트」 Ch.14 — 자율성의 무게](../grad-notes/guide.html#chapter-14) § 1
- 꾸준함 vs 폭발적 성장 + 옆 사람 속도 비교 함정 → [「대학원노트」 Ch.15 — 비교의 함정](../grad-notes/guide.html#chapter-15) § 3
- 견고한 기초 — *리젝 이유가 없는 논문* frame → [「연구노트」 Ch.16 — 마음가짐](../research-notes/guide.html#chapter-16) § 1

### 20.8.1 논문 쓰기

Abstract → Introduction → Related Work → Method → Experiments → Conclusion 구조와 Introduction의 문제 정의·기존 한계·접근·contribution 구성은 [「연구노트」 Part 2 — 쓰기](../research-notes/guide.html#chapter-16)와 [「연구노트」 Ch.23 — Introduction](../research-notes/guide.html#chapter-23)에서 다룬다.

### 20.8.2 실험 설계와 Ablation

SLAM·CV 실험에서는 ablation, 변인 통제, 반복 실험, 같은 데이터·split·하드웨어에서의 비교가 필요하다. 다른 논문의 baseline 숫자를 그대로 가져오면 조건 차이 때문에 공정한 비교가 되지 않을 수 있다.

### 20.8.3 학회 발표

5분·20분 발표의 slide budget, 포스터 30초 elevator pitch와 2분 walk-through — 본격 가이드는 [「연구노트」 Part 4 — 발표](../research-notes/guide.html#chapter-33).

### 20.8.4 논문 리뷰 — Peer Review

리뷰어 관점 체크리스트(novelty·soundness·experiments·clarity·reproducibility), 건설적 피드백, rebuttal — 본격 가이드는 [「연구노트」 Ch.10 — Reviewer로 읽기](../research-notes/guide.html#chapter-10) (reviewer로 읽기) + [「연구노트」 Ch.32 — Revision/Rebuttal](../research-notes/guide.html#chapter-32) (rebuttal 작성).

### 20.8.5 도구

- LaTeX: Overleaf 또는 로컬 (texlive + vscode)
- 참고 문헌: PDF 리더에서 원문을 표시하고 Zotero + Better BibTeX로 서지 정보를 관리한다. AI 요약·related work 비교·BibTeX 초안은 원문과 DOI 메타데이터를 대조한 뒤 사용한다.
- 파이프라인 그림: TikZ (정밀), draw.io (빠른 제작), Inkscape (SVG)
- 테이블: booktabs (\toprule, \midrule, \bottomrule)
- 알고리즘: algorithm2e
- 수식: notation table을 따로 만들어 논문 전체에서 통일

LaTeX 표기와 notation table, 수식 설명을 통일하는 방법은 [「연구노트」 Ch.30 — 수식·정리·증명 쓰기](../research-notes/guide.html#chapter-30)에서 다룬다.

> 추천 자료:
> - [How to Write a Great Research Paper (Simon Peyton Jones, Microsoft Research)](https://www.microsoft.com/en-us/research/academic-program/write-great-research-paper/) — 논문 쓰기의 고전 강연
> - [How to Read a Paper (S. Keshav)](http://ccr.sigcomm.org/online/files/p83-keshavA.pdf) — 3-pass reading method
> - [Tips for Writing Technical Papers (Jennifer Widom, Stanford)](https://cs.stanford.edu/people/widom/paper-writing.html) — 간결한 실전 조언

---

# Ch.21 — 부록

## A. 용어 사전

### A.1 약어

| 약어 | 풀이 | 설명 |
| --- | --- | --- |
| SLAM | Simultaneous Localization and Mapping | 동시적 위치추정 및 지도작성 |
| VO | Visual Odometry | 시각 주행거리계 |
| VIO | Visual-Inertial Odometry | 시각-관성 주행거리계 |
| LIO | LiDAR-Inertial Odometry | 라이다-관성 주행거리계 |
| IMU | Inertial Measurement Unit | 관성 측정 장치 |
| DoF | Degrees of Freedom | 자유도 |
| SE(3) | Special Euclidean Group (3D) | 3D 강체 변환 그룹 |
| SO(3) | Special Orthogonal Group (3D) | 3D 회전 그룹 |
| FoV | Field of View | 시야각 |
| ToF | Time of Flight | 비행 시간 (거리 측정 방식) |
| CNN | Convolutional Neural Network | 합성곱 신경망 |
| ViT | Vision Transformer | 비전 트랜스포머 |
| VFM | Vision Foundation Model | 비전 기반 모델 |
| VLA | Vision-Language-Action | 시각-언어-행동 모델 |
| VLM | Vision-Language Model | 시각-언어 모델 |
| LLM | Large Language Model | 대규모 언어 모델 |
| mAP | mean Average Precision | 평균 정밀도 |
| ICP | Iterative Closest Point | 반복적 최근접점 |
| NDT | Normal Distributions Transform | 정규분포 변환 |
| NeRF | Neural Radiance Fields | 신경 방사장 |
| 3DGS | 3D Gaussian Splatting | 3D 가우시안 스플래팅 |
| BEV | Bird's Eye View | 조감도 |
| TSDF | Truncated Signed Distance Function | 절단 부호 거리 함수 |
| BA | Bundle Adjustment | 번들 조정 |
| PGO | Pose Graph Optimization | 포즈 그래프 최적화 |
| DDS | Data Distribution Service | ROS2의 통신 미들웨어 |
| ONNX | Open Neural Network Exchange | 모델 변환 포맷 |
| TRT | TensorRT | NVIDIA 추론 최적화 엔진 |
| ATE | Absolute Trajectory Error | 절대 궤적 오차 |
| RPE | Relative Pose Error | 상대 포즈 오차 |

### A.2 용어

**Keyframe**: 중요 정보를 포함하도록 선택한 프레임. 모든 프레임을 처리하면 너무 느리므로 의미 있는 변화가 있는 프레임만 골라 쓴다.

**Loop Closure**: 이전에 방문한 장소를 다시 인식해 drift를 보정하는 과정. "아, 여기 아까 왔던 곳이네"라고 알아차려 누적 오차를 한꺼번에 교정한다.

**Drift**: 오차가 누적되는 현상. 보폭 1 m로 100 m를 걸으며 매 걸음 1 cm의 오차가 같은 방향으로 쌓이면 도착할 때 100 cm가 된다. 오차의 방향이 무상관이면 걸음 수의 제곱근에 비례해 10 cm 규모로 남는다.

**Reprojection Error**: 3D 점을 이미지에 재투영했을 때의 오차. "이 3D 점이 카메라 이미지의 어디에 보여야 하는가"라는 예측값과 실제값의 차이다.

**Feature Descriptor**: 특징점 주변을 설명하는 벡터. 두 이미지에서 같은 점을 찾을 때, 이 벡터를 비교한다.

**Homography**: 평면 간의 변환. 책상 위를 찍은 두 사진을 정합할 때 사용.

**Essential Matrix**: 캘리브레이션된 카메라 쌍의 기하 관계. 5DoF(회전 3 + 이동 방향 2).

**Fundamental Matrix**: 캘리브레이션되지 않은 카메라 쌍의 기하 관계. 7DoF.

**Epipole**: 한 카메라의 중심이 다른 카메라 이미지에 투영된 점.

**Zero-shot**: 학습 없이 새로운 태스크 수행. "고양이"를 학습 안 했는데 "고양이 찾아줘"가 되는 것.

**Few-shot**: 적은 예제로 새로운 태스크 학습. 예시 3~5개만 주면 학습.

**Fine-tuning**: 사전학습 모델을 특정 태스크에 맞게 재학습. 대형 모델을 내 데이터에 맞게 조정.

**Domain Adaptation**: 소스 도메인에서 타겟 도메인으로 적응. 시뮬레이션에서 학습 → 실제 환경 적용.

**Sim-to-Real**: 시뮬레이션에서 실제 환경으로 전이. Domain Adaptation의 대표적 사례.

**Gaussian Splatting**: 3D 장면을 수백만 개의 3D 가우시안으로 표현하는 방법. NeRF보다 빠르고 편집 가능.

**Factor Graph**: 변수 간의 제약 조건을 그래프로 표현한 SLAM 최적화의 핵심 자료구조다.

**Knowledge Distillation**: 큰 모델(teacher)의 지식을 작은 모델(student)에 전달하는 기법.

## B. 자주 묻는 질문 (FAQ)

**Q: Python과 C++ 중 어떤 것을 먼저 배워야 하나요?**

A: 먼저 읽고 실행할 연구실 코드의 언어부터 시작한다. SLAM, ROS 패키지, 실시간 제어 모듈에는 C++가 널리 쓰이므로 연구실 코드를 읽고 수정하려면 C++가 필요하다. Python은 딥러닝 스크립트와 데이터 전처리에 주로 쓴다. AI 코딩 에이전트가 작성을 도울 수는 있지만, 두 언어 모두 기존 코드를 읽고 동작을 검증할 수 있어야 한다.

**Q: GPU가 없으면 연구를 할 수 없나요?**

A: 간단한 실험은 CPU로도 가능하다. 다만 딥러닝 모델을 학습하려면 대개 GPU가 필요하다. Google Colab(무료)이나 연구실 서버를 활용할 수 있고, Colab 무료 버전으로도 YOLO fine-tuning 정도는 가능하다.

**Q: ROS1과 ROS2 중 어떤 것을 배워야 하나요?**

A: 새로 배운다면 ROS2를 권장한다. ROS1은 2025년에 공식 지원이 종료(EOL)되었다. 하지만 사용하려는 패키지가 ROS1만 지원하면 어쩔 수 없이 ROS1을 먼저 배울 수도 있다. 다만 ROS1을 알면 ROS2는 금방 익힌다.

**Q: 논문을 어디서 찾나요?**

A: [arXiv](https://arxiv.org/) (무료 프리프린트 서버)와 [Google Scholar](https://scholar.google.com/) (논문 검색)를 주로 활용한다. 코드도 필요하면 논문의 GitHub 링크나 [Hugging Face Papers](https://huggingface.co/papers/trending)를 보고(Papers With Code는 2025년 종료), 학회별로 보고 싶다면 [CVPR Open Access](https://openaccess.thecvf.com/)나 [IEEE Xplore](https://ieeexplore.ieee.org/)도 유용하다.

**Q: SLAM을 공부하려면 어디서 시작해야 하나요?**

A: Cyrill Stachniss의 [YouTube SLAM 강의](https://www.youtube.com/playlist?list=PLgnQpQtFTOGQrZ4O5QzbIHgl3b1JHimN_)로 시작해 ORB-SLAM3 코드를 분석해 보자. 그전에 본 문서의 9장(카메라 모델)과 14장(Visual Odometry)을 읽으면 강의를 이해하기가 한결 쉽다.

**Q: 연구 아이디어는 어떻게 찾나요?**

A: 최신 학회 논문의 Limitation 섹션을 읽어보자. 해결되지 않은 문제에서 아이디어를 얻을 수 있다. 또 다른 방법은 아직 덜 합쳐진 두 분야를 잇는 것이다. 다만 조합의 이름만으로 미개척을 판단할 수는 없다. "3D Gaussian Splatting + Semantic SLAM"은 이미 여러 연구가 나와 있고 이 책의 연구 방향 장도 의미 정보를 담은 Gaussian 지도를 다룬다. 선행 연구를 먼저 확인한 뒤 남은 자리를 찾아야 한다.

**Q: 어떤 GPU를 사야 하나요?**

A: 연구 대상 모델과 옵티마이저 상태, activation, 배치 크기가 VRAM에 적재되는지 실제 설정으로 먼저 실측한다. 이어 정밀도 지원 범위, 메모리 대역폭, 소비 전력, 프레임워크 호환성 및 실측 벤치마크를 종합 점검한다. 본 표는 일률적인 서열 매기기가 목적이 아니며, VRAM 등급별 적정 후보군을 합리적으로 압축하기 위한 참조 기준이다.

개인용 (데스크톱)

| VRAM | 카드 예시 | 검토할 역할 | 구매 전 확인 |
|------|-----------|-------------|--------------|
| 8GB | RTX 4060, RTX 5060 Ti 8GB | 작은 CNN 학습, 제한된 batch의 추론 | intended model의 peak memory; VFM fine-tuning은 설정에 따라 부족할 수 있음 |
| 12GB | RTX 3060 12GB, RTX 4070 | 중간 크기 inference·학습 실험 | 세대별 연산 차이와 중고 상태 |
| 16GB | RTX 5060 Ti 16GB, RTX 4070 Ti Super | 더 큰 batch, VFM inference, 중간 규모 학습 | model별 activation·optimizer memory |
| 24GB | RTX 3090, RTX 4090 | 24GB 안에 드는 학습, 3DGS·VLA 실험 | 전력·냉각·중고 보증; 3090과 4090의 runtime 차이 |
| 32GB | RTX 5090 | 24GB를 넘는 로컬 실험 | 전력·케이스·PSU와 software support |

서버/연구실용 (데이터센터)

| GPU memory | 카드 | 특징 | 공식 사양 |
|------------|------|------|-----------|
| 16/32GB | V100 SXM2 | 1세대 Tensor Core, TF32/BF16 미지원 | [V100 data center GPU](https://www.nvidia.com/en-us/data-center/v100/) |
| 24GB | A10 | PCIe inference·graphics 계열 | [A10 datasheet](https://www.nvidia.com/content/dam/en-zz/Solutions/Data-Center/a10/pdf/a10-datasheet.pdf) |
| 40/80GB | A100 | TF32·BF16, MIG, PCIe/SXM 변형 | [A100 specifications](https://www.nvidia.com/en-us/data-center/a100/) |
| 80GB | H100 SXM | Hopper, Transformer Engine, NVLink | [H100 specifications](https://www.nvidia.com/en-us/data-center/h100/) |
| 141GB | H200 SXM | 141GB HBM3e, 4.8TB/s memory bandwidth | [H200 specifications](https://www.nvidia.com/en-us/data-center/h200/) |
| 180GB | B200 | Blackwell, 180GB HBM3e; server configuration에 따라 제공 | [DGX B200 specifications](https://www.nvidia.com/en-us/data-center/dgx-b200/) |

사양을 읽을 때는 precision(FP32·TF32·BF16·FP16·FP8), CUDA core와 Tensor Core, dense와 structured sparsity, PCIe와 SXM을 구분한다. 제조사 표의 peak TFLOPS가 같아도 memory bandwidth, kernel, batch, data loading 때문에 실제 학습 시간은 달라진다. `torch.amp`의 효과도 model과 hardware에 따라 달라지므로, 같은 repository·batch·precision으로 짧은 benchmark를 돌려 비교한다.

**참고 사항**:
- RTX 5060 Ti처럼 8GB와 16GB 모델이 함께 나오는 카드에서는 사용할 모델과 batch가 요구하는 VRAM을 먼저 계산한다. VFM을 로컬에서 다룬다면 8GB는 빠듯할 수 있다.
- AMD GPU를 고려할 때는 사용할 프레임워크와 라이브러리가 ROCm을 지원하는지 확인한다. CUDA 전용 의존성이 있다면 이식 비용도 구매 조건에 포함한다.
- 연구실 서버에 A100/H100이 있다면 개인 GPU의 역할은 디버깅과 프로토타이핑에 가까워진다. 구매 전에 서버의 사용 가능 시간과 사양을 확인한다.
- 중고 RTX 3090은 24GB 선택지지만 가격·보증·냉각 상태가 매물마다 다르다. 정격 전력과 PSU·케이스 조건도 확인한다.
- Colab과 cloud GPU의 요금·할당 GPU·사용 제한은 수시로 바뀐다. 구매 전 실제 workload를 대여 GPU에서 측정하되, 현재 provider 페이지의 가격과 quota를 확인한다.

**Q: 논문은 하루에 몇 편 읽어야 하나요?**

A: 하루에 몇 편을 읽는지보다 읽는 목적과 깊이가 중요하다. 처음에는 20.4절의 3-패스 방법에 따라 일주일에 한 편을 자세히 읽는 편이 도움이 된다. 경험이 쌓이면 Abstract만으로도 논문의 유형과 관련성을 빠르게 가늠할 수 있다. 랩미팅 발표를 위한 읽기와 자신의 연구를 위한 읽기도 깊이가 다르며, 후자는 코드 분석까지 이어질 수 있다.

**Q: 코딩을 잘 못하는데 연구를 할 수 있나요?**

A: 코딩 에이전트(Claude, Copilot 등)는 "KITTI 데이터셋 로더를 만들어줘"나 "이 학습 루프에 wandb 로깅을 추가해줘" 같은 요청으로 초안을 빠르게 만들 수 있다. 덕분에 직접 타이핑하는 시간은 줄었지만, 결과를 검토하는 일은 남는다.

생성된 코드를 판단하려면 도메인 지식이 필요하다. 에이전트가 `num_workers=0`일 때 DataLoader가 느린 이유, loss가 NaN이 되는 원인, SLAM 코드에서 좌표계가 뒤집힌 지점을 충분한 실행 정보 없이 찾기는 어렵다(14장 참고). 코드를 실행하고 기존 구현과 비교한 뒤 받아들여야 한다.

ORB-SLAM3, Ultralytics, HuggingFace Transformers 같은 오픈소스를 읽고 설계 이유를 추적하면 코드 검토 능력을 익히는 데 도움이 된다.

**Q: 학회 발표는 어떻게 준비하나요?**

A: 학회 발표는 크게 구두 발표(oral)와 포스터 발표(poster)로 나뉜다. 발표 시간·포스터 크기·언어는 학회마다 다르므로 공식 발표자 안내가 우선한다.

- 포스터: A0는 자주 보이는 크기지만 학회 지정 규격을 확인한다. Figure를 크게 두고 텍스트를 줄여, 지나가는 사람이 짧은 시간에 주제와 결과를 찾을 수 있게 한다. 발표 전에는 연구실 동료들 앞에서 연습한다.
- 구두 발표: 15~20분은 흔한 예일 뿐이며 세션별 제한 시간이 우선한다. 시간에 맞춰 슬라이드 수를 정하고 한 장의 메시지를 하나로 좁힌다. 데모 영상과 질문용 보충 슬라이드는 필요할 때 준비한다.
- 공통: 발표 언어를 확인한 뒤 스크립트로 연습하되 문장 암기보다 내용 전달과 시간 준수에 집중한다.

**Q: 영어 논문 읽기가 너무 힘든데요?**

A: 반복해서 읽고 분야 배경지식이 쌓이면 부담이 줄어든다.

- 구조를 먼저 파악하라: 많은 실험 논문은 Introduction → Related Work → Method → Experiments → Conclusion 순서를 쓰지만, 기여는 문제 정의·데이터·평가나 분석에도 놓일 수 있다. 제목과 헤딩으로 실제 구조부터 확인한다.
- 분야별 어휘를 먼저 익혀라: "ablation study", "state-of-the-art", "we empirically show" 같은 표현은 반복된다. 익숙해지는 데 필요한 논문 수는 배경지식과 분야에 따라 다르다.
- 번역 도구를 부끄러워하지 마라: DeepL, Google Translate로 모르는 문장을 번역하는 건 전혀 부끄러운 일이 아니다. 다만, 번역에만 의존하면 영어 실력이 안 는다. "원문 → 번역 확인 → 다시 원문" 순서로 읽자.
- PDF 리더의 형광펜을 활용하라: 중요한 문장을 표시하면 다시 찾기 쉽다. Adobe Acrobat이나 Zotero 내장 뷰어처럼 자신에게 편한 도구를 쓰면 된다.

## C. 트러블슈팅 가이드

**자주 쓰는 apt 명령어**

```bash
sudo apt update                  # 패키지 목록 갱신
sudo apt upgrade                 # 설치된 패키지 업그레이드
sudo apt install <package>       # 패키지 설치
sudo apt remove <package>        # 패키지 제거 (설정 파일 유지)
sudo apt purge <package>         # 패키지 + 설정 파일 완전 제거
sudo apt autoremove              # 사용하지 않는 의존성 제거
apt list --installed             # 설치된 패키지 목록
apt search <keyword>             # 패키지 검색
sudo apt --fix-broken install    # 의존성 깨졌을 때 복구
```

(참고: [정진용 블로그](https://jinyongjeong.github.io/2016/06/07/Ubuntu_apt_get_commend/))

**SSH 키 설정 (비밀번호 없이 서버 접속)**

```bash
# 키 생성 (Enter 연타로 기본값 사용)
ssh-keygen -t ed25519

# 공개키를 서버에 복사
ssh-copy-id user@server_ip

# 이후 비밀번호 없이 접속 가능
ssh user@server_ip
```

GitHub에도 같은 공개키(`~/.ssh/id_ed25519.pub`)를 등록하면 SSH remote(`git@github.com:...`)에서 `git push`에 비밀번호가 필요 없다. HTTPS로 clone한 저장소는 토큰이나 credential helper를 쓰므로 이 키가 적용되지 않는다. (참고: [정진용 블로그](https://jinyongjeong.github.io/2016/06/02/SSH_keygen_setting/))

**CPU 성능 모드 설정 (실험 시)**

SLAM이나 딥러닝 실험에서 CPU throttling 때문에 성능이 들쭉날쭉한 경우가 있다.

```bash
# 현재 CPU governor 확인
cat /sys/devices/system/cpu/cpu0/cpufreq/scaling_governor

# performance 모드로 변경 (모든 코어)
echo performance | sudo tee /sys/devices/system/cpu/cpu*/cpufreq/scaling_governor

# 영구 설정 (재부팅 후에도 유지)
sudo apt install cpufrequtils
echo 'GOVERNOR="performance"' | sudo tee /etc/default/cpufrequtils
sudo systemctl restart cpufrequtils
```

노트북에서는 배터리 소모가 커지니 전원 연결 상태에서만 사용할 것. (참고: [정진용 블로그](https://jinyongjeong.github.io/2020/02/04/Ubuntu_cpu_freq_change/))

### C.1 CUDA / PyTorch 관련

**문제**: `CUDA out of memory`

**해결**:

```python
import torch

# 1. 배치 사이즈 줄이기 (가장 먼저 시도)
batch_size = 16  # → 8 또는 4

# 2. 메모리 정리
torch.cuda.empty_cache()

# 3. Gradient accumulation + AMP
accumulation_steps = 4
num_batches = len(dataloader)
scaler = torch.amp.GradScaler("cuda")
optimizer.zero_grad()

for i, (inputs, labels) in enumerate(dataloader):
    group_start = (i // accumulation_steps) * accumulation_steps
    group_size = min(accumulation_steps, num_batches - group_start)

with torch.autocast(device_type="cuda", dtype=torch.float16):
        output = model(inputs)
        loss = criterion(output, labels) / group_size

scaler.scale(loss).backward()

is_update_step = ((i + 1) % accumulation_steps == 0) or ((i + 1) == num_batches)
    if is_update_step:
        scaler.step(optimizer)
        scaler.update()
        optimizer.zero_grad()
```

AMP의 메모리 절감과 속도 향상 효과는 모델·GPU·연산자에 따라 다르다. 같은 batch와 모델로 peak memory와 학습 안정성을 측정한다.

**문제**: `CUDA version mismatch` (주로 CUDA 확장을 직접 빌드할 때)

**해결**:

```bash
# 설치된 CUDA 버전 확인
nvcc --version

# PyTorch에서 인식하는 CUDA 버전 확인
python -c "import torch; print(torch.version.cuda)"

# 두 값은 달라도 정상이다. wheel이 자체 CUDA 런타임을 포함하므로
# 드라이버가 그 런타임을 지원하면 그대로 쓴다. 단 CUDA 확장을 직접 빌드할 때는
# nvcc와 torch.version.cuda의 메이저 버전을 맞춰야 하며, 그때만 재설치한다.
pip install torch --index-url https://download.pytorch.org/whl/cu121
```

**문제**: `RuntimeError: CUDA error: device-side assert triggered`

**해결**: 이건 보통 라벨 인덱스가 범위를 벗어났을 때 발생한다. CPU에서 돌려보면 더 자세한 에러 메시지를 얻을 수 있다. 아래 명령은 GPU에서 CUDA 호출을 동기화하여 오류가 발생한 코드 위치를 찾는 별도의 방법이다.

```bash
CUDA_LAUNCH_BLOCKING=1 python train.py
```

### C.2 ROS 관련

**문제**: `Package not found`

**해결**:

```bash
# Workspace 소싱 확인
source ~/ros2_ws/install/setup.bash

# 패키지 설치 확인
ros2 pkg list | grep package_name

# .bashrc에 소싱 추가 (매번 수동으로 안 해도 됨)
echo "source ~/ros2_ws/install/setup.bash" >> ~/.bashrc
```

**문제**: `TF tree not connected`

**해결**:

```bash
# TF 트리 확인
ros2 run tf2_tools view_frames

# Static transform 추가 (예시)
ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 base_link camera_link
```

**문제**: `Topic not published` / 데이터가 안 들어옴

**해결**:

```bash
# 현재 활성 토픽 확인
ros2 topic list

# 특정 토픽 데이터 확인
ros2 topic echo /camera/image_raw --once

# QoS 설정 불일치 확인 (ROS2에서 흔한 문제)
ros2 topic info /camera/image_raw -v
```

### C.3 Docker 관련

**문제**: `Permission denied`

**해결**:

```bash
# 도커 그룹에 사용자 추가
sudo usermod -aG docker $USER
# 로그아웃 후 재로그인
```

**문제**: GUI 프로그램 실행 안 됨

**해결**:

```bash
# X11 forwarding
xhost +local:docker
docker run -it --env DISPLAY=$DISPLAY -v /tmp/.X11-unix:/tmp/.X11-unix ...
```

**문제**: Docker 안에서 GPU가 안 잡힘

**해결**:

```bash
# nvidia-container-toolkit 설치 — NVIDIA apt 저장소와 키를 먼저 등록해야 패키지가 보인다
curl -fsSL https://nvidia.github.io/libnvidia-container/gpgkey | \
  sudo gpg --dearmor -o /usr/share/keyrings/nvidia-container-toolkit-keyring.gpg
curl -s -L https://nvidia.github.io/libnvidia-container/stable/deb/nvidia-container-toolkit.list | \
  sed 's#deb https://#deb [signed-by=/usr/share/keyrings/nvidia-container-toolkit-keyring.gpg] https://#g' | \
  sudo tee /etc/apt/sources.list.d/nvidia-container-toolkit.list
sudo apt-get update && sudo apt-get install -y nvidia-container-toolkit
sudo nvidia-ctk runtime configure --runtime=docker   # Docker 런타임에 훅 등록
sudo systemctl restart docker

# GPU 옵션 추가해서 실행
docker run --gpus all -it nvidia/cuda:12.1.0-base-ubuntu22.04 nvidia-smi
```

### C.4 OpenCV 관련

**문제**: `cv2.imshow() not working`

**해결**:

```bash
# OpenCV headless 버전 제거 후 재설치
pip uninstall opencv-python-headless
pip install opencv-python
```

**문제**: OpenCV와 ROS의 cv_bridge 충돌

**해결**:

```bash
# ROS의 cv_bridge가 시스템 OpenCV를 참조하는 경우
# conda/venv 환경의 OpenCV와 충돌할 수 있다
# 해결: ROS workspace 빌드 시 Python 경로 명시

colcon build --cmake-args -DPython3_EXECUTABLE=/usr/bin/python3
```

### C.5 빌드/컴파일 관련

**문제**: ORB-SLAM3 빌드 에러 (OpenCV 버전 충돌)

**해결**:

```bash
# OpenCV 4.x에서는 일부 API가 변경됨
# CMakeLists.txt에서 OpenCV 버전 확인
find_package(OpenCV 4 REQUIRED)

# Pangolin 빌드 에러 시
sudo apt-get install libglew-dev libpython2.7-dev
```

**문제**: Eigen 버전 관련 에러

**해결**:

```bash
# 시스템 Eigen 버전 확인
pkg-config --modversion eigen3

# 저장소에서 제공하는 Eigen 개발 패키지 설치
sudo apt-get install libeigen3-dev
```

## D. 체크리스트: 연구 시작 전 확인 사항

### D.1 환경 설정

- [ ] Ubuntu 설치 완료 (22.04 LTS 권장)
- [ ] NVIDIA 드라이버 설치 (`nvidia-smi`로 확인)
- [ ] CUDA toolkit 설치 (`nvcc --version`으로 확인)
- [ ] cuDNN 설치 (시스템 설치는 `cudnn_version.h`로 확인. `torch.backends.cudnn.version()`은 PyTorch에 번들된 cuDNN을 보여준다)
- [ ] Conda 또는 venv 환경 설정
- [ ] PyTorch GPU 동작 확인 (`torch.cuda.is_available()`)
- [ ] ROS2 설치 (필요 시. Ubuntu 22.04이면 Humble, 24.04이면 Jazzy)
- [ ] Git 설정 (`git config --global user.name/email`)
- [ ] Docker 설치 (선택, 재현성을 위해 권장)
- [ ] VS Code + 필수 확장 설치 (Python, Remote-SSH, Jupyter)

### D.2 기초 지식

- [ ] Python 기본 문법 (클래스, 데코레이터, 리스트 컴프리헨션)
- [ ] NumPy 배열 연산 (broadcasting, indexing, reshape)
- [ ] OpenCV 이미지 처리 (읽기, 변환, 필터, 특징점)
- [ ] 선형대수 기초 (행렬 곱셈, 고유값 분해, SVD)
- [ ] 확률/통계 기초 (베이즈 정리, 가우시안 분포, MLE/MAP)

### D.3 연구 도구

논문 읽기 도구(노트 양식·인용 관리)는 [「연구노트」 Ch.15](../research-notes/guide.html#chapter-15)에서, 쓰기 도구는 [「연구노트」 Part 2 — 쓰기](../research-notes/guide.html#chapter-16)에서, 학회 준비 도구는 [「연구노트」 Ch.34 — 학회 2~3주 전 체크리스트](../research-notes/guide.html#chapter-34)에서 확인할 수 있다. Spatial AI 분야의 도구와 학습 순서는 §20.4와 §20.7을 참고한다.

### D.4 데이터셋 준비

- [ ] 연구 관련 데이터셋 다운로드
- [ ] 데이터 포맷 이해 (이미지 크기, depth 단위, 좌표계)
- [ ] DataLoader 구현 (PyTorch Dataset/DataLoader)
- [ ] 데이터 시각화 코드 작성 (디버깅용)

## E. 첫 주 생존 가이드

첫 주에는 계정과 실행 환경을 준비하고, 연구 대상의 코드·데이터·문서를 찾는 최소 작업 목록이 필요하다. 아래 Day 1~7 배치는 예시이며 계정 발급, 장비 일정, 연구실 onboarding 방식에 맞춰 순서를 바꾼다.

### Day 1~2: 환경 구축

```
[ ] 연구실 서버 계정 받기 (관리자에게 요청)
[ ] SSH로 서버 접속 확인
[ ] VS Code Remote-SSH 설정
[ ] 서버에 conda 환경 만들기
[ ] PyTorch + CUDA 동작 확인
[ ] 연구실 GitHub organization에 가입
[ ] Slack/Discord 채널 가입
```

> 팁: 서버 환경 설정에서 막히면 시도한 명령과 예상·실제 결과를 함께 정리해 선배에게 묻는다. 확인한 내용을 보여 주면 문제를 훨씬 빨리 좁힐 수 있다.

### Day 3~4: 기존 코드 파악

```
[ ] 연구실의 기존 코드/프로젝트 리포지토리 클론
[ ] README 읽기 (있다면)
[ ] 기존 코드 빌드/실행 해보기
[ ] 데이터셋 다운로드 및 경로 설정
[ ] 간단한 데모 돌려보기
```

> 팁: 처음 실행한 코드가 바로 동작하지 않는 경우가 많다. 환경, 경로, 버전 차이를 확인하고 에러 메시지로 공식 문서와 issue를 먼저 찾아본다.

### Day 5: 논문 읽기 시작

첫 논문을 추천받고 연구실 구성원과 대화하는 방법은 [「대학원노트」 Ch.4 — 관계는 양방향](../grad-notes/guide.html#chapter-4)에서, 첫 주의 연구 방향 설정은 [「대학원노트」 Ch.7 — 내 연구를 갖기](../grad-notes/guide.html#chapter-7)에서 다룬다.

> 팁: 처음 읽는 논문은 이해가 안 되는 게 정상이다. "이 논문이 무슨 문제를 풀려고 하는가?"만 파악해도 첫 주로서는 충분하다.

### Day 6~7: 연구 방향 파악

Ch.18의 연구 방향을 읽고 연구실의 최근 논문과 프로젝트가 어느 주제에 해당하는지 정리한다. 선배들의 연구 주제와 겹치는 지점도 함께 표시한다.

### 첫 주에 하지 않아도 되는 것들

- 논문을 완벽하게 이해하기 — 시간이 해결해준다
- 최신 연구 트렌드를 전부 파악하기 — 점진적으로
- 코드를 처음부터 짜기 — 기존 코드를 수정하는 것부터
- GPU 서버를 완벽하게 세팅하기 — 연구실이 검증한 환경 파일·container·설치 절차에서 시작한다
- 연구 아이디어를 완성된 형태로 내놓기 — 먼저 연구실의 문제와 도구를 배워도 늦지 않다

### 생존을 위한 마인드셋

연구 초기에 필요한 태도와 작업 습관은 research-notes와 grad-notes에서 주제별로 다룬다.

관련 장은 다음과 같다.

- *모르는 건 당연하다* → [「대학원노트」 Ch.14 — 자율성의 무게](../grad-notes/guide.html#chapter-14) § 2 (대학원의 가치 재정의)
- *"안 돼요"는 보고가 아니다* + 보고 형식(예측·시도·결과) → [「대학원노트」 Ch.10 — 한 메일 한 질문](../grad-notes/guide.html#chapter-10) § 3 (형식의 사소한 표준)
- *기록하라 — 과거의 내가 미래의 나를 도와준다* → [「대학원노트」 Ch.8 — 시간 쓰는 법](../grad-notes/guide.html#chapter-8) § 5 (퇴근 전 포스트잇)
- *작게 시작하라 — 작은 코드 조각부터* → [「대학원노트」 Ch.11 — 도구의 함정](../grad-notes/guide.html#chapter-11) § 1 (셋업 시간은 연구 시간이 아니다)
- *비교하지 마라 — 3개월 후의 나* → [「대학원노트」 Ch.15 — 비교의 함정](../grad-notes/guide.html#chapter-15) § 3 (단거리 vs 장거리)

---

# Ch.22 — 마무리: 어디서부터 시작할까?

앞선 21개 장에서는 센서와 좌표계부터 로봇의 운동, 공간 인식, 딥러닝 기반 방법까지 Spatial AI의 주요 요소를 살펴봤다. 마지막 장에서는 배경과 프로젝트에 따라 어디서 시작할지 정리한다.

## 22.1 지금까지의 지도

가이드는 네 부분으로 구성된다.

- **기초 지식** (Ch.1~3): Spatial AI의 범위, 센서가 환경을 측정하는 방식, 그리고 그 데이터를 다루는 수학. 회전·변환·최적화·확률은 이후 장에서도 반복해서 사용한다.
- **로봇** (Ch.4~8): 관절과 링크로 이루어진 물리 시스템. 기구학은 자세를 구하고, 동역학은 힘을 계산하며, 제어와 모션 플래닝은 원하는 상태와 경로를 만든다. 로봇 학습은 이 과정의 일부를 데이터로 익힌다.
- **인식과 공간 이해** (Ch.9~14): 이미지 처리에서 3D 공간 인식까지. 고전 CV, 딥러닝, foundation model, VLA를 거쳐 SLAM이 시간에 따른 위치와 지도를 연결한다.
- **연구 실전** (Ch.15~21): 코드를 실행하는 데 필요한 프레임워크와 개발 도구, 데이터셋, 연구 자료.

실제 프로젝트는 이 구분을 가로지른다. 로봇을 운용하다 보면 Ch.3의 수식, Ch.14의 SLAM, Ch.16의 Docker를 한꺼번에 참고하게 된다.

## 22.2 프로필별 시작점

시작할 장은 독자의 배경에 따라 달라진다.

로보틱스를 처음 접하는 학부 3~4학년이라면 Ch.1 → Ch.3 → Ch.9 → Ch.14 순서가 알맞다. Spatial AI의 범위를 파악하고 수학적 표현을 익힌 뒤 이미지와 SLAM을 통해 각 요소가 어떻게 연결되는지 볼 수 있다. Ch.16의 실습 환경도 함께 준비하면 예제를 바로 실행할 수 있다.

딥러닝 배경의 석사 신입생은 Ch.2 → Ch.3 → Ch.10 → Ch.11부터 읽을 수 있다. 딥러닝 배경을 바탕으로 센서와 수학을 먼저 보완하고, foundation model이 로보틱스에 쓰이는 방식을 확인하는 순서다. 이후 Ch.13과 Ch.14에서 3D 비전과 SLAM으로 범위를 넓힌다.

고전 로보틱스에 익숙하지만 딥러닝 경험이 적다면 Ch.8 → Ch.10 → Ch.11 → Ch.12를 먼저 읽는다. 이미 아는 Ch.4~7은 필요한 부분만 확인하고, 로봇 학습에서 VFM과 VLA로 이어지는 방법을 집중해서 살펴본다.

구체적인 프로젝트가 있다면 Ch.17의 데이터셋과 벤치마크부터 확인한다. 비슷한 과제의 논문을 한두 편 고른 뒤 그 논문에 필요한 장만 거슬러 올라가면 된다.

1개월·3개월·6개월 단위의 학습 계획은 Ch.20.7에 정리되어 있다.

논문을 읽고 쓰고 발표하는 책상 위의 작업은 [「연구노트」](../research-notes/guide.html)에서 다룬다.

## 22.3 하지 말아야 할 것

신입 연구자는 논문 수를 늘리는 데 몰두하거나, 실험 환경을 완벽하게 갖춘 뒤 시작하려 하거나, 모든 코드를 처음부터 직접 작성하는 데 시간을 쓰기 쉽다. 반대로 AI가 만든 결과를 검증 없이 받아들이는 것도 문제다. 이런 함정은 [「연구노트」 Ch.6 — 왜 읽는가](../research-notes/guide.html#chapter-6)와 [「대학원노트」 Ch.11 — 도구의 함정](../grad-notes/guide.html#chapter-11)에서 자세히 다룬다.

SLAM과 CV에는 ORB-SLAM3, COLMAP, Gaussian Splatting처럼 널리 쓰이는 공개 구현이 있다. 교육 목적이나 새로운 기여가 분명한 경우가 아니라면, 기존 구현을 실행하고 한계를 분석하는 편이 연구 문제에 더 빨리 접근하는 방법일 수 있다.

## 22.4 장기전의 감각

박사과정의 첫해, 첫 여름, 5년이라는 시간 단위는 [「대학원노트」 Ch.7 — 내 연구를 갖기](../grad-notes/guide.html#chapter-7)와 [「대학원노트」 Ch.1 — 박사를 결정한다는 일](../grad-notes/guide.html#chapter-1)에서 다룬다.

로보틱스 실험은 하드웨어 준비, 안전 절차, 데이터 수집과 반복 실행 때문에 한 번의 주기가 길어질 수 있다. 준비 기간과 주기 길이는 장비·환경·연구실에 따라 크게 다르므로 하루의 진척보다 실험 주기 전체를 기준으로 계획하는 편이 현실적이다.

## 22.5 다음 단계

가이드를 처음부터 다시 읽을 필요는 없다. 프로젝트를 진행하다 막히는 지점에서 관련 장으로 돌아와 수식, 구현, 데이터셋을 확인하면 된다.

논문 읽기와 쓰기는 [「연구노트」](../research-notes/guide.html)에서, 박사과정 운영은 [「대학원노트」](../grad-notes/guide.html)에서 이어진다.

초안 작성 일자: 2025.12.28 · 개정 일자: 2026.05.01

# Ch.1 — Introduction: What Is Spatial AI?

Begin with a map of the Spatial AI field. It provides the context needed to see why the techniques in later chapters matter and how they connect.

## 1.1 Defining Spatial AI

**Spatial AI** is the umbrella term for AI techniques that let machines understand 3D space and act within it. Beyond classifying images or recognizing objects, it must answer questions like:

- "Where am I right now?" (Localization)
- "What does the surrounding environment look like?" (Mapping)
- "What is that object, and where is it?" (Object Detection & Localization)
- "How do I get to the destination?" (Navigation & Planning)

General AI/deep learning answers "is there a cat in this image?", whereas Spatial AI must answer "how many meters away is that cat, which direction is it moving, and how do I move to avoid it?". That is, spatial context is the crux.

Spatial AI is the fusion of these techniques:
- **Computer Vision**: extracting information from camera images
- **3D Vision**: depth perception, point cloud processing
- **SLAM**: simultaneous localization and mapping
- **Deep Learning**: learning-based recognition and prediction
- **Sensor Fusion**: integrating information from multiple sensors

All of these techniques face a common obstacle: uncertainty. Thrun, Burgard, Fox (2005) *Probabilistic Robotics*, §1.1 identifies five sources. First, the environment itself is inherently unpredictable. Second, every measurement carries resolution limits and noise. Third, motor torque variation and wheel slip mean the actual motion differs from the commanded motion — robot actuation is never exact. Fourth, the moment you abstract an environment or a robot into equations, the model is already an approximation. Fifth, under real-time constraints, approximate solutions replace optimal ones. These five sources are the shared motivation for every algorithm this guide covers, from Bayes filters to SLAM; the detailed treatment appears in Ch.3 §3.9–3.11 and Ch.14 §14.16.

> **Further reading**
> - [Andrew Davison — From SLAM to Spatial AI (MIT Robotics)](https://www.youtube.com/watch?v=BRRtlR0C_CY) — Prof. Andrew Davison's talk laying out the vision for Spatial AI. Worth watching to orient yourself in this field.
> - [FutureMapping paper (arXiv:1803.11288)](https://arxiv.org/abs/1803.11288) — A 2018 position paper discussing how SLAM is evolving into Spatial AI perception that unifies geometry and semantics, and examining the computational structure of those algorithms together with processor and sensor co-design.
> - [Cyrill Stachniss — SLAM Course (2013)](https://www.youtube.com/playlist?list=PLgnQpQtFTOGQrZ4O5QzbIHgl3b1JHimN_) — Prof. Cyrill Stachniss's SLAM lectures from his Freiburg years. Graduate-level material covering EKF-SLAM, FastSLAM, and graph SLAM. A well-organized treatment of the foundational concepts behind Spatial AI.

## 1.2 Why It Matters

The techniques to study in depth depend on the application domain. Autonomous driving places more emphasis on LiDAR and sensor fusion, whereas AR/VR relies heavily on visual-inertial systems. A view of the broader application landscape helps readers choose an appropriate learning path.

Spatial AI is the core technology in the following fields:

| Field | Example applications |
| --- | --- |
| **Autonomous driving** | Vehicle localization, obstacle detection, path planning |
| **Service robots** | Indoor navigation, object manipulation, human collaboration |
| **Drones** | Autonomous flight, 3D map generation, inspection/delivery |
| **AR/VR** | Spatial tracking, virtual object placement, hand tracking |
| **Industrial automation** | Logistics robots, quality inspection, assembly automation |

## 1.3 Why Robotics Is Hard

"Won't advances in AI solve every problem in robotics?" The answer is no: AI improves some parts of a robotic system, while many of the field's difficulties arise elsewhere.

Many of the difficulties in robotics arise at the interface with the physical world:

- A code bug that causes a collision can damage equipment or injure a person, so the outcome cannot be rolled back like a software deployment.
- Each experiment may require editing, uploading, resetting the environment, establishing safety, running, and physical inspection. One cycle can take minutes or tens of minutes.
- Sensor data contains backlight, motion blur, drift, and dropped frames. Performance on clean data alone does not predict behavior in the field.
- Functions such as obstacle avoidance must satisfy response-time requirements as well as accuracy requirements.
- A rare edge case can still lead to a collision or safety incident.
- A simulator approximates friction, inertia, and noise, and accumulated modeling error can change the behavior of a physical robot.

| General software | Robotics |
|---|---|
| Bug → log → fix → redeploy | Bug → crash → damage → repair → retry |
| Iteration in seconds | Iteration in minutes to hours |
| Structured inputs | Sensor data riddled with noise |
| 99% accuracy is excellent | 99.9999% may not be enough |
| Response lag → inconvenience | Response lag → accident |
| Same input → same output | Same code, different results depending on environment |

Advances in AI improve areas such as recognition accuracy and natural-language command understanding. The problems in the right column of the table—iteration speed, sensor noise, real-time constraints, and risk of damage—arise from interaction with the physical world. Increasing model scale alone does not resolve them, and training a single AI model does not produce a working robotic system.

### What Roboticists Do in the AI Era

In an era where AI writes code, summarizes papers, and proposes experiments, where does a roboticist's value lie?

- **Problem definition**: AI can help solve a stated problem, but it cannot determine which problem deserves attention. Choosing a sensor suite for an environment, deciding what accuracy an application requires, and accepting the right trade-offs all demand domain knowledge. (*The general framework for problem definition is covered in [Research Notes Ch.1](../../research-notes/guide.html#chapter-1) and [Research Notes Ch.2](../../research-notes/guide.html#chapter-2) (Korean only).*)
- **System integration**: Turning perception modules, control modules, communication stacks, and hardware into a single working system. AI can write code for each module, but designing the interfaces, timing, and exception handling between modules is the engineer's job.
- **Interface with the physical world**: A loose cable, dust on a sensor lens, or an overheating motor can be difficult to diagnose through a remote connection alone. Someone must inspect the robot and its hardware directly.
- **Judging reliability**: Even when AI reports "99% accuracy", the engineer has to check what that number counted and against what denominator, and what kind of failure the remaining 1% consists of. The same 99% carries different weight for a task whose failures injure people than for one whose failures end in a retry. The criterion is the cost of a failure and how often the system is exposed to it—not the name of the industry.

Even when AI writes the code and summarizes the papers, it can't do these four things for you.

## 1.4 How to Use This Document

Use this document as a **reference**:

1. **On first read**: skim the table of contents and grasp the overall picture.
2. **When starting research**: read the relevant sections in depth and work through the further reading.
3. **When stuck**: consult the glossary and troubleshooting in the appendix.

**Recommended study order**:

```
Mathematical foundations → Sensors → Computer vision basics → SLAM → Deep learning → VFM/VLA → Lab direction
```

The mathematics explains the equations in a SLAM paper, while sensor characteristics explain why an algorithm fails under particular conditions. Studying these foundations in sequence reduces the gaps that otherwise appear in later chapters.

### Staged Learning Path

Below is a more concrete staged roadmap. Adjust the pace to your background, but don't skip any stage.

**Entry stage — learning the tools**

The goal of this stage is to become comfortable with the basic tools used in research. You should be able to read code, run it, and interpret the results.

**What to learn**:
1. **Reading C++ code** — the lab's core code (SLAM, ROS packages) is in C++. You don't need to write it from scratch at first, but you need to be able to read and modify its structure.
2. **Python basics** — used for deep learning training scripts, data preprocessing, and visualization. A language well-suited to AI-agent assistance.
3. **Linear algebra and probability/statistics refresh** — reorganizing what you learned as an undergraduate from a robotics perspective. Refer to Ch.3.
4. **ROS2 basics** — topics, services, actions, launch files. The robot framework used in the lab.
5. **Git usage** — up through branch, merge, rebase. Code management in the lab goes through Git.

**Practice exercises**:
- Build an image processing pipeline with OpenCV (read → filter → feature extraction → visualization)
- Write a simple ROS2 node (publisher/subscriber)
- Perform a camera calibration (using a chessboard pattern)

**Intermediate stage — practicing the core techniques**

At this stage, you should be able to run the core Spatial AI algorithms yourself and analyze the results. It is also the stage where you start reading papers.

**What to learn**:
1. **Deep learning basics (PyTorch)** — tensors, autodiff, training loops, model design. In research, PyTorch dominates over TensorFlow.
2. **Object Detection (YOLO family)** — bounding boxes, NMS, mAP, and other basic concepts of a recognition pipeline.
3. **Understanding Visual SLAM (ORB-SLAM3)** — a representative feature-based SLAM system. Run it and inspect the code.
4. **Point cloud processing (Open3D)** — how to handle 3D data. Filtering, registration, visualization.
5. **Understanding and using VFMs (DINOv2, SAM)** — understanding how foundation models reshape existing pipelines.

**Practice exercises**:
- Run benchmark experiments on the KITTI dataset
- Fine-tune YOLOv8 (on a custom dataset)
- Run ORB-SLAM3 and analyze the trajectory
- Evaluate SLAM accuracy on the TUM RGB-D dataset

**Advanced stage — first steps as a researcher**

From this stage onward, you experience the full research cycle: reading papers, generating ideas, running experiments, and writing.

**What to learn**:
1. **Reading and implementing papers** — at least 1-2 papers per week. For key papers, analyze down to the code.
2. **Experimenting with new ideas** — identify limitations of existing methods and experiment with improvement ideas.
3. **Benchmark evaluation** — master evaluation protocols for fair comparison.

**Practice exercises**:
- Analyze and reproduce code from recent papers
- Experiment with your own improvement ideas and compare quantitatively
- Attempt paper writing (targeting conference workshop submission)

> **Further reading**
> - [Missing Semester of Your CS Education (MIT)](https://missing.csail.mit.edu/) — An MIT course that teaches the practical tools research demands: Git, shell, debugging, and more.
> - [ROS2 official tutorials](https://docs.ros.org/en/humble/Tutorials.html) — Official learning materials based on ROS2 Humble.
> - [Andrej Karpathy — Neural Networks: Zero to Hero](https://www.youtube.com/playlist?list=PLAqhIrjkxbuWI23v9cThsA9GvCAUhRvKZ) — An outstanding series that teaches deep learning by implementing it from scratch.

## 1.5 Prerequisite Checklist

Before starting research, check the following items. Each entry also notes why it is necessary, so don't just check boxes — understand "why this is needed" before moving on.

**Required**:
- [ ] **Ability to read C++**
  - The lab's core code (SLAM, real-time control, ROS packages) is in C++. To understand and modify open-source projects like ORB-SLAM3 and LOAM, you must be comfortable with C++.
- [ ] **Basic Linux commands (cd, ls, cp, mv, grep)**
  - Lab servers are almost 100% Ubuntu. To SSH into a GPU server and run experiments, you need to be comfortable in the terminal.
- [ ] **Basic Git usage (clone, commit, push, pull)**
  - Research code management, pulling paper code, sharing code within the lab — all through Git. Cloning open-source code from GitHub and running it is daily work.

**Recommended**:
- [ ] **Python basics (functions, classes, modules)**
  - Used for deep learning training scripts, data preprocessing, and visualization. Since it is a language AI agents handle well, the need to write it directly is decreasing, but you must be able to read and understand it.
- [ ] **Basic NumPy usage**
  - Matrix operations, broadcasting, indexing. Used for sensor data processing and coordinate transformations.
- [ ] **Linear algebra basics (matrix operations, eigenvalues)**
  - 3D transformations, camera models, and optimization are all linear algebra. To understand "what this equation means", you need to know the geometric meaning of matrices. Continued in Ch.3.
- [ ] **Probability/statistics basics (normal distribution, Bayes' theorem)**
  - Sensor noise modeling, state estimation, and filtering are all probability-based. Expressing "how much can I trust this sensor's measurement?" mathematically requires this knowledge.
- [ ] **Calculus basics (partial derivatives, chain rule)**
  - The foundation of gradient descent, Jacobians, and optimization algorithms. Backpropagation in deep learning and bundle adjustment in SLAM both come down to differentiation.

> **Further reading**
> - [3Blue1Brown — Essence of Linear Algebra](https://www.youtube.com/playlist?list=PLZHQObOWTQDPD3MizzM2xVFitgF8hE_ab) — A video series that explains the geometric intuition of linear algebra well. Helpful before diving into the equations.
> - [3Blue1Brown — Essence of Calculus](https://www.youtube.com/playlist?list=PLZHQObOWTQDMsr9K-rj53DwVRMYO3t5Yr) — Intuitive understanding of calculus. Shows visually why the chain rule and partial derivatives matter.
> - [Python for Data Analysis (Wes McKinney)](https://wesmckinney.com/book/) — The standard text on NumPy, Pandas, and other data analysis tools. Free online version available.

> **Technical Timeline: the Spatial AI field as a whole**
> - **~2005**: Classical robotics — mathematical model-based, Kalman filter, EKF-SLAM. Hand-crafted features and geometric methods dominated.
> - **2007~2015**: The rise of real-time Visual SLAM — MonoSLAM (2007), PTAM (2007), ORB-SLAM (2015). Real-time localization and map generation became possible with a camera alone.
> - **2012~2018**: The deep learning revolution — starting with AlexNet (2012), recognition performance surged with ResNet (2015), Faster R-CNN (2015), and others. Learning-based methods started entering Spatial AI as well.
> - **2020~2023**: The foundation model era — CLIP (2021), SAM (2023), DINOv2 (2023), and other large-scale pretrained models appeared. Previously, every new environment required repeated data collection → labeling → training; the tasks that can be handled zero-shot have grown sharply.
> - **2024~**: End-to-end systems and embodied AI — VLA models, world models, 3D Gaussian Splatting + SLAM, and related work reduce the boundaries between perception, planning, and control. Physical systems compare end-to-end and modular designs according to safety, latency, and verification requirements.
> - **Recent direction**: Work continues on open-vocabulary SLAM, VFM-based scene understanding, and systems that combine classical geometry with learned components.

> **Interactive materials**: Interactive exercises for the key concepts in this document are available [here](https://alexjunholee.github.io/robotics-practice/).

---

# Ch.2 — Sensors

A robot needs sensors to perceive its environment. Understanding each sensor's characteristics enables proper sensor selection and algorithm design.

Diagnosing whether a SLAM tracking failure began with rolling shutter, LiDAR reflectance, or IMU bias requires the sensor's measurement and error model. That model determines the data received by the downstream perception and estimation algorithms.

## 2.1 Camera

A camera measures color and texture at high spatial resolution. Monocular, stereo, RGB-D, and event cameras differ in depth measurement, temporal resolution, and response to illumination, so the task conditions determine which type is appropriate.

### 2.1.1 Monocular Camera

The most basic visual sensor, capturing a 2D image with a single lens.

A monocular camera uses one lens to capture color and texture for tasks such as visual SLAM, object recognition, and semantic understanding. Because it does not measure depth directly, monocular depth estimation and SfM infer scene structure from other cues. Stereo and depth cameras address this ambiguity through different measurements.

**Pros**:
- Cheap and lightweight
- Rich color and texture information
- High resolution

**Cons**:
- Cannot directly measure depth from a single image
- Scale ambiguity: the real size of an object is unknown

**Key specifications**:
- Resolution: 720p, 1080p, 4K, etc.
- Frame rate: 30fps, 60fps, 120fps, etc.
- Field of View (FoV): narrow FoV vs. wide FoV (fisheye)
- Global shutter vs. rolling shutter

```
Typical camera sensors:
- Webcams: Logitech C920, C930e
- Industrial: FLIR (Point Grey), Basler, Allied Vision
- Embedded: Raspberry Pi Camera, OAK-D
```

> **Further reading**
> - [First Principles of Computer Vision — Camera and Imaging](https://www.youtube.com/playlist?list=PL2zRqk16wsdoCCLpou-dGo7QQNks1Ppzo) — Columbia University Prof. Shree Nayar's lectures on camera principles. Covers from pinhole models to lens distortion.
> - [OpenCV Camera Calibration Tutorial](https://docs.opencv.org/4.x/dc/dbb/tutorial_py_calibration.html) — Hands-on guide to performing camera calibration yourself.

### 2.1.2 Stereo Camera

Two cameras placed at a fixed interval (baseline) measure depth. The principle is similar to human binocular vision.

Obtaining depth outdoors requires stereo vision. A stereo camera is almost the only passive way (without actively emitting light) to measure depth. In outdoor settings such as autonomous driving and drones, structured light and ToF break down under sunlight, so the principles of stereo vision matter. It ties directly to epipolar geometry and therefore to the mathematical foundations.

**Depth computation principle**:

```
Depth (Z) = (focal_length × baseline) / disparity
```

- **Disparity**: the difference in x-coordinate of the same point in the left and right images
- **Baseline**: the distance between the two cameras

**Pros**:
- Passive sensor (no active illumination device required)
- Usable in outdoor environments
- Acquires RGB information and depth simultaneously

**Cons**:
- Matching fails on textureless surfaces (white walls, glass)
- High computational cost
- Measurement range limited by baseline

**Representative products**:
- Intel RealSense D435/D455: active IR pattern projection to assist matching
- ZED 2: wide baseline, long-range measurement
- OAK-D: built-in edge AI

> **Further reading**
> - [Cyrill Stachniss — Stereo Vision](https://www.youtube.com/watch?v=SyB7Wg1e62A) — Explains the mathematical principles of stereo vision clearly.
> - [Stanford CS231A — Epipolar Geometry and Stereo](https://web.stanford.edu/class/cs231a/) — Stanford's computer vision course. Covers epipolar geometry well.

> **Exercise**: [Stereo Disparity visualization](https://alexjunholee.github.io/robotics-practice/app.html#stereo_disparity)
> Compute disparity from a stereo image pair and observe how baseline and focal length affect depth estimation.

### 2.1.3 RGB-D Camera

A sensor that directly provides an RGB image and a depth image.

The first sensor you are likely to encounter in a lab is an RGB-D camera, because it is the most convenient for experimenting with SLAM or 3D reconstruction in a desktop environment. Without knowing the difference between ToF and structured light, you cannot explain why depth values break down outdoors or why running several units at once causes interference.

**ToF (Time of Flight) method**:
- Emits infrared light and measures the return time
- Pros: texture-independent, real-time processing
- Cons: sunlight interference, issues with reflective surfaces
- Examples: Microsoft Azure Kinect, PMD Pico Flexx

**Structured light method**:
- Projects a known pattern and analyzes its deformation
- Pros: high accuracy, low cost
- Cons: hard to use outdoors, multi-sensor interference
- Examples: Intel RealSense D400 series, Orbbec Astra

**Comparison**:
| Characteristic | ToF | Structured Light |
|------|-----|------------------|
| Outdoor use | Limited | Difficult |
| Accuracy | Medium | High |
| Range | 0.2-5m | 0.2-10m |
| Multi-sensor | Possible | Interference occurs |

> **Further reading**
> - [Intel RealSense — Depth Cameras D415 & D435](https://www.youtube.com/watch?v=A4Kjvosvx5I) — Intel's own explanation of depth camera principles.
> - [Open3D RGB-D Reconstruction Tutorial](http://www.open3d.org/docs/release/tutorial/pipelines/rgbd_integration.html) — Hands-on tutorial for 3D reconstruction with RGB-D data.

**Installing the RealSense driver (Ubuntu 22.04)**

```bash
# Install the Intel RealSense SDK
sudo mkdir -p /etc/apt/keyrings
curl -sSf https://librealsense.intel.com/Debian/librealsense.pgp | sudo tee /etc/apt/keyrings/librealsense.pgp > /dev/null
echo "deb [signed-by=/etc/apt/keyrings/librealsense.pgp] https://librealsense.intel.com/Debian/apt-repo `lsb_release -cs` main" | \
    sudo tee /etc/apt/sources.list.d/librealsense.list
sudo apt-get update
sudo apt-get install -y librealsense2-dkms librealsense2-utils librealsense2-dev

# Test
realsense-viewer
```

For use with ROS2, additionally:
```bash
sudo apt install ros-humble-realsense2-camera
ros2 launch realsense2_camera rs_launch.py
```

(Reference: [Jinyong Jeong's blog](https://jinyongjeong.github.io/2020/06/20/Realsense-Ubuntu-driver-%EC%84%A4%EC%B9%98/))

### 2.1.4 Event Camera

A sensor with a different paradigm from conventional cameras. Instead of capturing frame by frame, each pixel asynchronously outputs an event only when a **brightness change** occurs.

Event-camera research has continued to expand around high-speed motion and HDR conditions, where frame cameras often struggle. Because an event sensor records per-pixel brightness changes asynchronously, it avoids the motion blur caused by frame exposure. If your work involves fast motion or HDR scenes, start with the Gallego et al. survey (TPAMI 2020) and the rpg_dvs_ros package.

**Event output format**:

```
(x, y, timestamp, polarity)
- x, y: pixel coordinates
- timestamp: time in microseconds
- polarity: brighter (+1) or darker (-1)
```

**Pros**:
- Very high temporal resolution (microseconds)
- High dynamic range (140dB vs. 60dB for a typical camera)
- Low power consumption, low latency
- No motion blur

**Cons**:
- No output without a brightness change (i.e., when both the scene and the camera are static and the lighting is constant)
- Difficult to apply traditional CV algorithms
- Relatively expensive

**Representative products**:
- Prophesee: high-resolution event sensors
- iniVation: DAVIS (simultaneous event + frame output)
- Samsung: mobile event sensor in development

> **Further reading**
> - [Davide Scaramuzza — Event Cameras: A Paradigm Shift for Computer Vision](https://www.youtube.com/watch?v=LauQ6LWTkxM) — Overview lecture by Prof. Scaramuzza, a pioneer in event cameras.
> - [Gallego et al. — Event-based Vision: A Survey (TPAMI 2020)](https://arxiv.org/abs/1904.08405) — Comprehensive survey of event camera technology. A good starting point for understanding this field.
> - [rpg_dvs_ros — Event Camera ROS driver](https://github.com/uzh-rpg/rpg_dvs_ros) — Open-source package for handling event cameras in ROS.

## 2.2 LiDAR

**LiDAR (Light Detection and Ranging)** is a sensor that measures distance using lasers. The dimensionality of the output depends on the type: 3D LiDAR directly produces a point cloud, while 2D LiDAR returns a scan of a single plane.

Cameras record color and texture, but passive monocular images do not directly determine metric depth. LiDAR measures the range of each return and forms points (3D points for a 3D LiDAR, points on a plane for a 2D LiDAR). Range and error depend on the model, surface reflectivity, incidence angle, atmosphere, sunlight, and return mode; some long-range automotive units specify ranges beyond 100 m under stated reflectivity conditions. Compared with a passive monocular camera, this direct range measurement is LiDAR's core strength. ToF and structured-light RGB-D cameras and radar also measure range directly, so what sets LiDAR apart is not the measurement itself but its angular resolution and its range accuracy at long distances.

Solid-state LiDAR is replacing spinning (mechanical) LiDAR in some applications. With fewer moving parts, it offers advantages in durability and mass production for automotive systems. Scan patterns such as Livox's non-repetitive design also differ from those of spinning sensors, and point-cloud processing algorithms must account for those differences.

### 2.2.1 2D LiDAR vs. 3D LiDAR

**2D LiDAR**:
- Single-plane scan
- Use cases: indoor robot navigation, obstacle avoidance
- Examples: SICK TiM, Hokuyo URG, RPLIDAR

**3D LiDAR**:
- Generates 3D point clouds via multiple layers or rotational scanning
- Use cases: autonomous driving, large-scale mapping
- Examples: Velodyne VLP-16 / VLP-32C / HDL-64E, Ouster OS1, Hesai

> **Further reading**
> - [Cyrill Stachniss — LiDAR-based SLAM](https://www.youtube.com/watch?v=vrdlk2p9AZI) — Explains the principles of SLAM using LiDAR data.
> - [PCL (Point Cloud Library) official tutorials](https://pcl.readthedocs.io/projects/tutorials/en/latest/) — a widely used public point-cloud processing library.

### 2.2.2 Spinning vs. Solid-State

**Spinning (mechanical)**:
- Laser and receiver rotate
- Provides 360° FoV
- Cons: durability issues due to moving parts
- Examples: Velodyne, Ouster

**Solid-State**:
- No large spinning assembly (MEMS and flash designs have no moving parts; Livox's non-repetitive scan uses internal rotating prisms)
- Limited FoV (usually under 120°)
- Pros: high durability, potential for low cost
- Examples: Livox (non-repetitive scan pattern), Innoviz

The difference directly affects algorithm design. Spinning LiDAR produces a uniform 360° point cloud, so existing SLAM algorithms (LOAM, LeGO-LOAM, etc.) were designed on that assumption. Solid-state LiDAR changes the scan pattern significantly and forces algorithm changes. That is why FAST-LIO2 and similar algorithms, targeting Livox's non-repetitive scans, have emerged.

### 2.2.3 Key specifications

| Specification | Description |
| --- | --- |
| Channels | Number of vertical layers (16, 32, 64, 128) |
| Range | Maximum measurement distance (50m ~ 300m) |
| Points/sec | Points per second (300K ~ 2M) |
| Accuracy | Measurement accuracy (±2cm ~ ±5cm) |
| FoV | Horizontal/vertical field of view |

> **Further reading**
> - [Livox technical documents](https://www.livoxtech.com/downloads) — Technical material explaining the non-repetitive scan pattern of solid-state LiDAR and its advantages.
> - [Xu et al. — FAST-LIO2 (T-RO 2022)](https://arxiv.org/abs/2107.06829) — A LiDAR-inertial odometry paper that drops feature extraction in favor of direct registration and an ikd-Tree, handling both spinning and solid-state LiDAR.

## 2.3 IMU (Inertial Measurement Unit)

An IMU is a sensor that measures motion using inertia.

When SLAM drifts badly, without understanding IMU characteristics you cannot even identify the cause. Questions like "is the IMU bias being properly corrected?" or "can this grade of IMU deliver this level of accuracy?" require a proper grasp of the IMU error model. In visual-inertial odometry (VIO) or LiDAR-inertial odometry (LIO), the IMU fills the gaps between camera/LiDAR frames, and filling that role correctly demands knowing the limits of IMU data.

### 2.3.1 Components

**Accelerometer**:
- Measures 3-axis linear acceleration (m/s²)
- Includes gravitational acceleration

**Gyroscope**:
- Measures 3-axis angular velocity (rad/s or deg/s)
- Detects rotational speed

**Magnetometer** (on some IMUs):
- Measures 3-axis magnetic field
- Can estimate absolute heading
- Vulnerable to magnetic field distortion

### 2.3.2 Key error characteristics

If you cannot model IMU errors, the entire sensor fusion system wobbles.

**Bias**:
- Nonzero output even at rest
- Drifts slowly over time (bias instability, defined as the floor of the Allan deviation curve). Thermal drift with temperature change is a separate term

**Noise**:
- High-frequency random noise
- Characterized by Allan variance

**Integration drift**:
- Double integration of acceleration → accumulated position error
- Integration of angular velocity → accumulated orientation error
- Trustworthy only for a short time (usually a few seconds)

Double-integrating acceleration accumulates noise, bias, scale-factor error, and initial-attitude error. The growth depends on the sensor, motion, calibration, temperature, and initialization, so no single elapsed time characterizes it. Unaided position from a low-cost MEMS IMU can quickly exceed a long-term position-error budget; systems that need sustained position therefore use external observations from a camera, LiDAR, GNSS, or another reference to limit drift.

**IMU grades** are not a single standardized price ladder. Compare bias stability, noise density, scale-factor error, temperature calibration, vibration tolerance, and certification requirements. Consumer MEMS prioritizes size and power; products described as industrial, tactical, or navigation grade generally add long-term stability and calibration. For devices such as the `VN-100`, `MTi`, `KVH 1750`, and `HG1700`, compare current datasheets in common units and confirm them with Allan measurements.

> **Further reading**
> - [Probabilistic Robotics, Ch.5–6 — Robot Motion / Robot Perception (Thrun, Burgard, Fox)](https://www.probabilistic-robotics.org/) — A leading reference on motion models (Ch.5) and sensor measurement models (Ch.6). It does not cover IMU error models (bias, random walk, Allan variance); see Titterton & Weston for those.
> - [Titterton & Weston — Strapdown Inertial Navigation Technology](https://ieeexplore.ieee.org/book/5765860) — The textbook on IMU principles and inertial navigation.
> - [Cyrill Stachniss — IMU and Inertial Navigation](https://www.youtube.com/watch?v=uHbRKvD8TWg) — Explains IMU operating principles and error characteristics visually.
> - [Allan Variance — IMU noise analysis guide (Vectornav)](https://www.vectornav.com/resources/inertial-navigation-primer/specifications--background/specifications--allan-variance) — How to extract IMU noise parameters using Allan variance.
> - [Jinyong Jeong's blog — IMU Filter (AHRS)](https://jinyongjeong.github.io/2020/01/10/IMU_filter/) — Overview of AHRS filters for IMU sensors. Introduces the Madgwick filter and ROS packages.

## 2.4 GPS/GNSS

**GNSS (Global Navigation Satellite System)** is a position measurement system using satellite signals. GPS is the U.S. system, and GNSS is the umbrella term covering GPS, GLONASS (Russia), Galileo (Europe), BeiDou (China), and others.

GNSS provides outdoor autonomous vehicles and drones with coordinates in an Earth-fixed frame. SLAM estimates position relative to a starting point, whereas GNSS expresses location as latitude, longitude, and altitude. Outdoor robots combine these frames, and RTK-GPS measurements are also used as ground truth in high-precision localization evaluations.

**Interpreting accuracy**: a standalone code solution is commonly meter-class in open sky, while differential corrections may produce sub-meter or meter-class results depending on the setup. RTK can reach horizontal centimeter-class results when a short baseline, sufficient satellite geometry, low multipath, and fixed ambiguities are maintained. Any quoted number should state the metric (CEP, RMS, or 95%), horizontal versus vertical component, baseline, correction link, and fix state.

**RTK-GPS principle**:
- A fixed base station provides correction data
- The rover receives the correction data to improve accuracy
- Requires real-time communication (radio or internet)

**Limitations**:
- Unusable indoors, in tunnels, and in urban canyons
- Multipath errors (building reflections)
- Altitude accuracy is lower than horizontal

> **Further reading**
> - [Cyrill Stachniss — Robot Localization Overview](https://www.youtube.com/watch?v=8VJ-A9OlhAE) — Overview of the principles and methods of robot localization.
> - [u-blox GNSS guide](https://www.u-blox.com/en/technologies/gnss) — A practical guide from GNSS basics to RTK.

## 2.5 Other sensors

**Radar**

Autonomous-driving and robotics systems use radar alongside cameras and LiDAR. Radio waves can be more robust than visible light and some LiDAR wavelengths in fog, rain, dust, and backlight, but rain attenuation, clutter, multipath, wet radomes, and limited angular resolution remain. Prices overlap with LiDAR product families as antenna count, bandwidth, imaging capability, and automotive qualification change, so compare current quotations for the required configuration.

**FMCW (Frequency Modulated Continuous Wave) Radar**:
- Transmits a frequency modulated over time and uses the frequency difference with the reflected wave to measure both range and velocity simultaneously.
- Output: range-Doppler map (distance × velocity 2D map), range-azimuth map
- 77 GHz automotive radar is the most common.

**Applications in robotics**:
- Autonomous driving: forward collision detection, adaptive cruise control (ACC)
- Radar odometry: estimating ego-motion from radar alone
- Radar SLAM: radar-based map building + localization

**Comparison with camera/LiDAR**:

| Characteristic | Camera | LiDAR | Radar |
|------|--------|-------|-------|
| Resolution | Very high | High | Low |
| Range measurement | Not possible (monocular) | Accurate | Possible |
| Velocity measurement | Not possible | Not possible (directly) | Possible (Doppler) |
| Adverse weather | Weak | Weak (rain, fog) | Robust |
| Price | Cheap | Expensive | Medium |
| Nighttime | Not possible | Possible | Possible |

**Representative products**: Texas Instruments AWR1843, Continental ARS548, Navtech CTS350-X (spinning radar)

> **Further reading**
> - [Giseop Kim's blog — ICRA 2021 Radar in Robotics Workshop summary](https://gisbi-kim.github.io/blog/2021/05/31/icra21-radar-ws.html) — Overall trends in radar robotics.
> - [Giseop Kim's blog — Radar Odometry Results on MulRan dataset](https://gisbi-kim.github.io/blog/2021/05/30/yeti-radar-odom-mulran1.html) — Radar odometry experimental results. LiDAR-level performance in urban environments.
> - [Kim et al., "MulRan: Multimodal Range Dataset for Urban Place Recognition" (ICRA 2020)](https://sites.google.com/view/mulran-pr/home) — LiDAR + radar + GPS multimodal dataset.

**Ultrasonic**:
- Detects obstacles at short range (0.2-5m)
- Low cost
- Parking assistance, proximity sensing

**Wheel encoder**:
- Measures wheel rotation
- Position estimation via dead reckoning
- Vulnerable to slip

Classifying these as "other" does not make them unimportant. Radar acts as the safety net in adverse weather where LiDAR fails in autonomous driving, and the wheel encoder is the most basic odometry source for ground robots. In sensor fusion, such "auxiliary" sensors determine the robustness of the whole system.

> **Further reading**
> - [Probabilistic Robotics, Ch.6 — Robot Perception (Thrun, Burgard, Fox)](https://www.probabilistic-robotics.org/) — Thoroughly covers probabilistic models of various sensors. The textbook on sensor modeling.

## 2.6 Sensor Fusion

Since each single sensor has its own limits, multiple sensors are combined to complement each other.

A single sensor cannot cover every combination of illumination, range, occlusion, and drift. Autonomous-vehicle sensor suites vary with the vehicle and operating conditions, combining several of camera, LiDAR, radar, IMU, and GNSS. When, where, and how the selected sensors are fused determines system performance.

**Why is it needed?**

| Sensor | Pros | Cons |
| --- | --- | --- |
| Camera | Rich information, cheap | Lighting-dependent, no depth |
| LiDAR | Accurate 3D, lighting-independent | Expensive, sparse |
| IMU | High frequency, lighting-independent | Drift |
| GPS | Global position | Outdoor only, low frequency |

**Fusion approaches**:
1. **Early Fusion**: combine at the raw data level
2. **Late Fusion**: combine the results from each sensor
3. **Mid-Level Fusion**: combine at the feature level

Each approach has trade-offs. Early fusion loses less information but is computationally expensive; late fusion allows each sensor to be processed independently, which helps modularity, but some information is lost. Mid-level fusion sits in between and is heavily used in recent deep-learning-based fusion.

**Representative combinations**:
- Camera + IMU → VIO (visual-inertial odometry)
- LiDAR + IMU → LIO (LiDAR-inertial odometry)
- Camera + LiDAR + IMU → multimodal SLAM

The probabilistic foundation of fusion — how sensor measurements are formally expressed as likelihoods — is covered in §2.7 Measurement Models.

> **Further reading**
> - [State Estimation for Robotics (Tim Barfoot) — free PDF](http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17.pdf) — A leading textbook covering the mathematical foundations of sensor fusion. Covers both Kalman filter and factor-graph-based estimation.
> - [Cyrill Stachniss — Kalman Filter & EKF](https://www.youtube.com/watch?v=E-6paM_Iwfc) — Explains the Kalman filter and EKF, the core of sensor fusion.
> - [Qin et al. — VINS-Mono (TRO 2018)](https://arxiv.org/abs/1708.03852) — A representative paper on visual-inertial fusion. Shows how a real VIO system is implemented.

---

## 2.7 Deep Dive: Measurement Models — Probabilistic Formulation

Bayes filters, SLAM, and MCL need a number that says "how much to trust this reading" every time sensor data arrives. That number is the measurement model $p(z_t \mid x_t, m)$.

### 2.7.1 The Distribution a Sensor Produces

Fire a laser range sensor at the same wall from the same pose one hundred times, and the hundred measurements differ. Reflectance angle, a passing person, and multi-path reflections each leave a different variance signature. The probability distribution $p(z_t^k \mid x_t, m)$ captures that variance structure. $z_t^k$ is the $k$-th beam reading at time $t$, $x_t$ is the robot pose, and $m$ is the map.

A single scan contains tens to hundreds of beams. PR §6.2 assumes the error on each beam is independent (conditional independence assumption); under that assumption the full-scan likelihood is the product of the per-beam likelihoods:

$$p(z_t \mid x_t, m) = \prod_{k=1}^{K} p(z_t^k \mid x_t, m)$$

This conditional independence assumption does not fully hold in practice. Adjacent beams looking at the same wall are correlated, and ignoring that correlation concentrates the likelihood too sharply around a particular pose. This is revisited in §2.7.8.

The map $m$ comes in two forms. A feature-based map is a list of landmarks indexed by ID. A location-based map is an array of occupancy probabilities over grid cells, indexed by coordinate. The four measurement model families each depend on one of these two map types.

The four measurement model families:
- Beam model: a mixture that approximates possible causes of a measurement. Location-based map.
- Likelihood field: endpoint-to-nearest-obstacle distance. Location-based map.
- Correlation-based (map matching): normalized correlation between local and global map.
- Feature-based (landmark model): extracted features modeled as (range, bearing, signature). Feature-based map.

### 2.7.2 Beam Model — Four-Component Mixture

A range reading is approximated using four hypotheses about how it was produced. [Thrun et al. 2005](https://www.probabilistic-robotics.org/) (PR §6.3.1) assigns a probability distribution to each hypothesis and builds the likelihood as a weighted mixture. These components are terms in an observation model, not independent physical channels inside the sensor.

The most frequent component is **hit** — the beam actually detects the obstacle. A truncated Gaussian centered on the predicted range $z_t^{k*}$ with variance $\sigma_{\text{hit}}^2$ models this. The truncation removes probability mass outside $[0, z_{\max}]$.

$$p_{\text{hit}}(z_t^k \mid x_t, m) = \eta\, \mathcal{N}(z_t^k;\, z_t^{k*},\, \sigma_{\text{hit}}^2), \quad 0 \le z_t^k \le z_{\max}$$

**short (unexpected nearby obstacle)**: An unmapped obstacle — a passing person, another robot — blocks the beam. The reading is always shorter than $z_t^{k*}$. An exponential distribution over $[0, z_t^{k*}]$ models this.

$$p_{\text{short}}(z_t^k \mid x_t, m) = \eta\, \lambda_{\text{short}}\, e^{-\lambda_{\text{short}} z_t^k}, \quad 0 \le z_t^k \le z_t^{k*}$$

**max (maximum-range failure)**: Dark surfaces, mirror angles, and fog cause the return signal to vanish. The sensor outputs $z_{\max}$ directly. A Dirac delta at $z_{\max}$ models this.

$$p_{\text{max}}(z_t^k \mid x_t, m) = \mathbf{1}[z_t^k = z_{\max}]$$

**rand (unexplained noise)**: Sonar crosstalk, multi-path, and other unknown sources produce readings with no identifiable cause. A uniform distribution over $[0, z_{\max}]$ models this.

$$p_{\text{rand}}(z_t^k \mid x_t, m) = \frac{1}{z_{\max}}$$

The final likelihood is the weighted mixture of the four components (PR Eq. 6.13):

The weights must sum to one: $z_{\text{hit}} + z_{\text{short}} + z_{\text{max}} + z_{\text{rand}} = 1$.

The predicted range $z_t^{k*}$ is computed from pose $x_t$ and map $m$ by ray casting: follow the beam direction until it hits the first occupied cell; that distance is $z_t^{k*}$. (Ray casting applies the same geometric principle seen in §2.1's camera projection model and §2.2's LiDAR beam structure, now to an occupancy grid.)

**Algorithm: beam_range_finder_model** (adapted from PR Table 6.1)

```
Input:  z_t = {z_t^1, ..., z_t^K}, x_t, m
Output: p(z_t | x_t, m)

1. q ← 1
2. for k = 1 to K do:
3.     z_t^{k*} ← ray_cast(x_t, k, m)   // predicted range
4.     p ← z_hit  * p_hit(z_t^k | z_t^{k*}, σ_hit)
             + z_short * p_short(z_t^k | z_t^{k*}, λ_short)
             + z_max   * p_max(z_t^k | z_max)
             + z_rand  * p_rand(z_t^k | z_max)
5.     q ← q * p
6. return q
```

### 2.7.3 Beam Model — EM Parameter Learning

Every time the sensor type, environment configuration, or mounting position changes, the hit/short/max/rand ratios and variances shift. Setting parameters by hand produces values that fit one environment and drift in another.

The four-component mixture has six intrinsic parameters: $z_{\text{hit}}, z_{\text{short}}, z_{\text{max}}, z_{\text{rand}}, \sigma_{\text{hit}}, \lambda_{\text{short}}$. PR §6.3.2 estimates these by maximum likelihood using the EM algorithm on data $\{(z_t^k, z_t^{k*})\}$ collected while the robot navigates a known environment.

The EM formulation introduces a correspondence variable. For each measurement $z_t^k$, the latent variable $c_i \in \{\text{hit, short, max, rand}\}$ indicates which component generated the value.

**E-step**: Use the current parameter estimates to compute the expected value of $c_i$. For each measurement, compute the posterior probability of each of the four components (PR Eq. 6.15–6.32):

**M-step**: Update the parameters using the expectations from the E-step. Closed-form solutions exist for $\sigma_{\text{hit}}$ and $\lambda_{\text{short}}$:

The mixture weights $z_{\text{hit}}, z_{\text{short}}, z_{\text{max}}, z_{\text{rand}}$ are updated as the fraction of measurements assigned to each component.

**Algorithm: learn_intrinsic_parameters** (condensed from PR Table 6.2)

```
Input:  {(z^i, z^{i*})} — (measured, predicted) pairs
Output: z_hit, z_short, z_max, z_rand, σ_hit, λ_short

Initialize: set parameters to uniform or arbitrary values
repeat until convergence:
    // E-step
    for each i:
        e_hit^i, e_short^i, e_max^i, e_rand^i ← posterior(z^i, z^{i*}, params)
    // M-step
    z_hit  ← mean(e_hit^i);   z_short ← mean(e_short^i)
    z_max  ← mean(e_max^i);   z_rand  ← mean(e_rand^i)
    σ_hit² ← weighted variance of (z^i - z^{i*}) by e_hit^i
    λ_short ← sum(e_short^i) / sum(e_short^i * z^i)
return params
```

EM estimates these parameters for the sensor, map, and environment represented in its training data. AMCL implementations expose `sigma_hit`, `lambda_short`, and the mixture weights as configurable values, but package defaults should not be interpreted as universal EM convergence values across environments.

Once parameters are in hand, putting the model to work in a real system needs a few more practical adjustments.

### 2.7.4 Beam Model — Practical Considerations

The main computational bottleneck of the beam model is ray casting. In MCL, running ray casting for every beam of every particle requires (number of particles) × (number of beams) operations.

One option is to reduce the beam count. A uniformly spaced subset lowers computation and removes some redundancy between adjacent beams. The number of beams still needs validation against scan resolution, environment structure, and particle count.

**Exponentiation correction $p^{\alpha}$**: When the conditional independence assumption is violated, the likelihood $p(z_t \mid x_t, m)$ can become overconfident, concentrating too sharply. Replacing it with $p(z_t \mid x_t, m)^{\alpha}$ ($0 < \alpha < 1$) reduces each beam's contribution and flattens the distribution. $\alpha$ is set empirically or by cross-validation.

**Precomputed range table**: Precomputing ray casting results for all (cell, direction) combinations in the map and storing them in a table makes range lookup $O(1)$ at runtime. Memory cost is high for large maps, but the approach is practical for real-time MCL (see Ch.3 §3.11 on particle filters).

### 2.7.5 Likelihood Field

Two weaknesses of the beam model cause problems in real systems. First, ray casting is expensive. Second, a small change in pose $x_t$ can cause a beam to hit a different obstacle first, making $z_t^{k*}$ jump discontinuously. The likelihood is discontinuous with respect to pose, which interferes with gradient-based scan matching and hill-climbing optimization.

The likelihood field drops ray casting entirely. It transforms the beam endpoint into global coordinates, then evaluates the likelihood using the Euclidean distance $\text{dist}$ from that endpoint to the nearest occupied cell in the map.

Beam endpoint transformation to global coordinates (PR Eq. 6.33):

Here $(x_{k,\text{sens}}, y_{k,\text{sens}})$ is the position of the $k$-th beam's sensor in the robot frame, and $\theta_{k,\text{sens}}$ is the beam's angular offset.

Beam likelihood (PR Eq. 6.34–6.35):

$$p(z_t^k \mid x_t, m) = z_{\text{hit}} \cdot \mathcal{N}(\text{dist};\, 0,\, \sigma_{\text{hit}}^2) + z_{\text{rand}} \cdot \frac{1}{z_{\max}}$$

Here $\text{dist}$ is the Euclidean distance from the beam endpoint to the nearest occupied cell, and the Gaussian models the distance error as zero-mean. Max-range beams ($z_t^k = z_{\max}$) are ignored in this model: projecting their endpoint is meaningless.

**Algorithm: likelihood_field_range_finder_model** (adapted from PR Table 6.3)

```
Input:  z_t = {z_t^1, ..., z_t^K}, x_t = (x, y, θ)^T, m
Output: p(z_t | x_t, m)

1. q ← 1
2. for each k do:
3.     if z_t^k == z_max: continue   // skip max-range readings
4.     // transform beam endpoint to global coordinates
5.     x_ep ← x + x_{k,sens}·cos(θ) - y_{k,sens}·sin(θ) + z_t^k · cos(θ + θ_{k,sens})
6.     y_ep ← y + y_{k,sens}·cos(θ) + x_{k,sens}·sin(θ) + z_t^k · sin(θ + θ_{k,sens})
7.     // distance to nearest obstacle (lookup from precomputed distance transform table)
8.     dist ← nearest_obstacle_distance(x_ep, y_ep, m)
9.     q ← q * (z_hit · N(dist; 0, σ_hit²) + z_rand / z_max)
10. return q
```

When the map is fixed, the distance transform can be computed once and stored as a table, making every $\text{dist}$ lookup $O(1)$. The table corresponds to the positive region of an SDF (Signed Distance Field). The likelihood is differentiable with respect to pose $x_t$, which makes it suitable for gradient-based scan matching.

The model has limits. It does not model dynamic obstacles explicitly (no short component). It also scores only the endpoint distance and never accounts for the path the beam traveled, so a beam that passes straight through occupied space on its way is not penalized at all. That is why it can "see through walls," and map uncertainty is ignored as well.

In 2D LiDAR indoor navigation, AMCL uses the likelihood field as its default — not the beam model — because it is faster and produces a pose-continuous likelihood. For 3D LiDAR and RGB-D, ICP and NDT have taken over that role (Ch.3 §3.10; see also Ch.14 §14.7 for Kalman-filter integration).

When the goal is not pose estimation but loop closure detection — quickly deciding whether two maps cover the same place — even faster methods are needed, even if they sacrifice probabilistic rigor.

### 2.7.6 Correlation-Based Model (Map Matching)

The correlation-based model is the most ad hoc of the four. It builds a local map $m_{\text{local}}$ from a recent set of scans, then compares it to the global map $m$ using the normalized correlation coefficient $\rho$. PR §6.5 uses this comparison directly as the likelihood:

$$p(m_{\text{local}} \mid x_t, m) = \max\{\rho(m_{\text{local}}, m \mid x_t),\ 0\}$$

$\rho$ is the Pearson correlation coefficient between corresponding cells when the two maps are aligned by $x_t$. Computation is fast and the implementation is simple. The weakness: this likelihood has no probabilistic justification. $\rho$ is normalized, and values below zero are simply clipped. It is used in settings like loop closure detection where a fast similarity score matters more than a proper likelihood.

Unlike the three models above, which work with raw range measurements directly, the final model works with structured features extracted from sensor data.

### 2.7.7 Feature-Based Measurement — Landmark Model

The beam model and likelihood field work with raw range measurements. The landmark model works with features $f(z_t)$ extracted from sensor data. Inference over low-dimensional features is cheaper, and the model pairs naturally with feature-based maps.

Feature extraction takes different forms depending on the sensor. From range scans: line segments, corners, local minima. From cameras: edges, corners, SIFT/ORB-style local patterns (see §2.1.1 on monocular camera texture, and §2.6 for VIO/Visual SLAM). Each extracted feature is represented as a triple $(r, \phi, s)$: $r$ is range, $\phi$ is bearing, and $s$ is signature (ID, color, descriptor, etc.).

The $j$-th landmark in the map sits at $(m_{j,x}, m_{j,y})$ with signature $s_j$. From pose $x_t = (x, y, \theta)^T$, the relationship between predicted and observed measurement is (PR Eq. 6.41):

$\varepsilon_{\sigma^2}$ denotes zero-mean Gaussian noise with variance $\sigma^2$. The three channels carry independent Gaussian noise. Adding Gaussian noise directly to the bearing channel $\varepsilon_{\sigma_\phi^2}$ can produce wrap-around errors near $\pm\pi$; in real implementations, angular differences are normalized to $[-\pi, \pi]$ or modeled with the von Mises distribution instead.

When the correspondence $c_t^i = j$ (the $i$-th feature corresponds to the $j$-th landmark) is known, the likelihood is the product of Gaussians over the three channels.

**Algorithm: landmark_model_known_correspondence** (adapted from PR Table 6.4)

```
Input:  f_t^i = (r_t^i, φ_t^i, s_t^i)^T, correspondence c_t^i = j,
        x_t = (x, y, θ)^T, m
Output: p(f_t^i | c_t^i = j, x_t, m)

Assuming conditional independence across features in the full scan, the full scan likelihood is $\prod_i$.

**Reverse direction — pose sampling** (condensed from PR Table 6.5): the model can also run in reverse, sampling possible poses from a measurement. A single $(r, \phi)$ reading gives only two constraints in pose space; the set of compatible poses lies on a circle (in 2D) or a helix (in 3D) around the landmark. A free parameter $\hat{\gamma} \sim U(0, 2\pi)$ samples positions on that circle. This is the geometric explanation for why a single observation of one landmark is not enough to determine position.

The reprojection residual $\| \pi(K[R|t]\, X_w) - u \|^2_\Sigma$ used in visual SLAM has the same broad structure: predict an observation from pose and landmark, then compare it with the measurement. The pixel reprojection model and the range-bearing model are nevertheless different sensor models, and ORB/SIFT descriptors are normally used for data association rather than as a continuous signature term in the likelihood. AprilTag and ArUco IDs reduce correspondence ambiguity substantially, but they do not rule out false detections or misread IDs.

### 2.7.8 Practical Summary: Choosing a Model

The four families can be compared qualitatively as follows. Accuracy and speed depend on the sensor, map resolution, implementation, and parameters.

| Model | Accuracy | Speed | Differentiable | Primary use |
|------|--------|-----------|------------|-----------|
| Beam model | High | Slow (ray casting) | Low (discontinuous) | MCL high-fidelity, diagnostics |
| Likelihood field | Medium | Fast (DT lookup) | High | AMCL default, gradient matching |
| Correlation-based | Low | Very fast | Low | Loop closure detection |
| Landmark model | High (feature-dependent) | Fast (low-dimensional) | High | Visual SLAM, fiducial |

One more practical concern is over-confidence. When the conditional independence assumption is violated, $p(z_t \mid x_t, m)$ can become too sharply peaked at a particular pose. Tempering it as $p(z_t \mid x_t, m)^{\alpha}$ ($\alpha < 1$), as in §2.7.4, reduces each scan's influence and flattens the distribution. Beam subsampling, models that account for correlation, and robust likelihoods are alternatives; $\alpha$ must be chosen with calibration or validation data rather than treated as a universal constant.

With the model limits and mitigations understood, the natural next question is which of these models survived into production systems.

### 2.7.9 What Survived

The four families in PR §6 remain useful for classifying and designing systems. It would be misleading, however, to label every modern scan matcher or visual SLAM method a **direct descendant** of them. Methods can share the broad observation-versus-prediction structure while using different objectives and map representations.

The [Nav2 AMCL documentation](https://docs.nav2.org/configuration/packages/configuring-amcl.html) exposes three laser models: `beam`, `likelihood_field`, and `likelihood_field_prob`; its default is `likelihood_field`. `max_beams` selects an evenly spaced subset of a scan. By contrast, the `beam_skip_*` parameters belong to `likelihood_field_prob` and skip beams that disagree with many particles, so they are not ordinary beam subsampling. Defaults such as `sigma_hit` and `lambda_short` are implementation starting points, not universally converged values traceable to one EM experiment.

Other LiDAR systems use distinct matching objectives. Cartographer combines correlative scan matching on a probability grid with nonlinear optimization, while `hdl_localization` uses NDT/GICP-family registration on 3D point clouds. They share the broad idea of comparing an observation against a map, but they do not all implement a likelihood-field distance transform. ESDF planners and neural implicit maps also use distance or implicit fields; similarity in data structure alone does not establish descent from the likelihood-field sensor model.

The same distinction applies to landmarks and visual SLAM. The range-bearing model in Eq. 6.41, camera reprojection models, and DROID-SLAM's dense bundle adjustment all predict observations from pose and scene structure and form residuals. Their measurement spaces, association procedures, noise models, and optimization variables differ, so one should not assert a single historical lineage among them. Fiducial IDs simplify association but do not eliminate false detections.

`hit`, `short`, `max`, and `rand` are mixture components that approximate possible causes of a range measurement, not independent physical channels. The choice of components and map representation depends on the sensor, environment, outliers, compute budget, and calibration data. These four families are a starting point for comparison, not a genealogy that covers every modern system.

Ch.14 §14.7 shows how `beam_range_finder_model` is called inside MCL and how `inverse_sensor_model` connects to occupancy mapping — where these models sit in the full pipeline.

> **⚠ Sensor connection check**: When sensor data is missing, inspect the cable, IP configuration, power, and USB bandwidth as well as the driver. Commands such as `dmesg`, `lsusb`, and `ping` record the device and connection state and help narrow the fault.

> **Technical Timeline: sensor technology**
> - **~2010**: 2D LiDAR and frame cameras were common in mobile-robot research. The practical range of real-time stereo depended strongly on the available compute and scene conditions.
> - **2010s**: RGB-D cameras and compact multi-beam 3D LiDAR broadened the options for indoor 3D perception and outdoor mapping. Camera–IMU VIO also moved beyond research prototypes into several robotics and AR systems.
> - **Late 2010s to early 2020s**: Non-repetitive scanning, MEMS, and flash architectures appeared under the broad label `solid-state LiDAR`; research in event cameras and automotive imaging radar also expanded. Price and performance trends varied too much across product classes to summarize with one number.
> - **2020s**: Spinning and solid-state LiDAR coexist because field of view, range, resolution, motion distortion, and cost impose different trade-offs. Adoption of event cameras and Doppler radar is likewise application-dependent, including high-speed, HDR, and adverse-weather settings.
> - **Design implication**: A change in scan pattern or timestamp structure changes assumptions used by deskewing, calibration, and data association. Inspect the actual sampling geometry and noise characteristics instead of relying on the hardware category name.

---

# Ch.3 — Mathematical Foundations

Reading and implementing Spatial AI papers requires a mathematical foundation. If "optimization on SE(3)" is unfamiliar, the main argument of a SLAM paper may be lost; if a sentence such as "we derived the Jacobian and solved the system with Gauss-Newton" is unclear, the methodology becomes hard to reconstruct. The mathematics here is a working toolkit for robotics, connecting undergraduate linear algebra to its use in research papers and code.

Differentiable Programming and Auto-Differentiation have changed how classical mathematical tools enter an optimization pipeline. PyTorch and JAX can compute Jacobians and gradients that were once derived by hand, enabling systems such as end-to-end learned SLAM and differentiable rendering with NeRF or 3D Gaussian Splatting. Interpreting those derivatives and debugging the pipeline still requires the linear algebra and optimization developed here.

## 3.1 Linear Algebra

Linear algebra is the common language of coordinate transformations, camera models, optimization, and deep learning. This section connects undergraduate definitions to the calculations used in robotics.

### 3.1.1 Vectors and Matrices

**Vector**: a quantity with magnitude and direction.

```
v = [v_x, v_y, v_z]^T  (column vector)
```

In robotics, vectors represent points in 3D space, forces, velocities, and so on. "The robot is at (3, 2, 1) in the world frame" expresses a position as a vector.

**Matrix operations**:
- Addition/subtraction: element-wise
- Multiplication: row-by-column inner product
- Transpose: A^T
- Inverse: A^(-1), AA^(-1) = I

Coordinate transformation, rotation, and projection are all expressed as matrix multiplications. A camera projecting a 3D point to a 2D image, a robot transforming between coordinate frames — all of it is matrix multiplication.

> **Further reading**
> - [3Blue1Brown — Essence of Linear Algebra](https://www.youtube.com/playlist?list=PLZHQObOWTQDPD3MizzM2xVFitgF8hE_ab) — Visualizes matrix multiplication and eigenvalues geometrically. It presents linear algebra as transformations of space rather than only calculation rules.
> - [Introduction to Applied Linear Algebra (Boyd & Vandenberghe) — free PDF](https://web.stanford.edu/~boyd/vmls/) — Applied linear algebra textbook by Stanford's Professor Boyd. Practical perspective, includes Python examples.
> - [Dark Programmer — Linear Algebra series (6 posts: basic formulas to PCA)](https://darkpgmr.tistory.com/103) — Summarizes key terms, inverse, eigenvalues, SVD, linear systems, and PCA in Korean.
> - [Dark Programmer — Vector and Matrix Calculus](https://darkpgmr.tistory.com/141) — Rules for vector/matrix differentiation. Foundation needed for Jacobian computation.

### 3.1.2 Eigenvalue Decomposition

```
Av = λv
```

- v: eigenvector
- λ: eigenvalue

**Uses**: PCA, covariance matrix analysis, stability analysis.

You'll use this the moment you handle a point cloud. When PCA (Principal Component Analysis) finds the principal axes of a point cloud, the eigenvectors of the covariance matrix are the principal axis directions and the eigenvalues are the variances along them. Deciding whether "this point cloud is a plane or a line" also comes from the ratio of eigenvalues. Normal vector estimation uses the eigenvector corresponding to the smallest eigenvalue.

> **Further reading**
> - [3Blue1Brown — Eigenvectors and Eigenvalues](https://www.youtube.com/watch?v=PFDu9oVAE-g) — Intuitive explanation of the geometric meaning of eigenvalues.
> - [MIT 18.06 Linear Algebra — Gilbert Strang (YouTube)](https://www.youtube.com/playlist?list=PLE7DDD91010BC51F8) — A widely known linear algebra lecture series. Covers all of linear algebra in depth, including eigenvalue decomposition.

> **Exercise**: [PCA 3D · Dimensionality Reduction](https://alexjunholee.github.io/robotics-practice/app.html#pca_3d)
> Manipulate the process by which the eigenvectors of the covariance matrix become the principal axes of a 3D distribution, and simultaneously visualize dimensionality reduction onto the PC1·PC2 plane (3D→2D) and the PC1 axis (2D→1D).

### 3.1.3 Singular Value Decomposition (SVD)

```
A = UΣV^T
```

- U: left singular vectors (m×m orthogonal matrix)
- Σ: diagonal matrix of singular values (m×n)
- V: right singular vectors (n×n orthogonal matrix)

**Uses**: least squares solutions, matrix approximation, fundamental matrix computation.

SVD shows up constantly in robotics. It is the most numerically stable way to compute the least-squares solution of an overdetermined system. Camera calibration for the fundamental matrix, point cloud registration for the optimal transformation — all use SVD. The final step of the "8-point algorithm," which computes the fundamental matrix from eight or more correspondences, is SVD.

> **Further reading**
> - [Steve Brunton — Singular Value Decomposition (YouTube)](https://www.youtube.com/watch?v=nbBvuuNVfco) — Lectures by a University of Washington professor that clearly explain the mathematical meaning of SVD and its applications.
> - [Linear Algebra and Its Applications (Gilbert Strang)](https://math.mit.edu/~gs/linearalgebra/ila6/indexila6.html) — Standard linear algebra textbook. The SVD chapter is particularly well written.

## 3.2 3D Geometry

3D geometry expresses the positions and orientations of robots, cameras, and objects in one mathematical language. SLAM uses this representation to connect observations across viewpoints.

### 3.2.1 Coordinate Frames

In Spatial AI you move between multiple coordinate frames. The **World Frame (W)** is the globally fixed frame, the **Camera Frame (C)** is centered on the camera, the **Body Frame (B)** is centered on the robot, and the **IMU Frame (I)** is the IMU sensor's frame.

An object observed by a camera is located in the camera frame. Before a robot can use that position, it must transform the coordinates into the body or world frame. Because each sensor has its own frame, sensor fusion uses the extrinsic calibration between them.

**Coordinate transformation**:

```
p_W = T_WC × p_C
```

T_WC: Camera → World transformation matrix (4×4).

> **Further reading**
> - [State Estimation for Robotics, Ch.6 — Coordinate Frames (Tim Barfoot) — free PDF](http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17.pdf) — A treatment of coordinate-frame transformations from the perspective of robotics state estimation.
> - [Stanford CS231A — Camera Models](https://web.stanford.edu/class/cs231a/) — The part of Stanford's CV course that covers camera frames and projection models.

### 3.2.2 Rotation Representations

Rotation matrices, Euler angles, quaternions, and axis-angle representations differ in parameter count, constraints, singularities, and interpolation. SLAM code chooses among them according to these properties and the optimization method.

**Rotation Matrix R** is a 3×3 orthogonal matrix (det(R) = 1, R^T = R^(-1)) with 9 parameters and 6 constraints, giving 3 actual degrees of freedom.

**Euler Angles** express rotation with three angles: Roll (φ), Pitch (θ), Yaw (ψ). Intuitive, but has the **Gimbal Lock** problem, and results depend on the application order (ZYX, XYZ, etc.).

**Quaternion q = [w, x, y, z]** (||q|| = 1) expresses 3 DoF with 4 parameters. It has no Gimbal Lock and supports smooth interpolation (Slerp), so it is widely used for attitude representation in robotics.

**Axis-Angle** combines a rotation axis n and angle θ into a 3-parameter representation. It converts to a rotation matrix via Rodrigues' formula.

ROS uses quaternions as its default rotation representation, OpenCV provides Rodrigues vectors (axis-angle), and optimization libraries such as Ceres and GTSAM support Lie-group representations. Check component order and normalization when connecting these interfaces.

> **Further reading**
> - [3Blue1Brown — Quaternions and 3D Rotation](https://www.youtube.com/watch?v=zjMuIxRvygQ) — Visualizes the geometric meaning of quaternions. An intuitive answer to why four dimensions are needed for 3D rotation.
> - [State Estimation for Robotics, Ch.7 — Rotation (Tim Barfoot)](http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17.pdf) — Clean treatment of every rotation representation and the conversions between them.
> - [Sola — Quaternion Kinematics for the Error-State Kalman Filter (Tech Report)](https://arxiv.org/abs/1711.02508) — Mathematical foundations of quaternion-based error-state Kalman filtering for VIO/INS implementations. A very practical technical report.
> - [3D Rotation Converter](https://www.andre-gaschler.com/rotationconverter/) — Online tool for checking conversions between quaternions, Euler angles, and rotation matrices.

> **Exercise**: [Rotation Representations and Gimbal Lock](https://alexjunholee.github.io/robotics-practice/app.html#rotation_gimbal) | [6DoF Pose Visualization](https://alexjunholee.github.io/robotics-practice/app.html#xyzrpy_6dof)
> Manipulate and compare Euler-angle Gimbal Lock and quaternion rotation directly, and interactively explore a 6-DoF pose (x, y, z, roll, pitch, yaw).

### 3.2.3 Homogeneous Coordinates

Extend a 3D point to 4D so that transformations become a single matrix:

```
[X, Y, Z, 1]^T  (3D point)

T = | R   t |   (4×4 transformation matrix)
    | 0   1 |
```

Why use homogeneous coordinates: rotation and translation can be expressed as one matrix multiplication. In ordinary coordinates p' = Rp + t (multiplication + addition), but in homogeneous coordinates it becomes p' = Tp (multiplication only). When chaining multiple transformations you just multiply the matrices, which is convenient for things like the chain of joint transformations in a robot arm.

### 3.2.4 SE(3) and SO(3)

**SE(3)** (Special Euclidean Group) is the set of all 3D rigid-body transformations (rotation + translation) with 6 DoF. **SO(3)** (Special Orthogonal Group) is the set of rotations alone with 3 DoF.

SE(3) and SO(3) are **Lie groups**. When optimizing, you need to "update while satisfying the rotation matrix constraints (orthogonality, determinant 1)," and Lie group theory solves this elegantly. You optimize without constraints on the corresponding **Lie algebra** (se(3), so(3)) and then map back to the Lie group via the exponential map. This concept is central to pose graph optimization in SLAM.

> **Further reading**
> - [State Estimation for Robotics, Ch.7 (Tim Barfoot) — free PDF](http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17.pdf) — A treatment of SE(3), SO(3), and Lie groups and algebras from the perspective of robotics state estimation.
> - [Sola — A Micro Lie Theory for State Estimation in Robotics (arXiv:1812.01537)](https://arxiv.org/abs/1812.01537) — A paper that summarizes Lie group theory just as far as state estimation in robotics requires. Very practical.

## 3.3 Probability & Statistics

Sensor data always has noise, and the robot's state always has uncertainty. Probability and statistics express and manipulate that uncertainty mathematically. "The sensor value is exactly 3.0 m" is not meaningful; "3.0 m ± 0.05 m (95% confidence interval)" is. Propagating and updating this uncertainty is the basis of state estimation.

### 3.3.1 Gaussian Distribution

```
p(x) = (1 / √(2πσ²)) × exp(-(x-μ)²/(2σ²))
```

**Multivariate Gaussian**:

```
p(x) = N(μ, Σ)
```

- μ: mean vector
- Σ: covariance matrix

Widely used for modeling sensor noise and position uncertainty.

The Gaussian is used this heavily because of mathematical convenience. The central limit theorem explains why sums of many small independent effects can be approximated by a Gaussian under suitable conditions. The sum of independent Gaussian random variables is Gaussian, and the product of Gaussian densities over the same variable is Gaussian after normalization, which makes analysis easy. The Kalman filter assumes a Gaussian for the same reason.

> **Further reading**
> - [3Blue1Brown — But what is the Central Limit Theorem?](https://www.youtube.com/watch?v=zeJD6dqJ5lo) — Visual explanation of the central limit theorem. An intuitive answer to why the Gaussian appears everywhere.
> - [Kalman Filter — How it works, in pictures](http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/) — Visual explanation of how the Kalman filter works. Good for building intuition before the equations.

> **Exercise**: [Kalman Filter](https://alexjunholee.github.io/robotics-practice/app.html#kalman_filter)
> Interactively manipulate the Kalman filter's predict-update cycle and observe Gaussian-based state estimation in action.

**Mahalanobis Distance**

Euclidean distance treats every direction equally. But sensor data has different uncertainty in different directions. GPS, for example, has much larger error vertically (tens of meters) than horizontally (a few meters).

Mahalanobis distance is distance that accounts for covariance:

```
d_M = sqrt((x - μ)^T Σ^{-1} (x - μ))
```

If Σ is the identity, it reduces to Euclidean distance. If Σ is diagonal, it is a per-axis scaled distance. For a general Σ, distance is redefined along the principal axes of the covariance.

Use in SLAM: for data association, Mahalanobis distance decides "did this observation come from this landmark?" Something close in Euclidean but far in Mahalanobis (not aligned with the uncertainty direction) is likely a wrong association.

(See: [Dark Programmer — Mean, Standard Deviation, Variance, and Mahalanobis Distance](https://darkpgmr.tistory.com/41))

### 3.3.2 Bayes' Rule

```
P(A|B) = P(B|A) × P(A) / P(B)
```

Bayes' rule updates the probability of a state given a sensor measurement. Kalman and particle filters implement this update with different distribution representations and approximations.

**Recursive state estimation**:

```
P(x_t | z_{1:t}) ∝ P(z_t | x_t) × P(x_t | z_{1:t-1})
```

- P(z_t | x_t): measurement model — "given the robot is at this pose, what is the probability the sensor outputs this value?"
- P(x_t | z_{1:t-1}): prior — "based on all prior information, what is the probability the robot is here?"

Each new measurement combines the prior with the measurement likelihood to form a posterior. Under linear-Gaussian assumptions, the Kalman filter's predict-update cycle implements this recursion.

> **Further reading**
> - [3Blue1Brown — Bayes' Theorem](https://www.youtube.com/watch?v=HZGCoVF3YvM) — A good visual introduction to Bayes' theorem.
> - [Probabilistic Robotics (Thrun, Burgard, Fox)](https://www.probabilistic-robotics.org/) — The essential textbook on probabilistic robotics. Organizes Bayes filters, Kalman filters, particle filters, and SLAM cleanly from a probabilistic viewpoint.
> - [Giseop Kim's blog — Bayesian Filtering series (2 posts)](https://gisbi-kim.github.io/blog/2021/03/09/bayesfiltering-1.html) — Korean-language walkthrough of Bayes filtering. A foundation that leads into the Kalman filter.

### 3.3.3 MLE and MAP

**MLE (Maximum Likelihood Estimation)**:

```
x* = argmax P(z | x)
```

MLE finds the parameter most likely given the data.

**MAP (Maximum A Posteriori)**:

```
x* = argmax P(x | z) = argmax P(z | x) × P(x)
```

MAP incorporates the prior into the estimate.

The difference in SLAM is between "find the optimal position from observations alone (MLE)" and "find the optimal position using prior position information too (MAP)." Real SLAM systems mostly use MAP. A prior makes estimation stable even with noisy observations. With Gaussian observation noise and fixed covariance, minimizing the negative log posterior for MAP gives a "weighted sum of squared observation errors + a regularization term from the negative log prior," which is the same form as regularized least squares from an optimization standpoint.

> **Further reading**
> - [Probabilistic Robotics, Ch.2 — Recursive State Estimation (Thrun)](https://www.probabilistic-robotics.org/) — Explains the relationships between MLE, MAP, and Bayes filtering in a robotics context.
> - [Cyrill Stachniss — Maximum Likelihood and MAP Estimation](https://www.youtube.com/watch?v=XepXtl9YKwc) — A clear, example-driven explanation of the difference between MLE and MAP.

**Intuitive difference between MLE and MAP**

Both share the goal of "find the most plausible parameter," but their approaches differ.

MLE (Maximum Likelihood) asks "which parameter maximizes the probability of observing this data?" It looks only at the data. MAP (Maximum A Posteriori) adds a prior: "given the data and combining prior knowledge, which value maximizes the posterior probability of the parameter?"

In formulas: MAP = MLE + prior. With a Gaussian prior, MAP equals MLE with L2 regularization added. Weight decay in deep learning can be seen as an implementation of MAP.

In SLAM: multiply the likelihood of odometry measurements by the likelihood of sensor observations, combine with the prior from the previous state, and do MAP estimation. Each factor in a factor graph corresponds to one of these likelihoods or priors.

(See: [Dark Programmer — Bayes' Rule, ML and MAP, and Image Processing](https://darkpgmr.tistory.com/62))

## 3.4 Optimization Basics

SLAM bundle adjustment, camera calibration, and deep-learning training all minimize objective functions. Distinguishing the residual, Jacobian, and update rule helps locate convergence failures in the model, initialization, or solver.

### 3.4.1 Least Squares

```
x* = argmin ||Ax - b||²
```

**Normal equation**:

```
x* = (A^T A)^(-1) A^T b
```

Least squares is the most basic way to "find the best-fitting model parameters from noisy measurements." From line fitting to camera calibration, it is the starting point of almost every estimation problem.

> **Further reading**
> - [Cyrill Stachniss — Least Squares for Robotics](https://www.youtube.com/watch?v=r2cyMQ5NB1o) — Concrete explanation of applying least squares to robotics problems.
> - [Giseop Kim's blog — SLAM back-end series (3 posts)](https://gisbi-kim.github.io/blog/2021/03/04/slambackend-1.html) — An introduction to the back-end that starts from "SLAM is solving Ax=b." Three-part series leading up to factor graphs.
> - [Giseop Kim's blog — Iterative Optimization, Part 1](https://gisbi-kim.github.io/blog/2021/03/16/leastsquare-1.html) — An intuitive Korean-language walkthrough of nonlinear optimization.

**Intuition for least squares**

"Why minimize the square of the error?" The reason for square rather than absolute value is twofold: it is differentiable, and it penalizes large errors more. As a bonus, under a Gaussian noise assumption it gives the same solution as Maximum Likelihood Estimation.

In an over-determined system (more equations than unknowns), there may be no x that exactly satisfies Ax = b. Instead, find the x that minimizes ||Ax - b||², which gives the normal equation `A^T A x = A^T b`. That is all there is to least squares.

Caveat: if `A^T A` is singular (rank deficient), there is no unique solution. In that case, instead of the pseudo-inverse `x = (A^T A)^{-1} A^T b`, use SVD for numerical stability.

(See: [Dark Programmer — Understanding Least Squares and Various Uses](https://darkpgmr.tistory.com/56))

### 3.4.2 Gradient Descent

```
x_{k+1} = x_k - α × ∇f(x_k)
```

- α: learning rate
- ∇f: gradient

Gradient descent is used daily in deep learning, but it is also the baseline for optimization in robotics. It is the intuitive method of stepping opposite to the gradient to find a minimum. The limits are that tuning the learning rate is difficult, it can get stuck in local minima, and convergence is slow, so robotics typically uses more efficient methods (Gauss-Newton, LM).

**Relationships between gradient, Jacobian, and Hessian**

The **gradient** ∇f is the first derivative of a scalar function f and outputs an n×1 vector. It tells you "in which direction does f increase fastest?" The **Jacobian** J extends this to a vector function f: R^n → R^m as an m×n matrix. It holds the partial derivatives of each output with respect to each input. The **Hessian** H is the second derivative of a scalar function f, an n×n symmetric matrix that carries curvature information and is used in Newton's method.

Relationships:
```
For cost function C(x) = ||r(x)||²:
  Gradient:  ∇C = J^T r           (J is the Jacobian of r)
  Hessian:   H ≈ J^T J            (Gauss-Newton approximation: drop the second-derivative term)
  Update:    δx = -(J^T J)^{-1} J^T r
```

Why Gauss-Newton uses J^T J as the Hessian approximation: the exact Hessian is expensive to compute, and in regions where the residual r is small, the second-order term is negligible.

(See: [Dark Programmer — Gradient, Jacobian, Hessian, Laplacian](https://darkpgmr.tistory.com/132))

### 3.4.3 Gauss-Newton

Solves nonlinear least squares problems by iteratively linearizing:

```
(J^T J) Δx = -J^T r
x_{k+1} = x_k + Δx
```

- J: Jacobian matrix
- r: residual

Why Gauss-Newton is preferred over gradient descent in robotics: it uses second-order information (J^T J as a Hessian approximation) and converges much faster. When SLAM optimizes thousands to tens of thousands of variables, gradient descent takes far too long to converge, whereas Gauss-Newton can converge in a handful of iterations.

### 3.4.4 Levenberg-Marquardt (LM)

A hybrid of Gauss-Newton and gradient descent:

```
(J^T J + λI) Δx = -J^T r
```

- λ: damping factor
- small λ → Gauss-Newton (fast convergence)
- large λ → gradient descent (stable)

Used for bundle adjustment and pose graph optimization in SLAM.

Gauss-Newton can converge quickly from a good initial value but may diverge otherwise. LM adjusts λ so that large damping produces a gradient-descent-like update and small damping approaches Gauss-Newton. Optimization libraries such as Ceres Solver, g2o, and GTSAM provide this method.

> **Further reading**
> - [Cyrill Stachniss — Gauss-Newton and Levenberg-Marquardt for SLAM](https://www.youtube.com/watch?v=hRyL5KwFLAE) — Step-by-step explanation of how Gauss-Newton and LM are used in SLAM.
> - [State Estimation for Robotics, Ch.4 — Nonlinear Optimization (Tim Barfoot) — free PDF](http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17.pdf) — Solid treatment of nonlinear optimization from a robotics state estimation perspective.
> - [Ceres Solver Tutorial](http://ceres-solver.org/tutorial.html) — Google's nonlinear least squares optimization library. Hands-on practice with how to use the LM algorithm in code.
> - [Dark Programmer — Intuitive Understanding of Optimization Methods](https://darkpgmr.tistory.com/149) — Geometric intuition for gradient descent, Newton, LM, and others.
> - [Giseop Kim's blog — Five recommended resources for SLAM back-end study](https://gisbi-kim.github.io/blog/2021/10/03/slam-textbooks.html) — A curated list covering error-state KF, factor graphs, bundle adjustment, and more.
> - [Derivative Calculator](https://www.derivative-calculator.net/) — Online tool that shows step-by-step symbolic differentiation. Useful for checking Jacobian derivations.

**Intuition for LM: switching between Gauss-Newton and gradient descent**

In nonlinear least squares, Gauss-Newton converges fast but can diverge with a bad initial value. Gradient descent is slow but stable. LM switches between them automatically via the damping factor λ.

Small λ is close to Gauss-Newton and converges fast near the solution; large λ is close to gradient descent and is stable in the early stages far from the solution. If an update reduces the cost, decrease λ; if it increases, increase λ. This adaptive switching is the core of LM, and it is why the default solver in Ceres Solver is LM.

(See: [Dark Programmer — Summary of Function Optimization Methods (LM, etc.)](https://darkpgmr.tistory.com/142))

## 3.5 Advanced: Lie Group and Lie Algebra

One of the most frequent mathematical hurdles in robotics is "how do you optimize rotations?" Lie groups and Lie algebras provide a systematic framework for handling rotations and rigid-body transformations. They are essential for understanding SLAM back-ends, visual-inertial odometry, and bundle adjustment.

### 3.5.1 Why We Need Lie Groups

Section 3.2 covered several ways to represent rotations. Problems arise when you try to optimize with them.

- **Rotation matrix R**: 3x3 with 9 parameters but only 3 actual degrees of freedom, because of the constraints R^T R = I and det(R) = 1. Applying ordinary unconstrained optimization means that after an update, R is no longer a valid rotation matrix.
- **Quaternion**: 4 parameters with a normalization constraint (||q|| = 1). You must re-normalize on every update, and numerical errors accumulate in the process.
- **Euler angles**: have the gimbal lock problem, and angle wrapping is tricky.

The core issue: rotations live on a nonlinear manifold, but the optimization algorithms we know (Gauss-Newton, LM) work in Euclidean space. Lie group theory bridges this gap. It defines a tangent space (the Lie algebra) at a point (a rotation matrix) on the manifold, runs Euclidean optimization in that tangent space, and lifts the result back onto the manifold.

Background: a "group" here is a set equipped with an operation satisfying closure, associativity, identity, and inverse. For example, the set of invertible n x n matrices forms a group under matrix multiplication, called the general linear group GL(n). Its subgroup with det = 1 is the special linear group SL(n). The orthogonal group O(n) is the set of matrices preserving the inner product; collecting only those with det = 1 gives SO(n) — that is, the rotation group. Those with det = -1 include reflections, and because they are not closed under the group operation, they do not form a subgroup.

### 3.5.2 SO(3): The 3D Rotation Group

**Definition:**
```
SO(3) = { R in R^{3x3} | R^T R = I, det(R) = 1 }
```

SO(3) is a group. The group operation is matrix multiplication, and the composition R_1 R_2 of two rotations R_1, R_2 is again in SO(3). The identity is the identity matrix I, and the inverse is R^T (= R^{-1}). Because it is orthogonal, the transpose equals the inverse. Matrix multiplication is associative but not commutative (in general R_1 R_2 != R_2 R_1).

**Lie algebra so(3):**

The Lie algebra of SO(3) is the space of 3x3 skew-symmetric matrices, which is 3-dimensional.

The **hat operator** `[.]x` converts a 3D vector into a skew-symmetric matrix:

```
w = [w1, w2, w3]^T  (in R^3)

[  0   -w3   w2 ]
[w]x =  [  w3   0   -w1 ]   in so(3)
        [ -w2   w1   0  ]
```

This matrix corresponds to the cross product of vectors: `[w]x v = w x v`.

The **vee operator** `(.)v` is the inverse: it extracts a 3D vector from a skew-symmetric matrix.

Intuitively, an element w of so(3) encodes "rotation axis direction" and "rotation magnitude" as a single vector. It corresponds directly to the axis-angle representation.

### 3.5.3 Exponential Map and Logarithmic Map

**Exponential map**: so(3) -> SO(3)

The exponential map sends an element (vector) of the Lie algebra to an element (rotation matrix) of the Lie group.

Suppose we have a matrix R(t) that rotates continuously over time (R(0) = I). R(t) is always in SO(3), so `R(t) R(t)^T = I`. Differentiating both sides with respect to t:

```
d/dt (R R^T) = R_dot R^T + R R_dot^T = 0
→  R_dot R^T = -(R_dot R^T)^T
```

So `R_dot R^T` is skew-symmetric. It can be written as the hat form of some vector w(t):

```
R_dot(t) R^T(t) = [w(t)]x
→  R_dot(t) = [w(t)]x R(t)
```

When w is constant (constant angular velocity), the solution to this differential equation is:

```
R(t) = exp([w]x * t) = sum_{n=0}^{inf} ([w]x * t)^n / n!
```

Here `exp([w]x)` is the matrix that rotates by ||w|| radians around the axis w. Concretely, setting theta = ||w|| yields the closed form via **Rodrigues' formula**:

```
exp([w]x) = I + (sin(theta) / theta) [w]x + ((1 - cos(theta)) / theta^2) [w]x^2
```

This formula is derived by substituting the Taylor series of `sin(t)` and `cos(t)` into powers of `[w]x`. Using the identity `[w]x^3 = -theta^2 [w]x`, the series collapses into sin and cos terms.

When theta is small (|theta| < eps), sin(theta)/theta ≈ 1 and (1-cos(theta))/theta^2 ≈ 1/2, so:

```
exp([w]x) ≈ I + [w]x + (1/2)[w]x^2   (first-order approximation)
```

Caveat: for a given rotation matrix R, the w satisfying `R = exp([w]x)` is not unique. ||w|| + 2*pi*k (integer k) give the same R. This is the subtle point in the logarithmic map.

**Logarithmic map**: SO(3) -> so(3)

The inverse. Recover the axis-angle vector w from a given rotation matrix R.

```
theta = arccos((tr(R) - 1) / 2)
[w]x = (theta / (2 sin(theta))) (R - R^T)
```

Special handling is needed near theta = 0 (identity rotation) or theta = pi (180-degree rotation).

**Intuition**: the Lie algebra is the tangent space at a point on the group (usually the identity I). "Small rotations" can be expressed as vectors in the tangent space, and the exponential map sends such a vector to an actual rotation on the manifold. This is why it is central to optimization: compute the update dw in the tangent space (R^3), then multiply exp([dw]x) onto the current rotation to move along the manifold.

### 3.5.4 SE(3): The 3D Rigid-Body Transformation Group

A robot's pose combines rotation and translation; SE(3) represents both in one rigid-body transformation.

**Definition:**
```
SE(3) = { T = [ R  t ] | R in SO(3), t in R^3 }
              [ 0  1 ]
```

T is a 4x4 homogeneous transformation matrix. SE(3) is also a group. The group operation is matrix multiplication T_1 T_2, the identity is the 4x4 identity matrix, and the inverse is T^{-1} = [ R^T  -R^T t ; 0  1 ].

**Lie algebra se(3):**

The Lie algebra of SE(3) is 6-dimensional. Its elements are called **twist** vectors:

```
xi = [rho; w] in R^6     (rho in R^3: translation part, w in R^3: rotation part)
```

The **hat operator** converts a 6D vector into a 4x4 matrix:

```
        [ [w]x  rho ]
xi^ =   [  0     0  ]   in se(3)    (4x4 matrix)
```

**Exponential map**: se(3) -> SE(3)

```
exp(xi^) = [ exp([w]x)   J rho ]   in SE(3)
           [    0           1   ]
```

Here J is the left Jacobian of SO(3):

```
J = I + ((1 - cos(theta)) / theta^2) [w]x + ((theta - sin(theta)) / theta^3) [w]x^2
```

A 6-DoF pose (3 rotation + 3 translation) can be parameterized by a 6D vector xi in R^6. Optimization is performed in unconstrained 6D Euclidean space, after which the exponential map lifts the result onto the SE(3) manifold. This is why SLAM optimization uses Lie groups.

> **Exercise**: [SE(3) Pose Composition](https://alexjunholee.github.io/robotics-practice/app.html#pose_composition_3d)
> Manipulate the composition of SE(3) transformations in 3D and see how combined rotation-and-translation rigid-body transformations chain together.

### 3.5.5 Perturbation Model and Jacobian

When optimizing a pose with Gauss-Newton or LM, there are two ways to apply a small perturbation d_xi to the current estimate T.

**Left perturbation (global frame):**
```
T' = exp(d_xi^) * T
```

**Right perturbation (body frame):**
```
T' = T * exp(d_xi^)
```

Either works, as long as you stay mathematically consistent. Conventions differ across the literature, so pay attention. Barfoot's textbook uses left predominantly, and Strasdat's Sophus uses right by default.

**Jacobian computation:**

For an error function e(T), the Jacobian with respect to the perturbation is:

```
de/d(d_xi) = lim_{d_xi->0}  (e(exp(d_xi^) * T) - e(T)) / d_xi     (for left perturbation)
```

This Jacobian is a 6-column matrix (error dimension x 6).

In ordinary optimization the update is `x <- x + dx` (Euclidean addition). But on SE(3), addition is not defined. Instead:

1. Compute d_xi in R^6 (the tangent space) via Gauss-Newton: `d_xi = -(J^T J)^{-1} J^T e`.
2. Update on the manifold: `T <- exp(d_xi^) * T`.

This guarantees that T remains a valid SE(3) element after the update. No separate constraint handling is needed.

g2o, GTSAM, and Ceres (with local parameterization / manifold) use this approach internally. GTSAM's `Pose3` implements SE(3) directly and provides `Pose3::Expmap()` and `Pose3::Logmap()`. In Ceres, the same concept is implemented through `LocalParameterization` (or `Manifold` in the newer API).

### 3.5.6 Adjoint Representation

Use the adjoint when you need to transform a twist into a different coordinate frame.

For an element T of SE(3), the adjoint matrix Ad_T is a 6x6 matrix:

```
Ad_T = [ R    [t]x R ]   in R^{6x6}
       [ 0      R    ]
```

Twist transformation:
```
xi_a = Ad_{T_ab} * xi_b
```

**Practical meaning**: use the adjoint to convert a twist (angular velocity and linear velocity) expressed in the sensor frame to its body- or world-frame representation. An IMU measures angular velocity and specific force rather than linear velocity, and rotation matrices transform the axes of these measurement vectors. In a VIO system fusing multiple sensors, coordinate frame conversions happen frequently, so you need to understand what the adjoint means.

### 3.5.7 Use in Practice

**Sophus (C++)**: A Lie group library written by Strasdat. It implements SO(3), SE(3), their exponential/logarithmic maps, the adjoint, and more. Several SLAM systems, including ORB-SLAM3, use it. Kimera handles Lie group operations through GTSAM's Pose3/Rot3 instead.

```cpp
#include <sophus/se3.hpp>

// Initialize an SE(3) pose (identity transformation)
Sophus::SE3d T_world_body;

// se(3) perturbation (6-vector): [translation; rotation]
Sophus::SE3d::Tangent delta;
delta << 0.01, 0.0, 0.0, 0.0, 0.0, 0.001;  // small x-translation + small z-rotation

// Left perturbation update
T_world_body = Sophus::SE3d::exp(delta) * T_world_body;

// Log map: SE(3) -> se(3)
Sophus::SE3d::Tangent xi = T_world_body.log();
```

**Jaxlie (Python/JAX)**: A JAX-based Lie group library by Brent Yi. Because automatic differentiation works, you don't need to derive Jacobians by hand. Useful for research prototyping.

```python
import jaxlie
import jax.numpy as jnp

T = jaxlie.SE3.identity()
delta = jnp.array([0.01, 0.0, 0.0, 0.0, 0.0, 0.001])
T_updated = jaxlie.SE3.exp(delta) @ T
```

**GTSAM**: `gtsam::Pose3` uses SE(3) internally. It automatically handles perturbations on the Lie group during factor graph optimization.

> **Further reading**
> - [State Estimation for Robotics, Ch.7-8 (Barfoot)](http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17.pdf) — The key reference for Lie groups in robotics state estimation.
> - [A micro Lie theory for state estimation in robotics (Sola et al., arXiv:1812.01537)](https://arxiv.org/abs/1812.01537) — A 20-page summary of the essentials of Lie groups.
> - [TUM Multiple View Geometry, Ch.2 -- Rigid Body Motion](https://cvg.cit.tum.de/teaching/online/mvg) — Lectures by Professor Daniel Cremers. Visual explanation of SO(3) and SE(3).
> - [Sophus GitHub](https://github.com/strasdat/Sophus) — C++ Lie group library. Reading the code accelerates understanding.
> - [Jinyong Jeong's blog — SE(3) and SO(3) transformation](https://jinyongjeong.github.io/2016/06/07/se3_so3_transformation/) — Korean-language summary of SE(3) and SO(3) transformations. Explains systematically starting from GL(3) and O(3).
> - [T-Robotics: Lie Group Formulation for Robot Mechanics](http://t-robotics.blogspot.com/2015/07/lie-group-formulation-for-robot.html) — Korean-language explanation of Lie groups. Summarizes the use of Lie groups in robot dynamics.

## 3.6 Advanced: Factor Graph

Factor graph is the framework for systematically defining and efficiently solving SLAM problems. The back-end of modern SLAM systems is almost without exception based on factor graphs.

### 3.6.1 What Is a Factor Graph

A factor graph is a bipartite graph made of two kinds of nodes:

- **Variable nodes**: the states to be estimated. Robot poses (x_1, x_2, ...), landmark positions (l_1, l_2, ...), etc.
- **Factor nodes**: constraints or measurements among variables. Each factor defines a cost function over the variables it connects.

Probabilistically, the full posterior decomposes as a product of factors:

```
p(X | Z) proportional to  prod_i  f_i(X_i)
```

Here X_i is the subset of variables connected to factor f_i.

**MAP estimation** = maximize the product of all factors = take the negative log to minimize the sum = a **nonlinear least squares** problem:

```
X* = argmin_X  sum_i  ||e_i(X_i)||^2_{Sigma_i}
```

e_i is the error function and Sigma_i is the covariance (uncertainty weighting) of the measurement.

### 3.6.2 Expressing SLAM as a Factor Graph

Representative factor types in SLAM:

| Factor | Role |
|---|---|
| Prior factor | Prior information about the initial pose. Example: "the start point is the origin" |
| Odometry factor | Relative transformation between two consecutive poses. Comes from IMU preintegration or wheel odometry |
| Landmark observation factor | Measurement of a landmark observed from a pose. Reprojection error is the classic example |
| Loop closure factor | Added when a previously visited place is recognized again. Core mechanism for correcting drift across the whole trajectory |
| IMU preintegration factor | Summarizes IMU measurements between two keyframes as a single factor |

A simple ASCII sketch:

x3 ---[loop closure]--- x1
```

Each factor carries a measurement and a covariance (noise model). Once the graph is built, Gauss-Newton or LM optimizes all variables simultaneously.

### 3.6.3 Solving: Variable Elimination and the Bayes Tree

Optimizing a factor graph requires solving the normal equation `H d = -b` (H is the Hessian approximation, b is the gradient). Understanding the structure of this system is the key to efficient solutions.

**Variable elimination**: the process of eliminating variables one by one. This is mathematically equivalent to sparse Cholesky factorization. The elimination order changes the fill-in (originally zero entries becoming non-zero), which directly affects computational cost.

**Variable ordering**: optimizing the elimination order matters. Heuristics like COLAMD (Column Approximate Minimum Degree) are widely used. Intuitively, eliminating variables with few connections first keeps fill-in low.

**Bayes tree**: a data structure proposed by Kaess et al. (WAFR 2010), the core of iSAM2 (Kaess et al., IJRR 2012). Eliminating a factor graph yields a Bayes net, and reorganizing it into a tree gives the Bayes tree. When a new measurement arrives, only the affected subtree needs re-elimination.

In real-time SLAM, new factors are added every frame. Re-solving the entire system from scratch is O(n^3), but incremental updates via the Bayes tree refresh only the affected part, making real-time processing possible.

> **Further reading**
> - [Factor Graphs and GTSAM (Dellaert & Kaess)](https://gtsam.org/tutorials/intro.html) — The official GTSAM tutorial. Explains the connection from factor graphs to SLAM.
> - [Factor Graphs for Robot Perception (Dellaert & Kaess, 2017)](https://www.cs.cmu.edu/~kaess/pub/Dellaert17fnt.pdf) — A 100-page comprehensive reference.
> - [CMU 16-833 Lecture Notes](https://www.cs.cmu.edu/~kaess/teaching/16833/) — Professor Michael Kaess's SLAM course. Covers factor graphs and iSAM2 in depth.

> **Exercise**: [Factor Graph Visualization](https://alexjunholee.github.io/robotics-practice/app.html#factor_graph_viz)
> Construct variable nodes and factor nodes of a factor graph directly, and see how the graph structure affects optimization.

### 3.6.4 Implementing Pose Graph Optimization with Ceres Solver

Beyond GTSAM, Google's Ceres Solver can also implement factor graph-based optimization. Ceres is a general-purpose nonlinear least squares solver with no SLAM-specific features, which makes it a good way to understand the internals directly. The following is an analysis based on the official Ceres example `pose_graph_3d`.

**Error Term definition:**

Given a relative transformation measurement `T_ab_measured` between two poses `x_a` and `x_b`, the residual is the difference between the estimated relative transformation and the measurement.

template <typename T>
  bool operator()(const T* const p_a_ptr, const T* const q_a_ptr,
                  const T* const p_b_ptr, const T* const q_b_ptr,
                  T* residuals_ptr) const {
    // Compute the estimated relative transformation
    Eigen::Quaternion<T> q_a_inverse = q_a.conjugate();
    Eigen::Quaternion<T> q_ab_estimated = q_a_inverse * q_b;
    Eigen::Matrix<T, 3, 1> p_ab_estimated = q_a_inverse * (p_b - p_a);

// Difference from the measurement
    Eigen::Quaternion<T> delta_q =
        t_ab_measured_.q.cast<T>() * q_ab_estimated.conjugate();

// residual = [position_error; orientation_error]
    residuals.block<3,1>(0,0) = p_ab_estimated - t_ab_measured_.p.cast<T>();
    residuals.block<3,1>(3,0) = T(2.0) * delta_q.vec();

// Apply the information matrix (inverse of covariance)
    residuals.applyOnTheLeft(sqrt_information_.cast<T>());
    return true;
  }
};
```

- **template \<typename T\>**: inside Ceres, `T=double` is used when residual values are needed, and `T=Jet<double>` when Jacobians are needed, switching automatically. That is the mechanism of AutoDiff.
- **sqrt_information**: the Cholesky decomposition of the covariance. Computed as `information.llt().matrixL()`.
- **AutoDiffCostFunction dimensions**: `<PoseGraph3dErrorTerm, 6, 3, 4, 3, 4>` — residual 6D, pos_a 3D, quat_a 4D, pos_b 3D, quat_b 4D.
- **SetManifold**: a quaternion has 4 parameters but only 3 DoF, so specify `EigenQuaternionManifold` to optimize on the manifold. In the older API this was `LocalParameterization`.

**Problem setup:**

```cpp
ceres::Problem problem;
ceres::LossFunction* loss_function = nullptr;  // HuberLoss etc. if robust loss is needed
ceres::Manifold* quaternion_manifold = new EigenQuaternionManifold;

// Fix the first pose (remove gauge freedom)
problem.SetParameterBlockConstant(poses.begin()->second.p.data());
problem.SetParameterBlockConstant(poses.begin()->second.q.coeffs().data());
```

**Solve:**

`SPARSE_NORMAL_CHOLESKY` suits sparse problems like pose graphs. As the number of variables grows, `SPARSE_SCHUR` is also worth considering.

**GTSAM vs Ceres comparison**

| | GTSAM | Ceres |
|---|---|---|
| Character | SLAM-specialized | general-purpose nonlinear least squares |
| Built-ins | predefined factors like `BetweenFactor`, `PriorFactor` | none; all cost functions defined manually |
| Incremental optimization | supported via iSAM2 | not supported |
| Manifold | built-in Lie group support | set manually via `LocalParameterization` / `Manifold` |
| Best suited for | building SLAM systems | when flexible structure is needed, or large-scale BA |

> **Further reading**
> - [Official Ceres Solver pose_graph_3d example](https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/slam/pose_graph_3d/) — Full version of the code above.
> - [Ceres Solver Tutorial](http://ceres-solver.org/tutorial.html) — Explains AutoDiff and Manifold concepts.
> - [Jinyong Jeong's blog — Ceres Solver Tutorial](https://jinyongjeong.github.io/2023/07/22/Ceres_tutorial/) — Ceres Solver presentation slides and GitHub exercise code. A good entry point to nonlinear optimization.

## 3.7 Advanced: Robust Estimation

Real-world data is not clean. False data associations, dynamic objects, and sensor failures produce outliers, and outliers seriously distort optimization results. Robust estimation is the set of techniques for producing sensible estimates even in such situations.

### 3.7.1 Why It's Needed

Standard least squares minimizes the sum of squared errors: `rho(r) = r^2`. Because this function weights large residuals heavily, a single outlier can drag the whole solution.

Concrete cases in SLAM:
- A single wrong loop closure twists the entire map.
- A false positive in visual feature matching ruins the BA result.
- Features attached to dynamic objects (people, cars) violate the static-scene assumption.

### 3.7.2 M-Estimator

An M-estimator uses a different cost function rho instead of `rho(r) = r^2` to reduce the influence of outliers.

| M-Estimator | rho(r) | Characteristics |
|---|---|---|
| **L2 (standard)** | r^2 | Vulnerable to outliers |
| **Huber** | r^2 (abs(r) <= k), 2k*abs(r) - k^2 (abs(r) > k) | L2 for small residuals, L1 for large ones. Supported by many optimization libraries |
| **Cauchy** | c^2 * log(1 + (r/c)^2) | Suppresses outliers more strongly than Huber |
| **Geman-McClure** | r^2 / (1 + r^2) | Effectively ignores extreme outliers |

Huber is the safe default in most cases. With high outlier ratios or extreme cases, consider Cauchy or Geman-McClure. The parameter (k or c) must be tuned to the statistical distribution of the residuals.

In practice, Ceres Solver applies `ceres::HuberLoss`, `ceres::CauchyLoss`, and so on by wrapping the cost function. GTSAM uses `gtsam::noiseModel::mEstimator::Huber`.

> **Exercise**: [M-Estimator comparison](https://alexjunholee.github.io/robotics-practice/app.html#m_estimator)
> Interactively compare how various cost functions such as L2, Huber, Cauchy, and Geman-McClure respond to outliers.

### 3.7.3 RANSAC and Variants

RANSAC (Random Sample Consensus) is an iterative algorithm for fitting a model to data that contains outliers. Unlike M-estimators, it classifies data explicitly as inlier or outlier.

**Basic RANSAC algorithm:**
1. Randomly pick a minimal sample.
2. Fit the model with that sample.
3. Count inliers across the full data (points with residuals within a threshold).
4. Iterate → pick the model with the most inliers.
5. Finally, re-fit the model using all inliers.

**Variants:**

| Variant | Core idea | Trade-off |
|---|---|---|
| RANSAC (basic) | random samples → iterate | Simple and easy to implement but sensitive to threshold and iteration count |
| PROSAC | try good samples first by matching score | Converges fast, but depends on the quality of the prior score |
| Lo-RANSAC | add a local optimization when a good model is found | Higher accuracy, lower speed |
| MAGSAC++ | auto-estimates noise scale sigma, soft inlier/outlier | Close to parameter-free, but computationally expensive |

In OpenCV, you can use MAGSAC++ via the `cv::USAC_MAGSAC` flag in functions like `cv::findHomography` and `cv::findFundamentalMat`.

> **Further reading**
> - [State Estimation for Robotics, Ch.5 (Barfoot)](http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17.pdf) — A practical chapter on biases, correspondence problems, and outliers.
> - [Hartley & Zisserman, Ch.4 -- Estimation: 2D Projective Transforms](https://www.robots.ox.ac.uk/~vgg/hzbook/) — A textbook treatment of RANSAC (originally proposed by Fischler & Bolles, 1981) and robust estimation theory.
> - [Dark Programmer — Understanding RANSAC and Its Use in Image Processing](https://darkpgmr.tistory.com/61) — Explains the principle of RANSAC, threshold setting, and iteration-count computation in Korean.
> - [Jinyong Jeong's blog — Jacobian Computation in Bundle Adjustment](https://jinyongjeong.github.io/2020/03/01/Jacobian_of_BA/) — Derives the BA reprojection error Jacobian with Lie algebra and quaternions. Includes handwritten equations.

> **Exercise**: [RANSAC Visualization](https://alexjunholee.github.io/robotics-practice/app.html#ransac)
> Step through how RANSAC classifies inliers and outliers and fits a model on data with outliers.

## 3.8 Advanced: Information Theory Basics

Information-theoretic concepts are used in active SLAM, exploration, and uncertainty-based decision making.

**Shannon entropy**: measures the uncertainty of a random variable X.

```
H(X) = -sum  p(x) log p(x)
```

Higher entropy means greater uncertainty. For a Gaussian, larger covariance means higher entropy.

**KL divergence (Kullback-Leibler divergence)**: measures the "difference" between two probability distributions p and q.

```
D_KL(p || q) = sum  p(x) log(p(x) / q(x))
```

It is asymmetric: D_KL(p||q) != D_KL(q||p). It can be read as "the information loss when you assumed p but the truth is q."

**Mutual information**: measures how much you learn about X by observing Y.

```
I(X; Y) = H(X) - H(X|Y)
```

H(X) is the uncertainty of X before observing Y, and H(X|Y) is the uncertainty after. The difference is the amount of information Y provides about X.

**Active SLAM application**: when a robot decides where to go next, mutual information quantifies "by how much would this action reduce uncertainty in the map/pose?" Choosing the action with the largest expected information gain is the core of information-theoretic exploration.

```
a* = argmax_a  I(X; Z_a)  =  argmax_a  [ H(Z_a) - H(Z_a | X) ]
```

Here a is the action, Z_a is the observation obtained through that action, and X is the environment state.

> **Further reading**
> - [Elements of Information Theory (Cover & Thomas)](https://onlinelibrary.wiley.com/doi/book/10.1002/047174882X) — An information theory textbook.
> - Placed et al., "A Survey on Active Simultaneous Localization and Mapping: State of the Art and New Frontiers" (IEEE T-RO 2023) — A survey of how information theory is used in active SLAM.

> **Technical Timeline: robotics math and optimization**
> - **~2005**: State estimation centered on the Kalman filter (EKF). Linear-approximation-based, suited to small-scale problems. Real-time processing was difficult, which constrained problem size.
> - **2006~2015**: Factor graph-based optimization (iSAM, g2o, GTSAM) emerged. Sparse matrix structure made large-scale SLAM efficient. Lie groups/algebras became standard tools in the SLAM community.
> - **2016~2020**: Real-time large-scale optimization became practical. Incremental optimization enabled per-frame updates. Open nonlinear least-squares libraries such as Ceres Solver became widely used in research and industrial applications.
> - **2021~**: The era of differentiable programming. End-to-end optimization using auto-differentiation (Auto-Diff) in PyTorch/JAX. With the rise of differentiable rendering such as NeRF and 3D Gaussian Splatting, Jacobians that used to be derived by hand were replaced by auto-diff. Differentiable optimization libraries like Theseus (Meta) also appeared.
> - **Now**: Classical math (Lie groups, probability, optimization) is still essential. Differentiable programming is changing how we approach optimization problems, but understanding what auto-diff does internally still requires the foundations covered here. Knowing only the tools means you can't debug.

---

## 3.9 Advanced: Bayes Filter

Source: Thrun, Burgard, Fox (2005) *Probabilistic Robotics*, Ch.2 (Recursive State Estimation).

§3.8 information theory gave us a tool to *measure* uncertainty. The question now is how to *update* that uncertainty as new observations accumulate over time. Running the Bayes' rule of §3.3.2 recursively along the time axis produces the structure that answers "where is the robot right now?" The factor graph of §3.6 already carries time relations of its own, through the odometry and IMU factors that tie consecutive poses together. What separates the Bayes filter is not whether time is handled at all, but whether past states are marginalized out so that only the current belief is kept — filtering.

Estimating the robot's current location is a **recursive estimation** problem because each new state depends on the previous belief and the latest observation. The Bayes filter gives the general form of this recursion, and both the Kalman filter and particle filter implement it under different representations.

### 3.9.1 State and the Markov Assumption

**State $x_t$** is the collection of variables that contains all information needed to predict the robot's and environment's future. A "complete state" is a sufficient statistic on its own for predicting the future. That property is the **Markov property**.

$$p(x_{t+1} \mid x_t,\, x_{0:t-1},\, z_{1:t},\, u_{1:t}) = p(x_{t+1} \mid x_t)$$

Under the complete-state assumption, the future depends only on the current $x_t$. The past is irrelevant.

State variables fall into several categories. Along the time axis: dynamic states that change (robot position, velocity) and static states that do not (wall positions, landmarks). By value type: continuous states (pose), discrete states (whether a sensor has failed), and hybrid states (both). In practice, a complete state is almost never achievable, so filters always run on partial approximations. The main sources of Markov assumption violations are model inaccuracies and unmodeled dynamics, along with errors arising from the approximation itself.

### 3.9.2 Environment Interaction: Measurements and Controls

Interactions between the robot and the environment decompose into two data streams.

- **Measurement data** $z_t$: information the environment gives the robot (LiDAR range, camera image). Increases the robot's knowledge over the interval $(t-1, t]$.
- **Control data** $u_t$: actions the robot applies to the environment (motor commands). State prediction under control can increase uncertainty because of motion noise.

$$z_{t_1:t_2} = z_{t_1},\, z_{t_1+1},\, \ldots,\, z_{t_2} \qquad u_{t_1:t_2} = u_{t_1},\, \ldots,\, u_{t_2}$$

**Odometry is treated as control data.** Wheel encoders are physically sensors, but because they carry information about state change (how far the robot moved), they are classified as $u_t$. A do-nothing command also counts as control: time passing is itself control information.

### 3.9.3 Defining Belief

Belief is the robot's internal posterior distribution over the true state $x_t$, which cannot be measured directly.

$$\text{bel}(x_t) = p(x_t \mid z_{1:t},\, u_{1:t})$$

The predicted belief *before* incorporating measurement $z_t$ is written separately:

$$\overline{\text{bel}}(x_t) = p(x_t \mid z_{1:t-1},\, u_{1:t})$$

The transition $\overline{\text{bel}} \to \text{bel}$ is called the **correction** or **measurement update**. Even GPS does not directly hand the robot its pose — belief is always the result of indirect inference. This $\text{bel}/\overline{\text{bel}}$ distinction is the conceptual foundation of the two phases of the Bayes filter.

### 3.9.4 Generative Laws: Motion Model and Measurement Model

The complete-state assumption yields two conditional independences:

$$p(x_t \mid x_{0:t-1},\, z_{1:t-1},\, u_{1:t}) = p(x_t \mid x_{t-1},\, u_t) \quad \text{(motion model)}$$

$$p(z_t \mid x_{0:t},\, z_{1:t-1},\, u_{1:t}) = p(z_t \mid x_t) \quad \text{(measurement model)}$$

Because $x_{t-1}$ is a sufficient statistic for all past data, the next state depends only on the immediately preceding state and control, and measurements depend only on the current state. Under a time-invariant assumption these collapse to $p(x' \mid u, x)$ and $p(z \mid x)$.

The complete generative model = motion model + measurement model + initial distribution $p(x_0)$. This structure is a Hidden Markov Model / Dynamic Bayes Network.

### 3.9.5 The General Bayes Filter

The most general form of any belief calculator. It alternates a **prediction** step and a **correction** step.

$$\overline{\text{bel}}(x_t) = \int p(x_t \mid u_t,\, x_{t-1})\, \text{bel}(x_{t-1})\, dx_{t-1} \tag{prediction}$$

$$\text{bel}(x_t) = \eta\, p(z_t \mid x_t)\, \overline{\text{bel}}(x_t) \tag{correction}$$

$\eta$ is a normalization constant corresponding to the reciprocal of the total-probability denominator ($P(B)$) in the Bayes rule of §3.3.2.

```
# Algorithm Bayes_filter (Table 2.1, adapted)
# Input:  bel(x_{t-1}), u_t, z_t
# Output: bel(x_t)

for all x_t do
    # prediction: integrate over x_{t-1} via the motion model
    bel_bar(x_t) = ∫ p(x_t | u_t, x_{t-1}) · bel(x_{t-1}) dx_{t-1}

# correction: weight by measurement model and normalize
    bel(x_t) = η · p(z_t | x_t) · bel_bar(x_t)
endfor
return bel(x_t)
```

In a discrete state space the integral becomes a summation. An initial belief $\text{bel}(x_0)$ is required — set it to a point mass if the initial state is known exactly, or a uniform distribution if not. This general form is only directly implementable when the integral has a closed form or the state space is small enough. The Kalman filter (§3.10) and the particle filter (§3.11) each approximate this general form in different ways.

### 3.9.6 Door Estimation: A Worked Example

A two-state (open/closed) door is used to trace by hand how belief updates.

**Model setup:**

- Measurement model: $p(\text{sense\_open} \mid \text{is\_open}) = 0.6$, $p(\text{sense\_open} \mid \text{is\_closed}) = 0.2$
- Motion model push: if already open stays open (probability 1); if closed opens with probability 0.8
- Motion model do_nothing: deterministic identity (state unchanged)
- Initial: $\text{bel}(X_0 = \text{open}) = \text{bel}(X_0 = \text{closed}) = 0.5$

**Step 1: $u_1$ = do_nothing (apply control)**

do_nothing is the identity transform, so $\overline{\text{bel}}(X_1) = (0.5,\; 0.5)$, unchanged.

**Step 2: $z_1$ = sense_open (incorporate measurement)**

Unnormalized posterior: $(0.6 \times 0.5,\; 0.2 \times 0.5) = (0.30,\; 0.10)$. Normalization constant $\eta = 1/(0.30 + 0.10) = 2.5$.

$$\text{bel}(X_1 = \text{open}) = 0.75, \quad \text{bel}(X_1 = \text{closed}) = 0.25$$

**Step 3: $u_2$ = push (apply control)**

$$\overline{\text{bel}}(X_2 = \text{open}) = 1 \cdot 0.75 + 0.8 \cdot 0.25 = 0.95$$
$$\overline{\text{bel}}(X_2 = \text{closed}) = 0 \cdot 0.75 + 0.2 \cdot 0.25 = 0.05$$

**Step 4: $z_2$ = sense_open (incorporate measurement)**

Unnormalized: $(0.6 \times 0.95,\; 0.2 \times 0.05) = (0.570,\; 0.010)$. $\eta = 1/0.580 \approx 1.724$.

$$\text{bel}(X_2 = \text{open}) \approx 0.983, \quad \text{bel}(X_2 = \text{closed}) \approx 0.017$$

| Step | $\text{bel(open)}$ | $\text{bel(closed)}$ |
|------|:-----------------:|:-------------------:|
| Initial | 0.500 | 0.500 |
| After $z_1$ | 0.750 | 0.250 |
| After $u_2$ | 0.950 | 0.050 |
| After $z_2$ | 0.983 | 0.017 |

Even with substantial sensor noise (60% / 20%) and nondeterministic control, accumulated measurements and controls drive the belief rapidly toward one hypothesis. This example alone, however, cannot determine whether 0.983 is a sufficient threshold for autonomous decision-making.

### 3.9.7 Mathematical Derivation

The two update equations of the Bayes filter follow from three tools alone: Bayes' rule, the law of total probability, and the Markov (complete-state) assumption.

**Correction step derivation:**

Apply Bayes' rule:
$$p(x_t \mid z_{1:t},\, u_{1:t}) = \eta\, p(z_t \mid x_t,\, z_{1:t-1},\, u_{1:t})\, p(x_t \mid z_{1:t-1},\, u_{1:t})$$

The complete-state assumption gives $p(z_t \mid x_t,\, z_{1:t-1},\, u_{1:t}) = p(z_t \mid x_t)$, so:
$$\text{bel}(x_t) = \eta\, p(z_t \mid x_t)\, \overline{\text{bel}}(x_t)$$

**Prediction step derivation:**

Expand $\overline{\text{bel}}$ using the law of total probability:
$$\overline{\text{bel}}(x_t) = \int p(x_t \mid x_{t-1},\, z_{1:t-1},\, u_{1:t})\, p(x_{t-1} \mid z_{1:t-1},\, u_{1:t})\, dx_{t-1}$$

The complete-state assumption reduces the first factor to $p(x_t \mid x_{t-1},\, u_t)$. In the second factor, $u_t$ arrives later than $x_{t-1}$ and can be dropped:
$$\overline{\text{bel}}(x_t) = \int p(x_t \mid u_t,\, x_{t-1})\, \text{bel}(x_{t-1})\, dx_{t-1}$$

The entire derivation rests on the Markov assumption. When the Markov assumption breaks, the equations themselves become inaccurate.

The two lines of the algorithm in §3.9.5 are consequences of Bayes' rule, the law of total probability, and the Markov assumption — nothing else. Knowing where each assumption enters tells you exactly where this filter will fail.

> **Further reading**
> - [Thrun, Burgard, Fox — Probabilistic Robotics (2005)](https://www.probabilistic-robotics.org/) — Ch.2 is the primary source for this section. Algorithm, examples, and derivation are covered completely.
> - [Cyrill Stachniss — Bayes Filter Lecture (YouTube)](https://www.youtube.com/watch?v=0lKHFJpaZkI) — Lecture from Freiburg University. A clear slide-based walkthrough of the Bayes filter.

---

## 3.10 Advanced: Gaussian Filters (KF, EKF, IF)

Source: Thrun, Burgard, Fox (2005) *Probabilistic Robotics*, Ch.3 (Gaussian Filters).

The Bayes filter of §3.9 handles arbitrary beliefs, but the integrals cannot be solved in closed form, which makes direct implementation difficult. The Gaussian filter family resolves this by restricting belief to a Gaussian $\mathcal{N}(\mu_t, \Sigma_t)$. The Kalman filter (KF), extended Kalman filter (EKF), and information filter (IF) all belong to this family, and all three carry over the prediction-correction structure of §3.9 unchanged.

### 3.10.1 Kalman Filter

#### Linear Gaussian System Assumptions

For the KF to be an exact Bayes filter, belief must remain Gaussian at every step. Three assumptions guarantee this.

**State transition (motion model):**
$$x_t = A_t x_{t-1} + B_t u_t + \varepsilon_t, \quad \varepsilon_t \sim \mathcal{N}(0, R_t)$$

$A_t$ is the $n \times n$ state-transition matrix, $B_t$ is the $n \times m$ control-input matrix, and $R_t$ is the process-noise covariance.

**Measurement model:**
$$z_t = C_t x_t + \delta_t, \quad \delta_t \sim \mathcal{N}(0, Q_t)$$

$C_t$ is the $k \times n$ measurement matrix and $Q_t$ is the measurement-noise covariance.

**Initial belief:**
$$\text{bel}(x_0) = \mathcal{N}(\mu_0, \Sigma_0)$$

Under these three assumptions, belief at every time step remains Gaussian:
$$p(x_t \mid u_t, x_{t-1}) = \mathcal{N}(x_t;\; A_t x_{t-1} + B_t u_t,\; R_t)$$
$$p(z_t \mid x_t) = \mathcal{N}(z_t;\; C_t x_t,\; Q_t)$$

#### Kalman Filter Algorithm

The KF represents belief with two quantities $(\mu_t, \Sigma_t)$ and completes one cycle in five steps: two lines of prediction followed by three lines of update.

```
# Algorithm Kalman_filter (Table 3.1, adapted)
# Input:  μ_{t-1}, Σ_{t-1}, u_t, z_t
# Output: μ_t, Σ_t

# --- prediction ---
1: μ̄_t = A_t μ_{t-1} + B_t u_t          # state prediction: apply motion model
2: Σ̄_t = A_t Σ_{t-1} A_t^T + R_t        # covariance prediction: uncertainty grows

# --- correction ---
3: K_t  = Σ̄_t C_t^T (C_t Σ̄_t C_t^T + Q_t)^{-1}   # Kalman gain
4: μ_t  = μ̄_t + K_t (z_t - C_t μ̄_t)    # correct mean using innovation
5: Σ_t  = (I - K_t C_t) Σ̄_t             # covariance shrinks

return μ_t, Σ_t
```

Lines 1–2 are prediction (incorporating control $u_t$; uncertainty grows), and lines 3–5 are the measurement update (incorporating observation $z_t$; uncertainty shrinks).

**Meaning of Kalman gain $K_t$:** $K_t$ sets the confidence balance between prediction and measurement. Large measurement noise $Q_t$ shrinks $K_t$, reducing trust in the measurement; large prediction uncertainty $\bar\Sigma_t$ grows $K_t$, increasing trust in the measurement.

**Innovation:** $z_t - C_t \bar\mu_t$ is the difference between the predicted measurement and the actual measurement. When this is zero, the mean is unchanged, but the covariance can still decrease.

#### 1D KF Illustration: How Information Combines

Visualizing each step of the KF in a 1D position estimation problem makes the intuition clear.

- **Prior $\text{bel}(x_{t-1})$**: a narrow Gaussian — high confidence from the previous estimate.
- **After prediction**: motion adds variance ($\bar\Sigma_t = A_t \Sigma_{t-1} A_t^T + R_t$). The Gaussian flattens.
- **Measurement $z_t$**: represented as a separate Gaussian. Sensor precision $Q_t$ determines the width of this curve.
- **After correction**: multiplying the two Gaussians produces a result narrower than either one — the effect of information combination. The mean sits at the weighted average of the two Gaussians.
- Next motion: variance grows again. Next measurement: variance shrinks again.

In this example, **measurements shrink variance, while motion grows it.** Their alternation forms the basic estimation cycle. The same interpretation applies to the EKF in §3.10.2, the particle filter in §3.11.3, EKF localization in Ch.14 §14.7, and IMU preintegration in §14.10.

#### Mathematical Derivation of the KF (Key Steps)

The five lines of the KF are the closed-form solution to the two integrals of the Bayes filter (§3.9.5) under the linear Gaussian assumption.

**Part 1 (Prediction).** In the prediction integral of the Bayes filter, the exponent $L_t$ is quadratic in both $x_{t-1}$ and $x_t$. Splitting $L_t$ into a part quadratic in $x_{t-1}$ and a part depending only on $x_t$ makes the $x_{t-1}$ integral a constant, absorbed into normalization. The first- and second-order coefficients in the remaining $x_t$ quadratic yield directly $\bar\mu_t = A_t \mu_{t-1} + B_t u_t$ and $\bar\Sigma_t = A_t \Sigma_{t-1} A_t^T + R_t$.

**Part 2 (Measurement update).** From the correction integral $\text{bel}(x_t) \propto \exp\{-J_t\}$, the first and second derivatives of $J_t$ give $\Sigma_t^{-1} = C_t^T Q_t^{-1} C_t + \bar\Sigma_t^{-1}$. Inverting this directly requires $n \times n$ operations, but the **inversion lemma** (Woodbury identity) transforms it into:
$$K_t = \bar\Sigma_t C_t^T (C_t \bar\Sigma_t C_t^T + Q_t)^{-1}$$
which needs only a $k \times k$ ($k$ = measurement dimension) inversion. When $k \ll n$, the computational cost drops considerably.

**Complexity:** $O(k^{2.8} + n^2)$ per cycle ($k$: measurement dimension, $n$: state dimension).

The derivation pattern — quadratic splitting plus the inversion lemma — recurs identically in the EKF derivation of §3.10.2 and the Gauss-Newton update of the factor graph in §3.6.

The brevity of the five KF lines rests on the linear Gaussian assumption. Relaxing that assumption to the nonlinear case leads to §3.10.2 EKF.

### 3.10.2 Extended Kalman Filter (EKF)

#### Extension to Nonlinear Systems

Real robotic systems are not linear. The motion model $g$ for a robot that rotates while moving and the measurement model $h$ for a range sensor are both nonlinear.

$$x_t = g(u_t, x_{t-1}) + \varepsilon_t, \quad \varepsilon_t \sim \mathcal{N}(0, R_t)$$
$$z_t = h(x_t) + \delta_t, \quad \delta_t \sim \mathcal{N}(0, Q_t)$$

A Gaussian passed through a nonlinear $g$ is generally no longer Gaussian. The EKF addresses this with a **first-order Taylor expansion**: it linearizes $g$ around the previous posterior mean and $h$ around the predicted mean to maintain a Gaussian approximation.

$$g(u_t, x_{t-1}) \approx g(u_t, \mu_{t-1}) + G_t (x_{t-1} - \mu_{t-1})$$

$$G_t := \frac{\partial g(u_t, x_{t-1})}{\partial x_{t-1}}\bigg|_{\mu_{t-1}} \quad (n \times n \text{ Jacobian})$$

$$h(x_t) \approx h(\bar\mu_t) + H_t (x_t - \bar\mu_t)$$

$$H_t := \frac{\partial h(x_t)}{\partial x_t}\bigg|_{\bar\mu_t} \quad (k \times n \text{ Jacobian})$$

Linearization quality depends on two factors: how nonlinear the function is, and how wide the belief is. Larger variance makes the tangent-plane approximation break down sooner — EKF works well when variance is small.

#### EKF Algorithm

Going from KF to EKF requires only replacing the linear terms with nonlinear functions and their Jacobians.

```
# Algorithm Extended_Kalman_filter (Table 3.3, adapted)
# Input:  μ_{t-1}, Σ_{t-1}, u_t, z_t
# Output: μ_t, Σ_t

# --- prediction ---
1: μ̄_t = g(u_t, μ_{t-1})                         # nonlinear motion model
2: Σ̄_t = G_t Σ_{t-1} G_t^T + R_t                  # covariance propagated through Jacobian

# --- correction ---
3: K_t  = Σ̄_t H_t^T (H_t Σ̄_t H_t^T + Q_t)^{-1}  # Kalman gain (H_t substituted in)
4: μ_t  = μ̄_t + K_t (z_t - h(μ̄_t))               # nonlinear predicted measurement
5: Σ_t  = (I - K_t H_t) Σ̄_t

return μ_t, Σ_t
```

The KF and EKF differ in two lines: (line 1) $A_t \mu_{t-1} + B_t u_t \to g(u_t, \mu_{t-1})$, and (line 4) $C_t \bar\mu_t \to h(\bar\mu_t)$. In the covariance propagation, $A_t \to G_t$ and $C_t \to H_t$.

#### Derivation Summary and Practical Comparison

The derivation parallels §3.10.1 KF: replace the nonlinear $g$ and $h$ with their first-order Taylor expansions, then run the same quadratic-splitting plus inversion-lemma procedure. The result:
$$\bar\mu_t = g(u_t, \mu_{t-1}), \quad \bar\Sigma_t = G_t \Sigma_{t-1} G_t^T + R_t$$
$$\mu_t = \bar\mu_t + K_t (z_t - h(\bar\mu_t)), \quad \Sigma_t = (I - K_t H_t) \bar\Sigma_t, \quad K_t = \bar\Sigma_t H_t^T (H_t \bar\Sigma_t H_t^T + Q_t)^{-1}$$

**Practical comparison:**

The EKF was widely used in SLAM, VIO, and IMU fusion through the mid-2010s. Several alternatives now compete.

- **UKF (Unscented KF):** Uses sigma points to propagate nonlinearity more accurately. No hand-computed Jacobians needed. Useful when state dimension is low.
- **IEKF (Iterated EKF):** Re-computes the Jacobian by repeating the update point. More accurate than EKF under strong nonlinearity.
- **LIEKF (Left-Invariant EKF):** For SO(3)/SE(3) states, uses manifold linearization in place of Taylor linearization. Improves rotation estimation accuracy.

The EKF/IEKF used in Ch.14 §14.7 EKF localization and §14.10 IMU preintegration takes this algorithm box directly. IMU preintegration in §14.10 uses IEKF to reduce rotation drift; localization in §14.7 feeds odometry into EKF prediction and landmark observations into correction. Understanding what $g$, $h$, $G_t$, and $H_t$ are here means the algorithm skeleton in Ch.14 does not need to be rederived when you encounter specific sensor models there.

### 3.10.3 Information Filter (IF)

#### Canonical Form: $(\Omega, \xi)$

The KF and EKF represented Gaussians with $(\mu, \Sigma)$. Writing the same Gaussian in different coordinates swaps the computational complexity of prediction and measurement update. When measurements from multiple robots or sensors must be fused independently, this coordinate system is substantially more convenient.

There is a second way to represent a Gaussian: instead of mean and covariance $(\mu, \Sigma)$, use the **information matrix** $\Omega$ and the **information vector** $\xi$.

$$\Omega = \Sigma^{-1}, \quad \xi = \Sigma^{-1} \mu$$

Inverse: $\Sigma = \Omega^{-1}$, $\mu = \Omega^{-1} \xi$.

In these coordinates the negative log-likelihood of the Gaussian is quadratic in the state $x$:

$$p(x) = \eta \exp\!\left\{-\tfrac{1}{2} x^T \Omega x + x^T \xi\right\}$$

$$-\log p(x) = \mathrm{const} + \tfrac{1}{2} x^T \Omega x - x^T \xi$$

The minimum is at $\Omega x = \xi$, i.e., $x = \Omega^{-1} \xi = \mu$. This has exactly the same structure as the normal equation $H \delta x = -b$ of the Gauss-Newton method in §3.4.

**Intuition:** $\Omega = 0$ means total absence of information (complete uncertainty, uniform distribution). In the moments representation $\Sigma = \infty$ is unrepresentable, but the information representation handles it naturally. It is as if you were directly measuring certainty.

#### Information Filter Algorithm

The information filter is the dual of the KF. Where the KF's prediction step was additive, the information filter's **measurement update is additive**.

```
# Algorithm Information_filter (Table 3.4, adapted)
# Input:  ξ_{t-1}, Ω_{t-1}, u_t, z_t
# Output: ξ_t, Ω_t

# --- prediction (requires two matrix inversions) ---
1: Ω̄_t = (A_t Ω_{t-1}^{-1} A_t^T + R_t)^{-1}
2: ξ̄_t = Ω̄_t (A_t Ω_{t-1}^{-1} ξ_{t-1} + B_t u_t)

# --- correction (simple addition!) ---
3: Ω_t  = C_t^T Q_t^{-1} C_t + Ω̄_t       # one measurement = one term added to Ω
4: ξ_t  = C_t^T Q_t^{-1} z_t + ξ̄_t       # one measurement = one term added to ξ

return ξ_t, Ω_t
```

**Complexity duality between KF and IF:**

| Step | KF | IF |
|------|:---:|:---:|
| Prediction | $O(n^2)$ additive | $O(n^{2.8})$, two inversions |
| Measurement update | $O(k^{2.8})$, inversion needed | $O(n^2)$ additive |

$k$: measurement dimension, $n$: state dimension. When measurements touch only part of the state (sparse $C_t$), the IF's measurement update is even cheaper.

#### EIF (Extended Information Filter)

As with the EKF, replacing linear $g, h$ with Jacobians $G_t, H_t$ for nonlinear systems yields the EIF. Substituting $A_t \to G_t$ in prediction and $C_t \to H_t$ in correction gives the EIF algorithm.

```
# Algorithm Extended_Information_filter (key changes only)
# prediction  (μ_{t-1} = Ω_{t-1}^{-1} ξ_{t-1})
Ω̄_t = (G_t Ω_{t-1}^{-1} G_t^T + R_t)^{-1}
ξ̄_t = Ω̄_t · g(u_t, μ_{t-1})       # replaces linear IF's A_t μ_{t-1}+B_t u_t

# correction
Ω_t  = H_t^T Q_t^{-1} H_t + Ω̄_t
ξ_t  = H_t^T Q_t^{-1} z_t + ξ̄_t - H_t^T Q_t^{-1} h(μ̄_t) + H_t^T Q_t^{-1} H_t μ̄_t
```

#### Additivity and the SLAM Connection

The measurement update $\Omega_t = \bar\Omega_t + C_t^T Q_t^{-1} C_t$ adds one term to $\Omega$ for each measurement. Independent measurements from several robots can likewise be combined as $\Omega_{\text{total}} = \sum_i \Omega_i$ and $\xi_{\text{total}} = \sum_i \xi_i$.

This additivity generalizes in §3.6 factor graphs to "one measurement factor = one term $H^T Q^{-1} H$ and one term $H^T Q^{-1} z$ added." What makes the information matrix sparse, however, is not additivity itself but locality: each factor connects only a handful of variables. Few non-zero columns in $H$ mean that the blocks being summed are themselves sparse. Whether sparse Cholesky actually preserves that sparsity is settled by the fill-in the elimination order produces.

EIF-SLAM and SEIF use this information-form additivity in their SLAM representations. Ch.14 §14.16 traces the historical connection.

> **Further reading**
> - [Thrun, Burgard, Fox — Probabilistic Robotics (2005)](https://www.probabilistic-robotics.org/) — Ch.3 is the primary source. KF, EKF, and IF derivations appear side by side.
> - [Cyrill Stachniss — Kalman Filter and EKF Lectures](https://www.youtube.com/watch?v=PiCC-SxWlH8) — Freiburg lectures. Strong visual explanations.
> - [Welch & Bishop — An Introduction to the Kalman Filter (2006)](https://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf) — A standard KF introduction. Derivation and intuition are balanced throughout.

---

## 3.11 Advanced: Nonparametric Filters

Source: Thrun, Burgard, Fox (2005) *Probabilistic Robotics*, Ch.4 (Nonparametric Filters).

The Gaussian filters of §3.10 compress belief into two numbers $(\mu, \Sigma)$, but at the cost of being unable to handle nonlinearity or multi-modal distributions properly. Nonparametric filters lift this restriction and can represent arbitrary distributions. The price is computational cost.

### 3.11.1 Histogram Filter / Discrete Bayes Filter

#### Finite State Spaces: Integral Becomes Summation

When the Bayes filter integral of §3.9.5 cannot be solved in closed form, the most direct escape is to make the state space finite. If the state takes only $K$ discrete values $\{x_1, x_2, \ldots, x_K\}$, the integral of §3.9.5 becomes a summation:

$$\bar p_{k,t} = \sum_i p(x_k \mid u_t, x_i)\, p_{i,t-1} \quad \text{(prediction)}$$
$$p_{k,t} = \eta\, p(z_t \mid x_k)\, \bar p_{k,t} \quad \text{(correction)}$$

```
# Algorithm Discrete_Bayes_filter (Table 4.1, adapted)
# Input:  {p_{k,t-1}}, u_t, z_t
# Output: {p_{k,t}}

for all k do
    # prediction: sum transitions from all prior states to x_k
    p̄_{k,t} = Σ_i p(X_t = x_k | u_t, X_{t-1} = x_i) · p_{i,t-1}

# correction: weight by measurement likelihood
    p_{k,t} = η · p(z_t | X_t = x_k) · p̄_{k,t}
endfor
return {p_{k,t}}
```

This algorithm has the same structure as the HMM forward algorithm in speech recognition. For problems where the state space is naturally discrete (door open/closed, semantic class) it remains the shortest path.

#### Continuous State: Histogram Filter

Partition the continuous state space into a finite collection of regions $\{\mathbf{x}_{k,t}\}$ and assume belief is piecewise uniform within each region:

$$p(x_t) = \frac{p_{k,t}}{|\mathbf{x}_{k,t}|} \quad x_t \in \mathbf{x}_{k,t}$$

Approximate the model using a representative value (mean state) $\hat x_{k,t}$ per region:

$$p(z_t \mid \mathbf{x}_{k,t}) \approx p(z_t \mid \hat x_{k,t})$$
$$p(\mathbf{x}_{k,t} \mid u_t, \mathbf{x}_{i,t-1}) \approx \eta\,|\mathbf{x}_{k,t}|\, p(\hat x_{k,t} \mid u_t, \hat x_{i,t-1})$$

When all regions have equal size, the $|\mathbf{x}_{k,t}|$ factors are absorbed into normalization. The resulting discrete Bayes filter is called a **histogram filter**.

**Limitations:** The curse of dimensionality makes it impractical above five dimensions or so. It is unsuitable for 6-DoF pose estimation. Decomposition approaches include density trees (non-uniform partitioning by state density), selective updating (updating only regions where change occurred), and mixed topological-metric representations.

Occupancy grid mapping in Ch.14 is the direct descendant of the histogram filter.

### 3.11.2 Binary Bayes Filter (Log-Odds Form)

#### Binary Estimation of a Static State

When estimating a binary state that does not change over time (e.g., "is this cell occupied?"), there is no state-transition model and the prediction step disappears. Only the correction step repeats.

Directly computing the posterior $p(x \mid z_{1:t})$ at every measurement risks numerical underflow as likelihood products accumulate, and the $[0, 1]$ clamping also needs handling. **Log-odds representation** solves both.

$$l(x) := \log \frac{p(x)}{1 - p(x)} \in (-\infty, +\infty)$$

Log-odds has the entire real line as its range, so clamping is not an issue. The multiplicative Bayes update becomes **additive**:

$$l_t = l_{t-1} + \log\frac{p(x \mid z_t)}{1 - p(x \mid z_t)} - l_0$$

Here $l_0 = \log\frac{p(x)}{1-p(x)}$ is the prior log-odds.

Recovering belief: $\text{bel}_t(x) = 1 - \dfrac{1}{1 + \exp(l_t)}$

```
# Algorithm Binary_Bayes_filter (Table 4.2, adapted)
# Input:  l_{t-1}, z_t
# Output: l_t
# (static state assumed: no prediction step)

1: l_t = l_{t-1}
         + log( p(x|z_t) / (1 - p(x|z_t)) )   # incorporate measurement via inverse sensor model
         - log( p(x) / (1 - p(x)) )            # subtract prior (avoid double-counting)
return l_t
```

#### Inverse Sensor Model

The **inverse sensor model** $p(x \mid z)$ is the reverse of the forward measurement model $p(z \mid x)$. When the camera "sees an open door," the probability that a cell is empty is an example of an inverse model that can be easier to specify than the forward direction. The binary Bayes filter takes this inverse model directly as input.

The log-odds update equation is applied cell by cell in Ch.14 Occupancy Grid Mapping.

### 3.11.3 Particle Filter

#### Principle of Nonparametric Representation

The histogram filter's grid grows exponentially with dimension. The particle filter sidesteps this by approximating the distribution with samples instead of a grid.

The particle filter represents belief with $M$ random samples (particles):

$$\mathcal{X}_t = \{x_t^{[1]},\, x_t^{[2]},\, \ldots,\, x_t^{[M]}\}$$

The particles $x_t^{[m]}$ are more densely concentrated where belief is high. Without any Gaussian assumption, arbitrary distribution shapes — multi-modal, heavy-tailed — can be represented.

#### Particle Filter Algorithm: Sampling, Weighting, Resampling

```
# Algorithm Particle_filter (Table 4.3, adapted)
# Input:  X_{t-1}, u_t, z_t
# Output: X_t (M particles)

X̄_t = X_t = ∅
for m = 1 to M do
    # Step 1: sampling — propagate each particle through the motion model
    x_t^[m] ~ p(x_t | u_t, x_{t-1}^[m])

# Step 2: weighting — compute importance weight via measurement likelihood
    w_t^[m] = p(z_t | x_t^[m])

X̄_t = X̄_t ∪ {x_t^[m], w_t^[m]}
endfor

for m = 1 to M do
    # Step 3: resampling — draw M new particles proportional to weights
    draw i with probability ∝ w_t^[i] from X̄_t
    add x_t^[i] to X_t
endfor
return X_t
```

In the limit $M \to \infty$, $x_t^{[m]} \sim p(x_t \mid z_{1:t}, u_{1:t})$.

#### Importance Sampling Intuition

When direct sampling from a target distribution $f$ is difficult, draw from a proposal distribution $g$ and correct with weights $w = f/g$:

$$w^{[m]} = \frac{f(x^{[m]})}{g(x^{[m]})}$$

The weighted empirical distribution converges, for any Borel set $A$:
$$\left[\sum_{m=1}^M w^{[m]}\right]^{-1} \sum_{m=1}^M \mathbf{1}(x^{[m]} \in A)\, w^{[m]} \;\longrightarrow\; \int_A f(x)\, dx$$

The convergence rate is $O(1/\sqrt{M})$. The closer the proposal is to the target, the smaller the constant.

In the particle filter, the proposal propagates each particle through the motion model $p(x_t \mid u_t, x_{t-1})$, and the target is $\text{bel}(x_t)$ which also incorporates the measurement. The "missing information" between a proposal that has not seen $z_t$ and a target that has is $p(z_t \mid x_t^{[m]})$, which gives the Step 2 weights their intuitive justification.

Why this falls out to exactly $p(z_t \mid x_t^{[m]})$ becomes clear when the argument is made rigorously in sequence space.

#### Convergence and Implementation

The rigorous derivation works by treating each particle not as a single state $x_t^{[m]}$ but as a state sequence $x_{0:t}^{[m]}$. Two applications of Bayes plus the Markov property factorize the target; the proposal factorizes inductively; their ratio reduces to exactly $\eta\, p(z_t \mid x_t^{[m]})$. This holds exactly only as $M \to \infty$.

Without resampling, weights concentrate on a small number of particles — **weight degeneracy** — which is why Step 3 is needed. §3.11.4 covers the sources of error in particle filters.

### 3.11.4 Four Sources of Error in Particle Filters

The particle filter is an approximation and carries structural errors. Resampling is a weight-based selection that makes low-weight particles less likely to be selected. Understanding the four error sources makes particle filter debugging systematic.

#### (1) Systematic Bias from Finite $M$

Imagine $M = 1$. The single weight normalizes against itself: $w/w = 1$. The measurement is completely ignored. With finite $M$, weights are confined to the $M-1$-dimensional simplex and random errors accumulate. Bias decreases as $M$ grows, but computational cost grows linearly.

#### (2) Sample Impoverishment from Resampling

In a static state ($x_t = x_{t-1}$) with no transition noise, each particle simply holds on to its own initial state. The problem is what happens next. With no new diversity injected, repeated resampling thins out the variety of surviving particles through sampling variance alone (sample impoverishment), until the filter collapses to a single state.

Mitigation: hold resampling when the robot is stationary. Alternatively, resample only when weight variance is high and otherwise accumulate weights multiplicatively:

$$w_t^{[m]} = \begin{cases} 1 & \text{(immediately after resampling)} \\ p(z_t \mid x_t^{[m]})\, w_{t-1}^{[m]} & \text{(when not resampling)} \end{cases}$$

#### (3) Proposal-Target Divergence

When sensors are very accurate but motion is imprecise, the target belief is narrow while the proposal is wide, and efficiency collapses. In the extreme, a noiseless range sensor would confine the support of $p(z \mid x)$ to a low-dimensional manifold, leaving most particles with weight $\approx 0$.

Mitigation: deliberately inflate measurement noise (at the cost of some precision) or use a measurement-aware proposal that incorporates measurement information at the sampling stage.

#### (4) Particle Deprivation

In a high-dimensional space, there may be no particle near the true state. The randomness of resampling has a nonzero probability every cycle of sweeping out all particles near the true state. Once lost, they are hard to recover.

Mitigation: inject a small number of **random particles** drawn from the prior every cycle. This slightly distorts the posterior but prevents catastrophic failure.

#### Low-Variance Sampler

The standard resampling implementation is the **low-variance (systematic) sampler**. A single random number draws $M$ particles at regular intervals, achieving $O(M)$ complexity.

```
# Algorithm Low_variance_sampler (Table 4.4, adapted)
# Input:  X̄_t (weighted particles), W_t (weight array)
# Output: X̄_t (resampled particles)

r = rand(0, M^{-1})    # single uniform random number in [0, 1/M)
c = w_t^[1]             # cumulative weight
i = 1
X̄_t = ∅
for m = 1 to M do
    u = r + (m-1) · M^{-1}   # advance at regular intervals
    while u > c do
        i = i + 1
        c = c + w_t^[i]       # accumulate weight
    endwhile
    add x_t^[i] to X̄_t       # select particle at this position
endfor
return X̄_t
```

When all weights are equal, the output is identical to the input — no particles are lost in steps with no measurement. $O(M)$ versus $O(M \log M)$ for independent sampling.

With this, both why the particle filter works and where it breaks down are clear. Each of the four error sources has a mitigation, and choosing the right tradeoff for the situation is the core of practical implementation. The four errors are addressed by augmented MCL and mixture MCL in Ch.14 §14.7.

> **Further reading**
> - [Thrun, Burgard, Fox — Probabilistic Robotics (2005)](https://www.probabilistic-robotics.org/) — Ch.4 is the primary source. Algorithms and analysis for histogram, binary Bayes, and particle filters are covered completely.
> - [Arulampalam et al. — A Tutorial on Particle Filters (IEEE Trans. Signal Processing 2002)](https://ieeexplore.ieee.org/document/978374) — A standard tutorial on particle filter theory and applications.
> - [Thrun — Particle Filters in Robotics (UAI 2002)](https://www.aaai.org/Papers/UAI/2002/UAI02-079.pdf) — A short paper explaining the connection to Rao-Blackwellized PF and FastSLAM.
> - [ROS AMCL package](https://wiki.ros.org/amcl) — The theory of §3.11.3–3.11.4 implemented in a production package. Augmented MCL and the low-variance sampler are applied directly.

---

The Bayes filter is the framework. The KF, EKF, and IF are closed-form implementations of that framework under the Gaussian assumption. The histogram filter and particle filter buy flexibility without the Gaussian assumption, at the cost of computation. Which filter to choose is determined by state-space dimension and whether the distribution is multi-modal.

When you encounter EKF localization (§14.7), MCL (§14.7), and IMU preintegration (§14.10) in Ch.14, there is no need to follow each algorithm's derivation from scratch. The filter vocabulary built here makes it possible to identify what $g$, $h$, and the proposal are in each algorithm, and the skeleton becomes immediately visible.

---

# Ch.4 — Kinematics & Mechatronics

Place a single robot arm on a desk. What angle must each of the six motors take so that the fingertip reaches a coffee cup? Kinematics is the discipline that answers this question. And the real-world problem of actually spinning those motors, reading sensors, and running a control loop at 1kHz is mechatronics.

The equations connect to hardware selection and communication protocols, and eventually to motion on a physical robot.

---

## 4.1 Why Study Kinematics

A robot manipulator is built from multiple joints and links. What we want is the position and pose of the end-effector. What we directly control, however, is the angle (or displacement) of each joint.

The mathematical description of the relationship between these two is **kinematics**.

- **Forward Kinematics (FK)**: joint angles → end-effector position/pose
- **Inverse Kinematics (IK)**: end-effector position/pose → joint angles

This is different from dynamics. Kinematics does not consider forces and masses. It is the question of "where is it," not "what force is required." Dynamics is the subject of the next chapter.

Kinematics is used in the following tasks:
- Robot arm path planning (motion planning)
- Teleoperation (master-slave mapping in remote control)
- Calibration (correcting errors between the real robot and the model)
- Collision avoidance (you must know where each link sits in space to avoid it)

---

## 4.2 Forward Kinematics

### 4.2.1 Homogeneous Transformation Matrix

The basic tool of kinematics is the 4×4 homogeneous transformation matrix:

```
T = | R  p |
    | 0  1 |
```

Here R is a 3×3 rotation matrix and p is a 3×1 position vector. A homogeneous transformation matrix represents a rigid body's position and orientation together, and matrix multiplication chains multiple transformations.

Given a transformation T_01 between two frames and another transformation T_12:

```
T_02 = T_01 * T_12
```

Forward kinematics applies this composition from the base to the end-effector, multiplying each joint transformation in order.

### 4.2.2 DH Parameters (Denavit-Hartenberg)

A convention proposed in 1955 by Denavit and Hartenberg. It remains widely used in robot kinematics, with four parameters defining the relationship between adjacent links:

| Parameter | Meaning |
|---------|------|
| **a_i** (link length) | distance from z_{i-1} to z_i along the x_i axis |
| **α_i** (link twist) | rotation angle from z_{i-1} to z_i about the x_i axis |
| **d_i** (link offset) | distance from x_{i-1} to x_i along the z_{i-1} axis |
| **θ_i** (joint angle) | rotation angle from x_{i-1} to x_i about the z_{i-1} axis |

For a revolute joint, θ_i is the variable and the other three are constants.
For a prismatic joint, d_i is the variable.

The transformation matrix for each joint:

```
T_i = Rot_z(θ_i) * Trans_z(d_i) * Trans_x(a_i) * Rot_x(α_i)

Caveat: the DH convention comes in two flavors — "standard" and "modified (Craig convention)." If you use Craig's textbook you will see modified DH; many other texts use standard DH. The two differ in how frames are attached. Mixing them yields wrong results, so always state which convention you are using.

### 4.2.3 Example: FK of a 2-link Planar Arm

Start with the simplest example. A 2-link robot arm in the plane.

```
       q1         q2
  O────────O────────O → end-effector
  (base)   L1       L2
```

DH table (standard convention):

| Link | a    | α   | d   | θ    |
|------|------|-----|-----|------|
| 1    | L1   | 0   | 0   | θ_1  |
| 2    | L2   | 0   | 0   | θ_2  |

The end-effector position follows simply from trigonometry:

```
x = L1*cos(θ_1) + L2*cos(θ_1 + θ_2)
y = L1*sin(θ_1) + L2*sin(θ_1 + θ_2)
```

Implemented in Python:

```python
import numpy as np

def fk_2link(theta1, theta2, L1=1.0, L2=1.0):
    """Forward kinematics of a 2-link planar arm."""
    x = L1 * np.cos(theta1) + L2 * np.cos(theta1 + theta2)
    y = L1 * np.sin(theta1) + L2 * np.sin(theta1 + theta2)
    phi = theta1 + theta2  # absolute orientation of the end-effector
    return x, y, phi

# θ_1=30°, θ_2=45°, link lengths of 1m each
x, y, phi = fk_2link(np.radians(30), np.radians(45))
print(f"End-effector position: ({x:.3f}, {y:.3f}), orientation: {np.degrees(phi):.1f}°")
# Output: End-effector position: (0.259, 1.366), orientation: 75.0°
```

If this looks overly simple, that is normal. The FK of a real 6-axis arm works on the same principle — it just multiplies six 4×4 matrices.

### 4.2.4 Product of Exponentials (PoE)

As an alternative to DH parameters, there is the PoE (Product of Exponentials) method, based on Lie group / Lie algebra. This is the method adopted in Lynch & Park's "Modern Robotics."

PoE represents each joint as a twist (screw motion) and computes the transformation via the matrix exponential.

```
T(θ) = e^{[S_1]θ_1} * e^{[S_2]θ_2} * ... * e^{[S_n]θ_n} * M
```

Where:
- S_i is the screw axis of the i-th joint (6×1 vector)
- [S_i] is the 4×4 matrix representation of S_i (an element of se(3), with a skew-symmetric upper-left 3×3 rotation block)
- M is the end-effector pose when all joints are at the zero (home) configuration
- θ_i is the joint variable

**DH vs PoE comparison:**

| Item | DH | PoE |
|------|-----|-----|
| Frame attachment | a frame needed on each link | only the base frame and end-effector frame needed |
| Convention confusion | beware standard vs modified | none (though space form vs body form exists) |
| Mathematical basis | matrix multiplication | Lie group, matrix exponential |
| Singularity analysis | requires separate treatment | naturally integrated |
| Industry adoption | very high | academia-centered, spreading |
| Textbook | Craig, Siciliano | Lynch & Park |

DH parameters remain common in textbooks and industrial robot manuals, while URDF encodes the equivalent link and joint transformations directly. PoE provides a cleaner Lie-group formulation and is widely used in research. Familiarity with both conventions makes it easier to move between manuals, robot descriptions, and derivations.

```python
# Example of DH-based FK with robotics-toolbox-python (Puma 560)
import roboticstoolbox as rtb

puma = rtb.models.DH.Puma560()
q = [0, -np.pi/4, np.pi/4, 0, np.pi/6, 0]  # six joint angles
T = puma.fkine(q)
print(T)  # print the 4x4 SE(3) homogeneous transformation matrix
print(f"Position: {T.t}")  # end-effector position
print(f"RPY angles: {T.rpy()}")  # Roll-Pitch-Yaw
```

> **Further reading**
> - Lynch & Park, *Modern Robotics*, Chapter 4 — a PoE-centered treatment with a free PDF and Coursera course: https://modernrobotics.org
> - Craig, *Introduction to Robotics*, Chapter 3 — a textbook treatment using the Modified DH convention.
> - Peter Corke, *Robotics, Vision and Control* — lets you practice FK together with Python code: https://github.com/petercorke/robotics-toolbox-python

---

## 4.3 Inverse Kinematics

FK is easy. Matrix multiplication suffices. The problem is IK.

"I want to place the end-effector at (x, y, z). What must each joint angle be?"

Why this problem is hard:
1. **Nonlinear equations** — trigonometric functions are tangled together
2. **Multiple solutions** — several combinations of joint angles may reach the same end-effector position (elbow-up, elbow-down, etc.)
3. **No solution may exist** — points outside the workspace are unreachable
4. **Infinitely many solutions** — if degrees of freedom remain (a redundant manipulator), the number of solutions is infinite

### 4.3.1 Analytical IK

This method derives a closed-form solution. When one exists, candidate solutions can be computed without iterative optimization, but numerical accuracy and runtime still depend on the implementation and singularity handling.

**IK of a 2-link planar arm:**

Given a target position (x, y):

```
cos(θ_2) = (x² + y² - L1² - L2²) / (2 * L1 * L2)
θ_2 = atan2(±√(1 - cos²(θ_2)), cos(θ_2))

θ_1 = atan2(y, x) - atan2(L2*sin(θ_2), L1 + L2*cos(θ_2))
```

The ± sign reveals two solutions (elbow-up and elbow-down). The existence of multiple solutions is one source of difficulty in IK.

```python
def ik_2link(x, y, L1=1.0, L2=1.0, elbow_up=True):
    """Inverse kinematics of a 2-link planar arm. Returns None if no solution."""
    d_sq = x**2 + y**2
    # check reachability
    if d_sq > (L1 + L2)**2 or d_sq < (L1 - L2)**2:
        return None

cos_q2 = (d_sq - L1**2 - L2**2) / (2 * L1 * L2)
    cos_q2 = np.clip(cos_q2, -1.0, 1.0)  # numerical safety

if elbow_up:
        q2 = np.arctan2(np.sqrt(1 - cos_q2**2), cos_q2)
    else:
        q2 = np.arctan2(-np.sqrt(1 - cos_q2**2), cos_q2)

q1 = np.arctan2(y, x) - np.arctan2(L2 * np.sin(q2), L1 + L2 * np.cos(q2))
    return q1, q2

# Verify: FK → IK → FK
target_x, target_y = 1.2, 0.8
result = ik_2link(target_x, target_y)
if result:
    q1, q2 = result
    x_check, y_check, _ = fk_2link(q1, q2)
    print(f"Target: ({target_x}, {target_y})")
    print(f"IK solution: q1={np.degrees(q1):.2f}°, q2={np.degrees(q2):.2f}°")
    print(f"FK check: ({x_check:.6f}, {y_check:.6f})")
    print(f"Error: {np.sqrt((x_check-target_x)**2 + (y_check-target_y)**2):.2e}")
```

**Analytical IK of a 6R manipulator:**

Among 6-axis robots, those satisfying Pieper's condition — where the last three axes meet at a single point (a spherical wrist) — can be solved analytically. Traditional industrial 6-axis robots (KUKA, ABB, etc.) have this structure. UR arms have an offset wrist whose three wrist axes do not meet at a single point, so even though a closed-form solution exists, it requires a separate derivation.

In this case the position problem (first three axes) and the orientation problem (last three axes) can be decoupled and solved. Up to eight solutions exist, and it is common to pick the one that respects joint limits and stays close to the previous joint angles.

### 4.3.2 Numerical IK

When an analytical solution is not available (complex structures, 7 or more axes, non-standard structures), one must solve it numerically. This is an iterative optimization problem.

**Jacobian pseudo-inverse method:**

```
Δq = J†(q) * Δx
```

Here J† is the pseudo-inverse of the Jacobian. Iterating this converges to the target.

```python
def numerical_ik_2link(target_x, target_y, L1=1.0, L2=1.0,
                        max_iter=100, tol=1e-6):
    """Numerical IK based on the Jacobian pseudo-inverse."""
    # initial guess (random or current joint angles)
    q = np.array([0.5, 0.5])

for i in range(max_iter):
        # current FK
        x = L1 * np.cos(q[0]) + L2 * np.cos(q[0] + q[1])
        y = L1 * np.sin(q[0]) + L2 * np.sin(q[0] + q[1])

# error
        error = np.array([target_x - x, target_y - y])
        if np.linalg.norm(error) < tol:
            print(f"Converged: {i+1} iterations")
            return q

# Jacobian
        J = np.array([
            [-L1*np.sin(q[0]) - L2*np.sin(q[0]+q[1]), -L2*np.sin(q[0]+q[1])],
            [ L1*np.cos(q[0]) + L2*np.cos(q[0]+q[1]),  L2*np.cos(q[0]+q[1])]
        ])

# update joint angles via pseudo-inverse
        dq = np.linalg.pinv(J) @ error
        q += dq

print("Failed to converge")
    return q
```

**Damped Least Squares (DLS, Levenberg-Marquardt):**

The problem with the pseudo-inverse is that joint velocities blow up near singularities. DLS mitigates this by adding a damping factor λ:

```
Δq = J^T (J * J^T + λ²I)^{-1} * Δx
```

Large λ is stable near singularities but slow to converge; small λ approaches the pseudo-inverse. Adaptive adjustment of λ (Nakamura & Hanafusa, 1986) is widely used in practice.

### 4.3.3 Singularity

A joint configuration where the rank of the Jacobian drops is called a singularity. At a singularity:

1. **The end-effector cannot move in a particular direction** — loss of a degree of freedom
2. **Joint velocities go to infinity for infinitesimal motion** — real motors cannot follow
3. **IK solutions are discontinuous** — abrupt joint jumps during path following

Singularities of the 2-link arm are simple: θ_2 = 0 (arm fully extended) or θ_2 = π (fully folded). Here the end-effector can only move in the radial direction; no tangential velocity is achievable.

Representative singularities of 6-axis robots:
- **Wrist singularity**: axes 4 and 6 are aligned (q5 ≈ 0)
- **Shoulder singularity**: the end-effector lies on axis 1
- **Elbow singularity**: the arm is fully extended

Practical countermeasures:
- Path planning that avoids the neighborhood of singularities
- Velocity limiting while passing through singularities with the DLS method
- Use of redundancy (extra degrees of freedom)

### 4.3.4 IK Solvers

It is rare to implement IK from scratch. Using a proven solver is the sensible choice.

| Solver | Method | Notes |
|------|------|------|
| **KDL** | Numerical (Newton-Raphson) | available in the ROS ecosystem; sensitive to joint limits, initialization, and singularities |
| **IKFast** (OpenRAVE) | Analytical (code generation) | auto-generates C++ code for specific structures. Fast |
| **TRAC-IK** | KDL + SQP dual | higher solve rate than stock KDL on the paper's tested chains; ROS package available |
| **MoveIt2 IK** | Integrates the solvers above | ROS2 ecosystem, integrated collision avoidance |
| **pinocchio** | Jacobian-based numerical iteration (CLIK) | rigid-body dynamics library, fast, provides analytical derivatives |

```python
# Beeson & Ames (2015) tested 10,000 reachable poses per robot model
# on five models, with a 5 ms limit per solve.
# In that experiment TRAC-IK had a higher solve rate than stock KDL,
# but the rate varied with the chain, seed, and error tolerance.
```

> **Further reading**
> - [Beeson & Ames, "TRAC-IK: An Open-Source Library for Improved Solving of Generic Inverse Kinematics" (2015)](https://doi.org/10.1109/HUMANOIDS.2015.7363472) — see the paper's tables for per-model conditions and solve rates
> - MoveIt2 IK documentation: https://moveit.picknik.ai/main/doc/concepts/inverse_kinematics.html
> - Pinocchio (rigid body dynamics library): https://github.com/stack-of-tasks/pinocchio

---

## 4.4 Jacobian

The Jacobian is one of the most heavily used tools in kinematics. If FK is the problem of "position," the Jacobian is the problem of "velocity."

### 4.4.1 Joint Velocity → End-Effector Velocity

The relationship between end-effector velocity (linear v, angular ω) and joint velocity q̇:

```
ẋ = J(q) * q̇

where ẋ = [v; ω] ∈ ℝ^6 (for a 6-axis case)
      q̇ ∈ ℝ^n
      J(q) ∈ ℝ^{6×n}
```

For a six-dimensional end-effector task, n < 6 provides too few joint degrees of freedom, n = 6 matches the task dimension, and n > 6 provides redundant degrees of freedom. The available motion depends on the rank of J(q), while underactuation compares the number of independent control inputs with the system degrees of freedom.

### 4.4.2 Force/Torque Relation (Duality)

The transpose of the Jacobian maps end-effector force to joint torque:

```
τ = J^T(q) * F
```

Here τ is joint torque and F is the force/moment acting on the end-effector.

This is **static duality**. Velocity and force are dual through the Jacobian and its transpose. It follows naturally from the principle of power conservation:

```
P = F^T * ẋ = F^T * J * q̇ = (J^T * F)^T * q̇ = τ^T * q̇
```

This relation is central to force control. To apply a desired force F at the end-effector, apply joint torques τ = J^T * F.

### 4.4.3 Manipulability Ellipsoid

The Jacobian also tells how "well" the robot can move at its current configuration.

```
manipulability index = √det(J * J^T)
```

For a robot whose J normally has full row rank for the six-dimensional task, a zero value indicates a singularity. A larger value means a larger manipulability ellipsoid volume, while uniformity across directions depends on the ratios of its axis lengths.

The eigenvalues and eigenvectors of J * J^T trace out an ellipsoid. Large eigenvalues mean fast motion in that direction; small ones mean slow. If the eigenvalues are all similar the motion is isotropic; if they differ greatly it is anisotropic.

```python
import roboticstoolbox as rtb
import numpy as np

# Jacobian and manipulability of the Puma 560
puma = rtb.models.DH.Puma560()
q = [0, -np.pi/4, np.pi/4, 0, np.pi/6, 0]

J = puma.jacob0(q)  # 6x6 Jacobian (in the base frame)

# Manipulability index
m = np.sqrt(np.linalg.det(J @ J.T))
print(f"Manipulability index: {m:.4f}")

# Principal axes of the velocity ellipsoid (eigenvalue analysis)
JJT = J[:3, :] @ J[:3, :].T  # linear-velocity part only
eigenvalues, eigenvectors = np.linalg.eigh(JJT)
print(f"Velocity ellipsoid semi-axes: {np.sqrt(eigenvalues)}")

# Condition number: isotropy indicator (closer to 1 is better)
sigma = np.linalg.svd(J, compute_uv=False)
cond = sigma[0] / sigma[-1]
print(f"Condition number: {cond:.2f}")
# cond = 1 is perfect isotropy; infinite means a singularity
```

### 4.4.4 Practical Code: Jacobian-Based Velocity Control

```python
import numpy as np

def jacobian_velocity_control(robot_fk, robot_jacob, q_current,
                               desired_twist, dt=0.001):
    """
    Jacobian-based resolved rate control.

Args:
        robot_fk: FK function (q -> SE3)
        robot_jacob: Jacobian function (q -> 6xn matrix)
        q_current: current joint angles
        desired_twist: desired end-effector velocity [vx, vy, vz, wx, wy, wz]
        dt: control period
    Returns:
        q_new: new joint angles
    """
    J = robot_jacob(q_current)

# Damped least squares
    lambda_dls = 0.01
    n = J.shape[1]
    JJT = J @ J.T
    J_dls = J.T @ np.linalg.inv(JJT + lambda_dls**2 * np.eye(JJT.shape[0]))

q_dot = J_dls @ desired_twist

# joint velocity limits (essential on a real robot)
    max_qdot = 2.0  # rad/s
    scale = np.max(np.abs(q_dot)) / max_qdot
    if scale > 1.0:
        q_dot /= scale

q_new = q_current + q_dot * dt
    return q_new
```

> **Further reading**
> - Siciliano et al., *Robotics: Modelling, Planning and Control*, Chapter 3 — a broad treatment of Jacobians in kinematics and dynamics
> - Corke, *Robotics, Vision and Control*, Chapter 8 — includes code examples and visualization: https://petercorke.com/rvc/
> - robotics-toolbox-python documentation: https://github.com/petercorke/robotics-toolbox-python

---

## 4.5 Mechatronics Basics

Specifying joint angles does not make the robot move. It also needs motors, sensors, and the electronics and communication that connect them. This is mechatronics.

### 4.5.1 Actuators

**DC motor:**
The most basic actuator. Apply a voltage and it spins. Torque is proportional to current (τ = K_t * i), and back-EMF is proportional to speed (V_emf = K_e * ω). Easy to control and cheap, but the brushes wear.

**BLDC (Brushless DC) motor:**
Switches current electronically without brushes. Long life, high torque density, and good efficiency make it a common choice in modern robots. FOC (Field-Oriented Control) is used to reduce torque ripple.

**Servo motors (Dynamixel series):**
A product that bundles motor + reducer + encoder + controller into a single unit. Robotis's Dynamixel is a widely used servo family on research and educational platforms.

| Model | Example advertised maximum torque (Nm) | Communication | Use |
|------|-----------|------|------|
| XL330 | 0.5 | TTL | small grippers, small low-cost arms |
| XM540 | 10.0 | RS-485 | mid-sized robot arms |
| PH54  | 44.7 | RS-485 | large manipulators, mobile robots |

Torque in the table depends on model and supply voltage, so actual selection must use each e-Manual's rated or stall conditions and continuous-duty limits. Dynamixel strengths include daisy-chain wiring, position/velocity/current-based control modes, and adjustable PID gains. Its communication, control-cycle, and thermal limits are product-specific; verify that the required bandwidth and control mode are supported by the stock firmware.

**Quasi-Direct Drive (QDD):**

The approach drew attention with the MIT Mini Cheetah (2019) and uses a **lower gear ratio**.

Typical robot joint: gear ratio of 100:1 or higher (harmonic drive)
QDD: gear ratio of 6:1 to 10:1 (planetary gears or belt)

Advantages of a low gear ratio:
- **High backdrivability**: the joint yields more readily under external force, which can simplify collision-response and force-control design.
- **High transparency**: with a suitable friction model, joint torque can be approximated from motor current more readily.
- **Potential for high bandwidth**: lower reducer friction and compliance leave more room for a fast torque response.

Drawbacks: lower output torque for the same size. For large torques, you must use a larger motor.

Recent systems using QDD:
- MIT Mini Cheetah / Cheetah 3
- Unitree robot series

```
# Torque-control comparison: QDD vs traditional reducer
#
# Traditional (gear ratio 100:1, harmonic drive):
#   reflected inertia = N² × I_motor
#   → motor inertia 0.001 kg·m² × 100² = 10 kg·m²
#   → motor inertia reflected to the joint output is very large
#   → precise force control is difficult
#
# QDD (gear ratio 8:1):
#   with the same motor, reflected inertia = 8² × 0.001 = 0.064 kg·m²
#   → about 156× smaller in this gear-ratio-only example
#   → actual force-control performance also depends on link inertia, friction, and control
```

**Reducer types:**

| Type | Gear ratio | Backlash | Efficiency | Price | Use |
|------|--------|--------|------|------|------|
| Planetary | 3~100:1 | medium | 85-95% | cheap | general-purpose, suited to QDD |
| Harmonic Drive | 30~320:1 | very low | 65-85% | expensive | industrial robots, precision |
| Cycloidal | 6~120:1 | low | 85-93% | medium | emerging as a recent alternative |

Gear ratio, efficiency, and backlash vary substantially by design and product. Treat these ranges as a starting point and use the manufacturer's rated-load data for selection.

**Actuator selection criteria:**

For a robot joint, combine static torque, dynamic torque, and impact loads, then choose a design margin appropriate to load uncertainty, lifetime, and the consequence of failure. The factor of two in the code below is an illustration, not a universal rule. Convert maximum joint speed to motor RPM through the gear ratio, and evaluate backdrivability, package mass, continuous torque, and thermal limits together. The choice between QDD and a harmonic drive depends on torque density, transparency, precision, and cost requirements.

```python
# Simple actuator-sizing example
import numpy as np

# Goal: lift a 1 kg object at the arm tip (arm length 0.5 m)
m_payload = 1.0  # kg
m_link = 0.5     # weight of the link itself
L = 0.5          # m
g = 9.81         # m/s²

# Worst-case torque (arm fully horizontal)
tau_static = (m_payload * L + m_link * L/2) * g
print(f"Static torque: {tau_static:.2f} Nm")

# Acceleration torque (max angular acceleration 10 rad/s²)
alpha_max = 10.0  # rad/s²
I_total = m_payload * L**2 + m_link * (L/2)**2  # moment of inertia (simplified)
tau_dynamic = I_total * alpha_max
print(f"Dynamic torque: {tau_dynamic:.2f} Nm")

# Total required torque (safety factor 2)
tau_required = (tau_static + tau_dynamic) * 2.0
print(f"Required torque (safety factor 2): {tau_required:.2f} Nm")

# Max angular velocity → motor RPM
omega_max = 3.0  # rad/s (joint)
gear_ratio = 8    # QDD
motor_rpm = omega_max * gear_ratio * 60 / (2 * np.pi)
print(f"Required motor RPM: {motor_rpm:.0f}")
```

> **Further reading**
> - Katz, "A Low Cost Modular Actuator for Dynamic Robots" (MIT, 2018) — the core QDD paper: https://dspace.mit.edu/handle/1721.1/118671
> - Dynamixel product lineup and documentation: https://emanual.robotis.com/
> - Seok et al., "Design Principles for Energy-Efficient Legged Locomotion and Implementation on the MIT Cheetah Robot" (2015)

### 4.5.2 Sensor Interfacing

**Encoder:**

An encoder is the most basic sensor for measuring joint angle.

*Incremental encoder*: counts pulses on two channels (A, B) to measure relative rotation. Loses position when power is cut (requires homing). Cheap, with high resolution (10,000 PPR or higher is common).

*Absolute encoder*: outputs the current position as an absolute value. Knows its position the moment power is applied. Multi-turn absolute encoders remember multiple revolutions. They cost more but avoid homing, so they are widely used on industrial robots that must recover position after a restart.

```
Resolution example:
  Incremental encoder, 4096 PPR, quadrature decoding (x4)
  → resolution = 360° / (4096 × 4) = 0.022° ≈ 0.38 mrad
  → at a 100:1 reduced joint → output resolution 0.0038 mrad
```

**Torque sensors:**

Directly measure joint torque or end-effector force. Most are based on strain gauges.

*Joint Torque Sensor (JTS)*: mounted on the output side of the reducer. The KUKA LBR iiwa set the benchmark for force control by fitting a JTS to all seven joints.

*Force/Torque sensor (F/T sensor)*: mounted at the end-effector to measure six axes (Fx, Fy, Fz, Tx, Ty, Tz). ATI Industrial Automation and other vendors supply research sensors; select one by measurement range, resolution, overload limit, interface, and a current quotation.

**Inertial sensors (IMU):**

Already covered in Ch.2, so only briefly noted here. Accelerometer + gyroscope + (magnetometer). Used for body-pose estimation in mobile robots and legged robots. In manipulators, per-link IMUs are sometimes used for vibration damping.

### 4.5.3 Communication Protocols

How sensors and actuators connect to a microcontroller or PC. Communication causes more trouble in robot systems than one might expect. Too much latency destabilizes control; too little bandwidth drops data.

**Basic protocols:**

| Protocol | Wiring | Speed | Distance | Notes |
|---------|------|------|------|------|
| **UART** | 2-wire (TX, RX) | ~1 Mbps | ~15m | simplest, 1:1 communication |
| **SPI** | 4-wire (MOSI, MISO, SCK, CS) | ~50 Mbps | ~1m (on-PCB) | fast; multiple slaves need extra CS lines |
| **I2C** | 2-wire (SDA, SCL) | 100k~3.4 Mbps | ~1m | address-based, convenient for sensor buses |

These three are microcontroller-level basics. Robot systems need more robust protocols.

**CAN Bus:**

Originating in the automotive industry, it is also used for motor and sensor networks in robots. Differential signaling makes it noise-resistant, and the multi-master architecture supports priority-based arbitration.

- Speed: up to 1 Mbps (CAN 2.0), 5 Mbps (CAN FD)
- Distance: up to about 1 km (at 50 kbps; about 500 m at 125 kbps, about 40 m at 1 Mbps)
- Topology: bus (daisy-chain possible)

Usage in robots: communication between motor drivers and the main controller. The MIT Cheetah and many legged robots use CAN.

```cpp
// Example of sending a motor command over CAN bus (pseudo-code, STM32 HAL)
#include "can.h"

struct MotorCommand {
    float position;   // rad
    float velocity;   // rad/s
    float torque;     // Nm
    float kp;         // position gain
    float kd;         // velocity gain
};

void send_motor_command(CAN_HandleTypeDef* hcan, uint8_t motor_id,
                        MotorCommand cmd) {
    CAN_TxHeaderTypeDef header;
    header.StdId = motor_id;   // unique CAN ID for each motor
    header.DLC = 8;            // 8 bytes (CAN 2.0 standard)
    header.RTR = CAN_RTR_DATA;

// pack floats as integers (typical for robot motor drivers)
    uint8_t data[8];
    int16_t pos_int = (int16_t)(cmd.position / 0.001f);   // 0.001 rad units
    int16_t vel_int = (int16_t)(cmd.velocity / 0.01f);    // 0.01 rad/s units
    int16_t tau_int = (int16_t)(cmd.torque / 0.01f);      // 0.01 Nm units
    int16_t kp_int  = (int16_t)(cmd.kp / 0.01f);

uint32_t mailbox;
    HAL_CAN_AddTxMessage(hcan, &header, data, &mailbox);
}
```

**EtherCAT:**

An industrial real-time Ethernet protocol. It uses standard Ethernet hardware while providing deterministic communication at the microsecond scale.

Why robots use it:
- **Speed**: 100 Mbps, synchronizing dozens to hundreds of nodes on microsecond cycles
- **Deterministic timing**: constant packet delay → suited to real-time control
- **Processing model**: slaves read and write on-the-fly as the master's frame passes through (processed as the frame flows by). Extremely high bandwidth efficiency.

KUKA, Beckhoff, and many recent research robot platforms use EtherCAT.

Drawbacks: a dedicated master stack is required (SOEM, IgH EtherCAT Master, etc.), and configuration is complicated. It is overkill at the hobbyist level.

**RS-485 / Dynamixel Protocol:**

The communication scheme used by Dynamixel servos. RS-485 is differential-signal serial communication that can reach several Mbps or more over short distances (the 1 Mbps in the example below is the Dynamixel setting), with multiple devices daisy-chained.

```python
# Example of servo control using the Dynamixel SDK
from dynamixel_sdk import *

PROTOCOL_VERSION = 2.0
BAUDRATE = 1000000
DEVICENAME = '/dev/ttyUSB0'
DXL_ID = 1

# open the port
port = PortHandler(DEVICENAME)
packet = PacketHandler(PROTOCOL_VERSION)
port.openPort()
port.setBaudRate(BAUDRATE)

# enable torque
ADDR_TORQUE_ENABLE = 64
packet.write1ByteTxRx(port, DXL_ID, ADDR_TORQUE_ENABLE, 1)

# move to target position (units: 0~4095, 0~360 degrees)
ADDR_GOAL_POSITION = 116
goal_position = 2048  # center (180 degrees)
packet.write4ByteTxRx(port, DXL_ID, ADDR_GOAL_POSITION, goal_position)

# read current position
ADDR_PRESENT_POSITION = 132
pos, _, _ = packet.read4ByteTxRx(port, DXL_ID, ADDR_PRESENT_POSITION)
print(f"Current position: {pos} (= {pos * 360 / 4096:.1f}°)")
```

### 4.5.4 Real-Time Systems

In robot control, "real-time" does not mean "fast" but **"guaranteed to complete within a specified time."** For a 1kHz control loop, every 1ms the sequence of sensor read → control computation → motor command transmission must complete. A single missed deadline can destabilize the robot.

**RTOS (Real-Time Operating System):**

| RTOS | Notes | Use |
|------|------|------|
| **FreeRTOS** | lightweight, for microcontrollers, free | STM32, ESP32, etc. |
| **Zephyr** | modern, broad hardware support, Linux Foundation | IoT, robot embedded |
| **VxWorks** | commercial, used by NASA | aerospace, industrial |

When driving motors directly from a microcontroller, use an RTOS. Set task priorities so the control loop is not pre-empted by other tasks.

**PREEMPT_RT Linux:**

The problem: ROS2 runs on Linux. But a stock Linux kernel is not real-time. The scheduler can interrupt the control thread at any time, and delays of several milliseconds can occur.

The solution: a Linux kernel patched with PREEMPT_RT. Most of the kernel's code paths are made preemptible, delivering performance close to real-time.

Setup overview:
```bash
# 1. Install a kernel with PREEMPT_RT (Debian example)
sudo apt install linux-image-rt-amd64   # Debian metapackage. Ubuntu has no package by this name; it uses a separate path such as the Ubuntu Pro real-time kernel

# 2. Configure GRUB to boot the RT kernel

# 3. Give the control thread real-time priority
sudo chrt -f 99 ./my_robot_controller

# 4. CPU isolation (optional but recommended)
#    add isolcpus=2,3 in /etc/default/grub
#    → isolate CPUs 2, 3 from ordinary processes
#    → pin the control thread to those CPUs (affinity)

# 5. Verify performance
sudo cyclictest -m -p 99 -t 1 -n
# compare worst-case latency with the target control period and timing margin
```

**Choosing a control period:**

1 kHz (1 ms) is a common design point for torque and impedance control, not a universal standard. Choose the period from closed-loop bandwidth, mechanical resonances, sensor and actuator latency, solver time, and jitter margin. Twice the bandwidth in the Nyquist criterion is only an anti-aliasing lower bound; it does not guarantee control performance. In practice, sample sufficiently faster than the target closed-loop bandwidth and verify the frequency response and delay margins. CAN bandwidth cannot be inferred from motor count alone: include frame size, arbitration, bus load, and feedback rate in the calculation.

Some lightweight robots, high-speed collision responses, and tactile controllers use multi-kilohertz loops. EtherCAT or an FPGA is not automatically required; choose a fieldbus, MCU, or FPGA according to the required determinism, bandwidth, and I/O structure.

> **Further reading**
> - FreeRTOS official documentation: https://www.freertos.org/
> - PREEMPT_RT Wiki: https://wiki.linuxfoundation.org/realtime/start
> - Dynamixel SDK: https://github.com/ROBOTIS-GIT/DynamixelSDK
> - IgH EtherCAT Master (open-source for Linux): https://etherlab.org/en/ethercat/
> - SOEM (Simple Open EtherCAT Master): https://github.com/OpenEtherCATsociety/SOEM

---

## 4.6 Advanced: Workspace Analysis and Optimal Design

Kinematics can also be used to compare robot designs. The following topics relate workspace geometry and manipulability to design optimization.

### 4.6.1 Reachable Workspace vs Dexterous Workspace

**Reachable workspace**: the set of all points that the end-effector can reach in at least one orientation. "How far the hand can reach."

**Dexterous workspace**: the set of points the end-effector can reach in any orientation. "Where it can move freely." Naturally a subset of the reachable workspace, and usually much smaller.

For a 6-DOF robot, the dexterous workspace can be quite limited. This is one reason 7-DOF robots exist.

Workspace analysis can be performed with a Monte Carlo method: randomly sample the joint space and compute end-effector positions via FK to build a point cloud.

```python
import numpy as np
import roboticstoolbox as rtb

# Workspace visualization of the Puma 560 (Monte Carlo)
puma = rtb.models.DH.Puma560()
n_samples = 50000
positions = []

for _ in range(n_samples):
    # random sample within each joint's range
    q = puma.random_q()
    T = puma.fkine(q)
    positions.append(T.t)  # [x, y, z]

positions = np.array(positions)

# Visualization (matplotlib)
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

### 4.6.2 Condition Number and Isotropy

The Jacobian's condition number (κ) indicates how "well" the robot can move at a given configuration.

```
κ(J) = σ_max / σ_min
```

σ_max and σ_min are the maximum and minimum singular values of the Jacobian.

- κ = 1: perfect isotropy. Uniform motion in every direction. Attainable for suitable designs and configurations.
- κ → ∞: singularity. No motion at all in one direction.

In robot design, minimizing the condition number across the entire workspace can be the goal. This is called **kinematic optimization** or **optimal design**.

Caveat: when computing the Jacobian's condition number, linear velocity (m/s) and angular velocity (rad/s) have different units, so comparing them directly is meaningless. Normalize by a characteristic length, or analyze linear and angular velocities separately. This issue is a long-standing debate in the optimization of robot kinematics.

### 4.6.3 Redundancy Resolution (7-DOF Arms)

A 7-DOF robot arm (Kinova Gen3, KUKA LBR iiwa, Franka Emika Panda, etc.) has one extra degree of freedom relative to a 6-DOF task space. This extra freedom is called **kinematic redundancy**.

The overall arm configuration can be changed while holding the same end-effector pose. It is like a human raising or lowering the elbow while keeping the fist in place.

Strategies for exploiting this freedom:
1. **Singularity avoidance**: use the extra freedom to maximize the Jacobian's manipulability
2. **Joint-limit avoidance**: as a joint nears its limit, use the extra freedom to return toward center
3. **Obstacle avoidance**: adjust the configuration so the elbow does not collide with obstacles
4. **Energy optimization**: choose the pose that minimizes torque

Mathematically, the extra freedom corresponds to the null space of the Jacobian:

```
q̇ = J† * ẋ + (I - J† * J) * q̇_0
```

The first term is the minimum-norm joint velocity that achieves the end-effector velocity. The second term (I - J†J) is the null-space projector — it moves the joints without affecting the end-effector velocity. q̇_0 is the gradient of a secondary objective (e.g., maximizing manipulability).

```python
def redundancy_resolution(J, x_dot, q, q_center, k_null=0.5):
    """
    Redundancy resolution for a 7-DOF robot.

Args:
        J: 6x7 Jacobian
        x_dot: 6x1 desired end-effector velocity
        q: 7x1 current joint angles
        q_center: 7x1 joint center values (null-space target)
        k_null: null-space gain
    Returns:
        q_dot: 7x1 joint velocities
    """
    # Damped pseudo-inverse
    lam = 0.01
    J_pinv = J.T @ np.linalg.inv(J @ J.T + lam**2 * np.eye(6))

# primary objective: track end-effector velocity
    q_dot_primary = J_pinv @ x_dot

# secondary objective: return toward joint center (null space)
    null_projector = np.eye(7) - J_pinv @ J
    q_dot_null = null_projector @ (k_null * (q_center - q))

return q_dot_primary + q_dot_null
```

> **Further reading**
> - Siciliano et al., *Robotics: Modelling, Planning and Control*, Chapter 3.9 — detailed treatment of redundancy resolution
> - Nakamura, "Advanced Robotics: Redundancy and Optimization" (1991) — a classic
> - Dietrich et al., "An Overview of Null Space Projections for Redundant, Torque-Controlled Robots" (2015)
> - Franka Emika research interface: https://frankaemika.github.io/docs/

Everything so far has been deterministic kinematics. §4.7 layers probability on top of it.

---

## 4.7 Advanced: Probabilistic Motion Models

### 4.7.1 From Determinism to Probability

§4.2 forward kinematics and §4.3 inverse kinematics are deterministic. Feed in joint angles and one end-effector position comes out; feed in an end-effector position and a set of joint angles comes out. The output is a point estimate given the input. That is a property of the model, however, not of the platform. Manipulators have joint backlash, link compliance, and calibration error of their own, which is why the introduction listed calibration — correcting errors between the real robot and the model — among the uses of kinematics.

Wheeled mobile robots are different. The wheels do not roll at precisely the commanded speed. There is slip, effective radius changes with wear, and asymmetric wear on left and right wheels generates straight-line error. The result: even after issuing a control command $u_t$, the next pose $x_t$ is not a single point but a probability distribution. Formalizing that distribution is what a **probabilistic motion model** does.

The state is a planar pose, $x_t = (x, y, \theta)^T \in SE(2)$. The motion model defines the conditional probability distribution of the next pose given the previous pose $x_{t-1}$ and the control input $u_t$:

$$p(x_t \mid u_t, x_{t-1})$$

Two representations exist for this distribution.

**Velocity model**: the control input is given as linear and angular velocity, $u_t = (v, \omega)^T$. Usable at the planning stage. The error between the robot's commanded speed and its actual speed is modeled as noise.

**Odometry model**: the control input is a pair of poses obtained by integrating the wheel encoders, $u_t = (\bar{x}_{t-1}, \bar{x}_t)$. It is retrospective, so it cannot be used for planning. The pose pair itself is obtained by integrating encoder rotations through the kinematic model; it is not a direct measurement of pose. Even so, it tends to be more accurate in practice than a velocity model that takes the commanded speed at face value.

For each of the two models there are two ways to use it: **closed-form density evaluation** and **sampling**. Closed-form returns a probability density value answering "how plausible is this hypothesized pose $x_t$?" That is what is needed when the Bayes filter integral is evaluated directly, as in grid localization. EKF and UKF prediction does not use this density value; it propagates the mean and covariance through a Jacobian or through sigma points. Sampling is forward simulation that generates the next pose. A particle filter (MCL) uses this form directly. The four combinations are covered in §4.7.2–§4.7.5.

### 4.7.2 Velocity Motion Model — Closed-Form

**Intuition.** Without noise, a robot moving at linear velocity $v$ and angular velocity $\omega$ traces a circular arc. At $\omega = 0$ the arc degenerates to a straight line. Noise means the arc actually traced differs from the commanded one. Closed-form evaluation inverts this logic: given two poses $x_{t-1}$ and $x_t$, it back-computes the center $(x_c, y_c)$ and radius $r^*$ of the arc connecting them, recovers the hypothetical velocity $(\hat{v}, \hat{\omega})$ that would have produced that arc, and then evaluates the difference from the commanded velocity $(v, \omega)$ under a noise distribution.

**Equations.** Given $x_{t-1} = (x, y, \theta)^T$ and a hypothesized $x_t = (x', y', \theta')^T$:

$$\mu = \frac{1}{2} \cdot \frac{(x - x')\cos\theta + (y - y')\sin\theta}{(y - y')\cos\theta - (x - x')\sin\theta}$$

$$x_c = \frac{x + x'}{2} + \mu(y - y'), \quad y_c = \frac{y + y'}{2} + \mu(x' - x)$$

$$r^* = \sqrt{(x - x_c)^2 + (y - y_c)^2}$$

$$\Delta\theta = \text{atan2}(y' - y_c,\ x' - x_c) - \text{atan2}(y - y_c,\ x - x_c)$$

$$\hat{v} = \frac{\Delta\theta \cdot r^*}{\Delta t}, \quad \hat{\omega} = \frac{\Delta\theta}{\Delta t}, \quad \hat{\gamma} = \frac{\theta' - \theta}{\Delta t} - \hat{\omega}$$

The noise model is additive, with variance proportional to command magnitude. The second argument $b$ (variance) of `prob(a, b)` is:

$b_v$ is the noise variance on linear velocity, $b_\omega$ on angular velocity. $\hat{\gamma}$ is a final-heading correction term. With only two noise variables $(v, \omega)$, hypothesized poses are confined to a 2D manifold within 3D pose space — a *degeneracy* problem. Adding $\hat{\gamma}$ secures full 3D support.

The six parameters physically: $\alpha_1, \alpha_2$ weight the variance of linear-velocity noise; $\alpha_3, \alpha_4$ weight angular-velocity noise; $\alpha_5, \alpha_6$ weight final-heading noise. Because variance scales linearly with command magnitude, faster motion becomes more uncertain — which matches intuition. Each robot needs its $\alpha_i$ calibrated from straight-line, circular, and figure-eight trajectories.

**Algorithm box (PR Table 5.1: `motion_model_velocity`).**

```
Algorithm motion_model_velocity(x_t, u_t, x_{t-1}):
  # inputs: x_t=(x',y',θ'), u_t=(v,ω), x_{t-1}=(x,y,θ)
  # output: p(x_t | u_t, x_{t-1}) probability density

p1 = prob(v − v̂,  α₁|v| + α₂|ω|)
  p2 = prob(ω − ω̂,  α₃|v| + α₄|ω|)
  p3 = prob(γ̂,       α₅|v| + α₆|ω|)

return p1 · p2 · p3
```

`prob(a, b)` is the density of a zero-mean normal or triangular distribution with variance $b$.

Generating a pose sample from the same noise parameters works in the opposite direction, covered next.

### 4.7.3 Velocity Motion Model — Sampling

Closed-form evaluated a hypothesized pose by inverting the arc. Sampling runs in the opposite direction: draw noise first, perturb the commanded velocity, then integrate the arc forward to produce one pose sample. A particle filter needs exactly this one sample per particle, and the implementation is simpler than the closed-form version.

Perturbed controls:

Forward arc integration:

Note: when $|\hat{\omega}| < \epsilon$ the above diverges. The implementation must fall back to a straight-line approximation: $x' = x + \hat{v}\cos\theta\,\Delta t,\ y' = y + \hat{v}\sin\theta\,\Delta t$.

`sample(b)` draws a zero-mean sample with variance $b$. Normal approximation: $\frac{\sqrt{b}}{2}\sum_{i=1}^{12}\text{rand}(-1,1)$ (central-limit-theorem approximation using 12 uniform draws).

**Algorithm box (PR Table 5.3: `sample_motion_model_velocity`).**

```
Algorithm sample_motion_model_velocity(u_t, x_{t-1}):
  # inputs: u_t=(v,ω), x_{t-1}=(x,y,θ)
  # output: sample x_t ~ p(x_t | u_t, x_{t-1})

v̂ = v + sample(α₁|v| + α₂|ω|)
  ω̂ = ω + sample(α₃|v| + α₄|ω|)
  γ̂ = sample(α₅|v| + α₆|ω|)

if |ω̂| < ε:   # straight-line fallback
    x' = x + v̂·cosθ·Δt
    y' = y + v̂·sinθ·Δt
  else:
    x' = x − (v̂/ω̂)sinθ + (v̂/ω̂)sin(θ + ω̂Δt)
    y' = y + (v̂/ω̂)cosθ − (v̂/ω̂)cos(θ + ω̂Δt)
  θ' = θ + ω̂Δt + γ̂Δt

return (x', y', θ')ᵀ
```

**Closed-form vs. sampling.** `motion_model_velocity` returns a probability density value, used where the density itself is needed — for example grid localization, which evaluates the Bayes filter integral directly. `sample_motion_model_velocity` produces one pose and is called directly to propagate each particle in a particle filter (MCL, §14.7). Both algorithms share the same noise parameters $\alpha_1..\alpha_6$, but their directions are opposite: closed-form *evaluates* a hypothesized pose, sampling *generates* the next pose.

```python
import numpy as np

def sample_normal(b):
    """Zero-mean normal approximation sample with variance b (12 uniform draws)."""
    return (np.sqrt(b) / 2.0) * sum(np.random.uniform(-1, 1) for _ in range(12))

return x_new, y_new, theta_new
```

The two velocity model variants share the same noise parameters $\alpha_1..\alpha_6$ and the same assumption that the control input is a commanded velocity $(v, \omega)$. Changing that assumption leads to the second model family.

### 4.7.4 Odometry Motion Model — Closed-Form

**Intuition.** The velocity model estimates motion from commanded velocity. The odometry model goes the other way: it treats the pair of poses $u_t = (\bar{x}_{t-1}, \bar{x}_t)$ measured by the wheel encoder as if they were the control. The relative motion between these two poses is decomposed into three parameters $(\delta_{\text{rot1}}, \delta_{\text{trans}}, \delta_{\text{rot2}})$: first rotate toward the destination, then translate straight, then correct the final heading. This decomposition can represent any planar motion.

Strictly speaking, the odometry measurement is a sensor reading, but here it is treated as a control input. Treating it as a genuine measurement model would require adding velocity to the state space, which enlarges the dimension. This is a practical simplification.

**Equations.** Extract relative motion from the odometry measurement $u_t = (\bar{x}_{t-1}, \bar{x}_t)$:

Noise model (four parameters $\alpha_1..\alpha_4$). The variance argument to `prob()` depends on the $(\hat\delta_{\text{rot1}}, \hat\delta_{\text{trans}}, \hat\delta_{\text{rot2}})$ back-computed from the hypothesized pose:

$\alpha_1$: how much rotation disturbs rotation (rotational slip); $\alpha_2$: how much translation disturbs rotation; $\alpha_3$: translation's own variance; $\alpha_4$: how much rotation disturbs translation. The final-heading correction trick from the velocity model ($\alpha_5, \alpha_6$) is unnecessary here. Three independent noise variables naturally secure 3D support.

One implementation note: angular differences must be wrapped to $[-\pi, \pi]$. Omitting this wrap is a common bug that causes the distribution to blow up.

**Algorithm box (PR Table 5.5: `motion_model_odometry`).**

```
Algorithm motion_model_odometry(x_t, u_t, x_{t-1}):
  # inputs: x_t=(x',y',θ'), u_t=(x̄_{t-1}, x̄_t), x_{t-1}=(x,y,θ)
  # output: p(x_t | u_t, x_{t-1}) probability density

# extract (δ_rot1, δ_trans, δ_rot2) from odometry measurement
  δ_rot1  = atan2(ȳ' − ȳ, x̄' − x̄) − θ̄
  δ_trans = sqrt((x̄ − x̄')² + (ȳ − ȳ')²)
  δ_rot2  = θ̄' − θ̄ − δ_rot1

# same decomposition from the hypothesized pose pair (inverse model)
  δ̂_rot1  = atan2(y' − y, x' − x) − θ
  δ̂_trans = sqrt((x − x')² + (y − y')²)
  δ̂_rot2  = θ' − θ − δ̂_rot1

return p1 · p2 · p3
```

The three parameters $(\delta_{\text{rot1}}, \delta_{\text{trans}}, \delta_{\text{rot2}})$ are treated as independent noise variables, which yields a density with support in all three directions, so the likelihood of a given hypothesized pose can be evaluated directly.

### 4.7.5 Odometry Model — Sampling for Particle Filters

The odometry closed-form used an inverse model to evaluate a hypothesized pose. Sampling reverses the direction: add noise to the $(\delta_{\text{rot1}}, \delta_{\text{trans}}, \delta_{\text{rot2}})$ extracted from the odometry, then compose the perturbed values forward to generate a new pose. No inverse model is needed at all, making the implementation considerably simpler than the closed-form version.

**Equations.** Forward composition (PR eq. 5.40):

This approximates motion as *two rotations plus one translation* rather than a circular arc — a first-order arc approximation for small $\Delta t$. Unlike velocity sampling, there is no need for an $\omega \to 0$ branch.

**Algorithm box (PR Table 5.6: `sample_motion_model_odometry`).**

```
Algorithm sample_motion_model_odometry(u_t, x_{t-1}):
  # inputs: u_t=(x̄_{t-1}, x̄_t), x_{t-1}=(x,y,θ)
  # output: sample x_t ~ p(x_t | u_t, x_{t-1})

# extract relative motion from odometry
  δ_rot1  = atan2(ȳ' − ȳ, x̄' − x̄) − θ̄
  δ_trans = sqrt((x̄ − x̄')² + (ȳ − ȳ')²)
  δ_rot2  = θ̄' − θ̄ − δ_rot1

# forward composition
  x' = x + δ̂_trans · cos(θ + δ̂_rot1)
  y' = y + δ̂_trans · sin(θ + δ̂_rot1)
  θ' = θ + δ̂_rot1 + δ̂_rot2

return (x', y', θ')ᵀ
```

ROS2 Nav2's `nav2_amcl` implements this as the `differential` motion model. It is the direct application to wheeled AMR localization.

```python
import numpy as np

d_rot1  = np.arctan2(by_ - by, bx_ - bx) - bt
    d_trans = np.sqrt((bx - bx_)**2 + (by - by_)**2)
    d_rot2  = bt_ - bt - d_rot1

def sample_normal(b):
        return (np.sqrt(b) / 2.0) * sum(np.random.uniform(-1, 1) for _ in range(12))

x_new     = x + dh_trans * np.cos(theta + dh_rot1)
    y_new     = y + dh_trans * np.sin(theta + dh_rot1)
    theta_new = theta + dh_rot1 + dh_rot2

return x_new, y_new, theta_new
```

All four algorithms so far model motion alone, without a map. In a localization problem, a map $m$ is present.

### 4.7.6 Motion + Map: Map-Conditioned Motion Model

The models above ignored map information. In localization, a map $m$ is available, and it can be used to filter out physically impossible poses.

**Equations.** Computing the map-conditioned transition distribution exactly is hard. A practical approximate factorization:

$$p(x_t \mid u_t, x_{t-1}, m) \propto p(x_t \mid u_t, x_{t-1}) \cdot p(x_t \mid m)$$

The first factor $p(x_t \mid u_t, x_{t-1})$ is the motion model from §4.7.2–§4.7.5. The second factor $p(x_t \mid m)$ is the map-conditioned probability: in an occupancy grid, it is close to 1 when $x_t$ lies in a free cell and close to 0 in a wall or occupied cell.

**Effect.** Particles in the particle filter are kept from ending up inside walls. The simplest implementation samples a new pose, queries the map, and sets that particle's weight to 0 (or very low) when the cell is occupied. Only the endpoint is checked, so with a large time step a path that jumped clear across a wall is not filtered out. Strictly, $p(x_t \mid m)$ acts as a prior rather than a likelihood here, so the approximation assumes the motion model and the map information are independent — a limitation worth noting.

In practice an occupancy grid has unknown regions in addition to free space. Whether $p(x_t \mid m)$ is set to 1 or to some intermediate value for unknown cells affects localization quality. ROS2 Nav2's default treats unknown cells as free.

The mathematical basis for this factorization is in §3.3 (Bayes' theorem and conditional independence, Ch.3). The product factorization is exact only when the motion model and the map prior are independent.

### 4.7.7 What Survived

The sampled velocity and odometry formulations describe motion priors for particle-filter localization on wheeled robots. ROS2 Nav2 `nav2_amcl`'s `differential` motion model corresponds to the odometry-based formulation. Choose the particle count and update rate by measurement on the target map, sensor update rate, CPU, beam count, and error parameters.

**Platforms where VIO and IMU preintegration are standard.** Humanoids, drones, and legged robots are hard to describe in terms of body velocity $(v, \omega)$. Legs bring a different slip model entirely, and drones move in SE(3) rather than SE(2). On these platforms, IMU preintegration provides the motion prior. That topic is in §14.10. The formal framework — $p(x_t \mid u_t, x_{t-1})$ — is the same; the content is entirely different.

**Limits of the model.** Every model here is SE(2)-only. Holonomic robots (Mecanum wheels) or vehicles with lateral dynamics require separate models.

**Where these models are used next.** §14.7 Monte Carlo Localization (MCL) calls `sample_motion_model_odometry` directly in the prediction step of the particle filter (Ch.14). §14.10 IMU preintegration shows how the wheeled odometry model and the IMU model compare (Ch.14). In both contexts, the $p(x_t \mid u_t, x_{t-1})$ derived here enters the prediction term of the Gaussian filter and nonparametric filter families from §3.10 and §3.11 (Ch.3).

---

Deterministic FK maps joint angles to a single pose, while IK finds the set of joint-angle solutions for a target pose. A probabilistic motion model maps input to an output distribution. Two models (velocity, odometry) times two uses (density evaluation, sampling) yields four combinations that are the basic building blocks of real localization systems. The odometry model, being retrospective information obtained by integrating encoder rotations, cannot be used for planning but is more accurate in practice; the velocity model can be used for planning but does not capture actual slip. Sampling serves the particle filter; density evaluation serves cases such as grid localization, where the density value itself is needed.

One question remains. Every model here stacks noise on top of the kinematic constraint that wheels do not slip. In mud or on inclines — environments where that constraint itself breaks down — how far can $\alpha_i$ calibration compensate? §4.8 collects the textbooks and software referenced across this chapter.

---

## 4.8 Further Reading

To study kinematics and mechatronics seriously, the most effective approach is to work through one textbook cover to cover.

**Textbooks:**

- **Craig, "Introduction to Robotics: Mechanics and Control"** — covers DH parameters and kinematics using the Modified DH convention. Check its prerequisites and examples against the course in which it will be used.

- **Lynch & Park, "Modern Robotics: Mechanics, Planning, and Control"** — PoE-based. Provides a free PDF and Coursera course, making it highly accessible. Mathematically cleaner but hard on first read. https://modernrobotics.org

- **Corke, "Robotics, Vision and Control"** — practice kinematics alongside MATLAB/Python code. robotics-toolbox-python is the companion library for this book. The 3rd edition is Python-based. https://petercorke.com/rvc/

- **Siciliano et al., "Robotics: Modelling, Planning and Control"** — a broad graduate text covering kinematics, dynamics, and control.

**Online courses:**

**Software / libraries:**

- robotics-toolbox-python: https://github.com/petercorke/robotics-toolbox-python
- Pinocchio (fast dynamics, differentiable kinematics): https://github.com/stack-of-tasks/pinocchio
- MoveIt2 (ROS2 motion planning): https://moveit.picknik.ai/
- Drake (simulation + optimization + control): https://drake.mit.edu/

---

## Technical Timeline

```
1955 ── DH parameters proposed (Denavit & Hartenberg)
1969 ── Stanford Arm (an early electric, computer-controlled robot arm)
1970s–80s ── Harmonic Drive becomes the standard reducer for industrial articulated-robot joints
1985 ── Product of Exponentials formalized
2019 ── MIT Mini Cheetah: QDD actuator
2019 ── MoveIt2 (ROS2-based motion planning framework)
2023 ── ALOHA: low-cost bimanual teleoperation platform
2024 ── SO-ARM100: open-source five-axis arm with a published BOM and assembly documentation
```

---

*Layering force and mass on top of this kinematics leads to dynamics and control. The viewpoint shifts from "sending joint angles to a desired value" to "applying a desired torque."*

---

# Ch.5 — Rigid Body Dynamics

---

## 5.1 Why Study Dynamics

If kinematics addresses "*where* the robot moves", dynamics addresses "*with what forces* it moves". Kinematics alone is enough to control a robot in some cases — slow industrial manipulators are one example. When joint velocities are low enough, inertial and Coriolis forces are negligible, and a PID controller handles the rest.

But the following situations cannot be handled without dynamics:

- **High-speed manipulation**: Reducing cycle time in industrial settings requires moving the robot fast. Moving fast increases inertial, centrifugal, and Coriolis forces. Ignoring them inflates path-tracking error and, in the worst case, saturates the joint motors.
- **Legged robots**: Whether bipedal or quadrupedal, the robot must manage ground contact forces without falling. This is a purely dynamic problem.
- **Simulation**: A physics simulator takes forces/torques, computes accelerations, and integrates to obtain the next state. The dynamics model is the core of the simulator.
- **Optimal control**: Finding trajectories that minimize energy or time requires the dynamics model as a constraint.
- **Collision/contact handling**: Grasping, pushing, or throwing objects is impossible without contact dynamics.

Kinematics is the "geometry" of the robot; dynamics is its "physics". Just as geometry alone cannot capture the world, kinematics alone cannot fully control a robot.

> **Further reading**
> - Featherstone, *Rigid Body Dynamics Algorithms*, Chapter 1 — a concise account of why dynamics is needed.
> - Russ Tedrake, *Underactuated Robotics* Ch.1 (https://underactuated.csail.mit.edu/) — gives intuition for why dynamics-based control is more powerful than kinematics-based.

---

## 5.2 Newton-Euler Formulation

### Basic Principles

Newtonian mechanics treats translational and rotational motion separately.

**Translational motion:**
```
F = ma
```
The net force F on a body equals mass m times the acceleration a of the center of mass (CoM).

**Rotational motion:**
```
τ = Iα + ω × (Iω)
```
The net torque τ on a body equals the product of the inertia tensor I and the angular acceleration α, plus the gyroscopic term ω × (Iω). In 2D the latter term vanishes and the equation reduces to τ = Iα, but a 3D model must retain it. Omitting the term produces unrealistic rotational behavior in simulation.

### Recursive Newton-Euler Algorithm (RNEA)

RNEA solves the inverse dynamics of a serial manipulator in two passes:

**Forward pass (base → end-effector):** Propagate velocities and accelerations of each link forward. The velocity of link i equals the velocity of link i-1 plus the contribution from joint i.

**Backward pass (end-effector → base):** Propagate forces and torques acting on each link backward. Use the Newton-Euler equations to obtain the net force/torque required at link i, then convert to the torque of joint i.

Why solve it recursively? The dynamics of a single rigid body is O(1). Handling n links sequentially gives O(n). Solving the Lagrange equations directly, in contrast, costs O(n^3) for computing the M(q) matrix. For robots with many joints (e.g., a humanoid with 30+ DOF), this difference determines whether real-time control is feasible.

The pseudo-code for RNEA is:

```
RNEA(model, q, q̇, q̈):
    # Forward pass: i = 1, 2, ..., n
    for i = 1 to n:
        v[i] = v[i-1] + S[i] * q̇[i]        # add velocity along joint axis
        a[i] = a[i-1] + S[i] * q̈[i] + v[i] × (S[i] * q̇[i])
        f[i] = I[i] * a[i] + v[i] × (I[i] * v[i])  # Newton-Euler

# Backward pass: i = n, n-1, ..., 1
    for i = n downto 1:
        τ[i] = S[i]^T * f[i]               # extract joint torque
        f[parent(i)] += f[i]                # propagate to parent link

return τ
```

Here S[i] is the motion subspace matrix of joint i (the joint axis direction), v[i] is the spatial velocity of link i, and I[i] is the spatial inertia of link i. The notation follows Featherstone's spatial vector convention. §5.7 covers it in more detail.

### Real Code: Pinocchio

Pinocchio is a C++/Python library that implements RNEA and a variety of other dynamics algorithms. Here is an example of computing inverse dynamics with RNEA:

```python
import pinocchio as pin
import numpy as np

# Load model from URDF
model = pin.buildModelFromUrdf("robot.urdf")
data = model.createData()

# Set current state
q = pin.randomConfiguration(model)     # joint positions
v = np.random.randn(model.nv)          # joint velocities
a = np.random.randn(model.nv)          # joint accelerations

# RNEA: (q, v, a) → τ
tau = pin.rnea(model, data, q, v, a)
print("Joint torques:", tau)

# Compute gravity torques only (v=0, a=0)
tau_g = pin.rnea(model, data, q, np.zeros(model.nv), np.zeros(model.nv))
print("Gravity compensation torques:", tau_g)
```

Gravity compensation drops out of RNEA immediately by setting the joint velocities and accelerations to zero and initializing the base acceleration to a[0] = -g. The pseudocode above omits this initialization. Even this alone keeps the robot from sagging under gravity. It is one of the first controllers implemented in practice.

The same computation in Drake:

```python
from pydrake.multibody.plant import MultibodyPlant
from pydrake.multibody.parsing import Parser
import numpy as np

plant = MultibodyPlant(time_step=0.0)
Parser(plant).AddModels("robot.urdf")
plant.Finalize()

context = plant.CreateDefaultContext()

q = np.random.randn(plant.num_positions())
v = np.random.randn(plant.num_velocities())
vdot = np.random.randn(plant.num_velocities())

plant.SetPositions(context, q)
plant.SetVelocities(context, v)

# Inverse dynamics: vdot → τ
tau = plant.CalcInverseDynamics(context, vdot, MultibodyForces(plant))
```

> **Further reading**
> - Featherstone, *Rigid Body Dynamics Algorithms*, Chapter 5 — the original description of RNEA
> - Pinocchio documentation (https://github.com/stack-of-tasks/pinocchio) — the easiest library for trying RNEA in practice
> - Luh, Walker, Paul (1980), "On-Line Computational Scheme for Mechanical Manipulators" — the original RNEA paper

---

## 5.3 Lagrangian Mechanics

### What Is a Lagrangian

Lagrangian mechanics derives the equations of motion through energy, instead of handling forces and torques directly.

The **Lagrangian** L is defined as:

```
L(q, q̇) = T(q, q̇) - V(q)
```

where T is the kinetic energy of the system and V is the potential energy.

**Euler-Lagrange equation:**

```
d/dt (∂L/∂q̇_i) - ∂L/∂q_i = τ_i
```

Writing this equation for each generalized coordinate q_i yields the equations of motion of the system.

The coordinate frame can be chosen freely. The Newton-Euler approach solves link by link, passing forces and moments between neighbors, so one has to decide which frame to express them in and how to handle the constraints. In Lagrangian mechanics, choosing joint angles as generalized coordinates makes the constraints drop out of the equations — but only for open chains with holonomic constraints, where an independent set of generalized coordinates exists. With closed chains or nonholonomic constraints, Lagrange multipliers remain.

### Manipulator Equation

Organizing the Euler-Lagrange equations for an n-DOF serial manipulator produces the following standard form:

```
M(q)q̈ + C(q, q̇)q̇ + g(q) = τ
```

The meaning of each term:

- **M(q)**: mass/inertia matrix. An n×n symmetric positive definite matrix. It depends on the robot configuration q — extend the arm and the inertia grows; fold it and the inertia shrinks, by the same principle.

- **C(q, q̇)q̇**: Coriolis and centrifugal terms. Inertial coupling that arises when joints move simultaneously. Negligible at low speeds, but large at high speeds.

- **g(q)**: gravity vector. The gravitational torque on each joint when the robot is in a gravitational field.

- **τ**: joint torque vector. The forces produced by the motors. Friction is typically modeled separately and added on.

This equation is the heart of robotic dynamics. Control, simulation, and trajectory optimization all start from it.

### 2-Link Planar Arm Example

The 2-link planar arm is a staple example in any introduction to dynamics. Deriving it by hand on paper is strongly recommended — going through it once makes the structure of the n-DOF case clear.

Setup:
- Link lengths: l_1, l_2
- Link masses: m_1, m_2 (assume mass concentrated at the link tip — point mass)
- Joint angles: q_1, q_2 (measured from the base)
- Gravity: g (pointing down)

**Kinetic energy T:**

Tip position of link 1:
```
x_1 = l_1 cos(q_1)
y_1 = l_1 sin(q_1)
```

Tip position of link 2:
```
x_2 = l_1 cos(q_1) + l_2 cos(q_1 + q_2)
y_2 = l_1 sin(q_1) + l_2 sin(q_1 + q_2)
```

Computing each mass's velocity and expanding T = (1/2)m_1 v_1^2 + (1/2)m_2 v_2^2 gives:

```
T = (1/2)(m_1 + m_2) l_1^2 q̇_1^2
  + (1/2) m_2 l_2^2 (q̇_1 + q̇_2)^2
  + m_2 l_1 l_2 cos(q_2) q̇_1 (q̇_1 + q̇_2)
```

**Potential energy V:**

```
V = m_1 g l_1 sin(q_1) + m_2 g [l_1 sin(q_1) + l_2 sin(q_1 + q_2)]
```

**M(q) matrix:**

M(q) depends on q_2. Look at M_{11}, the effective inertia seen by the base joint: it is maximal at q_2 = 0, with the arm fully extended, and minimal at q_2 = π, with the arm folded.

**C(q, q̇) matrix:**

```
C(q, q̇) = [ -m_2 l_1 l_2 sin(q_2) q̇_2    -m_2 l_1 l_2 sin(q_2)(q̇_1 + q̇_2) ]
            [  m_2 l_1 l_2 sin(q_2) q̇_1     0                                     ]
```

There are several ways to derive C (Christoffel symbols among them). The most systematic route is Christoffel symbols, but for a 2-link arm it is faster to collect the terms directly from the Euler-Lagrange equations.

**g(q) vector:**

```
g(q) = [ (m_1 + m_2) g l_1 cos(q_1) + m_2 g l_2 cos(q_1 + q_2) ]
        [ m_2 g l_2 cos(q_1 + q_2)                                 ]
```

Code that verifies this with SymPy:

```python
import sympy as sp

q1, q2, dq1, dq2, ddq1, ddq2 = sp.symbols('q1 q2 dq1 dq2 ddq1 ddq2')
m1, m2, l1, l2, g = sp.symbols('m1 m2 l1 l2 g', positive=True)

# Positions
x1 = l1 * sp.cos(q1)
y1 = l1 * sp.sin(q1)
x2 = x1 + l2 * sp.cos(q1 + q2)
y2 = y1 + l2 * sp.sin(q1 + q2)

# Velocities (chain rule)
vx1 = sp.diff(x1, q1) * dq1
vy1 = sp.diff(y1, q1) * dq1
vx2 = sp.diff(x2, q1) * dq1 + sp.diff(x2, q2) * dq2
vy2 = sp.diff(y2, q1) * dq1 + sp.diff(y2, q2) * dq2

# Kinetic energy
T = sp.Rational(1,2)*m1*(vx1**2 + vy1**2) + sp.Rational(1,2)*m2*(vx2**2 + vy2**2)
T = sp.trigsimp(sp.expand(T))

# Potential energy
V = m1*g*y1 + m2*g*y2

# Lagrangian
L = T - V

# Euler-Lagrange equations
# d/dt(∂L/∂q̇_i) - ∂L/∂q_i = τ_i
# d/dt here must account for time derivatives of q1, q2, so substitutions are required.
# For a clean extraction of M, C, g, consult a textbook.

print("T =", T)
print("V =", V)
```

This code prints the kinetic energy T and potential energy V, which can be compared with the energy expressions derived above. After SymPy applies trigsimp, the form comes out cleanly.

> **Further reading**
> - Murray, Li, Sastry, *A Mathematical Introduction to Robotic Manipulation*, Ch. 4 (https://www.cds.caltech.edu/~murray/mlswiki/) — the most rigorous treatment of Lagrangian mechanics in a robotics context. Free PDF available.
> - Spong, Hutchinson, Vidyasagar, *Robot Modeling and Control*, Ch. 6-7 — the most accessible explanation at an undergraduate level
> - Craig, *Introduction to Robotics*, Ch. 6 — contains a detailed 2-link arm example

---

## 5.4 Newton-Euler vs. Lagrangian

These are the same physics viewed from different perspectives. The final result (the equations of motion) is identical. The difference lies in derivation and computational efficiency.

| Item | Newton-Euler (RNEA) | Lagrangian |
|------|-------------------|---------|
| Perspective | force/torque (force-based) | energy (energy-based) |
| Computational complexity | O(n) | O(n^3) (when computing the M matrix directly) |
| Derivation difficulty | recursive, same pattern as n grows | partial derivatives explode as n grows |
| Physical intuition | forces/torques on each link are directly visible | energy conservation/transformation is visible |
| Primary use | real-time control, simulation | model derivation, energy-based analysis, Lyapunov stability |
| Constraint forces | explicitly computable | automatically eliminated when using generalized coordinates |

The practical workflow is typically as follows:

1. **Model derivation**: Use Lagrangian mechanics to understand the structure of the manipulator equation.
2. **Numerical computation**: Use RNEA (or ABA) for real-time evaluation.
3. **Controller design**: Design computed torque control, passivity-based control, and similar schemes that exploit the structure (M, C, g) of the manipulator equation.
4. **Code implementation**: Pinocchio or Drake use RNEA/ABA internally, so calling the library is sufficient.

The two formulations serve complementary roles. Lagrangian mechanics exposes the structure used by control theory, while Newton-Euler methods support efficient real-time computation.

> **Further reading**
> - Featherstone, *Rigid Body Dynamics Algorithms*, Ch. 3 — a clear account of the relationship between the two formulations
> - Siciliano et al., *Robotics: Modelling, Planning and Control*, Ch. 7 — a comparative example deriving the dynamics of the same robot with both methods

---

## 5.5 Forward Dynamics vs. Inverse Dynamics

Dynamics has two "directions":

**Inverse Dynamics:**
```
Given: q, q̇, q̈
Find: τ
```
Answers "what torque must the motor produce to follow this trajectory?". Used primarily in control. The core of computed torque control.

**Forward Dynamics:**
```
Given: q, q̇, τ
Find: q̈
```
It answers the question, "How does the robot accelerate under this torque?" A simulator repeats this computation at every time step.

Mathematically, forward dynamics is solving for q̈ in the manipulator equation:

```
q̈ = M(q)^{-1} [τ - C(q, q̇)q̇ - g(q)]
```

Simply inverting M(q) is O(n^3). This is slow for robots with many joints.

### Articulated Body Algorithm (ABA)

Featherstone's ABA computes forward dynamics in O(n). Just as RNEA is the O(n) algorithm for inverse dynamics, ABA is the O(n) algorithm for forward dynamics.

ABA treats each link as an "articulated body" and recursively accumulates the inertia of its subtree. It computes q̈ directly without explicitly forming the M matrix.

```
ABA(model, q, q̇, τ):
    # Pass 1 (forward): propagate velocities
    for i = 1 to n:
        v[i] = v[parent(i)] + S[i] * q̇[i]
        c[i] = v[i] × (S[i] * q̇[i])  # Coriolis acceleration

# Pass 2 (backward): compute articulated body inertia
    for i = n downto 1:
        I_A[i] = I[i]  # spatial inertia
        p_A[i] = v[i] × (I[i] * v[i]) - f_ext[i]  # bias force
        # accumulate contributions from child links (omitted)
        # compute intermediate joint acceleration values

# Pass 3 (forward): propagate accelerations
    for i = 1 to n:
        q̈[i] = ...  # computed using articulated body inertia
        a[i] = a[parent(i)] + S[i] * q̈[i] + c[i]

return q̈
```

The actual implementation is quite involved. Rather than writing it from scratch, using Pinocchio or Drake is the sensible choice.

### Role in Simulators

The algorithm differs between simulators:

- **MuJoCo**: uses its own algorithm for forward dynamics. Its hallmark is an integrated solver that includes contact. Internally it exploits sparse factorization and is specialized for branching structures.
- **Drake**: MultibodyPlant uses ABA. Contact is handled by a separate solver (time-stepping, hydroelastic, etc.).
- **Bullet (PyBullet)**: builds on Featherstone's ABA, with contact handled by a sequential impulse solver.

In code:

```python
# Pinocchio: forward dynamics (ABA)
import pinocchio as pin
import numpy as np

model = pin.buildModelFromUrdf("robot.urdf")
data = model.createData()

q = pin.randomConfiguration(model)
v = np.random.randn(model.nv)
tau = np.random.randn(model.nv)

# ABA: (q, v, τ) → q̈
qdd = pin.aba(model, data, q, v, tau)
print("Joint accelerations:", qdd)

# Verify: recompute with RNEA
tau_check = pin.rnea(model, data, q, v, qdd)
print("Torque error:", np.linalg.norm(tau - tau_check))  # ≈ 0
```

RNEA and ABA are inverses of each other. RNEA(q, v, ABA(q, v, τ)) ≈ τ holds (within floating-point error).

> **Further reading**
> - Featherstone, *Rigid Body Dynamics Algorithms*, Ch. 7 — the original description of ABA
> - MuJoCo documentation: Computation (https://mujoco.readthedocs.io/en/latest/computation/) — describes MuJoCo's dynamics pipeline
> - Drake MultibodyPlant tutorial (https://drake.mit.edu/doxygen_cxx/classdrake_1_1multibody_1_1_multibody_plant.html) — the dynamics computation API in Drake

---

## 5.6 Contact Dynamics

The moment a robot makes contact with its environment, dynamics becomes one step more complicated. Dynamics in free space is expressed cleanly as an ODE (ordinary differential equation), but once contact is introduced, inequality constraints and discontinuities appear.

### Rigid Contact vs. Compliant Contact

There are two large frameworks for modeling contact:

**Rigid contact:**
- Directly imposes the constraint that bodies do not interpenetrate.
- Contact forces come out as the Lagrange multipliers of the constraint.
- Mathematically clean but numerically difficult — discontinuities appear at contact/non-contact transitions, and handling them requires solving an LCP (Linear Complementarity Problem) or NCP (Nonlinear Complementarity Problem).
- LCP time-stepping methods of the Stewart-Trinkle family belong to this category.

**Compliant contact:**
- Places a virtual spring-damper at the contact surface. It generates a restoring force proportional to penetration depth.
- Numerically stable and easy to implement.
- Increasing spring stiffness approaches rigid contact, but the integrator's time step must shrink accordingly (stiff ODE).
- MuJoCo's default contact model belongs to this family.

### Coulomb Friction Model

Where there is contact, there is friction. The most basic friction model is Coulomb friction:

```
|f_t| ≤ μ f_n          (static friction)
|f_t| = μ f_n, f_t ∥ -v_t  (sliding friction)
```

Here f_t is the tangential friction force, f_n is the normal force, μ is the friction coefficient, and v_t is the tangential relative velocity.

Problems with this model:
- The transition from static to sliding friction is discontinuous.
- In 3D the friction cone is nonlinear. Linearizing it produces a friction pyramid, which loses accuracy.
- Painleve's paradox: under certain conditions, rigid contact + Coulomb friction admits no solution, or a non-unique one.

### Why Contact-Rich Manipulation Is Hard

Why are tasks like grasping, rotating, and inserting objects (peg-in-hole, in-hand manipulation, etc.) so difficult?

1. **Hybrid dynamics**: the contact mode changes frequently (contact/no-contact, stick/slip). Each mode has different dynamics, and predicting mode-switch timing is hard.
2. **Discontinuous dynamics**: state can change discontinuously at mode transitions (impact).
3. **Sensitivity to parameters**: without accurate values of the friction coefficient, contact stiffness, and the like, the sim-to-real gap grows large.
4. **Combinatorial complexity**: as the number of contact points grows, combinations of contact/separation and stick/slip grow exponentially. The exact mode count depends on the friction model and how tangential directions are discretized.

### Why Contact Handling Differs Across Simulators

Because contact can be approximated numerically in several ways. Each simulator picks a different trade-off between accuracy, speed, and stability:

- **MuJoCo**: compliant contact + convex optimization. Fast and stable, but not physically exact. In particular, interpenetration is allowed and treated as part of "soft contact". This stability is one reason MuJoCo is popular as an RL environment.
- **Drake**: compliant point contact (TAMSI/SAP discrete solvers) and hydroelastic contact, both of which are compliant approaches. More physically rigorous but potentially more expensive. Hydroelastic contact even computes the pressure distribution over the contact surface.
- **Bullet**: velocity-level LCP + sequential impulse. Originating from games/VR, the engine is optimized for speed. Contact accuracy depends on the settings and the task, so how it ranks against MuJoCo and Drake has to be compared directly on the target task.
- **DART**: LCP-based rigid contact. Academically rigorous, but with a smaller user base than MuJoCo or Drake.

The choice of simulator depends on the research goal. MuJoCo is widely used for locomotion RL, while both Drake and MuJoCo can represent contact-rich manipulation. Choose from the required contact model, gradients, throughput, target hardware, and validation cases rather than from a universal ranking.

```python
# Accessing contact information in MuJoCo
import mujoco
import numpy as np

model = mujoco.MjModel.from_xml_path("scene.xml")
data = mujoco.MjData(model)

mujoco.mj_step(model, data)

# Number of contacts
n_contacts = data.ncon
print(f"Number of contacts: {n_contacts}")

# Information about each contact
for i in range(n_contacts):
    contact = data.contact[i]
    print(f"Contact {i}:")
    print(f"  Position: {contact.pos}")
    print(f"  Normal: {contact.frame[:3]}")  # contact normal
    print(f"  Signed distance (negative means penetration): {contact.dist}")
    print(f"  Geom pair: ({contact.geom1}, {contact.geom2})")
```

> **Further reading**
> - Stewart, "Rigid-Body Dynamics with Friction and Impact", SIAM Review 2000 — the mathematical foundation of contact dynamics
> - Todorov, "Convex and analytically-invertible dynamics with contacts and constraints", ICRA 2014 — the paper behind MuJoCo's contact model
> - [Todorov et al., "MuJoCo: A Physics Engine for Model-Based Control" (IROS 2012)](https://ieeexplore.ieee.org/document/6386109) — the original paper describing MuJoCo's convex contact formulation and velocity stepping.
> - Drake's contact model documentation (https://drake.mit.edu/doxygen_cxx/group__hydroelastic__user__guide.html) — describes hydroelastic contact
> - Russ Tedrake, *Underactuated Robotics*, Ch. "Contact" (https://underactuated.csail.mit.edu/) — an introduction to contact dynamics

---

## 5.7 Advanced: Featherstone Algorithms and Spatial Algebra

From here on the material is at the graduate level. Featherstone's spatial vector algebra is a mathematical framework for expressing dynamics algorithms concisely and efficiently.

### Spatial Vectors (6D Vectors)

The motion of a rigid body in 3D space is translation (3 DOF) + rotation (3 DOF) = 6 DOF. A spatial vector bundles this into a single 6D vector.

**Motion vector (spatial velocity, twist):**
```
v = [ω; v_O]
```
The top 3 entries are the angular velocity (ω); the bottom 3 are the linear velocity at the reference point O (v_O).

**Force vector (spatial force, wrench):**
```
f = [n_O; f]
```
The top 3 entries are the moment about the reference point O (n_O); the bottom 3 are the force (f).

The key advantage of this notation: the inner product of spatial velocity and spatial force is exactly power.
```
P = f^T v = n_O · ω + f · v_O
```

This is no accident — spatial vectors are designed to have this property.

### Spatial Inertia

The 6×6 spatial inertia matrix bundles mass, CoM position, and rotational inertia into a single matrix:

```
I_sp = [ I_cm + m·[c]×[c]×^T    m·[c]× ]
       [ m·[c]×^T                 m·1    ]
```

Here m is the mass, c is the vector to the CoM, I_cm is the rotational inertia about the CoM, and [c]× is the skew-symmetric matrix of c.

Advantages of spatial inertia:
- Inertias of multiple rigid bodies expressed in the same frame about the same reference point combine by addition: I_composite = I_1 + I_2 + ...
- Coordinate transformation is a single congruence transform: I_B = X^T I_A X

### Spatial Vector Form of RNEA and ABA

The pseudo-code shown in §5.2 and §5.5 was in fact spatial vector notation. S[i] is the motion subspace of joint i (for a revolute joint, [e_z; 0]; for a prismatic joint, [0; e_z]); v[i] is a spatial velocity; f[i] is a spatial force.

With spatial vectors, revolute and prismatic joints are handled by the same code. This is why libraries like Pinocchio and Drake use spatial algebra internally.

### Accessing Spatial Quantities in Pinocchio

```python
import pinocchio as pin
import numpy as np

model = pin.buildModelFromUrdf("robot.urdf")
data = model.createData()

q = pin.randomConfiguration(model)
v = np.random.randn(model.nv)

# Forward kinematics + velocity computation
pin.forwardKinematics(model, data, q, v)

# Spatial velocity of each frame
for i in range(model.njoints):
    # Spatial velocity in the world frame
    v_world = pin.getVelocity(model, data, i, pin.ReferenceFrame.WORLD)
    print(f"Joint {i} spatial velocity (world): {v_world}")

# Composite Rigid Body Algorithm (CRBA): compute M(q)
M = pin.crba(model, data, q)
print("Mass matrix M(q):\n", data.M)

# Centroidal momentum matrix
pin.computeCentroidalMap(model, data, q)
Ag = data.Ag  # 6 x nv matrix
# h = Ag @ v is the centroidal momentum (linear + angular)
```

Using Pinocchio from C++:

```cpp
#include <pinocchio/algorithm/rnea.hpp>
#include <pinocchio/algorithm/aba.hpp>
#include <pinocchio/parsers/urdf.hpp>

pinocchio::Model model;
pinocchio::urdf::buildModel("robot.urdf", model);
pinocchio::Data data(model);

Eigen::VectorXd q = pinocchio::randomConfiguration(model);
Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv);
Eigen::VectorXd tau = Eigen::VectorXd::Random(model.nv);

// Inverse dynamics (RNEA)
Eigen::VectorXd tau_id = pinocchio::rnea(model, data, q, v, Eigen::VectorXd::Zero(model.nv));

// Forward dynamics (ABA)
Eigen::VectorXd qdd = pinocchio::aba(model, data, q, v, tau);
```

Pinocchio's C++ API is Eigen-based and exposes nearly the same interface as its Python API. kHz-rate real-time control uses the C++ API to avoid Python overhead.

> **Further reading**
> - Featherstone, *Rigid Body Dynamics Algorithms*, Ch. 2 — the original description of spatial vector algebra.
> - Featherstone, "A Beginner's Guide to 6-D Vectors" (IEEE Robotics & Automation Magazine, 2010) — a more accessible introduction than the textbook
> - Pinocchio GitHub (https://github.com/stack-of-tasks/pinocchio) — the source code itself is a good implementation example of spatial algebra

---

## 5.8 Advanced: Floating Base Systems

An industrial manipulator has its base bolted to the floor. Legged robots, drones, and underwater robots have a moving base. Its position and orientation add degrees of freedom and change the structure of the dynamics.

### Configuration of a Floating Base

For a fixed-base robot, the configuration is q ∈ R^n. For a floating-base robot, the configuration is:

```
q = [q_base; q_joints]
```

q_base is an element of SE(3) — position (3) + orientation (3, or 4 with a quaternion). This is why in Pinocchio the dimension of q (nq) and the dimension of v (nv) can differ (with a quaternion, nq = nv + 1).

Because q and v do not live in the same vector space, one cannot simply do q += v*dt when integrating or differencing. In Pinocchio one must use `pin.integrate(model, q, v*dt)`.

### Underactuated Systems

Floating-base systems are usually **underactuated**. Legged robots have no actuator directly attached to the base, so they must push against the ground with their feet to move it, and a quadrotor, although its rotors are mounted on the base, has to cover 6 DOF with 4 inputs. What decides underactuation is not the floating base itself but the number of independent inputs and the contact conditions. A free-flyer with thrusters along all 6 directions, or an omnidirectional multirotor, is fully actuated, and a legged robot can control all 6 base DOF as well when it has enough contact.

Splitting the manipulator equation into base and joints:

```
[ M_bb  M_bj ] [ a_base  ]   [ C_b ]   [ g_b ]   [  0  ]   [ J_{c,b}^T ]
[ M_jb  M_jj ] [ q̈_joints] + [ C_j ] + [ g_j ] = [ τ_j ] + [ J_{c,j}^T ] λ
```

The top component `0` of the input vector on the right-hand side indicates that no joint torque acts on the base. λ is the contact force, and J_{c,b} (6×k) and J_{c,j} (n×k) are the blocks of the contact Jacobian J_c split into its base columns and joint columns. Contact forces and gravity accelerate the base.

This constraint is what makes locomotion control hard. For a fixed-base manipulator, the desired joint torque can simply be commanded to the motors; a legged robot, in contrast, must generate appropriate contact forces to make the base move as desired.

### Centroidal Dynamics

Expressing the total system momentum at the center of mass (CoM) yields centroidal dynamics:

**Linear momentum:**
```
p = m v_CoM = Σ m_i v_i
```

**Angular momentum about CoM:**
```
L = Σ (r_i - r_CoM) × (m_i v_i) + I_i ω_i
```

**Time derivative of centroidal momentum:**
```
ṗ = m g + Σ f_contact
L̇ = Σ (r_contact - r_CoM) × f_contact
```

Why this matters in locomotion control:

1. **CoM dynamics determines balance.** At rest, the vertical projection of the CoM must lie inside the support polygon (the static stability condition). While walking, it is the ZMP (the center of pressure over the contact surface) that must stay inside the support polygon (the ZMP condition). More generally, centroidal momentum must be regulated appropriately.

2. **Dimensionality reduction.** The full dynamics of an n-DOF legged robot is n-dimensional, but centroidal dynamics is 6-dimensional (3 linear + 3 angular momentum). A common approach is to first plan the desired momentum trajectory in this 6D space, then decompose down to the full joint level.

3. **Direct connection to contact force planning.** As the equations above show, the rate of change of centroidal momentum is determined only by external forces (contact forces + gravity). Planning which contact force pattern produces the desired momentum trajectory is the central problem of locomotion.

```python
# Computing centroidal dynamics in Pinocchio
import pinocchio as pin
import numpy as np

model = pin.buildModelFromUrdf("humanoid.urdf", pin.JointModelFreeFlyer())
data = model.createData()

q = pin.randomConfiguration(model)
v = np.random.randn(model.nv)

# Centroidal momentum
pin.computeCentroidalMomentum(model, data, q, v)
h = data.hg  # 6D centroidal momentum; Pinocchio orders it [linear; angular]
print("Angular momentum:", h.angular)
print("Linear momentum:", h.linear)

# Centroidal momentum matrix: h = A_g(q) * v
pin.computeCentroidalMap(model, data, q)
Ag = data.Ag  # 6 x nv
h_check = Ag @ v
print("Centroidal momentum (via Ag):", h_check)

# CoM position and velocity
pin.centerOfMass(model, data, q, v)
print("CoM position:", data.com[0])
print("CoM velocity:", data.vcom[0])
```

### Structure of Centroidal-Dynamics-Based Locomotion Control

A typical modern legged-robot control pipeline has the following structure:

```
[Contact Schedule] → [Centroidal Trajectory Optimization] → [Whole-Body Control] → [Joint Torques]

Stage 1: Decide which foot touches the ground and when (gait pattern)
Stage 2: Plan CoM trajectory + contact forces consistent with centroidal dynamics
Stage 3: Compute torques that meet the centroidal target while satisfying joint-level constraints
Stage 4: Command torques to the motors
```

This structure applies to systems with a floating base. Drone trajectory optimization and underwater-robot control use similar structures.

> **Further reading**
> - Orin et al., "Centroidal Dynamics of a Humanoid Robot", Autonomous Robots 2013 — an analysis of centroidal dynamics for humanoids
> - Wensing et al., "Optimization-Based Control for Dynamic Legged Robots", IEEE T-RO 2024 (arXiv 2022) — a survey of locomotion control
> - Russ Tedrake, *Underactuated Robotics*, Ch. "Walking" (https://underactuated.csail.mit.edu/) — the relationship between underactuated systems and walking
> - Carpentier, Mansard, "Pinocchio: fast forward and inverse dynamics for poly-articulated systems" (https://github.com/stack-of-tasks/pinocchio) — Pinocchio's centroidal dynamics implementation

---

## 5.9 Further Reading

The recommended reading order depends on the reader's background.

**Undergraduate junior/senior, introduction to dynamics:**

> - Spong, Hutchinson, Vidyasagar, *Robot Modeling and Control* — an undergraduate introduction with a detailed derivation of the manipulator equation.
> - Craig, *Introduction to Robotics: Mechanics and Control* — a shop-floor perspective. Practical but relatively shallow on the mathematical side.

**Graduate level, when mathematically rigorous understanding is needed:**

> - Murray, Li, Sastry, *A Mathematical Introduction to Robotic Manipulation* (https://www.cds.caltech.edu/~murray/mlswiki/) — a rigorous treatment of dynamics from a Lie group/algebra perspective. The PDF is free, but difficult as a first text.
> - Featherstone, *Rigid Body Dynamics Algorithms* — the central reference for dynamics algorithms. All the core algorithms — spatial vector algebra, RNEA, ABA, composite rigid body algorithm — are here. Required reading for graduate students.

**Dynamics + control integrated:**

> - Russ Tedrake, *Underactuated Robotics* (https://underactuated.csail.mit.edu/) — covers how to use dynamics models in control and optimization. Lecture videos are also on MIT OCW. Free.

**Libraries and tools:**

> - Pinocchio (https://github.com/stack-of-tasks/pinocchio) — a C++/Python library for pure dynamics computation. Supports RNEA, ABA, CRBA, centroidal dynamics, analytical derivatives, and more. Autodiff via CasADi and CppAD is also available (Pinocchio 3.x).
> - Drake (https://drake.mit.edu/) — a framework integrating simulation + optimization + control. MultibodyPlant is the dynamics engine. Its powerful mathematical programming interface is particularly useful for trajectory optimization.
> - MuJoCo (https://mujoco.org/) — a physics simulator maintained by DeepMind and widely used for robot learning with contact.
> - PyBullet (https://pybullet.org/) — the Python interface to Bullet Physics. The low entry barrier makes it suitable for teaching, but contact physics accuracy and speed depend on the articulated-body setup, the contact settings, and the integration time step, so they have to be compared directly on the target task.

---

## Technical Timeline

```
1687 ── Newton's laws of motion (Principia Mathematica)
1788 ── Lagrange's analytical mechanics (Mécanique Analytique)
1965 ── Uicker's dynamics equations (symbolic, inefficient)
1980 ── Luh, Walker, Paul's recursive Newton-Euler algorithm (RNEA, O(n))
1983 ── Featherstone's Articulated Body Algorithm (ABA, O(n) forward dynamics)
1987 ── Featherstone formalizes spatial vector algebra
2000 ── Stewart's mathematical treatment of rigid contact dynamics (SIAM Review)
2004 ── ODE (Open Dynamics Engine) — early open-source physics engine
2012 ── MuJoCo released (Todorov, Erez, Tassa)
2015 ── Bullet Physics 2.x → PyBullet interface
2016 ── Pinocchio 1.0 released (LAAS-CNRS)
2021 ── DeepMind acquires MuJoCo and makes it free (source released May 2022 under Apache 2.0)
2022 ── Drake 1.0 (MIT → Toyota Research Institute)
2022 ── MuJoCo 2.3 released (the implicitfast integrator arrived in 2.3.x in 2023; the elliptic friction cone was available earlier)
2023 ── MuJoCo 3.0: MJX (JAX backend for GPU parallelism)
2024 ── Pinocchio 3.0 stable release: CasADi and CppAD autodiff support
```

---

## Summary

Practical points:

1. The manipulator equation `M(q)q̈ + C(q,q̇)q̇ + g(q) = τ` is the standard form for dynamics calculations.
2. Use RNEA for inverse dynamics (computing τ) and ABA for forward dynamics (computing q̈). Both are O(n).
3. Contact adds constraints and discontinuities, so the contact model and simulator choice must be considered together.
4. In floating-base systems, centroidal dynamics is the key tool.
5. In practice, use libraries such as Pinocchio or Drake, while checking the quantities and assumptions they implement internally.

This dynamics model leads directly to computed torque control, operational space control, and whole-body control.

---

# Ch.6 — Control Theory

Perception estimates the robot's state and environment; control uses that information to produce the desired motion. An incorrect control input can make a robot unstable or cause a collision, so the system needs an appropriate model and feedback.

---

## 6.1 Why Learn Control

Perception estimates the environment and state, whereas control computes actuator inputs that move the robot toward a target state.

Control is needed for three practical reasons:

- **Accurate position tracking**: Without feedback, a large current can drive a joint past its target angle and produce oscillation. A controller adjusts the input to reduce the error and reach the target smoothly.
- **Disturbance rejection**: Slippery floors, wind, and unexpected payloads introduce changes not captured by the model. Closing the sensor-actuator loop lets the system respond to them.
- **Safe contact**: An industrial robot working near people must limit contact forces as well as position error.

Control theory is broad; robotics practice usually needs the path from PID to state-space control, MPC, impedance control, and whole-body control.

This chapter assumes familiarity with matrix operations, eigenvalues, and differential equations.

---

## 6.2 PID Control

PID (Proportional-Integral-Derivative), proposed by Minorsky in 1922 for a ship steering system, has remained widely used in industrial control. Its simple structure and interpretable terms also make it a common first controller in control courses.

### Basic Structure

Define the error e(t) = r(t) - y(t). r(t) is the reference (target) and y(t) is the current output.

```
u(t) = Kp * e(t) + Ki * integral(e(τ)dτ, 0, t) + Kd * de(t)/dt
```

Role of each term:

- **P (Proportional)**: generates the control input in proportion to the current error. A large Kp gives a fast response but causes more overshoot and oscillation. Give a step input to a plant with no free integrator and the P term alone leaves a steady-state error. This is because as the error becomes small near the target, the control input also becomes small. If the plant does have a free integrator, the steady-state error for step tracking can be zero; constant disturbances such as gravity are a separate source of error.

- **I (Integral)**: proportional to the accumulated error. Its role is to eliminate steady-state error. It is essential when there are constant disturbances like gravity or friction. However, excessive use produces wind-up. The issue is that when the error has been accumulating for a long time and the control input is saturated, the accumulated integral value causes a large overshoot even after reaching the target. In practice, anti-windup logic must be implemented.

- **D (Derivative)**: proportional to the rate of change of the error. If the error is decreasing quickly, it reduces the control input to suppress overshoot. A kind of "brake." The problem is that differentiation is extremely sensitive to noise. On real systems with sensor noise, the D term must be passed through a low-pass filter. For this reason, the field often drops the D term entirely and uses PI control only.

### Python Implementation

```python
class PIDController:
    """Discrete-time PID controller. Includes anti-windup."""

self.integral = 0.0
        self.prev_error = 0.0
        self.prev_d_filtered = 0.0

def compute(self, error: float) -> float:
        # Proportional
        p_term = self.kp * error

# Integral (trapezoidal integration)
        self.integral += 0.5 * (error + self.prev_error) * self.dt
        i_term = self.ki * self.integral

# Control output
        output = p_term + i_term + d_term

# Output saturation + anti-windup (clamping)
        lo, hi = self.output_limit
        if output > hi:
            output = hi
            # Anti-windup: back out the integral on saturation
            self.integral -= 0.5 * (error + self.prev_error) * self.dt
        elif output < lo:
            output = lo
            self.integral -= 0.5 * (error + self.prev_error) * self.dt

self.prev_error = error
        self.prev_d_filtered = d_filtered
        return output

# Usage example: 1-DOF position control
import numpy as np

dt = 0.001  # 1 kHz control period
pid = PIDController(kp=100.0, ki=10.0, kd=5.0, dt=dt,
                    output_limit=(-50.0, 50.0))

position = 0.0
velocity = 0.0
mass = 1.0
target = 1.0

positions = []
for step in range(5000):
    error = target - position
    force = pid.compute(error)

# Simple second-order position dynamics: F = ma, with damping
    acceleration = (force - 0.5 * velocity) / mass
    velocity += acceleration * dt
    position += velocity * dt
    positions.append(position)
```

### Tuning Methods

**Ziegler-Nichols method**: a classical tuning method. Set Ki = 0 and Kd = 0, raise Kp, and find the critical gain Ku at which the system exhibits sustained oscillation and its oscillation period Tu. Then set the gains according to the following table.

```
PID:  Kp = 0.6 * Ku,  Ki = 2 * Kp / Tu,  Kd = Kp * Tu / 8
PI:   Kp = 0.45 * Ku,  Ki = 1.2 * Kp / Tu
P:    Kp = 0.5 * Ku
```

Ziegler-Nichols tuning can produce substantial overshoot. It is best used to obtain initial gains, followed by adjustment based on the measured response.

**Empirical tuning in practice**: the following sequence adjusts the gains empirically.

1. Set D and I to 0.
2. Raise P. Stop at the point where the system reacts quickly but does not oscillate.
3. If a steady-state error remains, raise I little by little. Watch out for wind-up.
4. If overshoot is large, add a little D. Check the noise filter.

When an adequate system model is available, tune the gains in simulation first and then transfer them to the hardware.

### Limits of PID

PID is powerful but has clear limits:

- **It is SISO (Single-Input Single-Output) only.** In systems with inter-joint coupling like a 6-axis robot arm, applying independent PID to each joint degrades performance. The motion of one joint acts as a disturbance on the others.
- **It is weak on nonlinear systems.** PID is a linear controller, whereas robot dynamics are nonlinear. It performs well only near an operating point.
- **It cannot handle constraints.** Within the PID structure there is no way to explicitly handle physical constraints such as torque limits, joint angle limits, or velocity limits.
- **It does not predict the future.** It reacts to the current error, accumulated error, and rate of change of the error. Without feedforward, tracking performance is limited.

PID is comparatively simple to implement and analyze. It can be sufficient when plant coupling is weak and the performance requirements are met, and PID-family controllers are also used for industrial robot joint servos.

---

## 6.3 State-Space Representation

PID sees only the input-output relationship. It does not know what is happening "inside" the system. The state-space representation explicitly describes the internal state of the system.

### Basic Form

Continuous-time linear system:

```
x_dot(t) = A * x(t) + B * u(t)    (state equation)
y(t)     = C * x(t) + D * u(t)    (output equation)
```

- x(t): state vector (n x 1). The minimal set of variables needed to fully describe the system.
- u(t): input vector (m x 1). The control input.
- y(t): output vector (p x 1). The measurable outputs.
- A: system matrix (n x n). Determines the intrinsic dynamics of the system.
- B: input matrix (n x m). The influence of the input on the state.
- C: output matrix (p x n). The mapping from state to output.
- D: direct transmission matrix (p x m). Zero in most physical systems.

For example, for the mass-spring-damper system (m * x_ddot + c * x_dot + k * x = F), taking the state as x1 = position and x2 = velocity gives:

```
A = [[0, 1], [-k/m, -c/m]]
B = [[0], [1/m]]
C = [[1, 0]]   (position measured only)
D = [[0]]
```

### Relationship to Transfer Functions

The transfer function is G(s) = C * (sI - A)^(-1) * B + D. Transfer functions are convenient for SISO systems, while state-space models expose coupling directly when robot controllers handle several inputs and outputs. The system's input-output structure and the purpose of the analysis determine which representation to use.

### Controllability

A system is controllable if, from any initial state, it can reach any final state in finite time. The controllability matrix:

```
C_ctrl = [B, A*B, A^2*B, ..., A^(n-1)*B]
```

If this matrix has rank n, the system is controllable. If the rank is less than n, there exist states unreachable by the control input. A stabilizing infinite-horizon LQR solution requires stabilizability rather than full controllability. Conditions such as detectability through the state cost and a positive-definite input cost must also be checked.

### Observability

A system is observable if the initial state x(0) can be uniquely determined by observing the output y(t). The observability matrix:

```
O = [C; C*A; C*A^2; ...; C*A^(n-1)]
```

If the rank is n, the system is observable. If it is not observable, state estimation (observer, Kalman filter) will not work properly.

### Why Move from PID to State-Space

Controlling each joint independently with PID ignores the dynamic coupling between joints. Even in a 2-DOF robot arm, when one joint moves quickly, centrifugal and Coriolis forces act on the other. Treating this as a disturbance makes the I term in PID work hard to compensate, but the response is slow and performance is poor.

In state-space, the entire system is described by a single model, and the control input is computed by considering all state variables simultaneously. This becomes the foundation of LQR and MPC in the next sections.

```python
import numpy as np
from scipy import signal
import control  # pip install control

# Inverted pendulum state-space model
# State: [x, x_dot, theta, theta_dot]
# x: cart position, theta: pendulum angle (from vertical)
M = 1.0    # Cart mass (kg)
m = 0.1    # Pendulum mass (kg)
l = 0.5    # Pendulum length (m)
g = 9.81   # Gravity (m/s^2)

# Linearized state-space matrices (around theta ≈ 0)
A = np.array([
    [0, 1, 0, 0],
    [0, 0, -m * g / M, 0],
    [0, 0, 0, 1],
    [0, 0, (M + m) * g / (M * l), 0]
])
B = np.array([[0], [1 / M], [0], [-1 / (M * l)]])
C = np.array([[1, 0, 0, 0],
              [0, 0, 1, 0]])  # Measure cart position and pendulum angle
D = np.zeros((2, 1))

# Check controllability
ctrb_matrix = control.ctrb(A, B)
print(f"Controllability matrix rank: {np.linalg.matrix_rank(ctrb_matrix)}")  # 4 = controllable

# Check observability
obsv_matrix = control.obsv(A, C)
print(f"Observability matrix rank: {np.linalg.matrix_rank(obsv_matrix)}")  # 4 = observable

# System poles (eigenvalues of A)
eigenvalues = np.linalg.eigvals(A)
print(f"System poles: {eigenvalues}")
# If there is a pole with positive real part → unstable system (the inverted pendulum is such a case)
```

---

## 6.4 LQR (Linear-Quadratic Regulator)

If PID relies on "experience and tuning," LQR is a controller based on "optimization." The control input that minimizes a given cost function is obtained analytically.

### Cost Function

```
J = integral_0^inf (x(t)^T * Q * x(t) + u(t)^T * R * u(t)) dt
```

- Q (n x n, positive semi-definite): penalty on state error. "How much is the state departing from zero disliked."
- R (m x m, positive definite): penalty on the control input. "How much is control energy to be saved."

Increasing Q makes the state converge to zero quickly, but the control input grows. Increasing R reduces the control input at the cost of slower state convergence. LQR adjusts the balance between the two.

### Tuning Q and R

Practical method: make Q and R diagonal matrices, and set each diagonal entry to the inverse square of the allowable range of the corresponding state or input.

```
Q_ii = 1 / (maximum allowable value of x_i)^2
R_jj = 1 / (maximum allowable value of u_j)^2
```

Example: cart position within 0.5 m, pendulum angle within 0.1 rad, force within 20 N:

```
Q = diag(1/0.5^2, 0, 1/0.1^2, 0) = diag(4, 0, 100, 0)
R = [1/20^2] = [0.0025]
```

This is only a starting point. Adjust afterward by running simulations.

### Algebraic Riccati Equation (ARE)

The optimal LQR gain K is obtained from the solution P of the following Algebraic Riccati Equation:

```
A^T * P + P * A - P * B * R^(-1) * B^T * P + Q = 0
```

Optimal state feedback gain: K = R^(-1) * B^T * P

Control law: u(t) = -K * x(t)

All eigenvalues of the closed-loop system (A - BK) are guaranteed to lie in the left half-plane. The result therefore provides a mathematical guarantee of stability.

### Python Implementation

```python
import numpy as np
from scipy.linalg import solve_continuous_are

# Use the inverted pendulum model from the previous section
M, m, l, g = 1.0, 0.1, 0.5, 9.81

A = np.array([
    [0, 1, 0, 0],
    [0, 0, -m * g / M, 0],
    [0, 0, 0, 1],
    [0, 0, (M + m) * g / (M * l), 0]
])
B = np.array([[0], [1 / M], [0], [-1 / (M * l)]])

# Cost function weights
Q = np.diag([4.0, 0.0, 100.0, 0.0])  # Position, velocity, angle, angular velocity
R = np.array([[0.0025]])

# Solve the ARE
P = solve_continuous_are(A, B, Q, R)

# Compute the optimal gain
K = np.linalg.inv(R) @ B.T @ P
print(f"LQR gain K: {K}")

# Check closed-loop poles
A_cl = A - B @ K
eigenvalues_cl = np.linalg.eigvals(A_cl)
print(f"Closed-loop poles: {eigenvalues_cl}")
# All real parts are negative → stable

def simulate_lqr(A, B, K, x0, dt=0.001, t_final=5.0):
    """Closed-loop LQR simulation (Euler integration)."""
    n_steps = int(t_final / dt)
    n = A.shape[0]
    x_history = np.zeros((n_steps, n))
    u_history = np.zeros((n_steps, 1))

x = x0.copy()
    for i in range(n_steps):
        u = -K @ x
        x_history[i] = x.flatten()
        u_history[i] = u.flatten()
        x_dot = A @ x + B @ u
        x = x + x_dot * dt

return x_history, u_history

# Initial condition: pendulum tilted 10 degrees
x0 = np.array([[0.0], [0.0], [np.radians(10)], [0.0]])
x_hist, u_hist = simulate_lqr(A, B, K, x0)

# Success if x_hist[:, 2] converges to 0
print(f"Final pendulum angle: {np.degrees(x_hist[-1, 2]):.4f} deg")
```

### Limits of LQR

- **A linear model is required.** Nonlinear systems have to be linearized around an operating point. Performance drops sharply away from the operating point.
- **Constraints cannot be handled explicitly.** Physical constraints such as torque limits or velocity limits cannot be encoded in the cost function. Once the control input saturates, optimality breaks down.
- **The full state is required.** Since u = -Kx, every state variable must be either measured or estimated (by an observer).
- **Not directly applicable to tracking.** Basic LQR is a regulator; it only solves the problem of driving the state to zero. Extensions are needed to track a time-varying target.

MPC emerges to overcome these limits.

---

## 6.5 MPC (Model Predictive Control)

MPC (Model Predictive Control) solves a finite-horizon optimization problem repeatedly to compute control inputs. Its ability to represent constraints directly makes it widely studied and applied in robot control.

### Basic Concept

At every time step k, perform the following:

1. Measure or estimate the current state x(k).
2. Predict N steps ahead using the model.
3. Find the input sequence {u(k), u(k+1), ..., u(k+N-1)} that minimizes the cost function. Constraints are explicitly included at this step.
4. Apply only the first input u(k); discard the rest.
5. At the next time step, return to step 1.

This is the "receding horizon" strategy. Since the optimization is re-solved each time, feedback effects against model error and disturbances arise naturally.

### Why MPC Is Useful in Robotics

- **Constraint handling**: torque limits, joint angle limits, velocity limits, and collision avoidance can be represented directly in the optimization. Basic PID and LQR do not include such constraints, although saturation handling, reference governors, and constrained-LQR extensions exist.
- **Nonlinear models**: Nonlinear MPC uses the nonlinear dynamics model as-is.
- **Future prediction**: rather than reacting to the current error, MPC predicts the future trajectory and responds proactively. A legged robot shifting its center of mass before taking the next step is based on this principle.
- **Multi-objective optimization**: multiple objectives fit into the cost function simultaneously. "Track the target trajectory while saving energy and respecting torque limits."

### Linear MPC vs Nonlinear MPC

**Linear MPC**: uses a linear model (x(k+1) = A*x(k) + B*u(k)). When the cost is quadratic and the constraints are linear, the problem becomes a QP. A feasible convex QP has a global optimum; actual solve time depends on problem size, sparsity, solver, and hardware.

**Nonlinear MPC (NMPC)**: uses a nonlinear dynamics model. The problem is generally non-convex and does not guarantee a global optimum. CasADi + IPOPT is one widely used combination of automatic differentiation and a general NLP solver; alternatives include acados, FORCESPRO, and SNOPT.

Choice in practice: if the system is sufficiently close to linear or the control period is very short, Linear MPC; if the nonlinearity is large and there is slack in the control period, NMPC.

### Real-Time Issues

A central challenge in MPC is repeatedly solving the optimization within a time budget. If MPC runs synchronously at the same 1 kHz rate as an inner loop, state handling and the QP must finish within 1 ms. Many systems instead run MPC as a slower outer loop with a faster tracking controller.

Major QP solvers:
- **OSQP** (https://osqp.org/): operator splitting based, strong on sparse QPs. First choice for most Linear MPC setups.
- **qpOASES**: active-set based, supports warm-starting, efficient for sequences of QPs.
- **ECOS/Clarabel**: handles up to second-order cone programming.

For NMPC:
- **CasADi** + **IPOPT**: a combination of automatic-differentiation modeling and a general interior-point NLP solver.
- **acados** (https://docs.acados.org/): CasADi-based but optimized for real time. Generates C code.

A synchronous solve that takes 5 ms cannot nominally complete more than 200 updates per second. End-to-end control timing also includes preprocessing, communication, and jitter; asynchronous solves or a separate inner loop change the architecture.

### Linear MPC Python Example

```python
import numpy as np
from scipy import sparse
import osqp

def linear_mpc(A, B, Q, R, Q_f, x0, N, x_min, x_max, u_min, u_max):
    """
    Linear MPC: convert to QP and solve with OSQP.

A, B: discrete-time system matrices
    Q: state cost (stage)
    R: input cost
    Q_f: terminal cost
    x0: current state
    N: prediction horizon
    x_min, x_max: state constraints
    u_min, u_max: input constraints
    """
    n = A.shape[0]  # State dimension
    m = B.shape[1]  # Input dimension

# Decision variable: z = [x(0), x(1), ..., x(N), u(0), ..., u(N-1)]
    n_var = (N + 1) * n + N * m

# --- Cost function matrices (P, q) ---
    # min 0.5 * z^T P z + q^T z
    P_blocks = [sparse.kron(sparse.eye(N), Q)]      # x(0) ~ x(N-1)
    P_blocks.append(Q_f)                              # x(N) terminal cost
    P_blocks.append(sparse.kron(sparse.eye(N), R))   # u(0) ~ u(N-1)
    P = sparse.block_diag(P_blocks, format='csc')
    q = np.zeros(n_var)

# --- Equality constraints: dynamics ---
    # x(k+1) = A*x(k) + B*u(k)
    # → A*x(k) + B*u(k) - x(k+1) = 0
    Ax_eq = sparse.kron(sparse.eye(N + 1), -sparse.eye(n), format='lil')  # format that allows slice assignment
    Au_shift = sparse.kron(sparse.eye(N, N + 1, 1), sparse.eye(n))
    # Fix: add A in the lower-left block
    for i in range(N):
        row_start = (i + 1) * n
        col_start = i * n
        Ax_eq[row_start:row_start + n, col_start:col_start + n] = A

Bu_eq = sparse.lil_matrix(((N + 1) * n, N * m))
    for i in range(N):
        Bu_eq[(i + 1) * n:(i + 2) * n, i * m:(i + 1) * m] = B
    Bu_eq = sparse.csc_matrix(Bu_eq)

A_eq = sparse.hstack([Ax_eq, Bu_eq], format='csc')
    l_eq = np.zeros((N + 1) * n)
    l_eq[:n] = -x0.flatten()  # Initial condition
    u_eq = l_eq.copy()

# --- Inequality constraints: state and input bounds ---
    A_ineq = sparse.eye(n_var, format='csc')
    l_ineq = np.concatenate([
        np.tile(x_min, N + 1),
        np.tile(u_min, N)
    ])
    u_ineq = np.concatenate([
        np.tile(x_max, N + 1),
        np.tile(u_max, N)
    ])

# --- Combine all constraints ---
    A_total = sparse.vstack([A_eq, A_ineq], format='csc')
    l_total = np.concatenate([l_eq, l_ineq])
    u_total = np.concatenate([u_eq, u_ineq])

# --- Solve with OSQP ---
    solver = osqp.OSQP()
    solver.setup(P, q, A_total, l_total, u_total,
                 warm_starting=True, verbose=False,
                 eps_abs=1e-6, eps_rel=1e-6)
    result = solver.solve()

if result.info.status != 'solved':
        print(f"MPC solve failed: {result.info.status}")
        return None, None

# Return only the first input
    u_opt = result.x[(N + 1) * n:(N + 1) * n + m]
    x_pred = result.x[:(N + 1) * n].reshape(N + 1, n)
    return u_opt, x_pred

# Usage example: 2D double integrator
dt = 0.1
A_d = np.array([[1, dt], [0, 1]])   # Discrete time
B_d = np.array([[0.5 * dt**2], [dt]])
n, m_ctrl = 2, 1

Q_mpc = sparse.diags([10.0, 1.0])
R_mpc = sparse.diags([0.1])
Q_f_mpc = sparse.diags([100.0, 10.0])  # Make terminal cost large

x0 = np.array([5.0, 0.0])  # Initial position 5 m, velocity 0
N_horizon = 20

x_min_val = np.array([-10.0, -5.0])
x_max_val = np.array([10.0, 5.0])
u_min_val = np.array([-1.0])   # Force limit
u_max_val = np.array([1.0])

u_opt, x_pred = linear_mpc(
    A_d, B_d,
    Q_mpc, R_mpc, Q_f_mpc,
    x0, N_horizon,
    x_min_val, x_max_val,
    u_min_val, u_max_val
)
print(f"Optimal control input: {u_opt}")
print(f"Predicted trajectory (position): {x_pred[:5, 0]}")
```

### Industry Cases

- **Boston Dynamics Atlas (2019~)**: a combination of MPC + Whole-Body Control. Nonlinear MPC predicts contact sequences, and WBC distributes joint torques in real time.
- **Unitree H1/G1 (2023~)**: a hybrid structure where a learning-based policy (reinforcement learning) generates high-level commands and MPC handles low-level trajectory tracking.
- **Figure 01 (2024)**: an LLM does task-level planning and MPC optimizes manipulation trajectories. An example of combining control with AI.

---

## 6.6 Impedance/Admittance Control

The control techniques covered so far focus mostly on "sending the position to a desired place." But the moment the robot physically contacts the environment, position control alone is not enough.

### Position Control vs Force Control vs Impedance Control

- **Position Control**: tracks a target position. Suitable in free space or in a compliant environment. In a stiff environment it is instead dangerous, because the contact force grows as the product of the environment stiffness and the position error. When a robot arm tries to pick up a cup from a table and the table height differs by even 1 mm, the position controller does not know and tries to push in, so excessive force is generated.

- **Force Control**: tracks a target force. Needed in contact tasks such as grinding and assembly. But pure force control is unstable when not in contact. It is also sensitive to force sensor noise.

- **Impedance Control**: controls the relationship between position and force. Makes the robot behave like a virtual spring-damper system. On contact with the environment, force arises naturally; in non-contact it behaves like position control.

### Virtual Spring-Damper Model

Impedance control expresses the desired mass-spring-damper relationship as:

```
F = M_d * (x_ddot_d - x_ddot) + D_d * (x_dot_d - x_dot) + K_d * (x_d - x)
```

Or a simplified version ignoring the inertia term:

```
F = K_d * (x_d - x) + D_d * (x_dot_d - x_dot)
```

- K_d: virtual stiffness. Large values give accurate position tracking but large forces on contact.
- D_d: virtual damping. Suppresses oscillation.
- M_d: virtual inertia. Usually hard to tune, so the inertia term is often omitted.

The task determines the appropriate stiffness K_d and damping D_d:
- Picking up a glass: low K_d (gentle), high D_d (stable).
- Tightening a bolt: high K_d (precise).
- Collaborating with a person: very low K_d (safe).

```python
import numpy as np

class ImpedanceController:
    """Cartesian-space impedance controller (1-DOF simplified)."""

def __init__(self, k_d: float, d_d: float, m_d: float = 0.0):
        self.k_d = k_d   # Virtual stiffness (N/m)
        self.d_d = d_d   # Virtual damping (N*s/m)
        self.m_d = m_d   # Virtual inertia (kg)

def compute_force(self, x_d, x, x_dot_d, x_dot,
                      x_ddot_d=0.0, x_ddot=0.0) -> float:
        """Compute force according to the target impedance relation."""
        f = (self.k_d * (x_d - x)
             + self.d_d * (x_dot_d - x_dot)
             + self.m_d * (x_ddot_d - x_ddot))
        return f

# Simulation: robot approaches a wall and makes contact
dt = 0.001
controller = ImpedanceController(k_d=500.0, d_d=50.0)

# Robot + environment
robot_mass = 2.0
position = 0.0
velocity = 0.0
target_position = 0.15  # Target position
wall_position = 0.10    # Wall position (closer than the target)
wall_stiffness = 10000.0  # Wall stiffness

positions = []
forces = []
contact_forces = []

for step in range(10000):
    # Environment contact force
    if position > wall_position:
        f_env = -wall_stiffness * (position - wall_position)
    else:
        f_env = 0.0

# Impedance control output
    f_ctrl = controller.compute_force(
        x_d=target_position, x=position,
        x_dot_d=0.0, x_dot=velocity
    )

# Dynamics
    acceleration = (f_ctrl + f_env) / robot_mass
    velocity += acceleration * dt
    position += velocity * dt

positions.append(position)
    forces.append(f_ctrl)
    contact_forces.append(-f_env)

# Result: position stabilizes near wall_position
# Without crushing the wall, pushing with an appropriate contact force
print(f"Final position: {positions[-1]:.4f} m (wall: {wall_position} m)")
print(f"Final contact force: {contact_forces[-1]:.2f} N")
# Pure position control would have hit the wall with 10000 N/m * 0.05 m = 500 N
```

### Admittance Control

If impedance control is "position deviation → force output," admittance control is the opposite: "force input → position output."

```
x_d_new = x_d + (1 / K_d) * F_ext      # stiffness term: force -> position offset
x_dot_d = (1 / D_d) * F_ext             # damping term: force -> desired velocity (not the force derivative)
```

More precisely, the measured external force F_ext is fed into a virtual impedance model to modify the target position, and the modified target is passed to the existing (high-stiffness) position controller.

Why admittance control is widely used on industrial robots: industrial robots already have very precise position controllers built in, and torque cannot usually be commanded directly from outside. So measuring the external force with a force sensor (F/T sensor) and modifying the position command — the admittance approach — is more practical.

On research robots with torque control (such as Franka Emika Panda), impedance control is more natural.

---

## 6.7 Advanced: Whole-Body Control

Humanoid and quadruped robots have dozens of joints, must manage multiple contact points (feet, hands) simultaneously, and must maintain balance. In such systems, "put a PID on each joint" is practically meaningless. Integrated control at the whole-body level is required.

### Task-Space vs Joint-Space

- **Joint-space control**: controls the joint angles q directly. Simple, but to achieve task-level goals (end-effector position, center-of-mass position) inverse kinematics (IK) must be solved first.

- **Task-space control**: controls directly in task coordinates (Cartesian position, orientation). Task goals are described naturally. Mapping to joint space is handled inside the controller.

### Operational Space Control (Khatib, 1987)

Khatib's Operational Space Framework underpins task-space control. It derives the dynamics directly in task space.

Joint-space dynamics:

```
M(q) * q_ddot + C(q, q_dot) * q_dot + g(q) = tau + J^T * F_ext
```

Conversion to task space:

```
Lambda(q) * x_ddot + mu(q, q_dot) * x_dot + p(q) = F + F_ext
```

Here Lambda = (J * M^(-1) * J^T)^(-1) is the task-space inertia matrix.

Joint torques to achieve a desired task-space acceleration x_ddot_d:

```
tau = J^T * Lambda * (x_ddot_d - J_dot * q_dot) + C * q_dot + g(q)
```

Combining impedance control on top of this framework realizes desired dynamic behavior (impedance) in task space.

### QP-Based Whole-Body Control

Modern WBC handles multiple tasks simultaneously by solving a QP (Quadratic Program) at every control cycle.

Basic structure:

```
minimize    || J_task * q_ddot - x_ddot_d ||^2  (task tracking)
subject to  M(q)*q_ddot + h(q,q_dot) = S^T*tau + J_c^T*F_c  (dynamics)
            F_c ∈ friction cone                   (contact force constraint)
            tau_min ≤ tau ≤ tau_max               (torque limits)
```

Here:
- J_task: task Jacobian
- J_c: contact Jacobian
- F_c: contact force
- S: selection matrix (removes underactuated DoFs)

**Multi-task priority**: on real robots multiple tasks conflict. For example, "send the right hand to a target position" + "maintain balance" + "respect joint limits." Priorities are assigned:

1. Highest priority: contact constraints (feet must stay on the ground), joint limits.
2. High priority: balance maintenance (CoM control).
3. Medium priority: end-effector position control.
4. Low priority: posture maintenance (null-space).

To implement this as a strict hierarchy, use null-space projection, or solve the QP at each priority level sequentially (hierarchical QP). Alternatively, soft priorities combine them into a single QP with different weights.

### Contact-Consistent Control

On legged robots, contact forces must be physically plausible:

- **Unilateral contact**: a foot cannot pull the ground. F_z >= 0.
- **Friction cone**: the tangential force must be less than the normal force times the friction coefficient. sqrt(F_x^2 + F_y^2) <= mu * F_z.
- **ZMP/CoP constraint**: the center of pressure must lie within the support polygon to avoid tipping over.

Putting all these constraints into the QP yields physically feasible control inputs. The friction cone is originally nonlinear (second-order cone), but approximated as a polyhedron (linearized friction cone) it fits into a QP.

```python
import numpy as np

def linearized_friction_cone(mu, n_edges=8):
    """
    Polyhedral approximation of the friction cone.
    Returns: constraint matrix in the form A_cone * F <= 0.
    F = [fx, fy, fz]^T
    """
    A_rows = []
    for i in range(n_edges):
        theta = 2 * np.pi * i / n_edges
        # mu * fz >= cos(theta)*fx + sin(theta)*fy
        # → cos(theta)*fx + sin(theta)*fy - mu*fz <= 0
        row = [np.cos(theta), np.sin(theta), -mu]
        A_rows.append(row)
    # fz >= 0 → -fz <= 0
    A_rows.append([0, 0, -1])
    return np.array(A_rows)

# Friction coefficient 0.7, octagonal approximation
A_friction = linearized_friction_cone(mu=0.7)
print(f"Friction cone constraint matrix shape: {A_friction.shape}")
# (9, 3) → 9 linear inequalities approximate the 3D friction cone
```

---

## 6.8 Advanced: Lyapunov Stability and Adaptive Control

Once a controller has been designed, "does this controller really make the system stable?" must be proven. Working in simulation and mathematically guaranteed stability are entirely different matters. Lyapunov theory is the central tool for this proof.

### Lyapunov Stability

For a nonlinear system x_dot = f(x), let the origin be an equilibrium (f(0) = 0).

Lyapunov's direct method: if a function V(x) satisfies the following, the origin is stable.

1. V(0) = 0
2. V(x) > 0 for all x != 0 (positive definite)
3. V_dot(x) = dV/dx * f(x) <= 0 (non-increasing)

If V_dot(x) < 0, the origin is asymptotically stable — the state converges to the origin over time.

Physical intuition: V(x) is energy. If the energy is always positive and decreases over time, the system converges to the equilibrium that minimizes the energy.

The hard part: finding V(x). There is no general methodology. For mechanical systems, mechanical energy (kinetic + potential) is a natural Lyapunov function candidate. For linear systems, V(x) = x^T * P * x (where P is the solution of the ARE) serves as a Lyapunov function. This is where the LQR stability proof comes from.

### Adaptive Control

Used when the model parameters are not precisely known. For example, the payload mass carried by a robot arm is unknown, or the friction coefficient changes over time.

Basic idea: embed a parameter estimator inside the controller and run control and estimation simultaneously.

Robot dynamics can be written in a form linear in the parameters:

```
M(q)*q_ddot + C(q,q_dot)*q_dot + g(q) = Y(q, q_dot, q_ddot) * theta
```

Here Y is the regressor matrix and theta is the dynamics parameter vector (mass, inertia, friction, etc.).

Adaptive control law:

```
tau = Y * theta_hat - K_d * s
theta_hat_dot = -Gamma * Y^T * s
```

Here s is the sliding variable, theta_hat is the parameter estimate, and Gamma is the adaptation gain matrix.

With a suitable Lyapunov function (V = 0.5*s^T*M*s + 0.5*theta_tilde^T*Gamma^(-1)*theta_tilde), V_dot <= 0 can be shown and the tracking error proven to converge to zero. Note that theta_hat is not guaranteed to converge to the true theta. Only the tracking error converges.

### Robust Control

Used when there is model uncertainty but the bound is known.

- **H-infinity control**: optimizes performance against the worst-case disturbance. The guarantee takes the form of a bound on the induced L2 norm: the energy gain from disturbance to error stays at or below $\gamma$. It is not an absolute bound on the error, since the error can grow in proportion as the energy of the disturbance grows. The math is heavy (Riccati inequalities, LMI) and tends to be conservative.

- **Sliding Mode Control**: drives the state onto a sliding surface in finite time, then follows the desired dynamics on the sliding surface. Very robust to model uncertainty. The issue is chattering: high-frequency switching near the sliding surface stresses the actuator. Mitigated with a boundary layer approach or higher-order sliding mode.

### When to Use, When Not to Use

| Situation | Recommended | Not recommended |
|------|------|--------|
| Accurate model, sufficiently linear | LQR, MPC | Adaptive control (overdesign) |
| Large parameter uncertainty | Adaptive control | Relying on PID alone |
| Known uncertainty bound | Robust control (H-inf) | Adaptive control (unnecessary) |
| Safety certification required | Lyapunov-based proofs | "It worked in simulation so OK" |
| Rapid prototyping | PID + feedforward | H-infinity from the start |

Adaptive control and sliding mode are selected according to the structure of the uncertainty and the required guarantees. A stability claim requires a Lyapunov function or comparable mathematical argument. Functional safety standards call for hazard analysis, the assignment of a safety level, and a body of verification and validation evidence; they do not prescribe any particular proof technique. Analyses of this kind are a strong way to build that evidence.

---

## 6.9 Further Reading

> **Åström & Murray, "Feedback Systems: An Introduction for Scientists and Engineers"**
> https://fbswiki.org/
> Free PDF. An introductory text that connects PID, state-space methods, and frequency response.

> **Steve Brunton, "Control Bootcamp" (YouTube)**
> https://www.youtube.com/playlist?list=PLMrJAkhIeNNR20Mz-VpzgfQs5zrYi085m
> Explains state-space, controllability, observability, and LQR in videos of roughly 15 minutes each. Useful for orienting the concepts before following a textbook derivation.

> **Slotine & Li, "Applied Nonlinear Control"**
> A textbook on nonlinear control, Lyapunov stability, and adaptive control. It extends the material in Section 6.8 and is currently out of print.

> **Russ Tedrake, "Underactuated Robotics" (MIT OCW)**
> https://underactuated.csail.mit.edu/
> Free online textbook and lectures. Goes deep on MPC, trajectory optimization, and the connection between control and planning. Also the theoretical background of the Drake library.

> **python-control library**
> https://python-control.readthedocs.io/
> A Python library for analyzing and designing control systems. The Python alternative to MATLAB's Control System Toolbox. Supports Bode plots, root locus, and state-space analysis.

> **CasADi**
> https://web.casadi.org/
> A widely used framework connecting automatic differentiation to NLP solvers such as IPOPT and SNOPT. It offers Python, MATLAB, and C++ interfaces.

> **OSQP (Operator Splitting Quadratic Program)**
> https://osqp.org/
> A QP solver for Linear MPC. Fast, robust, and capable of code generation, allowing deployment on embedded systems. C implementation with bindings for Python, MATLAB, Julia.

> **Key papers**
> - [Hogan, "Impedance Control: An Approach to Manipulation" (ASME JDSMC 1985)](https://doi.org/10.1115/1.3140702) — the original paper on impedance control. Presents a framework that unifies position control and force control.
> - [Khatib, "A Unified Approach for Motion and Force Control of Robot Manipulators: The Operational Space Formulation" (IEEE RA 1987)](https://doi.org/10.1109/JRA.1987.1087068) — the original paper on Operational Space Control. Foundations of task-space dynamics derivation and control.
> - [Khazoom et al., "Tailoring Solution Accuracy for Fast Whole-Body MPC" (RA-L 2024, arXiv:2407.10789)](https://arxiv.org/abs/2407.10789) — a recent approach to real-time whole-body MPC.

---

## Technical Timeline

```
1922 ── PID control concept formalized (Minorsky)
1960 ── State-space theory (Kalman)
1960 ── LQR (Kalman)
1985 ── Impedance Control concept (Hogan)
1987 ── Operational Space Control (Khatib)
1990s ─ Robust control (H-infinity) theory established; early application attempts
2004 ── Real-time MPC becomes practical
2019 ── Boston Dynamics Atlas: MPC + WBC
2023 ── Unitree H1/G1: learning-based + MPC hybrid
2024 ── Figure 01: LLM + MPC + manipulation
```

---

The further reading develops the mathematical details of each technique. Run the code and change its parameters to observe how the system response changes.

---

# Ch.7 — Motion Planning & Trajectory Optimization

A robot moving from A to B needs a path that satisfies obstacle, joint-limit, and dynamic constraints. Finding such a path is motion planning; following it optimally over time is trajectory optimization.

---

## 7.1 Why Study Motion Planning

Suppose you tell a 6-axis robot arm, "pick up that cup." IK gives the target joint angles. But linearly interpolating the joints from the current pose to the target pose can drive the arm through the table or into its own body. A straight line in joint space is not a straight line in task space.

Motion planning answers:
- Does a collision-free path to the goal exist?
- If so, what is the shortest / fastest / smoothest path?
- Can that path be followed while satisfying dynamic constraints (torque limits, velocity limits)?

---

## 7.2 Configuration Space (C-space)

Represent all possible states of the robot as a single space.

**Joint space = Configuration space**: for an n-DOF robot, the configuration is q = (q1, q2, ..., qn). The n-dimensional space where q lives is the C-space.

**C-space obstacle**: task-space (3D) obstacles transformed into the C-space. Configurations that fall inside the obstacle region in C-space are in collision.

Why think in C-space: the robot is not a point. Checking in 3D space that every link avoids the obstacles means computing FK at each configuration and running a collision check. In C-space the robot becomes a "point," and obstacle avoidance reduces to finding a path for a point.

The problem is that computing the exact shape of a C-space obstacle is hard. In practice you do not obtain C-space obstacles explicitly; you use a collision checker that tests whether a given configuration is in collision.

---

## 7.3 Graph Search-Based Planning

The most classical approach: discretize the C-space and find a path with a graph search algorithm.

### Dijkstra's Algorithm

Finds shortest paths in a graph with nonnegative edge weights. With a binary heap and adjacency lists, its time complexity is $O((V+E)\log V)$; a single-goal search may stop once the goal is settled rather than processing every edge.

### A* Algorithm

Dijkstra plus a heuristic. An estimated distance to the goal guides the search direction. Graph-search A* returns an optimal path when the heuristic is admissible and consistent. A useful heuristic can reduce node expansions, but A* is not guaranteed to run faster than Dijkstra on every graph.

```python
import heapq
import numpy as np

def astar_2d(grid, start, goal):
    """A* path search on a 2D grid.
    grid: 0=free, 1=obstacle
    """
    rows, cols = grid.shape
    open_set = [(0, start)]  # (f_score, node)
    came_from = {}
    g_score = {start: 0}

def heuristic(a, b):
        return np.hypot(a[0] - b[0], a[1] - b[1])  # Euclidean distance consistent with diagonal step costs

neighbors = [(-1,0), (1,0), (0,-1), (0,1), (-1,-1), (-1,1), (1,-1), (1,1)]

while open_set:
        f, current = heapq.heappop(open_set)
        if current == goal:
            # reconstruct the path
            path = [current]
            while current in came_from:
                current = came_from[current]
                path.append(current)
            return path[::-1]

return None  # no path
```

### Pros and Cons

An algorithm that searches every reachable cell in a finite grid is complete for that **discrete problem**. Grid resolution can still miss a narrow passage in the continuous space, and the method suffers from the **curse of dimensionality**. Discretizing the C-space of a 6-DOF robot arm into 100 cells per axis gives 100^6 = 10^12 cells.

Sampling-based planners emerged to address this.

---

## 7.4 Sampling-Based Planners

Rather than covering the C-space with a uniform fixed grid, draw samples and search for a path. This is an important option in high-dimensional spaces and is also combined with optimization-based planning and search.

### RRT (Rapidly-exploring Random Tree)

Proposed by LaValle (1998). The idea is simple:

```
1. Initialize the tree at the start.
2. Sample a random point q_rand in the C-space.
3. Find the node q_near in the tree closest to q_rand.
4. Extend from q_near toward q_rand by step_size to produce q_new.
5. If the path q_near → q_new is collision-free, add it to the tree.
6. If q_new is near the goal, terminate. Otherwise go back to 2.
```

```python
import numpy as np

class RRT:
    def __init__(self, start, goal, obstacle_fn, bounds, step_size=0.3, max_iter=5000):
        self.start = np.array(start)
        self.goal = np.array(goal)
        self.obstacle_fn = obstacle_fn  # config → bool (True if in collision)
        self.bounds = np.array(bounds)  # [[min_q1, max_q1], ...]
        self.step_size = step_size
        self.max_iter = max_iter
        self.nodes = [self.start]
        self.parents = {0: -1}

def sample_random(self):
        # sample the goal with probability 10% (goal bias)
        if np.random.random() < 0.1:
            return self.goal
        return np.random.uniform(self.bounds[:, 0], self.bounds[:, 1])

def nearest(self, q):
        dists = [np.linalg.norm(node - q) for node in self.nodes]
        return np.argmin(dists)

if self.collision_free(q_near, q_new):
                idx_new = len(self.nodes)
                self.nodes.append(q_new)
                self.parents[idx_new] = idx_near

if np.linalg.norm(q_new - self.goal) < self.step_size:
                    # reconstruct the path
                    path = [q_new]
                    idx = idx_new
                    while self.parents[idx] != -1:
                        idx = self.parents[idx]
                        path.append(self.nodes[idx])
                    return path[::-1]

return None  # failure
```

### RRT* (Optimal RRT)

Karaman & Frazzoli (2011). RRT finds a solution but not an optimal one. RRT* re-wires nearby nodes when adding a new node, guaranteeing asymptotic optimality. As the number of samples goes to infinity, it converges to the optimal path.

In practice, RRT* finds better paths than RRT but converges slowly. Under real-time deadlines, RRT-Connect is often more practical.

### PRM (Probabilistic Roadmap)

Kavraki et al. (1996). RRT is single-query (one start-goal pair at a time); PRM is suited to multi-query settings.

Phase 1 (offline): sample many points in the C-space and connect nearby points with collision-free edges to build a roadmap (graph).
Phase 2 (online): connect start and goal to the roadmap and find a path by graph search (A*, etc.).

When many path queries are needed in the same environment (e.g., an industrial robot cell), PRM is efficient.

### RRT-Connect

Kuffner & LaValle (2000). Grow trees from the start and goal and connect them when they meet. It is widely used for finding an initial path quickly, and several MoveIt2 OMPL example configurations select `RRTConnect` as the default planner config. The actual default depends on the distribution and user configuration.

### The OMPL Library

Open Motion Planning Library (https://ompl.kavrakilab.org/). A C++ library from the Kavraki Lab (Rice University) providing dozens of sampling-based planners — RRT, RRT*, RRT-Connect, PRM, EST, KPIECE, and more.

OMPL itself does not perform collision checking. The user supplies a state validity checker. MoveIt2 combines OMPL with FCL (Flexible Collision Library) to form a complete motion planning pipeline.

```python
# OMPL-based motion planning in MoveIt2 (ROS2 Python API, simplified)
from moveit.planning import MoveItPy

moveit = MoveItPy(node_name="motion_planner")
arm = moveit.get_planning_component("manipulator")

# set the goal
arm.set_goal_state(configuration_name="home")

# plan (default: OMPL RRT-Connect)
plan_result = arm.plan()

if plan_result:
    # execute
    arm.execute()
```

> **Further reading**
> - [LaValle, "Planning Algorithms"](http://lavalle.pl/planning/) — free online textbook. The standard reference for motion planning.
> - [OMPL](https://ompl.kavrakilab.org/) — open-source motion planning library.
> - [MoveIt2 Tutorials](https://moveit.picknik.ai/) — hands-on motion planning guide on ROS2.

---

## 7.5 Trajectory Optimization

Sampling-based planners hand back a "collision-free path." But that path is:
- jagged (because of random sampling)
- oblivious to dynamics (only the kinematic path)
- without timing (no speed to follow it at)

Trajectory optimization fills the gap. It finds a trajectory that minimizes a cost function (time, energy, smoothness) while satisfying dynamic constraints, collision avoidance, and joint limits.

### Direct Collocation

Partition the trajectory into time intervals and treat the state and input at each interval as decision variables. Dynamics equations are handled as equality constraints.

```
minimize    Σ_k L(x_k, u_k) * dt                    (cost)
subject to  x_{k+1} = f(x_k, u_k)   for all k       (dynamics)
            g(x_k) <= 0              for all k       (inequality constraints: collisions, joint limits)
            x_0 = x_init                             (initial condition)
            x_N = x_goal                             (terminal condition)
```

Cast this as one large nonlinear program (NLP) and solve it with a solver such as IPOPT.

Pros: handles dynamics and constraints at the same time, smooth trajectories.
Cons: sensitive to the initial guess; non-convex, so it can fall into local optima.

### Direct Shooting

Drop the state from the decision variables and keep only the input sequence {u_0, u_1, ..., u_{N-1}} as decision variables. States are computed by dynamics simulation.

Fewer decision variables than collocation, but if the simulation is unstable (e.g., an inverted pendulum) the optimization becomes unstable too.

### CHOMP (Covariant Hamiltonian Optimization for Motion Planning)

Ratliff et al. (2009). Start from an initial trajectory (usually linear interpolation) and iteratively improve it by following the gradient of a collision cost plus a smoothness cost. A covariant gradient keeps the updates smooth.

Pros: intuitive, improves an existing trajectory incrementally.
Cons: struggles with narrow passages, local optima.

### TrajOpt

Schulman et al. (2014). Based on sequential convex optimization: each iteration solves a linear or quadratic approximation, while a trust region limits where that approximation is trusted. Because the original problem is non-convex, this does not guarantee a global optimum. Collision avoidance uses a signed-distance-based cost so that gradients are available.

### Trajectory Optimization with CasADi

CasADi is a widely used framework combining symbolic computation, automatic differentiation, and connections to NLP solvers. For trajectory optimization it is one option alongside Drake, direct solver APIs, and JAX-based implementations.

```python
import casadi as ca
import numpy as np

# Simple example: minimize squared control effort for a 1D double integrator over a fixed duration
# x = [position, velocity], u = force
# x_dot = [velocity, force/mass]

N = 50           # number of intervals
dt = 0.1         # time step
mass = 1.0

opti = ca.Opti()

# decision variables
X = opti.variable(2, N + 1)  # state trajectory
U = opti.variable(1, N)      # input trajectory

# cost: minimize squared control effort over a fixed duration
cost = 0
for k in range(N):
    cost += U[0, k]**2 * dt  # squared control effort integral
opti.minimize(cost)

# dynamics constraint (Euler integration)
for k in range(N):
    x_next = X[:, k] + ca.vertcat(X[1, k], U[0, k] / mass) * dt
    opti.subject_to(X[:, k + 1] == x_next)

# boundary conditions
opti.subject_to(X[:, 0] == ca.vertcat(0, 0))    # start: position 0, velocity 0
opti.subject_to(X[:, N] == ca.vertcat(1, 0))     # end: position 1, velocity 0

# input constraint
opti.subject_to(opti.bounded(-5.0, U, 5.0))

# state constraint (velocity limit)
opti.subject_to(opti.bounded(-2.0, X[1, :], 2.0))

# solver setup
opti.solver('ipopt', {'print_time': False}, {'print_level': 0})
sol = opti.solve()

x_opt = sol.value(X)
u_opt = sol.value(U)
print(f"Optimal trajectory - final position: {x_opt[0, -1]:.4f}")
print(f"Max force: {np.max(np.abs(u_opt)):.4f} N")
```

> **Further reading**
> - [Matthew Kelly, "An Introduction to Trajectory Optimization" (SIAM Review 2017)](https://www.matthewpeterkelly.com/research/MatthewKelly_IntroTrajectoryOptimization_SIAM_Review_2017.pdf) — solid tutorial comparing collocation and shooting.
> - [CasADi](https://web.casadi.org/) — automatic differentiation and NLP-solver integration.
> - [Drake Trajectory Optimization](https://drake.mit.edu/) — includes direct collocation examples.

---

## 7.6 MoveIt2: Motion Planning in Practice

MoveIt2 is a public ROS2-based motion-planning framework widely used in robot-arm research and applications.

**Architecture:**
- **Planning Scene**: manages the 3D model of the robot plus the environment (obstacles). The basis for collision checking.
- **Planning Pipeline**: call a planner such as OMPL → validate the path → time parameterization.
- **Move Group Interface**: the user-facing API. Abstracts goal setting, planning, and execution.

**OMPL integration**: OMPL is a representative planning-pipeline plugin available in MoveIt2. Planner type and parameters are set in `ompl_planning.yaml`, and other pipelines can also be selected.

```yaml
# ompl_planning.yaml example
manipulator:
  planner_configs:
    - RRTConnectkConfigDefault
    - RRTstarkConfigDefault
    - PRMkConfigDefault
  default_planner_config: RRTConnectkConfigDefault
  projection_evaluator: joints(joint1, joint2)
  longest_valid_segment_fraction: 0.01
```

**Pick-and-Place pipeline:**
1. Object recognition (Perception) → estimate the object's 6-DoF pose.
2. Grasp planning → decide the grasp location/pose.
3. Approach trajectory → plan motion to the approach point above the object.
4. Grasp → close the gripper.
5. Retreat trajectory → lift the object.
6. Place trajectory → plan motion to the placement location.
7. Release → open the gripper.

At every stage, MoveIt2 handles collision avoidance and joint limits automatically.

---

## 7.7 Advanced: Optimization-Based Planning

### Constrained Nonlinear Optimization

Trajectory optimization for real robots is mostly a constrained NLP:

```
minimize    Σ L(x_k, u_k) + Φ(x_N)
subject to  x_{k+1} = f(x_k, u_k)           (dynamics)
            h(x_k, u_k) = 0                  (equality constraints)
            g(x_k, u_k) <= 0                 (inequality constraints: collisions, torque limits, etc.)
```

IPOPT (Interior Point Optimizer) is the standard solver for this problem. CasADi uses IPOPT by default.

### Contact-Implicit Trajectory Optimization

Rather than fixing the contact mode in advance (what touches what, what is separated), let the optimization decide automatically. Useful for tasks with contact transitions, such as walking and grasping.

Include contact forces in the decision variables and add complementarity constraints:

```
F_n >= 0                   (contact force cannot pull)
d >= 0                     (object cannot go below the floor)
F_n * d = 0                (zero force when separated, zero distance when in contact)
```

Mathematically this is an MPCC (Mathematical Program with Complementarity Constraints), and it is hard to solve. Relaxation techniques or smoothed contact models are used.

Drake has no class that wraps this method as-is; it is built by adding the contact complementarity constraints directly to a `MathematicalProgram`.

### Connection to Real-Time Re-planning and MPC

In a static environment, planning once is enough; in a dynamic environment you must re-plan in real time. Trajectory optimization and MPC meet here.

MPC can be viewed as trajectory optimization over a short horizon. At every control cycle, optimize the trajectory over a short interval, apply only the first input, then optimize again. The MPC of the previous chapter is exactly this.

The difference: motion-planning trajectory optimization usually computes the full trajectory offline in one pass, while MPC recomputes a short interval online.

---

## 7.8 Advanced: Task and Motion Planning (TAMP)

To carry out "place the cup on the shelf":

1. Recognize where the cup is.
2. Decide a grasp pose that can pick up the cup.
3. Plan the sequence approach → grasp → lift → move → place.
4. Motion plan each stage.

Step 1 is perception, step 2 is a continuous geometric decision (which grasp pose to take), step 3 is **symbolic planning** (which actions, in what order), and step 4 is **motion planning** (which concrete trajectory to move along). TAMP combines symbolic and motion planning, and PDDLStream below handles the choice of continuous parameters — the kind step 2 calls for — by factoring it out into streams.

### PDDLStream

A TAMP framework developed at MIT. Symbolic actions are defined in PDDL (Planning Domain Definition Language); streams generate continuous parameters (grasp pose, placement pose).

### LLM-Based Task Planning

Recently, attempts to replace the symbolic planner with an LLM have been active:

- **SayCan** (Google, 2022): the LLM scores natural-language descriptions of possible actions, and an affordance model filters for actions executable in the current state. The product of the two picks the next action.
- **Code as Policies** (Google, 2023): the LLM generates robot control code directly. Natural-language command → Python code → robot execution.
- **Inner Monologue** (Google, 2022): completes a task through iterative dialogue between the LLM and environment feedback.

Practical limits: LLM-based TAMP is still experimental. Complex geometric constraints (manipulation in tight spaces, precision assembly) are hard for LLMs to handle, and traditional motion planners are still needed in the end. A realistic division of labor: LLM for high-level planning, motion planner for low-level execution.

TAMP assumes the environment dynamics and actions are deterministic. When both dynamics and observations are stochastic, see §7.9 Advanced: POMDP.

---

## 7.9 Advanced: Decision-Making Under Uncertainty (POMDP and Belief Space Planning)

§7.1–§7.8 assumed the robot knows its own state and the environment exactly. Real robots observe only partial information through noisy sensors. A robot that does not know which side of a symmetric corridor it is on, a situation where it is uncertain whether a door is open or closed — planning from the "current best-estimate state" fails in these cases. The plan must be built directly over the belief (the posterior distribution). Thrun, Burgard, and Fox's *Probabilistic Robotics* §15.2 and §16 address this problem.

### 7.9.1 Introduction: Three Paradigms

Three planners give different answers in the same environment. Take a left-right symmetric corridor with a Goal, a Pit, and a Robot.

Classical planning assumes the state is fully known and actions are deterministic. A* from §7.3 belongs here: compute the shortest path once, and no sensing is needed during execution.

**MDP (Markov Decision Process)**: full state observability, stochastic actions. A policy $\pi: s \to a$ maps every state to an action. In a narrow passage the planner can choose a wider route to reduce the risk of hitting a wall. Ch.8 §8.2 falls in this category.

**POMDP**: both actions and observations are stochastic. The policy $\pi: b \to a$ is defined over belief $b$. In the symmetric corridor the robot starts without knowing its position, so it deliberately detours into an asymmetric region to gather information before heading for the goal. This is **active information gathering**.

The three paradigms are nested: classical $\subset$ MDP $\subset$ POMDP. Uncertainty has two axes: action uncertainty (where you tried to go versus where you actually went) and perceptual uncertainty (where you actually are versus what the sensor read). MDP handles the first; POMDP handles both.

Ch.3's filters *tracked* the belief; this section addresses what to *do* with the tracked belief.

### 7.9.2 Value Iteration over Belief

Comparing the equations of the three paradigms shows immediately where POMDP becomes hard. The core equation of MDP value iteration is the Bellman equation:

$$C^T(s) = \max_a \int \left[ c(s') + C^{T-1}(s') \right] P(s' \mid a, s)\, ds'$$

Replace state $s$ with belief $b$ and the POMDP value iteration follows:

$$C^T(b) = \max_a \int \left[ c(b') + C^{T-1}(b') \right] P(b' \mid a, b)\, db' \tag{16.2}$$

The policy is:

$$\pi^T(b) = \arg\max_a \int \left[ c(b') + C^{T-1}(b') \right] P(b' \mid a, b)\, db' \tag{16.3}$$

Both $b$ and $b'$ are probability distributions over the state space $\mathcal{S}$. The belief transition distribution $P(b' \mid a,b)$ is defined over the space of these distributions, and the equation integrates over that belief space. For a finite state space its dimension is $|\mathcal{S}|-1$, while a general belief over a continuous state space is infinite-dimensional.

In the infinite-horizon limit, if this recursion converges, we get the standard Bellman equation:

$$V(b) = \max_a \left[ r(b, a) + \gamma \sum_{o'} P(o' \mid b, a)\, V(B(b, a, o')) \right]$$

where $r(b,a) = \sum_s b(s)\, c(s,a)$ is the expected immediate reward over the belief. Written as a finite-horizon recursion this is eq. (16.2).

There is a key trick for handling this. Once observation $o'$ is determined, the posterior belief $B(b, a, o')$ is *uniquely* determined by the Bayes filter. So the integral over the entire belief space can be recast as an integral over the observation space:

$$C^T(b) = \max_a \int \left[ c(B(b, a, o')) + C^{T-1}(B(b, a, o')) \right] P(o' \mid a, b)\, do' \tag{16.34}$$

The belief update operator is:

$$B(b, a, o')(s') = \frac{1}{P(o' \mid a, b)}\, P(o' \mid s') \int P(s' \mid a, s)\, b(s)\, ds$$

In discrete state and observation spaces the integrals become sums. This reformulation is the starting point for all modern POMDP solvers.

### 7.9.3 Four-State Toy Example

To see the PWLC (piecewise-linear convex) structure directly, a small example helps. Work through a 4-state, 2-action, 2-observation problem by hand.

**Setup:**
- States $s_1, s_2, s_3, s_4$. Initially in one of $(s_1, s_2)$.
- Action $a_1$: information gathering. Swaps $s_1 \leftrightarrow s_2$ with probability 0.9.
- Action $a_2$: termination. Moves to $s_3$ (reward +80) or $s_4$ (reward −80).
- Observations $o_1, o_2$: probabilities $(0.7, 0.3)$ from $s_1$, $(0.4, 0.6)$ from $s_2$.
- Belief $b = (p_1, p_2)$ with $p_1 + p_2 = 1$, so it is one-dimensional.

**Horizon 1 computation:**

The immediate reward is linear in belief: $c(b) = \sum_i c(s_i) p_i$.

Taking $a_2$ gives the $T=1$ value ($\gamma = 0.9$):
$$C^1(b, a_2) = \gamma(80 p_1 - 80 p_2) = 72 p_1 - 72 p_2$$

Taking $a_1$ yields no termination, so only the immediate reward: $C^1(b, a_1) \approx 0$.

Therefore:
$$C^1(b) = \max\{ 0,\; 72p_1 - 72p_2 \}$$

$C^1(b)$ is the max of two linear functions. It bends at $p_1 = 0.5$. If $p_1 > 0.5$, take $a_2$; otherwise take $a_1$.

**Horizon 2 computation:**

Integrating over the probabilities of observing $o_1, o_2$ after $a_1$:
$$C^2(b, a_1) \approx \max\{0,\; -33.05 p_1 + 13.61 p_2\}$$

(Coefficients are computed through the observation probabilities and the belief update.)

Full $T=2$:
$$C^2(b) = \max\{ 0,\; -33.05 p_1 + 13.61 p_2,\; 72 p_1 - 72 p_2 \}$$

The max of three linear pieces. As the horizon grows, more pieces are added.

"Knowledge always helps" — $\beta C(b) + (1-\beta) C(b') \geq C(\beta b + (1-\beta) b')$. The value at a certain belief is always at least as high as at an uncertain one.

### 7.9.4 PWLC Structure and Alpha-Vectors

The four-state example showed that the value function takes the form of a *max of linear pieces*. This is not a coincidence; the following inductive argument shows it holds in general.

Base case ($T=1$): the immediate reward $c(b) = \sum_i c(s_i) p_i$ is linear in belief. So $C^1(b) = \max_a \sum_i C^1_{a,i}\, p_i$ — one linear function per action.

Inductive step: suppose $C^{T-1}(b)$ is PWLC. Expanding $C^{T-1}(B(b,a,o'))$ as a function of $b$ in eq. (16.34): the nonlinear normalization factor $1/P(o'\mid a, b)$ in the belief update cancels with the weight $P(o'\mid a, b)$ in eq. (16.34), so each inner product $\langle \phi, B(b,a,o') \rangle \cdot P(o'\mid a, b)$ reduces to a linear function of $b$. The max of a max of linear functions is still a max of linear functions. So $C^T(b)$ is PWLC.

Each coefficient vector of a linear piece is called an **alpha-vector** $\phi$. The value function is:

$$V(b) = \max_\phi \langle \phi, b \rangle$$

With $\Phi$ the set of alpha-vectors, $V(b) = \max_{\phi \in \Phi} \sum_i \phi_i\, p_i$.

Each alpha-vector corresponds to one *conditional policy* (current action plus subsequent policy contingent on observations). Before pruning, the candidate count $|\Phi^T| = |A| \cdot |\Phi^{T-1}|^{|\mathcal{O}|}$ grows doubly exponentially. With two actions, two observations, and $|\Phi^0| = 1$, the counts are $|\Phi^1| = 2$, $|\Phi^2| = 2 \cdot 2^2 = 8$, $|\Phi^3| = 2 \cdot 8^2 = 128$, and $|\Phi^4| = 2 \cdot 128^2 = 32768$. This rapid growth makes exact solutions impractical for long horizons.

### 7.9.5 LP Solution

If the doubly exponential growth in alpha-vector count is the problem, there is a way to reduce the max–sum–max structure to a linear program (LP) that identifies and prunes dominated alpha-vectors while computing an exact solution.

**Reduction principle**: $C = \max_a x(a)$ is solved as $\min C$ subject to $\{C \geq x(a) \;\forall a\}$. For $C = \sum_i \max_a x(a,i)$, introduce a function $a(\cdot)$ selecting an action per state and add $\{C \geq \sum_i x(a(i),i)\}$ for every combination. The number of constraints is $|A|^{|\mathcal{S}|}$.

POMDP horizon $T$ constraints (eq. 16.67):

$$\bigcup_a \bigcup_{k(o'):1 \leq k(o') \leq |\Phi^{T-1}|} \left\{ C^T(b) \geq \gamma \sum_{o'} \sum_i \left(c_i + C^{T-1}_{k(o'),i}\right) P(o' \mid s_i') \sum_j P(s_i' \mid a, s_j)\, p_j \right\}$$

Before pruning, the number of constraints is $|\Phi^T| = |A| \cdot |\Phi^{T-1}|^{|\mathcal{O}|}$.

---

**Algorithm: finite_world_POMDP** (Thrun et al., Table 16.1, adapted)

```
Algorithm finite_world_POMDP(T):
  Φ¹ = { φ : C¹(b) = γ Σᵢ c(sᵢ) pᵢ }     # single alpha-vector for horizon 1

for t = 2 to T:
    Φᵗ = ∅
    for each action a:
      for each assignment k(o') ∈ {1, …, |Φᵗ⁻¹|} for each o':
        # compute new alpha-vector
        for each state sⱼ:
          φⱼ = γ Σₒ' Σᵢ (cᵢ + Φᵗ⁻¹[k(o'), i]) · P(o'|sᵢ') · P(sᵢ'|a, sⱼ)
        Φᵗ = Φᵗ ∪ { ⟨a, φ⟩ }

# remove dominated alpha-vectors (pruning)
  Φᵀ = prune(Φᵀ)
  return Φᵀ
```

---

$|\Phi^T|$ grows doubly exponentially. With a horizon of 3, 3 actions, and 5 observations, the pre-pruning alpha-vector count is already on the order of $10^{14}$, and even with pruning the number is unmanageable in realistic domains. The exact solution is a proof of concept; approximation is essential in practice.

### 7.9.6 General POMDP

If the LP solution is already impractical for discrete finite-state problems, continuous state spaces make things worse.

With a continuous state space, alpha-vectors become continuous functions. Eq. (16.34) still holds in principle, but $\Phi^{T-1}$ becomes a set of functions — infinite-dimensional.

---

**Algorithm: POMDP(T)** (Thrun et al., Table 16.2, adapted, compressed)

```
Algorithm POMDP(T):
  initialize: Φ¹ ← value function at horizon 1 (continuous)

for t = 2 to T:
    for each action a:
      for each "conditional plan" k(·) mapping observations to Φᵗ⁻¹ elements:
        new function φ(b) = γ ∫ₒ' [ c(B(b,a,o')) + Φᵗ⁻¹[k(o')](B(b,a,o')) ] P(o'|a,b) do'
        Φᵗ ← Φᵗ ∪ { φ }

return Φᵀ
```

---

In continuous spaces, storing and comparing sets of functions is itself impractical. This algorithm is an in-principle solution; practical algorithms (MC-POMDP, AMDP) emerge as alternatives.

### 7.9.7 MC-POMDP

With exact solutions blocked, the next step is to approximate the belief with samples and bring computation down to a tractable level.

Represent the belief with a particle filter and approximate the value-iteration update on a sample basis. In ch.3 §3.11, the particle filter served for estimation; here it serves for *planning*.

Belief $\theta$ is a set of weighted particles $\langle s^{(i)}, w^{(i)} \rangle$. The belief update $B(b, a, o')$ is implemented in particle form:

```
Algorithm particle_filter_belief_update(θ, a, o'):
  θ' = ∅
  for i = 1 to N:
    s ~ θ                         # sample a particle
    s' ~ P(s'|a, s)               # motion model
    w' = P(o'|s')                 # measurement model
    θ' ← θ' ∪ { ⟨s', w'⟩ }
  normalize weights in θ'
  return θ'
```

The value-iteration update learns a Q-value $Q(\theta, a)$ per action for each belief $\theta$. Sample $N$ times at each belief, take the max Q from the next belief, and average:

---

**Algorithm: MC-POMDP** (Thrun et al., Table 16.3, skeleton adapted)

return V, policy σ(θ) = argmax_a Q(θ, a)
```

---

Q-function update (eq. 16.78):

$$Q(\theta_t, a_t) \leftarrow \mathbb{E}\left[ R(o_{t+1}) + \gamma \max_{\bar{a}} Q(\theta_{t+1}, \bar{a}) \right]$$

Policy (eq. 16.79):

$$\sigma^Q(\theta) = \arg\max_{\bar{a}} Q(\theta, \bar{a})$$

Q-value function approximation uses nearest-neighbor lookup. Because belief $\theta$ is an unordered particle set, it could not be fed as-is into the feedforward networks of the time, which take a vector with a fixed ordering. Thrun et al. maintain a database of $\langle \theta, a, Q \rangle$ tuples, and for a new belief $\theta'$ the $k$ nearest neighbors by KL divergence yield the average Q-value. Today a set-based encoder could also turn the belief into a network input.

KL divergence between two beliefs is approximated with Gaussian KDE. KL-based kNN acts as the function approximator. Modern implementations replace this with neural function approximation, but the algorithmic skeleton is the same.

The outer loop either holds a static belief database or generates beliefs naturally through $\varepsilon$-greedy simulation trials — the latter concentrates computation on beliefs the real robot is likely to visit.

### 7.9.8 Experiments: Heaven/Hell and Find-and-Fetch

**Heaven/Hell problem**: in a T-shaped corridor, one end is heaven (+1) and the other is hell (−1). Only a priest near the entrance knows which is which. The robot must first ask the priest (information gathering), then head in the correct direction. In this toy model, the POMDP planner learns a policy that detours to the priest. Under the experiment's assumed reliable observation, going directly reaches hell half the time whereas consulting the priest reveals the correct direction.

**Find-and-Fetch (monocular camera)**: the robot must find and retrieve a target object using a monocular camera. The camera gives the object's direction but not its distance. MC-POMDP learns a policy that actively changes viewpoint to reduce distance uncertainty, observing the object from multiple angles to narrow down its position before approaching.

Both experiments track the belief and include *information-gathering actions* in the plan. State estimation followed by greedy action selection alone never produces these detour policies.

### 7.9.9 AMDP — Dimensionality Reduction via Belief Statistics

MC-POMDP tracks the belief directly as a particle set. AMDP (Augmented MDP) rests on the observation that the same uncertainty can be summarized with far fewer statistics. The two extremes of POMDP are MDP (polynomial in $|S|$) and exact POMDP (doubly exponential). AMDP sits in between.

The idea: along real robot trajectories, belief does not fill the entire belief space but occupies a narrow manifold. Summarize that manifold with *low-dimensional statistics* $\bar{b} = f(b)$ and apply standard MDP value iteration over $\bar{b}$.

**Standard statistics** (eq. 16.80):

$$\bar{b} = \langle \arg\max_s b(s),\; H[b] \rangle$$

Most likely state plus belief entropy. Entropy:

$$H[b] = -\int b(s) \ln b(s)\, ds \tag{16.81}$$

An infinite-dimensional belief is summarized by the pair of its most likely state and entropy. Whether this is a *sufficient statistic* is not guaranteed — Thrun et al. explicitly note that "the sufficient statistic assumption rarely holds" — but the coastal navigation experiments confirm it is enough to select reasonable actions.

Using $\arg\max_s b(s)$ alone is a standard MDP. Adding entropy to the state encodes "how much I do not know."

---

**Algorithm: Augmented_MDP_value_iteration** (Thrun et al., Table 16.4, adapted)

```
Algorithm Augmented_MDP_value_iteration():
  for all b̄:
    Ĉ(b̄) = 0

repeat until convergence:
    for all b̄:
      Ĉ(b̄) ← max_a ∫ [c(b̄') + Ĉ(b̄')] P(b̄'|a, b̄) db̄'

return Ĉ
  policy: π(b̄) = argmax_a ∫ [c(b̄') + Ĉ(b̄')] P(b̄'|a, b̄) db̄'
```

---

The form is identical to MDP_value_iteration (Probabilistic Robotics §15.3.3). The only difference is that the state is $\bar{b}$ instead of $s$.

Transition probability $P(\bar{b}' \mid a, \bar{b})$ (eq. 16.85):

$$P(\bar{b}' \mid a, \bar{b}) = \int\!\!\int\!\!\int I_{f(b)=\bar{b}}\, I_{f(B(o',a,b))=\bar{b}'}\, P(o' \mid s') P(s' \mid a, s) P(s \mid b)\, ds\, ds'\, do'\, db$$

In practice this is approximated by simulation with a lookup-table cache, estimating transitions statistically over many random trials.

### 7.9.10 Coastal Navigation Example

Coastal navigation is the easiest emergent behavior produced by AMDP to describe.

The motivation: crossing a wide open space, a conventional MDP planner takes the straight-line path because it is short. But in open space, lidar or a camera sees only featureless walls, so the entropy of the position belief grows substantially. The robot reaches the destination without knowing where it is.

In the same environment, an AMDP planner chooses a **curved path that follows the wall**. Near the wall, lidar measurements tightly constrain the position and entropy stays low. Entropy is part of the cost function, so preferring "information-rich" paths comes out automatically.

Analogy: a ship navigating without GPS follows the coastline. Landmarks are plentiful near the coast, which keeps the position estimate sharp.

In Thrun et al.'s Figure 16.5, as sensor range decreases the arrival entropy of the conventional planner rises steeply, while the coastal planner's arrival entropy barely changes. This is where the robustness of information-aware path planning shows up.

Selecting paths to reduce position uncertainty in Active SLAM, and moving toward the highest-information viewpoint in next-best-view planning, are both modern forms of coastal navigation.

### 7.9.11 What Survived

Coastal navigation is the conclusion a planner reaches automatically once entropy is included in the cost function. The value iteration over belief that started in §7.9.2 produces coastal navigation as its answer, in concrete form.

The exact solutions (§7.9.5 and §7.9.6) are impractical but remain useful as conceptual tools. Modern POMDP solvers have branched into three families.

Point-based value iteration (SARSOP, HSVI, PBVI) performs alpha-vector backup only at sampled belief points, not across the full belief space. The alpha-vector structure from §7.9.4 is intact; restricting the search range prevents the explosion.

**MCTS family**: POMCP (Silver & Veness, 2010) and DESPOT. Rollouts estimate Q-values and belief trees are searched with MCTS. This combines the Q estimation structure of §7.9.7's MC-POMDP with tree search.

**Deep POMDP**: DRQN (Recurrent Q-network), DVRL (Igl et al.). The RNN's hidden state serves as an implicit belief. MC-POMDP's nearest-neighbor function approximation replaced by neural function approximation.

The same principle appears in other methods. Bayes-adaptive MDP (BAMDP) uses the posterior distribution over model parameters as an augmented state. Active SLAM includes the variance of the position belief in the cost function, and NeRF-based active perception also uses entropy-augmented planning.

Ch.8 §8.3's deep RL methods (PPO, SAC) learn from experience and need no model (transition probabilities, observation model). POMDP planning computes the optimal policy when the model is known. Without a model, MC-POMDP does not run either. MC-POMDP sits at the intersection: belief tracked with the model, Q-values learned from experience.

Value iteration over belief is mathematically clean but scales doubly exponentially in the horizon and the number of observations — that is why approximate solvers exist. MC-POMDP represents belief with a particle filter and learns Q-values from samples. AMDP summarizes belief as a (most likely state, entropy) pair and reduces to a standard MDP. The methods differ, but the goal is one: active information gathering. Its modern forms are MCTS-based POMCP, Deep POMDP, and entropy-augmented planning in Active SLAM.

---

## 7.10 Further Reading

> **LaValle, "Planning Algorithms"**
> http://lavalle.pl/planning/
> Free online. The most comprehensive textbook on motion planning. Written by the originator of RRT, so naturally strong.

> **Russ Tedrake, "Underactuated Robotics" Ch.10: Trajectory Optimization**
> https://underactuated.csail.mit.edu/trajopt.html
> Hands-on trajectory optimization with Drake. Code and theory together.

> **Matthew Kelly, "An Introduction to Trajectory Optimization" (SIAM Review 2017)**
> https://www.matthewpeterkelly.com/research/MatthewKelly_IntroTrajectoryOptimization_SIAM_Review_2017.pdf
> Solid tutorial comparing direct collocation and shooting. Example code included.

> **OMPL**
> https://ompl.kavrakilab.org/
> Open-source motion planning library. Implements dozens of algorithms including RRT, RRT*, and PRM.

> **MoveIt2 Tutorials**
> https://moveit.picknik.ai/
> Hands-on motion planning on ROS2. From pick-and-place to advanced configuration.

> **Drake**
> https://drake.mit.edu/
> Integrates trajectory optimization with simulation. Contact-implicit support.

> **CasADi**
> https://web.casadi.org/
> Standard tool for implementing nonlinear trajectory optimization.

> **Additional papers**
> - [Garrett et al., "Integrated Task and Motion Planning" (2021, arXiv:2010.01083)](https://arxiv.org/abs/2010.01083) — the standard TAMP survey paper.
> - [Janner et al., "Planning with Diffusion for Flexible Behavior Synthesis" (ICML 2022, arXiv:2205.09991)](https://arxiv.org/abs/2205.09991) — the start of trajectory-level diffusion-based planning.

---

## Technical Timeline

```
1979 ── Visibility graph-based path planning
1996 ── PRM (Kavraki et al.) — the start of sampling-based planning
1998 ── RRT (LaValle) — an influential single-query sampling planner
2000 ── RRT-Connect (Kuffner & LaValle) — a variant widely provided by practical motion-planning libraries
2009 ── CHOMP (Ratliff et al.) — gradient-based trajectory optimization
2011 ── RRT* (Karaman & Frazzoli) — asymptotic optimality guarantee
2012 ── OMPL introductory paper (IEEE RAM) — unified library of sampling-based planners (1.0 release in 2014)
2014 ── TrajOpt (Schulman et al.) — sequential convex optimization
2019 ── MoveIt2 (ROS2) — an open motion-planning framework used in research and industry
2022 ── SayCan (Google) — LLM + motion planning
2023 ── Contact-implicit trajectory optimization becomes practical
2024 ── LLM-based TAMP research spreads
```

---

# Ch.8 — Robot Learning

Robot learning is the field where robots learn behavior from data and experience instead of explicit programming. The main pieces are reinforcement learning (RL), sim-to-real transfer, imitation learning, and recent foundation-model-based approaches.

---

## 8.1 Why Study Robot Learning

**Where traditional methods work well**

Traditional control and planning methods such as PID, MPC, and RRT work well when the dynamics model is accurate and the environment is structured. An industrial robot arm picking and assembling parts at predetermined positions is a representative example. Mathematical stability or optimality guarantees hold only under each method's model, constraint, and solver assumptions; on tasks that meet those assumptions, a learned method must establish its additional benefit empirically.

**Where traditional methods struggle**

The problem is that the real world is not clean.

- **Dynamics that are hard to model**: Building an accurate physical model for deformable objects like cloth, rope, or fluids is practically impossible.
- **Complex contact**: Tasks like turning an object in hand or inserting it involve contact modes that change frequently. Accurately modeling contact dynamics is still an open problem.
- **Unstructured environments**: Operating in environments that cannot be pre-modeled, such as home kitchens or disaster sites. You cannot know in advance what objects are where.

In these situations, learning-based approaches approximate the input-output relation directly from data, so they can operate without an explicit model.

**Limits of learning-based methods**

- **Sample efficiency**: RL often requires millions of interaction steps. Collecting this data on a real robot is unrealistic in terms of time and cost.
- **Safety**: During learning, the robot can damage itself or its surroundings. Exploration therefore carries physical risk.
- **Generalization**: Performance often drops sharply when conditions differ even slightly from those seen during training.

If a problem can be solved with traditional methods, use traditional methods. Learning is a tool to apply where traditional methods hit their limits. Combining the two appropriately is the most realistic approach in practice.

Where, then, are these limits in concrete terms? Chapter 15 of *Probabilistic Robotics* (Thrun, Burgard, and Fox) uses four domains — a manipulator, an underwater vehicle, a helicopter, and a team of planetary exploration robots — to show why action selection under uncertainty is necessary. Read from the perspective of learning, those examples go as follows. An industrial manipulator working inside a controlled workspace is well served by traditional control. An autonomous underwater vehicle, however, faces changing currents, limited visibility, and shifting buoyancy — building a prior model is not realistic. A helicopter contends with disturbances such as gusts large enough that the question becomes how much clearance to keep from obstacles, and planetary exploration robots cannot know the terrain or their positions relative to one another in advance. Across these domains, how much learning is needed tracks directly with how large the model uncertainty is.

---

## 8.2 RL Basics

### MDP (Markov Decision Process)

The mathematical framework for RL is the MDP. Its components are as follows.

- **State (s)**: The current state of the environment. Robot joint angles, velocities, object positions, etc.
- **Action (a)**: The action taken by the agent. Joint torques, target joint angles, etc.
- **Reward (r)**: A scalar reward signal received as a result of an action. r = R(s, a).
- **Transition (T)**: The state transition probability. T(s'|s, a). The distribution of the next state given the current state and action.
- **Discount factor (γ)**: The discount rate for future rewards. 0 < γ ≤ 1. In robot RL, γ = 0.99 is a common starting value.

The goal is to find a policy π(a|s) that maximizes the cumulative discounted reward.

```
J(π) = E[ Σ_{t=0}^{∞} γ^t · r_t ]
```

The Markov property is the assumption that "the next state depends only on the current state and action." It means you do not need to look at the entire prior history. What breaks down on a real robot is not the Markov property of the state transition but the assumption that the observation *is* the state (partial observability, i.e., a POMDP situation). The latent state still transitions in a Markovian way; a policy built from observations alone does not. In that case, observation history or a belief is used as the state, or a recurrent policy is used.

The MDP definition establishes the criterion for what is optimal. How to *compute* that optimal policy when the environment model $p(x'|x,a)$ and reward $r$ are known is the subject of the next section.

### MDP Value Iteration

When the environment model $p(x'|x,a)$ and reward $r$ are **known**, dynamic programming computes the optimal policy directly. Following the notation of *Probabilistic Robotics* (Thrun, Burgard, and Fox), the state is written as $x$ here — the same concept as $s$ in the preceding section.

#### Payoff and Horizon

The reward $r(x, a)$ is called the payoff function: the scalar value received immediately when action $a$ is taken in state $x$. A policy $\pi: x \mapsto a$ maps every state to an action. The quality of a policy is measured by the expected cumulative discounted reward.

$$V^\pi(x_0) = \mathbb{E}\left[\sum_{t=0}^{T} \gamma^t \, r(x_t, \pi(x_t))\right]$$

The horizon $T$ splits into three cases.

- **T=1 (greedy)**: Maximize the reward for the next single step only. Simple, but ignores long-term consequences.
- **Finite-horizon**: Optimize up to a fixed $T$ steps. The policy must vary with time $t$ (a time-dependent policy), which makes representation more complex.
- **Infinite-horizon (T=∞)**: When $\gamma < 1$, $V^\pi$ remains finite ($|V^\pi| \leq r_{\max}/(1-\gamma)$), and a time-independent stationary policy exists. Infinite-horizon with discounting is the default setting in robot RL.

The intuition behind $\gamma$: a reward one step away is worth $\gamma$ times as much; two steps away, $\gamma^2$. A lower $\gamma$ makes the robot short-sighted; a higher $\gamma$ makes it plan further ahead. The reason $\gamma = 0.99$ is a common starting point is that it looks far enough ahead to matter while still guaranteeing convergence.

#### Bellman Equation

Under infinite horizon, the optimal value function $V^*(x)$ satisfies the Bellman equation.

$$V^*(x) = \max_a \left[ r(x, a) + \gamma \sum_{x'} p(x' \mid x, a) \, V^*(x') \right]$$

The meaning: the optimal value at state $x$ is the maximum, over all actions $a$, of the immediate reward $r(x,a)$ plus the discounted sum of optimal values of the next states. The structure is recursive.

The optimal policy is extracted greedily from this equation.

$$\pi^*(x) = \arg\max_a \left[ r(x, a) + \gamma \sum_{x'} p(x' \mid x, a) \, V^*(x') \right]$$

Taking the T=1 optimum, extending recursively to T=2, then to $T \to \infty$ derives the Bellman equation above. Each step combines "optimal now plus optimal remainder" — the standard dynamic programming structure.

#### Value Iteration Algorithm

The Bellman equation is a fixed-point equation for $V^*$. Rather than solving it directly, substitute the current estimate $V_k$ on the right-hand side to produce $V_{k+1}$ and repeat. That is Value Iteration.

```
Algorithm MDP_value_iteration():
  For all states x:
    V_0(x) ← 0

Repeat until convergence:           ε: tolerance (e.g., 1e-6)
    For all states x:
      V_{k+1}(x) ← max_a [ r(x, a) + γ · Σ_{x'} p(x'|x, a) · V_k(x') ]
    if max_x |V_{k+1}(x) - V_k(x)| < ε: break

Extract optimal policy:
    π*(x) ← argmax_a [ r(x, a) + γ · Σ_{x'} p(x'|x, a) · V_k(x') ]

return V_k, π*
```

Variable summary: $V_k$ is the value estimate at iteration $k$; $V^*$ is the optimal value after convergence; $\pi^*$ is the optimal policy; $r$ is reward; $\gamma$ is the discount factor; $p(x'|x,a)$ is the transition probability.

The update order is arbitrary. According to Sutton & Barto (2018) §4.5, convergence is guaranteed as long as each state is updated infinitely often. The formula above uses a sum (Σ) for a discrete state space; the corresponding continuous-state expression uses an integral.

Convergence guarantee: the Bellman update operator is a contraction mapping with contraction rate $\gamma$, so the iteration converges to the unique fixed point $V^*$. After $k$ iterations, the error is at most $\gamma^k$ times the initial error: $\|V_k - V^*\|_\infty \leq \gamma^k \|V_0 - V^*\|_\infty$.

#### 2D Grid World Example

Imagine a $5 \times 5$ grid. Each cell is a state $x$, and the four actions are up, down, left, and right. Reaching the goal cell yields $r = +100$; hitting an obstacle cell yields $r = -10$; all other moves cost $r = -1$. Transitions go in the intended direction with probability 0.8 and in each orthogonal direction with probability 0.1 (stochastic transitions).

Running Value Iteration produces a value function shaped like a contour map — high around the goal cell, low around obstacles. In the early iterations, only the states adjacent to the goal hold positive values; as the iterations proceed, the values propagate outward. After convergence, picking in each cell the action with the largest sum of immediate reward and expected next-state value yields the optimal policy, and behavior that detours around obstacles emerges naturally. A policy is a mapping that assigns an action to every state; under stochastic transitions, the trajectory actually executed differs from run to run.

The resulting state-to-action mapping is $\pi^*$. No explicit path search (RRT, A*) is needed — the value function alone determines the action.

MDP Value Iteration applies when the environment model $p(x'|x,a)$ and $r$ are *given*. Learning by experience when the model is *unknown* is covered in §8.3 (model-free RL). Planning under *partial observation* is in Ch.7 §7.9 (advanced POMDP).

On real robots, the transition probability $p(x'|x,a)$ is rarely known in advance. A method that improves the policy directly, without a model, is what is needed next.

### Policy Gradient Intuition

Policy gradient improves the policy in three steps.

1. Collect several trajectories with the current policy.
2. Increase the probability of actions in trajectories with high return.
3. Decrease the probability of actions in trajectories with low return.

Written as an equation:

```
∇J(θ) = E[ Σ_t ∇log π_θ(a_t|s_t) · A_t ]
```

A_t is the advantage function, which indicates how much better the action was compared to the average. Parameters θ are updated along this gradient.

Intuitively, the gradient of `log π(a|s)` points in the direction of increasing the probability of action a, and multiplying by the advantage makes good actions selected more often and bad actions less often.

### Value Function, Q-function

- **Value function V^π(s)**: The expected cumulative reward when following policy π from state s.
- **Q-function Q^π(s, a)**: The expected cumulative reward when taking action a in state s and then following π.
- **Advantage A^π(s, a) = Q^π(s, a) - V^π(s)**: How much better action a is compared to the average.

Learning a value function separately reduces variance. Most modern RL algorithms use an actor-critic structure that trains a policy network and a value network together.

### On-policy vs Off-policy

- **On-policy**: Trains only on data collected by the current policy. A current rollout can be used for multiple epochs, but data from old policies is not retained in a replay buffer for continued reuse. PPO is representative. Stable but with low sample efficiency.
- **Off-policy**: Reuses data collected by past policies (replay buffer). SAC and TD3 are representative. Sample efficient but training can be unstable.

In robotics, data collection is costly, so the sample efficiency of off-policy methods is attractive. However, if large-scale parallel environments can be run in simulation, on-policy PPO is also sufficiently competitive.

---

## 8.3 Major RL Algorithms

### PPO (Proximal Policy Optimization)

PPO is an on-policy algorithm proposed by Schulman et al. (2017). It clips the probability ratio inside the surrogate objective so that changes far from the previous policy earn no gain. A trust-region constraint that binds the policy change itself belongs to TRPO; PPO fills that role through the design of the objective instead.

```
L_CLIP(θ) = E[ min( r_t(θ) · A_t, clip(r_t(θ), 1-ε, 1+ε) · A_t ) ]
```

Here r_t(θ) = π_θ(a_t|s_t) / π_θ_old(a_t|s_t) is the probability ratio, and ε has a default value of 0.2 in the original paper.

PPO is popular because it is relatively simple to implement and its defaults work reasonably well across a fairly wide range of settings. Reproducibility studies have shown, however, that its performance depends heavily on implementation details such as advantage normalization and value clipping. It also trains stably. When combined with large-scale parallel simulation such as NVIDIA Isaac Lab, data can be collected from thousands of environments simultaneously, so the sample efficiency problem can be solved by sheer volume.

### SAC (Soft Actor-Critic)

SAC is an off-policy algorithm characterized by the addition of entropy regularization. It maximizes reward while simultaneously maximizing the entropy of the policy. That is, it encourages trying as diverse a set of actions as possible.

```
J(π) = E[ Σ_t γ^t ( r_t + α · H(π(·|s_t)) ) ]
```

α is the temperature parameter that balances entropy and reward. There are also methods that adjust α automatically.

It is sample efficient in continuous action spaces. This is because a replay buffer lets collected data be reused multiple times. When collecting data directly on a real robot, off-policy SAC has an advantage over on-policy PPO in terms of data efficiency.

### TD3 (Twin Delayed DDPG)

TD3 is an improved version of DDPG, an off-policy algorithm similar to SAC. Three key improvements:

1. **Twin Q-networks**: Trains two Q-functions and uses the smaller value to reduce overestimation bias.
2. **Delayed policy update**: Updates the policy once after updating the critic multiple times.
3. **Target policy smoothing**: Adds noise to the target action.

Performance is similar to SAC, but since entropy tuning is not needed, there are slightly fewer hyperparameters. However, exploration can be weaker than in SAC.

### Algorithm Selection Guide

| Situation | Recommended algorithm | Reason |
|------|-------------|------|
| Simulation, GPU parallelization possible | PPO | Parallel environments compensate for sample efficiency |
| Real robot, little data | SAC | Off-policy, sample efficient |
| Continuous action space, stability important | SAC or TD3 | Both strong in continuous spaces |
| Discrete action space | PPO or DQN | The original SAC targets continuous actions; a discrete variant requires a separate implementation |
| Project just getting started | PPO | Easy to tune, easy to debug |

### Stable-Baselines3 Code Example

Basic code for training PPO on the MuJoCo Ant environment.

```python
import gymnasium as gym
from stable_baselines3 import PPO
from stable_baselines3.common.env_util import make_vec_env
from stable_baselines3.common.evaluation import evaluate_policy

# Create parallel environments (8)
vec_env = make_vec_env("Ant-v4", n_envs=8)

# Create PPO agent
model = PPO(
    "MlpPolicy",
    vec_env,
    learning_rate=3e-4,
    n_steps=2048,        # number of steps to collect per rollout
    batch_size=64,
    n_epochs=10,         # number of epochs to train on collected data
    gamma=0.99,
    gae_lambda=0.95,     # GAE (Generalized Advantage Estimation)
    clip_range=0.2,
    verbose=1,
    tensorboard_log="./ppo_ant_tb/",
)

# Train (2M steps total)
model.learn(total_timesteps=2_000_000)

# Evaluate
eval_env = gym.make("Ant-v4")
mean_reward, std_reward = evaluate_policy(model, eval_env, n_eval_episodes=20)
print(f"Mean reward: {mean_reward:.1f} +/- {std_reward:.1f}")

# Save/load model
model.save("ppo_ant")
loaded_model = PPO.load("ppo_ant")
```

The SAC example has a similar structure.

```python
from stable_baselines3 import SAC

model = SAC(
    "MlpPolicy",
    "Ant-v4",
    learning_rate=3e-4,
    buffer_size=1_000_000,    # replay buffer size
    learning_starts=10_000,   # start training after this many steps
    batch_size=256,
    tau=0.005,                # target network soft update rate
    gamma=0.99,
    verbose=1,
)

model.learn(total_timesteps=1_000_000)
```

> Stable-Baselines3 is good for fast prototyping. If you want to understand algorithm internals, CleanRL is recommended. Every algorithm is implemented in a single file, making it easy to follow along with the code.

---

## 8.4 Simulation Environments

Robot RL usually relies on simulation because collecting millions of interaction steps on physical hardware is costly and can be unsafe. The main simulators differ in contact models, parallelism, and available robot assets.

### MuJoCo (Multi-Joint dynamics with Contact)

After being acquired by DeepMind, it was open-sourced in 2022. Thanks to its contact simulation quality and stable numerical integration, it has become the standard benchmark environment for RL research. The default engine is CPU-based, and MuJoCo 3.0+ supports GPU parallelization through MJX (JAX backend), but its ecosystem is smaller than Isaac Lab's. It is suited for algorithm benchmarks and small-scale experiments.

### Isaac Lab (NVIDIA)

A robot learning framework built on top of NVIDIA Isaac Sim. With GPU parallel simulation, it can run thousands to tens of thousands of environments simultaneously and supports photorealistic rendering and sensor simulation. An NVIDIA GPU is required, and installation and configuration are complex. It is used for large-scale locomotion training and sim-to-real pipelines.

### PyBullet

An open-source physics engine suitable for beginners. It installs in a single `pip install` line. Its physical accuracy and speed are lower than MuJoCo's, but it is sufficient for first runs of RL code or for quickly validating ideas.

### Brax

A JAX-based physics engine developed by Google. Thanks to JAX's JIT compilation and automatic differentiation, it runs at very high speed on GPU/TPU and can be used for differentiable physics research. However, physical accuracy is limited and it is weak on complex contact scenarios.

### Environment Comparison Table

| Simulator | Physical accuracy | Speed | GPU parallelization | Installation difficulty | Main use |
|-----------|-----------|------|-----------|-----------|---------|
| MuJoCo | High | Moderate | Possible via MJX | Easy | Algorithm benchmarks |
| Isaac Lab | High | Very fast | Thousands to tens of thousands | Hard | Large-scale robot learning |
| PyBullet | Moderate | Slow | No | Very easy | Introduction/education |
| Brax | Low | Very fast | Yes | Moderate | Fast iteration experiments |

For getting started, the MuJoCo + Gymnasium combination is recommended. Move to Isaac Lab when large-scale experiments become necessary.

---

## 8.5 Sim-to-Real Transfer

Applying a policy trained in simulation to a real robot is called sim-to-real transfer. In theory, you train enough in simulation and deploy to the real robot, and you are done. In practice, it does not work that way.

### Reality Gap

A gap exists between simulation and reality.

- **Physical parameter differences**: Friction coefficients, masses, moments of inertia, etc., differ from simulation.
- **Sensor noise**: Real sensors have noise, latency, and drift.
- **Actuator modeling error**: Motor nonlinearity, gear backlash, compliance, etc.
- **Contact model differences**: Simulation's contact models are only approximations of reality.

Even if you hit a reward of 10,000 in simulation, it is common for the real robot to fall over.

### Domain Randomization

The idea is to randomly vary the physical parameters of the simulation so that the policy is trained to be robust and does not depend on specific parameters. OpenAI's Dactyl (2018) randomized hundreds of physical parameters simultaneously and succeeded at sim-to-real, demonstrating the potential of this approach.

Representative parameters to randomize:

- Friction coefficient: uniform sampling between 0.5 and 1.5
- Object mass: 0.8 to 1.2 times the default
- Joint damping: 0.5 to 2.0 times the default
- Sensor noise: add Gaussian noise
- Actuator strength: 0.8 to 1.2 times the default
- Communication delay: random delay of 0 to 2 steps

```python
# Example domain randomization config in Isaac Lab style (pseudo-code)
class RandomizationConfig:
    # Randomized at the start of each episode
    friction_range = (0.5, 1.5)
    mass_scale_range = (0.8, 1.2)
    joint_damping_scale_range = (0.5, 2.0)

# Applied every step
    obs_noise_std = 0.05           # Gaussian noise on observations
    action_delay_steps = (0, 2)    # delay before applying actions
    push_force_range = (-5.0, 5.0) # external disturbance (N)

def randomize_env(env, config):
    """Called at the start of each episode."""
    import numpy as np
    friction = np.random.uniform(*config.friction_range)
    mass_scale = np.random.uniform(*config.mass_scale_range)
    damping_scale = np.random.uniform(*config.joint_damping_scale_range)
    env.set_friction(friction)
    env.scale_mass(mass_scale)
    env.scale_joint_damping(damping_scale)

def add_obs_noise(obs, config):
    """Add noise to the observation at each step."""
    import numpy as np
    noise = np.random.normal(0, config.obs_noise_std, size=obs.shape)
    return obs + noise
```

With a wide enough randomization range, reality is likely to fall within that range. In exchange, performance measured at one specific parameter setting drops. On a real system those parameters are hard to identify exactly, so a randomized policy sometimes does better than a specialized one.

### System Identification (Sys-ID)

This is the opposite approach from domain randomization. The physical parameters of the real robot are measured or estimated as accurately as possible and reflected in the simulation.

Methods:
- Direct measurement: measure mass with an electronic scale, measure friction coefficient experimentally
- Parameter optimization: find parameters that minimize the difference between real robot trajectories and simulation trajectories
- Online adaptation: continuously estimate and update parameters during actual operation

Sys-ID is often used together with domain randomization. The common pattern is to use Sys-ID to pin down approximate parameters and cover the remaining uncertainty with domain randomization.

### Teacher-Student Structure

A method that leverages privileged information, accessible in simulation but not in reality. Training proceeds in two stages.

1. **Teacher training**: In simulation, a policy is trained with privileged information (exact terrain height, exact friction coefficient, exact object position, etc.) included in the state. With abundant information, training is easy.
2. **Student training**: The student is trained to imitate the teacher's behavior using only observations available on the real robot (IMU, joint encoders, cameras, etc.).

This approach had major success in locomotion research on the ANYmal quadruped robot. The teacher knows the exact terrain height map, but the student learns behavior similar to the teacher using only proprioception history.

### Case Studies

**ANYmal Locomotion (ETH Zurich / Robotic Systems Lab)**
- Quadruped locomotion trained with PPO + domain randomization + teacher-student
- Billions of simulation steps followed by zero-shot transfer to the real robot
- Robust walking across stairs, gravel, slopes, and other varied terrain
- Key: large-scale domain randomization + privileged learning + proprioception history

**Dexterous Hand Manipulation (OpenAI, NVIDIA, etc.)**
- Solving a Rubik's cube with the Shadow Hand (OpenAI, 2019)
- Large-scale domain randomization is key: hundreds of physical parameters randomized simultaneously
- Trained on roughly 13,000 years' worth of simulated experience
- Real-world success rate was considerably lower than in simulation, but demonstrated the potential of the learning-based approach

---

## 8.6 Imitation Learning

RL requires designing a reward function and a large amount of training data. Imitation learning, in contrast, learns a policy directly from demonstration data provided by an expert (a human). This is "learning by watching."

### Behavioral Cloning (BC)

The simplest form of imitation learning. Expert (observation, action) pairs are collected, and a policy is trained with supervised learning.

```python
import torch
import torch.nn as nn
from torch.utils.data import DataLoader, TensorDataset

def forward(self, obs):
        return self.net(obs)

# Load expert data (NumPy -> Tensor)
# expert_obs: (N, obs_dim), expert_act: (N, act_dim)
dataset = TensorDataset(
    torch.FloatTensor(expert_obs),
    torch.FloatTensor(expert_act),
)
loader = DataLoader(dataset, batch_size=256, shuffle=True)

policy = BCPolicy(obs_dim=48, act_dim=7)
optimizer = torch.optim.Adam(policy.parameters(), lr=1e-3)
loss_fn = nn.MSELoss()

# Training
for epoch in range(100):
    total_loss = 0.0
    for obs_batch, act_batch in loader:
        pred_act = policy(obs_batch)
        loss = loss_fn(pred_act, act_batch)
        optimizer.zero_grad()
        loss.backward()
        optimizer.step()
        total_loss += loss.item()
    if (epoch + 1) % 10 == 0:
        print(f"Epoch {epoch+1}, Loss: {total_loss/len(loader):.4f}")
```

**Compounding error problem**: A structural limitation of BC. Once the learned policy deviates even slightly from the expert trajectory, it reaches states not in the training data. Behavior there is unpredictable, deviation grows, and errors accumulate. Over a horizon $T$ the error can grow as $O(\epsilon T^2)$ (Ross & Bagnell, 2010).

### DAgger (Dataset Aggregation)

DAgger is a method for addressing compounding error.

1. Train a BC policy on initial expert data.
2. Run the trained policy to collect new trajectories.
3. Label what action the expert would take at each state in these trajectories.
4. Add the new data to the existing dataset and retrain.
5. Repeat steps 2-4.

DAgger adds the expert action at states that the learned policy actually visits. The original paper's no-regret bound holds under its assumptions about the online learner and expert queries.

The downside is that the expert must label repeatedly. A human has to provide corrections one by one, which is labor-intensive.

### ACT (Action Chunking with Transformers)

Proposed in Stanford's ALOHA project, ACT combines two mechanisms:

1. **Action chunking**: Instead of predicting one action at a time, predict a sequence of k future action steps at once. This captures temporal correlation and reduces compounding error.
2. **CVAE (Conditional Variational Autoencoder)**: Models the multimodal distribution of actions. Even in the same situation, there can be multiple valid actions, and a plain MSE loss averages them out, producing mediocre actions.

The architecture uses a Transformer encoder-decoder, taking joint positions and camera images as input.

### Diffusion Policy

A method proposed by Chi et al. (2023), applying diffusion models to action generation.

Diffusion policy can express arbitrarily complex action distributions through a denoising process. BC implemented as MSE regression effectively assumes a unimodal Gaussian, whereas diffusion policy handles multimodal distributions naturally. BC is the name of a training scheme, not of a distribution family — implemented with an energy-based or mixture-density head, it can represent multiple modes.

```python
# Action generation process of Diffusion Policy (pseudo-code)
# 1. Start from pure noise
action = torch.randn(batch_size, horizon, action_dim)

# 2. K denoising steps
for k in reversed(range(K)):
    # Predict noise conditioned on current observation
    predicted_noise = noise_pred_net(action, k, obs_encoding)
    # Remove noise (using a DDPM or DDIM scheduler)
    action = scheduler.step(predicted_noise, k, action)

# 3. Output the final action sequence
```

Diffusion policy and ACT have become the main baselines for manipulation imitation learning since 2023. Both are implemented in public frameworks such as LeRobot (HuggingFace).

### Data Collection Methods

Imitation-learning performance depends heavily on data quality. The main data-collection methods are:

- **Teleoperation**: A human remotely controls the robot. ALOHA uses a leader-follower structure and can collect bimanual manipulation data relatively cheaply.
- **VR controller**: A VR controller specifies end-effector position/orientation. Intuitive, but may lack force feedback in contact-rich tasks.
- **Kinesthetic teaching**: Grab the robot arm directly and move it. The most intuitive, but difficult for large or heavy robots.
- **Space mouse**: A 6-DoF input device. Operable with one hand. Useful for precision work.

The amount of data varies by task and method. The Chi et al. (2023) Diffusion Policy paper showed meaningful performance with about 100-200 demonstrations. More is better, but there is a trade-off with collection cost.

---

## 8.7 Advanced: Foundation Models for Robot Control

Inspired by the success of LLMs and VLMs, robotics researchers are building large-scale pretrained models (foundation models). They train a generalist policy on large quantities of robot data and then adapt it to new robots or tasks.

### RT-1, RT-2 (Google DeepMind)

**RT-1 (2022)**: A Transformer-based policy trained on 130,000 robot demonstrations (collected over about 17 months). It takes images and natural language commands as input and outputs actions. A single model performs more than 700 tasks.

**RT-2 (2023)**: A VLM (Vision-Language Model) fine-tuned directly to produce action outputs. PaLM-E and PaLI-X are used as base models. It showed that web-scale pretrained knowledge transfers to robot control. It generalized to some extent even to objects not seen in the training data.

### Octo

An open-source generalist robot policy developed by UC Berkeley and others. It was trained on the Open X-Embodiment dataset (data collected from diverse robots and diverse institutions). It uses a diffusion-based action head and is designed to be fine-tuned to new robots.

### π0 (Physical Intelligence)

The [π0 technical report (2024)](https://arxiv.org/abs/2410.24164) proposes a generalist robot policy that places a flow-matching action expert on a pretrained VLM. The authors train on single-arm, dual-arm, and mobile-manipulator data, then evaluate zero-shot behavior, language following, and fine-tuning; demonstrations include laundry folding, table cleaning, and box assembly. Performance claims should be read within the report's robots, tasks, and baselines.

### OpenVLA

An open-source VLA (Vision-Language-Action) model. A 7B-parameter VLM was fine-tuned to output action tokens. Its core contribution is being open source and accessible to anyone.

### Realistic Assessment

Foundation models for robotics are still in an early stage. To be honest:

- On specific tasks, task-specialized traditional methods or task-specific learning often do better.
- The cost of large-scale robot data collection is very high. The scale is different from internet text/image data.
- There is no safety guarantee. It is hard to predict foundation model behavior.
- Inference latency may exceed the time allowed for real-time control.

### Research Directions

- **Data scaling**: Efforts to combine data from multiple institutions, like Open X-Embodiment. Whether more data improves generalization is still being verified.
- **Cross-embodiment transfer**: Research on transferring a policy trained on one robot to another. The core problem is how to unify different action spaces.
- **Efficient fine-tuning**: Rapid adaptation to new tasks via parameter-efficient fine-tuning such as LoRA.
- **Action representation**: How to tokenize/represent actions. Discretization, continuous distributions, diffusion, and other approaches are competing.

---

## 8.8 Advanced: Reward Design and Safe RL

The reward function shapes what an RL policy learns. Applying RL to a physical robot also requires explicit safety measures.

### Reward Shaping

**The problem with sparse rewards**: Sparse rewards like "+1 if the goal is reached, 0 otherwise" are easy to define, but the agent has to search randomly until it happens to receive a reward. In a large state-action space, the agent may rarely obtain a learning signal.

**Dense reward**: Add rewards for intermediate progress. For example, in an object-grasping task:

```python
def compute_reward(gripper_pos, object_pos, target_pos, is_grasped):
    # 1. Bring the gripper close to the object
    dist_to_object = np.linalg.norm(gripper_pos - object_pos)
    reaching_reward = -1.0 * dist_to_object

# 2. Bonus if the object is grasped
    grasp_reward = 5.0 if is_grasped else 0.0

# 3. Bring the object close to the target position
    if is_grasped:
        dist_to_target = np.linalg.norm(object_pos - target_pos)
        place_reward = -1.0 * dist_to_target
    else:
        place_reward = 0.0

# 4. Goal-reached bonus
    success_reward = 10.0 if (is_grasped and
        np.linalg.norm(object_pos - target_pos) < 0.05) else 0.0

return reaching_reward + grasp_reward + place_reward + success_reward
```

**Curriculum learning**: A method of starting from easy tasks and gradually moving to harder ones. For example, in locomotion, start with walking on flat ground, then move to small obstacles, then stairs. This way, the agent can accumulate success experiences early in training even under sparse rewards.

### Reward Hacking

A phenomenon where the agent maximizes reward but in ways unintended by the designer.

Representative examples:
- A robot arm told to "move" an object instead pushes the object to the target position (without grasping)
- A walking robot told to "move fast" slides while falling
- Told to learn to jump, it evolves into an abnormally elongated shape (when combined with morphology optimization)

Countermeasures:
- Review the learned behavior and adjust the reward function iteratively.
- Add penalty terms for undesired behavior.
- Review qualitatively by watching video. This is a part that is difficult to automate.

### Constrained RL

RL that explicitly handles safety constraints. While standard RL maximizes reward, constrained RL maximizes reward subject to keeping cost below a bound.

```
max_π E[ Σ γ^t r_t ]   subject to   E[ Σ γ^t c_t ] ≤ d
```

c_t is cost (e.g., exceeding joint torque limits, colliding with obstacles), and d is the allowed bound.

Representative algorithms include CPO (Constrained Policy Optimization), PCPO, and Lagrangian-relaxation-based methods. On real robots, putting torque limits and joint angle limits as constraints for hardware protection is the realistic approach.

### Human-in-the-loop RL

An approach that uses human feedback as a reward signal. It applies the same idea as RLHF (RL from Human Feedback) in LLMs to robotics.

Method:
1. Show pairs of robot behaviors and have humans indicate preferences (A is better than B).
2. Train a reward model on the preference data.
3. Run RL using the learned reward model.

Useful for tasks where reward is hard to define numerically (e.g., "walk naturally", "place objects carefully"). The drawbacks are that it takes a lot of human time, and the reward model can be inaccurate.

---

## 8.9 Further Reading

> **Sutton & Barto, "Reinforcement Learning: An Introduction" (2nd edition)**
> http://incompleteideas.net/book/the-book-2nd.html
> A standard textbook covering topics from MDPs to policy gradients, with a free PDF. Ch.1-6 and Ch.13 provide a focused starting path through the foundations.

> **Sergey Levine, CS285: Deep Reinforcement Learning**
> https://rail.eecs.berkeley.edu/deeprlcourse/
> A graduate-level course focused on robot RL. Lecture videos and slides are publicly available. Covers most of this chapter's topics in more depth.

> **Stable-Baselines3**
> https://stable-baselines3.readthedocs.io/
> A PyTorch-based RL algorithm library. PPO, SAC, TD3, and other major algorithms are implemented. Suitable for fast prototyping.

> **CleanRL**
> https://github.com/vwxyzjn/cleanrl
> A collection of single-file RL implementations. Each file contains an entire algorithm, making it easy to study by following along with the code. If you want to understand algorithm internals, this is recommended over SB3.

> **Isaac Lab**
> https://isaac-sim.github.io/IsaacLab/
> NVIDIA's GPU parallel robot simulation framework. Widely adopted in large-scale training projects such as ANYmal locomotion and dexterous manipulation.

> **LeRobot (HuggingFace)**
> https://github.com/huggingface/lerobot
> A framework for imitation learning and robot learning. Includes implementations of ACT, Diffusion Policy, and others. Datasets are provided as well.

> **robomimic**
> https://robomimic.github.io/
> An imitation learning algorithm benchmark. Various imitation learning methods such as BC, BC-RNN, and HBC can be compared under identical conditions.

> **Additional papers**
> - [Andrychowicz et al., "Hindsight Experience Replay" (NeurIPS 2017, arXiv:1707.01495)](https://arxiv.org/abs/1707.01495) — A cornerstone for solving the sparse reward problem. Relabels failed trajectories as successes.
> - [Hafner et al., "Mastering Diverse Domains through World Models" (DreamerV3, arXiv:2301.04104)](https://arxiv.org/abs/2301.04104) — Trains more than 150 tasks with a single fixed hyperparameter configuration. A representative recent result in world-model-based RL.
> - [Chi et al., "Universal Manipulation Interface" (UMI, RSS 2024, arXiv:2402.10329)](https://arxiv.org/abs/2402.10329) — Data collection with a handheld gripper, zero-shot deployment across diverse robots.
> - [Fu et al., "Mobile ALOHA" (CoRL 2024, arXiv:2401.02117)](https://arxiv.org/abs/2401.02117) — Mobile base + bimanual teleoperation. Co-training significantly improves success rate.

---

## Technical Timeline

```
1992 ── REINFORCE algorithm (Williams)
         An influential early Monte Carlo policy-gradient method whose score-function estimator has high variance.

2013 ── DQN (Mnih et al., Atari)
         The beginning of Deep RL. A replay buffer mitigated the correlated-sample problem.
         The fixed target network was added in the 2015 Nature version.

2015 ── TRPO (Schulman et al.)
         Stable policy updates with a trust region constraint. Strong theory, complex implementation.

2017 ── PPO (Schulman et al.)
         A practical alternative to TRPO. Simple to implement with clipping. The default baseline for robot RL experiments.

2018 ── SAC (Haarnoja et al.)
         Entropy regularization + off-policy. Sample efficient in continuous spaces.

2019 ── ANYmal: sim-to-real locomotion (ETH Zurich)
         Succeeded at quadruped sim-to-real by modeling the drivetrain dynamics with an actuator network trained on real robot data.

2020 ── Widespread practical adoption of DAgger
         Imitation learning proved practical. Applied across diverse robot platforms.

2022 ── RT-1 (Google)
         Large-scale robot data + Transformer. A multi-task generalist policy.

2023 ── ACT/ALOHA (Stanford), Diffusion Policy (Columbia / TRI / MIT)
         A new standard for imitation learning. Performance gains via action chunking and diffusion.

2023 ── RT-2 (Google)
         VLM used directly for action generation. Transfer of web knowledge to robotics.

2024 ── Octo, OpenVLA, pi0
         Emergence of open-source generalist policy models. Start of cross-embodiment learning.

2025 ── Spread of cross-embodiment learning research
         Policy transfer across different robots. Data scaling laws under verification.
```

---

# Ch.9 — Computer Vision Fundamentals

Computer vision turns raw camera data into information a robot can use. Understanding image processing and camera geometry makes it possible to trace failures in SLAM and manipulation pipelines.

---

## 9.1 Image Processing

Raw camera images contain noise and irrelevant variation. Filtering, edge detection, and morphological operations prepare the input and help determine whether an error in a downstream pipeline began during preprocessing.

### 9.1.1 Introduction to OpenCV

**OpenCV (Open Source Computer Vision Library)** is a widely used public CV library.

OpenCV provides C++ and Python bindings for common operations ranging from image I/O and filtering to feature extraction and geometric computation.

**Installation**:

```bash
pip install opencv-python opencv-contrib-python
```

**Basic usage**:

```python
import cv2
import numpy as np

# Read an image
img = cv2.imread('image.jpg')

# Convert to grayscale
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

# Display the image
cv2.imshow('Image', img)
cv2.waitKey(0)
cv2.destroyAllWindows()
```

**Caveat**: OpenCV uses BGR rather than RGB channel order. Before passing an image to an RGB-based library such as Matplotlib, convert it with `cv2.cvtColor(img, cv2.COLOR_BGR2RGB)`.

> **Further reading**
> - [OpenCV official tutorials](https://docs.opencv.org/4.x/d9/df8/tutorial_root.html) — Python/C++ examples, well organized.
> - [First Principles of Computer Vision](https://www.youtube.com/channel/UCf0WB91t8Ky6AuYcQV0CcLw) — Columbia's Prof. Shree Nayar channel. Intuitive explanations of image processing principles.
> - [Szeliski, "Computer Vision: Algorithms and Applications"](https://szeliski.org/Book/) — Free PDF. The standard textbook in CV.
> - [Stanford CS131 — Computer Vision: Foundations and Applications](http://vision.stanford.edu/teaching/cs131_fall1415/schedule.html) — More introductory than CS231n. Start here if you want to begin from image processing.

### 9.1.2 Filtering

Filtering retains selected image structure while reducing unwanted noise. Blur is often applied before edge detection or segmentation to suppress high-frequency variation in the input.

**Blur**:

```python
# Gaussian Blur
blurred = cv2.GaussianBlur(img, (5, 5), 0)

# Median Blur (effective for noise removal)
median = cv2.medianBlur(img, 5)
```

**Edge Detection**:

```python
# Canny Edge Detection
edges = cv2.Canny(gray, threshold1=50, threshold2=150)

# Sobel Operator
sobel_x = cv2.Sobel(gray, cv2.CV_64F, 1, 0, ksize=3)
sobel_y = cv2.Sobel(gray, cv2.CV_64F, 0, 1, ksize=3)
```

Edges highlight object contours and locations with large intensity changes. Canny output is sensitive to its thresholds, so compare which boundaries disappear and which noise remains as the values change.

> **Further reading**
> - [First Principles of Computer Vision — Edge Detection](https://www.youtube.com/playlist?list=PL2zRqk16wsdoCCLpouGuRbcJFBVVJlvgr) — Visual explanation of the mathematics of edge detection.
> - [OpenCV filtering tutorial](https://docs.opencv.org/4.x/d4/d13/tutorial_py_filtering.html) — Follow along immediately with code.
> - [Papers With Code — Edge Detection](https://paperswithcode.com/task/edge-detection) — Latest benchmarks and papers on edge detection.

> **Exercise**: [Canny Edge Detection](https://alexjunholee.github.io/robotics-practice/app.html#canny_edge)
> Adjust the threshold parameters of the Canny edge detector in real time and observe how the output changes.

> **Exercise**: [Convolution Visualization](https://alexjunholee.github.io/robotics-practice/app.html#convolution)
> Apply various kernels to an image and build intuition for how the convolution operation performs filtering.

### 9.1.3 Morphology

Morphological operations modify the shape of regions in a binary image. They can remove small noise specks from a segmentation output or reconnect broken regions.

```python
kernel = np.ones((5, 5), np.uint8)

# Erosion
eroded = cv2.erode(binary_img, kernel, iterations=1)

# Dilation
dilated = cv2.dilate(binary_img, kernel, iterations=1)

# Opening (erosion -> dilation): removes noise
opening = cv2.morphologyEx(binary_img, cv2.MORPH_OPEN, kernel)

# Closing (dilation -> erosion): fills holes
closing = cv2.morphologyEx(binary_img, cv2.MORPH_CLOSE, kernel)
```

Opening first erodes and then dilates, removing small protrusions and noise. Closing first dilates and then erodes, filling small holes.

> **Further reading**
> - [OpenCV Morphological Operations](https://docs.opencv.org/4.x/d9/d61/tutorial_py_morphological_ops.html) — Explained with visual examples.
> - [First Principles of Computer Vision — Binary Image Processing](https://www.youtube.com/watch?v=IcBzsP-fvPo) — Principles of morphological operations.

---

## 9.2 Camera Model

If you do not understand how a camera reads the world, recovering 3D from a 2D image is impossible. SLAM, 3D reconstruction, visual servoing — all of these start from the camera model. If you have learned linear algebra, this is where you can feel how matrices are actually used.

### 9.2.1 Pinhole Model

The pinhole model is an idealized camera model that projects a 3D point onto a 2D image.

To invert the projection and recover a real-world 3D position from a pixel coordinate (u, v), you need to know this projection relation precisely. The Pinhole Model expresses this relation as equations.

**Projection equation**:

```
[u]   [f_x  0   c_x] [X/Z]
[v] = [0   f_y  c_y] [Y/Z]
[1]   [0    0    1 ] [ 1 ]
```

**Intrinsic Parameters**:
- f_x, f_y: Focal length (in pixels)
- c_x, c_y: Principal point (intersection of the optical axis with the image plane, which can differ from the image center)
- Intrinsic Matrix K (3x3)

**Extrinsic Parameters**:
- R: rotation matrix (3x3)
- t: translation vector (3x1)
- World -> Camera transform

K represents the lens characteristics of the camera, and [R|t] represents where the camera sits in the world and how it is oriented. Multiplying the two maps a 3D point to a 2D pixel.

> **Further reading**
> - [Stanford CS231A — Camera Models](https://web.stanford.edu/class/cs231a/) — Core lectures on geometry-based CV.
> - [First Principles of CV — Camera and Imaging](https://www.youtube.com/playlist?list=PL2zRqk16wsdoYzrWStQ2SQHXXS2K6ofd4) — From pinhole to real lenses, explained step by step.
> - [Szeliski Ch.2 — Image Formation](https://szeliski.org/Book/) — Mathematical foundations of the camera model.
> - [Jinyong Jeong blog — Camera Models and Distortion (Perspective, Fisheye, Omni)](https://jinyongjeong.github.io/2020/06/15/Camera_and_distortion_model/) — Comparison of Perspective, Equidistant, and Omni camera models.
> - [Jinyong Jeong blog — OpenCV Camera model notes](https://jinyongjeong.github.io/2020/06/19/SLAM-Opencv-Camera-model-%EC%A0%95%EB%A6%AC/) — Notes on OpenCV's pinhole/fisheye camera model implementation.

> **Exercise**: [Camera Projection](https://alexjunholee.github.io/robotics-practice/app.html#camera_projection)
> Check interactively how a point in 3D space is projected onto a 2D image through the intrinsic and extrinsic parameters.

### 9.2.2 Distortion Models

Real lenses introduce distortion.

An image taken with a real camera is not as clean as the Pinhole Model assumes. In particular, with wide-angle or fisheye lenses, the distortion that bends straight lines into curves is severe. Skipping distortion correction drops SLAM accuracy sharply and warps 3D reconstruction output.

Camera lenses are not perfect pinholes. Light bends as it passes through the lens, and this bending appears in the image as distortion.

**Radial distortion**: gets worse as you move away from the image center. Modeled by parameters k1, k2, k3. When k1 < 0, you get barrel distortion (straight lines bulge outward); when k1 > 0, pincushion distortion (straight lines curve inward). Most lenses have barrel distortion, and it is more pronounced in wider lenses.

**Tangential distortion**: occurs when the lens is not perfectly parallel to the image sensor. Parameters p1, p2. Usually smaller in effect than radial distortion, but it is not negligible in modules with loose lens-to-sensor alignment tolerances.

**Distortion correction**:

```python
# Simple correction (computed per frame - slow)
undistorted = cv2.undistort(distorted, K, dist_coeffs)

# Precompute correction maps and reuse (a common SLAM pipeline pattern)
map1, map2 = cv2.initUndistortRectifyMap(K, dist_coeffs, None, K, (w, h), cv2.CV_32FC1)
undistorted = cv2.remap(distorted, map1, map2, cv2.INTER_LINEAR)
```

With fixed camera parameters, precomputing maps through `initUndistortRectifyMap()` and reusing them with `cv2.remap()` avoids rebuilding the mapping every frame. This is a common way to reduce work in a real-time pipeline.

**Fisheye lenses**: a low-order radial-tangential pinhole model may not represent a wide-angle lens adequately. Fisheye models express projection as a function of incidence angle θ (the equidistant ideal is r = f·θ); OpenCV provides matching routines in `cv2.fisheye`. Select the model from the lens projection and held-out reprojection residuals rather than from a single FoV cutoff.

(See: [Dark Programmer — Camera distortion correction](https://darkpgmr.tistory.com/31), [Jinyong Jeong blog — Camera Models and Distortion](https://jinyongjeong.github.io/2020/06/15/Camera_and_distortion_model/))

> **Further reading**
> - [OpenCV Camera Calibration and 3D Reconstruction](https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html) — The equations of the distortion models are well organized.
> - [First Principles of CV — Lens Related Issues](https://www.youtube.com/watch?v=hzOeqCb2Fg4) — Physical intuition for why lens distortion occurs.

> **Exercise**: [Lens Distortion Visualization](https://alexjunholee.github.io/robotics-practice/app.html#lens_distortion)
> Adjust radial and tangential distortion parameters and see directly how the image deforms.

### 9.2.3 Calibration

Calibration is the process of estimating a camera's intrinsic and extrinsic parameters.

You can only use the camera model once you actually know K (the intrinsic matrix) and the distortion coefficients. If calibration is inaccurate, everything built on top of it — SLAM, stereo depth estimation, hand-eye calibration — loses accuracy. A textbook case of "garbage in, garbage out".

**Checkerboard method**:

```python
# Detect checkerboard corners
ret, corners = cv2.findChessboardCorners(gray, (9, 6), None)

# Refine corners
corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)

# Calibration
ret, K, dist, rvecs, tvecs = cv2.calibrateCamera(
    object_points, image_points, gray.shape[::-1], None, None
)
```

Evaluate a calibration set by observability rather than image count. Cover the center and edges of the image and vary range and tilt about several axes. Inspect per-image and image-space residuals and error on held-out views, not only one global RMS value.

Camera calibration estimates the parameters that map points in the 3D world to positions in a 2D image. Capturing a checkerboard from several angles produces dozens to hundreds of correspondences between its known 3D coordinates and the detected 2D image points. From these pairs, calibration estimates:

1. **Intrinsic parameters** (fx, fy, cx, cy): focal length and principal point. Recheck them when focus, zoom, resolution, crop, temperature, or mechanical assembly changes.
2. **Distortion coefficients** (k1, k2, p1, p2, k3): coefficients that approximate lens and assembly distortion under the chosen projection model. Price alone does not predict their magnitude.
3. **Extrinsic parameters** (R, t): the camera pose at each capture position. These are a byproduct of calibration itself, but are used separately in settings like hand-eye calibration.

Capture the checkerboard at several angles and distances, with observations spread across the image. The required count depends on parameter uncertainty, conditioning, and detection quality. Expected reprojection error also depends on resolution, lens model, target, and corner detector, so a universal pixel grading table is inappropriate. Before removing a high-residual image, inspect blur, glare, and detection failure; compare parameter stability and held-out error before and after exclusion.

(See: [Dark Programmer — Camera calibration](https://darkpgmr.tistory.com/32))

**Kalibr**: Multi-camera and Camera-IMU calibration tool
- ROS-based
- Uses AprilTag boards
- Estimates time offsets as well

> **Further reading**
> - [OpenCV camera calibration tutorial](https://docs.opencv.org/4.x/dc/dbb/tutorial_py_calibration.html) — Step-by-step checkerboard calibration.
> - [Kalibr official Wiki](https://github.com/ethz-asl/kalibr/wiki) — a widely used public Camera-IMU calibration tool.
> - [Zhang, "A Flexible New Technique for Camera Calibration" (2000)](https://www.microsoft.com/en-us/research/publication/a-flexible-new-technique-for-camera-calibration/) — The paper behind OpenCV's current calibration.
> - [Tangram Vision Blog](https://www.tangramvision.com/blog) — Practical engineering posts on camera calibration, sensor fusion, and more.

---

## 9.3 Features

A distinguishable point (keypoint) in an image together with a vector (descriptor) describing its surroundings.

SLAM and Visual Odometry must find the same points across images as the camera moves. Features provide a repeatable way to establish those correspondences.

### 9.3.1 Keypoint Detection

**Harris Corner**:
- Classical method for corner detection
- Slow, not robust to scale changes

**FAST (Features from Accelerated Segment Test)**:
- Very fast corner detection
- Suitable for real-time systems
- Not scale-invariant

**ORB (Oriented FAST and Rotated BRIEF)**:
- FAST detection + BRIEF descriptor + orientation
- Patent-free
- Widely used in real-time SLAM

The ORB-SLAM family uses ORB. It carries no patent restriction, and its speed suits real-time systems.

**SIFT (Scale-Invariant Feature Transform)**:
- Scale- and rotation-invariant
- High repeatability
- High computational cost (previously patented, now released)

Lowe published SIFT in 2004. Its scale- and rotation-invariant keypoints provide a useful reference for comparing later methods such as SURF and ORB.

**SuperPoint** (deep-learning-based):
- Self-supervised training
- High repeatability and accuracy
- Requires GPU

> **Further reading**
> - [Lowe, "Distinctive Image Features from Scale-Invariant Keypoints" (2004)](https://www.cs.ubc.ca/~lowe/papers/ijcv04.pdf) — The original SIFT paper.
> - [Rublee et al., "ORB: An efficient alternative to SIFT or SURF" (2011)](https://ieeexplore.ieee.org/document/6126544) — The original ORB paper.
> - [First Principles of CV — Feature Detection](https://www.youtube.com/playlist?list=PL2zRqk16wsdqXEMpHrc4Qnb5rA1Cylrhx) — Principles of keypoint detection, visually.
> - [DeTone et al., "SuperPoint: Self-Supervised Interest Point Detection and Description" (2018)](https://arxiv.org/abs/1712.07629) — The starting point of deep-learning-based features.
> - [Dark Programmer — Image keypoint extraction methods](https://darkpgmr.tistory.com/131) — Comparison of SIFT, HOG, Haar, Ferns, LBP, MCT, and other features.

### 9.3.2 Descriptor

Once you have a keypoint, the descriptor is about "how to describe" its surroundings. To find the same physical point across two images, you have to express the pattern around that point as numbers so they can be compared.

**BRIEF (Binary Robust Independent Elementary Features)**:
- Binary descriptor (0 or 1)
- Fast matching (Hamming distance)
- Not rotation-invariant

**ORB Descriptor**:
- BRIEF + orientation
- 256-bit binary vector

The advantage of binary descriptors is matching speed. Because the distance between two descriptors is computed as Hamming distance (an XOR operation), it is much faster than SIFT's Euclidean distance comparison. On embedded systems, this difference is large.

**SuperGlue** (deep-learning-based):
- Graph Neural Network-based matching
- Robust to repetitive patterns and low texture
- LightGlue: a lightweight version

> **Further reading**
> - [Sarlin et al., "SuperGlue: Learning Feature Matching with Graph Neural Networks" (2020)](https://arxiv.org/abs/1911.11763) — Representative work of deep-learning-based matching.
> - [OpenCV Feature Matching tutorial](https://docs.opencv.org/4.x/dc/dc3/tutorial_py_matcher.html) — How to use BFMatcher and FLANN.

### 9.3.3 Feature Matching

As the camera moves, SLAM must find the same points in the previous and current frames. Inspecting the feature matches helps distinguish tracking loss caused by too few correspondences from loss caused by incorrect ones.

```python
# Extract ORB keypoints and descriptors
orb = cv2.ORB_create()
kp1, des1 = orb.detectAndCompute(img1, None)
kp2, des2 = orb.detectAndCompute(img2, None)

# BFMatcher (Brute-Force)
bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
matches = bf.match(des1, des2)

# Ratio Test (Lowe's ratio)
bf = cv2.BFMatcher(cv2.NORM_HAMMING)
matches = bf.knnMatch(des1, des2, k=2)
good = [m for m, n in matches if m.distance < 0.75 * n.distance]
```

Lowe's ratio test uses kNN to find the two nearest matches and keeps a match only when the ratio of the first distance to the second falls below a threshold. This removes ambiguous cases in which the two distances are similar. Lowe used 0.8 in the original paper, while the example above uses 0.75; the threshold can be adjusted between 0.6 and 0.8 for the application.

> **Further reading**
> - [OpenCV Feature Matching](https://docs.opencv.org/4.x/dc/dc3/tutorial_py_matcher.html) — Examples of BFMatcher, FLANN, and ratio test.
> - [Computerphile — SIFT Features](https://www.youtube.com/watch?v=ram-jbLJjFg) — Intuitive explanation of feature matching.

> **Exercise**: [Feature Matching](https://alexjunholee.github.io/robotics-practice/app.html#feature_matching)
> Experiment interactively with feature matching between two images and the application of Lowe's ratio test.

---

## 9.4 Epipolar Geometry

Epipolar geometry deals with the geometric relation between two camera viewpoints.

Given two photos of the same object, the goal is to recover how the camera moved (relative pose) and from there reconstruct the 3D structure. This is the mathematical foundation of Visual Odometry and Structure from Motion (SfM). It is also where SVD and eigenvalue decomposition from linear algebra are directly used.

### 9.4.1 Essential Matrix (E)

**Definition**: encodes the relative pose between a pair of calibrated cameras

```
x2^T E x1 = 0
```

- x1, x2: normalized image coordinates
- E = [t]_× R (skew-symmetric matrix of t × R)

**5-point algorithm**: estimates E from at least 5 correspondence pairs (used with RANSAC)

Decomposing the essential matrix into R and t gives candidate relative rotations and translation directions between the two cameras. The positive-depth condition selects a candidate, while the translation magnitude requires separate scale information. This is the core principle of Visual Odometry.

> **Exercise**: [Epipolar Geometry Visualization](https://alexjunholee.github.io/robotics-practice/app.html#epipolar)
> Examine interactively the epipolar lines and epipoles between two camera viewpoints, and understand the geometric meaning of the essential/fundamental matrix.

### 9.4.2 Fundamental Matrix (F)

**Definition**: the relation between a pair of uncalibrated cameras

```
p2^T F p1 = 0
```

- p1, p2: pixel coordinates
- F = K2^(-T) E K1^(-1)

**8-point algorithm**: estimates F from at least 8 correspondence pairs

For the relation between E and F: F is the version "you can use directly on pixel coordinates", while E is the version "you use when you already know the camera intrinsics". If you have calibrated, use E; if not, use F.

### 9.4.3 Triangulation

Triangulation computes the 3D position of a point observed from two viewpoints.

It is the same principle as how you perceive depth with two eyes. Observing the same point from two cameras (or one camera after it has moved) allows you to compute its 3D position geometrically.

```python
# OpenCV triangulation
points_4d = cv2.triangulatePoints(P1, P2, pts1, pts2)
points_3d = points_4d[:3] / points_4d[3]  # Homogeneous -> Cartesian
```

Caveat: if the baseline (distance between the two cameras) is too small, triangulation accuracy drops; if it is too large, it becomes hard to observe the same point from both sides at once. You have to understand this trade-off well.

> **Further reading**
> - [Stanford CS231A — Epipolar Geometry](https://web.stanford.edu/class/cs231a/) — Lecture material with clear mathematical derivations.
> - [Hartley & Zisserman, "Multiple View Geometry in Computer Vision"](https://www.robots.ox.ac.uk/~vgg/hzbook/) — The key reference on multi-view geometry. A must-read if you want to go deep.
> - [First Principles of CV — Stereo Vision](https://www.youtube.com/playlist?list=PL2zRqk16wsdoYzrWStQ2SQHXXS2K6ofd4) — Intuitive explanation of epipolar geometry.
> - [Dark Programmer — Image Geometry series (7 parts: coordinate frames to Epipolar)](https://darkpgmr.tistory.com/77) — A systematic Korean-language treatment of coordinate frames, homogeneous coordinates, 2D/3D transforms, homography, imaging, and epipolar geometry.

> **Exercise**: [Homography Visualization](https://alexjunholee.github.io/robotics-practice/app.html#homography)
> Manipulate the homography between planes interactively and see how four correspondence points determine the projective transform.

---

## 9.5 Optical Flow

Optical flow estimates pixel motion between consecutive frames.

As a robot sees the world through its camera while moving, knowing where each pixel goes in the next frame is useful. It is used directly in Visual Odometry pose estimation, dynamic object detection, collision avoidance, and so on. While feature matching only handles sparse points, dense optical flow estimates the motion of every pixel.

### 9.5.1 Lucas-Kanade Method

- Sparse optical flow (specific points only)
- Brightness constancy assumption
- Small-motion assumption

```python
# Compute optical flow
p1, status, err = cv2.calcOpticalFlowPyrLK(
    prev_gray, curr_gray, p0, None, **lk_params
)
```

In "PyrLK", "Pyr" stands for Pyramid. It uses an image pyramid to capture large motions too — a technique for overcoming the small-motion assumption of Lucas-Kanade.

### 9.5.2 Dense Optical Flow

- Computes motion for every pixel
- Farneback, RAFT (deep learning)

```python
# Farneback dense flow
flow = cv2.calcOpticalFlowFarneback(prev_gray, curr_gray, None, 0.5, 3, 15, 3, 5, 1.2, 0)
```

RAFT (Recurrent All-Pairs Field Transforms) is a common learned baseline for dense optical flow. Its paper reports strong benchmark accuracy, but latency and accuracy relative to newer methods depend on resolution, hardware, training data, and evaluation protocol.

> **Further reading**
> - [First Principles of CV — Optical Flow](https://www.youtube.com/playlist?list=PL2zRqk16wsdp8KbDfHKvPYNGF2L-zQASc) — Mathematical principles of optical flow.
> - [Teed & Deng, "RAFT: Recurrent All-Pairs Field Transforms for Optical Flow" (2020)](https://arxiv.org/abs/2003.12039) — Representative work of deep-learning-based optical flow.
> - [Huang et al., "FlowFormer: A Transformer Architecture for Optical Flow" (ECCV 2022, arXiv:2203.16194)](https://arxiv.org/abs/2203.16194) — Transformer-based optical flow.
> - [OpenCV Optical Flow tutorial](https://docs.opencv.org/4.x/d4/dee/tutorial_optical_flow.html) — Code examples for Lucas-Kanade and Farneback.

> **Exercise**: [Optical Flow Visualization](https://alexjunholee.github.io/robotics-practice/app.html#optical_flow)
> Compare the behavior of Lucas-Kanade and Dense Optical Flow algorithms interactively and observe the pixel-motion estimation process.

---

## 9.6 Advanced: PnP Problem

**Perspective-n-Point (PnP)** is the problem of estimating the camera pose (rotation R and translation t) given 3D points in space and their 2D correspondences in the image. In SLAM, per-frame camera tracking is precisely a PnP problem, and in AR, marker-based localization is also solved with PnP.

**Problem statement**: given n 3D-2D correspondences {(X_i, x_i)}, estimate the camera extrinsics [R|t].

$$x_i = K [R | t] X_i$$

Here K is the camera intrinsics.

**P3P (3-Point Problem)**:
- Solvable with at least 3 correspondences.
- 3 points yield up to 4 solutions; a 4th point is used for disambiguation.
- Combined with RANSAC, it can be solved robustly in the presence of outliers.

**EPnP (Efficient PnP)**:
- O(n) complexity, efficient when there are many correspondences.
- Represents the 3D points as 4 virtual control points and estimates those control points' camera coordinates.
- When there are many points, it is faster and more stable than P3P+RANSAC.

**Practical usage**:

```python
import cv2
import numpy as np

# 3D world coordinates (n x 3)
object_points = np.array([...], dtype=np.float64)
# Corresponding 2D image coordinates (n x 2)
image_points = np.array([...], dtype=np.float64)
# Camera intrinsics
camera_matrix = np.array([[fx, 0, cx],
                          [0, fy, cy],
                          [0,  0,  1]], dtype=np.float64)
dist_coeffs = np.zeros(4)

# EPnP: a non-iterative closed-form solver, so no initial guess is needed
success, rvec, tvec = cv2.solvePnP(
    object_points, image_points, camera_matrix, dist_coeffs,
    flags=cv2.SOLVEPNP_EPNP
)

# RANSAC version - essential when outliers are present
success, rvec, tvec, inliers = cv2.solvePnPRansac(
    object_points, image_points, camera_matrix, dist_coeffs,
    iterationsCount=1000, reprojectionError=3.0
)
```

**Connection to SLAM**: the per-frame procedure in Visual SLAM is as follows.
1. From the previous frame, create 3D map points via triangulation.
2. In the new frame, predict the 2D reprojection of those map points.
3. Match them against the observed 2D keypoints.
4. Solve PnP on these 3D-2D correspondences to obtain the camera pose of the new frame.

Tracking in ORB-SLAM3 broadly follows this flow. Note, however, that in monocular mode, creating new map points by triangulating between keyframes is mainly handled by Local Mapping (in stereo and RGB-D, Tracking creates close points directly at keyframe insertion).

> **Further reading**
> - [Lepetit et al., "EPnP: An Accurate O(n) Solution to the PnP Problem" (2009)](https://doi.org/10.1007/s11263-008-0152-6) — The original EPnP paper.
> - [OpenCV solvePnP documentation](https://docs.opencv.org/4.x/d5/d1f/calib3d_solvePnP.html) — Explanation of the various PnP algorithm flags.
> - [Multiple View Geometry — Ch. 7](https://www.robots.ox.ac.uk/~vgg/hzbook/) — Mathematical background of the PnP problem.

**Practical tips for solvePnP**

OpenCV's `cv2.solvePnP()` estimates the camera pose from 3D-2D correspondences. The returned `rvec` is a Rodrigues vector (axis-angle representation).

```python
# rvec -> rotation matrix
R, _ = cv2.Rodrigues(rvec)

# Camera position in world coordinates
camera_position = -R.T @ tvec
```

Things to watch out for:
- The result of `solvePnP` is the **world-to-camera transform**. To get the camera's world position, you need to invert it.
- At least 4 points are required, but the more points, the more robust to noise. The RANSAC version `cv2.solvePnPRansac()` filters outliers automatically.
- The `flags` parameter selects the algorithm: `cv2.SOLVEPNP_ITERATIVE` (default, LM), `cv2.SOLVEPNP_P3P` (exactly 4 points in `solvePnP`), `cv2.SOLVEPNP_EPNP` (fast and stable, good when there are many points).

The direction of the Rodrigues vector is the rotation axis, and its magnitude (norm) is the rotation angle. This is exactly the axis-angle representation from the Lie algebra so(3) in Ch.3. `cv2.Rodrigues()` is an implementation of the exp/log map.

(See: [Dark Programmer — solvePnP usage and Rodrigues representation](https://darkpgmr.tistory.com/99))

---

## 9.7 Advanced: RANSAC Variants

RANSAC was introduced in the robust estimation section of Ch.3. In actual research, vanilla RANSAC is rarely used as is. There are several variants that improve convergence speed and accuracy, and which one you pick can change the result a lot.

**Main variants**:

| Method | Core idea | Characteristics |
|------|-------------|------|
| **Lo-RANSAC** | local optimization on inliers | converges in fewer iterations than vanilla |
| **PROSAC** | samples in order of matching confidence | tries good matches first, accelerating convergence |
| **MAGSAC++** | marginalizes over σ (inlier threshold) | less sensitive to threshold tuning; still requires validation on the data |

**Lo-RANSAC (Locally Optimized RANSAC)**:
- When a good model is found, it re-estimates the model from that model's inliers (local optimization).
- A simple idea with a large effect. Particularly useful when the inlier ratio is low.

**PROSAC (Progressive Sample Consensus)**:
- Samples correspondences in order of matching score, highest first.
- When good matches are concentrated at the top, it finds a good model in the very first iterations.

**MAGSAC++ (Marginalizing Sample Consensus)**:
- Marginalizes the most troublesome hyperparameter, the inlier threshold σ.
- Instead of fixing the threshold, it integrates over multiple σ values, so it is less sensitive to the threshold choice. An upper bound on σ still has to be specified.
- OpenCV exposes it through the `USAC_MAGSAC` option.

**Using MAGSAC++ in OpenCV**:

```python
import cv2

# Use MAGSAC++ for fundamental matrix estimation
F, mask = cv2.findFundamentalMat(
    pts1, pts2,
    method=cv2.USAC_MAGSAC,
    ransacReprojThreshold=1.0,
    confidence=0.999,
    maxIters=10000
)

# The same applies to homography estimation
H, mask = cv2.findHomography(
    src_pts, dst_pts,
    method=cv2.USAC_MAGSAC,
    ransacReprojThreshold=3.0
)
```

**Practical tips**:
- Iteration count: controlled by the `confidence` parameter. 0.999 sets the iteration count so that, at the assumed inlier ratio, the probability of drawing at least one all-inlier minimal sample is 99.9% — it is not the probability that the finally selected model is correct. The lower the inlier ratio, the more iterations are needed, growing exponentially.
- Threshold: with MAGSAC++ you are less sensitive to the threshold, but you still need to provide an initial value. The values in the example are starting points and should be validated for the image resolution and noise level.
- The speed-accuracy relationship between PROSAC and MAGSAC++ depends on match-score quality, outlier ratio, and implementation. Compare them on the same data and time budget.

> **Further reading**
> - [Barath et al., "MAGSAC++, a Fast, Reliable and Accurate Robust Estimator" (2020)](https://arxiv.org/abs/1912.05909) — The original MAGSAC++ paper.
> - [OpenCV USAC documentation](https://docs.opencv.org/4.x/d1/df1/md__build_4rdparty_ippicv_ippicv_lnx_doc_USAC.html) — OpenCV's universal RANSAC framework.
> - [Chum & Matas, "Matching with PROSAC" (2005)](https://doi.org/10.1109/CVPR.2005.221) — The original PROSAC paper.

---

## 9.8 Advanced: Learning-Based Feature Matching

Hand-crafted features like ORB and SIFT have worked well for decades, but they fail on repetitive patterns, lack of texture, or extreme illumination changes. Since 2018, deep-learning-based feature extraction and matching have started to surpass classical methods.

**Pipeline evolution**:

```
SuperPoint (2018) -> SuperGlue (2020) -> LightGlue (2023)
  [keypoint detection+description]    [graph neural network matching]     [lightweight matching]
```

**SuperPoint**:
- Trains a keypoint detector and descriptor jointly via self-supervised learning.
- Homographic adaptation: applies synthetic transforms and inverts them to generate pseudo ground truth.
- In the paper's evaluation, it achieved higher repeatability than several classical baselines.

**SuperGlue**:
- Treats the keypoints of two images as a graph and matches them via an attention mechanism.
- Self-attention learns keypoint relations within the same image, and cross-attention performs matching between the two images.
- Solves the optimal assignment problem with the Sinkhorn algorithm.
- It achieved strong matching results in the paper's evaluation, while its compute cost and latency depend on feature count and hardware.

**LightGlue**:
- A lightweight version of SuperGlue. Adaptive early stopping pushes easy image pairs through quickly, while harder pairs go through more layers.
- In the paper's evaluation, adaptive depth and point pruning reduced latency relative to SuperGlue at comparable accuracy. The ratio depends on hardware and configuration.

**LoFTR (Detector-Free Local Feature Matching)**:
- Removes the keypoint detection step altogether. Performs dense matching across the whole image.
- Transformer-based, coarse-to-fine matching.
- Its biggest advantage is being able to match even in texture-poor regions.
- Downside: slow and uses a lot of GPU memory.

**Classical vs. learning-based comparison**:

| Item | ORB/SIFT | SuperPoint+LightGlue | LoFTR |
|------|----------|---------------------|-------|
| Speed (CPU) | fast | slow | very slow |
| Speed (GPU) | not applicable | moderate | slow |
| Texture-poor regions | fails | moderate | strong |
| Repetitive patterns | weak | strong | strong |
| GPU dependence | none | high | very high |
| Real-time robot use | easy | conditionally feasible | difficult |

**Code example — LightGlue (using kornia)**:

```python
import torch
import kornia
from kornia.feature import LightGlueMatcher, KeyNetAffNetHardNet

# Build the extractor and matcher
extractor = KeyNetAffNetHardNet(num_features=2048).eval()
matcher = LightGlueMatcher("keynet_affnet_hardnet").eval()

# Move to GPU
device = torch.device("cuda" if torch.cuda.is_available() else "cpu")
extractor = extractor.to(device)
matcher = matcher.to(device)

# Load images (kornia format: B x C x H x W, 0-1 range; KeyNet takes 1-channel grayscale input)
img0 = kornia.io.load_image(path0, kornia.io.ImageLoadType.GRAY32).unsqueeze(0).to(device)
img1 = kornia.io.load_image(path1, kornia.io.ImageLoadType.GRAY32).unsqueeze(0).to(device)

# Feature extraction
with torch.no_grad():
    lafs0, resp0, desc0 = extractor(img0)   # (lafs, responses, descriptors) tuple
    lafs1, resp1, desc1 = extractor(img1)

# Matching — LightGlue takes the keypoint geometry (lafs) along with the descriptors
    dists, match_idxs = matcher(desc0[0], desc1[0], lafs0, lafs1)
```

The LightGlue family (SuperPoint+LightGlue, or KeyNetAffNetHardNet+LightGlue as in the example above) is worth evaluating when a GPU is available and learned-matching robustness matters. When there is no GPU or the latency and power budgets are tight, a classical feature such as ORB is a simpler baseline. Make the final choice by measuring accuracy, latency, and memory on the target data.

> **Further reading**
> - [DeTone et al., "SuperPoint: Self-Supervised Interest Point Detection and Description" (2018)](https://arxiv.org/abs/1712.07629) — The original SuperPoint paper.
> - [Lindenberger et al., "LightGlue: Local Feature Matching at Light Speed" (2023)](https://arxiv.org/abs/2306.13643) — The original LightGlue paper.
> - [Sun et al., "LoFTR: Detector-Free Local Feature Matching with Transformers" (2021)](https://arxiv.org/abs/2104.00680) — The original LoFTR paper.

---

> **Technical Timeline: Computer Vision Fundamentals (Classical Methods)**
> - **~2004**: the era of classical features. Hand-crafted features like Harris Corner (1988) and SIFT (2004) dominate. Mathematically precise but computationally heavy.
> - **2006~2011**: lightweighting for real time. SURF (2006), FAST (2006), BRIEF (2010), ORB (2011) arrive. The speed problem gets solved, and the ORB family also escapes the patent constraints of SIFT and SURF, so real-time SLAM becomes feasible.
> - **2015~2019**: deep learning seeps in. Learning-based features like SuperPoint (2018) begin to surpass classical methods in performance.
> - **2020~**: fusion of geometry and learning. Learned matchers such as SuperGlue (2020) appear. Detector-free matching such as LoFTR (2021) and lightweight learned matching such as LightGlue (2023) appear. Classical geometry remains central in the SLAM/VO back-end.
> - **Recent direction**: learned features and matchers are entering the front-end, while the back-end still uses epipolar geometry and triangulation. The two families therefore coexist in the same systems.

---

# Ch.10 — Deep Learning for Perception

Classical CV designs image-processing and geometric operations explicitly. Learning-based perception instead fits representations from data to predict an object's class, location, or region. Classification, detection, and segmentation provide different outputs for locating and manipulating objects in a scene.

---

## 10.1 Choosing a Framework

Framework choice determines which research code and pretrained models can be run directly. Learning model definition, training, inference, and debugging in one framework also makes other codebases easier to compare.

### 10.1.1 PyTorch (Recommended)

**Strengths**:
- Intuitive dynamic graph (eager execution)
- Easy to debug
- Large ecosystem of research code and pretrained models
- Rich pretrained models (torchvision, timm)

There is also a practical reason. Many public research repositories and pretrained models provide PyTorch implementations, so familiarity with PyTorch makes it easier to run and modify recent work. Usage shares for conference code depend on how repositories are sampled, so they should not be presented as a fixed percentage without a documented census.

**Install**:

```bash
# CUDA 12.1 build
pip install torch torchvision torchaudio --index-url https://download.pytorch.org/whl/cu121
```

**Basic usage**:

```python
import torch
import torch.nn as nn

# Create a tensor
x = torch.randn(32, 3, 224, 224)  # (batch, channel, height, width)

# Use GPU
device = torch.device('cuda' if torch.cuda.is_available() else 'cpu')
x = x.to(device)
```

> **Further reading**
> - [PyTorch Official Tutorials](https://pytorch.org/tutorials/) — organized systematically from beginner to advanced.
> - [d2l.ai (Dive into Deep Learning)](https://d2l.ai/) — interactive textbook. PyTorch code and math appear together.
> - [Andrej Karpathy — Neural Networks: Zero to Hero](https://www.youtube.com/playlist?list=PLAqhIrjkxbuWI23v9cThsA9GvCAUhRvKZ) — the former Tesla AI Director explains neural networks from scratch.
> - [Jaejun Yoo's Playground](http://jaejunyoo.blogspot.com/search/label/kr) — a Korean blog that explains generative models like GAN and VAE well.

### 10.1.2 TensorFlow / JAX

**TensorFlow**: strong for production deployment, TF Lite mobile support
**JAX**: high-performance computation, functional programming, for research

Since most recent research code is released in PyTorch, learn PyTorch first.

That said, TensorFlow Lite is still widely used when deploying models to a robot's edge devices (Jetson, Raspberry Pi, etc.), and JAX is heavily used in Google DeepMind-style research, so at least be aware they exist.

> **Further reading**
> - [TensorFlow Official Guide](https://www.tensorflow.org/guide) — covers TFLite conversion.
> - [JAX Official Docs](https://jax.readthedocs.io/) — functional deep learning framework.

---

## 10.2 Deep Learning Fundamentals

CNNs provide the architectural context for ResNet, while the Transformer explains how ViT and DETR differ from earlier convolutional approaches.

### 10.2.1 Convolutional Neural Network (CNN)

A CNN extracts spatial features by applying learned filters across an image.

A CNN learns local patterns such as edges, corners, and textures from data. Unlike the hand-designed SIFT and ORB features covered earlier, its filters are optimized with the rest of the network for the training objective.

**Main components**:
- **Convolution Layer**: extract features with filters
- **Pooling Layer**: reduce spatial size (Max, Average)
- **Activation**: introduce nonlinearity (ReLU, GELU)
- **Batch Normalization**: stabilize training

```python
# Simple CNN block
class ConvBlock(nn.Module):
    def __init__(self, in_ch, out_ch):
        super().__init__()
        self.conv = nn.Conv2d(in_ch, out_ch, kernel_size=3, padding=1)
        self.bn = nn.BatchNorm2d(out_ch)
        self.relu = nn.ReLU(inplace=True)

def forward(self, x):
        return self.relu(self.bn(self.conv(x)))
```

From a linear algebra perspective, a convolution is "the inner product of a filter (kernel) with an image patch." The matrix multiplication taught in class is used directly here. With `kernel_size=3, padding=1` the output size is kept the same as the input — this pattern shows up very often.

> **Further reading**
> - [Stanford CS231n — Convolutional Neural Networks for Visual Recognition](https://www.youtube.com/playlist?list=PLoROMvodv4rMFqRtEuo6SGjY4XbRIVRd4) — a lecture series covering CNN foundations and visual-recognition models.
> - [d2l.ai — CNN chapter](https://d2l.ai/chapter_convolutional-neural-networks/index.html) — code and math explained together.
> - [3Blue1Brown — But what is a Neural Network?](https://www.youtube.com/watch?v=aircAruvnKk) — intuitive understanding of neural networks.

### 10.2.2 Attention & Transformer

**Self-Attention**: learns relationships between all positions within a sequence

```
Attention(Q, K, V) = softmax(QK^T / √d_k) V
```

A convolution aggregates a local neighborhood in one layer and expands its receptive field as layers accumulate. Transformer self-attention can relate distant positions within a single layer. Since 2020, vision systems have used pure Transformers and CNN-Transformer hybrids for classification, detection, and segmentation.

**Vision Transformer (ViT)** splits an image into fixed-size patches such as 16×16, treats each patch as a token, and feeds them into a Transformer encoder. The original paper reported higher image-classification performance than its comparison CNNs under large-scale pretraining, and Transformer families later became major options across several vision tasks.

> **Further reading**
> - [Vaswani et al., "Attention Is All You Need" (2017)](https://arxiv.org/abs/1706.03762) — the original Transformer paper.
> - [Dosovitskiy et al., "An Image is Worth 16x16 Words" (2020)](https://arxiv.org/abs/2010.11929) — the original ViT paper.
> - [Yannic Kilcher — Vision Transformer explanation](https://www.youtube.com/watch?v=TrdevFK_am4) — accessible walk-through of the paper.
> - [Andrej Karpathy — Let's build GPT from scratch](https://www.youtube.com/watch?v=kCc8FmEb1nY) — builds a Transformer from scratch. NLP-focused but directly relevant to understanding ViT.

---

## 10.3 Image Classification

Classification asks what an image contains. Object-detection and segmentation models also contain classifiers. The backbone of a pretrained classification model, such as ResNet or ViT, can serve as the feature extractor for another task.

**Representative models**:

| Model | Characteristics | Use |
| --- | --- | --- |
| ResNet | Residual connection, stable training | Backbone network |
| EfficientNet | Compound scaling, efficient | Mobile, efficiency-focused |
| ViT | Transformer-based | Large-scale data, high performance |
| ConvNeXt | Modernized CNN | Competes with ViT |

ResNet's residual connection adds a block's input to its output, stabilizing the training of deep networks. Many architectures have adopted this structure since its 2015 release.

**Using a pretrained model**:

```python
import torchvision.models as models

# Pretrained ResNet50
model = models.resnet50(weights='IMAGENET1K_V2')

# Use as a feature extractor
model.fc = nn.Identity()  # remove the last FC
features = model(x)  # (batch, 2048)
```

An ImageNet-pretrained model can be reused by removing its final classification layer and treating the preceding output as features. This transfer-learning pattern supplies an initial representation when labeled data for a new recognition problem is limited.

> **Further reading**
> - [He et al., "Deep Residual Learning for Image Recognition" (2015)](https://arxiv.org/abs/1512.03385) — the original ResNet paper and an influential basis for later vision models.
> - [Papers With Code — Image Classification](https://paperswithcode.com/task/image-classification) — a starting point for public implementations and historical leaderboards; verify current numbers and protocols on the benchmark's official page and in the source paper.
> - [timm (PyTorch Image Models) library](https://github.com/huggingface/pytorch-image-models) — loads hundreds of pretrained models in a single line.
> - [Stanford CS231n — Training Neural Networks](https://www.youtube.com/playlist?list=PLoROMvodv4rMFqRtEuo6SGjY4XbRIVRd4) — training techniques and tricks.

---

## 10.4 Object Detection

Classification can tell a robot that a cup is present, but manipulation also requires its location. Object detection predicts both the class and the bounding box of each object and is used in applications such as robot manipulation and autonomous driving.

### 10.4.1 Two-Stage Detectors

**Faster R-CNN**:
1. Region Proposal Network (RPN): proposes candidate regions
2. ROI Pooling: extracts features from each region
3. Classification + Bounding Box Regression

Strength: high accuracy
Weakness: slow speed

Faster R-CNN is the representative two-stage detector and is still used where accuracy matters (e.g., industrial inspection). The structure of "propose candidates first, then analyze them in detail" is intuitive, and it later led to Mask R-CNN and others.

### 10.4.2 One-Stage Detectors

**YOLO (You Only Look Once)**:
- Divides the image into a grid and predicts in one pass
- Real-time processing (30+ FPS)
- Versions: YOLOv5, YOLOv8, YOLOv11 (Ultralytics)

Unlike a two-stage detector, YOLO processes the full image without first generating region proposals. It can be used when a robot system needs real-time inference. Ultralytics' YOLOv8/v11 have a short setup and inference path, which suits prototyping.

```python
from ultralytics import YOLO

# Load the model and run inference
model = YOLO('yolov8n.pt')  # nano model
results = model('image.jpg')

# Visualize the results
results[0].show()
```

**SSD (Single Shot Detector)**:
- Predicts from feature maps at various scales
- Small objects are a known weakness of SSD, and the YOLOv3-and-later generations, which adopted FPN, do better on them

> **Further reading**
> - [Redmon et al., "You Only Look Once: Unified, Real-Time Object Detection" (2016)](https://arxiv.org/abs/1506.02640) — the original YOLO paper. Concise and a good read.
> - [Ultralytics YOLOv8 docs](https://docs.ultralytics.com/) — well organized from installation to custom training.
> - [Papers With Code — Object Detection](https://paperswithcode.com/task/object-detection) — a starting point for public implementations and historical leaderboards; verify current numbers and protocols on the benchmark's official page and in the source paper.
> - [Dark Programmer — Understanding precision and recall](https://darkpgmr.tistory.com/162) — an intuitive explanation of detection evaluation metrics.

### 10.4.3 Transformer-based

**DETR (Detection Transformer)** redefined detection as a "set prediction problem." A fixed number of learnable vectors called Object Queries correspond to each object, and training is end-to-end without NMS. This contrasts with prior methods, which used a complex pipeline of generating thousands of anchor boxes and removing duplicates with NMS. It had the drawback of slow initial training, but thanks to its clean structure, it spawned many follow-up works such as Deformable DETR, DINO, and Co-DETR.

> **Further reading**
> - [Carion et al., "End-to-End Object Detection with Transformers" (2020)](https://arxiv.org/abs/2005.12872) — the original DETR paper.
> - [Yannic Kilcher — DETR explanation](https://www.youtube.com/watch?v=T35ba_VXkMY) — accessible walk-through of the paper.
> - [HuggingFace — Object Detection guide](https://huggingface.co/docs/transformers/tasks/object_detection) — using DETR through the Transformers library.
> - [Zhao et al., "DETRs Beat YOLOs on Real-time Object Detection" (RT-DETR, CVPR 2024, arXiv:2304.08069)](https://arxiv.org/abs/2304.08069) — reports speed-accuracy improvements for real-time DETRs under the paper's comparison protocol.
> - [Cheng et al., "YOLO-World: Real-Time Open-Vocabulary Object Detection" (CVPR 2024, arXiv:2401.17270)](https://arxiv.org/abs/2401.17270) — adds text-prompt-based open-vocabulary detection to YOLO. Practical for detecting arbitrary objects in robotics.

---

## 10.5 Semantic Segmentation

This is the task of predicting a class for every pixel.

Robot manipulation makes the difference clear. Detection predicts a bounding box, whereas semantic segmentation predicts a class per pixel and can represent object-background boundaries more finely. A predicted boundary is not the exact physical contour, so grasping also needs depth, instance separation, and uncertainty checks. Pixel-level classification is likewise used to distinguish roads, sidewalks, and lane markings in autonomous driving.

**Representative models**:

| Model | Characteristics |
| --- | --- |
| FCN | An early representative of fully convolutional end-to-end segmentation |
| U-Net | Encoder-Decoder structure, originated in medical imaging |
| DeepLab v3+ | Atrous convolution, multi-scale |
| SegFormer | Transformer-based, lightweight decoder |

U-Net's encoder-decoder plus skip-connection structure has become the default pattern for segmentation. The encoder extracts features while reducing resolution, and the decoder restores the resolution while using skip connections to add back fine detail. This pattern is also widely used in other tasks such as depth estimation and image generation.

```python
# Using a segmentation model (transformers library)
from transformers import SegformerForSemanticSegmentation

model = SegformerForSemanticSegmentation.from_pretrained(
    "nvidia/segformer-b0-finetuned-ade-512-512"
)
```

> **Further reading**
> - [Papers With Code — Semantic Segmentation](https://paperswithcode.com/task/semantic-segmentation) — latest benchmarks.
> - [HuggingFace — Image Segmentation](https://huggingface.co/docs/transformers/tasks/semantic_segmentation) — how to use SegFormer and others.
> - [Two Minute Papers — videos on semantic segmentation](https://www.youtube.com/@TwoMinutePapers) — summarizes recent research in two minutes.

---

## 10.6 Instance & Panoptic Segmentation

**Instance Segmentation**: distinguishes each object instance
- Mask R-CNN: Faster R-CNN + Mask branch

**Panoptic Segmentation**: unifies semantic + instance
- "Things" (objects): instances distinguished
- "Stuff" (background): no instance distinction

Going one step further, semantic segmentation only tells you "this area is chair," not "there are three chairs and here is where each one ends." For a robot to carry out a command like "pick up the chair on the left," instance segmentation is required. Panoptic segmentation unifies the two and is used to understand the entire scene completely.

> **Further reading**
> - [He et al., "Mask R-CNN" (2017)](https://arxiv.org/abs/1703.06870) — the representative work of instance segmentation.
> - [Detectron2](https://github.com/facebookresearch/detectron2) — Meta's detection/segmentation framework. Makes it easy to use Mask R-CNN and related models.

---

## 10.7 Depth Estimation

This is the task of predicting depth from a single image.

If you can obtain depth information from a single monocular camera without a stereo camera or LiDAR, you can greatly cut hardware cost and weight. It is especially useful for systems with limited payload, such as drones or small robots. Recently, with models that show foundation-model-level generalization, practicality has improved considerably.

**Representative models**:
- **MiDaS**: trained on diverse datasets, general-purpose
- **Depth Anything**: foundation-model-level generalization
- **ZoeDepth**: metric depth estimation

```python
# Using Depth Anything
from transformers import pipeline

pipe = pipeline("depth-estimation", model="LiheYoung/depth-anything-base-hf")
result = pipe("image.jpg")
depth = result['depth']
```

A caveat: MiDaS and Depth Anything by default estimate **relative depth**. You can tell "A is closer than B," but you cannot tell "exactly how many meters to A." When metric depth is required, use ZoeDepth or the metric version of Depth Anything V2.

> **Further reading**
> - [Yang et al., "Depth Anything: Unleashing the Power of Large-Scale Unlabeled Data" (2024)](https://arxiv.org/abs/2401.10891) — the original Depth Anything paper.
> - [Godard et al., "Digging Into Self-Supervised Monocular Depth Estimation" (Monodepth2, ICCV 2019, arXiv:1806.01260)](https://arxiv.org/abs/1806.01260) — the baseline for self-supervised depth.
> - [HuggingFace — Monocular Depth Estimation](https://huggingface.co/docs/transformers/tasks/monocular_depth_estimation) — runnable code out of the box.
> - [Papers With Code — Monocular Depth Estimation](https://paperswithcode.com/task/monocular-depth-estimation) — check the latest benchmarks.

---

## 10.8 Advanced: Training Recipes

Model architecture is only one part of an experiment. Learning-rate and data-augmentation settings can change convergence and final accuracy even when the architecture stays fixed. This section collects training techniques that recur in practice.

**Learning Rate Schedule**:

- **Cosine Annealing with Warm-up**: one commonly used option. For the first few epochs, the learning rate is ramped linearly from 0 up to the target value (warm-up), then decayed along a cosine curve.

$$\eta_t = \eta_{min} + \frac{1}{2}(\eta_{max} - \eta_{min})\left(1 + \cos\left(\frac{t \cdot \pi}{T}\right)\right)$$

- **OneCycleLR**: a policy that raises the learning rate once and then lowers it. It can achieve super-convergence and converges quickly in few epochs.

```python
import torch.optim as optim

# Cosine Annealing (no warm-up). If you need warm-up, add it like pct_start in OneCycleLR below
optimizer = optim.AdamW(model.parameters(), lr=1e-4, weight_decay=0.05)
scheduler = optim.lr_scheduler.CosineAnnealingLR(optimizer, T_max=100)

# OneCycleLR
scheduler = optim.lr_scheduler.OneCycleLR(
    optimizer, max_lr=1e-3, total_steps=len(dataloader) * num_epochs,
    pct_start=0.1  # use the first 10% for warm-up
)
```

**Data Augmentation**:

| Technique | Description | Main use |
|------|------|---------|
| **RandAugment** | applies N transformations at magnitude M at random | general classification |
| **CutMix** | replaces an image region with another image and mixes the labels proportionally | classification |
| **MixUp** | linearly interpolates two images and their labels | classification |
| **Mosaic** | composes 4 images into one | detection (YOLO family) |

```python
import torchvision.transforms.v2 as T

# RandAugment
transform = T.Compose([
    T.RandAugment(num_ops=2, magnitude=9),
    T.ToTensor(),
    T.Normalize(mean=[0.485, 0.456, 0.406], std=[0.229, 0.224, 0.225]),
])
```

**Regularization**:

- **Label Smoothing**: use soft labels (e.g., 0.1, 0.9) instead of hard labels (0 or 1). Prevents overconfidence. `nn.CrossEntropyLoss(label_smoothing=0.1)`
- **Stochastic Depth**: randomly skip some layers during training. Effective at preventing overfitting in ResNet-family models.
- **Weight Decay**: set `weight_decay=0.01~0.05` in the optimizer. With AdamW, use decoupled weight decay.

**Gradient Clipping**: prevents gradients from exploding. Almost mandatory in Transformer training.

```python
torch.nn.utils.clip_grad_norm_(model.parameters(), max_norm=1.0)
```

**Diagnosing Problems from the Loss Curve**:

| Pattern | Diagnosis | Response |
|------|------|------|
| train loss decreasing, val loss increasing | Overfitting | add augmentation, increase dropout/weight decay, get more data |
| train loss stuck at a high value | Underfitting | increase model size, adjust learning rate, reduce augmentation |
| train loss oscillates heavily | Learning rate too high | decrease learning rate |
| train loss becomes NaN | Gradient explosion | gradient clipping, sharply reduce learning rate, validate data |
| val loss drops early then completely plateaus | Learning rate too low or schedule issue | add warm-up, apply cosine schedule |

**Distributed Training — PyTorch DDP basics**:

When the model gets large, a single GPU runs out of time. DistributedDataParallel (DDP) is the most basic parallel training method; it replicates the model across multiple GPUs and synchronizes gradients.

```python
# Minimal DDP structure (launch with torchrun)
import os
import torch
import torch.distributed as dist
from torch.nn.parallel import DistributedDataParallel as DDP

dist.init_process_group("nccl")
local_rank = int(os.environ["LOCAL_RANK"])
torch.cuda.set_device(local_rank)
model = model.to(local_rank)
model = DDP(model, device_ids=[local_rank])

# Launch: torchrun --nproc_per_node=4 train.py
```

> **Further reading**
> - [Goyal et al., "Accurate, Large Minibatch SGD" (2017)](https://arxiv.org/abs/1706.02677) — the learning rate scaling rule for large-scale training.
> - [PyTorch DDP Tutorial](https://pytorch.org/tutorials/intermediate/ddp_tutorial.html) — the official guide to distributed training.
> - [Wightman et al., "ResNet strikes back" (2021)](https://arxiv.org/abs/2110.00476) — a paper that shows the importance of training recipes. Using the same ResNet, changing only the training techniques improves accuracy substantially.

> **Exercise**: [Data Augmentation visualization](https://alexjunholee.github.io/robotics-practice/app.html#data_augmentation)
> Interactively see how various augmentation techniques such as RandAugment, CutMix, and MixUp transform images.

> **Exercise**: [Learning Rate Schedule visualization](https://alexjunholee.github.io/robotics-practice/app.html#lr_schedule)
> Compare curves of various learning rate schedules such as Cosine Annealing and OneCycleLR, and see the effect of hyperparameters.

---

## 10.9 Advanced: Self-Supervised and Contrastive Learning

Robotics data is label-scarce. A robot collects thousands or tens of thousands of images, but labeling each of them with bounding boxes or segmentation masks is impractical. Self-supervised learning creates a training signal from the data itself without labels.

**Contrastive Learning**:

Contrastive learning places different augmentations of the same image close together in the embedding space (a positive pair) and different images far apart (negative pairs).

- **SimCLR**: apply different augmentations to the same image to form positive pairs. Other images in the batch form the negative pairs. Requires a large batch size.
- **MoCo (Momentum Contrast)**: uses a momentum encoder and a queue to secure many negatives without needing a large batch.

**InfoNCE Loss**:

$$\mathcal{L} = -\log \frac{\exp(\text{sim}(z_i, z_j) / \tau)}{\sum_{k=1}^{2N} \mathbb{1}_{[k \neq i]} \exp(\text{sim}(z_i, z_k) / \tau)}$$

Here, sim is cosine similarity and τ is the temperature. The numerator raises the similarity of positive pairs, while the denominator trains the model to distinguish them from negative pairs.

**Masked Image Modeling — MAE**:

Based on ViT, this approach randomly masks 75% of image patches and reconstructs the masked portions from the remaining 25%. It follows the same principle as BERT in NLP masking and recovering words.

- Why the masking ratio is as high as 75%: images have much more redundancy than text, so a high masking ratio makes the task harder and forces better representations.
- Masking 75% of the patches leaves the encoder processing only 25% of them, which makes training efficient. The original paper reports that pre-training time drops by more than 3x. The masking ratio and the reduction in compute are not the same number: attention cost grows superlinearly in the number of tokens, and the decoder's cost is added on top.

**Connection to DINOv2**:

DINOv2 is trained via self-distillation. It uses a teacher-student structure, but the teacher is the EMA (exponential moving average) of the student.

- **Self-distillation**: student and teacher share the same architecture. The teacher's weights are an EMA of the student's weights.
- **Sharpening + batch normalization**: sharpening (low temperature) is applied to the teacher output, and mode collapse is prevented with SwAV's Sinkhorn-Knopp batch normalization instead of the centering (subtracting the mean) used in DINO v1.
- The resulting DINOv2 features are comparable to supervised methods even with the backbone frozen. ImageNet classification is evaluated with k-NN, which trains nothing, while ADE20K semantic segmentation is evaluated with a linear probe — a linear head is trained — and reported in mIoU. The two evaluations differ in what is trained and in what they measure, so check the original paper's numbers, together with the model size, before citing them.

**Practice — Fine-tuning a self-supervised backbone on HuggingFace**:

```python
from transformers import AutoModel, AutoImageProcessor
import torch.nn as nn

# Load the DINOv2 backbone
processor = AutoImageProcessor.from_pretrained("facebook/dinov2-base")
backbone = AutoModel.from_pretrained("facebook/dinov2-base")

# Freeze the backbone and train only the classification head
for param in backbone.parameters():
    param.requires_grad = False

def forward(self, pixel_values):
        features = self.backbone(pixel_values).last_hidden_state[:, 0]  # CLS token
        return self.head(features)
```

> **Further reading**
> - [Chen et al., "A Simple Framework for Contrastive Learning of Visual Representations (SimCLR)" (2020)](https://arxiv.org/abs/2002.05709) — the representative work of contrastive learning.
> - [He et al., "Masked Autoencoders Are Scalable Vision Learners" (2022)](https://arxiv.org/abs/2111.06377) — the original MAE paper.
> - [Oquab et al., "DINOv2: Learning Robust Visual Features without Supervision" (2024)](https://arxiv.org/abs/2304.07193) — the original DINOv2 paper.

---

## 10.10 Advanced: Knowledge Distillation

Knowledge distillation transfers the "knowledge" of a large model (teacher) to a small model (student). In robotics, it is used to run large models such as VFMs on edge devices. It can also be applied when running SAM in real time on a Jetson.

**Teacher-Student Structure**:

The student model (small model) is trained to imitate the output of the teacher model (large model, already trained). The teacher's soft prediction carries more information than a hard label (ground truth).

For example, for an image of "cat" the hard label is [1, 0, 0], but the teacher's soft prediction might be [0.85, 0.10, 0.05]. This soft prediction contains the information that "cat and dog are somewhat similar," and the student learns that too.

**Soft Targets and Temperature Scaling**:

$$\mathcal{L}_{KD} = \text{KL}\left(\sigma\left(\frac{z_t}{\tau}\right) \| \sigma\left(\frac{z_s}{\tau}\right)\right)$$

Here z_t and z_s are the teacher and student logits, respectively, and τ is the temperature. When τ > 1, the probability distribution becomes "softer," so more information about inter-class relationships is conveyed. A value of τ = 3~5 is used.

The total loss is a weighted sum of the hard label loss and the distillation loss:

$$\mathcal{L} = \alpha \cdot \mathcal{L}_{CE}(y, \sigma(z_s)) + (1 - \alpha) \cdot \tau^2 \cdot \mathcal{L}_{KD}$$

The reason for multiplying by τ^2: it compensates for the fact that gradient magnitudes shrink by 1/τ^2 due to temperature scaling.

**Feature-based Distillation (FitNets)**:

Feature-based distillation aligns intermediate feature maps as well as the final logits with those of the teacher.

$$\mathcal{L}_{feat} = \|f_t(x) - r(f_s(x))\|^2$$

Here r is a projection layer that matches the student feature dimension to the teacher's. This trains intermediate representations that are hard to transfer with logit distillation alone.

**Applications to VFM lightweighting**:

| Teacher | Student | Method |
|---------|---------|------|
| SAM (ViT-H) | MobileSAM | Replace the image encoder with a lightweight ViT, distillation |
| SAM (ViT-H) | FastSAM | Replace the entire pipeline with a YOLO architecture |
| DINOv2-giant | DINOv2-small | Distill into a smaller version of the same architecture |

```python
import torch
import torch.nn.functional as F

# Hard target loss
    hard_loss = F.cross_entropy(student_logits, labels)

return alpha * hard_loss + (1 - alpha) * soft_loss
```

> **Further reading**
> - [Hinton et al., "Distilling the Knowledge in a Neural Network" (2015)](https://arxiv.org/abs/1503.02531) — the original knowledge distillation paper.
> - [Zhang et al., "Faster Segment Anything (MobileSAM)" (2023)](https://arxiv.org/abs/2306.14289) — a case of SAM distillation.
> - [Romero et al., "FitNets: Hints for Thin Deep Nets" (2015)](https://arxiv.org/abs/1412.6550) — the original feature-based distillation paper.

---

## 10.11 Advanced: Domain Adaptation

When a model trained in simulation is deployed to a real robot, performance drops sharply. The same goes for training on indoor data and deploying outdoors. This problem is called **domain shift**, and the research that addresses it is domain adaptation. In robotics, it is directly tied to the sim-to-real gap problem.

**Problem Setup**:
- Source domain D_s (labeled): simulation data or an existing dataset
- Target domain D_t (unlabeled or small): the real deployment environment
- Goal: make a model trained on D_s also work well on D_t.

**Domain Randomization**:

The simplest yet effective approach. When generating training data in the simulator, randomize environment parameters to the extreme.

- Texture: randomly change the textures of walls, floor, and objects every episode
- Lighting: randomize position, color, and intensity
- Camera parameters: add noise to focal length, position, and angle
- Physical parameters: randomly set friction coefficient, mass, inertia, etc. within a range

The idea is that after seeing a sufficiently diverse set of simulated environments, the real environment can be treated as "just another variant."

**Adversarial Domain Adaptation**:

Introduces a domain discriminator so that the feature extractor learns domain-invariant features that the discriminator cannot distinguish between source and target.

```
Input --> Feature Extractor --> [Task Classifier]      --> Task Loss
                              \--> [Domain Discriminator] --> Domain Loss (GRL)
```

- Gradient Reversal Layer (GRL): reverses the gradient from the domain discriminator so that the feature extractor trains in the direction of not being able to distinguish domains.
- The task classifier trains normally on the source domain.
- The feature extractor learns representations that are useful for the task yet invariant to the domain.

$$\mathcal{L} = \mathcal{L}_{task}(D_s) - \lambda \cdot \mathcal{L}_{domain}(D_s, D_t)$$

The minus sign matters. The feature extractor is trained in the direction of "maximizing" the domain loss (adversarial training analogous to GANs).

**Test-Time Adaptation (TTA)**:

A way for the model to adapt to new environments even after deployment. Without accessing the training data, it adjusts the model using only the data that arrives at inference time.

- **TENT**: adjusts the affine parameters of batch normalization via entropy minimization.
- **CoTTA**: continual TTA. Adapts even when the distribution changes over time.

```python
# Simplified TENT update
model.eval()
model.requires_grad_(False)          # freeze everything first
params = []
for m in model.modules():
    if isinstance(m, nn.BatchNorm2d):
        m.requires_grad_(True)       # re-enable only the BN affine parameters
        m.track_running_stats = False
        m.running_mean = None        # clear the running buffers so batch statistics are used in eval mode
        m.running_var = None
        params += [m.weight, m.bias]

optimizer = optim.SGD(params, lr=1e-4)  # optimize only the BN parameters

# Adaptation at inference time
for batch in test_loader:
    output = model(batch)
    loss = entropy(output)  # minimize prediction entropy
    loss.backward()
    optimizer.step()
    optimizer.zero_grad()
```

**Connection to the sim-to-real gap**:

In real-world robotics these techniques can be combined according to the target and available data:
1. Generate diverse data in the simulator with domain randomization.
2. Perform adversarial adaptation with a small amount of real-environment data.
3. After deployment, continually adapt to environmental changes via TTA.

Using all three stages is not a standard recipe. A system may use domain randomization alone, add fine-tuning on real data, or prohibit adaptation after deployment; choose the combination according to safety and compute constraints.

> **Further reading**
> - [Tobin et al., "Domain Randomization for Transferring Deep Neural Networks from Simulation to the Real World" (2017)](https://arxiv.org/abs/1703.06907) — the original domain randomization paper.
> - [Ganin et al., "Domain-Adversarial Training of Neural Networks" (2016)](https://arxiv.org/abs/1505.07818) — the original adversarial domain adaptation paper (proposes GRL).
> - [Wang et al., "TENT: Fully Test-Time Adaptation by Entropy Minimization" (2021)](https://arxiv.org/abs/2006.10726) — the representative TTA work.
> - [Wen et al., "FoundationPose: Unified 6D Pose Estimation and Tracking of Novel Objects" (CVPR 2024, arXiv:2312.08344)](https://arxiv.org/abs/2312.08344) — 6D pose estimation for novel objects. Operates from a CAD model or a few reference images.

---

> **Technical Timeline: Deep Learning for Perception**
> - **2012**: AlexNet wins the ImageNet competition by a large margin over prior methods. The start of the "deep learning revolution." The era of hand-crafted features begins to end.
> - **2014~2016**: VGGNet, GoogLeNet, and ResNet appear. In particular, ResNet's (2015) residual connection makes it possible to train networks hundreds of layers deep. During this period, Faster R-CNN (2015) and YOLO (2016) make real-time object detection possible.
> - **2017**: "Attention Is All You Need" — Transformer is announced. Originally for NLP, but later extended to vision.
> - **2020~2021**: ViT (Vision Transformer) appears and patch-sequence processing spreads; DETR applies a Transformer to detection. The original Swin Transformer paper reports scores above its comparison models on several public vision benchmarks of that period.
> - **2022~**: ConvNeXt shows that "CNNs are not dead yet." Segment Anything (SAM) elevates segmentation to a foundation model. Extension to Spatial AI — depth estimation and 3D scene understanding become the next battleground for deep learning.
> - **Recent direction**: researchers are testing a single foundation-model representation across detection, segmentation, and depth estimation. Reusing DINOv2 features for several downstream tasks is one example.

---

# Ch.11 — Vision Foundation Models (VFM)

The models in the previous chapter were trained mainly for a specific dataset and task. A vision foundation model instead pretrains a representation on a larger corpus and adapts it across several tasks. Since 2023, more ICRA and IROS papers have applied these representations to robot perception.

---

## 11.1 What Is a Foundation Model?

A **foundation model** is a model pretrained on large-scale data and applicable to a wide range of downstream tasks.

Task-specific models often require new data collection, labeling, and training when the environment or target objects change. Foundation models aim to reduce that cost by reusing a large pretrained representation. Transfer to unseen objects and environments still varies by model and target domain, so zero-shot results should be distinguished from results after adaptation.

**Characteristics**:
- **Scale**: hundreds of millions to billions of parameters
- **Pretraining**: large-scale data (hundreds of millions of images)
- **Zero-shot / Few-shot**: performs new tasks with no training or only a few examples
- **Transfer**: transfers to diverse domains

Reusing a pretrained representation can reduce the need to build labels from scratch for each environment. Actual generalization still has to be evaluated separately for the target domain and adaptation setting.

Scaling laws describe empirical power-law relationships between performance and model size, data, or compute. They have informed the development of large models such as GPT, CLIP, and SAM. Gains from scale depend on how these resources are balanced: Hoffmann et al. (2022), for example, showed that increasing model size alone can be less effective than allocating data and compute together.

> **Further reading**
> - [Bommasani et al., "On the Opportunities and Risks of Foundation Models" (2021)](https://arxiv.org/abs/2108.07258) — Stanford report that defined the term "foundation model".
> - [Two Minute Papers — videos on foundation models](https://www.youtube.com/@TwoMinutePapers) — a quick way to keep up with the latest VFM research.
> - [HuggingFace Model Hub](https://huggingface.co/models) — thousands of pretrained models ready to use.

---

## 11.2 Major VFMs

This section compares DINOv2, SAM, CLIP, Depth Anything, and GroundingDINO through representation learning, segmentation, image-text alignment, monocular depth estimation, and open-vocabulary detection.

### 11.2.1 DINOv2

A **self-supervised Vision Transformer** that learns rich features from images without labels.

DINOv2 learns general-purpose visual features without labels. These features can be used as-is for diverse tasks such as classification, segmentation, and matching. In robotics in particular, DINOv2's dense features provide stable matching even in textureless regions, and are used in SLAM and visual odometry to reduce tracking failure rates in textureless environments.

**Characteristics**:
- self-distillation (DINO) + masked image modeling (iBOT) + KoLeo regularization
- strong transfer performance across diverse tasks
- provides dense visual features

**Uses**:
- image retrieval
- semantic segmentation (linear probe)
- feature matching for SLAM/VO
- feature backbone for 3D reconstruction

```python
import torch
from transformers import AutoModel, AutoImageProcessor

processor = AutoImageProcessor.from_pretrained('facebook/dinov2-base')
model = AutoModel.from_pretrained('facebook/dinov2-base')

inputs = processor(images=image, return_tensors="pt")
outputs = model(**inputs)
features = outputs.last_hidden_state  # (1, num_patches+1, 768): CLS + patch features
```

The first token of `last_hidden_state` is the [CLS] token summarizing the whole image, and the rest are per-patch features. The [CLS] token is used for classification, while the patch features are used for dense prediction (segmentation, matching, etc.).

> **Further reading**
> - [Oquab et al., "DINOv2: Learning Robust Visual Features without Supervision" (2023)](https://arxiv.org/abs/2304.07193) — the original DINOv2 paper.
> - [DINOv2 GitHub](https://github.com/facebookresearch/dinov2) — official code and pretrained models.
> - [HuggingFace — DINOv2](https://huggingface.co/docs/transformers/model_doc/dinov2) — ready to use on HuggingFace.
> - [Yannic Kilcher — DINO explained](https://www.youtube.com/watch?v=h3ij3F3cPIk) — explains self-distillation in DINOv1 and also helps with understanding DINOv2.

### 11.2.2 SAM (Segment Anything Model)

**Promptable segmentation**: segments any object from point, box, or mask prompts. Text prompts were an exploratory experiment in the original paper and are supported by neither the public checkpoints nor the API, so text-driven segmentation is covered later as a composition in which GroundingDINO turns text into boxes.

Conventional segmentation models could only segment the classes used during training. Trained on "chair, table, person", they cannot segment "cup". SAM is trained on 1.1B masks and aims to segment arbitrary objects without being tied to its training classes. Its performance drops in domains with a different distribution, such as medical, satellite, or underwater imagery (§11.5). For robots that have to manipulate objects they encounter for the first time in a new environment, the approach to segmentation has changed since SAM.

**Components**:
- Image Encoder: embeds images with a ViT
- Prompt Encoder: points, boxes, masks, etc.
- Mask Decoder: a lightweight decoder that produces masks

**SAM2**: video support, higher speed

SAM2 extends prompt-based segmentation from individual images to video. A point or box in the first frame identifies an object that the model then tracks and segments in later frames. This capability can support robots that must maintain an object mask while the camera or object moves.

```python
from segment_anything import sam_model_registry, SamPredictor

sam = sam_model_registry["vit_h"](checkpoint="sam_vit_h.pth")
predictor = SamPredictor(sam)

predictor.set_image(image)
masks, scores, logits = predictor.predict(
    point_coords=np.array([[500, 375]]),
    point_labels=np.array([1]),  # 1: foreground
    multimask_output=True,
)
```

With `multimask_output=True`, three mask candidates are returned (whole object, part, smaller part). `scores` is a predicted mask-quality (IoU) estimate, not a measure of which extent you wanted, so selecting a particular extent means changing the prompt or comparing the three candidates yourself.

> **Further reading**
> - [Kirillov et al., "Segment Anything" (2023)](https://arxiv.org/abs/2304.02643) — the original SAM paper.
> - [Ravi et al., "SAM 2: Segment Anything in Images and Videos" (2024)](https://arxiv.org/abs/2408.00714) — the original SAM2 paper. Extension to video segmentation.
> - [Segment Anything GitHub](https://github.com/facebookresearch/segment-anything) — official code.
> - [Segment Anything Explained](https://www.youtube.com/watch?v=KRAJd4_rNrc) — understand SAM's architecture and impact.
> - [HuggingFace — SAM](https://huggingface.co/docs/transformers/model_doc/sam) — ready to use on HuggingFace.

> **Exercise**: [SAM2 Interactive Segmentation](https://alexjunholee.github.io/robotics-practice/app.html#hf_sam)
> Try prompt-based segmentation on images using the SAM2 model directly (HuggingFace Space).

### 11.2.3 CLIP

**Vision-language model**: maps images and text into a shared embedding space.

Before CLIP, classifying an image required a predefined list of classes. CLIP places images and text in the same space, so arbitrary text can be used to retrieve or classify images. It becomes possible to tell a robot its target object in natural language, such as "red mug on a wooden table". This is the start of open-vocabulary, and the foundation for robots understanding natural language.

**Training**: contrastive learning on 400M image-text pairs

**Uses**:
- zero-shot image classification
- image-text retrieval
- basis for open-vocabulary detection

```python
import clip
import torch

model, preprocess = clip.load("ViT-B/32", device="cuda")

image = preprocess(Image.open("image.jpg")).unsqueeze(0).to("cuda")
text = clip.tokenize(["a dog", "a cat", "a car"]).to("cuda")

with torch.no_grad():
    image_features = model.encode_image(image)
    text_features = model.encode_text(text)

# CLIP only yields a cosine similarity if both features are L2-normalized before the inner product
    image_features /= image_features.norm(dim=-1, keepdim=True)
    text_features /= text_features.norm(dim=-1, keepdim=True)
    similarity = (100.0 * image_features @ text_features.T).softmax(dim=-1)  # 100.0 ≈ model.logit_scale.exp(), the learned temperature scale
    print(similarity)  # similarity between each text and the image
```

`@` is matrix multiplication. CLIP L2-normalizes both features before taking the inner product, which is what makes the value a cosine similarity; the result is then multiplied by a learned scale and passed through a softmax. Skip the normalization and what you get is a plain dot product, not a cosine. This is the principle of zero-shot classification.

> **Further reading**
> - [Radford et al., "Learning Transferable Visual Models From Natural Language Supervision" (2021)](https://arxiv.org/abs/2103.00020) — the original CLIP paper.
> - [OpenAI CLIP GitHub](https://github.com/openai/CLIP) — official code and pretrained models.
> - [Yannic Kilcher — CLIP explained](https://www.youtube.com/watch?v=T9XSU0pKX2E) — a clear walk-through of the CLIP idea.
> - [HuggingFace — CLIP](https://huggingface.co/docs/transformers/model_doc/clip) — use various CLIP variants on HuggingFace.

### 11.2.4 Depth Anything

**Monocular depth foundation model**: estimates relative depth from a single image.

Depth Anything is a monocular depth model trained with 1.5M labeled and 62M unlabeled images. It has been evaluated indoors (NYU), outdoors (KITTI), and in zero-shot domains, but accuracy can drop in settings far from its training data, such as endoscopic or underwater imagery. Results without additional training should therefore be distinguished from results adapted to the target environment.

**Characteristics**:
- trained on 1.5M labeled + 62M unlabeled images
- robust across diverse domains
- metric-depth models available for both V1 and V2 (V2's distinguishing point is improved relative-depth quality from retraining on synthetic labels)

Depth Anything V2 also provides a model for metric, or absolute, depth. This variant is useful when a robotics system needs distances in physical units rather than only relative ordering.

> **Further reading**
> - [Yang et al., "Depth Anything: Unleashing the Power of Large-Scale Unlabeled Data" (2024)](https://arxiv.org/abs/2401.10891) — the original Depth Anything paper.
> - [Yang et al., "Depth Anything V2" (2024)](https://arxiv.org/abs/2406.09414) — the original V2 paper. Metric depth support.
> - [Depth Anything GitHub](https://github.com/LiheYoung/Depth-Anything) — official code.
> - [HuggingFace — Depth Anything](https://huggingface.co/docs/transformers/model_doc/depth_anything) — ready to use on HuggingFace.

> **Exercise**: [Depth Anything V2](https://alexjunholee.github.io/robotics-practice/app.html#hf_depth)
> Try the Depth Anything V2 model to estimate depth from images directly (HuggingFace Space).

### 11.2.5 GroundingDINO

**Open-vocabulary object detection**: detects arbitrary objects from text prompts.

Closed-set detectors such as standard YOLO and Faster R-CNN models predict a fixed label set. GroundingDINO instead accepts text queries and returns boxes for image regions that match them. A phrase such as "red cup" can therefore be used as the detection query without training a new class-specific output head.

```
Input: image + "person. car. traffic light."
Output: bounding boxes for the corresponding objects
```

**Grounded-SAM**: GroundingDINO + SAM combined
→ text-prompted object detection + segmentation

In Grounded-SAM, GroundingDINO finds a box for a query such as "red cup", and SAM produces a mask within that box. This composition supplies text-conditioned regions and masks to a manipulation pipeline.

> **Further reading**
> - [Liu et al., "Grounding DINO: Marrying DINO with Grounded Pre-Training for Open-Set Object Detection" (2023)](https://arxiv.org/abs/2303.05499) — the original GroundingDINO paper.
> - [Grounded-SAM GitHub](https://github.com/IDEA-Research/Grounded-Segment-Anything) — text-based detection+segmentation pipeline.
> - [HuggingFace — Grounding DINO](https://huggingface.co/docs/transformers/model_doc/grounding-dino) — use on HuggingFace.

> **Exercise**: [Grounding DINO Demo](https://alexjunholee.github.io/robotics-practice/app.html#hf_grounding_dino)
> Try open-vocabulary detection that finds arbitrary objects in images from text prompts (HuggingFace Space).

---

## 11.3 Spatial AI Applications of VFMs

We look at how the VFMs covered above are combined in real robotics systems. The capability of each individual model matters, but the goal in robotics is to combine them to build an AI that understands space.

**Open-vocabulary scene understanding**:
- scene understanding without predefined classes
- handling natural-language commands such as "navigate to the red chair"

For robots to operate in real environments, they cannot rely on a predetermined list of objects. They must understand a person's natural-language command, find the corresponding object, and act accordingly. This pipeline can be implemented with a CLIP + SAM + GroundingDINO combination.

**Zero-shot semantic segmentation**:
- segmentation in new environments without labeling
- implemented with a CLIP + SAM combination

**Dense features for SLAM**:
- use DINOv2 features in place of keypoints
- matching is possible even in textureless regions
- recent work: DROID-SLAM + DINOv2

Classical SLAM relies on keypoints such as ORB and SIFT, which are difficult to obtain on textureless walls and floors. DINOv2's dense features carry semantic information and can distinguish locations even within such regions. SLAM systems can use these features for matching to reduce tracking failures in textureless environments.

**3D scene understanding**:
- lift 2D VFM features into 3D
- Semantic NeRF, Feature 3DGS

Embedding 2D-extracted VFM features into a 3D representation (NeRF, 3D Gaussian Splatting) carries semantic information in the 3D space itself. A question like "where is the chair in this 3D map?" can be answered with a text query. Research in this direction is growing in Spatial AI (LERF, LangSplat, ConceptGraphs, etc.).

> **Further reading**
> - [Kerr et al., "LERF: Language Embedded Radiance Fields" (2023)](https://arxiv.org/abs/2303.09553) — work that embeds CLIP features into NeRF. A representative example of Spatial AI.
> - [Tschernezki et al., "Neural Feature Fusion Fields: 3D Distillation of Self-Supervised 2D Image Representations" (2022)](https://arxiv.org/abs/2209.03494) — early work on lifting 2D features into 3D.
> - [Papers With Code — 3D Scene Understanding](https://paperswithcode.com/task/3d-scene-understanding) — latest research trends.

---

## 11.4 Lightweight Models and Edge Deployment

Using VFMs in a robot's Local Module requires making them lightweight.

VFMs have hundreds of millions of parameters, so their compute and memory demands are high. A ViT-B-class model runs on a desktop GPU, but meeting real-time deadlines on onboard compute while sharing resources with other modules is hard. Field robots, on the other hand, have to run them on onboard compute, and the throughput they need is set by the control loop and the task: retrieval, map update, and manipulation each carry a different latency budget. Bridging this gap is the job of lightweight modeling and edge deployment. No matter how good a model is, if it cannot run in real time on a robot, it only shines inside a paper.

**Lightweight techniques**:
| Technique | Description |
|------|------|
| **Distillation** | transfer knowledge from a large model to a small one |
| **Quantization** | reduce precision from FP32 → INT8/INT4 |
| **Pruning** | remove unimportant weights |

Understanding the trade-offs of each technique matters. Quantization does not change the model structure, so it is easiest to apply; pruning reduces actual compute but can incur accuracy loss. Distillation trains a small model from scratch, so its effect is largest but its cost is also highest.

**Lightweight VFMs**:
- **FastSAM**: lightweight version of SAM (YOLO-based)
- **MobileSAM**: SAM for mobile
- **EfficientViT-SAM**: efficient ViT backbone

**Edge deployment tools**:
- **TensorRT**: optimization for NVIDIA GPUs
- **ONNX Runtime**: cross-platform
- **TFLite**: mobile/embedded

```python
# TensorRT conversion example (PyTorch → ONNX → TensorRT)
import torch

# 1. Export to ONNX
torch.onnx.export(model, dummy_input, "model.onnx")

# 2. Convert to TensorRT (using trtexec)
# trtexec --onnx=model.onnx --saveEngine=model.trt --fp16
```

On NVIDIA Jetson, TensorRT is one available inference backend. FP16 or INT8 latency, memory use, and task-metric changes depend on the model graph, input size, batch, Jetson power mode, and TensorRT/CUDA versions. Benchmark against an FP32 baseline on the target device and the same validation set; INT8 also needs representative calibration data and a fresh accuracy check.

> **Further reading**
> - [NVIDIA TensorRT documentation](https://docs.nvidia.com/deeplearning/tensorrt/) — TensorRT usage and optimization guide.
> - [ONNX Runtime](https://onnxruntime.ai/) — cross-platform inference optimization.
> - [MobileSAM GitHub](https://github.com/ChaoningZhang/MobileSAM) — mobile-lightweight version of SAM.
> - [FastSAM GitHub](https://github.com/CASIA-IVA-Lab/FastSAM) — YOLO-based lightweight SAM.
> - [NVIDIA Jetson AI Courses](https://developer.nvidia.com/embedded/learn/jetson-ai-certification-programs) — edge deployment practice.

---

## 11.5 Advanced: VFM Fine-tuning and Adaptation

Using a VFM as-is yields zero-shot performance, but performance drops in specific domains (medical, satellite, underwater, etc.). Fine-tuning is needed, but training all of its hundreds of millions of parameters is costly. Parameter-efficient fine-tuning (PEFT) trains only a tiny fraction of the model's parameters while achieving performance close to full fine-tuning.

**Comparison of fine-tuning strategies**:

| Strategy | Fraction of trained params | Performance | GPU memory | Application difficulty |
|------|-------------------|------|-----------|------------|
| **Full fine-tuning** | 100% | best (given enough data) | very high | low |
| **Linear probing** | <1% (head only) | low | low | very low |
| **LoRA** | 0.1~1% | high | low | moderate |
| **Adapter** | 1~5% | high | moderate | moderate |
| **Prompt tuning** | <0.1% | moderate | low | high |

**LoRA (Low-Rank Adaptation)**:

LoRA adds a low-rank update to a pretrained weight matrix $\mathbf{W}$.

$$\mathbf{W}' = \mathbf{W} + \Delta\mathbf{W} = \mathbf{W} + \mathbf{B}\mathbf{A}$$

Here $\mathbf{W}$ is a $d \times d$ matrix, $\mathbf{B}$ is $d \times r$, and $\mathbf{A}$ is $r \times d$ ($r \ll d$). Instead of the $d^2$ parameters of the original $\mathbf{W}$, only $2dr$ parameters are trained.

For example, with $d = 768$ and $r = 8$, instead of the original 589,824 parameters, only 12,288 are trained (about 2%).

```python
from peft import LoraConfig, get_peft_model
from transformers import AutoModelForImageClassification

# Load the base model
model = AutoModelForImageClassification.from_pretrained(
    "facebook/dinov2-base",
    num_labels=10
)

# LoRA configuration
lora_config = LoraConfig(
    r=16,                      # rank (low-rank matrix dimension)
    lora_alpha=32,             # scaling factor
    target_modules=["query", "value"],  # apply only to attention Q, V
    lora_dropout=0.1,
    bias="none",
)

# Create the PEFT model
model = get_peft_model(model, lora_config)
model.print_trainable_parameters()
# Example output: trainable params: 589,824 || all params: 86,567,178 || trainable%: 0.68%
```

**Adapter**:

Insert small bottleneck layers between Transformer blocks. The original weights are frozen, and only the adapter layers are trained.

```
Input → [Frozen Attention] → [Adapter: down_proj → ReLU → up_proj] → [Frozen FFN] → Output
```

LoRA merges into the existing weights, so there is no extra cost at inference; adapters are extra layers, so they add a small inference latency.

**Prompt tuning**:

Add learnable virtual tokens to the input. The model itself is left untouched; only the input is manipulated.

- Visual Prompt Tuning (VPT): adds learnable tokens to the input of each ViT layer.
- Parameter efficiency is the highest, but performance tends to be slightly below LoRA.

**Adapting SAM to specific domains**:

Domain adaptation for SAM can adjust the prompts and their generator or fine-tune the image encoder.

1. **Grid prompt**: split the image into an NxN grid and use each intersection as a point prompt.
2. **Learned prompt generator**: train a lightweight network that takes an image as input and automatically generates point/box prompts.
3. **LoRA + SAM**: apply LoRA to the image encoder to learn domain-specific features.

```python
# SAM + LoRA application example (conceptual)
from segment_anything import sam_model_registry
from peft import LoraConfig, get_peft_model

sam = sam_model_registry["vit_b"](checkpoint="sam_vit_b.pth")

# Apply LoRA only to the image encoder
lora_config = LoraConfig(
    r=4,
    lora_alpha=8,
    target_modules=["qkv"],  # SAM attention qkv projection
)

sam.image_encoder = get_peft_model(sam.image_encoder, lora_config)
# Full fine-tuning for the mask decoder (since it has few parameters)
for param in sam.mask_decoder.parameters():
    param.requires_grad = True
```

**Evaluation methodology**:

The following protocol matrix can be used to compare VFM adaptation methods.

| Protocol | Description | Purpose of comparison |
|---------|------|----------|
| **Zero-shot** | evaluate without training | check the baseline generality of the VFM |
| **Few-shot (1/5/10-shot)** | train with a small number of samples per class | compare data efficiency |
| **Full fine-tune** | use the full training set | check the upper bound |
| **PEFT (LoRA, etc.)** | train with few parameters | efficiency-performance trade-off |

For fair comparison, the same backbone, the same data split, and the same augmentation must be used. In few-shot settings the variance across seeds is large, so results should be reported as the mean and standard deviation over 3-5 repetitions.

> **Further reading**
> - [Hu et al., "LoRA: Low-Rank Adaptation of Large Language Models" (2022)](https://arxiv.org/abs/2106.09685) — the original LoRA paper (for LLMs, but directly applicable to ViTs).
> - [HuggingFace PEFT library](https://github.com/huggingface/peft) — implementations of LoRA, Adapter, and other PEFT methods.
> - [Chen et al., "SAM Fails to Segment Anything? — SAM-Adapter" (2023)](https://arxiv.org/abs/2304.09148) — a case study of SAM domain adaptation.

---

> **Technical Timeline: Vision Foundation Models**
> - **2021**: CLIP (OpenAI) released. It trained a shared embedding on 400M image-text pairs and demonstrated zero-shot classification. DINO (ICCV 2021) demonstrates the potential of self-supervised ViTs.
> - **2022**: Self-supervised pretraining methods such as Masked Autoencoders (MAE, CVPR 2022) begin to draw attention.
> - **2023**: SAM (Segment Anything Model) released. Trained on 11M images and 1.1B masks. Achieves foundation-model-level generality with "segment anything". DINOv2 released the same year — a new standard for self-supervised vision features.
> - **2024**: Rapid evolution of VFMs including SAM2 (extension to video segmentation), Depth Anything V2 (metric depth support), and Florence-2 (unified vision model). Lightweight modeling and edge deployment become active.
> - **2025~**: 3D extensions of VFMs and multimodal unification accelerate. The direction is a single foundation model that jointly handles detection, segmentation, depth, and tracking. In robotics, VFMs are on course to become the standard perception backbone.
> - **Recent direction**: Zero-shot transfer is one reason to use foundation models in robot perception. Systems such as NLMap and ConceptGraphs combine representations from CLIP, SAM, and DINOv2 for open-vocabulary perception. On a physical robot, lightweight variants such as FastSAM and MobileSAM must be evaluated for both latency and accuracy.

---

# Ch.12 — Vision-Language-Action (VLA) & Embodied AI

The goal of VLA is for a robot to take a natural-language command like "pick up the red cup and place it on the table" and execute it. It is the field that unifies vision, language models, and control. This conceptual bridge explains why text-only language models cannot directly move a robot and why a policy trained in simulation often degrades on physical hardware.

## 12.1 VLA Concepts

Earlier robot systems separated visual perception, language understanding, and action generation into independent pipelines.

**VLA (Vision-Language-Action)** handles these three roles with a single model.

```
Input: image + natural-language command ("pick up the red cup")
Output: robot action (joint angles, gripper commands, etc.)
```

**Embodied AI**: AI that learns while interacting in a physical environment
- Extends beyond perception to include action
- The gap between simulation and the real environment (sim-to-real)

What sets embodied AI apart from earlier AI is that the model does not stop at classifying "this is a cup" — it must physically pick the cup up. This process has to account for gravity, friction, and collisions, which makes it far harder than simple image classification.

> **Further reading**
> - [Google DeepMind Robotics Blog](https://deepmind.google/discover/blog/) — official blog posts on RT-1, RT-2, PaLM-E, and more
> - [Brohan et al., "RT-2: Vision-Language-Action Models" (2023)](https://arxiv.org/abs/2307.15818) — an early paper that set the VLA framing

## 12.2 Key Models and Research

### 12.2.1 RT-1, RT-2 (Google DeepMind)

RT-1 and RT-2 are examples of combining large robot datasets with web-scale visual and language knowledge in a single policy. In the environment reported by its paper, RT-1 showed that one model could perform hundreds of tasks.

RT-1 (Robotics Transformer 1) was trained on large-scale robot demonstrations: 130K episodes spanning more than 700 tasks. Its output is a tokenized action.

RT-2 (Robotics Transformer 2) fine-tunes VLMs such as PaLI-X and PaLM-E to produce robot actions. Its paper reports transfer from web-scale data and a separate robot-chain-of-thought experiment.

The idea behind RT-2 is simple. Large language/vision models trained on the internet already carry "knowledge about the world," so fine-tuning them to produce robot actions lets them handle new objects or situations zero-shot. For example, RT-2 can pick up objects it never saw in training by leveraging its language knowledge.

> **Further reading**
> - [Google DeepMind — RT-2 Demo Video](https://deepmind.google/discover/blog/rt-2-new-model-translates-vision-and-language-into-action/) — footage and commentary on RT-2 in action
> - [Brohan et al., "RT-1: Robotics Transformer" (2022)](https://arxiv.org/abs/2212.06817) — original RT-1 paper
> - [Brohan et al., "RT-2" (2023)](https://arxiv.org/abs/2307.15818) — original RT-2 paper

### 12.2.2 PaLM-E

PaLM-E is a 562B-parameter embodied multimodal language model that combines PaLM, a ViT, and robot-state inputs. It handles several robot tasks in one model.

What makes PaLM-E interesting is that it demonstrated "positive transfer." Jointly training on robot data, web images, and text actually improves robot task performance compared to training on each separately. It empirically showed that general-purpose knowledge also helps robot actions.

> **Further reading**
> - [Driess et al., "PaLM-E: An Embodied Multimodal Language Model" (2023)](https://arxiv.org/abs/2303.03378) — original PaLM-E paper

### 12.2.3 OpenVLA

The full weights of RT-2 and PaLM-E are not public. OpenVLA publishes its code and weights, so a lab with sufficient compute can download, fine-tune, and deploy it on a robot.

OpenVLA has 7B parameters, is based on Llama 2, and was trained on 970K robot episodes from multiple embodiments.

```python
# OpenVLA usage example (conceptual)
from openvla import OpenVLAModel

model = OpenVLAModel.from_pretrained("openvla/openvla-7b")

action = model.predict(
    image=current_image,
    instruction="pick up the blue block and place it on the red target"
)
```

The RT-X project is another project to know alongside OpenVLA. It combines robot data from many institutions into the Open X-Embodiment dataset and uses it to train general-purpose robot policies. The dataset includes data from 22 robot embodiments collected by 21 institutions.

**Octo**: Another open-source model trained on RT-X data. It is smaller than OpenVLA (93M parameters) and therefore lighter to use. The model is designed for quick fine-tuning on diverse robot platforms.

> **Further reading**
> - [OpenVLA GitHub](https://github.com/openvla/openvla) — code and model weights released
> - [Kim et al., "OpenVLA" (2024)](https://arxiv.org/abs/2406.09246) — OpenVLA paper
> - [Open X-Embodiment Collaboration, "Open X-Embodiment" (2023)](https://arxiv.org/abs/2310.08864) — RT-X dataset paper
> - [Octo GitHub](https://github.com/octo-models/octo) — lightweight open-source robot policy model

### 12.2.4 Navigation

Beyond manipulation, moving (navigation) within an environment is also a core problem for embodied AI. The studies below apply the language understanding of LLMs to navigation.

LINGO is a family of driving-domain VLA models released by Wayve. LINGO-1 is an open-loop model that narrates driving scenes in language, while LINGO-2 is a closed-loop model that handles language instructions and driving control together.

**SayCan**: separates what the LLM "can do" from what it "should do"
- Affordance function: the actions the robot can currently perform
- LLM: the actions required to achieve the goal

Consider how SayCan handles a simple request. Tell an LLM to "make me coffee," and it can plan: "1. grab a cup, 2. walk to the coffee machine, 3. press the button..." But if the robot is not near a cup, it cannot execute the first step. SayCan combines the LLM's plan (what should be done) with the robot's feasible actions (what can be done) to select an action that is both executable and likely to advance the goal.

> **Further reading**
> - [Ahn et al., "Do As I Can, Not As I Say: Grounding Language in Robotic Affordances" (2022)](https://arxiv.org/abs/2204.01691) — SayCan paper
> - [SayCan project page](https://say-can.github.io/) — includes demo videos

## 12.3 World Models

Repeated trial and error on physical robots carries substantial time and equipment costs. A world model predicts a next state or observation from the current state and action, allowing a model-based policy to evaluate candidate actions without executing all of them on hardware.

A world model can be used for model-based RL rollouts and for evaluating risky actions before hardware execution.

In autonomous driving, GAIA-1 predicts action-conditioned driving video, DriveDreamer generates text-conditioned driving scenes, and MILE jointly learns future states and a driving policy through an implicit world model.

Its structure resembles a state-space model.

```
z_{t+1} = f(z_t, a_t)     # Dynamics model (current state + action → next state)
o_t = g(z_t)              # Observation model (latent state → observation)
r_t = h(z_t, a_t)         # Reward model (reward prediction)
```

You may have noticed this has a structure similar to the state-space model you learned in linear algebra. Think of it as x_{t+1} = Ax_t + Bu_t extended to a nonlinear neural-network version.

> **Further reading**
> - [Hu et al., "GAIA-1: A Generative World Model for Autonomous Driving" (2023)](https://arxiv.org/abs/2309.17080) — Wayve's world model paper
> - [Wang et al., "DriveDreamer" (2023)](https://arxiv.org/abs/2309.09777) — driving-scenario generation paper
> - [Yannic Kilcher — World Models Explained](https://www.youtube.com/watch?v=dPsXxLyqpfs) — video explainer on world models

## 12.4 End-to-End vs Modular

End-to-end and modular architectures differ in where they separate perception, planning, and control.

**End-to-End**:

```
sensor input → [single neural network] → action output
```

- Pros: simple pipeline, no bottleneck from intermediate representations
- Cons: lack of interpretability, requires large-scale data
- Examples: NVIDIA PilotNet, Tesla FSD (presumed)

End-to-end autonomous driving has developed in several forms. UniAD (2023) retains detection, tracking, mapping, prediction, and planning modules inside an end-to-end framework and received the CVPR 2023 Best Paper award. VAD (2023) converts a scene into a vectorized representation, while GenAD (2024) uses a generative formulation for driving scenarios.

**Modular**:

```
sensors → [perception] → [prediction] → [planning] → [control] → action
```

- Pros: each module can be developed/debugged independently, interpretable
- Cons: information loss between modules, hard to jointly optimize
- Examples: Apollo, Autoware

Hybrid designs are another option: perception can be learning-based while planning and control retain explicit models and safety checks, or explicit modules can sit inside an end-to-end framework as in UniAD.

> **Further reading**
> - [Hu et al., "Planning-oriented Autonomous Driving (UniAD)" (2023)](https://arxiv.org/abs/2212.10156) — CVPR 2023 Best Paper
> - [Jiang et al., "VAD" (2023)](https://arxiv.org/abs/2303.12077) — vectorization-based autonomous driving
> - [Andrej Karpathy — Tesla AI Day 2022 Presentation](https://www.youtube.com/watch?v=ODSJsviD_SU) — end-to-end autonomous driving from a practitioner's view

### End-to-End vs Modular in Practice

Which architecture is preferable depends on the application and its verification requirements. Practical systems often retain modular or hybrid structure for the following reasons.

- **Debugging**: when an end-to-end model fails, the cause is hard to find. For "why did the robot drop the cup?", a modular system lets you narrow it down to "depth estimation was wrong" or "grasp planning was wrong," but with end-to-end you do not know where it went wrong.
- **Safety guarantees**: a modular system lets you insert safety checks into each module (speed limits, collision detection, etc.). Putting such guarantees into an end-to-end system is difficult.
- **Partial updates**: a modular system can replace only the perception module. In an end-to-end system, a change may require joint retraining or revalidation of several components.
- **Data efficiency**: the general-purpose policies discussed here use large datasets. RT-1's dataset contained about 130K episodes, and OpenVLA was trained on 970K episodes. Most labs cannot collect the same scale of data themselves.

One practical direction is **hybrid**: use a VFM for perception while retaining explicit planning, control, and safety checks. The Local/Global Module design in Ch.18 follows this direction.

For end-to-end systems to replace modular designs broadly, they must demonstrate debuggability, data efficiency on small datasets, and application-specific safety guarantees. Public systems address these requirements over different scopes; no single architecture establishes all three across applications.

## 12.5 Spatial AI + VLA Integration

A VLA interprets long-horizon tasks such as "bring me coffee," while a local controller handles real-time obstacle avoidance and stabilization. A physical system must connect outputs at both time scales.

Connection to the lab's 2-Module Architecture:

**Local (Fast) Perception**:
- Geometric understanding: depth, obstacles, pose
- Real-time response (10–100 Hz)
- Classical or lightweight learned models

**Global (Heavy) Understanding**:
- Semantic understanding: objects, relations, context
- VFM/VLA-based
- Server or cloud processing (1–10 Hz)

**Integration scenario**:

```
1. Local: real-time obstacle avoidance, odometry
2. Global: "find a cup in the kitchen and bring it to the table"
   - Recognize the cup with a VLM
   - Plan a route on the semantic map
3. Local receives Global's waypoints and carries out the actual motion
```

## 12.6 Sim-to-Real & Simulation Platforms

Gathering data on a real robot is slow, expensive, and risky. Sim-to-real therefore trains first in simulation and transfers the result to a physical robot. A "reality gap" remains between simulation and reality; the main techniques for narrowing it are summarized below.

**Domain Randomization**: randomly varies textures, lighting, physics parameters, and so on in simulation during training. Once the model is exposed to many conditions, the real environment can be treated as just one more variation among them.

Four major simulation platforms illustrate the range of uses. NVIDIA Isaac Sim/Lab provides GPU-accelerated physics and can run thousands of environments in parallel; Isaac Lab is an integrated robot-learning framework. AI2-THOR provides household environments such as kitchens and living rooms for indoor interaction tasks. Habitat supports navigation learning in large-scale scanned environments such as Matterport3D and Gibson and hosts the Habitat Challenge. MuJoCo emphasizes contact dynamics and is used for manipulation and locomotion; after acquiring it, DeepMind released it as open source.

> **Further reading**
> - [NVIDIA Isaac Lab Documentation](https://isaac-sim.github.io/IsaacLab/) — a simulation framework for robot learning
> - [AI2-THOR Documentation](https://ai2thor.allenai.org/) — indoor-environment simulator
> - [Habitat Documentation](https://aihabitat.org/) — Meta's embodied AI platform
> - [Tobin et al., "Domain Randomization for Transferring Deep Neural Networks from Simulation to the Real World" (2017)](https://arxiv.org/abs/1703.06907) — original Domain Randomization paper

## 12.7 Advanced: Imitation Learning

Policy learning in VLA and embodied AI broadly divides into reinforcement learning (RL) and imitation learning (IL). The two approaches differ in how they collect data, define rewards, and pay for exploration on physical robots.

**Behavioral Cloning (BC)**

The simplest IL method. Collect demonstration data `{(s_t, a_t)}` from an expert (a human or a script) and perform supervised learning that predicts action `a_t` from state `s_t`.

```
Loss = E[ || π_θ(s_t) - a_t ||^2 ]
```

The method is simple and easy to implement, but it has an important limitation: **distribution shift**. During training, the policy follows the expert's state distribution. During inference, its own imperfect actions determine each subsequent state. Small errors can accumulate until the policy reaches states the expert never visited, where it may fail to choose an appropriate action.

**DAgger (Dataset Aggregation)**

DAgger is a representative method for mitigating distribution shift. It collects data with the learned policy while querying the expert for labels, then adds those labeled examples to the dataset.

```
1. Train policy π_1 on initial data D = {expert demonstrations}
2. for i = 1, 2, ...
     rollout with π_i → collect visited states {s_t}
     query the expert for actions {a_t^*} at {s_t}
     D = D ∪ {(s_t, a_t^*)}
     train π_{i+1} on D
```

Because querying the expert every time is expensive, human-in-the-loop variants or approximate versions of DAgger (HG-DAgger, ThriftyDAgger, etc.) are used.

The table summarizes typical tradeoffs. The actual sample count and safety profile depend on the algorithm, simulator, and quality of the expert data.

| Criterion | RL | IL |
|------|----|----|
| Sample efficiency | may require extensive environment interaction | depends on the number and diversity of expert demonstrations |
| Reward function | must be designed directly (reward engineering) | not needed |
| Safety | dangerous actions possible during exploration | imitates expert, so relatively safe |
| Sim-to-Real | reward function's sim-real gap is also a problem | using real demo data reduces the gap |

In robotics, designing a reward function properly is very hard. How do you define the reward for "grasp the cup"? Distance between the cup and the gripper? Then the robot may stop just next to the cup. Whether it was grasped? Then you hit the sparse-reward problem. IL sidesteps this.

> **Further reading**
> - [Ross et al., "A Reduction of Imitation Learning and Structured Prediction to No-Regret Online Learning" (2011)](https://arxiv.org/abs/1011.0686) — original DAgger paper
> - [Florence et al., "Implicit Behavioral Cloning" (CoRL 2021)](https://arxiv.org/abs/2109.00137) — an implicit approach to overcome BC's limitations
> - Zare et al., "A Survey of Imitation Learning: Algorithms, Recent Developments, and Challenges" (IEEE Trans. Cybernetics, 2024) — a survey of IL

## 12.8 Advanced: Diffusion Policy

Diffusion Policy, proposed by Chi et al. (RSS 2023), is used in robot manipulation as an alternative to BC-family methods. It generates an action trajectory through a denoising diffusion process.

Standard BC deterministically predicts a single action as `π_θ(s) → a`. But in reality, several actions are possible from the same state (multi-modality). For example, when grasping a cup on a table you can grab it from the left or from the right. Deterministic BC outputs the average of the two actions and fails at both. Gaussian mixture models are another option, but you must fix the number of modes in advance.

Diffusion Policy represents this multi-modal distribution naturally.

```
1. Start from random noise a_T ~ N(0, I) (T = diffusion steps)
2. Denoise iteratively conditioned on the current observation s:
   a_{t-1} = denoise_θ(a_t, s, t)    for t = T, T-1, ..., 1
3. The final a_0 is the action trajectory to execute
```

An action trajectory is not a single action but a sequence of actions over several future steps `[a(0), a(1), ..., a(H)]`. The number in parentheses is the execution time index, a different axis from the subscript (diffusion step) in the pseudocode above. Only the first few steps are executed (receding horizon), and a new trajectory is generated from the next observation.

This formulation represents a multi-modal action distribution without fixing the number of modes in advance, and it produces a temporally connected action sequence in one trajectory. Training uses denoising score matching.

Its main cost is speed. Inference requires repeated denoising steps (often 10–100), so it may be unsuitable for control above 100 Hz. DDIM-style sampling or consistency distillation can reduce the cost.

The original project page reports an average improvement of 46.9% over prior robot-learning methods across 12 tasks from four benchmarks. Interpret that number within the tasks, metrics, and baselines used in the paper.

> **Further reading**
> - [Chi et al., "Diffusion Policy: Visuomotor Policy Learning via Action Diffusion" (RSS 2023)](https://arxiv.org/abs/2303.04137) — original Diffusion Policy paper
> - [Diffusion Policy project page](https://diffusion-policy.cs.columbia.edu/) — code, demos, videos
> - [Ho et al., "Denoising Diffusion Probabilistic Models" (NeurIPS 2020)](https://arxiv.org/abs/2006.11239) — foundational diffusion-model paper

## 12.9 Advanced: Sim-to-Real Transfer

Section 12.6 introduced simulation platforms and domain randomization. The techniques below address the visual and physical differences encountered when a policy moves from simulation to a physical robot.

**1. Domain Randomization (DR)**

Randomly varies the parameters of the simulation environment each training step. The assumption is that if the model trains under a sufficiently wide variety of conditions, the real environment will be contained among those variations.

Randomization targets:
- **Visual**: textures, lighting direction/intensity, camera position/field of view, background
- **Physical**: friction coefficients, moments of inertia, link masses, joint damping
- **Dynamics**: actuator latency, sensor noise, control period

If the randomization range is too wide, the training problem becomes harder; if it is too narrow, the simulated variations may not cover the physical system. Choosing a suitable range is therefore important.

**2. System Identification (SysID)**

Measures or estimates the physical parameters of the real system and calibrates the simulator accordingly.

```
1. Execute a specific trajectory on the real robot to collect data
2. Optimize the simulator's parameters φ:
   φ* = argmin_φ || f_sim(φ) - f_real ||^2
3. Train the policy in the calibrated simulator
```

Traditional and effective, but estimating every parameter accurately is hard, and it is powerless against phenomena the simulator does not model (cable compliance, microscopic deformation of contact surfaces, etc.).

**3. Real-to-Sim-to-Real (R2S2R)**

This approach calibrates the simulator with real data, trains the policy inside that calibrated simulator, and brings it back to the real robot. If randomization is used alongside it, the pipeline must state explicitly which parameters are perturbed, over which ranges, and at which stage.

```
1. Collect a small amount of real data
2. Use the real data to calibrate the simulator (SysID) or model the discrepancy
3. Train a policy in the calibrated simulator
4. Apply the learned policy to the real robot
5. (Repeat) recalibrate the simulator with the real-world results
```

**4. Judging whether transfer succeeded**

The most direct quantitative check: compare the success rate of the same task in sim and real.

- **Sim success rate ≈ Real success rate**: treat transfer as successful only if both rates are high enough and the failure modes look similar. When both are low, closeness alone is no evidence of transfer, so read the absolute performance and the number of trials together.
- **Sim >> Real**: large reality gap. Expand DR range or correct with SysID.
- **Sim < Real**: rare but happens. The simulator was set to a harder (conservative) condition than reality.

Trajectory similarity, contact-force comparisons, and other metrics are sometimes used as additional indicators.

> **Further reading**
> - [Tobin et al., "Domain Randomization for Transferring Deep Neural Networks from Simulation to the Real World" (2017)](https://arxiv.org/abs/1703.06907) — original DR paper
> - Muratore et al., "Robot Learning from Randomized Simulations: A Review" (Frontiers in Robotics and AI, 2022) — a systematic treatment of DR
> - [Hanna & Stone, "Grounded Action Transformation for Robot Learning in Simulation" (AAAI 2017)](https://ojs.aaai.org/index.php/AAAI/article/view/11044) — transfer methodology
> - [NVIDIA Isaac Lab Tutorials](https://isaac-sim.github.io/IsaacLab/) — hands-on DR/SysID pipelines

> **Additional papers (3D/spatial understanding + benchmarks)**
> - [Hong et al., "3D-LLM: Injecting the 3D World into Large Language Models" (NeurIPS 2023, arXiv:2307.12981)](https://arxiv.org/abs/2307.12981) — gives LLMs 3D spatial understanding. 3D captioning, QA, navigation
> - [Chen et al., "SpatialVLM: Endowing Vision-Language Models with Spatial Reasoning" (CVPR 2024, arXiv:2401.12168)](https://arxiv.org/abs/2401.12168) — adds spatial reasoning about distance/size to VLMs
> - [Nasiriany et al., "RoboCasa: Large-Scale Simulation of Everyday Tasks for Generalist Robots" (RSS 2024, arXiv:2406.02523)](https://arxiv.org/abs/2406.02523) — 100 kitchen tasks, 150+ object categories. A household-robot benchmark
> - [Puig et al., "Habitat 3.0: A Co-Habitat for Humans, Avatars, and Robots" (ICLR 2024, arXiv:2310.13724)](https://arxiv.org/abs/2310.13724) — human-robot coexistence simulation. Social navigation, collaborative tasks

> **Technical Timeline: VLA & Embodied AI**
> - **~2015**: per-task imitation learning, research centered on single-object grasping
> - **2017–**: sim-to-real transfer via domain randomization takes off, research on MuJoCo/PyBullet
> - **2020–**: first attempts to combine large language models (LLMs) with vision. Language-based robot control such as CLIPort and SayCan emerges
> - **2022–**: foundation-model-based robot policies appear, including RT-1, RT-2, and PaLM-E. The Open X-Embodiment dataset is built
> - **2024–**: open-source VLA models such as OpenVLA and Octo are released. World-model-based planning, end-to-end autonomous driving (UniAD, VAD, GenAD), and modular or hybrid designs are all active research directions
> - **Recent direction**: foundation-model-based robot policies published since 2023 include RT-2, OpenVLA, Octo, and pi0. OpenVLA and Octo provide public code and weights for adaptation experiments.

---

# Ch.13 — 3D Vision

With only 2D images, a robot has trouble answering "how far is that object?" or "what is behind that wall?" 3D vision gives a robot the spatial information needed to answer such questions. Point-cloud processing, 3D object detection, and scene reconstruction all belong to this field, and the same concepts support both SLAM and robot manipulation.

## 13.1 Point Cloud Basics

The data coming out of a LiDAR or a depth camera is a point cloud. An image is 2D data aligned on a pixel grid, whereas a point cloud is a set of points irregularly scattered in 3D space. How to handle this unstructured data is the starting point of 3D vision.

A **point cloud** is a set of points in 3D space. Each point has at least (x, y, z) coordinates, and may additionally carry attributes such as color (RGB), reflectance (intensity), or a normal.

### 13.1.1 Data Structures and Formats

**Typical structure**:

```
Point: [x, y, z, r, g, b, intensity, ...]
Point Cloud: N × D matrix (N points, D-dimensional attributes)
```

In linear-algebra terms, a point cloud is just an N×D matrix. N ranges from tens of thousands to millions of points, and D is the attribute dimension per point. A transformation (rotation, translation) amounts to writing each point's position in homogeneous form and multiplying it by a 4×4 transformation matrix. Attributes such as color or reflectance are not touched by that transformation, and normals take the rotation only.

**Major file formats**:
| Format | Characteristics |
|---|---|
| **PCD** | PCL standard, ASCII/Binary |
| **PLY** | General-purpose, also supports meshes |
| **LAS/LAZ** | Geospatial standard, LAZ is compressed |
| **XYZ** | Simple text, coordinates only |
| **BIN** | Used by KITTI and others, binary |

> **Further reading**
> - [Open3D Documentation](http://www.open3d.org/docs/release/) — modern library for point cloud processing.
> - [PCL (Point Cloud Library) Tutorials](https://pcl.readthedocs.io/projects/tutorials/en/latest/) — classic library for point cloud processing.

### 13.1.2 Libraries

**PCL (Point Cloud Library)**:
- C++ based, the most comprehensive
- Integrated with ROS
- Filtering, segmentation, registration, and more

```cpp
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPCDFile<pcl::PointXYZ>("cloud.pcd", *cloud);
```

**Open3D**:
- Python/C++, modern API
- Strong on visualization
- Deep-learning friendly

```python
import open3d as o3d

# Load point cloud
pcd = o3d.io.read_point_cloud("cloud.pcd")

# Visualize
o3d.visualization.draw_geometries([pcd])

# Convert to NumPy
points = np.asarray(pcd.points)  # (N, 3)
```

In practice, Open3D is good for prototyping because you can use it directly from Python, while PCL is mostly used when building C++ ROS nodes. If you are just starting out, I recommend starting with Open3D.

> **Further reading**
> - [Open3D Getting Started](http://www.open3d.org/docs/release/getting_started.html) — introduction to handling point clouds in Python.
> - [PCL Tutorials — Basic Usage](https://pcl.readthedocs.io/projects/tutorials/en/latest/#basic-usage) — C++-based point cloud processing.
> - [Open3D YouTube Channel](https://www.youtube.com/@Open3D) — visualization and processing tutorials.

## 13.2 Point Cloud Processing

### 13.2.1 Filtering

Raw point clouds are noisy and have uneven point density. Using them as-is slows down downstream algorithms (registration, segmentation, and so on) or degrades their results. Filtering is the first stage of every point cloud pipeline.

**Voxel grid downsampling**:
Partition the space into a grid and collapse the points in each cell into one.

```python
# Open3D
voxel_pcd = pcd.voxel_down_sample(voxel_size=0.05)  # 5cm grid
```

**Statistical outlier removal**:
Remove outliers based on distance statistics to neighbors.

```python
# Outlier removal
cl, ind = pcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0)
filtered_pcd = pcd.select_by_index(ind)
```

**Radius outlier removal**:
Remove points that have too few neighbors within a given radius.

> **Further reading**
> - [Open3D — Point Cloud Filtering Tutorial](http://www.open3d.org/docs/release/tutorial/geometry/pointcloud.html) — examples of voxel downsampling and outlier removal.
> - [PCL — Filtering Tutorial](https://pcl.readthedocs.io/projects/tutorials/en/latest/passthrough.html) — PCL-based filtering.

### 13.2.2 Normal Estimation

Estimate the surface normal vector at each point. This is a preprocessing step for many algorithms.

Surface reconstruction, lighting computation, and point-to-plane ICP require normals; point-to-point ICP runs without them. Without normals, there is no way to tell "whether a point is part of a plane or part of an edge."

```python
# Normal estimation
pcd.estimate_normals(
    search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30)
)
```

Internally, for each point you gather its k nearest neighbors, compute the covariance matrix, and take the eigenvector corresponding to the smallest eigenvalue as the normal. This is exactly the same principle as PCA (Principal Component Analysis) from linear algebra class.

### 13.2.3 Registration

The process of aligning two point clouds. SLAM needs it to stitch consecutive frames together, or to merge scans captured from multiple viewpoints.

**ICP (Iterative Closest Point)**:
1. Find the closest point pairs
2. Compute the transformation (least squares)
3. Apply the transformation
4. Repeat until convergence

The intuition behind ICP: "find the closest point pairs between the two point clouds, then solve for the rotation + translation that makes those pairs overlap as much as possible. One pass is not perfect, so iterate." In linear-algebra terms, you use SVD (singular value decomposition) to solve for the optimal rotation matrix R and translation vector t.

**Point-to-Plane ICP**: minimizes point-to-plane distance (more accurate).

**GICP (Generalized ICP)**: accounts for point distributions.

**NDT (Normal Distributions Transform)**: partitions space into cells and matches the normal distribution of each cell.

**Feature-based registration**:
- Extract features such as FPFH or SHOT
- Initial registration via RANSAC
- Refine with ICP

> **Further reading**
> - [Open3D — ICP Registration Tutorial](http://www.open3d.org/docs/release/tutorial/pipelines/icp_registration.html) — hands-on ICP code.
> - [Open3D — Global Registration (RANSAC + Feature)](http://www.open3d.org/docs/release/tutorial/pipelines/global_registration.html) — feature-based registration.
> - [Cyrill Stachniss — ICP & Point Cloud Registration](https://www.youtube.com/watch?v=dhzLQfDBx2Q) — intuitive explanation of the ICP algorithm.

> **Exercise**: [Step-by-step 2D ICP visualization](https://alexjunholee.github.io/robotics-practice/app.html#icp_steps) | [3D ICP](https://alexjunholee.github.io/robotics-practice/app.html#icp_3d)
> Inspect ICP registering two point clouds iteration by iteration, and compare convergence in 2D and 3D environments.

## 13.3 3D Object Detection

Predict 3D bounding boxes from a point cloud. In autonomous driving, this is the core technology for knowing "where that car is and how big it is."

### 13.3.1 Point-based Methods

PointNet applies a neural network directly to raw points without converting the point cloud to voxels or images. Unlike a CNN, which assumes a regular grid, it accepts an irregular set of points as input.

**PointNet (2017)**:
- Applied directly to raw points
- Permutation invariant (independent of point order)
- Global feature via max pooling

**PointNet++ (2017)**:
- Hierarchical feature learning
- Set Abstraction: extracts features per region
- Can learn local patterns

```python
# PointNet++ conceptual structure
# 1. Sampling: pick centers via FPS
# 2. Grouping: gather neighbors via ball query
# 3. PointNet: extract features in each group
```

PointNet must produce the same result when the order of the points changes (permutation invariance). It passes each point independently through an MLP and aggregates the outputs with max pooling. Mathematically, it takes the form f({x1, ..., xn}) = g(MAX(h(x1), ..., h(xn))).

### 13.3.2 Voxel-based Methods

**VoxelNet (2018)**:
- Converts the point cloud into 3D voxels
- Voxel Feature Encoding
- Processed by a 3D CNN

**SECOND (Sparsely Embedded Convolutional Detection)**:
- Uses sparse convolution
- Much faster than VoxelNet
- A widely used baseline

**PointPillars (2019)**:
- Processes data in pillars (vertical columns)
- Converts to a 2D CNN for high speed
- Real-time capable

PointPillars divides 3D space into vertical pillars, compresses the points inside each pillar into a feature vector, and arranges the vectors like a 2D image. This representation can use a 2D CNN directly and is faster to process than a 3D CNN.

### 13.3.3 Multi-modal Methods

Combining multiple sensors lets each one cover the others' weaknesses. Cameras are rich in color and texture but lack depth, while LiDAR has accurate 3D information but no texture. How to fuse the two is the key question.

**BEVFusion**:
- Camera + LiDAR fusion
- Integrated in Bird's Eye View (BEV) space

**TransFusion**:
- Transformer-based fusion
- Query-based detection

> **Further reading**
> - [Qi et al., "PointNet: Deep Learning on Point Sets" (2017)](https://arxiv.org/abs/1612.00593) — the starting point of 3D deep learning.
> - [Lang et al., "PointPillars" (2019)](https://arxiv.org/abs/1812.05784) — real-time 3D detection.
> - [Liu et al., "BEVFusion" (2023)](https://arxiv.org/abs/2205.13542) — a widely used reference in multi-modal fusion.
> - [MMDetection3D GitHub](https://github.com/open-mmlab/mmdetection3d) — unified 3D object detection framework.

> **Exercise**: [BEV Projection Visualization](https://alexjunholee.github.io/robotics-practice/app.html#bev_projection)
> Interactively inspect the process of converting a camera image into BEV, and grasp the principle of BEV-based 3D detection.

## 13.4 3D Reconstruction

Generate a 3D model from multiple views or depth information. A robot needs this technology to "remember" the environment in 3D.

### 13.4.1 Structure from Motion (SfM)

You can recover 3D structure from just a handful of 2D photographs. A few smartphone photos can yield a 3D model of a building. It is also the preprocessing step that produces the input data (camera poses) for NeRF or 3D Gaussian Splatting (3DGS).

Recover camera poses and 3D structure simultaneously from multiple images.

**Pipeline**:
1. Feature extraction and matching
2. Triangulation from an initial two views
3. Incremental addition of cameras
4. Bundle adjustment (BA)

Bundle adjustment "jointly optimizes all camera poses and 3D point positions." It uses nonlinear least squares (Levenberg-Marquardt and the like), and the number of variables can reach tens of thousands to hundreds of thousands. Think of it as a large-scale nonlinear extension of the least squares you learned in linear algebra.

**Tools**:
- **COLMAP**: a widely used public SfM/MVS reference implementation, GUI/CLI
- **OpenMVG**: library-style

```bash
# Using COLMAP
colmap feature_extractor --database_path db.db --image_path ./images
colmap exhaustive_matcher --database_path db.db
mkdir -p sparse
colmap mapper --database_path db.db --image_path ./images --output_path ./sparse
```

> **Further reading**
> - [COLMAP Documentation](https://colmap.github.io/) — documentation for a widely used public SfM/MVS implementation.
> - [Daniel Cremers — Multiple View Geometry (TUM)](https://www.youtube.com/playlist?list=PLTBdjV_4f-EJn6udZ34tht9EVIW7lbeo4) — core lectures on multiple view geometry.
> - [Schönberger & Frahm, "Structure-from-Motion Revisited" (2016)](https://openaccess.thecvf.com/content_cvpr_2016/papers/Schonberger_Structure-From-Motion_Revisited_CVPR_2016_paper.pdf) — the COLMAP paper.

### 13.4.2 Multi-View Stereo (MVS)

Generate a dense point cloud from the SfM result.

Where SfM recovers "where the cameras were" and "sparse 3D points," MVS uses those camera poses to build a **dense** 3D point cloud. SfM → MVS → mesh generation is the canonical 3D reconstruction pipeline.

**Tools**: COLMAP (dense reconstruction), OpenMVS

### 13.4.3 Volumetric Reconstruction

**TSDF (Truncated Signed Distance Function)**:
- Partition space into voxels and store the distance to the surface in each voxel
- Integrate multiple views
- Extract a mesh via Marching Cubes

A TSDF stores the signed distance to the nearest surface in each voxel. Positive values indicate the outside of the surface and negative values the inside. A weighted average of depths observed from multiple views reduces noise, and the zero crossing identifies the surface.

mesh = volume.extract_triangle_mesh()
```

> **Further reading**
> - [Open3D — TSDF Integration Tutorial](http://www.open3d.org/docs/release/tutorial/pipelines/rgbd_integration.html) — hands-on TSDF code.
> - [Curless & Levoy, "A Volumetric Method for Building Complex Models from Range Images" (1996)](https://graphics.stanford.edu/papers/volrange/volrange.pdf) — the original TSDF paper (a classic, but still worth reading).

## 13.5 Neural Rendering

A new deep-learning-based approach to 3D representation and rendering. Prior methods (mesh, point cloud) had limits in representing complex scenes (reflections, transparent objects, thin structures). Neural rendering represents a scene as a learnable function, handling such effects naturally. Recently it has also been combined with SLAM for online mapping.

### 13.5.1 NeRF (Neural Radiance Fields)

**Concept**: represent a 3D scene as a continuous function.

```
F: (x, y, z, θ, φ) → (r, g, b, σ)
- Position (x, y, z) and viewing direction (θ, φ)
- Outputs color (r, g, b) and density (σ)
```

Intuitively: NeRF learns, via a neural network, "for every point in 3D space, what color and density it has when viewed from a given direction." Once training is done, you can synthesize novel views from arbitrary camera positions (novel view synthesis).

**Rendering**: integrate color and density along a ray (volume rendering).

**Pros**:
- Photorealistic novel view synthesis
- Handles complex effects such as reflection and transparency

**Cons**:
- Training takes a long time
- Dynamic scenes are hard

**Extensions**:
- Instant-NGP: fast training via hash encoding (minutes)
- Mip-NeRF: anti-aliasing
- Block-NeRF: large-scale scenes

> **Further reading**
> - [Mildenhall et al., "NeRF: Representing Scenes as Neural Radiance Fields" (2020)](https://arxiv.org/abs/2003.08934) — the original NeRF paper.
> - [NeRFStudio Documentation](https://docs.nerf.studio/) — a unified framework that makes NeRF experiments easy. Start here if you want to run NeRF yourself.
> - [Yannic Kilcher — NeRF Explained](https://www.youtube.com/watch?v=CRlN-cYFxTk) — an intuitive explanation of NeRF's representation.
> - [Jon Barron — Understanding NeRF (ECCV 2022 Tutorial)](https://www.youtube.com/watch?v=HfJpQCBTqZs) — from a NeRF author.

### 13.5.2 3D Gaussian Splatting (3DGS)

3DGS was adopted quickly because it substantially reduced the rendering time associated with NeRF. NeRF can take seconds to render a frame, whereas reported 3DGS implementations render at more than 100 FPS. This difference makes robotics applications such as SLAM and online mapping practical.

**Concept**: represent a scene with millions of 3D Gaussians.

Each Gaussian has:
- Position (mean)
- Covariance (shape/size/orientation)
- Color (spherical harmonics)
- Opacity

Recall the covariance matrix from linear algebra. The eigenvectors of a 3×3 covariance matrix determine an ellipsoid's axis directions, and the eigenvalues determine the axis lengths. 3DGS uses this idea directly to represent each Gaussian's shape and size.

**Rendering**: project Gaussians onto the image (splatting).

**Pros**:
- **Real-time rendering** (100+ FPS) compared with NeRF
- Fast training (minutes)
- Explicit representation (easy to edit)

**Applications**:
- SLAM: SplaTAM, Gaussian Splatting SLAM
- Mapping: large-scale environment representation
- Dynamic scenes: extensions to dynamic scenes

```python
# 3DGS basic idea (pseudo-code)
# Each Gaussian: position, covariance, color, opacity
# Rendering: project to camera view to generate the image
```

**3DGS + SLAM (current trend)**:

As 3D Gaussian Splatting merges with SLAM, a new direction for neural SLAM is emerging. Whereas traditional SLAM built sparse point maps or voxel maps, 3DGS-SLAM builds photorealistic 3D maps. The 100+ FPS figures quoted as real time are the rendering throughput of an already-trained scene; end-to-end throughput including tracking and mapping optimization is reported to be far lower.

- **SplaTAM (2024)**: runs 3DGS-based dense SLAM from RGB-D camera input. It alternates between tracking (camera pose estimation) and mapping (adding/updating Gaussians), and greatly improves both rendering quality and speed compared with prior neural SLAM.
- **MonoGS (2024)**: runs 3DGS-based SLAM using only a monocular camera. It is drawing attention because it can build a dense 3D map without a depth sensor.
- **Gaussian-SLAM (2024)**: runs 3DGS SLAM at large scale via a sub-map approach.

If a robot can build photorealistic 3D maps in real time while moving around, applications such as AR/VR content creation, digital twins, and building inspection open up.

> **Further reading**
> - [Kerbl et al., "3D Gaussian Splatting for Real-Time Radiance Field Rendering" (2023)](https://arxiv.org/abs/2308.04079) — the original 3DGS paper.
> - [Huang et al., "2D Gaussian Splatting for Geometrically Accurate Radiance Fields" (SIGGRAPH 2024, arXiv:2403.17888)](https://arxiv.org/abs/2403.17888) — improves surface reconstruction quality via 2D Gaussians.
> - [Keetha et al., "SplaTAM: Splat, Track & Map 3D Gaussians for Dense RGB-D SLAM" (2024)](https://arxiv.org/abs/2312.02126) — an early 3DGS + SLAM system.
> - [Matsuki et al., "Gaussian Splatting SLAM" (2024)](https://arxiv.org/abs/2312.06741) — the MonoGS paper.
> - [Wang et al., "DUSt3R: Geometric 3D Vision Made Easy" (CVPR 2024, arXiv:2312.14132)](https://arxiv.org/abs/2312.14132) — reconstructs dense 3D from image pairs without camera intrinsics or extrinsics.
> - [Leroy et al., "Grounding Image Matching in 3D with MASt3R" (ECCV 2024, arXiv:2406.09756)](https://arxiv.org/abs/2406.09756) — adds local feature matching to DUSt3R. Provides reconstruction and precise correspondences simultaneously.
> - [NeRFStudio Documentation](https://docs.nerf.studio/) — unified framework for NeRF/3DGS experiments.
> - [3DGS Original Implementation (GitHub)](https://github.com/graphdeco-inria/gaussian-splatting) — the official code.

> **Exercise**: [3D Gaussian Splatting Visualization](https://alexjunholee.github.io/robotics-practice/app.html#gaussian_splatting)
> Manipulate the position, covariance, and color of 3D Gaussians to interactively understand the splatting rendering process.

## 13.6 Advanced: Neural Implicit Representations

Section 13.5 covered NeRF and 3DGS. NeRF uses a density field to perform volume rendering, but extracting a clear surface from the density is hard. For robotics — grasping objects or checking collisions — an accurate surface is required. This is where signed-distance-function (SDF) based approaches come in.

**SDF (Signed Distance Function)**

A function that returns, at each point `x` in space, the signed distance to the nearest surface.

```
f(x) > 0  : outside the surface
f(x) < 0  : inside the surface
f(x) = 0  : on the surface (zero level set)
```

The key property of an SDF: its gradient has unit magnitude everywhere (Eikonal equation).

```
||∇f(x)|| = 1
```

Only functions satisfying this condition are proper distance functions. When learning an SDF with a neural network, this condition is added as a regularization term, called the **Eikonal loss**.

```
L_eikonal = E_x[ (||∇f_θ(x)|| - 1)^2 ]
```

**DeepSDF**

An early work on learning SDFs with neural networks. It uses a decoder-only architecture and represents each object's shape with a latent code `z`.

```
f_θ(z, x) → SDF value
```

For a new object, `z` is estimated via test-time optimization.

**NeuS**

Combines the rendering quality of NeRF's volume rendering with the clean surfaces of SDFs. It introduces a function that converts SDF values into densities, so SDFs can be learned within the volume rendering framework.

```
density ρ(t) = max(-dΦ_s(f(r(t)))/dt, 0) / Φ_s(f(r(t))),   r(t) = o + t·d is a point along the ray
```

Here `Φ_s` is a sigmoid controlled by a learnable parameter `s`. The S-density defined as its derivative has standard deviation `1/s`, so as training converges `s` grows and the density narrows toward the surface.

**VolSDF**

A similar approach, but defines density as the CDF of a Laplace distribution of the SDF.

```
σ(x) = (1/β) · Ψ_β(-f(x))
```

As `β` decreases, density concentrates on the surface.

**Surface extraction**

The standard method for converting the iso-surface `f(x) = 0` of a learned SDF into a mesh is the **Marching Cubes** algorithm. It partitions space into a grid, checks the SDF sign at each grid vertex, and interpolates to determine where the surface crosses.

**Comparison table**

| Representation | Pros | Cons | Examples |
|------|------|------|------|
| NeRF (density) | High rendering quality | Surface extraction is hard | Instant-NGP |
| SDF (neural) | Clean surfaces | Hard to train | NeuS, VolSDF |
| 3DGS (explicit) | Real-time rendering | High memory usage | Gaussian Splatting |
| Occupancy | Simple via binary classification | Limited surface detail | ConvONet |

> **Further reading**
> - [Wang et al., "NeuS: Learning Neural Implicit Surfaces by Volume Rendering" (NeurIPS 2021)](https://arxiv.org/abs/2106.10689) — the original NeuS paper.
> - [Yariv et al., "Volume Rendering of Neural Implicit Surfaces" (NeurIPS 2021)](https://arxiv.org/abs/2106.12052) — the VolSDF paper.
> - [Park et al., "DeepSDF: Learning Continuous Signed Distance Functions for Shape Representation" (CVPR 2019)](https://arxiv.org/abs/1901.05103) — the original DeepSDF paper.
> - [Mescheder et al., "Occupancy Networks" (CVPR 2019)](https://arxiv.org/abs/1812.03828) — an early reference for occupancy-based approaches.

## 13.7 Advanced: Differentiable Rendering

NeRF, 3DGS, NeuS, and other recent core 3D vision techniques share one principle: **make the rendering process differentiable, so the difference between the rendered result and the real image optimizes the 3D representation**. This paradigm is called analysis-by-synthesis.

**Volume rendering equation**

The basic rendering formula used by the NeRF family. It integrates color along a ray `r(t) = o + td` cast from the camera.

```
C(r) = ∫ T(t) · σ(t) · c(t) dt

where T(t) = exp( -∫_{t_n}^{t} σ(s) ds )
```

- `σ(t)`: density at location `t` (volume density, in units of inverse length)
- `c(t)`: color (RGB) at location `t`
- `T(t)`: accumulated transmittance (the probability that the ray reaches `t`)

In practice, this continuous integral is discretized and approximated at N samples along the ray (ray marching).

```
C(r) ≈ Σ_i T_i · α_i · c_i
where α_i = 1 - exp(-σ_i · δ_i),  T_i = Π_{j<i} (1 - α_j)
```

**Rasterization-based rendering in 3DGS**

Instead of the ray marching above, 3DGS uses a rasterization approach that projects each Gaussian onto the image plane (splatting). Each pixel's color is the weighted sum of the Gaussians that influence that pixel.

```
C(p) = Σ_i c_i · α_i · Π_{j<i} (1 - α_j)
```

The equation is structurally similar to volume rendering, but instead of sampling along a ray, Gaussians are sorted by depth and composited in order. This compatibility with the GPU rasterization pipeline is why real-time rendering works.

**Differentiable rasterization libraries**

Tools for rendering mesh-based 3D representations in a differentiable way.

- **PyTorch3D** (Meta): provides a differentiable mesh renderer and point cloud renderer.
- **nvdiffrast** (NVIDIA): high-performance CUDA-based differentiable rasterizer.
- **Kaolin** (NVIDIA): a library for 3D deep learning in general.

**Analysis-by-synthesis pipeline**

```
1. Set up a hypothesized 3D scene (NeRF, 3DGS, mesh, etc.)
2. Render that scene from the camera viewpoint → predicted image
3. Compare with the actually observed image → compute loss
4. Backpropagate the loss to update 3D scene parameters
5. Iterate
```

The advantage of this paradigm is that, regardless of the 3D representation form, you can use the common supervision of "images." No separate 3D ground truth is needed.

**Connection to SLAM**

When differentiable rendering is applied to SLAM, both tracking and mapping can be optimized through rendering loss.

- **NeRF-SLAM**: uses NeRF as the map representation. When a new frame comes in, it renders the current map to estimate the pose (tracking), then updates the map (mapping).
- **3DGS-SLAM** (SplaTAM, MonoGS, and so on): uses 3DGS as the map. Real-time rendering makes it faster than NeRF-SLAM.

```
[new frame] → [render current map] → [compare render vs. real]
                                      ↓
                              [update pose (tracking)]
                              [update map (mapping)]
```

> **Further reading**
> - [Tewari et al., "Advances in Neural Rendering" (EUROGRAPHICS 2022 STAR)](https://arxiv.org/abs/2111.05849) — survey of differentiable rendering.
> - [Ravi et al., "Accelerating 3D Deep Learning with PyTorch3D" (2020)](https://arxiv.org/abs/2007.08501) — the PyTorch3D paper.
> - [Laine et al., "Modular Primitives for High-Performance Differentiable Rendering" (2020)](https://arxiv.org/abs/2011.03277) — the nvdiffrast paper.

## 13.8 Advanced: 3D Scene Graph

If you tell a robot "bring me the red cup in the kitchen," a point cloud or mesh alone cannot carry out the command. The robot has to understand where "the kitchen" is, which object is "the red cup," and the relation that it is "inside" the kitchen. A 3D scene graph represents the environment as a semantic relation graph beyond a purely geometric representation.

**Structure**

- **Node**: objects, rooms, buildings, etc. — a hierarchical structure
  - building → floor → room → object
  - each node carries a 3D position, bounding box, and semantic label
- **Edge**: relations between nodes
  - "on", "in", "near", "support", and so on

**Hydra**

A real-time 3D scene graph construction system developed at MIT. It takes RGB-D or LiDAR input and incrementally builds a hierarchical scene graph as the robot moves.

Pipeline:
1. Build a metric-semantic mesh (TSDF + semantic segmentation)
2. Partition into rooms (free-space clustering)
3. Extract object nodes and establish relations
4. Connect the hierarchy

The core of Hydra is that this entire process runs online in real time. The scene graph is updated while the robot explores.

**ConceptGraphs**

Research that builds open-vocabulary scene graphs using foundation models (CLIP, LLM).

Prior scene graphs relied on a predefined set of categories (chair, table, and so on). ConceptGraphs uses CLIP to find objects matching arbitrary natural-language queries, and uses an LLM to infer relations between objects.

```
1. Detect objects in RGB-D frames with an open-vocabulary detector
2. Extract object embeddings with CLIP features
3. Merge identical objects in 3D space (multi-view association)
4. Infer inter-object relations with an LLM
5. Build the scene graph
```

The resulting graph can handle queries such as "red cup" even when that phrase did not appear during training.

**Why is this needed?**

| Representation | Can execute "bring me the red cup in the kitchen"? | Reason |
|------|------|------|
| Point Cloud | No | No semantic information |
| Semantic Map | Partial | Can find "cup" but struggles with the relation "in the kitchen" |
| 3D Scene Graph | Yes | Expresses objects, relations, and hierarchy together |

In task planning, natural-language-driven navigation, and human-robot interaction, a scene graph bridges 3D representations and high-level reasoning.

> **Further reading**
> - [Hughes et al., "Hydra: A Real-time Spatial Perception System for 3D Scene Graph Construction and Optimization" (RSS 2022)](https://arxiv.org/abs/2201.13360) — the original Hydra paper.
> - [Gu et al., "ConceptGraphs: Open-Vocabulary 3D Scene Graphs for Perception and Planning" (2023)](https://arxiv.org/abs/2309.16650) — the ConceptGraphs paper.
> - [Rosinol et al., "3D Dynamic Scene Graphs: Actionable Spatial Perception with Places, Objects, and Humans" (RSS 2020)](https://arxiv.org/abs/2002.06289) — introduces the Dynamic Scene Graph concept.
> - [Armeni et al., "3D Scene Graph: A Structure for Unified Semantics, 3D Space, and Camera" (ICCV 2019)](https://arxiv.org/abs/1910.02527) — early work on 3D scene graphs.

> **Technical Timeline: 3D Vision**
> - **~2010**: the classical era of point cloud processing. The PCL library, ICP registration, and TSDF-based volumetric reconstruction dominate.
> - **2010~**: the release of the Kinect (2010) and KinectFusion (2011, TSDF + ICP) opened the door to real-time RGB-D 3D reconstruction and drove its popularization.
> - **2017~**: point cloud deep learning begins with PointNet/PointNet++. 3D object detection research such as VoxelNet and PointPillars also surges in this period.
> - **2020~**: the arrival of NeRF brings neural rendering into the spotlight. A handful of photos can yield a photorealistic 3D scene, and follow-ups such as Instant-NGP and Mip-NeRF arrive in quick succession.
> - **2023~**: 3D Gaussian Splatting overcomes NeRF's speed limits. It combines real-time rendering with the advantages of explicit representation, and multi-modal 3D detection such as BEVFusion becomes the reference in autonomous driving.
> - **2024~**: the combination of 3DGS + SLAM (SplaTAM, MonoGS, Gaussian-SLAM) is opening up a new direction for neural SLAM. Robots build photorealistic 3D maps as they move; rendering is real time, but end-to-end throughput including tracking and mapping does not yet reach that level.
> - **Recent direction**: more SLAM and robotics systems are using 3DGS-based scene representations. NeRFStudio allows NeRF and 3DGS to be compared on the same data, including their representations and rendering speed.

---

# Ch.14 — SLAM & Odometry

Simultaneous localization and mapping (SLAM) estimates a robot's pose while building a map of an unfamiliar environment. It provides the basis for autonomous motion in GPS-denied spaces such as indoor and underground environments.

---

## Part 1. Foundations and Systems

### 14.1 Concept Introduction

Navigation and path planning require the robot's current pose and information about its surroundings. SLAM estimates both.

**SLAM (simultaneous localization and mapping)** is the problem of estimating one's own pose while at the same time building a map of the surrounding environment.

Chicken-and-egg problem:
- You need a map to know your position
- You need your position to build a map
→ Solve both at once

Sensors always carry noise. Wheels slip, camera images shake. This uncertainty accumulates over time and the pose estimate gradually drifts (drift). The core challenge of SLAM is to correct this drift and produce a consistent map.

**Odometry vs SLAM**:
| Feature | Odometry | SLAM |
|---|---|---|
| Output | Relative motion | Pose + map |
| Loop closure | None | Present |
| Drift | Accumulates | Can be corrected |
| Compute | Light | Heavy |

> **Further reading**
> - [Cyrill Stachniss — SLAM Course (University of Bonn)](https://www.youtube.com/playlist?list=PLgnQpQtFTOGQrZ4O5QzbIHgl3b1JHimN_) — a public lecture series running from Bayes filters to graph-based SLAM.
> - [Thrun, Burgard, Fox, "Probabilistic Robotics" (Textbook)](https://mitpress.mit.edu/9780262201629/probabilistic-robotics/) — Textbook covering the mathematical foundations of SLAM. Kalman filter, particle filter, EKF-SLAM, and more.
> - [Barfoot, "State Estimation for Robotics" (Free PDF)](http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17.pdf) — Textbook with deep coverage of the mathematics of state estimation. Free PDF available.
> - [Awesome-SLAM GitHub](https://github.com/SilenceOverflow/Awesome-SLAM) — Curated list of SLAM-related papers, libraries, and datasets.
> - [Jinyong Jeong's blog — SLAM lecture series (based on Freiburg Robot Mapping)](https://jinyongjeong.github.io/2017/02/13/lec01_SLAM_bayes_filter/) — a 15-part Korean-language series covering Bayes filters, EKF/UKF/particle filters, Graph SLAM, and Robust SLAM.
> - [Giseop Kim's blog — 5 recommended study materials for SLAM back-end](https://gisbi-kim.github.io/blog/2021/10/03/slam-textbooks.html) — A curated list of core materials including Error-state KF, Factor Graphs, and Bundle Adjustment.
> - [Robot Mapping Course (Uni Freiburg, Cyrill Stachniss)](http://ais.informatik.uni-freiburg.de/teaching/ws13/mapping/) — Lecture slides and assignments for the SLAM course. Pairs well with the video lectures.
> - [EKF-SLAM slides (Freiburg)](http://ais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/slam04-ekf-slam.pdf) — The EKF-SLAM portion of the course above. The derivations are cleanly organized.

> **Practice**: [SE(2) Odometry](https://alexjunholee.github.io/robotics-practice/app.html#se2_odometry)
> Interactively drive the odometry accumulation process on a 2D plane and observe how drift arises.

### 14.2 Visual Odometry (VO)

Estimate relative motion from the camera alone. This corresponds to the SLAM "front-end"; if the motion estimated here is inaccurate, the entire SLAM system falls apart.

#### 14.2.1 Feature-based vs Direct Method

These two approaches have sharply different strengths and weaknesses. The choice depends on the environment in which you operate the robot.

**Feature-based** methods (ORB-SLAM family) extract invariant distinctive points (corners, blobs, and so on) from images and infer camera motion by matching them between frames. In linear-algebra terms the problem splits into two stages. During initialization, when no map exists yet, 2D-2D correspondences are used to solve for the essential or fundamental matrix (a homography for planar scenes); once a map has been built, tracking instead minimizes the reprojection error between 3D map points and their 2D observations. They are robust to illumination changes and the methodology is well-established, but they have limits in environments where keypoints are hard to extract, such as white walls or textureless floors.

```
Image → Feature extraction → Matching → Motion estimation
```

**Direct methods** (DSO, LSD-SLAM family) compare pixel intensities directly. They exploit the assumption that "if the same 3D point is observed in consecutive frames, the intensity must be the same" (brightness constancy), so they do not need to extract keypoints and can operate even in low-texture environments. In return, they are sensitive to illumination changes.

```
Image → Direct pixel intensity comparison → Motion estimation
```

#### 14.2.2 Mono vs Stereo vs RGB-D

You have to understand each configuration's trade-offs to choose a sensor that fits your robot.

| Configuration | Scale | Characteristics | Suitable environment |
|---|---|---|---|
| **Monocular** | Unavailable (ambiguity) | Light and simple; scale cannot be recovered without an IMU | Low-cost drones, mobile |
| **Stereo** | Available | Baseline limits the measurement range | General indoor/outdoor |
| **RGB-D** | Available | Measures depth directly; weak outdoors and under direct sunlight | Indoor structured environments |

To elaborate on scale ambiguity: with a single camera you cannot tell "a small object up close" from "a large object far away." A monocular SLAM map comes out at an arbitrary scale, and an IMU or another sensor must recover it. The reason "sufficient motion is required during initialization" is worth splitting in two. What monocular structure initialization needs is translation that generates parallax; under pure rotation the two-view geometry degenerates. Motion excitation for the sake of observability of scale, IMU bias, and the gravity direction belongs to visual-inertial initialization. In the purely monocular case, more motion never makes absolute scale observable.

> **Further reading**
> - [Daniel Cremers — Multiple View Geometry (TUM)](https://www.youtube.com/playlist?list=PLTBdjV_4f-EJn6udZ34tht9EVIW7lbeo4) — public lectures on the multiple-view geometry used by Visual Odometry.
> - [EuRoC MAV Dataset](https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets) — Benchmark dataset for Visual(-Inertial) Odometry.
> - [TUM RGB-D Benchmark](https://cvg.cit.tum.de/data/datasets/rgbd-dataset) — a widely used indoor dataset and evaluation toolkit for RGB-D SLAM/VO.

### 14.3 Visual SLAM

#### 14.3.1 ORB-SLAM2/3

The ORB-SLAM family is one of the public baselines frequently used in visual-SLAM papers. Its open implementation can be built to inspect assumptions and failure cases directly.

**Structure**:
1. **Tracking**: Pose estimation on the current frame
2. **Local Mapping**: Keyframe-based local map management
3. **Loop Closing**: Loop detection and global optimization

This three-thread structure is the core design of ORB-SLAM. Tracking runs in real-time on every frame, Local Mapping runs when a keyframe arrives, and Loop Closing runs when a loop is detected. Each runs in parallel at a different rate, allowing real-time performance while preserving global consistency.

**ORB-SLAM3 features**:
- Visual-inertial mode supported
- Multi-map supported
- Fish-eye cameras supported

Historical context of ORB-SLAM:
- **MonoSLAM (2007)**: an early representative system for real-time monocular SLAM. It ran on an EKF, but suffered from compute growth as the map grew.
- **PTAM (Parallel Tracking and Mapping, 2007)**: an influential early system that split tracking and mapping into parallel threads. This architecture strongly influenced later ORB-SLAM.
- **ORB-SLAM (2015)**: A complete SLAM system that inherited PTAM's design and added ORB keypoints, loop closure, and relocalization.
- **ORB-SLAM2 (2017)**: Added stereo and RGB-D support.
- **ORB-SLAM3 (2021)**: Added visual-inertial, multi-map, and more.

```bash
# ORB-SLAM3 run example
./Examples/Monocular/mono_euroc \
    Vocabulary/ORBvoc.txt \
    Examples/Monocular/EuRoC.yaml \
    ~/Datasets/EuRoC/MH01
```

> **Further reading**
> - [Campos et al., "ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM" (2021)](https://arxiv.org/abs/2007.11898) — The ORB-SLAM3 paper.
> - [ORB-SLAM3 GitHub](https://github.com/UZ-SLAMLab/ORB_SLAM3) — Official code.
> - [EuRoC MAV Dataset](https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets) — Standard test dataset for ORB-SLAM3.
> - [Jinyong Jeong's blog — Visual SLAM comparison experiment (KAIST Urban Dataset)](https://jinyongjeong.github.io/2019/10/22/visual_slam_compare/) — Head-to-head ORB-SLAM2 vs VINS-Fusion on real data. Analyzes performance differences on actual datasets.

#### 14.3.2 DSO (Direct Sparse Odometry)

**Direct Method** + **Sparse Points**

Direct methods are often used densely (every pixel), and sparse representations are typical in feature-based methods, but DSO takes the combination of "direct and sparse." It minimizes photometric error using only a selected set of high-quality points.

- Uses pixel intensities directly, no keypoint extraction
- Uses only selected points (sparse)
- Photometric bundle adjustment

> **Further reading**
> - [Engel et al., "Direct Sparse Odometry" (2018)](https://arxiv.org/abs/1607.02565) — The DSO paper.

#### 14.3.3 VINS-Mono/Fusion

Monocular tracking can fail under fast motion or in texture-poor environments. An IMU supplements the image stream with high-rate motion constraints. VINS-Mono combines the two in a visual-inertial SLAM system for drones and mobile robots.

**Visual-Inertial Navigation System**

- Camera + IMU tight coupling
- Sliding window optimization
- Loop closure supported
- Widely used on mobile robots and drones

```
Sensor input → IMU Preintegration →
Visual Feature Tracking →
Sliding Window Optimization →
Loop Closure (optional)
```

VINS-Mono uses IMU preintegration to combine the measurements between two keyframes into one relative-motion constraint. Optimization uses this constraint instead of reintegrating every raw measurement.

> **Further reading**
> - [Qin et al., "VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator" (2018)](https://arxiv.org/abs/1708.03852) — The VINS-Mono paper.
> - [VINS-Mono GitHub](https://github.com/HKUST-Aerial-Robotics/VINS-Mono) — Official code, with ROS support.

### 14.4 LiDAR Odometry & SLAM

Camera-based methods depend on illumination and texture. LiDAR measures 3D range directly and therefore does not depend on image brightness or texture in the same way. This difference motivates the use of LiDAR SLAM in autonomous driving and outdoor robots.

#### 14.4.1 LOAM (Lidar Odometry and Mapping)

LOAM's separation of edge and planar features, together with its dual-rate odometry and mapping structure, influenced later systems such as LeGO-LOAM and LIO-SAM.

- Classify edge points and planar points
- Minimize point-to-edge and point-to-plane distances
- Separate odometry and mapping (at different rates)

It extracts points associated with edges and planes, then builds constraints from those selected features instead of matching every point. This reduces the amount of computation.

#### 14.4.2 LeGO-LOAM

**Lightweight and Ground-Optimized LOAM**:
- Ground separation reduces compute
- Uses the ground plane for an initial estimate
- Suited to mobile robots

#### 14.4.3 LIO-SAM

A representative work that applies factor graph-based optimization to LiDAR-inertial SLAM. The core strength of a factor graph is extensibility. To add one more sensor, you just add one factor.

**LiDAR-Inertial Odometry via Smoothing and Mapping**:
- Factor graph based
- Tight IMU-LiDAR coupling
- Integrates GPS and loop closure

What a factor graph is: a graph that represents the relationships between variables (robot poses, landmark positions) and constraints (sensor measurements). An IMU measurement is one factor, a LiDAR match is one factor, GPS is one factor, a loop closure is one factor... To add a sensor, you simply add its factor. The GTSAM library carries out this optimization efficiently.

> **Further reading**
> - [Shan et al., "LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping" (2020)](https://arxiv.org/abs/2007.00258) — The LIO-SAM paper.
> - [Vizzo et al., "KISS-ICP: In Defense of Point-to-Point ICP" (RA-L 2023, arXiv:2209.15397)](https://arxiv.org/abs/2209.15397) — A well-built vanilla ICP matches complex LiDAR odometry in performance. The power of simplicity.
> - [LIO-SAM GitHub](https://github.com/TixiaoShan/LIO-SAM) — Official code, with ROS support.
> - [GTSAM Documentation](https://gtsam.org/) — Factor graph optimization library. Used as the back-end of many SLAM systems including LIO-SAM.
> - [Frank Dellaert — Factor Graphs for Perception and Action (MIT Robotics)](https://www.youtube.com/watch?v=-yCC7mpgL4w) — The GTSAM developer explaining factor graphs himself.
> - [Giseop Kim's blog — Scan Context-based LiDAR Pose-graph SLAM implementation](https://gisbi-kim.github.io/blog/2021/05/17/sclidarslam.html) — A walk-through of integrating Scan Context into LiDAR SLAM.

#### 14.4.4 FAST-LIO / FAST-LIO2

**Fast LiDAR-Inertial Odometry**:
- Iterated Kalman filter based (an iterated update equivalent to Gauss-Newton)
- ikd-Tree: dynamic KD-tree for fast mapping
- Real-time performance

Why FAST-LIO is fast: LIO-SAM uses factor graph optimization (nonlinear least squares), whereas FAST-LIO uses an iterated extended Kalman filter (IEKF). The iterated update of an IEKF is equivalent to Gauss-Newton on a MAP objective, so it is not the case that no optimization is being solved. The actual source of the speed is a formulation that makes the Kalman gain computation depend on the state dimension rather than the measurement dimension, together with a structure that does not accumulate states in a sliding window. The follow-up FAST-LIO2 adds an incremental KD-tree, ikd-Tree, on top of this, which also cuts the cost of inserting new points into the map.

> **Further reading**
> - [Xu & Zhang, "FAST-LIO: A Fast, Robust LiDAR-Inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter" (2021)](https://arxiv.org/abs/2010.08196) — The FAST-LIO paper.
> - [Xu et al., "FAST-LIO2: Fast Direct LiDAR-Inertial Odometry" (2022)](https://arxiv.org/abs/2107.06829) — The FAST-LIO2 paper.
> - [FAST-LIO2 GitHub](https://github.com/hku-mars/FAST_LIO) — Official code.

### 14.5 Multi-sensor Fusion

A single sensor struggles to cover every situation. A camera does not like the dark, a LiDAR has a hard time in the rain, and an IMU alone drifts heavily. Combining sensors (fusion) lets each sensor compensate for the others' weaknesses.

#### 14.5.1 Camera + IMU (VIO)

There are two strategies for combining visual and inertial. **Loosely-coupled** has the camera and the IMU each estimate state separately and then fuses the results based on covariance. Implementation is simple, but it fails to exploit the information fully. **Tightly-coupled** names a coupling level in which the reprojection error of camera keypoints and the IMU measurements constrain one shared state directly. How that state is actually solved is a separate matter: VINS-Mono optimizes a single cost function, whereas MSCKF is a filter — a sliding-window EKF with feature null-space marginalization. The tightly coupled approach is more accurate but more complex to implement.

**IMU Preintegration**:
Pre-integrate IMU measurements between two keyframes to compute a relative transformation. The raw measurements then do not have to be reintegrated at every optimization iteration.

#### 14.5.2 LiDAR + IMU (LIO)

LiDAR and IMU rates vary by product, but platform motion within one LiDAR scan causes motion distortion. LIO uses higher-rate inertial measurements and timestamps to de-skew the scan and estimate state jointly with LiDAR observations. The benefit depends on synchronization, IMU bias, motion, and scan pattern.

#### 14.5.3 Camera + LiDAR + IMU

**Recent trend**: integrate all sensors
- Examples: R3LIVE, LVI-SAM
- Exploits each sensor's strengths

R3LIVE combines LiDAR geometry, camera texture and color, and IMU motion measurements. It estimates the pose while building a dense, colored 3D map in real time.

> **Further reading**
> - [KITTI Odometry Benchmark](https://www.cvlibs.net/datasets/kitti/eval_odometry.php) — The standard benchmark for LiDAR/Visual Odometry.
> - [EuRoC MAV Dataset](https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets) — Benchmark dataset for VIO.
> - [Lin & Zhang, "R3LIVE: A Robust, Real-time, RGB-colored, LiDAR-Inertial-Visual tightly-coupled state Estimation and mapping package" (2022)](https://arxiv.org/abs/2109.07982) — A representative three-sensor fusion work.
> - [Giseop Kim's blog — Filter-based VIO: a history of the MSCKF family](https://gisbi-kim.github.io/blog/2021/04/27/msckf-history.html) — From the original MSCKF to stereo extensions, a lineage summary.

### 14.6 Loop Closure & Global Optimization

As SLAM runs, the map gradually distorts. A robot may complete a large loop and return to its starting point while the start and end remain misaligned on the map. Loop closure recognizes the previously visited place and corrects the accumulated error. Large-scale environments need this process to keep drift bounded.

#### 14.6.1 Place Recognition

Recognize previously visited places to correct drift.

The same place looks entirely different when the time of day, lighting, or season changes. If you mistake a similar-looking different place for the same place (a false positive), the map gets even worse. This is why the precision of place recognition has to be very high.

**Bag of Words (BoW)**: computes similarity between images based on a visual vocabulary. The DBoW2 library is the representative and is used in ORB-SLAM. Fast and well-tested, but fragile against illumination and viewpoint changes.

**NetVLAD**: a deep learning-based end-to-end trained global descriptor that is robust against illumination and weather changes. (see Section 14.14)

**LiDAR Place Recognition**: Scan Context compresses a point cloud into a 2D bird-eye-view descriptor, and PointNetVLAD learns directly from point clouds.

#### 14.6.2 Pose Graph Optimization

When a loop is detected, correct the entire trajectory.

```
Nodes: robot poses
Edges: relative transformations (odometry, loop closure)

Goal: find node positions that satisfy all edge constraints
```

Intuitively: a trajectory built from odometry is "locally roughly correct but globally twisted." When loop closure adds a constraint that "this place and that place are the same spot," pose graph optimization "smoothly adjusts the entire trajectory to satisfy all the constraints as well as possible." This is a nonlinear least squares problem.

The main tools are **g2o**, a lightweight library dedicated to pose graph / BA (ORB-SLAM); **GTSAM**, based on factor graphs and iSAM2 (LIO-SAM); and **Ceres Solver**, a general-purpose nonlinear least squares library developed by Google. For the selection criteria, refer to the comparison table in Section 14.9.4.

> **Further reading**
> - [GTSAM Documentation & Tutorials](https://gtsam.org/) — Factor graph-based optimization library. Includes pose graph optimization examples.
> - [Cyrill Stachniss — Graph-based SLAM](https://www.youtube.com/watch?v=uHbRKvD8TWg) — Intuitive explanation of pose graph optimization.
> - [g2o GitHub](https://github.com/RainerKuemmerle/g2o) — Graph optimization framework.
> - [Jinyong Jeong's blog — Robust Graph SLAM](https://jinyongjeong.github.io/2017/03/04/lec15_Robust_Graph_SLAM/) — Korean-language walkthrough of robust SLAM techniques including M-estimators, max-mixture, and DCS.

> **Practice**: [Pose Graph Optimization](https://alexjunholee.github.io/robotics-practice/app.html#pose_graph)
> Manipulate the nodes (poses) and edges (constraints) of a pose graph and observe how the trajectory is corrected when you add a loop closure.

### 14.7 Localization

Estimate the current pose given a prior map. If SLAM is "estimate pose while building a map," then localization is "estimate pose only, in an already built map." In practice, service robots often build a map with SLAM ahead of time and then run only localization during operation.

The derivation of the MCL algorithm is in §3.11 (Ch.3). The mathematical foundation of EKF is in §3.10 (Ch.3). For localization extended with IMU coupling, see §14.10. What follows covers the classification of localization scenarios and algorithm variants.

Map-based localization uses a pre-built map and is thus lighter than SLAM, but the map has to be updated when the environment changes.

**Monte Carlo Localization (MCL)**:
- Particle filter based
- 2D LiDAR + occupancy grid map
- ROS AMCL package

Intuition of MCL: scatter thousands of "virtual robots (particles)" across the map. Each particle is a hypothesis of the form "I am here, facing this direction." Compare against the actual sensor measurements; particles that match survive, and the ones that don't match die out. Over time, the particles cluster around the true position.

**LiDAR Localization**: estimate pose precisely by matching against a point cloud map with ICP or NDT.

> **Further reading**
> - [Cyrill Stachniss — Monte Carlo Localization](https://www.youtube.com/watch?v=MsYlueVDLI0) — Intuitive explanation of MCL/particle filter.
> - [ROS Navigation Stack — AMCL](http://wiki.ros.org/amcl) — Using MCL in ROS.

> **Practice**: [Particle Filter](https://alexjunholee.github.io/robotics-practice/app.html#particle_filter)
> Visualize the process of particle filter-based robot localization and observe particle convergence interactively.

> **Practice**: [Occupancy Grid](https://alexjunholee.github.io/robotics-practice/app.html#occupancy_grid)
> Visualize the construction of a 2D occupancy grid map and observe how sensor measurements turn into a probabilistic map.

#### 14.7.1 Classification of Localization Problems

The difficulty of localization does not collapse into a single number. Four axes interact to determine the algorithm choice.

| Axis | Options | Note |
|---|---|---|
| Prior knowledge | position tracking → global localization → kidnapped robot | increasing difficulty |
| Environment | static (robot only moves) → dynamic (people, doors, lighting) | harder as dynamism grows |
| Agency | passive (observe only) → active (choose exploration actions) | active converges faster |
| Robot count | single → multi (belief sharing via mutual observation) | multi yields richer information |

Two scenarios are canonical, one is an extension.

**Position tracking**: The initial pose is known and belief stays as a narrow unimodal Gaussian. EKF Localization fits well.

**Global localization**: The initial pose is unknown. The belief must start from a uniform distribution and converge as measurements accumulate. Multi-modal belief representation is needed, so Grid Localization or MCL is appropriate.

**Kidnapped robot**: The robot is forcibly moved to a different location during operation. It is harder than global localization because the robot does not detect the displacement itself. Every algorithm will eventually face this situation, so recovery capability is itself a measure of robot autonomy.

ROS Nav2's `recovery_alpha_slow/fast` parameters are designed with the kidnapped scenario in mind. Warehouse AGV and cleaning robot boot-up corresponds to global localization; normal operation corresponds to tracking.

#### 14.7.2 Markov Localization

Markov localization is less an algorithm than a name for **the direct application of the Bayes filter to the localization problem**. EKF Localization, Grid Localization, and MCL all branch off from this shared Bayes filter framework; what differs is how each one represents the belief.

The only difference from the Bayes filter (Ch.3 §3.9) is that the motion model and observation model both take **the map m** as an additional input.

The initial belief bel(x_0) is initialized differently for each scenario:
- Position tracking: $\text{bel}(x_0) = \mathcal{N}(x_0;\, \bar{x}_0, \Sigma)$ — narrow Gaussian
- Global localization: $\text{bel}(x_0) = 1/|X|$ — uniform over all valid poses
- Partial knowledge: uniform over the known vicinity, zero elsewhere

The algorithms in §14.7.3–§14.7.7 are variations on "how to implement the bel representation in the box above."

#### 14.7.3 EKF Localization

EKF Localization is a special case of Markov localization that represents belief as a Gaussian $(\mu_t, \Sigma_t)$. **The unimodal assumption makes it suitable only for position tracking.** Global localization and the kidnapped problem require multi-modal belief, so EKF cannot address them.

It applies the EKF of §3.10.2 (Ch.3) to localization. What follows is the assumption structure — a feature-based map with known landmark correspondences — and the concrete algorithm.

**Assumptions**: The map m is feature-based (a set of point landmarks). Each measurement $z_t^i = (r, \phi, s)^T$ (range, bearing, signature). The correspondence $c_t^i$ is known (identifiable landmarks such as ARTags, QR codes, or the Eiffel Tower).

```
EKF_localization_known_correspondences(μ_{t-1}, Σ_{t-1}, u_t, z_t, c_t, m):
  // Motion update (linearized velocity model)
  μ̄_t = μ_{t-1} + [velocity model displacement]
  G_t = ∂g/∂x |_{μ_{t-1}, u_t}         // 3×3 Jacobian
  Σ̄_t = G_t Σ_{t-1} G_t^T + R_t

// Measurement update (sequential updates over landmarks)
  μ_t = μ̄_t
  Σ_t = Σ̄_t
  for each observed z_t^i = (r, φ, s)^T do
    j = c_t^i
    δ = (m_{j,x} − μ_{t,x},  m_{j,y} − μ_{t,y})^T,   q = δ^T δ
    ẑ_t^i = (√q,  atan2(δ_y, δ_x) − μ_{t,θ},  m_{j,s})^T
    H_t^i = Jacobian (3×3, last row is 0 — signature independent of pose)
    K_t^i = Σ_t H_t^{i,T} (H_t^i Σ_t H_t^{i,T} + Q_t)^{-1}
    μ_t = μ_t + K_t^i (z_t^i − ẑ_t^i)
    Σ_t = (I − K_t^i H_t^i) Σ_t
  endfor
  return μ_t, Σ_t
```

Under the **conditional independence assumption** $p(z_t | x_t, m) = \prod_i p(z_t^i | x_t, m)$, measurements can be stacked into one update or conditioned on sequentially. The pseudocode above is sequential: each measurement uses the $(\mu_t, \Sigma_t)$ produced by the previous one. With nonlinear models, relinearization can make the stacked and sequential forms differ slightly.

Practical limit: *Probabilistic Robotics* uses heading uncertainty of roughly ±20° as an example of a regime where linearization becomes risky. It is not a universal threshold. The observation geometry, motion, and noise call for NIS/NEES or Monte Carlo consistency checks. EKF localization remains applicable to problems where belief stays unimodal, such as identifiable ARTag or AprilTag landmarks and some GNSS+IMU fusion systems.

**Unknown correspondences**: In practice $c_t^i$ is usually unknown. Maximum likelihood (ML) data association selects the map landmark with the smallest Mahalanobis distance.

$$j(i) = \arg\min_k (z_t^i - \hat{z}_t^k)^T \Psi_k^{-1} (z_t^i - \hat{z}_t^k), \quad \Psi_k = H_t^k \bar\Sigma_t H_t^{k,T} + Q_t$$

Minimizing Mahalanobis distance corresponds to maximizing Gaussian log likelihood when candidate determinants and priors are equal. Practical systems add (1) a $\chi^2$ gate matched to measurement dimension and (2) one-to-one assignment constraints across measurements in a frame. ORB-SLAM's descriptor matching and geometric verification are comparable as candidate generation followed by outlier rejection, but they do not directly implement this EKF ML-association rule.

#### 14.7.4 Multi-Hypothesis Tracking (MHT)

EKF uses a unimodal Gaussian and cannot represent data association ambiguity. MHT represents belief as a **Gaussian mixture** and maintains multiple hypotheses in parallel.

Each hypothesis $h$ runs an independent EKF. When a measurement arrives, each hypothesis is extended, and hypotheses whose weight (posterior probability) falls below the threshold $\psi_{\min}$ are pruned. A pruning policy is essential to prevent hypothesis count from exploding.

Multi-object tracking also commonly combines Mahalanobis gating with Hungarian assignment. That selects one assignment and should be distinguished from MHT, which maintains several association hypotheses over time.

#### 14.7.5 Grid Localization

Where MHT represented belief as a Gaussian mixture, Grid Localization takes a more direct approach: it divides the entire pose space into cells and accumulates probability per cell.

A **histogram filter** that discretizes pose space into cells. It can represent global and multi-modal belief that EKF cannot, but the computational cost proportional to the number of cells $K$ is the trade-off.

$\text{bel}(x_t) = \{p_{k,t}\}$: one probability per cell $x_k$, summing to 1.

**Resolution trade-off**: a finer grid reduces the quantization error of the pose estimate. But smaller cells mean sharply higher CPU time for global localization. Practical tricks include caching raycast results, scan subsampling, and selective updates (only cells above a threshold).

It serves as an educational bridge: represents global belief on a discrete grid and illustrates why particle filters perform better. ROS `amcl` is Grid Localization with the grid cells replaced by particles.

#### 14.7.6 MCL Algorithm (Expanded)

The derivation and principles of MCL are in §3.11 (Ch.3). Here the full skeleton of the algorithm as a localization method is stated explicitly.

```
MCL(X_{t-1}, u_t, z_t, m):
  X̄_t = X_t = ∅
  for k = 1 to M do
    x_t^[k] = sample_motion_model(u_t, x_{t-1}^[k])    // motion proposal
    w_t^[k] = measurement_model(z_t, x_t^[k], m)        // likelihood weight
    X̄_t += ⟨x_t^[k], w_t^[k]⟩
  endfor
  for k = 1 to M do
    i ~ Categorical(w_t^[1], ..., w_t^[M])              // importance-proportional resample
    X_t += x_t^[i]
  endfor
  return X_t
```

Three phases: **predict (sample) → weight → resample**. Initialization depends on the scenario: for global localization, sample $M$ particles from a uniform distribution over free space; for position tracking, sample from a narrow Gaussian.

**Computational adaptability**: rather than fixing $M$, sampling "as many as possible before the next measurement arrives" means faster CPUs yield larger $M$ and automatically better accuracy.

The proposal is the motion model, so with a perfect sensor (extremely narrow measurement likelihood) nearly all particle weights approach zero. This is what Mixture MCL (§14.7.8) fixes.

ROS2 Nav2's `nav2_amcl` implements this structure directly.

#### 14.7.7 Augmented MCL — Kidnapping Recovery

Standard MCL is fragile against kidnapping. Once particles converge on a single pose and the robot is forcibly moved, no particle sits near the new location and there is no recovery path. Augmented MCL **injects random particles when the short-term average of measurement likelihood suddenly drops relative to the long-term average.** "The sensor suddenly stops matching the map" equals "the robot is lost" — this intuition is quantified as the ratio of two exponential moving averages.

```
Augmented_MCL(X_{t-1}, u_t, z_t, m):
  static w_slow, w_fast
  X̄_t = X_t = ∅,  w_avg = 0
  for k = 1 to M do
    x_t^[k] = sample_motion_model(u_t, x_{t-1}^[k])
    w_t^[k] = measurement_model(z_t, x_t^[k], m)
    X̄_t += ⟨x_t^[k], w_t^[k]⟩
    w_avg += w_t^[k] / M
  endfor
  w_slow += α_slow (w_avg − w_slow)    // long-term average (slow to change)
  w_fast += α_fast (w_avg − w_fast)    // short-term average (fast to change)
  for k = 1 to M do
    with probability max(0, 1 − w_fast/w_slow) do
      X_t += random pose from bel(x_0)  // random particle injection
    else
      i ~ Categorical(w_t^[1], ..., w_t^[M])
      X_t += x_t^[i]
  endfor
  return X_t
```

Requirement: $0 \le \alpha_{\text{slow}} \ll \alpha_{\text{fast}}$ (e.g., $\alpha_{\text{slow}} = 0.001$, $\alpha_{\text{fast}} = 0.1$).

$$p_{\text{inject}} = \max\!\left(0,\, 1 - \frac{w_{\text{fast}}}{w_{\text{slow}}}\right)$$

Normally $w_{\text{fast}} \approx w_{\text{slow}}$ → ratio $\approx 1$ → injection probability $\approx 0$ → identical to standard MCL. Immediately after kidnapping, measurements no longer match anywhere → $w_{\text{fast}}$ drops sharply → injection probability rises. When the long-term average catches up, the ratio returns to 1 → injection stops.

A transient noise spike also drives $w_{\text{fast}}$ down and raises the injection probability. The slow movement of $w_{\text{slow}}$ is the condition that makes the ratio meaningful, not a filter against spikes, so cutting false positives means tuning the smoothing coefficient of $w_{\text{fast}}$ ($\alpha_{\text{fast}}$).

The `recovery_alpha_slow` and `recovery_alpha_fast` parameters in ROS `amcl` control this adaptive random-pose injection. Research systems sometimes mix pose candidates from NetVLAD-like place recognition and PnP into particle proposals, but this is not default AMCL behavior and must be validated for the target environment.

#### 14.7.8 Mixture MCL

Where Augmented MCL injects random poses, Mixture MCL **changes the proposal distribution itself**. A fraction of particles is sampled directly from the **measurement model** rather than the motion model.

$$x_t^{[k]} \sim \begin{cases} p(z_t | x_t, m) & \text{with probability } \rho \\ \text{sample\_motion\_model}(u_t, x_{t-1}^{[k]}) & \text{with probability } 1 - \rho \end{cases}$$

Particles sampled directly from measurements concentrate in regions of strong sensor information, fixing the proposal inefficiency of basic MCL in low-noise sensor environments. The advantage over Augmented MCL is that it handles both kidnapping recovery and low-noise sensor failure. The implementation burden is that sampling directly from $p(z_t | x_t, m)$ requires an inverse sensor model.

#### 14.7.9 Dynamic Environment Filtering

When dynamic objects (people, vehicles) are present, some beams observe obstacles not in the map. The posterior probability of the short-hit component $p_{\text{short}}(z | x, m)$ from the beam sensor model is used to exclude suspicious beams from the localization weight computation.

For each beam $z_t^k$, the four-component mixture model (§2.7, Ch.2) is evaluated and beams with high posterior probability on the short component are excluded from the weight calculation. Without this filtering, MCL becomes unstable when many people occupy a corridor.

#### 14.7.10 Filter Comparison Summary

| Algorithm | Belief representation | Position tracking | Global loc | Kidnapped | Compute cost |
|---|---|---|---|---|---|
| EKF Loc | Gaussian (μ, Σ) | good | impossible | impossible | O(N) |
| MHT | Gaussian mixture | good | limited | limited | O(H·N) |
| Grid Loc | histogram | good | possible | possible | O(K) |
| MCL | particle set | good | possible | possible with Augmented MCL | O(M) |

N is the landmark count, H the hypothesis count, K the grid-cell count, and M the particle count. EKF cannot address global/kidnapped problems because it assumes a unimodal Gaussian. Grid and MCL allow users to choose a balance between computation and accuracy by adjusting the resource budget.

#### 14.7.11 Implementation Notes: Landmark Efficiency and Negative Information

Several issues arise frequently when implementing EKF Localization in practice.

**Efficient landmark search**: a full search over N landmarks costs O(N) per observation. In low dimensions a balanced KD-tree has average query behavior near O(log N), but worst-case cost is O(N); a grid index depends on cell occupancy and search radius.

**Mutual exclusion**: Two measurements within one frame cannot correspond to the same landmark. ML data association is a component-wise optimization and does not enforce this constraint automatically. When conflicting pairs occur, a repair step is needed: choose the measurement with the smaller Mahalanobis distance and discard the other.

**Outlier rejection** removes measurements whose Mahalanobis distance exceeds the $\chi^2_{95\%}$ threshold. This single step substantially reduces EKF brittleness.

**Negative information**: "No landmark was observed in this angular range" can also be informative for localization, but the correct probabilistic treatment is complex and the implementation burden is high. Most practical systems ignore negative information.

---

### 14.7B Occupancy Grid Mapping

§14.7 estimated location given a map. This section reverses the direction: Occupancy Grid Mapping **estimates the occupancy probability of each cell given known poses.** It is the core post-processing step that produces the final map from the pose trajectory delivered by pose graph optimization. In a real SLAM pipeline, pose graph optimization fixes the pose trajectory, and then the algorithm in this section completes the final map. The foundation in binary Bayes filters is in §3.11.2 (Ch.3).

#### 14.7B.1 Introduction: Why Mapping Is Hard

Mapping is said to be harder than localization. A pose is a continuous variable $x_t \in \mathbb{R}^3$, but a map m is a high-dimensional discrete variable composed of tens of thousands to millions of cells. The number of possible maps is $2^{|m|}$, making direct search impossible.

Two assumptions prevent combinatorial explosion: (1) **poses are known** ($x_{1:t}$ given), (2) **cells are conditionally independent**. The second assumption lets the map posterior factor into a product of per-cell marginals, splitting the whole problem into independent binary Bayes filters — one per cell.

$$p(m \mid z_{1:t}, x_{1:t}) = \prod_i p(m_i \mid z_{1:t}, x_{1:t})$$

Additional difficulties include sensor noise, perceptual aliasing (different measurements from the same location), environmental dynamics, and error accumulation over closed loops.

#### 14.7B.2 Standard Algorithm: Log-Odds Accumulation

The occupancy posterior of each cell is accumulated in **log-odds** form.

$$l_{t,i} = \log \frac{p(m_i \mid z_{1:t}, x_{1:t})}{1 - p(m_i \mid z_{1:t}, x_{1:t})}$$

Prior log-odds: $l_0 = \log[p(m_i) / (1 - p(m_i))]$. From the binary Bayes filter derivation (§3.11.2, Ch.3), the update rule is:

$$l_{t,i} = l_{t-1,i} + \text{inverse\_sensor\_model}(m_i, x_t, z_t) - l_0$$

Intuition: when a new measurement gives hit evidence for cell $m_i$, the log-odds rises; free evidence lowers it. The $-l_0$ term prevents the prior from being counted twice.

```
occupancy_grid_mapping({l_{t-1,i}}, x_t, z_t):
  for all cells m_i do
    if m_i is in perceptual field of z_t then
      l_{t,i} = l_{t-1,i} + inverse_sensor_model(m_i, x_t, z_t) − l_0
    else
      l_{t,i} = l_{t-1,i}    // outside sensing range — no change
  endfor
  return {l_{t,i}}
```

Recovery to probability: $p(m_i | z_{1:t}, x_{1:t}) = 1 - 1/(1 + \exp\{l_{t,i}\})$.

**inverse_sensor_model** (simplified example for a range finder):
```
inverse_range_sensor_model(m_i, x_t, z_t):
  compute range r and bearing φ to cell center
  nearest beam index k = argmin_j |φ − θ_{j,sens}|
  if outside beam or beyond z_t^k + α/2:
    return l_0                   // no information
  if |r − z_t^k| < α/2:
    return l_occ                 // hit (> l_0)
  if r ≤ z_t^k:
    return l_free                // free (< l_0)
```

$\alpha$ is the obstacle thickness parameter, $\beta$ is the beam opening angle. Cartographer's submap probability grid implements this accumulation as a product of odds, backed by a lookup table. ROS Nav2's `costmap_2d` instead works with 0-254 cost values updated by raytracing-based marking and clearing, and SLAM Toolbox (of the Karto family) thresholds a per-cell hit/visit count ratio, so their representations and update rules differ.

#### 14.7B.3 Multi-Sensor Fusion

Camera, LiDAR, sonar, and infrared each have a different inverse_sensor_model. The simplest fusion strategy is **conservative max per cell**: if any sensor reports a hit, that cell is classified as occupied. This conservative policy is safe for collision avoidance but tends to underestimate free space.

An alternative is to accumulate each sensor's log-odds updates independently and then sum per cell. When sensors carry different amounts of information, weighted summation is needed.

#### 14.7B.4 Learning the inverse_sensor_model

A hand-designed inverse_sensor_model is a simple geometric model. **If the forward model $p(z | x, m)$ is already available, the inverse can be derived by learning.**

The procedure: generate triples $\{(x^{(k)}, z^{(k)}, m_i^{(k)})\}$ from simulation, then train a function approximator with cross-entropy loss.

$$\mathcal{L} = -\sum_k \left[m_i^{(k)} \log \hat{p}_i + (1 - m_i^{(k)}) \log(1 - \hat{p}_i)\right]$$

A neural network with input $(x, z)$ and output $\hat{p}_i = p(m_i | x, z)$ takes over the role of inverse_sensor_model. This is useful when complex sensor geometry (sonar reflection patterns, LiDAR behavior on glass) is too difficult to model explicitly.

#### 14.7B.5 MAP Occupancy Mapping (Advanced)

The cell independence assumption of the standard algorithm creates one contradiction: adjacent cells inside the same beam cone share correlated evidence in reality, but the independence assumption ignores this correlation. The problem is most noticeable with wide-beam sensors such as sonar.

MAP Occupancy Mapping directly maximizes the mode of the map posterior.

$$m^* = \arg\max_m \left[\sum_t \log p(z_t \mid x_t, m) + \log p(m)\right]$$

Rather than an inverse model, it uses the **forward model** $p(z_t | x_t, m)$ directly. Starting from an all-free map, hill-climbing flips cells one at a time in the direction that increases log-likelihood.

```
MAP_occupancy_grid_mapping(x_{1:t}, z_{1:t}):
  m ← initialize all cells free
  repeat until convergence:
    for all cells m_i do
      m_i ← argmax_{k ∈ {0,1}} [k·l_0 + Σ_t log measurement_model(z_t, x_t, m | m_i=k)]
  return m
```

Practical limits: it is batch and does not fit incremental SLAM; hill-climbing gets trapped in local maxima; posterior uncertainty disappears. But the insight that **"the cell independence assumption must be broken"** carries forward.

#### 14.7B.6 Comparison with Other Spatial Representations: OctoMap, Voxblox, NeRF, 3DGS

**OctoMap** stores 3D occupancy in an octree and can be viewed as a direct 3D extension of an occupancy grid. **Voxblox** and **nvblox** belong to a different distance-field family: a TSDF stores signed distance to a surface. **NeRF** density and **3D Gaussian Splatting** opacity also composite transparency and color along rays, but they are learned novel-view representations rather than direct descendants of a binary occupancy Bayes filter. The families share mathematical ideas such as forward sensor models and ray integration, but their lineage and objectives should not be equated. Occupancy grids remain in navigation components such as SLAM Toolbox and the Nav2 costmap.

> **Practice**: [Occupancy Grid](https://alexjunholee.github.io/robotics-practice/app.html#occupancy_grid)
> Visualize the log-odds accumulation process cell by cell and observe how the hit/free regions of the inverse_sensor_model build into a map.

---

## Part 2. Recent Trends

### 14.8 Learning-based & Neural SLAM

Traditional SLAM uses hand-designed keypoints, matching algorithms, and optimization pipelines. Recent work has been replacing part or all of this pipeline with deep learning.

**DROID-SLAM (2021)**:
- SLAM based on dense recurrent optical flow
- Without keypoint extraction/matching, it iteratively refines dense optical flow to jointly estimate camera pose and depth
- Improved robustness in settings where existing methods fail, such as textureless environments and illumination changes
- Uses a differentiable dense bundle adjustment (DBA) layer for end-to-end training

Why DROID-SLAM drew attention: existing feature-based SLAM (ORB-SLAM) fails in keypoint-starved environments, and direct methods (DSO) are weak against illumination changes. DROID-SLAM uses learned representations, and as a result it overcomes these limits to a significant degree. That said, it requires a GPU, and its real-time performance does not always match the older methods.

**3DGS-SLAM fusion**:
The 3D Gaussian Splatting covered in 13.5.2 is also being used as the map representation in SLAM. SplaTAM and MonoGS are representative examples; they replace the sparse/dense point maps of classic SLAM with 3D Gaussians as the environment representation. The scene's visual fidelity improves, and rendering-based applications (virtual view synthesis, AR overlays, and so on) become possible.

> **Further reading**
> - [Teed & Deng, "DROID-SLAM: Deep Visual SLAM for Monocular, Stereo, and RGB-D Cameras" (2021)](https://arxiv.org/abs/2108.10869) — The DROID-SLAM paper.
> - [Keetha et al., "SplaTAM" (2024)](https://arxiv.org/abs/2312.02126) — Dense SLAM based on 3DGS.
> - [Awesome-SLAM GitHub](https://github.com/SilenceOverflow/Awesome-SLAM) — Collection of recent SLAM papers and projects.

---

## Part 3. Advanced

### 14.9 Advanced: SLAM Back-end Optimization

For the history of information-form SLAM before the factor graph era (EKF-SLAM, EIF, SEIF, EM), see §14.16 Advanced: History of Information-Form SLAM.

The SLAM front-end processes sensor data to produce constraints; the back-end finds the optimal state (poses, landmarks) that jointly satisfies these constraints. This process is a nonlinear least squares problem. What we cover here is the mathematical background needed to understand "why you configure libraries like g2o, GTSAM, and Ceres the way you do."

**Intuition for the problem the SLAM back-end solves**

After linearization, each SLAM back-end iteration reduces to a system of the form **Ax = b**.

The robot produces two kinds of data as it drives:
1. **Odometry**: "I moved 1 m forward" (relative motion)
2. **Observations**: "that landmark is visible at 3 m"

You want to find poses and landmark positions that satisfy all of these measurements, but because of sensor noise no solution satisfies them perfectly. Instead, you look for the solution that "minimizes the sum of squared errors against all the measurements." This is the nonlinear least squares problem, and solving it efficiently is the role of the SLAM back-end.

Because the objective is nonlinear, the back-end linearizes it around the current estimate and updates the state iteratively. Gauss-Newton repeats the sequence "linearize → solve Ax=b → update."

(Reference: [Giseop Kim's blog — SLAM back-end series](https://gisbi-kim.github.io/blog/2021/03/04/slambackend-1.html))

#### 14.9.1 Gauss-Newton on a Manifold

A SLAM pose lies on the Lie group SE(3), rather than in a Euclidean vector space. The usual Gauss-Newton update `x ← x + δx` therefore does not apply directly: adding a vector to a rotation matrix does not preserve a valid rotation.

The fix is to define the perturbation on the Lie algebra se(3).

**Update step (left perturbation)**:
```
T ← exp(δξ^) · T
```
Here `δξ ∈ R^6` is a small perturbation on se(3), `exp(·)` is the exponential map, and `^` (the hat operator) converts a 6-vector into a 4x4 matrix.

**Jacobian computation**: Compute the Jacobian of the error function `e(T)` with respect to `δξ`.
```
J = ∂e / ∂δξ
```
By the chain rule this becomes `∂e/∂(Tp) · ∂(exp(δξ^)Tp)/∂δξ`, where the second term is the derivative of the group action induced by the left perturbation; for a 3D point it takes the form `[I, -(Tp)^]` (with δξ ordered as translation then rotation). The left Jacobian of SE(3) is the 6x6 matrix that relates Lie algebra perturbations through the BCH relation, so it is a different object from this term.

**Normal equation**:
```
(J^T Σ^{-1} J) δξ* = -J^T Σ^{-1} e
```
- `Σ` is the measurement noise covariance
- `H = J^T Σ^{-1} J` is the Gauss-Newton approximation of the Hessian; this is the **information matrix**
- With multiple constraints, sum the per-constraint `J^T Σ^{-1} J` (additive property)

Iterate this process until convergence. At every iteration, recompute the Jacobian at the current estimate and apply the update.

#### 14.9.2 Schur Complement (Marginalization)

In bundle adjustment (BA) the state variables are of two kinds: camera poses (p) and landmarks (l). The Hessian `H` of the normal equation has the following block structure:

```
[H_pp  H_pl] [δp]   [b_p]
[H_lp  H_ll] [δl] = [b_l]
```

Let the number of poses be `m` and the number of landmarks be `n`; typically `n >> m`. Solving this large system directly is expensive.

Use the **Schur complement** to marginalize out the landmarks:

```
(H_pp - H_pl · H_ll^{-1} · H_lp) δp = b_p - H_pl · H_ll^{-1} · b_l
```

This is possible because **`H_ll` is block diagonal**. Each landmark is not directly coupled to other landmarks (no shared factor between two landmarks), so the inverse of `H_ll` can be computed by inverting each block independently. The cost is a cheap `O(n)`.

After the Schur complement, the reduced camera-system dimension is set by pose count `m`, but landmark elimination and back-substitution still depend on the observation and landmark counts. Block sparsity enables efficient large BA solves; throughput still depends on graph structure, solver, and hardware.

Once `δp` is found, recover `δl` by back-substitution:
```
δl = H_ll^{-1} (b_l - H_lp · δp)
```

#### 14.9.3 Sparsity and Variable Ordering

In pose-graph optimization, `H` is usually **sparse** because each factor connects only a small subset of poses. Odometry factors connect neighbors and loop factors connect distant poses. Node degree depends on the dataset and closure count, while dense loop proposals or a poor elimination order can increase fill-in.

When solving a sparse linear system you use Cholesky factorization (`H = L L^T`), and the **fill-in** problem shows up here. Positions that were originally zero become non-zero during factorization. Heavy fill-in blows up memory and compute cost.

To minimize fill-in, you have to choose the variable ordering well:
- **COLAMD** (Column Approximate Minimum Degree): A common heuristic for sparse least-squares problems that approximates a column ordering with limited fill-in.
- **AMD** (Approximate Minimum Degree): Similar to COLAMD but specialized for symmetric matrices.
- **Nested dissection**: Determines the ordering by recursively partitioning the graph. Effective on large-scale problems.

When configuring solvers in libraries such as g2o, GTSAM, and Ceres, inspect both the linear solver type (DENSE_SCHUR, SPARSE_NORMAL_CHOLESKY, and so on) and the ordering strategy. The effect of ordering on fill-in and runtime depends on graph structure, so compare memory and latency on representative data.

```python
# Example of setting the ordering in Ceres Solver (Python binding)
options = ceres.SolverOptions()
options.linear_solver_type = ceres.LinearSolverType.SPARSE_NORMAL_CHOLESKY
options.sparse_linear_algebra_library_type = ceres.SparseLinearAlgebraLibraryType.SUITE_SPARSE
# ordering typically defaults to COLAMD automatically, but manual configuration is possible
```

#### 14.9.4 Comparison of Optimization Libraries

| Library | Characteristics | Primary uses |
|---|---|---|
| **g2o** | Dedicated to pose graph / BA, lightweight, C++ only | ORB-SLAM2/3, LSD-SLAM |
| **GTSAM** | Factor graph based, supports Bayes tree (iSAM2), strong at incremental optimization | LIO-SAM, research |
| **Ceres Solver** | General-purpose nonlinear least squares, supports auto-diff, developed by Google | Cartographer, VINS-Fusion, various projects |

Selection criteria:
- SLAM-only and want to stay lightweight → g2o
- Need factor graph modeling, and incremental update (progressive optimization as keyframes are added) matters → GTSAM (iSAM2)
- Need general-purpose optimization beyond SLAM, and do not want to derive Jacobians by hand → Ceres (auto-diff)

> **Further reading**
> - Barfoot, "State Estimation for Robotics" Ch.4 (Nonlinear Estimation) — Systematic treatment of optimization on manifolds.
> - [Dellaert & Kaess, "Factor Graphs for Robot Perception" (Foundations and Trends in Robotics, 2017)](https://www.cs.cmu.edu/~kaess/pub/Dellaert17fnt.pdf) — Factor graphs and SLAM back-end theory.
> - [g2o Tutorial](https://github.com/RainerKuemmerle/g2o) / [GTSAM Tutorial](https://gtsam.org/tutorials/intro.html) — Hands-on tutorials per library.
> - [Giseop Kim's blog — Gauss-Newton Opt == IEKF update?](https://gisbi-kim.github.io/blog/2022/03/05/gn-iekf-same.html) — An exposition of the mathematical equivalence between GN optimization and iterated Kalman filtering. A reference for the filter vs optimization debate.

> **Practice**: [Bundle Adjustment Visualization](https://alexjunholee.github.io/robotics-practice/app.html#bundle_adjustment)
> Observe the bundle adjustment process — jointly optimizing camera poses and 3D points — interactively.

### 14.10 Advanced: IMU Preintegration

For localization extended with IMU coupling, see §14.7 and §14.7B for the mapping foundation. When introducing VINS-Mono in 14.3.3 we mentioned IMU preintegration briefly. Here we look at the mathematical background.

**Problem statement**: An IMU typically outputs acceleration and angular velocity at 200–1000 Hz. In contrast, SLAM optimization is done on a keyframe basis (a few Hz to tens of Hz). Hundreds of IMU measurements sit between two keyframes. An IMU measurement is itself the observation of a factor, but adding a state (pose, velocity, bias) at every one of those instants makes the problem size explode.

**The idea of preintegration**: Compress the IMU measurements between two keyframes `i` and `j` into a single "relative motion measurement." This compressed measurement enters optimization as a factor.

**Preintegrated measurements**: Compute three relative quantities from keyframe `i` to `j`.

```
ΔR_ij = Π_{k=i}^{j-1} Exp((ω_k - b_g) · Δt)          # relative rotation
Δv_ij = Σ_{k=i}^{j-1} ΔR_ik · (a_k - b_a) · Δt        # relative velocity
Δp_ij = Σ_{k=i}^{j-1} (Δv_ik · Δt + 0.5 · ΔR_ik · (a_k - b_a) · Δt^2)  # relative position
```

Here `ω_k` and `a_k` are IMU measurements, `b_g` and `b_a` are the gyroscope/accelerometer biases, and `Δt` is the IMU sampling interval.

These preintegrated measurements are computed **in the coordinate frame of keyframe `i`**. Even if the absolute pose of keyframe `i` changes during optimization, you do not need to recompute the preintegrated measurement.

**Covariance propagation**: Compute how the IMU measurement noise propagates into the preintegrated measurement. Discrete-time propagation updates the covariance at every IMU measurement.

```
Σ_{k+1} = A_k · Σ_k · A_k^T + B_k · Q · B_k^T
```
- `A_k`: state transition matrix (the Jacobian at the current state)
- `B_k`: noise input matrix
- `Q`: IMU noise covariance (from the datasheet)

This covariance becomes the information matrix (`Σ^{-1}`) of the corresponding factor in optimization.

**Correction for bias changes**: During optimization the IMU bias estimate can change. If the bias changes, in principle you should redo the preintegration from scratch. But that is expensive. Instead, you correct it with a **first-order approximation**:

```
ΔR_ij ≈ ΔR_ij^0 · Exp(∂ΔR/∂b_g · δb_g)
Δv_ij ≈ Δv_ij^0 + ∂Δv/∂b_g · δb_g + ∂Δv/∂b_a · δb_a
Δp_ij ≈ Δp_ij^0 + ∂Δp/∂b_g · δb_g + ∂Δp/∂b_a · δb_a
```

`^0` denotes the value computed with the previous bias estimate, `δb` is the bias change, and the partial derivatives are accumulated alongside the preintegration. As long as the bias change is not large (which is usually the case), this approximation is accurate enough.

**Why preintegrate on the manifold**: Preintegration itself was first proposed by Lupton and Sukkarieh, whose formulation parameterized rotation with Euler angles. Forster et al. recast it on SO(3). Integrating ahead of time on the Lie group 1) keeps rotation within the group structure and avoids the singularities of Euler angles, and 2) produces a result that plugs directly into a factor graph as a relative motion measurement. Discrete-time integration error and measurement noise remain either way. This recasting is the core contribution of Forster et al. (2015 RSS, 2017 TRO).

**Tightly-coupled vs loosely-coupled**: Using LIO-SAM as an example:
- **Loosely-coupled**: This label covers two variants. In one, the IMU is used only for point-cloud de-skewing and as the initial guess for scan matching; in the other, LiDAR odometry and the IMU estimate state independently and are combined afterwards based on covariance. LeGO-LOAM belongs to the former.
- **Tightly-coupled**: Optimizes an IMU preintegration factor jointly with the LiDAR odometry factor inside the same factor graph. The IMU acts not as a mere initial guess but as an independent observation of the relative pose between keyframes. LIO-SAM follows this approach.

The advantage of tightly-coupled shows up in aggressive motion (fast rotation, sharp acceleration/deceleration). The IMU factor catches fast changes that LiDAR scan matching alone cannot capture. A practical advantage is that, because it is in factor graph form, GPS factors, loop closure factors, and others can be plugged in like modules.

**Structure of LIO-SAM**: Built on GTSAM, it optimizes IMU preintegration factors + LiDAR odometry factors + GPS factors + loop closure factors in a single graph. LiDAR odometry extracts edge features and planar features separately and manages them in voxel maps of different resolutions. During scan matching, planar features solve for the relative transformation that minimizes point-to-plane distance, and edge features minimize point-to-line distance.

Modern VIO/LIO systems such as VINS-Mono, ORB-SLAM3 (visual-inertial mode), and LIO-SAM use this technique directly to implement their IMU factors.

> **Further reading**
> - [Forster et al., "On-Manifold Preintegration for Real-Time Visual-Inertial Odometry" (TRO 2017, arXiv:1512.02363)](https://arxiv.org/abs/1512.02363) — The original on-manifold preintegration paper (the preintegration concept itself is from Lupton & Sukkarieh 2012). Equation-heavy, but required reading for the field.
> - [Forster et al., "IMU Preintegration on Manifold for Efficient VIO" (2015 RSS)](https://rpg.ifi.uzh.ch/docs/RSS15_Forster.pdf) — An earlier version of the paper above that explains the method more concisely.
> - [Shan et al., "LIO-SAM" (IROS 2020)](https://github.com/TixiaoShan/LIO-SAM) — Reference implementation of tightly-coupled LIO. Read both the code and the paper.
> - [Sola et al., "A micro Lie theory for state estimation in robotics" (arXiv:1812.01537)](https://arxiv.org/abs/1812.01537) — A practical summary of Lie groups and algebras. Good to read before preintegration.
> - Source code of GTSAM's `PreintegratedImuMeasurements` class — see how the theory turns into code.
> - [IMU Preintegration MATLAB implementation](https://github.com/GentleDell/imu_preintegration_matlab) — MATLAB code tested on KITTI. Good for studying by cross-referencing equations and code.

### 14.11 Advanced: Observability Analysis

Run a SLAM/VIO system and you run into phenomena like "why is drift so bad in this situation?" and "why does the pose wobble when I stand still?" Many of these phenomena stem from limits of the system's **observability**.

**Unobservable states of visual-inertial systems**: VIO has 4 degrees of freedom that cannot be estimated (unobservable):

1. **Global position (3 DoF)** — The absolute position is unknown. Without an absolute reference like GPS, you can only treat the starting point as the origin.
2. **Global yaw (1 DoF)** — The rotation (heading) about the gravity-direction axis. Without a compass, you cannot tell "which way is north."

On the other hand, the following are observable:
- **Roll/pitch**: The IMU accelerometer senses the gravity direction, so roll/pitch relative to gravity can be estimated.
- **Scale** (when stereo/IMU is present): The stereo camera's baseline or the IMU's acceleration measurement lets you recover scale. However, **with a monocular camera alone, scale is unobservable**.

**Degenerate motion** — Under specific motion patterns, additional states become unobservable:

- **Pure rotation**: In monocular VO, translation cannot be estimated. The reason is that in epipolar geometry the epipole goes to infinity. This is the cause of the practical phenomenon "tracking breaks when you rotate the camera in place."
- **Constant velocity**: The IMU accelerometer distinguishes gravity from acceleration, and when there is no acceleration (constant velocity), the accelerometer bias cannot be distinguished from a small error in the gravity direction. IMU bias becomes unobservable.
- **Stationary**: A special case of constant velocity. Stand still and there is no parallax in the visual features and no IMU acceleration, so both bias and scale are unobservable. This is the answer to "why does VINS drift when I stand still?"

**Problem in EKF-based systems**: Apply a standard EKF to VIO and, due to linearization error, the covariance shrinks even along theoretically unobservable directions (the uncertainty is reduced artificially). This is a major cause of inconsistency.

**OC-EKF (Observability-Constrained EKF)**: To fix this problem, the EKF's Jacobian is modified to preserve the null space of the unobservable directions. It forces the estimator to "keep not knowing what it does not know."

Practical implications:
- When using a VIO system, initialize by **moving in diverse directions**. With motion in only one direction, IMU bias cannot be estimated reliably.
- A VIO without loop closure will always drift over long-term operation. Error along the unobservable yaw direction keeps accumulating.
- Monocular VIO's scale is observable only when there is acceleration/deceleration. Moving at a constant velocity produces scale drift.

> **Further reading**
> - [Hesch et al., "Consistency Analysis and Improvement of Vision-aided Inertial Navigation" (TRO 2014)](https://ieeexplore.ieee.org/document/6672119) — The original paper for OC-EKF/OC-VINS.
> - Barfoot, "State Estimation for Robotics" Ch.9 — Theoretical foundation of observability analysis.
> - [Huang & Dissanayake, "A critique of current developments in Simultaneous Localization and Mapping" (IJRR 2016)](https://journals.sagepub.com/doi/10.1177/0278364916643566) — A critical summary of observability/consistency issues in SLAM.

> **Practice**: [Odometry Uncertainty Visualization](https://alexjunholee.github.io/robotics-practice/app.html#odom_uncertainty)
> Observe interactively how the uncertainty of odometry accumulates over time and how the covariance ellipse grows.

#### 14.11.1 Filter-based vs Optimization-based: Which Is Better?

Whether filtering or optimization is preferable has long been debated in SLAM and VIO. Mathematically, Gauss-Newton optimization and the iterated EKF (IEKF) solve the same problem in different forms.

- **Filter (EKF, MSCKF, and so on)**: updates state and covariance as measurements arrive. A simple EKF odometry can retain only the current state, whereas MSCKF keeps a bounded set of past camera clones in its state. Bounding the retained history can use less memory than full smoothing.
- **Optimization (BA, factor graph)**: Keeps all past states and optimizes them jointly. Because past data can be relinearized, accuracy is higher. But compute grows with the number of states (mitigated by sliding window or iSAM2).

Performance differences between VINS-Mono (optimization) and MSCKF (filter) arise mainly from the **system structure**, including which states are retained and which measurements are used. Optimization-based systems can also reduce past linearization error through relinearization.

Practical choice:
- IMU-centric + lightweight → filter (MSCKF, the IEKF in FAST-LIO2)
- Camera-centric + accuracy → optimization (VINS-Mono, ORB-SLAM3)
- Both needed → hybrid (LIO-SAM: optimization with IMU preintegration plugged in as a factor)

(Reference: [Giseop Kim's blog — Gauss-Newton Opt == IEKF update?](https://gisbi-kim.github.io/blog/2022/03/05/gn-iekf-same.html))

### 14.12 Advanced: Semantic SLAM

Classic SLAM produces a purely geometric map. Point clouds, meshes, occupancy grids and so on record only "the shape of space." It knows there is a wall, but not whether that wall is a wall, a door, or a bookshelf. Semantic SLAM adds semantic information to the map.

Approaches split by landmark representation. **Object-level SLAM** (CubeSLAM, QuadricSLAM) estimates objects as landmarks — 3D cuboids, dual quadrics, and the like — rather than points. It depends on an object detector, but data association is more robust than point-based, and object-level reasoning becomes possible. **Panoptic SLAM** fuses panoptic segmentation results into 3D to produce a map where every pixel carries a semantic label. The robot can directly query "there are 3 chairs in this room" on the map. **Open-vocabulary SLAM** (ConceptGraphs) stores features from a vision-language model like CLIP in the map, so places can be searched with natural language. It connects directly to the 3D Scene Graph (Hydra, etc.) discussed in Chapter 13.

**Handling dynamic objects**: Semantic labels are also used to improve SLAM robustness in dynamic environments. If you drop features from classes likely to be dynamic — "person," "car," and so on — from tracking/mapping, you can do clean SLAM with the static environment alone.
- DynaSLAM: ORB-SLAM2 + Mask R-CNN to mask dynamic objects
- DS-SLAM: semantic segmentation to filter dynamic regions

```python
# Pseudocode for dynamic object filtering
dynamic_labels = {'person', 'car', 'bicycle', 'dog'}
for feature in detected_features:
    pixel = feature.pixel_coords
    label = semantic_map[pixel.y, pixel.x]
    if label in dynamic_labels:
        feature.ignore = True  # exclude from SLAM
```

> **Further reading**
> - [Nicholson et al., "QuadricSLAM: Dual Quadrics from Object Detections as Landmarks in Object-Oriented SLAM" (RA-L 2019)](https://arxiv.org/abs/1804.04011) — Representative paper for object-level SLAM.
> - [ConceptGraphs (arXiv:2309.16650)](https://arxiv.org/abs/2309.16650) — Open-vocabulary 3D scene graph. Read alongside Chapter 13.
> - [Bescos et al., "DynaSLAM: Tracking, Mapping and Inpainting in Dynamic Scenes" (RA-L 2018)](https://arxiv.org/abs/1806.05620) — SLAM in dynamic environments.

### 14.13 Advanced: Multi-Robot SLAM

Having one robot explore a large environment takes a long time. Multiple robots exploring in parallel can cut the time, but merging each robot's partial map (submap) into one consistent global map is not trivial.

The **centralized approach** sends sensor data or local maps from several robots to a server for joint optimization. Global information is easier to access, but communication and server compute can become bottlenecks, and the server is a single point of failure. Centralization itself does not guarantee a global optimum for non-convex SLAM.

The **distributed approach** has each robot run local SLAM and create relative-pose constraints at rendezvous or inter-robot loop closures. Exchanging descriptors or submaps rather than raw data can reduce communication, but may also remove evidence needed for verification. Which variables and factors each robot stores and exchanges, and when asynchronous optimization converges, depend on the algorithm.

A distributed system must address **inter-robot loop closure**, **coordinate-frame alignment**, and **outlier rejection** together. Relative SE(3) may arrive as one verified 6-DoF pose constraint; when estimated from 3D point correspondences, it needs at least three non-degenerate points and enough inliers. PCM, GNC, distributed Gauss-Seidel, and ADMM make different assumptions and communication tradeoffs, so selection depends on the system.

**Representative systems**:
| System | Characteristics |
|---|---|
| **Kimera-Multi** | Distributed, 3D mesh + semantic, Kimera based |
| **DOOR-SLAM** | Distributed, outlier-robust, DGS optimization |
| **Swarm-SLAM** | ROS2 based, supports diverse sensors, lightweight |

> **Further reading**
> - [Lajoie et al., "DOOR-SLAM: Distributed, Online, and Outlier Resilient SLAM for Robotic Teams" (RA-L 2020)](https://arxiv.org/abs/1909.12198) — Distributed SLAM + robust optimization.
> - [Tian et al., "Kimera-Multi: Robust, Distributed, Dense Metric-Semantic SLAM for Multi-Robot Systems" (T-RO 2022)](https://arxiv.org/abs/2106.14386) — Multi-robot semantic SLAM.
> - [Cieslewski et al., "Data-Efficient Decentralized Visual SLAM" (ICRA 2018)](https://arxiv.org/abs/1710.05772) — Early work on communication-efficient distributed SLAM.

### 14.14 Advanced: Place Recognition

The core question of loop closure: "have I seen this scene before?" This is an image retrieval problem. The descriptor of the current frame is compared against the descriptors of all past keyframes and the most similar one is found. The accuracy of SLAM depends on loop closure, and loop closure depends on place recognition.

**Classical approach: Bag of Visual Words (BoVW)**

The DBoW2 library is representative and is used in ORB-SLAM2/3.
1. Extract local features (ORB, etc.) from a large image set
2. Build a visual vocabulary (word dictionary) with k-means clustering
3. Represent each image as a histogram (BoW vector) of "how often each visual word appears"
4. Compare images by similarity between BoW vectors (L1-score, etc.)

Strengths: fast (via an inverted index) and well-tested. Weaknesses: fragile against viewpoint/illumination changes, and the vocabulary needs training.

**Learning-based approach: global descriptors**

This approach compresses a whole image into a single compact vector. **NetVLAD** (2016) combined CNN features with VLAD aggregation and reported higher recall than its comparison methods on the paper's city-scale benchmarks. **CosPlace** (2022) and **MixVPR** (2023) evaluated changes to descriptor learning and aggregation under their respective datasets and protocols. **AnyLoc** (2023) used DINOv2 features and reported results without place-recognition fine-tuning across several indoor, outdoor, and aerial datasets. None of these results guarantees greater robustness than BoVW in every environment; measure recall and false positives in the target domain.

**LiDAR-based place recognition**:

Recognize places from 3D structure alone, so the descriptor does not directly depend on image illumination. Weather and dynamic objects can still change point returns, and structurally similar environments such as long corridors can be confused.

- **Scan Context** (IROS 2018): Projects a 3D point cloud into a bird-eye view and generates a 2D descriptor based on range/height. Supports rotation-invariant matching.
- **OverlapTransformer** (2022): Learns a global descriptor on LiDAR range images with a Transformer.

**Cross-modal place recognition**: Query with a camera image and retrieve from a LiDAR map, or vice versa. Important for multi-robot SLAM across robots with different sensors.

**Sequence matching**: To overcome the limits of single-image matching, match sequences of consecutive frames together.
- **SeqSLAM** (2012): Individual image similarities can be low, but if the sequence pattern matches, the system declares it the same place. Works even under dramatic appearance changes (day vs night).
- Recent methods: learn a sequence descriptor for more efficient sequence matching.

Practical tip: DBoW2 is a public baseline used by the ORB-SLAM family. Under large illumination or seasonal changes, compare learned descriptors after NetVLAD on the same target data. AnyLoc constructs features without task-specific fine-tuning, but backbone compute, descriptor memory, and target-domain recall still need validation.

The cycle-posterior method in §14.16.5 also detects a place-consistency event and corrects past trajectory estimates. This is a functional analogy, not evidence that modern loop closure directly descends from that algorithm.

> **Further reading**
> - [Arandjelovic et al., "NetVLAD: CNN architecture for weakly supervised place recognition" (arXiv:1511.07247)](https://arxiv.org/abs/1511.07247) — Starting point of learning-based place recognition.
> - [Keetha et al., "AnyLoc: Towards Universal Visual Place Recognition" (arXiv:2308.00688)](https://arxiv.org/abs/2308.00688) — Foundation model-based zero-shot place recognition.
> - [Kim & Kim, "Scan Context: Egocentric Spatial Descriptor for Place Recognition within 3D Point Cloud Map" (IROS 2018)](https://ieeexplore.ieee.org/document/8593953) — Representative method for LiDAR place recognition.
> - [Giseop Kim's blog — Scan Context-based LiDAR Pose-graph SLAM implementation](https://gisbi-kim.github.io/blog/2021/05/17/sclidarslam.html) — A walk-through of integrating Scan Context into LiDAR SLAM.
> - [Dark Programmer — Bag of Words technique](https://darkpgmr.tistory.com/125) — Explains the principles of BoW by connecting it to image retrieval.

> **Technical Timeline: SLAM & Odometry**
> - **~2007**: The classical era. EKF-SLAM and FastSLAM (particle-filter based) dominated. MonoSLAM (2007) announced the arrival of real-time monocular SLAM. PTAM (2007) proposed the tracking/mapping split architecture.
> - **2010–2015**: direct methods such as LSD-SLAM and SVO emerged. LOAM (2014) presented an influential LiDAR odometry-and-mapping structure, and ORB-SLAM (2015) provided a public feature-based visual-SLAM baseline.
> - **2015–2020**: public VIO implementations and applications expanded around VINS-Mono, the MSCKF family, and others. DSO presented direct sparse odometry, while LiDAR-inertial systems such as LIO-SAM appeared.
> - **2020–2023**: public systems including FAST-LIO/FAST-LIO2, ORB-SLAM3, DROID-SLAM, and R3LIVE combined filtering, optimization, and learning in different ways.
> - **2024–**: 3DGS-based SLAM (SplaTAM, MonoGS, Gaussian-SLAM) is changing the direction of Neural SLAM. Research combining foundation models with SLAM (for instance, finding a location by describing a place in natural language) is also beginning.
> - **Recent direction**: geometric SLAM has accumulated public implementations and benchmark results around systems such as ORB-SLAM3, LIO-SAM, and FAST-LIO2. Meanwhile, 3DGS-SLAM and learning-based methods expand the choices for scene representation and the front-end. KITTI, EuRoC, and TUM RGB-D provide common data for comparing their assumptions and failure cases.

### 14.15 Advanced: Long-term Mapping

When you operate a robot in a real environment, "build the map once and be done" is not the reality. You visit the same place multiple times, update the map, remove dynamic objects (people, vehicles), and integrate data from multiple sessions. This is long-term mapping, and it is unavoidable in practical robot systems.

#### 14.15.1 Incremental Smoothing: from iSAM to iSAM2

Filter-based SLAM (EKF, etc.) struggles with real-time processing as the Jacobian matrix grows with the number of states. iSAM (Kaess et al., TRO 2008) showed that the R matrix of the QR factorization can be updated incrementally with Givens rotations. When a new measurement is added, rather than recomputing the whole thing, only the affected part is updated. However, as non-zero elements accumulate, periodic re-ordering is needed.

iSAM2 (Kaess et al., IJRR 2012) overcame this limit by introducing the Bayes tree structure. Only the affected subtree is re-eliminated, delivering consistent performance even on large-scale problems. The Bayes tree is exactly the core engine of GTSAM.

#### 14.15.2 Dynamic Object Removal

Removing dynamic objects from the map is an essential task in long-term mapping.

**Removert** (Kim et al., 2020): Classifies static and dynamic points with multi-resolution range images. It projects a point cloud into a range image and compares the ranges with observations from other viewpoints. The two-stage design first selects static points conservatively and then restores points removed in error. Multiple confidence levels control the trade-off between the stages.

Compared with prior approaches: voxel ray-casting is accurate but expensive; visibility-based methods assume that static points behind dynamics are preserved; segmentation-based methods are weak on unknown labels and ignore the scan-to-map relationship. Removert compensates for the drawbacks of these three using multi-resolution range-image comparison.

**SuMa++** (Chen et al., IROS 2019): Adds semantic labels to surfel-based mapping. It augments LiDAR points with normals and semantic information, then removes only the surfels classified as dynamic by both semantics and motion. In motion-degenerate environments, moving points can still provide useful geometric constraints.

#### 14.15.3 Multi-Session SLAM

When you map the same environment across multiple days, the trajectories of the sessions have to be merged into one. The problem is gauge freedom — each session's coordinate frame is different, so naively merging does not align them.

**LT-mapper** (Kim et al., 2021): Aligns multiple sessions via Scan Context-based anchor nodes and updates the map with positive/negative change detection. Splits changes into high dynamic and low dynamic, and further divides low dynamic into positive difference (newly appeared points) and negative difference (vanished points), managing a delta map.

**Continuous-Time Estimation** (Furgale et al., ICRA 2012): Representing the trajectory with B-spline basis functions instead of discrete time lets you integrate sensors at different Hz with fewer variables. It is also applicable to self-calibration between fast sensors (IMU) and slow sensors (LiDAR, camera).

> **Further reading**
> - [Kaess et al., "iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree" (IJRR 2012)](https://www.cs.cmu.edu/~kaess/pub/Kaess12ijrr.pdf) — Original paper on Bayes tree-based incremental SLAM.
> - [Kim et al., "Remove, then Revert: Static Point Cloud Map Construction using Multiresolution Range Images" (IROS 2020)](https://github.com/irapkaist/removert) — Practical method for dynamic point removal. Code released.
> - [Kim et al., "LT-mapper: A Modular Framework for LiDAR-based Lifelong Mapping" (ICRA 2022)](https://github.com/gisbi-kim/lt-mapper) — Multi-session SLAM framework.
> - [Chen et al., "SuMa++: Efficient LiDAR-based Semantic SLAM" (IROS 2019)](https://github.com/PRBonn/semantic_suma) — LiDAR SLAM that exploits semantic information.

---

### 14.16 Advanced: History of Information-Form SLAM

*Cross-reference with §14.9 factor graph optimization (bidirectional).*

When *Probabilistic Robotics* was published in 2005, several representations of SLAM uncertainty competed. EKF-SLAM used a joint state and covariance, EIF/SEIF exploited information-form additivity, and EM mapping treated unknown data association statistically. Factor graphs and GTSAM/iSAM2 later carried ideas such as additivity, variable elimination, and incremental updates into a modern optimization framework.

#### 14.16.1 EKF-SLAM (PR §10)

The lineage begins with **Smith, Self, and Cheeseman (1986/1990)**, "Estimating Uncertain Spatial Relationships in Robotics." Their proposal of a "stochastic map" — bundling robot pose and landmarks into a single random variable — is the prototype of EKF-SLAM. Leonard and Durrant-Whyte in the 1990s, then Dissanayake et al. (2001, IEEE T-RA), completed the formalization.

**Algorithm skeleton**: The pose $x_t = (x, y, \theta)$ and N landmarks $(m_{j,x}, m_{j,y}, s_j)$ are packed into a $(3N+3)$-dimensional state vector $y_t$ and an EKF is run over it.

```
EKF_SLAM_known_correspondences(μ_{t-1}, Σ_{t-1}, u_t, z_t, c_t):
  // Motion: lift 3D motion into (3N+3)D via F_x
  // F_x = [I_3 | 0_{3×3N}] — (3×(3N+3)) projection, F_x^T is (3N+3)×3
  // G_t = I_{3N+3} + F_x^T G_t^{pose} F_x — (3N+3)×(3N+3), G_t^{pose} is 3×3 pose Jacobian
  μ̄_t = μ_{t-1} + F_x^T · g(u_t, μ_{t-1}[pose part])
  Σ̄_t = G_t Σ_{t-1} G_t^T + F_x^T R_t F_x

// Measurement loop
  for each observation z_t^i with j = c_t^i do
    if j is new landmark:
      μ̄_{j} ← initialize via inverse range-bearing transform
    ẑ_t^i = h(μ̄_t, j),   H_t^i = Jacobian  // H_t^i: 3×(3N+3)
    K_t^i = Σ̄_t H_t^{i,T} (H_t^i Σ̄_t H_t^{i,T} + Q_t)^{-1}
  endfor

// Update
  μ_t = μ̄_t + Σ_i K_t^i (z_t^i − ẑ_t^i)
  Σ_t = (I − Σ_i K_t^i H_t^i) Σ̄_t
  return μ_t, Σ_t
```

**The Kalman gain $K_t^i$ is a $(3N+3) \times 3$ matrix** — a single landmark observation updates the entire state. Covariance off-diagonals propagate that observation to other landmark estimates, while the update cost grows as $O(N^2)$.

With unknown correspondences, a provisional $(N_t+1)$-th landmark is temporarily appended to the map; Mahalanobis distances to all candidates are computed and the ML correspondence is selected. If the distance exceeds threshold $\alpha$, the observation is registered as a new landmark. This greedy ML decision, once wrong, cannot be undone — the fundamental weakness of ML data association.

EKF-SLAM's limits appeared on three axes. The covariance matrix $\Sigma \in \mathbb{R}^{(3N+3) \times (3N+3)}$ requires memory proportional to $N^2$ — 100 landmarks means a 303×303 matrix, 1000 landmarks means 3003×3003. As landmarks accumulate, past linearization error compounds and estimation becomes inconsistent (Bailey et al. 2006). Because past poses are marginalized out, full posterior optimization over them is impossible.

For large landmark maps, dense covariance and accumulated linearization error make smoothing and factor-graph formulations more common than classical full-state EKF-SLAM. There is no universal landmark-count boundary below which EKF-SLAM is valid. It remains usable for small landmark or fiducial-mapping problems when compute, observation sparsity, and consistency requirements permit. Sliding-window filters such as MSCKF use EKF linearization and covariance propagation but do not keep landmarks permanently in the state. JCBB is an alternative to greedy ML association. GTSAM and iSAM2 solve the same broad SLAM problem through smoothing and factor graphs; they are a later formulation, not direct algorithmic descendants of EKF-SLAM.

#### 14.16.2 GraphSLAM — Information-Form Batch SLAM (PR §11)

In EKF-SLAM, $\Sigma$ carries dense correlations that must be updated with measurements. In the information form $\Omega = \Sigma^{-1}$, contributions from independent factors can be added locally, making graph sparsity easier to expose. Motion updates and marginalization can still create fill-in, so the information matrix does not stay sparse automatically. This motivates EIF and sparse graph formulations.

##### The intuition of information form: spring-mass analogy

The central idea of EIF SLAM is that **information is additive**. Instead of covariance $\Sigma$, information matrix $\Omega = \Sigma^{-1}$ and information vector $\xi = \Omega \mu$ are used.

Viewed as a spring-mass system: each variable (pose, landmark) is a node; off-diagonal elements of $\Omega$ are springs connecting two nodes.
- Control $u_t$: a spring between $x_{t-1}$ and $x_t$. Stiffness = $R_t^{-1}$ (stronger coupling when motion noise is smaller).
- Measurement $z_t^i$: a spring between pose $x_t$ and landmark $m_j$. Stiffness = $Q_t^{-1}$.
- No direct spring between two different landmarks — they have never been observed relative to each other.

**Information-form update rule**:
$$\Omega \leftarrow \Omega + H_t^{iT} Q_t^{-1} H_t^i, \qquad \xi \leftarrow \xi + H_t^{iT} Q_t^{-1}[z_t^i - h(\mu_t) + H_t^i \mu_t]$$

Measurement information can be introduced by **local addition**. Motion updates and marginalization can still require elimination and create fill-in, so not every operation is local. A factor graph supports a similar spring analogy because each factor connects a subset of variables, but the history and formulation of factor graphs do not begin with SEIF.

##### Four-step pipeline

GraphSLAM processes the full posterior $p(x_{0:t}, m | z_{1:t}, u_{1:t})$ offline in batch, in information form. EIF and SEIF share that information form, but they are online filters that marginalize past states, so their objective and the states they retain differ.

```
GraphSLAM_known_correspondence(u_{1:t}, z_{1:t}, c_{1:t}):
  1. Initialize:  μ_{0:t} ← initial estimate from motion model alone (ignore observations)
  2. Construct:   starting from Ω = 0, ξ = 0,
                  accumulate prior, controls, and measurements by local addition
  3. Reduce:      for each landmark j, eliminate via Schur complement
                  Ω̄ ← Ω̄ − Ω_{τ(j),j} Ω_{j,j}^{-1} Ω_{j,τ(j)}
                  ξ̄ ← ξ̄ − Ω_{τ(j),j} Ω_{j,j}^{-1} ξ_j
                  → reduced Ω̄, ξ̄ with poses only
  4. Solve:       Σ_{0:t} = Ω̄^{-1},  μ_{0:t} = Σ_{0:t} ξ̄
                  each landmark: μ_j = Ω_{j,j}^{-1}(ξ_j − Ω_{j,τ(j)} μ_{τ(j)})
  iterate 2-3 times total (to improve linearization)
  return μ_{0:t}, {μ_j}
```

$\tau(j)$ denotes all pose time steps at which landmark $j$ was observed. The Reduce step *creates new springs between poses adjacent to each landmark and then detaches the landmark node*. This is mathematically identical to the **block diagonal Schur complement** trick in Bundle Adjustment (see §14.9.2).

**Marginalization Lemma**: In a linear Gaussian information form, a marginal is expressed through a Schur complement. The operation can create fill-in among the variables that remain.
$$\bar\Omega_{xx} = \Omega_{xx} - \Omega_{xy} \Omega_{yy}^{-1} \Omega_{yx}$$

GraphSLAM (Thrun and Montemerlo, 2006, IJRR) batch-optimizes a full trajectory and map posterior as a sparse information graph. It shares information-form additivity with EIF, but online filtering and batch smoothing should be distinguished. Lu and Milios (1997) were earlier work on global optimization of pose relations.

One unknown-correspondence procedure for GraphSLAM evaluates whether feature pairs $(m_j, m_k)$ denote the same object, adds selected constraints to the graph, and reoptimizes. When factors are retained explicitly, a selected constraint can be removed before reoptimization. Switchable constraints (Sünderhauf & Protzel 2012) are a separate robust formulation that jointly optimizes a continuous activation variable for a loop factor; they should not be treated as a direct lineage claim.

The sequence initialize → construct factors → eliminate variables → solve can be compared with modern batch SLAM. iSAM and iSAM2 perform incremental smoothing by updating the parts of linearization and elimination affected by new factors, while the Bayes tree organizes that factorization. This is more than a simple renaming or linear evolution of GraphSLAM.

#### 14.16.3 SEIF — Sparse Extended Information Filter (PR §12)

The GraphSLAM formulation above is batch smoothing. A general EIF can also be used online, but an exact motion update can densify its information matrix. **SEIF** limits the active-feature set and introduces a sparsification approximation to target map-size-independent online updates. On its Victoria Park 3.5 km experiment, Thrun et al. (2004, IJRR) report similar error to their EKF-SLAM implementation at roughly half the time and one-quarter the memory.

##### Four-step update

```
SEIF_SLAM_known_correspondences(ξ_{t-1}, Ω_{t-1}, μ_{t-1}, u_t, z_t, c_t):
  1. Motion update:       ξ̄_t, Ω̄_t, μ̄_t ← update in information form using u_t
                          (only active features + robot pose change; sparsity preserved)
  2. Measurement update:  Ω_t ← Ω̄_t + Σ_i H_t^{iT} Q_t^{-1} H_t^i  [additive]
                          ξ_t ← ξ̄_t + corresponding additive term
  3. Sparsification:      force some active features to passive
                          — sever link to robot and redistribute information to neighboring nodes
  4. State estimate:      update active feature estimates only, via amortized coordinate descent
  return ξ_t, Ω_t, μ_t
```

##### Sparsification

This is the core mechanism. The direct dependency between variables $a, b$ is approximated by the product of two marginals, creating a zero element in $\Omega$.

$$\tilde p(a,b,c) = \frac{p(a,c)\, p(b,c)}{p(c)} \quad \Longrightarrow \quad \Omega_{a,b} = 0$$

This approximation can be described as a KL projection enforcing $a \perp b | c$. The effect of sparsification and repeated linearization on filter consistency must still be evaluated; it cannot be summarized as a universal guarantee that variance never decreases.

Fixing the active feature count $K$ bounds the core local matrix in the motion and measurement updates at $(2K+3) \times (2K+3)$. This supports constant update complexity with respect to map size, but does not automatically make full-map state recovery or data association O(1).

The example in *Probabilistic Robotics* uses about six active features. This is not a universal recommendation; consistency and compute must be checked for the sensor geometry and map. Eustice et al. (2006), "Exactly Sparse EIF," studies how to avoid the sparsification approximation under particular structures. PR Figure 12.3 shows how links change across measurement, motion, and sparsification.

##### Tree-based data association

The additivity of information form gives a special capability in data association: **soft correspondence constraints can be added or subtracted**. A soft constraint that features $m_i$ and $m_j$ are the same object is added as

$$\Omega \leftarrow \Omega + F_{m_i - m_j}^T C\, F_{m_i - m_j}$$

and can be removed if that factor is retained separately. This structure permits an A*-type frontier search over a data-association tree, but the number of hypotheses is exponential in the worst case. Switchable constraints (Sünderhauf & Protzel 2012) and Max-mixtures (Olson & Agarwal 2013) share the goal of robustness to false loop closures, but use separate formulations based on switch variables and mixture factors.

##### Multi-robot map fusion

Information form makes the contribution of independent observation factors easy to add in multi-robot fusion. After expressing two robots' states in a common frame, their information terms can be added only when their priors and observations do not contain duplicated information.

$$\Omega^{\text{fused}} = \Omega^{j \leftarrow k\text{-aligned}} + \Omega^k, \qquad \xi^{\text{fused}} = \xi^{j \leftarrow k\text{-aligned}} + \xi^k$$

Adding without tracking common information double-counts observations and produces over-confidence. Covariance-form estimates also cannot be fused by simple addition, although conservative methods such as covariance intersection exist. Nettleton et al. (2003) study distributed information fusion; DDF-SAM, Kimera-Multi, and Swarm-SLAM also address multi-robot estimation but use different communication and common-information assumptions (see §14.13).

SEIF and iSAM2 address different settings: approximate information filtering versus incremental smoothing. For nonlinear problems, iSAM2 is also affected by its linearization point and relinearization policy. ESEIF belongs to the SEIF family, whereas sliding-window marginalization in VINS-Mono and OKVIS and feature elimination in MSCKF are separate ways to bound state and computation. Their common theme is managing sparsity and compute, not a direct chain of inheritance (see §14.9.2).

#### 14.16.4 EM Mapping

SEIF is information-form additivity plus sparsification. The line of work that treats ambiguous data association statistically, however, came earlier: MHT and JPDA established multiple hypotheses and probabilistic association, and FastSLAM carried an association per particle. EM Mapping is the member of that family that alternates between estimating the map and resolving the association.

EKF-SLAM, EIF SLAM, and SEIF all assumed that data association was either known or decided greedily by ML. EM Mapping **treats unknown data association as an EM latent variable, exploiting ambiguous data instead of discarding it.** The prototype is from Thrun, Burgard, and Fox (1998–2000, AAAI/JAIR); a variant was used in the RHINO museum guide robot (Burgard et al. 1999).

##### E-step / M-step skeleton

```
EM_mapping(d):
  m ← initialize uniform map
  repeat until satisfied:
    // E-step (forward α)
    α^(0) = δ(⟨0,0,0⟩)
    for t = 1 to T:
      α^(t) = η P(o^(t)|s^(t),m) ∫ P(s^(t)|a^(t-1),s^(t-1)) α^(t-1) ds^(t-1)

// E-step (backward β)
    β^(T) = uniform
    for t = T-1 downto 0:
      β^(t) = ∫ P(o^(t+1)|s^(t+1),m) P(s^(t+1)|a^(t),s^(t)) β^(t+1) ds^(t+1)

// E-step (combine)
    Bel(s^(t)) = α^(t) · β^(t)   [normalize]

// M-step
    for each cell ⟨x,y⟩, property l:
      m_{⟨x,y⟩=l} ∝ Σ_t ∫ P(o^(t)|s^(t),m_{⟨x,y⟩}=l) · I_{⟨x,y⟩ ∈ range} · Bel(s^(t)) ds^(t)
    normalize
  return m
```

$\alpha$ is forward localization (Markov localization); $\beta$ is backward (correcting past belief using future data). The $\beta$ term lets past belief be corrected backwards when a loop closes — the statistical core of EM mapping. The forward-backward structure is identical to the Baum-Welch algorithm for HMMs.

The M-step is a frequentist count: "number of times the cell was observed as property l / total observations of anything," weighted by belief. Convergence typically takes 3–5 iterations.

##### Layered EM Mapping

A variant that fixes a problem in the basic EM_mapping M-step, where geometric consistency within the sensor cone is broken. A **local occupancy grid** is built from each short motion segment first; EM then optimizes only the *position* of those local maps. **Deterministic annealing** ($\sigma: 1.0 \to 0$ cooling) prevents EM from getting trapped in local maxima.

```
layered_EM_mapping(d):
  1. for each t: m^(t) = occupancy_grid(o^(t))  [local map construction]
     Bel(s^(t)) ← uniform initialization
  2. repeat until satisfied  [σ = 1.0 → 0]:
     E-step (α, β)  [using layered perceptual model]
     M-step (annealed): Bel(s^(t)) = η (α^(t) β^(t))^{1/σ}
     σ ← 0.9σ
  3. extract ML pose of each local map → compose global map via occupancy_grid()
  return m_global
```

Deterministic annealing, GNC (Yang et al. 2020), and robust-kernel scheduling share a design principle: increase the difficulty of the objective gradually to avoid poor local optima.

##### Why EM Mapping Left the Mainstream

EM_mapping and layered_EM_mapping are not mainstream components of modern SLAM systems. Alternating pose and map through E/M-steps became less attractive as joint optimization with factor graphs became practical; their batch/offline character also fits real-time SLAM poorly. Cartographer (Hess et al. 2016), for example, solves the problem directly with scan matching and pose graph optimization, without EM. GMapping (Grisetti et al. 2007) likewise avoids EM but is structured differently: it is a Rao-Blackwellized particle filter that improves the proposal distribution with scan matching, and it uses no pose graph.

Layered EM and Cartographer's local/global SLAM do share a **submap + global alignment** pattern: construct local maps first, then align them globally. This is a structural analogy, not evidence of a direct historical lineage.

#### 14.16.5 Cycle Posterior

The stepwise ML mapper has two limits: it cannot handle large odometry errors, and it cannot correct past poses backward in time. The cycle posterior approach runs a pose posterior estimator in parallel with the ML mapper to fix both.

**Algorithm skeleton**:
```
Incremental Mapping with Posterior Estimation:
  1. incremental_ML_mapping(o, a, s, m)  → ⟨m', s'⟩    [ML update]
  2. Bel(s') = P(o,s') ∫ P(s'|a,s) Bel(s) ds            [posterior one step]
  3. s'' = argmax Bel(s')                                 [posterior mode]
  4. s'' ≠ s'  →  cycle closure detected
                   distribute s'' − s' linearly along the cycle path
  5. run incremental_ML_mapping backwards in time          [nested ML refinement]
```

A sudden narrowing of the posterior signals cycle closure, and the difference between the narrowed mode and the ML estimate is the correction signal. Because two estimators (ML mapper + posterior estimator) run simultaneously, an MCL-based implementation is natural. The algorithm is designed to operate without odometry.

This is an early example that combines explicit cycle detection with backward correction in one framework, so it is useful to compare with later loop-closure systems. It does not, however, establish that modern place recognition and incremental graph optimization descended directly from this algorithm.

The particular combination of MCL, linear distribution, and nested ML is not widely used now. A similar division of labor does remain common: a detector proposes a revisited place, and graph optimization corrects the state after geometric verification. iSAM (Kaess et al. 2008), iSAM2 (2012), and GTSAM's incremental smoothing update the parts of the problem affected by new constraints.

#### 14.16.6 Summary: What Survived

The information-form SLAM methods in *Probabilistic Robotics* (2005) contributed several ideas that remain visible in current systems:

| PR algorithm | Core contribution | Comparable modern structure | Scope of the relationship |
|---|---|---|---|
| EKF-SLAM | unified landmark state, off-diagonal covariance | MSCKF-family filters, fiducial mapping | shares EKF machinery but not the same state design |
| GraphSLAM | additive information factors, variable elimination, full trajectory | GTSAM, g2o, Ceres, iSAM2 | sparse least squares and smoothing viewpoints remain relevant |
| SEIF | bounded active set, sparsification | ESEIF, bounded-state estimators | distinguish the SEIF family from other sparse estimators |
| EM Mapping | latent association, forward-backward localization, submaps | EM-based mapping, submap systems | a statistical or structural comparison, not direct lineage |
| Cycle Posterior | combined online closure detection and correction | place recognition + graph optimization | similar functional separation |

Information-form **additivity** can be compared directly with the sum of modern factor costs. SEIF **sparsification**, sliding-window marginalization, and Bayes-tree elimination all manage computation but are different operations. The detector/corrector division in cycle posterior and the submap organization in EM mapping admit structural comparisons with modern systems; they should not be presented as direct lineage claims (see §14.9).

> **Further reading**
> - [Thrun et al., "Simultaneous Localization and Mapping with Sparse Extended Information Filters" (IJRR 2004)](https://journals.sagepub.com/doi/10.1177/0278364904045026) — Original SEIF paper.
> - [Thrun & Montemerlo, "The GraphSLAM Algorithm with Applications to Large-Scale Mapping of Urban Structures" (IJRR 2006)](https://journals.sagepub.com/doi/10.1177/0278364906065390) — EIF/GraphSLAM formalization.
> - [Dissanayake et al., "A Solution to the Simultaneous Localization and Map Building (SLAM) Problem" (IEEE T-RA 2001)](https://ieeexplore.ieee.org/document/938381) — Classic EKF-SLAM formalization.
> - [Kaess et al., "iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree" (IJRR 2012)](https://www.cs.cmu.edu/~kaess/pub/Kaess12ijrr.pdf) — Original paper on incremental smoothing and the Bayes tree.

---

In service-robot localization, the choice of method depends in part on how the belief is represented, while a pose-graph estimate requires a separate map-generation process to produce the final map. The rise of factor graphs as a widely used formulation can be understood along the same line. EKF localization also remains a viable choice for ceiling-marker systems and platforms with limited compute. Their coexistence shows that convergence on one architecture does not answer every operating condition.

---

# Ch.15 — Robot Frameworks

Robot software must run sensor drivers, path planning, and motor control concurrently while managing communication among them. A framework supplies shared functions such as thread management, message serialization, and coordinate transforms. This chapter begins with ROS communication and package structure, then moves to simulators and related tools.

## 15.1 ROS (Robot Operating System)

ROS is an open-source framework for robot software development. It is not an operating system but **middleware**, providing inter-process communication, package management, and tooling.

A robot runs modules such as cameras, LiDAR, motors, and controllers concurrently. ROS standardizes their communication and message formats so that each function can be developed as a separate node.

### 15.1.1 ROS1 vs ROS2

| Feature | ROS1 | ROS2 |
| --- | --- | --- |
| First public release | 2010 (ROS 1.0 Box Turtle; development began 2007) | 2017 (Ardent Apalone) |
| Communication | Custom (TCPROS) | DDS-based |
| Real-time | Not supported | Supported |
| Security | None | SROS2 |
| Multi-robot | Difficult | Easy |
| Master | Required (roscore) | Not required |
| Python | 2/3 | 3 only |

**Current recommendation**: For a new long-lived project, evaluate ROS2 Jazzy LTS first. Projects tied to Ubuntu 22.04 or existing packages may still remain on Humble.

Choose only after checking the supported ROS distribution, Ubuntu version, and EOL date of the required drivers and packages. Official support for ROS1 Noetic ended in May 2025.

For a new project, start with ROS2 unless there is a specific reason not to. If an existing ROS1 package is required, evaluate `ros1_bridge` only after checking the supported ROS1, ROS2, and Ubuntu combination. Nav2 and MoveIt 2 are available for ROS2.

> **Further reading**
> - [ROS2 Official Tutorials](https://docs.ros.org/en/jazzy/Tutorials.html) — Official step-by-step guide for ROS2 Jazzy LTS. If it is your first time, start from "Beginner: CLI tools"
> - [The Construct - ROS2 Basics](https://www.youtube.com/@TheConstruct) — ROS-focused education channel. Hands-on inside a simulator
> - [ROS1 to ROS2 Migration Guide](https://docs.ros.org/en/jazzy/How-To-Guides/Migrating-from-ROS1.html) — Official guide for porting existing ROS1 code

### 15.1.2 Core Concepts

Topics, services, and actions differ in timing and response behavior. They represent sensor streams, short request-response operations, and long-running tasks that expose intermediate state, respectively.

**Node**:
- A functional unit that participates in the graph. In ROS 1 a node was effectively one process; in ROS 2 nodes can be built as components so that several of them live in a single process
- Single-purpose (sensor driver, controller, etc.)

**Topic**:
- Asynchronous message stream
- Publisher/subscriber pattern
- Examples: sensor data, commands

```python
# ROS2 Publisher example
import rclpy
from rclpy.node import Node
from std_msgs.msg import String

def timer_callback(self):
        msg = String()
        msg.data = 'Hello, World!'
        self.publisher_.publish(msg)
```

**Service**:
- A request/response interface. The ROS 2 client APIs are asynchronous by default; blocking on a call synchronously from the thread that runs the executor (i.e., inside a callback) deadlocks
- Suited to one-shot operations
- Examples: changing settings, querying state

**Action**:
- Asynchronous goal-directed task
- Provides feedback
- Cancelable
- Examples: navigation, manipulation

Use a Topic for continuous data such as camera images. A Service fits a single request and response, such as "tell me the current battery level," whereas an Action fits a task that takes time, such as "go over there." Distinguishing the three leads to a clearer communication design.

**Parameter**:
- Node configuration values
- Changeable at runtime

> **⚠ Generated-code check**: Include the sensor topic's QoS when requesting ROS2 code. Verify that the generated subscriber's reliability and durability are compatible with the publisher's; an incompatible pair can prevent delivery without an obvious error message. The requirement is compatibility, not equality. A RELIABLE publisher connects to a BEST_EFFORT subscriber but not the reverse, and for durability a TRANSIENT_LOCAL publisher connects to a VOLATILE subscriber but not the reverse.

> **Further reading**
> - [ROS2 Concepts — Understanding nodes, topics, services, actions](https://docs.ros.org/en/humble/Concepts.html) — Official concepts document
> - [The Construct - ROS2 Topics vs Services vs Actions](https://www.youtube.com/@TheConstruct) — Video comparing the three communication patterns

### 15.1.3 Tools

When developing a robot, the "just write code and throw it on the robot" approach is dangerous. You need to be able to see with your own eyes whether sensor data is arriving properly and whether coordinate frames line up; that is what cuts debugging time. The tools below are daily essentials for any ROS developer.

**rviz / rviz2**:
- 3D visualization tool
- Displays sensor data, TF, paths, etc.

**rqt**:
- Qt-based collection of GUI tools
- rqt_graph: visualizes node/topic relationships
- rqt_plot: plots data

**rosbag / ros2 bag**:
- Records and replays data
- Essential for debugging and algorithm development

Knowing these cuts experiment time substantially. Testing algorithms by running the real robot every time is costly in both time and money. Record once with rosbag and you can repeat experiments on the same data as many times as you want. In terms of reproducibility, it is an essential tool.

```bash
# Record a ROS2 bag
ros2 bag record -a -o my_bag

# Replay
ros2 bag play my_bag
```

**tf2 (Transform Library)**:
- Manages coordinate frame transforms
- Tracks transforms over time

A robot has camera, LiDAR, base, and world coordinate frames all existing at the same time. Computing "where is this point seen from the camera in the robot's frame?" requires coordinate transforms, and tf2 manages them automatically. If you have studied linear algebra, think of it as SE(3) transformation matrices.

```python
# tf2 listener example
import rclpy
from rclpy.node import Node
from tf2_ros import Buffer, TransformListener, LookupException, ExtrapolationException

class TfDemo(Node):
    def __init__(self):
        super().__init__('tf_demo')
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)
        # Look up inside a timer so the listener has time to fill the buffer
        self.create_timer(0.1, self.lookup)

def lookup(self):
        try:
            # Look up the transform from camera_link coordinates to base_link coordinates
            t = self.tf_buffer.lookup_transform('base_link', 'camera_link', rclpy.time.Time())
        except (LookupException, ExtrapolationException) as e:
            self.get_logger().warn(f'tf not ready: {e}')
```

> **Further reading**
> - [ROS2 tf2 Tutorials](https://docs.ros.org/en/humble/Tutorials/Intermediate/Tf2/Tf2-Main.html) — Official coordinate transform tutorial
> - [The Construct channel](https://www.youtube.com/@TheConstruct) — ROS2 lecture channel, including videos on using rviz2
> - [ros2 bag CLI documentation](https://docs.ros.org/en/humble/Tutorials/Beginner-CLI-Tools/Recording-And-Playing-Back-Data/Recording-And-Playing-Back-Data.html) — Official guide for recording and replaying data

### 15.1.4 Key Packages

The packages below provide standard messages, image and point-cloud conversion, and navigation functions.

| Package | Purpose |
| --- | --- |
| sensor_msgs | Sensor message types |
| geometry_msgs | Geometry messages (Pose, Twist, etc.) |
| cv_bridge | OpenCV ↔︎ ROS image conversion |
| image_transport | Compressed image transport |
| pcl_ros | PCL ↔︎ ROS point cloud |
| nav2 | Navigation stack (ROS2) |

> **Further reading**
> - [Nav2 Documentation](https://docs.nav2.org/) — Official documentation for the ROS2 Navigation stack
> - [ROS2 Package Index](https://index.ros.org/packages/) — ROS2 package search

## 15.2 Simulation

Experimenting directly on a real robot can damage the hardware or injure people. A simulator provides a place to check motion limits and failure conditions before hardware tests, and to run the many repeated episodes required by methods such as reinforcement learning.

Recently, as **embodied AI** research has expanded, the role of simulators in which robots learn autonomously within virtual environments has grown as well. Platforms like NVIDIA Isaac Sim, AI2-THOR, and Habitat are leading this trend, and sim-to-real transfer, moving policies learned in simulation onto real robots, is a central research topic.

### 15.2.1 Gazebo

Gazebo integrates with ROS, and many ROS packages provide Gazebo simulation demos and robot models.

**Components**:
- **SDF (Simulation Description Format)**: environment definition
- **URDF (Unified Robot Description Format)**: robot model

**Gazebo Classic vs Gazebo Sim (Ignition)**:
- Gazebo Sim: the newer version, recommended for ROS2
- Modular architecture with better extensibility

```xml

<robot name="my_robot">
  <link name="base_link">
    <visual>
      <geometry>
        <box size="0.5 0.3 0.1"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <box size="0.5 0.3 0.1"/>
      </geometry>
    </collision>
  </link>
</robot>
```

> **Further reading**
> - [Gazebo Sim Official Tutorials](https://gazebosim.org/docs) — Official guide for Gazebo Sim (formerly Ignition)
> - [URDF Tutorial (ROS2)](https://docs.ros.org/en/humble/Tutorials/Intermediate/URDF/URDF-Main.html) — Robot modeling basics
> - [The Construct - Gazebo Sim with ROS2](https://www.youtube.com/@TheConstruct) — Hands-on video of Gazebo + ROS2

### 15.2.2 NVIDIA Isaac Sim

Isaac Sim is widely used in embodied AI research for large-scale synthetic-data generation and sim-to-real training. It combines RTX rendering with the PhysX 5 physics engine, generates synthetic data through domain randomization, integrates with ROS 2, and is used mainly for manipulation research.

**Embodied AI simulator comparison**: Besides Isaac Sim, several simulators are widely used in embodied AI research.

| Simulator | Primary use | Features |
| --- | --- | --- |
| NVIDIA Isaac Sim | General purpose (Manipulation, Navigation) | RTX rendering, PhysX 5, large-scale synthetic data |
| AI2-THOR | Indoor navigation, object interaction | 120+ indoor scenes, realistic interaction |
| Habitat (Meta) | Visual navigation, embodied QA | Ultra-fast rendering (thousands of FPS), large-scale training |
| iGibson | Indoor robot tasks | Physically-based rendering, home environments |
| MuJoCo | Robot control, reinforcement learning | Accurate contact dynamics, fast simulation |

> **Further reading**
> - [NVIDIA Isaac Sim Official Documentation](https://docs.omniverse.nvidia.com/isaacsim/latest/index.html) — From installation to advanced usage
> - [AI2-THOR Documentation](https://ai2thor.allenai.org/ithor/documentation) — Indoor simulator for embodied AI research
> - [Habitat Documentation](https://aihabitat.org/docs/habitat2/) — Meta's embodied AI platform

### 15.2.3 CARLA

Autonomous driving papers very often use CARLA as their experimental environment. If you want to work on autonomous driving research, it is worth learning how to use CARLA.

**Features**:
- Urban environment simulation
- Various weather and time-of-day conditions
- Sensor simulation (camera, LiDAR, radar)
- ROS bridge provided

> **Further reading**
> - [CARLA Documentation](https://carla.readthedocs.io/) — Official documentation and Python API reference
> - [CARLA Simulator YouTube](https://www.youtube.com/channel/UC1llP9ekCwt8nEJzMJBQekg) — Demo videos of simulator usage

## 15.3 Other Frameworks

Besides ROS and simulators, there are frameworks and libraries specialized for particular purposes. Being able to pull these off the shelf instead of building them yourself is one of the advantages of the robotics ecosystem.

**Isaac ROS**:
- NVIDIA GPU-accelerated ROS packages
- DNN inference, Visual SLAM, 3D perception
- Optimized for Jetson

**Autoware**:
- Complete autonomous driving stack
- Includes perception, planning, and control
- ROS2-based (Autoware.Universe)

**Tools outside ROS**:
- **OpenCV**: computer vision
- **Open3D**: 3D processing/visualization
- **Eigen**: linear algebra (C++)
- **Sophus**: SE(3), SO(3) operations

For example, Eigen and Sophus are core libraries in robotics for handling coordinate transforms. If you learned linear algebra in class, think of Eigen as a C++ implementation of those matrix operations. Sophus builds on top of it, adding convenient handling of rotations (SO(3)) and rigid-body transforms (SE(3)).

> **Further reading**
> - [OpenCV Tutorials](https://docs.opencv.org/4.x/d9/df8/tutorial_root.html) — Computer vision from basics to advanced
> - [Open3D Documentation](http://www.open3d.org/docs/) — Official documentation for the 3D data processing library
> - [Eigen Getting Started](https://eigen.tuxfamily.org/dox/GettingStarted.html) — Introduction to the C++ linear algebra library

## 15.4 Advanced: System Design

**15.4.1 Latency Budgeting**
- Allocate the latency of the full pipeline segment by segment
- Example: autonomous driving — sensor input (10 ms) → perception (50 ms) → planning (30 ms) → control (10 ms) = 100 ms total
- The per-segment figures are design targets that divide up the end-to-end deadline. Unless each segment enforces its own deadline, slack in one segment can absorb an overrun in another. For throughput, the slowest segment is the bottleneck
- Profiling methods: ROS2 callback duration, `ros2 topic delay`, tracing (ros2_tracing)

**15.4.2 Behavior Tree**
- A robot behavior design method with better extensibility than finite state machines (FSM)
- Node types: Sequence, Fallback, Action, Condition
- Advantage: modular — subtrees can be tested and reused independently
- In ROS2: BehaviorTree.CPP, used in Nav2
- As states grow, FSM transitions blow up exponentially. BT manages complexity via a tree structure

**15.4.3 Safety and Failsafe**
- Watchdog timer: safe stop if no heartbeat within a given time
- E-stop (Emergency Stop): hardware-level power cutoff
- Software safety: speed limits, workspace limits, collision checks
- ISO 13482: service robot safety standard (overview only)
- In practice: when deploying a new algorithm, build the safety wrapper first and experiment inside it

**15.4.4 Deployment and Field Testing**
- CI/CD: automated colcon build + test, Docker image builds
- Log replay testing: test new code while replaying recorded sensor data
- Hardware-in-the-Loop (HIL): test with the real hardware in a closed loop while the plant side is simulated in real time
- Field test protocol: controlled environment → semi-controlled → real environment, in stages
- Log collection: rosbag + system logs (journalctl) + sensor status monitoring

> **Further reading**
> - [BehaviorTree.CPP Documentation](https://www.behaviortree.dev/) — BT design patterns and tutorials
> - [Nav2 Documentation](https://docs.nav2.org/) — ROS2 Navigation2 stack. A real-world example of BT-based design

## Technical Timeline: Robot Frameworks — Past → Present → Future

```
2007 ─── ROS development begins (Stanford → Willow Garage; 1.0 released in 2010)
  │       Widely adopted as robot middleware
  │
2012 ─── Gazebo Classic becomes an independent project
  │       Simulation becomes an essential step in robot development
  │
2017 ─── First ROS2 release
  │       DDS-based communication, real-time support, security added
  │
2019 ─── NVIDIA Isaac Sim released
  │       RTX-based high-quality rendering + synthetic data generation
  │
2020 ─── Embodied AI simulators like Habitat and AI2-THOR rise
  │       Large-scale learning-based research on robot policies takes off
  │
2022 ─── ROS2 Humble LTS released
  │       Industry adoption accelerates, Nav2/MoveIt2 stabilize
  │
2024 ─── ROS1 Noetic EOL (end of support)
  │       The effective deadline for the ROS2 transition
  │
2025+ ── Era of embodied AI + foundation models
          Large-scale pretraining in simulators → sim-to-real transfer
          Language-instructed robot manipulation (VLA)
          NVIDIA Isaac Lab and others begin offering a unified simulator → training → real-robot deployment pipeline
```

---

# Ch.16 — Development Environment & Tools

Robotics research code depends heavily on precise combinations of CUDA, Python, ROS, and system library versions. Reproducible workflows rely on disciplined language toolchains, isolated package environments, and containerized Docker images.

## 16.1 Programming Languages

Even when AI coding agents assist with much of the writing, researchers still need to read and understand existing code. They must inspect the structure of cloned research code, verify generated code, and identify where to make corrections when something goes wrong.

### 16.1.1 C++

**Use cases**: real-time systems, ROS nodes, SLAM, performance-critical modules

Most of the core code in a lab is C++. SLAM, real-time control, and the core logic of ROS packages are all written in C++, and you often have to read and modify this code. To understand code like ORB-SLAM3, LOAM, or VINS-Mono, you need to be comfortable with C++.

**Pros**:
- Fast execution
- Direct memory control
- Most ROS/SLAM code is C++

**Cons**:
- Hard to learn
- Slow development
- Memory management mistakes

**modern C++ (C++17/20)**:

```cpp
// Smart pointer
auto ptr = std::make_shared<MyClass>();

// Range-based for
for (const auto& item : container) { ... }

// Lambda
auto func = [&](int x) { return x * 2; };
```

> **Further reading**
> - [C++ Core Guidelines](https://isocpp.github.io/CppCoreGuidelines/CppCoreGuidelines) — modern C++ coding guide (Bjarne Stroustrup, Herb Sutter)
> - [The Cherno - C++ Playlist](https://www.youtube.com/playlist?list=PLlrATfBNZ98dudnM48yfGUldqGD0S4FFb) — video series covering C++ from basics to advanced topics
> - [Modernes C++](https://www.modernescpp.com/index.php) — blog with a systematic treatment of modern C++ (C++17/20/23) features

> **⚠ Target-environment check**: State whether the target is x86 or Jetson (ARM) and whether cross-compilation is involved. Then verify that the generated dependencies and build flags support that architecture.

### 16.1.2 Python

**Use cases**: prototyping, deep-learning training/inference, data analysis, visualization

Python is widely used for PyTorch training scripts, data preprocessing, and related tasks. An agent can help produce a first draft, but the researcher still needs to read the code and inspect its outputs and performance bottlenecks.

**Frequently used libraries**:

```bash
pip install numpy scipy matplotlib
pip install opencv-python open3d
pip install torch torchvision
pip install transformers  # HuggingFace
```

> **Further reading**
> - [Real Python](https://realpython.com/) — systematic tutorials covering Python from basics to advanced
> - [Fireship - Python in 100 Seconds](https://www.youtube.com/watch?v=x7X9w_GIm1s) — video that skims all of Python quickly

## 16.2 Development Environment Setup

### 16.2.1 Ubuntu

Ubuntu is the primary supported platform for ROS, and it has extensive documentation for GPU driver, CUDA, and cuDNN combinations. Some development is also possible on macOS and Windows; the appropriate choice depends on the ROS distribution and the operating system deployed on the robot.

**Recommended versions**:
- Ubuntu 22.04 LTS (ROS2 Humble)
- Ubuntu 24.04 LTS (ROS2 Jazzy)

**Initial setup**:

```bash
# Basic tools
sudo apt update && sudo apt upgrade -y
sudo apt install -y build-essential cmake git curl wget

# Python-related
sudo apt install -y python3-pip python3-venv

# Development tools
sudo apt install -y vim tmux htop
```

> **Further reading**
> - [The Missing Semester of Your CS Education (MIT)](https://missing.csail.mit.edu/) — systematic treatment of shell, vim, tmux, Git, and other "development tools you use every day but no class teaches". Recommended
> - [Fireship - Linux in 100 Seconds](https://www.youtube.com/watch?v=rrB13utjYV4) — quickly get a feel for what Linux is

### 16.2.2 CUDA / cuDNN

Deep-learning models are usually trained with GPU acceleration. In an NVIDIA environment, if the driver does not support the CUDA runtime bundled with PyTorch, the GPU will not be picked up.

**Installation check**:

```bash
nvidia-smi          # GPU status
nvcc --version      # CUDA version
```

**Recommended versions**: CUDA 12.x, cuDNN 8.x

**Caveat**: always verify compatibility between the CUDA version and your PyTorch/TensorFlow version.

> **Further reading**
> - [PyTorch - Previous Versions](https://pytorch.org/get-started/previous-versions/) — check PyTorch-CUDA version matching. Consult before installing
> - [NVIDIA CUDA Toolkit Documentation](https://docs.nvidia.com/cuda/) — official CUDA documentation

**NVIDIA driver install troubleshooting**

When installing NVIDIA drivers on Ubuntu, the most common issue is a conflict with `nouveau` (the open-source driver).

```bash
# Disable nouveau
sudo bash -c "echo blacklist nouveau > /etc/modprobe.d/blacklist-nvidia-nouveau.conf"
sudo bash -c "echo options nouveau modeset=0 >> /etc/modprobe.d/blacklist-nvidia-nouveau.conf"
sudo update-initramfs -u
sudo reboot

# Install the driver (recommended: use apt)
sudo apt install nvidia-driver-535  # adjust the version to your GPU
sudo reboot

# Verify
nvidia-smi
```

If the GPU does not show up in `nvidia-smi` after installation: (1) check with `sudo dkms status` whether the module was built and installed, and with `lsmod | grep nvidia` whether it is actually loaded; (2) if Secure Boot is on, unsigned modules will not load, so enroll a MOK key, use a distribution-signed driver package, or turn Secure Boot off. (Reference: [Jinyong Jeong's blog](https://jinyongjeong.github.io/2016/11/22/ubuntu_graphic_driver_install/))

**CUDA/cuDNN version compatibility**

Official PyTorch wheels ship their own CUDA runtime, so the host's CUDA toolkit (`nvcc`) version does not have to match. What actually matters is whether the driver supports the CUDA runtime bundled in the wheel. When that condition breaks, `import torch` still succeeds but `torch.cuda.is_available()` returns False or the first CUDA call errors out. Check in this order:

```bash
# 1. Check the GPU
nvidia-smi  # The "CUDA Version" in the top right is the "max version supported by the driver"

# 2. Check the installed CUDA toolkit
nvcc --version

# 3. Check which CUDA PyTorch is using
python -c "import torch; print(torch.version.cuda)"

# With wheels there is only one condition: does the driver (1) support PyTorch's CUDA runtime (3)?
# nvcc (2) only matters when building from source or compiling custom CUDA extensions. Check combinations on the PyTorch official site:
# https://pytorch.org/get-started/locally/
```

(Reference: [Jinyong Jeong's blog](https://jinyongjeong.github.io/2016/09/19/cuda_setting/))

**Building OpenCV + CUDA from source**

`python3-opencv` installed via apt and `pip install opencv-python` do not have CUDA acceleration. If you need GPU acceleration (DNN module, optical flow, etc.), you have to build from source.

```bash
# Install dependencies
sudo apt install -y build-essential cmake git libgtk2.0-dev pkg-config \
    libavcodec-dev libavformat-dev libswscale-dev \
    libtbb-dev libjpeg-dev libpng-dev \
    python3-dev python3-numpy   # required for BUILD_opencv_python3=ON. Check the Python 3 entry in the cmake summary

# OpenCV + contrib source
git clone https://github.com/opencv/opencv.git
git clone https://github.com/opencv/opencv_contrib.git

# Build (enable CUDA)
cd opencv && mkdir build && cd build
# Adjust CUDA_ARCH_BIN to your GPU (RTX 3090=8.6, RTX 4090=8.9). A comment cannot follow a line continuation (\)
cmake -D CMAKE_BUILD_TYPE=Release \
      -D CMAKE_INSTALL_PREFIX=/usr/local \
      -D OPENCV_EXTRA_MODULES_PATH=../../opencv_contrib/modules \
      -D WITH_CUDA=ON \
      -D CUDA_ARCH_BIN="8.6" \
      -D WITH_CUDNN=ON \
      -D OPENCV_DNN_CUDA=ON \
      -D BUILD_opencv_python3=ON \
      ..
make -j$(nproc)
sudo make install
```

`CUDA_ARCH_BIN` must match your GPU. If it is wrong, the build succeeds but you get slow runtime or errors. Check [NVIDIA GPU Compute Capability](https://developer.nvidia.com/cuda-gpus).

Caveat: pip opencv and apt cv_bridge conflict in ROS environments (see Ch.21 Appendix C.4, troubleshooting). Building CUDA OpenCV directly can make this problem more tangled, so isolating it with Docker is recommended.

(Reference: [Dark Programmer — building OpenCV + CUDA from source](https://darkpgmr.tistory.com/184))

### 16.2.3 Environment Management

Each project needs different Python and library versions. Without an environment manager, running `pip install` globally puts you in "dependency hell", where a library required by project A conflicts with project B. Creating an isolated per-project environment with Conda or venv is the baseline.

**Conda** (recommended):

```bash
# Install Miniconda
wget https://repo.anaconda.com/miniconda/Miniconda3-latest-Linux-x86_64.sh
bash Miniconda3-latest-Linux-x86_64.sh

# Create an environment
conda create -n myenv python=3.10
conda activate myenv

# Install packages
# PyTorch stopped publishing to the pytorch Anaconda channel as of 2.6. Use the pip index inside the conda environment
pip install torch torchvision --index-url https://download.pytorch.org/whl/cu121
```

**venv** (lightweight):

```bash
python3 -m venv myenv
source myenv/bin/activate
pip install -r requirements.txt
```

For example, SLAM research might need Python 3.8 while a recent Transformer model might need Python 3.10. With Conda, `conda activate slam_env` or `conda activate transformer_env` switches between them.

> **Further reading**
> - [Conda Documentation - Managing Environments](https://docs.conda.io/projects/conda/en/latest/user-guide/tasks/manage-environments.html) — official Conda environment management guide
> - [Python venv Documentation](https://docs.python.org/3/library/venv.html) — official Python virtual environment docs

## 16.3 Docker

### 16.3.1 Why Docker?

Docker packages the operating-system user space, libraries, and environment settings into an image. Sharing the same image reduces dependency differences across machines and lets a paper distribute its execution environment with the code.

- **Reproducibility**: pins the user-space image and reduces environment drift; host kernel, driver, hardware, and external services still matter
- **Isolation**: avoids polluting the system
- **Deployment**: easy sharing and deployment
- **Dependencies**: handles complex dependencies

### 16.3.2 Basic Usage

```bash
# Pull an image
docker pull nvidia/cuda:12.1.0-devel-ubuntu22.04

# Run a container
docker run -it --rm \
    --gpus all \
    -v $(pwd):/workspace \
    nvidia/cuda:12.1.0-devel-ubuntu22.04 bash

# Build a Dockerfile
docker build -t my_image .
```

**Dockerfile example**:

```dockerfile
FROM nvidia/cuda:12.1.0-devel-ubuntu22.04

RUN apt-get update && apt-get install -y \
    python3-pip git

COPY requirements.txt /tmp/
RUN pip3 install -r /tmp/requirements.txt

WORKDIR /workspace
```

> **Further reading**
> - [Docker official Getting Started Guide](https://docs.docker.com/get-started/) — start here if Docker is new to you. Explains containers, images, and volumes well
> - [NetworkChuck - Docker Tutorial](https://www.youtube.com/watch?v=eGz9DS-aIeY) — video that explains Docker in a fun, accessible way. Good for beginners
> - [Fireship - Docker in 100 Seconds](https://www.youtube.com/watch?v=Gjnup-PuquQ) — quick skim of Docker's core concepts

### 16.3.3 NVIDIA Container Toolkit

A plain Docker container does not see the GPU. Deep-learning training and CUDA-based computation require nvidia-container-toolkit and the `--gpus all` flag.

Note: NVIDIA's current documentation uses `nvidia-container-toolkit`; with modern Docker, GPU access is requested with `--gpus all` rather than the older `--runtime=nvidia` pattern.

```bash
# Install nvidia-container-toolkit (Ubuntu 22.04/24.04)
curl -fsSL https://nvidia.github.io/libnvidia-container/gpgkey | \
    sudo gpg --dearmor -o /usr/share/keyrings/nvidia-container-toolkit-keyring.gpg
curl -s -L https://nvidia.github.io/libnvidia-container/stable/deb/nvidia-container-toolkit.list | \
    sed 's#deb https://#deb [signed-by=/usr/share/keyrings/nvidia-container-toolkit-keyring.gpg] https://#g' | \
    sudo tee /etc/apt/sources.list.d/nvidia-container-toolkit.list
sudo apt-get update && sudo apt-get install -y nvidia-container-toolkit

# Register the NVIDIA runtime with the Docker daemon
sudo nvidia-ctk runtime configure --runtime=docker
sudo systemctl restart docker

# Test
docker run --rm --gpus all nvidia/cuda:12.4.1-base-ubuntu22.04 nvidia-smi
```

### 16.3.4 Practical Recipe: ROS2 + GPU + GUI + Sensors

A robotics container may need GPU, GUI (RViz/Gazebo), and USB-sensor access together. The recipe below configures these permissions and the runtime in one place.

The recipe adapts the structure of [turlucode/ros-docker-gui](https://github.com/turlucode/ros-docker-gui) to an nvidia-container-toolkit and ROS2 Humble environment.

**Step 1: Prepare X11 forwarding (host)**

```bash
# Run once on the host
sudo apt-get install -y xauth
xhost +local:docker
```

`xhost +local:docker` adds an entry that allows local connections. xhost is host-based access control and cannot tell containers apart, so this does not open access to Docker alone. To restrict access to a specific container, use `xauth` cookie-based authentication.

**Step 2: Run script**

```bash
#!/bin/bash
# run_ros2_docker.sh — full setup for GPU + GUI + USB sensors

What each flag does:

| Flag | Role |
|--------|------|
| `--gpus all` | GPU passthrough (nvidia-container-toolkit) |
| `--privileged` | Full access to USB/serial devices. In production, map individually with `--device` |
| `--net=host` | Share host network for DDS multicast. Essential for ROS2 inter-node communication |
| `--ipc=host` | Shared memory. Required by GUI tools like RViz |
| `-e QT_X11_NO_MITSHM=1` | Without this, RViz crashes with a segfault. MIT-SHM does not work in Docker |
| `-e ROS_DOMAIN_ID=42` | Isolates from other ROS2 systems on the same network. Essential when multiple people use the lab |
| `-v /dev:/dev` | Sensor USB may be plugged in at any time, so mount all of /dev. Pairs with `--privileged` |
| `-v .Xauthority` | X11 authentication. If you are not using the xhost setup from Step 1, without it you get `cannot open display` |

**Step 3: Save the container after work** — if you ran Step 2 with `--rm`, the container is deleted the moment you leave the shell, so nothing is left to commit. Either commit from another shell while the container is still running, or run it without `--rm`.

```bash
# If you installed packages or did other work inside the container, commit it
docker commit ros2_dev my_ros2_workspace:v1

# Next time, run from the saved image
# In run_ros2_docker.sh just change the image name
```

**Managing it with a Dockerfile** (more recommended):

```dockerfile
FROM osrf/ros:humble-desktop

# Basic tools
RUN apt-get update && apt-get install -y \
    python3-pip git wget curl vim \
    ros-humble-rviz2 \
    ros-humble-rqt* \
    && rm -rf /var/lib/apt/lists/*

# Python packages
RUN pip3 install torch torchvision numpy opencv-python-headless

# ROS2 workspace
RUN mkdir -p /root/ros2_ws/src
WORKDIR /root/ros2_ws

# If there are packages that need source build, put them here
# RUN cd src && git clone https://github.com/...
# RUN . /opt/ros/humble/setup.sh && colcon build

ENTRYPOINT ["/ros_entrypoint.sh"]
CMD ["bash"]
```

Why a Dockerfile beats `docker commit`: later you can trace "what is installed in this image?". An image built by commit has no history, so you cannot reproduce it.

> **Further reading**
> - [turlucode/ros-docker-gui](https://github.com/turlucode/ros-docker-gui) — reference for ROS + NVIDIA + GUI Docker setup. Supports Melodic through Humble
> - [NVIDIA Container Toolkit Documentation](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/index.html) — official install guide
> - [OSRF Docker Images](https://hub.docker.com/r/osrf/ros) — official ROS Docker images. `humble-desktop` is the version that includes GUI

> **Docker requirements check**: State up front whether the setup combines ROS2, a GPU, USB sensors, and GUI visualization. Independently generated snippets can conflict in their permissions and network options. Check whether the final command needs `QT_X11_NO_MITSHM`, `ROS_DOMAIN_ID`, and explicit device mappings.

## 16.4 Remote Management: Git, SSH, File Transfer

A remote experiment environment is managed through SSH access, Git history, and file transfer between servers.

### 16.4.1 Git/GitHub

### 16.4.1.1 Basic Workflow

Git records code changes and the state used for each experiment. Recording the commit with a result makes it possible to reproduce an earlier state or compare the source of a change.

```bash
# Clone a repository
git clone https://github.com/user/repo.git

# Check changes
git status
git diff

# Commit
git add .
git commit -m "feat: add new feature"

# Push
git push origin main
```

> **Further reading**
> - [GitHub's Git Handbook](https://docs.github.com/en/get-started/using-git/about-git) — official guide that cleanly organizes Git's core concepts
> - [The Missing Semester - Version Control (Git)](https://missing.csail.mit.edu/2020/version-control/) — MIT lecture. Explains Git's internal model (DAG), giving a deeper understanding

### 16.4.1.2 Branching Strategy

**Git Flow**:
- `main`: stable version
- `develop`: development version
- `feature/*`: feature development
- `hotfix/*`: urgent fixes

**GitHub Flow** (simple):
- `main`: always deployable
- `feature-branch`: per-feature branch → PR → merge

When several people edit the same code, feature branches can isolate each change before review and merge into `main`. A simple GitHub Flow is often enough to separate conflict scope and change intent.

### 16.4.1.3 Collaboration

**Pull Request (PR)**:
1. Fork or create a branch
2. Commit changes
3. Open a PR and request review
4. Merge after code review

**Commit message convention** (Conventional Commits):

```
feat: new feature
fix: bug fix
docs: documentation change
refactor: refactoring
test: add/modify tests
chore: build/config changes
```

> **Further reading**
> - [GitHub's Git Handbook](https://docs.github.com/en/get-started/using-git/about-git) — Git introduction written by GitHub itself
> - [Conventional Commits Specification](https://www.conventionalcommits.org/) — official spec for commit message conventions

### 16.4.2 SSH

The basic tool for accessing a lab GPU server. Using key authentication instead of a password is both convenient and secure.

```bash
# Generate keys (once, the first time)
ssh-keygen -t ed25519

# Register the public key on the server
ssh-copy-id user@server_ip

# Connect
ssh user@server_ip

# Port forwarding (view the server's Jupyter/TensorBoard locally)
ssh -L 8888:localhost:8888 user@server_ip
```

Configuring **~/.ssh/config** removes the need to type the IP and username every time:

```
Host lab-server
    HostName 192.168.1.100
    User junholee
    IdentityFile ~/.ssh/id_ed25519
```

After this, `ssh lab-server` connects directly. VS Code Remote-SSH also reads this config.

### 16.4.3 SCP & rsync

Tools for file transfer between server and local. **SCP** is simple file copy; **rsync** transfers only changed parts.

**SCP**:

```bash
# Local → server
scp model.pth user@server:/home/user/weights/

# Server → local
scp user@server:/home/user/results/log.txt ./

# Directory copy
scp -r dataset/ user@server:/data/
```

**rsync** — better for large datasets or repeated transfers:

```bash
# Local → server (transfer only changes, show progress)
rsync -avz --progress dataset/ user@server:/data/dataset/

# Server → local
rsync -avz user@server:/home/user/results/ ./results/

# Mirror deleted files as well
rsync -avz --delete source/ user@server:/data/source/
```

`scp` copies the whole thing every time; `rsync` sends only the diff, which makes a big difference when syncing datasets of tens of GB.

### 16.4.4 Tailscale

If the lab server sits behind NAT or a firewall, SSH from outside does not work. Tailscale is a WireGuard-based VPN; install it and you can connect directly to the lab server from anywhere.

```bash
# Install (on both server and local)
curl -fsSL https://tailscale.com/install.sh | sh
sudo tailscale up

# Check status — list of connected devices and IPs
tailscale status

# Then SSH to the Tailscale IP
ssh user@100.x.y.z
```

**Pros**:
- No port forwarding or router configuration needed
- Reach the server from cafe, home, or school at the same IP
- With Tailscale SSH, SSH key management is also automated

Registering the Tailscale IP in **~/.ssh/config** is convenient:

```
Host lab-gpu
    HostName 100.x.y.z
    User junholee
```

> **Further reading**
> - [Tailscale official docs](https://tailscale.com/kb/) — from installation to ACL configuration
> - [The Missing Semester - Remote Machines](https://missing.csail.mit.edu/2020/command-line/#remote-machines) — SSH, port forwarding, tmux, and other remote work basics

## 16.5 Experiment Management

### 16.5.1 Weights & Biases (wandb)

When running deep-learning experiments, the question "what were the hyperparameters of the model I ran yesterday?" comes up every day. Logging in Excel or a notebook hits its limits quickly. wandb automatically logs and visualizes training, and it also makes sharing results with teammates easy. It can track metrics and hyperparameters, version model artifacts, and share dashboards with a team.

```python
import wandb

# Initialize
wandb.init(project="my-project", config={
    "learning_rate": 0.001,
    "epochs": 100
})

# Logging
for epoch in range(epochs):
    loss = train_one_epoch()
    wandb.log({"loss": loss, "epoch": epoch})

# Finish
wandb.finish()
```

> **Further reading**
> - [Weights & Biases official docs and Quickstart](https://docs.wandb.ai/quickstart) — wandb getting-started guide. You can log your first experiment in 5 minutes
> - [Weights & Biases YouTube](https://www.youtube.com/@WeightsBiases) — tutorials and MLOps talks

### 16.5.2 MLflow

Where wandb is a cloud-based service, MLflow is an open-source alternative you can run on your own server. Useful where data security matters.

```python
import mlflow

mlflow.set_experiment("my-experiment")

with mlflow.start_run():
    mlflow.log_param("lr", 0.001)
    mlflow.log_metric("accuracy", 0.95)
    mlflow.pytorch.log_model(model, "model")
```

### 16.5.3 TensorBoard

```python
from torch.utils.tensorboard import SummaryWriter

writer = SummaryWriter("runs/experiment1")
writer.add_scalar("Loss/train", loss, epoch)
writer.add_image("Sample", image, epoch)
writer.close()
```

```bash
tensorboard --logdir runs
```

TensorBoard works directly with PyTorch and visualizes locally without a separate account. For simple experiments, TensorBoard alone is enough, without wandb.

> **Further reading**
> - [PyTorch TensorBoard Tutorial](https://pytorch.org/tutorials/recipes/recipes/tensorboard_with_pytorch.html) — official guide to using TensorBoard from PyTorch
> - [MLflow Documentation](https://mlflow.org/docs/latest/index.html) — official MLflow documentation

## 16.6 Code Formatting

### 16.6.1 Linting & Formatting

When code style differs between people, code reviews spend more time on style disputes than on logic. Automatic formatters remove this problem. Even when working alone, a consistent code style helps a lot when you re-read your own code later.

**Python**:

```bash
# Ruff (fast linter, Black-compatible formatter)
pip install ruff
ruff check .
ruff format .

# Black (formatter)
pip install black
black .

# Type checking
pip install mypy
mypy .
```

**C++**:

```bash
# clang-format
clang-format -i src/*.cpp
```

### 16.6.2 Testing

Writing tests matters even for research code. Just having basic tests for "does the model forward pass work" or "is the data preprocessing output as expected" makes refactoring much less stressful.

**Python (pytest)**:

```python
# test_module.py
def test_addition():
    assert 1 + 1 == 2

def test_function():
    result = my_function(input)
    assert result == expected
```

```bash
pytest tests/ -v
```

**C++ (gtest)**:

```cpp
#include <gtest/gtest.h>

TEST(MyTest, BasicTest) {
    EXPECT_EQ(1 + 1, 2);
}
```

> **Further reading**
> - [Real Python - Python Testing with pytest](https://realpython.com/pytest-python-testing/) — detailed tutorial on using pytest
> - [The Missing Semester (MIT)](https://missing.csail.mit.edu/) — shell, editor, debugging, profiling, and development tools broadly. Worth a full pass before you start graduate school
> - [Fireship YouTube](https://www.youtube.com/@Fireship) — skim various development tools quickly through the "100 Seconds" series
> - [Jinyong Jeong's blog — robot software development culture](https://jinyongjeong.github.io/2025/02/14/developmen_culture/) — establishing code review, CI/CD, style guides, and other development-culture practices in a robotics team
> - [Jinyong Jeong's blog — robot development and test code](https://jinyongjeong.github.io/2025/02/19/test_code/) — six reasons test code is essential in robot software

## Technical Timeline: Development Environment & Tools — Past → Present → Future

```
2005 ─── Git is born (Linus Torvalds)
  │       the start of distributed version control
  │
2008 ─── GitHub launches
  │       becomes the hub of open-source collaboration
  │
2012 ─── Conda (Anaconda) appears
  │       becomes a widely used Python environment manager in data science
  │
2013 ─── Docker released
  │       container images make user-space dependencies easier to reproduce
  │
2015 ─── TensorBoard (released with TensorFlow)
  │       the beginning of deep-learning training visualization
  │
2017 ─── nvidia-docker released
  │       GPU usage inside Docker containers becomes possible
  │
2018 ─── Weights & Biases launches
  │       experiment tracking, visualization, and team collaboration in the cloud
  │
2020~2022 ─── Black goes mainstream, Ruff appears (2022)
  │       Python code-quality tooling speeds up
  │
2023+ ── AI-assisted development tools spread
          AI coding assistants such as Copilot and Cursor
          Dev Container standardization (VS Code Remote)
          Docker + wandb combination for reproducible research becomes common
```

---

# Ch.17 — Datasets & Benchmarks

A dataset's sensors, collection conditions, splits, and annotations determine what can be learned and compared. This chapter contrasts the structure of major benchmarks and outlines procedures for collecting and managing new data.

The share of **synthetic data** has been growing recently. Collecting and labeling real data is costly and time-consuming, so a workflow of pretraining on automatically generated synthetic data from a simulator and then fine-tuning on a small amount of real data has taken hold. NVIDIA Isaac Sim's Domain Randomization and Habitat's large-scale scene generation are representative examples. **Sim-to-Real datasets** — datasets that provide simulator data paired with the corresponding real data — are also being actively constructed.

## 17.1 Autonomous Driving / Robotics Datasets

### 17.1.1 KITTI / KITTI360

KITTI is a long-standing dataset that became the starting point for autonomous driving research.

Since its release in 2012, KITTI has provided a common comparison point for autonomous driving and 3D vision. Even after larger and more varied datasets appeared, it has remained useful for comparing VO, SLAM, and stereo-depth results with earlier work.

**Composition**:
- Stereo cameras
- 3D LiDAR (Velodyne HDL-64E)
- GPS/IMU
- 2D/3D labels

**Tasks**:
- Stereo depth estimation
- Optical flow
- Visual odometry / SLAM
- 3D object detection
- Semantic segmentation

**Download**: https://www.cvlibs.net/datasets/kitti/

> **Further reading**
> - [KITTI Benchmark official site](https://www.cvlibs.net/datasets/kitti/) — dataset download and per-task leaderboards.
> - [KITTI-360 site](https://www.cvlibs.net/datasets/kitti-360/) — a broader 360-degree dataset.
> - [Dark Programmer — Using KITTI Data (LiDAR-camera transforms)](https://darkpgmr.tistory.com/190) — hands-on with coordinate frame transforms and LiDAR-camera mapping on KITTI.

### 17.1.2 nuScenes

nuScenes is a large-scale autonomous driving dataset.

It has a richer sensor suite than KITTI (360-degree cameras, Radar included) and is much larger in scale. Alongside KITTI, it is one of the most cited datasets in recent autonomous driving papers. It is central to 3D Object Detection and BEV (Bird's Eye View) based perception research.

**Composition**:
- 6 cameras (360° coverage)
- 5 Radars
- 1 LiDAR
- 1000 scenes, 40K keyframes

**Features**:
- 23 object classes
- Rich annotations (attributes, visibility)
- Diverse conditions including night and rain

**Evaluation metrics**: mAP, NDS

> **Further reading**
> - [nuScenes devkit Documentation](https://www.nuscenes.org/nuscenes) — dataset usage, devkit API, tutorial notebooks.
> - [nuScenes devkit GitHub](https://github.com/nutonomy/nuscenes-devkit) — Python devkit code and examples.

### 17.1.3 Waymo Open Dataset

The Waymo Open Dataset is a large-scale autonomous driving dataset released by Waymo (an Alphabet subsidiary).

Together with nuScenes, it is one of the two main benchmarks in current autonomous driving research. It leads in data quality and scale, and its annual challenge lets you track the latest technical trends.

**Scale**:
- 1,150 scenes (20 seconds each)
- 12M LiDAR labels
- 12M camera labels

**Features**:
- High-quality sensors
- Diverse environments (urban, suburban, night)
- Annual challenge

> **Further reading**
> - [Waymo Open Dataset official site](https://waymo.com/open/) — dataset download and challenge participation.
> - [Waymo Open Dataset GitHub](https://github.com/waymo-research/waymo-open-dataset) — official tools and example code.

### 17.1.4 Datasets for RGB-D SLAM and VIO / VINS

TUM RGB-D and EuRoC MAV are used for indoor RGB-D SLAM and drone VIO evaluation, respectively.

**TUM RGB-D**:
- RGB-D camera sequences
- Precise ground truth (motion capture)
- Indoor environments
- Standard for Visual SLAM evaluation

**EuRoC MAV**:
- Drone flight data
- Stereo + IMU
- Standard for VIO evaluation
- Varied difficulty levels

> **Further reading**
> - [TUM RGB-D Benchmark](https://cvg.cit.tum.de/data/datasets/rgbd-dataset) — standard Visual SLAM evaluation dataset and evaluation tools.
> - [EuRoC MAV Dataset](https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets) — standard VIO evaluation dataset.

## 17.2 Computer Vision Datasets

### 17.2.1 ImageNet

ImageNet is the standard benchmark for image classification.

This is the dataset that marked the turn into deep learning. After AlexNet's overwhelming performance on ImageNet in 2012, nearly every vision model started using ImageNet-pretrained weights. In robotics too, the backbone of camera-based perception modules is mostly an ImageNet-pretrained model.

- 1000 classes
- 1.2M training images
- Pretraining standard

### 17.2.2 COCO

COCO evaluates object detection and instance segmentation.

COCO mAP averages AP across several IoU thresholds. This differs from the PASCAL VOC convention that uses a single IoU threshold.

**Features**:
- 80 object categories
- 330K images, 1.5M object instances
- Dense annotation (bounding box, segmentation mask)

**Tasks**:
- Object detection
- Instance segmentation
- Keypoint detection
- Captioning

### 17.2.3 ScanNet / NYU Depth V2

**ScanNet**:
- 1513 indoor scenes
- RGB-D sequences
- 3D semantic segmentation
- Camera poses and meshes provided

**NYU Depth V2**:
- Indoor RGB-D
- Depth estimation benchmark
- 464 scenes, 407K frames

If you work on indoor robots (home, service robots, and so on), ScanNet and NYU Depth V2 are core benchmarks. ScanNet in particular is indispensable for 3D Scene Understanding research.

> **Further reading**
> - [COCO Dataset](https://cocodataset.org/) — official site, dataset download, and evaluation tools.
> - [ScanNet Benchmark](http://www.scan-net.org/) — 3D Scene Understanding benchmark.
> - [Papers With Code - Datasets](https://paperswithcode.com/datasets) — integrated site for task-wise dataset search and leaderboards.

## 17.3 How to Use Datasets

### 17.3.1 Download and Format Understanding

Each dataset has its own directory structure and format.

If you download a dataset but do not properly understand the directory structure and label format, writing a data loader alone can take days. For 3D labels in particular, the coordinate frame differs across datasets (camera frame vs LiDAR frame, y-up vs z-up, and so on), so read the documentation carefully.

**Example: KITTI Object Detection**:

**Example of reading a label file**:

> **Further reading**
> - [KITTI Benchmark official site - Object Detection DevKit](https://www.cvlibs.net/datasets/kitti/eval_object.php?obj_benchmark=3d) — label format description and evaluation code.
> - [nuScenes devkit Tutorial Notebooks](https://github.com/nutonomy/nuscenes-devkit/tree/master/python-sdk/tutorials) — Jupyter notebooks for understanding the data structure.

### 17.3.2 DataLoader Implementation

This is the standard pattern for data loading in PyTorch.

PyTorch's `Dataset` defines sample loading and preprocessing, while `DataLoader` handles batching and parallel loading. Preprocessing cost in `__getitem__` and the `num_workers` setting both affect training throughput.

```python
from torch.utils.data import Dataset, DataLoader

class MyDataset(Dataset):
    def __init__(self, root_dir, transform=None):
        self.root_dir = root_dir
        self.transform = transform
        self.samples = self._load_samples()

def _load_samples(self):
        # Load file list
        return list_of_samples

def __len__(self):
        return len(self.samples)

def __getitem__(self, idx):
        sample = self.samples[idx]
        image = load_image(sample['image_path'])
        label = sample['label']

if self.transform:
            image = self.transform(image)

return image, label

# Usage
dataset = MyDataset(root_dir='./data')
dataloader = DataLoader(dataset, batch_size=32, shuffle=True, num_workers=4)
```

> **Further reading**
> - [PyTorch Data Loading Tutorial](https://pytorch.org/tutorials/beginner/data_loading_tutorial.html) — official guide for writing a custom Dataset.
> - [Real Python - PyTorch DataLoader](https://realpython.com/python-data-loading/) — detailed walkthrough of DataLoader usage.

## 17.4 Collecting Your Own Data

Public datasets often cannot give you data that fits your own research exactly. You sometimes have to collect data yourself to match your robot's sensor configuration or particular environmental conditions. If you do not handle sensor synchronization, calibration, and labeling systematically at this stage, the data becomes unusable later.

### 17.4.1 Sensor Synchronization

If you do not time-synchronize data from multiple sensors, fusion itself is meaningless.

A 10 ms offset between camera and LiDAR timestamps produces a position error of tens of centimeters at high driving speeds. The basic premise of sensor fusion is "data from the same instant", and without synchronization that premise collapses.

**Hardware synchronization**:
- Simultaneous capture via trigger signals
- PPS (Pulse Per Second) signals

**Software synchronization**:
- Approximate synchronization based on timestamps
- Use of interpolation

**In ROS, `message_filters` synchronizes on the stamp in the message header. It falls back to arrival time only for headerless messages, and only when `allow_headerless` is set:**

```python
import message_filters

# Approximate Time Synchronizer
image_sub = message_filters.Subscriber(self, Image, '/camera/image')
lidar_sub = message_filters.Subscriber(self, PointCloud2, '/lidar/points')

sync = message_filters.ApproximateTimeSynchronizer(
    [image_sub, lidar_sub], queue_size=10, slop=0.1
)
sync.registerCallback(self.callback)
```

### 17.4.2 Calibration

**Camera Intrinsic**: use a checkerboard (OpenCV calibrateCamera).

**Camera-LiDAR Extrinsic**:
- Checkerboard-based (plane fitting)
- Target-based (using a special target)
- Target-less (automatic feature matching)

**Camera-IMU**: Kalibr is recommended.

If calibration is inaccurate, the object position seen by the camera and the one seen by the LiDAR do not match. Sensor fusion accuracy depends on calibration quality. In linear algebra terms, the intrinsic is the 3×3 camera matrix K. The extrinsic is the 3×4 block [R|t] when it enters the camera projection, and a 4×4 homogeneous transformation — the same block with a [0 0 0 1] row appended below — when it is used as a coordinate transformation between sensors.

> **Further reading**
> - [OpenCV Camera Calibration Tutorial](https://docs.opencv.org/4.x/dc/dbb/tutorial_py_calibration.html) — checkerboard-based camera calibration.
> - [Kalibr GitHub](https://github.com/ethz-asl/kalibr) — standard tool for Camera-IMU calibration.

### 17.4.3 Labeling Tools

Once data is collected, you have to annotate it. Labeling is one of the most time-consuming tasks in research, and label quality determines model performance. Recently, semi-automatic labeling using foundation models such as SAM (Segment Anything Model) has become widespread.

**CVAT (Computer Vision Annotation Tool)**:
- Web-based, free
- Image and video annotation
- Supports various tasks (bbox, polygon, points)

**Labelbox**:
- Cloud-based
- Team collaboration features
- Supports 3D annotation

**3D Labeling**:
- SUSTechPOINTS: LiDAR point clouds
- KITTI-360 labeling tool

**Automatic labeling via synthetic data**: when you generate data in a simulator (NVIDIA Isaac Sim, AI2-THOR, and so on), labels are produced along with the data, so no manual labeling is needed. Domain randomization, which randomly varies texture, lighting, and background, can also improve a model's generalization. Collection cost is close to zero compared to real data.

> **Further reading**
> - [CVAT Documentation](https://docs.cvat.ai/) — official documentation for the open-source labeling tool.
> - [Roboflow](https://roboflow.com/) — integrated platform for labeling, data augmentation, and model training.
> - [NVIDIA Isaac Sim - Synthetic Data Generation](https://docs.omniverse.nvidia.com/isaacsim/latest/replicator_tutorials/index.html) — synthetic data generation guide.

## Technical Timeline

```
2009 ─── ImageNet released
  │       Start of large-scale image classification benchmarks
  │
2012 ─── KITTI released / AlexNet dominates ImageNet
  │       Birth of the autonomous driving benchmark, start of the deep learning revolution
  │
2014 ─── COCO released
  │       Standard benchmark for Object Detection and Segmentation
  │
2017 ─── ScanNet released
  │       Indoor 3D Scene Understanding research takes off
  │
2019 ─── nuScenes and Waymo Open Dataset released
  │       Era of large-scale, high-quality autonomous driving datasets
  │
2020 ─── Synthetic data research in full swing
  │       Domain Randomization, Sim-to-Real Transfer
  │       Large-scale synthetic data generation based on NVIDIA Isaac Sim
  │
2023 ─── Datasets for the Foundation Model era
  │       SA-1B (for training SAM, 1 billion masks)
  │       Open X-Embodiment (unified robot manipulation data)
  │
2024+ ── Future trends in datasets
          Mixed training on synthetic + real data becomes standard
          Sim-to-Real datasets (paired sim/real data)
          Automatic labeling (Foundation Model based)
          Large-scale collection and sharing of robot manipulation data (Open X-Embodiment)
          Multimodal datasets (vision + language + tactile + force/torque)
```

---

# Ch.18 — Lab Research Directions

Our lab designs a Spatial AI system as two modules. This split is forced by physical constraints and real-time requirements, and it is where the concepts built up in earlier chapters come together.

## 18.1 Overview

We split a Spatial AI system into **two modules**.

```
┌──────────────────────────────────────────────────────────────┐
│                     Spatial AI System                         │
├──────────────────────────────────────────────────────────────┤
│  ┌─────────────────────┐     ┌─────────────────────────────┐ │
│  │   Local Module      │     │      Global Module          │ │
│  │   (lightweight,     │ ←→  │   (heavy, server/cloud)     │ │
│  │    on-board)        │     │                             │ │
│  │ • Real-time Geometry│     │ • VFM-based Understanding   │ │
│  │ • Odometry          │     │ • Semantic Scene Graph      │ │
│  │ • Local Obstacle    │     │ • Long-term Memory          │ │
│  │ • control-budget rate│    │ • task-budget rate          │ │
│  └─────────────────────┘     └─────────────────────────────┘ │
└──────────────────────────────────────────────────────────────┘
```

### Why the System Uses Two Modules

A single onboard computer cannot handle every function within the robot's weight, power, and response-time limits.

Start with the **physical constraints**. An NVIDIA A100 GPU server weighs tens of kilograms and draws hundreds of watts, so a battery-powered drone cannot carry one. Robots commonly use embedded boards such as the Jetson Orin, but these boards cannot run large models such as DINOv2 or SAM in real time.

The **time constraints** also differ. Obstacle avoidance must respond within tens of milliseconds, whereas semantic interpretation can run more slowly. The former cannot wait for a server response; the latter has room to use a larger model.

The two modules divide their roles accordingly:

1. **The reality of compute**: on-board computers on the robot (Jetson, etc.) may be unable to run large models in real time, depending on model size, power, and response-time requirements
2. **Real-time requirements**: obstacle avoidance needs a response within tens of milliseconds
3. **Semantic understanding**: VFMs and VLAs distinguish an object's class and state, such as identifying a broken glass
4. **Complementary roles**: combine geometric precision in the Local Module with semantic understanding in the Global Module

> By analogy, the Local Module provides the robot's **reflexes**, while the Global Module acts as its **cerebral cortex**. Touch a hot pot and you pull your hand away first (reflex), then think "ah, the stove was on" (cognition). Robots work the same way.

## 18.2 Local Module: Lightweight Geometry

The Local Module runs directly on the robot and processes the information needed for safe, real-time motion.

### 18.2.1 Goals

- **Odometry**: estimate the robot's own motion — "where am I right now?"
- **Obstacle Detection**: immediate obstacle sensing — "something is in front, dodge!"
- **Local Mapping**: a geometric map of the surroundings — "within 3 m of me, the world looks like this"

**Operating example**: when a child suddenly runs in front of a delivery robot in an apartment hallway, the Local Module detects the obstacle with a depth sensor, estimates pose through odometry, and computes an avoidance path within the deadline derived by the control and safety analysis. At this stage, collision risk matters before the object class. The Global Module interprets meaning separately.

### 18.2.2 Characteristics

- **Latency budget**: derive update rate and deadline from platform speed, braking distance, control bandwidth, and sensor rate
- **Resource budget**: choose the embedded module, power mode, and cooling for the measured workload
- **Timing evidence**: measure worst-case latency, jitter, and deadline misses as well as mean FPS on the target hardware

### 18.2.3 Tech Stack

**Classical methods**:
- ORB-SLAM3: feature-based Visual SLAM — pose estimation from a single camera (see Ch.9, Ch.14)
- VINS-Mono: Visual-Inertial Odometry — camera + IMU fusion (see Ch.14)
- FAST-LIO2: LiDAR-Inertial Odometry — LiDAR + IMU fusion (see Ch.2, Ch.14)

**Lightweight learning models**:
- Lightweight depth estimation — compressed with a MobileNet backbone (see Ch.10)
- Compressed segmentation models — knowledge distillation applied (see Ch.10, Ch.11)
- TensorRT optimization — a candidate for graph, kernel, and precision optimization on NVIDIA GPUs

**Edge deployment**:

```bash
# TensorRT optimization example
trtexec --onnx=model.onnx --saveEngine=model.trt --fp16 --memPoolSize=workspace:4096
```

> TensorRT builds an inference engine for NVIDIA GPUs. FP16 can reduce memory and latency, but gains and task-metric changes depend on the model, input, batch, power mode, and software versions. Compare end-to-end latency and validation metrics on the target Jetson before adopting it.

### 18.2.4 Example Implementation

```python
# Local Module conceptual code
class LocalModule:
    def __init__(self):
        self.odometry = FastLIO2()
        self.obstacle_detector = LightweightObstacleNet()  # TensorRT

def process(self, sensor_data):
        # 1. Odometry update (IMU input, 100 Hz)
        pose = self.odometry.update(sensor_data.imu, sensor_data.lidar)

# 2. Obstacle detection (camera input, 30 Hz)
        obstacles = self.obstacle_detector(sensor_data.image)

# 3. Send keyframe to the Global Module
        if self.is_keyframe(pose):
            self.send_to_global(sensor_data, pose)

return pose, obstacles
```

**Reading it as a scenario**: assume that in the code above, `process()` is called once per fusion tick. IMU data comes in at 100 Hz (100 times per second) and camera images at 30 Hz, so the structure is one where each sensor's own callback drops its latest measurement into a buffer and the tick consumes them together. Every tick, it computes "where am I now?" (odometry) and "what is in front?" (obstacle), and only at important moments (keyframes) sends data to the Global Module. Sending every frame would saturate the network.

## 18.3 Global Module: VFM-based Understanding

The Global Module runs on a server or in the cloud and interprets object classes and relations. When the Local Module detects an obstacle, the Global Module can classify it as a broken glass and connect it to a location in the scene graph.

### 18.3.1 Goals

- **Global map understanding**: grasp spatial structure and meaning — "this is the kitchen, that is the living room"
- **Semantic Scene Graph**: represent relations between objects — "the cup is on the table"
- **Long-term Memory**: track environmental changes — "yesterday there was no chair here, today there is"

**A real scenario**: a home service robot moves around the house every day and learns the environment. The Global Module maintains a high-level map like "the living room has a sofa, a TV, and a table; the kitchen has a refrigerator and a sink." When the user says "bring the remote from the living room table," it looks up the remote's location in the Scene Graph and hands a waypoint to the Local Module.

### 18.3.2 Characteristics

- **Model choice**: DINOv2 and SAM2 have variants of different sizes; they are not all billion-parameter models
- **Compute choice**: select hardware from the variant, input resolution, precision, scene count, and measured memory/latency
- **Update budget**: some global tasks can run outside the local control deadline, while interaction and change detection still need an explicit end-to-end latency budget

### 18.3.3 Tech Stack

**Vision Foundation Models** (see Ch.11):
- DINOv2: dense feature extraction — splits the image into a grid of patches and produces a feature vector per patch (using it at pixel resolution requires interpolation or upsampling)
- SAM2: promptable image/video segmentation — tracks a target mask from point, box, or mask prompts
- GroundingDINO: text-guided detection — say "red cup" and it finds it

**3D Understanding** (see Ch.11, Ch.13):
- Gaussian Splatting with semantic features — pretty, fast 3D reconstruction plus semantic information
- 3D Scene Graph construction — represent object relations as a graph
- 3D lifting of VFM features — lift features pulled from 2D images into 3D space

**Language Integration** (see Ch.11, Ch.12):
- CLIP features for open-vocabulary — even a "never-before-seen object" is searchable by text
- LLM for scene reasoning — inferring "what is this room used for?"
- VLA for action planning — "how should the arm move to pick up the cup?"

### 18.3.4 Example Implementation

```python
# Global Module conceptual code
class GlobalModule:
    def __init__(self):
        self.dinov2 = load_dinov2()
        self.sam = load_sam2()
        self.scene_graph = SemanticSceneGraph()
        self.gaussian_map = GaussianSplatMap()

def process_keyframe(self, image, depth, pose):
        # 1. Extract VFM features
        features = self.dinov2.extract(image)

# 2. Promptable segmentation (text-vocabulary prompts come from GroundingDINO/CLIP)
        masks = self.sam.segment(image, prompts=self.get_prompts())

# 3. Update the 3D Scene Graph
        self.scene_graph.update(masks, depth, pose, features)

# 4. Update the Gaussian Map
        self.gaussian_map.add_keyframe(image, depth, pose, features)

def query(self, text_prompt):
        # "Where is the red cup?" -> return location
        return self.scene_graph.find(text_prompt)
```

**Runtime behavior**: whenever a keyframe arrives from the Local Module, `process_keyframe()` is called. DINOv2 extracts image features, SAM segments the objects, and the results accumulate in the 3D Scene Graph and Gaussian Map. Later, when the user asks "where is the red cup?", `query()` looks it up. This process can take about a second because the Local Module handles real-time safety.

## 18.4 Cooperation Between the Two Modules

The two modules operate independently, but exchange information and cooperate. It resembles the relationship between a driver (Local) and a navigation app (Global) — the driver watches the road in front of them while the navigation guides the full route.

### 18.4.1 Local → Global

**What is transmitted**:
- Keyframe images / point clouds
- Local pose
- Sensor metadata

**Keyframe selection criteria**:
- Thresholds on travel distance / rotation — "send one after moving 1 m or rotating 30 degrees"
- Scene-change detection — "entered a new room"
- Information content (feature count, coverage) — "this frame carries a lot of new information"

### 18.4.2 Global → Local

**What is transmitted**:
- Prior map (for needed regions) — "obstacle information near the kitchen"
- Semantic information (object locations, classes) — "table here, chair there"
- Navigation waypoints — "follow this path"

**Example scenario**:

```
1. User: "Go to the kitchen and bring the cup"

2. Global:
   - Understand the command via VLM
   - Look up kitchen, cup locations in the Scene Graph
   - Plan the path

3. Global -> Local:
   - Waypoints: [current -> hallway -> kitchen -> in front of cup]
   - Local map of the kitchen area
   - Expected location of the cup

4. Local:
   - Follow the waypoints
   - Real-time obstacle avoidance
   - Precision approach near the cup
```

**Communication failure**: if WiFi drops while the robot is working in an underground parking lot, it must continue on the Local Module alone. It estimates its position through odometry, avoids obstacles, and moves toward the last waypoint it received. When WiFi returns, it sends the accumulated data to the Global Module and receives an updated plan. Real robots need this form of **graceful degradation**.

### 18.4.3 Communication and Synchronization

**Communication methods**:
- ROS2 DDS: local network (within the same building)
- WebSocket: cloud connection (remote server)
- 5G/WiFi: mobile robots (outdoor environments)

**Synchronization strategy**:
- Keyframe-based (no continuous streaming) — saves bandwidth
- Asynchronous processing (does not wait for Global to finish) — Local never stops
- Caching (frequently visited regions) — avoid resending the same data every time

## 18.5 Example Research Topics

The research topics below are ones our lab is actively working on or could take on. For each, we note the **prerequisite chapters**, so if a topic interests you, start from those chapters.

### Local Module Research

1. **Lighter SLAM**
    - Neural-network-based lightweight VO — replace classical VO with a neural network, but make it run on a Jetson
    - Event camera utilization — ultra-fast, low-power cameras for SLAM in extreme environments
    - Hardware acceleration (FPGA) — implement SLAM's core operations in hardware
    - **Prerequisites**: Ch.9 (camera models) and Ch.14 (Visual Odometry, SLAM) required. Ch.3 (optimization) also recommended
2. **Efficient obstacle recognition**
    - Depth-only obstacle detection — detect obstacles from depth alone, without RGB
    - Temporal consistency — maintain consistency across frames (no flickering in and out frame by frame)
    - Uncertainty-aware — also use the signal of "not sure whether this is an obstacle"
    - **Prerequisites**: Ch.10 (depth estimation, object detection) required. Ch.3 (coordinate transforms) also important
3. **Sensor fusion optimization**
    - Lightweight tight coupling — fuse IMU + camera + LiDAR tightly, but keep it light
    - Coping with sensor dropout — keep running even when one sensor fails
    - **Prerequisites**: Ch.2 (sensors), Ch.14 (Visual Odometry), and Ch.3 (optimization) required

### Global Module Research

1. **3D extension of VFMs**
    - DINOv2 features in 3D — lift 2D features into 3D space and use them there
    - Semantic Gaussian Splatting — bake semantic information into the 3D reconstruction
    - 3D scene understanding — understanding "what kind of structure this space has"
    - **Prerequisites**: Ch.10 (depth), Ch.13 (3D representation), and Ch.11 (VFM) required. Ch.9 (camera models) is basic
2. **VLA integration**
    - Open-vocabulary manipulation — control a robot arm via commands like "pick up that red thing"
    - Language-guided navigation — move via natural-language commands
    - Context-aware behavior — "there is a child nearby, move slowly"
    - **Prerequisites**: Ch.11 (VFM usage) and Ch.12 (VLA) required. Ch.10 (detection) is also useful
3. **Scalability**
    - Large-scale environment representation — an entire apartment complex or campus in a single map
    - Map compression and updating — efficiently manage multi-gigabyte maps
    - Multi-robot collaboration — multiple robots build and share a map together
    - **Prerequisites**: Ch.14 (SLAM), Ch.3 (optimization), and Ch.11 (VFM) required

### Integration Research

1. **Efficient communication**
    - What to send, and when? — sending everything wastes bandwidth; sending nothing makes Global useless
    - Optimal strategy under bandwidth limits — what if 5G drops? what if WiFi is slow?
    - **Prerequisites**: Ch.14 (SLAM, keyframe selection), plus the Local/Global module understanding above
2. **Fallback strategies**
    - Local-only operation when communication drops — perform the basic mission even without a server connection
    - Graceful degradation — capabilities shrink gradually instead of stopping abruptly
    - **Prerequisites**: a whole-system understanding is needed. Read at least Ch.3–14 first
3. **Consistency maintenance**
    - Local/Global map synchronization — if the two modules' maps disagree, the robot gets confused
    - Semantic consistency — prevent unsupported flips from "chair" to "table" while allowing updates supported by new observations
    - **Prerequisites**: Ch.3 (optimization) and Ch.14 (SLAM, map management) required

## 18.6 Questions That Separate Motivation from Novelty

Motivation explains why a problem needs to be solved; novelty identifies what an approach changes and how. The two are often conflated when choosing a research direction or writing a first paper.

Saying only that "an existing method cannot do X, so we added a module" rarely goes beyond motivation. Novelty becomes concrete only when the paper explains why the module is necessary and why it must take that particular form.

The following three papers illustrate the difference between posing a problem and contributing a design.

### Case 1 — ORB-SLAM2 (Mur-Artal & Tardós 2017)

- **Motivation**: Extend the map-reuse, loop-closing, and relocalization structure of monocular ORB-SLAM to stereo and RGB-D inputs.
- **Direct extension**: Build a separate SLAM system for each input modality.
- **Paper's design**: All three modalities share the tracking, local-mapping, and loop-closing structure and use ORB features. Stereo obtains depth from disparity, while RGB-D synthesizes a virtual right-image coordinate from the measured depth, so both are unified into the stereo observation form before entering metric-scale bundle adjustment.
- **Design principle**: Preserve a common system architecture while placing modality-specific differences in observation construction and bundle-adjustment residuals.

The primary source is [*ORB-SLAM2: An Open-Source SLAM System for Monocular, Stereo and RGB-D Cameras*](https://doi.org/10.1109/TRO.2017.2705103). The 2015 ORB-SLAM paper describes a monocular system and therefore cannot support the three-modality example.

### Case 2 — 3D Gaussian Splatting (Kerbl et al. 2023)

- **Motivation**: NeRF rendering is too slow for the desired interactive use.
- **Direct extension**: Add acceleration modules such as sparse sampling, pruning, or distillation on top of NeRF.
- **Paper's design**: Treat ray marching as the bottleneck and replace the representation with explicit 3D Gaussian primitives that can be rasterized directly.
- **Design principle**: Locate the speed limit in the combination of representation and rendering, rather than in one isolated operation.

### Case 3 — DUSt3R (Wang et al. 2024)

Traditional SfM requires camera intrinsics and is sensitive to errors passed between stages. One could replace only matching or triangulation with a neural network, but Wang et al. changed the output representation itself. Given two views, the model predicts pointmaps in a common coordinate frame, obtaining correspondence and structure together; camera intrinsics can then be recovered from the pointmaps instead of being supplied as an input. DUSt3R's contribution is this reformulation of the staged SfM pipeline as pointmap prediction.

### The Design Question Shared by the Three Papers

All three papers ask *why must this module take this form?* The answer lies less in the number of modules than in how the interface, representation, and output format are chosen.

> A contribution section should answer *why this module must take this form*. Explaining only why the problem matters remains motivation; novelty appears when the basis for the design choice is also explicit.

For a fuller treatment of motivation and method in a paper, see [*Research Notes* Ch.23 — Introduction](../../research-notes/guide.html#chapter-23) and [Ch.25 — Method](../../research-notes/guide.html#chapter-25).

---

# Ch.19 — Using AI Coding Agents

## 19.1 The Order of Work at the Robot

AI coding agents are useful for small ROS2 nodes, launch files, log summaries, and experiment-script cleanup. In a robot experiment, first capture the current state in command output: whether the sensors are visible, the topics exist, the QoS settings match, and the devices are mapped into the container.

A detailed workflow is available in [*Researching with AI and Robotics*, Ch.11 — Record the Conditions of Robot Experiments](../../ai-research-practice/guide.html#part-03-rules-11장-로봇-실험의-조건을-기록한다). This chapter keeps only the runtime observations needed while reading `robotics-practice`.

## 19.2 Runtime Signals to Capture First

The robot's current state is not visible from code alone. Check ROS2 topics, DDS QoS, `/clock`, the TF buffer, Docker device mapping, the USB bus, the LiDAR IP address, camera exposure, and the Jetson architecture in command output.

When a problem appears, first inspect these signals.

```bash
ros2 topic list
ros2 topic info /camera/image_raw --verbose
ros2 node list
ros2 topic hz /cmd_vel
dmesg | tail -30
lsusb -t
```

For LiDAR and networked sensors, inspect the packets first.

```bash
ping 192.168.1.201
sudo tcpdump -i eth0 udp port 2368 -c 10
```

Use the commands below to inspect camera devices and supported video formats.

```bash
v4l2-ctl --list-devices
v4l2-ctl -d /dev/video0 --list-formats-ext
```

Without these outputs, defer the diagnosis. Correct code still fails when a required device is missing inside the container, when the QoS settings form an incompatible combination, or when multiple sensors share the same USB controller and run short of bandwidth and power.

## 19.3 Information to Attach to a Question

For a robot runtime problem, attach at least the following bundle.

This bundle is the basis for evaluating an answer. Before executing a suggestion, verify package support, the current configuration, device permissions, the target architecture, and the metric conditions.

## 19.4 Where to Read Next

For a detailed checklist on robot runtime and collaboration with an AI agent, see [`ai-research-practice` Ch.11 and Appendix F](../../ai-research-practice/guide.html#part-03-rules-11장-로봇-실험의-조건을-기록한다).

Chapters 1–10 and 12–13 of the same guide cover the general principles for turning an AI answer into research action. Reading them together with the runtime checklist clarifies how to compare papers with code, preserve the conditions behind experimental numbers, and connect reviewer claims to evidence.

---

# Ch.20 — Further Reading

The resources are grouped by topic, and the **Learning Path** section at the bottom orders them by prerequisite background.

## 20.1 Textbooks

### Computer Vision

**Multiple View Geometry in Computer Vision** (Hartley & Zisserman)
- The core reference on multi-view geometry
- Camera models, Epipolar Geometry, 3D reconstruction
- Mathematically rigorous and useful as a reference for selected topics
- Link: [Cambridge University Press](https://www.cambridge.org/core/books/multiple-view-geometry-in-computer-vision/0B6F289C78B2B23F596CAA76D3D43F7A)
- Some chapter PDFs are available on the authors' page: https://www.robots.ox.ac.uk/~vgg/hzbook/

**Computer Vision: Algorithms and Applications** (Szeliski)
- A comprehensive CV textbook
- The latest edition (2022) includes deep learning
- **Free PDF available**
- Free PDF: https://szeliski.org/Book/

### Robotics

**Probabilistic Robotics** (Thrun, Burgard, Fox)
- The standard text on probabilistic robotics
- Kalman Filter, Particle Filter, SLAM
- Required reading — if you want to research SLAM, you must read it
- Link: [MIT Press](https://mitpress.mit.edu/9780262201629/probabilistic-robotics/)
- The PDF is not officially free, but the authors' lecture slides cover most of the content

**State Estimation for Robotics** (Tim Barfoot)
- Advanced state estimation
- Lie Groups, Factor Graph — mathematically deep but the exposition is approachable
- **Free PDF available**
- Free PDF: http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17.pdf

### Deep Learning

**Deep Learning** (Goodfellow, Bengio, Courville)
- The standard textbook on deep learning theory
- **Free online** edition
- Free online (HTML, chapter by chapter): https://www.deeplearningbook.org/

**Dive into Deep Learning** (d2l.ai)
- Hands-on — you learn alongside code
- **Free and interactive**
- Link: https://d2l.ai/
- Supports PyTorch, TensorFlow, and JAX versions

### Math supplements

**Introduction to Linear Algebra** (Gilbert Strang)
- A classic that explains linear algebra intuitively
- Maximum effect when paired with the MIT OCW lectures
- Link: https://math.mit.edu/~gs/linearalgebra/ila6/indexila6.html

**Convex Optimization** (Boyd & Vandenberghe)
- The standard text on optimization theory
- **Free PDF available**
- Free PDF: https://web.stanford.edu/~boyd/cvxbook/

## 20.2 Online Courses

### Computer Vision

**CS231n: Convolutional Neural Networks for Visual Recognition** (Stanford)
- A course on the foundations of deep-learning-based vision
- Free materials and videos
- Lectures: https://www.youtube.com/playlist?list=PL3FW7Lu3i5JvHM8ljYj-zLfQRF3EO8sYv
- Notes: https://cs231n.github.io/

**CS231A: Computer Vision, From 3D Reconstruction to Recognition** (Stanford)
- 3D Vision focused
- Geometry-based
- Materials: https://web.stanford.edu/class/cs231a/

### SLAM

**Cyrill Stachniss SLAM Course** (YouTube)
- SLAM theory lectures — German accent, but the explanations are genuinely clear
- The most recommended course for SLAM beginners
- YouTube: https://www.youtube.com/playlist?list=PLgnQpQtFTOGQrZ4O5QzbIHgl3b1JHimN_

**Multiple View Geometry** (TUM, Prof. Daniel Cremers)
- Available on YouTube — mathematically rigorous lectures
- YouTube: https://www.youtube.com/playlist?list=PLTBdjV_4f-EJn6udZ34tht9EVIW7lbeo4

**SLAM introduction (Korean)**:
- Study materials from the SLAM KR community (a Korean SLAM researchers' group)

### ROS

**ROS2 official tutorials**
- The most up-to-date information
- Link: https://docs.ros.org/en/humble/Tutorials.html (Humble)
- For other versions such as ROS2 Iron/Jazzy, switch via the dropdown at the top

**The Construct** (online platform)
- Dedicated ROS courses
- Partly free
- Link: https://www.theconstructsim.com/

### Deep learning fundamentals

**CS229: Machine Learning** (Stanford, Andrew Ng)
- ML foundations — recommended to watch before deep learning
- YouTube: https://www.youtube.com/playlist?list=PLoROMvodv4rMiGQp3WXShtMGgzqpfVfbU

**Neural Networks: Zero to Hero** (Andrej Karpathy)
- Learn neural networks by building them from scratch
- Explanations paired with code
- YouTube: https://www.youtube.com/playlist?list=PLAqhIrjkxbuWI23v9cThsA9GvCAUhRvKZ

## 20.3 Recommended YouTube Channels

These YouTube channels divide the material into shorter segments than a textbook or full course.

| Channel | Topic | Notes |
| --- | --- | --- |
| **Cyrill Stachniss** | SLAM, Robotics | Systematic SLAM explanations at an undergraduate-course level |
| **First Principles of Computer Vision** (Shree Nayar) | Computer Vision | Explains CV fundamentals concept by concept |
| **Andrej Karpathy** | Deep Learning, AI | Former Tesla AI Director. Builds neural nets from scratch |
| **Yannic Kilcher** | Paper reviews | Weekly reviews of the latest ML/AI papers. You learn how to read papers |
| **Two Minute Papers** | AI research trends | Introduces the latest research in 2-3 minute videos. "What a time to be alive!" |
| **3Blue1Brown** | Math visualization | Visual explanations of linear algebra and calculus. When math gets stuck |
| **Computerphile** | CS broadly | Wide range of computer science topics explained simply |
| **sentdex** | Python, ML | ML/robotics practice with Python. Code-centric |
| **The Coding Train** | Algorithm visualization | Understand algorithms visually. Energetic delivery |

**Link list**:
- Cyrill Stachniss: https://www.youtube.com/@CyrillStachniss
- First Principles of Computer Vision: https://www.youtube.com/@firstprinciplesofcomputerv3258
- Andrej Karpathy: https://www.youtube.com/@AndrejKarpathy
- Yannic Kilcher: https://www.youtube.com/@YannicKilcher
- Two Minute Papers: https://www.youtube.com/@TwoMinutePapers
- 3Blue1Brown: https://www.youtube.com/@3blue1brown
- Computerphile: https://www.youtube.com/@Computerphile
- sentdex: https://www.youtube.com/@sentdex
- The Coding Train: https://www.youtube.com/@TheCodingTrain

## 20.4 Reading Papers

### How to read them

[*Research Notes* Ch.6–15 — Reading](../../research-notes/guide.html#chapter-6) covers how to select papers, Keshav's three-pass method, the 5 Cs, the reviewer perspective, and the CCC lens *(Korean only)*.

### Must-read paper list

**Deep Learning fundamentals**:
- ResNet: He et al., 2015 — [arXiv:1512.03385](https://arxiv.org/abs/1512.03385)
- Transformer (Attention Is All You Need): Vaswani et al., 2017 — [arXiv:1706.03762](https://arxiv.org/abs/1706.03762)
- ViT: Dosovitskiy et al., 2020 — [arXiv:2010.11929](https://arxiv.org/abs/2010.11929)

**Recent trends**:
- RT-2: Brohan et al., 2023 — [arXiv:2307.15818](https://arxiv.org/abs/2307.15818)
- 3D Gaussian Splatting: Kerbl et al., 2023 — [arXiv:2308.04079](https://arxiv.org/abs/2308.04079)
- Depth Anything: Yang et al., 2024 — [arXiv:2401.10891](https://arxiv.org/abs/2401.10891)

> For paper search, use [Google Scholar](https://scholar.google.com/), [Semantic Scholar](https://www.semanticscholar.org/), and [arXiv](https://arxiv.org/). Papers With Code, which used to show benchmark rankings alongside code links, shut down in 2025; its domain now redirects to Hugging Face.

### Paper-writing tools

> **Further reading**
> - [Overleaf](https://www.overleaf.com/) — online LaTeX editor with collaborative editing
> - [Mathpix](https://mathpix.com/) — convert equation screenshots into LaTeX code
> - [Detexify](http://detexify.kirelabs.org/classify.html) — draw a symbol by hand to search for its LaTeX
> - [Tables Generator](https://www.tablesgenerator.com/) — LaTeX/HTML table generator
> - [QuillBot](https://quillbot.com/) — English paraphrasing tool. Useful for paper writing in English
> - [Ludwig](https://ludwig.guru/) — English phrase search engine. Check what native speakers actually write
> - [DL Monitor (deeplearn.org)](https://deeplearn.org/) — automatically tracks deep learning papers from major venues and arXiv

## 20.5 Major Conferences

The *Conferences by field* table in [Research Notes Ch.34](../../research-notes/guide.html#chapter-34) summarizes the schedules and character of venues in CV, robotics, and autonomous driving *(Korean only)*.

The same chapter discusses why researchers attend conferences and how to open a presentation.

## 20.6 Useful GitHub Repositories

### SLAM

```
# ORB-SLAM3 — reference for Visual(-Inertial) SLAM
https://github.com/UZ-SLAMLab/ORB_SLAM3

# VINS-Fusion — multi-camera + IMU fusion
https://github.com/HKUST-Aerial-Robotics/VINS-Fusion

# LIO-SAM — LiDAR-Inertial SLAM (factor graph based)
https://github.com/TixiaoShan/LIO-SAM

# FAST-LIO2 — fast LiDAR-Inertial Odometry
https://github.com/hku-mars/FAST_LIO

# RTAB-Map — RGB-D SLAM, supports large-scale environments
https://github.com/introlab/rtabmap

# SplaTAM — SLAM based on 3D Gaussian Splatting
https://github.com/spla-tam/SplaTAM
```

### Deep Learning

```
# Ultralytics YOLO — YOLOv8/v11, the easiest detection framework to use
https://github.com/ultralytics/ultralytics

# HuggingFace Transformers — NLP/Vision model hub
https://github.com/huggingface/transformers

# OpenMMLab — comprehensive framework for Detection, Segmentation, 3D, etc.
https://github.com/open-mmlab

# PyTorch Lightning — structuring training code
https://github.com/Lightning-AI/pytorch-lightning

# timm (PyTorch Image Models) — collection of pretrained Vision models
https://github.com/huggingface/pytorch-image-models
```

### 3D Vision

```
# Open3D — point cloud and mesh processing
https://github.com/isl-org/Open3D

# 3D Gaussian Splatting — original implementation
https://github.com/graphdeco-inria/gaussian-splatting

# NeRF Studio — unified framework for NeRF/3DGS
https://github.com/nerfstudio-project/nerfstudio

# Depth Anything V2 — general-purpose depth estimation
https://github.com/DepthAnything/Depth-Anything-V2

# COLMAP — Structure from Motion pipeline
https://github.com/colmap/colmap
```

### VFM/VLA

```
# Segment Anything (SAM) — Meta's general-purpose segmentation
https://github.com/facebookresearch/segment-anything

# SAM 2 — extended to video
https://github.com/facebookresearch/sam2

# DINOv2 — Self-supervised vision features
https://github.com/facebookresearch/dinov2

# Grounded-SAM — find objects by text + segmentation
https://github.com/IDEA-Research/Grounded-Segment-Anything

# OpenVLA — open-source Vision-Language-Action model
https://github.com/openvla/openvla
```

### ROS / robot development

```
# ROS2 official repository
https://github.com/ros2

# Nav2 — ROS2 navigation stack
https://github.com/ros-navigation/navigation2

# MoveIt2 — motion planning for robot arms
https://github.com/moveit/moveit2

# micro-ROS — ROS for microcontrollers
https://github.com/micro-ROS
```

### Useful Awesome lists

```
# Awesome SLAM — comprehensive SLAM resources
https://github.com/SilenceOverflow/Awesome-SLAM

# Awesome Robotics — comprehensive robotics resources
https://github.com/kiloreux/awesome-robotics

# Awesome 3D Gaussian Splatting — 3DGS papers and code
https://github.com/MrNeRF/awesome-3D-gaussian-splatting
```

## 20.7 Recommended Learning Path

This extends the learning path from Section 1.4. Each stage lists concrete materials and links, so start from whichever stage matches your level.

### Beginner stage (1-3 months)

**Goal**: Acquire basic tools — reach a state of "being able to run something".

| Topic | What to learn | Recommended resources |
| --- | --- | --- |
| Python fluency | Syntax, classes, file I/O | [Jump to Python](https://wikidocs.net/book/1) (free, Korean) |
| NumPy, OpenCV basics | Array operations, reading/processing images | [OpenCV official tutorials](https://docs.opencv.org/4.x/d6/d00/tutorial_py_root.html) |
| Linear algebra review | Matrices, eigenvalues, SVD | [3Blue1Brown: Essence of Linear Algebra](https://www.youtube.com/playlist?list=PLZHQObOWTQDPD3MizzM2xVFitgF8hE_ab) |
| Probability/statistics review | Bayes' rule, Gaussians | [StatQuest](https://www.youtube.com/@statquest) |
| ROS2 basics | Nodes, topics, services | [ROS2 official tutorials](https://docs.ros.org/en/humble/Tutorials.html) |
| Git usage | commit, branch, PR | [Git introduction](https://backlog.com/git-tutorial/kr/) (Korean) |

**Exercises**:
- Image processing with OpenCV (grayscale conversion, edge detection, feature extraction)
- Write a simple ROS2 node (publisher/subscriber)
- Perform camera calibration — see Chapter 9 of this document
- Read **Chapters 3 and 9** of this document to understand coordinate transformations and camera models

**Milestone**: If you can read an image in Python, extract keypoints, and visualize matches between two images, you have graduated from the beginner stage.

### Intermediate stage (3-6 months)

**Goal**: Understand the core techniques — reach a state of "being able to read a paper and run its code".

| Topic | What to learn | Recommended resources |
| --- | --- | --- |
| Deep learning basics (PyTorch) | CNN, training, backpropagation | [CS231n](https://www.youtube.com/playlist?list=PL3FW7Lu3i5JvHM8ljYj-zLfQRF3EO8sYv) + [PyTorch official tutorials](https://pytorch.org/tutorials/) |
| Object Detection | YOLO, Faster R-CNN | [Ultralytics docs](https://docs.ultralytics.com/) + Chapter 10 of this document |
| Understanding Visual SLAM | ORB-SLAM3 analysis | [Cyrill Stachniss SLAM lectures](https://www.youtube.com/playlist?list=PLgnQpQtFTOGQrZ4O5QzbIHgl3b1JHimN_) + Chapter 14 of this document |
| Point cloud processing | Using Open3D | [Open3D tutorials](http://www.open3d.org/docs/release/tutorial/) + Chapter 13 of this document |
| Depth Estimation | Monocular depth estimation | Chapter 10 of this document + [Depth Anything code](https://github.com/DepthAnything/Depth-Anything-V2) |

**Exercises**:
- Work with the KITTI dataset — [KITTI homepage](https://www.cvlibs.net/datasets/kitti/)
- YOLOv8 fine-tuning — fine-tune on a custom dataset
- Run and analyze ORB-SLAM3 — evaluate on the TUM RGB-D dataset
- TUM RGB-D benchmark — compute ATE and RPE yourself
- Read **Chapters 9-14** of this document to solidify the theoretical background

**Milestone**: If you can build ORB-SLAM3 yourself, run it on a dataset, and compare the trajectory against ground truth, you have graduated from the intermediate stage.

### Advanced stage (6 months+)

**Goal**: Develop research ability — reach a state of "being able to propose and test new ideas".

| Topic | What to learn | Recommended resources |
| --- | --- | --- |
| VFMs in depth (extending the intermediate-stage item from Section 1.4) | DINOv2, SAM, CLIP | Chapters 10-11 of this document + read the papers directly |
| Advanced 3D reconstruction | NeRF, 3D Gaussian Splatting | [NeRF Studio](https://github.com/nerfstudio-project/nerfstudio) + Chapter 13 of this document |
| Reading and implementing papers | Analyzing the latest papers | [Hugging Face Papers](https://huggingface.co/papers/trending) + [Yannic Kilcher's channel](https://www.youtube.com/@YannicKilcher) — *the full guide is [*Research Notes* Ch.6–15 — Reading](../../research-notes/guide.html#chapter-6)* |
| Experimenting with new ideas | Hypothesis formulation, experimental design | Lab seminars + attending conference workshops — *the full guide is [*Research Notes* Ch.1–5 — Starting](../../research-notes/guide.html#chapter-1)* |
| Benchmark evaluation | Quantitative comparison | Standard benchmarks per subfield (KITTI, ScanNet, Replica, etc.) — *the result-interpretation frame is [Research Notes Ch.32](../../research-notes/guide.html#chapter-32)* |

**Exercises**:
- Analyze the code of recent papers — clone from GitHub and run it yourself
- Experiment with your own improvement ideas — try "what if I change this part?"
- Attempt paper writing — *the full frame is [Research Notes Part 2](../../research-notes/guide.html#chapter-16)*
- Read **Chapters 10-13** of this document to track recent research directions

**Milestone**: If you can run an experiment that modifies or improves an existing paper's method and compare the result quantitatively, you have entered the advanced stage. Aim to reach a level where the work could be submitted to a conference workshop.

### Learning order summary

```
Beginner (1-3 months)            Intermediate (3-6 months)          Advanced (6 months+)
─────────────                   ─────────────                    ─────────────
Python + NumPy                  PyTorch + CNN                    VFM (DINOv2, SAM)
OpenCV basics                   YOLO fine-tuning                 3DGS / NeRF
Linear algebra/prob review      ORB-SLAM3 analysis               Paper implementation
ROS2 basics                     KITTI/TUM benchmarks             Idea experimentation
Git usage                       Point clouds (Open3D)            Paper writing
                                Depth Estimation
     ↓                                ↓                                ↓
 "can run code"                "can read and reproduce papers"   "can experiment with new ideas"
```

## 20.8 Research Skills

*Graduate level.* Paper reading and writing, experimental design, conference presentations, and peer review are treated in detail in [Research Notes](../../research-notes/guide.html); long-term PhD management is covered in [Grad Notes](../../grad-notes/guide.html). This section links the parts that apply directly to SLAM, CV, and robotics.

Research practice requires attention to direction, a sustainable way of working, and tools at the same time.

### 20.8.0 Researcher Mindset

- Direction, engine, and tools as three layers within one frame → [*Grad Notes* Ch.17 — Conditions Under Which Research Becomes a Life](../../grad-notes/guide.html#chapter-17), §5
- The weight of autonomy, Hyun's *everything is optimization*, and the optimization horizon as a long game → [*Grad Notes* Ch.14 — The Weight of Autonomy](../../grad-notes/guide.html#chapter-14), §1
- Consistency versus explosive growth, and the trap of comparing pace with the person next to you → [*Grad Notes* Ch.15 — The Comparison Trap](../../grad-notes/guide.html#chapter-15), §3
- Solid foundations and the frame of a paper with no obvious reason for rejection → [*Research Notes* Ch.16 — Mindset](../../research-notes/guide.html#chapter-16), §1

### 20.8.1 Writing a Paper

The Abstract → Introduction → Related Work → Method → Experiments → Conclusion structure, and the progression from problem and prior limitation to approach and contributions in an introduction, are covered in [*Research Notes* Part 2 — Writing](../../research-notes/guide.html#chapter-16) and [Ch.23 — Introduction](../../research-notes/guide.html#chapter-23).

### 20.8.2 Experimental Design and Ablation

SLAM and CV experiments require ablation, control of variables, repeated trials, and comparisons on the same data, split, and hardware. Copying baseline numbers from another paper can make a comparison unfair when the conditions differ.

### 20.8.3 Conference Presentations

Slide budgets for 5-minute and 20-minute talks, and the 30-second elevator pitch and 2-minute walk-through for posters, are discussed more fully in [*Research Notes* Part 4 — Presenting](../../research-notes/guide.html#chapter-33).

### 20.8.4 Reviewing a Paper — Peer Review

For a reviewer-oriented checklist covering novelty, soundness, experiments, clarity, and reproducibility, as well as constructive feedback and rebuttals, see [*Research Notes* Ch.10 — Reading as a Reviewer](../../research-notes/guide.html#chapter-10) and [Ch.32 — Revision/Rebuttal](../../research-notes/guide.html#chapter-32).

### 20.8.5 Tools

- LaTeX: Overleaf or local installation (TeX Live + VS Code)
- References: Mark the primary text in a PDF reader and manage metadata with Zotero + Better BibTeX. Check AI-generated summaries, related-work comparisons, and draft BibTeX against the paper and DOI metadata.
- Pipeline figures: TikZ for precision, draw.io for speed, or Inkscape for SVG
- Tables: `booktabs` (`\toprule`, `\midrule`, `\bottomrule`)
- Algorithms: `algorithm2e`
- Equations: maintain a notation table and apply it consistently across the paper

For consistent LaTeX notation, notation tables, and equation explanations, see [*Research Notes* Ch.30 — Writing Equations, Theorems, and Proofs](../../research-notes/guide.html#chapter-30).

> Recommended references:
> - [How to Write a Great Research Paper (Simon Peyton Jones, Microsoft Research)](https://www.microsoft.com/en-us/research/academic-program/write-great-research-paper/) — a classic talk on paper writing
> - [How to Read a Paper (S. Keshav)](http://ccr.sigcomm.org/online/files/p83-keshavA.pdf) — the 3-pass reading method
> - [Tips for Writing Technical Papers (Jennifer Widom, Stanford)](https://cs.stanford.edu/people/widom/paper-writing.html) — concise, practical advice

---

# Ch.21 — Appendix

## A. Glossary

### A.1 Abbreviations

| Abbr. | Expansion | Description |
| --- | --- | --- |
| SLAM | Simultaneous Localization and Mapping | simultaneous localization and mapping |
| VO | Visual Odometry | visual odometry |
| VIO | Visual-Inertial Odometry | visual-inertial odometry |
| LIO | LiDAR-Inertial Odometry | LiDAR-inertial odometry |
| IMU | Inertial Measurement Unit | inertial measurement unit |
| DoF | Degrees of Freedom | degrees of freedom |
| SE(3) | Special Euclidean Group (3D) | 3D rigid-body transformation group |
| SO(3) | Special Orthogonal Group (3D) | 3D rotation group |
| FoV | Field of View | field of view |
| ToF | Time of Flight | time of flight (distance-measurement method) |
| CNN | Convolutional Neural Network | convolutional neural network |
| ViT | Vision Transformer | vision Transformer |
| VFM | Vision Foundation Model | vision foundation model |
| VLA | Vision-Language-Action | vision-language-action model |
| VLM | Vision-Language Model | vision-language model |
| LLM | Large Language Model | large language model |
| mAP | mean Average Precision | mean average precision |
| ICP | Iterative Closest Point | iterative closest point |
| NDT | Normal Distributions Transform | normal distributions transform |
| NeRF | Neural Radiance Fields | neural radiance fields |
| 3DGS | 3D Gaussian Splatting | 3D Gaussian splatting |
| BEV | Bird's Eye View | bird's-eye view |
| TSDF | Truncated Signed Distance Function | truncated signed distance function |
| BA | Bundle Adjustment | bundle adjustment |
| PGO | Pose Graph Optimization | pose graph optimization |
| DDS | Data Distribution Service | ROS2's communication middleware |
| ONNX | Open Neural Network Exchange | model conversion format |
| TRT | TensorRT | NVIDIA's inference optimization engine |
| ATE | Absolute Trajectory Error | absolute trajectory error |
| RPE | Relative Pose Error | relative pose error |

### A.2 Terms

**Keyframe**: A selected frame that carries significant information. Processing every frame is too slow, so only frames with meaningful changes are picked and used.

**Loop Closure**: Drift correction through recognition of a previously visited location. "Ah, we were here before" → accumulated error gets corrected all at once.

**Drift**: Accumulation of error. Walking 100 m at a 1 m stride with 1 cm of error per step, if those errors all accumulate in the same direction, leaves 100 cm of error on arrival. If their directions are uncorrelated, the error grows with the square root of the number of steps and stays on the order of 10 cm.

**Reprojection Error**: The error when a 3D point is reprojected onto the image. The difference between the predicted "where should this 3D point appear in the camera image" and the actual observation.

**Feature Descriptor**: A vector that describes the neighborhood around a keypoint. When finding the same point across two images, these vectors are compared.

**Homography**: A transformation between planes. Used when registering two photos taken of a desktop.

**Essential Matrix**: The geometric relation between a calibrated camera pair. 5 DoF (3 for rotation + 2 for translation direction).

**Fundamental Matrix**: The geometric relation between an uncalibrated camera pair. 7 DoF.

**Epipole**: The point where the center of one camera is projected onto the other camera's image.

**Zero-shot**: Performing a new task without training. "Find the cat" works even though "cat" was never trained on.

**Few-shot**: Learning a new task from a small number of examples. Learns from just 3–5 examples.

**Fine-tuning**: Retraining a pretrained model for a specific task. Adjusting a large model to your own data.

**Domain Adaptation**: Adapting from a source domain to a target domain. Train in simulation → deploy in the real environment.

**Sim-to-Real**: Transferring from simulation to the real environment. The canonical case of domain adaptation.

**Gaussian Splatting**: A method that represents a 3D scene with millions of 3D Gaussians. Faster than NeRF and editable.

**Factor Graph**: Representing constraints between variables as a graph. The core data structure of SLAM optimization.

**Knowledge Distillation**: A technique for transferring the knowledge of a large model (teacher) to a small model (student).

## B. Frequently Asked Questions (FAQ)

**Q: Should I learn Python or C++ first?**

A: Start with the language of the lab code you need to read and run first. C++ is common in SLAM, ROS packages, and real-time control modules, so researchers need it to read and modify lab code. Python is used mainly for deep-learning scripts and data preprocessing. AI coding agents can assist with writing either language, but researchers must still understand existing code and verify its behavior.

**Q: Can I do research without a GPU?**

A: Simple experiments are possible on CPU. But a GPU is essential for deep-learning training. Use Google Colab (free) or the lab server. The free version of Colab is enough for something like YOLO fine-tuning.

**Q: Should I learn ROS1 or ROS2?**

A: If you are learning from scratch, ROS2 is recommended. ROS1's official support ended (EOL) in 2025. However, if the package you want to use only supports ROS1, you may have no choice but to learn ROS1 first. That said, knowing ROS1 makes ROS2 quick to pick up.

**Q: Where do I find papers?**

A: Use [arXiv](https://arxiv.org/) (free preprint server) and [Google Scholar](https://scholar.google.com/) (paper search). If you also need code, check the paper's GitHub link or [Hugging Face Papers](https://huggingface.co/papers/trending) (Papers With Code shut down in 2025). If you want to browse by venue, [CVPR Open Access](https://openaccess.thecvf.com/) and [IEEE Xplore](https://ieeexplore.ieee.org/) are also useful.

**Q: Where should I start to study SLAM?**

A: Start with Cyrill Stachniss's [YouTube SLAM lectures](https://www.youtube.com/playlist?list=PLgnQpQtFTOGQrZ4O5QzbIHgl3b1JHimN_) and analyze the ORB-SLAM3 code. Reading Ch.9 (camera models) and Ch.14 (visual odometry) of this document beforehand will make the lectures much easier to follow.

**Q: How do I find research ideas?**

A: Read the Limitations section of recent conference papers. You can get ideas from unresolved problems. Another approach is to connect two fields that have not yet been fully merged. The name of a combination alone, however, does not establish that the space is unexplored. Several papers on "3D Gaussian Splatting + Semantic SLAM" already exist, and Ch.18 of this document also covers Gaussian maps that carry semantic information. Survey the prior work first, then look for the space that is left.

**Q: Which GPU should I buy?**

A: First measure whether the model, optimizer state, activations, and batch fit in **VRAM** under the intended configuration. Then consider precision support, memory bandwidth, power, framework compatibility, and an application-level benchmark. The table narrows candidates by memory class; it is not a purchase ranking.

**Personal (desktop)**

| VRAM | Example cards | Role to evaluate | Check before buying |
|------|---------------|------------------|---------------------|
| 8GB | RTX 4060, RTX 5060 Ti 8GB | Small-CNN training, inference with a limited batch | Peak memory of the intended model; VFM fine-tuning may not fit |
| 12GB | RTX 3060 12GB, RTX 4070 | Mid-sized inference and training experiments | Generation-specific runtime and used-card condition |
| 16GB | RTX 5060 Ti 16GB, RTX 4070 Ti Super | Larger batches, VFM inference, mid-scale training | Model-specific activation and optimizer memory |
| 24GB | RTX 3090, RTX 4090 | Training that fits within 24GB, 3DGS and VLA experiments | Power, cooling, used warranty, and runtime differences |
| 32GB | RTX 5090 | Local experiments that exceed 24GB | Power, case, PSU, and software support |

**Server/lab (data center)**

| GPU memory | Card | Characteristics | Official specifications |
|------------|------|-----------------|-------------------------|
| 16/32GB | V100 SXM2 | First-generation Tensor Cores; no TF32 or BF16 | [V100 data center GPU](https://www.nvidia.com/en-us/data-center/v100/) |
| 24GB | A10 | PCIe inference and graphics family | [A10 datasheet](https://www.nvidia.com/content/dam/en-zz/Solutions/Data-Center/a10/pdf/a10-datasheet.pdf) |
| 40/80GB | A100 | TF32, BF16, MIG, PCIe/SXM variants | [A100 specifications](https://www.nvidia.com/en-us/data-center/a100/) |
| 80GB | H100 SXM | Hopper, Transformer Engine, NVLink | [H100 specifications](https://www.nvidia.com/en-us/data-center/h100/) |
| 141GB | H200 SXM | 141GB HBM3e and 4.8TB/s memory bandwidth | [H200 specifications](https://www.nvidia.com/en-us/data-center/h200/) |
| 180GB | B200 | Blackwell and 180GB HBM3e; delivered in server configurations | [DGX B200 specifications](https://www.nvidia.com/en-us/data-center/dgx-b200/) |

TFLOPS denotes trillions of floating-point operations per second at a stated precision; it is a theoretical peak, not an application benchmark. FP32 is the conventional 32-bit format, while TF32, BF16, FP16, and FP8 trade precision and range differently for accelerated tensor operations. When reading specifications, distinguish precision, CUDA cores from Tensor Cores, dense from structured-sparsity figures, and PCIe from SXM variants. Equal peak TFLOPS does not imply equal training time because memory bandwidth, kernels, batch size, and data loading differ. The benefit of `torch.amp` is also model- and hardware-dependent, so compare cards with a short run of the same repository, batch, and precision.

**Notes**:
- For cards such as the RTX 5060 Ti that come with 8GB and 16GB options, estimate the VRAM required by the intended model and batch. Eight gigabytes can be restrictive for local VFM work.
- When considering an AMD GPU, check whether the required frameworks and libraries support ROCm. CUDA-only dependencies add migration cost.
- If the lab server has an A100 or H100, a personal GPU may serve mainly for debugging and prototyping. Check the server specifications and availability before purchasing.
- A used RTX 3090 is a 24GB option, but price, warranty, and cooling condition vary by listing. Check rated power and the PSU and case requirements.
- Colab and cloud-GPU prices, assigned GPU types, and usage limits change. Benchmark the real workload on a rented GPU before buying, but verify the current price and quota on the provider page.

**Q: How many papers should I read per day?**

A: The purpose and depth of the reading matter more than a daily paper count. At first, reading one paper carefully each week with the three-pass method in §20.4 can be more useful. With experience, the abstract alone becomes enough to judge a paper's type and relevance. Reading for a lab meeting also differs from reading for one's own research, which may extend to code analysis.

**Q: I am not good at coding — can I still do research?**

A: Coding agents such as Claude and Copilot can quickly draft requests such as "build a KITTI dataset loader" or "add wandb logging to this training loop." They reduce the time spent typing, but the output still needs review.

Judging generated code still requires domain knowledge. An agent may not reliably identify why a DataLoader is slow with `num_workers=0`, why a loss becomes NaN, or where a coordinate frame is reversed in SLAM code (see Ch.14). Run the code and compare it with existing implementations before accepting the result.

Reading projects such as ORB-SLAM3, Ultralytics, and HuggingFace Transformers and tracing their design choices helps develop code-review skills.

**Q: How do I prepare a conference presentation?**

A: Conference presentations are largely divided into **oral presentations** and **poster presentations**. Talk length, poster dimensions, and presentation language vary by venue, so the official presenter instructions take precedence.

- **Poster**: A0 is a common size, but check the venue's specification. Use large figures and little text so a passerby can locate the topic and result quickly. Rehearse in front of lab colleagues.
- **Oral**: Fifteen to twenty minutes is a common example, not a rule; the session limit comes first. Set the slide count from that limit and keep one message per slide. Prepare a demo video and supplementary slides when they help.
- **Common**: Confirm the presentation language, then rehearse from a script while prioritizing delivery and timing over memorization.

**Q: Reading English papers is too hard — what do I do?**

A: Repeated reading and accumulated domain knowledge reduce the burden. A few practical steps help:

- **Grasp the structure first**: Many experimental papers use Introduction → Related Work → Method → Experiments → Conclusion, but contributions can also lie in problem formulation, data, evaluation, or analysis. Use the title and headings to identify the paper's actual structure.
- **Learn field-specific vocabulary first**: Expressions like "ablation study", "state-of-the-art", and "we empirically show" recur. The number of papers needed for familiarity depends on the reader's background and field.
- **Do not be embarrassed to use translation tools**: Translating unknown sentences with DeepL or Google Translate is not embarrassing at all. That said, if you rely only on translation, your English will not improve. Read in the order "original → check translation → back to original".
- **Use the highlighter in your PDF reader**: Marking important sentences makes them easier to find again. Use whichever tool is comfortable, such as Adobe Acrobat or Zotero's built-in viewer.

## C. Troubleshooting Guide

**Frequently used apt commands**

```bash
sudo apt update                  # refresh the package list
sudo apt upgrade                 # upgrade installed packages
sudo apt install <package>       # install a package
sudo apt remove <package>        # remove a package (keep config files)
sudo apt purge <package>         # fully remove package + config files
sudo apt autoremove              # remove unused dependencies
apt list --installed             # list installed packages
apt search <keyword>             # search for a package
sudo apt --fix-broken install    # recover from broken dependencies
```

(Reference: [Jinyong Jeong's blog](https://jinyongjeong.github.io/2016/06/07/Ubuntu_apt_get_commend/))

**SSH key setup (server access without a password)**

```bash
# Generate a key (hit Enter repeatedly to accept defaults)
ssh-keygen -t ed25519

# Copy the public key to the server
ssh-copy-id user@server_ip

# Connect without a password afterward
ssh user@server_ip
```

Registering the same public key (`~/.ssh/id_ed25519.pub`) on GitHub also removes the need for a password on `git push` for SSH remotes (`git@github.com:...`). A repository cloned over HTTPS uses a token or a credential helper instead, so this key does not apply to it. (Reference: [Jinyong Jeong's blog](https://jinyongjeong.github.io/2016/06/02/SSH_keygen_setting/))

**CPU performance mode setup (for experiments)**

In SLAM or deep-learning experiments, CPU throttling sometimes makes performance uneven.

```bash
# Check the current CPU governor
cat /sys/devices/system/cpu/cpu0/cpufreq/scaling_governor

# Switch to performance mode (all cores)
echo performance | sudo tee /sys/devices/system/cpu/cpu*/cpufreq/scaling_governor

# Persistent setting (survives reboot)
sudo apt install cpufrequtils
echo 'GOVERNOR="performance"' | sudo tee /etc/default/cpufrequtils
sudo systemctl restart cpufrequtils
```

On laptops, battery drain increases, so use it only while plugged in. (Reference: [Jinyong Jeong's blog](https://jinyongjeong.github.io/2020/02/04/Ubuntu_cpu_freq_change/))

### C.1 CUDA / PyTorch

**Problem**: `CUDA out of memory`

**Solution**:

```python
# 1. Reduce batch size (try this first)
batch_size = 16  # → 8 or 4

# 2. Clear memory
torch.cuda.empty_cache()

# 3. Use gradient accumulation (keep the effective batch, save memory)
accumulation_steps = 4
for i, (inputs, labels) in enumerate(dataloader):
    loss = model(inputs, labels) / accumulation_steps
    loss.backward()
    if (i + 1) % accumulation_steps == 0:
        optimizer.step()
        optimizer.zero_grad()

# 4. Mixed Precision Training (halves memory)
from torch.cuda.amp import autocast, GradScaler
scaler = GradScaler()
with autocast():
    output = model(input)
    loss = criterion(output, target)
```

**Problem**: `CUDA version mismatch` (mostly when building CUDA extensions yourself)

**Solution**:

```bash
# Check the installed CUDA version
nvcc --version

# Check the CUDA version PyTorch sees
python -c "import torch; print(torch.version.cuda)"

# The two values may differ, and that is normal. The wheel bundles its own CUDA
# runtime, so if the driver supports that runtime, leave the install alone.
# The exception is building CUDA extensions yourself: then the major versions of
# nvcc and torch.version.cuda must match, and only then should you reinstall.
pip install torch --index-url https://download.pytorch.org/whl/cu121
```

**Problem**: `RuntimeError: CUDA error: device-side assert triggered`

**Solution**: This usually happens when a label index is out of range. Running on CPU can produce a more detailed error message. The command below is a separate method that synchronizes CUDA calls on the GPU to help locate the failing operation.

```bash
CUDA_LAUNCH_BLOCKING=1 python train.py
```

### C.2 ROS

**Problem**: `Package not found`

**Solution**:

```bash
# Check that the workspace is sourced
source ~/ros2_ws/install/setup.bash

# Check that the package is installed
ros2 pkg list | grep package_name

# Add sourcing to .bashrc (so you do not do it manually every time)
echo "source ~/ros2_ws/install/setup.bash" >> ~/.bashrc
```

**Problem**: `TF tree not connected`

**Solution**:

```bash
# Check the TF tree
ros2 run tf2_tools view_frames

# Add a static transform (example)
ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 base_link camera_link
```

**Problem**: `Topic not published` / data is not coming in

**Solution**:

```bash
# List currently active topics
ros2 topic list

# Check data on a specific topic
ros2 topic echo /camera/image_raw --once

# Check for QoS mismatches (common in ROS2)
ros2 topic info /camera/image_raw -v
```

### C.3 Docker

**Problem**: `Permission denied`

**Solution**:

```bash
# Add the user to the docker group
sudo usermod -aG docker $USER
# Log out and log back in
```

**Problem**: GUI programs do not run

**Solution**:

```bash
# X11 forwarding
xhost +local:docker
docker run -it --env DISPLAY=$DISPLAY -v /tmp/.X11-unix:/tmp/.X11-unix ...
```

**Problem**: GPU not detected inside Docker

**Solution**:

```bash
# Install nvidia-container-toolkit — register the NVIDIA apt repository and key first, or the package will not be found
curl -fsSL https://nvidia.github.io/libnvidia-container/gpgkey | \
  sudo gpg --dearmor -o /usr/share/keyrings/nvidia-container-toolkit-keyring.gpg
curl -s -L https://nvidia.github.io/libnvidia-container/stable/deb/nvidia-container-toolkit.list | \
  sed 's#deb https://#deb [signed-by=/usr/share/keyrings/nvidia-container-toolkit-keyring.gpg] https://#g' | \
  sudo tee /etc/apt/sources.list.d/nvidia-container-toolkit.list
sudo apt-get update && sudo apt-get install -y nvidia-container-toolkit
sudo nvidia-ctk runtime configure --runtime=docker   # Register the hook with the Docker runtime
sudo systemctl restart docker

# Run with the GPU option added
docker run --gpus all -it nvidia/cuda:12.1.0-base-ubuntu22.04 nvidia-smi
```

### C.4 OpenCV

**Problem**: `cv2.imshow() not working`

**Solution**:

```bash
# Remove the headless OpenCV build and reinstall
pip uninstall opencv-python-headless
pip install opencv-python
```

**Problem**: OpenCV conflicts with ROS's cv_bridge

**Solution**:

```bash
# When ROS's cv_bridge references the system OpenCV,
# it can conflict with OpenCV in a conda/venv environment.
# Fix: specify the Python path explicitly when building the ROS workspace.

colcon build --cmake-args -DPython3_EXECUTABLE=/usr/bin/python3
```

### C.5 Build/compile

**Problem**: ORB-SLAM3 build error (OpenCV version conflict)

**Solution**:

```bash
# Some APIs changed in OpenCV 4.x
# Check the OpenCV version in CMakeLists.txt
find_package(OpenCV 4 REQUIRED)

# On Pangolin build errors
sudo apt-get install libglew-dev libpython2.7-dev
```

**Problem**: Eigen version errors

**Solution**:

```bash
# Check the system Eigen version
pkg-config --modversion eigen3

# Install the Eigen development package provided by the repository
sudo apt-get install libeigen3-dev
```

## D. Checklist: Things to Confirm Before Starting Research

### D.1 Environment setup

- [ ] Ubuntu installed (22.04 LTS recommended)
- [ ] NVIDIA driver installed (confirm with `nvidia-smi`)
- [ ] CUDA toolkit installed (confirm with `nvcc --version`)
- [ ] cuDNN installed (confirm a system install with `cudnn_version.h`; `torch.backends.cudnn.version()` reports the cuDNN bundled with PyTorch)
- [ ] Conda or venv environment set up
- [ ] PyTorch GPU operation confirmed (`torch.cuda.is_available()`)
- [ ] ROS2 installed (if needed; Humble on Ubuntu 22.04, Jazzy on 24.04)
- [ ] Git configured (`git config --global user.name/email`)
- [ ] Docker installed (optional, recommended for reproducibility)
- [ ] VS Code + essential extensions installed (Python, Remote-SSH, Jupyter)

### D.2 Foundational knowledge

- [ ] Python basics (classes, decorators, list comprehensions)
- [ ] NumPy array operations (broadcasting, indexing, reshape)
- [ ] OpenCV image processing (read, transform, filter, keypoints)
- [ ] Linear algebra basics (matrix multiplication, eigenvalue decomposition, SVD)
- [ ] Probability/statistics basics (Bayes' theorem, Gaussian distribution, MLE/MAP)

### D.3 Research tools

See [Research Notes Ch.15](../../research-notes/guide.html#chapter-15) for paper-reading tools (note templates, citation management), [Research Notes Part 2](../../research-notes/guide.html#chapter-16) for writing tools, and [Research Notes Ch.34](../../research-notes/guide.html#chapter-34) for conference preparation *(Korean only)*. Sections 20.4 and 20.7 cover tools and learning paths specific to Spatial AI.

### D.4 Dataset preparation

- [ ] Download the dataset relevant to your research
- [ ] Understand the data format (image size, depth units, coordinate frame)
- [ ] Implement a DataLoader (PyTorch Dataset/DataLoader)
- [ ] Write data visualization code (for debugging)

## E. First-Week Survival Guide

The first week needs a minimum task list for preparing accounts and the execution environment, then locating the code, data, and documents for the research topic. The Day 1–7 allocation below is an example; reorder it around account provisioning, equipment schedules, and the lab's onboarding process.

### Day 1–2: Build the environment

```
[ ] Get a lab server account (ask the admin)
[ ] Confirm SSH access to the server
[ ] Configure VS Code Remote-SSH
[ ] Create a conda environment on the server
[ ] Confirm PyTorch + CUDA work
[ ] Join the lab's GitHub organization
[ ] Join the Slack/Discord channel
```

> Tip: When asking a senior about the server environment, include the command you tried, the expected result, and the actual output. This information makes the problem much faster to narrow down.

### Day 3–4: Get to know the existing code

```
[ ] Clone the lab's existing code/project repositories
[ ] Read the README (if any)
[ ] Try building and running the existing code
[ ] Download datasets and configure paths
[ ] Run a simple demo
```

> Tip: Code often fails on its first run because environments, paths, and versions differ. Use the error message to check the official documentation and issue tracker before changing the setup.

### Day 5: Start reading papers

[Grad Notes Ch.4](../../grad-notes/guide.html#chapter-4) discusses how to ask for a first paper recommendation and talk with lab members, while [Grad Notes Ch.7](../../grad-notes/guide.html#chapter-7) covers setting a research direction during the first week *(Korean only)*.

> Tip: It is normal not to understand a paper on the first read. Even grasping just "what problem is this paper trying to solve?" is enough for the first week.

### Day 6–7: Get the research direction

Read Ch.18 and classify the lab's recent papers and projects by research topic. Mark where they overlap with the work of senior lab members.

### Things you do not need to do in the first week

- Understand papers perfectly — time will take care of this
- Grasp every latest research trend — gradually
- Write code from scratch — start by modifying existing code
- Set up the GPU server perfectly — begin from an environment file, container, or installation procedure the lab has already verified
- Produce a fully formed research idea — it is fine to learn the lab's problems and tools first

### Mindset for survival

Research Notes and Grad Notes discuss the habits needed at the beginning of a project. The individual links preserve the five decisions summarized in the Korean edition *(linked chapters are Korean only)*.

- *Not knowing at first is expected* → [Grad Notes Ch.14 — The Weight of Autonomy](../../grad-notes/guide.html#chapter-14), §2
- *"It does not work" is not a report; give prediction, attempt, and result* → [Grad Notes Ch.10 — One Question per Email](../../grad-notes/guide.html#chapter-10), §3
- *Keep records that let your future self reconstruct the work* → [Grad Notes Ch.8 — Using Time](../../grad-notes/guide.html#chapter-8), §5
- *Start from a small code path* → [Grad Notes Ch.11 — The Tool Trap](../../grad-notes/guide.html#chapter-11), §1
- *Compare against your past work, not a peer's current position* → [Grad Notes Ch.15 — The Comparison Trap](../../grad-notes/guide.html#chapter-15), §3

In a SLAM, CV, or robotics lab, begin with an existing project from a senior member. Reproduce its environment and run the pipeline before modifying it. Reusing a known CUDA, ROS, and simulator configuration reduces setup time and leaves more time for understanding the method.

---

# Ch.22 — Closing: Where to Start?

The previous twenty-one chapters covered the main elements of Spatial AI, from sensors and coordinate frames to robot motion, spatial perception, and learning-based methods. This final chapter identifies where to begin for different backgrounds and projects.

## 22.1 The Map So Far

The guide consists of four parts.

- **Foundations** (Ch.1–3): the scope of Spatial AI, how sensors measure the environment, and the mathematics used to process those measurements. Rotation, transformation, optimization, and probability recur throughout the guide.
- **Robots** (Ch.4–8): physical systems built from joints and links. Kinematics determines pose, dynamics computes forces, and control and motion planning produce desired states and paths. Robot learning acquires parts of this process from data.
- **Perception and Spatial Understanding** (Ch.9–14): the path from image processing to 3D spatial perception. Classical CV, deep learning, foundation models, and VLA lead into SLAM, which connects pose and maps over time.
- **Research in Practice** (Ch.15–21): the frameworks, development tools, datasets, and references needed to run experiments.

Real projects cross these boundaries. Operating a robot may require the equations in Ch.3, the SLAM methods in Ch.14, and the Docker setup in Ch.16 at the same time.

## 22.2 Starting Points by Profile

The best starting point depends on the reader's background.

For a **third- or fourth-year undergraduate new to robotics**, Ch.1 → Ch.3 → Ch.9 → Ch.14 is a useful sequence. It introduces the scope of Spatial AI and its mathematical language before connecting images, 3D geometry, and SLAM. Setting up the environment in Ch.16 alongside these chapters makes it possible to run the examples immediately.

A **new master's student with a deep-learning background** can begin with Ch.2 → Ch.3 → Ch.10 → Ch.11. Building on the reader’s deep-learning background, this sequence first fills in the sensor and mathematical foundations and then shows how robotics uses foundation models. Ch.13 and Ch.14 extend the path to 3D vision and SLAM.

Readers with a **classical robotics background and less experience in deep learning** can start with Ch.8 → Ch.10 → Ch.11 → Ch.12. They can consult Ch.4–7 as needed and concentrate on the progression from robot learning to VFMs and VLAs.

Readers with a **specific project** can begin with the datasets and benchmarks in Ch.17. After selecting one or two papers on a similar task, they can work backward to the chapters those papers require.

Ch.20.7 provides learning plans for one-, three-, and six-month periods.

## 22.3 What Not to Do

New researchers may focus on increasing the paper count, postpone experiments until the environment is perfect, or spend too much time implementing every component from scratch. Accepting AI-generated output without verification creates the opposite problem. [Research Notes Ch.6](../../research-notes/guide.html#chapter-6) and [Grad Notes Ch.11](../../grad-notes/guide.html#chapter-11) discuss these issues in detail *(Korean only)*.

SLAM and CV already have widely used public implementations, including ORB-SLAM3, COLMAP, and Gaussian Splatting. Unless the goal is education or a clearly new contribution, running an existing implementation and analyzing its limits may lead to the research problem more directly.

## 22.4 A Sense of the Long Game

The first year, the first summer, and the five-year horizon of a PhD are discussed in [Grad Notes Ch.7](../../grad-notes/guide.html#chapter-7) and [Grad Notes Ch.1](../../grad-notes/guide.html#chapter-1) *(Korean only)*.

Robotics experiments can have long cycles because they require hardware preparation, safety procedures, data collection, and repeated runs. Since preparation time and cycle length vary widely by equipment, environment, and laboratory, planning around complete experiment cycles is more realistic than judging progress day by day.

## 22.5 Next Steps

There is no need to reread the guide from beginning to end. When a project stalls, return to the relevant chapter and check the equations, implementation details, or datasets.

Continue with [Research Notes](../../research-notes/guide.html) for reading and writing papers, and [Grad Notes](../../grad-notes/guide.html) for managing the PhD process *(Korean only)*.

Draft date: 2025.12.28 · Revision date: 2026.05.01