int main(int argc, char ** argv){
if (argc<2){
cerr << "usage "<<argv[0]<<" <filename> <outfilename>" << endl;
cerr << "or "<<argv[0]<<" <filename> for standard output" << endl;
exit (-1);
}
ifstream is(argv[1]);
if (! is){
cerr << "no file " << argv[1] << " found" << endl;
exit (-1);
}
ostream *os;
if (argc<3)
os=&cout;
else{
os=new ofstream(argv[2]);
if (! os){
cerr << "no file " << argv[1] << " found" << endl;
exit (-1);
}
}
CarmenConfiguration conf;
conf.load(is);
SensorMap m=conf.computeSensorMap();
//for (SensorMap::const_iterator it=m.begin(); it!=m.end(); it++)
// cout << it->first << " " << it->second->getName() << endl;
SensorLog log(m);
is.close();
ifstream ls(argv[1]);
log.load(ls);
ls.close();
cerr << "log size" << log.size() << endl;
for (SensorLog::iterator it=log.begin(); it!=log.end(); it++){
RangeReading* rr=dynamic_cast<RangeReading*>(*it);
if (rr){
*os << rr->getSensor()->getName() << " ";
*os << rr->size()<< " ";
for (RangeReading::const_iterator it=rr->begin(); it!=rr->end(); it++){
*os << (*it)*0.001 << " ";
}
*os<< rr->getPose().x*0.001 << " " << rr->getPose().y*0.001 << " " << rr->getPose().theta << endl;
}
}
}
int main (int argc, char** argv)
{
if (argc < 2)
{
cout << "usage log_test <filename>" << endl;
exit (-1);
}
ifstream is (argv[1]);
if (! is)
{
cout << "no file " << argv[1] << " found" << endl;
exit (-1);
}
CarmenConfiguration conf;
conf.load (is);
SensorMap m = conf.computeSensorMap();
//for (SensorMap::const_iterator it=m.begin(); it!=m.end(); it++)
// cout << it->first << " " << it->second->getName() << endl;
SensorLog log (m);
is.close();
ifstream ls (argv[1]);
log.load (ls);
ls.close();
cerr << "log size" << log.size() << endl;
for (SensorLog::iterator it = log.begin(); it != log.end(); it++)
{
RangeReading* rr = dynamic_cast<RangeReading*> (*it);
if (rr)
{
//cerr << rr->getSensor()->getName() << " ";
//cerr << rr->size()<< " ";
//for (RangeReading::const_iterator it=rr->begin(); it!=rr->end(); it++){
// cerr << *it << " ";
//}
cout << rr->getPose().x << " " << rr->getPose().y << " " << rr->getPose().theta << " " << rr->getTime() << endl;
}
}
}
int main(int argc, const char * const * argv) {
string filename;
string outfilename;
double xmin=-100.;
double ymin=-100.;
double xmax=100.;
double ymax=100.;
double delta=1.;
double patchDelta=0.1;
double sigma=0.02;
double maxrange=81.9;
double maxUrange=81.9;
double regscore=1e4;
double lstep=.05;
double astep=.05;
int kernelSize=0;
int iterations=4;
double critscore=0.;
double maxMove=1.;
bool computeCovariance=false;
bool readFromStdin=false;
bool useICP=false;
double laserx=.0,lasery=.0,lasertheta=.0;
// bool headingOnly=false;
if (argc<2) {
cout << "usage main {arglist}" << endl;
cout << "where the arguments are: " << endl;
cout << "\t -xmin <value>" << endl;
cout << "\t -xmax <value>" << endl;
cout << "\t -ymin <value>" << endl;
cout << "\t -ymax <value>" << endl;
cout << "\t -maxrange <value> : maxmimum preception range" << endl;
cout << "\t -delta <value> : patch size" << endl;
cout << "\t -patchDelta <value> : patch cell size" << endl;
cout << "\t -lstep <value> : linear serach step" << endl;
cout << "\t -astep <value> : ìangular search step" << endl;
cout << "\t -regscore <value> : registration scan score" << endl;
cout << "\t -filename <value> : log filename in carmen format" << endl;
cout << "\t -sigma <value> : convolution kernel size" << endl;
cout << "Look the code for discovering another thousand of unuseful parameters" << endl;
return -1;
}
CMD_PARSE_BEGIN(1,argc);
parseString("-filename",filename);
parseString("-outfilename",outfilename);
parseDouble("-xmin",xmin);
parseDouble("-xmax",xmax);
parseDouble("-ymin",ymin);
parseDouble("-ymax",ymax);
parseDouble("-delta",delta);
parseDouble("-patchDelta",patchDelta);
parseDouble("-maxrange",maxrange);
parseDouble("-maxUrange",maxUrange);
parseDouble("-regscore",regscore);
parseDouble("-critscore",critscore);
parseInt("-kernelSize",kernelSize);
parseDouble("-sigma",sigma);
parseInt("-iterations",iterations);
parseDouble("-lstep",lstep);
parseDouble("-astep",astep);
parseDouble("-maxMove",maxMove);
parseFlag("-computeCovariance",computeCovariance);
parseFlag("-stdin", readFromStdin);
parseFlag("-useICP", useICP);
parseDouble("-laserx",laserx);
parseDouble("-lasery",lasery);
parseDouble("-lasertheta",lasertheta);
CMD_PARSE_END;
if (!filename.size()) {
cout << "no filename specified" << endl;
return -1;
}
ifstream is;
is.open(filename.c_str());
if (! is) {
cout << "no file found" << endl;
return -1;
}
DEBUG << "scanmatcher processor construction" << endl;
ScanMatcherProcessor scanmatcher(xmin, ymin, xmax, ymax, delta, patchDelta);
//double range, double sigma, int kernsize, double lopt, double aopt, int iterations
scanmatcher.setMatchingParameters(maxUrange, maxrange, sigma, kernelSize, lstep, astep, iterations, computeCovariance);
scanmatcher.setRegistrationParameters(regscore, critscore);
scanmatcher.setmaxMove(maxMove);
scanmatcher.useICP=useICP;
scanmatcher.matcher().setlaserPose(OrientedPoint(laserx,lasery,lasertheta));
CarmenConfiguration conf;
conf.load(is);
is.close();
SensorMap sensorMap=conf.computeSensorMap();
scanmatcher.setSensorMap(sensorMap);
InputSensorStream* input=0;
ifstream plainStream;
if (! readFromStdin) {
plainStream.open(filename.c_str());
input=new InputSensorStream(sensorMap, plainStream);
cout << "Plain Stream opened="<< (bool) plainStream << endl;
} else {
input=new InputSensorStream(sensorMap, cin);
cout << "Plain Stream opened on stdin" << endl;
}
/*
SensorLog log(sensorMap);
ifstream logstream(filename);
log.load(logstream);
logstream.close();
cout << "Log loaded " << log.size() << " records" << endl;
*/
ostream* output;
ofstream poseStream;
if (! readFromStdin) {
if (! outfilename.size()) {
outfilename=string("scanmatched")+filename;
}
poseStream.open(outfilename.c_str());
output=&poseStream;
} else {
output=&cout;
}
scanmatcher.init();
ofstream odopathStream("odopath.dat");
while (*input) {
const SensorReading* r;
(*input) >> r;
if (! r)
continue;
const RangeReading* rr=dynamic_cast<const RangeReading*>(r);
if (rr) {
const RangeSensor* s=dynamic_cast<const RangeSensor*>(r->getSensor());
bool isFront= s->getPose().theta==0;
if (! readFromStdin) {
cout << "." << flush;
}
const RangeSensor* rs=dynamic_cast<const RangeSensor*>(rr->getSensor());
assert (rs && rs->beams().size()==rr->size());
odopathStream << rr->getPose().x << " " << rr->getPose().y << endl;
scanmatcher.processScan(*rr);
OrientedPoint p=scanmatcher.getPose();
if (isFront)
*output << "FLASER "<< rr->size() << " ";
else
*output << "RLASER "<< rr->size() << " ";
for (RangeReading::const_iterator b=rr->begin(); b!=rr->end(); b++) {
*output << *b << " ";
}
*output << p.x << " " << p.y << " " << p.theta << " ";
//p=rr->getPose();
double t=rr->getTime(); //FIXME
*output << p.x << " " << p.y << " " << p.theta << " ";
*output << t << " nohost " << t << endl;
}
}
if (! readFromStdin) {
poseStream.close();
}
}
