X-lanni
407 lines
9.3 KiB
C++
407 lines
9.3 KiB
C++
#include <cstring>
|
|
#include <iostream>
|
|
#include <fstream>
|
|
#include <sstream>
|
|
#include <vector>
|
|
#include <list>
|
|
#include <gmapping/utils/point.h>
|
|
|
|
#define MAX_LINE_LENGHT (1000000)
|
|
|
|
using namespace GMapping;
|
|
using namespace std;
|
|
|
|
struct Record{
|
|
unsigned int dim;
|
|
double time;
|
|
virtual ~Record(){}
|
|
virtual void read(istream& is)=0;
|
|
virtual void write(ostream& os){};
|
|
};
|
|
|
|
struct CommentRecord: public Record{
|
|
string text;
|
|
virtual void read(istream& is){
|
|
char buf[MAX_LINE_LENGHT];
|
|
memset(buf,0, MAX_LINE_LENGHT*sizeof(char));
|
|
is.getline(buf, MAX_LINE_LENGHT);
|
|
text=string(buf);
|
|
}
|
|
virtual void write(ostream& os){
|
|
os << "#GFS_COMMENT: " << text << endl;
|
|
}
|
|
};
|
|
|
|
struct PoseRecord: public Record{
|
|
PoseRecord(bool ideal=false){
|
|
truePos=ideal;
|
|
}
|
|
bool truePos;
|
|
OrientedPoint pose;
|
|
void read(istream& is){
|
|
is >> pose.x >> pose.y >> pose.theta;
|
|
time = 0;
|
|
if (is)
|
|
is >> time;
|
|
}
|
|
virtual void write(ostream& os){
|
|
if (truePos)
|
|
os << "POS-CORR";
|
|
else
|
|
os << "POS ";
|
|
// FIXME os << floor(time) << " " << (int) (time-floor(time)*1e6) << ": ";
|
|
os << "0 0: ";
|
|
os << pose.x*100 << " " << pose.y*100 << " " << 180/M_PI*pose.theta << endl;
|
|
}
|
|
};
|
|
|
|
struct NeffRecord: public Record{
|
|
double neff;
|
|
void read(istream& is){
|
|
is >> neff;
|
|
}
|
|
virtual void write(ostream& os){
|
|
os << "NEFF " << neff << endl;
|
|
}
|
|
};
|
|
|
|
|
|
struct OdometryRecord: public Record{
|
|
vector<OrientedPoint> poses;
|
|
virtual void read(istream& is){
|
|
is >> dim;
|
|
for (unsigned int i=0; i< dim; i++){
|
|
OrientedPoint p;
|
|
double w;
|
|
is >> p.x;
|
|
is >> p.y;
|
|
is >> p.theta;
|
|
is >> w;
|
|
poses.push_back(p);
|
|
}
|
|
time = 0;
|
|
if (is)
|
|
is >> time;
|
|
}
|
|
};
|
|
|
|
|
|
struct ScanMatchRecord: public Record{
|
|
vector<OrientedPoint> poses;
|
|
vector<double> weights;
|
|
virtual void read(istream& is){
|
|
is >> dim;
|
|
for (unsigned int i=0; i< dim; i++){
|
|
OrientedPoint p;
|
|
double w;
|
|
is >> p.x;
|
|
is >> p.y;
|
|
is >> p.theta;
|
|
is >> w;
|
|
poses.push_back(p);
|
|
weights.push_back(w);
|
|
}
|
|
}
|
|
};
|
|
|
|
struct LaserRecord: public Record{
|
|
vector<double> readings;
|
|
OrientedPoint pose;
|
|
virtual void read(istream& is){
|
|
is >> dim;
|
|
for (unsigned int i=0; i< dim; i++){
|
|
double r;
|
|
is >> r;
|
|
readings.push_back(r);
|
|
}
|
|
is >> pose.x;
|
|
is >> pose.y;
|
|
is >> pose.theta;
|
|
time = 0;
|
|
if (is)
|
|
is >> time;
|
|
}
|
|
|
|
// dummy, &sec, &usec, &nLas, &nVal, &range ) == EOF) {
|
|
|
|
virtual void write(ostream& os){
|
|
os << "POS ";
|
|
// FIXME os << floor(time) << " " << (int) (time-floor(time)*1e6) << ": ";
|
|
os << "0 0: ";
|
|
os << pose.x*100 << " " << pose.y*100 << " " << 180/M_PI*pose.theta << endl;
|
|
|
|
os << "LASER-RANGE ";
|
|
// FIXME os << floor(time) << " " << (int) (time-floor(time)*1e6) << ": ";
|
|
os << " 0 0 0 " << dim << " 180. : ";
|
|
for (unsigned int i=0; i< dim; i++){
|
|
os <<" "<< readings[i]*100 ;
|
|
}
|
|
os << endl;
|
|
};
|
|
};
|
|
|
|
struct ResampleRecord: public Record{
|
|
vector<unsigned int> indexes;
|
|
virtual void read(istream& is){
|
|
is >> dim;
|
|
for (unsigned int i=0; i< dim; i++){
|
|
unsigned int j;
|
|
is >> j;
|
|
indexes.push_back(j);
|
|
}
|
|
}
|
|
};
|
|
|
|
struct RecordList: public list<Record*>{
|
|
mutable int sampleSize;
|
|
|
|
istream& read(istream& is){
|
|
while(is){
|
|
char buf[8192];
|
|
is.getline(buf, 8192);
|
|
istringstream lineStream(buf);
|
|
string recordType;
|
|
lineStream >> recordType;
|
|
Record* rec=0;
|
|
if (recordType=="LASER_READING"){
|
|
rec=new LaserRecord;
|
|
cout << "l" << flush;
|
|
}
|
|
if (recordType=="ODO_UPDATE"){
|
|
rec=new OdometryRecord;
|
|
cout << "o" << flush;
|
|
}
|
|
if (recordType=="SM_UPDATE"){
|
|
rec=new ScanMatchRecord;
|
|
cout << "m" << flush;
|
|
}
|
|
if (recordType=="SIMULATOR_POS"){
|
|
rec=new PoseRecord(true);
|
|
cout << "t" << flush;
|
|
}
|
|
if (recordType=="RESAMPLE"){
|
|
rec=new ResampleRecord;
|
|
cout << "r" << flush;
|
|
}
|
|
if (recordType=="NEFF"){
|
|
rec=new NeffRecord;
|
|
cout << "n" << flush;
|
|
}
|
|
if (recordType=="COMMENT"){
|
|
rec=new CommentRecord;
|
|
cout << "c" << flush;
|
|
}
|
|
if (rec){
|
|
rec->read(lineStream);
|
|
push_back(rec);
|
|
}
|
|
}
|
|
return is;
|
|
}
|
|
|
|
double getLogWeight(unsigned int i) const{
|
|
double weight=0;
|
|
unsigned int currentIndex=i;
|
|
for(RecordList::const_reverse_iterator it=rbegin(); it!=rend(); it++){
|
|
ScanMatchRecord* scanmatch=dynamic_cast<ScanMatchRecord*>(*it);
|
|
if (scanmatch){
|
|
weight+=scanmatch->weights[currentIndex];
|
|
}
|
|
ResampleRecord* resample=dynamic_cast<ResampleRecord*>(*it);
|
|
if (resample){
|
|
currentIndex=resample->indexes[currentIndex];
|
|
}
|
|
}
|
|
return weight;
|
|
}
|
|
unsigned int getBestIdx() const {
|
|
if (empty())
|
|
return 0;
|
|
const ScanMatchRecord* scanmatch=0;
|
|
const_reverse_iterator it=rbegin();
|
|
while(!scanmatch){
|
|
scanmatch=dynamic_cast<const ScanMatchRecord*>(*it);
|
|
it++;
|
|
}
|
|
unsigned int dim=scanmatch->dim;
|
|
sampleSize=(int)dim;
|
|
double bestw=-1e200;
|
|
unsigned int best=scanmatch->dim+1;
|
|
for (unsigned i=0; i<dim; i++){
|
|
double w=getLogWeight(i);
|
|
if (w>bestw){
|
|
best=i;
|
|
bestw=w;
|
|
}
|
|
}
|
|
return best;
|
|
}
|
|
|
|
void printPath(ostream& os, unsigned int i, bool err=false) const{
|
|
unsigned int currentIndex=i;
|
|
OrientedPoint p(0,0,0);
|
|
|
|
RecordList rl;
|
|
|
|
//reconstruct a path
|
|
for(RecordList::const_reverse_iterator it=rbegin(); it!=rend(); it++){
|
|
const NeffRecord* neff=dynamic_cast<const NeffRecord*>(*it);
|
|
if (neff){
|
|
NeffRecord* n=new NeffRecord(*neff);
|
|
rl.push_front(n);
|
|
}
|
|
const ScanMatchRecord* scanmatch=dynamic_cast<const ScanMatchRecord*>(*it);
|
|
if (scanmatch){
|
|
PoseRecord* pose=new PoseRecord;
|
|
pose->dim=0;
|
|
p=pose->pose=scanmatch->poses[currentIndex];
|
|
rl.push_front(pose);
|
|
}
|
|
const OdometryRecord* odometry=dynamic_cast<const OdometryRecord*>(*it);
|
|
if (odometry){
|
|
PoseRecord* pose=new PoseRecord;
|
|
pose->dim=0;
|
|
p=pose->pose=odometry->poses[currentIndex];
|
|
pose->time=odometry->time;
|
|
rl.push_front(pose);
|
|
}
|
|
const PoseRecord* tpose=dynamic_cast<const PoseRecord*>(*it);
|
|
if (tpose){
|
|
PoseRecord* pose=new PoseRecord(*tpose);
|
|
rl.push_front(pose);
|
|
}
|
|
const LaserRecord* laser=dynamic_cast<const LaserRecord*>(*it);
|
|
if (laser){
|
|
LaserRecord* claser=new LaserRecord(*laser);
|
|
claser->pose=p;
|
|
rl.push_front(claser);
|
|
}
|
|
const CommentRecord* comment=dynamic_cast<const CommentRecord*>(*it);
|
|
if (comment){
|
|
CommentRecord* ccomment=new CommentRecord(*comment);
|
|
rl.push_front(ccomment);
|
|
}
|
|
const ResampleRecord* resample=dynamic_cast<const ResampleRecord*>(*it);
|
|
if (resample){
|
|
currentIndex=resample->indexes[currentIndex];
|
|
ResampleRecord* r= new ResampleRecord(*resample);
|
|
rl.push_front(r);
|
|
}
|
|
}
|
|
bool started=false;
|
|
double ox=0, oy=0, rxx=0, rxy=0, ryx=0, ryy=0, rth=0;
|
|
bool computedTransformation=false;
|
|
bool truePosFound=false;
|
|
OrientedPoint truePose(0,0,0);
|
|
OrientedPoint currPose(0,0,0);
|
|
bool tpf=false;
|
|
double neff=0;
|
|
unsigned int count=0;
|
|
for(RecordList::iterator it=rl.begin(); it!=rl.end(); it++){
|
|
NeffRecord* neffr=dynamic_cast<NeffRecord*>(*it);
|
|
if (neffr)
|
|
neff=neffr->neff/(double)sampleSize;
|
|
started=started || dynamic_cast<const LaserRecord*>(*it)?true:false;
|
|
if (started && ! truePosFound){
|
|
PoseRecord* tpose=dynamic_cast<PoseRecord*>(*it);
|
|
if (tpose && tpose->truePos){
|
|
truePosFound=true;
|
|
tpf=true;
|
|
truePose=tpose->pose;
|
|
os << "# ";
|
|
(*it)->write(os);
|
|
}
|
|
}
|
|
if (started && truePosFound && ! computedTransformation){
|
|
PoseRecord* pos=dynamic_cast<PoseRecord*>(*it);
|
|
if (pos && !pos->truePos){
|
|
OrientedPoint pose=pos->pose;
|
|
rth=truePose.theta-pose.theta;
|
|
double s=sin(rth), c=cos(rth);
|
|
rxx=ryy=c;
|
|
rxy=-s; ryx=s;
|
|
ox=truePose.x-(rxx*pose.x+rxy*pose.y);
|
|
oy=truePose.y-(ryx*pose.x+ryy*pose.y);
|
|
computedTransformation=true;
|
|
os << "# ";
|
|
(*it)->write(os);
|
|
|
|
}
|
|
}
|
|
ResampleRecord* resample=dynamic_cast<ResampleRecord*>(*it);
|
|
if(resample){
|
|
os << "MARK-POS 0 0: " <<currPose.x*100 << " " << currPose.y*100 << " 0 " << count++ << endl;
|
|
}
|
|
if (computedTransformation){
|
|
PoseRecord* pos=dynamic_cast<PoseRecord*>(*it);
|
|
if (pos){
|
|
if (pos->truePos){
|
|
tpf=true;
|
|
truePose=pos->pose;
|
|
} else {
|
|
if (tpf){
|
|
tpf=false;
|
|
OrientedPoint pose=pos->pose;
|
|
double ex, ey, eth=truePose.theta-pose.theta-rth;
|
|
ex=truePose.x-(ox+rxx*pose.x+rxy*pose.y);
|
|
ey=truePose.y-(oy+ryx*pose.x+ryy*pose.y);
|
|
eth=atan2(sin(eth), cos(eth));
|
|
if (! err)
|
|
os << "# ERROR ";
|
|
os << neff << " "
|
|
<< ex << " " << ey << " " << eth
|
|
<< " " << sqrt(ex*ex+ey*ey) << " " << fabs(eth) << endl;
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|
|
PoseRecord* pos=dynamic_cast<PoseRecord*>(*it);
|
|
if (pos)
|
|
currPose=pos->pose;
|
|
|
|
if (! err)
|
|
(*it)->write(os);
|
|
delete *it;
|
|
}
|
|
}
|
|
};
|
|
|
|
|
|
|
|
int main (int argc, const char * const * argv){
|
|
if (argc<3){
|
|
cout << "usage gfs2rec [-err] <infilename> <outfilename>" << endl;
|
|
return -1;
|
|
}
|
|
bool err=0;
|
|
bool neff=0;
|
|
unsigned int c=1;
|
|
if (!strcmp(argv[c],"-err")){
|
|
err=true;
|
|
c++;
|
|
}
|
|
if (!strcmp(argv[c],"-neff")){
|
|
neff=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);
|
|
os.close();
|
|
return 0;
|
|
}
|