这一篇先讲我对gmapping源码的理解(难免有错，欢迎指正，相互学习)。
参考论文:
Improved Techniques for Grid Mapping with Rao-Blackwellized Particle Filters
参考博客：
http://blog.csdn.net/heyijia0327/article/details/40899819 pf原理讲解
http://blog.csdn.net/u010545732/article/details/17462941 pf代码实现
http://www.cnblogs.com/yhlx125/p/5634128.html gmapping分析
http://wenku.baidu.com/view/3a67461550e2524de4187e4d.html?from=search gmapping 分析
其他参考 ：
http://ishare.iask.sina.com.cn/f/24615049.html
从ros官网上下载 slam_gmapping 包以及在openslam （ http://openslam.org/ ）上下载openslam_gmapping包。
为了方便的阅读源码，这里强力推荐一款源码阅读软件 understand （聪明的你一定找的到资源），可以方便实现各种跳转与生成图、表、树，流程等。
废话不多说了，开始看源码，对于我这种c++都没有过关的菜鸟，看着大几千行的c++的代码，简直是身体和精神上的蹂躏。
先说说 slam_gmapping 包与openslam_gmapping包
进入slam_gmapping 的main.cpp文件的关系，slam_gmapping 是openslam_gampping在ros下的二次封装，你可以直接用这个包，而真正的核心代码实现都在openslam_gampping里面。
进入代码
先用understand 看看代码的调用关系。 （调用太复杂了，图太大，我就截取了一小部分）
转到
gn.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); scan_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1)); transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_));}
也没写啥，主要就是一些消息的回调以及发布一些服务，重点在
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) if(!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") } } else ROS_DEBUG("cannot process scan")}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
在 processScan 函数里依次执行了
运动模型
更新t时刻的粒子群，（模型中加入了高斯噪声）
你要问我是为啥是这样的公式，请自行参考《Probabilistic Robot 》一书的P107页 ，里程计运动模型
relPose 当前时刻的位姿（x,y，theta） ,m_odoPose 上一时刻的位姿
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) if (noisypoint.theta>M_PI) noisypoint.theta-=2*M_PI return absoluteSum(p,noisypoint)}
计算t-1 —> t 时刻的 位移增量，以及角位移增量
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 (! m_count ||m_linearDistance>=m_linearThresholdDistance || m_angularDistance>=m_angularThresholdDistance || (period_ >= 0.0 && (reading.getTime() - last_update_time_) > period_)) { //... }
if 里面有几个重要的函数，如下
扫描匹配
通过匹配选取最优的粒子，如果匹配失败，则返回一个默认的似然估计
原理就参考 《Probabilistic Robot》 一书的P143 页 ， likelihood_field_range_finder_mode
inline void GridSlamProcessor::scanMatch(const double* plainReading){ 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); 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; } } m_matcher.likelihoodAndScore(s, l, it->map, it->pose, plainReading); sumScore+=score; it->weight+=l; it->weightSum+=l; 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; }
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
权重更新
重采样前更新过一次，重采样后又更新过一次，更新原理，参见论文（表示不是太懂）
void GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized){ if (!weightsAlreadyNormalized) { normalize(); } resetTree(); propagateWeights(); }
重采样
粒子集对目标分布的近似越差，则权重的方差越大，可用Neff来度量，具体原理参见论文，以及白巧克力亦唯心的那篇博客
代码太长了就不粘贴了
重采样里还调用了registerScan ，这个函数和computeActive 函数有点像，不同的是，registerScan用于注册每个单元格
的状态，自由、障碍，调用update()以及entroy()函数更新，最后是障碍物的概率 p=n/visits ,
障碍物的坐标用平均值来算完了后，又有一次权重计算。
至此，processScan 结束，回到laserCallback，还有最优一步updateMap
resample(plainReading, adaptParticles, reading_copy);
地图更新
先得到最优的粒子（用权重和 weightSum判断 ），得到机器人最优轨迹
地图膨胀更新
voidSlamGMapping::updateMap(const sensor_msgs::LaserScan& scan){ ROS_DEBUG("Update map") boost::mutex::scoped_lock map_lock (map_mutex_) GMapping::ScanMatcher matcher 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 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])) } if(map_.map.info.width != (unsigned int) smap.getMapSizeX() || map_.map.info.height != (unsigned int) smap.getMapSizeY()) { GMapping::Point wmin = smap.map2world(GMapping::IntPoint(0, 0)) GMapping::Point wmax = smap.map2world(GMapping::IntPoint(smap.getMapSizeX(), smap.getMapSizeY())) xmin_ = wmin.x xmax_ = wmax.x 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) } for(int x=0 { for(int y=0 { /// @todo Sort out the unknown vs. free vs. obstacle thresholding GMapping::IntPoint p(x, y) double occ=smap.cell(p) assert(occ <= 1.0) if(occ < 0) map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = -1 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)] = 100 } else map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 0 } } 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)}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
- 72
- 73
- 74
- 75
- 76
- 77
- 78
- 79
- 80
- 81
- 82
- 83
- 84
- 85
- 86
- 87
- 88
- 89
- 90
- 91
- 92
- 93
- 94
- 95
- 96
- 97
- 98
至此，整个gmapping 的主线已经挑出来了，当然里面还有很多初始化我没讲，而且有很多细节我也还没有弄清楚，
还有待进一步的研究（比如地图是如何膨胀的，扫描匹配得分计算的具体实施过程）。
