Lecture 16 — SLAM (VIO, Loop Closure, Gà–Trứng)

Chương C · không biết pose và map — siết đồng thời; hết Chương C

Khung 6 câu hỏi

#	Câu hỏi	Trả lời ngắn
1	Tại sao cần?	UAV trong nhà / GPS-denied thường không có map sẵn và không có pose tuyệt đối.
2	So với cái khác?	Vs loc/mapping tách: bỏ giả định một bên đã biết. Vs odometry thuần: có loop closure chống trôi.
3	Giải quyết gì & thế nào?	Ước lượng đồng thời \(x_{1:t}, m\); graph/VIO; loop closure tối ưu lại chuỗi.
4	Kết quả ra sao?	Posê + map/local map cho control, planning, obs RL.
5	Nên dùng khi nào?	GPS-denied; cần map tái sử dụng; nghiên cứu active SLAM.
6	Không nên khi nào?	Gọi nhầm VO không loop là “SLAM đầy đủ”; bỏ qua drift khi so sim–real.

🖼️ Hình nhanh: gà–trứng + lò xo loop closure
```
Không loop: posê lệch → map vẽ lệch → loc còn lệch hơn
●──●──●──●──●→ → → (trôi một phía)

Có loop: nhận ra chỗ cũ ★
●══●══●══●══★ lò xo co cả chuỗi về nhất quán
↑ loop edge
```

Hình minh họa (seminar-ready)

Hình gốc hoặc minh họa cho ebook — không cắt từ slide Princeton. Dùng Present: bật Slide · F toàn màn.

**Drift odometry và loop closure.**HiếuTC · gốc

**VIO/SLAM trong hẻm mất GPS.**Generated for ebook · seminar OK

🇻🇳 Chuyện thật (sân B+A): Trong hẻm GPS-denied, VIO là “mắt + tai trong” — không có map sẵn thì phải tự vẽ vừa đi.
Trên đồi chè, texture luống lặp như giấy kẻ ngang: correspondence dễ nhầm, VO trôi dù camera vẫn “thấy xanh”.
Loop closure là khoảnh khắc nhận ra quán cũ trên phố — lò xo siết cả chuỗi pose–map, hết gà–trứng một phía.
Chốt: SLAM/VIO = ước lượng đồng thời \(x\) và \(m\); texture lặp + không loop → drift dù có hình. ../00C.

0. Mục tiêu học

Phát biểu bài toán SLAM (full vs online).
Hiểu vòng gà–trứng và vai trò loop closure.
Nắm các họ thuật toán ở mức đủ đọc paper.
Biết pipeline VIO thực dụng trên drone + gắn RL–UAV.

1. Trực giác

Lạc trong rừng không GPS: bạn vừa đánh dấu cây lạ, vừa đoán mình đã đi bao xa.
Pose sai → map vẽ sai → dùng map sai lại làm pose sai hơn. Đó là vòng gà–trứng.
SLAM (Simultaneous Localization and Mapping) siết hai thứ cùng lúc.

Khi loop closure (nhận ra “chỗ này mình đã từng qua”): có ràng buộc mạnh để
sửa cả một đoạn quỹ đạo quá khứ — map hết bị trôi một phía.

  "Tôi nghĩ tôi ở đây"  ←→  "Bản đồ tôi vừa vẽ"
         ▲                        │
         └──── mỗi bên nuôi / phá phía kia ────┘
         ★ Loop = mốc tuyệt đối nội bộ ("chỗ cũ")

2. Cốt lõi + sơ đồ

2.1. Full vs online

	Full SLAM	Online SLAM
Ước lượng	\(p(x_{1:t}, m \mid z_{1:t}, u_{1:t})\)	Chủ yếu \(p(x_t, m \mid \ldots)\)
Dùng khi	Tối ưu cả quỹ đạo (graph)	Realtime bay

2.2. Gà–trứng (đọc chiều mũi tên)

        ┌────── pose sai ──────┐
        ▼                      │
   dựng map lệch               │
        │                      │
        ▼                      │
   loc trên map lệch ──────────┘
        ★ Loop closure cắt vòng bằng ràng buộc "chỗ cũ"

