目前已经有很多博客分享了gmapping的源码分析，之前我也看过几遍，不过一直没有记录，在此记录一下。
从应用gmapping包来讲，ROS中的slam_gmapping包也是调用了openslam_gmapping开源算法，gmapping的代码分为两部分：
https://github.com/ros-perception/openslam_gmapping
https://github.com/ros-perception/slam_gmapping
一、先看数据结构：
（1）粒子的数据结构：
/**This class defines a particle of the filter. Each particle has a map, a pose, a weight and retains the current node in the trajectory tree*/
struct Particle{
/**constructs a particle, given a map
@param map: the particle map
*/
Particle(const ScanMatcherMap& map);
/** @returns the weight of a particle */
inline operator double() const {return weight;}
/** @returns the pose of a particle */
inline operator OrientedPoint() const {return pose;}
/** sets the weight of a particle
@param w the weight
*/
inline void setWeight(double w) {weight=w;}
/** The map */
ScanMatcherMap map;
/** The pose of the robot */
OrientedPoint pose;
/** The pose of the robot at the previous time frame (used for computing thr odometry displacements) */
OrientedPoint previousPose;
/** The weight of the particle */
double weight;
/** The cumulative weight of the particle */
double weightSum;
double gweight;
/** The index of the previous particle in the trajectory tree */
int previousIndex;
/** Entry to the trajectory tree */
TNode* node;
};
typedef std::vector<Particle> ParticleVector;
...
...
inline const ParticleVector& getParticles() const {return m_particles; }
...
...
/**the particles*/
ParticleVector m_particles;
（2）激光lidar获取的数据格式：
RangeReading(unsigned int n_beams, const double* d, const RangeSensor* rs, double time=0);
bool GridSlamProcessor::processScan(const RangeReading & reading, int adaptParticles)
//这个用来准备激光lidar数据为openslam_gmapping能处理的格式
RangeReading* reading_copy = new RangeReading(reading.size(),
&(reading[0]),
static_cast<const RangeSensor*>(reading.getSensor()),
reading.getTime());
扫描二维码关注公众号，回复：
4766343 查看本文章
class RangeSensor: public Sensor{
friend class Configuration;
friend class CarmenConfiguration;
friend class CarmenWrapper;
public:
struct Beam{
OrientedPoint pose; //pose relative to the center of the sensor
double span; //spam=0 indicates a line-like beam
double maxRange; //maximum range of the sensor
double s,c; //sinus and cosinus of the beam (optimization);
};
RangeSensor(std::string name);
RangeSensor(std::string name, unsigned int beams, double res, const OrientedPoint& position=OrientedPoint(0,0,0), double span=0, double maxrange=89.0);
inline const std::vector<Beam>& beams() const {return m_beams;}
inline std::vector<Beam>& beams() {return m_beams;}
inline OrientedPoint getPose() const {return m_pose;}
void updateBeamsLookup();
bool newFormat;
protected:
OrientedPoint m_pose;
std::vector<Beam> m_beams;
};二、函数入口：
int
main(int argc, char** argv)
{
ros::init(argc, argv, "slam_gmapping");
SlamGMapping gn;
gn.startLiveSlam();
ros::spin();
return(0);
}startLiveSlam()中，
（1）先订阅激光雷达的数据： scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "scan", 5);
（2）通过boost::bind开启两个线程：laserCallback()和 publishLoop()，
在核心线程laserCallback中 ：
laserCallback()
——>initMapper() //进行参数初始化
//gsp_->setUpdateDistances(linearUpdate_, angularUpdate_, resampleThreshold_);
//gsp_->GridSlamProcessor::init()。这两个接口也是在openslam_gmapping中定义
——>addScan() //调用openslam_gmappinig核心算法
//gsp_->processScan（）
——>updateMap() //对地图进行更新三、在 addScan()中调用gsp_->processScan()函数，该函数即是openslam_gmappinig核心算法，该函数有一下注意先
bool GridSlamProcessor::processScan(const RangeReading & reading, int adaptParticles)
{
/**retireve the position from the reading, and compute the odometry*/
//得到当前的历程计的位置
OrientedPoint relPose=reading.getPose();
//m_count的作用：仅用于区分第0次和其他，第0次是特殊处理
if (!m_count)
{
m_lastPartPose=m_odoPose=relPose;
}
//write the state of the reading and update all the particles using the motion model
//对于每一个粒子，都从里程计运动模型中采样
//问题1：为什么要从第一个粒子开始获取pose？？？？？？？？
for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++)
{
OrientedPoint& pose(it->pose);
//it->pose 表示机器人上一时刻的最优位置
//relPose 表示里程计算出来的当前位置
//m_odoPose 表示里程计上一次的位置
pose=m_motionModel.drawFromMotion(it->pose, relPose, m_odoPose);
}
// update the output file
if (m_outputStream.is_open())
{
m_outputStream << setiosflags(ios::fixed) << setprecision(6);
m_outputStream << "ODOM ";
m_outputStream << setiosflags(ios::fixed) << setprecision(3) << m_odoPose.x << " " << m_odoPose.y << " ";
m_outputStream << setiosflags(ios::fixed) << setprecision(6) << m_odoPose.theta << " ";
m_outputStream << reading.getTime();
m_outputStream << endl;
}
if (m_outputStream.is_open())
{
m_outputStream << setiosflags(ios::fixed) << setprecision(6);
m_outputStream << "ODO_UPDATE "<< m_particles.size() << " ";
//for循环对于每一个粒子都进行了运动模型的预测更新
for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++)
{
OrientedPoint& pose(it->pose);
m_outputStream << setiosflags(ios::fixed) << setprecision(3) << pose.x << " " << pose.y << " ";
m_outputStream << setiosflags(ios::fixed) << setprecision(6) << pose.theta << " " << it-> weight << " ";
}
m_outputStream << reading.getTime();
m_outputStream << endl;
}
//invoke the callback
onOdometryUpdate();
// accumulate the robot translation and rotation
//根据两次里程计的数据，计算出机器人的线性位移和角度位移。
//relPose表示当前的里程计位姿，m_odoPose表示上一次里程计的位姿。
OrientedPoint move=relPose-m_odoPose;
move.theta=atan2(sin(move.theta), cos(move.theta));
//计算机器人在进行激光雷达更新之前，走了多远的距离，以及平移了多少角度
m_linearDistance+=sqrt(move*move);
m_angularDistance+=fabs(move.theta);
// if the robot jumps throw a warning
if (m_linearDistance>m_distanceThresholdCheck)
{
cerr << "***********************************************************************" << endl;
cerr << "********** Error: m_distanceThresholdCheck overridden!!!! *************" << endl;
cerr << "m_distanceThresholdCheck=" << m_distanceThresholdCheck << endl;
cerr << "Old Odometry Pose= " << m_odoPose.x << " " << m_odoPose.y
<< " " <<m_odoPose.theta << endl;
cerr << "New Odometry Pose (reported from observation)= " << relPose.x << " " << relPose.y
<< " " <<relPose.theta << endl;
cerr << "***********************************************************************" << endl;
cerr << "** The Odometry has a big jump here. This is probably a bug in the **" << endl;
cerr << "** odometry/laser input. We continue now, but the result is probably **" << endl;
cerr << "** crap or can lead to a core dump since the map doesn't fit.... C&G **" << endl;
cerr << "***********************************************************************" << endl;
}
m_odoPose=relPose;
bool processed=false;
// process a scan only if the robot has traveled a given distance or a certain amount of time has elapsed
//问题2：m_count表示调用processSacn的次数，仅表示初始第0次与其余次数的区分
if (! m_count
|| m_linearDistance>=m_linearThresholdDistance
|| m_angularDistance>=m_angularThresholdDistance
|| (period_ >= 0.0 && (reading.getTime() - last_update_time_) > period_))
{
last_update_time_ = reading.getTime();
if (m_outputStream.is_open())
{
m_outputStream << setiosflags(ios::fixed) << setprecision(6);
m_outputStream << "FRAME " << m_readingCount;
m_outputStream << " " << m_linearDistance;
m_outputStream << " " << m_angularDistance << endl;
}
if (m_infoStream)
{
m_infoStream << "update frame " << m_readingCount << endl
<< "update ld=" << m_linearDistance << " ad=" << m_angularDistance << endl;
}
cerr << "Laser Pose= " << reading.getPose().x << " " << reading.getPose().y << " " << reading.getPose().theta << endl;
//this is for converting the reading in a scan-matcher feedable form
assert(reading.size()==m_beams);
double * plainReading = new double[m_beams];
for(unsigned int i=0; i<m_beams; i++)
{
plainReading[i]=reading[i];
}
m_infoStream << "m_count " << m_count << endl;
//把激光雷达数据转换成openslam_gmapping可以计算的格式
RangeReading* reading_copy = new RangeReading(reading.size(),
&(reading[0]),
static_cast<const RangeSensor*>(reading.getSensor()),
reading.getTime());
//如果不是第一帧数据
if (m_count>0)
{
//为每个粒子进行scanMatch，计算出每个粒子的最优位姿，
scanMatch(plainReading);
if (m_outputStream.is_open())
{
m_outputStream << "LASER_READING "<< reading.size() << " ";
m_outputStream << setiosflags(ios::fixed) << setprecision(2);
for (RangeReading::const_iterator b=reading.begin(); b!=reading.end(); b++)
{
m_outputStream << *b << " ";
}
OrientedPoint p=reading.getPose();
m_outputStream << setiosflags(ios::fixed) << setprecision(6);
m_outputStream << p.x << " " << p.y << " " << p.theta << " " << reading.getTime()<< endl;
m_outputStream << "SM_UPDATE "<< m_particles.size() << " ";
for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++)
{
const OrientedPoint& pose=it->pose;
m_outputStream << setiosflags(ios::fixed) << setprecision(3) << pose.x << " " << pose.y << " ";
m_outputStream << setiosflags(ios::fixed) << setprecision(6) << pose.theta << " " << it-> weight << " ";
}
m_outputStream << endl;
}
onScanmatchUpdate();
//由于scanmatch之中对粒子的权重进行了更新，那么这时候 各个粒子的轨迹上的累计权重都需要重新计算
updateTreeWeights(false);
if (m_infoStream)
{
m_infoStream << "neff= " << m_neff << endl;
}
if (m_outputStream.is_open())
{
m_outputStream << setiosflags(ios::fixed) << setprecision(6);
m_outputStream << "NEFF " << m_neff << endl;
}
//粒子重采样，根据Neff的大小来进行重采样，不但进行了重采样，也对地图进行了更新
//重采样的过程里面有第二步的优化，
//问题3：resample和scanMatch里都进行了地图的更新
resample(plainReading, adaptParticles, reading_copy);
}
else //如果是第一帧激光数据
{
m_infoStream << "Registering First Scan"<< endl;
//如果是第一帧数据，则可以直接结算ActiveArea，因为这个时候，机器人的位姿是十分确定的，就是（0，0，0）
for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++)
{
m_matcher.invalidateActiveArea();
m_matcher.computeActiveArea(it->map, it->pose, plainReading);
m_matcher.registerScan(it->map, it->pose, plainReading);
// cyr: not needed anymore, particles refer to the root in the beginning!
//为每个粒子创建路径的第一个节点，该节点的权重为0，父节点为it->node（这个时候为NULL）
//因为第一个节点就是轨迹的根，所以没有父节点
TNode* node=new TNode(it->pose, 0., it->node, 0);
//node->reading=0;
node->reading = reading_copy;
it->node=node;
}
}
//cerr << "Tree: normalizing, resetting and propagating weights at the end..." ;
updateTreeWeights(false);
//cerr << ".done!" <<endl;
delete [] plainReading;
m_lastPartPose=m_odoPose; //update the past pose for the next iteration
m_linearDistance=0;
m_angularDistance=0;
m_count++;
processed=true;
//keep ready for the next step
for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++)
{
it->previousPose=it->pose;
}
}
if (m_outputStream.is_open())
{
m_outputStream << flush;
}
m_readingCount++;
return processed;
}gmapping 算法三大步骤：
（1）、drawFromMotion() ：进行里程计运动模型更新
添加噪声。
（2）、scanMatch()：进行权重计算。利用最近一次的观测来提高Proposal distribution进行优化。
rbpf-gmapping直接使用的是运动模型作为提议分布
optimize()里面有个score()函数进行权重计算；
likelihoodAndScore()重新计算权重。
inline void GridSlamProcessor::scanMatch(const double* plainReading)
{
// sample a new pose from each scan in the reference
double sumScore=0;
//对于每个粒子都进行优化
//问题四：为什么对每个粒子都进行了优化？？？？
for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++)
{
OrientedPoint corrected;
double score, l, s;
//计算粒子的最优位姿
score=m_matcher.optimize(corrected, it->map, it->pose, plainReading);
// it->pose=corrected;
if (score>m_minimumScore)
{
it->pose=corrected;
}
else
{
if (m_infoStream)
{
m_infoStream << "Scan Matching Failed, using odometry. Likelihood=" << l <<std::endl;
m_infoStream << "lp:" << m_lastPartPose.x << " " << m_lastPartPose.y << " "<< m_lastPartPose.theta <<std::endl;
m_infoStream << "op:" << m_odoPose.x << " " << m_odoPose.y << " "<< m_odoPose.theta <<std::endl;
}
}
//粒子的最优位姿计算了之后，重新计算粒子的权重，optimize得到了最优位姿，likelihoodAndScore算最优位姿的得分，
m_matcher.likelihoodAndScore(s, l, it->map, it->pose, plainReading);
sumScore+=score;
it->weight+=l;
it->weightSum+=l;
//set up the selective copy of the active area
//by detaching the areas that will be updated
m_matcher.invalidateActiveArea();
//建图的过程，生成地图的函数，使用Bresenham算法
m_matcher.computeActiveArea(it->map, it->pose, plainReading);
}
if (m_infoStream)
m_infoStream << "Average Scan Matching Score=" << sumScore/m_particles.size() << std::endl;
}（3）、resample()重采样：第二步分优化，计算 Neff 有效粒子数. ——》gridslamprocessor.hxx
GridSlamProcessor::resample(const double* plainReading, int adaptSize, const RangeReading* reading)
参数parameter：
（A）plainReading
（B）adaptSize
（C）reading即 ==reading_copy = new RangeReading(reading.size(),
&(reading[0]),
static_cast<const RangeSensor*>(reading.getSensor()),
reading.getTime());
inline bool GridSlamProcessor::resample(const double* plainReading, int adaptSize, const RangeReading* reading){
bool hasResampled = false;
TNodeVector oldGeneration;
for (unsigned int i=0; i<m_particles.size(); i++){
oldGeneration.push_back(m_particles[i].node);
}
//当m_neff小于阈值m_resampleThreshold的时候，才进行重采样，否则不进行重采样
if (m_neff<m_resampleThreshold*m_particles.size())
{
if (m_infoStream)
m_infoStream << "*************RESAMPLE***************" << std::endl;
//采取重采样方法决定，哪些粒子会保留，保留的粒子会保留下标，有些粒子会重复采样
//而另外一些粒子会消失掉
uniform_resampler<double, double> resampler;
m_indexes=resampler.resampleIndexes(m_weights, adaptSize);
if (m_outputStream.is_open())
{
m_outputStream << "RESAMPLE "<< m_indexes.size() << " ";
for (std::vector<unsigned int>::const_iterator it=m_indexes.begin(); it!=m_indexes.end(); it++)
{
m_outputStream << *it << " ";
}
m_outputStream << std::endl;
}
onResampleUpdate();
//BEGIN: BUILDING TREE
//重采样之后的粒子
ParticleVector temp;
unsigned int j=0;
//要删除的粒子下标
std::vector<unsigned int> deletedParticles; //this is for deleteing the particles which have been resampled away.
// cerr << "Existing Nodes:" ;
for (unsigned int i=0; i<m_indexes.size(); i++)
{
// cerr << " " << m_indexes[i];
while(j<m_indexes[i])
{
deletedParticles.push_back(j);
j++;
}
if (j==m_indexes[i])
j++;
Particle & p=m_particles[m_indexes[i]];
TNode* node=0;
TNode* oldNode=oldGeneration[m_indexes[i]];
// cerr << i << "->" << m_indexes[i] << "B("<<oldNode->childs <<") ";
node=new TNode(p.pose, 0, oldNode, 0);
//node->reading=0;
node->reading=reading;
// cerr << "A("<<node->parent->childs <<") " <<endl;
temp.push_back(p);
temp.back().node=node;
temp.back().previousIndex=m_indexes[i];
}
while(j<m_indexes.size())
{
deletedParticles.push_back(j);
j++;
}
// cerr << endl;
std::cerr << "Deleting Nodes:";
for (unsigned int i=0; i<deletedParticles.size(); i++)
{
std::cerr <<" " << deletedParticles[i];
delete m_particles[deletedParticles[i]].node;
m_particles[deletedParticles[i]].node=0;
}
std::cerr << " Done" <<std::endl;
//END: BUILDING TREE
std::cerr << "Deleting old particles..." ;
m_particles.clear();
std::cerr << "Done" << std::endl;
std::cerr << "Copying Particles and Registering scans...";
for (ParticleVector::iterator it=temp.begin(); it!=temp.end(); it++)
{
it->setWeight(0);
m_matcher.invalidateActiveArea();
m_matcher.registerScan(it->map, it->pose, plainReading);
//问题五：在容器最后增加一个新的位置
m_particles.push_back(*it);
}
std::cerr << " Done" <<std::endl;
hasResampled = true;
}
else
{
int index=0;
std::cerr << "Registering Scans:";
TNodeVector::iterator node_it=oldGeneration.begin();
for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++)
{
//create a new node in the particle tree and add it to the old tree
//BEGIN: BUILDING TREE
TNode* node=0;
node=new TNode(it->pose, 0.0, *node_it, 0);
//node->reading=0;
node->reading=reading;
it->node=node;
//END: BUILDING TREE
m_matcher.invalidateActiveArea();
m_matcher.registerScan(it->map, it->pose, plainReading);
it->previousIndex=index;
index++;
node_it++;
}
std::cerr << "Done" <<std::endl;
}
//END: BUILDING TREE
return hasResampled;
}
四、地图更新——》在scanmatcher.cpp文件中
(1)computeActiveArea()：采用breshham划线算法计算出激光激光经过哪些栅格区域，这些栅格区域应该要分配内存
(2)registerScan()：分配内存，生成地图