gmapping原理和代码流程

Gmapping的程序框架是依托Open_slam，该框架主要分成slam_gmapping和openslam_gmapping。在slam_gmapping可以从lasercallback出发，作为整个框架的起点（虽然有main函数，main函数在main.cpp中，从这里出发不影响这个流程的分析）。Lasercallback函数在slam_gmapping.cpp文件中。
InitMapper函数：如果是首次调用lasercallback函数，则进入InitMapper，该函数在同样的.cpp文件中。激光雷达测得数据数据，是在激光雷达为坐标系的数据。在InitMapper设置一个比激光雷达Z轴上高一个单位的一个点，用这个点来判断激光雷达是否发生了倾斜。接下来就是根据激光雷达的安装位置（正反安放）。初始化并设置一些参数。
addScan函数：成功将测量值加入之后，在Lasercallback下面就是两个坐标系的变换。在addscan函数是主要函数的。在该函数了成功获取到里程计位姿后，根据激光雷达的安装方式，对角度进行修改。然后将ROS的激光雷达采集的信息转换成gmapping能看懂的格式。设置和激光数据时间戳匹配的机器人的位姿。调用processscan函数。
Processscan函数：processscan函数在gridslamprocessor.cpp中，首先获取当前的位姿，然后在从里程计运动模型获取位姿，这里调用了drawFromMotion函数，这个函数在motionmodel.cpp中，drawFromMotion函数中的sample函数是形参作为方差，均值为0的高斯分布。sample函数是数值分析所近似生成的高斯分布，具体的函数实现在stat.cpp中。当前位姿与上一次位姿做差，计算做累计角度偏差和位移偏差。利用激光雷达测得距离做得分处理。非首帧调用scanMatch，upDateTreeWeight,resample。首帧则调用invalidActiveArea,computeActiveArea,registerScan;
gmapping是基于FastSlam，FastSlam是将Slam分解成机器人定位和建图的问题，而FastSlam的主要原理是粒子滤波。生动形象的介绍粒子滤波的例子为一群警犬追捕逃犯，网上有很多相关说明，这里不赘述了。主要就是说一下基本原理，粒子滤波是一种非参数滤波，粒子则是样本，利用这些粒子去近似后验分布，粒子越多，状态空间的子区域被样本填充的越密集。在FastSlam里，粒子滤波是对机器人的轨迹进行估计，然后分别为每一个粒子计算地图。粒子包含了机器人轨迹以及与之对应的环境地图，但是不对环境地图进行估计。有时候直接从目标函数提取样本不容易，因此我们会用通过建议分布去近似目标分布

当I是真为1，是假为0。为x的粒子的权重。这反映了建议分布g(x)与目标分布f(x)的匹配程度。相似度越大，权重越大，重要性采样也就是根据建议分布样本匹配获取f(x)的样本。接下来将权重进行归一化处理，为重采样做准备。归一化之后，权重大的样本说明与目标分布匹配高是下次采样的重点，重新分布样本，在重点地区，重点查，将权重进行了0-1分布。权重大的粒子所占的比例就大，选中的机率也就大，这也就解决如何“查”重点地区的问题。这些就是粒子滤波的重要流程，这些在Processscan函数里有体现。
scanMach:scanMach，该函数在gridslamprocessor.hxx中，对粒子的最优位姿进行计算：optimize，该函数在scanmacher.cpp；计算粒子最优位姿后，重新计算粒子的权重；粒子的权重由粒子的似然表示的，计算出来最优的位姿后，进行了地图的扩充。Optimize首先计算当前位置的得分，调用score函数，score函数在scanmacher.h。当前得分与上一次的得分差，要减少搜索步长，得到周围的方向里面最好的一个位姿和对应的得分。返回最优额位置和得分。计算当前的位姿和初始位姿的区别，区别越大增益越小。增益的计算odo_gain*=exp(-m_angularOdometryReliability*dth)，-m_angularOdometryReliability为角度里程计的依赖，就是相信传感器传来的数据程度。dth为角度变化量。线性距离也是这样的。计算当前位姿的角度和初始角度的区别，如果里程计比较可靠的话，那么进行匹配的时候就需要对离初始位姿比较远的位姿施加惩罚
得分=增益得分。
Score：score函数和likelihoodandscore函数都在scanmacher.h。这两个函数比较类似。score函数该函数先将激光雷达的坐标转换到世界坐标，先转换到机器人的坐标系。然后转到世界坐标。激光雷达击中到某一点，沿着激光方向的前一个点必是未击中的，得到击中点的栅格和前一个点的栅格。如果搜索出最优最可能被这个激光束击中的点，计算得分并返回该得分，得分公式为，likelihoodandscore与其类似。
回到scanmatch函数，invalidateActiveArea和computeActiveArea，UpdateTreeweight。UpdateTreeweight在gridslamprocessor_tree.cpp。这里主要是更新权重，在这里调用normalize，然后调用resetTree把所有的粒子的所有轨迹清零，最后调用propagateweights。在propgateweight更新归一化的粒子权重。因为每一个粒子的路径都是从叶子节点开始的，得到了叶子节点，就得到了路径，所以叶子节点的子节点累计权重就等于每个粒子的权重。
Normalize：该函数在gridslamprocessor.hxx。增益为所有的粒子数。求出粒子中最大的权重，权重=
权重=该权重/总和。有效率为
Resample：该函数在gridslamprocessor.hxx。首先是备份老的粒子的轨迹，即保留叶子的节点。然后是需要重采样还是不需要重采样，如果不需要重采样，则权值不变。只为轨迹创建一个新的节点，每个粒子更新地图。当有效值小于阈值的时候需要重采样，通过resampleIndexes提取到需要删除的粒子。删除粒子后，保留当前的粒子并在保存的粒子的节点里新增一个节点。删除要删除粒子的节点，保留的粒子进行数据更新，将每个粒子的设置同一个权重。最后更新一下地图。
resampleIndexes：该函数在particlefilter.h中，首先计算总的权重，计算平均权重值（interval），根据权值进行采样,target是0-1分布随机选取的一数值，当总权重大于目标权重的，记录该粒子的索引，target在加上一个interval。如果某个粒子的权重比较大的话，那么他肯定会被采样到很多次。
invalidateActiveArea和computeActiveArea在scanmacher.cpp中。computeActiveArea计算有效区域，通过激光雷达的数据计算出来哪个地图栅格应该要被更新了。
InitMapper 激光雷达测到数据是在激光雷达坐标系下测的，在InitMapper设置一个比激光雷达高一米的点用来判断激光雷达是否倾斜，根据激光雷达排放位置对角度进行调整。设置参数和初始化参数。

