gfs2rec.cpp
Go to the documentation of this file.
00001 #include <cstring>
00002 #include <iostream>
00003 #include <fstream>
00004 #include <sstream>
00005 #include <vector>
00006 #include <list>
00007 #include <gmapping/utils/point.h>
00008 
00009 #define MAX_LINE_LENGHT (1000000)
00010 
00011 using namespace GMapping;
00012 using namespace std;
00013 
00014 struct Record{
00015         unsigned int dim;
00016         double time;
00017         virtual ~Record(){}
00018         virtual void read(istream& is)=0;
00019         virtual void write(ostream& os){};
00020 };
00021 
00022 struct CommentRecord: public Record{
00023         string text;
00024         virtual void read(istream& is){
00025                 char buf[MAX_LINE_LENGHT];
00026                 memset(buf,0, MAX_LINE_LENGHT*sizeof(char));
00027                 is.getline(buf, MAX_LINE_LENGHT);
00028                 text=string(buf);
00029         }
00030         virtual void write(ostream& os){
00031                 os << "#GFS_COMMENT: " << text << endl;
00032         }
00033 };
00034 
00035 struct PoseRecord: public Record{
00036         PoseRecord(bool ideal=false){
00037                 truePos=ideal;
00038         }
00039         bool truePos;
00040         OrientedPoint pose;
00041         void read(istream& is){
00042                 is >> pose.x >> pose.y >> pose.theta;
00043                 time = 0;
00044                 if (is)
00045                         is >> time;
00046         }
00047         virtual void write(ostream& os){
00048                 if (truePos) 
00049                         os << "POS-CORR";
00050                 else
00051                         os << "POS ";
00052                 // FIXME os << floor(time) << " " << (int) (time-floor(time)*1e6) << ": ";
00053                 os << "0 0: ";
00054                 os << pose.x*100 << " " << pose.y*100 << " " << 180/M_PI*pose.theta <<  endl;
00055         }
00056 };
00057 
00058 struct NeffRecord: public Record{
00059         double neff;
00060         void read(istream& is){
00061                 is >> neff;
00062         }
00063         virtual void write(ostream& os){
00064                 os << "NEFF " << neff << endl;
00065         }
00066 };
00067 
00068 
00069 struct OdometryRecord: public Record{
00070         vector<OrientedPoint> poses;
00071         virtual void read(istream& is){
00072                 is >> dim;
00073                 for (unsigned int i=0; i< dim; i++){
00074                         OrientedPoint p;
00075                         double w;
00076                         is >> p.x;
00077                         is >> p.y;
00078                         is >> p.theta;
00079                         is >> w;
00080                         poses.push_back(p);
00081                 }
00082                 time = 0;
00083                 if (is)
00084                         is >> time;
00085         }
00086 };
00087 
00088 
00089 struct ScanMatchRecord: public Record{
00090         vector<OrientedPoint> poses;
00091         vector<double> weights;
00092         virtual void read(istream& is){
00093                 is >> dim;
00094                 for (unsigned int i=0; i< dim; i++){
00095                         OrientedPoint p;
00096                         double w;
00097                         is >> p.x;
00098                         is >> p.y;
00099                         is >> p.theta;
00100                         is >> w;
00101                         poses.push_back(p);
00102                         weights.push_back(w);
00103                 }
00104         }
00105 };
00106 
00107 struct LaserRecord: public Record{
00108         vector<double> readings;
00109         OrientedPoint pose;
00110         virtual void read(istream& is){
00111                 is >> dim;
00112                 for (unsigned int i=0; i< dim; i++){
00113                         double r;
00114                         is >> r;
00115                         readings.push_back(r);
00116                 }
00117                 is >> pose.x;
00118                 is >> pose.y;
00119                 is >> pose.theta;
00120                 time = 0;
00121                 if (is)
00122                         is >> time;
00123         }
00124         
00125         //      dummy, &sec, &usec, &nLas, &nVal, &range ) == EOF) {
00126 
00127         virtual void write(ostream& os){
00128                 os << "POS ";
00129                 // FIXME os << floor(time) << " " << (int) (time-floor(time)*1e6) << ": ";
00130                 os << "0 0: ";
00131                 os << pose.x*100 << " " << pose.y*100 << " " << 180/M_PI*pose.theta <<  endl;
00132                 
00133                 os << "LASER-RANGE ";
00134                 // FIXME os << floor(time) << " " << (int) (time-floor(time)*1e6) << ": ";
00135                 os << " 0 0 0 " << dim << " 180. : ";
00136                 for (unsigned int i=0; i< dim; i++){
00137                         os <<" "<< readings[i]*100 ;
00138                 }
00139                 os << endl;
00140         };
00141 };
00142 
00143 struct ResampleRecord: public Record{
00144         vector<unsigned int> indexes;
00145         virtual void read(istream& is){
00146                 is >> dim;
00147                 for (unsigned int i=0; i< dim; i++){
00148                         unsigned int j;
00149                         is >> j;
00150                         indexes.push_back(j);
00151                 }
00152         }
00153 };
00154 
00155 struct RecordList: public list<Record*>{
00156         mutable int sampleSize;
00157         
00158         istream& read(istream& is){
00159                 while(is){
00160                         char buf[8192];
00161                         is.getline(buf, 8192);
00162                         istringstream lineStream(buf);
00163                         string recordType;
00164                         lineStream >> recordType;
00165                         Record* rec=0;
00166                         if (recordType=="LASER_READING"){
00167                                 rec=new LaserRecord;
00168                                 cout << "l" << flush;
00169                         }
00170                         if (recordType=="ODO_UPDATE"){
00171                                 rec=new OdometryRecord;
00172                                 cout << "o" << flush;
00173                         }
00174                         if (recordType=="SM_UPDATE"){
00175                                 rec=new ScanMatchRecord;
00176                                 cout << "m" << flush;
00177                         }
00178                         if (recordType=="SIMULATOR_POS"){
00179                                 rec=new PoseRecord(true);
00180                                 cout << "t" << flush;
00181                         }
00182                         if (recordType=="RESAMPLE"){
00183                                 rec=new ResampleRecord;
00184                                 cout << "r" << flush;
00185                         }
00186                         if (recordType=="NEFF"){
00187                                 rec=new NeffRecord;
00188                                 cout << "n" << flush;
00189                         }
00190                         if (recordType=="COMMENT"){
00191                                 rec=new CommentRecord;
00192                                 cout << "c" << flush;
00193                         }
00194                         if (rec){
00195                                 rec->read(lineStream);
00196                                 push_back(rec);
00197                         }
00198                 }
00199                 return is;
00200         }
00201         
00202         double getLogWeight(unsigned int i) const{
00203                 double weight=0;
00204                 unsigned int currentIndex=i;
00205                 for(RecordList::const_reverse_iterator it=rbegin(); it!=rend(); it++){
00206                         ScanMatchRecord* scanmatch=dynamic_cast<ScanMatchRecord*>(*it); 
00207                         if (scanmatch){
00208                                 weight+=scanmatch->weights[currentIndex];
00209                         }
00210                         ResampleRecord* resample=dynamic_cast<ResampleRecord*>(*it); 
00211                         if (resample){
00212                                 currentIndex=resample->indexes[currentIndex];
00213                         }
00214                 }
00215                 return weight;
00216         }
00217         unsigned int getBestIdx() const {
00218                 if (empty())
00219                         return 0;
00220                 const ScanMatchRecord* scanmatch=0;
00221                 const_reverse_iterator it=rbegin();
00222                 while(!scanmatch){
00223                         scanmatch=dynamic_cast<const ScanMatchRecord*>(*it); 
00224                         it++;
00225                 }
00226                 unsigned int dim=scanmatch->dim;
00227                 sampleSize=(int)dim;
00228                 double bestw=-1e200;
00229                 unsigned int best=scanmatch->dim+1;
00230                 for (unsigned i=0; i<dim; i++){
00231                         double w=getLogWeight(i);
00232                         if (w>bestw){
00233                                 best=i;
00234                                 bestw=w;
00235                         }
00236                 }
00237                 return best;
00238         }
00239         
00240         void printPath(ostream& os, unsigned int i, bool err=false) const{
00241                 unsigned int currentIndex=i;
00242                 OrientedPoint p(0,0,0);
00243         
00244                 RecordList rl;
00245                 
00246                 //reconstruct a  path
00247                 for(RecordList::const_reverse_iterator it=rbegin(); it!=rend(); it++){
00248                         const NeffRecord* neff=dynamic_cast<const NeffRecord*>(*it);
00249                         if (neff){
00250                                 NeffRecord* n=new NeffRecord(*neff);
00251                                 rl.push_front(n);
00252                         }
00253                         const ScanMatchRecord* scanmatch=dynamic_cast<const ScanMatchRecord*>(*it); 
00254                         if (scanmatch){
00255                                 PoseRecord* pose=new PoseRecord;
00256                                 pose->dim=0;
00257                                 p=pose->pose=scanmatch->poses[currentIndex];
00258                                 rl.push_front(pose);
00259                         }
00260                         const OdometryRecord* odometry=dynamic_cast<const OdometryRecord*>(*it);
00261                         if (odometry){
00262                                 PoseRecord* pose=new PoseRecord;
00263                                 pose->dim=0;
00264                                 p=pose->pose=odometry->poses[currentIndex];
00265                                 pose->time=odometry->time;
00266                                 rl.push_front(pose);
00267                         }
00268                         const PoseRecord* tpose=dynamic_cast<const PoseRecord*>(*it);
00269                         if (tpose){
00270                                 PoseRecord* pose=new PoseRecord(*tpose);
00271                                 rl.push_front(pose);
00272                         }
00273                         const LaserRecord* laser=dynamic_cast<const LaserRecord*>(*it); 
00274                         if (laser){
00275                                 LaserRecord* claser=new LaserRecord(*laser);
00276                                 claser->pose=p;
00277                                 rl.push_front(claser);
00278                         }
00279                         const CommentRecord* comment=dynamic_cast<const CommentRecord*>(*it); 
00280                         if (comment){
00281                                 CommentRecord* ccomment=new CommentRecord(*comment);
00282                                 rl.push_front(ccomment);
00283                         }
00284                         const ResampleRecord* resample=dynamic_cast<const ResampleRecord*>(*it); 
00285                         if (resample){
00286                                 currentIndex=resample->indexes[currentIndex];
00287                                 ResampleRecord* r= new ResampleRecord(*resample);
00288                                 rl.push_front(r);
00289                         }
00290                 }
00291                 bool started=false;
00292                 double ox=0, oy=0, rxx=0, rxy=0, ryx=0, ryy=0, rth=0;
00293                 bool computedTransformation=false;
00294                 bool truePosFound=false;
00295                 OrientedPoint truePose(0,0,0);
00296                 OrientedPoint currPose(0,0,0);
00297                 bool tpf=false;
00298                 double neff=0;
00299                 unsigned int count=0;
00300                 for(RecordList::iterator it=rl.begin(); it!=rl.end(); it++){
00301                         NeffRecord* neffr=dynamic_cast<NeffRecord*>(*it);
00302                         if (neffr)
00303                                 neff=neffr->neff/(double)sampleSize;
00304                         started=started || dynamic_cast<const LaserRecord*>(*it)?true:false;
00305                         if (started && ! truePosFound){
00306                                 PoseRecord* tpose=dynamic_cast<PoseRecord*>(*it);
00307                                 if (tpose && tpose->truePos){
00308                                         truePosFound=true;
00309                                         tpf=true;
00310                                         truePose=tpose->pose;
00311                                         os << "# ";
00312                                         (*it)->write(os);
00313                                 }
00314                         }
00315                         if (started && truePosFound && ! computedTransformation){
00316                                 PoseRecord* pos=dynamic_cast<PoseRecord*>(*it);
00317                                 if (pos && !pos->truePos){
00318                                         OrientedPoint pose=pos->pose;
00319                                         rth=truePose.theta-pose.theta;
00320                                         double s=sin(rth), c=cos(rth);
00321                                         rxx=ryy=c;
00322                                         rxy=-s; ryx=s;
00323                                         ox=truePose.x-(rxx*pose.x+rxy*pose.y);
00324                                         oy=truePose.y-(ryx*pose.x+ryy*pose.y);
00325                                         computedTransformation=true;
00326                                         os << "# ";
00327                                         (*it)->write(os);
00328                                         
00329                                 }
00330                         }
00331                         ResampleRecord* resample=dynamic_cast<ResampleRecord*>(*it);
00332                         if(resample){
00333                                 os << "MARK-POS 0 0: " <<currPose.x*100 << " " << currPose.y*100 << " 0 " << count++ << endl;
00334                         } 
00335                         if (computedTransformation){
00336                                 PoseRecord* pos=dynamic_cast<PoseRecord*>(*it);
00337                                 if (pos){
00338                                         if (pos->truePos){
00339                                                 tpf=true;
00340                                                 truePose=pos->pose;
00341                                         } else {
00342                                                 if (tpf){
00343                                                         tpf=false;
00344                                                         OrientedPoint pose=pos->pose;
00345                                                         double ex, ey, eth=truePose.theta-pose.theta-rth;
00346                                                         ex=truePose.x-(ox+rxx*pose.x+rxy*pose.y);
00347                                                         ey=truePose.y-(oy+ryx*pose.x+ryy*pose.y);
00348                                                         eth=atan2(sin(eth), cos(eth));
00349                                                         if (! err)
00350                                                                 os << "# ERROR ";
00351                                                         os << neff << " "
00352                                                            << ex << " " << ey << " " << eth 
00353                                                            << " " << sqrt(ex*ex+ey*ey) << " " << fabs(eth) << endl;
00354                                                 }
00355                                         }
00356                                 }
00357                                 
00358                         }
00359                         PoseRecord* pos=dynamic_cast<PoseRecord*>(*it);
00360                         if (pos)
00361                                 currPose=pos->pose;
00362                         
00363                         if (! err)
00364                                 (*it)->write(os);
00365                         delete *it;
00366                 }
00367         }
00368 };
00369 
00370 
00371 
00372 int main (int argc, const char * const * argv){
00373         if (argc<3){
00374                 cout << "usage gfs2rec [-err] <infilename> <outfilename>" << endl;
00375                 return -1;
00376         }
00377         bool err=0;
00378         bool neff=0;
00379         unsigned int  c=1;
00380         if (!strcmp(argv[c],"-err")){
00381                 err=true;
00382                 c++;
00383         }
00384         if (!strcmp(argv[c],"-neff")){
00385                 neff=true;
00386                 c++;
00387         }
00388         ifstream is(argv[c]);
00389         if (!is){
00390                 cout << "could read file "<< endl;
00391                 return -1;
00392         }
00393                 c++;
00394         RecordList rl;
00395         rl.read(is);
00396         unsigned int bestidx=rl.getBestIdx();
00397         cout << endl << "best index = " <<  bestidx<< endl;
00398         ofstream os(argv[c]);
00399         if (! os){
00400                 cout << "could write file "<< endl;
00401                 return -1;
00402         }
00403         rl.printPath(os,bestidx,err);
00404         os.close();
00405         return 0;
00406 }


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