[SLAM] GMapping SLAM源碼閱讀（草稿）
目前能夠從不少地方獲得RBPF的代碼，主要看的是Cyrill Stachniss的代碼，據此進行理解。html
Author：Giorgio Grisetti; Cyrill Stachniss http://openslam.org/ node
https://github.com/Allopart/rbpf-gmapping 和文獻[1]上結合的比較好，方法均可以找到對應的原理。ios
https://github.com/MRPT/mrpt MRPT中能夠採用多種掃描匹配的方式，能夠經過配置文件進行配置。git
雙線程和程序的基本執行流程github
GMapping採用GridSlamProcessorThread後臺線程進行數據的讀取和運算，在gsp_thread.h和相應的實現文件gsp_thread.cpp中能夠看到初始化，參數配置，掃描數據讀取等方法。算法
最關鍵的流程是在後臺線程調用的方法void * GridSlamProcessorThread::fastslamthread(GridSlamProcessorThread* gpt)app
而這個方法中最主要的處理掃描數據的過程在428行，bool processed=gpt->processScan(*rr);同時能夠看到GMapping支持在線和離線兩種模式。dom
注意到struct GridSlamProcessorThread : public GridSlamProcessor ，這個後臺線程執行類GridSlamProcessorThread 繼承自GridSlamProcessor，因此執行的是GridSlamProcessor的processScan方法。ide
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."
相關文章
- 1. slam-gmapping 源碼閱讀
- 2. slam-gmapping 源碼閱讀2.md
- 3. gmapping源碼閱讀
- 4. 十四課 slam&gmapping
- 5. laravel5.5源碼閱讀草稿——路由
- 6. laravel5.5源碼閱讀草稿——application
- 7. ROS功能包之gmapping（SLAM）
- 8. Gmapping Slam主要代碼分析
- 9. SLAM文獻閱讀（不按期更新）
- 10. 文獻閱讀|Dynamic-SLAM
- 更多相關文章...
- • RSS 閱讀器 - RSS 教程
- • PHP 實例 - AJAX RSS 閱讀器 - PHP教程
- • Java Agent入門實戰（二）-Instrumentation源碼概述
- • JDK13 GA發佈：5大特性解讀
相關標籤/搜索
每日一句
-
每一个你不满意的现在，都有一个你没有努力的曾经。
歡迎關注本站公眾號,獲取更多信息
