Gmapping 在专栏 (1) 中主要介绍了来源,论文,解析的原理与主要函数,本节开始对 Gmapping的代码进行阅读,其中在 Github 中关于 Gmapping 的有两版:
GMapping ： https://github.com/ros-perception/slam_gmapping
Openslam_gmapping:https://github.com/OpenSLAM-org/openslam_gmapping
其中 GMapping 是其本体在 ROS 下的封装,而Openslam_gmapping是对 gmapping 源码，对于 Openslam_gmapping 本次 Gmapping 计划中不会进行更新，主要解析 ROS对其的封装，即GMapping。
1、主要topic与param
上方为 GMapping 网址的 ROS WIKI 地址，其中我们可以看到主要的topic为：
----------------------------------------------------
Node [/slam_gmapping]
Publications:
* /map_metadata [nav_msgs/MapMetaData]
* /tf [tf2_msgs/TFMessage]
* /map [nav_msgs/OccupancyGrid]
* /rosout [rosgraph_msgs/Log]
* /slam_gmapping/entropy [std_msgs/Float64]
Subscriptions:
* /tf [tf2_msgs/TFMessage]
* /scan [sensor_msgs/LaserScan]
* /tf_static [tf2_msgs/TFMessage]
* /clock [rosgraph_msgs/Clock]
Services:
* /slam_gmapping/set_logger_level
* /slam_gmapping/get_loggers
* /dynamic_map运行时可以通过 rosnode info 查看，其中 Publications: 中对应的topic即为wiki中看到的
Published Topics：
（1）/map_metadata：数据类型为 nav_msgs/MapMetaData ，其中主要包含了分辨率，长，宽，原点信息。实际而言其为 /map 的一部分。
（2）/map：数据类型为 nav_msgs/OccupancyGrid ，这个数据是一个经常见到的数据，就是一个地图，用来描述占用栅格地图，具体该数据类型中包含：
》std_msgs/Header header。消息头，时间戳和帧ID。
》nav_msgs/MapMetaData info。地图的分辨率、尺寸、原点信息等，就是[/map_metadata]发布的消息内容。
》int8[] data。以row-major的形式记录占用栅格地图，它用[0-100]的数值来表示对应栅格的占用概率，其中主要为上节所提到的栅格地图占用状态，其中灰色部分未知则标记为-1。
（3）/slam_gmapping/entropy：指的是关于粒子滤波器对位姿估计的不确定性。
其余Topic不做解释。
2、初始化变量与参数
从代码结构中看到，其包含 main.cpp、nodelet.cpp、replay.cpp、slam_gmapping.cpp、slam_gmapping.h。main.cpp最简单也是运行开始主文件。
初始化与参数载入在 slam_gmapping.cpp 中，利用SlamGMapping::SlamGmapping();的默认构造函数，初始化了地图到里程计的坐标变换即map_to_odom_，雷达的技术器laser_count_，并且对一些指针进行NULL初始化。对数据数种子进行初始化（seed_ = time((Null)）。
SlamGMapping::SlamGMapping():
map_to_odom_(tf::Transform(tf::createQuaternionFromRPY( 0, 0, 0 ), tf::Point(0, 0, 0 ))),
laser_count_(0), private_nh_("~"), scan_filter_sub_(NULL), scan_filter_(NULL), transform_thread_(NULL)
{
seed_ = time(NULL);
init();
}后续在 init() 初始化函数中，New 了一个 GMapping::GridSlamProcessor 对象的 gsp_，即 Openslam_gmapping 中基本的 GridFastSLAM 算法，主要依赖Rao-Blackwellized粒子滤波器（上文提到的RBPF），每个粒子包含位姿估计和地图观测。
New了一个 tf::TransformBroadcaster(); 对象的 tfB_ ，即广播器，发布map_to_odom_。
void SlamGMapping::init()
{
// log4cxx::Logger::getLogger(ROSCONSOLE_DEFAULT_NAME)->setLevel(ros::console::g_level_lookup[ros::console::levels::Debug]);
// The library is pretty chatty
//gsp_ = new GMapping::GridSlamProcessor(std::cerr);
gsp_ = new GMapping::GridSlamProcessor();
ROS_ASSERT(gsp_);
tfB_ = new tf::TransformBroadcaster();
ROS_ASSERT(tfB_);下面将算法中用到的激光和里程计置空，在第一次扫描数据时构建。got_first_scan_标识着是否接收到了第一次的扫描数据， got_map_表示已经生成了地图。初始化两个变量都为false。
gsp_laser_ = NULL;
gsp_odom_ = NULL;
got_first_scan_ = false;
got_map_ = false;后续为参数的加载，也就是GMapping建图时的需要调整的主要参数，其中这些参数最主要的设置为必须对应你所使用的坐标系。
// Parameters used by our GMapping wrapper
if(!private_nh_.getParam("throttle_scans", throttle_scans_))
throttle_scans_ = 1;
if(!private_nh_.getParam("base_frame", base_frame_))
base_frame_ = "base_link";
if(!private_nh_.getParam("map_frame", map_frame_))
map_frame_ = "map";
if(!private_nh_.getParam("odom_frame", odom_frame_))
odom_frame_ = "odom";
private_nh_.param("transform_publish_period", transform_publish_period_, 0.05);主要的参数与含义如下：
<remap from="scan" to="base_scan"/>
//每收到n个激光数据就进行一次更新，默认=1
<param name="throttle_scans" value="1"/>
//机器人 地图 里程计 坐标系
<param name="base_frame" value="base_link"/>
<param name="map_grame" value="map"/>
<param name="odom_frame" value="odom"/>
//地图更新周期，单位为s
<param name="map_update_interval" value="5.0"/>
<param name="maxUrange" value="16.0"/> //传感器测量最大距离
<param name="sigma" value="0.05"/> //扫描匹配中cell的标准差
<param name="kernelSize" value="1"/> //扫描匹配搜索窗口大小
<param name="lstep" value="0.05"/> //匹配初始化距离步长
<param name="astep" value="0.05"/> //匹配初始化角度步长
<param name="iterations" value="5"/> //扫描匹配迭代步长
//Tips:
// 最终匹配距离精度=lstep*2^(iterations);
// 最终匹配角度精度=astep*2^(iterations);
<param name="lsigma" value="0.075"/> //扫描匹配中单激光束的标准差
<param name="ogain" value="3.0"/> //似然度平滑增益
<param name="lskip" value="0"/> //没n+1个扫描进行一次匹配
<param name="minimumScore" value="0"/> //扫描匹配质量最小值，增大可消除跳跃现象
<param name="srr" value="0.1"/> //位置噪声
<param name="srt" value="0.2"/> //方位角噪声
<param name="str" value="0.1"/> //位置->方位角的协方差
<param name="stt" value="0.2"/> //方位角->位置的协方差
<param name="linearUpdate" value="1.0"/> //运动该距离进行新的测量
<param name="angularUpdate" value="0.5"/> //运动该角度进行新的测量
<param name="resampleThreshold" value="0.5"/> //重采样阈值
<param name="particles" value="30"/> //粒子数量，机器人位姿可能路径
<param name="xmin" value="-50.0"/> //初始尺寸最小x值
<param name="ymin" value="-50.0"/>
<param name="xmax" value="50.0"/>
<param name="ymax" value="50.0"/>
<param name="delta" value="0.05"/> //地图分辨率。像素尺度
<param name="llsamplerange" value="0.01"/> //距离采样范围
<param name="llsamplestep" value="0.01"/> //距离采样步长
<param name="lasamplerange" value="0.005"/> //角度采样范围
<param name="lasamplestep" value="0.005"/> //角度采用步长
<param name="occ_thresh" value="0.25"/> //栅格占用概率阈值
3、初始化Mapper（距离引擎/里程计引擎/建图引擎/GridFastSLAM算法粒子）
上面在初始化 SlamGMapping::init()中创建了 GridlamProcessor 对象，即 gsp_ ， 并且参数获取到了 private_nh_ 中 ，但是参数生效在 slam_gmapping.cpp 中第 405 行中的
bool SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan)进行了最终的参数载入，这个函数的调用即完成建图引擎的配置。在该函数中，根据输入的传感器测量数据，获取各传感器例如激光传感器的坐标系“scan.header.frame_id”，与时间戳等等信息，为了保证后续处理的数据统一，创建了 tf::Stamped<tf::Pose> ident; ，该参数获取了雷达的frame_id与时间戳，后续通过 tf 库中的 transformPose 将 ident 坐标系变换到 base_frame_ 坐标系，并将二者的变化结果（即激光雷达相对于底盘坐标系的相对变换）存储到 laser_pose 中。
laser_frame_ = scan.header.frame_id;
tf::Stamped<tf::Pose> ident;
tf::Stamped<tf::Transform> laser_pose;
ident.setIdentity();
ident.frame_id_ = laser_frame_;
ident.stamp_ = scan.header.stamp
tf_.transformPose(base_frame_,ident, laser_pose);Tips：在 tf 的运行机制中，对于 tf 而言是从监听器中获取关系并放入缓存中。后续的所有操作都是从缓存中获取进行实现的变换。不可避免的存在时延的问题，也就是 SLAM 中经常遇到的 time_out问题，因此在 transformPose 中会遇到该函数的异常抛出，并通过 try-catch 语句对该异常进行捕获：
try
{
tf_.transformPose(base_frame_, ident, laser_pose);
}
catch(tf::TransformException e)
{
ROS_WARN("Failed to compute laser pose, aborting initialization (%s)",
e.what());
return false;
}现在我们获取了 lidar->base_link 的关系，那么接下来我们在 base_link 坐标系下，在雷达传感器正上方 1m 处构建一个点，并且将点转换到雷达坐标系下，在雷达坐标系下这个点通常为 (0,0,1+（lidar->base_link）.z())。
tf::Vector3 v;
v.setValue(0, 0, 1 + laser_pose.getOrigin().z());
tf::Stamped<tf::Vector3> up(v, scan.header.stamp,base_frame_);
try
{
tf_.transformPoint(laser_frame_, up, up);
ROS_DEBUG("Z-Axis in sensor frame: %.3f", up.z());
}
catch(tf::TransformException& e)
{
ROS_WARN("Unable to determine orientation of laser: %s",e.what());
return false;
}这个点的作用是用来判断雷达的安装方式。对于GMapping而言要求激光雷达的安装 是水平的。则对雷达坐标系而言，z 轴只存在向上或者向下两种方式，所以这里会有判断雷达是否水平的情况。laser_pose.getOrigin().z() 获取该雷达原点坐标的Z轴分量。
// gmapping doesnt take roll or pitch into account. So check for correct sensor alignment.
if (fabs(fabs(up.z()) - 1) > 0.001)
{
ROS_WARN("Laser has to be mounted planar! Z-coordinate has to be 1 or -1, but gave: %.5f",up.z());
return false;
}后续中从传感器中的 const sensor_msgs::LaserScan& scan 中获取激光扫描数据的角度量和角度中值。
gsp_laser_beam_count_ = scan.ranges.size();
double angle_center = (scan.angle_min + scan.angle_max)/2;上面我们判定了雷达安装方式已经激光雷达的波束角度后，来初始化传感器的中心位置即 centered_laser_pose_ 。对波束角度而言，我们获取的最小最大角度说明了扫描方向，如果扫描为反向，即逆雷达坐标系（倒转），那么在后续的数据处理将对雷达的激光波束进行反向处理。通过上述的 z 判断 ，并且用 do_reverse_range_ 描述 angle_min 与 angle_max。
if (up.z() > 0)
{
do_reverse_range_ = scan.angle_min > scan.angle_max;
centered_laser_pose_ = tf::Stamped<tf::Pose>
(tf::Transform(tf::createQuaternionFromRPY(0,0,angle_center),
tf::Vector3(0,0,0)), ros::Time::now(), laser_frame_);
ROS_INFO("Laser is mounted upwards.");
}
//反向处理
else
{
do_reverse_range_ = scan.angle_min < scan.angle_max;
centered_laser_pose_ = tf::Stamped<tf::Pose>
(tf::Transform(tf::createQuaternionFromRPY(M_PI,0,-angle_center),
tf::Vector3(0,0,0)), ros::Time::now(), laser_frame_);
ROS_INFO("Laser is mounted upside down.");
}获取了雷达数据的角度量，而这个角度量描述的是数据长度，这里初始化了雷达的扫描角度表 laser_angles_ ， 它存储了波束的角度值（个人理解），从小到达增长。
laser_angles_.resize(scan.ranges.size());
double theta = -std::fabs(scan.angle_min - scan.angle_max)/2;
for(unsigned int i = 0; i < scan.ranges.size(); ++i) {
laser_angles_[i] = theta;
theta += std::fabs(scan.angle_increment);
}后续打印了两个ROS_DEBUG表示的是雷达数据。接着创建 GMapping::OrientedPoint 类型的 gmap_pose(0,0,0)；用于初始化构建下一步的距离参量。接下来配置参数 maxRange 和 maxUrange 。默认情况下我们可以看到 maxRange 的取值是为第一个扫描最大值减去 1cm ，而 maxUrange 与取值相同。
// setting maxRange and maxUrange here so we can set a reasonable default
ros::NodeHandle private_nh_("~");
if(!private_nh_.getParam("maxRange", maxRange_))
maxRange_ = scan.range_max - 0.01;
if(!private_nh_.getParam("maxUrange", maxUrange_))
maxUrange_ = maxRange_;接着构建距离传感器（即GMapping::RangeSensor）gsp_laser_，名词为 "FLASER"，即激光雷达。另外传递的扫描角度即 scan.angle_increment 取绝对值，如果实际扫描方向与其相反，在传递给建图的时候会将其反向。
GMapping::OrientedPoint gmap_pose(0, 0, 0);
gsp_laser_ = new GMapping::RangeSensor("FLASER", gsp_laser_beam_count_,
fabs(scan.angle_increment), gmap_pose, 0.0, maxRange_);接着构建出建图引擎 smap，类型为 GMapping::SensorMap ，这个类型实际为标准库中的 map，因为这个是 GMapping 的壳，所以可以看 slam_gmapping.h中头文件的#include "gmapping/sensor/sensor_base/sensor.h" 实际如果你想看的话这个 sensor.h中就可以看到所谓的建图引擎定义 typedef std::map<std::string, Sensor*> SensorMap。
GMapping::SensorMap smap;
smap.insert(make_pair(gsp_laser_->getName(), gsp_laser_));
gsp_->setSensorMap(smap);接着创建了里程计传感器(即 GMapping::OdometrySensor) gsp_odom_ ，获取初始的位姿 initialPose 。
/// @todo Expose setting an initial pose
GMapping::OrientedPoint initialPose;
if(!getOdomPose(initialPose, scan.header.stamp))
{
ROS_WARN("Unable to determine inital pose of laser! Starting point will be set to zero.");
initialPose = GMapping::OrientedPoint(0.0, 0.0, 0.0);
}后续就算获取参数配置，并且将参数写入一开始 init() 函数中的GMapping:: GridSlamProcessor 对象的 gsp_，即 Openslam_gmapping 中基本的 GridFastSLAM 算法，那么这个算法基本构建完成。
gsp_->setMatchingParameters(maxUrange_, maxRange_, sigma_,
kernelSize_, lstep_, astep_, iterations_,
lsigma_, ogain_, lskip_);
gsp_->setMotionModelParameters(srr_, srt_, str_, stt_);
gsp_->setUpdateDistances(linearUpdate_, angularUpdate_, resampleThreshold_);
gsp_->setUpdatePeriod(temporalUpdate_);
gsp_->setgenerateMap(false);
gsp_->GridSlamProcessor::init(particles_, xmin_, ymin_, xmax_, ymax_,
delta_, initialPose);后面的参数在我看到源码的时候有注释对参数读取进行了修正，所以你看到的后面可能是这个样子，但是这都不重要，如果你想深究 GMapping:: GridSlamProcessor 对象的操作，那你可以跳转到 。
gsp_->setllsamplerange(llsamplerange_);
gsp_->setllsamplestep(llsamplestep_);
/// @todo Check these calls; in the gmapping gui, they use
/// llsamplestep and llsamplerange intead of lasamplestep and
/// lasamplerange. It was probably a typo, but who knows.
gsp_->setlasamplerange(lasamplerange_);
gsp_->setlasamplestep(lasamplestep_);
gsp_->setminimumScore(minimum_score_);最后的 GMapping::sampleGaussian(1,seed_);我理解为粒子滤波器更新初始化随机粒子（官方称之为调用采样函数设置随机种），后面就一个 ROS_INFO 打印信息告诉你初始化完成，并且这个函数为 bool 型，那么需要告诉我们初始化完成， return true;。
ROS_INFO("Initialization complete");
return true;4、总结
本中主要介绍了整个 GMapping 建图的初始化过程，首先读取参数，初始化构建需要的对象，包含SLAM算法、tf广播器构建。激光数据，里程计数据初始化。激光数据，地图数据标志位。之后构建距离引擎对激光数据进行判断与处理，初始化距离引擎。将距离引擎加入到建图引擎中，获取初始位姿创建里程计引擎。建立基本SLAM算法并初始化粒子。到这里整个的初始化与构建就完成了。
后续我们将总体分为四个部分：运动模型与更新，扫描匹配，权重更新，重采样四个过程，并且对主要构建的地图数据，传感器数据等等做一些个人的理解与代码的解读。其中建立的基本SLAM算法就是利用RBPF粒子滤波器来完成建图任务，所以我们重点关注运动模型与更新、重采样这两个过程。
在这个之前建议去理解 GMapping系列第一篇 ，第一对其中的粒子滤波器（贝叶斯估计，采样，重采样中的粒子退化解决）进行了解，第二将滤波器如何用于SLAM问题，即RBPF是如何进行的SLAM问题分离与结合。有兴趣或者研究方向的童鞋建议读一读两本掉头发书。
概率机器人《Probabilistic Robotics》 ----- Sebastian Thrun/Wolfram Burgard/Dieter Fox
机器人学中的状态估计《State Estimation for Robotics》 ----- Timothy Barfoot
后续的想法是对这两本书也开专栏进行详细阅读，因为我的理解并不深入，不过现在开的坑太多了，先把实践的一些更完，大多数好像都是直接使用复现SLAM算法，哈哈哈！
不积跬步无以至千里，不积小流无以成江河 ------------------------20:32
585
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
