gmapping代码走读_Mountain_xiu的博客-程序员宅基地

技术标签: slam  gmapping  SLAM学习系列教程  code  

Gmapping 代码走读
主要流程

  1. 1.1、laserCallback()

功能描述

  • 接受到激光雷达数据的回调函数 在这里面调用addScan()函数
  • 如果addScan()函数调用成功,也就是说激光数据被成功的插入到地图中后,
  • 如果到了地图更新的时间,则对地图进行更新,通过调用updateMap()函数来进行相应的操作。
  • laserCallback() ->addScan() ->gmapping::processScan() ->updateMap()

####1.2、addScan()
功能描述

  • 加入一个激光雷达的数据 主要函数 这里面会调用processScan()函数
  • 这个函数被laserCallback()函数调用
    addScan流程图
    ####1.3、updateMap()
    功能描述
    更新地图:这里的更新地图,只是为了可视化。因为真正的地图都存储在粒子里面。
    这里会拿一个权值最大的粒子的地图发布出来.
    得到权值最大的粒子,然后遍历这个粒子的整个轨迹,根据轨迹上记录的信息来进行建图
    然后把得到的地图发布出去
    这个函数被laserCallback()调用,每次addScan()成功了,就会调用这个函数来生成地图,并发布出去

2.1 processScan()

功能描述
 在scanmatcherprocessor里面也有一个这样的函数 但是那个函数是没有使用的
@desc 处理一帧激光数据 这里是gmapping算法的主要函数。
在这个函数里面调用其他的函数,包括里程计预测、激光测量更新、粒子采样等等步骤。
主要步骤:
利用运动模型更新里程计分布
利用最近的一次观测来提高proposal分布。(scan-match)
利用proposal分布+激光雷达数据来确定各个粒子的权重
对粒子进行重采样
这里写图片描述
####2.2 drawFromMotion()
功能描述
里程计运动模型。把加入了噪声的值,加入到粒子估计的最优位置上,得到新的位置(根据运动模型推算出来的位置)。
####2.3 scanMatch()
功能描述
为每一个粒子都计算扫描匹配。扫描匹配即为在里程计的基础上,通过优化求得位姿
这个函数是最终被使用的函数,这个函数不但对每个位姿进行scan-match来优化,再优化之后,还会计算每个粒子的权重
这里的权重用似然表示。
主要步骤:
1、对每个粒子迭代计算最有位姿,返回粒子得分。采用optimize()计算
2、如果矫正成功则更新位姿
  如果匹配不上,则采用历程数据。使用里程计数据无需进行更新,因为再扫描匹配前已经更新完成。
3、重新计算粒子的权重,相当于粒子滤波器中的观测步骤,计算p(z|x,m)),粒子的权重由粒子的似然来表示。采用
likelihoodAndScore()计算
4、计算出来最优的位姿之后,进行地图的扩充。采用
computeActiveArea
计算

#####2.3.1 optimize()
功能描述
根据地图、激光数据、位姿迭代求解一个最优的新的位姿出来
主要步骤
1、计算当前位置的得分,调用score函数
2、do-while循环
2.1 如果这一次(currentScore)算出来比上一次(bestScore)差,则有可能是走太多了,要减少搜索步长 这个策略跟LM有点像
2.2 把8个方向都搜索一次 得到这8个方向里面最好的一个位姿和对应的得分
2.3 返回最优位置和得分
#####2.3.2score()
功能描述
根据地图、机器人位置、激光雷达数据,计算出一个得分:原理为likelihood_field_range_finder_model
主要步骤

