GMapping代码分析的随手笔记记载如下：
参考博客： http://blog.csdn.net/roadseek_zw/article/details/53316177 gmapping分析
都说最好的理解就是说出来让别人听懂，所以我也写下我的分析来加深自己的理解，同时给刚刚入门的同学们带个头。这一篇是我对gmapping源码的分析与理解(刚入门，难免有错，欢迎指正，相互学习)。恩，现在还只是大概的分析，还有很多代码没有懂，由于博主不需要很深入的研究SLAM，所以只是浅浅的谈一下，不喜勿喷。
首先我们从Slam_gamping-hydro-devel包的主函数开始看起：
int
main(int argc, char** argv)
{
ros::init(argc, argv, "slam_gmapping");
SlamGMapping gn;
gn.startLiveSlam(); //这里进入slam_gammping.cpp的startLiveSlam()函数
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);
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)); //从这里进入同一个文件的laserCallback函数
transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_));// line 339
}
SlamGMapping::laserCallback, this, _1)); //从这里进入同一个文件的laserCallback函数
transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_));// line 339
}
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 scan
if(!got_first_scan_)
{
if(!initMapper(*scan)) // 进入initMapper 函数
return;
got_first_scan_ = true;
}
GMapping::OrientedPoint odom_pose;
if(addScan(*scan, odom_pose)) // 进入addScan函数，
{
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");
}
addScan函数：
bool
SlamGMapping::addScan(const sensor_msgs::LaserScan& scan, GMapping::OrientedPoint& gmap_pose)
{
if(!getOdomPose(gmap_pose, scan.header.stamp))
return false;
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.
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];
}
}
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("scanpose (%.3f): %.3f %.3f %.3f\n",
scan.header.stamp.toSec(),
gmap_pose.x,
gmap_pose.y,
gmap_pose.theta);
*/
ROS_DEBUG("processing scan");
return gsp_->processScan(reading); // 从这里进入particle filter
}
processScan 在openslam.org上下载的gmapping源码包里定义：
gridslamprocessor.h 中
namespace GMapping {
bool processScan(const RangeReading & reading, int adaptParticles=0);
｝
之后进入gridslamprocessor.cpp中看一下该方法过程，之后就是一个160多行的代码了。。。在这里暴露了博主c++小白一个，哈哈
bool GridSlamProcessor::processScan(const RangeReading & reading, int adaptParticles){
/**retireve the position from the reading, and compute the odometry*/
OrientedPoint relPose=reading.getPose();
if (!m_count){
m_lastPartPose=m_odoPose=relPose;
}
// ...省略一些行
m_odoPose=relPose;
bool processed=false;
// process a scan only if the robot has traveled a given distance
if (! m_count
|| m_linearDistance>m_linearThresholdDistance
|| m_angularDistance>m_angularThresholdDistance){ // 只有当距离超过阈值时才进行一个scan
// ... 省略
if (m_count>0){
scanMatch(plainReading); // 进入扫描匹配
if (m_outputStream.is_open()){
m_outputStream << "LASER_READING "<< reading.size() << " ";
m_outputStream << setiosflags(ios::fixed) << setprecision(2);
for (RangeReading::const_iterator b=reading.begin(); b!=reading.end(); b++){
m_outputStream << *b << " ";
}
OrientedPoint p=reading.getPose();
m_outputStream << setiosflags(ios::fixed) << setprecision(6);
m_outputStream << p.x << " " << p.y << " " << p.theta << " " << reading.getTime()<< endl;
m_outputStream << "SM_UPDATE "<< m_particles.size() << " ";
for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){
const OrientedPoint& pose=it->pose;
m_outputStream << setiosflags(ios::fixed) << setprecision(3) << pose.x << " " << pose.y << " ";
m_outputStream << setiosflags(ios::fixed) << setprecision(6) << pose.theta << " " << it-> weight << " ";
}
m_outputStream << endl;
}
onScanmatchUpdate();
updateTreeWeights(false);
if (m_infoStream){
m_infoStream << "neff= " << m_neff << endl;
}
if (m_outputStream.is_open()){
m_outputStream << setiosflags(ios::fixed) << setprecision(6);
m_outputStream << "NEFF " << m_neff << endl;
}
resample(plainReading, adaptParticles);
} else {
m_infoStream << "Registering First Scan"<< endl;
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);
// cyr: not needed anymore, particles refer to the root in the beginning!
TNode* node=new TNode(it->pose, 0., it->node, 0);
node->reading=0;
it->node=node;
}
}
// cerr << "Tree: normalizing, resetting and propagating weights at the end..." ;
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;
}
}
if (m_outputStream.is_open())
m_outputStream << flush;
m_readingCount++;
return processed;
}
今天先写到这里，之后再改，目前代码看到这里
上图是processScan 的函数调用
scanMatch 函数体：
inline void GridSlamProcessor::scanMatch(const double* plainReading){
// sample a new pose from each scan in the reference
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); // optimize 将调用 score();
// it->pose=corrected;
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;
//set up the selective copy of the active area
//by detaching the areas that will be updated
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;
}
恩，现在看不懂数学过程，具体的还得仔细看书啊。
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;
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;
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 (*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-map.getDelta()*freeDelta)*cos(lp.theta+*angle);
pfree.y+=(*r-map.getDelta()*freeDelta)*sin(lp.theta+*angle);
pfree=pfree-phit;
IntPoint ipfree=map.world2map(pfree);
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;
//AccessibilityState s=map.storage().cellState(pr);
//if (s&Inside && s&Allocated){
const PointAccumulator& cell=map.cell(pr);
const PointAccumulator& fcell=map.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);
}
return s;
}
之后 权重计算 updateTreeWeights(false);
void GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized){
if (!weightsAlreadyNormalized) {
normalize();
}
resetTree();
propagateWeights();
}
resample();
inline bool GridSlamProcessor::resample(const double* plainReading, int adaptSize, const RangeReading* ){
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.
// cerr << "Existing Nodes:" ;
for (unsigned int i=0; i<m_indexes.size(); i++){
// cerr << " " << m_indexes[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]];
// cerr << i << "->" << m_indexes[i] << "B("<<oldNode->childs <<") ";
node=new TNode(p.pose, 0, oldNode, 0);
node->reading=0;
// cerr << "A("<<node->parent->childs <<") " <<endl;
temp.push_back(p);
temp.back().node=node;
temp.back().previousIndex=m_indexes[i];
}
while(j<m_indexes.size()){
deletedParticles.push_back(j);
j++;
}
// cerr << endl;
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;
//END: BUILDING TREE
std::cerr << "Deleting old particles..." ;
m_particles.clear();
std::cerr << "Done" << std::endl;
std::cerr << "Copying Particles and Registering scans...";
for (ParticleVector::iterator it=temp.begin(); it!=temp.end(); it++){
it->setWeight(0);
m_matcher.invalidateActiveArea();
m_matcher.registerScan(it->map, it->pose, plainReading);
m_particles.push_back(*it);
}
std::cerr << " Done" <<std::endl;
hasResampled = true;
} else {
int index=0;
std::cerr << "Registering Scans:";
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;
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;
}
