获取到简化版gmapping源码后，首先我们要搞明白头文件的定义声明，才能帮助我们理解后续的函数。所以，从这篇文章开始，我会依次讲解分析gmapping用到的头文件。但是由于头文件众多，并且存在互相包含的情况，所以要先找到最基本的头文件，一步步理清之间的关系。我的建议是，依次看part_ros文件夹的utils，sensor_range,grid,particlefilter,motionmodel,scanmatcher,gridfastslam所包含的头文件。具体文件夹下的头文件也存在前后关系，这个等到后面具体讲解的时候再具体分析。
今天，我将分析讲解utils下的头文件。首先，是point.h文件。该文件定义了需要使用的点，位姿及对应的操作符重载等，后面会多次用到，希望大家一定要搞明白。接下来，直接上源码。在定义中，会多次出现inline关键字，大家把它理解成提高处理效率就好，后面具体分析代码时，就不再多做解释了。
1、point.h
首先，是文件需要包含的头文件及其功能
#include <assert.h>//assert()宏接收一个整型表达式作为参数，如果表达式为假，就在标准错误stderr中写入一条错误信息，如测试名，文件名，行号等，并调用abort()终止程序
#include <math.h> //数学函数库，一些数学计算的公式的具体实现，如sqrt()，sin()等, 此处用到fabs()函数，用来获取绝对值; fmod()函数，用来获取一个数的余数
#include <iostream> //输入输出流，用来输入输出数据，如cout<<"hello"<<endl;
接着，是点point的定义及运算符重载，每行代码都有具体的解释（过于简单的略过），这里就不解释了
template <class T> //使用模板
struct point //点的结构体
{
//初始化列表，初始化x和y坐标值
inline point():x(0),y(0) {}
inline point(T _x, T _y):x(_x),y(_y){}
T x, y;
};
//点的 + 运算符的重载 两个点对应的x值相加，y值相加
template <class T>
inline point<T> operator+ (const point<T>& p1, const point<T>& p2){
return point<T>(p1.x+p2.x, p1.y+p2.y);
}
//点的 - 运算符的重载 两个点对应的x值相减，y值相减
template <class T>
inline point<T> operator - (const point<T> & p1, const point<T> & p2){
return point<T>(p1.x-p2.x, p1.y-p2.y);
}
/*点的 * 运算符的重载 乘以一个数*/
template <class T>
inline point<T> operator * (const point<T>& p, const T& v){
return point<T>(p.x*v, p.y*v);
}
/*点的 * 运算符的重载 乘以一个数*/
template <class T>
inline point<T> operator * (const T& v, const point<T>& p){
return point<T>(p.x*v, p.y*v);
}
//上两个重载函数的区别：传入参数的位置不一样，第一个函数先传入一个点，然后传入一个数，第二个函数先传入一个数，然后传入一个点
/*点的 * 运算符的重载 point的内积*/
template <class T>
inline T operator * (const point<T>& p1, const point<T>& p2){
return p1.x*p2.x+p1.y*p2.y;
}
然后，是位姿orientedpoint的定义及其运算符重载和函数
//*带方向的点 即除了位置之外，还有角度，可以理解为机器人的位姿*/
template <class T, class A> //模板
struct orientedpoint: public point<T> //继承了point
{
//构造函数
inline orientedpoint() : point<T>(0,0), theta(0) {};
inline orientedpoint(const point<T>& p);
inline orientedpoint(T x, T y, A _theta): point<T>(x,y), theta(_theta){}
inline void normalize();
inline orientedpoint<T,A> rotate(A alpha) //旋转alpha度
{
T s=sin(alpha), c=cos(alpha);//为下面计算x,y做准备
A a=alpha+theta; //旋转后的角度
a=atan2(sin(a),cos(a)); //double atan2 (double,double); 反正切（整圆值），结果介于[-PI,PI]
return orientedpoint(
c*this->x-s*this->y, //旋转之后的x坐标
s*this->x+c*this->y, //旋转之后的y坐标
a); //旋转之后的角度
}
A theta;
};
/*角度归一化 即把角度化成-PI~PI*/
template <class T, class A>
void orientedpoint<T,A>::normalize()
{
if (theta >= -M_PI && theta < M_PI) // 如果角度在-PI～PI之间，则不需要处理
return;
int multiplier = (int)(theta / (2*M_PI)); //计算角度的倍数，将得到的值强制转换为int类型
theta = theta - multiplier*2*M_PI; //计算角度的余数
if (theta >= M_PI) //如果角度大于PI，则减去2PI
theta -= 2*M_PI;
if (theta < -M_PI) //如果角度小于-PI，则 加上2PI
theta += 2*M_PI;
}
/*构造函数*/
template <class T, class A>
orientedpoint<T,A>::orientedpoint(const point<T>& p) //构造函数
{
this->x=p.x; //把点的x坐标赋值给orientedpoint的x坐标
this->y=p.y; //把点的y坐标赋值给orientedpoint的y坐标
this->theta=0.; //角度为0 请注意，为什么不直接写为0，而要写成0.呢，大家可以想一下，相信你一定能想出
}
//位姿的 + 操作符的重载 下面的运算操作符跟点差不多，一看就会，就不具体细讲了
template <class T, class A>
orientedpoint<T,A> operator+(const orientedpoint<T,A>& p1, const orientedpoint<T,A>& p2){
return orientedpoint<T,A>(p1.x+p2.x, p1.y+p2.y, p1.theta+p2.theta);
}
/*位姿的 - 操作符的重载*/
template <class T, class A>
orientedpoint<T,A> operator - (const orientedpoint<T,A> & p1, const orientedpoint<T,A> & p2){
return orientedpoint<T,A>(p1.x-p2.x, p1.y-p2.y, p1.theta-p2.theta);
}
/*位姿的 * 操作符的重载 乘以一个数*/
template <class T, class A>
orientedpoint<T,A> operator * (const orientedpoint<T,A>& p, const T& v){
return orientedpoint<T,A>(p.x*v, p.y*v, p.theta*v);
}
/*位姿的 * 操作符的重载 乘以一个数*/
template <class T, class A>
orientedpoint<T,A> operator * (const T& v, const orientedpoint<T,A>& p){
return orientedpoint<T,A>(p.x*v, p.y*v, p.theta*v);
}
/*
@desc 两个位姿的差值，算出来P1在以P2为原点的坐标系里面的坐标。
*/
template <class T, class A>
orientedpoint<T,A> absoluteDifference(const orientedpoint<T,A>& p1,const orientedpoint<T,A>& p2)
{
orientedpoint<T,A> delta=p1-p2;
delta.theta=atan2(sin(delta.theta), cos(delta.theta));
double s=sin(p2.theta), c=cos(p2.theta);
return orientedpoint<T,A>(c*delta.x+s*delta.y,
-s*delta.x+c*delta.y, delta.theta);
}
/*
@desc 两个位姿的和 p2表示增量。该函数表示在P1的位姿上，加上一个P2的增量
*/
template <class T, class A>
orientedpoint<T,A> absoluteSum(const orientedpoint<T,A>& p1,const orientedpoint<T,A>& p2)
{
double s=sin(p1.theta), c=cos(p1.theta);
return orientedpoint<T,A>(c*p2.x-s*p2.y,
s*p2.x+c*p2.y, p2.theta) + p1;
}
/*一个位姿和一个位置的和*/
template <class T, class A>
point<T> absoluteSum(const orientedpoint<T,A>& p1,const point<T>& p2){
double s=sin(p1.theta), c=cos(p1.theta);
return point<T>(c*p2.x-s*p2.y, s*p2.x+c*p2.y) + (point<T>) p1;
}
/*点的比较函数 一般用于排序*/
template <class T>
struct pointcomparator{
bool operator ()(const point<T>& a, const point<T>& b) const {
return a.x<b.x || (a.x==b.x && a.y<b.y);
}
};
/*点构成的角度的比较函数 一般用于排序*/
template <class T>
struct pointradialcomparator{
point<T> origin;
bool operator ()(const point<T>& a, const point<T>& b) const {
point<T> delta1=a-origin;
point<T> delta2=b-origin;
return (atan2(delta1.y,delta1.x)<atan2(delta2.y,delta2.x));
}
};
/*把两个点的最大的X和最大的Y返回*/
template <class T>
inline point<T> max(const point<T>& p1, const point<T>& p2){
point<T> p=p1;
p.x=p.x>p2.x?p.x:p2.x;
p.y=p.y>p2.y?p.y:p2.y;
return p;
}
/*把两个点的最小的X和最小的Y返回*/
template <class T>
inline point<T> min(const point<T>& p1, const point<T>& p2){
point<T> p=p1;
p.x=p.x<p2.x?p.x:p2.x;
p.y=p.y<p2.y?p.y:p2.y;
return p;
}
/*
P1P2这份方向的线性插值
T1表示P1，T2表示P2
T3表示P1和P2之间的一个点
*/
template <class T, class F>
inline point<T> interpolate(const point<T>& p1, const F& t1, const point<T>& p2, const F& t2, const F& t3){
F gain=(t3-t1)/(t2-t1);
point<T> p=p1+(p2-p1)*gain;
return p;
}
/*
两个位姿之间的插值
T1.T2.T3的定义如上个函数
*/
template <class T, class A, class F>
inline orientedpoint<T,A>
interpolate(const orientedpoint<T,A>& p1, const F& t1, const orientedpoint<T,A>& p2, const F& t2, const F& t3){
F gain=(t3-t1)/(t2-t1);
orientedpoint<T,A> p;
p.x=p1.x+(p2.x-p1.x)*gain;
p.y=p1.y+(p2.y-p1.y)*gain;
double s=sin(p1.theta)+sin(p2.theta)*gain,
c=cos(p1.theta)+cos(p2.theta)*gain;
p.theta=atan2(s,c);
return p;
}
/*两个点的欧式距离*/
template <class T>
inline double euclidianDist(const point<T>& p1, const point<T>& p2){
return hypot(p1.x-p2.x, p1.y-p2.y);
}
/*两个位姿的欧式距离*/
//方法1：使用hypot函数
template <class T, class A>
inline double euclidianDist(const orientedpoint<T,A>& p1, const orientedpoint<T,A>& p2){
return hypot(p1.x-p2.x, p1.y-p2.y); //hypot（）函数：是求横纵坐标的平方和的平方根，即求两点间的距离
}
//方法2：使用欧氏距离公式
template <class T,class A>
inline double squareDist(const orientedpoint<T,A>& p1,const orientedpoint<T,A>& p2)
{
return (p1.x - p2.x)*(p1.x-p2.x) + (p1.y-p2.y)*(p1.y-p2.y);
}
/*位姿和点的欧式距离*/
template <class T, class A>
inline double euclidianDist(const orientedpoint<T,A>& p1, const point<T>& p2){
return hypot(p1.x-p2.x, p1.y-p2.y);
}
/*点和位姿的欧式距离*/
template <class T, class A>
inline double euclidianDist(const point<T>& p1, const orientedpoint<T,A>& p2 ){
return hypot(p1.x-p2.x, p1.y-p2.y);
}
重定向指令，后面的代码基本都使用这几个类型，请大家牢记。
typedef point<int> IntPoint; //整型的点
typedef point<double> Point; //double类型的点
typedef orientedpoint<double, double> OrientedPoint; //double类型的位姿 回到前面提到的一个问题，这下你知道为什么是0.而不是0了吧
2、stat.h
接下来，是stat.h文件，该文件定义了协方差矩阵的类和高斯函数等。需要包含的头文件是上面的point.h和vector，就不细讲了。
namespace GMapping
{
double sampleGaussian(double sigma,unsigned int S=0); //样本正态分布函数
double evalLogGaussian(double sigmaSquare, double delta); //计算对数高斯函数的值
struct Covariance3 //协方差矩阵
{
Covariance3 operator + (const Covariance3 & cov) const; //协方差矩阵的加法重载符
static Covariance3 zero; //协方差矩阵的静态常量
double xx, yy, tt, xy, xt, yt; //协方差矩阵的各个元素
};
struct EigenCovariance3 //协方差矩阵的特征值和特征向量
{
EigenCovariance3(); //构造函数
EigenCovariance3(const Covariance3& c); //构造函数重载，用于初始化特征值和特征向量
EigenCovariance3 rotate(double angle) const; //旋转协方差矩阵
OrientedPoint sample() const; //目前，这步没看懂
double eval[3]; //特征值
double evec[3][3]; //特征向量
};
struct Gaussian3 //高斯函数
{
OrientedPoint mean; //高斯函数的均值
EigenCovariance3 covariance; //高斯函数的协方差矩阵（采用Eigen实现的）
Covariance3 cov; //高斯函数的协方差矩阵
double eval(const OrientedPoint& p) const; //OrientedPoint:x,y,theta都是double类型
};
};
namespace GMapping
{
double pf_ran_gaussian(double sigma) //样本正太分布函数
{
double x1, x2, w; //x1,x2是两个随机数，w是他们的积
double r; //r是随机数
do
{
do { r = drand48(); } while (r == 0.0);//drand48(): 返回服从均匀分布的·[0.0, 1.0) 之间的 double 型随机数。
x1 = 2.0 * r - 1.0;
do { r = drand48(); } while (r == 0.0);
x2 = 2.0 * drand48() - 1.0;
w = x1*x1 + x2*x2;
} while(w > 1.0 || w==0.0); //直到w<1为止
return(sigma * x2 * sqrt(-2.0*log(w)/w)); //sqrt():计算一个非负实数的平方根
}
double sampleGaussian(double sigma, unsigned int S) //样本正太分布函数
{
if(S!=0) //如果S不等于0，则返回S个样本正太分布函数的均值
srand(S); //S是随机数种子,srand():生成随机数
if (sigma==0) //如果sigma等于0，则返回0
return 0;
return pf_ran_gaussian (sigma); //返回样本正太分布函数的均值
}
double evalLogGaussian(double sigmaSquare, double delta) //计算对数高斯函数的值
{
if (sigmaSquare<=0) //如果sigmaSquare小于等于0 sigmaSquare是协方差矩阵的平方，
sigmaSquare=1e-4; //如果小于等于0，则设置为1e-4
return -.5*delta*delta/sigmaSquare-.5*log(2*M_PI*sigmaSquare); //计算对数高斯函数的值的具体实现
}
double Gaussian3::eval(const OrientedPoint& p) const //计算高斯函数的值
{
OrientedPoint q=p-mean; //q是p减去均值的差值，p是传入的OrientedPoint类型的变量，mean是高斯函数的均值
q.theta=atan2(sin(p.theta-mean.theta),cos(p.theta-mean.theta)); //计算q的theta值
double v1,v2,v3; //v1,v2,v3是协方差矩阵的各个元素
v1 = covariance.evec[0][0]*q.x+covariance.evec[1][0]*q.y+covariance.evec[2][0]*q.theta; //计算v1的值
v2 = covariance.evec[0][1]*q.x+covariance.evec[1][1]*q.y+covariance.evec[2][1]*q.theta; //计算v2的值
v3 = covariance.evec[0][2]*q.x+covariance.evec[1][2]*q.y+covariance.evec[2][2]*q.theta; //计算v3的值
return evalLogGaussian(covariance.eval[0], v1)+evalLogGaussian(covariance.eval[1], v2)+evalLogGaussian(covariance.eval[2], v3); //计算对数高斯函数的值(具体实现)
}
};
3、autoptr.h和macro_pramas.h
接下来，是autoptr.h文件，功能是定义了各种指针，类似于智能指针
namespace GMapping
{
template <class X>
class autoptr //创建指针类
{
protected:
public:
struct reference //引用的结构体
{
X* data; //引用的数据
unsigned int shares; //引用的次数
};
inline autoptr(X* p=(X*)(0)); //构造函数，p为指针，默认为0
inline autoptr(const autoptr<X>& ap); //拷贝构造函数
inline autoptr& operator=(const autoptr<X>& ap); //赋值操作符
inline ~autoptr(); //析构函数
inline operator int() const; //返回引用的次数
inline X& operator*(); //返回引用的数据
inline const X& operator*() const; //返回引用的数据（重载）
reference * m_reference; //引用的指针
protected:
};
template <class X>
autoptr<X>::autoptr(X* p) //构造函数，p为指针
{
m_reference=0; //引用的指针为0 上一个模板对m_reference已经定义了，所以这里不需要再定义
if (p) //如果p不为0
{
m_reference=new reference; //申请一个引用的指针
m_reference->data=p; //引用的数据为p
m_reference->shares=1; //引用的次数为1
}
}
template <class X>
autoptr<X>::autoptr(const autoptr<X>& ap) //拷贝构造函数
{
m_reference=0; //引用的指针为0
reference* ref=ap.m_reference; //引用的指针为ap的引用的指针
if (ap.m_reference) //如果ap的引用的指针不为0
{
m_reference=ref; //引用的指针为ap的引用的指针
m_reference->shares++; //引用的次数加1
}
}
template <class X>
autoptr<X>& autoptr<X>::operator=(const autoptr<X>& ap)//赋值操作符
{
reference* ref=ap.m_reference; //引用的指针为ap的引用的指针
if (m_reference==ref) //如果引用的指针与ap的引用的指针相等
{
return *this; //返回自身
}
if (m_reference && !(--m_reference->shares)) //如果引用的指针不为0且引用的次数减1后为0
{
delete m_reference->data; //删除引用的数据
delete m_reference; //删除引用的指针
m_reference=0; //更新引用的指针为0
}
if (ref) //如果ap的引用的指针不为0
{
m_reference=ref; //更新引用的指针为ap的引用的指针
m_reference->shares++; //引用的次数加1
}
else
m_reference=0; //更新引用的指针为0
return *this; //返回自身
}
template <class X>
autoptr<X>::~autoptr() //析构函数
{
if (m_reference && !(--m_reference->shares)){
delete m_reference->data;
delete m_reference;
m_reference=0;
}
}
template <class X>
autoptr<X>::operator int() const //返回引用的次数
{
return m_reference && m_reference->shares && m_reference->data;
}
template <class X>
X& autoptr<X>::operator*() //返回引用的数据
{
assert(m_reference && m_reference->shares && m_reference->data); //断言，如果引用的指针不为0且引用的次数不为0且引用的数据不为0
return *(m_reference->data); //返回引用的数据
}
template <class X>
const X& autoptr<X>::operator*() const //返回引用的数据（重载）
{
assert(m_reference && m_reference->shares && m_reference->data); //断言，如果引用的指针不为0且引用的次数不为0且引用的数据不为0
return *(m_reference->data); //返回引用的数据
}
};
最后，是macro_pramas.h文件，功能是设置各种参数，后面会用到这些参数，等后面需要用到的时候再讲。
好啦，这就是本篇文章的内容啦。如果还没有获取资料的朋友，可以关注我的公众号 巧学slam ,输入关键字 算法源码，即可获取。
这也是我初次分享个人所学心得，如有错误之处，欢迎大家指正。
next
下回开始讲解sensor_range和grid文件，我们不见不散。
7969
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
