gridslamprocessor.cpp
Go to the documentation of this file.
00001 #include <string>
00002 #include <deque>
00003 #include <list>
00004 #include <map>
00005 #include <set>
00006 #include <fstream>
00007 #include <iomanip>
00008 #include <gmapping/utils/stat.h>
00009 #include <gmapping/gridfastslam/gridslamprocessor.h>
00010 
00011 //#define MAP_CONSISTENCY_CHECK
00012 //#define GENERATE_TRAJECTORIES
00013 
00014 namespace GMapping {
00015 
00016 const double m_distanceThresholdCheck = 20;
00017  
00018 using namespace std;
00019 
00020   GridSlamProcessor::GridSlamProcessor(): m_infoStream(cout){
00021     
00022     period_ = 5.0;
00023     m_obsSigmaGain=1;
00024     m_resampleThreshold=0.5;
00025     m_minimumScore=0.;
00026   }
00027   
00028   GridSlamProcessor::GridSlamProcessor(const GridSlamProcessor& gsp) 
00029     :last_update_time_(0.0), m_particles(gsp.m_particles), m_infoStream(cout){
00030     
00031     period_ = 5.0;
00032 
00033     m_obsSigmaGain=gsp.m_obsSigmaGain;
00034     m_resampleThreshold=gsp.m_resampleThreshold;
00035     m_minimumScore=gsp.m_minimumScore;
00036     
00037     m_beams=gsp.m_beams;
00038     m_indexes=gsp.m_indexes;
00039     m_motionModel=gsp.m_motionModel;
00040     m_resampleThreshold=gsp.m_resampleThreshold;
00041     m_matcher=gsp.m_matcher;
00042     
00043     m_count=gsp.m_count;
00044     m_readingCount=gsp.m_readingCount;
00045     m_lastPartPose=gsp.m_lastPartPose;
00046     m_pose=gsp.m_pose;
00047     m_odoPose=gsp.m_odoPose;
00048     m_linearDistance=gsp.m_linearDistance;
00049     m_angularDistance=gsp.m_angularDistance;
00050     m_neff=gsp.m_neff;
00051         
00052     cerr << "FILTER COPY CONSTRUCTOR" << endl;
00053     cerr << "m_odoPose=" << m_odoPose.x << " " <<m_odoPose.y << " " << m_odoPose.theta << endl;
00054     cerr << "m_lastPartPose=" << m_lastPartPose.x << " " <<m_lastPartPose.y << " " << m_lastPartPose.theta << endl;
00055     cerr << "m_linearDistance=" << m_linearDistance << endl;
00056     cerr << "m_angularDistance=" << m_linearDistance << endl;
00057     
00058                 
00059     m_xmin=gsp.m_xmin;
00060     m_ymin=gsp.m_ymin;
00061     m_xmax=gsp.m_xmax;
00062     m_ymax=gsp.m_ymax;
00063     m_delta=gsp.m_delta;
00064     
00065     m_regScore=gsp.m_regScore;
00066     m_critScore=gsp.m_critScore;
00067     m_maxMove=gsp.m_maxMove;
00068     
00069     m_linearThresholdDistance=gsp.m_linearThresholdDistance;
00070     m_angularThresholdDistance=gsp.m_angularThresholdDistance;
00071     m_obsSigmaGain=gsp.m_obsSigmaGain;
00072     
00073 #ifdef MAP_CONSISTENCY_CHECK
00074     cerr << __PRETTY_FUNCTION__ <<  ": trajectories copy.... ";
00075 #endif
00076     TNodeVector v=gsp.getTrajectories();
00077     for (unsigned int i=0; i<v.size(); i++){
00078                 m_particles[i].node=v[i];
00079     }
00080 #ifdef MAP_CONSISTENCY_CHECK
00081     cerr <<  "end" << endl;
00082 #endif
00083 
00084 
00085     cerr  << "Tree: normalizing, resetting and propagating weights within copy construction/cloneing ..." ;
00086     updateTreeWeights(false);
00087     cerr  << ".done!" <<endl;
00088   }
00089   
00090   GridSlamProcessor::GridSlamProcessor(std::ostream& infoS): m_infoStream(infoS){
00091     period_ = 5.0;
00092     m_obsSigmaGain=1;
00093     m_resampleThreshold=0.5;
00094     m_minimumScore=0.;
00095         
00096   }
00097 
00098   GridSlamProcessor* GridSlamProcessor::clone() const {
00099 # ifdef MAP_CONSISTENCY_CHECK
00100     cerr << __PRETTY_FUNCTION__ << ": performing preclone_fit_test" << endl;
00101     typedef std::map<autoptr< Array2D<PointAccumulator> >::reference* const, int> PointerMap;
00102     PointerMap pmap;
00103         for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){
00104           const ScanMatcherMap& m1(it->map);
00105           const HierarchicalArray2D<PointAccumulator>& h1(m1.storage());
00106           for (int x=0; x<h1.getXSize(); x++){
00107             for (int y=0; y<h1.getYSize(); y++){
00108               const autoptr< Array2D<PointAccumulator> >& a1(h1.m_cells[x][y]);
00109               if (a1.m_reference){
00110                 PointerMap::iterator f=pmap.find(a1.m_reference);
00111                 if (f==pmap.end())
00112                   pmap.insert(make_pair(a1.m_reference, 1));
00113                 else
00114                   f->second++;
00115               }
00116             }
00117           }
00118         }
00119         cerr << __PRETTY_FUNCTION__ <<  ": Number of allocated chunks" << pmap.size() << endl;
00120         for(PointerMap::const_iterator it=pmap.begin(); it!=pmap.end(); it++)
00121           assert(it->first->shares==(unsigned int)it->second);
00122 
00123         cerr << __PRETTY_FUNCTION__ <<  ": SUCCESS, the error is somewhere else" << endl;
00124 # endif
00125         GridSlamProcessor* cloned=new GridSlamProcessor(*this);
00126         
00127 # ifdef MAP_CONSISTENCY_CHECK
00128         cerr << __PRETTY_FUNCTION__ <<  ": trajectories end" << endl;
00129         cerr << __PRETTY_FUNCTION__ << ": performing afterclone_fit_test" << endl;
00130         ParticleVector::const_iterator jt=cloned->m_particles.begin();
00131         for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){
00132           const ScanMatcherMap& m1(it->map);
00133           const ScanMatcherMap& m2(jt->map);
00134           const HierarchicalArray2D<PointAccumulator>& h1(m1.storage());
00135           const HierarchicalArray2D<PointAccumulator>& h2(m2.storage());
00136           jt++;
00137           for (int x=0; x<h1.getXSize(); x++){
00138             for (int y=0; y<h1.getYSize(); y++){
00139               const autoptr< Array2D<PointAccumulator> >& a1(h1.m_cells[x][y]);
00140               const autoptr< Array2D<PointAccumulator> >& a2(h2.m_cells[x][y]);
00141               assert(a1.m_reference==a2.m_reference);
00142               assert((!a1.m_reference) || !(a1.m_reference->shares%2));
00143             }
00144           }
00145         }
00146         cerr << __PRETTY_FUNCTION__ <<  ": SUCCESS, the error is somewhere else" << endl;
00147 # endif
00148         return cloned;
00149 }
00150   
00151   GridSlamProcessor::~GridSlamProcessor(){
00152     cerr << __PRETTY_FUNCTION__ << ": Start" << endl;
00153     cerr << __PRETTY_FUNCTION__ << ": Deleting tree" << endl;
00154     for (std::vector<Particle>::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
00155 #ifdef TREE_CONSISTENCY_CHECK           
00156       TNode* node=it->node;
00157       while(node)
00158         node=node->parent;
00159       cerr << "@" << endl;
00160 #endif
00161       if (it->node)
00162         delete it->node;
00163       //cout << "l=" << it->weight<< endl;
00164     }
00165     
00166 # ifdef MAP_CONSISTENCY_CHECK
00167     cerr << __PRETTY_FUNCTION__ << ": performing predestruction_fit_test" << endl;
00168     typedef std::map<autoptr< Array2D<PointAccumulator> >::reference* const, int> PointerMap;
00169     PointerMap pmap;
00170     for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){
00171       const ScanMatcherMap& m1(it->map);
00172       const HierarchicalArray2D<PointAccumulator>& h1(m1.storage());
00173       for (int x=0; x<h1.getXSize(); x++){
00174         for (int y=0; y<h1.getYSize(); y++){
00175           const autoptr< Array2D<PointAccumulator> >& a1(h1.m_cells[x][y]);
00176           if (a1.m_reference){
00177             PointerMap::iterator f=pmap.find(a1.m_reference);
00178             if (f==pmap.end())
00179               pmap.insert(make_pair(a1.m_reference, 1));
00180             else
00181               f->second++;
00182           }
00183         }
00184       }
00185     }
00186     cerr << __PRETTY_FUNCTION__ << ": Number of allocated chunks" << pmap.size() << endl;
00187     for(PointerMap::const_iterator it=pmap.begin(); it!=pmap.end(); it++)
00188       assert(it->first->shares>=(unsigned int)it->second);
00189     cerr << __PRETTY_FUNCTION__ << ": SUCCESS, the error is somewhere else" << endl;
00190 # endif
00191   }
00192 
00193 
00194                 
00195   void GridSlamProcessor::setMatchingParameters (double urange, double range, double sigma, int kernsize, double lopt, double aopt, 
00196                                                  int iterations, double likelihoodSigma, double likelihoodGain, unsigned int likelihoodSkip){
00197     m_obsSigmaGain=likelihoodGain;
00198     m_matcher.setMatchingParameters(urange, range, sigma, kernsize, lopt, aopt, iterations, likelihoodSigma, likelihoodSkip);
00199     if (m_infoStream)
00200       m_infoStream << " -maxUrange "<< urange
00201                    << " -maxUrange "<< range
00202                    << " -sigma     "<< sigma
00203                    << " -kernelSize "<< kernsize
00204                    << " -lstep "    << lopt
00205                    << " -lobsGain " << m_obsSigmaGain
00206                    << " -astep "    << aopt << endl;
00207     
00208     
00209   }
00210   
00211 void GridSlamProcessor::setMotionModelParameters
00212 (double srr, double srt, double str, double stt){
00213   m_motionModel.srr=srr;
00214   m_motionModel.srt=srt;
00215   m_motionModel.str=str;
00216   m_motionModel.stt=stt;        
00217   
00218   if (m_infoStream)
00219     m_infoStream << " -srr "<< srr      << " -srt "<< srt  
00220                  << " -str "<< str      << " -stt "<< stt << endl;
00221   
00222 }
00223   
00224   void GridSlamProcessor::setUpdateDistances(double linear, double angular, double resampleThreshold){
00225     m_linearThresholdDistance=linear; 
00226     m_angularThresholdDistance=angular;
00227     m_resampleThreshold=resampleThreshold;      
00228     if (m_infoStream)
00229       m_infoStream << " -linearUpdate " << linear
00230                    << " -angularUpdate "<< angular
00231                    << " -resampleThreshold " << m_resampleThreshold << endl;
00232   }
00233   
00234   //HERE STARTS THE BEEF
00235 
00236   GridSlamProcessor::Particle::Particle(const ScanMatcherMap& m):
00237     map(m), pose(0,0,0), weight(0), weightSum(0), gweight(0), previousIndex(0){
00238     node=0;
00239   }
00240   
00241   
00242   void GridSlamProcessor::setSensorMap(const SensorMap& smap){
00243     
00244     /*
00245       Construct the angle table for the sensor
00246       
00247       FIXME For now detect the readings of only the front laser, and assume its pose is in the center of the robot 
00248     */
00249     
00250     SensorMap::const_iterator laser_it=smap.find(std::string("FLASER"));
00251     if (laser_it==smap.end()){
00252       cerr << "Attempting to load the new carmen log format" << endl;
00253       laser_it=smap.find(std::string("ROBOTLASER1"));
00254       assert(laser_it!=smap.end());
00255     }
00256     const RangeSensor* rangeSensor=dynamic_cast<const RangeSensor*>((laser_it->second));
00257     assert(rangeSensor && rangeSensor->beams().size());
00258     
00259     m_beams=static_cast<unsigned int>(rangeSensor->beams().size());
00260     double* angles=new double[rangeSensor->beams().size()];
00261     for (unsigned int i=0; i<m_beams; i++){
00262       angles[i]=rangeSensor->beams()[i].pose.theta;
00263     }
00264     m_matcher.setLaserParameters(m_beams, angles, rangeSensor->getPose());
00265     delete [] angles;
00266   }
00267   
00268   void GridSlamProcessor::init(unsigned int size, double xmin, double ymin, double xmax, double ymax, double delta, OrientedPoint initialPose){
00269     m_xmin=xmin;
00270     m_ymin=ymin;
00271     m_xmax=xmax;
00272     m_ymax=ymax;
00273     m_delta=delta;
00274     if (m_infoStream)
00275       m_infoStream 
00276         << " -xmin "<< m_xmin
00277         << " -xmax "<< m_xmax
00278         << " -ymin "<< m_ymin
00279         << " -ymax "<< m_ymax
00280         << " -delta "<< m_delta
00281         << " -particles "<< size << endl;
00282     
00283 
00284     m_particles.clear();
00285     TNode* node=new TNode(initialPose, 0, 0, 0);
00286     ScanMatcherMap lmap(Point(xmin+xmax, ymin+ymax)*.5, xmax-xmin, ymax-ymin, delta);
00287     for (unsigned int i=0; i<size; i++){
00288       m_particles.push_back(Particle(lmap));
00289       m_particles.back().pose=initialPose;
00290       m_particles.back().previousPose=initialPose;
00291       m_particles.back().setWeight(0);
00292       m_particles.back().previousIndex=0;
00293       
00294                 // this is not needed
00295                 //              m_particles.back().node=new TNode(initialPose, 0, node, 0);
00296 
00297                 // we use the root directly
00298                 m_particles.back().node= node;
00299     }
00300     m_neff=(double)size;
00301     m_count=0;
00302     m_readingCount=0;
00303     m_linearDistance=m_angularDistance=0;
00304   }
00305 
00306   void GridSlamProcessor::processTruePos(const OdometryReading& o){
00307     const OdometrySensor* os=dynamic_cast<const OdometrySensor*>(o.getSensor());
00308     if (os && os->isIdeal() && m_outputStream){
00309       m_outputStream << setiosflags(ios::fixed) << setprecision(3);
00310       m_outputStream <<  "SIMULATOR_POS " <<  o.getPose().x << " " << o.getPose().y << " " ;
00311       m_outputStream << setiosflags(ios::fixed) << setprecision(6) << o.getPose().theta << " " <<  o.getTime() << endl;
00312     }
00313   }
00314   
00315   
00316   bool GridSlamProcessor::processScan(const RangeReading & reading, int adaptParticles){
00317     
00319     OrientedPoint relPose=reading.getPose();
00320     if (!m_count){
00321       m_lastPartPose=m_odoPose=relPose;
00322     }
00323     
00324     //write the state of the reading and update all the particles using the motion model
00325     for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
00326       OrientedPoint& pose(it->pose);
00327       pose=m_motionModel.drawFromMotion(it->pose, relPose, m_odoPose);
00328     }
00329 
00330     // update the output file
00331     if (m_outputStream.is_open()){
00332       m_outputStream << setiosflags(ios::fixed) << setprecision(6);
00333       m_outputStream << "ODOM ";
00334       m_outputStream << setiosflags(ios::fixed) << setprecision(3) << m_odoPose.x << " " << m_odoPose.y << " ";
00335       m_outputStream << setiosflags(ios::fixed) << setprecision(6) << m_odoPose.theta << " ";
00336       m_outputStream << reading.getTime();
00337       m_outputStream << endl;
00338     }
00339     if (m_outputStream.is_open()){
00340       m_outputStream << setiosflags(ios::fixed) << setprecision(6);
00341       m_outputStream << "ODO_UPDATE "<< m_particles.size() << " ";
00342       for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
00343         OrientedPoint& pose(it->pose);
00344         m_outputStream << setiosflags(ios::fixed) << setprecision(3) << pose.x << " " << pose.y << " ";
00345         m_outputStream << setiosflags(ios::fixed) << setprecision(6) << pose.theta << " " << it-> weight << " ";
00346       }
00347       m_outputStream << reading.getTime();
00348       m_outputStream << endl;
00349     }
00350     
00351     //invoke the callback
00352     onOdometryUpdate();
00353     
00354 
00355     // accumulate the robot translation and rotation
00356     OrientedPoint move=relPose-m_odoPose;
00357     move.theta=atan2(sin(move.theta), cos(move.theta));
00358     m_linearDistance+=sqrt(move*move);
00359     m_angularDistance+=fabs(move.theta);
00360     
00361     // if the robot jumps throw a warning
00362     if (m_linearDistance>m_distanceThresholdCheck){
00363       cerr << "***********************************************************************" << endl;
00364       cerr << "********** Error: m_distanceThresholdCheck overridden!!!! *************" << endl;
00365       cerr << "m_distanceThresholdCheck=" << m_distanceThresholdCheck << endl;
00366       cerr << "Old Odometry Pose= " << m_odoPose.x << " " << m_odoPose.y 
00367            << " " <<m_odoPose.theta << endl;
00368       cerr << "New Odometry Pose (reported from observation)= " << relPose.x << " " << relPose.y 
00369            << " " <<relPose.theta << endl;
00370       cerr << "***********************************************************************" << endl;
00371       cerr << "** The Odometry has a big jump here. This is probably a bug in the   **" << endl;
00372       cerr << "** odometry/laser input. We continue now, but the result is probably **" << endl;
00373       cerr << "** crap or can lead to a core dump since the map doesn't fit.... C&G **" << endl;
00374       cerr << "***********************************************************************" << endl;
00375     }
00376     
00377     m_odoPose=relPose;
00378     
00379     bool processed=false;
00380 
00381     // process a scan only if the robot has traveled a given distance or a certain amount of time has elapsed
00382     if (! m_count 
00383         || m_linearDistance>=m_linearThresholdDistance 
00384         || m_angularDistance>=m_angularThresholdDistance
00385     || (period_ >= 0.0 && (reading.getTime() - last_update_time_) > period_)){
00386       last_update_time_ = reading.getTime();      
00387 
00388       if (m_outputStream.is_open()){
00389         m_outputStream << setiosflags(ios::fixed) << setprecision(6);
00390         m_outputStream << "FRAME " <<  m_readingCount;
00391         m_outputStream << " " << m_linearDistance;
00392         m_outputStream << " " << m_angularDistance << endl;
00393       }
00394       
00395       if (m_infoStream)
00396         m_infoStream << "update frame " <<  m_readingCount << endl
00397                      << "update ld=" << m_linearDistance << " ad=" << m_angularDistance << endl;
00398       
00399       
00400       cerr << "Laser Pose= " << reading.getPose().x << " " << reading.getPose().y 
00401            << " " << reading.getPose().theta << endl;
00402       
00403       
00404       //this is for converting the reading in a scan-matcher feedable form
00405       assert(reading.size()==m_beams);
00406       double * plainReading = new double[m_beams];
00407       for(unsigned int i=0; i<m_beams; i++){
00408         plainReading[i]=reading[i];
00409       }
00410       m_infoStream << "m_count " << m_count << endl;
00411 
00412       RangeReading* reading_copy = 
00413               new RangeReading(reading.size(),
00414                                &(reading[0]),
00415                                static_cast<const RangeSensor*>(reading.getSensor()),
00416                                reading.getTime());
00417 
00418       if (m_count>0){
00419         scanMatch(plainReading);
00420         if (m_outputStream.is_open()){
00421           m_outputStream << "LASER_READING "<< reading.size() << " ";
00422           m_outputStream << setiosflags(ios::fixed) << setprecision(2);
00423           for (RangeReading::const_iterator b=reading.begin(); b!=reading.end(); b++){
00424             m_outputStream << *b << " ";
00425           }
00426           OrientedPoint p=reading.getPose();
00427           m_outputStream << setiosflags(ios::fixed) << setprecision(6);
00428           m_outputStream << p.x << " " << p.y << " " << p.theta << " " << reading.getTime()<< endl;
00429           m_outputStream << "SM_UPDATE "<< m_particles.size() << " ";
00430           for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){
00431             const OrientedPoint& pose=it->pose;
00432             m_outputStream << setiosflags(ios::fixed) << setprecision(3) <<  pose.x << " " << pose.y << " ";
00433             m_outputStream << setiosflags(ios::fixed) << setprecision(6) <<  pose.theta << " " << it-> weight << " ";
00434           }
00435           m_outputStream << endl;
00436         }
00437         onScanmatchUpdate();
00438         
00439         updateTreeWeights(false);
00440                                 
00441         if (m_infoStream){
00442           m_infoStream << "neff= " << m_neff  << endl;
00443         }
00444         if (m_outputStream.is_open()){
00445           m_outputStream << setiosflags(ios::fixed) << setprecision(6);
00446           m_outputStream << "NEFF " << m_neff << endl;
00447         }
00448         resample(plainReading, adaptParticles, reading_copy);
00449         
00450       } else {
00451         m_infoStream << "Registering First Scan"<< endl;
00452         for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){     
00453           m_matcher.invalidateActiveArea();
00454           m_matcher.computeActiveArea(it->map, it->pose, plainReading);
00455           m_matcher.registerScan(it->map, it->pose, plainReading);
00456           
00457           // cyr: not needed anymore, particles refer to the root in the beginning!
00458           TNode* node=new       TNode(it->pose, 0., it->node,  0);
00459           //node->reading=0;
00460       node->reading = reading_copy;
00461           it->node=node;
00462           
00463         }
00464       }
00465       //                cerr  << "Tree: normalizing, resetting and propagating weights at the end..." ;
00466       updateTreeWeights(false);
00467       //                cerr  << ".done!" <<endl;
00468       
00469       delete [] plainReading;
00470       m_lastPartPose=m_odoPose; //update the past pose for the next iteration
00471       m_linearDistance=0;
00472       m_angularDistance=0;
00473       m_count++;
00474       processed=true;
00475       
00476       //keep ready for the next step
00477       for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
00478         it->previousPose=it->pose;
00479       }
00480       
00481     }
00482     if (m_outputStream.is_open())
00483       m_outputStream << flush;
00484     m_readingCount++;
00485     return processed;
00486   }
00487   
00488   
00489   std::ofstream& GridSlamProcessor::outputStream(){
00490     return m_outputStream;
00491   }
00492   
00493   std::ostream& GridSlamProcessor::infoStream(){
00494     return m_infoStream;
00495   }
00496   
00497   
00498   int GridSlamProcessor::getBestParticleIndex() const{
00499     unsigned int bi=0;
00500     double bw=-std::numeric_limits<double>::max();
00501     for (unsigned int i=0; i<m_particles.size(); i++)
00502       if (bw<m_particles[i].weightSum){
00503         bw=m_particles[i].weightSum;
00504         bi=i;
00505       }
00506     return (int) bi;
00507   }
00508 
00509   void GridSlamProcessor::onScanmatchUpdate(){}
00510   void GridSlamProcessor::onResampleUpdate(){}
00511   void GridSlamProcessor::onOdometryUpdate(){}
00512 
00513   
00514 };// end namespace
00515 
00516 
00517 
00518 


openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Fri Aug 28 2015 11:56:21