slam_gmapping 浅析

Slamgmapping  Src 内有五个文件

1  main.cpp 文件   

2  nodelet.cpp   

3  replay.cpp     

4  slam_gmapping.h    

5  slam_gmapping.cpp  

 

主要研究后两个，在slam_gmapping.h文件里，有一个SlamGmapping 类：

 

Public :   七个方法：三个构造函数 一个析构函数

 

下面是七个方法

Init()

Startliveslam()

Startreplay()

Pulishtransform()

Lasercallback()

Mapcallback()

pulishloop()

 

Private：  五个方法    25个对象   30个参数   （可以理解为55个对象（好像算的不对但是无所谓）  有的是整形 有的是class   有的是struct ）

 

 

Updatemap()

Getodompose()

Initmapper()

Addscan()

Computeposeentropy()

 

 

PS  以下标注   2.2.5   这个函数一定是在2.2这个函数中调用的。

 

（一）Main文件里头  

用到的三个方法

1.    slamgmapping::slamgmapping
2.    slamgmapping::startliveslam
3.    slamgmapping::~slamgmapping

 

首先启动节点

实体化一个对象    进行对象构造函数     无参数   运行构造函数里头的1.1        slamgmapping::init

 

Init 里  做了两件事情 一个是初始化一些参数  一个是获取launch文件传递进来的参数

 （二） 然后启动了类     2  Slamgmapping::startliveslam   

2.1    slamgmapping::mapcallback

2.2    slamgmapping::lasercallcack

2.3    slamgmapping::pulishloop

2.1 不太知道怎么理解

2.2 Startliveslam 函数：启动了发布三个消息 ：map map_metadata  entropy 

                              启动了一个服务：dynamic_map

                           实体化了两个对象，scan_filter_sub_ 和 scan_filter_  (不是很理解，好像是一起用的，把头一个的指针放到第二个参数里头，然后调用一个方法registercall就注册了回调函数)

这三句话好像是一起用的  暂时是这么理解的     

 

 

下面这个说是线程  但是不知道怎么理解  

2.3  Pulishloop 好像是定时发送tf

 

（三）然后进入回调函数 2.2    slamgmapping::lasercallcack

2.2.1   slamgmapping::initmapper  初始化地图

2.2.2   slamgmapping::addscan     叠加激光

2.2.3   slamgmapping::updatemap   更新地图

 

里头重要函数  三个    

 

 

 

 

2.2.1初始化地图  以后再看

 

Odom_pose是包含x,y,theta信息的结构体

 

 

（四）进入2.2.2   slamgmapping::addscan     叠加激光函数

 

2.2.2.1 SlamGMapping::getOdomPose

获取位姿信息 gmap_pose  和 scan信息  先处理scan信息  放到ranges_double里头  然后将 gmap_pose  ranges_double一起当道reading 里   然后进入2.2.2.2  GridSlamProcessor::processScan

 

  GridSlamProcessor::processScan  方法在另外的文件   gridslamprocessor.cpp

自己去openslam上下

 

（五）2.2.2.2  GridSlamProcessor::processScan     ps ： 好像这部分才是核心算法的一部分

从reading里获取位姿信息

里头的m_particles  从  GridSlamProcessor类初始化的时候构造函数中赋值    从最外头的launch文件中传进来的   

Motionmodel是从motionmodel  这个类来的   可以去看motionmodel.cpp 

然后进入    m_motionmodel.drawfrommotion 

应该是从上次的pold（定位过的）pnew(odom或者tf)  和 p（粒子带的位姿）

从三个位姿算出来一个  上面循环了   有多少个粒子 就算换了多少次  就得到了多少个新的位姿 然后应该是更新了所有粒子的位姿      

以上  好像就是做了两件事情  获取位姿和更新粒子的位姿   这个称之为算法1

看看以后能不能找到理论支撑   数学公式啥的  

 

然后有一大堆的outputstream  不知道干啥的

这个东西在gridslamporcessor中  但是没有任何方法体   不知道干啥的

 

这段是计算新老位姿在直线和角度上的变化

 

这两个字节流   把好多数据都塞进去   还不知道干啥用的

 

 

此处要理解的reading   这个东西既是一个类的对象  又带着容器的性质  打开看  

退回到slamgmapping  中   

 

然后进入rangereading

 

Std::vector  就是容器   

然后看构造函数  

 

 

 

最后一句就是给容器赋值      d  是ranges_double  就是每次激光扫描多少条线的数据  beams都是多少条线的意思   是个个数    

 

那么整体缕下来 就有了beam个 reading  如下所示；

 

 

在rangereading里有个rs  可以看rangesensor 这个类

 

 

这里的beams  是一个容器    这个容器在构造函数里头赋值了大小

 

 

就是把beams_num 弄进去  然那个m_beams函数  弄成一个容器   容器里头挨个放激光的距离    其实还是放ranges_double    跟上头那个d一样 但是跟reading不同 这个是对象里头有一个容器   reading   自己就是个容器

 

 

 

然后退回到2.2.2.2  prosessscan  里头   看这个

 

M_beams 是一次激光  的点个数   把reading里头 的激光距离 挨个放到plainreading里头   

 

 

下面好像是个大头  激光匹配  

 

 

 

进去看看   在gridslamprocessor.h里头  发现是个内联函数  

 

 

 

在gridslamprocessor.hxx  文件里

 

 

个人感觉是一个评分系统  进去就是一个循环   所有粒子循环一遍   粒子是gridslamprocessor 这个类的一个参数  在slam_gmapping.cpp赋值了  好像是调用了    

也搞不太懂

 

在这  

 

初始化的时候   赋值了  

 

 

 

退回到scanmatch

 

返回一个分数   应该是评分的  进去看看

 

发现  

去看

 

四个参数    空白位姿   粒子携带的题图   粒子的位姿   当前扫描的激光信息

 

 

进去看score 

 

这个应该是核心算东西了

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

退回到2.2.2.2  prosessscan

 

 

上头那个 没有函数体 不知道干啥的  下头那个也没有