[SLAM] GMapping SLAM源码阅读（草稿）
目前可以从很多地方得到RBPF的代码，主要看的是Cyrill Stachniss的代码，据此进行理解。
Author：Giorgio Grisetti; Cyrill Stachniss http://openslam.org/
https://github.com/Allopart/rbpf-gmapping 和文献[1]上结合的比较好，方法都可以找到对应的原理。
https://github.com/MRPT/mrpt MRPT中可以采用多种扫描匹配的方式，可以通过配置文件进行配置。
双线程和程序的基本执行流程
GMapping采用GridSlamProcessorThread后台线程进行数据的读取和运算，在gsp_thread.h和相应的实现文件gsp_thread.cpp中可以看到初始化，参数配置，扫描数据读取等方法。
最关键的流程是在后台线程调用的方法void * GridSlamProcessorThread::fastslamthread(GridSlamProcessorThread* gpt)
而这个方法中最主要的处理扫描数据的过程在428行，bool processed=gpt->processScan(*rr);同时可以看到GMapping支持在线和离线两种模式。
注意到struct GridSlamProcessorThread : public GridSlamProcessor ，这个后台线程执行类GridSlamProcessorThread 继承自GridSlamProcessor，所以执行的是GridSlamProcessor的processScan方法。
//后台线程处理扫描数据的方法
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;
} //write the state of the reading and update all the particles using the motion model
for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++)
{
OrientedPoint& pose(it->pose);
pose=m_motionModel.drawFromMotion(it->pose, relPose, m_odoPose);//运动模型，更新t时刻的粒子
} // update the output file
if (m_outputStream.is_open())
{
m_outputStream << setiosflags(ios::fixed) << setprecision();
m_outputStream << "ODOM ";
m_outputStream << setiosflags(ios::fixed) << setprecision() << m_odoPose.x << " " << m_odoPose.y << " ";
m_outputStream << setiosflags(ios::fixed) << setprecision() << m_odoPose.theta << " ";
m_outputStream << reading.getTime();
m_outputStream << endl;
}
if (m_outputStream.is_open())
{
m_outputStream << setiosflags(ios::fixed) << setprecision();
m_outputStream << "ODO_UPDATE "<< m_particles.size() << " ";
for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++)
{
OrientedPoint& pose(it->pose);
m_outputStream << setiosflags(ios::fixed) << setprecision() << pose.x << " " << pose.y << " ";
m_outputStream << setiosflags(ios::fixed) << setprecision() << pose.theta << " " << it-> weight << " ";
}
m_outputStream << reading.getTime();
m_outputStream << endl;
} //invoke the callback
onOdometryUpdate(); // accumulate the robot translation and rotation
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 the robot jumps throw a warning
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
if (! m_count || m_linearDistance>m_linearThresholdDistance || m_angularDistance>m_angularThresholdDistance)
{
if (m_outputStream.is_open())
{
m_outputStream << setiosflags(ios::fixed) << setprecision();
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; 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
assert(reading.size()==m_beams);
double * plainReading = new double[m_beams];
for(unsigned int i=; i<m_beams; i++)
{
plainReading[i]=reading[i];
}
m_infoStream << "m_count " << m_count << endl;
if (m_count>)
{
scanMatch(plainReading);//扫描匹配
if (m_outputStream.is_open())
{
m_outputStream << "LASER_READING "<< reading.size() << " ";
m_outputStream << setiosflags(ios::fixed) << setprecision();
for (RangeReading::const_iterator b=reading.begin(); b!=reading.end(); b++)
{
m_outputStream << *b << " ";
}
OrientedPoint p=reading.getPose();
m_outputStream << setiosflags(ios::fixed) << setprecision();
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() << pose.x << " " << pose.y << " ";
m_outputStream << setiosflags(ios::fixed) << setprecision() << 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();
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, ., it->node, );
node->reading=;
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=;
m_angularDistance=;
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;
}
GridSlamProcessor::processScan
可以依次看到下面介绍的1-7部分的内容。
1.运动模型
t时刻粒子的位姿最初由运动模型进行更新。在初始值的基础上增加高斯采样的noisypoint，参考MotionModel::drawFromMotion()方法。原理参考文献[1]5.4.1节，sample_motion_model_odometry算法。
p是粒子的t-1时刻的位姿，pnew是当前t时刻的里程计读数，pold是t-1时刻的里程计读数。参考博客：小豆包的学习之旅：里程计运动模型
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, *M_PI);
if (noisypoint.theta>M_PI)
noisypoint.theta-=*M_PI;
return absoluteSum(p,noisypoint);
}
2.扫描匹配
扫描匹配获取最优的采样粒子。GMapping默认采用30个采样粒子。
/**Just scan match every single particle.
If the scan matching fails, the particle gets a default likelihood.*/
inline void GridSlamProcessor::scanMatch(const double* plainReading){
// sample a new pose from each scan in the reference
double sumScore=;
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);
// 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;
}
GridSlamProcessor::scanMatch
注意ScanMatcher::score()函数的原理是likehood_field_range_finder_model方法，参考《概率机器人》手稿P143页。
ScanMatcher::optimize()方法获得了一个最优的粒子，基本流程是按照预先设定的步长不断移动粒子的位置，根据提议分布计算s，得到score最小的那个作为粒子的新的位姿。
//此处的方法是likehood_field_range_finder_model方法，参考《概率机器人》手稿P143
inline double ScanMatcher::score(const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const{
double s=;
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=;
double freeDelta=map.getDelta()*m_freeCellRatio;
for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++){
skip++;
skip=skip>m_likelihoodSkip?: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(.,.);
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) )> 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(-./m_gaussianSigma*bestMu*bestMu);//高斯提议分布
}
return s;
}
ScanMatcher::likelihoodAndScore()和ScanMatcher::score()方法基本一致，但是是将新获得的粒子位姿进行计算q，为后续的权重计算做了准备。
ScanMatcher::optimize()方法——粒子的运动+score()中激光扫描观测数据。
其他的扫描匹配方法也是可以使用的：比如ICP算法(rbpf-gmapping的使用的是ICP方法，先更新了旋转角度，然后采用提议分布再次优化粒子)、Cross Correlation、线特征等等。
3.提议分布 （Proposal distribution）
注意是混合了运动模型和观测的提议分布，将扫描观测值整合到了提议分布中[2]（公式9）。因此均值和方差的计算与单纯使用运动模型作为提议分布的有所区别。提议分布的作用是获得一个最优的粒子，同时用来计算权重，这个体现在ScanMatcher::likelihoodAndScore()和ScanMatcher::score()方法中，score方法中采用的是服从0均值的高斯分布近似提议分布（文献[1] III.B节)。关于为什么采用混合的提议分布，L(i)区域小的原理在文献[1]中III.A节有具体介绍。
rbpf-gmapping的使用的是运动模型作为提议分布。
4.权重计算
在重采样之前进行了一次权重计算updateTreeWeights(false);
重采样之后又进行了一次。代码在gridslamprocessor_tree.cpp文件中。
void GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized)
{
if (!weightsAlreadyNormalized)
{
normalize();
}
resetTree();
propagateWeights();
}
5.重采样
原理：粒子集对目标分布的近似越差，则权重的方差越大。
因此用Neff作为权重值离差的量度。
//判断并进行重采样
inline bool GridSlamProcessor::resample(const double* plainReading, int adaptSize, const RangeReading* )
{
bool hasResampled = false;
TNodeVector oldGeneration;
for (unsigned int i=; 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=;
std::vector<unsigned int> deletedParticles; //this is for deleteing the particles which have been resampled away. //cerr << "Existing Nodes:" ;
for (unsigned int i=; 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=;
TNode* oldNode=oldGeneration[m_indexes[i]];
//cerr << i << "->" << m_indexes[i] << "B("<<oldNode->childs <<") ";
node=new TNode(p.pose, , oldNode, );
node->reading=;
//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=; i<deletedParticles.size(); i++)
{
std::cerr <<" " << deletedParticles[i];
delete m_particles[deletedParticles[i]].node;
m_particles[deletedParticles[i]].node=;
}
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();
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=;
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=;
node=new TNode(it->pose, 0.0, *node_it, ); node->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;
}
//END: BUILDING TREE
return hasResampled;
}
GridSlamProcessor::resample
6.占用概率栅格地图
此处的方法感觉有点奇怪，在resample方法中执行ScanMatcher::registerScan()方法，计算占用概率栅格地图。采样两种方式，采用信息熵的方式和文献[1] 9.2节的计算方法不一样。
double ScanMatcher::registerScan(ScanMatcherMap& map, const OrientedPoint& p, const double* readings)
{
if (!m_activeAreaComputed)
computeActiveArea(map, p, readings); //this operation replicates the cells that will be changed in the registration operation
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;
double esum=;
for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++)//每一条扫描线
if (m_generateMap)
{
double d=*r;
if (d>m_laserMaxRange)
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[] ;
GridLineTraversalLine line;
line.points=linePoints;
GridLineTraversal::gridLine(p0, p1, &line);//计算扫描线占据的栅格？
for (int i=; i<line.num_points-; i++)
{
PointAccumulator& cell=map.cell(line.points[i]);
double e=-cell.entropy();
cell.update(false, Point(,));
e+=cell.entropy();
esum+=e;
}
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) 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>= && p1.y>=);
map.cell(p1).update(true,phit);
}
//cout << "informationGain=" << -esum << endl;
return esum;
}
ScanMatcher::registerScan
rbpf-gmapping的使用的是文献[1] 9.2节的计算方法，在Occupancy_Grid_Mapping.m文件中实现，同时调用Inverse_Range_Sensor_Model方法。
gridlineTraversal实现了beam转成栅格的线。对每一束激光束，设start为该激光束起点，end为激光束端点（障碍物位置），使用Bresenham划线算法确定激光束经过的格网。
7.计算有效区域
第一次扫描，count==0时，如果激光观测数据超出了范围，更新栅格地图的范围。同时确定有效区域。
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(,));
Point max(map.map2world(map.getMapSizeX()-,map.getMapSizeY()-));
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) continue;
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(,));
Point lmax(map.map2world(map.getMapSizeX()-,map.getMapSizeY()-));
//cerr << "CURRENT MAP " << lmin.x << " " << lmin.y << " " << lmax.x << " " << lmax.y << endl;
//cerr << "BOUNDARY OVERRIDE " << min.x << " " << min.y << " " << max.x << " " << max.y << endl;
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)
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);
IntPoint linePoints[] ;
GridLineTraversalLine line;
line.points=linePoints;
GridLineTraversal::gridLine(p0, p1, &line);
for (int i=; i<line.num_points-; i++)
{
assert(map.isInside(linePoints[i]));
activeArea.insert(map.storage().patchIndexes(linePoints[i]));
assert(linePoints[i].x>= && linePoints[i].y>=);
}
if (d<m_usableRange){
IntPoint cp=map.storage().patchIndexes(p1);
assert(cp.x>= && cp.y>=);
activeArea.insert(cp);
}
}
else
{
if (*r>m_laserMaxRange||*r>m_usableRange) 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>= && p1.y>=);
IntPoint cp=map.storage().patchIndexes(p1);
assert(cp.x>= && cp.y>=);
activeArea.insert(cp);
}
//this allocates the unallocated cells in the active area of the map
//cout << "activeArea::size() " << activeArea.size() << endl;
/*
cerr << "ActiveArea=";
for (HierarchicalArray2D<PointAccumulator>::PointSet::const_iterator it=activeArea.begin(); it!= activeArea.end(); it++){
cerr << "(" << it->x <<"," << it->y << ") ";
}
cerr << endl;
*/
map.storage().setActiveArea(activeArea, true);
m_activeAreaComputed=true;
}
computeActiveArea
每次扫描匹配获取t时刻的最优粒子后会计算有效区域。
重采样之后，调用ScanMatcher::registerScan() 方法，也会重新计算有效区域。
参考文献：
[1]Sebastian Thrun et al. "Probabilistic Robotics（手稿）."
[2]Grisetti, G. and C. Stachniss "Improved Techniques for Grid Mapping with Rao-Blackwellized Particle Filters."
[SLAM] GMapping SLAM源码阅读（草稿）的更多相关文章
- laravel5.5源码阅读草稿——路由
laravel 里的路由是由RouteServiceProvider提供的,其中的boot方法为启动项,调用了父类的boot方法. RouteServiceProvider中的boot方法设置了自己与 ...
- laravel5.5源码阅读草稿——application
构建方法传入整个项目根目录路径(public文件夹上一级)将其设为基础路径(存在本类basePath属性中). __construct > setBasePath > bindPathsI ...
- laravel5.5源码阅读草稿——入口
laravel的启动需要通过路由.中间件.控制器.模型.视图最后出现在浏览器.而路由.中间件.模型,这些功能都有自己的类,比如Route::any().DB::table().$this->mi ...
- 转-OpenJDK源码阅读导航跟编译
OpenJDK源码阅读导航 OpenJDK源码阅读导航 博客分类: Virtual Machine HotSpot VM Java OpenJDK openjdk 这是链接帖.主体内容都在各链接中. ...
- openjdk源码阅读导航
转自:http://rednaxelafx.iteye.com/blog/1549577 这是链接帖.主体内容都在各链接中. 怕放草稿箱里过会儿又坑掉了,总之先发出来再说…回头再慢慢补充内容. 先把I ...
- 【原】FMDB源码阅读（三）
[原]FMDB源码阅读(三) 本文转载请注明出处 —— polobymulberry-博客园 1. 前言 FMDB比较优秀的地方就在于对多线程的处理.所以这一篇主要是研究FMDB的多线程处理的实现.而 ...
- 【原】FMDB源码阅读（二）
[原]FMDB源码阅读(二) 本文转载请注明出处 -- polobymulberry-博客园 1. 前言 上一篇只是简单地过了一下FMDB一个简单例子的基本流程,并没有涉及到FMDB的所有方方面面,比 ...
- 【原】FMDB源码阅读（一）
[原]FMDB源码阅读(一) 本文转载请注明出处 —— polobymulberry-博客园 1. 前言 说实话,之前的SDWebImage和AFNetworking这两个组件我还是使用过的,但是对于 ...
- 【原】AFNetworking源码阅读（六）
[原]AFNetworking源码阅读(六) 本文转载请注明出处 —— polobymulberry-博客园 1. 前言 这一篇的想讲的,一个就是分析一下AFSecurityPolicy文件,看看AF ...
- 【原】AFNetworking源码阅读（五）
[原]AFNetworking源码阅读(五) 本文转载请注明出处 —— polobymulberry-博客园 1. 前言 上一篇中提及到了Multipart Request的构建方法- [AFHTTP ...
随机推荐
- 阴影：box-shaw
box-shaw:0px 0px 2px 3px while; 水平方向 垂直方向 模糊半径 模糊系数(大小)颜色
- hadoop编程小技巧（5）---自定义输入文件格式类InputFormat
Hadoop代码测试环境:Hadoop2.4 应用:在对数据需要进行一定条件的过滤和简单处理的时候可以使用自定义输入文件格式类. Hadoop内置的输入文件格式类有: 1)FileInputForma ...
- PHP 进行数据庫对比工具
<?php /** * author jackluo * net.webjoy@gmail.com */ class IMysqlDiff { private $master,$slave; p ...
- 终于懂浏览器里面的cookies和session了
在PHP开发中对比起Cookie,session 是存储在服务器端的会话,相对安全,并且不像 Cookie 那样有存储长度限制: (Php.Asp.Jsp)---: cookie(客户端)界面没有刷新 ...
- OpenCV 3.1 VS 2010 Cuda 7.5 TBB Configuration 配置
Download OpenCV 3.1 Download OpenCV Extra Modules Download VS2010 Download CMake 3.2.0 Download Cuda ...
- java web(三) Tomcat虚拟目录映射方式
Tomact服务器虚拟目录的映射方式 web应用开发好后若想被外界访问,需要将web应用所在的目录交给web服务器管理,这个过程称为虚拟目录的映射. 方式一:在server.xml文件的host元素中 ...
- odoo报表条码无法显示解决[转]
当服务器为Linux(Ubuntu)时,ODOO打印的报表上是有条码的,却显示空白框框.问题在于服务器上没有安装条码的字体,reportlab渲染条码图形失败,导致显示不正常. 将附件中的字体下载,解 ...
- Odoo Many2many 指定默认分组过滤
在odoo里如果想单击某个菜单打开的页面是自带过滤的,可以在打开菜单的动作中添加默认过滤来实现,今天有同学在群里问,如何在Many2many的添加更多的弹出窗口中添加类似的过滤,其实是非常非常简单的, ...
- [转载]对于GetBuffer() 与 ReleaseBuffer() 的一些分析
先 转载一段别人的文章 CString类的这几个函数, 一直在用, 但总感觉理解的不够透彻, 不时还有用错的现象. 今天抽时间和Nico一起分析了一下, 算是拨开了云雾: GetBuffer和Rele ...
- PHP后门新玩法：一款猥琐的PHP后门分析
0x00 背景 近日,360网站卫士安全团队近期捕获一个基于PHP实现的webshell样本,其巧妙的代码动态生成方式,猥琐的自身页面伪装手法,让我们在分析这个样本的过程中感受到相当多的乐趣.接下来就 ...