[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方法。
1 //后台线程处理扫描数据的方法 2 bool GridSlamProcessor::processScan(const RangeReading & reading, int adaptParticles) 3 { 4 /**retireve the position from the reading, and compute the odometry*/ 5 OrientedPoint relPose=reading.getPose(); 6 if (!m_count) 7 { 8 m_lastPartPose=m_odoPose=relPose; 9 } 10 11 //write the state of the reading and update all the particles using the motion model 12 for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++) 13 { 14 OrientedPoint& pose(it->pose); 15 pose=m_motionModel.drawFromMotion(it->pose, relPose, m_odoPose);//运动模型，更新t时刻的粒子 16 } 17 18 // update the output file 19 if (m_outputStream.is_open()) 20 { 21 m_outputStream << setiosflags(ios::fixed) << setprecision(6); 22 m_outputStream << "ODOM "; 23 m_outputStream << setiosflags(ios::fixed) << setprecision(3) << m_odoPose.x << " " << m_odoPose.y << " "; 24 m_outputStream << setiosflags(ios::fixed) << setprecision(6) << m_odoPose.theta << " "; 25 m_outputStream << reading.getTime(); 26 m_outputStream << endl; 27 } 28 if (m_outputStream.is_open()) 29 { 30 m_outputStream << setiosflags(ios::fixed) << setprecision(6); 31 m_outputStream << "ODO_UPDATE "<< m_particles.size() << " "; 32 for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++) 33 { 34 OrientedPoint& pose(it->pose); 35 m_outputStream << setiosflags(ios::fixed) << setprecision(3) << pose.x << " " << pose.y << " "; 36 m_outputStream << setiosflags(ios::fixed) << setprecision(6) << pose.theta << " " << it-> weight << " "; 37 } 38 m_outputStream << reading.getTime(); 39 m_outputStream << endl; 40 } 41 42 //invoke the callback 43 onOdometryUpdate(); 44 45 // accumulate the robot translation and rotation 46 OrientedPoint move=relPose-m_odoPose; 47 move.theta=atan2(sin(move.theta), cos(move.theta)); 48 m_linearDistance+=sqrt(move*move); 49 m_angularDistance+=fabs(move.theta); 50 51 // if the robot jumps throw a warning 52 if (m_linearDistance>m_distanceThresholdCheck) 53 { 54 cerr << "***********************************************************************" << endl; 55 cerr << "********** Error: m_distanceThresholdCheck overridden!!!! *************" << endl; 56 cerr << "m_distanceThresholdCheck=" << m_distanceThresholdCheck << endl; 57 cerr << "Old Odometry Pose= " << m_odoPose.x << " " << m_odoPose.y 58 << " " <<m_odoPose.theta << endl; 59 cerr << "New Odometry Pose (reported from observation)= " << relPose.x << " " << relPose.y 60 << " " <<relPose.theta << endl; 61 cerr << "***********************************************************************" << endl; 62 cerr << "** The Odometry has a big jump here. This is probably a bug in the **" << endl; 63 cerr << "** odometry/laser input. We continue now, but the result is probably **" << endl; 64 cerr << "** crap or can lead to a core dump since the map doesn't fit.... C&G **" << endl; 65 cerr << "***********************************************************************" << endl; 66 } 67 68 m_odoPose=relPose; 69 70 bool processed=false; 71 72 // process a scan only if the robot has traveled a given distance 73 if (! m_count || m_linearDistance>m_linearThresholdDistance || m_angularDistance>m_angularThresholdDistance) 74 { 75 if (m_outputStream.is_open()) 76 { 77 m_outputStream << setiosflags(ios::fixed) << setprecision(6); 78 m_outputStream << "FRAME " << m_readingCount; 79 m_outputStream << " " << m_linearDistance; 80 m_outputStream << " " << m_angularDistance << endl; 81 } 82 83 if (m_infoStream) 84 m_infoStream << "update frame " << m_readingCount << endl 85 << "update ld=" << m_linearDistance << " ad=" << m_angularDistance << endl; 86 87 88 cerr << "Laser Pose= " << reading.getPose().x << " " << reading.getPose().y 89 << " " << reading.getPose().theta << endl; 90 91 92 //this is for converting the reading in a scan-matcher feedable form 93 assert(reading.size()==m_beams); 94 double * plainReading = new double[m_beams]; 95 for(unsigned int i=0; i<m_beams; i++) 96 { 97 plainReading[i]=reading[i]; 98 } 99 m_infoStream << "m_count " << m_count << endl; 100 if (m_count>0) 101 { 102 scanMatch(plainReading);//扫描匹配 103 if (m_outputStream.is_open()) 104 { 105 m_outputStream << "LASER_READING "<< reading.size() << " "; 106 m_outputStream << setiosflags(ios::fixed) << setprecision(2); 107 for (RangeReading::const_iterator b=reading.begin(); b!=reading.end(); b++) 108 { 109 m_outputStream << *b << " "; 110 } 111 OrientedPoint p=reading.getPose(); 112 m_outputStream << setiosflags(ios::fixed) << setprecision(6); 113 m_outputStream << p.x << " " << p.y << " " << p.theta << " " << reading.getTime()<< endl; 114 m_outputStream << "SM_UPDATE "<< m_particles.size() << " "; 115 for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++) 116 { 117 const OrientedPoint& pose=it->pose; 118 m_outputStream << setiosflags(ios::fixed) << setprecision(3) << pose.x << " " << pose.y << " "; 119 m_outputStream << setiosflags(ios::fixed) << setprecision(6) << pose.theta << " " << it-> weight << " "; 120 } 121 m_outputStream << endl; 122 } 123 onScanmatchUpdate(); 124 updateTreeWeights(false);//更新权重 125 126 if (m_infoStream) 127 { 128 m_infoStream << "neff= " << m_neff << endl; 129 } 130 if (m_outputStream.is_open()) 131 { 132 m_outputStream << setiosflags(ios::fixed) << setprecision(6); 133 m_outputStream << "NEFF " << m_neff << endl; 134 } 135 resample(plainReading, adaptParticles);//重采样 136 } 137 else 138 { 139 m_infoStream << "Registering First Scan"<< endl; 140 for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++) 141 { 142 m_matcher.invalidateActiveArea(); 143 m_matcher.computeActiveArea(it->map, it->pose, plainReading);//计算有效区域 144 m_matcher.registerScan(it->map, it->pose, plainReading); 145 146 // cyr: not needed anymore, particles refer to the root in the beginning! 147 TNode* node=new TNode(it->pose, 0., it->node, 0); 148 node->reading=0; 149 it->node=node; 150 151 } 152 } 153 //cerr << "Tree: normalizing, resetting and propagating weights at the end..." ; 154 updateTreeWeights(false);//再次更新权重 155 //cerr << ".done!" <<endl; 156 157 delete [] plainReading; 158 m_lastPartPose=m_odoPose; //update the past pose for the next iteration 159 m_linearDistance=0; 160 m_angularDistance=0; 161 m_count++; 162 processed=true; 163 164 //keep ready for the next step 165 for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++) 166 { 167 it->previousPose=it->pose; 168 } 169 170 } 171 if (m_outputStream.is_open()) 172 m_outputStream << flush; 173 m_readingCount++; 174 return processed; 175 }
可以依次看到下面介绍的1-7部分的内容。
1.运动模型
t时刻粒子的位姿最初由运动模型进行更新。在初始值的基础上增加高斯采样的noisypoint，参考MotionModel::drawFromMotion()方法。原理参考文献[1]5.4.1节，sample_motion_model_odometry算法。
p是粒子的t-1时刻的位姿，pnew是当前t时刻的里程计读数，pold是t-1时刻的里程计读数。参考博客：小豆包的学习之旅：里程计运动模型
1 OrientedPoint MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const{
2 double sxy=0.3*srr;
3 OrientedPoint delta=absoluteDifference(pnew, pold);
4 OrientedPoint noisypoint(delta);
5 noisypoint.x+=sampleGaussian(srr*fabs(delta.x)+str*fabs(delta.theta)+sxy*fabs(delta.y));
6 noisypoint.y+=sampleGaussian(srr*fabs(delta.y)+str*fabs(delta.theta)+sxy*fabs(delta.x));
7 noisypoint.theta+=sampleGaussian(stt*fabs(delta.theta)+srt*sqrt(delta.x*delta.x+delta.y*delta.y));
8 noisypoint.theta=fmod(noisypoint.theta, 2*M_PI);
9 if (noisypoint.theta>M_PI)
10 noisypoint.theta-=2*M_PI;
11 return absoluteSum(p,noisypoint);
12 }
2.扫描匹配
扫描匹配获取最优的采样粒子。GMapping默认采用30个采样粒子。
1 /**Just scan match every single particle.
2 If the scan matching fails, the particle gets a default likelihood.*/
3 inline void GridSlamProcessor::scanMatch(const double* plainReading){
4 // sample a new pose from each scan in the reference
5 double sumScore=0;
6 for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
7 OrientedPoint corrected;
8 double score, l, s;
9 score=m_matcher.optimize(corrected, it->map, it->pose, plainReading);
10 // it->pose=corrected;
11 if (score>m_minimumScore){
12 it->pose=corrected;
13 } else {
14 if (m_infoStream){
15 m_infoStream << "Scan Matching Failed, using odometry. Likelihood=" << l <<std::endl;
16 m_infoStream << "lp:" << m_lastPartPose.x << " " << m_lastPartPose.y << " "<< m_lastPartPose.theta <<std::endl;
17 m_infoStream << "op:" << m_odoPose.x << " " << m_odoPose.y << " "<< m_odoPose.theta <<std::endl;
18 }
19 }
20
21 m_matcher.likelihoodAndScore(s, l, it->map, it->pose, plainReading);
22 sumScore+=score;
23 it->weight+=l;
24 it->weightSum+=l;
25
26 //set up the selective copy of the active area
27 //by detaching the areas that will be updated
28 m_matcher.invalidateActiveArea();
29 m_matcher.computeActiveArea(it->map, it->pose, plainReading);
30 }
31 if (m_infoStream)
32 m_infoStream << "Average Scan Matching Score=" << sumScore/m_particles.size() << std::endl;
33 }
注意ScanMatcher::score()函数的原理是likehood_field_range_finder_model方法，参考《概率机器人》手稿P143页。
ScanMatcher::optimize()方法获得了一个最优的粒子，基本流程是按照预先设定的步长不断移动粒子的位置，根据提议分布计算s，得到score最小的那个作为粒子的新的位姿。
1 //此处的方法是likehood_field_range_finder_model方法，参考《概率机器人》手稿P143
2 inline double ScanMatcher::score(const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const{
3 double s=0;
4 const double * angle=m_laserAngles+m_initialBeamsSkip;
5 OrientedPoint lp=p;
6 lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
7 lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
8 lp.theta+=m_laserPose.theta;
9 unsigned int skip=0;
10 double freeDelta=map.getDelta()*m_freeCellRatio;
11 for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++){
12 skip++;
13 skip=skip>m_likelihoodSkip?0:skip;
14 if (*r>m_usableRange) continue;
15 if (skip) continue;
16 Point phit=lp;
17 phit.x+=*r*cos(lp.theta+*angle);
18 phit.y+=*r*sin(lp.theta+*angle);
19 IntPoint iphit=map.world2map(phit);
20 Point pfree=lp;
21 pfree.x+=(*r-map.getDelta()*freeDelta)*cos(lp.theta+*angle);
22 pfree.y+=(*r-map.getDelta()*freeDelta)*sin(lp.theta+*angle);
23 pfree=pfree-phit;
24 IntPoint ipfree=map.world2map(pfree);
25 bool found=false;
26 Point bestMu(0.,0.);
27 for (int xx=-m_kernelSize; xx<=m_kernelSize; xx++)
28 for (int yy=-m_kernelSize; yy<=m_kernelSize; yy++){
29 IntPoint pr=iphit+IntPoint(xx,yy);
30 IntPoint pf=pr+ipfree;
31 //AccessibilityState s=map.storage().cellState(pr);
32 //if (s&Inside && s&Allocated){
33 const PointAccumulator& cell=map.cell(pr);
34 const PointAccumulator& fcell=map.cell(pf);
35 if (((double) )> m_fullnessThreshold && ((double)fcell )<m_fullnessThreshold){
36 Point mu=phit-cell.mean();
37 if (!found){
38 bestMu=mu;
39 found=true;
40 }else
41 bestMu=(mu*mu)<(bestMu*bestMu)?mu:bestMu;
42 }
43 //}
44 }
45 if (found)
46 s+=exp(-1./m_gaussianSigma*bestMu*bestMu);//高斯提议分布
47 }
48 return s;
49 }
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文件中。
1 void GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized)
2 {
3 if (!weightsAlreadyNormalized)
4 {
5 normalize();
6 }
7 resetTree();
8 propagateWeights();
9 }
5.重采样
原理：粒子集对目标分布的近似越差，则权重的方差越大。
因此用Neff作为权重值离差的量度。
1 //判断并进行重采样
2 inline bool GridSlamProcessor::resample(const double* plainReading, int adaptSize, const RangeReading* )
3 {
4 bool hasResampled = false;
5 TNodeVector oldGeneration;
6 for (unsigned int i=0; i<m_particles.size(); i++)
7 {
8 oldGeneration.push_back(m_particles[i].node);
9 }
10 if (m_neff<m_resampleThreshold*m_particles.size())
11 {
12 if (m_infoStream)
13 m_infoStream << "*************RESAMPLE***************" << std::endl;
14
15 uniform_resampler<double, double> resampler;
16 m_indexes=resampler.resampleIndexes(m_weights, adaptSize);//执行重采样
17
18 if (m_outputStream.is_open())
19 {
20 m_outputStream << "RESAMPLE "<< m_indexes.size() << " ";
21 for (std::vector<unsigned int>::const_iterator it=m_indexes.begin(); it!=m_indexes.end(); it++)
22 {
23 m_outputStream << *it << " ";
24 }
25 m_outputStream << std::endl;
26 }
27 onResampleUpdate();
28 //BEGIN: BUILDING TREE
29 ParticleVector temp;//重采样的粒子集合临时变量
30 unsigned int j=0;
31 std::vector<unsigned int> deletedParticles; //this is for deleteing the particles which have been resampled away.
32
33 //cerr << "Existing Nodes:" ;
34 for (unsigned int i=0; i<m_indexes.size(); i++)
35 {
36 //cerr << " " << m_indexes[i];
37 while(j<m_indexes[i])
38 {
39 deletedParticles.push_back(j);
40 j++;
41 }
42 if (j==m_indexes[i])
43 j++;
44 Particle & p=m_particles[m_indexes[i]];
45 TNode* node=0;
46 TNode* oldNode=oldGeneration[m_indexes[i]];
47 //cerr << i << "->" << m_indexes[i] << "B("<<oldNode->childs <<") ";
48 node=new TNode(p.pose, 0, oldNode, 0);
49 node->reading=0;
50 //cerr << "A("<<node->parent->childs <<") " <<endl;
51
52 temp.push_back(p);
53 temp.back().node=node;
54 temp.back().previousIndex=m_indexes[i];
55 }
56 while(j<m_indexes.size())
57 {
58 deletedParticles.push_back(j);
59 j++;
60 }
61 //cerr << endl;
62 std::cerr << "Deleting Nodes:";
63 //删除粒子
64 for (unsigned int i=0; i<deletedParticles.size(); i++)
65 {
66 std::cerr <<" " << deletedParticles[i];
67 delete m_particles[deletedParticles[i]].node;
68 m_particles[deletedParticles[i]].node=0;
69 }
70 std::cerr << " Done" <<std::endl;
71
72 //END: BUILDING TREE
73 std::cerr << "Deleting old particles..." ;
74 m_particles.clear();
75 std::cerr << "Done" << std::endl;
76 std::cerr << "Copying Particles and Registering scans...";
77 for (ParticleVector::iterator it=temp.begin(); it!=temp.end(); it++)
78 {
79 it->setWeight(0);
80 m_matcher.invalidateActiveArea();
81 m_matcher.registerScan(it->map, it->pose, plainReading);
82 m_particles.push_back(*it);
83 }
84 std::cerr << " Done" <<std::endl;
85 hasResampled = true;
86 }
87 else
88 {
89 int index=0;
90 std::cerr << "Registering Scans:";
91 TNodeVector::iterator node_it=oldGeneration.begin();
92 for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++)
93 {
94 //create a new node in the particle tree and add it to the old tree
95 //BEGIN: BUILDING TREE
96 TNode* node=0;
97 node=new TNode(it->pose, 0.0, *node_it, 0);
98
99 node->reading=0;
100 it->node=node;
101
102 //END: BUILDING TREE
103 m_matcher.invalidateActiveArea();
104 m_matcher.registerScan(it->map, it->pose, plainReading);//注册到全局地图中
105 it->previousIndex=index;
106 index++;
107 node_it++;
108 }
109 std::cerr << "Done" <<std::endl;
110 }
111 //END: BUILDING TREE
112 return hasResampled;
113 }
6.占用概率栅格地图
此处的方法感觉有点奇怪，在resample方法中执行ScanMatcher::registerScan()方法，计算占用概率栅格地图。采样两种方式，采用信息熵的方式和文献[1] 9.2节的计算方法不一样。
1 double ScanMatcher::registerScan(ScanMatcherMap& map, const OrientedPoint& p, const double* readings)
2 {
3 if (!m_activeAreaComputed)
4 computeActiveArea(map, p, readings);
5
6 //this operation replicates the cells that will be changed in the registration operation
7 map.storage().allocActiveArea();
8
9 OrientedPoint lp=p;
10 lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
11 lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
12 lp.theta+=m_laserPose.theta;//激光器中心点
13 IntPoint p0=map.world2map(lp);//转到地图坐标？
14
15
16 const double * angle=m_laserAngles+m_initialBeamsSkip;
17 double esum=0;
18 for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++)//每一条扫描线
19 if (m_generateMap)
20 {
21 double d=*r;
22 if (d>m_laserMaxRange)
23 continue;
24 if (d>m_usableRange)
25 d=m_usableRange;
26 Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));//扫描到的点
27 IntPoint p1=map.world2map(phit);//转到地图坐标？
28 IntPoint linePoints[20000] ;
29 GridLineTraversalLine line;
30 line.points=linePoints;
31 GridLineTraversal::gridLine(p0, p1, &line);//计算扫描线占据的栅格？
32 for (int i=0; i<line.num_points-1; i++)
33 {
34 PointAccumulator& cell=map.cell(line.points[i]);
35 double e=-cell.entropy();
36 cell.update(false, Point(0,0));
37 e+=cell.entropy();
38 esum+=e;
39 }
40 if (d<m_usableRange)
41 {
42 double e=-map.cell(p1).entropy();
43 map.cell(p1).update(true, phit);
44 e+=map.cell(p1).entropy();
45 esum+=e;
46 }
47 }
48 else
49 {
50 if (*r>m_laserMaxRange||*r>m_usableRange) continue;
51 Point phit=lp;
52 phit.x+=*r*cos(lp.theta+*angle);
53 phit.y+=*r*sin(lp.theta+*angle);
54 IntPoint p1=map.world2map(phit);
55 assert(p1.x>=0 && p1.y>=0);
56 map.cell(p1).update(true,phit);
57 }
58 //cout << "informationGain=" << -esum << endl;
59 return esum;
60 }
rbpf-gmapping的使用的是文献[1] 9.2节的计算方法，在Occupancy_Grid_Mapping.m文件中实现，同时调用Inverse_Range_Sensor_Model方法。
gridlineTraversal实现了beam转成栅格的线。对每一束激光束，设start为该激光束起点，end为激光束端点（障碍物位置），使用Bresenham划线算法确定激光束经过的格网。
7.计算有效区域
第一次扫描，count==0时，如果激光观测数据超出了范围，更新栅格地图的范围。同时确定有效区域。
1 void ScanMatcher::computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p, const double* readings){
2 if (m_activeAreaComputed)
3 return;
4 OrientedPoint lp=p;
5 lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
6 lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
7 lp.theta+=m_laserPose.theta;
8 IntPoint p0=map.world2map(lp);
9
10 Point min(map.map2world(0,0));
11 Point max(map.map2world(map.getMapSizeX()-1,map.getMapSizeY()-1));
12
13 if (lp.x<min.x) min.x=lp.x;
14 if (lp.y<min.y) min.y=lp.y;
15 if (lp.x>max.x) max.x=lp.x;
16 if (lp.y>max.y) max.y=lp.y;
17
18 /*determine the size of the area*/
19 const double * angle=m_laserAngles+m_initialBeamsSkip;
20 for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++){
21 if (*r>m_laserMaxRange) continue;
22 double d=*r>m_usableRange?m_usableRange:*r;
23 Point phit=lp;
24 phit.x+=d*cos(lp.theta+*angle);
25 phit.y+=d*sin(lp.theta+*angle);
26 if (phit.x<min.x) min.x=phit.x;
27 if (phit.y<min.y) min.y=phit.y;
28 if (phit.x>max.x) max.x=phit.x;
29 if (phit.y>max.y) max.y=phit.y;
30 }
31 //min=min-Point(map.getDelta(),map.getDelta());
32 //max=max+Point(map.getDelta(),map.getDelta());
33
34 if ( !map.isInside(min) || !map.isInside(max)){
35 Point lmin(map.map2world(0,0));
36 Point lmax(map.map2world(map.getMapSizeX()-1,map.getMapSizeY()-1));
37 //cerr << "CURRENT MAP " << lmin.x << " " << lmin.y << " " << lmax.x << " " << lmax.y << endl;
38 //cerr << "BOUNDARY OVERRIDE " << min.x << " " << min.y << " " << max.x << " " << max.y << endl;
39 min.x=( min.x >= lmin.x )? lmin.x: min.x-m_enlargeStep;
40 max.x=( max.x <= lmax.x )? lmax.x: max.x+m_enlargeStep;
41 min.y=( min.y >= lmin.y )? lmin.y: min.y-m_enlargeStep;
42 max.y=( max.y <= lmax.y )? lmax.y: max.y+m_enlargeStep;
43 map.resize(min.x, min.y, max.x, max.y);
44 //cerr << "RESIZE " << min.x << " " << min.y << " " << max.x << " " << max.y << endl;
45 }
46
47 HierarchicalArray2D<PointAccumulator>::PointSet activeArea;
48 /*allocate the active area*/
49 angle=m_laserAngles+m_initialBeamsSkip;
50 for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++)
51 if (m_generateMap)
52 {
53 double d=*r;
54 if (d>m_laserMaxRange)
55 continue;
56 if (d>m_usableRange)
57 d=m_usableRange;
58 Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
59 IntPoint p0=map.world2map(lp);
60 IntPoint p1=map.world2map(phit);
61
62 IntPoint linePoints[20000] ;
63 GridLineTraversalLine line;
64 line.points=linePoints;
65 GridLineTraversal::gridLine(p0, p1, &line);
66 for (int i=0; i<line.num_points-1; i++)
67 {
68 assert(map.isInside(linePoints[i]));
69 activeArea.insert(map.storage().patchIndexes(linePoints[i]));
70 assert(linePoints[i].x>=0 && linePoints[i].y>=0);
71 }
72 if (d<m_usableRange){
73 IntPoint cp=map.storage().patchIndexes(p1);
74 assert(cp.x>=0 && cp.y>=0);
75 activeArea.insert(cp);
76 }
77 }
78 else
79 {
80 if (*r>m_laserMaxRange||*r>m_usableRange) continue;
81 Point phit=lp;
82 phit.x+=*r*cos(lp.theta+*angle);
83 phit.y+=*r*sin(lp.theta+*angle);
84 IntPoint p1=map.world2map(phit);
85 assert(p1.x>=0 && p1.y>=0);
86 IntPoint cp=map.storage().patchIndexes(p1);
87 assert(cp.x>=0 && cp.y>=0);
88 activeArea.insert(cp);
89 }
90
91 //this allocates the unallocated cells in the active area of the map
92 //cout << "activeArea::size() " << activeArea.size() << endl;
93 /*
94 cerr << "ActiveArea=";
95 for (HierarchicalArray2D<PointAccumulator>::PointSet::const_iterator it=activeArea.begin(); it!= activeArea.end(); it++){
96 cerr << "(" << it->x <<"," << it->y << ") ";
97 }
98 cerr << endl;
99 */
100 map.storage().setActiveArea(activeArea, true);
101 m_activeAreaComputed=true;
102 }
每次扫描匹配获取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."
作者：太一吾鱼水
文章未经说明均属原创，学习笔记可能有大段的引用，一般会注明参考文献。
欢迎大家留言交流，转载请注明出处。