inline double ScanMatcher::score(const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const
{
    double s=0;
    const double * angle=m_laserAngles+m_initialBeamsSkip;
    OrientedPoint lp=p;
    /*
    把激光雷达的坐标转换到世界坐标系
    先旋转到机器人坐标系,然后再转换到世界坐标系
    p表示base_link在map中的坐标
    m_laserPose 表示base_laser在base_link坐标系中的坐标
    */
    lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
    lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
    lp.theta+=m_laserPose.theta;

    /*
     * map.getDelta表示地图分辨率 m_freeCellRatio = sqrt(2)
     * 意思是如果激光hit了某个点 那么沿着激光方向的freeDelta距离的地方要是空闲才可以
    */
    unsigned int skip=0;
    double freeDelta=map.getDelta()*m_freeCellRatio;


    //枚举所有的激光束
    for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++)
    {
        skip++;
        skip=skip>m_likelihoodSkip?0:skip;
        if (skip||*r>m_usableRange||*r==0.0) continue;

        /*被激光雷达击中的点 在地图坐标系中的坐标*/
        Point phit=lp;
        phit.x+=*r*cos(lp.theta+*angle);
        phit.y+=*r*sin(lp.theta+*angle);
        IntPoint iphit=map.world2map(phit);

        /*该束激光的最后一个空闲坐标,即紧贴hitCell的freeCell 原理为:假设phit是被激光击中的点,这样的话沿着激光方向的前面一个点必定的空闲的*/
        Point pfree=lp;
        //理论上来说 这个应该是一个bug。修改成下面的之后,改善不大。
        //		pfree.x+=(*r-map.getDelta()*freeDelta)*cos(lp.theta+*angle);
        //		pfree.y+=(*r-map.getDelta()*freeDelta)*sin(lp.theta+*angle);
        pfree.x+=(*r - freeDelta)*cos(lp.theta+*angle);
        pfree.y+=(*r - freeDelta)*sin(lp.theta+*angle);

        /*phit 和 freeCell的距离*/
        pfree=pfree-phit;
        IntPoint ipfree=map.world2map(pfree);

        /*在kernelSize大小的窗口中搜索出最优最可能被这个激光束击中的点 这个kernelSize在大小为1*/
        bool found=false;
        Point bestMu(0.,0.);
        for (int xx=-m_kernelSize; xx<=m_kernelSize; xx++)
            for (int yy=-m_kernelSize; yy<=m_kernelSize; yy++)
            {
                /*根据已开始的关系 搜索phit的时候,也计算出来pfree*/
                IntPoint pr=iphit+IntPoint(xx,yy);
                IntPoint pf=pr+ipfree;

                /*得到各自对应的Cell*/
                const PointAccumulator& cell=map.cell(pr);
                const PointAccumulator& fcell=map.cell(pf);
                /*
            (double)cell返回的是该cell被占用的概率
            这束激光要合法必须要满足cell是被占用的,而fcell是空闲的
            原理为:假设phit是被激光击中的点,这样的话沿着激光方向的前面一个点必定的空闲的
            */
                if (((double)cell )> m_fullnessThreshold && ((double)fcell )<m_fullnessThreshold)
                {
                    /*计算出被击中的点与对应的cell的currentScore距离*/
                    Point mu=phit-cell.mean();
                    if (!found)
                    {
                        bestMu=mu;
                        found=true;
                    }
                    else
                    {
                        bestMu=(mu*mu)<(bestMu*bestMu)?mu:bestMu;
                    }
                }
            }
        /*socre的计算公式exp(-d^2 / sigma)) 这里的sigma表示方差 不是标准差*/
        if (found)
        {
            double tmp_score = exp(-1.0/m_gaussianSigma*bestMu*bestMu);
            s += tmp_score;
            //只在周围的9个栅格里面寻找,因此平方的意义不大。
            //s += tmp_score*tmp_score;
        }
    }
        return s;
}

#####2.3.3 likelihoodAndScore()
功能描述
根据地图、机器人位置、激光雷达数据,同时计算出一个得分和似然:原理为likelihood_field_range_finder_model
计算出来的似然即为粒子的权重,这个函数被scanmatcher.cpp里面的optimize()调用
主要步骤

#####2.3.4 computeActiveArea()
功能描述
计算有效区域,通过激光雷达的数据计算出来哪个地图栅格应该要被更新了。(这里只是计算出来栅格的位置,然后插入地图中,并不对数据进行更新)
这里计算的有效区域的坐标都是patch坐标,不是cell坐标
这个函数在正常的进行SLAM算法的过程中,使用了m_generateMap = false。这个时候不会为了空闲区域分配内存。当要生成可视化的地图的时候,m_generateMap = true。这个时候就会为空闲区域分配内存
主要步骤

####2.4 updateTreeWeights()
功能描述
更新权重,调用*normalize()*把粒子的权重归一化
主要功能为归一化粒子的权重,同时计算出neff

主要步骤

####2.5 resample()
功能描述
粒子滤波器重采样。
主要步骤      
1.需要重采样,则所有保留下来的粒子的轨迹都加上一个新的节点,然后进行地图更新。
2.不需要冲采样,则所有的粒子的轨迹都加上一个新的节点,然后进行地图的更新
在重采样完毕之后,会调用registerScan函数来更新地图

版权声明:本文为博主原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明。
本文链接:https://blog.csdn.net/weixin_42232742/article/details/82427801

智能推荐

python进阶系列-03 字典_字典是由多个键值对组成,键和值之间通过冒号分 割,所有的键值用大括号括起来,-程序员宅基地

字典dict,编程大杀器,掌握之,佛挡杀佛 魔来斩魔!字典是一个无序、可变和索引集合。 一个字典由多个键值对组成。 每个键值对将键(key)映射到其关联的值(value)。 字典是用大括号{}来声明。 每个 key 和它的 value之间用冒号 (:) 分隔,item之间用冒号分隔逗号:。如下:my_dict = { "name": "Max", "age": 28, "city": "New York"}创建字典使用大括号{}创建字典,或者使用内建的 dict()._字典是由多个键值对组成,键和值之间通过冒号分 割,所有的键值用大括号括起来,

MariaDB [Warning] Could not increase number of max_open_files to more than 1024-程序员宅基地

