我用MATLAB撸了一个2D LiDAR SLAM

0 引言

刚刚入门学了近一个月的SLAM，但对理论推导一知半解，因此在matlab上捣鼓了个简单的2D LiDAR SLAM的demo来体会体会SLAM的完整流程。

(1)数据来源：德意志博物馆Deutsches Museum)的2D激光SLAM数据，链接如下：

Public Data - Cartographer ROS documentationgoogle-cartographer-ros.readthedocs.io

(2)SLAM过程展示(戳下面的小视频)

allowfullscreen="" src="https://www.zhihu.com/video/1050063244621930496">

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