inlinevoidGridSlamProcessor::scanMatch(constdouble* plainReading) { // sample a new pose from each scan in the reference /* 每个粒子都要进行 scan-match */ double sumScore=0; int particle_number = m_particles.size(); //可以用openMP的方式来进行并行化，因此这里不能用迭代器，只能用下标的方式 //并行化之后，里面的循环会均匀分到各个不同的核里面 for (int i = 0; i < particle_number; i++) { OrientedPoint corrected; double score, l, s; /*进行scan-match 计算粒子的最优位姿，这是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; } } // 粒子的最优位姿计算了之后，重新计算粒子的权重,相当于粒子滤波器中的观测步骤 /* 计算p(z|x,m)，粒子的权重由粒子的似然来表示, 计算粒子的得分和权重(似然) 注意粒子的权重经过ScanMatch之后已经更新了 * 在论文中 粒子的权重不是用最优位姿的似然值来表示的 * 是用所有的似然值的和来表示的, s是得分 l是似然,也就是权重 */ 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 of all particles=" << sumScore/m_particles.size(); }
对于栅格地图的环境地图模型，一般采用基于似然场模型（likelihood field range finder mode）的扫描匹配算法来进行激光雷达数据与地图的匹配。 该算法通过评估当前时刻位姿的激光雷达数据中每一个激光点与该时刻栅格地图的匹配程度，并将评估得到的每个激光点得分进行累加，得到当前时刻激光雷达数据与地图匹配的总分数，得分越高说明该时刻激光雷达信息与地图匹配程度越高，即机器人位姿越精确。由于该算法能够根据机器人当前时刻状态、 观测值和地图信息直接得到该时刻位姿的匹配程度，因而该算法常与基于粒子滤波的定位算法结合使用，用于选取各粒子中得分最高的位姿，并计算得分， 从而完成机器人位姿的确定，其原理如图：
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);
// the map may have expanded, so resize ros message as well // 扩充地图的大小 if(map_.map.info.width != (unsignedint) smap.getMapSizeX() || map_.map.info.height != (unsignedint) 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_);
//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_ );
boolGridSlamProcessor::processScan(const RangeReading & reading, int adaptParticles) { /* retireve the position from the reading, and compute the odometry */ /*当前雷达在里程计坐标系的位置 addScan最后的setPose */ OrientedPoint relPose = reading.getPose();
/*m_count表示这个函数被调用的次数，开始为0,如果是第0次调用,则所有的位姿都是一样的*/ if (!m_count) m_lastPartPose = m_odoPose = relPose; /*对于每一个粒子，都从里程计运动模型中采样，得到车子的初步估计位置 这一步对应里程计的更新 */ int tmp_size = m_particles.size();
bool SlamGMapping::getOdomPose(GMapping::OrientedPoint& gmap_pose, const ros::Time& t) { // Get the pose of the centered laser at the right time centered_laser_pose_.stamp_ = t;
