源码注释地址:https://github.com/Lcp1/Lcp-gmapping.git
第二篇：粒子更新过程代码汇总（gmapping数据结构解析精华）
1、从代码Gmapping包的main.cpp进入
int
main(int argc, char** argv)
{
ros::init(argc, argv, "slam_gmapping");
SlamGMapping gn;
gn.startLiveSlam();//这里进入主函数
ros::spin();
return(0);
}
2.由于调用了slam_gmapping.cpp的gn.startLiveSlam()函数,在函数中,进行了回调函数:laserCallback
scan_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1));
函数完整代码
void SlamGMapping::startLiveSlam()
{
entropy_publisher_ = private_nh_.advertise<std_msgs::Float64>("entropy", 1, true);
sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);
scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "scan", 5);
scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
// 通过boost::bind开启两个线程：laserCallback()和 publishLoop()，
scan_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1));
transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_));
}
在函数中laserCalbscke,主要有三个函数
//主要有三个函数.
initMapper(*scan)//进行参数初始化
addScan(*scan, odom_pose))//调用openslam_gmappinig核心算法
updateMap(*scan);//对地图进行更新
laserCallback函数完整代码,
void
SlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
laser_count_++;
if ((laser_count_ % throttle_scans_) != 0)
return;
static ros::Time last_map_update(0,0);
// We can't initialize the mapper until we've got the first scan
//没有获取激光点云信息需要进行初始化
if(!got_first_scan_)
{
if(!initMapper(*scan))//进行参数初始化------------------1\initMapper----------------
return;
got_first_scan_ = true;
}
GMapping::OrientedPoint odom_pose;//定义里程计位姿信息
if(addScan(*scan, odom_pose))//调用openslam_gmappinig核心算法 叠加激光---------------------2\addScan----------------------
{
ROS_DEBUG("scan processed");
GMapping::OrientedPoint mpose = gsp_->getParticles()[gsp_->getBestParticleIndex()].pose;
ROS_DEBUG("new best pose: %.3f %.3f %.3f", mpose.x, mpose.y, mpose.theta);
ROS_DEBUG("odom pose: %.3f %.3f %.3f", odom_pose.x, odom_pose.y, odom_pose.theta);
ROS_DEBUG("correction: %.3f %.3f %.3f", mpose.x - odom_pose.x, mpose.y - odom_pose.y, mpose.theta - odom_pose.theta);
tf::Transform laser_to_map = tf::Transform(tf::createQuaternionFromRPY(0, 0, mpose.theta), tf::Vector3(mpose.x, mpose.y, 0.0)).inverse();
tf::Transform odom_to_laser = tf::Transform(tf::createQuaternionFromRPY(0, 0, odom_pose.theta), tf::Vector3(odom_pose.x, odom_pose.y, 0.0));
map_to_odom_mutex_.lock();
map_to_odom_ = (odom_to_laser * laser_to_map).inverse();//计算世界到里程计坐标
map_to_odom_mutex_.unlock();
//如果当前激光雷达时间与上一桢时间超过更新阈值,就更新地图
if(!got_map_ || (scan->header.stamp - last_map_update) > map_update_interval_)
{
updateMap(*scan);//对地图进行更新----------------------3\updateMap---------------------------
last_map_update = scan->header.stamp;//更新上一桢时间戳
ROS_DEBUG("Updated the map");
}
} else
ROS_DEBUG("cannot process scan");
}
2.1 initMapper(*scan)函数
主要是进行对激光雷达安装的姿态,确定激光参数的范围,正着还是反着安装,向上向下;初始化粒子的地图,设定粒子地图范围值,该地图map用来给后面扫描匹配计算定位得分,并不是用来发布地图信息,发布地图信息是选取粒子每个node点云进行重建地图.
总过程:
—雷达,
—里程计信息OdomPose,
—运动模型MotionModel,
—地图SensorMap,
—采样器sample初始化,
完整代码:
SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan)
{
laser_frame_ = scan.header.frame_id;
// Get the laser's pose, relative to base.
tf::Stamped<tf::Pose> ident; //定义位姿
tf::Stamped<tf::Transform> laser_pose; //定义激光雷达位姿信息
ident.setIdentity(); //设为单位矩阵
ident.frame_id_ = laser_frame_; //一般为laser_link
ident.stamp_ = scan.header.stamp; //激光雷达时间戳
try
{
tf_.transformPose(base_frame_, ident, laser_pose);
}
// 作用：如果请求的坐标系id之间存在连接，但一个或多个变换已过期，则抛出。
catch(tf::TransformException e)
{
ROS_WARN("Failed to compute laser pose, aborting initialization (%s)",
e.what());
return false;
}
// create a point 1m above the laser position and transform it into the laser-frame
// 在激光位置上方1米处创建一个点，并将其转换为激光帧
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 doesnt take roll or pitch into account. So check for correct sensor alignment.
// 不考虑滚动或俯仰。因此，检查传感器是否正确对齐
if (fabs(fabs(up.z()) - 1) > 0.001)
{
//“激光器必须安装在平面上！Z坐标必须是1或-1，-1表示雷达倒着
ROS_WARN("Laser has to be mounted planar! Z-coordinate has to be 1 or -1, but gave: %.5f",
up.z());
return false;
}
// 获取激光点云数量
gsp_laser_beam_count_ = scan.ranges.size();
double angle_center = (scan.angle_min + scan.angle_max)/2;
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.");
}
// Compute the angles of the laser from -x to x, basically symmetric and in increasing order
laser_angles_.resize(scan.ranges.size());//分配激光雷达角度容器空间
// Make sure angles are started so that they are centered
double theta = - std::fabs(scan.angle_min - scan.angle_max)/2;//求出中间角度的负数即使其实起始位姿角度
//将点云角度从theta起始角度开始,然后后面每一个点都添加角度增量
for(unsigned int i=0; i<scan.ranges.size(); ++i)
{
laser_angles_[i]=theta;
theta += std::fabs(scan.angle_increment);
}
ROS_DEBUG("Laser angles in laser-frame: min: %.3f max: %.3f inc: %.3f", scan.angle_min, scan.angle_max,
scan.angle_increment);
ROS_DEBUG("Laser angles in top-down centered laser-frame: min: %.3f max: %.3f inc: %.3f", laser_angles_.front(),
laser_angles_.back(), std::fabs(scan.angle_increment));
GMapping::OrientedPoint gmap_pose(0, 0, 0);//初始化姿态
// setting maxRange and maxUrange here so we can set a reasonable default
// 创建话题句柄
ros::NodeHandle private_nh_("~"); //显示话题的Publisher
if(!private_nh_.getParam("maxRange", maxRange_))//获取话题信息
maxRange_ = scan.range_max - 0.01;
if(!private_nh_.getParam("maxUrange", maxUrange_))//
maxUrange_ = maxRange_;
// The laser must be called "FLASER".
// We pass in the absolute value of the computed angle increment, on the
// assumption that GMapping requires a positive angle increment. If the
// actual increment is negative, we'll swap the order of ranges before
// feeding each scan to GMapping.
// 创建RangeSensor对象
gsp_laser_ = new GMapping::RangeSensor("FLASER",//激光名称
gsp_laser_beam_count_,//激光点数量
fabs(scan.angle_increment),//角度跨度
gmap_pose,//初始位姿
0.0,
maxRange_);//最大长度
ROS_ASSERT(gsp_laser_);//判断是否创建指针成功
GMapping::SensorMap smap;
// make_pair将元素组成结构体
smap.insert(make_pair(gsp_laser_->getName(), gsp_laser_));
gsp_->setSensorMap(smap);
gsp_odom_ = new GMapping::OdometrySensor(odom_frame_);//创建里程计指针对象
ROS_ASSERT(gsp_odom_);
/// @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);
}
// 设置匹配参数
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);//栅格地图SLAM过程初始化
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_);//得分初始化
// Call the sampling function once to set the seed.
GMapping::sampleGaussian(1,seed_);
ROS_INFO("Initialization complete");
return true;
}
2.2 addScan(*scan, odom_pose)函数
该函数主要有两步
第一步是读取激光雷达信息,重定义激光数据类型,加入辅助信息,时间,数量
//RangeReading(unsigned int n_beams, const double* d, const RangeSensor* rs, double time=0);
GMapping::RangeReading reading(scan.ranges.size(),
ranges_double,
gsp_laser_,
scan.header.stamp.toSec());
第二部是处理激光雷达信息,包括根据里程计位姿进行优化,激光点云得分计算,添加粒子节点,更新粒子节点点云,用于后期构建发布地图/map
gsp_->processScan(reading);
由于gsp_ 是栅格处理过程的对象,所以是处理栅格的过程
GMapping::GridSlamProcessor* gsp_; //栅格处理SLAM过程
完整代码
bool
SlamGMapping::addScan(const sensor_msgs::LaserScan& scan, GMapping::OrientedPoint& gmap_pose)
{
if(!getOdomPose(gmap_pose, scan.header.stamp))//根据激光点云头时间,获取里程计位姿
return false;
if(scan.ranges.size() != gsp_laser_beam_count_)//激光点云数量不符合实际硬件参数,停止
return false;
// GMapping wants an array of doubles...
double* ranges_double = new double[scan.ranges.size()];//分配激光空间
// If the angle increment is negative, we have to invert the order of the readings.
if (do_reverse_range_)//激光装反了的
{
ROS_DEBUG("Inverting scan");
int num_ranges = scan.ranges.size();
for(int i=0; i < num_ranges; i++)
{
// Must filter out short readings, because the mapper won't
if(scan.ranges[num_ranges - i - 1] < scan.range_min)
ranges_double[i] = (double)scan.range_max;//最大值即是异常值
else
ranges_double[i] = (double)scan.ranges[num_ranges - i - 1];//否则逆序存储
}
} else
{
for(unsigned int i=0; i < scan.ranges.size(); i++)//正着安转激光雷达
{
// Must filter out short readings, because the mapper won't
if(scan.ranges[i] < scan.range_min) //比最小测距距离还小,则定义为最大值,后面清理
ranges_double[i] = (double)scan.range_max;
else
ranges_double[i] = (double)scan.ranges[i];//正序保存
}
}
// 读取激光点云数据
//RangeReading(unsigned int n_beams, const double* d, const RangeSensor* rs, double time=0);
GMapping::RangeReading reading(scan.ranges.size(),
ranges_double,
gsp_laser_,
scan.header.stamp.toSec());
// ...but it deep copies them in RangeReading constructor, so we don't
// need to keep our array around.
delete[] ranges_double;
reading.setPose(gmap_pose);
/*
ROS_DEBUG("scanpose (%.3f): %.3f %.3f %.3f\n",
scan.header.stamp.toSec(),
gmap_pose.x,
gmap_pose.y,
gmap_pose.theta);
*/
ROS_DEBUG("processing scan");
// GMapping::GridSlamProcessor* gsp_; //栅格处理SLAM过程
return gsp_->processScan(reading);
}
2.3 updateMap
通过读取权重最大的粒子,然后逐个读取该粒子的每个节点点云,转化成坐标信息,加入到smap地图容器中,通过判断smap每个栅格的占用概率,设定发布话题的每个栅格值.最后发布
读取权重最大的粒子:
GMapping::GridSlamProcessor::Particle best =
gsp_->getParticles()[gsp_->getBestParticleIndex()];
逐个读取该粒子的每个节点点云:
for(GMapping::GridSlamProcessor::TNode* n = best.node;
n;
n = n->parent)
{
ROS_DEBUG(" %.3f %.3f %.3f",
n->pose.x,
n->pose.y,
n->pose.theta);
if(!n->reading)
{
ROS_DEBUG("Reading is NULL");
continue;
}
matcher.invalidateActiveArea();//计算激活区域,标记位
matcher.computeActiveArea(smap, n->pose, &((*n->reading)[0]));//计算激活区域，通过激光雷达的数据计算出来哪个地图栅格应该要被更新了。
//上面应该重复了computeActiveArea
matcher.registerScan(smap, n->pose, &((*n->reading)[0]));//计算激光束上所有点的熵,如果不更新地图,默认熵为0,只更新hit,返回熵值
}
通过判断smap每个栅格的占用概率,:
double occ=smap.cell(p);
3.进入: addScan 的 GridSlamProcessor::processScan函数
1.运动模型：
drawFromMotion(it->pose, relPose, m_odoPose);// 运动模型更新
更新t时刻粒子群（在模型中加入高斯噪声）drawfrommotion（）,计算t-1时刻到t时刻的位移增量，以及角位移增量
2.扫描匹配：
接下来一个if判断，包含了大部分程序：
通过匹配选取最优粒子，如果匹配失败，返回一个默认的似然估计scanMatch（）
scanMatch(plainReading);//--------------------------扫描匹配-
权重更重采样前更新一次，重采样后又更新一次updateTreeWeights（）
重采样，粒子集对目标分布的近似越差则权重方差越大…resample（）
resample(plainReading, adaptParticles, reading_copy);
resample(plainReading, adaptParticles, reading_copy);
地图更新，先得到最优的粒子（用权重和weightSum判断）
完整代码:
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
// 对于每一个粒子，都从里程计运动模型中采样,由relPose, m_odoPose的里程计位姿变化量进行更新粒子位姿
for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
OrientedPoint& pose(it->pose);
//it->pose 表示机器人上一时刻的最优位置 粒子的位姿(机器人的位姿)
//relPose 表示里程计算出来的当前位置
//m_odoPose 表示里程计上一次的位置
//由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 (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
// 计算机器人平移和旋转
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
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);
// 这是格式化输出流
// ios::fixed操作符定义在<iotream>中,意思是用小数点形式来显示数据，
// 有效数字位数默认为为6位，你可以通过setprecision(n)来修改显示有效数字的位数
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与cout的主要区分就是，cout输出的信息可以重定向，而cerr只能输出到标准输出（显示器）上
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;//第几桢的点云???????????????????
//激光数据复制???????
RangeReading* reading_copy =
new RangeReading(reading.size(),
&(reading[0]),
static_cast<const RangeSensor*>(reading.getSensor()),
reading.getTime());
if (m_count>0){
scanMatch(plainReading);//--------------------------扫描匹配---------------------------
if (m_outputStream.is_open()){
//如果文件可以打开
m_outputStream << "LASER_READING "<< reading.size() << " ";//显示激光点云数量有效位数2位
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 << " ";//坐标精度3位效数字
m_outputStream << setiosflags(ios::fixed) << setprecision(6) << pose.theta << " " << it-> weight << " ";//角度精度6位有效数字
}
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;
}
resample(plainReading, adaptParticles, reading_copy);
} else {
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;
}
}
// 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;
}
3.1 运动模型更新m_motionModel.drawFromMotion( )
//该函数主要通过里程计运动模型对gmapping内部粒子位姿进行估计 srr_ = 0.1
OrientedPoint
MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const{
double sxy=0.3*srr;
OrientedPoint delta=absoluteDifference(pnew, pold);//做差值,并转成矩阵
OrientedPoint noisypoint(delta);//里程计增量高斯噪声
noisypoint.x+=sampleGaussian(srr*fabs(delta.x)+str*fabs(delta.theta)+sxy*fabs(delta.y));
noisypoint.y+=sampleGaussian(srr*fabs(delta.y)+str*fabs(delta.theta)+sxy*fabs(delta.x));
noisypoint.theta+=sampleGaussian(stt*fabs(delta.theta)+srt*sqrt(delta.x*delta.x+delta.y*delta.y));
noisypoint.theta=fmod(noisypoint.theta, 2*M_PI);//fmod(x,y)返回x 除以y 的余数
if (noisypoint.theta>M_PI)//超过180度 // 角度归一计算
noisypoint.theta -= 2*M_PI;//取-180到0度
return absoluteSum(p,noisypoint);//将noisypoint进行p矩阵变换
}
3.2 扫描匹配GridSlamProcessor::scanMatch
首先是通过m_matcher.optimize( );优化函数, 进入score( )函数计算给定位姿附近的六个点得分和当前位置的得分计算最高得分,返回最高得分和最高得分的位姿:scanMatch–>optimize( )—>score( ).
第一部分代码:
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算最优位姿的得分，分值为:l
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();
m_matcher.computeActiveArea(it->map, it->pose, plainReading);
}
if (m_infoStream)
m_infoStream << "Average Scan Matching Score=" << sumScore/m_particles.size() << std::endl; //打印平均分
}
进入优化函数optimize( )
double ScanMatcher::optimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings) const{
//double ScanMatcher::optimize(OrientedPoint& _mean, ScanMatcher::CovarianceMatrix& _cov, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings)
double bestScore=-1;//默认为-1
OrientedPoint currentPose=init;//设置当前位姿
double currentScore=score(map, currentPose, readings);//计算该位姿的得分
double adelta=m_optAngularDelta, ldelta=m_optLinearDelta;//设置角度和先距离增量
unsigned int refinement=0;
enum Move{
Front, Back, Left, Right, TurnLeft, TurnRight, Done};//设定枚举类变量
/* cout << __PRETTY_FUNCTION__<< " readings: ";
for (int i=0; i<m_laserBeams; i++){
cout << readings[i] << " ";
}
cout << endl;
*/ int c_iterations=0;
do{
if (bestScore>=currentScore){
//如果当前位姿的得分不高,里程计变化量减半
refinement++;
adelta*=.5;
ldelta*=.5;
}
bestScore=currentScore;
// cout <<"score="<< currentScore << " refinement=" << refinement;
// cout << "pose=" << currentPose.x << " " << currentPose.y << " " << currentPose.theta << endl;
OrientedPoint bestLocalPose=currentPose;//初始化当前位姿和最优位姿
OrientedPoint localPose=currentPose;
//
Move move=Front;
do {
localPose=currentPose;//重新回到原来位姿
switch(move){
case Front:
localPose.x+= ldelta;
move=Back;
break;
case Back:
localPose.x-= ldelta;
move=Left;
break;
case Left:
localPose.y-= ldelta;
move=Right;
break;
case Right:
localPose.y+= ldelta;
move=TurnLeft;
break;
case TurnLeft:
localPose.theta+= adelta;
move=TurnRight;
break;
case TurnRight:
localPose.theta-= adelta;
move=Done;
break;
default:;
}
double odo_gain=1;
//里程计的角度可靠性
if (m_angularOdometryReliability>0.){
double dth=init.theta-localPose.theta; dth=atan2(sin(dth), cos(dth)); dth*=dth;//dth将角度归一化后 进行平方
odo_gain*=exp(-m_angularOdometryReliability*dth);
}
//里程计的线距离可靠性
if (m_linearOdometryReliability>0.){
double dx = init.x-localPose.x;
double dy = init.y-localPose.y;
double drho=dx*dx+dy*dy;
odo_gain *= exp( -m_linearOdometryReliability * drho);//线和角可靠性乘积
}
double localScore = odo_gain * score(map, localPose, readings);//得分是考虑了里程计的可靠性
// 比较最大得分
if (localScore > currentScore){
currentScore = localScore;
bestLocalPose = localPose;
}
c_iterations++;
} while(move!=Done);
currentPose=bestLocalPose;//更新最高得分位姿
// cout << "currentScore=" << currentScore<< endl;
//here we look for the best move;
}while (currentScore>bestScore || refinement<m_optRecursiveIterations);
//cout << __PRETTY_FUNCTION__ << "bestScore=" << bestScore<< endl;
//cout << __PRETTY_FUNCTION__ << "iterations=" << c_iterations<< endl;
pnew=currentPose;
return bestScore;
}
再进入score得分函数
// p 当前位姿 readings点云数据
inline double ScanMatcher::score(const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const{
double s=0;
const double * angle=m_laserAngles+m_initialBeamsSkip;//应该是点云的角度
OrientedPoint lp=p; //获取机器人位姿
//先获取雷达在世界的坐标系
lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
//根据雷达坐标将点云变换到世界坐标系
lp.theta+=m_laserPose.theta;
unsigned int skip=0;
/*
* map.getDelta表示地图分辨率 m_freeCellRatio = sqrt(2)
* 意思是如果激光hit了某个点 那么沿着激光方向的freeDelta距离的地方要是空闲才可以
*/
double freeDelta=map.getDelta()*m_freeCellRatio;
//枚举所有激光束,r为激光束的长度
for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++){
//设定跳过激光束
skip++;
skip=skip>m_likelihoodSkip?0:skip;//达到似然域跳过数量????????,就重置skip
if (skip||*r>m_usableRange||*r==0.0) continue;//
Point phit=lp;
//计算出hit的点在世界的坐标
phit.x+=*r*cos(lp.theta+*angle);
phit.y+=*r*sin(lp.theta+*angle);
//激光点世界坐标转到地图坐标
IntPoint iphit=map.world2map(phit);
Point pfree=lp;//激光束方向的空闲终点
pfree.x+=(*r-map.getDelta()*freeDelta)*cos(lp.theta+*angle);//将雷达坐标加上hit前的空闲激光束坐标（相对激光雷达坐标）=空闲点的世界坐标
pfree.y+=(*r-map.getDelta()*freeDelta)*sin(lp.theta+*angle);
pfree=pfree-phit;//空闲点终点到激光hit点的向量
IntPoint ipfree=map.world2map(pfree);//转成地图坐标
bool found=false;
Point bestMu(0.,0.);
/*在kernelSize大小的窗口中搜索出最优最可能被这个激光束击中的点 这个kernelSize在大小为1*/
//m_kernelSize对应概率机器人p130的第七行m
for (int xx=-m_kernelSize; xx<=m_kernelSize; xx++)
for (int yy=-m_kernelSize; yy<=m_kernelSize; yy++){
IntPoint pr=iphit+IntPoint(xx,yy);
IntPoint pf=pr+ipfree;
//AccessibilityState s=map.storage().cellState(pr);
//if (s&Inside && s&Allocated){
//根据坐标对应栅格,//PointAccumulator将击中的点位置叠加,并且求均值
const PointAccumulator& cell=map.cell(pr);//记录击中的坐标值
const PointAccumulator& fcell=map.cell(pf);//记录空闲坐标值
if (((double)cell )> m_fullnessThreshold && ((double)fcell )<m_fullnessThreshold){
//m_fullnessThreshold =0.1
Point mu=phit-cell.mean();//mean 1./n*Point(acc.x, acc.y)其中cell的坐标计算如下，n为被击中的次数，acc.x和acc.y是击中的累加值：
if (!found){
//第一次时候将bestMu=mu
bestMu=mu;
found=true;
}else
bestMu=(mu*mu)<(bestMu*bestMu)?mu:bestMu;//选取距离最短的
}
//}
}
if (found)
//高斯分布获取得分,对概率机器人p130的prob(dist,6hit)
s+=exp(-1./m_gaussianSigma*bestMu*bestMu);
}
return s;
}
继续更新ing…
第二篇：粒子更新过程代码汇总（gmapping数据结构解析精华）
https://blog.csdn.net/weixin_44023934/article/details/119302012