main主函数
startLiveSlam函数
laserCallback激光处理回调函数
initMapper初始化地图函数
addScan激光数据处理函数
processScan函数
drawFromMotion运动模型数据更新函数
scanMatch扫描匹配函数
optimize位姿优化函数
score函数
likelihoodAndScore函数
invalidateActiveArea函数
computeActiveArea计算有效区域函数
updateTreeWeights更新粒子权重函数
normalize归一化函数
resetTree重置粒子树函数
propagateWeights计算权重函数
registerScan地图状态更新函数
resample粒子重采样函数
updateMap地图更新函数

Gmapping主要流程

main函数

#include <ros/ros.h>#include "slam_gmapping.h"int
main(int argc, char** argv)
{ros::init(argc, argv, "slam_gmapping");//**初始化slam节点**SlamGMapping gn;gn.startLiveSlam();//开始slam进程ros::spin();return(0);
}

startLiveSlam函数

/*订阅一些主题 发布一些主题*/
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);{//订阅激光数据 同时和odom_frame之间的转换同步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);scan_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1));std::cout <<"Subscribe LaserScan & odom!!!"<<std::endl;}/*发布转换关系的线程*/transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_));
}

laserCallback函数

/** 接受到激光雷达数据的回调函数 在这里面调用addScan()函数* 如果addScan()函数调用成功，也就是说激光数据被成功的插入到地图中后，* 如果到了地图更新的时间，则对地图进行更新，通过调用updateMap()函数来进行相应的操作。** laserCallback()->addScan()->gmapping::processScan()*                ->updateMap()*
*/
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 scanif(!got_first_scan_){if(!initMapper(*scan))return;got_first_scan_ = true;}GMapping::OrientedPoint odom_pose;if(addScan(*scan, odom_pose))//进入激光数据处理函数{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);last_map_update = scan->header.stamp;ROS_DEBUG("Updated the map");}}elseROS_DEBUG("cannot process scan");
}

initMapper函数

/*** @brief SlamGMapping::initMapper  gmapping算法的初始化* 这个函数在收到的第一帧激光雷达数据的时候会被调用一次，之后就再也不会被调用了。* 这个函数的功能主要就是对gmapping算法中需要的一些参数进行赋值，即对gmapping算法进行初始化* 这个参数一开始由gmapping wrapper读取，但是在这个函数里面真正的会赋值给gmapping算法* 1.判断激光雷达是否是水平放置的，如果不是 则报错。* 2.假设激光雷达数据的角度是对称的 & 递增的 为每个激光束分配角度。* 3.为gmapping算法设置各种需要的参数。*** @param scan* @return*/
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的坐标也依然是1tf::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 centereddouble 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算法设置sensormapGMapping::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;
}

addScan激光数据处理函数

/** 加入一个激光雷达的数据 主要函数 这里面会调用processScan()函数* 这个函数被laserCallback()函数调用
*/
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'tif(scan.ranges[num_ranges - i - 1] < scan.range_min)ranges_double[i] = (double)scan.range_max;elseranges_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'tif(scan.ranges[i] < scan.range_min)ranges_double[i] = (double)scan.range_max;elseranges_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);
}

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 forfor(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 stepfor (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){it->previousPose=it->pose;}}m_readingCount++;return processed;}

drawFromMotion运动模型数据更新函数

/*
@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);
}

scanMatch扫描匹配函数

/**Just scan match every single particle.
If the scan matching fails, the particle gets a default likelihood.
@desc 为每一个粒子都计算扫描匹配。扫描匹配即为在里程计的基础上，通过优化求得位姿
这个函数是最终被使用的函数这个函数不但对每个位姿进行scan-match来优化，再优化之后，还会计算每个粒子的权重
这里的权重用似然表示。
*/
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的方式来进行并行化，因此这里不能用迭代器 只能用下标的方式进行访问//并行话之后会把里面的循环均匀的分到各个不同的核里面去。/* 计算最优的粒子optimize 调用了 score 这个函数 （计算粒子得分）在score 函数里，首先计算障碍物的坐标phit，然后将phit转换成网格坐标iPhit计算光束上与障碍物相邻的非障碍物网格坐标pfree,pfrree由phit沿激光束方向移动一个网格的距离得到，将pfree转换成网格坐标ipfree（增量，并不是实际值）在iphit 及其附近8个（m_kernelSize:default=1）栅格（pr,对应自由栅格为pf）搜索最优可能是障碍物的栅格。最优准则： pr 大于某一阈值，pf小于该阈值，且pr栅格的phit的平均坐标与phit的距离bestMu最小。得分计算： s +=exp(-1.0/m_gaussianSigma*bestMu*besMu)  参考NDT算法,距离越大，分数越小，分数的较大值集中在距离最小值处，符合正态分布模型至此 score 函数结束并返回粒子（currentPose）得分，然后回到optimize函数optimize 干的事就是 currentPose 的位姿进行微调，前、后、左、右、左转、右转 共6次，然后选取得分最高的位姿，返回最终的得分*///#pragma omp parallel forfor (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;
}

optimize位姿优化函数

/*
/ zq commit
@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;
}

score函数

/*
@desc      根据地图、机器人位置、激光雷达数据，计算出一个得分：原理为likelihood_field_range_finder_model
这个函数被scanmatcher.cpp里面的optimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings)
调用
@map       对应的地图
@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;/*把激光雷达的坐标转换到世界坐标系先旋转到机器人坐标系，然后再转换到世界坐标系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;
}

likelihoodAndScore函数

/*
@desc              根据地图、机器人位置、激光雷达数据，同时计算出一个得分和似然：原理为likelihood_field_range_finder_model计算出来的似然即为粒子的权重这个函数被scanmatcher.cpp里面的optimize(OrientedPoint& _mean, ScanMatcher::CovarianceMatrix& _cov, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings)调用这个函数跟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.5double 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;
}

invalidateActiveArea函数

/*** @brief ScanMatcher::invalidateActiveArea* 每次调用computeActiveArea()之前，都必须要调用这个函数*/
void ScanMatcher::invalidateActiveArea()
{m_activeAreaComputed=false;
}

