slam_gmapping 是gmapping的 基于ros的壳，我们先从壳入手
参考论文:
Improved Techniques for Grid Mapping with Rao-Blackwellized Particle Filters
参考博客：
白巧克力 pf原理讲解GMapping SLAM源码阅读（草稿）
gmapping分析
主要是breaham 算法分析
概率运动模型
里程计运动模型
我是分割线*************************
PS: 2022年05月20日01:27:29 更新
添加白茶清欢整理的流程图，非常直观对原生的代码进行了整理，并添加了去畸变。代码注释也更详细，如有需要可以留言或直接移步原作者博客或者公众号获取。
- 处理流程
- 程序框架，和自己整理的没什么两样，只不过更简洁明了
- 关于part_slam 中核心算法fastslam 代码流程
- 关于gsp_中 map的代码流程
算法流程目录：
- 1. 主函数中调用 startLiveSlam()
- 2. laserCallback()
- 3. 然后就是本文的主角了addScan
- 4. gridslamprocessor.cpp 中的processScan
- 5. 里程计的更新 调用里程计模型
- 6. 扫描匹配
- 7. 扫描匹配中 调用 scan_mather 中的 optimize 计算粒子的最后位姿
- 8. optimize 中计算得分的 score()函数
- 9. 权重计算
- 10. 计算地图区域
- 11. 权重更新
- 12. 重采样
- 13. 在重采样之后 粒子权重又会发生变化，因此需要再次更新粒子轨迹的积累权重
- 14. 更新地图
1. 主函数中调用 startLiveSlam()
主要订阅一些主题，发布一些主题，核心的是回调函数 laserCallBack()
2. laserCallback()
接受到激光雷达数据的回调函数 在这里面调用addScan()函数
如果addScan()函数调用成功，也就是说激光数据被成功的插入到地图中后，
如果到了地图更新的时间，则对地图进行更新，通过调用updateMap()函数来进行相应的操作。
首先调用initMapper 对 gmapping算法初始化
\* 这个函数在收到的第一帧激光雷达数据的时候会被调用一次，之后就再也不会被调用了。
\* 这个函数的功能主要就是对gmapping算法中需要的一些参数进行赋值，即对gmapping算法进行初始化
\* 这个参数一开始由gmapping wrapper读取，但是在这个函数里面真正的会赋值给gmapping算法
\* 1.判断激光雷达是否是水平放置的，如果不是 则报错。
\* 2.假设激光雷达数据的角度是对称的 & 递增的 为每个激光束分配角度。
\* 3.为gmapping算法设置各种需要的参数。bool SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan)
{
//得到激光雷达相对于车身坐标系(base_link)的位姿
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_;
ident.stamp_ = scan.header.stamp;
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;
}
// create a point 1m above the laser position and transform it into the laser-frame
// 创造一个比激光雷达高1m的一个点，然后把这个点转换到激光雷达坐标系 这个点主要用来检测激光雷达和车身的位姿关系
// 来判断激光雷达是否是倾斜的 判断条件：如果激光雷达是水平放置的，那么转换回去之后，z的坐标也依然是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)
{
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
// 这里认为激光雷达数据的角度是对称的，且是从小到大递增的。
// 因此根据激光雷达数据中的angle_min,angle_max,angle_increment等数据为每个激光束分配角度。
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;
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_("~");
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.
// 根据上面得到的激光雷达的数据信息 来初始化一个激光传感器
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
GMapping::SensorMap smap;
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
/// 得到里程计的初始位姿，如果没有 则把初始位姿设置为(0,0,0)
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);
}
//为gmapping算法设置各种参数
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);
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;
}3. 然后就是本文的主角了addScan
加入激光雷达的数据 主要函数 这立面会调用processScan()bool SlamGMapping::addScan(const sensor_msgs::LaserScan& scan, GMapping::OrientedPoint& gmap_pose)
{
//得到与激光的时间戳相对应的机器人的里程计的位姿
if(!getOdomPose(gmap_pose, scan.header.stamp))
return false;
//检测是否所有帧的数据都是相等的 如果不相等就进行计算 不知道为什么 感觉完全没必要啊。
//特别是对于champion_nav_msgs的LaserScan来说 激光束的数量经常变
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.
// 如果激光是反着装的，这激光的顺序需要反过来，同时这里会排除掉所有激光距离小于range_min的值。
// 排除的方式是把他们设置为最大值
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];
}
}
//把ROS的激光雷达数据信息 转换为 GMapping算法看得懂的形式
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("processing scan");
//调用gmapping算法进行处理
return gsp_->processScan(reading);
}4. gridslamprocessor.cpp 中的processScan
在scanmatcherprocessor里面也有一个这样的函数 但是那个函数是没有使用的
@desc 处理一帧激光数据 这里是gmapping算法的主要函数。
在这个函数里面调用其他的函数，包括里程计预测、激光测量更新、粒子采样等等步骤。
主要步骤如下：利用运动模型更新里程计分布
利用最近的一次观测来提高proposal分布。(scan-match)
利用proposal分布+激光雷达数据来确定各个粒子的权重
对粒子进行重采样
bool GridSlamProcessor::processScan(const RangeReading & reading, int adaptParticles)
{
/**retireve the position from the reading, and compute the odometry*/
/*得到当前的里程计的位置*/
OrientedPoint relPose=reading.getPose();
//relPose.y = m_odoPose.y;
/*m_count表示这个函数被调用的次数 如果是第0次调用,则所有的位姿都是一样的*/
if (!m_count)
{
m_lastPartPose=m_odoPose=relPose;
}
//write the state of the reading and update all the particles using the motion model
/*对于每一个粒子，都从里程计运动模型中采样，得到车子的初步估计位置 这一步对应于 里程计的更新 */
int tmp_size = m_particles.size();
//这个for循环显然可以用OpenMP进行并行化
//#pragma omp parallel for
for(int i = 0; i < tmp_size;i++)
{
OrientedPoint& pose(m_particles[i].pose);
pose = m_motionModel.drawFromMotion(m_particles[i],relPose,m_odoPose);
}
//invoke the callback
/*回调函数 实际上什么都没做*/
onOdometryUpdate();
// accumulate the robot translation and rotation
/*根据两次里程计的数据 计算出来机器人的线性位移和角度位移的累积值 m_odoPose表示上一次的里程计位姿 relPose表示新的里程计的位姿*/
OrientedPoint move=relPose-m_odoPose;
move.theta=atan2(sin(move.theta), cos(move.theta));
//统计机器人在进行激光雷达更新之前 走了多远的距离 以及 平移了多少的角度
m_linearDistance+=sqrt(move*move);
m_angularDistance+=fabs(move.theta);
// cerr <<"linear Distance:"<<m_linearDistance<<endl;
// cerr <<"angular Distance:"<<m_angularDistance<<endl;
// if the robot jumps throw a warning
/*
* 如果机器人在走了m_distanceThresholdCheck这么远的距离都没有进行激光雷达的更新
* 则需要进行报警。这个误差很可能是里程计或者激光雷达的BUG造成的。
* 例如里程计数据出错 或者 激光雷达很久没有数据等等
* 每次进行激光雷达的更新之后 m_linearDistance这个参数就会清零
*/
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();
std::cout <<std::endl<<"Start to Process Scan##################"<<std::endl;
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;
// m_infoStream << "FRAME " << m_readingCount<<endl;
// m_infoStream <<"Scan-Match Number: "<<m_count<<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
/*复制一帧数据 把激光数据转换为scan-match需要的格式*/
// if(reading.getSize() != m_beams)
// {
// cerr<<"********************************************"<<endl;
// cerr<<"********************************************"<<endl;
// cerr<<"reading_size:"<<reading.getSize()<<" m_beams:"<<m_beams<<endl;
// cerr<<"********************************************"<<endl;
// cerr<<"********************************************"<<endl;
// }
int beam_number = reading.getSize();
double * plainReading = new double[beam_number];
for(unsigned int i=0; i<beam_number; i++)
{
plainReading[i]=reading.m_dists[i];
}
//这个备份主要是用来储存的。
RangeReading* reading_copy;
//champion_nav_msgs激光数据
if(reading.m_angles.size() == reading.m_dists.size())
{
reading_copy = new RangeReading(beam_number,
&(reading.m_dists[0]),
&(reading.m_angles[0]),
static_cast<const RangeSensor*>(reading.getSensor()),
reading.getTime());
}
//ros的激光数据
else
{
reading_copy = new RangeReading(beam_number,
&(reading.m_dists[0]),
static_cast<const RangeSensor*>(reading.getSensor()),
reading.getTime());
}
/*如果不是第一帧数据*/
if (m_count>0)
{
/*
为每个粒子进行scanMatch，计算出来每个粒子的最优位姿，同时计算改最优位姿的得分和似然 对应于gmapping论文中的用最近的一次测量计算proposal的算法
这里面除了进行scanMatch之外，还对粒子进行了权重的计算，并计算了粒子的有效区域 但不进行内存分配 内存分配在resample()函数中
这个函数在gridslamprocessor.hxx里面。
*/
scanMatch(plainReading);
//至此 关于proposal的更新完毕了，接下来是计算权重
onScanmatchUpdate();
/*
由于scanMatch中对粒子的权重进行了更新，那么这个时候各个粒子的轨迹上的累计权重都需要重新计算
这个函数即更新各个粒子的轨迹上的累计权重是更新
GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized) 函数在gridslamprocessor_tree.cpp里面实现
*/
updateTreeWeights(false);
/*
* 粒子重采样 根据neff的大小来进行重采样 不但进行了重采样，也对地图进行更新
* GridSlamProcessor::resample 函数在gridslamprocessor.hxx里面实现
*/
std::cerr<<"plainReading:"<<m_beams<<std::endl;
resample(plainReading, adaptParticles, reading_copy);
}
/*如果是第一帧激光数据*/
else
{
//如果是第一帧数据，则可以直接计算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);
//m_matcher.registerScan(it->lowResolutionMap,it->pose,plainReading);
//为每个粒子创建路径的第一个节点。该节点的权重为0,父节点为it->node(这个时候为NULL)。
//因为第一个节点就是轨迹的根，所以没有父节点
TNode* node=new TNode(it->pose, 0., it->node, 0);
node->reading = reading_copy;
it->node=node;
}
}
// cerr << "Tree: normalizing, resetting and propagating weights at the end..." ;
//进行重采样之后，粒子的权重又会发生变化，因此需要再次更新粒子轨迹的累计权重
//GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized) 函数在gridslamprocessor_tree.cpp里面实现
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;
}
}
m_readingCount++;
return processed;
}5. 里程计的更新 调用里程计模型
这里用的模型是 《概率机器人》中的里程计模型
@desc 里程计运动模型
@p 表示粒子估计的最优位置(机器人上一个时刻的最优位置)
@pnew 表示里程计算出来的新的位置
@pold 表示里程计算出来的旧的位置(即上一个里程计的位置)
OrientedPoint
MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const{
double sxy=0.3*srr;
/*
* 计算出pnew 相对于 pold走了多少距离
* 这里的距离表达是相对于车身坐标系来说的。
*/
OrientedPoint delta=absoluteDifference(pnew, pold);
/*初始化一个点*/
OrientedPoint noisypoint(delta);
/*走过的X轴方向的距离加入噪声*/
noisypoint.x+=sampleGaussian(srr*fabs(delta.x)+str*fabs(delta.theta)+sxy*fabs(delta.y));
/*走过的Y轴方向的距离加入噪声*/
noisypoint.y+=sampleGaussian(srr*fabs(delta.y)+str*fabs(delta.theta)+sxy*fabs(delta.x));
/*走过的Z轴方向的距离加入噪声*/
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);
if (noisypoint.theta>M_PI)
noisypoint.theta-=2*M_PI;
/*把加入了噪声的值 加到粒子估计的最优的位置上 即得到新的位置(根据运动模型推算出来的位置)*/
return absoluteSum(p,noisypoint);
}6. 扫描匹配
在里程计的基础上为每个粒子进行扫描匹配，计算出来每个粒子的最优位姿，同时计算该最优位姿的得分和似然 对应于gmapping论文中的用最近的一次测量计算proposal的算法。
这里面除了进行scanMatch之外，还对粒子进行了权重的计算，并计算了粒子的有效区域 但不进行内存分配 内存分配在resample()函数中，这个函数在gridslamprocessor.hxx里面。
inline void GridSlamProcessor::scanMatch(const double* plainReading)
{
// sample a new pose from each scan in the reference
/*每个粒子都要进行scan-match*/
double sumScore=0;
int particle_number = m_particles.size();
//用openMP的方式来进行并行化，因此这里不能用迭代器 只能用下标的方式进行访问
//并行话之后会把里面的循环均匀的分到各个不同的核里面去。
//#pragma omp parallel for
for (int i = 0; i < particle_number;i++)
{
OrientedPoint corrected;
double score, l, s;
/*进行scan-match 计算粒子的最优位姿 调用scanmatcher.cpp里面的函数 --这是gmapping本来的做法*/
score=m_matcher.optimize(corrected, m_particles[i].map, m_particles[i].pose, plainReading);
/*矫正成功则更新位姿*/
if (score>m_minimumScore)
{
m_particles[i].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;
}
}
//粒子的最优位姿计算了之后，重新计算粒子的权重(相当于粒子滤波器中的观测步骤，计算p(z|x,m))，粒子的权重由粒子的似然来表示。
/*
* 计算粒子的得分和权重(似然) 注意粒子的权重经过ScanMatch之后已经更新了
* 在论文中 例子的权重不是用最有位姿的似然值来表示的。
* 是用所有的似然值的和来表示的。
*/
m_matcher.likelihoodAndScore(s, l, m_particles[i].map, m_particles[i].pose, plainReading);
sumScore+=score;
m_particles[i].weight+=l;
m_particles[i].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(m_particles[i].map, m_particles[i].pose, plainReading);
}
if (m_infoStream)
m_infoStream << "Average Scan Matching Score=" << sumScore/m_particles.size() << std::endl;
}7. 扫描匹配中 调用 scan_mather 中的 optimize 计算粒子的最后位姿
主要方法就是粒子的8个方向运动+score()中激光扫描观测数据。
在 scanMather 中还有另外一个optimize（）函数，该函数是根据地图、激光数据、位姿迭代求解一个最优位姿来。这个函数与实际使用的函数不同的是，这个函数求出来的位姿不是一个特定的最优的cell，而是假设在scan-match的过程中经过的所有的点服从高斯分布，然后根据经过的所有位姿的似然来求解得到一个加权位姿和位姿的方差。这里认为机器人在scan-match过程中经过的路径点合起来是服从高斯分布的，因此机器人的位姿和方差最终由所有经过的点的加权和来计算得到。
理论上来说这是一个更合理的选择。因为GMapping的论文里面也是选择了这样的方法，但是实际并没有采用
@desc 根据地图、激光数据、位姿迭代求解一个最优的新的位姿出来这个函数是真正被调用来进行scan-match的函数
@param pnew 新的最优位置
@param map 地图
@param init 初始位置
@param readings 激光数据
double ScanMatcher::optimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings) const
{
double bestScore=-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};
//enum Move{Front, Back, Left, Right, TurnLeft, TurnRight, Done};
int c_iterations=0;
do
{
/*如果这一次(currentScore)算出来比上一次(bestScore)差，则有可能是走太多了，要减少搜索步长 这个策略跟LM有点像*/
if (bestScore>=currentScore)
{
refinement++;
adelta*=.5;
ldelta*=.5;
}
bestScore=currentScore;
OrientedPoint bestLocalPose=currentPose;
OrientedPoint localPose=currentPose;
/*把8个方向都搜索一次 得到这8个方向里面最好的一个位姿和对应的得分*/
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;
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);
}
/*计算得分=增益*score*/
double localScore=odo_gain*score(map, localPose, readings);
/*如果得分更好，则更新*/
if (localScore>currentScore)
{
currentScore=localScore;
bestLocalPose=localPose;
}
c_iterations++;
} while(move!=Done);
/* 把当前位置设置为目前最优的位置 如果8个值都被差了的话，那么这个值不会更新*/
currentPose=bestLocalPose;
}while (currentScore>bestScore || refinement<m_optRecursiveIterations);
/*返回最优位置和得分*/
pnew=currentPose;
return bestScore;
}8. optimize 中计算得分的 score()函数
根据地图、机器人位置、激光雷达数据，计算出一个得分，得分原理为likelihood_field_range_finder_model
注意：代码中还提供了 icpOptimize（）函数，根据地图、激光数据、位姿用icp迭代求解出一个最优的位姿。理论上这种优化方式应该要比类似梯度下降方法的效果要好。
但实际上比那种的效果差，不知道为什么
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;
/*
把激光雷达的坐标转换到世界坐标系
先旋转到机器人坐标系，然后再转换到世界坐标系
p表示base_link在map中的坐标
m_laserPose 表示base_laser在base_link坐标系中的坐标
*/
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;
/*
* map.getDelta表示地图分辨率 m_freeCellRatio = sqrt(2)
* 意思是如果激光hit了某个点 那么沿着激光方向的freeDelta距离的地方要是空闲才可以
*/
unsigned int skip=0;
double freeDelta=map.getDelta()*m_freeCellRatio;
//枚举所有的激光束
for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++)
{
skip++;
skip=skip>m_likelihoodSkip?0:skip;
if (skip||*r>m_usableRange||*r==0.0) continue;
/*被激光雷达击中的点 在地图坐标系中的坐标*/
Point phit=lp;
phit.x+=*r*cos(lp.theta+*angle);
phit.y+=*r*sin(lp.theta+*angle);
IntPoint iphit=map.world2map(phit);
/*该束激光的最后一个空闲坐标，即紧贴hitCell的freeCell 原理为：假设phit是被激光击中的点，这样的话沿着激光方向的前面一个点必定的空闲的*/
Point pfree=lp;
//理论上来说 这个应该是一个bug。修改成下面的之后，改善不大。
// pfree.x+=(*r-map.getDelta()*freeDelta)*cos(lp.theta+*angle);
// pfree.y+=(*r-map.getDelta()*freeDelta)*sin(lp.theta+*angle);
pfree.x+=(*r - freeDelta)*cos(lp.theta+*angle);
pfree.y+=(*r - freeDelta)*sin(lp.theta+*angle);
/*phit 和 freeCell的距离*/
pfree=pfree-phit;
IntPoint ipfree=map.world2map(pfree);
/*在kernelSize大小的窗口中搜索出最优最可能被这个激光束击中的点 这个kernelSize在大小为1*/
bool found=false;
Point bestMu(0.,0.);
for (int xx=-m_kernelSize; xx<=m_kernelSize; xx++)
for (int yy=-m_kernelSize; yy<=m_kernelSize; yy++)
{
/*根据已开始的关系 搜索phit的时候，也计算出来pfree*/
IntPoint pr=iphit+IntPoint(xx,yy);
IntPoint pf=pr+ipfree;
/*得到各自对应的Cell*/
const PointAccumulator& cell=map.cell(pr);
const PointAccumulator& fcell=map.cell(pf);
/*
(double)cell返回的是该cell被占用的概率
这束激光要合法必须要满足cell是被占用的，而fcell是空闲的
原理为：假设phit是被激光击中的点，这样的话沿着激光方向的前面一个点必定的空闲的
*/
if (((double)cell )> m_fullnessThreshold && ((double)fcell )<m_fullnessThreshold)
{
/*计算出被击中的点与对应的cell的currentScore距离*/
Point mu=phit-cell.mean();
if (!found)
{
bestMu=mu;
found=true;
}
else
{
bestMu=(mu*mu)<(bestMu*bestMu)?mu:bestMu;
}
}
}
/*socre的计算公式exp(-d^2 / sigma)) 这里的sigma表示方差 不是标准差*/
if (found)
{
double tmp_score = exp(-1.0/m_gaussianSigma*bestMu*bestMu);
s += tmp_score;
//只在周围的9个栅格里面寻找，因此平方的意义不大。
//s += tmp_score*tmp_score;
}
}
return s;
}9. 权重计算
根据地图、机器人位置、激光雷达数据，同时计算出一个得分和似然：原理为likelihood_field_range_finder_model 计算出来的似然即为粒子的权重
这个函数跟score是非常像的，不同的是这个函数除了计算的得分之外，还计算的似然likelihood
这个函数的计算出来的似然在gmapping中用来表示粒子的权重
@param s 得分
@param l 似然 粒子的权重
@param map 对应的地图
@param p 机器人对应的初始位置
@param readings 激光雷达数据
inline unsigned int ScanMatcher::likelihoodAndScore(double& s, double& l, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const
{
using namespace std;
l=0;
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;
//如果没有击中的时候的似然值 nullLikehood = -0.5
double noHit=nullLikelihood/(m_likelihoodSigma);
unsigned int skip=0;
unsigned int c=0;
double freeDelta=map.getDelta()*m_freeCellRatio;
for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++)
{
/*每隔m_likelihoodSkip个激光束 就跳过一个激光束*/
skip++;
skip=skip>m_likelihoodSkip?0:skip;
if (*r>m_usableRange) continue;
if (skip) continue;
/*被激光击中的点*/
Point phit=lp;
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-freeDelta)*cos(lp.theta+*angle);
pfree.y+=(*r-freeDelta)*sin(lp.theta+*angle);
pfree=pfree-phit;
IntPoint ipfree=map.world2map(pfree);
/*在对应的kernerSize中搜索*/
bool found=false;
Point bestMu(0.,0.);
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;
const PointAccumulator& cell=map.cell(pr);
const PointAccumulator& fcell=map.cell(pf);
/*如果cell(pr)被占用 而cell(pf)没有被占用 则说明找到了一个合法的点*/
if (((double)cell )>m_fullnessThreshold && ((double)fcell )<m_fullnessThreshold)
{
Point mu=phit-cell.mean();
if (!found)
{
bestMu=mu;
found=true;
}
else
{
bestMu=(mu*mu)<(bestMu*bestMu)?mu:bestMu;
}
}
}
/*计算得分 得分是只计有用的激光束*/
if (found)
{
s+=exp(-1./m_gaussianSigma*bestMu*bestMu);
c++;
}
/*计算似然 似然是计算所有的激光束 如果某一个激光束打中了空地 那也需要计算进去*/
if (!skip)
{
//似然不是指数 似然只是指数的上标
double f=(-1./m_likelihoodSigma)*(bestMu*bestMu);
l+=(found)?f:noHit;
}
}
return c;
}10. 计算地图区域
> 计算有效区域，通过激光雷达的数据计算出来哪个地图栅格应该要被更新了。(这里只是计算出来栅格的位置，然后插入地图中,并不对数据进行更新)
>
> 这里计算的有效区域的坐标都是patch坐标，不是cell坐标
***注意！！！！！：：***
这个函数在正常的进行SLAM算法的过程中，使用了m_generateMap = false。这个时候不会为了空闲区域分配内存。
当要生成可视化的地图的时候,m_generateMap = true。这个时候就会为空闲区域分配内存
计算激光点经过栅格的区域应用的额bresenham算法 该算法可以参考
```
void ScanMatcher::computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p, const double* readings)
{
/*已经计算过了，则没必要计算了*/
if (m_activeAreaComputed)
return;
/*把激光雷达的位置转换到地图坐标系*/
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;
IntPoint p0=map.world2map(lp);
/*地图的范围*/
Point min(map.map2world(0,0));
Point max(map.map2world(map.getMapSizeX()-1,map.getMapSizeY()-1));
/*扩展地图的范围*/
if (lp.x<min.x) min.x=lp.x;
if (lp.y<min.y) min.y=lp.y;
if (lp.x>max.x) max.x=lp.x;
if (lp.y>max.y) max.y=lp.y;
/*determine the size of the area*/
/*根据激光数据扩展地图的范围*/
const double * angle=m_laserAngles+m_initialBeamsSkip;
for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++)
{
/*去除不合理的值*/
if (*r>m_laserMaxRange||*r==0.0||isnan(*r)) continue;
//根据设置截断gmapping的值
double d=*r>m_usableRange?m_usableRange:*r;
/*被激光击中的位置*/
Point phit=lp;
phit.x+=d*cos(lp.theta+*angle);
phit.y+=d*sin(lp.theta+*angle);
/*扩充范围*/
if (phit.x<min.x) min.x=phit.x;
if (phit.y<min.y) min.y=phit.y;
if (phit.x>max.x) max.x=phit.x;
if (phit.y>max.y) max.y=phit.y;
}
//min=min-Point(map.getDelta(),map.getDelta());
//max=max+Point(map.getDelta(),map.getDelta());
/*如果地图需要扩展*/
if ( !map.isInside(min) || !map.isInside(max))
{
/*得到目前地图的大小*/
Point lmin(map.map2world(0,0));
Point lmax(map.map2world(map.getMapSizeX()-1,map.getMapSizeY()-1));
//cerr << "CURRENT MAP " << lmin.x << " " << lmin.y << " " << lmax.x << " " << lmax.y << endl;
//cerr << "BOUNDARY OVERRIDE " << min.x << " " << min.y << " " << max.x << " " << max.y << endl;
/*如果需要扩充，则把对应的维度扩展m_enlargeStep的大小*/
min.x=( min.x >= lmin.x )? lmin.x: min.x-m_enlargeStep;
max.x=( max.x <= lmax.x )? lmax.x: max.x+m_enlargeStep;
min.y=( min.y >= lmin.y )? lmin.y: min.y-m_enlargeStep;
max.y=( max.y <= lmax.y )? lmax.y: max.y+m_enlargeStep;
map.resize(min.x, min.y, max.x, max.y);
//cerr << "RESIZE " << min.x << " " << min.y << " " << max.x << " " << max.y << endl;
}
/*地图的有效区域(地图坐标系)*/
HierarchicalArray2D<PointAccumulator>::PointSet activeArea;
/*allocate the active area*/
angle=m_laserAngles+m_initialBeamsSkip;
for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++)
{
/*如果需要生成地图*/
if (m_generateMap)
{
//排除错误的激光点
double d=*r;
if (d>m_laserMaxRange||d==0.0||isnan(d))
continue;
if (d>m_usableRange)
d=m_usableRange;
/*激光束的起点和终点*/
Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
IntPoint p0=map.world2map(lp);
IntPoint p1=map.world2map(phit);
/*bresenham算法来计算激光起点到终点要经过的路径*/
GridLineTraversalLine line;
line.points=m_linePoints;
GridLineTraversal::gridLine(p0, p1, &line);
/*更新地图 把画线算法计算出来的值都算进去*/
for (int i=0; i<line.num_points-1; i++)
{
assert(map.isInside(m_linePoints[i]));
activeArea.insert(map.storage().patchIndexes(m_linePoints[i]));
assert(m_linePoints[i].x>=0 && m_linePoints[i].y>=0);
}
//如果激光距离小于使用的值 则需要把定点也算进去 说明这个值是好的。
//同时如果d>=m_usableRange 那么说明这个值只用来进行标记空闲区域 不用来进行标记障碍物
if (d<m_usableRange)
{
IntPoint cp=map.storage().patchIndexes(p1);
assert(cp.x>=0 && cp.y>=0);
activeArea.insert(cp);
}
}
else
{
if (*r>m_laserMaxRange||*r>m_usableRange||*r==0.0||isnan(*r)) continue;
Point phit=lp;
phit.x+=*r*cos(lp.theta+*angle);
phit.y+=*r*sin(lp.theta+*angle);
IntPoint p1=map.world2map(phit);
assert(p1.x>=0 && p1.y>=0);
IntPoint cp=map.storage().patchIndexes(p1);
assert(cp.x>=0 && cp.y>=0);
activeArea.insert(cp);
}
}
map.storage().setActiveArea(activeArea, true);
m_activeAreaComputed=true;
}
```11. 权重更新
scanMatch中对粒子的权重进行了更新，接下来就是对各个粒子的轨迹上累计权重进行更新
```
void GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized)
{
if (!weightsAlreadyNormalized)
{
normalize();
}
//把所有粒子的所有的轨迹中各个节点的accWeight清零
resetTree();
//求累计权重
propagateWeights();
}
```
把粒子权重归一化，同时计算出neff
```
inline void GridSlamProcessor::normalize()
{
//normalize the log m_weights
double gain=1./(m_obsSigmaGain*m_particles.size());
/*求所有粒子中的最大的权重*/
double lmax= -std::numeric_limits<double>::max();
for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++)
{
lmax=it->weight>lmax?it->weight:lmax;
}
//cout << "!!!!!!!!!!! maxwaight= "<< lmax << endl;
/*权重以最大权重为中心的高斯分布*/
m_weights.clear();
double wcum=0;
m_neff=0;
for (std::vector<Particle>::iterator it=m_particles.begin(); it!=m_particles.end(); it++)
{
m_weights.push_back(exp(gain*(it->weight-lmax)));
wcum+=m_weights.back();
//cout << "l=" << it->weight<< endl;
}
/*
计算有效粒子数 和 归一化权重
权重=wi/w
neff = 1/w*w
*/
m_neff=0;
for (std::vector<double>::iterator it=m_weights.begin(); it!=m_weights.end(); it++)
{
*it=*it/wcum;
double w=*it;
m_neff+=w*w;
}
m_neff=1./m_neff;
}
```
```
/*
* 这个函数被下面的函数调用
* 递归函数，
* weight表示节点n的所有子节点的累计权重
* 迭代求一条路径上所有点的累计权重
* 节点的累计权重表示该节点的所有子节点的权重的和
*/
double propagateWeight(GridSlamProcessor::TNode* n, double weight)
{
if (!n)
return weight;
double w=0;
n->visitCounter++;
n->accWeight+=weight;
if (n->visitCounter==n->childs)
{
w=propagateWeight(n->parent,n->accWeight);
}
assert(n->visitCounter<=n->childs);
return w;
}
/*
这个函数被updateTreeWeights()调用
*/
double GridSlamProcessor::propagateWeights()
{
// don't calls this function directly, use updateTreeWeights(..) !
// all nodes must be resetted to zero and weights normalized
// the accumulated weight of the root
// 求所有根节点的累计权重之和
double lastNodeWeight=0;
// sum of the weights in the leafs
// 所有叶子节点的权重 也就是m_weights数组里面所有元素的和
double aw=0;
std::vector<double>::iterator w=m_weights.begin();
for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++)
{
//求解所有叶子节点的累计权重
double weight=*w;
aw+=weight;
//叶子节点的子节点累计权重就等于自己的权重 因为它没有子节点
//每一个粒子的路径都是从叶子节点开始的，得到了叶子节点，就得到了路径
TNode * n=it->node;
n->accWeight=weight;
lastNodeWeight+=propagateWeight(n->parent,n->accWeight);
w++;
}
if (fabs(aw-1.0) > 0.0001 || fabs(lastNodeWeight-1.0) > 0.0001)
{
cerr << "ERROR: ";
cerr << "root->accWeight=" << lastNodeWeight << " sum_leaf_weights=" << aw << endl;
assert(0);
}
return lastNodeWeight;
}
```12. 重采样
根据上面函数计算的neff大小来进行重采样，同时也对地图进行更新。resample 函数实际实现在gridslamprocessor.hxx里面实现
重采样分两步：
1.需要重采样，则所有保留下来的粒子的轨迹都加上一个新的节点，然后进行地图更新。
2.不需要重采样，则所有的粒子的轨迹都加上一个新的节点，然后进行地图的更新
在重采样完毕之后，会调用registerScan函数来更新地图
```
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);
}
/*如果需要进行重采样*/
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.
//枚举每一个要被保留的粒子
for (unsigned int i=0; i<m_indexes.size(); 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]];
//创建一个新的节点 改节点的父节点为oldNode
node=new TNode(p.pose, 0, oldNode, 0);
node->reading=reading;
//这个要保留下来的粒子，要保留的粒子的下标为m_indexs
temp.push_back(p);
temp.back().node=node;
temp.back().previousIndex=m_indexes[i];
}
while(j<m_indexes.size())
{
deletedParticles.push_back(j);
j++;
}
//把要删除的粒子的Node都删除掉，Node表示轨迹的起点(最新的点)
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;
std::cerr << "Deleting old particles..." ;
std::cerr << "Done" << std::endl;
//清楚全部的粒子 然后从tmp中读取保留下来的粒子
m_particles.clear();
//枚举要保留下来的所有的粒子 每个粒子都需要更新地图
std::cerr << "Copying Particles and Registering scans...";
//对于保留下来的粒子进行更新 这里是可以并行化操作的。
//在并行化操作里面 m_particles.push_back()会报错 因此并行化 需要把push_back()提出来。
//在外面的的for循环进行
int tmp_size = temp.size();
//#pragma omp parallel for
for(int i = 0; i<tmp_size;i++)
{
//对保留下来的粒子数据进行更新
//每个粒子的权重都设置为相同的值
temp[i].setWeight(0);
//为每个粒子更新running_scans
//增加了一帧激光数据 因此需要更新地图
m_matcher.registerScan(temp[i].map,temp[i].pose,plainReading);
//m_matcher.registerScan(temp[i].lowResolutionMap,temp[i].pose,plainReading);
}
//提取出来 防止并行优化时报错
for(int i = 0; i< tmp_size;i++)
m_particles.push_back(temp[i]);
std::cerr << " Done" <<std::endl;
hasResampled = true;
}
/*否则的话，进行扫描匹配*/
else
{
//不进行重采样的话，权值不变。只为轨迹创建一个新的节点
//为每个粒子更新地图 同样可以并行化
int particle_size = m_particles.size();
//#pragma omp parallel for
for(int i = 0; i < particle_size;i++)
{
//创建一个新的树节点
TNode* node = 0;
node = new TNode(m_particles[i].pose,0.0,oldGeneration[i],0);
//把这个节点接入到树中
node->reading = reading;
m_particles[i].node = node;
//更新各个例子的地图
m_matcher.invalidateActiveArea();
m_matcher.registerScan(m_particles[i].map, m_particles[i].pose, plainReading);
m_particles[i].previousIndex = i;
}
std::cerr<<std::endl;
// int index=0;
// 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;
}
```
已知位置的覆盖栅格地图算法(使用的模型为Counting Model) Counting Model：某一个点被覆盖的概率为被占用的次数与被访问的次数的比值。
```
double ScanMatcher::registerScan(ScanMatcherMap& map, const OrientedPoint& p, const double* readings)
{
if (!m_activeAreaComputed)
computeActiveArea(map, p, readings);
//每次registerScan的时候都进行计算
computeActiveArea(map, p, readings);
//this operation replicates the cells that will be changed in the registration operation
//为activeArea里面的没有分配内存的区域分配内存
map.storage().allocActiveArea();
/*把激光雷达的位置转换到地图坐标系*/
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;
IntPoint p0=map.world2map(lp);
const double * angle=m_laserAngles+m_initialBeamsSkip;
/*esum表示总体的熵的增加或者减少*/
double esum=0;
for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++)
{
if (m_generateMap)
{
/*去除非法的激光束*/
double d=*r;
if (d>m_laserMaxRange||d==0.0||isnan(d))
continue;
if (d>m_usableRange)
d=m_usableRange;
/*被该激光束击中的点的地图坐标*/
Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
IntPoint p1=map.world2map(phit);
//IntPoint linePoints[20000] ;
/*bresenham画线算法来计算 激光位置和被激光击中的位置之间的空闲位置*/
GridLineTraversalLine line;
line.points=m_linePoints;
GridLineTraversal::gridLine(p0, p1, &line);
/*更新空闲位置*/
for (int i=0; i<line.num_points-1; i++)
{
PointAccumulator& cell=map.cell(line.points[i]);
/*更新前的熵的负数*/
double e=-cell.entropy();
cell.update(false, Point(0,0));
/*加上更新后的熵，即更新后的熵减去更新前的熵，求得这次更新对于熵的变化*/
e+=cell.entropy();
esum+=e;
}
/*更新被击中的位置 只有小于m_usableRange的栅格来用来标记障碍物*/
if (d<m_usableRange)
{
double e=-map.cell(p1).entropy();
map.cell(p1).update(true, phit);
e+=map.cell(p1).entropy();
esum+=e;
}
}
else
{
if (*r>m_laserMaxRange||*r>m_usableRange||*r==0.0||isnan(*r)) continue;
/*被击中的点的地图坐标*/
Point phit=lp;
phit.x+=*r*cos(lp.theta+*angle);
phit.y+=*r*sin(lp.theta+*angle);
IntPoint p1=map.world2map(phit);
assert(p1.x>=0 && p1.y>=0);
/*更新对应的cell的值*/
map.cell(p1).update(true,phit);
}
}
return esum;
}
```13. 在重采样之后 粒子权重又会发生变化，因此需要再次更新粒子轨迹的积累权重
再次调用updateTreeWeights()。14. 更新地图
slam_gmapping 中的updateMap 主要是为了可视化，真正的地图都存在粒子中，这里就是拿一个最大的粒子地图发布出来
> 得到最大的粒子，然后遍历这个粒子的整个轨迹，根据轨迹记录的信息进行建图，然后把得到的地图发布出去
gsp_->getBestParticleIndex() 该函数得到累计权重最大的粒子的下标
注意不是当前的最大权重的粒子，是累计权重最大的粒子， 累计权重即该粒子在各个时刻的权重之和(轨迹上的各个节点的权重之和)
```
int GridSlamProcessor::getBestParticleIndex() const
{
unsigned int bi=0;
double bw=-std::numeric_limits<double>::max();
for (unsigned int i=0; i<m_particles.size(); i++)
{
if (bw<m_particles[i].weightSum)
{
bw=m_particles[i].weightSum;
bi=i;
}
}
return (int) bi;
}
```void SlamGMapping::updateMap(const sensor_msgs::LaserScan& scan)
{
ROS_DEBUG("Update map");
boost::mutex::scoped_lock map_lock (map_mutex_);
GMapping::ScanMatcher matcher;
/*设置scanmatcher的各个参数*/
matcher.setLaserParameters(scan.ranges.size(), &(laser_angles_[0]),
gsp_laser_->getPose());
matcher.setlaserMaxRange(maxRange_);
matcher.setusableRange(maxUrange_);
matcher.setgenerateMap(true);
/*得到权值最高的粒子*/
GMapping::GridSlamProcessor::Particle best =
gsp_->getParticles()[gsp_->getBestParticleIndex()];
//发布位姿的熵
std_msgs::Float64 entropy;
entropy.data = computePoseEntropy();
if(entropy.data > 0.0)
entropy_publisher_.publish(entropy);
//如果没有地图 则初始化一个地图
if(!got_map_)
{
map_.map.info.resolution = delta_;
map_.map.info.origin.position.x = 0.0;
map_.map.info.origin.position.y = 0.0;
map_.map.info.origin.position.z = 0.0;
map_.map.info.origin.orientation.x = 0.0;
map_.map.info.origin.orientation.y = 0.0;
map_.map.info.origin.orientation.z = 0.0;
map_.map.info.origin.orientation.w = 1.0;
}
/*地图的中点*/
GMapping::Point center;
center.x=(xmin_ + xmax_) / 2.0;
center.y=(ymin_ + ymax_) / 2.0;
/*初始化一个scanmatcherMap 创建一个地图*/
GMapping::ScanMatcherMap smap(center, xmin_, ymin_, xmax_, ymax_,
delta_);
/*更新地图*/
//遍历粒子的整条轨迹 按照轨迹上各个节点存储的信息来重新计算一个地图
ROS_DEBUG("Trajectory tree:");
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]));
//matcher.registerScan(smap, n->pose, &((*n->reading)[0]));
matcher.registerScan(smap, n->pose, &(n->reading->m_dists[0]));
}
// the map may have expanded, so resize ros message as well
// 扩充地图的大小
if(map_.map.info.width != (unsigned int) smap.getMapSizeX() || map_.map.info.height != (unsigned int) smap.getMapSizeY())
{
// NOTE: The results of ScanMatcherMap::getSize() are different from the parameters given to the constructor
// so we must obtain the bounding box in a different way
GMapping::Point wmin = smap.map2world(GMapping::IntPoint(0, 0));
GMapping::Point wmax = smap.map2world(GMapping::IntPoint(smap.getMapSizeX(), smap.getMapSizeY()));
xmin_ = wmin.x; ymin_ = wmin.y;
xmax_ = wmax.x; ymax_ = wmax.y;
ROS_DEBUG("map size is now %dx%d pixels (%f,%f)-(%f, %f)", smap.getMapSizeX(), smap.getMapSizeY(),
xmin_, ymin_, xmax_, ymax_);
map_.map.info.width = smap.getMapSizeX();
map_.map.info.height = smap.getMapSizeY();
map_.map.info.origin.position.x = xmin_;
map_.map.info.origin.position.y = ymin_;
map_.map.data.resize(map_.map.info.width * map_.map.info.height);
ROS_DEBUG("map origin: (%f, %f)", map_.map.info.origin.position.x, map_.map.info.origin.position.y);
}
//根据地图的信息计算出来各个点的情况:occ、free、noinformation
//这样对地图进行标记主要是方便用RVIZ显示出来
for(int x=0; x < smap.getMapSizeX(); x++)
{
for(int y=0; y < smap.getMapSizeY(); y++)
{
/// @todo Sort out the unknown vs. free vs. obstacle thresholding
/// 得到.xy被占用的概率
GMapping::IntPoint p(x, y);
double occ=smap.cell(p);
assert(occ <= 1.0);
//unknown
if(occ < 0)
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = GMAPPING_UNKNOWN;
//占用
else if(occ > occ_thresh_)
{
//map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = (int)round(occ*100.0);
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = GMAPPING_OCC;
}
//freespace
else
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = GMAPPING_FREE;
}
}
//到了这一步，肯定是有地图了。
got_map_ = true;
//make sure to set the header information on the map
//把计算出来的地图发布出去
map_.map.header.stamp = ros::Time::now();
map_.map.header.frame_id = tf_.resolve( map_frame_ );
sst_.publish(map_.map);
sstm_.publish(map_.map.info);
}
至此整个 建图的流程是梳理出来了。
本文章为转载内容，我们尊重原作者对文章享有的著作权。如有内容错误或侵权问题，欢迎原作者联系我们进行内容更正或删除文章。