2.3. Họ thuật toán (ý tưởng)

Họ	Ý tưởng	Mạnh / yếu
EKF-SLAM	State = pose + toàn bộ landmark	Cổ điển; \(O(n^2)\) theo landmark
FastSLAM	PF trên quỹ đạo; mỗi hạt một map	Đa giả thuyết tốt hơn EKF thuần
Graph-SLAM / pose-graph	Nút=pose; cạnh=odom/loop; LS	Chuẩn thực tế khi có loop
Visual SLAM / VIO	Ảnh (+ IMU): ORB-SLAM, VINS…	Chuẩn UAV GPS-denied

2.4. Loop closure như lò xo

  trước:   1——2——3——4——5——6≈1' (1' lệch khỏi 1 vì drift)
  thêm cạnh loop 6—1: tối ưu kéo 1' về 1
  sau:     1══2══3══4══5══6
                 (toàn chuỗi chỉnh nhẹ cho nhất quán)

2.5. Pipeline VIO trên drone

  IMU ──►┌─────────────┐
         │  VIO /      │──► posê(t), vận tốc, bias
  cam ──►│ Visual SLAM │──► local map / keyframes
         └──────┬──────┘
                │
       ┌────────┼────────┐
       ▼        ▼        ▼
   controller  planner  obs RL policy

Với UAV: VIO thường là lớp nền; “full SLAM có map tái sử dụng” là tầng trên
khi cần quay lại vùng cũ (loop).

3. Demo — drift vs “loop” co chuỗi (1D toy)

"""
Toy 1D: odometry cộng dồn nhiễu → pose trôi.
'Loop closure' tại bước cuối: biết về lại gốc,
phân bố đều hiệu chỉnh lên chuỗi (minh họa lò xo).
"""
import numpy as np

rng = np.random.default_rng(0)
n = 12
u = np.ones(n) * 1.0
noise = rng.normal(0, 0.15, size=n)
x_odom = np.cumsum(u + noise)
print("Cuối odom ≈", round(float(x_odom[-1]), 2), "(true khép vòng mục tiêu = 0)")

drift = x_odom[-1] - 0.0
t = np.linspace(0, 1, n)
x_closed = x_odom - t * drift
print("Cuối sau loop ≈", round(float(x_closed[-1]), 3))
print("RMS chỉnh:", round(float(np.sqrt(np.mean((x_closed - x_odom) ** 2))), 3))
print("Max |sửa| dọc chuỗi:", round(float(np.max(np.abs(x_closed - x_odom))), 3))
print("→ Drift phân tán ngược lại chuỗi; graph-SLAM làm tương tự với nhiều cạnh.")

🧪 Kỳ vọng demo: Cuối odom lệch khỏi 0 vài đơn vị; sau chỉnh ≈ 0; RMS chỉnh > 0 — cả chuỗi bị “lò xo” kéo. Bản thật: least-squares trên pose-graph, không chỉ nội suy tuyến tính.

4. Gắn RL–UAV / đa mục tiêu

Active SLAM: RL chọn hành vi vừa khám phá vừa giữ localization — hướng nghiên cứu thật.
Policy phụ thuộc map đang xây → non-stationarity; tách mapper chậm / policy nhìn local cost map.
Sim-to-real: drift VIO, mất texture, rung camera — reality gap kiểu estimation, không chỉ dynamics.
Paper phải nêu: train GT pose hay onboard VIO? Test dùng gì?
GPS urban / đồi chè: mất GPS → VIO; có GPS lại = “loop / absolute prior” rẻ.
Coverage + energy: đường explore cho SLAM có thể lệch đường tối ưu phủ — đa mục tiêu thêm term “giữ tractable localization”.
Gió: VIO attitude/velocity lệch → map nhòe; đừng đổ hết cho planner.

5. Ưu / nhược

✅ Nên	❌ Không	≈ Gần đúng
VIO nền + loop khi quay lại vùng cũ	Gọi mọi thứ có camera là “SLAM đầy đủ”	VO ngắn hạn + GPS thỉnh thoảng
Khai báo GT vs onboard trong MDP	Train không drift, test có drift rồi đổ lỗi policy	Noise / delay trên pose sim
Graph + covariance cạnh hợp lý	Loop sai (false positive) kéo map vỡ	Verify loop bằng geometry

