定位與地圖建構課程模組
智慧聯網應用無人載具人才培育先期計畫
台北科技大學機械系許志明
109. 06. 09
Chapter III
SLAM離線測試與分析
Chapter IV
實際場域地圖建置
NTUT校園地圖建置
NTUT無人工廠地圖建置
SLAM精度評估
預期達成目標:在實際場域中建立地圖並且學會如何評估結果的好壞
利用SLAM核心演算法執行bag file 做離線測試
利用ROS工具以及Matlab離線分析錄製bag file的成果
預期達成目標:用離線錄已製好的bag檔案測試SLAM演算法的可行性並運用工具分析bag
file
規劃時數:3hr
規劃時數:8hr
校園街景圖 校園街景點雲圖
校內無人工廠地圖
SLAM 模組課程
內 容
同步定位與地圖構建概念
同步定位與地圖構建模組課程大綱
3D同步定位與地圖構建技術
同步定位與地圖構建離線分析
實際場域同步定位與地圖構建測試
2
同步定位與地圖構建概念
3
同步定位與地圖構建
（Simultaneous Localization And Mapping,SLAM）
從未知環境的未知地點出發，在運動過程中通過重複觀測
到的地圖特徵定位自身位置和姿態
根據自身位置增量式的構建地圖，從而達到同時定位和地
圖構建的目的
同步定位與地圖構建概念
4
同步定位與地圖構建
（Simultaneous Localization And Mapping,SLAM）
是一個雞生蛋蛋生雞的問題
精確的定位需要用到一個無偏差的地圖
但這樣的地圖又需要精確的位置估測來繪製
將兩方面的演算合在一個循環中；不同演算中相互疊代的
反饋以增進雙方的演算連續解的提升。
學習課程大綱
6
Chapter ISLAM相關背景知識
機率網格地圖
掃描匹配技術
最佳化方法
迴環偵測
預期達成目標:學習者可以了解
SLAM的核心數學理論
SLAM 模組課程
規劃時數:4hr
高斯濾波
非參數濾波
預期達成目標:學習者可以了解SLAM的核心數
學理論
Chapter II 核心SLAM演算法
Gmapping演算法流程
粒子濾波
改善提議分佈
選擇性重新採樣
預期達成目標:熟悉Gmapping演算法流程
規劃時數:2hr
Hector演算法流程
機率網格地圖
地圖存取
掃描匹配
預期達成目標:熟悉Hector演算法流程
Google cartographer演算法流程
子地圖
閉環
掃描匹配
預期達成目標:熟悉
Cartographer演算法流程
規劃時數:2hr
規劃時數:2hr
3D SLAM 技術
ICP SLAM NDT SLAM
Feature-based SLAM
預期達成目標:熟悉3D SLAM
技術演算法流程
規劃時數:3hr
學習課程大綱
7
Chapter III
SLAM離線測試與分析
Chapter IV
實際場域地圖建置
NTUT校園地圖建置
NTUT無人工廠地圖建置
SLAM精度評估
預期達成目標:在實際場域中建立地圖並且學會如何評估結果的好壞
利用SLAM核心演算法執行bag file 做離線測試
利用ROS工具以及Matlab離線分析錄製bag file的成果
預期達成目標:用離線錄已製好的bag檔案測試SLAM演算法的可行性並運用工具分析bag
file
規劃時數:3hr
規劃時數:8hr
校園街景圖 校園街景點雲圖
校內無人工廠地圖
SLAM 模組課程
學習課程目標
8
以無人載具定位感知技術之實際應用場域為學習標的
專案式導向學習(Project-Based Learing)方法
藉由使用以ROS為基礎的主流SLAM 開源程式範例
展開相關主題理論和核心技術之學習
以實用場域導向教學，重點式地進行布局以補足人才
培育之缺口，以利後續發展實際產業鏈結。
10
Iterative Closest Point SLAM (迭代最近點)傳統使用的點雲配對法，速度較慢，精度普通
Normal Distributions Transform SLAM(正態分佈變換)近期推出的定位演算法，速度較ICP快且精度更高
Feature-based SLAM (基於特徵方法)LOAM(光達掃描邊緣與平面定位)，KITTI Benchmark 排名第一演算法
LeGO-LOAM(影像化計算邊緣與平面定位)速度快，可達到與LOAM相當的精度
3D同步定位與地圖構建技術
迭代最近點(ICP)
11
ICP演算法:最小化兩筆點雲資料的差異
藉由最小化兩筆點雲資料之誤差計算出轉移矩陣(R,T)
透過Cauchy Robust調整權重
(R,T)𝒙𝒙 = 𝒙𝒙𝟏𝟏,𝒙𝒙𝟐𝟐,⋯ ,𝒙𝒙𝒌𝒌
𝒑𝒑 = 𝒑𝒑𝟏𝟏,𝒑𝒑𝟐𝟐,⋯ ,𝒑𝒑𝒌𝒌
迭代最近點(ICP)
13
𝐸𝐸(𝑹𝑹,𝑻𝑻) = ⁄1 𝑁𝑁𝑃𝑃�𝑖𝑖=1
𝑁𝑁𝑃𝑃
𝑤𝑤𝑖𝑖 𝑥𝑥𝑖𝑖 − 𝑹𝑹𝑝𝑝𝑖𝑖 − 𝑻𝑻 2
𝑅𝑅:旋轉矩陣 𝑻𝑻:平移矩陣 NP:參考點雲數量 𝑤𝑤𝑖𝑖:權重值(標準ICP的權重值為1)
Cauchy Robust的加權式ICP :
計算誤差的中位數:𝐾𝐾𝑟𝑟𝑟𝑟𝑟𝑟 = median 𝑒𝑒𝑒𝑒𝑒𝑒𝑒𝑒𝑒𝑒 × 𝐾𝐾
其中𝐾𝐾 = 8.1776調整權重植𝑤𝑤𝑖𝑖
𝑤𝑤𝑖𝑖 = ⁄1 (1 + ( ⁄𝑒𝑒𝑒𝑒𝑒𝑒𝑒𝑒𝑒𝑒 𝐾𝐾𝑟𝑟𝑟𝑟𝑟𝑟))2
標準ICP 加權式ICP
迭代最近點(ICP)
14
當前掃描 參考掃描
點雲濾波 點雲濾波
應用當前姿態轉換
點雲匹配，建立對應關係
最小化誤差計算
檢查轉換是否收斂
定位結果輸出
否
是
-60 -40 -20 0 20 40 60-50
0
50
100
-60 -40 -20 0 20 40 60-50
0
50
100
正態分佈變換(NDT)
17
Biber, P., and W. Strasser. "The Normal Distributions Transform: A New Approach to Laser Scan Matching." Intelligent Robots and Systems Proceedings. 2003.
將參考點雲所佔的空間劃分成指定大小（CellSize）的網格或體素（Voxel）計算每個網格的多維正態分佈參數
多維正態分佈參數
正態分佈變換(NDT)
NDT流程
18
𝑠𝑠𝑠𝑠𝑒𝑒𝑒𝑒𝑒𝑒 𝑝𝑝 = �𝑖𝑖
exp(−(𝑥𝑥𝑖𝑖′ − 𝑝𝑝𝑖𝑖)𝑇𝑇(Σ𝑖𝑖)−1(𝑥𝑥𝑖𝑖′ − 𝑝𝑝𝑖𝑖)
2)
𝑥𝑥𝑖𝑖′ = 𝑅𝑅𝑥𝑥𝑖𝑖 + 𝑡𝑡
19
-60 -40 -20 0 20 40 60-50
0
50
100
參考點雲
正態分佈變換(NDT)
21
-60 -40 -20 0 20 40 60-50
0
50
100
匹配結果
正態分佈變換(NDT)
以特徵為基礎SLAM
LeGO-LOAM流程
22
地面分割
特徵提取位姿估計地圖建構
點雲轉換 點雲分群
Lidar Odometry And Mapping (LOAM)
Lightweight and Ground Optimized LOAM (LeGO-LOAM)
以特徵為基礎SLAM (LeGO-LOAM)
地面分割
24
a. 地面分割前的點雲
b. 地面分割後的點雲，地面點雲以紅色標示
以特徵為基礎SLAM (LeGO-LOAM)
點雲分群
25
θHorizontal scan NHorizontal scan N+1
Segmented Points
Outlier points Ground points
Raw point cloud
Point cloud group < 30 < 10θ 。
Outlier
Segmented
以特徵為基礎SLAM (LeGO-LOAM)
特徵提取
26
Roughness of point
C > edge Threshold = edge (green)
C < plane Threshold = edge (pink)
Edge
Plane
以特徵為基礎SLAM (LeGO-LOAM)
位姿估計
27
yawθpitchθ
rollθ
xt
yt
zt
以特徵為基礎SLAM (LeGO-LOAM)
地圖建構
28
異常點
地面點雲
分割點雲 位姿估計 地圖建構
回環檢測
回環檢測(loop closure detection)當來到曾經造訪之掃描環境的特徵時，可以重新校正目前的位姿，以降低SLAM累積誤差
Adaptive LeGO-LOAM
演算法流程
29
地面分割
自適應特徵提取
位姿估計地圖建構
點雲轉換自適應
點雲取樣和分群
ALeGO-LOAM
自適應點雲分群
30
短距離點雲群自適應點雲取樣與分群
相同物體位於不同距離使用不同門檻值
異常點異常點點雲群點雲分群
相同物體位於不同距離使用相同門檻值
遠距離點雲群中距離點雲群
ALeGO-LOAM
自適應點雲取樣和分群
31
= 即時運算點雲數量的極限值rN= 取樣點雲數pcS
原始點雲
地面點
自適應特徵提取
異常點自適應點雲取樣與分類
min pc rN S N< <
分類點雲pcS是
否
minN = 特徵提取所需最小點雲數量
ALeGO-LOAM 自適應特徵提取
32
Roughness of point
遠距離特徵
短距離邊緣門檻值seC C>
短距離平面門檻值spC C<
中距離邊緣門檻值meC C>
中距離平面門檻值mpC C<
遠距離邊緣門檻值feC C>
遠距離平面門檻值fpC C<
近距離特徵 中距離特徵
邊緣門檻值eC C> 平面門檻值pC C<
特徵提取誤判情形
自適應特徵提取
ALeGO-LOAM
33
自適應點特徵提取
min
min
e e
p p
F F
F F
N N
N N
>
>
特徵點雲
是
否
分類點雲
Lidar Odometry
pcS
eF pF
= 平面特徵篩選數pFN= 邊緣特徵篩選數
eFN
= 平面特徵定位所需下限值
= 邊緣特徵定位所需下限值mineFN
minpFN自適應特徵提取
資料集介紹
KITTI 資料集錄製設備
39
1. InertialNavigation System (GPS/IMU): OXTS RT 3003
2. Laserscanner: Velodyne HDL-64E
3. Color cameras, 1.4 Megapixels:
Point Grey Flea 2 (FL2-14S3C-C)
資料集介紹
KITTI 資料使用流程
40
KITTI資料庫
相機0
光達
IMU
相機1
相機2
相機3
感測器座標
ROSBAGKitti2Bag
ToolRos topic
Topic name
/image_raw0
/points_raw
/IMU_raw
/image_raw1
/image_raw2
/image_raw3
transform
資料集介紹
KITTI Benchmark Vision Odometry
41
場景 00 矮房市區
kitti data 00
資料集介紹
KITTI Benchmark Vision Odometry
42
場景 01 高速公路
kitti data 01
資料集介紹
KITTI Benchmark Vision Odometry
43
場景 05 矮房市區
kitti data 05
資料集介紹
KITTI Benchmark Vision Odometry
44
場景 07 矮房市區經過樹林
kitti data 07
演算法於資料集上的效能評估
KITTI 00場景
45
Ground Truth
LeGO-LOAM
NDT-Mapping
ALeGO-LOAM
MAE = Mean absolute error
MAE and run time
MAE Time
NDT-Mapping 3.11m 3.99s
Lego-Loam 2.52m 0.145s
ALego-Loam 1.88m 0.098s
起點
演算法於資料集上的效能評估
KITTI 00場景
46
LeGO-LOAMNDT-MappingALeGO-LOAM
ALeGO-LOAM LeGO-LOAM NDT-Mapping
演算法於資料集上的效能評估
KITTI 00場景
47
LeGO-LOAM
LeeGo-LOAMLeGO-LOAM
(X) Fixed points cloud sampling(X)Fixed Feature Extraction
Runtime 0.15s
ALeGo-LOAM(O)Adaptive Feature Extraction
Runtime 0.098s
(O) Adaptive points cloud sampling
演算法於資料集上的效能評估
KITTI 01場景
49
Ground Truth
LeGO-LOAM
NDT-Mapping
ALeGO-LOAM
MAE and run time
MAE Time
NDT-Mapping 43.9m 1.48s
Lego-Loam 359.8m 0.045s
ALego-Loam 55.2m 0.091s
MAE = Mean absolute error
起點
演算法於資料集上的效能評估
KITTI 01場景
50
LeGO-LOAMNDT-MappingALeGO-LOAM
ALeGO-LOAM LeGO-LOAM NDT-Mapping
MAE Time
NDT-Mapping 43.9m 1.48s
Lego-Loam 359.8m 0.045s
ALego-Loam 55.2m 0.091s
演算法於資料集上的效能評估
KITTI 01場景
51
Edge過少導致 發生偏移(綠色點)
足夠的Edge提供給 定位(綠色點)
, ,x y yawt t θ , ,x y yawt t θ
演算法於資料集上的效能評估
KITTI 05場景
54
Ground Truth
LeGO-LOAM
NDT-Mapping
ALeGO-LOAMMAE and run time
MAE Time
NDT-Mapping 1.72m 1.52s
Lego-Loam 0.60m 0.096s
ALego-Loam 0.34m 0.098s
MAE = Mean absolute error
起點
演算法於資料集上的效能評估
KITTI 05場景
55
LeGO-LOAMNDT-MappingALeGO-LOAM
演算法於資料集上的效能評估
KITTI 07場景
57
Ground Truth
LeGO-LOAM
NDT-Mapping
ALeGO-LOAM
MAE and run time
MAE Time
NDT-Mapping 1.14m 1.14s
Lego-Loam 0.62m 0.070s
ALego-Loam 0.33m 0.096s
MAE = Mean absolute error
起點
演算法於資料集上的效能評估
KITTI 07場景
58
ALeGO-LOAM LeGO-LOAM NDT-Mapping
LeGO-LOAMNDT-MappingALeGO-LOAM
演算法於資料集上的效能評估
演算法分別於 KITTI場景的效能
60
00 01 05 07
NDT-Mapping 3.99s 1.48s 1.52s 1.14s
LeGO-Loam 0.145s 0.045s 0.096s 0.070s
ALeGO-Loam 0.098s 0.091s 0.098s 0.096s
Tabel 1. MAE on the four sequence(m)
Tabel 2. Runtime average of odometry on the four sequence
Real-Time OdometryLower than 0.1s
00 01 05 07
NDT-Mapping 3.11 43.9 1.72 1.14
Lego-Loam 2.52 359.8 0.60 0.62
ALeGO-Loam 1.88 55.2 0.34 0.33
實際場域介紹
室外場域-大佳河濱公園-實驗設備介紹
66
實驗場域
RTK(NovAtel OEM617D)Velodyne VLP-16
X-sens MTI-10
Logitech Camera
實驗設備
演算法於實際場域上的效能評估
72
Ground TruthLeGO-LOAMNDT-MappingALeGO-LOAM
MAE(m)
Time(s)
NDT-Mapping 0.29 1.1Lego-Loam 0.31 0.02ALego-Loam 0.23 0.07
時速約 20km/hr
大佳河濱公園-實驗一
演算法於實際場域上的效能評估
74
大佳河濱公園-實驗二
Ground TruthLeGO-LOAMNDT-MappingALeGO-LOAM
MAE(m)
Time(s)
NDT-Mapping 0.52 1.1sLego-Loam 0.33 0.02sALego-Loam 0.28 0.07s
時速約 50km/hr
演算法於實際場域上的效能評估
76
大佳河濱公園-實驗二建圖情形
點雲地圖物件與實際物件之量測值比對圖
77
謝謝 !敬請指教 !
定位與地圖建構課程模組
台北科技大學機械系許志明
