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
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
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
00133
00134
00135
00136
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
00178 double t=rr->getTime();
00179 *output << p.x << " " << p.y << " " << p.theta << " ";
00180 *output << t << " nohost " << t << endl;
00181 }
00182 }
00183 if (! readFromStdin){
00184 poseStream.close();
00185 }
00186 }