FHWS/Fusion2016
Archived
3
0
Fork 0
Code Issues 1 Pull Requests Releases Wiki Activity

started working on the tex-part

Browse Source 
started working on eval-graphics
ned helper methods
tested some new aspects
some fixes and changes
added some graphics
new test-floorplan
many cleanups
This commit is contained in:
FrankE
2016-02-03 21:17:15 +01:00
parent 8a57b4cdbd
commit c5a5acbbf6
40 changed files with 69163 additions and 275 deletions
Download Patch File Download Diff File Expand all files Collapse all files

111
code/eval/EvalBase.h
View File

@@ -47,9 +47,16 @@ protected:
std::string runName;
GroundTruthWay gtw;
std::vector<int> way0 = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 2, 1, 0};
std::vector<int> way1 = {29, 28, 27, 26, 25, 24, 23, 22, 21, 20, 13, 14, 15, 16, 17, 18, 19, 2, 1, 0};
std::vector<int> way2 = {29, 28, 27, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0, 1, 2, 19, 18, 17, 16, 15, 14, 13, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29};
// OLD
//std::vector<int> way0 = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 2, 1, 0};
//std::vector<int> way1 = {29, 28, 27, 26, 25, 24, 23, 22, 21, 20, 13, 14, 15, 16, 17, 18, 19, 2, 1, 0};
//std::vector<int> way2 = {29, 28, 27, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0, 1, 2, 19, 18, 17, 16, 15, 14, 13, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29};
// NEW
std::vector<int> path1 = {29, 28,27,26,255,25,24,23,22,21,20};
std::vector<int> path2 = {19, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, 8, 23, 7, 6};
std::vector<int> path3 = {5, 27, 26, 255, 25, 4, 3, 2, 215, 1, 0, 30, 31};
public:
@@ -66,13 +73,17 @@ public:
}
static GridPoint conv(const Point3& p) {
return GridPoint(p.x, p.y, p.z);
}
GroundTruthWay getGroundTruthWay(SensorReader& sr, const std::unordered_map<int, Point3>& waypoints, std::vector<int> ids) {
// construct the ground-truth-path by using all contained waypoint ids
std::vector<Point3> path;
for (int id : ids) {
auto it = waypoints.find(id);
assert(it != waypoints.end());
if(it == waypoints.end()) {throw "not found";}
path.push_back(it->second);
}
@@ -138,6 +149,11 @@ public:
// the to-be-evaluated observation
MyObservation obs;
obs.step = new StepObservation(); obs.step->steps = 0;
obs.turn = new TurnObservation(); obs.turn->delta_heading = 0; obs.turn->delta_motion = 0;
// control data
MyControl ctrl;
std::vector<Point3> pathEst;
@@ -148,7 +164,8 @@ public:
K::Statistics<double> stats;
int cnt = 0;
// process each sensor reading
// process each single sensor reading
while(sr->hasNext()) {
// get the next sensor reading from the CSV
@@ -187,58 +204,39 @@ public:
}
// process all occurred turns
while (!step_observations.empty() && current_time > step_observations.front().ts) {
const StepObservation _so = step_observations.front(); step_observations.pop_front(); (void) _so;
obs.step->steps++;
ctrl.walked_m = obs.step->steps * 0.71;
}
// scheduled transition every 500 ms
if (lastTransitionTS == 0) {lastTransitionTS = se.ts;}
for ( ; se.ts - lastTransitionTS > MiscSettings::timeSteps; lastTransitionTS += MiscSettings::timeSteps) {
// process all occurred steps
while (!turn_observations.empty() && current_time > turn_observations.front().ts) {
const TurnObservation _to = turn_observations.front(); turn_observations.pop_front();
obs.turn->delta_heading += _to.delta_heading;
obs.turn->delta_motion += _to.delta_motion;
ctrl.headingChange_rad = Angle::degToRad(obs.turn->delta_heading);
//Steps are sorted in the list by timestamp.
//If the current observation timestamp is bigger/equal
//to the current step timestamp, use this step as observation
//and remove it from the list.
//The new first timestamp in the list will be then be the next one (timestamp-wise)
StepObservation so;
if(current_time >= step_observations.front().ts && !step_observations.empty()) {
so.step = true;
so.ts = current_time;
}
obs.step = &so;
step_observations.pop_front();
}
else {
so.step = false;
so.ts = current_time;
obs.step = &so;
}
TurnObservation to;
//same principal as for steps is applied for turns
if(current_time >= turn_observations.front().ts && !turn_observations.empty()) {
to = turn_observations.front();
obs.turn = &to;
turn_observations.pop_front();
}
else {
to.delta_heading = 0.0;
to.delta_motion = 0.0;
obs.turn = &to;
}
// time for a transition?
if (se.ts - lastTransitionTS > MiscSettings::timeSteps) {
lastTransitionTS = se.ts;
// timed updates
((MyTransition*)pf->getTransition())->setCurrentTime(lastTransitionTS);
// update the particle filter (transition + eval), estimate a new current position and add it to the estimated path
const MyState est = pf->update(nullptr, obs);
const MyState est = pf->update(&ctrl, obs);
const Point3 curEst = est.pCur;
// error calculation. compare ground-truth to estimation
const Point3 curGT = gtw.getPosAtTime(se.ts - 750);
const int offset = 0; // 750
const Point3 curGT = gtw.getPosAtTime(se.ts - offset);
const Point3 diff = curEst - curGT;
// skip the first 8 scans due to uniform distribution start
@@ -251,28 +249,31 @@ public:
// plot
vis.clearStates();
for (const K::Particle<MyState> p : pf->getParticles()) {vis.addState(p.state.walkState);}
for (int i = 0; i < (int) pf->getParticles().size(); i+=15) {
const K::Particle<MyState>& p = pf->getParticles()[i];
vis.addState(p.state.walkState);
}
vis.setTimestamp(se.ts);
vis.addGroundTruth(gtw);
vis.addEstPath(pathEst);
vis.setEstAndShould(curEst, curGT);
vis.show();;
vis.gp << "set label 111 '" <<ctrl.walked_m << ":" << ctrl.headingChange_rad << "' at screen 0.1,0.1\n";
Point2 p1(0.1, 0.1);
Point2 p2 = p1 + Angle::getPointer(ctrl.headingChange_rad) * 0.1;
vis.gp << "set arrow 111 from screen " << p1.x<<","<<p1.y << " to screen " << p2.x<<","<<p2.y<<"\n";
vis.show();
// prevent gnuplot errors
usleep(1000*33);
}
}
{
// vis.setShowParticles(false);
// vis.setShowTime(false);
// vis.setShowCurPos(false);
// vis.debugProcess(0, pathEst, gtw, pf, layers);
// std::ofstream out("/tmp/" + runName + ".data");
// out << vis.getDataset();
// out.close();
}
sleep(1000);
}