Thành phần	✅ Vai trò	❌ Thiếu thì…
Front-end (VIO/odom)	Pose liên tục ngắn hạn	Không bay nổi realtime
Loop closure	Chống trôi dài hạn	Map/path trôi một phía
Back-end (graph opt)	Nhất quán chuỗi	Loop “cứng” làm vỡ map

Nghĩa với năng lượng / gió / độ phủ / GPS: loop tốn đường bay (năng lượng) nhưng cứu map; phủ trên map trôi = metric ảo.

6. Lỗi thường gặp

⚠️ Lỗi hay gặp: Odometry/VO không loop mà gọi SLAM — map sẽ trôi.
⚠️ Đánh giá RL trong sim không drift rồi đổ lỗi “policy kém” khi lên máy thật.
⚠️ Tối ưu graph sai thông số nhiễu cạnh → loop kéo vỡ map.
⚠️ False loop closure (hai hành lang giống nhau) — cần kiểm chứng.

7. Checklist

Vì sao loop closure quan trọng hơn chỉ “odometry tốt”?
EKF-SLAM và pose-graph khác nhau ở cách biểu diễn bài toán như thế nào?
Với task RL–UAV của bạn, observation cuối lấy từ VIO hay từ state giả lập?
Bạn đã có metric riêng cho drift estimation trong thí nghiệm chưa?

⚡ Nhớ một câu: SLAM siết pose và map cùng lúc; không loop thì trôi — loop closure là lò xo kéo cả chuỗi về nhất quán.

🔗 Sang bài sau mang theo: Chương C xong — \(z\neq x\), belief, KF/PF, loc/map/SLAM. Chương D: khi observation chính là ảnh (vision + học).

  ★ Hình nhớ mãi (hết Chương C):
  gà⇄trứng ──(loop)──► map khép   |   IMU+cam → VIO → posê → RL obs

Trước: Lec15 · Sau: Lec17 · hết Chương C

Ebook Robotics · RL–UAV

Lecture 16 — SLAM (VIO, Loop Closure, Gà–Trứng)

Chương C** · không biết pose *và* map — siết đồng thời; hết Chương C

Chế độ Slide · phím ← → · F toàn màn · Esc về đọc · S mở slide

Chương C · không biết pose và map — siết đồng thời; hết Chương C

Khung 6 câu hỏi

#	Câu hỏi	Trả lời ngắn
1	Tại sao cần?	UAV trong nhà / GPS-denied thường không có map sẵn và không có pose tuyệt đối.
2	So với cái khác?	Vs loc/mapping tách: bỏ giả định một bên đã biết. Vs odometry thuần: có loop closure chống trôi.
3	Giải quyết gì & thế nào?	Ước lượng đồng thời \(x_{1:t}, m\); graph/VIO; loop closure tối ưu lại chuỗi.
4	Kết quả ra sao?	Posê + map/local map cho control, planning, obs RL.
5	Nên dùng khi nào?	GPS-denied; cần map tái sử dụng; nghiên cứu active SLAM.
6	Không nên khi nào?	Gọi nhầm VO không loop là “SLAM đầy đủ”; bỏ qua drift khi so sim–real.

🖼️ Hình nhanh: gà–trứng + lò xo loop closure
```
Không loop: posê lệch → map vẽ lệch → loc còn lệch hơn
●──●──●──●──●→ → → (trôi một phía)

Có loop: nhận ra chỗ cũ ★
●══●══●══●══★ lò xo co cả chuỗi về nhất quán
↑ loop edge
```

Hình minh họa (seminar-ready)

Hình gốc hoặc minh họa cho ebook — không cắt từ slide Princeton. Dùng Present: bật Slide · F toàn màn.

