2D激光雷达相关

一.先说数据集,数据来源是德意志博物馆的数据集.
数据集的来源:

https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/cartographer_paper_deutsches_museum.bag
或者是https://google-cartographer-ros.readthedocs.io/en/latest/data.html#data(里面包含3D和2D的数据集)

二.正题.

主要是看了一个比较好的代码,最终实现了<<我用MATLAB撸了一个2D LiDAR SLAM[1]>>,链接1,链接2
具体的代码可以去看链接.具体的解释如下:
数据处理:将提供的2DLiDAR数据集'b0-2014-07-11-10-58-16.bag'，转为matlab的.mat数据文件，这其中包括有5522批次扫描数据，每次扫描得到1079个强度点
程序流程与思路:
1 数据准备与参数设置。

2 遍历每一批次扫描数据(共5522批次)，且对于某一批次scanIdx进行如下流程：

(1) 求本批次扫描数据的局部(以移动机器人为原点)笛卡尔坐标（ReadAScan.m）
  主要是将LiDARd第idx次扫描数据从极坐标转化为笛卡尔坐标

%将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

(2) 判断该批次是否为第一批？若是，则初始化（Initialize.m）；若否，直接进入下一步。
   针对第一次扫描的初始化,把对于小车的局部坐标数据 转化为全局地图坐标

(3) 由本批次扫描数据(实际上这是一个含有1079个点的集合)的局部坐标，和当前位姿，求得当前扫描数据点的全局坐标（Transform.m与ExtractLocalMap.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;

  Transform.m里面的内容是坐标转化.这个内容其实就是可以看做坐标下的旋转,将初始的局部坐标系与世界坐标系的原点先看做一样,这样来看就是下面图中这种情况.
代码中的R= [ct, -st; st, ct];就是转换矩阵.之后对于局部坐标中的数据点来说,以世界坐标系作为参考来计算全局坐标时,就需要将局部坐标原点在世界坐标系中的初始点计算出来即可.一般来说初始点的局部坐标系与世界坐标系原点是重合的.

    [2]
 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, :);

(4) 以这批数据的全局坐标，构建该次扫描得到的栅格地图（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最小值构成的向量的全局坐标

 OccuGrid.m从点集创建占用栅格地图,代码中占用的栅格的二维坐标转化为一维坐标,就是根据横纵坐标值转为为1维的索引值.如下:

(3,1)	(3,2)	(3,3)
(2,1)	(2,2)	(2,3)
(1,1)	(1,2)	(1,3)

如表格,从左下角开始,分别将坐标(1,1)至(3,3)转化为1维的索引值,就需要y+(x-1)x列数(3)得到索引值1至9.

(5) 预测下一位姿(位姿为x坐标、y坐标、旋转角度theta这个三维向量)。

预测方法：若当前位姿为初始位姿，预测的下一位姿=当前位姿；否则，预测的下一位姿 = 当前位姿 +（当前位姿 - 前一位姿 ）。

(6) 根据当前位姿的栅格地图来优化预测的下一位姿（FastMatch.m）。

思路为：在预测的下一位姿上做一些细小的调整(对x、y、theta做细小调整)；对于某一次调整后的预测下一位姿，利用下一位姿的扫描数据，构建下一位姿的栅格地图；以下一位姿的栅格地图与当前位姿的栅格地图的重合度作为目标函数，求该目标函数的最大值；此时得到的下一位姿即为优化后的下一位姿。(得到的结果并一定是全局最优解，因为仅在原始预测下一位姿基础上做了有限次的细微调整)请注意这可能会陷入局部最小值)

%根据当前位姿的栅格地图 优化预测的下一位姿 使下一位姿的栅格地图与当前位姿的栅格地图达到最大的重合度
%快速扫描匹配(请注意这可能会陷入局部最小值)
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 maxDepth %分辨率提高次数不能超过maxDepth
            break;
        end
    end
    pose = bestPose;%最佳位姿作为预测的下一位姿
    iter = iter + 1;
end

代码中分别在旋转角,x,y值不同下的姿态值,根据新得到的姿态值筛选出下一位姿得到的扫描栅格落在当前扫描得到的栅格的坐标,进而寻找下一位姿扫描栅格与当前位姿扫描栅格的重合度score. score的值越大表示的是预测的栅格中扫描点即占据点在当前栅格中非占据到占据的距离和,总值越大表示之前没有占据的地方被占据的点数越多,这样一来就是重合度不高.因此score值越小.表示重合度越高.将最高重合度的预测位姿作为最佳预测位姿.
(7) 判断下一位姿与当前位姿间的差距是否达到设定的阈值？若是，进行更新（AddAKeyScan.m）；否则，不进行更新。

更新步骤为：判断预测的下一位姿和当前位姿在x或y或theta上是否存在较大的差别？若是，则判断为预测有误，此时，令当前位姿=上一位姿(因此要求将上一位姿保存起来)，从上一位姿开始重新开始遍历；否则，认为预测下一位姿正确，将下一位姿的数据集的全局坐标加入到全局地图中。

%将预测下一位姿的地图添加到全局地图中
%或者如果判断下一位姿出现了错误，回到的距其最近的正确位姿，重新往后进行
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);

(8) 把下一位姿并入路径中。
(9)绘图(全局地图、路径、当前位姿)（PlotMap.m）

最终的效果如下所示:

三.cartographer 安装以及学习

这篇博客讲解了cartographer的安装以及相关内容.https://www.cnblogs.com/hitcm/p/5939507.html
博主的github主页:https://github.com/hitcm/

REFERENCE:

[1].https://www.cnblogs.com/CV-life/p/10192534.html

[2].https://blog.csdn.net/roslei/article/details/72956020