[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 }
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時刻的里程計讀數。參考博客:小豆包的學習之旅:里程計運動模型

 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 }
GridSlamProcessor::scanMatch

注意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 }
View Code

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 }
GridSlamProcessor::resample

 

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 }
ScanMatcher::registerScan

rbpf-gmapping的使用的是文獻[1] 9.2節的計算方法,在Occupancy_Grid_Mapping.m文件中實現,同時調用Inverse_Range_Sensor_Model方法。

gridlineTraversal實現了beam轉成柵格的線。對每一束激光束,設start爲該激光束起點,end爲激光束端點(障礙物位置),使用Bresenham劃線算法肯定激光束通過的格網。

小豆包的學習之旅:佔用機率柵格地圖和cost-map

 

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 }
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."

相關文章
相關標籤/搜索