0 引言
剛剛入門學了近一個月的SLAM，但對理論推導一知半解，因此在matlab上搗鼓了個簡單的2D LiDAR SLAM的demo來體會體會SLAM的完整流程。
(1)數據來源：德意志博物館Deutsches Museum)的2D激光SLAM數據，鏈接如下：
Public Data - Cartographer ROS documentationgoogle-cartographer-ros.readthedocs.io
(2)SLAM過程展示(戳下面的小視頻)
2D LiDAR SLAM
(3)demo得到的SLAM效果：
SLAM效果(包含路徑、當前位姿、柵格地圖)
1 數據准備與參數設置
1.1 2DLiDAR數據集准備
將提供的2DLiDAR數據集'b0-2014-07-11-10-58-16.bag'，轉為matlab的.mat數據文件，這其中包括有5522批次掃描數據，每次掃描得到1079個強度點。如下：
1.2 LiDAR的傳感器參數設置
包括：掃描角度的范圍、掃描角度間隔、掃描點數、掃描時間、一次掃描的線束數等。(詳見源代碼)
1.3 位姿與地圖參數設置
包括：柵格地圖單元尺寸對於的實際長度、機器人移動多少更新一次地圖和位姿、初始位姿等。（詳見源代碼）
2 程序流程與思路
2.1 數據准備與參數設置。
2.2 遍歷每一批次掃描數據(共5522批次)，且對於某一批次scanIdx進行如下流程：
(1)求本批次掃描數據的局部(以移動機器人為原點)笛卡爾坐標（ReadAScan.m）。
(2)判斷該批次是否為第一批？若是，則初始化（Initialize.m）；若否，直接進入下一步。
(3)由本批次掃描數據(實際上這是一個含有1079個點的集合)的局部坐標，和當前位姿，求得當前掃描數據點的全局坐標（Transform.m與ExtractLocalMap.m）。
--------插入一段解釋---------
局部坐標：以某次掃描時，機器人所在的空間(二維)位置為原點，得到的該次掃描數據點在空間中的二維坐標。
全局坐標：以機器人初始位置為原點，來描述任意批次的掃描數據的二維坐標。
-------------------------------------
(4)以這批數據的全局坐標，構建該次掃描得到的柵格地圖（OccuGrid.m）。例如：
(5)預測下一位姿(位姿為x坐標、y坐標、旋轉角度theta這個三維向量)。
*預測方法為：若當前位姿為初始位姿，預測的下一位姿=當前位姿；否則，預測的下一位姿 = 當前位姿 + （ 當前位姿 - 前一位姿 ）。
(6)根據當前位姿的柵格地圖來優化預測的下一位姿（FastMatch.m）。
*思路為：在預測的下一位姿上做一些細小的調整(對x、y、theta做細小調整)；對於某一次調整后的預測下一位姿，利用下一位姿的掃描數據，構建下一位姿的柵格地圖；以下一位姿的柵格地圖與當前位姿的柵格地圖的重合度作為目標函數，求該目標函數的最大值；此時得到的下一位姿即為優化后的下一位姿。(得到的結果並一定是全局最優解，因為僅在原始預測下一位姿基礎上做了有限次的細微調整)
例如：預測位姿為[-16.5250; -0.7344; -3.9177]；優化后為[-16.5250; -0.7156; -3.9057]。
(7)判斷下一位姿與當前位姿間的差距是否達到設定的閾值？若是，進行更新（AddAKeyScan.m）；否則，不進行更新。
更新步驟為：判斷預測的下一位姿和當前位姿在x或y或theta上是否存在較大的差別？若是，則判斷為預測有誤，此時，令當前位姿=上一位姿(因此要求將上一位姿保存起來)，從上一位姿開始重新開始遍歷；否則，認為預測下一位姿正確，將下一位姿的數據集的全局坐標加入到全局地圖中。
(8)把下一位姿並入路徑中。
因此，路徑為位姿[x;y;theta]的集合，如下：
(9)繪圖(全局地圖、路徑、當前位姿)（PlotMap.m）
最終繪制的結果如下：
此外，得到的柵格地圖如下：
3 MATLAB源代碼(附有詳細注釋)
把下面這些函數放在路徑下，直接運行main.m即可。
**補充一下：horizental_lidar.mat 這個數據 可戳下面的網盤鏈接下載，然后放在路徑下：
https://pan.baidu.com/s/1KDb9zO1dDlvJq-ut9Xj0gg
----------------------
引用自:
https://github.com/meyiao/LaserSLAM?files=1
----------------------
函數列表：
(1)main.m
(2)SetLidarParameters.m
(3)ReadAScan.m
(4)Initializ.m
(5)Transform.m
(6)ExtractLocalMap.m
(7)OccuGrid.m
(8)FastMatch.m
(9)AddAKeyScan.m
(10)PlotMap.m
(11)DiffPose.m
----------------------
(1)main.m
%主函數
clear; close all; clc;
cfig = figure(1);
%cfig = figure('Position', [10,10,1280,1080]);
% 激光雷達的傳感器參數
lidar = SetLidarParameters();
% 地圖參數
borderSize = 1; % 邊界尺寸
pixelSize = 0.2; % 柵格地圖的一個單元的邊長 對應 實際距離pixelSize米(這里設置為0.2米)
miniUpdated = false; %
miniUpdateDT = 0.1; % 單位m 若機器人在x方向或y方向移動超過miniUpdateDT 則更新位姿
miniUpdateDR = deg2rad(5); % 單位rad 若機器人旋轉超過miniUpdateDR 則更新位姿
% 如果機器人從最后一次鍵掃描移動了0.1米或旋轉了5度，我們將添加一個新的鍵掃描並更新地圖
% 掃描匹配參數
fastResolution = [0.05; 0.05; deg2rad(0.5)]; % [m; m; rad]的分辨率
bruteResolution = [0.01; 0.01; deg2rad(0.1)]; % not used
% 讀取激光雷達數據
lidar_data = load('horizental_lidar.mat');
N = size(lidar_data.timestamps, 1);%掃描次數(控制下面的循環次數)
% 構造一個空全局地圖
map.points = [];%地圖點集
map.connections = [];
map.keyscans = [];%keyscans保存當前正確位姿的掃描數據 如果預測得到的下一位姿出現錯誤 則返回到距其最近的前一位姿重新計算
pose = [0; 0; 0];%初始位姿為(x=0,y=0,theta=0)
path = pose;%位姿並置構成路徑
%是否將繪制過程保存成視頻
saveFrame=0;
if saveFrame==1
% 視頻保存文件定義與打開
writerObj=VideoWriter('SLAMprocess.avi'); % 定義一個視頻文件用來存動畫
open(writerObj); % 打開該視頻文件
end
% Here we go!!!!!!!!!!!!!!!!!!!!
for scanIdx = 1 : 1 : N
disp(['scan ', num2str(scanIdx)]);
% 得到當前的掃描 [x1,y1; x2,y2; ...]
%time = lidar_data.timestamps(scanIdx) * 1e-9;%時間設置成每1e-9掃描一次
scan = ReadAScan(lidar_data, scanIdx, lidar, 24);%得到該次掃描數據的局部笛卡爾坐標
% 如果是第一次掃描 則初始化
if scanIdx == 1
map = Initialize(map, pose, scan);%把掃描數據scan坐標 通過位姿pose 轉換為全局地圖map坐標
miniUpdated = true;
continue;
end
% 1. 如果我們在最后一步執行了 mini更新，我們將更新 局部點集圖 和 局部柵格地圖（粗略）
% 1. If we executed a mini update in last step, we shall update the local points map and local grid map (coarse)
if miniUpdated
localMap = ExtractLocalMap(map.points, pose, scan, borderSize);%得到當前掃描的全局坐標
gridMap1 = OccuGrid(localMap, pixelSize);%從點集localMap 柵格單元尺寸對應實際長度以pixelSize 創建占用柵格地圖
gridMap2 = OccuGrid(localMap, pixelSize/2);%從點集localMap 柵格單元尺寸對應實際長度以pixelSize/2 創建占用柵格地圖
end
% 2. 使用恆定速度運動模型預測當前位姿(即用前一狀態到本狀態的過程 作為本狀態到下一狀態的過程 從而由本狀態預測下一狀態)
if scanIdx > 2
pose_guess = pose + DiffPose(path(:,end-1), pose);%預測下一位姿=當前位姿+(當前位姿與上一位姿的差) pose是一個全局坐標
else
pose_guess = pose;
end
% 3. 快速匹配
if miniUpdated
[pose, ~] = FastMatch(gridMap1, scan, pose_guess, fastResolution);%根據當前柵格地圖 優化 預測的下一位姿
else
[pose, ~] = FastMatch(gridMap2, scan, pose_guess, fastResolution);
end
% 4. 使用較高的分辨率再細化 預測下一位姿
% gridMap = OccuGrid(localMap, pixelSize/2);
[pose, hits] = FastMatch(gridMap2, scan, pose, fastResolution/2);%返回進一步更新的下一位姿pose
% 如果機器人移動了一定距離，則執行mini更新
dp = abs(DiffPose(map.keyscans(end).pose, pose));%兩次位姿的差
if dp(1)>miniUpdateDT || dp(2)>miniUpdateDT || dp(3)>miniUpdateDR
miniUpdated = true;
[map, pose] = AddAKeyScan(map, gridMap2, scan, pose, hits,...
pixelSize, bruteResolution, 0.1, deg2rad(3));
else
miniUpdated = false;
end
path = [path, pose]; %把當前位姿pose 並入路徑path
% ===== Loop Closing =========================================
% if miniUpdated
% if TryLoopOrNot(map)
% map.keyscans(end).loopTried = true;
% map = DetectLoopClosure(map, scan, hits, 4, pi/6, pixelSize);
% end
% end
%----------------------------------------------------------------------
% 繪圖
if mod(scanIdx, 30) == 0%每30步畫一次圖
PlotMap(cfig, map, path, scan, scanIdx);
%獲取視頻幀並保存成視頻
if saveFrame==1
frame = getframe(cfig);
writeVideo(writerObj, frame);
end
end
end
if saveFrame==1
close(writerObj); %關閉視頻文件句柄
end
(2)SetLidarParameters.m
%激光雷達傳感器參數
%Laser sensor's parameters
function lidar = SetLidarParameters()
lidar.angle_min = -2.351831;%最小掃描角
lidar.angle_max = 2.351831;%最大掃描角
lidar.angle_increment = 0.004363;%角度增量 即lidar相鄰線束之間的夾角
lidar.npoints = 1079;
lidar.range_min = 0.023;
lidar.range_max = 60;
lidar.scan_time = 0.025;%掃描時間
lidar.time_increment = 1.736112e-05;%時間增量
lidar.angles = (lidar.angle_min : lidar.angle_increment : lidar.angle_max)';%一次掃描各線束的角度
(3)ReadAScan.m
%將LiDARd第idx次掃描數據從極坐標轉化為笛卡爾坐標(相對於小車的局部坐標)
% Read a laser scan
function scan = ReadAScan(lidar_data, idx, lidar, usableRange)
%--------------------------------------------------------------------------
% 輸入:
%lidar_data為讀取的LiDAR掃描數據
%idx為掃描次數的索引值
%lidar為由SetLidarParameters()設置的LiDAR參數
%usableRange為可使用的范圍
%--------------------------------------------------------------------------
angles = lidar.angles;%
ranges = lidar_data.ranges(idx, :)';%選取LiDAR數據的ranges中idx索引對應的這次掃描的數據
% 刪除范圍不太可靠的點
% Remove points whose range is not so trustworthy
maxRange = min(lidar.range_max, usableRange);
isBad = ranges < lidar.range_min | ranges > maxRange;%ranges中小於最小角度或大於最大角度的 數據的 索引下標
angles(isBad) = [];
ranges(isBad) = [];
% 從極坐標轉換為笛卡爾坐標
% Convert from polar coordinates to cartesian coordinates
[xs, ys] = pol2cart(angles, ranges);%(angles, ranges)為極坐標中的(theta,rho)
scan = [xs, ys];
end
(4)Initializ.m
%針對第一次掃描的初始化
function map = Initialize(map, pose, scan)
%--------------------------------------------------------------------------
%輸入
% map為地圖(全局)
% pose為
% scan為
%--------------------------------------------------------------------------
% 把對於小車的局部坐標數據 轉化為 全局地圖坐標
% Points in world frame
map.points = Transform(scan, pose);%將轉化為全局坐標后的掃描數據scan放入全局地圖點集
%
% Key scans' information
k = length(map.keyscans);
map.keyscans(k+1).pose = pose;
map.keyscans(k+1).iBegin = 1;
map.keyscans(k+1).iEnd = size(scan, 1);
map.keyscans(k+1).loopClosed = true;
map.keyscans(k+1).loopTried = false;
(5)Transform.m
%把局部坐標轉化為全局坐標
function tscan = Transform(scan, pose)
%--------------------------------------------------------------------------
%輸入
% pose為當前位姿(x坐標tx y坐標ty 旋轉角theta)
% scan為某次掃描數據的局部笛卡爾坐標
%輸出
% tscan為 通過當前位姿pose 將當前掃描數據的局部笛卡爾坐標scan 轉換為的全局笛卡爾坐標
%--------------------------------------------------------------------------
tx = pose(1);
ty = pose(2);
theta = pose(3);
ct = cos(theta);
st = sin(theta);
R = [ct, -st; st, ct];
tscan = scan * (R');
tscan(:,1) = tscan(:,1) + tx;
tscan(:,2) = tscan(:,2) + ty;
(6)ExtractLocalMap.m
% 從全局地圖中 提取當前掃描周圍的局部地圖 的全局坐標
% Extract a local map around current scan
function localMap = ExtractLocalMap(points, pose, scan, borderSize)
%--------------------------------------------------------------------------
%輸入
% points為全局地圖點集
% pose為當前位姿
% scan為當前掃描數據的局部坐標
% borderSize為
%--------------------------------------------------------------------------
% 將當前掃描數據坐標scan 轉化為全局坐標scan_w
% Transform current scan into world frame
scan_w = Transform(scan, pose);
% 設置 左上角 和 右下角
% Set top-left & bottom-right corner
minX = min(scan_w(:,1) - borderSize);
minY = min(scan_w(:,2) - borderSize);
maxX = max(scan_w(:,1) + borderSize);
maxY = max(scan_w(:,2) + borderSize);
% 提取位於范圍內的全局地圖中的點
% Extract
isAround = points(:,1) > minX...
& points(:,1) < maxX...
& points(:,2) > minY...
& points(:,2) < maxY;
%從全局地圖中提取到的當前掃描點
localMap = points(isAround, :);
(7)OccuGrid.m
% 從點集創建占用柵格地圖
% Create an occupancy grid map from points
function gridmap = OccuGrid(pts, pixelSize)
%--------------------------------------------------------------------------
%輸入
% pts為當前掃描得到點集的全局坐標
% pixelSize表示 柵格地圖一個單元的邊長 對應 實際距離pixelSize米
%--------------------------------------------------------------------------
% 網格尺寸
% Grid size
minXY = min(pts) - 3 * pixelSize;%min(pts)返回x的最小值和y的最小值構成的向量(這並不一定是對應左下角,因為可能圖里面沒有左下角)
maxXY = max(pts) + 3 * pixelSize;% +3*pixelSize意思是 構成的柵格地圖中 占用柵格最邊界離地圖邊界留有3個柵格單元的余量
Sgrid = round((maxXY - minXY) / pixelSize) + 1;%Sgrid(1)為x軸向柵格數量,Sgrid(2)為y軸向柵格數量
N = size(pts, 1);%點集 里面 點的個數
%hits為被占用的柵格的二維坐標 (第hits(1)塊,第hits(2)塊)
hits = round( (pts-repmat(minXY, N, 1)) / pixelSize ) + 1;%點集里每個點的坐標 都減去它們的左下角坐標 再除單個柵格尺寸 再取整 再+1
%上面這一步使得 得到的柵格地圖會較原始地圖出現一個翻轉(當點集里不存在左下角時會出現翻轉)
idx = (hits(:,1)-1)*Sgrid(2) + hits(:,2);%把被占用的柵格的二維坐標轉化為一維坐標
%構造一個空的柵格地圖
grid = false(Sgrid(2), Sgrid(1));
%將被占用的柵格幅值為正邏輯
grid(idx) = true;
gridmap.occGrid = grid;%柵格地圖
gridmap.metricMap = min(bwdist(grid),10);%bwdist(grid)表示grid中0元素所在的位置靠近非零元素位置的最短距離構成的矩陣
gridmap.pixelSize = pixelSize;%柵格單元邊長對應的實際長度
gridmap.topLeftCorner = minXY;%柵格地圖的x最小值和y最小值構成的向量的全局坐標
(8)FastMatch.m
%根據當前位姿的柵格地圖 優化預測的下一位姿 使下一位姿的柵格地圖與當前位姿的柵格地圖達到最大的重合度
%快速掃描匹配(請注意這可能會陷入局部最小值)
function [pose, bestHits] = FastMatch(gridmap, scan, pose, searchResolution)
%--------------------------------------------------------------------------
%輸入
% gridmap為局部柵格地圖
% scan為構成gridmap的當前掃描點集的局部笛卡爾坐標
% pose為預測的下一位姿(預測得到的pose_guess)
% searchResolution為搜索的分辨率(為主函數中預設的掃描匹配參數 [0.05; 0.05; deg2rad(0.5)] )
%輸出
% pose為優化過后的 預測下一位姿 優化目標函數是使下一位姿的柵格地圖與當前位姿的柵格地圖達到最大的重合度
% bestHits 為pose對應的 最佳重合度score對應的 當前位姿柵格地圖的原始距離矩陣
%--------------------------------------------------------------------------
%局部柵格地圖信息
% Grid map information
metricMap = gridmap.metricMap;%柵格地圖中0元素所在的位置靠近非零元素位置的最短柵格距離構成的矩陣
ipixel = 1 / gridmap.pixelSize;%實際距離1m對應幾個柵格單元邊長 (柵格單元尺寸對應的實際距離的倒數)
minX = gridmap.topLeftCorner(1);%柵格地圖中的最左端的橫坐標(全局)
minY = gridmap.topLeftCorner(2);%柵格地圖中的最下端的縱坐標(全局)
nCols = size(metricMap, 2);
nRows = size(metricMap, 1);
%最大后驗占用柵格算法(爬山算法) ?
% Go down the hill
maxIter = 50;%最大循環次數
maxDepth = 3;%提高分辨率的次數的最大值
iter = 0;%循環變量
depth = 0;%分辨率提高次數
pixelScan = scan * ipixel;%將 掃描數據 實際坐標 轉化為 柵格地圖中的柵格坐標
bestPose = pose;
bestScore = Inf;
t = searchResolution(1);%x和y坐標的搜索分辨率
r = searchResolution(3);%theta的搜索分辨率
while iter < maxIter
noChange = true;
% 旋轉
% Rotation
for theta = pose(3) + [-r, 0, r]%遍歷這個三個旋轉角 [旋轉角-r 旋轉角 旋轉角+r]
ct = cos(theta);
st = sin(theta);
S = pixelScan * [ct, st; -st, ct];%把 掃描數據(單位:柵格) 逆時針旋轉theta 得到S
% 轉換
% Translation
for tx = pose(1) + [-t, 0, t]%遍歷這三個橫坐標 [預測位姿橫坐標-t 預測位姿橫坐標 預測位姿橫坐標+t]
Sx = round(S(:,1)+(tx-minX)*ipixel) + 1;%以柵格為單位的橫坐標 (下一位姿的預測 疊加 當前位姿的掃描數據)
for ty = pose(2) + [-t, 0, t]
Sy = round(S(:,2)+(ty-minY)*ipixel) + 1;
isIn = Sx>1 & Sy>1 & Sx<nCols & Sy<nRows;%篩選出 下一位姿得到的掃描柵格 落在 當前掃描得到的柵格中 的坐標
ix = Sx(isIn);%提取出下一位姿掃描柵格 落在當前柵格地圖區域的部分 的橫坐標(單位:柵格)
iy = Sy(isIn);%提取出下一位姿掃描柵格 落在當前柵格地圖區域的部分 的縱坐標(單位:柵格)
% metric socre
idx = iy + (ix-1)*nRows;%把下一位姿掃描柵格的二維坐標轉換為一維坐標idx
%metricMap為當前位姿柵格地圖中 非占用點距離占用點的距離矩陣
%score理解為 下一位姿掃描柵格與當前位姿掃描柵格的重合度(score約小 表示重合度越高)
hits = metricMap(idx);
score = sum(hits);
% update
if score < bestScore %目的是找到最低的score(即預測柵格與當前柵格達到最高重合度)
noChange = false;
bestPose = [tx; ty; theta];%將這個最高重合度的 預測位姿 作為最佳預測位姿
bestScore = score;
bestHits = hits;
end
end
end
end
% 找不到更好的匹配，提高分辨率
if noChange
r = r / 2;
t = t / 2;
depth = depth + 1;
if depth > maxDepth %分辨率提高次數不能超過maxDepth
break;
end
end
pose = bestPose;%最佳位姿作為預測的下一位姿
iter = iter + 1;
end
(9)AddAKeyScan.m
%將預測下一位姿的地圖添加到全局地圖中
%或者如果判斷下一位姿出現了錯誤，回到的距其最近的正確位姿，重新往后進行
function [map, pose] = AddAKeyScan(map,...
gridMap,...
scan,...
pose,...
hits,...
pixelSize,...
bruteResolution,...
tmax,...
rmax)
%--------------------------------------------------------------------------
%輸入
% map為全局地圖
% gridMap
% scan為當前掃描數據的局部笛卡爾坐標
% pose為優化后的預測的下一位姿
% hits為占用柵格地圖(一維形式)
% pixelSize
% bruteResolution
% tmax
% rmax
%輸出
% map為在當前全局地圖基礎上 添加了下一位姿測量數據的地圖
% pose為 如果預測的下一步位姿出現錯誤 返回到的距其最近的正確位姿 再重新往后進行
%--------------------------------------------------------------------------
% 首先，評估pose和hits，確保沒有大的錯誤
% 如果出現大的錯誤，則返回無錯誤最近的一步的位姿
lastKeyPose = map.keyscans(end).pose;
dp = DiffPose(lastKeyPose, pose);%若下一位姿與當前位姿出現了較大的差別則判斷下一位姿有錯
if abs(dp(1)) > 0.5 || abs(dp(2)) > 0.5 || abs(dp(3)) > pi
disp('Oh no no no nobody but you : So Large Error!');
pose = lastKeyPose;
end
% 細化相對位姿,估計位姿協方差. 並將它們放入map.connections，當我們關閉一個循環時（姿勢圖優化）將需要它。
scan_w = Transform(scan, pose);%將當前掃描數據利用下一位姿轉化為全局坐標(理解為估計的下一位姿的掃描數據)
newPoints = scan_w(hits>1.1, :);%把預測的下一位姿的掃描數據中，和當前柵格地圖的距離大於1.1的數據 篩選出來
%
if isempty(newPoints)%意思是 預測的下一位姿的掃描數據 完全落在當前位姿構成的柵格地圖中
return;
end
% keyscans
k = length(map.keyscans);
map.keyscans(k+1).pose = pose;
map.keyscans(k+1).iBegin = size(map.points, 1) + 1;
map.keyscans(k+1).iEnd = size(map.points, 1) + size(newPoints, 1);
map.keyscans(k+1).loopTried = false;
map.keyscans(k+1).loopClosed = false;
%把下一位姿的掃描數據添加到全局地圖中
map.points = [map.points; newPoints];
% connections
% 估計相對姿勢和協方差，當我們關閉循環時它們將是有用的（姿勢圖優化）
c = length(map.connections);
map.connections(c+1).keyIdPair = [k, k+1];
map.connections(c+1).relativePose = zeros(3,1);
map.connections(c+1).covariance = zeros(3);
(10)PlotMap.m
%繪圖(點集地圖、路徑、當前位姿、當前LiDAR掃描線)
function PlotMap(cfig, map, path, scan, scanIdx)
%--------------------------------------------------------------------------
%輸入
% cfig為plot繪制位置(將所有時刻的圖疊加在一張圖上)
% map為全局地圖
% path為路徑
% scan為當前位置的局部笛卡爾坐標
% scanIdx為當前掃描索引
%--------------------------------------------------------------------------
world = map.points;%全局地圖點集賦給world
scan = Transform(scan, path(:,end));%將當前位置的局部笛卡爾坐標 利用路徑 轉化為全局笛卡爾坐標
worldColor = [0 0 0];%地圖的顏色(黑色)
scanColor = [148/255 0 211/255];%當前位置顏色(深紫色)
pathColor = [0 0 1];%路徑顏色(藍色)
lidarColor=[205/255 38/255 38/255];%LiDAR掃描線顏色(磚紅色)
% Plot
cfig(1); clf;
set(0,'defaultfigurecolor','w')
set(gca,'box','on')
set(gca, 'color', [1,1,1]);%設置背景顏色(白色)
hold on; axis equal;
plot(world(:,1), world(:,2), '+', 'MarkerSize', 1, 'color', worldColor);%畫全局地圖點集
plot(scan(:,1), scan(:,2), '.', 'MarkerSize', 2, 'color', scanColor);%畫當前的掃描點圖
plot(path(1,:), path(2,:), '-.', 'LineWidth', 1, 'color', pathColor);%畫路徑
for i = 1:20:length(scan)
line([path(1,end), scan(i,1)], [path(2,end), scan(i,2)], 'color', lidarColor);%畫出當前位置的LiDAR掃描線
end
title(['Scan: ', num2str(scanIdx)]);%標題
drawnow
(11)DiffPose.m
%求位姿差
function dp = DiffPose(pose1, pose2)
dp = pose2 - pose1;
%dp(3) = angdiff(pose1(3), pose2(3));
dp(3) = pose2(3)-pose2(3);
end