SLAM GMapping（4）SLAM处理器
1. SLAM处理器
在 《SLAM GMapping（1）ROS封装》 中了解到使用的是一个类型为GridSlamProcessor的对象进行SLAM
SLAM处理器以激光传感器的扫描数据和里程计位姿作为输入
通过Raw-Blackwellized粒子滤波器完成SLAM任务
总体上可分为四个部分
- 运动更新
- 扫描匹配
- 权重更新
- 重采样
2. 构造
通过默认构造函数创建了一个GridSlamProcessor对象gsp_
在openslam_gmapping/gridfastslam/gridslamprocessor.cpp中可以看到就是相关参数的设置：
/**
* @brief 构造函数
*/
GridSlamProcessor::GridSlamProcessor() : m_infoStream(cout)
{
period_ = 5.0; /* 激光雷达数据扫描处理的间隔时间 */
m_obsSigmaGain = 1; /* 似然度的平滑因子 */
m_resampleThreshold = 0.5; /* 重新采样阀值 */
m_minimumScore = 0.; /* 扫描匹配结果理想所需的最低分数线 */
}
/**
* @brief 构造函数
*
* @param gsp SLAM处理器对象
*/
GridSlamProcessor::GridSlamProcessor(const GridSlamProcessor &gsp)
: last_update_time_(0.0), m_particles(gsp.m_particles), m_infoStream(cout)
{
period_ = 5.0; /* 激光雷达的扫描周期 */
m_obsSigmaGain = gsp.m_obsSigmaGain;
m_resampleThreshold = gsp.m_resampleThreshold; /* 重采样阀值 */
m_minimumScore = gsp.m_minimumScore; /* 扫描匹配结果理想所需的最低分数线 */
m_beams = gsp.m_beams; /* 激光波束数量 */
m_indexes = gsp.m_indexes; /* 重采样后的粒子指标 */
m_motionModel = gsp.m_motionModel; /* 运动模型 */
m_resampleThreshold = gsp.m_resampleThreshold; /* 重采样阀值（重复） */
m_matcher = gsp.m_matcher; /* 扫描匹配器 */
m_count = gsp.m_count; /* 扫描处理迭代的计数器 */
m_readingCount = gsp.m_readingCount; /* 读取激光雷达传感器数据次数 */
m_lastPartPose = gsp.m_lastPartPose; /* 上一次的位姿 */
m_pose = gsp.m_pose;
m_odoPose = gsp.m_odoPose; /* 里程的位姿 */
m_linearDistance = gsp.m_linearDistance; /* 直线距离 */
m_angularDistance = gsp.m_angularDistance; /* 角度距离 */
m_neff = gsp.m_neff; /* 衡量粒子权重的相似性，Neff越大，粒子权重差距越小 */
cerr << "FILTER COPY CONSTRUCTOR" << endl;
cerr << "m_odoPose=" << m_odoPose.x << " " << m_odoPose.y << " " << m_odoPose.theta << endl;
cerr << "m_lastPartPose=" << m_lastPartPose.x << " " << m_lastPartPose.y << " " << m_lastPartPose.theta << endl;
cerr << "m_linearDistance=" << m_linearDistance << endl;
cerr << "m_angularDistance=" << m_linearDistance << endl;
m_xmin = gsp.m_xmin; /* 物理地图x向初始最小尺寸 */
m_ymin = gsp.m_ymin; /* 物理地图y向初始最小尺寸 */
m_xmax = gsp.m_xmax; /* 物理地图x向初始最大尺寸 */
m_ymax = gsp.m_ymax; /* 物理地图y向初始最大尺寸 */
m_delta = gsp.m_delta; /* 分辨率 */
m_regScore = gsp.m_regScore; /* 如果扫描分数高于该阈值，则在地图上注册 */
m_critScore = gsp.m_critScore; /* 如果扫描分数低于该阈值，则上报扫描匹配失败 */
m_maxMove = gsp.m_maxMove; /* 在连续扫描之间允许的最大移动 */
m_linearThresholdDistance = gsp.m_linearThresholdDistance; /* 机器每平移该距离后处理一次激光扫描数据 */
m_angularThresholdDistance = gsp.m_angularThresholdDistance; /* 机器每旋转该弧度后处理一次激光扫描数据 */
m_obsSigmaGain = gsp.m_obsSigmaGain; /* 似然度的平滑因子 */
#ifdef MAP_CONSISTENCY_CHECK
cerr << __func__ << ": trajectories copy.... ";
#endif
TNodeVector v = gsp.getTrajectories();
for (unsigned int i = 0; i < v.size(); i++)
{
m_particles[i].node = v[i];
}
#ifdef MAP_CONSISTENCY_CHECK
cerr << "end" << endl;
#endif
cerr << "Tree: normalizing, resetting and propagating weights within copy construction/cloneing ...";
updateTreeWeights(false);
cerr << ".done!" << endl;
}
/**
* @brief 构造函数
*
* @param infoS 控制台输出
*/
GridSlamProcessor::GridSlamProcessor(std::ostream &infoS) : m_infoStream(infoS)
{
period_ = 5.0; /* 激光雷达的扫描处理周期 */
m_obsSigmaGain = 1; /* 似然度的平滑因子 */
m_resampleThreshold = 0.5; /* 重新采样阀值 */
m_minimumScore = 0.; /* 扫描匹配结果理想所需的最低分数线 */
}
3. 激光传感器
当装第一次获得激光数据时会初始化地图制作器
会用在《SLAM GMapping（2）传感器》 介绍的 RangeSensor 建立激光数据对象并且定义SensorMap传感器关联容器
最后调用gsp_的成员函数setSensorMap，来配置gsp_的激光传感器
函数定义在openslam_gmapping/gridfastslam/gridslamprocessor.cpp中：
/**
* @brief 激光传感器配置函数
*
* @param smap 传感器关联容器
*/
void GridSlamProcessor::setSensorMap(const SensorMap &smap)
{
/*
Construct the angle table for the sensor
FIXME For now detect the readings of only the front laser, and assume its pose is in the center of the robot
*/
/* 构建传感器角度表，假设"FLASER"激光雷达对机器人前面进行扫描，并且安装在机器人的中心 */
/* 获取传感器"FLASER"，如果map中没有相应的传感器就报错，即只支持"FLASER"激光雷达 */
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 = 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; /* 删除临时波束对应角度记录 */
}
只支持"FLASER"激光雷达，其实主要就是设置传感器波束数量的全局变量m_beams和配置扫描匹配器m_matcher的激光参数
4. 初始化
在初始化地图制作器中后续设置了相关的参数，最后做了一个整体的初始化GridSlamProcessor::init
/**
* @brief SLAM处理器初始化函数
*
* @param size 初始化粒子个数
* @param xmin 物理地图x向初始最小尺寸
* @param ymin 物理地图y向初始最小尺寸
* @param xmax 物理地图x向初始最大尺寸
* @param ymax 物理地图y向初始最大尺寸
* @param delta 分辨率
* @param 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;
/* 显示下关于粒子和地图的配置信息 */
if (m_infoStream)
m_infoStream
<< " -xmin " << m_xmin
<< " -xmax " << m_xmax
<< " -ymin " << m_ymin
<< " -ymax " << m_ymax
<< " -delta " << m_delta
<< " -particles " << size << endl;
/* 清空粒子集合 */
m_particles.clear();
/* 创建了一个粒子运动轨迹对象node和地图对象lmap */
TNode *node = new TNode(initialPose, 0, 0, 0);
ScanMatcherMap lmap(Point(xmin + xmax, ymin + ymax) * .5, xmax - xmin, ymax - ymin, delta);
/* 粒子初始化操作 */
for (unsigned int i = 0; i < size; i++)
{
/* 为每个粒子赋予了刚刚定义的地图对象和初始位置，并设定粒子权重为0 */
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;
// this is not needed
// m_particles.back().node=new TNode(initialPose, 0, node, 0);
// we use the root directly
/* 我们直接使用根结点 */
m_particles.back().node = node;
}
/* 初始化一些状态信息 */
m_neff = (double)size; /* 衡量粒子权重的相似性，用于判断重采样 */
m_count = 0; /* 扫描处理迭代的计数器 */
m_readingCount = 0; /* 读取激光雷达传感器数据次数 */
m_linearDistance = m_angularDistance = 0; /* 直线和角度距离 */
}
TNode是用于各个粒子记录机器人的运动轨迹，将在后续内容再介绍
5. 粒子滤波更新
每当有合适的激光传感器和对应的里程计数据
就会先经过一系列的格式转换操作，构建一个GMapping中的标准扫描读数对象reading
并最终通过引擎的成员函数processScan来处理读数
该函数返回一个布尔类型的数据，表示是否成功处理了扫描数据
/**
* @brief 扫描数据处理函数
*
* 粒子滤波迭代过程
*
* @param reading 激光雷达和里程位姿数据
* @param adaptParticles 在重采样过程中选用的粒子数量
* @return processed 是否成功更新了建图引擎
*/
bool GridSlamProcessor::processScan(const RangeReading &reading, int adaptParticles)
参数reading就是刚刚提到的标准扫描读数
adaptParticles指示了在重采样过程中选用的粒子数量
默认情况下该值为0，表示不指定粒子数量，每次重采样后的粒子集合与原来的粒子集合大小一样
根据运动模型，预测更新最新时刻的粒子群的位姿
/* 通过传感器读数的接口获取最新里程计坐标系下激光雷达传感器的位姿 此时已依靠理论里程更新了位姿 */
OrientedPoint relPose = reading.getPose();
/* 第一次扫描处理时，对上一次和里程计起始的位姿进行初始化 */
if (!m_count)
{
m_lastPartPose = m_odoPose = relPose;
}
// write the state of the reading and update all the particles using the motion model
/* 写入读取的状态，并使用运动模型更新所有的粒子 */
/* 1.通过运动模型进行预测 */
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);
}
// invoke the callback
/* 调用回调 */
/* 处理里程计更新事件 实际上源码中这个函数什么也没有做，它存在的意义就是提供一种回调机制，方便日后的扩展 */
onOdometryUpdate();
// accumulate the robot translation and rotation
/* 累积机器人的平移和旋转 */
/* 通过对最新数据中的位置坐标与上次机器人的位置坐标做差，获得机器人的位移向量move */
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)
{
...
}
/* 更新里程计位置 */
m_odoPose = relPose;
/* 标记是否对当前的激光雷达传感器数据处理，将用作本函数的返回值 */
bool processed = false;
在特定情况下才处理数据
/* 初次扫描处理更新，或者机器人有显著的线距离和角距离变化，或者时间上有显著距离的时候，处理激光扫描数据 */
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();
// 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];
}
/* 显示当扫描处理前计数，从0开始 */
m_infoStream << "m_count " << m_count << endl;
/* 拷贝一份激光雷达传感器数据 */
RangeReading *reading_copy =
new RangeReading(reading.size(),
&(reading[0]),
static_cast<const RangeSensor *>(reading.getSensor()),
reading.getTime());
首次处理数据时，对每个粒子进行初始化
/* 首次更新 */
m_infoStream << "Registering First Scan" << endl;
/* 对每个粒子进行初始化 */
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=0;
node->reading = reading_copy;
it->node = node;
}
非首次处理数据时，则正常按步骤进行扫描匹配、权重更新和重采样
/* 2.进行扫描匹配 */
scanMatch(plainReading);
/* 一些匹配后最优位姿日志输出 */
...
/* 源码为空函数，提供了扩展的可能，和onOdometryUpdate一样 */
onScanmatchUpdate();
/* 3.更新粒子轨迹权重 */
updateTreeWeights(false);
/* 一些衡量粒子权重的相似性日志输出 */
...
/* 4.重采样 */
resample(plainReading, adaptParticles, reading_copy);
最后每次读取数据后，再次更新一下粒子权重，并为下次迭代更新必要的累积数据
/* 再次更新粒子权重 */
updateTreeWeights(false);
delete[] plainReading; /* 删除临时用于扫描匹配器运算的激光数据 */
m_lastPartPose = m_odoPose; // update the past pose for the next iteration 为下一次迭代更新上一次的位姿
m_linearDistance = 0; /* 累积直线距离清零 */
m_angularDistance = 0; /* 累积角度距离清零 */
m_count++; /* 扫描处理前计数+1 */
processed = true; /* 标记当前当前的激光雷达传感器数据已处理 */
// keep ready for the next step 为下一步做好准备
for (ParticleVector::iterator it = m_particles.begin(); it != m_particles.end(); it++)
{
/* 先前的位姿等于当前位姿 */
it->previousPose = it->pose;
}
谢谢