转载过来方便学习

---------------------
作者：ai604233436
来源：CSDN
原文：https://blog.csdn.net/ai604233436/article/details/86476730
版权声明：本文为博主原创文章，转载请附上博文链接！

gmapping原理和代码流程相关推荐

（2）咚咚客户端核心设计原理分析 - 代码流程篇（套接字建立，发送消息流程，接收消息流程）
京麦Tcp建立连接流程: ConnectTask.run() -> connection.connect()(这里面也注册了一个连接状态的一个回调类) -> tryToConnectOnT ...
ORB_SLAM2 原理、论文解读、代码流程
ORB_SLAM2 原理+论文解读+代码流程算法原理 Tracking LocalMapping LoopClosing 代码流程文件的调用关系重要变量的数据结构 Tracking流程 Loca ...
视觉SLAM开源算法ORB-SLAM3 原理与代码解析
来源:深蓝学院,文稿整理者:何常鑫,审核&修改:刘国庆本文总结于上交感知与导航研究所科研助理--刘国庆关于[视觉SLAM开源算法ORB-SLAM3 原理与代码解析]的公开课. ORB-SLA ...
Android 10.0 PackageManagerService（一）工作原理及启动流程-[Android取经之路]
摘要:PackageManagerService是Android系统核心服务之一,在Android中的非常重要,主要负责APK.jar包等的管理. 阅读本文大约需要花费50分钟. 文章的内容主要还是从 ...
springboot启动原理_SpringBoot启动原理及相关流程
一.springboot启动原理及相关流程概览 springboot是基于spring的新型的轻量级框架,最厉害的地方当属自动配置.那我们就可以根据启动流程和相关原理来看看,如何实现传奇的自动配置二 ...
LoRa SX1278/76驱动原理附代码
LoRa SX1278/76驱动原理附代码原理解释 LoRa 关键参数说明前导码: 报头: 显式报头模式: 隐式报头模式: LoRa 调制解调: 扩频因子: 编码率: 信号带宽: 代码说明 SP ...
Pytorch|YOWO原理及代码详解(二)
Pytorch|YOWO原理及代码详解(二) 本博客上接,Pytorch|YOWO原理及代码详解(一),阅前可看. 1.正式训练 if opt.evaluate:logging('evaluating ...
对dpdk的rte_ring实现原理和代码分析
对dpdk的rte_ring实现原理和代码分析前言 dpdk的rte_ring是借鉴了linux内核的kfifo实现原理,这里统称为无锁环形缓冲队列. 环形缓冲区通常有一个读指针和一个写指针.读指针 ...
微信QQ的二维码登录原理js代码解析
这篇文章主要大家详细解析了微信QQ的二维码登录原理js代码, 具有一定的参考价值,感兴趣的小伙伴们可以参考一下在很多地方就是都出现了使用二维码登录,二维码付款,二维码账户等应用(这里的二维码种马,诈 ...

gmapping原理和代码流程

gmapping原理和代码流程相关推荐

最新文章

热门文章