🇻🇳 Chuyện thật (sân B+A): Trong hẻm GPS-denied, VIO là “mắt + tai trong” — không có map sẵn thì phải tự vẽ vừa đi.
Trên đồi chè, texture luống lặp như giấy kẻ ngang: correspondence dễ nhầm, VO trôi dù camera vẫn “thấy xanh”.
Loop closure là khoảnh khắc nhận ra quán cũ trên phố — lò xo siết cả chuỗi pose–map, hết gà–trứng một phía.
Chốt: SLAM/VIO = ước lượng đồng thời \(x\) và \(m\); texture lặp + không loop → drift dù có hình. ../00C.

0. Mục tiêu học

Phát biểu bài toán SLAM (full vs online).
Hiểu vòng gà–trứng và vai trò loop closure.
Nắm các họ thuật toán ở mức đủ đọc paper.
Biết pipeline VIO thực dụng trên drone + gắn RL–UAV.

1. Trực giác

Khi loop closure (nhận ra “chỗ này mình đã từng qua”): có ràng buộc mạnh để
sửa cả một đoạn quỹ đạo quá khứ — map hết bị trôi một phía.

  "Tôi nghĩ tôi ở đây"  ←→  "Bản đồ tôi vừa vẽ"
         ▲                        │
         └──── mỗi bên nuôi / phá phía kia ────┘
         ★ Loop = mốc tuyệt đối nội bộ ("chỗ cũ")

2. Cốt lõi + sơ đồ

2.1. Full vs online

	Full SLAM	Online SLAM
Ước lượng	\(p(x_{1:t}, m \mid z_{1:t}, u_{1:t})\)	Chủ yếu \(p(x_t, m \mid \ldots)\)
Dùng khi	Tối ưu cả quỹ đạo (graph)	Realtime bay

2. Cốt lõi + sơ đồ

2.2. Gà–trứng (đọc chiều mũi tên)

        ┌────── pose sai ──────┐
        ▼                      │
   dựng map lệch               │
        │                      │
        ▼                      │
   loc trên map lệch ──────────┘
        ★ Loop closure cắt vòng bằng ràng buộc "chỗ cũ"

2. Cốt lõi + sơ đồ

2.3. Họ thuật toán (ý tưởng)

Họ	Ý tưởng	Mạnh / yếu
EKF-SLAM	State = pose + toàn bộ landmark	Cổ điển; \(O(n^2)\) theo landmark
FastSLAM	PF trên quỹ đạo; mỗi hạt một map	Đa giả thuyết tốt hơn EKF thuần
Graph-SLAM / pose-graph	Nút=pose; cạnh=odom/loop; LS	Chuẩn thực tế khi có loop
Visual SLAM / VIO	Ảnh (+ IMU): ORB-SLAM, VINS…	Chuẩn UAV GPS-denied

2. Cốt lõi + sơ đồ

2.4. Loop closure như lò xo

  trước:   1——2——3——4——5——6≈1' (1' lệch khỏi 1 vì drift)
  thêm cạnh loop 6—1: tối ưu kéo 1' về 1
  sau:     1══2══3══4══5══6
                 (toàn chuỗi chỉnh nhẹ cho nhất quán)

2. Cốt lõi + sơ đồ

2.5. Pipeline VIO trên drone

  IMU ──►┌─────────────┐
         │  VIO /      │──► posê(t), vận tốc, bias
  cam ──►│ Visual SLAM │──► local map / keyframes
         └──────┬──────┘
                │
       ┌────────┼────────┐
       ▼        ▼        ▼
   controller  planner  obs RL policy

Với UAV: VIO thường là lớp nền; “full SLAM có map tái sử dụng” là tầng trên
khi cần quay lại vùng cũ (loop).

3. Demo — drift vs “loop” co chuỗi (1D toy)

"""
Toy 1D: odometry cộng dồn nhiễu → pose trôi.
'Loop closure' tại bước cuối: biết về lại gốc,
phân bố đều hiệu chỉnh lên chuỗi (minh họa lò xo).
"""
import numpy as np

rng = np.random.default_rng(0)
n = 12
u = np.ones(n) * 1.0
noise = rng.normal(0, 0.15, size=n)
x_odom = np.cumsum(u + noise)
print("Cuối odom ≈", round(float(x_odom[-1]), 2), "(true khép vòng mục tiêu = 0)")

