话不多说，直接开始。首先是particlefilter.h，功能是定义了采样需要使用的相关函数，因为gmapping算法采用粒子滤波，所以需要采样。
1、particlefilter.h
//采样器模板
template <class Particle, class Numeric>
struct uniform_resampler
{
std::vector<unsigned int> resampleIndexes(const std::vector<Particle> & particles, int nparticles=0) const;//采样索引 如果nparticles为0，则采样所有的粒子
std::vector<Particle> resample(const std::vector<Particle> & particles, int nparticles=0) const;//采样 particles是采样前的粒子集合，nparticles是采样后的粒子数量
Numeric neff(const std::vector<Particle> & particles) const; //计算有效粒子数 particles是采样前的粒子集合
};
//采样器索引模板
template <class Particle, class Numeric>
std::vector<unsigned int> uniform_resampler<Particle, Numeric>:: resampleIndexes(const std::vector<Particle>& particles, int nparticles) const //采样索引 particles是采样前的粒子集合，nparticles是采样后的粒子数量
{
Numeric cweight=0; //采样前的粒子的权重和
//compute the cumulative weights
/*计算总的权重*/
unsigned int n=0; //粒子的数量
for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it) //遍历采样前的粒子集合
{
cweight+=(Numeric)*it; //计算采样前的粒子的权重和
n++; //粒子的数量
}
if (nparticles>0) //如果采样后的粒子数量大于0
n=nparticles; //则采样后的粒子数量为nparticles
//compute the interval
Numeric interval=cweight/n; //计算采样前的粒子的权重和除以采样后的粒子数量 ,赋值给interval
//compute the initial target weight
Numeric target=interval*::drand48(); //计算初始目标权重 drand48()是一个随机数函数，返回一个0到1之间的随机数
//下面是计算采样后的粒子的索引
/*根据权值进行采样*/
//采用度盘轮转算法，target每次加interval。如果某个粒子的权重比较大的话，那么他肯定会被采样到很多次。
//比如说如果某个粒子的区间为4*interval。那么它至少被采样3次。
cweight=0; //重置权重和
std::vector<unsigned int> indexes(n); //创建一个索引集合，大小为采样后的粒子数量
n=0; //重置索引数量
unsigned int i=0; //索引的索引 i是索引的索引
for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it, ++i) //遍历采样前的粒子集合，i是索引的索引
{
cweight+=(Numeric)* it; //计算采样前的粒子的权重和
while(cweight>target) //如果采样前的粒子的权重和大于目标权重
{
indexes[n++]=i; //将索引i赋值给索引集合中的索引n++
target+=interval; //目标权重加上采样前的粒子的权重和
}
}
return indexes; //返回索引集合
}
template <class Particle, class Numeric>
std::vector<Particle> uniform_resampler<Particle,Numeric>::resample //重采样
(const typename std::vector<Particle>& particles, int nparticles) const
{
Numeric cweight=0; //采样前的粒子的权重和
//compute the cumulative weights
unsigned int n=0; //粒子的数量
for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it) //遍历采样前的粒子集合
{
cweight+=(Numeric)*it; //计算采样前的粒子的权重和
n++; //粒子的数量
}
if (nparticles>0) //如果采样后的粒子数量大于0
n=nparticles; //则采样后的粒子数量为nparticles
//weight of the particles after resampling
double uw=1./n; //计算采样后的粒子的权重和
//compute the interval
Numeric interval=cweight/n; //计算采样前的粒子的权重和除以采样后的粒子数量 ,赋值给interval
//compute the initial target weight
Numeric target=interval*::drand48(); //计算初始目标权重 drand48()是一个随机数函数，返回一个0到1之间的随机数
//compute the resampled indexes
cweight=0; //重置权重和
std::vector<Particle> resampled; //创建一个采样后的粒子集合
n=0; //重置索引数量
unsigned int i=0; //索引的索引 i是索引的索引
for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it, ++i) //遍历采样前的粒子集合，i是索引的索引
{
cweight+=(Numeric)*it; //计算采样前的粒子的权重和
while(cweight>target) //如果采样前的粒子的权重和大于目标权重
{
resampled.push_back(*it); //将采样前的粒子添加到采样后的粒子集合中
resampled.back().setWeight(uw); //设置采样后的粒子的权重
target+=interval; //目标权重加上采样前的粒子的权重和
}
}
return resampled; //返回采样后的粒子集合
}
template <class Particle, class Numeric>
Numeric uniform_resampler<Particle,Numeric>::neff(const std::vector<Particle> & particles) const //计算采样后的粒子的权重和
{
double cum=0; //采样后的粒子的权重和
double sum=0; //采样前的粒子的权重和
for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it) //遍历采样前的粒子集合
{
Numeric w=(Numeric)*it; //计算采样前的粒子的权重和 目前，到底是权重和还是权重？ 不清楚
cum+=w*w; //计算采样后的粒子的权重和
sum+=w; //计算采样前的粒子的权重和
}
return sum*sum/cum; //返回采样后的粒子的权重和
}
2、motionmodel.h
motionmodel.h功能是描述了运动模型。对于运动模型是什么，为什么使用运动模型，推荐大家去看b站博主忠厚老实的老王和DR_CAN，他们讲的非常清楚，就不再赘述了。
namespace GMapping
{
struct MotionModel{
/*
成员函数drawFromMotion功能：用于根据运动模型生成运动后的预测位置。
通过对最新里程计位姿读数pnew与上一时刻的系统位姿pold做差得到控制量。
实际上里程计的读数是一种传感器的反馈数据，根据《Probabilistic Robotics》的说法， 人们常常将里程计的反馈数据用作控制量，得到相对简单的运动模型。
*/
OrientedPoint drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const;//根据运动模型生成运动后的预测位置
double srr, str, srt, stt; //它们用于描述运动模型中的噪声项
};
};
/*
这里的MotionModel是一个十分粗糙的运动模型，只是简单的矢量加减运算。
相比于《Probabilistic Robotics》中提到的速度模型和里程计模型而言，有很多方面都没有考虑，精度上可能有折扣。
但是计算简单，计算量小。有时为了提高系统的实时性，这种以牺牲精度来换取计算效率的方式是很常用的手段。模型不必十分精确，够用就行。
*/
//motionmodel.cpp
namespace GMapping
{
//通过对最新里程计位姿读数pnew与上一时刻的系统位姿pold做差得到控制量。
//实际上里程计的读数是一种传感器的反馈数据，根据《Probabilistic Robotics》的说法， 人们常常将里程计的反馈数据用作控制量，得到相对简单的运动模型。
OrientedPoint MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const
{
double sxy=0.3*srr; //srr：噪声项的协方差矩阵的第一个元素，即噪声项的方差。 噪声项的方差是一个系数，它决定了噪声项的方差。
OrientedPoint delta=absoluteDifference(pnew, pold); //计算里程计读数与上一时刻系统位姿的差值
OrientedPoint noisypoint(delta); //将差值赋给噪声项的位姿
//fabs(x)就是返回x的绝对值
//下面代码：通过对正态分布进行采样，来为控制量添加噪声。
noisypoint.x+=sampleGaussian(srr*fabs(delta.x)+str*fabs(delta.theta)+sxy*fabs(delta.y)); //添加噪声
noisypoint.y+=sampleGaussian(srr*fabs(delta.y)+str*fabs(delta.theta)+sxy*fabs(delta.x)); //添加噪声
noisypoint.theta+=sampleGaussian(stt*fabs(delta.theta)+srt*sqrt(delta.x*delta.x+delta.y*delta.y)); //添加噪声
//fmod()可以对浮点型数据进行取模运算，后一个数可以为0，返回NaN(NaN，是Not a Number的缩写,用于处理计算中出现的错误情况，比如 0.0 除以 0.0 或者求负数的平方根)。
noisypoint.theta=fmod(noisypoint.theta, 2*M_PI); //将角度限制在[0,2*M_PI)之间
if (noisypoint.theta>M_PI) //如果角度大于180度，则减去360度
noisypoint.theta-=2*M_PI;
return absoluteSum(p,noisypoint); //将附加了噪声项的控制量添加到粒子的位姿向量上，并返回。可以认为该函数的返回值，就是对粒子位姿的预估
}
};
3、标准头文件
最后，将使用到的所有标准头文件一次性解释完。
#include <assert.h> //assert()宏接收一个整型表达式作为参数，如果表达式为假，就在标准错误stderr中写入一条错误信息，如测试名，文件名，行号等，并调用abort()终止程序
#include <math.h> //数学函数库，一些数学计算的公式的具体实现，如sqrt()，sin()等, 此处用到fabs()函数，用来获取绝对值; fmod()函数，用来获取一个数的余数
#include <iostream>//输入输出流，用来输入输出数据，如cout<<"hello"<<endl;
#include <vector>//容器类，用来存储数据，比如vector<int> v; v.push_back(1); v.push_back(2); v.push_back(3);
#include <stdlib.h>//标准函数库，包含了一些标准的函数，如rand()，drand48();动态内存相关的malloc, realloc,zalloc,calloc,free
#include <limits>//定义了一些有用的宏，如numeric_limits<double>::epsilon()，numeric_limits<double>::max()等
#include <sys/types.h>// 基本系统数据类型，是Unix/Linux系统的基本系统数据类型的头文件，含有size_t，time_t，pid_t等类型。
#include <set> //set的特性是，所有元素都会根据元素的键值自动排序
#include <utility>//该模板类用来将两个对象表示成一个对象-----》当你想要一个函数返回两个值，或者想用一个容器来存储具有成对值的元素时，就非常方便
#include <cmath>//常用的数学函数，如绝对值abs(),三角函数，指数，取整，取余等
#include <cstring>//包含有很多实用的字符串函数,如strcat(str1,str2),实现两个字符串的相加，并且将相加的结果储存在str1之中；strcmp(str1,str2)，比较两个字符串是否相等，一般用strcmp()返回值为0则表示相等
#include <list>//链表类，用来存储数据，比如list<int> l; l.push_back(1); l.push_back(2); l.push_back(3);
#include <omp.h>//多线程库，用来实现多线程，比如omp_set_num_threads(4); omp_get_num_threads(); omp_get_thread_num(); omp_get_max_threads(); omp_get_wtime(); omp_get_wtick();
#include<ros/ros.h> //ROS头文件，用来定义ROS程序的入口点，包括ros::init()和ros::NodeHandle()类，用来创建节点和获取节点句柄。
#include <tf/LinearMath/Scalar.h> //Scalar类，用来存储一个浮点数，比如Scalar s(1.0); s.x()返回1.0，Scalar s2(s); s2.x()返回1.0，Scalar s3 = s + s2; s3.x()返回2.0
#include <string>//字符串类，用来存储字符串，比如string s = "hello"; s.c_str()返回字符串"hello"，s.length()返回5，s.append(" world")，s.c_str()返回字符串"hello world"
#include <map>//map 是一种关联容器， 提供一对一的关联
#include <iomanip> //主要是对cin,cout之类的一些操纵运算子，比如setfill,setw,setbase,setprecision等等
#include <climits>//定义的符号常量，如INT_MAX：int 最大值；INT_MIN :int 最小值;LONG_MAX :long最大值;LONG_MIN :long最小值;CHAR_MIN :char的最小值
#include<fstream>//fstream是C++ STL中对文件操作的合集，包含了常用的所有文件操作，比如ifstream,ofstream,fstream,fstream::open等等。
#include <deque>//储存数据，用法类似于vector，但是它的容量是可变的，可以自动扩容，比如deque<int> d; d.push_back(1); d.push_back(2); d.push_back(3);
4、NEXT
下回讲解最后三个头文件gridlinetraversal.h,scanmatcher.h和gridslamprocessor.h。
扫一扫
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
