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 }