Guide on Becoming a Roboticist

가이드 렌더링 중...

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

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**: 여러 센서 정보의 통합

> **추천 자료**
> - [Andrew Davison — From SLAM to Spatial AI (MIT Robotics)](https://www.youtube.com/watch?v=BRRtlR0C_CY) — Spatial AI의 비전을 제시한 Andrew Davison 교수의 강연. 이 분야의 방향성을 잡는 데 볼 만하다.
> - [FutureMapping 논문 (arXiv:1803.11288)](https://arxiv.org/abs/1803.11288) — Spatial AI 개념을 처음 체계화한 논문.
> - [Cyrill Stachniss — Introduction to Mobile Robotics](https://www.youtube.com/playlist?list=PLgnQpQtFTOGQrZ4O5QzbIHgl3b1JHimN_) — 본 대학교 Cyrill Stachniss 교수의 모바일 로보틱스 강의. Spatial AI의 기반 개념들을 잘 정리해서 강의한다.

## 1.2 왜 중요한가?

자기가 어떤 도메인에서 일하고 싶은지에 따라 깊이 파야 할 기술이 달라진다. 자율주행을 할 거면 LiDAR와 센서 퓨전에 집중해야 하고, AR/VR을 할 거면 Visual-Inertial 시스템을 파야 한다. 전체 응용 분야를 알아야 자기 로드맵을 짤 수 있다.

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

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

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

"AI가 발전하면 로보틱스도 다 풀리지 않나?"라는 질문을 종종 받는다. 안 풀린다. AI가 개선하는 부분과 로보틱스가 어려운 부분이 다르기 때문이다.

로보틱스의 난이도는 대부분 물리 세계와의 접점에서 발생한다:

- **되돌릴 수 없다.** 코드 버그가 로봇 충돌로 이어지면 장비가 파손되거나 사람이 다친다. 롤백이 안 된다.
- **이터레이션이 느리다.** 코드 수정 → 업로드 → 환경 리셋 → 안전 확보 → 실행 → 물리적 확인. 한 사이클에 수 분에서 수십 분이 걸린다.
- **센서 데이터는 항상 더럽다.** 역광, 모션 블러, 드리프트, 프레임 드롭. "깨끗한 데이터에서 잘 작동한다"는 건 로보틱스에서 의미가 없다.
- **실시간 제약이 있다.** 장애물 회피가 200ms 늦으면 충돌한다. 정확도가 높아도 속도가 안 나오면 못 쓴다.
- **엣지 케이스가 치명적이다.** LLM이 100번 중 5번 틀려도 유용하다. 자율주행이 100만 번 중 1번 틀리면 사고다.
- **sim-to-real gap이 있다.** 시뮬레이터의 마찰 계수, 관성, 노이즈는 근사치다. 이 오차가 누적되면 실제 로봇에서 행동이 달라진다.

정리하면:

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

AI 발전으로 개선되는 영역이 분명히 있다. 인식 정확도, 의사결정 품질, 자연어 명령 이해 같은 것들. 하지만 위 표의 오른쪽 — 이터레이션 속도, 센서 노이즈, 실시간 제약, 파손 위험 — 이건 물리 법칙의 영역이다. 모델을 키운다고 해결되지 않는다.

이 문서가 수학, 센서, SLAM, 캘리브레이션을 다루는 이유가 여기에 있다. AI 모델 하나 학습시키는 것만으로는 로봇이 안 돌아간다.

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

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

- **문제 정의**: AI는 "이 문제를 풀어줘"라고 하면 풀지만, "어떤 문제를 풀어야 하는가"는 판단하지 못한다. 어떤 센서 조합이 이 환경에 적합한지, 어떤 정확도가 이 application에 충분한지, 어떤 trade-off가 acceptable한지 — 이건 도메인을 아는 사람만 판단할 수 있다. (*문제 정의의 본격 frame은 [`../research-notes/chapter_01_review_paper.md`](../research-notes/chapter_01_review_paper.md) + [`ch02_feynman_problem.md`](../research-notes/chapter_02_feynman_problem.md) 에서 다룬다.*)
- **시스템 통합**: 인식 모듈, 제어 모듈, 통신 스택, 하드웨어를 하나의 작동하는 시스템으로 만드는 것. AI는 각 모듈의 코드를 짤 수 있지만, 모듈 간의 인터페이스, 타이밍, 예외 처리를 설계하는 건 엔지니어의 일이다.
- **물리 세계와의 접점**: 케이블이 빠졌는지, 센서 렌즈에 먼지가 낀 건지, 모터가 과열된 건지 — AI는 ssh로 접근할 수 없는 문제를 해결하지 못한다. 로봇 앞에 서서 손으로 만지는 사람이 필요하다.
- **신뢰성 판단**: AI가 "99% 정확도"라고 보고해도, 그 1%가 안전 사고로 이어질 수 있는 상황인지 아닌지는 엔지니어가 판단해야 한다. 자율주행에서 99%는 충분하지 않고, 실내 서빙 로봇에서 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 등. 인식-계획-제어를 단일 모델로 통합하는 시도가 나오고 있으나, 2026년 기준 실제 배포 시스템은 여전히 모듈형이 주류다.
> - **지금 주목할 것**: Foundation Model을 로봇 인식에 접목하는 연구(예: Open-vocabulary SLAM, VFM 기반 Scene Understanding)가 CVPR/ICRA 2025에서 눈에 띄게 늘었다. 고전적 기하학과 학습 기반 방법을 결합하는 흐름도 계속되고 있다.

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

---

# Ch.2 — 센서 (Sensors)

로봇이 환경을 인식하기 위해서는 센서가 있어야 한다. 각 센서의 특성을 이해하면 적절한 센서 선택과 알고리즘 설계가 가능하다.

알고리즘을 아무리 잘 짜도, 센서 특성을 모르면 "왜 이 알고리즘이 이 상황에서 실패하는가?"를 진단할 수 없다. 예를 들어, SLAM을 돌렸는데 특정 구간에서 트래킹이 날아가는 원인이 Rolling Shutter 때문인지, LiDAR 반사 특성 때문인지, IMU 바이어스 때문인지를 센서 지식 없이는 파악하기 어렵다. 센서는 로보틱스 시스템의 입구이며, 입구에서 들어오는 데이터의 특성을 모르면 나머지 모든 것이 흔들린다.

## 2.1 카메라 (Camera)

카메라는 가장 정보량이 풍부한 센서이다. 사람이 시각으로 세상의 대부분을 이해하듯, 로봇도 카메라에서 가장 많은 정보를 얻는다. 하지만 카메라 종류마다 특성이 많이 다르기 때문에, 각각의 장단점을 이해하고 목적에 맞는 카메라를 선택하는 것이 중요하다.

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

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

단안 카메라는 가장 저렴하고 가벼운 센서이면서도 Visual SLAM, 객체 인식, 시맨틱 이해 등 대부분의 비전 태스크의 출발점이다. 다만 깊이를 직접 측정할 수 없다는 구조적 한계 때문에, 이를 극복하기 위한 다양한 알고리즘(Monocular Depth Estimation, SfM 등)이 발전해왔다. 이 한계를 이해해야 왜 스테레오 카메라나 깊이 카메라가 필요한지도 알 수 있다.

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

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

**주요 사양**:
- 해상도: 720p, 1080p, 4K 등
- Frame rate: 30fps, 60fps, 120fps 등
- 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 방식**:
- 알려진 패턴을 투사하고 변형을 분석
- 장점: 높은 정확도, 저비용
- 단점: 실외 사용 어려움, 다중 센서 간섭
- 예시: Intel RealSense D400 시리즈, Orbbec Astra

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

> **추천 자료**
> - [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 관련 논문은 CVPR 기준 2019년 5편 수준에서 2024년 30편 이상으로 늘었다. 고속 환경(드론 고속 비행, 자동차 급회전)에서 모션 블러 없이 동작하기 때문이다. 아직 주류는 아니지만, 고속(>100km/h) 환경이나 HDR 조건을 다룰 예정이라면 Gallego et al. survey (TPAMI 2020)와 rpg_dvs_ros 패키지를 살펴보라.

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

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

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

**단점**:
- 정지 장면에서는 출력 없음
- 전통적인 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는 "정확한 3D 좌표를 직접" 준다. 자율주행 분야에서 LiDAR가 핵심 센서로 자리잡은 이유가 바로 이 정확한 거리 측정 능력 때문이다. 카메라만으로 깊이를 추정하면 오차가 크고 날씨 영향을 받지만, LiDAR는 수 센티미터 정확도로 100m 이상 떨어진 물체까지 측정할 수 있다.

최근 주목할 트렌드: Solid-State LiDAR가 기존의 Spinning(기계식) LiDAR를 빠르게 대체하고 있다. 움직이는 부품이 없어 내구성이 높고 대량 생산에 유리하며, 자동차 양산에 적합하기 때문이다. Livox의 비반복 스캔 패턴 같은 새로운 접근도 나오고 있어, 포인트 클라우드 처리 알고리즘에도 변화가 필요한 시점이다.

### 2.2.1 2D LiDAR vs 3D LiDAR

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

**3D LiDAR**:
- 다중 레이어 또는 회전 스캔으로 3D 포인트 클라우드 생성
- 용도: 자율주행, 대규모 매핑
- 예시: Velodyne VLP-16/32/64, 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**:
- 움직이는 부품 없음
- 제한된 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 | 최대 측정 거리 (50m ~ 300m) |
| Points/sec | 초당 포인트 수 (300K ~ 2M) |
| Accuracy | 측정 정확도 (±2cm ~ ±5cm) |
| FoV | 수평/수직 화각 |

> **추천 자료**
> - [Livox 기술 문서](https://www.livoxtech.com/downloads) — Solid-State LiDAR의 비반복 스캔 패턴과 그 장점을 설명하는 기술 자료
> - [Xu et al. — FAST-LIO2 (RA-L 2022)](https://arxiv.org/abs/2107.06829) — Solid-State LiDAR에 최적화된 LiDAR-Inertial Odometry 논문

## 2.3 IMU (Inertial Measurement Unit)

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

SLAM을 돌렸는데 드리프트가 심할 때, IMU 특성을 이해하지 못하면 원인조차 파악하기 어렵다. "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)

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

**Integration Drift**:
- 가속도 이중 적분 → 위치 오차 누적
- 각속도 적분 → 자세 오차 누적
- 짧은 시간 동안만 신뢰 가능 (보통 수 초)

실제로 겪어보면 바로 느끼는데, 가속도를 두 번 적분해서 위치를 구하면 노이즈와 바이어스 오차가 시간의 제곱에 비례하여 누적된다. Consumer 등급 IMU(스마트폰 내장)로는 10초만 지나도 위치 오차가 수 미터에 달할 수 있다. 그래서 IMU를 단독으로 쓰는 경우는 거의 없고, 항상 카메라나 LiDAR와 퓨전하여 드리프트를 보정한다.

**IMU 등급**:
| 등급 | 용도 | 가격 | 예시 |
|------|------|------|------|
| Consumer | 스마트폰, 게임 | $1-10 | MPU6050, BMI160 |
| Industrial | 로봇, 드론 | $100-1K | VectorNav VN-100, Xsens MTi |
| Tactical | 자율주행, 항공 | $1K-10K | KVH 1750 |
| Navigation | 선박, 항공기 | $10K+ | Honeywell HG1700 |

> **추천 자료**
> - [Probabilistic Robotics, Ch.5 — Robot Motion (Thrun, Burgard, Fox)](https://www.probabilistic-robotics.org/) — 센서 노이즈 모델링과 모션 모델의 핵심 참고서. IMU 오차 모델링의 이론적 기초를 다룬다.
> - [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 GPS/GNSS

**GNSS (Global Navigation Satellite System)**는 위성 신호를 이용한 위치 측정 시스템이다. GPS는 미국 시스템이며, GNSS는 GPS, GLONASS(러시아), Galileo(유럽), BeiDou(중국) 등을 포함하는 총칭이다.

실외 자율주행이나 드론에서 GNSS는 "전역 좌표"를 제공하는 유일한 센서이다. SLAM은 상대적 위치(어디서 출발해서 얼마나 움직였나)를 추정하지만, GNSS는 지구상 절대 위치(위도, 경도, 고도)를 알려준다. 이 두 가지 정보를 결합하는 것이 실외 로봇의 핵심 과제이다. RTK-GPS의 센티미터급 정확도가 자율주행 고정밀 측위의 Ground Truth로도 사용되므로, 원리를 이해해두어야 한다.

**정확도**:
- 일반 GPS: 2-5m
- DGPS (Differential): 0.5-2m
- RTK-GPS (Real-Time Kinematic): 1-2cm

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

자율주행과 로보틱스에서 레이더의 중요성이 높아지고 있다. LiDAR와 카메라가 안 되는 환경 — 안개, 비, 먼지, 강한 역광 — 에서도 레이더는 안정적으로 동작한다. 가격도 LiDAR보다 저렴하다.

**FMCW (Frequency Modulated Continuous Wave) Radar**:
- 주파수를 시간에 따라 변조하여 송신하고, 반사파와의 주파수 차이로 거리와 속도를 동시에 측정한다.
- 출력: Range-Doppler map (거리 × 속도 2D 맵), Range-Azimuth map
- 자동차용 77GHz 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-5m)
- 저비용
- 주차 보조, 근접 센서

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

> **추천 자료**
> - [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 시스템이 어떻게 구현되는지 보여준다.

> **⚠ AI 에이전트 주의**: 센서가 "데이터가 안 온다"는 문제의 원인은 대부분 소프트웨어가 아니라 물리적 연결(케이블, IP 설정, 전원, USB 대역폭)이다. AI는 드라이버 재설치부터 권하지만, `dmesg`, `lsusb`, `ping` 같은 시스템 명령으로 물리 연결부터 확인하라.

> **기술 흐름: 센서 기술**
> - **~2010**: 2D LiDAR(SICK, Hokuyo)와 단안 카메라 중심. 센서가 비싸고 크며, 처리 능력도 제한적. Stereo 카메라는 계산 비용 때문에 실시간 처리가 어려웠다.
> - **2012~2017**: 3D LiDAR(Velodyne VLP-16) 보급, RGB-D 카메라(Kinect) 대중화. LiDAR 가격이 수만 달러에서 수천 달러로 하락. Visual-Inertial 시스템(VIO)도 실제 시스템에서 쓰이기 시작했다.
> - **2018~2022**: Solid-State LiDAR(Livox) 등장, 가격이 수백 달러 수준까지 하락. Event Camera 연구 활발해짐. 멀티모달 센서 퓨전(Camera + LiDAR + IMU)이 표준으로 자리잡음.
> - **2023~**: Solid-State LiDAR가 Spinning 방식을 빠르게 대체 중. 고속/HDR 응용에서 Event Camera 채택이 늘기 시작했다. 4D Radar(도플러 속도 포함)도 새로운 보조 센서로 부상 중.
> - **지금 주목할 것**: Solid-State LiDAR가 대중화되면서 기존 Spinning LiDAR 전제의 알고리즘을 재설계해야 하는 상황이다. Event Camera는 아직 주류가 아니지만, 고속 드론·자율주행처럼 기존 카메라의 한계가 명확한 분야에서 빠르게 채택되고 있다. 센서 하드웨어가 바뀌면 알고리즘 연구 방향도 따라 바뀐다.

---

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

Spatial AI를 제대로 이해하려면 수학적 기초가 필요하다. 여기서는 핵심 개념만 간략히 짚고, 깊은 학습은 추천 자료를 참고하자.

수학 파트를 건너뛰고 싶은 마음은 이해한다. 하지만 논문을 읽다 보면 결국 수식에서 막힌다. SLAM 논문에서 "SE(3) 위의 최적화"라는 말이 나오는데 SE(3)이 뭔지 모르면 논문의 핵심 아이디어를 놓치게 되고, "Jacobian을 유도하여 Gauss-Newton으로 풀었다"는 한 줄이 이해가 안 되면 그 논문의 방법론 전체를 이해할 수 없다. 여기서 다루는 수학은 로봇 논문을 읽고 구현하기 위한 수학이다. 공대 3학년이면 선형대수를 들었을 테니, 여기서는 학부 때 배운 것이 로보틱스에서 어떻게 쓰이는지 연결하는 데 집중한다.

고전적인 수학 도구가 여전히 핵심이지만, Differentiable Programming과 Auto-Differentiation(자동 미분)이 최적화 문제 접근 방식을 바꾸고 있다. 예전에는 Jacobian을 손으로 유도해야 했지만, 이제는 PyTorch나 JAX의 자동 미분으로 복잡한 파이프라인의 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) — 선형대수의 널리 알려진 강의. 고유값 분해를 포함한 전체 선형대수를 깊이 있게 다룬다.

> **실습**: [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 기하학은 Spatial AI의 핵심이다. "3D 공간에서 로봇은 어디에 있고, 카메라는 어디를 보고 있으며, 저 물체는 어디에 있는가?"를 수학적으로 표현한다. 이 파트를 모르면 SLAM 논문의 첫 페이지부터 막힌다.

### 3.2.1 좌표계 (Coordinate Frames)

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

직접 로봇 시스템을 만들어보면 바로 체감한다. 하나의 데이터가 여러 좌표계를 거쳐야 의미가 있기 때문이다. "카메라가 본 물체의 위치"는 카메라 좌표계에서 표현되어 있지만, 로봇이 물체에 접근하려면 그 위치를 로봇 좌표계 또는 월드 좌표계로 변환해야 한다. 센서마다 자기만의 좌표계가 있고, 이들 사이의 변환을 정확히 알아야(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)

회전 표현이 여러 가지인 이유는 각각 장단점이 다르기 때문이다. SLAM 최적화에서는 어떤 표현을 쓰느냐에 따라 수렴 속도와 안정성이 달라진다. 이 내용을 모르면 "왜 이 코드에서는 쿼터니언을 쓰고, 저 코드에서는 Rotation Matrix를 쓰는지" 이해할 수 없다.

**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 벡터(Axis-Angle)를 주로 사용하며, 최적화 라이브러리(Ceres, GTSAM)에서는 Lie Group 기반 표현(so(3) → SO(3) 매핑)을 사용하는 경우가 많다. 이들 사이의 변환을 자유자재로 할 수 있어야 한다.

> **추천 자료**
> - [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.0m"가 아니라 "3.0m ± 0.05m (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)
```

베이즈 정리는 상태 추정의 수학적 기반이다. "센서 측정값이 주어졌을 때, 로봇의 실제 상태는 무엇인가?"라는 질문에 답하는 공식이다. 베이즈 정리를 모르면 칼만 필터, 파티클 필터, Factor Graph 기반 SLAM 전부 이해할 수 없다.

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

```
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)을 업데이트하여 더 정확한 추정(posterior)을 얻는다. 칼만 필터의 predict-update 사이클이 바로 이 구조의 구현이다.

> **추천 자료**
> - [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)

최적화는 Spatial AI 알고리즘의 마지막 단계다. SLAM의 Bundle Adjustment, 카메라 캘리브레이션, 딥러닝 학습 전부 최적화 문제다. 이 섹션의 내용을 모르면 코드를 돌릴 수는 있어도, 왜 수렴하지 않는지, 왜 결과가 이상한지 디버깅할 수 없다.

### 3.4.1 Least Squares

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

**정규방정식 (Normal Equation)**:

```
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 부족) 유일한 해가 없다. 이때는 pseudo-inverse `x = (A^T A)^{-1} A^T b` 대신 SVD를 써야 수치적으로 안정적이다.

(참고: [다크 프로그래머 — 최소자승법 이해와 다양한 활용예](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) = ||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에서 핵심 알고리즘으로 쓰인다.

LM이 실전에서 가장 많이 쓰이는 이유: Gauss-Newton은 초기값이 좋으면 매우 빠르게 수렴하지만, 초기값이 나쁘면 발산할 수 있다. LM은 λ를 자동으로 조절하여, 초기에는 Gradient Descent처럼 안정적으로 시작하고, 해에 가까워지면 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 R_dot^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   (1차 근사)
```

주의: 하나의 회전 행렬 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를 주로 쓰고, Strasdat의 Sophus는 right를 기본으로 한다.

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

**실용적 의미**: 센서(예: IMU)가 측정한 속도(angular velocity, linear velocity)는 센서 프레임에서 표현된다. 이를 body 프레임이나 world 프레임으로 변환할 때 Adjoint를 쓴다. 여러 센서를 fusion하는 VIO 시스템에서 좌표계 간 변환이 빈번하게 일어나므로, Adjoint의 의미를 이해하고 있어야 한다.

### 3.5.7 실무에서의 사용

**Sophus (C++)**: Strasdat가 만든 Lie group 라이브러리. SO(3), SE(3)와 그 exponential/logarithmic map, Adjoint 등을 구현한다. ORB-SLAM3, Kimera 등 주요 SLAM 시스템이 사용한다.

```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. (2012)이 제안한 자료구조로, iSAM2의 핵심이다. Factor graph를 elimination하면 Bayes net이 되고, 이를 tree 구조로 재편하면 Bayes tree가 된다. 새로운 측정이 들어올 때, 영향을 받는 subtree만 re-elimination하면 된다.

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

> **추천 자료**
> - [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**: covariance의 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한 문제에 적합하다. 변수가 많아지면 `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의 원본 설명과 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: An exploration planner using information gain (2022)](https://arxiv.org/abs/2206.05193) — 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이 최적화 문제 접근 방식을 바꾸고 있지만, 자동 미분이 내부에서 무엇을 하는지 이해하려면 여기서 다룬 기초가 필요하다. 도구만 쓸 줄 알면 디버깅할 수 없다.

---

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

이것이 순기구학의 본질이다. 베이스에서 끝단까지 각 관절의 변환을 순서대로 곱하면 된다.

### 4.2.2 DH Parameters (Denavit-Hartenberg)

1955년 Denavit와 Hartenberg가 제안한 방법이다. 70년이 지났지만 여전히 산업계의 표준이다. 4개의 파라미터로 인접한 두 링크 사이의 관계를 정의한다:

| 파라미터 | 의미 |
|---------|------|
| **a_i** (link length) | x_i 축을 따른 z_{i-1}에서 z_i까지의 거리 |
| **α_i** (link twist) | z_{i-1}에서 z_i까지 x_i 축 기준 회전 각도 |
| **d_i** (link offset) | z_{i-1} 축을 따른 x_{i-1}에서 x_i까지의 거리 |
| **θ_i** (joint angle) | x_{i-1}에서 x_i까지 z_{i-1} 축 기준 회전 각도 |

회전 관절(revolute joint)에서는 θ_i가 변수이고, 나머지 3개는 상수이다.
직선 관절(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: (0.259, 1.366), 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"에서 채택한 방법이다.

핵심 아이디어: 각 관절을 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 skew-symmetric matrix 표현 (se(3) 원소)
- 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(로봇 기술 파일)에 들어가는 파라미터가 결국 DH 기반이고, 산업용 로봇 매뉴얼은 모두 DH 테이블을 제공한다. PoE는 이론적으로 더 깔끔하고 연구에서 선호되지만, 현장에서 DH를 모르면 곤란하다. 둘 다 익혀라.

```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 — DH 파라미터의 정석. 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)에 놓고 싶은데, 관절 각도를 각각 얼마로 해야 하는가?"

이 문제가 어려운 이유:
1. **비선형 방정식** — 삼각함수가 얽혀 있다
2. **다중 해** — 같은 끝단 위치에 도달하는 관절 각도 조합이 여러 개일 수 있다 (elbow-up, elbow-down 등)
3. **해가 없을 수도 있다** — workspace 밖의 점은 도달 불가
4. **무한히 많은 해** — 자유도가 남으면 (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) 경우 — 는 해석적으로 풀 수 있다. 대부분의 산업용 6축 로봇(UR, KUKA, ABB 등)이 이 구조이다.

이 경우 위치 문제(처음 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)이라 한다. 특이점에서는:

1. **특정 방향으로 끝단을 움직일 수 없다** — 자유도 상실
2. **미소 이동에 관절 속도가 무한대** — 실제 모터는 이를 따라갈 수 없다
3. **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** | PoE 기반 | 현대적, 빠름, 미분 가능 (differentiable) |

```python
# TRAC-IK가 KDL보다 나은 이유: 시간 내 해를 찾을 확률
# 벤치마크 결과 (Beeson & Ames, 2015):
#   KDL:     해 성공률 ~50-70% (시간 제한 5ms 기준)
#   TRAC-IK: 해 성공률 ~95-99% (동일 조건)
```

> **추천 자료**
> - Beeson & Ames, "TRAC-IK: An Open-Source Library for Improved Solving of Generic Inverse Kinematics" (2015): https://traclabs.com/projects/trac-ik/
> - 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}
```

n < 6이면 under-actuated, n = 6이면 fully-actuated, n > 6이면 redundant이다.

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

이 값이 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 | 소형 그리퍼, SO-ARM100 등 |
| XM540 | 10.0 | RS-485 | 중형 로봇 팔 |
| PH54  | 44.7 | RS-485 | 대형 매니퓰레이터, 모바일 로봇 |

Dynamixel의 장점은 데이지 체인 연결, 위치/속도/토크 제어 모드, PID 게인 조절, 가격 대비 성능이다. 단점은 통신 속도 한계이고, 고급 제어를 하려면 커스텀 펌웨어가 필요한 경우가 있다.

**Quasi-Direct Drive (QDD):**

MIT Mini Cheetah(2019)로 주목받은 방식이다. 핵심 아이디어는 단순하다: **감속비를 낮추는 것.**

일반적인 로봇 관절: 감속비 100:1 이상 (harmonic drive)
QDD: 감속비 6:1 ~ 10:1 (유성기어 또는 belt)

낮은 감속비의 장점:
- **높은 백드라이버빌리티(backdrivability)**: 외력이 가해졌을 때 관절이 자연스럽게 따라간다. 충돌 시 안전하고, 힘 제어가 용이하다.
- **높은 투명도(transparency)**: 토크 센서 없이도 모터 전류만으로 끝단 힘을 추정할 수 있다.
- **높은 대역폭**: 감속기의 마찰과 탄성이 적어 빠른 토크 응답이 가능하다.

단점: 동일 크기 대비 출력 토크가 낮다. 큰 토크가 필요하면 더 큰 모터를 써야 한다.

QDD를 사용하는 최근 시스템들:
- MIT Mini Cheetah / Cheetah 3
- ALOHA (low-cost bimanual teleop)
- 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.01 = 0.64 kg·m²
#   → 15배 이상 가볍다
#   → 힘 제어가 훨씬 쉽다
```

**감속기 종류:**

| 종류 | 감속비 | 백래시 | 효율 | 가격 | 용도 |
|------|--------|--------|------|------|------|
| Planetary | 3~100:1 | 중간 | 85-95% | 저렴 | 범용, QDD에 적합 |
| Harmonic Drive | 30~320:1 | 매우 낮음 | 65-85% | 비쌈 | 산업용 로봇, 정밀 |
| Cycloidal | 6~120:1 | 낮음 | 85-93% | 중간 | 최근 대안으로 부상 |

**액추에이터 선정 기준:**

로봇 관절의 액추에이터를 선정할 때 고려해야 할 것들:

1. **필요 토크 (torque)**: 정적 토크 (자세 유지) + 동적 토크 (가속). 안전 계수 2~3배.
2. **필요 속도 (speed)**: 관절의 최대 각속도. 감속비를 고려하여 모터 RPM 결정.
3. **백드라이버빌리티**: 협동 로봇이나 힘 제어가 필요하면 QDD, 아니면 harmonic drive.
4. **크기와 무게**: 로봇 링크에 장착해야 하므로 물리적 제약 존재.
5. **열 (thermal)**: 연속 토크 사양 확인. 피크 토크는 짧은 시간만 가능.

```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의 센서가 연구용 표준이다. 가격이 비싸다 ($3,000~$20,000).

**관성 센서 (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)
- 거리: 최대 1km (125 kbps에서)
- 토폴로지: 버스 (데이지 체인 가능)

로봇에서의 활용: 모터 드라이버와 메인 컨트롤러 사이 통신. 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는 차동 신호 기반의 시리얼 통신으로, 최대 1 Mbps, 여러 장치를 데이지 체인으로 연결할 수 있다.

```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)"은 "빠른"이 아니라 **"정해진 시간 안에 반드시 완료되는"**을 의미한다. 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 패치가 적용된 커널 설치 (Ubuntu 예시)
sudo apt install linux-image-rt-amd64   # Debian/Ubuntu

# 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
# 최대 지연(max latency)이 50μs 이하면 양호
```

**왜 1kHz인가:**

로봇 제어 루프의 표준 주기가 1kHz (1ms)인 이유:

1. **임피던스/힘 제어**: 기계적 공진 주파수보다 충분히 높은 제어 주파수가 필요하다. 대부분의 로봇 팔은 수~수십 Hz의 고유 진동수를 가지므로, 안정적 제어를 위해 적어도 10배 이상 (→ 수백 Hz~1kHz)이 필요하다.
2. **Nyquist 정리**: 100Hz의 동적 현상을 제어하려면 최소 200Hz 샘플링이 필요하고, 실제로는 5~10배(→ 1kHz) 정도가 바람직하다.
3. **통신 대역폭**: CAN bus 1 Mbps로 10개 모터를 1kHz로 제어하면 꽉 찬다. 그 이상은 EtherCAT이 필요하다.
4. **관행**: MIT Cheetah가 CAN + 1kHz 구성으로 동적 보행을 시연한 이후 QDD + 1kHz가 사실상 표준이 되었다.

고주파 제어 (5~10kHz)가 필요한 경우: 매우 가벼운 로봇 (낮은 관성), 고속 충돌 대응, 일부 촉각 제어. 이 경우 EtherCAT이나 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 추천 자료

기구학과 메카트로닉스를 진지하게 공부하려면 교재 하나를 처음부터 끝까지 풀어보는 것이 가장 효과적이다.

**교재:**

- **Craig, "Introduction to Robotics: Mechanics and Planning"** — 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 (최초의 전기식 컴퓨터 제어 로봇 팔)
1985 ── Product of Exponentials 정형화
1998 ── Harmonic Drive 로봇 적용 확산
2019 ── MIT Mini Cheetah: QDD 액추에이터
2019 ── MoveIt2 (ROS2 기반 모션 플래닝 프레임워크)
2023 ── ALOHA: 저비용 양팔 텔레오퍼레이션 플랫폼
2024 ── SO-ARM100: 오픈소스 5축 로봇 팔 ($200 이하)
```

---

*다음 장에서는 이 기구학 위에 힘과 질량을 얹는다: 동역학(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)을 푸는 가장 효율적인 방법이다. 핵심 아이디어는 간단하다:

**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^3)이 걸린다. 관절 수가 많은 로봇(예: humanoid의 30+ DOF)에서 이 차이는 실시간 제어 가능 여부를 결정한다.

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에서 v=0, a=0을 넣으면 바로 나온다. 이것만으로도 로봇이 중력에 처지지 않는다. 실무에서 가장 먼저 구현하는 제어기 중 하나이다.

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

> **추천 자료**
> - 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에 대해 이 방정식을 세우면, 시스템의 운동 방정식이 나온다.

좌표계를 자유롭게 선택할 수 있다는 것이 핵심이다. 뉴턴 역학에서는 각 링크의 질량중심 위치와 자세를 월드 프레임에서 표현하고, 구속 조건(constraint)을 관리해야 한다. 라그랑주 역학에서는 관절 각도를 일반화 좌표로 선택하면 구속 조건이 자동으로 사라진다.

### 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에 의존하는 것에 주목하라. q_2 = 0이면 팔이 쭉 펴진 상태이고, 관성이 최대다. q_2 = π이면 팔이 접힌 상태이고, 관성이 최소다.

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

이 코드를 직접 실행해보면 위에서 손으로 유도한 결과와 일치하는 것을 확인할 수 있다. 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를 사용하므로, 라이브러리를 호출하면 된다.

결국 둘 다 알아야 한다. 라그랑주를 모르면 제어 이론을 이해할 수 없고, 뉴턴-오일러를 모르면 실시간 구현을 할 수 없다.

> **추천 자료**
> - 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)를 풀어야 한다.
- Drake의 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이 되는데, 정확도가 떨어진다.
- Painleve's paradox: 특정 조건에서 rigid contact + Coulomb friction의 해가 존재하지 않거나 유일하지 않은 경우가 있다.

### Contact-Rich Manipulation이 어려운 이유

물체를 잡고, 돌리고, 끼우는 작업(peg-in-hole, in-hand manipulation 등)은 왜 그렇게 어려운가?

1. **Hybrid dynamics**: 접촉 모드가 수시로 바뀐다 (contact/no-contact, stick/slip). 각 모드마다 동역학이 다르고, 모드 전환의 시점을 예측하기 어렵다.
2. **Discontinuous dynamics**: 모드 전환 시 상태가 불연속적으로 변할 수 있다 (충격, impact).
3. **Sensitivity to parameters**: 마찰 계수, 접촉 강성 등의 정확한 값을 모르면 시뮬레이션과 실제의 괴리(sim-to-real gap)가 크다.
4. **Combinatorial complexity**: n개의 접촉점이 있으면 가능한 접촉 모드 조합이 3^n개다 (contact/separation, stick/slip 각 방향).

### 시뮬레이터마다 접촉 처리가 다른 이유

접촉 역학에 "정답"이 없기 때문이다. 각 시뮬레이터는 정확도, 속도, 안정성 사이의 트레이드오프를 다르게 선택한다:

- **MuJoCo**: compliant contact + convex optimization. 빠르고 안정적이지만, 물리적으로 완벽하지는 않다. 특히 관통이 허용되는데, 이를 "soft contact"의 일부로 받아들인다. RL 환경으로 인기가 많은 이유 중 하나가 이 안정성이다.
- **Drake**: rigid contact + time-stepping (Stewart-Trinkle) 또는 hydroelastic contact. 물리적으로 더 엄밀하지만, 계산 비용이 높을 수 있다. Hydroelastic contact은 접촉면의 압력 분포까지 계산한다.
- **Bullet**: velocity-level LCP + sequential impulse. 게임/VR에서 출발한 엔진이라 속도에 최적화되어 있으나, 정밀한 접촉이 필요한 로보틱스 작업에서는 한계가 있다.
- **DART**: LCP 기반의 rigid contact. 학술적으로 엄밀한 구현이지만, MuJoCo나 Drake에 비해 사용자 기반이 작다.

어떤 시뮬레이터를 쓸지는 연구 목적에 따라 다르다. RL로 locomotion을 학습하겠다면 MuJoCo가 표준이다. 접촉이 중요한 manipulation 연구라면 Drake나 MuJoCo 중 택하는데, 최근에는 MuJoCo도 접촉 품질이 많이 좋아졌다.

```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"  Penetration depth: {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) — 접촉 기반 제어 시뮬레이션의 표준. 볼록 접촉 모델과 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
```

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이다.

Spatial inertia의 장점:
- 여러 강체의 관성을 합칠 때 그냥 더하면 된다: 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와 거의 동일한 인터페이스를 제공한다. 실시간 제어에서는 C++ API를 써야 한다 — Python의 오버헤드가 kHz 급 제어 루프에서는 무시할 수 없다.

> **추천 자료**
> - 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 를 하면 안 된다. Pinocchio에서는 `pin.integrate(model, q, v*dt)`를 써야 한다.

### Underactuated Systems

부유 base 시스템의 핵심적인 특성: **underactuation**. Base에는 직접 구동기(actuator)가 없다. 보행 로봇은 발이 지면을 밀어야 base가 움직이고, 드론은 프로펠러의 추력으로 base를 움직인다.

Manipulator equation을 base와 joints로 나누면:

```
[ M_bb  M_bj ] [ a_base  ]   [ C_b ]   [ g_b ]   [  0  ]   [ J_c^T ]
[ M_jb  M_jj ] [ q̈_joints] + [ C_j ] + [ g_j ] = [ τ_j ] + [ J_c^T ] λ
```

왼쪽 위의 `0`이 핵심이다 — base에는 관절 토크가 없다. λ는 접촉력이고, J_c는 접촉 Jacobian이다. 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
```

이것이 locomotion 제어에서 왜 중요한가:

1. **CoM 역학이 balance를 결정한다.** 로봇이 넘어지지 않으려면 CoM의 궤적이 지지 영역(support polygon) 위에 있어야 한다 (ZMP 조건). 더 정확히는 centroidal momentum가 적절히 조절되어야 한다.

2. **차원 축소.** n-DOF 보행 로봇의 전체 동역학은 n차원이지만, centroidal dynamics는 6차원(선운동량 3 + 각운동량 3)이다. 이 6차원 공간에서 먼저 원하는 운동량 궤적을 계획하고, 그 다음 전체 관절 수준으로 분해하는 것이 일반적인 접근법이다.

3. **접촉력 계획과 직접 연결된다.** 위의 식에서 보듯이, 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 [angular; linear]
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 — Centroidal dynamics를 로보틱스에 도입한 핵심 논문
> - Wensing et al., "Optimization-Based Control for Dynamic Legged Locomotion", 2023 — 최신 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/JAX를 통한 autodiff도 가능하다 (Pinocchio 3.x).
> - Drake (https://drake.mit.edu/) — 시뮬레이션 + 최적화 + 제어를 통합한 프레임워크. MultibodyPlant가 동역학 엔진이다. Mathematical programming 인터페이스가 강력하여 trajectory optimization에 특히 유용하다.
> - MuJoCo (https://mujoco.org/) — DeepMind가 관리하는 물리 시뮬레이터. 접촉 처리가 빠르고 안정적이다. RL 연구의 사실상 표준 시뮬레이터.
> - PyBullet (https://pybullet.org/) — Bullet Physics의 Python 인터페이스. 진입 장벽이 낮아 교육용으로 적합하지만, 접촉 물리의 정밀도는 MuJoCo나 Drake에 미치지 못한다.

---

## 기술 흐름

```
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)
2022 ── Drake 1.0 (MIT → Toyota Research Institute)
2021 ── MuJoCo 오픈소스화 (DeepMind 인수 후)
2022 ── MuJoCo 2.3: implicit integration, elliptic friction cone
2023 ── Pinocchio 3.0: CasADi/JAX autodiff 지원
2023 ── MuJoCo 3.0: MJX (JAX backend for GPU parallelism)
```

---

## 정리

이 장에서 다룬 내용을 한 문장으로 요약하면: **동역학은 힘과 운동의 관계를 다루며, 로봇을 제어하고 시뮬레이션하는 데 필수적이다.**

실무적으로 기억해야 할 것:

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)

로봇이 세상을 인식하는 것과 세상에 실제로 영향을 미치는 것은 완전히 다른 문제다. 인식 파이프라인이 아무리 정교해도, 모터에 보내는 전류 하나를 잘못 계산하면 로봇은 넘어지거나 물건을 부수거나 사람을 다치게 한다. 제어 이론은 "원하는 동작을 실제로 실현하는 방법"에 대한 학문이다.

---

## 6.1 왜 제어를 배우는가

인식(Perception)이 "세상을 이해하는 것"이라면, 제어(Control)는 "세상에 영향을 미치는 것"이다. 둘 다 없으면 그냥 센서 덩어리 또는 모터 덩어리에 머문다.

제어를 배워야 하는 이유는 단순하다:

- **모터는 생각보다 멍청하다.** "관절을 30도로 보내라"라고 명령하면, 모터는 그냥 최대 전류를 때려넣고 30도를 지나쳐서 진동한다. 이걸 부드럽게 원하는 위치에 도달시키는 것이 제어다.
- **외란(disturbance)은 항상 존재한다.** 바닥이 미끄럽거나, 바람이 불거나, 페이로드 무게가 예상과 다르거나. 센서-액추에이터 루프를 닫아서(feedback) 이런 불확실성에 대응해야 한다.
- **안전이 걸려 있다.** 산업용 로봇 팔이 사람 옆에서 작업하는데, 힘 제어가 없으면 사람 팔뼈가 부러진다. 과장이 아니다.

제어 이론의 범위는 넓다. 여기서는 로보틱스 현장에서 실제로 쓰이는 것들에 집중한다. PID, 상태공간 제어, MPC, 임피던스 제어, Whole-Body Control 순으로 이어진다.

한 가지 미리 말해두자면, 제어 이론은 수학이 많이 나온다. 선형대수와 미분방정식에 익숙하지 않다면, 이 장을 읽기 전에 최소한 행렬 연산과 고유값(eigenvalue) 개념은 복습하고 오는 것을 권한다.

---

## 6.2 PID 제어

PID(Proportional-Integral-Derivative)는 1922년 Minorsky가 선박 조타 시스템을 위해 제안한 이래로, 100년이 넘게 산업 현장에서 가장 널리 쓰이는 제어기이다. 세상 모든 제어 엔지니어가 처음 배우는 것이고, 은퇴할 때까지 쓰는 것이다.

### 기본 구조

오차 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)가 남는다. 목표값 근처에서 오차가 작아지면 제어 입력도 작아지기 때문이다.

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

# 간단한 1차 동역학: 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가 100년째 쓰이는 이유는 간단하다: 구현이 쉽고, 이해하기 쉽고, 웬만한 시스템에서 "적당히" 동작한다. 제어 대상이 단순하고 성능 요구가 극단적이지 않으면 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 시스템에서 편리하지만, MIMO(Multi-Input Multi-Output) 시스템에서는 상태공간이 훨씬 자연스럽다. 로봇은 거의 항상 MIMO 시스템이므로, 상태공간 표현이 표준이다.

### 가제어성 (Controllability)

시스템이 가제어(controllable)하다는 것은, 임의의 초기 상태에서 임의의 최종 상태로 유한 시간 내에 이동할 수 있다는 것이다. 가제어성 행렬:

```
C_ctrl = [B, A*B, A^2*B, ..., A^(n-1)*B]
```

이 행렬의 rank가 n이면 가제어이다. rank가 n보다 작으면, 제어 입력으로 도달할 수 없는 상태가 존재한다는 뜻이다. 그런 시스템에 LQR을 적용하면 안 된다.

### 가관측성 (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 x n, 양의 반정치): 상태 오차에 대한 페널티. "상태가 0에서 벗어나는 것이 얼마나 싫은가."
- R (m x 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) 최적화 문제를 풀어 제어 입력을 계산하는 방법이다. 2020년대 로보틱스에서 가장 널리 쓰이는 제어 기법 중 하나다.

### 기본 개념

매 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로는 불가능하다.
- **비선형 모델 사용 가능**: Nonlinear MPC에서는 비선형 동역학 모델을 그대로 쓸 수 있다.
- **미래 예측**: 단순히 현재 오차에 반응하는 것이 아니라, 미래 궤적을 예측하여 능동적으로 대응한다. 보행 로봇이 다음 발을 내딛기 전에 미리 무게 중심을 이동시키는 것이 이 원리다.
- **다목적 최적화**: 비용 함수에 여러 목표를 동시에 넣을 수 있다. "목표 궤적을 추종하면서 에너지를 아끼고 토크 제한을 지켜라."

### Linear MPC vs Nonlinear MPC

**Linear MPC**: 선형 모델(x(k+1) = A*x(k) + B*u(k))을 사용하고, 비용 함수가 이차(quadratic), 제약이 선형이면 문제가 QP(Quadratic Program)가 된다. QP는 볼록(convex) 최적화이므로 전역 최적해를 빠르게 구할 수 있다. 실시간 제어에 적합하다.

**Nonlinear MPC (NMPC)**: 비선형 동역학 모델을 사용한다. 문제가 비볼록(non-convex)이 되어 풀기 어렵고, 전역 최적해를 보장하지 못한다. 하지만 로봇 동역학을 정확히 반영할 수 있으므로 성능이 좋다. CasADi + IPOPT 조합이 표준 도구다.

실무에서의 선택: 시스템이 충분히 선형에 가깝거나 제어 주기가 매우 짧아야 하면 Linear MPC를 쓰고, 비선형성이 크고 제어 주기에 여유가 있으면 NMPC를 쓴다.

### 실시간성 문제

MPC의 최대 난관은 매 제어 주기마다 최적화를 풀어야 한다는 것이다. 보행 로봇이 1kHz로 제어된다면, 1ms 안에 QP를 풀어야 한다.

주요 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 method. NMPC 구현의 사실상 표준.
- **acados** (https://docs.acados.org/): CasADi 기반이지만 실시간성에 최적화됨. C 코드 생성 가능.

solver 속도가 곧 제어 주기를 결정한다. solver가 5ms 걸리면 200Hz가 한계다. MPC 엔지니어가 solver를 매우 신경 쓰는 이유다.

### 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))
    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 (2019~)**: MPC + Whole-Body Control 조합. 비선형 MPC로 접촉 시퀀스를 예측하고, WBC로 관절 토크를 실시간 분배한다.
- **Unitree H1/G1 (2023~)**: 학습 기반 정책(reinforcement learning)이 high-level 명령을 생성하고, MPC가 low-level 궤적 추종을 담당하는 하이브리드 구조.
- **Figure 01 (2024)**: LLM이 태스크 레벨 계획을 세우고, MPC가 manipulation 궤적을 최적화한다. 제어와 AI의 결합 사례.

---

## 6.6 임피던스/어드미턴스 제어 (Impedance/Admittance Control)

지금까지 다룬 제어 기법들은 주로 "위치를 원하는 곳에 보내는 것"에 집중했다. 하지만 로봇이 환경과 물리적으로 접촉하는 순간, 위치 제어만으로는 부족해진다.

### 위치 제어 vs 힘 제어 vs 임피던스 제어

- **위치 제어(Position Control)**: 목표 위치를 추종한다. 환경이 없거나 매우 강성(rigid)인 환경에서 적합하다. 하지만 로봇 팔이 테이블 위의 컵을 집으려는데, 테이블 높이가 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 + (1 / D_d) * F_ext_dot
```

좀 더 정확히, 외력 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 + 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**: 최악의 외란에 대한 성능을 최적화한다. "어떤 외란이 들어오든 출력 오차가 이 이하로 유지된다"는 보장을 준다. 수학이 무겁고 (Riccati 부등식, LMI), 보수적인 경향이 있다. 1990년대에 산업 현장에서 널리 적용했다.

- **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를 실제 시스템에 쓰는 일은 많지 않다. MPC가 충분히 강력하고 직관적이기 때문이다. 하지만 "왜 이 제어기가 안정한가?"를 설명해야 할 때, 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절의 내용을 본격적으로 공부하려면 이 책이다. 절판이지만 PDF가 돌아다닌다 (알아서 찾아라).

> **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/
> Nonlinear MPC 구현의 사실상 표준 도구. 자동 미분(automatic differentiation)과 다양한 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 실용화 시작
2019 ── Boston Dynamics Atlas: MPC + WBC
2023 ── Unitree H1/G1: 학습 기반 + MPC 하이브리드
2024 ── Figure 01: LLM + MPC + manipulation
```

---

이 장은 제어 이론 전체 중 로보틱스에서 실제로 쓰이는 핵심을 골랐다. 각 기법의 수학적 세부사항은 추천 자료로 보충하기 바란다. 한 가지 조언하자면, 제어 이론은 시뮬레이션 없이 이해하기 어렵다. 이 장의 코드를 직접 실행하고, 파라미터를 바꿔가며 시스템 응답이 어떻게 달라지는지 관찰하는 것이 가장 효과적인 학습 방법이다. 교과서를 세 번 읽는 것보다 시뮬레이션을 한 번 돌리는 것이 낫다.

---

# 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 알고리즘

가중 그래프에서 최단 경로를 찾는다. 모든 간선을 탐색하므로 최적해를 보장한다. 시간 복잡도 O((V + E) log V).

### A* 알고리즘

Dijkstra에 휴리스틱을 추가한 것이다. 목표까지의 추정 거리(heuristic)를 이용하여 탐색 방향을 유도한다. 휴리스틱이 admissible(실제 거리 이하)이면 최적해를 보장하면서 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 abs(a[0] - b[0]) + abs(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  # 경로 없음
```

### 장단점

격자 기반 플래닝은 **완전성(completeness)**을 보장한다 — 해가 존재하면 반드시 찾는다. 하지만 **차원의 저주(curse of dimensionality)**에 시달린다. 6-DOF 로봇 팔의 C-space를 각 축 100개로 이산화하면 100^6 = 10^12개의 셀이 된다. 사실상 불가능하다.

이 문제를 해결하기 위해 샘플링 기반 플래너가 등장했다.

---

## 7.4 샘플링 기반 플래너

C-space를 이산화하지 않고, 무작위로 샘플링하여 경로를 탐색한다. 고차원 C-space에서 실용적인 유일한 방법이다.

### 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의 기본 플래너도 RRT-Connect(OMPL 내장)다.

### 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_py 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 기반으로, 매 반복에서 비선형 문제를 선형/이차 근사로 바꿔서 QP로 풀고, trust region으로 수렴을 보장한다. 충돌 회피를 signed distance function으로 처리하여 연속적인 gradient를 얻는다.

### CasADi를 이용한 Trajectory Optimization

CasADi는 symbolic computation + automatic differentiation + NLP solver 연결을 제공하는 프레임워크다. Trajectory optimization의 사실상 표준 도구이다.

```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/) — NLP 구현 표준 도구
> - [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 통합**: MoveIt2는 OMPL을 기본 플래닝 백엔드로 사용한다. `ompl_planning.yaml`에서 플래너 종류와 파라미터를 설정한다.

```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의 `ContactImplicitDirectCollocation`이 이 방법을 구현한다.

### 실시간 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-3은 **symbolic planning** (어떤 순서로 어떤 action을 할지), 4는 **motion planning** (구체적으로 어떤 궤적으로 움직일지). TAMP는 이 둘을 결합한다.

### 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, 2023): LLM + 환경 피드백의 반복적 대화로 태스크를 완수한다.

현실적 한계: LLM 기반 TAMP는 아직 실험 단계이다. 복잡한 기하학적 제약(좁은 공간에서의 조작, 정밀 조립)은 LLM이 처리하기 어렵고, 결국 전통적 motion planner가 필요하다. LLM은 high-level 계획, motion planner는 low-level 실행이라는 역할 분담이 현실적이다.

---

## 7.9 추천 자료

> **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 플래닝의 표준
2000 ── RRT-Connect (Kuffner & LaValle) — 실무에서 가장 많이 쓰이는 변종
2009 ── CHOMP (Ratliff et al.) — gradient 기반 궤적 최적화
2011 ── RRT* (Karaman & Frazzoli) — 점근적 최적성 보장
2012 ── OMPL 1.0 공개 — 샘플링 기반 플래너 통합 라이브러리
2014 ── TrajOpt (Schulman et al.) — sequential convex optimization
2019 ── MoveIt2 (ROS2) — 산업/연구 표준 프레임워크
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)**: 학습한 조건과 조금만 달라져도 성능이 급락하는 경우가 흔하다.

전통적 방법으로 풀 수 있으면 전통적 방법을 쓰는 게 낫다. 학습은 전통적 방법이 한계에 부딪히는 문제에 적용하는 도구다. 둘을 적절히 조합하는 것이 실무에서 가장 현실적이다.

---

## 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는 "다음 상태는 현재 상태와 행동에만 의존한다"는 가정이다. 이전 히스토리 전체를 볼 필요가 없다는 뜻인데, 실제 로봇에서는 이 가정이 깨지는 경우도 있다 (부분 관측, 즉 POMDP 상황). 이 경우 observation history를 state로 사용하거나 recurrent policy를 쓴다.

### Policy Gradient 직관

Policy gradient의 핵심 아이디어는 단순하다.

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가 수집한 데이터로만 학습한다. 데이터를 한번 쓰고 버린다. 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 알고리즘이다. 핵심 아이디어는 policy update의 크기를 제한하는 것이다. 이전 policy 대비 너무 크게 바뀌면 clipping으로 잘라낸다.

```
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가 인기 있는 이유는 구현이 비교적 간단하고, 하이퍼파라미터에 둔감하며, 안정적으로 학습된다는 점이다. 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가 인수한 후 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(2019)이 수백 개 물리 파라미터를 동시에 랜덤화해 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
```

충분히 넓은 범위로 랜덤화하면, 현실은 그 범위 안에 포함될 가능성이 높다. 대신 policy 성능의 상한은 내려간다. 특정 파라미터에 최적화한 policy보다 낮을 수밖에 없다.

### System Identification (Sys-ID)

Domain randomization과 반대 방향의 접근이다. 실제 로봇의 물리 파라미터를 가능한 한 정확하게 측정/추정해서 시뮬레이션에 반영한다.

방법:
- 직접 측정: 전자저울로 질량 측정, 마찰 계수 실험 측정
- 파라미터 최적화: 실제 로봇의 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에서 벗어나면, 학습 데이터에 없는 상태에 도달하게 된다. 거기서의 행동은 예측 불가능하고, 더 벗어나게 되고, 에러가 누적된다. 시간에 따라 에러가 기하급수적으로 커질 수 있다.

### DAgger (Dataset Aggregation)

DAgger는 compounding error를 해결하기 위한 방법이다.

1. 초기 전문가 데이터로 BC policy를 학습한다.
2. 학습된 policy를 실행하여 새로운 trajectory를 수집한다.
3. 이 trajectory의 각 state에서 전문가가 어떤 action을 취할지 레이블링한다.
4. 새 데이터를 기존 데이터에 추가하고 다시 학습한다.
5. 2-4를 반복한다.

핵심은 "policy가 실제로 방문하는 state"에서의 전문가 action을 학습 데이터에 포함시키는 것이다. 이론적으로 DAgger는 no-regret guarantee를 가진다.

단점은 전문가가 반복적으로 레이블링해야 한다는 점이다. 사람이 일일이 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

CMU의 Chi et al. (2023)이 제안한 방법으로, diffusion model을 action 생성에 적용한다.

기존 BC가 unimodal Gaussian으로 action을 모델링하는 반면, diffusion policy는 denoising 과정을 통해 임의의 복잡한 action 분포를 표현할 수 있다. 특히 다봉 분포를 잘 다룬다.

```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할 수 있도록 설계했다.

### pi0 (Physical Intelligence)

2024년에 공개된 diffusion 기반 범용 로봇 정책이다. VLM을 backbone으로 사용하고, flow matching으로 action을 생성한다. 다양한 manipulation 태스크에서 state-of-the-art 성능을 보였으며, 빨래 접기 같은 복잡한 장시간(long-horizon) 태스크에서도 동작했다.

### OpenVLA

오픈소스 VLA (Vision-Language-Action) 모델이다. 7B 파라미터의 VLM을 fine-tuning하여 action token을 출력하도록 학습했다. 누구나 접근 가능한 오픈소스라는 점이 핵심 기여다.

### 현실적 평가

Foundation model for robotics는 아직 초기 단계다. 솔직히 말하면:

- 특정 태스크에서는 해당 태스크에 특화된 전통적 방법이나 task-specific 학습이 더 나은 경우가 많다.
- 대규모 로봇 데이터 수집 비용이 매우 크다. 인터넷 텍스트/이미지 데이터와는 규모가 다르다.
- 안전성 보장이 없다. Foundation model의 행동을 예측하기 어렵다.
- 추론 속도(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

*연구자가 되고 싶다면 여기서부터 읽어라.*

RL의 성패는 reward 함수 설계에 달려 있다고 해도 과언이 아니다. 그리고 실제 로봇에 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 함수를 반복적으로 수정하고 학습된 행동을 검토한다 (사실상 trial-and-error).
- 원치 않는 행동에 대한 penalty term을 추가한다.
- 비디오를 보면서 정성적으로 검토한다. 자동화하기 어려운 부분이다.

### Constrained RL

Safety constraint를 명시적으로 다루는 RL이다. 일반적인 RL이 reward를 최대화하는 것이라면, 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
> RL의 필수 교재. 무료 PDF 제공. MDP부터 policy gradient까지 기초를 다지려면 반드시 읽어야 한다. 전부 읽을 시간이 없으면 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)
         최초의 policy gradient 방법. 수렴 증명은 있으나 variance가 높다.

2013 ── DQN (Mnih et al., Atari)
         Deep RL의 시작. replay buffer + target network로 안정적 학습.

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)
         대규모 domain randomization으로 사족보행 sim-to-real 성공.

2020 ── DAgger 실전 적용 확산
         모방학습의 실용성 입증. 다양한 로봇 플랫폼에 적용.

2022 ── RT-1 (Google)
         대규모 로봇 데이터 + Transformer. 다중 태스크 범용 정책.

2023 ── ACT/ALOHA (Stanford), Diffusion Policy (CMU)
         모방학습의 새로운 표준. action chunking과 diffusion으로 성능 향상.

2023 ── RT-2 (Google)
         VLM을 직접 action 생성에 사용. 웹 지식의 로봇 전이.

2024 ── Octo, OpenVLA, pi0
         오픈소스 범용 정책 모델 등장. Cross-embodiment 학습 시작.

2025 ── Cross-embodiment learning 연구 확산
         서로 다른 로봇 간 정책 전이. 데이터 스케일링 법칙 검증 중.
```

---

# 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는 BGR 순서를 사용한다 (RGB 아님). Matplotlib이나 다른 라이브러리와 섞어 쓸 때 색이 뒤집히는 원인이 여기에 있다. `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

이진 이미지(binary image)를 다룰 때 필수적인 도구이다. 예를 들어, segmentation 결과에서 작은 노이즈 점들을 제거하거나, 끊어진 영역을 이어 붙이거나 할 때 morphology를 쓴다. 이걸 모르면 이진화 결과를 후처리할 때 막막하다.

```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과 Closing의 순서를 헷갈리는 사람이 많은데 — 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, visual servoing — 이 모든 것의 출발점이 카메라 모델이다. 선형대수를 배웠다면 여기서 행렬이 어떻게 쓰이는지 체감할 수 있다.

### 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)**: 일반 핀홀 왜곡 모델로는 보정이 안 된다. 어안 렌즈는 빛의 입사각 θ에 대해 왜곡을 모델링한다 (equidistant model: r = f·θ). OpenCV의 `cv2.fisheye` 모듈을 별도로 사용해야 한다. 혼동하면 보정 결과가 오히려 나빠지니 주의.

(참고: [다크 프로그래머 — 카메라 왜곡보정](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
)
```

팁: 캘리브레이션 퀄리티를 높이려면 (1) 다양한 각도에서 20장 이상 촬영하고, (2) 체커보드가 이미지 전체를 고르게 커버하게 하고, (3) reprojection error가 0.5 픽셀 이하인지 확인하자.

**캘리브레이션이 하는 일을 직관적으로 이해하기**

카메라 캘리브레이션은 결국 "이 카메라가 3D 세상을 2D 이미지로 어떻게 변환하는지"의 파라미터를 알아내는 것이다. 체커보드 패턴을 여러 각도에서 촬영하면, 체커보드의 3D 좌표(알고 있음)와 이미지의 2D 좌표(검출함)의 대응 쌍이 수십~수백 개 생긴다. 이 대응 쌍으로부터:

1. **Intrinsic parameters** (fx, fy, cx, cy): 렌즈의 초점거리와 이미지 중심. 카메라 고유 속성이므로 한 번 구하면 렌즈를 바꾸지 않는 한 변하지 않는다.
2. **Distortion coefficients** (k1, k2, p1, p2, k3): 렌즈의 왜곡 정도. 저가 렌즈일수록 크다.
3. **Extrinsic parameters** (R, t): 각 촬영 위치에서의 카메라 자세. 캘리브레이션 자체에서는 부산물이지만, hand-eye calibration 등에서 별도로 쓰인다.

체커보드를 최소 10장 이상, 다양한 각도와 거리에서 촬영해야 한다. 한쪽으로 치우치면 해당 영역의 왜곡만 보정되고 나머지는 부정확하다. 이미지 전체에 걸쳐 체커보드가 고르게 분포하도록 촬영하는 것이 핵심이다.

reprojection error가 0.5 픽셀 이하면 양호, 0.1 이하면 매우 좋다. 1.0 이상이면 촬영을 다시 하거나 outlier 이미지를 제거해야 한다.

(참고: [다크 프로그래머 — 카메라 캘리브레이션](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을 이해하려면 특징점을 먼저 알아야 한다. 로봇이 카메라를 움직이면서 "지금 보는 장면이 아까 봤던 그곳인지"를 판단하려면 이미지 간에 같은 점을 찾아야 한다. 특징점은 그 대응점을 안정적으로 찾기 위한 핵심 도구다. SLAM, Visual Odometry, Object Recognition 등 거의 모든 시각 기반 로보틱스 알고리즘이 특징점에 의존한다.

### 9.3.1 Keypoint Detection

**Harris Corner**:
- 코너 검출의 고전적 방법
- 속도 느림, 스케일 변화에 취약

**FAST (Features from Accelerated Segment Test)**:
- 매우 빠른 코너 검출
- 실시간 시스템에 적합
- 스케일 불변 아님

**ORB (Oriented FAST and Rotated BRIEF)**:
- FAST 검출 + BRIEF 디스크립터 + 방향 정보
- 특허 무료
- 실시간 SLAM에서 널리 사용

ORB는 ORB-SLAM 시리즈의 핵심이다. 특허 무료라서 상업적으로도 자유롭게 쓸 수 있고, 속도가 빨라 실시간 시스템에 적합하다. 로보틱스에서 가장 먼저 접하게 될 feature이다.

**SIFT (Scale-Invariant Feature Transform)**:
- 스케일, 회전 불변
- 높은 반복성
- 계산 비용 높음 (과거 특허 문제, 현재 해제)

SIFT는 2004년 Lowe가 발표한 알고리즘으로, CV 분야에서 가장 많이 인용된 논문 중 하나다. 스케일과 회전에 불변하는 특징점 추출 원리를 이해하면, 이후 나온 SURF, ORB가 SIFT를 어떻게 개선했는지 자연스럽게 보인다.

**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이고, 이걸 제대로 이해하지 못하면 왜 SLAM이 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으로 가장 가까운 2개의 매치를 찾고, 1등과 2등의 거리 비가 일정 threshold 이하인 것만 "좋은 매치"로 남긴다. 이렇게 하면 애매한 매치(1등과 2등 거리가 비슷한)를 걸러낼 수 있다. 0.75라는 값은 Lowe가 원논문에서 제안한 것인데, 상황에 따라 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의 사실상 표준이 됐다. 딥러닝 기반이지만 정확도가 크게 높아서 품질이 중요한 경우에는 RAFT가 일반적인 선택이다.

> **추천 자료**
> - [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)

# 기본 PnP (iterative, 초기값 필요 없음)
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 단계가 정확히 이 과정이다.

> **추천 자료**
> - [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` (최소 3점), `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 ratio가 낮을 때 유용하다.

**PROSAC (Progressive Sample Consensus)**:
- 매칭 스코어가 높은 대응점부터 우선적으로 샘플링한다.
- 좋은 매칭이 앞에 많으면 초기 iteration에서 바로 좋은 모델을 찾는다.

**MAGSAC++ (Marginalizing Sample Consensus)**:
- 가장 골치 아픈 하이퍼파라미터인 inlier threshold σ를 marginalize한다.
- Threshold를 고정하지 않고 여러 σ 값에 대해 적분하므로, 수동 튜닝이 거의 필요 없다.
- 현재 OpenCV에서 권장하는 방법이다.

**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면 "99.9% 확률로 올바른 모델을 찾겠다"는 의미. inlier ratio가 낮을수록 필요한 iteration이 기하급수적으로 증가한다.
- Threshold: MAGSAC++를 쓰면 threshold에 덜 민감하지만, 초기값은 여전히 줘야 한다. Fundamental matrix는 1.0~3.0 pixel, Homography는 3.0~5.0 pixel이 일반적이다.
- 속도가 중요하면 PROSAC, 정확도가 중요하면 MAGSAC++를 쓴다.

> **추천 자료**
> - [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) 문제를 푼다.
- 정확도는 매우 높지만 속도가 느리다 (GPU 필수).

**LightGlue**:
- SuperGlue의 경량 버전. Adaptive early stopping으로 쉬운 이미지 쌍은 빨리, 어려운 쌍은 더 많은 layer를 통과시킨다.
- 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 kornia
from kornia.feature import LightGlueMatcher, KeyNetAffNetHardNet

# 특징 추출기 + 매칭기 구성
extractor = KeyNetAffNetHardNet(num_features=2048).eval()
matcher = LightGlueMatcher("keynetaffnethardnet").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 범위)
img0 = kornia.io.load_image(path0).unsqueeze(0).to(device)
img1 = kornia.io.load_image(path1).unsqueeze(0).to(device)

# 특징 추출
with torch.no_grad():
    feats0 = extractor(img0)
    feats1 = extractor(img1)

# 매칭
dists, match_idxs = matcher(feats0["descriptors"], feats1["descriptors"])
```

GPU가 있으면 SuperPoint+LightGlue 조합이 가장 균형이 좋다. GPU 없이 embedded에서 돌려야 하면 여전히 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) 등이 등장. 특허 문제와 속도 문제를 해결하면서 실시간 SLAM이 가능해짐
> - **2015~2019**: 딥러닝의 침투. SuperPoint (2018), SuperGlue (2020) 등 학습 기반 특징점이 고전 방법의 성능을 넘어서기 시작
> - **2020~**: Geometry + Learning의 융합. LoFTR (2021) 같은 detector-free matching, LightGlue (2023) 같은 경량 학습 매칭이 등장. 고전 기하학은 여전히 SLAM/VO의 백엔드에서 핵심
> - **지금 주목할 것**: 고전 기하학(epipolar geometry, triangulation)은 절대 사라지지 않는다. 딥러닝이 front-end(특징점 추출, 매칭)를 대체하는 추세이지만, back-end의 수학은 그대로이다. 둘 다 알아야 진짜 실력이다

---

# Ch.10 — 딥러닝 기반 인식 (Deep Learning for Perception)

로봇이 "무엇을 보고 있는지"를 이해하는 핵심 기술들이다. 고전 CV가 "이미지를 어떻게 처리하고, 기하학적 관계를 어떻게 추출하는가"에 집중했다면, 여기서는 "이미지 안에 뭐가 있는지"를 인식하는 데 집중한다. 물체 탐지, 분류, 분할 — 로봇이 "저기 빨간 컵이 있으니 집어" 같은 명령을 수행하려면, 이 기술들이 필수이다.

---

## 10.1 프레임워크 선택

어떤 딥러닝 프레임워크를 쓸지는 생각보다 중요한 결정이다. 연구 코드를 읽고 재현하려면 그 코드가 쓰는 프레임워크를 알아야 하고, 자기 모델을 만들려면 하나는 제대로 알아야 한다.

### 10.1.1 PyTorch (권장)

**장점**:
- 직관적인 동적 그래프 (eager execution)
- 디버깅 용이
- 연구 커뮤니티에서 표준
- 풍부한 사전학습 모델 (torchvision, timm)

현실적인 이유가 있다. 2024년 기준 NeurIPS, CVPR, ICLR 등 주요 학회에서 발표되는 논문의 코드 공개 중 80% 이상이 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를 모르면 ViT와 DETR이 왜 기존 접근을 대체하는지 이해할 수 없다.

### 10.2.1 CNN (Convolutional Neural Network)

이미지에서 공간적 특징을 추출하는 핵심 구조이다.

CNN은 "이미지의 지역적 패턴(에지, 코너, 텍스처)을 자동으로 학습"하는 구조이다. 앞서 배운 고전 CV에서는 SIFT, ORB 같은 특징을 사람이 설계(hand-craft)했지만, 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과의 차이를 알면 왜 Transformer가 뜨는지 바로 이해된다. CNN은 "가까운 픽셀끼리만" 정보를 교환하지만 (local receptive field), Transformer의 Self-Attention은 "이미지의 어떤 부분이든 서로 참조"할 수 있다 (global attention). 이 덕분에 물체의 전체적인 맥락을 파악하는 데 유리하다. 2020년 이후 비전 분야에서 Transformer가 CNN을 빠르게 대체하는 추세이다.

**Vision Transformer (ViT)**는 이미지를 16×16 같은 고정 크기 패치로 나누고, 각 패치를 NLP에서 "단어"처럼 취급해서 Transformer encoder에 넣는다. 아이디어 자체는 단순하지만, 대규모 데이터에서 CNN을 넘어서는 성능을 보여주면서 최근 비전 태스크의 주력이 되었다.

> **추천 자료**
> - [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) — 최신 벤치마크와 SOTA 모델 확인
> - [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

로봇이 "저 테이블 위에 컵이 어디 있지?"를 알려면, 단순 분류가 아니라 "어디에 뭐가 있는지"를 알아야 한다. 그것이 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에서 예측
- YOLO보다 작은 객체 탐지에 유리

> **추천 자료**
> - [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) — 최신 SOTA 확인
> - [다크 프로그래머 — 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 계열 최초로 YOLO급 실시간 속도 달성. 실시간 detection의 새로운 방향
> - [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로 대략적인 위치만 알려주지만, segmentation은 물체의 정확한 경계를 알려준다. 로봇이 물체를 잡으려면 bounding box가 아니라 정확한 윤곽이 필요하고, 자율주행에서 도로/인도/차선을 구분하려면 픽셀 단위의 분류가 필수이다.

**대표 모델**:

| 모델 | 특징 |
| --- | --- |
| FCN | 최초의 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 with Warm-up (manual)
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 torch.distributed as dist
from torch.nn.parallel import DistributedDataParallel as DDP

dist.init_process_group("nccl")
local_rank = int(os.environ["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가 크기 때문에, 높은 마스킹 비율이 더 어려운 과제를 만들어 좋은 표현을 학습시킨다.
- 인코더는 visible 패치만 처리하므로 학습이 효율적이다 (75% 연산량 감소).

**DINOv2와의 연결**:

DINOv2는 self-distillation 방식으로 학습한다. Teacher-student 구조이되, teacher는 student의 EMA(exponential moving average)이다.

- **Self-distillation**: student와 teacher가 같은 구조. teacher의 weight는 student weight의 EMA.
- **Centering + Sharpening**: teacher output에 centering(평균 빼기)과 sharpening(낮은 temperature)을 적용하여 mode collapse를 방지한다.
- 결과물인 DINOv2 feature는 별도 학습 없이도 ImageNet k-NN 83.0%, ADE20K linear probe 82.0% 등 supervised 방법에 필적하는 성능을 보인다.

**실무 — 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에서 돌려야 하기 때문이다. SAM을 Jetson에서 실시간으로 돌리고 싶다면, distillation이 거의 유일한 방법이다.

**Teacher-Student 구조**:

Teacher 모델(큰 모델, 이미 학습됨)의 출력을 student 모델(작은 모델)이 모방하도록 학습한다. Hard label(정답)보다 teacher의 soft prediction이 더 많은 정보를 담고 있다는 점이 핵심이다.

예를 들어, "고양이" 이미지에 대해 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()
# BN의 affine parameter만 학습 가능하게
for m in model.modules():
    if isinstance(m, nn.BatchNorm2d):
        m.requires_grad_(True)
        m.track_running_stats = False  # batch statistics 사용

optimizer = optim.SGD(model.parameters(), lr=1e-4)

# 추론 시 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로 환경 변화에 지속 적응한다.

이 조합이 현재 sim-to-real transfer에서 가장 널리 쓰이는 접근법이다.

> **추천 자료**
> - [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~2015**: 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가 다양한 비전 태스크에서 SOTA 달성
> - **2022~**: ConvNeXt가 "CNN도 아직 죽지 않았다"를 보여줌. Segment Anything(SAM)이 segmentation을 foundation model로 끌어올림. Spatial AI로의 확장 — depth estimation, 3D scene understanding이 딥러닝의 다음 전장
> - **지금 주목할 것**: 단일 태스크 모델에서 foundation model로의 전환. DINOv2 하나로 detection, segmentation, depth feature를 모두 추출하는 파이프라인이 실험적으로 등장하고 있다

---

# Ch.11 — Vision Foundation Models (VFM)

컴퓨터 비전의 패러다임이 바뀌고 있다. 앞서 배운 딥러닝 모델들이 특정 데이터셋에서 특정 태스크를 잘 하도록 학습된 specialist였다면, foundation model은 거대한 데이터로 범용적 시각 능력을 학습한 generalist이다. 이 차이가 보이면, 2023년 이후 ICRA/IROS에서 VFM 기반 인식 논문이 빠르게 늘어난 이유도 보인다.

---

## 11.1 Foundation Model이란?

**Foundation Model**은 대규모 데이터로 사전학습되어 다양한 downstream task에 적용 가능한 모델이다.

기존 접근을 생각해 보자. "새로운 환경 → 데이터 수집 → 라벨링 → 학습"의 사이클을 반복해야 했다. 공장이 바뀌면, 로봇이 인식해야 할 물체가 바뀌면, 처음부터 다시 해야 했다. Foundation model은 이 사이클을 끊는다. 한 번 학습된 모델이 본 적 없는 물체, 본 적 없는 환경에서도 작동한다. 로봇의 범용성(generalization) 문제를 푸는 가장 유력한 접근이다.

**특징**:
- **Scale**: 수억~수십억 파라미터
- **Pretraining**: 대규모 데이터 (수억 이미지)
- **Zero-shot / Few-shot**: 학습 없이 또는 적은 예제로 새 태스크 수행
- **Transfer**: 다양한 도메인으로 전이

**왜 중요한가?**
- 새로운 환경에서도 일반화 능력
- 특정 데이터셋에 대한 annotation 없이도 사용 가능
- 연구실의 Global Module에서 핵심 역할

여기서 Scale Law라는 개념이 중요하다. 모델 크기, 데이터 크기, 연산량을 키우면 성능이 power law로 향상된다는 경험적 법칙이다. GPT, CLIP, SAM 등 최근의 대형 모델들이 모두 이 법칙을 활용한다. "더 크면 더 좋다"가 일정 범위에서 성립한다 (Kaplan et al., 2020; Zhai et al., 2022 for ViT). 단, Hoffmann et al. (2022, Chinchilla)이 보여줬듯 모델 크기만 키우는 것보다 데이터와 연산량의 균형이 더 중요하다.

> **추천 자료**
> - [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

로보틱스에서 가장 많이 활용되는 Vision Foundation Model들을 본다. 각 모델이 어떤 문제를 풀고, 왜 로보틱스에서 중요하며, 어떻게 사용하는지를 중심으로 다룬다.

### 11.2.1 DINOv2

**Self-Supervised Vision Transformer**로, 라벨 없이 이미지에서 풍부한 특징을 학습한다.

DINOv2는 라벨 없이도 범용적인 시각 특징을 학습한다. 이 특징은 분류, 분할, 매칭 등 다양한 태스크에 그대로 쓸 수 있다. 특히 로보틱스에서는 DINOv2의 dense feature가 텍스처 없는 영역에서도 안정적인 매칭을 제공하여, SLAM이나 Visual Odometry에서 textureless 환경의 tracking failure rate를 줄이는 데 쓰인다.

**특징**:
- Contrastive learning + Self-distillation
- 다양한 태스크에서 우수한 전이 성능
- 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`의 첫 번째 토큰은 [CLS] 토큰으로 이미지 전체의 요약이고, 나머지는 각 패치의 feature이다. [CLS]는 분류에, 패치 features는 dense prediction(segmentation, matching 등)에 쓰인다.

> **추천 자료**
> - [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) — Self-distillation의 핵심 아이디어를 설명 (DINOv1 기반이지만 DINOv2 이해에 필수)

### 11.2.2 SAM (Segment Anything Model)

**Promptable Segmentation**: 점, 박스, 텍스트 등의 프롬프트로 어떤 객체든 분할한다.

기존 segmentation 모델은 학습에 사용된 클래스만 분할할 수 있었다. "의자, 테이블, 사람"으로 학습하면 "컵"은 분할하지 못한다. SAM은 1.1B개의 마스크로 학습되어, 본 적 없는 어떤 물체든 분할할 수 있다. 로봇이 새로운 환경에서 처음 보는 물체를 조작해야 할 때, 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`로 가장 적합한 마스크를 고르면 된다.

> **추천 자료**
> - [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)

similarity = (image_features @ text_features.T).softmax(dim=-1)
    print(similarity)  # 각 텍스트와 이미지 간 유사도
```

`@`는 행렬 곱(dot product)이다. 이미지 feature와 텍스트 feature의 코사인 유사도를 계산하는 것인데, 이미지와 각 텍스트가 얼마나 의미적으로 가까운지를 수치로 나타낸다. 이것이 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**: 단일 이미지에서 상대적 깊이를 추정한다.

10.7에서 depth estimation을 다뤘는데, Depth Anything은 그것을 foundation model 수준으로 끌어올린 모델이다. 1.5M 라벨 데이터에 62M 비라벨 데이터까지 활용해서, 실내(NYU), 실외(KITTI), zero-shot 도메인에서 안정적으로 깊이를 추정한다. 단, 학습 데이터와 크게 다른 도메인(내시경, 수중 등)에서는 정확도가 떨어질 수 있다. 로봇이 새로운 환경에 배치될 때 추가 학습 없이 바로 깊이 정보를 얻을 수 있다는 장점이 있다.

**특징**:
- 1.5M 라벨 데이터 + 62M 비라벨 데이터 학습
- 다양한 도메인에서 강건
- V2: 더 정확한 절대 깊이

Depth Anything V2는 V1에서 한 걸음 더 나아가, metric depth (절대 깊이)를 추정할 수 있는 버전도 제공한다. 로보틱스에서는 상대 깊이보다 "정확히 몇 미터 떨어져 있는가"가 중요한 경우가 많으므로, V2의 metric 버전에 주목하자.

> **추천 자료**
> - [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**: 텍스트 프롬프트로 임의의 객체를 탐지한다.

기존 detection 모델(YOLO, Faster R-CNN 등)은 학습에 사용된 클래스만 탐지할 수 있었다. "person, car, dog"으로 학습하면 "coffee mug"은 찾지 못한다. GroundingDINO는 CLIP처럼 텍스트로 어떤 물체든 지정해서 탐지할 수 있다. 로봇에게 "저기 있는 빨간 컵 찾아"라고 자연어로 지시하면, 학습 없이 바로 찾을 수 있다.

```
입력: 이미지 + "person. car. traffic light."
출력: 해당 객체들의 bounding box
```

**Grounded-SAM**: GroundingDINO + SAM 결합
→ 텍스트 프롬프트로 객체 탐지 + 세그멘테이션

Grounded-SAM은 로보틱스에서 실용적인 조합이다. "red cup"이라고 텍스트를 넣으면 GroundingDINO가 bounding box를 찾고, SAM이 그 안에서 정확한 마스크를 생성한다. 별도 학습 없이도 임의의 물체를 탐지하고 분할할 수 있어서, manipulation 파이프라인에서 활발히 쓰인다.

> **추천 자료**
> - [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의 robustness를 높이는 이유다.

**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은 성능은 좋지만, 수억 개의 파라미터가 있어서 GPU 서버가 아니면 실시간으로 돌리기 어렵다. 반면 로봇은 Jetson이나 임베디드 보드에서 30FPS로 돌려야 한다. 이 간극을 메우는 것이 경량화와 Edge 배포 기술이다. 아무리 좋은 모델도 로봇에서 실시간으로 돌릴 수 없으면 논문 안에서만 빛난다.

**경량화 기법**:
| 기법 | 설명 |
|------|------|
| **Distillation** | 큰 모델의 지식을 작은 모델로 전이 |
| **Quantization** | FP32 → INT8/INT4로 precision 감소 |
| **Pruning** | 중요하지 않은 weight 제거 |

각 기법의 trade-off를 파악하는 것이 중요하다. Quantization은 모델 구조를 바꾸지 않으므로 적용이 가장 쉽고, Pruning은 실제 연산량을 줄이지만 정확도 손실이 따를 수 있다. 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가 거의 필수이다. FP16 변환만으로도 속도가 2-3배 빨라지면서 정확도 손실은 거의 없다. INT8까지 가면 더 빨라지지만 calibration 데이터가 필요하다.

> **추천 자료**
> - [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)**:

핵심 아이디어: 사전학습된 weight matrix $\mathbf{W}$에 low-rank update를 추가한다.

$$\mathbf{W}' = \mathbf{W} + \Delta\mathbf{W} = \mathbf{W} + \mathbf{B}\mathbf{A}$$

여기서 $\mathbf{W}$는 $d \times d$ 행렬이고, $\mathbf{B}$는 $d \times r$, $\mathbf{A}$는 $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: 294,912 || all params: 86,567,178 || trainable%: 0.34%
```

**Adapter**:

Transformer block 사이에 작은 bottleneck layer를 삽입한다. 원래 weight는 고정하고 adapter layer만 학습한다.

```
Input → [Frozen Attention] → [Adapter: down_proj → ReLU → up_proj] → [Frozen FFN] → Output
```

LoRA는 기존 weight에 합쳐지므로 추론 시 추가 비용이 없고, Adapter는 추가 layer이므로 약간의 추론 지연이 생긴다.

**Prompt Tuning**:

입력에 학습 가능한 가상 토큰을 추가한다. 모델 자체는 전혀 건드리지 않고 입력만 조작한다.

- Visual Prompt Tuning(VPT): ViT의 각 layer 입력에 학습 가능한 토큰을 추가한다.
- 파라미터 효율이 가장 높지만, 성능은 LoRA보다 약간 낮은 경향이 있다.

**SAM을 특정 도메인에 맞추기**:

SAM의 prompt encoder에 도메인 특화 자동 prompt 생성기를 붙이는 전략이 자주 쓰인다.

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 연구에서는 다음 프로토콜로 비교하는 것이 표준이다.

| 프로토콜 | 설명 | 비교 목적 |
|---------|------|----------|
| **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) 발표. 이미지-텍스트 공유 임베딩으로 zero-shot 인식의 가능성을 열다. 4억 쌍의 이미지-텍스트 데이터로 학습. open-vocabulary 시대의 시작
> - **2022**: Masked Autoencoders (MAE) 등 self-supervised 사전학습 방법이 주목받기 시작. DINO가 self-supervised ViT의 가능성을 보여줌
> - **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의 표준 백본으로 자리잡는 추세
> - **지금 주목할 것**: Foundation model의 핵심 가치는 **zero-shot 능력**이다. 새 환경, 새 물체에서도 추가 학습 없이 작동하므로 로봇의 범용성을 높인다. CLIP+SAM+DINOv2 조합은 NLMap, ConceptGraphs 등에서 open-vocabulary 로봇 인식의 대표적 파이프라인으로 쓰이고 있다. 경량화(FastSAM, MobileSAM)를 통해 실제 로봇에 올리는 것까지가 완전한 파이프라인이다

---

# Ch.12 — Vision-Language-Action (VLA) & Embodied AI

로봇이 "빨간 컵을 집어서 테이블 위에 놓아줘"라는 자연어 명령을 받아 실행하는 것이 VLA의 목표다. 비전, 언어 모델, 제어를 하나로 합치는 분야이며, "왜 ChatGPT가 로봇을 움직이지 못하는지", "왜 시뮬레이션에서 잘 되던 정책이 실제 로봇에서 망하는지"를 이해하려면 이 챕터의 개념이 필요하다. CoRL, ICRA 2024-2025에서 VLA 관련 논문 비중이 크게 늘었다.

## 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/RT-2는 하나의 모델로 수백 가지 태스크를 수행할 수 있음을 보여줬다.

**RT-1 (Robotics Transformer 1)**:
- 대규모 로봇 데모 데이터로 학습
- 130K 에피소드, 700+ 태스크
- Tokenized action output

**RT-2 (Robotics Transformer 2)**:
- VLM (PaLI-X, PaLM-E)을 로봇 행동으로 파인튜닝
- Web-scale 데이터의 지식을 로봇에 전이
- "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

**Embodied Multimodal Language Model**:
- PaLM (Language) + ViT (Vision) + 로봇 상태
- 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는 Google 내부 인프라 없이는 쓸 수 없다. OpenVLA는 오픈소스로 공개된 VLA 모델로, 실제로 연구실에서 다운로드 받아 파인튜닝하고 로봇에 올릴 수 있는 모델이다.

**Open-source VLA**:
- 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와 함께 알아둘 것이 RT-X이다. 여러 연구 기관이 수집한 로봇 데이터를 모아 하나의 거대한 데이터셋(Open X-Embodiment)을 만들고, 이를 기반으로 범용 로봇 정책을 학습하는 프로젝트다. 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**: Language-guided Indoor Navigation

**SayCan**: LLM이 "할 수 있는 것"과 "해야 하는 것"을 분리
- Affordance function: 로봇이 현재 할 수 있는 행동
- LLM: 목표 달성을 위해 해야 하는 행동

SayCan의 핵심 아이디어를 조금 풀어보면: 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은 로봇이 "머릿속에서 시뮬레이션"을 돌려보고, 그 결과를 바탕으로 행동을 결정할 수 있게 해준다. 자율주행에서 특히 각광받고 있는데, "앞 차가 갑자기 멈추면 어떻게 될까?"를 실제로 경험하지 않고도 예측할 수 있기 때문이다.

**World Model**: 환경의 동작을 예측하는 모델

**왜 필요한가?**
- 실제 로봇 없이 model-based RL 가능
- 위험한 탐색을 시뮬레이션 안에서 처리

**자율주행 분야**:
- **GAIA-1 (Wayve)**: Video prediction + Action conditioning. 실제 주행 영상을 학습해서 "이렇게 핸들을 돌리면 이런 장면이 될 것"을 예측하는 생성 모델이다.
- **DriveDreamer**: 주행 시나리오 생성. 텍스트 조건으로 다양한 시나리오를 생성해 학습 데이터를 증강하는 데 활용된다.
- **MILE**: World model 기반 End-to-End driving. 미래 상태를 예측하는 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 (보상 예측)
```

선형대수 시간에 배운 상태 공간 모델(state-space 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**:

```
센서 입력 → [단일 신경망] → 행동 출력
```

- 장점: 간단한 파이프라인, 중간 표현의 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 (Vectorized Scene Representation for Efficient Autonomous Driving, 2023)**: 장면을 벡터화된 표현으로 변환하여 효율적인 End-to-End 주행 정책을 학습한다.
- **GenAD (Generalized Autonomous Driving, 2024)**: 생성 모델 기반으로 다양한 주행 시나리오에 일반화 가능한 End-to-End 시스템이다.

**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는 전체를 재학습해야 한다.
- **데이터 효율**: end-to-end 학습은 대규모 데이터가 필요하다. RT-2는 130k 에피소드, OpenVLA는 970k 에피소드를 사용했다. 대부분의 연구실은 이 규모의 데이터를 수집할 여력이 없다.

현실적 방향은 **하이브리드**다. 인식은 VFM(foundation model)으로 범용화하고, 계획/제어는 모듈형으로 안전성을 확보한다. 연구실의 Local/Global Module 설계(18장)가 이 방향이다.

end-to-end가 모듈형을 완전히 대체하려면 두 가지가 선행되어야 한다: 디버깅 가능한 end-to-end 구조, 그리고 소규모 데이터로도 학습되는 few-shot 정책. safety guarantee 문제도 여전히 열려 있다.

## 12.5 Spatial AI + VLA 통합

VLA 모델이 아무리 좋아도 실시간으로 장애물을 피하지 못하면 로봇은 벽에 부딪힌다. 반대로, 장애물 회피만 잘 하는 로봇은 "커피 가져와"라는 복잡한 명령을 수행하지 못한다. 두 레벨을 통합해야 실제 로봇 시스템이 돌아간다.

연구실의 2-Module Architecture와 연결:

**Local (Fast) Perception**:
- Geometric understanding: 깊이, 장애물, 자세
- 실시간 반응 (10-100Hz)
- Classical 또는 경량 학습 모델

**Global (Heavy) Understanding**:
- Semantic understanding: 객체, 관계, 맥락
- VFM/VLA 기반
- 서버 또는 클라우드 처리 (1-10Hz)

**통합 시나리오**:

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

실제 로봇으로 데이터를 모으는 건 느리고 비싸고 위험하다. 그래서 시뮬레이션에서 먼저 학습하고 실제 로봇에 전이(transfer)하는 Sim-to-Real이 사실상 기본이 되었다. 하지만 시뮬레이션과 현실 사이에는 "Reality Gap"이 존재한다. 이 간극을 좁히는 주요 기법들을 아래에 정리한다.

**Domain Randomization**: 시뮬레이션에서 텍스처, 조명, 물리 파라미터 등을 무작위로 변경하여 학습한다. 모델이 다양한 조건에 노출되면, 현실 환경도 그 중 하나의 변형(variation)으로 처리할 수 있게 된다.

**주요 시뮬레이션 플랫폼**:
- **NVIDIA Isaac Sim/Lab**: GPU 가속 물리 시뮬레이션. 수천 개의 환경을 병렬로 돌릴 수 있어 대규모 강화학습에 적합하다. Isaac Lab은 로봇 학습 연구를 위한 통합 프레임워크이다.
- **AI2-THOR (Allen Institute)**: 실내 환경 시뮬레이터. 주방, 거실 등 가정 환경에서 물체 조작(manipulation)을 연습할 수 있다. Embodied AI 연구에서 가장 많이 쓰이는 플랫폼 중 하나이다.
- **Habitat (Meta)**: 대규모 3D 스캔 환경(Matterport3D, Gibson 등)에서 내비게이션 학습이 가능하다. 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)으로 나뉜다. 로보틱스에서는 IL이 RL보다 훨씬 자주 쓰인다. 그 이유를 이해하려면 각 접근법의 구조를 알아야 한다.

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

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" (2024)](https://arxiv.org/abs/2309.15894) — IL 전반 서베이

## 12.8 심화: Diffusion Policy

*연구자가 되고 싶다면 여기서부터 읽어라.*

Chi et al.(RSS 2023)이 제안한 Diffusion Policy는 로봇 조작(manipulation) 분야에서 BC 계열 방법을 빠르게 대체하고 있는 정책 표현 방식이다. 핵심 아이디어는 행동 시퀀스(action trajectory)를 denoising diffusion 과정으로 생성하는 것이다.

**왜 Diffusion인가?**

기존 BC는 `π_θ(s) → a`로 단일 action을 결정론적으로 예측한다. 하지만 현실에서는 같은 상태에서도 여러 가능한 행동이 있다(multi-modality). 예를 들어 테이블 위의 컵을 잡을 때 왼쪽에서 잡아도 되고 오른쪽에서 잡아도 된다. 결정론적 BC는 이 두 행동의 평균을 출력해서 둘 다 실패한다. Gaussian Mixture Model 같은 방법도 있지만, 모드 수를 미리 정해야 한다.

Diffusion Policy는 이 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]`이다. 이 중 처음 몇 스텝만 실행하고(receding horizon), 다시 새 관측으로 다음 trajectory를 생성한다.

**장점**:
- Multi-modal action distribution을 명시적 가정 없이 표현
- Action sequence를 한 번에 생성하므로 temporally coherent한 행동
- 학습이 안정적 (denoising score matching은 잘 수렴함)

**단점**:
- Inference 시 여러 번의 denoising step이 필요하므로 느림 (10~100 steps)
- 실시간 제어(>100Hz)에는 부적합할 수 있음. DDIM 같은 가속 기법이나 consistency distillation으로 완화 가능

**실무 참고**: Chi et al.의 원 논문 실험에서 연속 action space 태스크 12개 중 11개에서 BC 계열을 앞섰다. 접촉이 많은 삽입·조립 태스크에서 차이가 특히 크다.

> **추천 자료**
> - [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)**

DR과 SysID의 장점을 결합한 최근 접근이다.

```
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" (2022)](https://arxiv.org/abs/2111.00137) — DR 체계적 정리
> - [Hanna & Stone, "Grounded Action Transformation for Sim-to-Real" (AAAI 2017)](https://arxiv.org/abs/1511.07461) — 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+ 물체 카테고리. 가정용 로봇 벤치마크
> - [Szot 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 방식을 대체하기 시작
> - **지금 주목할 것**: Foundation Model을 로봇에 적용하는 연구가 2023년 이후 빠르게 늘고 있다 (RT-2, OpenVLA, Octo, pi0 등). OpenVLA/Octo처럼 오픈소스 모델을 자기 로봇에 파인튜닝할 수 있으니, 직접 실험해보는 것을 추천한다.

---

# Ch.13 — 3D 비전 (3D Vision)

로봇이 2D 이미지만으로는 "저 물체가 얼마나 멀리 있는지", "저 벽 뒤에 뭐가 있는지"를 알기 어렵다. 3D 비전은 로봇에게 공간 감각을 부여하는 분야다. 포인트 클라우드 처리, 3D 물체 감지, 장면 복원이 여기에 속한다. SLAM도, 로봇 조작(manipulation)도 이 내용 없이는 온전히 이해하기 어렵다. 다음 챕터(SLAM)의 기반이 되는 내용이니 꼼꼼히 봐두자.

## 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가 Python에서 바로 쓸 수 있어서 프로토타이핑에 좋고, PCL은 C++ ROS 노드를 만들 때 주로 쓴다. 처음 시작한다면 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

각 점의 표면 법선 벡터를 추정한다. 많은 알고리즘의 전처리 단계이다.

ICP (정합), 표면 재구성(reconstruction), 조명 계산 등 거의 모든 3D 처리에서 법선 벡터를 요구한다. 법선이 없으면 "이 점이 평면의 일부인지 모서리의 일부인지"를 알 수 없다.

```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이 이를 해결했다.

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

여러 센서를 결합하면 각 센서의 단점을 서로 보완할 수 있다. 카메라는 색상과 텍스처 정보가 풍부하지만 깊이가 없고, LiDAR는 정확한 3D 정보가 있지만 텍스처가 없다. 이 둘을 어떻게 합치느냐가 핵심이다.

**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**: 가장 많이 사용, GUI/CLI
- **OpenMVG**: 라이브러리 형태

```bash
# COLMAP 사용
colmap feature_extractor --database_path db.db --image_path ./images
colmap exhaustive_matcher --database_path db.db
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)"를 저장한다. 양수면 표면 바깥, 음수면 표면 안쪽이다. 여러 뷰에서 관측한 깊이를 가중 평균하면, 노이즈가 줄어들고 깨끗한 표면을 얻는다. 부호가 바뀌는 지점(0을 지나는 곳)이 바로 표면이다.

```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)
    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 맵을 실시간으로 구축한다.

- **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.14737) — 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 복원. 3D reconstruction 패러다임 전환
> - [Leroy et al., "MASt3R: Matching And Stereo 3D Reconstruction" (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 σ(x) = max(-dΦ_s(f(x))/dt, 0) / Φ_s(f(x))
```

여기서 `Φ_s`는 learnable parameter `s`가 제어하는 sigmoid-like 함수다. 학습이 진행될수록 `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 비전의 핵심 기술들은 하나의 공통 원리를 공유한다: **렌더링 과정을 미분 가능하게 만들어서, 렌더링 결과와 실제 이미지의 차이로 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" (EGSR 2022)](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, 자연어 기반 내비게이션, human-robot interaction 등에서 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 기반 볼류메트릭 복원이 주류.
> - **2012~**: Kinect 출시로 RGB-D 기반 3D 복원이 대중화되었다. KinectFusion(TSDF + ICP)이 실시간 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 맵을 구축하는 방식이다.
> - **지금 주목할 것**: SLAM/로보틱스 응용에서 3DGS 기반 방법이 빠르게 늘고 있다. NeRFStudio에서 두 방법 모두 실험해볼 수 있으니 직접 비교해보는 것을 추천한다.

---

# Ch.14 — SLAM & Odometry

로봇이 낯선 환경에서 "나는 어디에 있고, 주변은 어떻게 생겼는가?"를 동시에 알아내는 문제가 SLAM이다. GPS가 안 되는 실내, 지하, 건물 내부에서 로봇이 자율적으로 움직이려면 SLAM이 필요하다. 로봇 소프트웨어 엔지니어에게 가장 자주 요구되는 기술 중 하나이므로, 이론과 실습 모두 탄탄히 잡아야 한다.

---

## Part 1. 기초와 시스템

### 14.1 개념 소개

지도 없이 로봇을 돌려보면 바로 느낄 수 있다. 로봇은 자기가 어디 있는지 모르면 아무것도 못 한다. 내비게이션, 장애물 회피, 경로 계획 — 모든 것의 전제 조건이 "현재 위치"와 "주변 환경 정보"이다.

**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_) — SLAM 강의의 정석. 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 입문 자료 중 가장 체계적
> - [김기섭 블로그 — 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 계열)는 이미지에서 변하지 않는 특징적인 점(코너, 블롭 등)을 추출하고, 프레임 간 매칭으로 카메라 움직임을 역추정한다. 선형대수적으로는 Essential Matrix 또는 Fundamental Matrix를 구하는 문제다. 조명 변화에 강건하고 방법론이 검증되어 있지만, 흰 벽·텍스처 없는 바닥처럼 특징점을 뽑기 어려운 환경에서는 한계가 있다.

```
이미지 → 특징점 추출 → 매칭 → 움직임 추정
```

**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나 다른 센서로 복원해야 한다. 초기화 때 충분한 이동이 필요한 이유도 여기에 있다.

> **추천 자료**
> - [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의 표준 베이스라인이다. 대부분의 Visual SLAM 논문이 ORB-SLAM과 비교하며, 코드가 공개되어 있어 직접 빌드해서 돌려볼 수 있다. SLAM을 공부한다면 한 번은 직접 해볼 것을 권한다.

**구성**:
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)**: 최초의 실시간 단안(monocular) SLAM. EKF 기반으로 작동했으나, 맵 크기가 커지면 계산량이 급증하는 한계가 있었다.
- **PTAM (Parallel Tracking and Mapping, 2007)**: Tracking과 Mapping을 별도 스레드로 분리한 최초의 시스템. 이 아키텍처가 이후 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는 실제 드론이나 모바일 로봇에서 가장 많이 쓰이는 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 측정을 하나의 상대 변환으로 압축한다. 이렇게 하면 최적화할 때 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은 LiDAR SLAM의 출발점이다. 이후 나온 LeGO-LOAM, LIO-SAM, FAST-LIO 등 거의 모든 LiDAR SLAM이 LOAM의 아이디어를 계승하거나 확장했다.

- 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, ORB-SLAM3 등 다양한 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**:
- Kalman Filter 기반 (optimization 대신)
- ikd-Tree: 동적 KD-트리로 빠른 매핑
- 실시간 성능

FAST-LIO가 왜 빠른지: LIO-SAM이 Factor Graph 최적화(비선형 최소자승법)를 사용하는 반면, FAST-LIO는 Iterated Extended Kalman Filter(IEKF)를 사용한다. 최적화 문제를 풀지 않고 필터링으로 처리하니, 계산이 훨씬 가볍다. 또한 ikd-Tree라는 증분적 KD-트리를 사용해서 맵에 새 점을 추가할 때도 빠르다.

> **추천 자료**
> - [Xu & Zhang, "FAST-LIO: A Fast, Robust LiDAR-Inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter" (2021)](https://arxiv.org/abs/2010.14709) — 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). 더 정확하지만 구현이 복잡하다.

**IMU Preintegration**:
두 키프레임 사이의 IMU 측정을 사전 적분하여 상대 변환 계산. 재선형화 없이 최적화 가능.

#### 14.5.2 LiDAR + IMU (LIO)

LiDAR는 10–20Hz로 스캔하는데, 로봇이 빠르게 움직이면 한 스캔 내에서도 로봇이 이동한다(motion distortion). 200–400Hz로 측정하는 IMU로 스캔 중 움직임을 보정(de-skewing)한 뒤, LiDAR로 정밀하게 맞추는 것이 LIO의 기본 구조다. 고속 움직임 상황에서 LiDAR 단독보다 유리한 이유가 여기에 있다.

#### 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가 바로 그 뒤틀림을 교정하는 핵심 메커니즘이다. 이것 없이는 대규모 환경에서 SLAM이 사실상 불가능하다.

#### 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는 포인트 클라우드를 bird-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만 수행하는 경우가 많다.

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 점유 격자 지도를 구축하는 과정을 시각화하며, 센서 측정이 어떻게 확률적 지도로 변환되는지 확인할 수 있다.

---

## 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이 왜 주목받는지: 기존 feature-based SLAM(ORB-SLAM)은 특징점이 없는 환경에서 실패하고, direct method(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 백엔드 최적화

*연구자가 되고 싶다면 여기서부터 읽어라.*

SLAM 프론트엔드가 센서 데이터를 처리해서 제약 조건(constraint)을 만들어내면, 백엔드는 이 제약 조건들을 동시에 만족하는 최적의 상태(포즈, 랜드마크)를 찾는다. 이 과정은 비선형 최소제곱(nonlinear least squares) 문제다. 여기서 다루는 내용은 g2o, GTSAM, Ceres 같은 라이브러리를 "왜 그렇게 설정하는지" 이해하기 위한 수학적 배경이다.

**SLAM 백엔드가 푸는 문제의 직관**

복잡한 수식을 보기 전에 한 가지만 기억하자: SLAM 백엔드는 결국 **Ax = b를 푸는 문제**이다.

로봇이 주행하면서 얻는 데이터는 두 종류다:
1. **오도메트리**: "나는 1m 앞으로 갔다" (상대적 이동)
2. **관측**: "저 랜드마크가 3m 거리에 보인다"

이 측정값들을 모두 만족시키는 포즈와 랜드마크 위치를 찾고 싶지만, 센서 노이즈 때문에 완벽히 만족시키는 해는 없다. 대신 "모든 측정값과의 오차 제곱합을 최소화"하는 해를 찾는다. 이것이 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 operator)는 6-벡터를 4x4 행렬로 변환한다.

**Jacobian 계산**: 오차 함수 `e(T)`의 Jacobian을 `δξ`에 대해 계산한다.
```
J = ∂e / ∂δξ
```
이것은 chain rule로 `∂e/∂T · ∂T/∂δξ`가 되는데, `∂T/∂δξ`가 바로 SE(3)의 left Jacobian이다.

**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)`으로 싸다.

풀어야 하는 시스템 크기가 `n`에 관계없이 포즈 수 `m`에만 비례하게 된다. BA가 수만 개의 랜드마크를 다루면서도 실시간에 가까운 성능을 내는 이유가 여기에 있다.

`δp`를 구한 뒤, `δl`은 back-substitution으로 복원한다:
```
δl = H_ll^{-1} (b_l - H_lp · δp)
```

#### 14.9.3 희소성과 Variable Ordering

Pose graph optimization에서 `H` 행렬은 **sparse**하다. 각 포즈는 시간적으로 인접한 포즈, 그리고 loop closure로 연결된 포즈와만 제약 관계를 갖는다. 전체 포즈가 1000개라 해도, 각 포즈가 연결된 포즈는 기껏해야 수 개~수십 개이다.

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): 가장 널리 쓰이는 heuristic. 연결이 적은 변수부터 제거하는 방식
- **AMD** (Approximate Minimum Degree): COLAMD와 유사하지만 symmetric 행렬에 특화
- **Nested dissection**: 그래프를 재귀적으로 분할하여 ordering을 결정. 대규모 문제에서 효과적

g2o, GTSAM, Ceres 같은 라이브러리에서 solver를 설정할 때 linear solver type(DENSE_SCHUR, SPARSE_NORMAL_CHOLESKY 등)과 ordering strategy를 선택해야 한다. 이 배경 지식 없이 기본 설정으로 돌리면 "느려서 다른 라이브러리로 갈아탔다"는 식의 비효율이 생긴다. ordering만 바꿔도 10배 이상 속도 차이가 날 수 있다.

```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, VINS-Fusion, 연구용 |
| **Ceres Solver** | 범용 nonlinear least squares, auto-diff 지원, Google 개발 | Cartographer, 다양한 프로젝트 |

선택 기준:
- SLAM 전용이고 가볍게 쓰고 싶다 → g2o
- Factor graph 모델링이 필요하고, incremental update(키프레임이 추가될 때마다 점진적으로 최적화)가 중요하다 → GTSAM (iSAM2)
- SLAM 이외의 범용 최적화도 해야 하고, Jacobian을 직접 유도하기 싫다 → Ceres (auto-diff)

> **추천 자료**
> - Barfoot, "State Estimation for Robotics" Ch.4 (Nonlinear Estimation) — Manifold 위의 최적화를 체계적으로 설명
> - [CMU 16-833 Robot Localization and Mapping Lecture Notes](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

*연구자가 되고 싶다면 여기서부터 읽어라.*

14.3.3에서 VINS-Mono를 소개할 때 IMU preintegration을 간단히 언급했다. 여기서는 그 수학적 배경을 본다.

**문제 정의**: IMU는 보통 200~1000 Hz로 가속도와 각속도를 출력한다. 반면 SLAM 최적화는 키프레임 단위(수 Hz~수십 Hz)로 수행한다. 키프레임 사이에 수백 개의 IMU 측정이 존재하는데, 최적화할 때 이것을 전부 상태 변수로 넣으면 문제 크기가 폭발한다.

**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하는가**: 이전 방식은 IMU 측정을 가장 가까운 키프레임 timestamp에 interpolation해서 사용했다. 그런데 회전은 SO(3)에 있으므로 단순 선형 보간이 정확하지 않다. Lie group 위에서 미리 integration해두면, 1) 회전 누적이 수학적으로 정확하고, 2) 결과가 relative motion measurement로서 factor graph에 바로 들어갈 수 있다. Forster et al. (2015 RSS, 2017 TRO)의 핵심 기여가 이 부분이다.

**Tightly-coupled vs Loosely-coupled**: LIO-SAM을 예로 설명하면:
- **Loosely-coupled**: IMU를 다음 포즈의 초기값(initial guess)으로만 사용한다. 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) — Preintegration의 원본 논문. 수식이 많지만 이 분야의 필수 논문
> - [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 시스템의 관측 불가능한 상태**: VIO에서 추정할 수 없는(unobservable) 자유도가 4개 있다:

1. **Global position (3 DoF)** — 절대 위치를 알 수 없다. GPS 같은 절대 기준이 없으면 시작점을 원점으로 잡을 수밖에 없다
2. **Global yaw (1 DoF)** — 중력 방향 축 기준의 회전(heading). 나침반 없이는 "어느 쪽이 북쪽인지" 알 수 없다

반면 다음은 관측 가능하다:
- **Roll/pitch**: IMU accelerometer가 중력 방향을 감지하므로, 중력 기준 roll/pitch는 추정 가능
- **Scale** (stereo/IMU가 있는 경우): stereo 카메라의 baseline이나 IMU의 가속도 측정으로 스케일 복원 가능. 단, **monocular 카메라만으로는 scale이 관측 불가능**하다

**Degenerate motion** — 특정 움직임 패턴에서 추가적인 상태가 관측 불가능해진다:

- **순수 회전(pure rotation)**: Monocular VO에서 translation을 추정할 수 없다. Epipolar geometry에서 epipole이 무한원점으로 가기 때문이다. 실무에서 "카메라를 제자리에서 돌리면 트래킹이 깨진다"는 현상의 원인
- **등속 직진(constant velocity)**: IMU의 accelerometer가 중력과 가속도를 구분하는데, 가속이 없으면(등속이면) accelerometer bias와 중력 방향의 미세한 오차를 구분할 수 없다. IMU bias가 관측 불가능해진다
- **정지(stationary)**: 등속의 특수 케이스. 가만히 서있으면 visual feature의 parallax도 없고 IMU 가속도도 없어서, bias와 스케일 모두 관측 불가능. "왜 가만히 서있으면 VINS가 drift 하는가?"의 답

**EKF 기반 시스템에서의 문제**: 표준 EKF를 VIO에 적용하면, linearization 오차로 인해 이론적으로 관측 불가능한 방향에서도 공분산이 줄어드는(uncertainty가 인위적으로 감소하는) 현상이 발생한다. 이것은 inconsistency의 주요 원인이다.

**OC-EKF (Observability-Constrained EKF)**: 이 문제를 해결하기 위해, EKF의 Jacobian을 수정하여 관측 불가능한 방향의 null space를 보존한다. 추정기가 "모르는 것은 모른다고 유지"하도록 강제하는 것이다.

실무적 함의:
- VIO 시스템을 사용할 때는 **다양한 방향으로 움직이면서** 초기화해야 한다. 한 방향으로만 걸으면 IMU bias 추정이 제대로 되지 않는다
- Loop closure가 없는 VIO는 장시간 운용 시 반드시 drift 한다. 관측 불가능한 yaw 방향의 오차가 축적되기 때문
- Monocular VIO의 스케일은 가속/감속이 있어야 관측 가능. 일정 속도로만 움직이면 스케일 drift가 생긴다

> **추천 자료**
> - [Hesch et al., "Observability-constrained Vision-aided Inertial Navigation" (TRO 2014)](https://ieeexplore.ieee.org/document/6672119) — OC-EKF/OC-VINS의 원본 논문
> - 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 등)**: 새 측정이 들어올 때마다 상태를 점진적으로 업데이트한다. 과거 상태는 marginalize하여 현재 상태만 유지한다. 메모리 효율적이고, proprioceptive 센서(IMU)와의 결합에 자연스럽다.
- **최적화 (BA, factor graph)**: 과거 상태를 전부 유지하고 한꺼번에 최적화한다. 과거 데이터를 relinearize할 수 있으므로 정확도가 높다. 하지만 상태 수가 늘어나면 계산량이 커진다 (sliding window나 iSAM2로 완화).

그러면 "VINS-Mono(최적화)가 MSCKF(필터)보다 낫다"는 건 solver 때문인가? 아니다. 차이는 solver가 아니라 **시스템 구조**(어떤 상태를 유지하는가, 어떤 측정을 사용하는가)에서 온다. 최적화 기반이 relinearize를 통해 과거 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)인 지도를 만든다. 포인트 클라우드, 메쉬, occupancy grid 등 "공간의 형태"만 기록한다. 벽이 있다는 건 알지만 그것이 벽인지, 문인지, 책장인지는 모른다. 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 접근**은 모든 로봇의 센서 데이터 또는 로컬 지도를 중앙 서버로 전송하고 서버에서 전체 SLAM을 수행한다. 구현이 단순하고 최적해에 가깝지만, 원본 데이터를 전부 보내면 통신 대역폭이 병목이 되고 서버가 단일 장애점(single point of failure)이 된다.

**Distributed 접근**은 각 로봇이 독립적으로 로컬 SLAM을 수행하고, rendezvous 또는 inter-robot loop closure가 발생했을 때 상대 포즈를 추정하여 지도를 정렬한다. 원본 데이터 대신 compressed descriptor(NetVLAD vector, summary map 등)를 교환하므로 통신 효율이 높다. 각 로봇은 자신의 포즈만 최적화하면서도 이웃 로봇과의 제약을 통해 전체가 수렴하는 구조다.

분산 시스템에서 반드시 해결해야 할 문제가 셋 있다. **inter-robot loop closure**는 로봇 A가 방문한 장소를 로봇 B가 나중에 방문했을 때 인식하는 것으로, 14.14절의 place recognition이 핵심이다. **좌표계 정렬**은 각 로봇이 독립 좌표계에서 시작하기 때문에 필요하다. 최소 3개의 inter-robot correspondence에서 상대 SE(3) 변환을 추정해야 정렬이 가능하다. **outlier rejection**은 inter-robot loop closure의 false positive가 많을 수 있어 PCM(Pairwise Consistency Maximization)이나 GNC(Graduated Non-Convexity) 같은 robust 기법이 필요하다. 분산 최적화는 Distributed Gauss-Seidel, ADMM 등을 사용한다.

**대표 시스템**:
| 시스템 | 특징 |
|---|---|
| **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" (ICRA 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로 압축하는 방식으로 BoVW보다 robust하다. **NetVLAD** (2016)는 CNN feature를 VLAD aggregation으로 결합하여 도시 규모 장소 인식에서 기존 방법을 크게 앞섰다. 이후 **CosPlace** (2022)는 contrastive learning으로 학습 파이프라인을 단순화하면서 성능을 높였다. **MixVPR** (2023)은 feature mixing으로 주간/야간·계절 변화 등 다양한 조건에서 robust하다. **AnyLoc** (2023)은 DINOv2 feature를 활용하여 별도 fine-tuning 없이 indoor/outdoor, aerial/ground 어떤 환경에서도 동작하는 zero-shot place recognition을 내놓았다.

**LiDAR 기반 Place Recognition**:

시각 정보 없이 3D 구조만으로 장소를 인식한다. 조명 변화에 완전히 면역이지만, 구조적으로 유사한 환경(예: 긴 복도)에서 혼동될 수 있다.

- **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를 학습하여 더 효율적으로 시퀀스 매칭

실무 팁: 대부분의 SLAM 시스템은 DBoW2를 기본으로 쓴다. 만약 조명/계절 변화가 큰 환경에서 운용해야 한다면, 학습 기반 방법(NetVLAD 이후의 방법들)으로 교체하는 것을 고려하라. AnyLoc은 fine-tuning 없이 쓸 수 있어서 진입 장벽이 낮다.

> **추천 자료**
> - [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 SLAM의 기틀 마련. ORB-SLAM(2015)이 Feature-based Visual SLAM의 완성형으로 등극
> - **2015~2020**: Visual-Inertial 시대. VINS-Mono(2018), MSCKF 등 VIO 시스템이 드론/모바일에서 표준으로 자리잡음. DSO(2018)가 Direct Sparse 방식 제안. LiDAR-Inertial 결합 본격화: LIO-SAM(2020)
> - **2020~2023**: FAST-LIO/FAST-LIO2(2021/2022)가 경량 LiDAR-Inertial 시스템의 새 표준. ORB-SLAM3(2021)가 Visual-Inertial + 멀티맵 지원. DROID-SLAM(2021)이 learning-based SLAM의 가능성을 보여줌. R3LIVE 등 멀티센서 통합 시스템 등장
> - **2024~**: 3DGS 기반 SLAM(SplaTAM, MonoGS, Gaussian-SLAM)이 Neural SLAM의 방향을 바꾸고 있다. Foundation Model과 SLAM의 결합(예: 자연어로 장소를 설명하여 위치를 찾는 등) 연구도 시작
> - **지금 주목할 것**: 기존 geometric SLAM(ORB-SLAM3, FAST-LIO2)은 이미 성숙한 기술이므로 꼭 익혀두고, 3DGS-SLAM과 learning-based 방법은 트렌드로 지켜보자. 실무에서는 LIO-SAM/FAST-LIO2(실외)와 ORB-SLAM3(실내)가 여전히 가장 많이 쓰인다. 벤치마크 데이터셋(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를 확보한 후, false removed point를 복원하는 two-stage 방식을 사용한다. 핵심은 multiple confidence level로 trade-off를 조절할 수 있다는 점이다.

기존 접근과 비교하면: voxel ray-casting은 정확하지만 연산이 비싸고, visibility-based는 뒤의 static point를 보존한다는 가정이 필요하며, segmentation-based는 unknown label에 약하고 scan-to-map 관계를 무시한다. 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한 환경에서 dynamic이지만 geometric으로 유용한 point가 있을 수 있기 때문이다.

#### 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, weak non-dynamic, strong positive-dynamic을 구분하여 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

---

# Ch.15 — 로봇 프레임워크 (Robot Frameworks)

로봇 소프트웨어를 처음 짜는 사람이 가장 당황하는 순간은, 센서 드라이버·경로 계획·모터 제어를 하나의 프로그램 안에서 전부 돌려야 한다는 걸 깨달을 때이다. 프레임워크는 이 문제를 구조적으로 해결해 준다. 프레임워크 없이 로봇을 만들면 "센서 데이터 받기 → 판단하기 → 모터 명령 보내기"만 해도 스레드 관리·메시지 직렬화·좌표 변환을 전부 직접 구현해야 하는데, 이걸 모르면 첫 프로젝트에서 벽에 부딪힌다. 여기서는 사실상 업계 표준인 ROS를 중심으로, 시뮬레이터와 주변 도구까지 폭넓게 본다.

## 15.1 ROS (Robot Operating System)

ROS는 로봇 소프트웨어 개발을 위한 오픈소스 프레임워크이다. 운영체제가 아닌 **미들웨어**로, 프로세스 간 통신, 패키지 관리, 도구 등을 제공한다.

로봇은 카메라·LiDAR·모터·제어기 같은 수십 가지 모듈이 동시에 돌아가야 하는데, 이 모듈들이 서로 데이터를 주고받는 방법을 통일해 주는 것이 ROS의 핵심 역할이다. ROS 없이 이 작업을 하면 소켓 프로그래밍부터 시작해야 하는데, 연구 시간의 대부분이 인프라 구축에 날아간다.

### 15.1.1 ROS1 vs ROS2

| 특징 | ROS1 | ROS2 |
| --- | --- | --- |
| 출시 | 2007 | 2017 |
| 통신 | Custom (TCPROS) | DDS 기반 |
| 실시간 | 미지원 | 지원 |
| 보안 | 없음 | SROS2 |
| Multi-robot | 어려움 | 쉬움 |
| Master | 필요 (roscore) | 불필요 |
| Python | 2/3 | 3 only |

**현재 권장**: ROS2 (Humble)

하지만 많은 패키지가 아직 ROS1만 지원하므로 상황에 따라 선택.

**ROS1 → ROS2 마이그레이션 현황**: 2024년을 기점으로 ROS1 Noetic의 공식 지원이 종료(EOL)되었다. 새 프로젝트라면 특별한 이유가 없는 한 ROS2로 시작한다. 기존 ROS1 패키지를 써야 한다면 `ros1_bridge`로 두 노드를 동시에 운용할 수 있다. Nav2, MoveIt2 등 핵심 패키지의 ROS2 포팅이 완료된 상태이므로, 대부분의 로봇 개발에서는 ROS2만으로 충분하다.

> **추천 자료**
> - [ROS2 공식 튜토리얼](https://docs.ros.org/en/humble/Tutorials.html) — ROS2 Humble 기준 공식 단계별 가이드. 처음이라면 "Beginner: CLI tools"부터 시작
> - [The Construct - ROS2 Basics](https://www.youtube.com/@TheConstruct) — ROS 전문 교육 채널. 시뮬레이터 내에서 실습 가능
> - [ROS1 to ROS2 Migration Guide](https://docs.ros.org/en/humble/How-To-Guides/Migrating-from-ROS1.html) — 기존 ROS1 코드 이전 공식 가이드

### 15.1.2 핵심 개념

이 개념들은 ROS의 뼈대이다. Topic, Service, Action의 차이를 정확히 모르면 "센서 데이터를 어떤 방식으로 보내야 하지?" 하는 질문에서 매번 막히게 된다. 각각이 언제 적합한지 감을 잡는 것이 중요하다.

**Node (노드)**:
- 실행 가능한 프로세스
- 단일 목적 (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 (서비스)**:
- 동기 요청/응답
- 일회성 작업에 적합
- 예: 설정 변경, 상태 조회

**Action (액션)**:
- 비동기 목표 지향 작업
- Feedback 제공
- 취소 가능
- 예: 네비게이션, 조작

정리하면, Topic은 카메라 영상처럼 계속 흘러가는 데이터에, Service는 "지금 배터리 잔량 알려줘"처럼 한 번 물어보고 답 받는 상황에, Action은 "저기까지 가"처럼 시간이 걸리는 작업에 쓰인다. 이 세 가지를 구분하지 못하면 시스템 설계에서 계속 꼬인다.

**Parameter (파라미터)**:
- 노드 설정값
- 런타임 변경 가능

> **⚠ AI 에이전트 주의**: AI에게 ROS2 코드를 짜달라고 할 때는 QoS 설정을 명시하라. AI는 기본적으로 QoS를 빠뜨리고, 센서 토픽이 조용히 안 들어와서 한참 헤매게 된다.

> **추천 자료**
> - [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 리스너 예시
from tf2_ros import Buffer, TransformListener

tf_buffer = Buffer()
tf_listener = TransformListener(tf_buffer, self)

# base_link → camera_link 변환 조회
transform = tf_buffer.lookup_transform('base_link', 'camera_link', rclpy.time.Time())
```

> **추천 자료**
> - [ROS2 tf2 Tutorials](https://docs.ros.org/en/humble/Tutorials/Intermediate/Tf2/Tf2-Main.html) — 좌표 변환 공식 튜토리얼
> - [The Construct - rviz2 Complete Guide](https://www.youtube.com/@TheConstruct) — rviz2 활용 영상
> - [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 주요 패키지

ROS의 진짜 힘은 커뮤니티가 만들어 놓은 패키지 생태계에 있다. 아래 패키지들은 거의 모든 로봇 프로젝트에서 한 번은 쓰게 되니, 이름이라도 기억해 두자.

| 패키지 | 용도 |
| --- | --- |
| 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 기반으로 제공되므로, 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) 연구
- 대규모 합성 데이터 생성
- Sim-to-Real 학습

**Embodied AI 시뮬레이터 비교**: 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

자율주행 연구를 위한 시뮬레이터이다.

자율주행 논문에서 실험 환경으로 CARLA를 사용하는 경우가 매우 많다. 자율주행 쪽 연구를 하고 싶다면 CARLA 환경을 다루는 법을 익혀 두면 좋다.

**특징**:
- 도시 환경 시뮬레이션
- 다양한 날씨, 시간대
- 센서 시뮬레이션 (카메라, LiDAR, Radar)
- ROS 브릿지 제공

> **추천 자료**
> - [CARLA Documentation](https://carla.readthedocs.io/) — 공식 문서 및 Python API 레퍼런스
> - [CARLA Simulator YouTube](https://www.youtube.com/@intaborlado5265) — 시뮬레이터 활용 데모 영상

## 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는 거기에 회전(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를 구간별로 할당
- 예시: 자율주행 — 센서 입력(10ms) → 인식(50ms) → 계획(30ms) → 제어(10ms) = 100ms total
- 각 구간이 budget을 초과하면 전체가 실패. 가장 느린 구간이 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 ─── ROS1 출시 (Willow Garage)
  │       로봇 미들웨어로 널리 사용됨
  │
2012 ─── Gazebo Classic 독립 프로젝트화
  │       시뮬레이션이 로봇 개발의 필수 단계로 자리잡음
  │
2017 ─── ROS2 첫 릴리즈
  │       DDS 기반 통신, 실시간 지원, 보안 추가
  │
2019 ─── NVIDIA Isaac Sim 공개
  │       RTX 기반 고품질 렌더링 + 합성 데이터 생성
  │
2020 ─── Habitat, AI2-THOR 등 Embodied AI 시뮬레이터 부상
  │       대규모 학습 기반 로봇 정책 연구 본격화
  │
2022 ─── ROS2 Humble LTS 출시
  │       산업계 채택 가속화, Nav2/MoveIt2 안정화
  │
2024 ─── ROS1 Noetic EOL (지원 종료)
  │       ROS2 전환의 사실상 마감 시점
  │
2025+ ── Embodied AI + Foundation Models 시대
          시뮬레이터에서 대규모 사전학습 → Sim-to-Real 전이
          언어 명령 기반 로봇 조작 (VLA)
          NVIDIA Isaac Lab 등에서 시뮬레이터→학습→실제 로봇 배포를 일관된 파이프라인으로 제공 시작
```

---

# Ch.16 — 개발 환경 & 도구

로봇 연구를 하다 보면, 알고리즘 자체보다 "환경 세팅"에 시간을 더 많이 쓰는 경우가 흔하다. CUDA 버전이 안 맞아서 하루를 날리거나, 다른 사람 코드를 클론했는데 파이썬 버전 차이로 안 돌아가는 경험을 한 번쯤 하게 된다. 여기서 소개하는 도구들은 그런 삽질을 최소화해 주는 것들이다. 화려하지 않지만, 연구 생산성에 직접 영향을 미치는 실전 지식이다.

## 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) 기능을 체계적으로 정리한 블로그

> **⚠ AI 에이전트 주의**: AI가 생성한 C++ 코드가 x86에서는 빌드되지만 Jetson(ARM)에서 실패하는 경우가 많다. 크로스 컴파일 환경이나 타겟 아키텍처를 알려줘라.

### 16.1.2 Python

**용도**: 프로토타이핑, 딥러닝 학습/추론, 데이터 분석, 시각화

PyTorch 학습 스크립트나 데이터 전처리 같은 작업에 사용된다. 연구에서 자주 마주치긴 하지만, AI 에이전트가 가장 잘 다루는 언어이기도 해서 직접 작성하는 비중은 줄어들고 있다. 읽고 이해할 수 있으면 충분하다.

**자주 쓰는 라이브러리**:

```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가 Ubuntu를 1차 지원 플랫폼으로 삼고 있고, GPU 드라이버·CUDA·cuDNN 등의 호환성도 Ubuntu에서 가장 잘 검증됐기 때문이다. macOS나 Windows에서도 일부 개발이 가능하지만, 결국 실제 로봇에 올릴 때는 Ubuntu로 돌아오게 된다.

**권장 버전**:
- 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 등 "수업에서는 안 가르치지만 매일 쓰는" 개발 도구를 체계적으로 정리. 추천한다
> - [Fireship - Linux in 100 Seconds](https://www.youtube.com/watch?v=rrB13utjYV4) — Linux가 뭔지 빠르게 감 잡기

### 16.2.2 CUDA / cuDNN

딥러닝 모델 학습은 CPU로는 현실적으로 불가능하다. GPU 가속을 위해 CUDA가 필요한데, PyTorch와 CUDA 버전이 안 맞으면 `import torch` 한 줄에서부터 에러가 난다. 로보틱스 연구자가 가장 많이 겪는 환경 문제 중 하나이다.

**설치 확인**:

```bash
nvidia-smi          # GPU 상태
nvcc --version      # CUDA 버전
```

**권장 버전**: CUDA 12.x, cuDNN 8.x

**주의**: PyTorch/TensorFlow 버전과 CUDA 버전 호환성 확인 필수

> **추천 자료**
> - [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) Secure Boot가 켜져 있는지 확인 (BIOS에서 끌 것), (2) `sudo dkms status`로 드라이버 모듈 상태 확인. (참고: [정진용 블로그](https://jinyongjeong.github.io/2016/11/22/ubuntu_graphic_driver_install/))

**CUDA/cuDNN 버전 호환성**

PyTorch와 CUDA 버전이 안 맞으면 `import torch` 한 줄에서 에러가 난다. 확인 순서:

```bash
# 1. GPU 확인
nvidia-smi  # 오른쪽 상단의 CUDA Version은 "드라이버가 지원하는 최대 버전"

# 2. 설치된 CUDA toolkit 확인
nvcc --version

# 3. PyTorch가 사용하는 CUDA 확인
python -c "import torch; print(torch.version.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 cmake git libgtk2.0-dev pkg-config \
    libavcodec-dev libavformat-dev libswscale-dev \
    libtbb-dev libjpeg-dev libpng-dev

# 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
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"  \  # GPU에 맞게 조정 (RTX 3090=8.6, RTX 4090=8.9)
      -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가 충돌하는 문제가 있다 (19장 AI 에이전트 트러블슈팅 참고). 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

# 패키지 설치
conda install pytorch torchvision pytorch-cuda=12.1 -c pytorch -c nvidia
```

**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, 라이브러리, 환경 설정을 통째로 패키징해서 어디서든 동일한 실행 환경을 보장하기 때문이다. 논문 코드를 재현할 때도 Docker 이미지가 제공되면 훨씬 수월하다.

- **재현성**: 동일한 환경 보장
- **격리**: 시스템 오염 방지
- **배포**: 쉬운 공유 및 배포
- **의존성**: 복잡한 의존성 관리

### 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` 플래그를 사용해야 한다. 로보틱스 연구에서 Docker를 쓴다면 사실상 필수다.

참고: 과거에 쓰던 `nvidia-docker2`는 deprecated됐다. 현재는 `nvidia-container-toolkit`이 표준이고, `--runtime=nvidia` 대신 `--gpus all`을 쓴다.

```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 + 센서

로보틱스에서 Docker를 쓸 때는 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`는 Docker 컨테이너에서만 X 서버 접근을 허용한다. `xhost +`(전체 허용)보다 안전하지만, 보안이 중요한 환경이라면 `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 통신을 위해 호스트 네트워크 공유. ROS2 노드 간 통신에 필수 |
| `--ipc=host` | 공유 메모리. Rviz 등 GUI 도구에서 필요 |
| `-e QT_X11_NO_MITSHM=1` | 이거 빠지면 RViz가 segfault로 죽는다. MIT-SHM이 Docker에서 안 됨 |
| `-e ROS_DOMAIN_ID=42` | 같은 네트워크의 다른 ROS2 시스템과 격리. 연구실에서 여러 명이 쓸 때 필수 |
| `-v /dev:/dev` | 센서 USB가 언제 꽂힐지 모르므로 /dev 전체 마운트. `--privileged`와 세트 |
| `-v .Xauthority` | X11 인증. 이걸 안 걸면 `cannot open display` |

**3단계: 작업 후 컨테이너 저장**

```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으로 만든 이미지는 히스토리가 없어서 재현이 안 된다.

> **추천 자료**
> - [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 포함 버전

> **AI 에이전트 주의**: AI에게 Docker 설정을 물어볼 때는 "ROS2 + GPU + USB 센서 + GUI 시각화"를 동시에 쓸 건지 한 번에 알려줘라. 각각을 따로 물어보면 서로 충돌하는 설정을 준다. 특히 `QT_X11_NO_MITSHM=1`과 `ROS_DOMAIN_ID`는 AI가 빠뜨리는 대표적인 항목이다.

## 16.4 원격 관리: Git, SSH, 파일 전송

연구실 서버에 SSH로 접속해서 실험을 돌리고, 코드는 Git으로 관리하고, 데이터는 서버 간에 주고받는 것이 일상이다. 여기 나오는 도구들을 익혀 두면 이 과정이 매끄러워진다.

### 16.4.1 Git/GitHub

### 16.4.1.1 기본 워크플로우

코드를 관리하지 않으면, "어제 돌아가던 코드가 오늘은 왜 안 되지?"라는 상황이 반복된다. Git은 모든 변경 이력을 추적해 주므로, 언제든 과거 상태로 돌아갈 수 있다. 연구 코드라도 Git으로 관리하는 습관을 들이자.

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

연구실에서 여러 사람이 같은 코드를 수정할 때 브랜치 전략 없이 `main`에 직접 push하면 충돌이 끊이질 않는다. 연구 코드라면 GitHub Flow 정도면 충분하다. 기능 하나당 브랜치 하나를 만들고, PR을 통해 머지하는 습관을 들이자.

### 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 출시
  │       오픈소스 협업의 중심지가 됨
  │
2010 ─── Conda (Anaconda) 등장
  │       Python 환경 관리 표준으로 자리 잡음
  │
2013 ─── Docker 공개
  │       컨테이너 기반 가상화로 재현성 문제 해결
  │
2015 ─── TensorBoard (TensorFlow와 함께 공개)
  │       딥러닝 학습 시각화의 시작
  │
2017 ─── nvidia-docker 공개
  │       Docker 컨테이너에서 GPU 사용 가능
  │
2018 ─── Weights & Biases 출시
  │       실험 추적·시각화·팀 협업을 클라우드로
  │
2020 ─── Ruff 등장 (2022), Black 대중화
  │       Python 코드 품질 도구의 고속화
  │
2023+ ── AI-assisted 개발 도구 확산
          Copilot, Cursor 등 AI 코딩 보조 도구
          Dev Container 표준화 (VS Code Remote)
          재현 가능한 연구를 위한 Docker + wandb 조합 보편화
```

---

# Ch.17 — 데이터셋 & 벤치마크

로보틱스와 컴퓨터 비전 연구에서 데이터셋은 알고리즘만큼 중요하다. 좋은 데이터 없이는 좋은 모델을 만들 수 없고, 공정한 벤치마크 없이는 논문에서 자기 방법이 진짜 좋은지 증명할 수 없다. 이 챕터는 주요 데이터셋의 구성과 특징, 자체 데이터 수집·관리 방법을 정리한다.

최근 **합성 데이터(Synthetic Data)**의 비중이 높아지고 있다. 실제 데이터 수집과 라벨링은 비용과 시간이 많이 드는데, 시뮬레이터에서 자동 생성한 합성 데이터로 사전학습한 뒤 소량의 실제 데이터로 미세조정(fine-tuning)하는 방식이 자리를 잡았다. NVIDIA Isaac Sim의 Domain Randomization이나 Habitat의 대규모 장면 생성이 대표적이다. **Sim-to-Real 데이터셋** — 시뮬레이터 데이터와 대응하는 실제 데이터를 쌍으로 제공하는 데이터셋 — 도 활발히 구축되고 있다.

## 17.1 자율주행/로봇 데이터셋

### 17.1.1 KITTI / KITTI360

자율주행 연구의 시작점이 된 오래된 데이터셋이다.

KITTI는 2012년에 공개된 이후 자율주행·3D 비전 연구의 사실상 표준 벤치마크 역할을 해 왔다. 비록 지금은 더 크고 다양한 데이터셋이 있지만, 많은 논문이 여전히 KITTI 결과를 보고하고 있어서 기준점으로 알아 두어야 한다. 특히 Visual Odometry, Stereo Depth Estimation 분야에서는 아직도 KITTI가 1차 벤치마크이다.

**구성**:
- 스테레오 카메라
- 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 포함), 데이터 규모도 훨씬 크다. 최근 자율주행 논문에서 KITTI와 함께 가장 많이 인용되는 데이터셋이다. 특히 3D Object Detection과 BEV(Bird's Eye View) 기반 인식 연구에서 핵심적으로 쓰인다.

**구성**:
- 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

Google의 대규모 자율주행 데이터셋이다.

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 VIO / VINS용 데이터셋

Visual-Inertial Odometry(VIO)나 SLAM 연구를 한다면 아래 데이터셋은 알아야 한다. 이 분야의 논문이라면 거의 예외 없이 이 데이터셋들에서 평가를 수행한다.

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

객체 탐지, 세그멘테이션의 표준이다.

Object detection 연구를 한다면 COCO 데이터셋의 평가 메트릭(COCO mAP)은 업계 표준이므로 이해해야 한다. IoU threshold별 AP를 계산하는 방식이 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` 패턴을 모르면 학습 코드를 짤 수 없다. 특히 `__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의 타임스탬프가 10ms만 어긋나도 고속 주행 시 수십 cm의 위치 오차가 생긴다. 센서 퓨전의 기본 전제가 "같은 시점의 데이터"인데, 동기화가 안 되면 그 전제가 무너진다.

**하드웨어 동기화**:
- 트리거 신호로 동시 촬영
- PPS (Pulse Per Second) 신호

**소프트웨어 동기화**:
- 타임스탬프 기반 근사 동기화
- 보간(interpolation) 사용

**ROS에서는 받는 시점만 필터링 가능:**

```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은 4×4 변환 행렬 [R|t]에 해당한다.

> **추천 자료**
> - [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) 같은 기초 모델을 활용한 반자동 라벨링이 보편화되고 있다.

**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으로 텍스처, 조명, 배경을 무작위 변형하면 모델의 일반화 성능도 높일 수 있다. 실제 데이터 대비 수집 비용이 거의 0에 가깝다.

> **추천 자료**
> - [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          │ │
│  │ • 10-100 Hz         │     │ • 1-10 Hz                   │ │
│  └─────────────────────┘     └─────────────────────────────┘ │
└──────────────────────────────────────────────────────────────┘
```

**왜 두 모듈인가? — 직관적으로 이해하기**

"그냥 좋은 컴퓨터 하나 올리면 안 되나요?"라고 생각할 수 있다. 솔직히 그게 가능하면 그렇게 하고 싶다. 하지만 현실은 그렇지 않다.

먼저 **물리적 제약**을 생각해 보자. 로봇은 움직여야 한다. NVIDIA A100 GPU 서버를 드론에 올릴 수는 없다 — 무게만 해도 수십 kg이고, 전력도 수백 와트를 먹는다. 배터리로 돌아가는 로봇에게는 비현실적이다. 그래서 로봇에 실제로 탑재할 수 있는 컴퓨터는 Jetson Orin 같은 임베디드 보드인데, 이 보드로는 DINOv2나 SAM 같은 대형 모델을 실시간으로 돌릴 수 없다.

다음으로 **시간 제약**이 있다. 로봇이 복도를 걸어가고 있는데, 벽에 부딪히기 0.1초 전에 "잠깐, 서버 응답 기다리는 중..."이면 안 된다. 장애물 회피처럼 "지금 당장" 반응해야 하는 것과, "저 물체가 뭔지 이해하기"처럼 좀 느려도 괜찮은 것은 다른 종류의 문제다.

그래서 우리는 이렇게 나눈다:

1. **계산 자원의 현실**: 로봇 온보드 컴퓨터(Jetson 등)는 대형 모델을 돌릴 여력이 없다
2. **실시간 요구사항**: 장애물 회피는 즉각 반응 필요 — 0.1초가 생사를 가른다
3. **깊은 이해**: VFM/VLA는 높은 계산량 필요 — "저건 깨진 유리컵이니 조심해" 같은 판단
4. **상호 보완**: 기하학적 정밀함(Local) + 의미론적 이해(Global) = 진짜 똑똑한 로봇

> 비유하자면, Local Module은 로봇의 **반사 신경**이고, Global Module은 로봇의 **대뇌 피질**이다. 뜨거운 냄비를 만지면 손을 먼저 떼고(반사), 그다음에 "아, 불이 켜져 있었구나"라고 이해한다(인지). 로봇도 마찬가지다.

## 18.2 Local Module: Lightweight Geometry

로봇에 직접 탑재되어 실시간으로 동작하는 모듈이다. 로봇이 "지금 이 순간" 안전하게 움직이기 위해 필요한 최소한의 정보를 처리한다.

### 18.2.1 목표

- **Odometry**: 자신의 움직임 추정 — "나는 지금 어디에 있는가?"
- **Obstacle Detection**: 즉각적인 장애물 감지 — "앞에 뭔가 있다, 피해!"
- **Local Mapping**: 주변 환경의 기하학적 지도 — "내 주변 3m 이내는 이렇게 생겼다"

**실제 시나리오**: 배달 로봇이 아파트 복도를 지나가고 있다고 하자. 갑자기 아이가 뛰어나온다. 이때 Local Module은 depth 센서로 즉시 장애물을 감지하고, odometry로 자기 위치를 파악해서, 0.05초 안에 회피 경로를 계산한다. "아이인지 강아지인지"는 몰라도 된다 — 그건 Global Module의 일이다. Local Module은 "앞에 뭔가 있으니 피하자"만 알면 된다.

### 18.2.2 특징

- **저지연**: 10-100 Hz 동작 (10ms~100ms마다 한 번씩 처리)
- **제한된 자원**: Jetson, 임베디드 GPU — 전력 15~30W 수준
- **확정적 동작**: 예측 가능한 응답 시간 — "최악의 경우에도 50ms 안에 답을 준다"

### 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에서 2-5배 속도 향상

**Edge 배포**:

```bash
# TensorRT 최적화 예시
trtexec --onnx=model.onnx --saveEngine=model.trt --fp16 --workspace=4096
```

> TensorRT가 뭐냐면, PyTorch로 학습한 모델을 NVIDIA GPU에 최적화된 형태로 변환해주는 도구다. FP16(반정밀도)으로 바꾸면 모델 크기가 절반이 되면서도 정확도는 거의 유지된다. Jetson에서 YOLO를 돌릴 때 TensorRT 없이는 5 FPS, 있으면 30 FPS — 이 차이가 실제 로봇에서는 "쓸 수 있다 vs 없다"의 차이다.

### 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 업데이트 (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()`는 센서 데이터가 들어올 때마다 호출된다. IMU 데이터는 100Hz(초당 100번)로 오고, 카메라 이미지는 30Hz로 온다. 매 프레임마다 "나 지금 어디?" (odometry)와 "앞에 뭐 있어?" (obstacle)를 계산하고, 중요한 순간(키프레임)에만 Global Module에 데이터를 보낸다. 모든 프레임을 보내면 네트워크가 터지니까.

## 18.3 Global Module: VFM-based Understanding

서버 또는 클라우드에서 동작하는 고수준 이해 모듈이다. Local Module이 "앞에 뭔가 있다"까지만 알면, Global Module은 "저건 깨진 유리잔이고, 주인이 거실에서 떨어뜨린 것 같다"까지 이해한다.

### 18.3.1 목표

- **전체 지도 이해**: 공간 구조와 의미 파악 — "여기는 주방이고, 저기는 거실이다"
- **Semantic Scene Graph**: 객체 간 관계 표현 — "컵이 테이블 위에 있다"
- **Long-term Memory**: 환경 변화 추적 — "어제는 여기에 의자가 없었는데 오늘은 있다"

**실제 시나리오**: 가정용 서비스 로봇이 매일 집 안을 돌아다니면서 환경을 학습한다. Global Module은 "거실에 소파, TV, 테이블이 있고, 주방에는 냉장고, 싱크대가 있다"는 고수준 지도를 유지한다. 사용자가 "거실 테이블에 있는 리모컨 가져와"라고 하면, Scene Graph에서 리모컨의 위치를 찾아 Local Module에 waypoint를 전달한다.

### 18.3.2 특징

- **높은 정확도**: 대형 VFM 활용 — DINOv2, SAM2 같은 수십억 파라미터 모델
- **풍부한 컴퓨팅**: GPU 서버, 클라우드 — RTX 4090, A100 수준의 GPU
- **비실시간 허용**: 1-10 Hz — "1초에 한 번 업데이트해도 괜찮다"

### 18.3.3 기술 스택

**Vision Foundation Models** (→ 11장 참고):
- DINOv2: Dense feature extraction — 이미지의 모든 픽셀에 대해 의미 있는 feature 벡터 생성
- SAM2: Open-vocabulary segmentation — "아무 물체나" 지정하면 정확하게 분리
- 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. Open-vocabulary segmentation
        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 — "1m 이동하거나 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: 모바일 로봇 (실외 환경)

**동기화 전략**:
- 키프레임 기반 (연속 스트리밍 X) — 대역폭 절약
- 비동기 처리 (Global 완료 안 기다림) — Local은 멈추지 않는다
- 캐싱 (자주 방문 영역) — 매번 같은 데이터를 보내지 않는다

## 18.5 연구 과제 예시

아래 연구 과제들은 실제로 우리 연구실에서 진행하거나 진행할 수 있는 주제들이다. 각 과제에 대해 **선행 학습이 필요한 챕터**를 표시해두었으니, 관심 있는 주제가 있으면 해당 챕터부터 공부하자.

### Local Module 연구

1. **더 가벼운 SLAM**
    - 신경망 기반 경량 VO — 기존 VO를 neural network로 대체하되, Jetson에서 돌아가게
    - 이벤트 카메라 활용 — 초고속, 저전력 카메라로 극한 환경에서 SLAM
    - 하드웨어 가속 (FPGA) — SLAM의 핵심 연산을 하드웨어로 구현
    - **선행 학습**: 9장(카메라 모델), 14장(Visual Odometry, SLAM) 필수. 3장(최적화)도 권장
2. **효율적 장애물 인식**
    - Depth-only obstacle detection — RGB 없이 depth 정보만으로 장애물 감지
    - Temporal consistency — 프레임 간 일관성 유지 (한 프레임에서 보였다 안 보였다 하면 안 됨)
    - 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 — "저 빨간 거 집어" 같은 명령으로 로봇팔 제어
    - Language-guided navigation — 자연어 명령으로 이동
    - 상황 인식 행동 — "아이가 있으니 천천히 움직여라"
    - **선행 학습**: 11장(VFM 활용), 12장(VLA) 필수. 10장(Detection)도 알면 좋다
3. **Scalability**
    - 대규모 환경 표현 — 아파트 단지 전체, 캠퍼스 전체를 하나의 지도로
    - 지도 압축 및 업데이트 — 수 GB짜리 지도를 효율적으로 관리
    - Multi-robot 협업 — 여러 로봇이 함께 지도를 만들고 공유
    - **선행 학습**: 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 지도 동기화 — 두 모듈의 지도가 다르면 로봇이 혼란
    - Semantic 정보 일관성 — "저건 의자"라고 했는데 나중에 "테이블"로 바뀌면 안 됨
    - **선행 학습**: 3장(최적화), 14장(SLAM, 맵 관리) 필수

## 18.6 Motivation ≠ Novelty — 명작 논문 3편에서 배우는 도약

연구 방향을 잡을 때 가장 자주 막히는 자리가 motivation과 novelty의 구분이다. 신입생이 자기 첫 글을 검토할 때 가장 먼저 부딪히는 벽이기도 하다.

**핵심 명제.** "기존이 X를 못한다 → 우리가 모듈을 붙였다"는 **motivation**이지 **novelty**가 아니다. Top-tier 논문은 항상 "왜 그 모듈이 그 형태여야 하는가"에 원리적으로 답한다. 같은 문제를 풀더라도 2티어 논문은 motivation 단계에서 멈추고, top-tier 논문은 한 걸음 더 나아간다.

세 편의 명작이 그 도약의 모양을 보여준다. 각 사례에서 motivation은 분야의 누구나 적을 수 있는 한 줄이지만, novelty는 그 한 줄이 *왜 그 형태로* 해결되어야 하는가에 대한 원리적 답이다.

### Case 1 — ORB-SLAM (Mur-Artal et al. 2015)

- **Motivation**: 기존 SLAM이 mono / stereo / RGB-D를 각각 별도로 처리 → 통합이 필요하다
- **2티어 답**: stereo branch · RGB-D branch · mono branch을 추가한 시스템 한 편
- **Novelty (top-tier)**: 모든 modality가 같은 factor-graph 백본을 공유하도록 frontend를 재정의. 차이는 frontend 측정 함수에 한정되고, backend는 동일한 bundle adjustment formulation으로 통합
- **도약의 한 줄**: backend와 frontend의 *분리* 자체가 modality 일반화의 충분조건이라는 원리적 답

### Case 2 — 3D Gaussian Splatting (Kerbl et al. 2023)

- **Motivation**: NeRF가 너무 느리다 → 빠르게 만들어야 한다
- **2티어 답**: NeRF 위에 sparse sampling · pruning · distillation 같은 가속 모듈 한 층
- **Novelty (top-tier)**: ray-marching이라는 비효율의 원천을 탐색 자체로 진단하고, 그 자리를 명시적 primitive(3D Gaussian)로 교체. Primitive를 직접 rasterization 가능한 형태로 설계
- **도약의 한 줄**: "암시적 표현 + ray-marching"이라는 NeRF의 근본 형식이 속도 한계의 원인이라는 진단, 그 자리에 직접 rasterization을 허용하는 primitive를 두는 *형식의 교체*

### Case 3 — DUSt3R (Wang et al. 2024)

- **Motivation**: 기존 SfM이 brittle하고 camera intrinsics가 필요하다 → 더 강건한 방법이 필요하다
- **2티어 답**: SfM 파이프라인 안의 한 단계(matching · triangulation 등)를 NN으로 대체
- **Novelty (top-tier)**: 2-view 입력으로부터 양쪽 view의 pointmap을 직접 예측하는 reformulation. Camera intrinsics 추정조차 pointmap에서 유도되는 부산물
- **도약의 한 줄**: SfM의 단계별 분해를 거치지 않고 *공통 좌표계의 pointmap*을 출력 형식으로 두면 intrinsics · correspondence · structure가 동시에 풀린다는 형식 재구성

### 도약의 공통 패턴

세 편 모두에서 motivation은 1년차 학생도 읽고 적을 수 있는 한 줄. Novelty가 다르다. 세 편이 모두 묻는 질문은 같은 모양을 한다 — *왜 그 모듈이 그 형태여야 하는가*. 답하는 방식은 *형식의 재구성*에 가까운 자리. 모듈을 더하는 자리에서 멈추지 않고, 형식 자체의 다른 좌표에 솔루션을 둔다.

> 본인 논문의 contribution 절을 읽었을 때 *왜 이 모듈이어야 하는가*에 한 단락이 답해 있는가. 답이 비어 있으면 그 자리는 아직 motivation 단계다. 한 단락이 채워지기 시작하는 자리에서 novelty가 생긴다.

논문 쓰기의 본격 메타 가이드는 [「연구노트」 Ch.23 Introduction](../research-notes/chapter_23_introduction.md) · [Ch.25 Method](../research-notes/chapter_25_method.md)에서 다룬다. 이 절은 *연구 방향 선택* 시점에 그 도약을 미리 의식하게 두는 자리다.

---

# Ch.19 — AI 코딩 에이전트와 협업하기

## 19.1 왜 이 챕터가 필요한가

AI 코딩 에이전트(Claude, Copilot, ChatGPT 등)는 일반 소프트웨어 개발에서는 강력한 도구다. 함수 하나 짜달라고 하면 꽤 쓸만한 코드가 나오고, 에러 메시지를 붙여넣으면 원인을 잘 짚어준다. 하지만 로보틱스는 다르다. 하드웨어, OS, 네트워크, 실시간성이 복잡하게 얽혀 있고, "코드는 맞는데 안 되는" 상황이 일상이다. AI는 이런 영역에서 자주 틀리거나, 자신 있게 엉뚱한 방향을 제시하거나, 아예 포기한다.

이 챕터는 AI에게 속지 않고, AI를 제대로 부려먹기 위한 가이드다. 여기 나오는 내용은 전부 실제로 겪은 문제들이다. AI가 틀리는 패턴을 미리 알아두면 삽질이 줄어든다.

## 19.2 ROS에서 AI가 자주 틀리는 것들

### 19.2.1 QoS 설정

AI는 거의 항상 QoS(Quality of Service)를 무시하거나 default(RELIABLE)로 놓는다. 문제는 센서 토픽(카메라, LiDAR)이 보통 BEST_EFFORT로 퍼블리시된다는 점이다. subscriber가 RELIABLE이면 데이터가 아예 안 들어온다. 에러 메시지도 안 뜨고 그냥 조용히 안 되니까, AI는 "토픽이 없나?" 하고 엉뚱한 방향으로 디버깅을 시작한다.

```bash
# 토픽의 QoS 프로파일 확인
ros2 topic info /camera/image_raw --verbose
```

출력에서 `Reliability: BEST_EFFORT`, `Durability: VOLATILE` 같은 정보를 확인하고, subscriber의 QoS를 맞춰야 한다.

```python
from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy

qos = QoSProfile(
    reliability=ReliabilityPolicy.BEST_EFFORT,
    durability=DurabilityPolicy.VOLATILE,
    depth=10
)
self.subscription = self.create_subscription(Image, '/camera/image_raw', self.callback, qos)
```

AI에게 코드를 요청할 때는 "이 토픽의 QoS는 BEST_EFFORT / SENSOR_DATA다"라고 명시적으로 알려줘야 한다. 안 그러면 기본값으로 만들어서 데이터가 안 들어오는데, AI는 원인을 못 찾는다.

### 19.2.2 use_sim_time과 tf2 타이밍

rosbag 재생 시 `use_sim_time:=true`를 안 하면 tf lookup이 전부 실패한다. AI는 "tf2 lookup failed" 에러를 보면 높은 확률로 `static_transform_publisher`를 추가하라고 한다. 엉뚱한 방향이다.

실제 원인은 시뮬레이션 클럭과 시스템 클럭의 불일치다. bag 파일의 타임스탬프는 과거 시점인데, 노드는 현재 시스템 시간을 기준으로 tf를 조회하니까 당연히 못 찾는다.

```bash
# 올바른 rosbag 재생
ros2 bag play my_bag --clock

# 노드 실행 시 sim_time 활성화
ros2 launch my_package my_launch.py use_sim_time:=true
```

tf2 lookup에는 timeout과 try/except가 필요한데, AI는 이걸 빼먹는 경우가 많다.

```python
from rclpy.duration import Duration

try:
    transform = tf_buffer.lookup_transform(
        'base_link', 'camera_link',
        rclpy.time.Time(),
        timeout=Duration(seconds=1.0)
    )
except tf2_ros.LookupException as e:
    self.get_logger().warn(f'TF lookup failed: {e}')
```

### 19.2.3 Workspace 소싱 순서

ROS2 workspace의 소싱 순서가 중요하다. `/opt/ros/humble/setup.bash`를 먼저 source하고, 그다음에 `~/ros2_ws/install/setup.bash`를 source해야 한다. AI는 하나만 source하거나, 순서를 뒤집거나, overlay workspace 개념 자체를 모르는 경우가 많다.

```bash
# 올바른 순서
source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash
```

`.bashrc`에 넣었는데 새 터미널에서 패키지를 못 찾으면, AI는 패키지 재설치를 권한다. 대부분 `source` 문제다. `echo $AMENT_PREFIX_PATH`로 현재 소싱된 workspace를 확인하자.

### 19.2.4 커스텀 메시지와 빌드

AI는 `.msg` 파일은 잘 만든다. 하지만 `CMakeLists.txt`와 `package.xml`의 의존성 추가를 빠뜨리는 경우가 잦다.

`rosidl_generate_interfaces` 설정이 빠지면 빌드는 되는데 Python에서 import할 때 실패한다. 이 에러가 나면 AI는 "패키지가 설치 안 됐다"고 오진하기 쉽다.

```cmake
# CMakeLists.txt에 반드시 추가
find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/MyCustomMsg.msg"
  DEPENDENCIES std_msgs geometry_msgs
)
```

```xml

<buildtool_depend>rosidl_default_generators</buildtool_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
```

또 하나: `--symlink-install` 없이 빌드하면 Python 코드 수정이 반영되지 않는다. AI는 이걸 "캐시 문제"라고 오진한다.

```bash
# Python 패키지 수정이 바로 반영되려면
colcon build --symlink-install
```

### 19.2.5 네임스페이스와 리매핑

`ros2 topic echo /camera/image_raw` 했는데 데이터가 안 오면, AI는 드라이버 문제라고 진단하기 쉽다. 하지만 네임스페이스 때문에 토픽 이름이 `/robot1/camera/image_raw`일 수 있다.

```bash
# 토픽 목록부터 확인하라
ros2 topic list

# 특정 패턴으로 필터링
ros2 topic list | grep camera
```

AI에게 디버깅을 시킬 때는 `ros2 topic list`와 `ros2 node list` 출력을 먼저 제공해야 한다. 이 정보 없이 "토픽이 안 들어와요"라고 하면 AI는 추측으로 답할 수밖에 없다.

### 19.2.6 Launch 파일

AI가 ROS2 Python launch 파일을 쓸 때 흔히 하는 실수들:

- ROS1 XML 문법을 섞는다 (ROS2 launch는 Python이 기본이다)
- `LaunchDescription`의 action 순서를 잘못 잡는다 (노드 의존성 고려 필요)
- `ComposableNode` vs 일반 `Node` 구분을 못 한다
- `PushRosNamespace`를 빠뜨려서 multi-robot 설정이 꼬인다

```python
# multi-robot launch 파일에서 네임스페이스 적용
from launch.actions import GroupAction, PushRosNamespace

robot1_group = GroupAction([
    PushRosNamespace('robot1'),
    Node(package='my_pkg', executable='my_node', name='sensor_node'),
])
```

AI에게 launch 파일을 요청할 때는 "ROS2 Python launch 파일", "multi-robot이면 네임스페이스 적용", "ComposableNode 사용 여부" 등을 명시해야 한다.

## 19.3 Docker에서 AI가 모르는 것들

### 19.3.1 GUI/시각화 문제

Docker 안에서 RViz나 Gazebo 같은 GUI 도구를 띄우려면 X11 포워딩이 필요하다. AI는 흔히 `xhost +local:docker`를 권하지만, 이는 모든 로컬 연결에 X 서버 접근을 허용하는 것이라 보안상 위험하다.

제대로 하려면 다음과 같이 설정한다:

```bash
docker run -it \
  --env DISPLAY=$DISPLAY \
  --env QT_X11_NO_MITSHM=1 \
  -v /tmp/.X11-unix:/tmp/.X11-unix:rw \
  --ipc=host \
  my_image
```

각 옵션의 역할:
- `QT_X11_NO_MITSHM=1` — AI는 이 옵션을 거의 모른다. 없으면 RViz가 segfault로 죽는다. MIT-SHM(공유 메모리) 확장이 Docker 환경에서 제대로 동작하지 않기 때문이다.
- `--ipc=host` — 호스트와 IPC 네임스페이스를 공유한다. 없으면 shared memory 문제로 시각화 도구가 죽는 경우가 많다.
- Wayland 환경(Ubuntu 22.04+ 기본)에서는 X11 소켓 마운트만으로 안 되는 경우가 있다. `XDG_SESSION_TYPE=x11`로 X11 세션을 강제하거나, XWayland를 거쳐야 할 수 있다.

### 19.3.2 USB 디바이스 패스스루

카메라, LiDAR, IMU 등 USB 장치를 Docker 안에서 쓰려면 디바이스를 명시적으로 매핑해야 한다. AI는 보통 이걸 모르고 "드라이버 설치하라"고 한다.

```bash
# 특정 디바이스만 매핑 (권장)
docker run -it --device=/dev/ttyUSB0 --device=/dev/video0 my_image

# 모든 디바이스 접근 허용 (보안상 비추, 디버깅용으로만)
docker run -it --privileged my_image
```

`--privileged`는 편하지만 컨테이너에 호스트의 거의 모든 권한을 주는 것이므로, 프로덕션에서는 필요한 device만 매핑하는 게 맞다.

한 가지 더: USB 장치가 Docker 컨테이너 시작 후에 꽂히면 인식이 안 된다. AI는 이 상황을 전혀 고려하지 못한다. 컨테이너를 재시작하거나, `--privileged` + `-v /dev:/dev` 조합을 써야 한다.

### 19.3.3 ROS 네트워킹

Docker 컨테이너 간 ROS2 통신에서 `--network=host`가 가장 간단하지만, 호스트의 포트를 전부 공유하므로 포트 충돌 위험이 있다.

AI는 bridge 네트워크에서 ROS2가 왜 안 되는지 모르는 경우가 많다. 원인은 DDS(Data Distribution Service)가 multicast를 사용하는데, Docker bridge 네트워크에서는 multicast가 기본적으로 안 되기 때문이다.

```bash
# 가장 간단한 방법 (개발 환경에서)
docker run -it --network=host my_ros2_image

# ROS_DOMAIN_ID로 다른 사람과 충돌 방지
docker run -it --network=host -e ROS_DOMAIN_ID=42 my_ros2_image
```

`ROS_DOMAIN_ID`가 같은 네트워크의 다른 사람 ROS2와 충돌할 수 있다. 연구실에서 여러 명이 동시에 ROS2를 쓰면 서로의 토픽이 보이는 상황이 생긴다.

DDS 설정을 세밀하게 해야 할 때는 Cyclone DDS config XML로 특정 네트워크 인터페이스만 사용하게 제한한다:

```xml

<CycloneDDS>
  <Domain>
    <General>
      <NetworkInterfaceAddress>eth0</NetworkInterfaceAddress>
    </General>
  </Domain>
</CycloneDDS>
```

```bash
export CYCLONEDDS_URI=file:///path/to/cyclone_dds.xml
```

### 19.3.4 파일 권한 문제

Docker 안에서 생성된 파일은 기본적으로 root 소유다. 호스트에서 편집하거나 삭제하려면 `sudo`가 필요하다.

```bash
# 호스트 유저 권한으로 실행
docker run -it --user $(id -u):$(id -g) my_image
```

하지만 일부 ROS 패키지가 root 권한을 필요로 해서 `--user` 옵션을 쓰면 또 안 되는 경우가 있다. AI는 이런 상황에서 `chmod 777`을 남발하는데, 실무에서는 이러면 안 된다. Dockerfile에서 non-root 유저를 만들고 필요한 디렉토리 권한만 설정하는 것이 올바른 방법이다.

```dockerfile
# Dockerfile에서 non-root 유저 설정
RUN useradd -m -s /bin/bash rosuser && \
    usermod -aG dialout rosuser
USER rosuser
```

## 19.4 하드웨어/드라이버에서 AI가 포기하는 것들

### 19.4.1 시리얼 포트 권한

`/dev/ttyUSB0` 접근 시 `Permission denied`가 뜨면, AI는 `sudo chmod 666 /dev/ttyUSB0`을 권한다. 되긴 되지만, 재부팅하면 리셋된다. 매번 이걸 치고 있을 수는 없다.

올바른 방법은 udev rule을 작성하는 것이다:

```bash
# 벤더/프로덕트 ID 확인
udevadm info -a -n /dev/ttyUSB0 | grep -E 'idVendor|idProduct'
```

```bash
# /etc/udev/rules.d/99-sensors.rules
SUBSYSTEM=="tty", ATTRS{idVendor}=="1546", ATTRS{idProduct}=="01a9", MODE="0666", SYMLINK+="gps"
```

```bash
# udev rule 적용
sudo udevadm control --reload-rules && sudo udevadm trigger
```

이렇게 하면 해당 USB 장치가 항상 `/dev/gps`라는 고정 이름으로 잡히고, 권한도 자동으로 설정된다. 여러 개의 동일 장치를 구분해야 할 때(예: IMU 2개)도 시리얼 넘버로 구분할 수 있다. AI는 udev를 거의 모른다.

### 19.4.2 USB 대역폭

USB3 카메라 3대를 같은 USB 허브에 꽂으면 대역폭 부족으로 프레임 드롭이 발생한다. AI는 "드라이버 업데이트하라" 또는 "해상도를 낮춰라"고 하지만, 실제 원인은 USB 컨트롤러의 대역폭 한계다.

```bash
# 어떤 카메라가 어떤 USB 컨트롤러에 붙어있는지 확인
lsusb -t
```

이 문제는 소프트웨어로 해결할 수 없다. 물리적으로 다른 USB 컨트롤러에 카메라를 분산 연결해야 한다. 데스크탑 PC의 앞면 USB와 뒷면 USB는 다른 컨트롤러에 연결된 경우가 많으니, `lsusb -t`의 Bus 번호를 확인하고 분산 배치하자.

### 19.4.3 LiDAR 연결 (IP 설정)

Velodyne이나 Ouster LiDAR에서 "데이터가 안 온다"는 문제의 90%는 네트워크 설정이 원인이다. AI는 드라이버 재설치나 ROS 패키지 재빌드를 권하지만, 그 전에 확인해야 할 것이 있다.

LiDAR는 고정 IP(예: `192.168.1.201`)를 사용한다. 호스트 PC의 이더넷 인터페이스를 같은 서브넷(예: `192.168.1.100`)으로 설정해야 통신이 된다.

```bash
# 1단계: LiDAR에 ping이 되는지 확인
ping 192.168.1.201

# 2단계: 호스트 이더넷 인터페이스 IP 설정
sudo ip addr add 192.168.1.100/24 dev eth0
sudo ip link set eth0 up

# 3단계: UDP 패킷이 오는지 Wireshark로 확인
sudo tcpdump -i eth0 udp port 2368 -c 10
```

`ping`부터 해보면 대부분 여기서 걸린다. Wireshark(또는 tcpdump)로 UDP 패킷이 오는지 확인하는 게 가장 확실한 디버깅 방법이다. 패킷이 오는데 ROS에서 안 보이면 그때 드라이버를 의심해도 늦지 않다.

### 19.4.4 카메라 드라이버 (v4l2)

AI는 `cv2.VideoCapture(0)`만 알지, `/dev/video*`가 여러 개일 때 어떤 게 실제 카메라인지 구분하지 못한다. USB 카메라 하나를 꽂아도 `/dev/video0`, `/dev/video1`이 생기는 경우가 흔한데(metadata용 디바이스), AI는 이걸 모른다.

```bash
# 카메라 디바이스 매핑 확인
v4l2-ctl --list-devices

# 지원하는 포맷과 해상도 확인
v4l2-ctl -d /dev/video0 --list-formats-ext
```

자동 노출(auto exposure), 자동 화이트밸런스 — 이런 자동 설정이 SLAM 성능을 망치는 경우가 많다. 밝기가 계속 변하면 특징점 추출이 불안정해진다.

```bash
# 수동 노출 설정 (SLAM용)
v4l2-ctl -d /dev/video0 --set-ctrl=exposure_auto=1
v4l2-ctl -d /dev/video0 --set-ctrl=exposure_absolute=100

# 화이트밸런스 고정
v4l2-ctl -d /dev/video0 --set-ctrl=white_balance_automatic=0
```

AI는 이런 low-level 카메라 제어를 거의 모른다. "SLAM이 불안정하다"고 하면 알고리즘 파라미터 튜닝을 권하지만, 카메라 자동 설정을 끄는 것만으로 극적으로 개선되는 경우가 있다.

### 19.4.5 Jetson (ARM) 환경

AI가 생성한 코드나 Docker 설정은 거의 100% x86 기준이다. NVIDIA Jetson(ARM64)에서는 안 돌아가는 경우가 많다.

주의해야 할 점:
- `pip install`로 설치할 때 pre-built 바이너리(wheel)가 ARM용으로 없는 패키지가 많다. 특히 `scipy`, `opencv-python`은 소스에서 빌드해야 해서 수십 분이 걸린다.
- JetPack 버전에 따라 CUDA, cuDNN, TensorRT 버전이 고정된다. AI가 최신 버전을 설치하라고 권하면 전체 시스템이 꼬인다.
- Docker를 쓸 때는 NVIDIA에서 제공하는 `l4t`(Linux for Tegra) 기반 이미지를 써야 한다.

```bash
# Jetson에서 Docker 이미지 — x86 이미지 쓰면 안 된다
docker pull nvcr.io/nvidia/l4t-pytorch:r35.2.1-pth2.0-py3

# JetPack 버전 확인
cat /etc/nv_tegra_release
```

AI에게 코드를 요청할 때는 "Jetson Orin, JetPack 5.1.2, CUDA 11.4 환경이다"라고 명시해야 한다.

### 19.4.6 실시간 제어와 타이밍

AI는 `time.sleep(0.01)`로 100Hz 루프를 만들라고 하지만, 정확하지 않다. `time.sleep()`의 정밀도는 OS 스케줄러에 의존하며, 일반 Linux 커널에서는 수 밀리초의 jitter가 발생한다.

```python
# AI가 주로 권하는 방법 (정확하지 않음)
import time
while True:
    do_control()
    time.sleep(0.01)  # 실제로는 10~15ms가 될 수 있다
```

Python의 GIL(Global Interpreter Lock) 때문에 멀티스레드 타이밍은 더 보장이 안 된다. 진짜 실시간 제어가 필요하면 C++과 RT(Real-Time) 커널(PREEMPT_RT)을 써야 한다.

```bash
# 실제 퍼블리시 주파수 확인 — 항상 이걸로 검증하라
ros2 topic hz /cmd_vel
```

AI가 "100Hz로 제어하면 된다"고 해도, `ros2 topic hz`로 실제 주파수를 확인해야 한다. 기대한 주파수와 실제 주파수가 다르면 제어가 제대로 안 된다.

## 19.5 AI 에이전트가 포기하는 패턴들

### 19.5.1 "It works in simulation"

Gazebo에서 잘 되는데 실제 로봇에서 안 되는 상황. AI는 "시뮬레이션에서 되니까 코드는 맞고 하드웨어 문제"라고 결론 내리기 쉽다. 하지만 실제 원인은 sim-to-real gap인 경우가 대부분이다:

- **센서 노이즈**: 시뮬레이션의 가우시안 노이즈와 실제 센서 노이즈는 분포가 다르다
- **통신 지연**: Gazebo 안에서는 토픽 전달이 즉시 이뤄지지만, 실제로는 수~수십 ms의 지연이 있다
- **타이밍 불일치**: 시뮬레이션은 완벽한 동기화를 보장하지만, 실제로는 센서 간 타임스탬프가 어긋난다
- **좌표계 불일치**: URDF와 실제 로봇의 센서 위치/각도가 미세하게 다르면 tf가 틀어진다

AI에게는 "시뮬레이션에서는 되는데 실제 로봇에서 안 된다. 센서 노이즈 수준은 X이고, 통신 지연은 Y ms이고, 좌표계 캘리브레이션은 Z 방법으로 했다"처럼 sim-to-real의 차이를 구체적으로 알려줘야 한다.

### 19.5.2 하드웨어 문제를 소프트웨어로 고치려 함

케이블 불량, 접촉 불량, 전원 부족 — AI는 이런 물리적 문제를 진단할 수 없다.

"센서 데이터가 간헐적으로 끊긴다"고 하면 AI는 버퍼 크기 조절, 타임아웃 설정, QoS 변경 등을 권한다. 하지만 USB 케이블이 느슨하거나 USB 허브의 전원이 부족한 경우가 많다.

```bash
# 커널 로그에서 하드웨어 문제 단서 찾기
dmesg | tail -20

# USB 연결 해제/재연결 이벤트 확인
dmesg | grep -i usb | tail -20
```

`dmesg`에 `USB disconnect`, `device descriptor read/64, error -71` 같은 메시지가 보이면 소프트웨어 문제가 아니다. 케이블을 교체하거나, 유전원 USB 허브를 쓰거나, 다른 포트에 꽂아보는 게 먼저다.

### 19.5.3 환경 문제 진단 포기

라이브러리 버전 충돌이 복잡하게 얽히면 AI는 "전부 재설치하라"고 한다. `pip show package_name`으로 버전을 확인하고 어떤 것이 충돌하는지 좁히는 게 먼저다.

특히 OpenCV 관련 충돌은 로보틱스의 클래식이다:

- `opencv-python` (기본)
- `opencv-python-headless` (GUI 없는 서버용)
- `opencv-contrib-python` (추가 모듈 포함)
- `cv_bridge` (ROS 패키지, 자체 OpenCV를 참조)

이 네 가지가 동시에 설치되면 서로 충돌한다.

```bash
# 현재 설치된 OpenCV 확인
pip show opencv-python opencv-python-headless opencv-contrib-python

# 해결: ROS 환경에서는 pip opencv를 설치하지 않는다
pip uninstall opencv-python opencv-python-headless opencv-contrib-python
sudo apt install ros-humble-cv-bridge
```

ROS 환경에서는 `apt install ros-humble-cv-bridge`만 쓰고, pip으로 opencv를 따로 설치하지 않는 것이 가장 깔끔하다.

### 19.5.4 "2-3번 시도 후 포기"

AI는 같은 접근을 두세 번 반복하다 안 되면 "다른 방법을 시도해 보세요"라며 넘긴다. 엔지니어는 여기서 포기하지 않는다. 로그를 뒤지고, strace를 걸고, 패킷을 캡처한다.

AI에게 더 좋은 답을 얻으려면, 에러 메시지뿐 아니라 low-level 정보를 함께 줘야 한다:

```bash
# 시스템 로그
dmesg | tail -30
journalctl -u my_service --since "5 minutes ago"

# 프로세스 추적
strace -f -e trace=open,read,write ros2 run my_pkg my_node 2>&1 | head -100

# 네트워크 패킷 캡처
sudo tcpdump -i eth0 -w capture.pcap
```

이런 정보를 AI에게 제공하면, "재설치하세요" 대신 실제 원인에 가까운 답을 받을 수 있다.

## 19.6 AI 에이전트를 제대로 쓰는 법

*AI 부려먹기의 일반 frame (writing·reading 영역)은 [`../research-notes/chapter_07_keshav_three_passes.md`](../research-notes/chapter_07_keshav_three_passes.md) + [`part2_writing/A_workflow/ch01_mindset.md`](../research-notes/chapter_16_mindset.md) 에서 본격 다룬다. 본 § 19.6은 코드·하드웨어 영역의 분야 적용.*

### 19.6.1 컨텍스트를 충분히 제공하라

AI에게 질문할 때 가장 중요한 것은 컨텍스트의 양과 질이다.

틀린 예: "카메라가 안 돼요"

맞는 예: "Ubuntu 22.04, ROS2 Humble, Intel RealSense D435, `rs-enumerate-devices`에서는 보이는데 `ros2 launch realsense2_camera rs_launch.py`하면 `Could not open device` 에러가 난다. Docker 안에서 실행 중이고, `--device=/dev/video0`은 매핑했다."

**AI에게 제공해야 할 정보 체크리스트**:
- OS 버전, ROS 버전
- 하드웨어 플랫폼 (x86 vs ARM/Jetson)
- 센서 모델명
- 에러 메시지 전문 (일부만 복사하지 말고 전체를 줘라)
- `ros2 topic list`, `ros2 node list` 출력
- Docker 사용 여부와 실행 옵션 (`docker run` 명령 전체)
- 네트워크 구성 (유선/무선, IP 대역)

### 19.6.2 AI의 답을 검증하는 방법

AI의 답을 그대로 실행하기 전에 다음을 확인하라:

- **"이 패키지를 설치하라"** → 해당 패키지가 자기 ROS 버전과 Ubuntu 버전을 지원하는지 먼저 확인. `apt search ros-humble-<패키지명>`으로 존재 여부를 체크한다.
- **"이 설정을 바꿔라"** → 바꾸기 전에 현재 설정을 백업하고, 왜 바꿔야 하는지 근거를 AI에게 물어본다. 근거를 설명하지 못하면 의심하라.
- **"재설치하라"** → 90%는 재설치 안 해도 된다. 먼저 정확한 에러 원인을 좁혀라. `pip show`, `dpkg -l | grep`, `apt policy` 등으로 현재 상태를 확인하는 게 먼저다.
- **AI가 코드를 줄 때** → 하드코딩된 경로(`/home/user/...`), 하드코딩된 IP(`192.168.1.100`), x86 전용 패키지(`amd64` wheel) 등이 포함되어 있는지 확인한다.

### 19.6.3 AI가 잘하는 것 vs 못하는 것

| AI가 잘하는 것 | AI가 못하는 것 |
|---|---|
| 알고리즘 구현 (SLAM, detection 등) | 하드웨어 디버깅 |
| ROS2 노드/서비스 코드 작성 | QoS/DDS 설정 최적화 |
| Python/C++ 코드 리팩토링 | USB/시리얼 권한 문제 |
| 논문 읽기/요약 | 네트워크 설정 (LiDAR IP 등) |
| CMakeLists.txt 작성 | 실시간 타이밍 문제 |
| 데이터 전처리 파이프라인 | Docker 안에서의 하드웨어 접근 |
| 시각화 코드 (matplotlib, Open3D) | 센서 간 시간 동기화 실전 |
| 에러 메시지 해석 (일반적인) | dmesg/커널 로그 기반 디버깅 |

결국 이런 구조다: AI는 "순수 소프트웨어" 영역에서는 강하지만, 하드웨어와 소프트웨어가 만나는 경계에서 약하다. 그리고 로보틱스 문제의 대부분이 그 경계에서 발생한다. AI가 강한 영역은 맡기고, AI가 약한 영역에서는 직접 디버깅한 결과를 AI에게 먹여서 분석하게 하는 게 맞다.

## 19.7 AI를 연구 도구로 쓰는 워크플로우

19.6절에서 AI의 강점과 약점을 짚었다. 여기서는 그걸 바탕으로, 로보틱스 연구자가 하루 일과에서 AI를 어떻게 쓸 수 있는지 실전 워크플로우를 본다.

### 19.7.1 논문 읽기

*논문 읽기 워크플로우 (3-pass + AI layer cameo)는 [`../research-notes/chapter_07_keshav_three_passes.md`](../research-notes/chapter_07_keshav_three_passes.md) 의 AI layer cameo에서 본격 다룬다.*

분야 적용은 분야 핵심 논문에 3-pass + AI summary 결합 — abstract 읽고 contribution 3줄 요청, Eq. 단계별 유도 요청.

### 19.7.2 코드 작성

- 프로토타이핑: "KITTI 데이터셋에서 ORB 특징점 뽑아서 매칭하는 코드 짜줘. OpenCV 쓰고, Lowe's ratio test 0.75로" — 이런 식으로 구체적으로 지시
- 디버깅: 에러 메시지 + 코드 + "이 에러의 원인이 뭐야" — AI가 잘하는 영역
- 리팩토링: "이 코드를 PyTorch Dataset 클래스로 바꿔줘" — 구조 변환에 강함
- AI가 못하는 것 (이 장 앞부분 참조): ROS QoS, 하드웨어 권한, 네트워크 설정, 실시간 타이밍

### 19.7.3 실험 설계

*실험 설계·ablation·결과 해석에 AI 부려먹기 frame은 [`../research-notes/chapter_32_revision_rebuttal.md`](../research-notes/chapter_32_revision_rebuttal.md) (또는 [`ch12_figures.md`](../research-notes/chapter_27_figures.md)) 에 cameo로 담겼다.*

분야 적용은 baseline 비교 표를 AI에게 던지고 *내가 놓친 비교 축*을 묻는 워크플로우.

### 19.7.4 논문 쓰기

- 초고 작성: 핵심 아이디어와 실험 결과를 AI에게 주고 "Introduction 초고를 써줘" — 구조 잡기에 유용
- 문법/표현 교정: 영어 논문의 어색한 표현 수정. Grammarly보다 AI가 context를 더 잘 이해한다
- 주의: AI가 쓴 문장을 그대로 제출하면 안 된다. 본인의 voice로 다시 써야 한다. 리뷰어는 AI 생성 문체를 알아본다
- BibTeX 생성: "이 논문의 BibTeX를 만들어줘" — Google Scholar에서 복사하는 것보다 빠를 때가 있다. 단, AI가 year이나 venue를 틀리는 경우가 있으니 반드시 검증

### 19.7.5 일상 워크플로우 예시

하루 일과에서 AI를 어떻게 쓰는지 구체적 시나리오:

```
09:00 — 새 논문 3편 arXiv에서 확인. AI에게 각각 1문장 요약 요청
09:30 — 흥미로운 1편 선택, 2패스 읽기. 모르는 수식은 AI에게 유도 요청
10:30 — 어제 학습 결과 분석. loss curve 캡처해서 AI에게 "이 패턴이 정상인가?" 확인
11:00 — 새 실험 코드 작성. AI에게 DataLoader 구조 생성 시킴. 수동으로 augmentation 로직 수정
14:00 — SLAM 코드 디버깅. ROS2 에러 → 이 장 참고해서 직접 해결 (AI는 QoS를 모른다)
16:00 — 논문 Related Work 섹션 초고. AI에게 비교 논문 5편의 차이점 표 만들게 시킴
17:00 — 표 검증. AI가 2편의 method를 혼동한 것 발견, 수동 수정
```

---

# 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)
- 딥러닝 이론 표준 교과서
- **무료 온라인** 제공
- 무료 PDF: 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 커뮤니티의 스터디 자료: https://github.com/slam-kr

### 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 | Columbia 교수가 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 렌즈, 독자 기대 진단까지 일곱 챕터에 걸쳐 [`../research-notes/part1_reading/`](../research-notes/part1_reading/) (ch01–ch07)에 정리되어 있다.

### 필독 논문 리스트

**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.14737](https://arxiv.org/abs/2308.14737)
- 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/), [Papers With Code](https://paperswithcode.com/), [arXiv](https://arxiv.org/)를 활용하자. Papers With Code는 특히 벤치마크 순위와 코드 링크를 같이 보여줘서 편하다.

### 논문 작성 도구

> **추천 자료**
> - [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 주요 학회

분야별 학회 reference 표는 별도 가이드에 정리되어 있다 — [`../research-notes/chapter_34_conference_prep.md`](../research-notes/chapter_34_conference_prep.md) 끝의 *분야별 학회 reference* 섹션이 CV·로보틱스·자율주행 학회 일정과 성격을 다룬다.

본 가이드는 SLAM/CV/로보틱스 분야 본체에 집중한다. 학회 일반론(왜 가는가·발표 opener·첫 마디 화법)과 분야별 표는 위 가이드에서 다룬다.

## 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 이해 및 활용 | DINOv2, SAM, CLIP | 본 문서 10-11장 + 논문 직접 읽기 |
| 3D 재구성 심화 | NeRF, 3D Gaussian Splatting | [NeRF Studio](https://github.com/nerfstudio-project/nerfstudio) + 본 문서 13장 |
| 논문 읽기 및 구현 | 최신 논문 분석 | [Papers With Code](https://paperswithcode.com/) + [Yannic Kilcher 채널](https://www.youtube.com/@YannicKilcher) — *본격은 [`../research-notes/part1_reading/`](../research-notes/part1_reading/)* |
| 새로운 아이디어 실험 | 가설 수립, 실험 설계 | 연구실 세미나 + 학회 워크숍 참여 — *본격은 [`../research-notes/part0_starting/`](../research-notes/part0_starting/)* |
| 벤치마크 평가 | 정량적 비교 | 각 분야별 표준 벤치마크 (KITTI, ScanNet, Replica 등) — *결과 해석 frame은 [`../research-notes/part2_writing/E_after/`](../research-notes/part2_writing/E_after/)* |

**실습 과제**:
- 최신 논문 코드 분석 — GitHub에서 코드를 받아 직접 돌려보기
- 자체 개선 아이디어 실험 — "이 부분을 바꾸면 어떨까?" 시도
- 논문 작성 시도 — *본격 frame은 [`../research-notes/part2_writing/`](../research-notes/part2_writing/)* 참고
- 본 문서의 **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 연구 실전 mindset (link)

*대학원 수준.* 연구자 마인드셋과 논문 쓰기·실험 설계·학회 발표·리뷰 같은 메타 운영은 별도 가이드 [research-notes](../research-notes/)·[grad-notes](../grad-notes/)에서 본격적으로 다룬다. 본 챕터에서는 분야 instantiation에 해당하는 자리만 짧게 남기고 메타 자리는 link로 안내한다.

핵심 frame 한 줄: 엔지니어는 정체성이고, 연구는 *방향·엔진·도구* 세 layer로 굴러간다 — 도구만 갈고닦으면 5년 후 *날카로운 칼 + 빈 방향*에 도착한다.

### 20.8.0 연구자 마인드셋 (link)

- 방향·엔진·도구 세 layer + 통합 frame → [`gradnotes/p4_ch04_integrated_life.md`](../grad-notes/chapter_17_integrated_life.md) § 5
- 자율성의 무게 + Hyun *모든 것이 optimization* + optimization horizon = 장기전 → [`gradnotes/p4_ch01_autonomy_weight.md`](../grad-notes/chapter_14_autonomy_weight.md) § 1
- 꾸준함 vs 폭발적 성장 + 옆 사람 속도 비교 함정 → [`gradnotes/p4_ch02_comparison_trap.md`](../grad-notes/chapter_15_comparison_trap.md) § 3
- 견고한 기초 — *리젝 이유가 없는 논문* frame → [`part2_writing/A_workflow/ch01_mindset.md`](../research-notes/chapter_16_mindset.md) § 1

### 20.8.1 논문 쓰기 (link)

Abstract → Introduction → Related Work → Method → Experiments → Conclusion 구조, Intro 4단 (문제 정의·기존 한계·접근·contribution), figure/table 먼저 그리기 — 본격 가이드는 [`part2_writing/A_workflow/`](../research-notes/part2_writing/A_workflow/) (mindset·outline·time budget) 및 [`part2_writing/C_sections/ch08_introduction.md`](../research-notes/chapter_23_introduction.md).

### 20.8.2 실험 설계와 Ablation

분야 본체 자리이므로 한 줄로 정리한다 — *Ablation·변인 통제·통계 유의성(최소 3회 반복)·공정 비교(같은 데이터·split·하드웨어)*는 분야 표준 작업 단위다. baseline 숫자를 다른 논문에서 그대로 가져오면 조건이 달라 reject 사유가 잡힌다.

### 20.8.3 학회 발표 (link)

발표 1+1+3+3+1 분 구조, 슬라이드 한 장 한 메시지, 포스터 3m 가독성, 데모 30초~1분 — 본격 가이드는 [`part3_presentations/`](../research-notes/part3_presentations/).

### 20.8.4 논문 리뷰 — Peer Review (link)

리뷰어 관점 체크리스트(novelty·soundness·experiments·clarity·reproducibility), 건설적 피드백, rebuttal — 본격 가이드는 [`part1_reading/ch05_reading_for_review.md`](../research-notes/chapter_10_reading_for_review.md) (reviewer로 읽기) + [`part2_writing/E_after/ch17_revision_rebuttal.md`](../research-notes/chapter_32_revision_rebuttal.md) (rebuttal 작성).

### 20.8.5 도구

- LaTeX: Overleaf 또는 로컬 (texlive + vscode)
- 참고 문헌: PDF 리더 + AI 조합이 효율적 — Acrobat 형광펜 + Claude/GPT로 요약·BibTeX 생성·related work 비교, Google Scholar Cite 버튼, Zotero + Better BibTeX는 100편 이상에서 유용
- 파이프라인 그림: TikZ (정밀), draw.io (빠른 제작), Inkscape (SVG)
- 테이블: booktabs (\toprule, \midrule, \bottomrule)
- 알고리즘: algorithm2e
- 수식: notation table을 따로 만들어 논문 전체에서 통일

표기법(LaTeX·notation·수식) 자리는 [`part2_writing/D_sentence/ch15_math_and_proofs.md`](../research-notes/chapter_30_math_and_proofs.md) (글쓰기 단위 통일)이 메타 layer로 다룬다.

> 추천 자료:
> - [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**: 오차의 누적. 100m를 걸어가면서 매 걸음마다 1cm씩 오차가 나면, 도착할 때는 100cm 오차가 된다.

**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: 연구실 코드의 핵심은 C++이다. SLAM, ROS 패키지, 실시간 제어 모듈 전부 C++로 되어 있고, 이 코드를 읽고 수정할 일이 많다. Python은 딥러닝 스크립트나 데이터 전처리에 쓰이지만, AI 코딩 에이전트가 잘 도와주는 영역이라 직접 숙달할 필요성은 줄었다. 둘 다 "코드를 읽고 이해하는 능력"이 핵심이고, 작성은 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/) (논문 검색), [Papers With Code](https://paperswithcode.com/) (코드 포함 논문 검색)를 활용하자. 학회별로 보고 싶으면 [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"처럼.

**Q: 어떤 GPU를 사야 하나요?**

A: GPU 선택에서 가장 중요한 스펙은 **VRAM**이다. 모델 크기가 VRAM에 안 들어가면 아예 못 돌린다. 그다음이 연산 속도(TFLOPS)이고, 학습 시간에 직접 영향을 준다.

**개인용 (데스크톱)**

| VRAM | 카드 | FP32 TFLOPS | FP16 TFLOPS | 용도 |
|------|------|-------------|-------------|------|
| 8GB | RTX 4060 | 15.1 | 15.1 | YOLOv8, ResNet 학습, 소규모 fine-tuning |
| 8GB | RTX 5060 Ti 8GB | ~30 | ~30 | 4070급 연산이지만 VRAM이 8GB라 VFM은 빠듯 |
| 12GB | RTX 3060 12GB | 12.7 | 12.7 | 연산은 느리지만 12GB VRAM이 이 가격대에서 유일. 중고로 저렴. 학생 엔트리용 |
| 12GB | RTX 4070 | 29.1 | 29.1 | Depth Anything, SegFormer. SAM은 batch 1로 겨우 가능 |
| 16GB | RTX 5060 Ti 16GB | ~30 | ~30 | SAM, DINOv2 inference. 중간 규모 학습. **가성비 추천** |
| 16GB | RTX 4070 Ti Super | 44.1 | 44.1 | 위와 VRAM 동일, 연산 속도가 1.5배 |
| 24GB | RTX 3090 (중고) | 35.6 | 35.6 | VRAM 24GB를 가장 싸게 확보. 학습 가능, 속도만 느림 |
| 24GB | RTX 4090 | 82.6 | 82.6 | 개인용 최고. VLA fine-tuning, 3DGS 대형 장면 |
| 32GB | RTX 5090 | 104.8 | 209.6 | 개인용 최대 VRAM. FP16에서 4090의 2.5배 |

**서버/연구실용 (데이터센터)**

| VRAM | 카드 | FP32 TFLOPS | TF32 TFLOPS | BF16 TFLOPS | 특징 |
|------|------|-------------|-------------|-------------|------|
| 16/32GB | V100 SXM2 | 15.7 | — | — | Tensor Core 1세대. TF32 미지원. 아직 많은 연구실에서 현역. 중고로 싸게 구할 수 있다 |
| 24GB | A10 | 31.2 | 62.5 | 125.0 | 추론 서버용. 학습에는 느림 |
| 40/80GB | A100 SXM | 19.5 | 156 | 312 | FP32는 느리지만 TF32/BF16에서 압도적. NVLink로 multi-GPU 확장 |
| 80GB | H100 SXM | 66.9 | 989 | 1979 | A100 대비 TF32 6배, BF16 6배. Transformer Engine 지원 |
| 80GB | H200 SXM | 66.9 | 989 | 1979 | H100과 동일 연산, HBM3e로 메모리 대역폭 1.5배 |
| 141GB | B200 | 90 | 2250 | 4500 | 최신. 단일 GPU로 70B+ 모델 학습 가능 |

숫자를 읽는 법:
- **FP32**: 전통적 부동소수점. OpenCV, 고전 SLAM 등에서 사용. 개인 GPU는 이 수치가 실질 성능.
- **TF32/BF16**: PyTorch에서 `torch.cuda.amp` (mixed precision) 사용 시 적용. 학습 속도가 2-6배 빨라진다. 데이터센터 GPU(A100, H100)는 이 모드에서 진가를 발휘하므로, FP32 TFLOPS만 보고 "A100이 4090보다 느리네?"라고 판단하면 안 된다.
- **TFLOPS**: Tera Floating Point Operations Per Second. 높을수록 빠르다.

**참고 사항**:
- RTX 5060 Ti는 8GB와 16GB 두 버전이 있다. 반드시 16GB를 사라. 8GB는 VRAM이 부족해서 금방 한계에 부딪힌다.
- AMD GPU (RX 7900 XTX 등)는 ROCm 지원이 개선되고 있으나, CUDA 생태계와의 호환성 문제가 아직 있다. 트러블슈팅에 시간 쓰기 싫으면 NVIDIA를 사라.
- 연구실 서버에 A100/H100이 있다면 개인 GPU는 디버깅/프로토타이핑용이다. 서버 사양을 먼저 확인하자.
- 중고 RTX 3090 (24GB)은 VRAM 대비 가격이 가장 낫다. 전력 소모(350W)가 크고 소음이 심하다는 점은 감안해야 한다.
- Google Colab Pro(월 $10)로 A100을 시간 단위로 쓸 수도 있다. GPU 구매 전에 먼저 시도해 볼 만하다.

**Q: 논문은 하루에 몇 편 읽어야 하나요?**

A: "하루에 N편" 같은 기준은 의미 없다. 처음에는 *일주일에 1편을 완벽하게* 이해하는 게 훨씬 낫다. 20.4절의 3-패스 방법을 따라서, 한 편을 깊이 파고들자. 6개월쯤 지나면 Abstract만 봐도 "아, 이런 류의 논문이구나" 감이 온다. 그때부터 속도가 붙는다. 참고로, 랩미팅 발표를 위해 논문을 읽는 것과 자기 연구를 위해 읽는 것은 깊이가 다르다. 후자는 코드까지 분석해야 한다.

**Q: 코딩을 잘 못하는데 연구를 할 수 있나요?**

A: 2026년 기준, *코딩 에이전트(Claude, Copilot 등)를 잘 활용하는 능력*이 핵심이 됐다. 에이전트에게 "KITTI 데이터셋 로더 만들어줘", "이 학습 루프에 wandb 로깅 추가해줘"라고 시키면 코드가 나온다. 직접 타이핑하는 시간은 크게 줄었다.

다만, 에이전트가 만든 코드가 맞는지 틀린지 판단하려면 도메인 지식이 있어야 한다. "이 DataLoader의 num_workers가 왜 0이면 느린지", "이 loss가 왜 NaN이 나오는지", "이 SLAM 코드에서 좌표계가 왜 뒤집혀 있는지" — 이런 건 에이전트가 알아서 못 잡는다(14장 참고). 결국 좋은 코드가 왜 좋은지, 나쁜 코드가 왜 나쁜지를 구분하는 눈이 필요하고, 그 눈은 좋은 코드를 많이 읽어야 생긴다.

추천 접근법: 유명 오픈소스(ORB-SLAM3, Ultralytics, HuggingFace Transformers 등)의 코드를 읽으면서 "왜 이렇게 짰는지"를 이해하라. 코딩 실력은 결국 코드를 읽고 판단하는 능력이다.

**Q: 학회 발표는 어떻게 준비하나요?**

A: 학회 발표는 크게 **구두 발표(oral)**와 **포스터 발표(poster)**로 나뉜다.

- **포스터**: 대부분의 첫 발표는 포스터다. A0 크기로 연구 내용을 요약한다. 핵심은 Figure를 크게, 텍스트를 적게 하는 것이다. 지나가는 사람이 3초 안에 관심을 가지게 해야 한다. 발표 연습은 연구실 동료들 앞에서 최소 3번은 하자.
- **구두 발표**: 보통 15-20분이다. 슬라이드는 20장 이내, 한 슬라이드에 한 메시지. 데모 영상이 있으면 좋다. 질문에 대비해서 supplementary 슬라이드도 준비하자.
- 공통: 영어 발표가 대부분이니, 스크립트를 써서 연습하되 외우지는 말자. 자연스러운 영어보다 내용 전달이 중요하다.

**Q: 영어 논문 읽기가 너무 힘든데요?**

A: 이건 정말 시간이 해결해준다. 몇 가지 팁:

- **구조를 먼저 파악하라**: 대부분의 논문은 Introduction → Related Work → Method → Experiments → Conclusion 구조다. Method만 진짜 새로운 내용이고, 나머지는 패턴이 비슷하다.
- **분야별 어휘를 먼저 익혀라**: "ablation study", "state-of-the-art", "we empirically show" 같은 표현은 반복된다. 처음 20편쯤 읽으면 이런 표현에 익숙해진다.
- **번역 도구를 부끄러워하지 마라**: 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`)를 등록하면 `git push`에 비밀번호가 필요 없다. (참고: [정진용 블로그](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
# 1. 배치 사이즈 줄이기 (가장 먼저 시도)
batch_size = 16  # → 8 또는 4

# 2. 메모리 정리
torch.cuda.empty_cache()

# 3. Gradient accumulation 사용 (배치 효과는 유지하면서 메모리 절약)
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 (메모리 절반으로)
from torch.cuda.amp import autocast, GradScaler
scaler = GradScaler()
with autocast():
    output = model(input)
    loss = criterion(output, target)
```

**문제**: `CUDA version mismatch`

**해결**:

```bash
# 설치된 CUDA 버전 확인
nvcc --version

# PyTorch에서 인식하는 CUDA 버전 확인
python -c "import torch; print(torch.version.cuda)"

# 둘이 다르면 PyTorch 재설치 (CUDA 버전에 맞춰서)
pip install torch --index-url https://download.pytorch.org/whl/cu121
```

**문제**: `RuntimeError: CUDA error: device-side assert triggered`

**해결**: 이건 보통 라벨 인덱스가 범위를 벗어났을 때 발생한다. CPU에서 돌려보면 더 자세한 에러 메시지가 나온다.

```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 설치
sudo apt-get install -y nvidia-container-toolkit
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

# 특정 버전 필요 시 직접 설치
sudo apt-get install libeigen3-dev
```

## D. 체크리스트: 연구 시작 전 확인 사항

### D.1 환경 설정

- [ ] Ubuntu 설치 완료 (22.04 LTS 권장)
- [ ] NVIDIA 드라이버 설치 (`nvidia-smi`로 확인)
- [ ] CUDA / cuDNN 설치 (`nvcc --version`으로 확인)
- [ ] Conda 또는 venv 환경 설정
- [ ] PyTorch GPU 동작 확인 (`torch.cuda.is_available()`)
- [ ] ROS2 설치 (필요시, Humble 또는 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 연구 도구

*연구 도구의 본격 list는 [`../research-notes/part2_writing/`](../research-notes/part2_writing/) + [`part3_presentations/ch02_conference_prep.md`](../research-notes/chapter_34_conference_prep.md) 에 흡수되었다. 분야 적용은 § 20.4 "논문 작성 도구" + § 20.7 추천 학습 경로 참고.*

### D.4 데이터셋 준비

- [ ] 연구 관련 데이터셋 다운로드
- [ ] 데이터 포맷 이해 (이미지 크기, depth 단위, 좌표계)
- [ ] DataLoader 구현 (PyTorch Dataset/DataLoader)
- [ ] 데이터 시각화 코드 작성 (디버깅용)

## E. 첫 주 생존 가이드

연구실에 처음 들어왔을 때, 뭘 해야 할지 막막한 건 당연하다. 이 가이드는 "첫 주에 최소한 이것만은 하자"를 정리한 것이다.

### Day 1-2: 환경 구축

```
[ ] 연구실 서버 계정 받기 (관리자에게 요청)
[ ] SSH로 서버 접속 확인
[ ] VS Code Remote-SSH 설정
[ ] 서버에 conda 환경 만들기
[ ] PyTorch + CUDA 동작 확인
[ ] 연구실 GitHub organization에 가입
[ ] Slack/Discord 채널 가입
```

> 팁: 서버 환경 설정에서 막히면 선배에게 물어보자. "이걸 해봤는데 예상과 다르게 이런 결과가 나왔다"라고 하면 거의 100% 도와준다. 아무것도 안 해보고 물어보면... 알아서 하라고 할 수도 있다.

### Day 3-4: 기존 코드 파악

```
[ ] 연구실의 기존 코드/프로젝트 리포지토리 클론
[ ] README 읽기 (있다면)
[ ] 기존 코드 빌드/실행 해보기
[ ] 데이터셋 다운로드 및 경로 설정
[ ] 간단한 데모 돌려보기
```

> 팁: 코드가 돌아가지 않는 건 정상이다. 환경이 다르고, 경로가 다르고, 버전이 다르니까. 에러 메시지를 복사해서 Google에 검색하면 대부분 Stack Overflow에 답이 있다.

### Day 5: 논문 읽기 시작

*Day 5·6-7의 메타 운영 (논문 추천받기·연구 방향 파악·자기소개 준비)은 [`../grad-notes/chapter_04_two_way_relationship.md`](../grad-notes/chapter_04_two_way_relationship.md) (Day 5) + [`p3_ch01_my_research.md`](../grad-notes/chapter_07_my_research.md) (Day 1-7 시퀀스)에서 본격 다룬다.*

> 팁: 처음 읽는 논문은 이해가 안 되는 게 정상이다. "이 논문이 무슨 문제를 풀려고 하는가?"만 파악해도 첫 주로서는 충분하다.

### Day 6-7: 연구 방향 파악

*위 link 참조. 분야 적용 1줄: 본 문서의 18장(연구실 연구 방향)을 훑으면서 연구실의 최근 논문/프로젝트와 선배들의 주제를 매핑한다.*

### 첫 주에 하지 않아도 되는 것들

- 논문을 완벽하게 이해하기 — 시간이 해결해준다
- 최신 연구 트렌드를 전부 파악하기 — 점진적으로
- 코드를 처음부터 짜기 — 기존 코드를 수정하는 것부터
- GPU 서버를 완벽하게 세팅하기 — 선배 환경을 복사하자
- 연구 아이디어를 내기 — 최소 1-2개월은 배우는 시간이다

### 생존을 위한 마인드셋

생존 마인드셋 5 frame은 별도 가이드(research-notes·grad-notes)에서 본격적으로 다룬다. 메타 layer — 채점자 없는 시스템에서의 자율성·비교·기록 — 가 거기서 풀리고, 본 챕터의 분야 layer는 그 메타와 한 쌍을 이룬다.

각 항목이 흡수된 자리:

- *모르는 건 당연하다* → [`../grad-notes/chapter_14_autonomy_weight.md`](../grad-notes/chapter_14_autonomy_weight.md) § 2 (대학원의 가치 재정의)
- *"안 돼요"는 보고가 아니다* + 보고 형식(예측·시도·결과) → [`../grad-notes/chapter_10_one_question_email.md`](../grad-notes/chapter_10_one_question_email.md) § 3 (형식의 사소한 표준)
- *기록하라 — 과거의 내가 미래의 나를 도와준다* → [`../grad-notes/chapter_08_time_use.md`](../grad-notes/chapter_08_time_use.md) § 5 (퇴근 전 포스트잇)
- *작게 시작하라 — 작은 코드 조각부터* → [`../grad-notes/chapter_11_tool_trap.md`](../grad-notes/chapter_11_tool_trap.md) § 1 (셋업 시간은 연구 시간이 아니다)
- *비교하지 마라 — 3개월 후의 나* → [`../grad-notes/chapter_15_comparison_trap.md`](../grad-notes/chapter_15_comparison_trap.md) § 3 (단거리 vs 장거리)

---

# Ch.22 — 마무리: 어디서부터 시작할까?

21개 챕터를 지나왔다. 센서가 세상을 숫자로 바꾸는 방식, 그 숫자가 좌표계를 타고 흐르는 경로, 로봇이 그 위에서 움직이는 법, 그리고 딥러닝이 이 파이프라인 어디에 끼어드는지 — 한 바퀴 돌았다. 남은 질문은 "그래서 내일부터 뭘 하냐"이다.

## 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의 도커를 동시에 써야 하는 순간이 온다.

## 22.2 프로필별 시작점

가장 자주 듣는 질문이 "어디서부터 읽느냐"이다. 배경에 따라 들어오는 문이 다르다.

**학부 3~4학년, 로보틱스 처음**이라면 Ch.1 → Ch.3 → Ch.9 → Ch.14 순서를 권한다. Spatial AI가 무엇인지 감을 잡고, 수학 언어를 익히고, 이미지에서 3D로 올라간 뒤 SLAM에서 모든 조각이 어떻게 맞물리는지를 본다. 중간에 Ch.16의 실습 환경을 세팅해두면 바로 손을 움직일 수 있다.

**석사 신입, 딥러닝 배경**에서 오는 사람은 Ch.2 → Ch.3 → Ch.10 → Ch.11을 먼저 본다. 익숙한 언어(딥러닝)에서 출발해 센서·수학이라는 새 문법을 익히고, foundation model이 로보틱스에 어떻게 들어오는지 확인하는 순서다. 그다음 Ch.14·Ch.13으로 3D·SLAM 쪽을 본다.

**고전 로보틱스 배경, 딥러닝이 약한 사람**은 Ch.8 → Ch.10 → Ch.11 → Ch.12를 먼저 봐야 한다. 이미 아는 Ch.4~7은 가볍게 훑고, 최근 5년 사이에 뭐가 바뀌었는지를 집중적으로 따라간다. VLA(Ch.12)까지 오면 분야 전체의 무게 중심이 어디로 이동했는지 보인다.

**구체적 프로젝트가 있는 사람**은 순서를 거꾸로 간다. Ch.17의 데이터셋·벤치마크부터 훑고, 비슷한 과제의 논문을 한두 편 골라 그 논문이 의존하는 챕터만 역추적한다. 전체를 다 읽을 필요가 없다.

Ch.20.7의 학습 경로에는 시기별(1개월·3개월·6개월) 권고가 더 자세히 있다. 이 챕터는 큰 방향만 잡는다.

*박사 운영 측면의 프로필별 진입 경로 (학생 단계·연구자 단계 가이드)는 [`../research-notes/README.md`](../research-notes/README.md) 사용법 5 갈래에서 다룬다.*

## 22.3 하지 말아야 할 것

신입이 자주 빠지는 함정 4 항목 — *논문 많이 읽기 / 환경 미리 완성 / 직접 짜기 / AI 의존*. 각 항목의 일반론은 [`../research-notes/chapter_06_why_read.md`](../research-notes/chapter_06_why_read.md) (논문 많이의 함정)과 [`../grad-notes/chapter_11_tool_trap.md`](../grad-notes/chapter_11_tool_trap.md) (장비병 — 환경/직접/AI 함정)에서 다룬다.

분야 적용으로 한 줄. SLAM/CV에서는 *분야 도구가 깊다* — ORB-SLAM3·Colmap·Gaussian Splatting은 공개 구현이 표준이다. 직접 짜기는 교육 목적이거나 새 기여가 분명할 때만 의미가 있다. 그 외에는 기존 구현을 돌리고 고장 난 부분을 찾는 작업이 연구에 가깝다.

## 22.4 장기전의 감각

박사 시간 단위(첫 해·1년 반·5년)의 frame은 별도 가이드에서 다룬다 — [`../grad-notes/chapter_07_my_research.md`](../grad-notes/chapter_07_my_research.md) (내 연구가 생기는 시점), [`../grad-notes/chapter_01_phd_decision.md`](../grad-notes/chapter_01_phd_decision.md) (박사 결정의 시간 지평).

로보틱스 실험은 *세팅 3–6개월·실험 사이클 2주*가 흔한 단위다. 첫 논문이 1년 반 걸리는 것도 같은 이유다. 분야의 시간 감각이 박사 운영의 frame과 합쳐지면 매일의 속도에 덜 흔들린다.

## 22.5 한 줄

여기까지 읽었다면 긴 글을 끝까지 읽는 능력은 이미 손에 있다. 분야 본체는 여기서 한 바퀴 끝난다.

박사 운영·읽기 mindset의 본격 가이드는 [`../research-notes/`](../research-notes/)·[`../grad-notes/`](../grad-notes/)에서 다룬다.

초안 작성일자: 2025.12.28 · 개정일자: 2026.05.01

# Ch.1 — Introduction: What Is Spatial AI?

Sketch the full map of the Spatial AI field in your head first. Only then do the individual techniques in later chapters feel necessary, and you see how they connect to each other.

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

> **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) — The paper that first systematized the Spatial AI concept.
> - [Cyrill Stachniss — Introduction to Mobile Robotics](https://www.youtube.com/playlist?list=PLgnQpQtFTOGQrZ4O5QzbIHgl3b1JHimN_) — Prof. Cyrill Stachniss's mobile robotics lectures at the University of Bonn. A well-organized treatment of the foundational concepts behind Spatial AI.

## 1.2 Why It Matters

The techniques you need to dig into depend on which domain you want to work in. For autonomous driving, focus on LiDAR and sensor fusion; for AR/VR, dig into visual-inertial systems. Knowing the full application landscape is what lets you build your own roadmap.

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 robotics be solved once AI advances?" — a question I hear often. It won't. What AI improves and what makes robotics hard are different things.

Most of the difficulty in robotics arises at the interface with the physical world:

- **No undo.** A code bug that triggers a robot collision damages equipment or injures people. There is no rollback.
- **Iteration is slow.** Edit code → upload → reset the environment → ensure safety → run → physically verify. One cycle takes minutes to tens of minutes.
- **Sensor data is always dirty.** Backlight, motion blur, drift, frame drops. "Works well on clean data" is meaningless in robotics.
- **Real-time constraints apply.** If obstacle avoidance is 200 ms late, you collide. High accuracy is useless without speed.
- **Edge cases are fatal.** An LLM that is wrong 5 times out of 100 is still useful. An autonomous vehicle that is wrong once in a million is an accident.
- **There is a sim-to-real gap.** A simulator's friction coefficients, inertia, and noise are approximations. When those errors accumulate, behavior on the real robot diverges.

In summary:

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

There are clearly areas that AI advances will improve: recognition accuracy, decision quality, natural language command understanding, and so on. But the right column of the table above — iteration speed, sensor noise, real-time constraints, breakage risk — belongs to the domain of physical law. Scaling models doesn't solve it.

This is why this document covers math, sensors, SLAM, and calibration. Training one AI model alone won't make a robot work.

### 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**: tell AI to "solve this problem" and it solves it, but it cannot judge "which problem should be solved". Which sensor combination suits this environment, which accuracy is sufficient for this application, which trade-offs are acceptable — only someone who knows the domain can judge these. (*The general frame for problem definition is covered in [`../../research-notes/chapter_01_review_paper.md`](../../research-notes/chapter_01_review_paper.md) and [`ch02_feynman_problem.md`](../../research-notes/chapter_02_feynman_problem.md) *(Korean; English version planned)*.*)
- **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**: Whether a cable came loose, whether dust settled on a sensor lens, whether a motor overheated — AI cannot solve problems it cannot reach via ssh. You need someone standing in front of the robot, touching it with their hands.
- **Judging reliability**: Even when AI reports "99% accuracy", whether that 1% leads to a safety incident is something an engineer must judge. 99% is not enough in autonomous driving, but 99% may be enough for an indoor serving robot.

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

Read a SLAM paper without the math and you get stuck on the equations; without knowing sensor characteristics, you cannot understand why an algorithm fails in particular situations. Building up from the fundamentals in order is ultimately the fast path.

### Staged Learning Path

Below is a more concrete staged roadmap. Adjust the pace to your background, but don't skip any stage.

**Entry stage — getting tools under your fingers**

The goal of this stage is to handle the basic tools research requires with full fluency. 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 — internalizing 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)** — the flagship algorithm for feature-based SLAM. Run it and tear into 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 (vision-language-action) models, world models, 3D Gaussian Splatting + SLAM, and more. Attempts to unify perception, planning, and control in a single model are emerging, but as of 2026 most deployed systems remain modular.
> - **What to watch now**: Research grafting foundation models onto robot perception (e.g., open-vocabulary SLAM, VFM-based scene understanding) grew noticeably at CVPR/ICRA 2025. The thread of combining classical geometry with learning-based methods also continues.

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

No matter how well an algorithm is written, without knowing sensor characteristics you cannot diagnose "why does this algorithm fail here?" For example, when SLAM loses tracking in a particular segment, telling apart rolling shutter, LiDAR reflectance, and IMU bias as the cause requires sensor knowledge. Sensors are the entry point of a robotics system; if you do not understand the data coming in at the entry, everything downstream wobbles.

## 2.1 Camera

The camera is the most information-rich sensor. Just as humans understand most of the world through vision, robots also get the most information from cameras. Camera types differ widely in their characteristics, so understanding the trade-offs of each and picking the right one for the task matters.

### 2.1.1 Monocular Camera

The most basic visual sensor, capturing a 2D image with a single lens.

A monocular camera is the cheapest and lightest sensor, and it is also the starting point for most vision tasks such as Visual SLAM, object recognition, and semantic understanding. It cannot measure depth directly, and various algorithms (monocular depth estimation, SfM, etc.) have been developed to work around this structural limit. Understanding this limit is also what makes clear why stereo cameras or depth cameras are needed.

**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 illumination 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 papers at CVPR have grown from around 5 in 2019 to over 30 in 2024, because they operate without motion blur in high-speed settings (drone high-speed flight, sharp vehicle turns). They are not yet mainstream, but if you plan to work with high-speed (>100 km/h) environments or HDR conditions, look at 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 for static scenes
- 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. It directly produces a 3D point cloud.

If cameras give "rich but depth-less" data, LiDAR gives "accurate 3D coordinates directly". This precise ranging is exactly why LiDAR became a core sensor in autonomous driving. Depth estimated from cameras alone carries large errors and depends on weather, while LiDAR measures objects over 100m away with centimeter-level accuracy.

A recent trend worth noting: solid-state LiDAR is rapidly replacing spinning (mechanical) LiDAR. No moving parts means higher durability and easier mass production, which fits automotive volume manufacturing. New approaches such as Livox's non-repetitive scan pattern are also emerging, so point cloud processing algorithms need to change as well.

### 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/32/64, 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/) — The de facto standard library for point cloud processing.

### 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 moving parts
- 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 (RA-L 2022)](https://arxiv.org/abs/2107.06829) — A LiDAR-inertial odometry paper optimized for 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
- Changes with temperature (bias instability)

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

You feel this immediately in practice: double-integrating acceleration to recover position accumulates noise and bias errors proportionally to time squared. With a consumer-grade IMU (as built into smartphones), position error can reach several meters after only 10 seconds. That is why IMUs are almost never used alone; they are always fused with a camera or LiDAR to correct drift.

**IMU grades**:
| Grade | Use | Price | Examples |
|------|------|------|------|
| Consumer | Smartphones, games | $1-10 | MPU6050, BMI160 |
| Industrial | Robots, drones | $100-1K | VectorNav VN-100, Xsens MTi |
| Tactical | Autonomous driving, aviation | $1K-10K | KVH 1750 |
| Navigation | Ships, aircraft | $10K+ | Honeywell HG1700 |

> **Further reading**
> - [Probabilistic Robotics, Ch.5 — Robot Motion (Thrun, Burgard, Fox)](https://www.probabilistic-robotics.org/) — A leading reference on sensor noise modeling and motion models. Covers the theoretical basis of IMU error modeling.
> - [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.

For outdoor autonomous driving or drones, GNSS is the only sensor that provides "global coordinates". SLAM estimates relative position (how far you have moved from where you started), while GNSS gives absolute position on Earth (latitude, longitude, altitude). Combining these two is the core challenge of outdoor robotics. RTK-GPS's centimeter-level accuracy is also used as ground truth for high-precision autonomous driving localization, so the principles are worth knowing.

**Accuracy**:
- Standard GPS: 2-5m
- DGPS (Differential): 0.5-2m
- RTK-GPS (Real-Time Kinematic): 1-2cm

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

Radar is growing in importance in autonomous driving and robotics. In environments where LiDAR and cameras fail — fog, rain, dust, strong backlight — radar still operates reliably. It is also cheaper than LiDAR.

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

In the real world, perfect perception with a single sensor is impossible. Autonomous vehicles use camera, LiDAR, radar, IMU, and GNSS all at once, and when, where, and how the data from these sensors are combined 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

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

> **⚠ Note for AI agents**: When a sensor "is not getting data", the cause is usually not software but a physical connection (cable, IP configuration, power, USB bandwidth). AI tends to suggest reinstalling the driver first, but check the physical connection first with system commands such as `dmesg`, `lsusb`, `ping`.

> **Technical Timeline: sensor technology**
> - **~2010**: Centered on 2D LiDAR (SICK, Hokuyo) and monocular cameras. Sensors were expensive and bulky, and processing power was limited. Stereo cameras were hard to run in real time due to computational cost.
> - **2012~2017**: 3D LiDAR (Velodyne VLP-16) became widespread, and RGB-D cameras (Kinect) reached the mass market. LiDAR prices dropped from tens of thousands to thousands of dollars. Visual-inertial systems (VIO) also began to be used in real systems.
> - **2018~2022**: Solid-state LiDAR (Livox) appeared, with prices falling to the hundreds of dollars. Event camera research became more active. Multimodal sensor fusion (camera + LiDAR + IMU) became the standard.
> - **2023~**: Solid-state LiDAR is rapidly replacing the spinning type. Event camera adoption is starting to grow in high-speed/HDR applications. 4D radar (including Doppler velocity) is also emerging as a new auxiliary sensor.
> - **Worth watching now**: As solid-state LiDAR goes mainstream, algorithms built on the assumption of spinning LiDAR need to be redesigned. Event cameras are not yet mainstream, but they are being adopted quickly in fields where the limits of conventional cameras are clear, such as high-speed drones and autonomous driving. When sensor hardware changes, algorithm research directions follow.

---

# Ch.3 — Mathematical Foundations

Understanding Spatial AI properly requires a mathematical foundation. This chapter touches on the core concepts briefly; for deeper study, consult the recommended references.

The urge to skip the math is understandable. But when you read papers, you end up stuck at the equations. A SLAM paper says "optimization on SE(3)" and if you don't know what SE(3) is, you miss the paper's core idea; when a single line says "we derived the Jacobian and solved with Gauss-Newton," if that doesn't parse, the whole methodology is out of reach. The math here is not "math for a math exam" but "math to read and implement robotics papers." A third-year engineering student will have taken linear algebra, so the focus here is on connecting what you learned as an undergraduate to how it gets used in robotics.

Classical mathematical tools are still central, but Differentiable Programming and Auto-Differentiation are changing how we approach optimization problems. Jacobians used to be derived by hand; now auto-diff in PyTorch or JAX computes gradients for complex pipelines automatically. This is the background that made end-to-end learning-based SLAM and Differentiable Rendering (NeRF, 3D Gaussian Splatting) possible. To understand what auto-diff does internally, you still need the fundamentals covered here.

## 3.1 Linear Algebra

Linear algebra is the base tool across Spatial AI. Coordinate transformations, camera models, optimization, deep learning — all are expressed with matrices and vectors. "I took linear algebra as an undergrad" and "I can apply linear algebra to robotics" are different levels. This section covers the concepts used most 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) — If you haven't watched this series, watch it. It shows visually what matrix multiplication means geometrically and why eigenvalues matter. It helps you understand linear algebra as "transformation" rather than "computation."
> - [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 is at the heart of Spatial AI. It expresses mathematically "where is the robot in 3D space, where is the camera looking, and where is that object?" Without this part, the first page of a SLAM paper is already a wall.

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

Once you build a robot system yourself, this becomes tangible. A single piece of data has to pass through several frames before it is meaningful. "The location of the object the camera sees" is expressed in the camera frame, but for the robot to approach the object, that position must be transformed into the robot or world frame. Each sensor has its own frame, and sensor fusion is possible only when you know the transformation between them accurately (extrinsic calibration).

**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) — The best robotics-oriented treatment of coordinate frame transformations.
> - [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

Multiple rotation representations exist because each has different strengths and weaknesses. In SLAM optimization, the choice of representation affects convergence speed and stability. Without this, you can't tell "why does this code use quaternions while that code uses a rotation matrix?"

**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 the most widely used.

**Axis-Angle** combines a rotation axis n and angle θ into a 3-parameter representation. It converts to a rotation matrix via Rodrigues' formula.

Practical tips: ROS uses quaternions as the default rotation representation, OpenCV mostly uses Rodrigues vectors (axis-angle), and optimization libraries (Ceres, GTSAM) often use Lie group-based representations (so(3) → SO(3) mapping). You need to convert between them freely.

> **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) — The best robotics-oriented treatment of SE(3), SO(3), and Lie groups/algebras. Essential reading if you dig into this area.
> - [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. Thanks to the central limit theorem, many natural phenomena follow a Gaussian, and operations between Gaussians (product, sum) are closed, 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 is the mathematical basis of state estimation. It is the formula answering "given the sensor measurement, what is the robot's actual state?" Without it, you cannot understand Kalman filters, particle filters, or factor graph-based SLAM.

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

Once you see this recursive structure, the Kalman filter follows immediately. Every time new sensor data arrives, update the existing belief (prior) to get a more accurate estimate (posterior). The Kalman filter's predict-update cycle is exactly this structure.

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

The parameter most likely given the data.

**MAP (Maximum A Posteriori)**:

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

An estimate that takes the prior into account.

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. Mathematically, taking the log of MAP turns it into "sum of squared observation errors + regularization term," 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

Optimization is the final stage of Spatial AI algorithms. SLAM bundle adjustment, camera calibration, and deep-learning training are all optimization problems. Without this section you can run the code but cannot debug why it fails to converge or why the result is wrong.

### 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), no x 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**

Three easily confused concepts, laid out:

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)

The core algorithm for bundle adjustment and pose graph optimization in SLAM.

Why LM dominates in practice: Gauss-Newton converges very quickly with a good initial value but can diverge with a bad one. LM adjusts λ automatically, starting safely like gradient descent early on and converging quickly like Gauss-Newton once near the solution. Ceres Solver, g2o, and GTSAM all adopt it as the default algorithm for robotics optimization libraries.

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

*If you want to become a researcher, read from here.*

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 map that sends an element (vector) of the Lie algebra to an element (rotation matrix) of the Lie group. Let's derive where it comes from.

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 R_dot^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 includes not only rotation but also translation. SE(3) handles this.

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

**Key point**: a 6-DoF pose (3 rotation + 3 translation) can be parameterized by a 6D vector xi in R^6. Run optimization in unconstrained 6D Euclidean space and lift the result onto the SE(3) manifold via the exponential map. This is why Lie groups are used in SLAM optimization.

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

**Why this matters:**

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.

**Practical note**: 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**: the velocity a sensor (e.g., an IMU) measures (angular velocity, linear velocity) is expressed in the sensor frame. Use the adjoint to convert it to the body or world frame. 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. Major SLAM systems like ORB-SLAM3 and Kimera use it.

```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) — Summarizes the essentials of Lie groups in 20 pages. Read this before the papers.
> - [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

*If you want to become a researcher, read from here.*

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

Factor types commonly used 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. (2012), the core of iSAM2. 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

*If you want to become a researcher, read from here.*

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. Most widely used |
| **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/) — The original treatment of RANSAC 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

*If you want to become a researcher, read from here.*

Information-theoretic concepts are used in active SLAM, exploration, and uncertainty-based decision making. Just the essentials.

**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: An exploration planner using information gain (2022)](https://arxiv.org/abs/2206.05193) — An example of using information theory 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 real-time updates. Ceres Solver became an industry standard.
> - **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.

---

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

This chapter covers everything from the math to real hardware selection and communication protocols. Equations show up, but the goal is "getting a robot to actually move."

---

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

Without kinematics, you get stuck in the following situations:
- 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. The key point is that this single matrix expresses the position and pose of a rigid body simultaneously, and multiple transformations can be chained through matrix multiplication.

Given a transformation T_01 between two frames and another transformation T_12:

```
T_02 = T_01 * T_12
```

This is the essence of forward kinematics. Multiply the transformation of each joint in order from the base to the end-effector.

### 4.2.2 DH Parameters (Denavit-Hartenberg)

A method proposed in 1955 by Denavit and Hartenberg. Seventy years on, it remains the industry standard. Four parameters define the relationship between two 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."

Core idea: represent each joint as a twist (screw motion) and compute 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 skew-symmetric matrix representation of S_i (an element of se(3))
- 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 |

Practical advice: you must know DH parameters. The parameters that go into a URDF (robot description file) are ultimately DH-based, and all industrial robot manuals provide DH tables. PoE is theoretically cleaner and preferred in research, but not knowing DH in the field will cause trouble. Learn both.

```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 — the textbook with the best exposition of PoE. Free PDF and Coursera course: https://modernrobotics.org
> - Craig, *Introduction to Robotics*, Chapter 3 — the standard reference for DH parameters. Uses 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

The method of deriving a closed-form solution. When possible, it is the fastest and most accurate.

**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 ± shows that there are two solutions (elbow-up, elbow-down). This is the essential difficulty of 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. Most industrial 6-axis robots (UR, KUKA, ABB, etc.) have this structure.

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) | ROS default, slow, fragile near singularities |
| **IKFast** (OpenRAVE) | Analytical (code generation) | auto-generates C++ code for specific structures. Fast |
| **TRAC-IK** | KDL + SQP dual | higher success rate than KDL, ROS package available |
| **MoveIt2 IK** | Integrates the solvers above | ROS2 ecosystem, integrated collision avoidance |
| **pinocchio** | PoE-based | modern, fast, differentiable |

```python
# Why TRAC-IK beats KDL: probability of finding a solution within the time budget
# Benchmark (Beeson & Ames, 2015):
#   KDL:     ~50-70% success rate (5ms time limit)
#   TRAC-IK: ~95-99% success rate (same condition)
```

> **Further reading**
> - Beeson & Ames, "TRAC-IK: An Open-Source Library for Improved Solving of Generic Inverse Kinematics" (2015): https://traclabs.com/projects/trac-ik/
> - 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}
```

n < 6 is under-actuated, n = 6 is fully-actuated, n > 6 is redundant.

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

When this value is zero, the robot is at a singularity. The larger it is, the more evenly the robot can move in all directions.

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 — the most comprehensive treatment of the Jacobian
> - 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

The math stops here. Back to reality. "Deciding" joint angles does not make the robot move. There must be motors, there must be sensors, and there must be 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, good efficiency. The standard for modern robots. With FOC (Field-Oriented Control), torque ripple can be minimized.

**Servo motors (Dynamixel series):**
A product that bundles motor + reducer + encoder + controller into a single unit. Robotis's Dynamixel series is the most widely used in research.

| Model | Torque (Nm) | Communication | Use |
|------|-----------|------|------|
| XL330 | 0.5 | TTL | small grippers, SO-ARM100, etc. |
| XM540 | 10.0 | RS-485 | mid-sized robot arms |
| PH54  | 44.7 | RS-485 | large manipulators, mobile robots |

Dynamixel strengths: daisy-chain wiring, position/velocity/torque control modes, adjustable PID gains, good performance for the price. Weaknesses: a ceiling on communication speed; for advanced control, custom firmware is sometimes required.

**Quasi-Direct Drive (QDD):**

The approach that drew attention with the MIT Mini Cheetah (2019). The core idea is simple: **lower the 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**: when external force is applied, the joint follows naturally. Safer under collision and easier for force control.
- **High transparency**: end-effector force can be estimated from motor current alone, without a torque sensor.
- **High bandwidth**: with less friction and elasticity in the reducer, fast torque response is possible.

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
- ALOHA (low-cost bimanual teleop)
- 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²
#   → the inertia felt at the end-effector is very large
#   → precise force control is difficult
#
# QDD (gear ratio 8:1):
#   reflected inertia = 8² × 0.01 = 0.64 kg·m²
#   → more than 15× lighter
#   → force control is much easier
```

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

**Actuator selection criteria:**

Things to consider when selecting the actuator for a robot joint:

1. **Required torque**: static torque (pose holding) + dynamic torque (acceleration). Safety factor of 2-3×.
2. **Required speed**: the joint's maximum angular velocity. Determine motor RPM accounting for the gear ratio.
3. **Backdrivability**: QDD for collaborative robots or when force control is needed, otherwise harmonic drive.
4. **Size and weight**: since the actuator mounts on a link, physical constraints exist.
5. **Thermal**: check the continuous torque rating. Peak torque is only available for short bursts.

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

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. Expensive but no homing required. Standard on industrial robots.

```
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). Sensors from ATI Industrial Automation are the research standard. They are expensive ($3,000–$20,000).

**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 has become a standard in robotics as well. 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 1 km (at 125 kbps)
- 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, up to 1 Mbps, 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 (Ubuntu example)
sudo apt install linux-image-rt-amd64   # Debian/Ubuntu

# 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
# max latency under 50μs is good
```

**Why 1kHz:**

Reasons the standard robot control loop runs at 1kHz (1ms):

1. **Impedance/force control**: the control frequency must be well above the mechanical resonance. Most robot arms have natural frequencies of a few to tens of Hz, so at least 10× higher (→ hundreds of Hz to 1kHz) is needed for stable control.
2. **Nyquist theorem**: controlling a 100 Hz dynamic phenomenon requires sampling at a minimum of 200 Hz; in practice 5–10× (→ 1kHz) is preferred.
3. **Communication bandwidth**: CAN bus at 1 Mbps driving ten motors at 1kHz already saturates the link. Anything beyond requires EtherCAT.
4. **Convention**: after the MIT Cheetah demonstrated dynamic walking with a CAN + 1kHz configuration, QDD + 1kHz became the de facto standard.

When higher-frequency control (5–10 kHz) is needed: very light robots (low inertia), high-speed collision response, some tactile control. These cases call for EtherCAT or FPGA-based control.

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

*If you want to become a researcher, start reading from here.*

Kinematics addresses "how to move a given robot," but also "what robot should be designed." This section covers advanced topics related 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. Unattainable but ideal.
- κ → ∞: 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/

---

## 4.7 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 Planning"** — the canonical text on DH parameters and kinematics. Uses the Modified DH convention. Best suited to the undergraduate level.

- **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"** — the most comprehensive graduate textbook. Covers kinematics, dynamics, and control together. Thick, but correspondingly thorough.

**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 (first electric computer-controlled robot arm)
1985 ── Product of Exponentials formalized
1998 ── Harmonic Drive adoption spreads in robots
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 5-axis robot arm (under $200)
```

---

*The next chapter layers force and mass on top of this kinematics: 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 rests on two points:

**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 in 3D it must be included. Omitting it makes the robot behave like a ghost in simulation.

### Recursive Newton-Euler Algorithm (RNEA)

This is the most efficient way to solve inverse dynamics for a serial manipulator. The core idea is simple:

**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 v=0, a=0. 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 key point is that the coordinate frame can be chosen freely. In Newtonian mechanics, the CoM position and orientation of each link must be expressed in the world frame, and constraints must be managed. In Lagrangian mechanics, choosing joint angles as generalized coordinates makes the constraints vanish automatically.

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

Note that M(q) depends on q_2. At q_2 = 0 the arm is fully extended and the inertia is maximal. At q_2 = π the arm is folded and the inertia is minimal.

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

Running this code confirms that the result matches the hand derivation 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.

In the end, both are required. Without Lagrangian mechanics, one cannot understand control theory; without Newton-Euler, one cannot implement real-time code.

> **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̈
```
Answers "how does the robot accelerate under this torque?". The core of simulation. The 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.

The core idea of ABA: treat each link as an "articulated body" and recursively accumulate the inertia of its subtree. q̈ can be computed 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).
- Drake's time-stepping approach belongs to this family.

**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**: with n contact points, there are 3^n possible contact mode combinations (contact/separation, stick/slip in each direction).

### Why Contact Handling Differs Across Simulators

Because there is no "right answer" in contact dynamics. 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**: rigid contact + time-stepping (Stewart-Trinkle), or hydroelastic contact. 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 but has limitations for robotics tasks that require accurate contact.
- **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. For learning locomotion with RL, MuJoCo is the standard. For manipulation research where contact matters, Drake or MuJoCo is the usual choice; recently MuJoCo's contact quality has also improved significantly.

```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"  Penetration depth: {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 standard for contact-based control simulation. Introduces the convex contact model 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

*If you want to become a researcher, start reading here.*

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 combine by simple 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. For real-time control the C++ API is necessary — Python overhead is not negligible in kHz-rate control loops.

> **Further reading**
> - Featherstone, *Rigid Body Dynamics Algorithms*, Ch. 2 — the original description of spatial vector algebra. Required reading for anyone who wants to do research in this area.
> - 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

*If you want to become a researcher, start reading here.*

An industrial manipulator has its base bolted to the floor. Legged robots, drones, and underwater robots, however, have a base that moves. In that case the base position and orientation become additional degrees of freedom, and the structure of the dynamics changes fundamentally.

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

This is subtly important. 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

A core property of floating-base systems: **underactuation**. The base has no actuator directly attached to it. Legged robots must push against the ground with their feet to move the base; drones move the base through propeller thrust.

Splitting the manipulator equation into base and joints:

```
[ M_bb  M_bj ] [ a_base  ]   [ C_b ]   [ g_b ]   [  0  ]   [ J_c^T ]
[ M_jb  M_jj ] [ q̈_joints] + [ C_j ] + [ g_j ] = [ τ_j ] + [ J_c^T ] λ
```

The `0` at the top left is the key — there is no joint torque on the base. λ is the contact force and J_c is the contact Jacobian. The base can accelerate only through contact forces and gravity.

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.** For the robot not to fall, the CoM trajectory must stay over the support polygon (the ZMP condition). More precisely, 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 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 is not limited to legged robots; it applies to any system with a floating base. Similar structures are used in drone trajectory optimization and underwater-robot control.

> **Further reading**
> - Orin et al., "Centroidal Dynamics of a Humanoid Robot", Autonomous Robots 2013 — the foundational paper introducing centroidal dynamics to robotics
> - Wensing et al., "Optimization-Based Control for Dynamic Legged Locomotion", 2023 — a recent 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

Where to begin is the most important question when studying this area. The recommended order depends on background.

**Undergraduate junior/senior, introduction to dynamics:**

> - Spong, Hutchinson, Vidyasagar, *Robot Modeling and Control* — the most accessible textbook at an undergraduate level. 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/) — dynamics from a Lie group/algebra perspective. Free PDF available. The most mathematically rigorous, but hard on first reading.
> - 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/JAX 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. Its contact handling is fast and stable. The de facto standard simulator in RL research.
> - PyBullet (https://pybullet.org/) — the Python interface to Bullet Physics. The low entry barrier makes it suitable for teaching, but its contact physics accuracy does not match MuJoCo or Drake.

---

## 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)
2022 ── Drake 1.0 (MIT → Toyota Research Institute)
2021 ── MuJoCo open-sourced (after DeepMind acquisition)
2022 ── MuJoCo 2.3: implicit integration, elliptic friction cone
2023 ── Pinocchio 3.0: CasADi/JAX autodiff support
2023 ── MuJoCo 3.0: MJX (JAX backend for GPU parallelism)
```

---

## Summary

One-sentence summary of this chapter: **dynamics concerns the relationship between forces and motion, and is essential for controlling and simulating robots.**

Practical takeaways:

1. The manipulator equation `M(q)q̈ + C(q,q̇)q̇ + g(q) = τ` is the starting point for everything.
2. Use RNEA for inverse dynamics (computing τ) and ABA for forward dynamics (computing q̈). Both are O(n).
3. Once contact enters, the problem becomes dramatically harder. Simulator choice matters.
4. In floating-base systems, centroidal dynamics is the key tool.
5. Do not implement these from scratch; use Pinocchio or Drake. But understand what these libraries compute internally.

The next chapter examines control techniques built on this dynamics model — computed torque control, operational space control, whole-body control.

---

# Ch.6 — Control Theory

Perceiving the world and actually affecting it are two entirely different problems for a robot. No matter how sophisticated the perception pipeline, a single miscalculated motor current makes the robot tip over, break things, or injure people. Control theory is the discipline of "how to actually realize a desired motion."

---

## 6.1 Why Learn Control

If perception is "understanding the world," then control is "affecting the world." Without both, a robot is just a pile of sensors or a pile of motors.

The reasons to learn control are simple:

- **Motors are dumber than expected.** Command "send the joint to 30 degrees," and the motor slams in maximum current, overshoots 30 degrees, and oscillates. Smoothly bringing it to the desired position is control.
- **Disturbances are always present.** The floor is slippery, the wind blows, the payload mass differs from expectation. The sensor-actuator loop must be closed (feedback) to cope with such uncertainty.
- **Safety is on the line.** When an industrial robot arm works next to a person, the lack of force control can break the person's arm. No exaggeration.

The scope of control theory is broad. This chapter focuses on what is actually used in robotics practice. The flow goes PID, state-space control, MPC, impedance control, and whole-body control.

One thing upfront: control theory involves a lot of math. If you are not comfortable with linear algebra and differential equations, review at least matrix operations and the eigenvalue concept before reading this chapter.

---

## 6.2 PID Control

PID (Proportional-Integral-Derivative), proposed by Minorsky in 1922 for a ship steering system, has been the most widely used controller in industry for over 100 years. Every control engineer in the world learns it first and keeps using it until retirement.

### 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. With the P term alone, a steady-state error remains. This is because as the error becomes small near the target, the control input also becomes small.

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

Frankly, Ziegler-Nichols tuning produces fairly large overshoot. Fine as a starting point, but manual fine-tuning is essential afterward.

**Empirical tuning in practice**: most people do it like this.

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.

A professor would not like to hear that this is done "by feel," but in the field it is done this way most of the time. If the system model is accurate, tuning first in simulation and then applying on the real hardware is far more efficient.

### 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 fundamentally a linear controller. Robot dynamics are nonlinear. It only performs well 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 only to the current error. Without feedforward, tracking performance is limited.

The reason PID is still in use after 100 years is simple: easy to implement, easy to understand, and works "reasonably well" on most systems. If the plant is simple and the performance requirements are not extreme, PID is enough. Per-joint servo control on industrial robots is still mostly PID-based today.

---

## 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, but for MIMO (Multi-Input Multi-Output) systems state-space is much more natural. Robots are almost always MIMO systems, so state-space is the standard representation.

### 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. LQR should not be applied to such a system.

### 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 makes the control input smaller, but state convergence slows down. This is the essential trade-off of LQR.

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

The key point of this result is that all eigenvalues of the closed-loop system (A - BK) are guaranteed to lie in the left half-plane. That is, stability is mathematically proven.

### 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) is a method that solves a finite-horizon optimization problem at every control cycle to compute the control input. It is one of the most widely used control techniques in robotics in the 2020s.

### 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 Dominates in Robotics

- **Constraint handling**: torque limits, joint angle limits, velocity limits, collision avoidance — all go directly into the optimization as constraints. Impossible with PID or LQR.
- **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 function is quadratic and the constraints are linear, the problem becomes a QP (Quadratic Program). QP is convex, so the global optimum is found quickly. Suitable for real-time control.

**Nonlinear MPC (NMPC)**: uses a nonlinear dynamics model. The problem becomes non-convex, making it hard to solve with no guarantee of the global optimum. However, it reflects robot dynamics accurately and performs well. CasADi + IPOPT is the standard toolkit.

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

The greatest obstacle of MPC is that optimization must be solved every control cycle. A legged robot controlled at 1 kHz must solve a QP within 1 ms.

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**: automatic differentiation + interior-point method. The de facto standard for NMPC implementation.
- **acados** (https://docs.acados.org/): CasADi-based but optimized for real time. Generates C code.

Solver speed determines the control rate. A solver taking 5 ms caps throughput at 200 Hz. This is why MPC engineers care so much about the solver.

### 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))
    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 when there is no environment or when the environment is highly rigid. But 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

Core idea of impedance control:

```
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 key is tuning K_d and D_d for the task:
- 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 + (1 / D_d) * F_ext_dot
```

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

*If you want to become a researcher, read from here.*

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 is the foundation of task-space control. Core idea: derive the dynamics directly in the 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 + 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

*If you want to become a researcher, read from here.*

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. Provides a guarantee of the form "whatever disturbance enters, the output error stays below this level." The math is heavy (Riccati inequalities, LMI) and tends to be conservative. Widely applied in industry in the 1990s.

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

Frankly, unless writing a paper, adaptive control or sliding mode is rarely used on real systems. MPC is powerful and intuitive enough. But when "why is this controller stable?" has to be explained, Lyapunov theory is unavoidable. Especially in safety-critical systems (medical robots, autonomous driving), mathematical stability proofs are essential.

---

## 6.9 Further Reading

> **Åström & Murray, "Feedback Systems: An Introduction for Scientists and Engineers"**
> https://fbswiki.org/
> Free PDF available. The most suitable introduction to control theory. Hits the core accurately without going overboard on math. Covers PID, state-space, and frequency response. Undergraduates should start here.

> **Steve Brunton, "Control Bootcamp" (YouTube)**
> https://www.youtube.com/playlist?list=PLMrJAkhIeNNR20Mz-VpzgfQs5zrYi085m
> Explains state-space, controllability, observability, and LQR intuitively. Each video is around 15 minutes, short and dense. Watching before reading a textbook makes understanding much faster.

> **Slotine & Li, "Applied Nonlinear Control"**
> The standard text on nonlinear control, Lyapunov stability, and adaptive control. The book for studying the content of Section 6.8 seriously. Out of print, but PDFs are around (find it yourself).

> **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/
> The de facto standard tool for implementing Nonlinear MPC. Supports automatic differentiation and multiple NLP solvers (IPOPT, SNOPT). 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) applied in industry
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
```

---

This chapter selected the core of control theory actually used in robotics. For the mathematical details of each technique, supplement with the further reading. One piece of advice: control theory is hard to understand without simulation. Running the code in this chapter, changing parameters, and observing how the system response changes is the most effective way to learn. Running one simulation beats reading a textbook three times.

---

# Ch.7 — Motion Planning & Trajectory Optimization

For a robot to go from A to B, it needs a path. You might think a straight line is enough, but obstacles, joint limits, and dynamic constraints apply at once. Finding a path that satisfies all of these is motion planning; following that path 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 the shortest path in a weighted graph. It explores every edge, so it guarantees the optimum. Time complexity O((V + E) log V).

### A* Algorithm

Dijkstra plus a heuristic. An estimated distance to the goal (heuristic) guides the search direction. If the heuristic is admissible (no greater than the true distance), A* guarantees the optimum while running faster than Dijkstra.

```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 abs(a[0] - b[0]) + abs(a[1] - b[1])  # Manhattan distance

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

Grid-based planning guarantees **completeness** — if a solution exists, it is found. But it 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. Effectively impossible.

Sampling-based planners emerged to address this.

---

## 7.4 Sampling-Based Planners

Rather than discretizing the C-space, sample it randomly and search for a path. In high-dimensional C-spaces this is the only practical approach.

### 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 the goal at the same time, and connect the paths when the two trees meet. The most widely used variant in practice; MoveIt2's default planner is also RRT-Connect (via OMPL).

### 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_py 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: at each iteration the nonlinear problem is replaced by a linear/quadratic approximation solved as a QP, and a trust region ensures convergence. Collision avoidance uses a signed distance function to yield a continuous gradient.

### Trajectory Optimization with CasADi

CasADi is a framework that combines symbolic computation, automatic differentiation, and connections to NLP solvers. It is the de facto standard tool for trajectory optimization.

```python
import casadi as ca
import numpy as np

# Simple example: time-optimal trajectory for a 1D double integrator
# 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 energy + time penalty
cost = 0
for k in range(N):
    cost += U[0, k]**2 * dt  # energy
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/) — standard tool for NLP implementation.
> - [Drake Trajectory Optimization](https://drake.mit.edu/) — includes direct collocation examples.

---

## 7.6 MoveIt2: Motion Planning in Practice

MoveIt2 is a ROS2-based motion planning framework. It is the most widely used robot arm planning tool in industry and research alike.

**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**: MoveIt2 uses OMPL as its default planning backend. Planner type and parameters are set in `ompl_planning.yaml`.

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

*If you want to become a researcher, start reading here.*

### 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's `ContactImplicitDirectCollocation` implements this method.

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

*If you want to become a researcher, start reading here.*

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.

Steps 1-3 are **symbolic planning** (which actions, in what order); step 4 is **motion planning** (which concrete trajectory to move along). TAMP combines the two.

### 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, 2023): 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.

---

## 7.9 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) — the standard for single-query planning
2000 ── RRT-Connect (Kuffner & LaValle) — the most widely used variant in practice
2009 ── CHOMP (Ratliff et al.) — gradient-based trajectory optimization
2011 ── RRT* (Karaman & Frazzoli) — asymptotic optimality guarantee
2012 ── OMPL 1.0 released — unified library of sampling-based planners
2014 ── TrajOpt (Schulman et al.) — sequential convex optimization
2019 ── MoveIt2 (ROS2) — industrial/research standard framework
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. This chapter covers reinforcement learning (RL) fundamentals, 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 like PID, MPC, and RRT work very 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. With an accurate model, you get the performance that optimal control theory mathematically guarantees. Learning-based methods have a hard time beating that.

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

**But it is not a silver bullet**

The limitations of learning-based methods must be understood clearly.

- **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 is inherently dangerous.
- **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.

---

## 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, but in real robots this assumption can break down (partial observability, i.e., a POMDP situation). In that case, observation history is used as the state, or a recurrent policy is used.

### Policy Gradient Intuition

The core idea of policy gradient is simple.

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. Data is used once and discarded. 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). The core idea is to limit the size of policy updates. If the policy changes too much from the previous one, the change is clipped.

```
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, insensitive to hyperparameters, and 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 | SAC is continuous-space only |
| 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

In robot RL, simulation is not optional but mandatory, because collecting millions of steps of data on a real robot is unrealistic. The main simulators are summarized below.

### 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 (2019) 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, the upper bound on policy performance drops. Performance will necessarily be lower than a policy optimized for specific parameters.

### 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. Errors can grow exponentially over time.

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

The key is to include the expert action at "states the policy actually visits" in the training data. Theoretically, DAgger has a no-regret guarantee.

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)

A method proposed in Stanford's ALOHA project. Two core ideas:

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) at CMU, applying diffusion models to action generation.

Whereas traditional BC models actions with a unimodal Gaussian, diffusion policy can express arbitrarily complex action distributions through a denoising process. It handles multimodal distributions particularly well.

```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 decisively on data quality. Main data collection methods:

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

*If you want to become a researcher, read from here.*

Inspired by the success of LLMs and VLMs, attempts to build large-scale pretrained models (foundation models) continue in robotics. The idea is to train a generalist policy on large quantities of robot data and adapt quickly 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.

### pi0 (Physical Intelligence)

A diffusion-based generalist robot policy released in 2024. It uses a VLM as backbone and generates actions via flow matching. It achieved state-of-the-art performance on diverse manipulation tasks and also worked on complex long-horizon tasks such as folding laundry.

### 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 not be sufficient 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

*If you want to become a researcher, read from here.*

It is no exaggeration to say that the success of RL hinges on reward function design. And applying RL to real robots requires addressing safety.

### 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. When the state-action space is large, learning effectively fails.

**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:
- Iteratively modify the reward function and review the learned behavior (essentially trial-and-error).
- 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
> The essential RL textbook. Free PDF available. Required reading to build foundations from MDPs to policy gradients. If you do not have time to read it all, prioritize Ch.1-6 and Ch.13.

> **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 150+ domains with a single set of hyperparameters. 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)
         The first policy gradient method. Convergence is proven but variance is high.

2013 ── DQN (Mnih et al., Atari)
         The beginning of Deep RL. Stable training via replay buffer + target network.

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 through large-scale domain randomization.

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 (CMU)
         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

The root of every process that turns raw camera data into meaningful information is here. Whether you are doing SLAM, picking objects, or driving autonomously — if this foundation is shaky, you will spend ages stuck on "why isn't this working?"

---

## 9.1 Image Processing

Raw images from a camera are noisy and unorganized. Before any algorithm can run on top of them, the image has to be cleaned up. Filtering, edge detection, morphological operations — these are the basic preprocessing tools, and without knowing them you cannot diagnose why the downstream pipeline produces strange results.

### 9.1.1 Introduction to OpenCV

**OpenCV (Open Source Computer Vision Library)** is the most widely used CV library.

Whether you are implementing a paper's algorithm directly or prototyping quickly, OpenCV is almost always on the path. With both C++ and Python bindings, it covers everything from research to production.

**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 order (not RGB). This is why colors get flipped when mixing it with Matplotlib or other libraries. Make `cv2.cvtColor(img, cv2.COLOR_BGR2RGB)` a habit.

> **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 is the most basic tool for extracting desired information from an image or removing unwanted noise. Without knowing it, you cannot explain why edge detection output is noisy, or why blur is applied as preprocessing for segmentation.

**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 carry the most information in an image. Object contours, structure, boundaries — edges are also what people look at first when recognizing an object. Canny is the most widely used edge detector, and its output changes a lot with the threshold values, so you have to experiment with the parameters yourself.

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

An essential tool when dealing with binary images. For example, morphology is used to remove small noise specks in a segmentation output, or to reconnect broken regions. Without knowing it, post-processing a binarization result feels hopeless.

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

Many people confuse the order of Opening and Closing — Opening "shrinks first (erosion), then grows back (dilation)", so small bumps or noise vanish; Closing "grows first, then shrinks back", so small holes get filled. Keep this intuition.

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

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 (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 with cheap cameras it is not negligible.

**Distortion correction**:

```python
# Simple correction (computed per frame - slow)
undistorted = cv2.undistort(distorted, K, dist_coeffs)

# Precompute correction maps and reuse (fast - SLAM pipeline standard)
map1, map2 = cv2.initUndistortRectifyMap(K, dist_coeffs, None, K, (w, h), cv2.CV_32FC1)
undistorted = cv2.remap(distorted, map1, map2, cv2.INTER_LINEAR)
```

Calling `cv2.undistort()` every frame is slow. The standard in real-time systems is to precompute the maps with `initUndistortRectifyMap()` and apply them via `cv2.remap()`.

**Fisheye lenses**: cannot be corrected with the standard pinhole distortion model. Fisheye lenses model distortion as a function of the incidence angle θ (equidistant model: r = f·θ). You must use OpenCV's separate `cv2.fisheye` module. Mixing them up can actually make the correction worse, so be careful.

(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

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

Tip: to improve calibration quality, (1) capture at least 20 images from various angles, (2) make the checkerboard cover the whole image evenly, and (3) check that the reprojection error is below 0.5 pixels.

**Understanding what calibration does, intuitively**

Camera calibration is ultimately about figuring out the parameters of "how this camera converts the 3D world into a 2D image". When you capture a checkerboard pattern from several angles, you obtain dozens to hundreds of correspondence pairs between the known 3D coordinates of the checkerboard and the detected 2D coordinates in the image. From these pairs:

1. **Intrinsic parameters** (fx, fy, cx, cy): the focal length and image center of the lens. These are camera-specific, so once you estimate them they do not change unless you swap the lens.
2. **Distortion coefficients** (k1, k2, p1, p2, k3): the amount of lens distortion. Cheaper lenses have larger values.
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.

You need to capture at least 10 images of the checkerboard from various angles and distances. If they are biased to one side, only that region's distortion gets corrected and the rest remain inaccurate. The key is to spread the checkerboard evenly across the whole image.

A reprojection error below 0.5 pixels is acceptable; below 0.1 is very good. Above 1.0, recapture or remove outlier images.

(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) — The de facto standard tool for Camera-IMU calibration.
> - [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.

To understand SLAM, you need to understand features first. As the robot moves its camera, deciding "is what I see now the same place I saw earlier?" requires finding the same point across images. Features are the core tool for finding those correspondences reliably. SLAM, Visual Odometry, Object Recognition — nearly every vision-based robotics algorithm relies on features.

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

ORB is at the core of the ORB-SLAM family. Being patent-free means you can use it commercially without issue, and its speed makes it suitable for real-time systems. It is the first keypoint you will encounter in robotics.

**SIFT (Scale-Invariant Feature Transform)**:
- Scale- and rotation-invariant
- High repeatability
- High computational cost (previously patented, now released)

SIFT is the algorithm Lowe published in 2004, and its paper is among the most cited in CV. Once you understand the principles behind extracting scale- and rotation-invariant keypoints, it becomes natural to see how later methods like SURF and ORB improved upon SIFT.

**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. Worth reading at least once.
> - [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

In SLAM, as the camera moves you need to find the same point between the previous and current frames. This is feature matching, and without a proper grasp of it you cannot tell why SLAM throws a tracking-lost error.

```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 is the key. kNN finds the two nearest matches, and only those whose ratio of first-to-second distance is below a threshold are kept as "good matches". This filters out ambiguous matches (where the first and second are roughly the same distance). The 0.75 value is what Lowe proposed in the original paper; tune it between 0.6 and 0.8 depending on the situation.

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

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 the relative rotation and translation between the two cameras. 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

To summarize 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

Given the same point observed from two viewpoints, compute its 3D position.

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

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

Recently, RAFT (Recurrent All-Pairs Field Transforms) has become the de facto standard for dense optical flow. It is deep-learning-based but much more accurate, so when accuracy matters RAFT is the common choice.

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

*If you want to become a researcher, read from here on.*

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

# Basic PnP (iterative, no initial guess 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.

The Tracking stage of ORB-SLAM3 is exactly this process.

> **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` (minimum 3 points), `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

*If you want to become a researcher, read from here on.*

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) | no threshold tuning needed, adapts automatically |

**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 manual tuning is almost unnecessary.
- This is the currently recommended method in OpenCV.

**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 means "find the correct model with 99.9% probability". 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. Typical choices are 1.0-3.0 pixels for the fundamental matrix and 3.0-5.0 pixels for homography.
- If speed matters, use PROSAC; if accuracy matters, use MAGSAC++.

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

*If you want to become a researcher, read from here on.*

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.
- Robust on repetitive patterns, and has higher repeatability than classical methods.

**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.
- Very high accuracy but slow (GPU required).

**LightGlue**:
- A lightweight version of SuperGlue. Adaptive early stopping pushes easy image pairs through quickly, while harder pairs go through more layers.
- Several times faster than SuperGlue at comparable accuracy.

**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 kornia
from kornia.feature import LightGlueMatcher, KeyNetAffNetHardNet

# Build the extractor and matcher
extractor = KeyNetAffNetHardNet(num_features=2048).eval()
matcher = LightGlueMatcher("keynetaffnethardnet").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)
img0 = kornia.io.load_image(path0).unsqueeze(0).to(device)
img1 = kornia.io.load_image(path1).unsqueeze(0).to(device)

# Feature extraction
with torch.no_grad():
    feats0 = extractor(img0)
    feats1 = extractor(img1)

# Matching
dists, match_idxs = matcher(feats0["descriptors"], feats1["descriptors"])
```

With a GPU, the SuperPoint+LightGlue combination is the most balanced choice. Without a GPU, on embedded platforms, ORB is still the realistic option.

> **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. Patent and speed issues get resolved, and real-time SLAM becomes feasible.
> - **2015~2019**: deep learning seeps in. Learning-based features like SuperPoint (2018) and SuperGlue (2020) begin to surpass classical methods in performance.
> - **2020~**: fusion of geometry and learning. 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.
> - **What to watch now**: classical geometry (epipolar geometry, triangulation) is not going away. Deep learning is replacing the front-end (feature extraction, matching), but the back-end mathematics stays the same. Knowing both is what real skill means.

---

# Ch.10 — Deep Learning for Perception

These are the core techniques for a robot to understand "what it is looking at." Classical CV focused on "how to process the image and extract geometric relationships," whereas here the focus is on recognizing "what is in the image." Object detection, classification, segmentation — for a robot to carry out commands like "there's a red cup over there, pick it up," these techniques are essential.

---

## 10.1 Choosing a Framework

Which deep learning framework to use is a more important decision than it seems. To read and reproduce research code, you need to know the framework it uses; to build your own model, you have to know at least one properly.

### 10.1.1 PyTorch (Recommended)

**Strengths**:
- Intuitive dynamic graph (eager execution)
- Easy to debug
- Standard in the research community
- Rich pretrained models (torchvision, timm)

There is a practical reason. As of 2024, more than about 80% of code released with papers at major conferences such as NeurIPS, CVPR, and ICLR is in PyTorch. Without PyTorch, you essentially cannot read, run, and modify the latest papers.

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

Without knowing the concepts in this section, you cannot understand "why deep learning overwhelms classical methods in image recognition." Without knowing the CNN structure, you don't see why ResNet matters; without knowing the Transformer, you can't understand why ViT and DETR replace earlier approaches.

### 10.2.1 Convolutional Neural Network (CNN)

This is the core architecture for extracting spatial features from images.

A CNN is an architecture that "automatically learns local patterns (edges, corners, textures) in images." In the classical CV covered earlier, features like SIFT and ORB were hand-crafted by humans, whereas a CNN learns the optimal features automatically from data. This is the turning point of deep learning.

**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) — the canonical lecture series for understanding CNNs. Worth watching.
> - [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
```

Once you see the difference from a CNN, it is immediately clear why the Transformer is rising. CNNs exchange information only between "nearby pixels" (local receptive field), but the Transformer's Self-Attention lets "any part of the image reference any other part" (global attention). This is advantageous for grasping the overall context of an object. Since 2020, Transformers have rapidly replaced CNNs in vision.

**Vision Transformer (ViT)** splits the image into fixed-size patches such as 16×16, treats each patch like a "word" as in NLP, and feeds them into a Transformer encoder. The idea itself is simple, but by outperforming CNNs on large-scale data, it has recently become the mainstay of vision tasks.

> **Further reading**
> - [Vaswani et al., "Attention Is All You Need" (2017)](https://arxiv.org/abs/1706.03762) — the original Transformer paper. The start of everything.
> - [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 is the most basic question: "what is in this image?" Object detection and segmentation models internally contain a classifier, so understanding classification models is the fundamental of fundamentals. The backbone of pretrained classification models (ResNet, ViT, etc.) is widely used as the feature extractor for other tasks.

**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 is the simple idea of "adding the input to the output," and this one thing made it possible to train networks dozens of layers deep. Since its 2015 release, it has become a basic building block of almost every deep learning architecture.

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

This pattern is used very often. Remove only the final classification layer of a model pretrained on ImageNet and use the output up to that point as "features." This is called transfer learning, and in robotics it is almost always the approach taken when teaching a system to recognize new objects.

> **Further reading**
> - [He et al., "Deep Residual Learning for Image Recognition" (2015)](https://arxiv.org/abs/1512.03385) — the original ResNet paper. One of the most cited papers in the history of deep learning.
> - [Papers With Code — Image Classification](https://paperswithcode.com/task/image-classification) — check the latest benchmarks and SOTA models.
> - [timm (PyTorch Image Models) library](https://github.com/huggingface/pytorch-image-models) — loads hundreds of pretrained models in a single line. Extremely useful in practice.
> - [Stanford CS231n — Training Neural Networks](https://www.youtube.com/playlist?list=PLoROMvodv4rMFqRtEuo6SGjY4XbRIVRd4) — training techniques and tricks.

---

## 10.4 Object Detection

For a robot to know "where is the cup on that table," it needs not just classification but "where is what." That is object detection — the task of predicting the location and class of objects simultaneously via bounding boxes, used in almost every application: robot manipulation, autonomous driving, and so on.

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

YOLO "only looks once" as its name says — instead of proposing candidates first like two-stage methods, it processes the whole image in one pass and detects every object. When real-time performance matters in a robot system, it is the first model to consider. Ultralytics' YOLOv8/v11 are very simple to install and use, making them well suited for 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
- Better at detecting small objects than YOLO

> **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) — check the latest SOTA.
> - [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) — first DETR-family model to reach YOLO-level real-time speed. A new direction for real-time detection.
> - [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 obvious. Object detection only gives you a rough location via a bounding box, but semantic segmentation gives you the exact boundary of the object. A robot needs the precise contour, not a bounding box, to grasp an object, and for autonomous driving to distinguish road, sidewalk, and lane markings, pixel-level classification is essential.

**Representative models**:

| Model | Characteristics |
| --- | --- |
| FCN | First 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

*If you want to become a researcher, start reading from here.*

Knowing the model architecture seems enough to do research, but in practice "making the training work well" takes more than half of the total research time. Even with the same model, a bad learning rate prevents convergence, and adding one augmentation can bump accuracy by several percent. This section summarizes training techniques used repeatedly in practice.

**Learning Rate Schedule**:

- **Cosine Annealing with Warm-up**: the most widely used schedule. 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 with Warm-up (manual)
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 torch.distributed as dist
from torch.nn.parallel import DistributedDataParallel as DDP

dist.init_process_group("nccl")
local_rank = int(os.environ["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

*If you want to become a researcher, start reading from here.*

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

The core idea is simple: place different augmentations of the same image close together in the embedding space (positive pair) and different images far apart (negative pair).

- **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.
- Since the encoder only processes the visible patches, training is efficient (75% reduction in compute).

**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.
- **Centering + Sharpening**: centering (subtracting the mean) and sharpening (low temperature) are applied to the teacher output to prevent mode collapse.
- The resulting DINOv2 features achieve performance comparable to supervised methods without additional training — e.g., 83.0% ImageNet k-NN and 82.0% ADE20K linear probe.

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

*If you want to become a researcher, start reading from here.*

This is the technique of transferring the "knowledge" of a large model (teacher) to a small model (student). It is especially important in robotics because giant models such as VFMs must run on edge devices. To run SAM in real time on a Jetson, distillation is almost the only way.

**Teacher-Student Structure**:

The student model (small model) is trained to imitate the output of the teacher model (large model, already trained). Crucially, 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. Typically τ = 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)**:

Not only logits, but also the intermediate layer feature maps are made similar to the teacher's.

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

*If you want to become a researcher, start reading from here.*

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
# Core idea of TENT (simplified)
model.eval()
# Make only the BN affine parameters trainable
for m in model.modules():
    if isinstance(m, nn.BatchNorm2d):
        m.requires_grad_(True)
        m.track_running_stats = False  # use batch statistics

optimizer = optim.SGD(model.parameters(), lr=1e-4)

# 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 are combined:
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.

This combination is currently the most widely used approach to sim-to-real transfer.

> **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~2015**: 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. A new paradigm of processing images as patch sequences. DETR applies Transformer to detection as well. Swin Transformer achieves SOTA on various vision tasks.
> - **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.
> - **What to watch now**: the shift from single-task models to foundation models. Pipelines that use a single DINOv2 to extract detection, segmentation, and depth features are beginning to appear experimentally.

---

# Ch.11 — Vision Foundation Models (VFM)

The paradigm of computer vision is shifting. The deep learning models covered so far were specialists trained to do a specific task well on a specific dataset; foundation models are generalists that learn general-purpose visual capabilities from massive data. Once this difference is clear, so is the reason VFM-based perception papers have grown rapidly at ICRA/IROS since 2023.

---

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

Consider the conventional approach. It required repeating the cycle "new environment → data collection → labeling → training". When the factory changed, or when the objects the robot had to recognize changed, everything had to be redone from scratch. Foundation models break this cycle. A model trained once works on objects it has never seen, in environments it has never seen. It is the most promising approach for solving the generalization problem in robotics.

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

**Why it matters**:
- generalization to new environments
- usable without annotations for a specific dataset
- a core role in the lab's Global Module

Here the concept of the scaling law is important. It is the empirical law that performance improves as a power law when model size, data size, and compute are scaled up. Recent large models such as GPT, CLIP, and SAM all exploit this law. "Bigger is better" holds within a certain range (Kaplan et al., 2020; Zhai et al., 2022 for ViT). However, as Hoffmann et al. (2022, Chinchilla) showed, balancing data and compute matters more than simply scaling model size.

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

We look at the vision foundation models most commonly used in robotics. The focus is on what problem each model solves, why it matters in robotics, and how to use it.

### 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**:
- contrastive learning + self-distillation
- 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 the core idea of self-distillation (based on DINOv1, but essential for understanding DINOv2).

### 11.2.2 SAM (Segment Anything Model)

**Promptable segmentation**: segments any object from prompts such as points, boxes, or text.

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 can segment any object it has never seen. 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 works not only on single images but also on videos. Specify an object with a point or box on the first frame, and it is automatically tracked and segmented in subsequent frames. This applies directly to scenarios in which a robot tracks and manipulates objects in real time.

```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). Use `scores` to pick the most suitable mask.

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

similarity = (image_features @ text_features.T).softmax(dim=-1)
    print(similarity)  # similarity between each text and the image
```

`@` is matrix multiplication (dot product). It computes the cosine similarity between image features and text features, expressing numerically how semantically close the image is to each text. 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.

Section 10.7 covered depth estimation; Depth Anything raises it to the level of a foundation model. By leveraging 1.5M labeled images plus 62M unlabeled images, it estimates depth stably indoors (NYU), outdoors (KITTI), and in zero-shot domains. Accuracy can drop, though, in domains that differ greatly from the training data (endoscopy, underwater, etc.). Its advantage is that a robot deployed to a new environment can obtain depth information immediately without additional training.

**Characteristics**:
- trained on 1.5M labeled + 62M unlabeled images
- robust across diverse domains
- V2: more accurate absolute depth

Depth Anything V2 goes a step beyond V1 and also provides a version that estimates metric depth (absolute depth). In robotics, "exactly how many meters away" often matters more than relative depth, so the metric version of V2 deserves attention.

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

Conventional detection models (YOLO, Faster R-CNN, etc.) could only detect the classes used during training. Trained on "person, car, dog", they cannot find "coffee mug". Like CLIP, GroundingDINO can specify and detect any object with text. Give a robot a natural-language instruction such as "find the red cup over there", and it can locate the cup immediately without training.

```
Input: image + "person. car. traffic light."
Output: bounding boxes for the corresponding objects
```

**Grounded-SAM**: GroundingDINO + SAM combined
→ text-prompted object detection + segmentation

Grounded-SAM is a practical combination for robotics. Feed in the text "red cup", and GroundingDINO finds the bounding box, then SAM produces a precise mask inside it. Because arbitrary objects can be detected and segmented without separate training, it is widely used in manipulation pipelines.

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

This is directly tied to problems encountered in the field. Classical SLAM relies on keypoints such as ORB and SIFT, but on textureless walls or floors, keypoints are hard to obtain. DINOv2's dense features carry semantic information, so even on a white wall one part can be distinguished from another. This is why SLAM robustness improves.

**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 perform well, but with hundreds of millions of parameters they are hard to run in real time without a GPU server. Robots, on the other hand, must run at 30 FPS on Jetson or embedded boards. 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
```

If you are using NVIDIA Jetson, TensorRT is almost mandatory. FP16 conversion alone yields a 2-3x speedup with almost no accuracy loss. Going to INT8 is faster still, but calibration data is required.

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

*If you want to become a researcher, start reading from here.*

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

Core idea: add 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: 294,912 || all params: 86,567,178 || trainable%: 0.34%
```

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

A common strategy is to attach a domain-specific automatic prompt generator to SAM's prompt 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 standard protocol for comparison in VFM adaptation research is the following.

| 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. A shared image-text embedding opens up zero-shot recognition. Trained on 400M image-text pairs. The beginning of the open-vocabulary era.
> - **2022**: Self-supervised pretraining methods such as Masked Autoencoders (MAE) begin to draw attention. DINO demonstrates the potential of self-supervised ViTs.
> - **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.
> - **What to watch now**: The core value of a foundation model is its **zero-shot ability**. Working in new environments and on new objects without additional training raises a robot's generalization. The CLIP+SAM+DINOv2 combination is used as a representative pipeline for open-vocabulary robot perception in NLMap, ConceptGraphs, and others. Making it lightweight (FastSAM, MobileSAM) and putting it on an actual robot completes the pipeline.

---

# 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, and you need the concepts in this chapter to understand "why ChatGPT cannot move a robot" and "why a policy that worked well in simulation falls apart on a real robot." At CoRL and ICRA 2024–2025, the share of VLA-related papers has grown sharply.

## 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) — the seminal VLA paper

## 12.2 Key Models and Research

### 12.2.1 RT-1, RT-2 (Google DeepMind)

RT-1 and RT-2 were the first models to empirically demonstrate the concept of "a general-purpose robot policy trained on large-scale data." Before RT-1, the standard approach was to train one task per robot. RT-1/RT-2 showed that a single model can perform hundreds of tasks.

**RT-1 (Robotics Transformer 1)**:
- Trained on large-scale robot demonstration data
- 130K episodes, 700+ tasks
- Tokenized action output

**RT-2 (Robotics Transformer 2)**:
- Fine-tunes a VLM (PaLI-X, PaLM-E) to produce robot actions
- Transfers web-scale knowledge to robots
- Capable of "chain of thought" reasoning

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

**Embodied Multimodal Language Model**:
- PaLM (language) + ViT (vision) + robot state
- 562B parameters
- "Multi-purpose" robot task execution

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

RT-2 and PaLM-E cannot be used without Google's internal infrastructure. OpenVLA is an open-source VLA model that a lab can actually download, fine-tune, and deploy on a robot.

**Open-source VLA**:
- 7B parameters (based on Llama 2)
- Trained on 970K episodes
- Applicable to diverse robot 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"
)
```

**RT-X project**: Another project to know alongside OpenVLA is RT-X. It collects robot data gathered by many research institutions into one large dataset (Open X-Embodiment) and uses it to train a general-purpose robot policy. The data covers more than 22 robot types.

**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**: Language-guided Indoor Navigation
**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

Unpacking SayCan's core idea a bit more: tell an LLM "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 right now, "grab a cup" is not executable. SayCan multiplies the LLM's plan (what should be done) with the robot's currently feasible actions (what can be done) to pick an action that is both executable and close to 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

Running tens of thousands of trial-and-error attempts on a real robot is nearly impossible in terms of time and cost. A world model lets the robot "run a simulation in its head" and decide actions based on the result. It is drawing particular attention in autonomous driving, because it can predict "what happens if the car ahead suddenly stops" without actually having to experience it.

**World Model**: a model that predicts how an environment behaves

**Why is it needed?**
- Enables model-based RL without a real robot
- Handles dangerous exploration inside simulation

**In autonomous driving**:
- **GAIA-1 (Wayve)**: video prediction + action conditioning. A generative model trained on real driving footage that predicts "this is the scene if you turn the wheel this way."
- **DriveDreamer**: driving-scenario generation. Uses text conditioning to generate diverse scenarios, applied to augmenting training data.
- **MILE**: end-to-end driving based on a world model. Trains an implicit world model that predicts future states and derives the driving policy from it.

**Structure**:

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

This is the first architectural decision to make when designing a robot system. Without understanding it, you cannot tell "why this system is designed the way it is" when reading a paper.

These are the two philosophies of robot system design.

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

**Recent end-to-end work in autonomous driving**:
- **UniAD (Unified Autonomous Driving, 2023)**: an end-to-end model that still keeps detection, tracking, mapping, prediction, and planning modules inside to preserve interpretability. CVPR 2023 Best Paper.
- **VAD (Vectorized Scene Representation for Efficient Autonomous Driving, 2023)**: converts scenes into a vectorized representation to train an efficient end-to-end driving policy.
- **GenAD (Generalized Autonomous Driving, 2024)**: an end-to-end system built on generative models that generalizes to diverse 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

**Recent trend**: hybrid approaches. Perception is learning-based while planning and control are model-based to ensure safety, and, as in UniAD, explicit modules sit inside an end-to-end framework.

> **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: the 2026 reality

End-to-end is the hot topic at conferences, but most robot systems actually deployed are modular. Why?

- **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**: when you want to improve only the perception module, a modular system lets you swap just that module. End-to-end requires retraining the whole thing.
- **Data efficiency**: end-to-end training needs large-scale data. RT-2 used 130k episodes, OpenVLA 970k. Most labs do not have the resources to collect data at that scale.

A realistic direction: **hybrid**. Perception is generalized with a VFM (foundation model), while planning/control stays modular for safety. The lab's Local/Global Module design (Ch.18) follows this direction.

For end-to-end to fully replace modular, two things must come first: a debuggable end-to-end structure, and few-shot policies that can learn from small-scale data. The safety-guarantee problem is also still open.

## 12.5 Spatial AI + VLA Integration

No matter how good a VLA model is, if it cannot avoid obstacles in real time the robot will run into a wall. Conversely, a robot that is only good at obstacle avoidance cannot carry out a complex command like "bring me coffee." A real robot system runs only when the two levels are integrated.

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. That is why sim-to-real — training first in simulation and transferring to the real robot — has effectively become the default. But a "reality gap" exists between simulation and reality. The main techniques for closing this gap 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.

**Major simulation platforms**:
- **NVIDIA Isaac Sim/Lab**: GPU-accelerated physics simulation. It can run thousands of environments in parallel, making it well suited for large-scale reinforcement learning. Isaac Lab is an integrated framework for robot learning research.
- **AI2-THOR (Allen Institute)**: an indoor-environment simulator. You can practice object manipulation in household settings like kitchens and living rooms. One of the most used platforms in embodied AI research.
- **Habitat (Meta)**: enables navigation learning in large-scale 3D-scanned environments (Matterport3D, Gibson, etc.). Provides an annual benchmark through the Habitat Challenge.
- **MuJoCo**: a simulator with strong contact physics. Widely used for robot-arm manipulation and locomotion learning. DeepMind acquired it and then 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

*If you want to become a researcher, start reading from here.*

In VLA and embodied AI, methods for learning a policy split broadly into reinforcement learning (RL) and imitation learning (IL). In robotics, IL is used far more often than RL. To understand why, you need to know the structure of each approach.

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

Simple and easy to implement, but it has a fatal problem: **distribution shift**. At training time it follows the expert's state distribution, but at inference time its own (imperfect) actions determine the next state. Small errors accumulate, the policy drifts into states the expert never visited, and there it does not know what to do.

**DAgger (Dataset Aggregation)**

A representative method for mitigating distribution shift. The core idea is to collect data with the learned policy while querying the expert for labels and adding them 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.

**Why is IL used more than RL in robotics?**

| Criterion | RL | IL |
|------|----|----|
| Sample efficiency | needs millions of episodes | hundreds to thousands of demos suffice |
| 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" (2024)](https://arxiv.org/abs/2309.15894) — a survey of IL

## 12.8 Advanced: Diffusion Policy

*If you want to become a researcher, start reading from here.*

Diffusion Policy, proposed by Chi et al. (RSS 2023), is a policy representation that is rapidly replacing BC-family methods in robot manipulation. The core idea is to generate an action trajectory through a denoising diffusion process.

**Why diffusion?**

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.

**How it works**

```
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]`. Only the first few steps are executed (receding horizon), and a new trajectory is generated from the next observation.

**Pros**:
- Represents multi-modal action distributions without explicit assumptions
- Generates an action sequence at once, producing temporally coherent behavior
- Training is stable (denoising score matching converges well)

**Cons**:
- Inference requires many denoising steps and is therefore slow (10–100 steps)
- May be unsuitable for real-time control (>100 Hz). Can be alleviated with acceleration techniques like DDIM or with consistency distillation

**Practical note**: In the experiments of Chi et al.'s original paper, Diffusion Policy beat BC-family methods on 11 of 12 continuous-action tasks. The gap is especially large on contact-rich insertion and assembly tasks.

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

*If you want to become a researcher, start reading from here.*

Section 12.6 briefly covered simulation platforms and domain randomization. Here we systematically lay out the concrete techniques for sim-to-real transfer.

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

DR's limitation: widening the randomization range too much makes training itself hard, while narrowing it too much fails to cover reality. Finding the right range is crucial in practice.

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

A recent approach that combines the strengths of DR and SysID.

```
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**: transfer succeeded. The simulator reflects reality well.
- **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" (2022)](https://arxiv.org/abs/2111.00137) — a systematic treatment of DR
> - [Hanna & Stone, "Grounded Action Transformation for Sim-to-Real" (AAAI 2017)](https://arxiv.org/abs/1511.07461) — 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
> - [Szot 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 draws attention in both autonomous driving and manipulation. End-to-end autonomous driving (UniAD, VAD, GenAD) starts to replace modular approaches
> - **What to watch now**: research applying foundation models to robots has grown rapidly since 2023 (RT-2, OpenVLA, Octo, pi0, etc.). You can fine-tune open-source models like OpenVLA/Octo on your own robot, so hands-on experimentation is recommended.

---

# 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 is the field that gives a robot spatial awareness. Point cloud processing, 3D object detection, and scene reconstruction fall under it. Neither SLAM nor robot manipulation is fully understandable without this material. It is the foundation of the next chapter (SLAM), so study it carefully.

## 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 multiplying each point by a 4×4 transformation matrix.

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

ICP (registration), surface reconstruction, lighting computation — nearly every 3D processing step requires normals. 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

The core idea of PointNet is to apply deep learning **directly to raw points** without converting the point cloud to voxels or images. Previously there was no answer to "how do we apply a CNN to irregular points?" — PointNet solved this.

**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's core idea: the result must be the same even if the point order changes (permutation invariance). To achieve this, each point is passed independently through an MLP and then aggregated via 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' core idea: if you divide 3D space into vertical pillars, you can compress the points inside each pillar into a single feature vector and arrange them like a 2D image. You can then reuse well-validated 2D CNNs as-is, which is much faster 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 landmark work 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**: the most widely used, 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
colmap mapper --database_path db.db --image_path ./images --output_path ./sparse
```

> **Further reading**
> - [COLMAP Documentation](https://colmap.github.io/) — the de facto standard tool for SfM/MVS.
> - [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

TSDF's core idea: each voxel stores "the signed distance to the nearest surface." Positive means outside the surface, negative means inside. Taking a weighted average of depths observed from multiple views reduces noise and yields a clean surface. The location where the sign flips (passes through zero) is the surface.

for i, (color, depth, pose) in enumerate(frames):
    rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(color, depth)
    volume.integrate(rgbd, intrinsic, np.linalg.inv(pose))

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 real-time 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 core idea.
> - [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 fixed NeRF's fatal weakness, slow rendering. NeRF takes seconds to render a single frame, whereas 3DGS renders in real time at 100+ FPS. This speed makes it directly usable in robot applications such as SLAM and real-time mapping.

**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 in real time.

- **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.14737) — 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) — a landmark for 3DGS + SLAM.
> - [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) — dense 3D reconstruction from image pairs without camera intrinsics/extrinsics. A paradigm shift for 3D reconstruction.
> - [Leroy et al., "MASt3R: Matching And Stereo 3D Reconstruction" (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

*If you want to become a researcher, read from here.*

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 landmark 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 σ(x) = max(-dΦ_s(f(x))/dt, 0) / Φ_s(f(x))
```

Here `Φ_s` is a sigmoid-like function controlled by a learnable parameter `s`. As training progresses, `s` shrinks and density concentrates near 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) — a landmark for occupancy-based approaches.

## 13.7 Advanced: Differentiable Rendering

*If you want to become a researcher, read from here.*

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 (opacity) at location `t`
- `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" (EGSR 2022)](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 want to become a researcher, read from here.*

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 key point is that it can handle queries like "red cup" that were never seen at training time.

**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.
> - **2012~**: the release of the Kinect popularized RGB-D-based 3D reconstruction. KinectFusion (TSDF + ICP) opened the door to real-time 3D reconstruction.
> - **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 can build photorealistic 3D maps in real time as they move.
> - **Worth watching now**: 3DGS-based methods are rapidly proliferating in SLAM/robotics applications. NeRFStudio lets you experiment with both, so I recommend comparing them directly.

---

# Ch.14 — SLAM & Odometry

The problem of a robot figuring out "where am I, and what does the surrounding environment look like?" at the same time, in an unfamiliar environment, is simultaneous localization and mapping (SLAM). When a robot moves autonomously in GPS-denied spaces — indoors, underground, inside a building — SLAM is not optional but essential. It is one of the skills most frequently required of a robotics software engineer, so both theory and practice need a solid foundation.

---

## Part 1. Foundations and Systems

### 14.1 Concept Introduction

Run a robot without a map and you feel it right away. A robot that does not know where it is cannot do anything. Navigation, obstacle avoidance, path planning — every one of them presupposes "current position" and "information about the surrounding environment."

**SLAM (simultaneous localization and mapping)**:
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_) — The canonical SLAM lecture series. If you are learning SLAM for the first time, watch this series.
> - [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 series covering Bayes filter through EKF/UKF/particle filter, Graph SLAM, and Robust SLAM. The most systematic Korean-language introduction to 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, it reduces to solving for the essential matrix or the fundamental matrix. 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. This is also why sufficient motion is required during initialization.

> **Further reading**
> - [Daniel Cremers — Multiple View Geometry (TUM)](https://www.youtube.com/playlist?list=PLTBdjV_4f-EJn6udZ34tht9EVIW7lbeo4) — The best resource for learning the mathematical foundations of 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) — The standard benchmark for RGB-D SLAM/VO.

### 14.3 Visual SLAM

#### 14.3.1 ORB-SLAM2/3

ORB-SLAM is the standard baseline for Visual SLAM. Most Visual SLAM papers compare against ORB-SLAM, and because the code is open-sourced you can build and run it yourself. If you are studying SLAM, doing this at least once is recommended.

**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)**: The first real-time monocular SLAM. It ran on an EKF, but suffered from compute blowup as the map grew.
- **PTAM (Parallel Tracking and Mapping, 2007)**: The first system to split tracking and mapping into separate threads. This architecture had a strong influence on 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

Run it yourself and you will see: with a camera alone, tracking easily fails under fast motion or in textureless environments. Combining an IMU keeps the system stable in these situations. It is the most widely used visual-inertial SLAM system on real 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)
```

Core contribution of VINS-Mono: with a technique called IMU preintegration, it compresses the hundreds of IMU measurements between two keyframes into a single relative transformation. You then no longer need to handle every IMU measurement during optimization; you just add the one compressed constraint. The gain in compute efficiency is substantial.

> **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 are sensitive to illumination and texture, whereas LiDAR measures 3D range directly and is free from these issues. In autonomous driving and outdoor robotics, LiDAR SLAM is effectively the standard.

#### 14.4.1 LOAM (Lidar Odometry and Mapping)

LOAM is the starting point of LiDAR SLAM. LeGO-LOAM, LIO-SAM, FAST-LIO, and nearly every LiDAR SLAM system that followed inherits or extends LOAM's ideas.

- Classify edge points and planar points
- Minimize point-to-edge and point-to-plane distances
- Separate odometry and mapping (at different rates)

It uses only the geometrically meaningful points (corners and planes) extracted from the point cloud. Matching every point is slow and fragile against noise, but picking only edge/planar points is both fast and accurate.

#### 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 and ORB-SLAM3.
> - [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**:
- Kalman filter based (instead of optimization)
- 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). It does not solve an optimization problem, just filtering, so compute is much lighter. It also uses an incremental KD-tree called ikd-Tree, which keeps insertion of new points into the map fast.

> **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.14709) — 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** puts the reprojection error of camera keypoints and the acceleration/angular-velocity measurements of the IMU into a single cost function and optimizes them jointly (VINS-Mono, MSCKF). More accurate, but more complex to implement.

**IMU Preintegration**:
Pre-integrate IMU measurements between two keyframes to compute a relative transformation. Optimization can proceed without relinearization.

#### 14.5.2 LiDAR + IMU (LIO)

A LiDAR scans at 10–20 Hz, but if the robot moves fast it travels within a single scan (motion distortion). The basic structure of LIO is to de-skew the within-scan motion using an IMU that measures at 200–400 Hz, then perform precise matching with the LiDAR. This is why LIO beats LiDAR-only in high-speed motion.

#### 14.5.3 Camera + LiDAR + IMU

**Recent trend**: integrate all sensors
- Examples: R3LIVE, LVI-SAM
- Exploits each sensor's strengths

R3LIVE combines LiDAR (geometric information) + camera (texture/color information) + IMU (high-speed motion compensation). It produces not just accurate pose estimation but also a colored, high-density 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

Run SLAM and you will see the map twist more and more over time. The robot walks a large loop back to the starting point, but on the map the start and the end no longer align. Loop closure is the core mechanism that corrects this twist. Without it, SLAM in large-scale environments is essentially impossible.

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

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.

---

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

*If you want to become a researcher, start reading here.*

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

Before looking at complicated equations, remember one thing: the SLAM back-end ultimately **solves 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 it is nonlinear you cannot solve it in one shot; you linearize around the current estimate and update iteratively. This loop of "linearize → solve Ax=b → update → repeat" is Gauss-Newton.

(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

The state variable in SLAM (a pose) lives on SE(3). SE(3) is not a Euclidean space but a Lie group, so you cannot simply apply the usual Gauss-Newton update `x ← x + δx`. Adding a vector to a rotation matrix no longer produces a rotation matrix.

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/∂T · ∂T/∂δξ`, where `∂T/∂δξ` is the left Jacobian of SE(3).

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

The system you actually solve now scales with the pose count `m` only, independent of `n`. This is why BA handles tens of thousands of landmarks while maintaining near-real-time performance.

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 the matrix `H` is **sparse**. Each pose has constraint relations only with temporally adjacent poses and with poses connected by loop closure. Even if there are 1000 poses in total, each pose is connected to at most a few or a few tens of others.

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): The most widely used heuristic. It eliminates the least-connected variables first.
- **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 like g2o, GTSAM, and Ceres, you have to choose a linear solver type (DENSE_SCHUR, SPARSE_NORMAL_CHOLESKY, and so on) and an ordering strategy. Running with the default settings, without this background, produces the kind of inefficiency that ends with "it was slow so I switched libraries." Changing the ordering alone can make a 10× or greater speed difference.

```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, VINS-Fusion, research |
| **Ceres Solver** | General-purpose nonlinear least squares, supports auto-diff, developed by Google | Cartographer, 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.
> - [CMU 16-833 Robot Localization and Mapping Lecture Notes](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

*If you want to become a researcher, start reading here.*

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, and if you put all of them into optimization as state variables the problem size explodes.

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

Key point: 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**: The earlier approach interpolated IMU measurements to the nearest keyframe timestamp. But rotation lives on SO(3), so simple linear interpolation is not accurate. Integrating ahead of time on the Lie group 1) accumulates rotation in a mathematically correct way, and 2) produces a result that plugs directly into a factor graph as a relative motion measurement. This 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**: Uses the IMU only as an initial guess for the next pose. LiDAR odometry and the IMU estimate state independently and are combined later based on covariance. LeGO-LOAM follows this approach.
- **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 preintegration paper. 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) — Earlier version of the paper above. The core idea is organized 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

*If you want to become a researcher, start reading here.*

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, you must initialize by **moving in diverse directions**. If you only walk in one direction, IMU bias estimation does not come out correctly.
- 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., "Observability-constrained 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?

This is a long-standing debate in SLAM/VIO. Here is the bottom line up front: mathematically, Gauss-Newton optimization and the iterated EKF (IEKF) are equivalent. They solve the same problem from different angles.

- **Filter (EKF, MSCKF, and so on)**: Updates state incrementally as new measurements arrive. Past states are marginalized out and only the current state is kept. Memory-efficient, and natural to combine with proprioceptive sensors (IMU).
- **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).

So is "VINS-Mono (optimization) being better than MSCKF (filter)" a matter of the solver? No. The difference comes not from the solver but from the **system structure** (which states are kept, which measurements are used). The practical edge of optimization-based systems is that they can 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

*If you want to become a researcher, start reading here.*

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

*If you want to become a researcher, start reading here.*

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 every robot's sensor data or local map to a central server that runs the full SLAM. It is simple to implement and stays close to the optimum, but sending all the raw data makes communication bandwidth the bottleneck, and the server becomes a single point of failure.

The **distributed approach** has each robot run local SLAM independently, and when a rendezvous or inter-robot loop closure occurs, it estimates relative poses to align the maps. Instead of raw data, robots exchange compressed descriptors (NetVLAD vectors, summary maps, and so on), which gives strong communication efficiency. Each robot optimizes only its own poses while constraints with neighboring robots drive the whole system toward convergence.

A distributed system has three problems it must solve. **Inter-robot loop closure** is when robot B later recognizes a place that robot A visited — the place recognition of Section 14.14 is the core. **Coordinate frame alignment** is needed because each robot starts in its own coordinate frame; the relative SE(3) transformation must be estimated from at least 3 inter-robot correspondences. **Outlier rejection** is required because inter-robot loop closures can produce many false positives, so robust methods such as PCM (Pairwise Consistency Maximization) or GNC (Graduated Non-Convexity) are needed. For distributed optimization, Distributed Gauss-Seidel, ADMM, and the like are used.

**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" (ICRA 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

*If you want to become a researcher, start reading here.*

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 and is more robust than BoVW. **NetVLAD** (2016) combines CNN features with VLAD aggregation and significantly outperformed prior methods on city-scale place recognition. **CosPlace** (2022) simplified the training pipeline with contrastive learning while raising performance. **MixVPR** (2023) uses feature mixing to stay robust across diverse conditions such as day/night and seasonal change. **AnyLoc** (2023) leverages DINOv2 features and delivered zero-shot place recognition that works indoors/outdoors and for aerial/ground without any fine-tuning.

**LiDAR-based place recognition**:

Recognize places from 3D structure alone, with no visual information. Fully immune to illumination changes, but can be confused in structurally similar environments (long corridors, for instance).

- **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: most SLAM systems use DBoW2 by default. If you have to operate in environments with large illumination/seasonal changes, consider swapping in a learning-based method (anything post-NetVLAD). AnyLoc has a low barrier to entry because it can be used without fine-tuning.

> **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**: LSD-SLAM, SVO, and other direct methods emerged. LOAM (2014) laid the groundwork for LiDAR SLAM. ORB-SLAM (2015) became the definitive feature-based Visual SLAM.
> - **2015–2020**: The visual-inertial era. VIO systems such as VINS-Mono (2018) and MSCKF became the standard on drones and mobile platforms. DSO (2018) proposed direct sparse. LiDAR-inertial integration took off in earnest: LIO-SAM (2020).
> - **2020–2023**: FAST-LIO/FAST-LIO2 (2021/2022) became the new standard for lightweight LiDAR-inertial systems. ORB-SLAM3 (2021) added visual-inertial and multi-map support. DROID-SLAM (2021) showed the potential of learning-based SLAM. Multi-sensor integrated systems like R3LIVE emerged.
> - **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.
> - **What to watch now**: Existing geometric SLAM (ORB-SLAM3, FAST-LIO2) is already mature technology, so master it; track 3DGS-SLAM and learning-based methods as trends. In practice, LIO-SAM/FAST-LIO2 (outdoor) and ORB-SLAM3 (indoor) are still used the most. Running them yourself on benchmark datasets (KITTI, EuRoC, TUM RGB-D) is the fastest way to learn.

### 14.15 Advanced: Long-term Mapping

*If you want to become a researcher, start reading here.*

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): Static/dynamic classification using multi-resolution range images. Projects a point cloud into a range image and compares against ranges observed from other viewpoints to decide whether each point is dynamic. It conservatively secures static points first, then restores falsely removed points — a two-stage design. The key point is that multiple confidence levels let you tune the trade-off.

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, and removes a surfel only when it is judged dynamic by both semantics and motion. It does not just erase everything because, in motion-degenerate environments, there can be points that are dynamic yet geometrically useful.

#### 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. Distinguishes high-dynamic, low-dynamic, weak non-dynamic, and strong positive-dynamic by the degree of change, 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.

---

# Ch.15 — Robot Frameworks

The moment that trips up most people writing robot software for the first time is realizing that sensor drivers, path planning, and motor control all have to run inside a single program. Frameworks solve this problem structurally. Build a robot without a framework and even "read sensor data → decide → send motor command" forces you to implement thread management, message serialization, and coordinate transforms yourself; if you do not know this, you hit a wall on your first project. This chapter centers on ROS, the de facto industry standard, and covers simulators and surrounding tools broadly.

## 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 has to run dozens of modules concurrently (cameras, LiDAR, motors, controllers), and ROS's core role is unifying how these modules exchange data. Without ROS you start from socket programming, and most of your research time evaporates into infrastructure work.

### 15.1.1 ROS1 vs ROS2

| Feature | ROS1 | ROS2 |
| --- | --- | --- |
| Release | 2007 | 2017 |
| 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**: ROS2 (Humble)

However, many packages still support only ROS1, so choose based on the situation.

**ROS1 → ROS2 migration status**: As of 2024, official support for ROS1 Noetic reached end of life (EOL). For new projects, start with ROS2 unless there is a specific reason not to. If you must use an existing ROS1 package, `ros1_bridge` lets you run nodes from both side by side. Core packages like Nav2 and MoveIt2 have been fully ported to ROS2, so ROS2 alone suffices for most robot development.

> **Further reading**
> - [ROS2 Official Tutorials](https://docs.ros.org/en/humble/Tutorials.html) — Official step-by-step guide based on ROS2 Humble. If it's 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/humble/How-To-Guides/Migrating-from-ROS1.html) — Official guide for porting existing ROS1 code

### 15.1.2 Core Concepts

These concepts form the skeleton of ROS. Without a precise grasp of the difference between Topic, Service, and Action, you get stuck every time on the question "how should I send this sensor data?" Building intuition for when each one fits is what matters.

**Node**:
- An executable 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**:
- Synchronous request/response
- Suited to one-shot operations
- Examples: changing settings, querying state

**Action**:
- Asynchronous goal-directed task
- Provides feedback
- Cancelable
- Examples: navigation, manipulation

In short: Topic is for continuously flowing data like camera images, Service is for situations like "tell me the current battery level" where you ask once and get an answer, and Action is for time-consuming tasks like "go over there". If you cannot tell these three apart, your system design keeps getting tangled.

**Parameter**:
- Node configuration values
- Changeable at runtime

> **⚠ AI agent caveat**: When asking an AI to write ROS2 code, specify the QoS settings explicitly. The AI will default to omitting QoS, and sensor topics silently fail to arrive, leaving you stuck for a long time.

> **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
from tf2_ros import Buffer, TransformListener

tf_buffer = Buffer()
tf_listener = TransformListener(tf_buffer, self)

# Look up base_link → camera_link transform
transform = tf_buffer.lookup_transform('base_link', 'camera_link', rclpy.time.Time())
```

> **Further reading**
> - [ROS2 tf2 Tutorials](https://docs.ros.org/en/humble/Tutorials/Intermediate/Tf2/Tf2-Main.html) — Official coordinate transform tutorial
> - [The Construct - rviz2 Complete Guide](https://www.youtube.com/@TheConstruct) — Video 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 real power of ROS lies in the package ecosystem the community has built. You will use each of the packages below at least once in nearly every robot project, so at least remember the names.

| 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. Testing thoroughly in a simulator first and then moving to hardware is safer and more efficient. In particular, for learning methods like reinforcement learning that require tens of thousands of episodes, research is effectively impossible without a simulator.

Recently, as **embodied AI** research has grown rapidly, 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

A physics simulation environment that integrates with ROS.

Gazebo is the simulator most tightly integrated with ROS. Most simulation demos for ROS projects are provided in Gazebo, so if you are using ROS you need to know Gazebo.

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

Widely used in embodied AI research for large-scale synthetic data generation and sim-to-real training. It combines photorealistic rendering with an accurate physics engine so that policies learned in the simulator also work on real robots.

**Features**:
- Photorealistic graphics (RTX rendering)
- Accurate physics simulation (PhysX 5)
- Synthetic data generation (domain randomization)
- ROS2 integration

**Primary uses**:
- Manipulation research
- Large-scale synthetic data generation
- Sim-to-real training

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

A simulator for autonomous driving research.

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/@intaborlado5265) — 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

*If you want to become a researcher, start reading here.*

**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
- If any segment exceeds its budget the whole system fails. 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
- Hardware-in-the-Loop (HIL): test new code while replaying real sensor data
- 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 ─── ROS1 released (Willow Garage)
  │       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

In robotics research, it is common to spend more time on "environment setup" than on the algorithms themselves. You lose a day to a CUDA version mismatch, or you clone someone else's code and it does not run because of a Python version difference — everyone hits this at least once. The tools introduced here minimize that kind of grind. They are not flashy, but they directly affect research productivity. Practical knowledge.

## 16.1 Programming Languages

AI coding agents now handle a substantial share of code writing. What matters is no longer the ability to write code from scratch but the **ability to read and understand existing code**. You need to clone someone else's research code and grasp its structure, verify code generated by AI, and judge where to fix things 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

> **⚠ AI agent caveat**: AI-generated C++ code often builds on x86 but fails on Jetson (ARM). Tell the agent about your cross-compile environment or target architecture.

### 16.1.2 Python

**Use cases**: prototyping, deep-learning training/inference, data analysis, visualization

Used for PyTorch training scripts, data preprocessing, and similar tasks. You run into it often in research, but it is also the language AI agents handle best, so the share you write by hand keeps shrinking. Being able to read and understand it is enough.

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

Robotics development is effectively done on Ubuntu. ROS treats Ubuntu as its primary supported platform, and GPU driver, CUDA, and cuDNN compatibility are best validated there. Some development is possible on macOS or Windows, but when you finally put code onto a real robot, you end up back on Ubuntu.

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

Training deep-learning models on CPU is not realistically feasible. CUDA is required for GPU acceleration, and if the PyTorch and CUDA versions do not match, you get an error on the very first `import torch`. This is one of the most common environment problems robotics researchers hit.

**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 whether Secure Boot is on (turn it off in the BIOS); (2) check driver module status with `sudo dkms status`. (Reference: [Jinyong Jeong's blog](https://jinyongjeong.github.io/2016/11/22/ubuntu_graphic_driver_install/))

**CUDA/cuDNN version compatibility**

If PyTorch and the CUDA version do not match, a single `import torch` 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)"

# All three versions must be compatible. 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 cmake git libgtk2.0-dev pkg-config \
    libavcodec-dev libavformat-dev libswscale-dev \
    libtbb-dev libjpeg-dev libpng-dev

# 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
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"  \  # adjust to your GPU (RTX 3090=8.6, RTX 4090=8.9)
      -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.19, AI agent 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
conda install pytorch torchvision pytorch-cuda=12.1 -c pytorch -c nvidia
```

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

The line you hear most often when sharing code in a lab is "it works on my machine…". Docker solves that. It packages the OS, libraries, and environment settings as a whole, guaranteeing the same execution environment anywhere. Reproducing a paper's code is much smoother when a Docker image is provided.

- **Reproducibility**: identical environment guaranteed
- **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. To run deep-learning training or CUDA-based computation, install nvidia-container-toolkit and use the `--gpus all` flag. If you use Docker for robotics research, you will need this almost 100% of the time.

Note: the previously used `nvidia-docker2` is deprecated. The current standard is `nvidia-container-toolkit`, and you use `--gpus all` instead of `--runtime=nvidia`.

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

When using Docker in robotics, you almost always need GPU, GUI (RViz/Gazebo), and USB sensors all at once. Adding them one at a time causes conflicts; setting everything up in one shot is better.

[turlucode/ros-docker-gui](https://github.com/turlucode/ros-docker-gui) organizes this combination well; use it as a reference. What follows is a recipe based on its structure, adapted to the current environment (nvidia-container-toolkit + ROS2 Humble).

**Step 1: Prepare X11 forwarding (host)**

```bash
# Run once on the host
sudo apt-get install -y xauth
xhost +local:docker
```

`xhost +local:docker` allows X server access only from Docker containers. Safer than `xhost +` (allow all), but in security-sensitive environments `xauth`-based authentication is the right choice.

**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. Without it you get `cannot open display` |

**Step 3: Save the container after work**

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

> **AI agent caveat**: when asking AI about Docker setup, tell it up front whether you will use "ROS2 + GPU + USB sensors + GUI visualization" all at once. Asking about each separately yields settings that conflict with each other. `QT_X11_NO_MITSHM=1` and `ROS_DOMAIN_ID` are items AI typically omits.

## 16.4 Remote Management: Git, SSH, File Transfer

A typical day is SSHing into the lab server to run experiments, managing code with Git, and moving data between servers. Learning the tools here makes this process smooth.

### 16.4.1 Git/GitHub

### 16.4.1.1 Basic Workflow

Without version control, "the code that worked yesterday, why not today?" repeats itself. Git tracks every change, so you can always go back to a previous state. Get in the habit of managing even research code with Git.

```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 in a lab modify the same code and push directly to `main` with no branching strategy, conflicts never stop. For research code, GitHub Flow is enough. Get in the habit of creating one branch per feature and merging via PR.

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

**Features**:
- Experiment logging and visualization
- Hyperparameter tracking
- Model version control
- Team collaboration

```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
  │
2010 ─── Conda (Anaconda) appears
  │       establishes itself as the standard for Python environment management
  │
2013 ─── Docker released
  │       container-based virtualization solves the reproducibility problem
  │
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 ─── Ruff appears (2022), Black goes mainstream
  │       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

In robotics and computer vision research, datasets are as important as algorithms. Without good data you cannot build good models, and without fair benchmarks you cannot prove in a paper that your method is genuinely better. This chapter surveys the structure and characteristics of the major datasets, along with methods for collecting and managing your own 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

A long-standing dataset that became the starting point for autonomous driving research.

Since its release in 2012, KITTI has served as the de facto standard benchmark for autonomous driving and 3D vision research. Larger and more diverse datasets exist today, but many papers still report KITTI results, so you need to know it as a reference point. For Visual Odometry and Stereo Depth Estimation in particular, KITTI is still the primary benchmark.

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

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

Google's large-scale autonomous driving dataset.

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 VIO / VINS

If you work on Visual-Inertial Odometry (VIO) or SLAM, you need to know the datasets below. Papers in this area almost always report results on them.

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

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

The standard for object detection and segmentation.

If you work on object detection, you need to understand the COCO dataset's evaluation metric (COCO mAP), since it is the industry standard. Note that the way AP is computed per IoU threshold differs from PASCAL VOC.

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

The standard pattern for data loading in PyTorch.

Without knowing PyTorch's `Dataset` and `DataLoader` pattern you cannot write training code. How you preprocess data in `__getitem__` and what you set `num_workers` to can change training speed significantly.

```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, filtering is only possible at reception time:**

```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 corresponds to the 3×3 camera matrix K, and the extrinsic corresponds to the 4×4 transformation matrix [R|t].

> **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          │ │
│  │ • 10-100 Hz         │     │ • 1-10 Hz                   │ │
│  └─────────────────────┘     └─────────────────────────────┘ │
└──────────────────────────────────────────────────────────────┘
```

**Why two modules? — an intuitive view**

You might think, "Can't we just put one good computer on the robot?" Honestly, if that were possible we would do it. But it is not.

First, consider the **physical constraints**. A robot has to move. You cannot mount an NVIDIA A100 GPU server on a drone — the weight alone is tens of kilograms, and it draws hundreds of watts. That is unrealistic for a battery-powered robot. So the computer actually carried on the robot is an embedded board like the Jetson Orin, and such a board cannot run large models like DINOv2 or SAM in real time.

Next are the **time constraints**. Suppose a robot is walking down a hallway, 0.1 seconds away from hitting a wall — it cannot pause and say "wait, waiting for the server to respond...". Things that must react "right now," like obstacle avoidance, are a different kind of problem from "understanding what that object is," which can afford to be slower.

So we split it this way:

1. **The reality of compute**: on-board computers on the robot (Jetson, etc.) do not have the headroom to run large models
2. **Real-time requirements**: obstacle avoidance needs immediate reaction — 0.1 seconds is the difference between life and death
3. **Deep understanding**: VFMs and VLAs need heavy compute — judgments like "that is a broken glass, be careful"
4. **Mutual complementarity**: geometric precision (Local) + semantic understanding (Global) = a truly intelligent robot

> By analogy, the Local Module is the robot's **reflex nervous system**, and the Global Module is 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

This module runs directly on the robot in real time. It processes the minimum information the robot needs to move safely "in this moment."

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

**A real scenario**: say a delivery robot is passing through an apartment hallway and a child suddenly runs out. The Local Module detects the obstacle instantly through the depth sensor, locates itself via odometry, and computes an avoidance path within 0.05 seconds. It does not need to know whether it is a child or a dog — that is the Global Module's job. The Local Module only needs to know "something is in front, so avoid it."

### 18.2.2 Characteristics

- **Low latency**: 10-100 Hz operation (one processing pass every 10-100 ms)
- **Limited resources**: Jetson, embedded GPU — 15-30 W power envelope
- **Deterministic behavior**: predictable response time — "an answer within 50 ms even in the worst case"

### 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 — 2-5x speedup on NVIDIA GPUs

**Edge deployment**:

```bash
# TensorRT optimization example
trtexec --onnx=model.onnx --saveEngine=model.trt --fp16 --workspace=4096
```

> What is TensorRT: a tool that converts a PyTorch-trained model into a form optimized for NVIDIA GPUs. Switching to FP16 (half precision) halves the model size while keeping accuracy nearly intact. Running YOLO on a Jetson: without TensorRT, 5 FPS; with it, 30 FPS — on a real robot, that gap is the difference between "usable" and "not usable."

### 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 (100 Hz)
        pose = self.odometry.update(sensor_data.imu, sensor_data.lidar)

# 2. Obstacle detection (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**: in the code above, `process()` is called every time sensor data arrives. IMU data comes in at 100 Hz (100 times per second), camera images at 30 Hz. Every frame, 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

A high-level understanding module that runs on a server or in the cloud. Where the Local Module only gets as far as "something is in front," the Global Module understands "that is a broken glass, probably dropped by the owner in the living room."

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

- **High accuracy**: uses large VFMs — models with billions of parameters like DINOv2 and SAM2
- **Abundant compute**: GPU servers, cloud — GPUs at the level of RTX 4090 or A100
- **Non-real-time is acceptable**: 1-10 Hz — "updating once per second is fine"

### 18.3.3 Tech Stack

**Vision Foundation Models** (see Ch.11):
- DINOv2: dense feature extraction — produces a meaningful feature vector for every pixel in an image
- SAM2: open-vocabulary segmentation — point at "any object" and it gets segmented accurately
- 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. Open-vocabulary segmentation
        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)
```

**Reading it as a scenario**: whenever a keyframe arrives from the Local Module, `process_keyframe()` is called. DINOv2 pulls rich features from the image, SAM segments the objects, and these accumulate into the 3D Scene Graph and the Gaussian Map. Later, when the user asks "where is the red cup?", `query()` looks it up. This whole process taking about a second is fine — real-time safety is the Local Module's job.

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

**Another scenario — unstable communication**: the robot is working in an underground parking lot and WiFi drops. In this case it must run on the Local Module alone. It estimates position via odometry and, while avoiding obstacles, moves to the last waypoint it received. When WiFi is restored, it sends the accumulated data to the Global module in one batch and receives an updated plan. This kind of **graceful degradation** is very important on real robots.

### 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 — "that is a chair" should not later flip to "table"
    - **Prerequisites**: Ch.3 (optimization) and Ch.14 (SLAM, map management) required

---

# Ch.19 — Collaborating with AI Coding Agents

## 19.1 Why This Chapter Is Needed

AI coding agents (Claude, Copilot, ChatGPT, and so on) are powerful tools in general software development. Ask for a single function and you get usable code; paste an error message and they usually pin down the cause. Robotics is different. Hardware, OS, networking, and real-time constraints are tangled together, and "the code is right but it doesn't work" is an everyday situation. AI often gets things wrong in this territory, confidently proposes the wrong direction, or simply gives up.

This chapter is a guide for not getting fooled by AI and for putting it to proper use. Everything here comes from problems encountered in practice. Knowing the patterns where AI goes wrong in advance reduces wasted effort.

## 19.2 Things AI Frequently Gets Wrong in ROS

### 19.2.1 QoS Settings

AI almost always ignores QoS (Quality of Service) or leaves it at the default (RELIABLE). The problem is that sensor topics (cameras, LiDAR) are typically published as BEST_EFFORT. If the subscriber is RELIABLE, no data arrives at all. There is no error message — it just silently fails, so AI starts debugging in the wrong direction with "is the topic missing?"

```bash
# Check the topic's QoS profile
ros2 topic info /camera/image_raw --verbose
```

Check the output for information like `Reliability: BEST_EFFORT`, `Durability: VOLATILE`, and match the subscriber's QoS.

```python
from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy

When asking AI for code, explicitly state "this topic's QoS is BEST_EFFORT / SENSOR_DATA." Otherwise it will generate defaults, data won't arrive, and AI won't figure out the cause.

### 19.2.2 use_sim_time and tf2 Timing

If you play back a rosbag without `use_sim_time:=true`, every tf lookup fails. When AI sees a "tf2 lookup failed" error, it will most likely suggest adding a `static_transform_publisher`. Wrong direction.

The real cause is a mismatch between the simulation clock and the system clock. The bag file's timestamps are in the past, while the node queries tf based on the current system time, so of course it can't find anything.

```bash
# Correct rosbag playback
ros2 bag play my_bag --clock

# Enable sim_time when launching the node
ros2 launch my_package my_launch.py use_sim_time:=true
```

tf2 lookups need a timeout and try/except, and AI often leaves these out.

```python
from rclpy.duration import Duration

### 19.2.3 Workspace Sourcing Order

The sourcing order of ROS2 workspaces matters. Source `/opt/ros/humble/setup.bash` first, then `~/ros2_ws/install/setup.bash`. AI often sources only one, reverses the order, or doesn't know about overlay workspaces at all.

```bash
# Correct order
source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash
```

If it's in `.bashrc` but a new terminal can't find the package, AI will recommend reinstalling the package. Most of the time it's a `source` problem. Check the currently sourced workspaces with `echo $AMENT_PREFIX_PATH`.

### 19.2.4 Custom Messages and Build

AI is good at writing `.msg` files. But it often omits dependency additions to `CMakeLists.txt` and `package.xml`.

If `rosidl_generate_interfaces` is missing, the build succeeds but imports fail in Python. Faced with this error, AI is likely to misdiagnose it as "the package isn't installed."

```cmake
# Must be added to CMakeLists.txt
find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/MyCustomMsg.msg"
  DEPENDENCIES std_msgs geometry_msgs
)
```

```xml

<buildtool_depend>rosidl_default_generators</buildtool_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
```

One more: building without `--symlink-install` means Python code changes are not reflected. AI misdiagnoses this as "a cache problem."

```bash
# So Python package changes take effect immediately
colcon build --symlink-install
```

### 19.2.5 Namespaces and Remapping

If `ros2 topic echo /camera/image_raw` produces no data, AI is prone to diagnose it as a driver issue. But the topic name might actually be `/robot1/camera/image_raw` due to a namespace.

```bash
# Check the topic list first
ros2 topic list

# Filter by a specific pattern
ros2 topic list | grep camera
```

When letting AI debug, provide the output of `ros2 topic list` and `ros2 node list` first. Without this information, saying "the topic isn't coming through" leaves AI no choice but to guess.

### 19.2.6 Launch Files

Common mistakes AI makes when writing ROS2 Python launch files:

- Mixes in ROS1 XML syntax (ROS2 launch is Python by default)
- Orders `LaunchDescription` actions incorrectly (node dependencies must be considered)
- Can't distinguish `ComposableNode` from a regular `Node`
- Omits `PushRosNamespace`, tangling up multi-robot setups

```python
# Applying a namespace in a multi-robot launch file
from launch.actions import GroupAction, PushRosNamespace

robot1_group = GroupAction([
    PushRosNamespace('robot1'),
    Node(package='my_pkg', executable='my_node', name='sensor_node'),
])
```

When asking AI for a launch file, specify "ROS2 Python launch file," "apply namespaces if multi-robot," "whether to use ComposableNode," and so on.

## 19.3 Things AI Doesn't Know About Docker

### 19.3.1 GUI/Visualization Issues

To run GUI tools like RViz or Gazebo inside Docker, X11 forwarding is required. AI commonly recommends `xhost +local:docker`, but this allows X server access from all local connections and is a security risk.

The proper setup is:

```bash
docker run -it \
  --env DISPLAY=$DISPLAY \
  --env QT_X11_NO_MITSHM=1 \
  -v /tmp/.X11-unix:/tmp/.X11-unix:rw \
  --ipc=host \
  my_image
```

Roles of each option:
- `QT_X11_NO_MITSHM=1` — AI is almost never aware of this option. Without it, RViz dies with a segfault. The reason is that the MIT-SHM (shared memory) extension doesn't work properly inside Docker.
- `--ipc=host` — shares the IPC namespace with the host. Without it, visualization tools often die due to shared memory issues.
- On Wayland environments (default on Ubuntu 22.04+), mounting the X11 socket alone may not be enough. You may need to force an X11 session with `XDG_SESSION_TYPE=x11` or route through XWayland.

### 19.3.2 USB Device Passthrough

To use USB devices like cameras, LiDAR, or IMUs inside Docker, devices must be mapped explicitly. AI usually doesn't know this and says "install the driver."

```bash
# Map only specific devices (recommended)
docker run -it --device=/dev/ttyUSB0 --device=/dev/video0 my_image

# Allow access to all devices (not recommended for security; only for debugging)
docker run -it --privileged my_image
```

`--privileged` is convenient but gives the container almost every host permission, so in production you should map only the devices you need.

One more thing: if a USB device is plugged in after the Docker container starts, it won't be recognized. AI doesn't consider this situation at all. You have to restart the container, or use the `--privileged` + `-v /dev:/dev` combination.

### 19.3.3 ROS Networking

For ROS2 communication between Docker containers, `--network=host` is the simplest, but it shares all of the host's ports and risks port conflicts.

AI often doesn't know why ROS2 fails on a bridge network. The reason is that DDS (Data Distribution Service) uses multicast, and multicast is off by default on Docker bridge networks.

```bash
# Simplest approach (for dev environments)
docker run -it --network=host my_ros2_image

# Prevent conflicts with others via ROS_DOMAIN_ID
docker run -it --network=host -e ROS_DOMAIN_ID=42 my_ros2_image
```

`ROS_DOMAIN_ID` can conflict with another person's ROS2 on the same network. When multiple people use ROS2 simultaneously in a lab, each other's topics can become visible.

When DDS needs fine-grained configuration, restrict it to a specific network interface via a Cyclone DDS config XML:

```xml

<CycloneDDS>
  <Domain>
    <General>
      <NetworkInterfaceAddress>eth0</NetworkInterfaceAddress>
    </General>
  </Domain>
</CycloneDDS>
```

```bash
export CYCLONEDDS_URI=file:///path/to/cyclone_dds.xml
```

### 19.3.4 File Permission Issues

Files created inside Docker are owned by root by default. Editing or deleting them from the host requires `sudo`.

```bash
# Run as the host user
docker run -it --user $(id -u):$(id -g) my_image
```

But some ROS packages need root, and using `--user` breaks them. AI throws around `chmod 777` in this situation, which you should not do in practice. The correct approach is to create a non-root user in the Dockerfile and set permissions only on the directories that need them.

```dockerfile
# Non-root user setup in the Dockerfile
RUN useradd -m -s /bin/bash rosuser && \
    usermod -aG dialout rosuser
USER rosuser
```

## 19.4 Things AI Gives Up On in Hardware/Drivers

### 19.4.1 Serial Port Permissions

When `Permission denied` appears on `/dev/ttyUSB0`, AI recommends `sudo chmod 666 /dev/ttyUSB0`. It works, but resets on reboot. You can't type this every single time.

The correct approach is to write a udev rule:

```bash
# Check vendor / product IDs
udevadm info -a -n /dev/ttyUSB0 | grep -E 'idVendor|idProduct'
```

```bash
# /etc/udev/rules.d/99-sensors.rules
SUBSYSTEM=="tty", ATTRS{idVendor}=="1546", ATTRS{idProduct}=="01a9", MODE="0666", SYMLINK+="gps"
```

```bash
# Apply the udev rule
sudo udevadm control --reload-rules && sudo udevadm trigger
```

This way the USB device always gets the fixed name `/dev/gps` and permissions are set automatically. You can also distinguish multiple identical devices (e.g., two IMUs) by serial number. AI barely knows udev.

### 19.4.2 USB Bandwidth

Plug three USB3 cameras into the same USB hub and you get frame drops from bandwidth shortage. AI says "update the driver" or "lower the resolution," but the real cause is the bandwidth limit of the USB controller.

```bash
# Check which camera is attached to which USB controller
lsusb -t
```

This problem cannot be solved in software. You must physically spread the cameras across different USB controllers. On desktop PCs, the front-panel USB and rear-panel USB are often on different controllers, so check the Bus numbers in `lsusb -t` and distribute them.

### 19.4.3 LiDAR Connection (IP Configuration)

90% of "no data" problems with Velodyne or Ouster LiDARs are caused by network settings. AI recommends reinstalling the driver or rebuilding the ROS package, but there are things to check first.

LiDARs use a fixed IP (e.g., `192.168.1.201`). The host PC's Ethernet interface must be on the same subnet (e.g., `192.168.1.100`) for communication to work.

```bash
# Step 1: check if the LiDAR pings
ping 192.168.1.201

# Step 2: set the host Ethernet interface IP
sudo ip addr add 192.168.1.100/24 dev eth0
sudo ip link set eth0 up

# Step 3: check that UDP packets arrive via Wireshark
sudo tcpdump -i eth0 udp port 2368 -c 10
```

Most cases get stuck right at `ping`. Confirming UDP packet arrival with Wireshark (or tcpdump) is the most reliable debugging method. If packets arrive but ROS doesn't see them, only then is it time to suspect the driver.

### 19.4.4 Camera Drivers (v4l2)

AI only knows `cv2.VideoCapture(0)` and can't tell which `/dev/video*` is the actual camera when there are several. It's common for a single USB camera to expose `/dev/video0` and `/dev/video1` (a metadata device), and AI doesn't know this.

```bash
# Check camera device mapping
v4l2-ctl --list-devices

# Check supported formats and resolutions
v4l2-ctl -d /dev/video0 --list-formats-ext
```

Auto exposure, auto white balance — these automatic settings often wreck SLAM performance. Constantly changing brightness destabilizes feature extraction.

```bash
# Manual exposure settings (for SLAM)
v4l2-ctl -d /dev/video0 --set-ctrl=exposure_auto=1
v4l2-ctl -d /dev/video0 --set-ctrl=exposure_absolute=100

# Lock white balance
v4l2-ctl -d /dev/video0 --set-ctrl=white_balance_automatic=0
```

AI barely knows this kind of low-level camera control. When told "SLAM is unstable," it recommends tuning algorithm parameters, but disabling the camera's automatic settings alone can give dramatic improvements.

### 19.4.5 Jetson (ARM) Environment

AI-generated code and Docker configs are almost 100% x86-based. On NVIDIA Jetson (ARM64) they often don't run.

Points to watch:
- When installing via `pip install`, many packages have no pre-built binaries (wheels) for ARM. `scipy` and `opencv-python` in particular must be built from source, which can take tens of minutes.
- The JetPack version pins CUDA, cuDNN, and TensorRT versions. If AI recommends installing the latest version, the whole system falls apart.
- When using Docker, you must use NVIDIA's `l4t` (Linux for Tegra) based images.

```bash
# Docker image on Jetson — don't use x86 images
docker pull nvcr.io/nvidia/l4t-pytorch:r35.2.1-pth2.0-py3

# Check the JetPack version
cat /etc/nv_tegra_release
```

When asking AI for code, specify "Jetson Orin, JetPack 5.1.2, CUDA 11.4 environment."

### 19.4.6 Real-Time Control and Timing

AI tells you to build a 100Hz loop with `time.sleep(0.01)`, but this isn't accurate. The precision of `time.sleep()` depends on the OS scheduler, and on a generic Linux kernel there is jitter of several milliseconds.

```python
# What AI commonly recommends (not accurate)
import time
while True:
    do_control()
    time.sleep(0.01)  # can actually be 10-15 ms
```

Python's GIL (Global Interpreter Lock) further weakens multithreaded timing guarantees. For actual real-time control, use C++ and an RT (Real-Time) kernel (PREEMPT_RT).

```bash
# Check the actual publish rate — always verify with this
ros2 topic hz /cmd_vel
```

Even when AI says "100Hz control is fine," verify the actual rate with `ros2 topic hz`. If the expected rate differs from the actual rate, control won't work properly.

## 19.5 Patterns Where AI Agents Give Up

### 19.5.1 "It works in simulation"

Works fine in Gazebo but fails on the real robot. AI is prone to conclude "it works in simulation, so the code is fine and it must be a hardware problem." But the real cause is usually the sim-to-real gap:

- **Sensor noise**: simulated Gaussian noise and real sensor noise have different distributions
- **Communication latency**: topic delivery is instantaneous inside Gazebo, but in reality there is latency of a few to tens of ms
- **Timing mismatch**: simulation guarantees perfect synchronization, but in reality timestamps drift between sensors
- **Frame mismatch**: if the URDF and the real robot's sensor positions/angles differ slightly, tf goes off

Tell AI the sim-to-real differences concretely: "it works in simulation but not on the real robot. Sensor noise level is X, communication latency is Y ms, and the frame calibration was done with method Z."

### 19.5.2 Trying to Fix Hardware Problems in Software

Bad cables, loose connections, insufficient power — AI cannot diagnose these physical issues.

Say "sensor data drops out intermittently" and AI will recommend adjusting buffer sizes, setting timeouts, changing QoS. But it's often a loose USB cable or an underpowered USB hub.

```bash
# Find hardware clues in the kernel log
dmesg | tail -20

# Check USB disconnect / reconnect events
dmesg | grep -i usb | tail -20
```

If `dmesg` shows messages like `USB disconnect` or `device descriptor read/64, error -71`, it's not a software problem. Replacing the cable, using a powered USB hub, or plugging into a different port comes first.

### 19.5.3 Giving Up on Environment Diagnosis

When library version conflicts get tangled, AI says "reinstall everything." Checking versions with `pip show package_name` and narrowing down what conflicts comes first.

OpenCV-related conflicts in particular are a robotics classic:

- `opencv-python` (default)
- `opencv-python-headless` (server-side, no GUI)
- `opencv-contrib-python` (with extra modules)
- `cv_bridge` (ROS package, references its own OpenCV)

When these four are installed simultaneously, they conflict with each other.

```bash
# Check the currently installed OpenCV
pip show opencv-python opencv-python-headless opencv-contrib-python

# Fix: don't install pip opencv in a ROS environment
pip uninstall opencv-python opencv-python-headless opencv-contrib-python
sudo apt install ros-humble-cv-bridge
```

In a ROS environment, using only `apt install ros-humble-cv-bridge` and not installing opencv separately via pip is the cleanest approach.

### 19.5.4 "Gives up after 2-3 tries"

AI repeats the same approach two or three times, and when it fails, moves on with "try a different approach." Engineers don't give up here. They dig through logs, run strace, and capture packets.

To get better answers from AI, provide not just the error message but low-level information:

```bash
# System logs
dmesg | tail -30
journalctl -u my_service --since "5 minutes ago"

# Process tracing
strace -f -e trace=open,read,write ros2 run my_pkg my_node 2>&1 | head -100

# Network packet capture
sudo tcpdump -i eth0 -w capture.pcap
```

Provide this information to AI and you'll get answers closer to the real cause instead of "reinstall it."

## 19.6 How to Use AI Agents Properly

*The general frame for putting AI to work (writing/reading territory) is covered in depth in [`../../research-notes/chapter_07_keshav_three_passes.md`](../../research-notes/chapter_07_keshav_three_passes.md) and [`part2_writing/A_workflow/ch01_mindset.md`](../../research-notes/chapter_16_mindset.md) *(Korean; English version planned)*. This §19.6 is the field-specific application to code and hardware.*

### 19.6.1 Provide Enough Context

The most important thing when asking AI is the quantity and quality of context.

Wrong example: "the camera isn't working"

Right example: "Ubuntu 22.04, ROS2 Humble, Intel RealSense D435. It shows up in `rs-enumerate-devices`, but `ros2 launch realsense2_camera rs_launch.py` gives a `Could not open device` error. Running inside Docker, and `--device=/dev/video0` is mapped."

**Checklist of information to provide to AI**:
- OS version, ROS version
- Hardware platform (x86 vs ARM/Jetson)
- Sensor model name
- Full error message (don't copy just a piece — give the whole thing)
- Output of `ros2 topic list`, `ros2 node list`
- Whether Docker is used and the run options (the full `docker run` command)
- Network configuration (wired/wireless, IP range)

### 19.6.2 How to Verify AI's Answers

Before running AI's answers as-is, check the following:

- **"Install this package"** → first check that the package supports your ROS version and Ubuntu version. Check existence with `apt search ros-humble-<package_name>`.
- **"Change this setting"** → back up the current setting before changing, and ask AI for the rationale. If it can't explain why, be suspicious.
- **"Reinstall"** → 90% of the time you don't need to reinstall. First narrow down the exact cause of the error. Checking current state with `pip show`, `dpkg -l | grep`, `apt policy`, etc. comes first.
- **When AI gives you code** → check for hardcoded paths (`/home/user/...`), hardcoded IPs (`192.168.1.100`), x86-only packages (`amd64` wheels), and the like.

### 19.6.3 What AI Is Good At vs Bad At

| What AI is good at | What AI is bad at |
|---|---|
| Algorithm implementation (SLAM, detection, etc.) | Hardware debugging |
| Writing ROS2 node/service code | QoS/DDS tuning |
| Python/C++ refactoring | USB/serial permission issues |
| Reading/summarizing papers | Network configuration (LiDAR IPs, etc.) |
| Writing CMakeLists.txt | Real-time timing issues |
| Data preprocessing pipelines | Hardware access inside Docker |
| Visualization code (matplotlib, Open3D) | Sensor time synchronization in practice |
| Interpreting error messages (generic) | Debugging based on dmesg/kernel logs |

The structure is this: AI is strong in "pure software" territory but weak where hardware and software meet. And most robotics problems occur right at that boundary. The right move is to delegate AI-strong areas and, in AI-weak areas, do the debugging yourself and feed the results to AI for analysis.

## 19.7 Workflows for Using AI as a Research Tool

Section 19.6 covered AI's strengths and weaknesses. Building on that, this section looks at practical workflows for how a robotics researcher uses AI throughout a daily routine.

### 19.7.1 Reading Papers

*The paper-reading workflow (3-pass + AI layer cameo) is covered in depth in the AI-layer cameo of [`../../research-notes/chapter_07_keshav_three_passes.md`](../../research-notes/chapter_07_keshav_three_passes.md) *(Korean; English version planned)*.*

Field-specific application: 3-pass + AI summary on the field's core papers — read the abstract, ask for a 3-line contribution summary, ask for step-by-step derivation of equations.

### 19.7.2 Writing Code

- Prototyping: "write code that extracts ORB features from the KITTI dataset and matches them. Use OpenCV, with Lowe's ratio test at 0.75" — give concrete instructions like this
- Debugging: error message + code + "what causes this error" — AI's strong area
- Refactoring: "turn this code into a PyTorch Dataset class" — strong at structural transformations
- What AI can't do (see earlier in this chapter): ROS QoS, hardware permissions, network settings, real-time timing

### 19.7.3 Experiment Design

*The frame for putting AI to work in experiment design, ablations, and result interpretation is captured as a cameo in [`../../research-notes/chapter_32_revision_rebuttal.md`](../../research-notes/chapter_32_revision_rebuttal.md) (or [`ch12_figures.md`](../../research-notes/chapter_27_figures.md)) *(Korean; English version planned)*.*

Field-specific application: throw the baseline-comparison table at AI and ask *which comparison axis I missed* — that workflow.

### 19.7.4 Writing Papers

- Drafting: give AI the core idea and experimental results and ask "draft an Introduction" — useful for structuring
- Grammar/expression correction: fixing awkward expressions in English papers. AI understands context better than Grammarly
- Caveat: don't submit AI's sentences as-is. You must rewrite them in your own voice. Reviewers can spot AI-generated prose
- BibTeX generation: "make a BibTeX entry for this paper" — sometimes faster than copying from Google Scholar. But AI sometimes gets the year or venue wrong, so always verify

### 19.7.5 Example Daily Workflow

A concrete scenario for how to use AI in a daily routine:

```
09:00 — Check 3 new papers on arXiv. Ask AI for a 1-sentence summary of each
09:30 — Pick one interesting paper, second-pass read. Ask AI to derive unfamiliar equations
10:30 — Analyze yesterday's training results. Screenshot the loss curve and ask AI "is this pattern normal?"
11:00 — Write new experiment code. Have AI generate the DataLoader structure. Manually fix the augmentation logic
14:00 — Debug SLAM code. ROS2 error → solve directly using this chapter (AI doesn't know QoS)
16:00 — Draft the Related Work section. Have AI build a comparison table for 5 papers
17:00 — Verify the table. Find that AI confused the methods of 2 papers, fix manually
```

---

# Ch.20 — Further Reading

A curated list of textbooks, courses, papers, and learning paths for starting robotics research. If there is too much material and you do not know where to look, start with the **Learning Path** section at the bottom.

## 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 — honestly painful to read cover to cover, but you can pick the chapters you need
- 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** — a blessing for students
- 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 PDF: 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)
- The foundation of deep learning vision — almost everyone starting in this field watches it
- 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: https://github.com/slam-kr

### 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
- Extremely intuitive explanations, paired with code
- YouTube: https://www.youtube.com/playlist?list=PLAqhIrjkxbuWI23v9cThsA9GvCAUhRvKZ

## 20.3 Recommended YouTube Channels

These are YouTube channels you can watch more casually than textbooks or full courses. Play them on your commute, during meals, or on breaks, and the intuition accumulates.

| Channel | Topic | Notes |
| --- | --- | --- |
| **Cyrill Stachniss** | SLAM, Robotics | The canonical SLAM lectures. Undergraduate-class level of systematic explanation |
| **First Principles of Computer Vision** (Shree Nayar) | Computer Vision | A Columbia professor walks through CV fundamentals one by one. Genuinely approachable |
| **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

The reading mindset (why we read, Keshav's 3-pass method, the 5 Cs, reading-as-reviewer, the CCC lens, diagnosing reader expectations) is covered in depth in a separate meta-skill guide — [`../../research-notes/part1_reading/`](../../research-notes/part1_reading/) (ch01–ch07, seven chapters) *(Korean; English version planned)*.

### 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.14737](https://arxiv.org/abs/2308.14737)
- 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/), [Papers With Code](https://paperswithcode.com/), and [arXiv](https://arxiv.org/). Papers With Code is especially convenient because it shows benchmark rankings alongside code links.

### Paper-writing tools

> **Further reading**
> - [Overleaf](https://www.overleaf.com/) — online LaTeX editor. The de facto standard for collaborative paper writing
> - [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 reference tables of conferences by field live in the meta-skill guide — see the *Conferences by field* section at the end of [`../../research-notes/chapter_34_conference_prep.md`](../../research-notes/chapter_34_conference_prep.md), which covers schedules and character of CV, robotics, and autonomous-driving venues *(Korean; English version planned)*.

This guide focuses on the SLAM/CV/robotics field core. The general theory of conferences (why attend, presentation openers, first-line craft) and the field reference tables are handled in the guide above.

## 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 |
| --- | --- | --- |
| Understanding and using VFMs | 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 | [Papers With Code](https://paperswithcode.com/) + [Yannic Kilcher's channel](https://www.youtube.com/@YannicKilcher) — *the full guide is [`../../research-notes/part1_reading/`](../../research-notes/part1_reading/)* |
| Experimenting with new ideas | Hypothesis formulation, experimental design | Lab seminars + attending conference workshops — *the full guide is [`../../research-notes/part0_starting/`](../../research-notes/part0_starting/)* |
| Benchmark evaluation | Quantitative comparison | Standard benchmarks per subfield (KITTI, ScanNet, Replica, etc.) — *the result-interpretation frame is [`../../research-notes/part2_writing/E_after/`](../../research-notes/part2_writing/E_after/)* |

**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/part2_writing/`](../../research-notes/part2_writing/)*
- 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.*

The research-skill content of this section — researcher mindset, paper writing, experimental design, presentations, peer review, paper-writing tools — is covered in depth in a separate meta-skill guide *(Korean; English version planned)*:

- Researcher mindset (engineer-as-identity, optimization horizon, direction-engine-tools, sustainable growth) → [`../../research-notes/part0_starting/`](../../research-notes/part0_starting/) (5 chapters)
- Paper reading → [`../../research-notes/part1_reading/`](../../research-notes/part1_reading/) (7 chapters; see also §20.4 above)
- Paper writing (structure, sentence craft, revision workflow) → [`../../research-notes/part2_writing/`](../../research-notes/part2_writing/) (workflow · structure · sections · sentence · after-submission)
- Conference presentations and peer review → [`../../research-notes/part3_presentations/`](../../research-notes/part3_presentations/) (3 chapters; see also §20.5 above)

Field-specific notes for SLAM/CV/robotics:

- *Experimental design.* Ablation removes components one at a time. Vary one variable at a time. Repeat at least 3 times and report mean/std. Compare on the same data, same split, same hardware — copying numbers from other papers often hides condition mismatches.
- *Tooling.* LaTeX on Overleaf or local (texlive + VS Code). Reference management: PDF reader + AI for under 100 papers; Zotero + Better BibTeX once the count grows. Pipeline figures with TikZ (precise) or draw.io (fast); tables with `booktabs`; algorithms with `algorithm2e`. Build a notation table and apply it across the whole paper.

> Classic external references (kept here as field-agnostic must-reads):
> - [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 with 1 cm of error at every step yields 100 cm of error on arrival.

**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: The core of lab code is C++. SLAM, ROS packages, and real-time control modules are all written in C++, and you will frequently read and modify this code. Python is used for deep-learning scripts and data preprocessing, but AI coding agents handle this area well, so the need to master it yourself has diminished. For both, "the ability to read and understand code" is the core, and writing can be done in collaboration with AI.

**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), [Google Scholar](https://scholar.google.com/) (paper search), and [Papers With Code](https://paperswithcode.com/) (paper search with code). 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: combining two fields that have not yet been merged, like "3D Gaussian Splatting + Semantic SLAM".

**Q: Which GPU should I buy?**

A: The most important spec in GPU selection is **VRAM**. If the model does not fit in VRAM, you cannot run it at all. Next is compute speed (TFLOPS), which directly affects training time.

**Personal (desktop)**

| VRAM | Card | FP32 TFLOPS | FP16 TFLOPS | Use case |
|------|------|-------------|-------------|------|
| 8GB | RTX 4060 | 15.1 | 15.1 | YOLOv8, ResNet training, small-scale fine-tuning |
| 8GB | RTX 5060 Ti 8GB | ~30 | ~30 | 4070-class compute, but 8GB VRAM is tight for VFMs |
| 12GB | RTX 3060 12GB | 12.7 | 12.7 | Compute is slow, but 12GB VRAM is unique at this price point. Cheap on the used market. Student entry-level |
| 12GB | RTX 4070 | 29.1 | 29.1 | Depth Anything, SegFormer. SAM is just barely possible at batch 1 |
| 16GB | RTX 5060 Ti 16GB | ~30 | ~30 | SAM, DINOv2 inference. Mid-scale training. **Best value recommendation** |
| 16GB | RTX 4070 Ti Super | 44.1 | 44.1 | Same VRAM as above, 1.5× compute speed |
| 24GB | RTX 3090 (used) | 35.6 | 35.6 | Cheapest way to secure 24GB of VRAM. Training works, just slow |
| 24GB | RTX 4090 | 82.6 | 82.6 | Top of the personal tier. VLA fine-tuning, large 3DGS scenes |
| 32GB | RTX 5090 | 104.8 | 209.6 | Maximum VRAM for personal use. 2.5× the 4090 in FP16 |

**Server/lab (data center)**

| VRAM | Card | FP32 TFLOPS | TF32 TFLOPS | BF16 TFLOPS | Characteristics |
|------|------|-------------|-------------|-------------|------|
| 16/32GB | V100 SXM2 | 15.7 | — | — | 1st-gen Tensor Cores. No TF32. Still active in many labs. Can be bought cheaply used |
| 24GB | A10 | 31.2 | 62.5 | 125.0 | For inference servers. Slow for training |
| 40/80GB | A100 SXM | 19.5 | 156 | 312 | FP32 is slow, but overwhelming in TF32/BF16. Multi-GPU scaling via NVLink |
| 80GB | H100 SXM | 66.9 | 989 | 1979 | 6× A100 in TF32, 6× in BF16. Transformer Engine support |
| 80GB | H200 SXM | 66.9 | 989 | 1979 | Same compute as H100, 1.5× memory bandwidth via HBM3e |
| 141GB | B200 | 90 | 2250 | 4500 | Latest. Can train 70B+ models on a single GPU |

How to read the numbers:
- **FP32**: Traditional floating point. Used in OpenCV, classical SLAM, etc. For personal GPUs, this number reflects actual performance.
- **TF32/BF16**: Applied when using `torch.cuda.amp` (mixed precision) in PyTorch. Training speed increases 2–6×. Data-center GPUs (A100, H100) show their true strength in this mode, so do not look only at FP32 TFLOPS and conclude "A100 is slower than a 4090?".
- **TFLOPS**: Tera Floating Point Operations Per Second. Higher is faster.

**Notes**:
- The RTX 5060 Ti comes in 8GB and 16GB versions. Be sure to buy the 16GB. The 8GB runs out of VRAM and hits a wall quickly.
- AMD GPUs (RX 7900 XTX, etc.) have improving ROCm support, but compatibility issues with the CUDA ecosystem remain. If you do not want to spend time troubleshooting, buy NVIDIA.
- If the lab server has an A100/H100, your personal GPU is for debugging/prototyping. Check the server specs first.
- Used RTX 3090 (24GB) offers the best price per VRAM. The high power draw (350W) and loud noise have to be factored in.
- You can also use an A100 by the hour on Google Colab Pro (\$10/month). Worth trying before buying a GPU.

**Q: How many papers should I read per day?**

A: A standard like "N papers per day" is meaningless. At the start, *thoroughly* understanding 1 paper per week is far better. Follow the three-pass method from §20.4 and dig deeply into a single paper. After about 6 months, even glancing at the abstract will give you a sense of "ah, this is that kind of paper". From then on the pace picks up. For reference, reading a paper for a lab-meeting presentation and reading one for your own research are different in depth. The latter requires analyzing the code too.

**Q: I am not good at coding — can I still do research?**

A: As of 2026, the ability to *make good use of coding agents (Claude, Copilot, etc.)* has become more important than the ability to write code line by line yourself. Tell the agent "build me a KITTI dataset loader" or "add wandb logging to this training loop" and the code appears. Time spent typing directly has dropped sharply.

That said, to judge whether the agent's code is right or wrong, you need domain knowledge. "Why this DataLoader is slow when num_workers is 0", "why this loss is coming out NaN", "why the coordinate frame in this SLAM code is flipped" — the agent cannot catch these on its own (see Ch.14). In the end, you need the eye to distinguish why good code is good and why bad code is bad, and that eye comes from reading a lot of good code.

Recommended approach: Read the code of well-known open source (ORB-SLAM3, Ultralytics, HuggingFace Transformers, etc.) and understand "why it was written this way". Coding skill is not typing speed but the ability to read code and make judgments.

**Q: How do I prepare a conference presentation?**

A: Conference presentations are largely divided into **oral presentations** and **poster presentations**.

- **Poster**: Most first presentations are posters. You summarize the research on an A0-size sheet. The key is big figures, little text. A passerby should become interested within 3 seconds. For practice, rehearse in front of lab colleagues at least 3 times.
- **Oral**: Usually 15–20 minutes. Keep slides under 20, one message per slide. A demo video is a plus. Prepare supplementary slides for questions.
- Common: Most presentations are in English, so write a script and practice, but do not memorize it. Content delivery matters more than natural English.

**Q: Reading English papers is too hard — what do I do?**

A: This really is something time resolves. A few tips:

- **Grasp the structure first**: Most papers follow Introduction → Related Work → Method → Experiments → Conclusion. Only the Method is genuinely new; the rest follow similar patterns.
- **Learn field-specific vocabulary first**: Expressions like "ablation study", "state-of-the-art", "we empirically show" repeat. After reading about the first 20 papers, you get used to them.
- **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**: Coloring key sentences while reading raises concentration. Use whatever tool you prefer — Adobe Acrobat, Zotero's built-in viewer, etc.

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

**Solution**:

```bash
# Check the installed CUDA version
nvcc --version

# Check the CUDA version PyTorch sees
python -c "import torch; print(torch.version.cuda)"

# If they differ, reinstall PyTorch (matching the CUDA version)
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 produces a more detailed error message.

```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
sudo apt-get install -y nvidia-container-toolkit
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 a specific version directly if needed
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 / cuDNN installed (confirm with `nvcc --version`)
- [ ] Conda or venv environment set up
- [ ] PyTorch GPU operation confirmed (`torch.cuda.is_available()`)
- [ ] ROS2 installed (if needed, Humble or Jazzy)
- [ ] 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

*The full list of research tools has been absorbed into [`../../research-notes/part2_writing/`](../../research-notes/part2_writing/) and [`part3_presentations/ch02_conference_prep.md`](../../research-notes/chapter_34_conference_prep.md) *(Korean; English version planned)*. Field-specific application: see §20.4 "Paper-writing tools" and §20.7 recommended learning roadmap.*

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

When you first join a lab, it is natural to feel lost about what to do. This guide sums up "at minimum, do at least this in your first week".

### 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: If you get stuck setting up the server environment, ask a senior. Say "I tried X, but got an unexpected result Y", and they will help you almost 100% of the time. If you ask without having tried anything... they may just tell you to figure it out.

### 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: It is normal for the code not to run. Environments differ, paths differ, versions differ. Copy the error message and search Google — most of the time the answer is on Stack Overflow.

### Day 5: Start reading papers

*The meta-skill operations of Day 5 and Day 6–7 (asking for paper recommendations, getting the research direction, preparing a self-introduction) are treated in depth in [`../../grad-notes/chapter_04_two_way_relationship.md`](../../grad-notes/chapter_04_two_way_relationship.md) (Day 5) and [`p3_ch01_my_research.md`](../../grad-notes/chapter_07_my_research.md) (the Day 1–7 sequence) *(Korean; English version planned)*.*

> 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

*See the link above. Field-specific one-liner: skim Ch.18 of this document (the lab's research directions) and map the lab's recent papers/projects to the seniors' topics.*

### 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 — copy a senior's environment
- Come up with research ideas — expect at least 1–2 months of learning time

### Mindset for survival

The general survival mindset (not-knowing is normal, leveraging seniors, writing it down, starting small, not comparing) is treated in the meta-skill guide — see [`../../research-notes/part0_starting/`](../../research-notes/part0_starting/) (5 chapters on entering research) *(Korean; English version planned)*.

Field-specific application: in SLAM/CV/robotics labs the *senior code* is the highest-leverage starting point — copy the senior's environment, run their pipeline first, and only then begin modifying. Setup pain in robotics is high (CUDA · ROS · simulator versions); the time saved by not reinventing it goes straight into reading papers.

---

# Ch.22 — Closing: Where to Start?

Twenty-one chapters behind us. How sensors turn the world into numbers, how those numbers flow through coordinate frames, how a robot moves on top of that, and where deep learning fits into this pipeline — one full loop. The remaining question is "so what do I do starting tomorrow?"

## 22.1 The Map So Far

Four parts.

- **Foundations** (Ch.1–3): the name Spatial AI, how sensors capture the world, and the math to handle it. The rotations, transformations, optimization, and probability introduced here came back in every later chapter.
- **Robots** (Ch.4–8): how to handle a physical system of joints and links. Kinematics solves pose, dynamics computes forces, control drives it to the desired state, motion planning lays out paths, and learning picks up the whole thing from data.
- **Perception and Spatial Understanding** (Ch.9–14): starting from images and climbing up to 3D space. Deep learning was stacked on top of classical CV, foundation models on top of that, and attempts like VLA that bind language and action follow. SLAM places this whole stack on a time axis.
- **Research in Practice** (Ch.15–21): what you need to actually run code. Frameworks, tools, datasets, and how to survive in a lab.

These four parts are not laid out to be read in order — they are closer to four rooms you can return to when needed. In practice, moments come when, standing in front of a robot, you have to use the equations of Ch.3, the SLAM of Ch.14, and the Docker of Ch.16 all at once.

## 22.2 Starting Points by Profile

The question I hear most often is "where do I start reading?" The door you enter depends on your background.

If you are a **third- or fourth-year undergraduate new to robotics**, I recommend Ch.1 → Ch.3 → Ch.9 → Ch.14. Get a feel for what Spatial AI is, pick up the mathematical language, climb from images to 3D, and then see how the pieces click together in SLAM. If you set up the practice environment of Ch.16 along the way, you can start moving your hands right away.

Someone arriving as a **new master's student with a deep learning background** should read Ch.2 → Ch.3 → Ch.10 → Ch.11 first. Starting from the language you already know (deep learning), learn the new grammar of sensors and math, and then see how foundation models enter robotics. After that, move to Ch.14 and Ch.13 for the 3D and SLAM side.

Those with a **classical robotics background who are weak on deep learning** should head straight to Ch.8 → Ch.10 → Ch.11 → Ch.12. Skim Ch.4–7, which you already know, and focus on what changed over the past five years. By the time you reach VLA (Ch.12), you can see where the field's center of gravity has shifted.

**Someone with a concrete project** goes in reverse. Start by skimming the datasets and benchmarks of Ch.17, pick one or two papers on a similar task, and trace backward only to the chapters those papers depend on. There is no need to read everything.

Ch.20.7's learning roadmap has more detailed recommendations by timeframe (1 month, 3 months, 6 months). This chapter only sets the broad direction.

## 22.3 What Not to Do

The four traps newcomers fall into most often — *reading many papers first / perfecting the environment first / writing everything yourself / skipping fundamentals because "AI will write it"* — are treated generally in [`../../research-notes/chapter_06_why_read.md`](../../research-notes/chapter_06_why_read.md) (the trap of reading-many) and [`../../grad-notes/chapter_11_tool_trap.md`](../../grad-notes/chapter_11_tool_trap.md) (the tool-fetish trap — environment / from-scratch / AI dependency) *(Korean; English version planned)*.

Field-specific application in one line. SLAM/CV has *deep field tooling* — ORB-SLAM3, Colmap, and Gaussian Splatting are open-source standards. Implementing from scratch is meaningful only for educational purposes or when there is a genuinely new contribution. Otherwise, running an existing implementation and finding where it breaks is closer to research.

## 22.4 A Sense of the Long Game

The PhD-time frame (the first year, the year-and-a-half mark, five years out) is treated in the meta-skill guide — [`../../grad-notes/chapter_07_my_research.md`](../../grad-notes/chapter_07_my_research.md) (when your own research appears) and [`../../grad-notes/chapter_01_phd_decision.md`](../../grad-notes/chapter_01_phd_decision.md) (the time-horizon of the PhD decision) *(Korean; English version planned)*.

Robotics experiments commonly run on a *3–6 month setup, 2-week experiment-cycle* unit. The year-and-a-half first-paper estimate comes from the same place. When the field's time-sense merges with the PhD's operating frame, daily pace shakes you less.

## 22.5 One Line

If you have read this far, you already have one ability — the ability to read a long text to the end. The field core ends here, in one full loop.

The full guide on PhD operation and reading mindset lives at [`../../research-notes/`](../../research-notes/)·[`../../grad-notes/`](../../grad-notes/) *(Korean; English version planned)*.

Draft date: 2025.12.28 · Revision date: 2026.05.01