启动mariadb,查看mariadb状态信息时:systemctl start mariadbsystemctl -l status mariadb报错[Warning] Could not increase number of max_open_files to more than 1024 (request: 8131)[Warning] Changed limits: max_open_files: 1024 max_connections: 594 (was 4096) tabl_could not increase number of max_open_files to more than 1024 (request: 1024

keras自定义loss_keras 自定义loss_方如一的博客-程序员宅基地

算‘myself_loss’,x2和trainY2算‘myself_loss’,x3和trainY3算‘myself_loss’,而模型的输入是。如果有多个loss,那么loss_weights对应每个loss的权重,最后输出loss的和。loss损失函数是编译模型model.compile时所需的参数之一,可以用损失函数名或者。Keras会为Model的每一个输出out构建一个loss,这些loss之间无法交互。同时,Model中每一个outut都必须在fit()有对应的y_ture。.........._keras 自定义loss

springboot 上传文件报错:java.io.IOException: The temporary upload location [/tmp/tomcat.xxx] is not valid-程序员宅基地

springboot 上传文件报错:java.io.IOException: The temporary upload location [/tmp/tomcat.xxx] is not valid前言:之前上线半个月后的项目,突然发现图片上传的接口有问题,无法上传,提示java.io.IOException: The temporary upload location [/tmp/tomcat.1920962677180834063.9092/work/Tomcat/localhost/ROOT] i

acm训练记1:2016沈阳现场赛-程序员宅基地

我实在也不是谦虚,我一个退役苟怎么就去搞acm了呢jcvb太神啦A:输入两个数,输出两个之和加上其中较大的那个,sb题;B:输入一个由C,O,H构成的物质,输出其相对分子质量,sb题(好像题目保证有这种物质);C:\(a_1=a\),\(a_2=b\),\(a_n=2*a_{n-2}+a_{n-1}+n^4\),球\(a_n\)。矩阵乘法sb题,记录\(a_{n-1}\),\(a..._jcvb

windows下 DLL 的学习_dll copy-on-write-程序员宅基地

windwos下的 DLL 有copy on write (写入拷贝机制)如果调用G e t M o d u l e H a n d l e函数,线程就能够确定D L L是否已经被映射到进程的地址空间HINSTANCE hInstance=GetMoudleHandle("MyLib");if(!hInstance ){HINSTANCE hInstanceDll=_dll copy-on-write

随便推点

金仓数据库kingbaseESV8同时操作两张表,分号隔开两个update语句就可以实现事务_update关联两张表kingbase-程序员宅基地

UPDATE TABLE_NAME_1 SET ... WHERE ID=1;UPDATE TABLE_NAME_2 SET ... WHERE ID=1_update关联两张表kingbase

基本的 CVS 命令_cvs 浏览目录-程序员宅基地

http://fedora.gro.clinux.org/docs/documentation-guide-zh_CN/sn-cvs-cvscommands.html10.4. 基本的 CVS 命令配置好系统的 CVS 访问之后,检出要处理的模块。技巧 关于是_cvs 浏览目录

Golang自定义DNS Nameserver_go语言 指定nameserver_qingwave的博客-程序员宅基地

某些情况下我们希望程序通过自定义Nameserver去查询域名,而不希望通过操作系统给定的Nameserver,本文介绍如何在Golang中实现自定义Nameserver。DNS解析过程Golang中一般通过net.Resolver的LookupHost(ctx context.Context, host string) (addrs []string, err error)去实现域名解析,解析过程如下:检查本地hosts文件是否存在解析记录,存在即返回解析地址不存在即根据resolv.conf中_go语言 指定nameserver

hive 连接beeline报错_hive beline连接报错_不自律的自律_input的博客-程序员宅基地

beeline> !connect jdbc:hive2://127.0.0.1:10000Connecting to jdbc:hive2://127.0.0.1:10000Enter username for jdbc:hive2://127.0.0.1:10000: rootEnter password for jdbc:hive2://127.0.0.1:10000: ******21/07/07 08:59:28 [main]: WARN jdbc.HiveConnection: F_hive beline连接报错

Node.js 单元测试:我要写测试 - Mocha - Nodejs开源项目里怎么样写测试、CI和代码测试覆盖率-程序员宅基地

---------------------------------------------//先了解一下nodejs的单元测试Node.js 单元测试:我要写测试: http://taobaofed.org/blog/2015/12/10/nodejs-unit-tests/nodejs单元测试ppt:http://html5ify.com/unittestin

《OpenACC并行编程实战》—— 第2章 OpenACC概览 2.1 OpenACC规范的内容-程序员宅基地

本节书摘来自华章出版社《OpenACC并行编程实战》一 书中的第1章,第1.2节,作者何沧平,更多章节内容可以访问云栖社区“华章计算机”公众号查看。第2章 OpenACC概览2007年出现的CUDA C/C++语言引爆了GPU通用计算热潮,但编程比较麻烦,挖掘硬件性能需要很多高超的优化技巧。为了降低编程门槛,2011年11月,Cray、PGI、CAP...

推荐文章

热门文章

相关标签