2D slam matlab仿真,我用MATLAB撸了一个2D LiDAR SLAM
原标题：我用MATLAB撸了一个2D LiDAR SLAM
本文由知乎博主北辰灬星星授权发布，内含大量代码，慎入
0 引言
虽然只接触了SLAM技术未曾深入学习，在MATLAB平台上尝试构建了一个基础型的基于LiDAR的2D SLAM演示模型来感受并理解整个SLAM运行流程的具体实现过程
(1) 数据来源：德意志博物馆Deutsches Museum)的2D激光SLAM数据，链接如下：
https://google-cartographer-ros.readthedocs.io/en/latest/data.html
(2) SLAM过程是这样的：
最终得到的包含路径、当前位姿、栅格地图的结果：
1 数据准备与参数设置
1.1 2DLiDAR数据集准备
将提供给用户的2DLiDAR数据集'b0-2014-07-11-10-58-16.bag'转化为matlab的.mat文件中这包括约5,522批扫描数据每次扫描获取了1,079个强度值以下
1.2 LiDAR的传感器参数设置
1.3 位姿与地图参数设置
涉及以下：栅格地图单元尺寸代表的实际距离、机器人每单位时间运动的频率影响到地图的更新频率以及初始位姿等信息。(详见后面代码)
2 程序流程与思路
2.1 数据准备与参数设置。
2.2 遍历每一批次扫描数据(共5522批次)，且对于某一批次scanIdx进行如下流程：
(1) 求本批次扫描数据的局部(以移动机器人为原点)笛卡尔坐标(ReadAScan.m)。
判定该批次是否属于第一批？如果是，则执行初始化操作(Initialize.m)；否则直接进入下一步。
由该批次扫描的数据组(具体而言是一个包含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]。
判定下一状态与当前状态之间的差异是否已达到预设的标准？如果是，则执行AddAKeyScan.m的操作；否则不执行该操作。
更新步骤如下：首先比较预测的下一个姿态与当前位置的姿态在x、y或theta坐标上的差异程度。若发现存在显著差异，则认为预测出现错误，并将当前位置姿态设为前一位置的姿态（即需记录前一位置的姿态信息）。此时从新的前一位置开始重新遍历过程；否则确认下一姿态正确后将其数据集全局坐标加入全局地图中
(8) 把下一位姿并入路径中。
因此，路径为位姿[x;y;theta]的集合，如下：
(9)绘图(全局地图、路径、当前位姿)(PlotMap.m)
最终绘制的结果如下：
此外，得到的栅格地图如下：
3 MATLAB源代码(附有详细注释)
把下面这些函数放在路径下，直接运行main.m即可。
补充信息如下：horizontal_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; % 当机器人在x或y轴方向上的移动超出miniUpdateDT时，则更新其位姿
miniUpdateDR = deg2rad(5); % 当机器人旋转的角度超过miniUpdateDR时，则执行位姿更新.
% 如果机器人从最后一次键扫描移动了 0.1米或旋转了 5
% 扫描匹配参数
fastResolution = [ 0.05; 0.05; deg2rad( 0.5)]; % [m; m; rad]的分辨率
bruteResolution = [ 0.01; 0.01; deg2rad( 0.1)]; % notused
% 读取激光雷达数据
lidar_data = load( 'horizental_lidar.mat');
N = size(lidar_data.timestamps, 1);%扫描次数(控制下面的循环次数)
% 构造一个空全局地图
map.points = [];%地图点集
map.connections = [];
map.keyscans = []; %该数组用于存储当前正确的位姿扫描数据。当预测所得下一姿态发生错误时，则需回溯至最近前一姿态进行重算
pose = [ 0; 0; 0];%初始位姿为(x= 0,y= 0,theta= 0)
path = pose;%位姿并置构成路径
%是否将绘制过程保存成视频
saveFrame= 0;
ifsaveFrame== 1
% 视频保存文件定义与打开
writerObj=VideoWriter( 'SLAMprocess.avi'); % 定义一个视频文件用来存动画
open(writerObj); % 打开该视频文件
end
% Here we go!!!!!!!!!!!!!!!!!!!!
forscanIdx = 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);%得到该次扫描数据的局部笛卡尔坐标
% 如果是第一次扫描 则初始化
ifscanIdx == 1
map= Initialize( map, pose, scan);%通过位姿pose将扫描数据scan的坐标映射到全局地图中的对应位置.
miniUpdated = true;
continue;
end
% 1.如果我们在最后一次操作中执行了 mini 更新，则系统将对 局部点集图 和 局部栅格地图（粗略版本）进行更新。
Upon completing a recent micro-update, the system will update both the local points map and the local grid map at a coarse resolution.
ifminiUpdated
localMap = ComputeLocalMap(map's points, position, scan, border dimensions);// Obtain the global coordinates of the current scan
gridMap1 = OccuGrid(localMap, pixelSize);%从点集localMap 栅格单元尺寸对应实际长度以pixelSize 创建占用栅格地图
gridMap2 被定义为基于与实际长度相对应的栅格单元尺寸设定为 pixelSize 的一半的 OccuGrid 实例，并使用 localMap 作为输入点集创建占用栅格地图
end
% 2. The constant-velocity motion model is used to estimate the current pose (i.e., transitioning from the previous state to the current state serves as a basis for transitioning from the current state to the next state, thus enabling the prediction of the next state).
ifscanIdx > 2
pose_guess = pose + DiffPose(path(:,end -1), pose);%估计下一个姿态=当前位置姿态+（当前位置姿态与前一姿态的差值） pose是一个全局坐标
else
pose_guess = pose;
end
% 3.快速匹配
ifminiUpdated
[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);%返回后续更新的下一个姿态
% 如果机器人移动了一定距离，则执行mini更新
dp = abs(DiffPose( map.keyscans(end).pose, pose));%两次位姿的差
ifdp(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 =========================================
% ifminiUpdated
% ifTryLoopOrNot( map)
% map.keyscans(end).loopTried = true;
% map= DetectLoopClosure( map, scan, hits, 4, pi/ 6, pixelSize);
% end
% end
%----------------------------------------------------------------------
% 绘图
ifmod(scanIdx, 30)== 0%每 30步画一次图
PlotMap(cfig, map, path, scan, scanIdx);
%获取视频帧并保存成视频
ifsaveFrame== 1
frame = getframe(cfig);
writeVideo(writerObj, frame);
end
end
end
ifsaveFrame== 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的所有角度参数被设定为此处指定的范围；该操作仅在单次扫描过程中执行；该步骤确保了所有线束均以一致的角度间隔进行扫描
(3) ReadAScan.m
将LiDARD第i次扫描数据从极坐标系转换为直角坐標系（基於小車本地坐標系統）
% Read a laser scan
function scan = ReadAScan(lidar_data, idx, lidar, usableRange)
%--------------------------------------------------------------------------
% 输入:
%lidar_data为读取的LiDAR扫描数据
%idx为扫描次数的索引值
%lidar为由SetLidarParameters()设置的LiDAR参数
%usableRange为可使用的范围
%--------------------------------------------------------------------------
angles = lidar.angles;%
range_values = lidar_data.ranges(idx, :)'; %获取LiDAR数据中第idx次扫描的相关数据
% 删除范围不太可靠的点
% Remove points whose range is notso trustworthy
maxRange = min(lidar.range_max, usableRange);
badIndices is defined as ranges that are less than lidar.range_min or exceeding maxRange.
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表示为 基于当前机器人姿态 将当前扫描数据的局部坐标转换为全局坐标
%--------------------------------------------------------------------------
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 maparound 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 mapfrom points
function gridmap = OccuGrid(pts, pixelSize)
%--------------------------------------------------------------------------
%输入
% pts为当前扫描得到点集的全局坐标
% pixelSize表示 栅格地图一个单元的边长 对应 实际距离pixelSize米
%--------------------------------------------------------------------------
% 网格尺寸
% Grid size
minXY由点集中的x坐标与y坐标各自取最小值得出；该函数返回点集中的x坐标与y坐标的最小值所构成的向量（这不一定是图像左下角的位置）
maxXY = max(pts) + 3 * pixelSize; % 在构建栅格地图时，在最外沿设置了一个预留区域以确保输出结果具有足够的空间 margins.
该代码通过计算栅格数目值并加1得到Sgrid变量；其中Sgrid(1)表示沿x轴方向的栅格数目,Sgrid(2)表示沿y轴方向的栅格数目
N = size(pts, 1);%点集 里面 点的个数
%hits为被占用的栅格的二维坐标 (第hits( 1)块,第hits( 2)块)
注
这一过程经过上述步骤后会使得生成的地图出现翻转（当点集不存在左下角时会出现翻转）
idx = (hits(:, 1) -1)*Sgrid( 2) + hits(:, 2);%把被占用的栅格的二维坐标转化为一维坐标
%构造一个空的栅格地图
grid = false(Sgrid( 2), Sgrid( 1));
%将被占用的栅格幅值为正逻辑
grid(idx) = true;
gridmap.occGrid = grid;%栅格地图
计算 metricMap 等于 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)]）。
%输出
poses经过优化后作为预测下一个姿态的基础元素。目标函数被设计为使其与当前姿态的栅格地图具有最大的重合度。
% bestHits 为pose对应的最优重合度 score 对应的当前位姿栅格地图 的 原始 距离 矩阵
%--------------------------------------------------------------------------
%局部栅格地图信息
% Grid mapinformation
metricMap ← gridmap.metricMap;
ipixel等于gridmap(pixelSize)的倒数；真实距离1米所代表的栅格单元数量是多少？
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;%分辨率提高次数
pixel coordinates of the mapped grid are represented by pixelScan = actual scan * image pixel;
bestPose = pose;
bestScore = Inf;
t = searchResolution( 1);%x和y坐标的搜索分辨率
r = searchResolution( 3);%theta的搜索分辨率
whileiter < maxIter
noChange = true;
% 旋转
% Rotation
fortheta = pose( 3) + [-r, 0, r]%遍历这个三个旋转角 [旋转角-r 旋转角 旋转角+r]
ct = cos(theta);
st = sin(theta);
S = pixelScan * [ct, st; -st, ct];%把 扫描数据(单位:栅格) 逆时针旋转theta 得到S
% 转换
% Translation
计算tx的值为pose(1)与向量[-t, 0, t]之和，并依次遍历这三个横坐标：
第一位置为"预测位姿减去t"，
第二位置为"预测位姿"，
第三位置为"预测位姿加上t"。
Sx = round(S(:, 1) + (tx - minX) * ipixel) + 1; % 基于栅格单元计算出的横坐标值（对下一位姿态预测结果与当前位姿态扫描数据进行叠加处理）
forty = pose( 2) + [-t, 0, t]
Sy = round(S(:, 2)+(ty-minY)*ipixel) + 1;
isIn = Sx> 1& Sy> 1& Sx
ix = Sx(isIn);%提取出下一位姿扫描栅格 落在当前栅格地图区域的部分 的横坐标(单位:栅格)
iy = Sy(isIn);%提取出下一位姿扫描栅格 落在当前栅格地图区域的部分 的纵坐标(单位:栅格)
% metric socre
idx = iy + (ix -1)*nRows;%把下一位姿扫描栅格的二维坐标转换为一维坐标idx
%metricMap为当前位姿栅格地图中 非占用点距离占用点的距离矩阵
%score定义为 两个相邻姿态扫描栅格之间在空间上的匹配程度（数值越小则表示匹配程度越高）
hits = metricMap(idx);
score = sum(hits);
% update
ifscore < bestScore %目标是确定最小的score（即预测单元格与当前单元格实现最大相似性）
noChange = false;
bestPose = [tx; ty; theta];%将这个最高重合度的 预测位姿 作为最佳预测位姿
bestScore = score;
bestHits = hits;
end
end
end
end
% 找不到更好的匹配，提高分辨率
ifnoChange
r = r / 2;
t = t / 2;
depth = depth + 1;
ifdepth > maxDepth %分辨率提高次数不能超过maxDepth
break;
end
end
pose = bestPose;%最佳位姿作为预测的下一位姿
iter = iter + 1;
end
(9) AddAKeyScan.m
%将预测下一位姿的地图添加到全局地图中
或者如果检测下一位姿态出现错误情况，则返回到最近正确的位姿位置进行再次向后处理
该函数执行键扫描，并返回包含映射和姿态的元组
%--------------------------------------------------------------------------
%输入
% map为全局地图
% gridMap
% scan为当前扫描数据的局部笛卡尔坐标
% pose为优化后的预测的下一位姿
% hits为占用栅格地图(一维形式)
% pixelSize
% bruteResolution
% tmax
% rmax
%输出
% map为在当前全局地图基础上 添加了下一位姿测量数据的地图
% pose表示 当预测的下一个姿态估计出现偏差时 将正确的当前姿态回退至 并随后继续向后推导
%--------------------------------------------------------------------------
% 首先，评估pose和hits，确保没有大的错误
% 如果出现大的错误，则返回无错误最近的一步的位姿
lastKeyPose = map.keyscans(end).pose;
dp = DiffPose(lastKeyPose, pose);%若下一位姿与当前位姿出现了较大的差别则判断下一位姿有错
ifabs(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的数据 筛选出来
%
检查预测的姿态扫描数据是否为空，并说明其功能
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
该代码行使用Transform函数将当前位置的局部笛卡尔坐标通过路径参数path(:,end)转换为全局笛卡尔坐标系。
worldColor = [ 000];%地图的颜色(黑色)
scanColor = [ 148/ 2550211/ 255];%当前位置颜色(深紫色)
pathColor = [ 001];%路径颜色(蓝色)
lidarColor=[ 205/ 25538/ 25538/ 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);%画路径
fori = 1: 20:length(scan)
draw([path1的末端, 扫描第i行的第一行], [路径2的末端, 扫描第i行的第二行], '颜色属性', 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
如有疑问点阅读原文返回搜狐，查看更多
责任编辑：