int main (int argc, const char * const * argv){
if (argc<3){
cout << "usage gfs2log [-err] [-neff] [-part] [-odom] <infilename> <outfilename>" << endl;
cout << " -odom : dump raw odometry in ODOM message instead of inpolated corrected one" << endl;
return -1;
}
bool err=0;
bool neff=0;
bool part=0;
bool odom=0;
// int particle_num;
unsigned int c=1;
if (!strcmp(argv[c],"-err")){
err=true;
c++;
}
if (!strcmp(argv[c],"-neff")){
neff=true;
c++;
}
if (!strcmp(argv[c],"-part")){
part=true;
c++;
}
if (!strcmp(argv[c],"-odom")){
odom=true;
c++;
}
ifstream is(argv[c]);
if (!is){
cout << "could read file "<< endl;
return -1;
}
c++;
RecordList rl;
rl.read(is);
unsigned int bestidx=rl.getBestIdx();
cout << endl << "best index = " << bestidx<< endl;
ofstream os(argv[c]);
if (! os){
cout << "could write file "<< endl;
return -1;
}
rl.printPath(os,bestidx,err,odom);
if(part)
rl.printLastParticles(os);
os.close();
return 0;
}
int main(int argc, char ** argv) {
if (argc<2) {
cout << "usage gfs2stat <infilename> <outfilename>" << endl;
return 0;
}
ifstream is(argv[1]);
if (!is) {
cout << "no file found: " << argv[1] << endl;
return 0;
}
ofstream os(argv[2]);
if (!os) {
cout << "cannot open file: " << argv[1] << endl;
return 0;
}
cout << "loading... "<< flush;
RecordList rl;
rl.read(is);
cout << " done" << endl;
int count=-1;
for (RecordList::const_iterator it=rl.begin(); it!=rl.end(); it++) {
count++;
const ScanMatchRecord* rec=dynamic_cast<const ScanMatchRecord*>(*it);
if (!rec)
continue;
Gaussian3 gaussian;
/*
vector<double> nweights;
cout << "N"<< flush;
back_insert_iterator< vector<double> > out(nweights);
toNormalForm(out,rec->weights.begin(), rec->weights.end());
cout << "G"<< flush;
gaussian.computeFromSamples(rec->poses, nweights);
*/
gaussian.computeFromSamples(rec->poses);
cout << "E"<< flush;
os << count <<" ";
os << gaussian.mean.x <<" ";
os << gaussian.mean.y <<" ";
os << gaussian.mean.theta <<" ";
os << gaussian.covariance.eval[0] <<" ";
os << gaussian.covariance.eval[1] <<" ";
os << gaussian.covariance.eval[2] <<endl;
}
os.close();
}
int main (int argc, char** argv)
{
QApplication app (argc, argv);
double maxrange = 50;
double delta = 0.1;
int scanSkip = 5;
const char* filename = 0;
const char* format = "PNG";
CMD_PARSE_BEGIN (1, argc)
parseDouble ("-maxrange", maxrange);
parseDouble ("-delta", delta);
parseInt ("-skip", scanSkip);
parseString ("-filename", filename);
parseString ("-format", format);
CMD_PARSE_END
double maxUrange = maxrange;
if (! filename)
{
cout << " supply a gfs file, please" << endl;
cout << " usage gfs2img [options] -filename <gfs_file>" << endl;
cout << " [options]:" << endl;
cout << " -maxrange <range>" << endl;
cout << " -delta <map cell size>" << endl;
cout << " -skip <frames to skip among images>" << endl;
cout << " -format <image format in capital letters>" << endl;
return -1;
}
ifstream is (filename);
if (!is)
{
cout << " supply an EXISTING gfs file, please" << endl;
return -1;
}
RecordList rl;
rl.read (is);
int particles = 0;
int beams = 0;
for (RecordList::const_iterator it = rl.begin(); it != rl.end(); it++)
{
const OdometryRecord* odometry = dynamic_cast<const OdometryRecord*> (*it);
if (odometry)
{
particles = odometry->dim;
}
const LaserRecord* s = dynamic_cast<const LaserRecord*> (*it);
if (s)
{
beams = s->readings.size();
}
if (particles && beams)
break;
}
cout << "Particles from gfs=" << particles << endl;
if (! particles)
{
cout << "no particles found, terminating" << endl;
return -1;
}
cout << "Laser beams from gfs=" << beams << endl;
if (! beams)
{
cout << "0 beams found, terminating" << endl;
return -1;
}
double laserBeamStep = 0;
if (beams == 180 || beams == 181)
{
laserBeamStep = M_PI / 180;
}
else if (beams == 360 || beams == 361)
{
laserBeamStep = M_PI / 360;
}
cout << "Laser beam step" << laserBeamStep << endl;
if (laserBeamStep == 0)
{
cout << "Invalid Beam Step, terminating" << endl;
return -1;
}
double laserAngles[MAX_LASER_BEAMS];
double theta = -M_PI / 2;
for (int i = 0; i < beams; i++)
{
laserAngles[i] = theta;
theta += laserBeamStep;
}
ScanMatcher matcher;
matcher.setLaserParameters (beams, laserAngles, OrientedPoint (0, 0, 0));
matcher.setlaserMaxRange (maxrange);
matcher.setusableRange (maxUrange);
matcher.setgenerateMap (true);
double xmin, ymin, xmax, ymax;
cout << "computing bounding box" << endl;
computeBoundingBox (xmin, ymin, xmax, ymax, rl, maxrange);
cout << "DONE" << endl << "BBOX= " << xmin << " " << ymin << " " << xmax << " " << ymax << endl;
Point center;
center.x = (xmin + xmax) / 2.;
center.y = (ymin + ymax) / 2.;
cout << "computing paths" << endl;
unsigned int frame = 0;
int scanCount = 0;
for (RecordList::const_iterator it = rl.begin(); it != rl.end(); it++)
{
const ScanMatchRecord* s = dynamic_cast<const ScanMatchRecord*> (*it);
if (!s)
continue;
scanCount++;
if (scanCount % scanSkip)
continue;
cout << "Frame " << frame << " ";
std::vector<RecordList> paths (particles);
int bestIdx = 0;
double bestWeight = -MAXDOUBLE;
for (int p = 0; p < particles; p++)
{
paths[p] = rl.computePath (p, it);
double w = rl.getLogWeight (p, it);
if (w > bestWeight)
{
bestWeight = w;
bestIdx = p;
}
}
cout << "bestIdx=" << bestIdx << " bestWeight=" << bestWeight << endl;
cout << "computing best map" << endl;
ScanMatcherMap smap (center, xmin, ymin, xmax, ymax, delta);
int count = 0;
for (RecordList::const_iterator mt = paths[bestIdx].begin(); mt != paths[bestIdx].end(); mt++)
{
const LaserRecord* s = dynamic_cast<const LaserRecord*> (*mt);
if (s)
{
double rawreadings[MAX_LASER_BEAMS];
for (uint i = 0; i < s->readings.size(); i++)
rawreadings[i] = s->readings[i];
matcher.invalidateActiveArea();
matcher.computeActiveArea (smap, s->pose, rawreadings);
// matcher.allocActiveArea(smap, s->pose, rawreadings);
matcher.registerScan (smap, s->pose, rawreadings);
count++;
}
}
cout << "DONE " << count << endl;
QPixmap pixmap (smap.getMapSizeX(), smap.getMapSizeY());
pixmap.fill (QColor (200, 200, 255));
QPainter painter (&pixmap);
for (int x = 0; x < smap.getMapSizeX(); x++)
for (int y = 0; y < smap.getMapSizeY(); y++)
{
double v = smap.cell (x, y);
if (v >= 0)
{
int grayValue = 255 - (int) (255.*v);
painter.setPen (QColor (grayValue, grayValue, grayValue));
painter.drawPoint (x, smap.getMapSizeY() - y - 1);
}
}
/*
cout << "painting trajectories" << endl;
for (int p=0; p<particles; p++){
painter.setPen(QColor(Qt::red));
if (p==bestIdx)
continue;
bool first=true;
IntPoint oldPoint(0,0);
for (RecordList::const_iterator mt=paths[p].begin(); mt!=paths[p].end(); mt++){
const LaserRecord* s=dynamic_cast<const LaserRecord*>(*mt);
if (s){
IntPoint ip=smap.world2map(s->pose);
ip.y=smap.getMapSizeY()-ip.y-1;
if (!first){
painter.drawLine( oldPoint.x, oldPoint.y, ip.x, ip.y);
}
oldPoint=ip;
first=false;
}
}
paths[p].destroyReferences();;
}
painter.setPen(QColor(Qt::black));
bool first=true;
IntPoint oldPoint(0,0);
for (RecordList::const_iterator mt=paths[bestIdx].begin(); mt!=paths[bestIdx].end(); mt++){
const LaserRecord* s=dynamic_cast<const LaserRecord*>(*mt);
if (s){
IntPoint ip=smap.world2map(s->pose);
ip.y=smap.getMapSizeY()-ip.y-1;
if (!first){
painter.drawLine( oldPoint.x, oldPoint.y, ip.x, ip.y);
}
oldPoint=ip;
first=false;
}
}
paths[bestIdx].destroyReferences();;
*/
cout << " DONE" << endl;
cout << "writing image" << endl;
QImage img = pixmap.convertToImage();
char ofilename[MAX_FILENAME];
sprintf (ofilename, "%s-%.4d.%s", filename, frame, format);
cout << ofilename << endl;
img.save (QString (ofilename), format, 0);
frame++;
}
cout << "For Cyrill: \"The Evil is Outside\"" << endl;
}