computeActiveArea计算有效区域函数

/*
@desc 计算有效区域，通过激光雷达的数据计算出来哪个地图栅格应该要被更新了。(这里只是计算出来栅格的位置，然后插入地图中,并不对数据进行更新)
这里计算的有效区域的坐标都是patch坐标，不是cell坐标注意！！！！！：：
这个函数在正常的进行SLAM算法的过程中，使用了m_generateMap = false。这个时候不会为了空闲区域分配内存。
当要生成可视化的地图的时候,m_generateMap = true。这个时候就会为空闲区域分配内存@param map          地图
@param p           机器人位置
@param readings        激光雷达数据
*/
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;
}

updateTreeWeights更新粒子权重函数

/*
更新权重
调用这个函数的目的是因此
*/
void  GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized)
{if (!weightsAlreadyNormalized) {normalize();}resetTree();propagateWeights();
}

normalize归一化函数

/*
@desc 把粒子的权重归一化
主要功能为归一化粒子的权重，同时计算出neff
*/
inline void GridSlamProcessor::normalize()
{//normalize the log m_weightsdouble 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/wneff = 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;}

resetTree重置粒子树函数

/*把所有粒子的所有的轨迹中各个节点的accWeight清零*/
void GridSlamProcessor::resetTree()
{// don't calls this function directly, use updateTreeWeights(..) !for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){TNode* n=it->node;while (n){n->accWeight=0;n->visitCounter=0;n=n->parent;}}
}

propagateWeights函数

/*
这个函数被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;/** * 递归函数，* accWeight)表示节点n的所有子节点的累计权重* 迭代求一条路径上所有点的累计权重* 节点的累计权重表示该节点的所有子节点的权重的和*/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;
}

registerScan函数

/*
@desc      已知位置的覆盖栅格地图算法(使用的模型为Counting Model)Counting Model：某一个点被覆盖的概率为被占用的次数与被访问的次数的比值
@map       对应的地图
@p         机器人的位姿      机器人的位姿  不是激光雷达的位姿
@reading    激光雷达的数据
*/
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;
}

resample粒子重采样函数

/*
@desc 粒子滤波器重采样。分为两步：
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]];//创建一个新的节点 改节点的父节点为oldNodenode=new   TNode(p.pose, 0, oldNode, 0);node->reading=reading;//这个要保留下来的粒子，要保留的粒子的下标为m_indexstemp.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 forfor(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 forfor(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;}

updateMap地图更新函数

/*
更新地图:这里的更新地图，只是为了可视化。因为真正的地图都存储在粒子里面。
这里会拿一个权值最大的粒子的地图发布出来.得到权值最大的粒子，然后遍历这个粒子的整个轨迹，根据轨迹上记录的信息来进行建图
然后把得到的地图发布出去
这个函数被laserCallback()调用，每次addScan()成功了，就会调用这个函数来生成地图，并发布出去*/
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 wayGMapping::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);//unknownif(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;}//freespaceelsemap_.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);
}

Gmapping Slam主要代码分析相关推荐

SLAM——ORB-SLAM3代码分析（七）Converter
2021SC@SDUSC Converter分析 Converter主要是一些常用的转换的实现:比如将g2o类型的数据,cv类型的数据,vector类型的数据进行转换. toDescriptorVec ...
3D激光SLAM：LeGO-LOAM---两步优化的帧间里程计及代码分析
3D激光SLAM:LeGO-LOAM---两步优化的帧间里程计及代码分析前言利用地面点优化利用角点优化代码部分 gazebo测试前言 LeGO-LOAM的全称是 Lightweight an ...
vins中imu融合_VINS-Mono代码分析与总结(最终版)
VINS-Mono代码分析总结参考文献前言 ??视觉与IMU融合的分类: 松耦合和紧耦合:按照是否把图像的Feature加入到状态向量区分,换句话说就是松耦合是在视觉和IMU各自求出的位姿的基础上 ...
VINS-Mono代码分析与总结(完整版)
VINS-Mono代码分析总结参考文献 1 VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator, ...
turtlebot3 在gazebo仿真下通过 gmapping slam 建立二维平面地图——全过程
turtlebot3 在gazebo仿真下通过 gmapping slam 建立二维平面地图--全过程 ROS中的地图使用rosbag记录数据 rosbag record rosbag play ...
ORBSLAM2单应矩阵计算及代码分析
单应矩阵代码分析 if(mpInitializer->Initialize(mCurrentFrame, //当前帧mvIniMatches, //当前帧和参考帧的特征点的匹配关系Rcw, tc ...
[SLAM]激光SLAM初学者代码及论文推荐【转】
目录 2D激光SLAM Gmapping Hector_slam Karto Cartographer 3D激光SLAM LOAM A-LOAM LeGO-LOAM Lio-mapping hdl_g ...
lego-loam代码分析（3）-激光里程计
lego-loam代码分析(3)-激光里程计匹配初始化 TransformToStart TransformToEnd 两次LM匹配平面匹配匹配点查找目标点到匹配平面的距离 LM求解角点匹配 ...
vin-slam中调用ceres库内部代码分析与性能优化
vin-slam中调用ceres库内部代码分析与性能优化 1,vin-slam中后端参数优化调用流程代码 2,ceres内部的求解流程(未完待续) 首先,很抱歉前几次上传的关于一些图像算法代码不全,主 ...

Gmapping Slam主要代码分析

目录