参考论文:
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
- 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
- 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判断 ),得到机器人最优轨迹
地图膨胀更新
void
SlamGMapping::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
- 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 的主线已经挑出来了,当然里面还有很多初始化我没讲,而且有很多细节我也还没有弄清楚,
还有待进一步的研究(比如地图是如何膨胀的,扫描匹配得分计算的具体实施过程)。