GMapping_过程
GMapping_过程
- 前言 1
- 前言 2 GMapping中使用地图结构
- 前言 3 GMapping中的坐标系们
- 前言 4 GMapping中的粒子
- 过程
- 1，由里程计更新粒子位姿
- 2，判定是否处理激光数据
- 3， 激光匹配
- 3.1，粒子位姿优化
- 3.1.1，粒子分数计算
- 3.2，粒子似然值及分数计算
转载请注明出处
前言 1
1，本文是描述GMapping的核心部分的计算流程，对应其核心代码(不含ROS部分)。确切的说是每当获得一帧激光数据后所进行的计算。
2，本人未阅读GMapping原作论文、未系统学习激光SLAM，难免错误之处，请谨慎参考
3，基本不含算法理论，后续可能增加理论与目前的内容对应介绍，也可能不。
前言 2 GMapping中使用地图结构
GMapping构建和维护的是一个栅格地图，一般的栅格地图使用一个二维数组，数组元素就是该栅格的占用概率。但是GMapping中除了要保存每个格子的占用概率还要保存一个位置坐标累计值。
因此在GMapping中定义了一个栅格类型PointAccumulator作为二维数组的元素类型，该类型有三个数据成员：n、visits、acc，分别该栅格表示被激光hit的次数、被访问的次数、位置坐标累计值。
该栅格的占用概率用n、visits计算。
那么这个 位置坐标累计值acc 是什么东西呢？
因为一个栅格对应真实世界中一块连续的区域，一般我们都将该栅格的左下角点(或左上\右下\右上\中心点)对应的实际世界的位置(即世界坐标系下的坐标)作为该栅格对应的世界坐标系下的坐标，因此不用特意保存该栅格对应的世界坐标，直接计算就可以。
但是，如果该连续区域中有一部分是障碍物，那么激光击中该栅格时，是hit了障碍物这部分，那么把障碍物的坐标作为该栅格的世界坐标显然更合理，这是因为我们将使用栅格的世界位置优化激光实际hit的栅格(详见 【粒子得分计算】 部分)
当然，障碍物的坐标我们不知道(知道的话就不用建图了)，于是我们把激光hit的点的坐标作为该栅格的世界坐标。
当然，每当激光hit此障碍物时，也一般在此障碍物的不同地方，也就是说每次激光hit的点的坐标每次都不同，于是我们每当激光hit此栅格时，将激光hit的点的坐标累加到这个位置坐标累计值上，当我们需要返回 此栅格的世界坐标时，就返回一个平均值 （位置坐标累计值/n）作为其世界坐标。
显然只有被hit的栅格，acc才有意义，没有被hit的acc默认值为(0,0)。
前言 3 GMapping中的坐标系们
世界坐标系world：单位是实际长度单位(没有仔细去看是m还是cm还是mm)；
地图坐标系map：显然，该坐标系中的度量是离散的；
机器人坐标系base_link：或者认为是里程计坐标系；
激光雷达坐标系base_laser；
粒子中保存的位姿是Tworld->base_link，即base_link在world中的表达
前言 4 GMapping中的粒子
GMapping使用粒子滤波器估计位姿，TODO：相关理论有空补充。
粒子类Particle：
其主要数据成员有{
ScanMatcherMap map; //地图
OrientedPoint pose; //位姿：Tworld->base_link
double weight; //权重
TNode* node; //节点
}
其中TNode指针类型的节点node描述了该粒子代表的轨迹。
节点类TNode：
其主要数据成员有{
OrientedPoint pose; //位姿：Tworld->base_link
double weight; // 权重
double accWeight; //该节点所有子节点的权重和
TNode* parent; //指向该节点的父节点的指针
const RangeReading* reading; //该节点对应的一帧激光雷达数据
unsigned int childs; // 该节点的子节点的数量
}
每个节点内部都由一个指向其父节点的指针，因此粒子类中的node成员，可以一直索引整条轨迹。这条轨迹上的每个节点都代表该粒子在过去某个时刻的状态，其中最新的节点，和粒子当前的状态一致。
过程
每当获得一帧激光数据后：
1，由里程计更新粒子位姿，详见【1，由里程计更新粒子位姿】；
2，判定是否处理激光数据，详见【2，判定是否处理激光数据】。如果不处理，则获取下一帧激光数据，转1；
3，如果是第一帧激光数据，转7；
4，进行激光匹配，详见【3， 激光匹配】；
5，更新每个粒子的node中的权重。在第4步中，粒子的权重更新了，但是粒子的数据成员node中记录的粒子的那些历史状态的accWeight还未更新。
6，判断是否进行重采样。
如果是：根据粒子权重重新采样，重采样后保留下来的粒子根据激光帧更新地图，并且其node成员要新增加一个对应当前状态的节点。
如果否：对每个粒子根据激光帧更新地图，并且其node成员要新增加一个对应当前状态的节点。
7，初始化的一系列操作：
根据激光数据更新每个粒子的地图，给粒子添加第一个节点，等。
8，执行与第5步相同的操作。因为：如果是初始化后，则要更新权重；如果是重采样后，也要更新权重，因为在重采样过程中权重被改变了
1，由里程计更新粒子位姿
获得当前帧激光数据时，里程计的测量值 relPose。
上一次更新粒子位姿时，里程计的测量值 m_odoPose。
则
粒子位姿 = 粒子位姿 + (relPose - m_odoPose) ；
2，判定是否处理激光数据
当机器人在上一次处理激光数据后：走过一定的距离 或者 旋转过一定的角度 或者 经历过一段指定的时间或者 这是第一帧激光数据 才处理激光数据。
其中走过的距离、 旋转过的角度 由里程计数据relPose和m_odoPose计算
3， 激光匹配
作用：
根据当前帧激光与地图的匹配程度，优化每一个粒子的由里程计更新的位姿。并计算其得分和似然值，更新粒子相关的参数
算法：
对应函数GridSlamProcessor::scanMatch()
对每一个粒子进行如下操作:
{
1，优化粒子位姿( 具体操作见【3.1，粒子位姿优化】部分)，并返回优化后的粒子分数score；
2，如果score >阈值m_minimumScore，转将粒子的位姿设置为优化后的位姿，否则仍保持优化前的位姿。
3，计算粒子分数s和似然值l(具体操作见【3.2,粒子似然值及分数计算】部分 )，并将似然值累加到该粒子的数据成员weight和weightSum上：weight += l; weightSum +=l ;
注：似然值即权重，这里权重是累加计算的，TODO：相关理论有空补充。
4，扩展粒子保存的地图，因为机器人运动有可能超过了原地图的表示范围；
}
3.1，粒子位姿优化
作用：
对粒子的位姿进行优化，使其分数更高，分数含义见【3.1.1，粒子分数计算】部分。
算法：
对应ScanMatcher::optimize()函数，下文中为描述方便，循环顺序有所更改，忽略了增量后位姿分数计算时要乘的系数。
1，首先计算当前粒子位姿currentPose的分数currentScore(方法见【粒子分数计算】部分)。
2，refinement = 0;
2，按照：[前、后、左、右、左转、右转] (x +=ldelta，x -=ldelta，y +=ldeltax, y -=ldeltax, theta += adeltax, theta -=adelta)的顺序，依次给currentPose这6个增量中的一个增量。每当给位姿一个增量后，就计算一次当前位姿localPose的分数 localScore，如果 localScore>currentScore，则令currentPose=localPose;
3，当[前、后、左、右、左转、右转] 6个增量都使用过之后，如果currentPose有所更新，则转4；
如果currentPose没有更新，则refinement++，ldelta*=0.5, adelta *=0.5 ，转4；
4，if (refinement<m_optRecursiveIterations )，则转1；
else，完成优化，优化后的值为currentPose。
3.1.1，粒子分数计算
作用：
根据地图、一帧激光计算每一个粒子的得分。
分数的含义是：如果该帧激光是在该粒子表示的机器人位姿下获得的，那么其与地图的匹配程度如何，得分越高表示匹配程度越好。
算法：
对应ScanMatcher::score()函数
一帧激光与地图的匹配程度(即得分)是每一束激光与地图的匹配程度(即得分)的和。
[公式1]：每一束激光与地图的匹配程度得分 = exp(-1.0/m_gaussianSigma*[激光hit的理论位置 与 激光hit的实际位置的距离]^2 )，即距离越小，得分越高。其中：
激光hit的理论位置:由粒子代表的位姿计算的 激光束hit的位置 在世界坐标系下的坐标
激光hit的实际位置:激光束hit的 地图中已经记录的障碍 在世界坐标系下的坐标
那么这个[激光hit的实际位置]怎么获得呢？
我们计算出 [激光hit的理论位置] 所在的栅格iphit，以其acc作为[激光hit的实际位置]是合理的；
但是有这种可能：激光hit的理论位置与[栅格iphit的acc]的距离 > phit与[与栅格iphit相临的栅格的acc]的距离，此时我们选择这个相临的栅格的acc作为[激光hit的实际位置]显然更合理。不理解的话可以回顾【GMapping中使用地图结构】部分。
当然无论是栅格iphit还是其相临的栅格，首先要满足[条件1]，才予以考虑。如果 iphit及其相临的栅格 这9个栅格 均不满足[条件1]，则该激光束得分为0；
[条件1]：该栅格为占据(占据概率>阈值)&&该栅格沿着激光束的前一个栅格为空闲(占据概率<阈值)
3.2，粒子似然值及分数计算
作用：
根据地图、一帧激光计算每一个粒子的似然值和得分。
似然值：在GMapping中作为粒子的权重
与【粒子分数计算】很像，就是在【粒子分数计算】中增加了计算似然值的部分，分数计算没有变化。
算法：
对应函数：ScanMatcher::likelihoodAndScore()
下面copy自【粒子分数计算】部分
一帧激光与地图的匹配程度(即得分)是每一束激光与地图的匹配程度(即得分)的和。
[公式1]：每一束激光与地图的匹配程度得分 = exp(-1.0/m_gaussianSigma*[激光hit的理论位置 与 激光hit的实际位置的距离]^2 )，即距离越小，得分越高。其中：
激光hit的理论位置:由粒子代表的位姿计算的 激光束hit的位置 在世界坐标系下的坐标
激光hit的实际位置:激光束hit的 地图中已经记录的障碍 在世界坐标系下的坐标
那么这个[激光hit的实际位置]怎么获得呢？
我们计算出 [激光hit的理论位置] 所在的栅格iphit，以其acc作为[激光hit的实际位置]是合理的；
但是有这种可能：激光hit的理论位置与[栅格iphit的acc]的距离 > phit与[与栅格iphit相临的栅格的acc]的距离，此时我们选择这个相临的栅格的acc作为[激光hit的实际位置]显然更合理。不理解的话可以回顾【GMapping中使用地图结构】部分。
当然无论是栅格iphit还是其相临的栅格，首先要满足[条件1]，才予以考虑。如果 iphit及其相临的栅格 这9个栅格 均不满足[条件1]，则该激光束得分为0；
[条件1]：该栅格为占据(占据概率>阈值)&&该栅格沿着激光束的前一个栅格为空闲(占据概率<阈值)
粒子的似然值 等于每一束激光的似然值之和
每一束激光的似然值由下面的[公式2]计算
[公式2]：
如果该束激光[ 9个栅格 均满足[条件1] ]，则
一束激光似然值 = (-1./m_likelihoodSigma)*[激光hit的理论位置 与 激光hit的实际位置的距离]^2
如果该束激光[ 9个栅格 均不满足[条件1] ]，则
一束激光似然值 = (-0.5/m_likelihoodSigma)
并且，每隔m_likelihoodSkip个激光束，才计算一个激光束的似然值，并累加到粒子的似然值上。
GMapping_过程相关推荐
- 总结一下在使用某里云服务器的过程中出现过的一些问题
此文总结在使用阿里云的过程中出现过的问题 想起来就记录一下 本人为言行负责! 海内选择腾讯华为,共勉! k8s不同命名空间的配置文件串掉了 k8s集群服务器购买上后,无法使用外网.连续购买了4台然 ...
- 【C#实践】三层实例：登录过程
关于三层,看完一遍,想着开始敲七层,看着别人的代码敲都有点找不到,于是重新敲了一遍三层,收获很多! 首先,它没有模板,也不是一下子就完成的,它是有思路的,根据思路走,整个过程就是很自然而然的过程! D ...
- 高斯回归过程应用例子
假设现在已经观察到了6个样本点,x为样本点特征(一维的),y为样本输出值. 现在新来了一个样本点,要求是用高斯回归过程来预测新来样本点的输出值.这些样本点显示如下; 其中前面6个点是已知输出值的训练样 ...
- 面向过程（或者叫结构化）分析方法与面向对象分析方法到底区别在哪里？
AutoSAR入门到精通系列讲解 将从2019年开始更新关于AutoSAR的知识,从入门到精通,博主xyfx和大家一起进步 雪云飞星 ¥29.90 去订阅 简单地说结构化分析方法主要用来分析系统的功能 ...
- PyTorch 学习笔记（六）：PyTorch hook 和关于 PyTorch backward 过程的理解 call
您的位置 首页 PyTorch 学习笔记系列 PyTorch 学习笔记(六):PyTorch hook 和关于 PyTorch backward 过程的理解 发布: 2017年8月4日 7,195阅读 ...
- 谷歌BERT预训练源码解析（三）：训练过程
目录 前言 源码解析 主函数 自定义模型 遮蔽词预测 下一句预测 规范化数据集 前言 本部分介绍BERT训练过程,BERT模型训练过程是在自己的TPU上进行的,这部分我没做过研究所以不做深入探讨.BE ...
- Compiler编译过程
Compiler编译过程 1.原理 编译器可以生成用来在与编译器本身所在的计算机和操作系统(平台)相同的环境下运行的目标代码,这种编译器又叫做"本地"编译器.另外,编译器也可以生成 ...
- 深度学习模型训练过程
深度学习模型训练过程 一．数据准备 基本原则: 1)数据标注前的标签体系设定要合理 2)用于标注的数据集需要无偏.全面.尽可能均衡 3)标注过程要审核 整理数据集 1)将各个标签的数据放于不同的文件夹 ...
- Android系统的启动过程
Android系统的启动过程可以简单地总结为以下几个流程: 加载BootLoader -> 初始化内核 -> 启动init进程 -> init进程fork出Zygote(孵化器)进程 ...
最新文章
- 参加中国十大IT杰出博客
- 2008 R2 辅域安装和卸载（加域、退域及组策略的测试）
- shell 脚本执行 sql
- 总说手机没有“好壁纸”，Python一次性抓取500张“美女”图片，够不够用！
- javascript中函数的全解简介
- mysql int 11 java_mysql中int（11）列的大小（以字节为单位）是多少？
- Java8 Stream详解~Stream 创建
- 用html5交换两个变量的值,Python判断两个对象相等的原理 python交换两个变量的值为什么不用中间变量...
- javascript 代码_如何使您JavaScript代码简单易读
- logrotate工具日志切割
- Linux之apache服务搭建以及浅析web安全
- 一个不完全恢复的疑惑？
- android学习笔记25——事件处理Handler
- ldconfig为空时会清已有的链接？
- k2 abc 官改固件下载_abc分析，k表示聚类
- fastlane二开java_从入门开始使用Fastlane
- 【运维】服务器的初步认识
- 关于如何在coursera.org上旁听好课
- 阿里云HaaS100物联网开发板学习笔记（六）做个智能灯---一个完整的开发例子
- 一文读懂身份证ocr识别