gfsreader.cpp
Go to the documentation of this file.
00001 #include <cstring>
00002 #include "gfsreader.h"
00003 #include <iomanip>
00004 #include <limits>
00005 
00006 namespace  GMapping { 
00007 
00008 namespace GFSReader{
00009 
00010 Record::~Record(){}
00011 void Record::write(ostream& os){};
00012 
00013 void CommentRecord::read(istream& is){
00014         char buf[MAX_LINE_LENGHT];
00015         memset(buf,0, MAX_LINE_LENGHT*sizeof(char));
00016         is.getline(buf, MAX_LINE_LENGHT);
00017         text=string(buf);
00018 }
00019 
00020 void CommentRecord::write(ostream& os){
00021         os << "#GFS_COMMENT: " << text << endl;
00022 }
00023 
00024 PoseRecord::PoseRecord(bool ideal){
00025         truePos=ideal;
00026 }
00027 void PoseRecord::read(istream& is){
00028         is >> pose.x >> pose.y >> pose.theta;
00029         time = 0;
00030         if (is)
00031                 is >> time;
00032 }
00033 void PoseRecord::write(ostream& os){
00034         if (truePos)
00035                 os << "TRUEPOS ";
00036         else
00037                 os << "ODOM ";
00038         os << setiosflags(ios::fixed) << setprecision(6);
00039         os << pose.x << " " << pose.y << " " << pose.theta << " 0 0 0 ";
00040         os << time << " pippo " << time << endl;
00041 }
00042 
00043 void NeffRecord::read(istream& is){
00044         is >> neff;
00045         time =0;
00046         if (is)
00047           is >> time;
00048 
00049 }
00050 
00051 void NeffRecord::write(ostream& os){
00052         os << "NEFF " << neff ;
00053         os << setiosflags(ios::fixed) << setprecision(6);
00054         os << " " << time << " pippo " << time << endl;
00055 }
00056 
00057 
00058 void OdometryRecord::read(istream& is){
00059         is >> dim;
00060         for (unsigned int i=0; i< dim; i++){
00061                 OrientedPoint p;
00062                 double w;
00063                 is >> p.x;
00064                 is >> p.y;
00065                 is >> p.theta;
00066                 is >> w;
00067                 poses.push_back(p);
00068         }
00069         time = 0;
00070         if (is)
00071                 is >> time;
00072 }
00073 
00074 void RawOdometryRecord::read(istream& is){
00075   is >> pose.x;
00076   is >> pose.y;
00077   is >> pose.theta;
00078   time = 0;
00079   assert(is);
00080     is >> time;
00081  
00082 }
00083 
00084 
00085 void EntropyRecord::read(istream& is){
00086   is >> poseEntropy >> trajectoryEntropy >> mapEntropy;
00087   time =0;
00088   if (is)
00089     is >> time;
00090 }
00091 
00092 void EntropyRecord::write(ostream& os){
00093   os << setiosflags(ios::fixed) << setprecision(6) << "ENTROPY " << poseEntropy << " " << trajectoryEntropy << " " << mapEntropy;
00094   os << " " << time << " pippo " << time << endl;
00095 }
00096 
00097 void ScanMatchRecord::read(istream& is){
00098         is >> dim;
00099         for (unsigned int i=0; i< dim; i++){
00100                 OrientedPoint p;
00101                 double w;
00102                 is >> p.x;
00103                 is >> p.y;
00104                 is >> p.theta;
00105                 is >> w;
00106                 poses.push_back(p);
00107                 weights.push_back(w);
00108         }
00109 }
00110 
00111 void LaserRecord::read(istream& is){
00112         is >> dim;
00113         for (unsigned int i=0; i< dim; i++){
00114                 double r;
00115                 is >> r;
00116                 readings.push_back(r);
00117         }
00118         is >> pose.x;
00119         is >> pose.y;
00120         is >> pose.theta;
00121         time = 0;
00122         if (is)
00123                 is >> time;
00124 }
00125 
00126 void LaserRecord::write(ostream& os){
00127         os << "WEIGHT " << weight << endl;
00128         os << "ROBOTLASER1 ";
00129         
00130         
00131         if ((dim == 541)||(dim == 540)) { // S300
00132           os <<" 4";  // laser type
00133           os <<" -2.351831";  // start_angle
00134           os <<" 4.712389";  // fov
00135           os <<" 0.008727";  // angular res
00136           os <<" 30.0" ;  // maxrange
00137         }
00138         else if ((dim == 180)||(dim == 181)) { // PLS
00139           os <<" 0";  // laser type
00140           os <<" -1.570796";  // start_angle
00141           os <<" 3.141593";  // fov
00142           os <<" 0.017453";  // angular res
00143           os <<" 81.9" ;  // maxrange
00144         }
00145         else if ((dim == 360)||(dim == 361)) { // LMS
00146           os <<" 0";  // laser type
00147           os <<" -1.570796";  // start_angle
00148           os <<" 3.141593";  // fov
00149           os <<" 0.008726";  // angular res
00150           os <<" 81.9" ;  // maxrange
00151         }
00152         else if ((dim == 682)||(dim == 683)) { // URG
00153           os <<" 0";  // laser type
00154           os <<" -2.094395";  // start_angle
00155           os <<" 4.1887902";  // fov
00156           os << " " <<   360.0/1024.0/180.0*M_PI;  // angular res
00157           os <<" 5.5" ;  // maxrange
00158         }
00159         else {     // PLS
00160           os <<" 0";  // laser type
00161           os <<" -1.570796";  // start_angle
00162           os <<" 3.141593";  // fov
00163           os <<" 0.017453";  // angular res
00164           os <<" 81.9" ;  // maxrange
00165         }
00166         os <<" 0.01"; // accuracy       
00167         os <<" 0" ;  // remission mode
00168         os <<" "<< dim; // num readings
00169         os << setiosflags(ios::fixed) << setprecision(2);
00170         for (unsigned int i=0; i< dim; i++){
00171                 os <<" "<< readings[i] ;
00172         }
00173         os << setiosflags(ios::fixed) << setprecision(6);
00174         os <<" 0"; // num remession values
00175         os <<" "<< pose.x;
00176         os <<" "<< pose.y;
00177         os <<" "<< pose.theta;
00178         os <<" "<< pose.x;
00179         os <<" "<< pose.y;
00180         os <<" "<< pose.theta;
00181         os <<" 0" ;  // tv
00182         os <<" 0" ;  // rv
00183         os <<" 0.55" ;  // forward_safety_dist
00184         os <<" 0.375" ;     // sideward_safety_dist
00185         os <<" 1000000.0" ; // turn_axis
00186         os <<" "<< time <<  " localhost " << time << endl;
00187 };
00188 
00189 void ResampleRecord::read(istream& is){
00190         is >> dim;
00191         for (unsigned int i=0; i< dim; i++){
00192                 unsigned int j;
00193                 is >> j;
00194                 indexes.push_back(j);
00195         }
00196 }
00197 
00198 istream& RecordList::read(istream& is){
00199         while(is){
00200                 char buf[MAX_LINE_LENGHT];
00201                 is.getline(buf, MAX_LINE_LENGHT);
00202                 istringstream lineStream(buf);
00203                 string recordType;
00204                 lineStream >> recordType;
00205                 Record* rec=0;
00206                 if (recordType=="LASER_READING"){
00207                         rec=new LaserRecord;
00208 //                      cout << "l" << flush;
00209                 }
00210                 else if (recordType=="ODO_UPDATE"){
00211                         rec=new OdometryRecord;
00212 //                      cout << "o" << flush;
00213                 }
00214                 else if (recordType=="ODOM"){
00215                         rec=new RawOdometryRecord;
00216 //                      cout << "O" << flush;
00217                 }
00218                 else if (recordType=="SM_UPDATE"){
00219                         rec=new ScanMatchRecord;
00220 //                      cout << "m" << flush;
00221                 }
00222                 else if (recordType=="SIMULATOR_POS"){
00223                         rec=new PoseRecord(true);
00224 //                      cout << "t" << flush;
00225                 }
00226                 else if (recordType=="RESAMPLE"){
00227                         rec=new ResampleRecord;
00228 //                      cout << "r" << flush;
00229                 }
00230                 else if (recordType=="NEFF"){
00231                         rec=new NeffRecord;
00232 //                      cout << "n" << flush;
00233                 }
00234                 else if (recordType=="COMMENT" || recordType=="#COMMENT"){
00235                         rec=new CommentRecord;
00236 //                      cout << "c" << flush;
00237                 }
00238                 else if (recordType=="ENTROPY"){
00239                         rec=new EntropyRecord;
00240 //                      cout << "c" << flush;
00241                 }
00242                 
00243                 if (rec){
00244                         rec->read(lineStream);
00245                         push_back(rec);
00246                 }
00247         }
00248         return is;
00249 }
00250 
00251 double RecordList::getLogWeight(unsigned int i) const{
00252         double weight=0;
00253         unsigned int currentIndex=i;
00254         for(RecordList::const_reverse_iterator it=rbegin(); it!=rend(); it++){
00255                 ScanMatchRecord* scanmatch=dynamic_cast<ScanMatchRecord*>(*it); 
00256                 if (scanmatch){
00257                         weight+=scanmatch->weights[currentIndex];
00258                 }
00259                 ResampleRecord* resample=dynamic_cast<ResampleRecord*>(*it); 
00260                 if (resample){
00261                         currentIndex=resample->indexes[currentIndex];
00262                 }
00263         }
00264         return weight;
00265 }
00266 
00267 double RecordList::getLogWeight(unsigned int i, RecordList::const_iterator frame) const{
00268         double weight=0;
00269         unsigned int currentIndex=i;
00270         for(RecordList::const_reverse_iterator it(frame); it!=rend(); it++){
00271                 ScanMatchRecord* scanmatch=dynamic_cast<ScanMatchRecord*>(*it); 
00272                 if (scanmatch){
00273                         weight+=scanmatch->weights[currentIndex];
00274                 }
00275                 ResampleRecord* resample=dynamic_cast<ResampleRecord*>(*it); 
00276                 if (resample){
00277                         currentIndex=resample->indexes[currentIndex];
00278                 }
00279         }
00280         return weight;
00281 }
00282 
00283 unsigned int RecordList::getBestIdx() const {
00284         if (empty())
00285                 return 0;
00286         const ScanMatchRecord* scanmatch=0;
00287         const_reverse_iterator it=rbegin();
00288         while(!scanmatch){
00289                 scanmatch=dynamic_cast<const ScanMatchRecord*>(*it); 
00290                 it++;
00291         }
00292         unsigned int dim=scanmatch->dim;
00293         sampleSize=(int)dim;
00294         double bestw=-std::numeric_limits<double>::max();
00295         unsigned int best=scanmatch->dim+1;
00296         for (unsigned i=0; i<dim; i++){
00297                 double w=getLogWeight(i);
00298                 if (w>bestw){
00299                         best=i;
00300                         bestw=w;
00301                 }
00302         }
00303         return best;
00304 }
00305 
00306 void RecordList::printLastParticles(ostream& os) const {
00307         if (empty())
00308                 return;
00309         const ScanMatchRecord* scanmatch=0;
00310         const_reverse_iterator it=rbegin();
00311         while(!scanmatch){
00312                 scanmatch=dynamic_cast<const ScanMatchRecord*>(*it); 
00313                 it++;
00314         }
00315         if (! scanmatch)
00316                 return;
00317         for (vector<OrientedPoint>::const_iterator it=scanmatch->poses.begin(); it!=scanmatch->poses.end(); it++){
00318                 os << "MARKER [color=black; circle=" << it->x*100 << "," << it->y*100 << ",10] 0 pippo 0" << endl;
00319         }
00320 }
00321 
00322 void RecordList::destroyReferences(){
00323         for(RecordList::iterator it=begin(); it!=end(); it++)
00324                 delete (*it);
00325         
00326 }
00327 
00328 RecordList RecordList::computePath(unsigned int i, RecordList::const_iterator frame) const{
00329         unsigned int currentIndex=i;
00330         OrientedPoint p(0,0,0);
00331         RecordList rl;
00332         
00333         //reconstruct a  path
00334         bool first=true;
00335         for(RecordList::const_reverse_iterator it(frame); it!=rend(); it++){
00336                 const ScanMatchRecord* scanmatch=dynamic_cast<const ScanMatchRecord*>(*it); 
00337                 if (scanmatch){
00338                         p=scanmatch->poses[currentIndex];
00339                         first=false;
00340                 }
00341                 const LaserRecord* laser=dynamic_cast<const LaserRecord*>(*it); 
00342                 if (laser && !first){
00343                         LaserRecord* claser=new LaserRecord(*laser);
00344                         claser->pose=p;
00345                         rl.push_front(claser);
00346                 }
00347                 const ResampleRecord* resample=dynamic_cast<const ResampleRecord*>(*it); 
00348                 if (resample){
00349                         currentIndex=resample->indexes[currentIndex];
00350                 }
00351         }
00352         return rl;
00353 }
00354 
00355         
00356 void RecordList::printPath(ostream& os, unsigned int i, bool err, bool rawodom) const{
00357         unsigned int currentIndex=i;
00358         OrientedPoint p(0,0,0);
00359         RecordList rl;
00360         double oldWeight=0;
00361         double w=0;
00362         //reconstruct a  path
00363         for(RecordList::const_reverse_iterator it=rbegin(); it!=rend(); it++){
00364                 const NeffRecord* neff=dynamic_cast<const NeffRecord*>(*it);
00365                 if (neff){
00366                         NeffRecord* n=new NeffRecord(*neff);
00367                         rl.push_front(n);
00368                 }
00369                 const EntropyRecord* entropy=dynamic_cast<const EntropyRecord*>(*it);
00370                 if (entropy){
00371                     EntropyRecord* n=new EntropyRecord(*entropy);
00372                     rl.push_front(n);
00373                 }
00374                 const ScanMatchRecord* scanmatch=dynamic_cast<const ScanMatchRecord*>(*it); 
00375                 if (scanmatch){
00376                         PoseRecord* pose=new PoseRecord;
00377                         pose->dim=0;
00378                         p=pose->pose=scanmatch->poses[currentIndex];
00379                         w=scanmatch->weights[currentIndex]-oldWeight;
00380                         oldWeight=scanmatch->weights[currentIndex];
00381                         
00382                         if (!rawodom) {
00383                           rl.push_front(pose);
00384                         }
00385                 }
00386                 const OdometryRecord* odometry=dynamic_cast<const OdometryRecord*>(*it);
00387                 if (odometry){
00388                   PoseRecord* pose=new PoseRecord;
00389                   pose->dim=0;
00390                   p=pose->pose=odometry->poses[currentIndex];
00391                   pose->time=odometry->time;
00392                   if (!rawodom) {
00393                     rl.push_front(pose);
00394                   }
00395                 }
00396                 const RawOdometryRecord* rawodometry=dynamic_cast<const RawOdometryRecord*>(*it);
00397                 if (rawodometry){
00398                   PoseRecord* pose=new PoseRecord;
00399                   pose->dim=0;
00400                   pose->pose=rawodometry->pose;
00401                   pose->time=rawodometry->time;
00402                   if (rawodom) {
00403                     rl.push_front(pose);
00404                   }
00405                 }
00406                 const PoseRecord* tpose=dynamic_cast<const PoseRecord*>(*it);
00407                 if (tpose){
00408                         PoseRecord* pose=new PoseRecord(*tpose);
00409                         rl.push_front(pose);
00410                 }
00411                 const LaserRecord* laser=dynamic_cast<const LaserRecord*>(*it); 
00412                 if (laser){
00413                         LaserRecord* claser=new LaserRecord(*laser);
00414                         claser->pose=p;
00415                         claser->weight=w;
00416                         rl.push_front(claser);
00417                 }
00418                 const CommentRecord* comment=dynamic_cast<const CommentRecord*>(*it); 
00419                 if (comment){
00420                         CommentRecord* ccomment=new CommentRecord(*comment);
00421                         rl.push_front(ccomment);
00422                 }
00423                 const ResampleRecord* resample=dynamic_cast<const ResampleRecord*>(*it); 
00424                 if (resample){
00425                         rl.push_front(new ResampleRecord(*resample));
00426                         currentIndex=resample->indexes[currentIndex];
00427                 }
00428                 
00429         }
00430         bool started=false;
00431         bool computedTransformation=false;
00432         bool truePosFound=false;
00433         OrientedPoint truePose;
00434         OrientedPoint oldPose;
00435         OrientedPoint trueStart, realStart;
00436         bool tpf=false;
00437         double neff=0;
00438         double totalError=0;
00439         int count=0;
00440         for(RecordList::iterator it=rl.begin(); it!=rl.end(); it++){
00441                 NeffRecord* neffr=dynamic_cast<NeffRecord*>(*it);
00442                 if (neffr)
00443                         neff=neffr->neff/(double)sampleSize;
00444                 started=started || dynamic_cast<const LaserRecord*>(*it)?true:false;
00445                 if (started && ! truePosFound){
00446                         PoseRecord* tpose=dynamic_cast<PoseRecord*>(*it);
00447                         if (tpose && tpose->truePos){
00448                                 truePosFound=true;
00449                                 tpf=true;
00450                                 truePose=tpose->pose;
00451                                 os << "# ";
00452                                 (*it)->write(os);
00453                         }
00454                 }
00455                 if (started && truePosFound && ! computedTransformation){
00456                         PoseRecord* pos=dynamic_cast<PoseRecord*>(*it);
00457                         if (pos && !pos->truePos){
00458                                 trueStart=truePose;
00459                                 realStart=pos->pose;
00460                                 os << "# ";
00461                                 (*it)->write(os);
00462                                 computedTransformation=true;
00463                         }
00464                 }
00465                 if (computedTransformation){
00466                   os << setiosflags(ios::fixed) << setprecision(6);
00467                         PoseRecord* pos=dynamic_cast<PoseRecord*>(*it);
00468                         if (pos){
00469                                 if (pos->truePos){
00470                                         tpf=true;
00471                                         truePose=pos->pose;
00472                                 } else {
00473                                         if (tpf){
00474                                                 tpf=false;
00475                                                 OrientedPoint realDelta=absoluteDifference(pos->pose,realStart);
00476                                                 OrientedPoint trueDelta=absoluteDifference(truePose,trueStart);
00477                                                 double ex=realDelta.x-trueDelta.x;
00478                                                 double ey=realDelta.y-trueDelta.y;
00479                                                 double eth=realDelta.theta-trueDelta.theta;
00480                                                 eth=atan2(sin(eth), cos(eth));
00481                                                 if (! err)
00482                                                         os << "# ERROR ";
00483                                                 os << neff << " "
00484                                                         << ex << " " << ey << " " << eth 
00485                                                         << " " << sqrt(ex*ex+ey*ey) << " " << fabs(eth) << endl;
00486                                                 totalError+=sqrt(ex*ex+ey*ey);
00487                                                 count++;
00488                                         }
00489                                 }
00490                         }
00491                         
00492                 }
00493                 PoseRecord* pos=dynamic_cast<PoseRecord*>(*it);
00494                 if (pos)
00495                         oldPose=pos->pose;
00496                 if (! err)
00497                         (*it)->write(os);
00498                 delete *it;
00499         }
00500         if (err)
00501                 cout << "average error" << totalError/count << endl;
00502 }
00503 
00504 }; //gfsreader
00505 
00506 }; //GMapping;


openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Thu Jun 6 2019 19:25:12