scanmatch_test.cpp
Go to the documentation of this file.
00001 #include <cstdlib>
00002 #include <fstream>
00003 #include <iostream>
00004 #include <log/carmenconfiguration.h>
00005 #include <log/sensorlog.h>
00006 #include <unistd.h>
00007 #include <utils/commandline.h>
00008 #include <log/sensorstream.h>
00009 #include "scanmatcherprocessor.h"
00010 
00011 using namespace std;
00012 using namespace GMapping;
00013 
00014 #define DEBUG cout << __PRETTY_FUNCTION__
00015 #define MAX_STRING_LENGTH 1024
00016 
00017 int main(int argc, const char * const * argv){
00018         string filename;
00019         string outfilename;
00020         double xmin=-100.;
00021         double ymin=-100.;
00022         double xmax=100.;
00023         double ymax=100.;
00024         double delta=1.;
00025         double patchDelta=0.1;
00026         double sigma=0.02;
00027         double maxrange=81.9;
00028         double maxUrange=81.9;
00029         double regscore=1e4;
00030         double lstep=.05;
00031         double astep=.05;
00032         int kernelSize=0;
00033         int iterations=4;
00034         double critscore=0.;
00035         double maxMove=1.;
00036         bool computeCovariance=false;
00037         bool readFromStdin=false;
00038         bool useICP=false;
00039         double laserx=.0,lasery=.0,lasertheta=.0;
00040 //      bool headingOnly=false;
00041 
00042 
00043         if (argc<2){
00044                 cout << "usage main {arglist}" << endl;
00045                 cout << "where the arguments are: " << endl;
00046                 cout << "\t -xmin     <value>" << endl;
00047                 cout << "\t -xmax     <value>" << endl;
00048                 cout << "\t -ymin     <value>" << endl;
00049                 cout << "\t -ymax     <value>" << endl;
00050                 cout << "\t -maxrange <value>   : maxmimum preception range" << endl;
00051                 cout << "\t -delta    <value>   : patch size" << endl;
00052                 cout << "\t -patchDelta <value> : patch cell size" << endl;
00053                 cout << "\t -lstep    <value> : linear serach step" << endl;
00054                 cout << "\t -astep    <value> : ìangular search step" << endl;
00055                 cout << "\t -regscore <value> : registration scan score" << endl;
00056                 cout << "\t -filename <value> : log filename in carmen format" << endl;
00057                 cout << "\t -sigma    <value> : convolution kernel size" << endl;
00058                 cout << "Look the code for discovering another thousand of unuseful parameters" << endl;
00059                 return -1;
00060         }
00061 
00062         CMD_PARSE_BEGIN(1,argc);
00063                 parseString("-filename",filename);
00064                 parseString("-outfilename",outfilename);
00065                 parseDouble("-xmin",xmin);
00066                 parseDouble("-xmax",xmax);
00067                 parseDouble("-ymin",ymin);
00068                 parseDouble("-ymax",ymax);
00069                 parseDouble("-delta",delta);
00070                 parseDouble("-patchDelta",patchDelta);
00071                 parseDouble("-maxrange",maxrange);
00072                 parseDouble("-maxUrange",maxUrange);
00073                 parseDouble("-regscore",regscore);
00074                 parseDouble("-critscore",critscore);
00075                 parseInt("-kernelSize",kernelSize);
00076                 parseDouble("-sigma",sigma);
00077                 parseInt("-iterations",iterations);
00078                 parseDouble("-lstep",lstep);
00079                 parseDouble("-astep",astep);
00080                 parseDouble("-maxMove",maxMove);
00081                 parseFlag("-computeCovariance",computeCovariance);
00082                 parseFlag("-stdin", readFromStdin);
00083                 parseFlag("-useICP", useICP);
00084                 parseDouble("-laserx",laserx);
00085                 parseDouble("-lasery",lasery);
00086                 parseDouble("-lasertheta",lasertheta);
00087         CMD_PARSE_END;
00088         
00089         if (!filename.size()){
00090                 cout << "no filename specified" << endl;
00091                 return -1;
00092         }
00093 
00094         ifstream is;
00095         is.open(filename.c_str());
00096         if (! is){
00097                 cout << "no file found" << endl;
00098                 return -1;
00099         }
00100 
00101         
00102         DEBUG << "scanmatcher processor construction" << endl;
00103         ScanMatcherProcessor scanmatcher(xmin, ymin, xmax, ymax, delta, patchDelta);
00104         
00105         //double range, double sigma, int kernsize, double lopt, double aopt, int iterations
00106         scanmatcher.setMatchingParameters(maxUrange, maxrange, sigma, kernelSize, lstep, astep, iterations, computeCovariance);
00107         scanmatcher.setRegistrationParameters(regscore, critscore);
00108         scanmatcher.setmaxMove(maxMove);
00109         scanmatcher.useICP=useICP;
00110         scanmatcher.matcher().setlaserPose(OrientedPoint(laserx,lasery,lasertheta));
00111         
00112         CarmenConfiguration conf;
00113         conf.load(is);
00114         is.close();
00115 
00116         SensorMap sensorMap=conf.computeSensorMap();
00117         scanmatcher.setSensorMap(sensorMap);
00118         
00119         InputSensorStream* input=0;
00120         
00121         ifstream plainStream;
00122         if (! readFromStdin){
00123                 plainStream.open(filename.c_str());
00124                 input=new InputSensorStream(sensorMap, plainStream);
00125                 cout << "Plain Stream opened="<< (bool) plainStream << endl;
00126         } else {
00127                 input=new InputSensorStream(sensorMap, cin);
00128                 cout << "Plain Stream opened on stdin" << endl;
00129         }
00130 
00131 /*      
00132         SensorLog log(sensorMap);
00133         ifstream logstream(filename);
00134         log.load(logstream);
00135         logstream.close();
00136         cout << "Log loaded " << log.size() << " records" << endl; 
00137 */
00138         ostream* output;
00139         ofstream poseStream;
00140         if (! readFromStdin){
00141                 if (! outfilename.size()){
00142                         outfilename=string("scanmatched")+filename;
00143                 }
00144                 poseStream.open(outfilename.c_str());
00145                 output=&poseStream;
00146         } else {
00147                 output=&cout;
00148         }
00149         scanmatcher.init();
00150         ofstream odopathStream("odopath.dat");
00151         while (*input){
00152                 const SensorReading* r;
00153                 (*input) >> r;
00154                 if (! r)
00155                         continue;
00156                 const RangeReading* rr=dynamic_cast<const RangeReading*>(r);
00157                 if (rr){
00158                         const RangeSensor* s=dynamic_cast<const RangeSensor*>(r->getSensor());
00159                         bool isFront= s->getPose().theta==0;
00160                                 
00161                         if (! readFromStdin){
00162                                 cout << "." << flush;
00163                         }
00164                         const RangeSensor* rs=dynamic_cast<const RangeSensor*>(rr->getSensor());
00165                         assert (rs && rs->beams().size()==rr->size());
00166                         odopathStream << rr->getPose().x << " " << rr->getPose().y << endl;
00167                         scanmatcher.processScan(*rr);
00168                         OrientedPoint p=scanmatcher.getPose();
00169                         if (isFront)
00170                                 *output << "FLASER "<< rr->size() << " ";
00171                         else
00172                                 *output << "RLASER "<< rr->size() << " ";
00173                         for (RangeReading::const_iterator b=rr->begin(); b!=rr->end(); b++){
00174                                 *output << *b << " ";
00175                         }
00176                         *output << p.x << " " << p.y << " " << p.theta << " ";
00177                         //p=rr->getPose();
00178                         double t=rr->getTime(); //FIXME
00179                         *output << p.x << " " << p.y << " " << p.theta << " ";
00180                         *output << t << " nohost " << t <<  endl;
00181                 }
00182         }
00183         if (! readFromStdin){
00184                 poseStream.close();
00185         }
00186 }


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