drift = x_odom[-1] - 0.0
t = np.linspace(0, 1, n)
x_closed = x_odom - t * drift
print("Cuối sau loop ≈", round(float(x_closed[-1]), 3))
print("RMS chỉnh:", round(float(np.sqrt(np.mean((x_closed - x_odom) ** 2))), 3))
print("Max |sửa| dọc chuỗi:", round(float(np.max(np.abs(x_closed - x_odom))), 3))
print("→ Drift phân tán ngược lại chuỗi; graph-SLAM làm tương tự với nhiều cạnh.")

🧪 Kỳ vọng demo: Cuối odom lệch khỏi 0 vài đơn vị; sau chỉnh ≈ 0; RMS chỉnh > 0 — cả chuỗi bị “lò xo” kéo. Bản thật: least-squares trên pose-graph, không chỉ nội suy tuyến tính.

4. Gắn RL–UAV / đa mục tiêu

Active SLAM: RL chọn hành vi vừa khám phá vừa giữ localization — hướng nghiên cứu thật.
Policy phụ thuộc map đang xây → non-stationarity; tách mapper chậm / policy nhìn local cost map.
Sim-to-real: drift VIO, mất texture, rung camera — reality gap kiểu estimation, không chỉ dynamics.
Paper phải nêu: train GT pose hay onboard VIO? Test dùng gì?
GPS urban / đồi chè: mất GPS → VIO; có GPS lại = “loop / absolute prior” rẻ.
Coverage + energy: đường explore cho SLAM có thể lệch đường tối ưu phủ — đa mục tiêu thêm term “giữ tractable localization”.
Gió: VIO attitude/velocity lệch → map nhòe; đừng đổ hết cho planner.

5. Ưu / nhược

✅ Nên	❌ Không	≈ Gần đúng
VIO nền + loop khi quay lại vùng cũ	Gọi mọi thứ có camera là “SLAM đầy đủ”	VO ngắn hạn + GPS thỉnh thoảng
Khai báo GT vs onboard trong MDP	Train không drift, test có drift rồi đổ lỗi policy	Noise / delay trên pose sim
Graph + covariance cạnh hợp lý	Loop sai (false positive) kéo map vỡ	Verify loop bằng geometry

Thành phần	✅ Vai trò	❌ Thiếu thì…
Front-end (VIO/odom)	Pose liên tục ngắn hạn	Không bay nổi realtime
Loop closure	Chống trôi dài hạn	Map/path trôi một phía
Back-end (graph opt)	Nhất quán chuỗi	Loop “cứng” làm vỡ map

Nghĩa với năng lượng / gió / độ phủ / GPS: loop tốn đường bay (năng lượng) nhưng cứu map; phủ trên map trôi = metric ảo.

6. Lỗi thường gặp

⚠️ Lỗi hay gặp: Odometry/VO không loop mà gọi SLAM — map sẽ trôi.
⚠️ Đánh giá RL trong sim không drift rồi đổ lỗi “policy kém” khi lên máy thật.
⚠️ Tối ưu graph sai thông số nhiễu cạnh → loop kéo vỡ map.
⚠️ False loop closure (hai hành lang giống nhau) — cần kiểm chứng.

7. Checklist

Vì sao loop closure quan trọng hơn chỉ “odometry tốt”?
EKF-SLAM và pose-graph khác nhau ở cách biểu diễn bài toán như thế nào?
Với task RL–UAV của bạn, observation cuối lấy từ VIO hay từ state giả lập?
Bạn đã có metric riêng cho drift estimation trong thí nghiệm chưa?

⚡ Nhớ một câu: SLAM siết pose và map cùng lúc; không loop thì trôi — loop closure là lò xo kéo cả chuỗi về nhất quán.

🔗 Sang bài sau mang theo: Chương C xong — \(z\neq x\), belief, KF/PF, loc/map/SLAM. Chương D: khi observation chính là ảnh (vision + học).

  ★ Hình nhớ mãi (hết Chương C):
  gà⇄trứng ──(loop)──► map khép   |   IMU+cam → VIO → posê → RL obs

Trước: Lec15 · Sau: Lec17 · hết Chương C