GMapping的建图引擎
通过对GMapping的ROS封装的分析[1, 2]，我们知道GMapping使用一个类型为GridSlamProcessor的建图引擎进行SLAM。 它实现了一个Raw-Blackwellized粒子滤波器，每个粒子都有它自己的一个地图和机器人位置。每当接收到一个新的数据对，它记录了里程计和激光扫描数据。 该引擎都会先根据运动模型来预测各个粒子的机器人位置，然后通过该位置来初始化扫描匹配算法。通过扫描匹配器对粒子进行局部优化， 最终根据各个粒子的地图来修正机器人的位置。
本文中，我们将详细拆解这个建图引擎。
1. 构造与初始化
在ROS封装的初始化过程中， 我们通过默认构造函数创建了一个GridSlamProcessor对象gsp_。 下面是该构造函数的实现，在构造之初就是用标准输出cout来初始化成员m_infoStream作为日志的输出目标。在函数中对一些配置赋予了初值，其中period_记录了激光雷达的扫描周期， m_obsSigmaGain则描述了似然度的平滑因子，m_resampleThreshold用于判定是否需要重采样，m_minimumScore是评价扫描匹配结果是否理想的最小得分。
GridSlamProcessor::GridSlamProcessor(): m_infoStream(cout) {
period_ = 5.0;
m_obsSigmaGain=1;
m_resampleThreshold=0.5;
m_minimumScore=0.;
}
当ROS封装第一次获得激光数据时，会调用建图引擎gsp_的成员函数setSensorMap，来配置gsp_的激光传感器。
该函数的唯一参数就是一个C++标准库中的map容器，
其定义是sensor.h文件中的
typedef std::map<std::string, Sensor*> SensorMap;。通过该容器，我们可通过字符串的形式获取传感器对象。
void GridSlamProcessor::setSensorMap(const SensorMap& smap) {
目前GMapping只支持一种"FLASER"的雷达，它假设雷达对机器人前面进行扫描，并且安装在机器人的中心(For now detect the readings of only the front laser, and assume its pose is in the center of the robot)。下面的代码片段用于获取传感器"FLASER"，如果map中没有相应的传感器就报错。
SensorMap::const_iterator laser_it=smap.find(std::string("FLASER"));
if (laser_it==smap.end()){
cerr << "Attempting to load the new carmen log format" << endl;
laser_it=smap.find(std::string("ROBOTLASER1"));
assert(laser_it!=smap.end());
}
接下来，获取实际的传感器对象，并检查对象存在并且激光波束数量不为0。
const RangeSensor* rangeSensor=dynamic_cast<const RangeSensor*>((laser_it->second));
assert(rangeSensor && rangeSensor->beams().size());
然后我们获取传感器的波束数量用m_beams记录，并用angles记录各个波束对应的角度。最后根据波束数量、波束角度和传感器位置配置扫描匹配器的参数。
m_beams=static_cast<unsigned int>(rangeSensor->beams().size());
double* angles=new double[rangeSensor->beams().size()];
for (unsigned int i=0; i < m_beams; i++){
angles[i]=rangeSensor->beams()[i].pose.theta;
m_matcher.setLaserParameters(m_beams, angles, rangeSensor->getPose());
delete [] angles;
}
在ROS封装的初始化的最后阶段，调用了建图引擎的init函数完成了整个系统的初始化操作。如下面的代码片段所示，该函数有7个参数。其中size表示粒子数量， xmin、ymin、xmax、ymax一起描述了地图尺寸，它们分别描述了地图中x和y的最小值和最大值。delta则是地图的分辨率或者说比例尺，即一个栅格所对应的实际地图尺寸。 initialPose则是机器人的起始位置。
void GridSlamProcessor::init(unsigned int size, double xmin, double ymin, double xmax,
double ymax, double delta, OrientedPoint initialPose) {
在函数的一开始，先用成员变量分别记录下关于粒子和地图的配置信息。
m_xmin=xmin; m_ymin=ymin;
m_xmax=xmax; m_ymax=ymax;
m_delta=delta;
然后为了创建初始的粒子集合，我们做一些准备工作。首先清空粒子集合，接着创建了一个TNode对象node和地图对象lmap。TNode是GridSlamProcessor内部定义的结构体， 它用于各个粒子记录机器人的运动轨迹，我们将在后序的内容中详细拆解它。 关于地图对象，我们已经在GMapping的地图结构中详细解析了。
m_particles.clear();
TNode* node=new TNode(initialPose, 0, 0, 0);
ScanMatcherMap lmap(Point(xmin+xmax, ymin+ymax)*.5, xmax-xmin, ymax-ymin, delta);
再然后，我们在一个循环中对每个粒子进行初始化操作。
m_particles是由typedef std::vector<Particle> ParticleVector;重定义的C++标准库中的vector容器。
在循环中，我们为每个粒子赋予了刚刚定义的地图对象和初始位置，并设定粒子权重为0。
for (unsigned int i=0; i<size; i++){
m_particles.push_back(Particle(lmap));
m_particles.back().pose=initialPose;
m_particles.back().previousPose=initialPose;
m_particles.back().setWeight(0);
m_particles.back().previousIndex=0;
m_particles.back().node= node;
}
最后，我们为引擎的一些状态信息赋予了初值。其中m_neff是GMapping用于自适应的进行重采样的参考依据。
m_neff=(double)size;
m_count=0;
m_readingCount=0;
m_linearDistance=m_angularDistance=0;
}
2. 粒子滤波更新过程
通过解析ROS封装的更新过程，我们了解到，每当有合适的激光传感器和对应的里程计数据， 就会先经过一系列的格式转换操作，构建一个GMapping中的标准扫描读数对象reading， 并最终通过引擎的成员函数processScan来处理读数。
如下是processScan函数的实现，它有两个参数。其中reading就是刚刚提到的标准扫描读数；adaptParticles指示了在重采样过程中选用的粒子数量， 默认情况下该值为0，表示不指定粒子数量，每次重采样后的粒子集合与原来的粒子集合大小一样。该函数返回一个布尔类型的数据，用于指示是否成功更新了建图引擎。
bool GridSlamProcessor::processScan(const RangeReading & reading, int adaptParticles) {
首先，我们通过传感器读数的接口获取传感器的位置。建图引擎的m_count是迭代的计数器，如果该值为0，意味着当前是第一次迭代，用该值对里程计的起始位置进行初始化。
OrientedPoint relPose=reading.getPose();
if (!m_count)
m_lastPartPose=m_odoPose=relPose;
接着遍历粒子集合，通过运动模型m_motionModel预测各个粒子的位置。关于运动模型，我们将在后序的内容中予以介绍。
for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
OrientedPoint& pose(it->pose);
pose=m_motionModel.drawFromMotion(it->pose, relPose, m_odoPose);
}
GMapping中有很多日志输出的代码，这里的分析将它们都省略了。然后调用了onOdometryUpdata函数用于处理里程计更新事件，实际上源码中这个函数什么也没有做， 它存在的意义就是提供一种回调机制，方便日后的扩展。
// 省略了一堆日志输出代码
onOdometryUpdate();
接下来，我们计算机器人的运动量。通过对最新数据中的位置坐标与上次机器人的位置坐标做差，我们可以获得机器人的位移向量move。通过一系列的三角函数运算，我们对转向做了调整， 使其处于区间\((-\pi, \pi]\)中。
// accumulate the robot translation and rotation
OrientedPoint move = relPose - m_odoPose;
move.theta = atan2(sin(move.theta), cos(move.theta));
m_linearDistance += sqrt(move*move);
m_angularDistance += fabs(move.theta);
机器人的速度是有限的，所以当机器人位置发生较大变化的时候，很可能系统出现了某些未知的错误，这里通过参数m_distanceThresholdCheck做出判定， 源码中只是输出了一堆日志没有做任何特殊的操作。然后更新里程计位置。
if (m_linearDistance > m_distanceThresholdCheck) {
// 省略日志
}
m_odoPose = relPose;
定义局部变量processed标记是否对当前的传感器数据处理，该变量将用作本函数的返回值。
bool processed=false;
接下来，只有在初次更新，或者机器人有显著的位移，或者时间上有显著距离的时候，处理传感器数据。m_count是对处理的传感器数据的计数，如果该值为零说明是初次更新。 m_linearDistance和m_angularDistance分别保留了当前运动的线距离和角距离，将之与用户定义的阈值做对比来判定是否存在显著位移。period_则是所谓的更新周期， 如果距离上次的更新时间超出了更新周期，则说明有显著的时间距离。
if (! m_count
|| m_linearDistance >= m_linearThresholdDistance
|| m_angularDistance >= m_angularThresholdDistance
|| (period_ >= 0.0 && (reading.getTime() - last_update_time_) > period_)) {
// 处理激光扫描数据
}
if (m_outputStream.is_open())
m_outputStream << flush;
m_readingCount++;
return processed;
}
下面的代码是上述更新条件满足的时候，用于处理传感器数据的片段。首先，用变量last_update_time_记录下当前传感器数据的获取时间。然后，断言传感器的激光波束数量与引擎的一致， 并使用临时变量plainReading保存传感器数据。
last_update_time_ = reading.getTime();
// 省略一堆日志
assert(reading.size() == m_beams);
double * plainReading = new double[m_beams];
for(unsigned int i = 0; i < m_beams; i++)
plainReading[i] = reading[i];
同时建立一个传感器数据拷贝。
RangeReading* reading_copy = new RangeReading(reading.size(), &(reading[0]),
static_cast(reading.getSensor()), reading.getTime());
除了第一次，以后的每个更新过程都是如下的几个步骤。通过函数scanMatch进行扫描匹配(参考GMapping的粒子滤波框架)。 函数onScanmatchUpdate与onOdometryUpdate一样，在源码中也是一个空函数，提供了扩展的可能。然后通过函数updateTreeWeights来更新粒子权重，最后调用resample进行重采样。 这个过程结合之前通过运动模型进行预测的操作，就是GMapping的粒子滤波迭代过程。
if (m_count>0) {
scanMatch(plainReading);
onScanmatchUpdate();
updateTreeWeights(false);
resample(plainReading, adaptParticles, reading_copy);
}
否则就是首次更新，我们需要对每个粒子进行初始化。
else {
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!
TNode* node=new TNode(it->pose, 0., it->node, 0);
node->reading = reading_copy;
it->node=node;
}
}
最后再次整理一下粒子权重。并未下次迭代记录必要的累积数据。
updateTreeWeights(false);
delete [] plainReading;
m_lastPartPose=m_odoPose; //update the past pose for the next iteration
m_linearDistance=0;
m_angularDistance=0;
m_count++;
processed=true;
for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++)
it->previousPose=it->pose;
3. 完
在本文中，我们讨论了GMapping的建图引擎。详细的分析了它的初始化和更新过程。在更新过程中涉及到四个子过程：运动更新过程、扫描匹配过程、权重更新过程、重采样过程。 其中运动更新过程将在运动模型中讨论； 在介绍扫描匹配器的时候探索扫描匹配过程； 分析粒子和轨迹树的时候说明权重更新过程； 并用一个专题来介绍重采样过程。