worked on grid-walking
worked on grid-generation added helper library for nav-meshes started working on nav meshes
This commit is contained in:
@@ -1,4 +1,4 @@
|
||||
#ifdef WITH_TESTS
|
||||
#ifdef TODO_________WITH_TESTS
|
||||
|
||||
#include "../../Tests.h"
|
||||
|
||||
@@ -9,190 +9,190 @@
|
||||
/** visualize the motionAxis */
|
||||
TEST(MotionDetection, motionAxis) {
|
||||
|
||||
MotionDetection md;
|
||||
MotionDetection md;
|
||||
|
||||
//plot.gp << "set arrow 919 from " << tt.pos.x << "," << tt.pos.y << "," << tt.pos.z << " to "<< tt.pos.x << "," << tt.pos.y << "," << tt.pos.z+1 << "lw 3\n";
|
||||
//plot.gp << "set arrow 919 from " << tt.pos.x << "," << tt.pos.y << "," << tt.pos.z << " to "<< tt.pos.x << "," << tt.pos.y << "," << tt.pos.z+1 << "lw 3\n";
|
||||
|
||||
//Walking with smartphone straight and always parallel to motion axis
|
||||
//std::string filename = getDataFile("motion/straight_potrait.csv");
|
||||
//Walking with smartphone straight and always parallel to motion axis
|
||||
//std::string filename = getDataFile("motion/straight_potrait.csv");
|
||||
|
||||
//straight_landscape_left/right: walking ~40 sec straight and changing every 5 seconds the mode. started with potrait. landscape routed either to left or right.
|
||||
std::string filename = getDataFile("motion/straight_landscape_left.csv");
|
||||
//std::string filename = getDataFile("motion/straight_landscape_right.csv");
|
||||
//straight_landscape_left/right: walking ~40 sec straight and changing every 5 seconds the mode. started with potrait. landscape routed either to left or right.
|
||||
std::string filename = getDataFile("motion/straight_landscape_left.csv");
|
||||
//std::string filename = getDataFile("motion/straight_landscape_right.csv");
|
||||
|
||||
//straight_inturn_landscape: walked straight made a left turn and change the phone to landscape mode during the turn-phase
|
||||
//std::string filename = getDataFile("motion/straight_inturn_landscape.csv");
|
||||
//straight_inturn_landscape: walked straight made a left turn and change the phone to landscape mode during the turn-phase
|
||||
//std::string filename = getDataFile("motion/straight_inturn_landscape.csv");
|
||||
|
||||
//rounds_potrait: walked 3 rounds holding the phone in potrait mode. always making left turns.
|
||||
//std::string filename = getDataFile("motion/rounds_potrait.csv");
|
||||
//rounds_potrait: walked 3 rounds holding the phone in potrait mode. always making left turns.
|
||||
//std::string filename = getDataFile("motion/rounds_potrait.csv");
|
||||
|
||||
//round_landscape: walked 3 rounds holding the phone in landscape mode. always making left turns.
|
||||
//std::string filename = getDataFile("motion/rounds_landscape.csv");
|
||||
//round_landscape: walked 3 rounds holding the phone in landscape mode. always making left turns.
|
||||
//std::string filename = getDataFile("motion/rounds_landscape.csv");
|
||||
|
||||
//round potrait_to_landscape: walked 1 round with potrait, 1 with landscape and again potrait. the mode was change while walking straight not in a turn. always making left turns.
|
||||
//std::string filename = getDataFile("motion/rounds_potrait_to_landscape.csv");
|
||||
//round potrait_to_landscape: walked 1 round with potrait, 1 with landscape and again potrait. the mode was change while walking straight not in a turn. always making left turns.
|
||||
//std::string filename = getDataFile("motion/rounds_potrait_to_landscape.csv");
|
||||
|
||||
//rounds_pocket: had the phone in my jeans pocket screen pointed at my body and the phone was headfirst. pulled it shortly out after 2 rounds and rotated the phone 180° z-wise (screen not showing at me)
|
||||
//std::string filename = getDataFile("motion/rounds_pocket.csv");
|
||||
//rounds_pocket: had the phone in my jeans pocket screen pointed at my body and the phone was headfirst. pulled it shortly out after 2 rounds and rotated the phone 180° z-wise (screen not showing at me)
|
||||
//std::string filename = getDataFile("motion/rounds_pocket.csv");
|
||||
|
||||
//table_flat: phone was flat on the table and moved slowly forward/backward for 60 cm.
|
||||
//std::string filename = getDataFile("motion/table_flat.csv");
|
||||
//table_flat: phone was flat on the table and moved slowly forward/backward for 60 cm.
|
||||
//std::string filename = getDataFile("motion/table_flat.csv");
|
||||
|
||||
Offline::FileReader fr(filename);
|
||||
|
||||
K::Gnuplot gp;
|
||||
K::GnuplotPlot plot;
|
||||
K::Gnuplot gp;
|
||||
K::GnuplotPlot plot;
|
||||
|
||||
gp << "set xrange[-1:1]\n set yrange[-1:1]\n";
|
||||
gp << "set xrange[-1:1]\n set yrange[-1:1]\n";
|
||||
|
||||
|
||||
Eigen::Vector2f curVec;
|
||||
float motionAxisAngleRad;
|
||||
Timestamp ts;
|
||||
Timestamp lastTs;
|
||||
Eigen::Vector2f curVec;
|
||||
float motionAxisAngleRad;
|
||||
Timestamp ts;
|
||||
Timestamp lastTs;
|
||||
|
||||
//calc motion axis
|
||||
//calc motion axis
|
||||
for (const Offline::Entry& e : fr.getEntries()) {
|
||||
|
||||
ts = Timestamp::fromMS(e.ts);
|
||||
ts = Timestamp::fromMS(e.ts);
|
||||
|
||||
if (e.type == Offline::Sensor::LIN_ACC) {
|
||||
md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data);
|
||||
md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data);
|
||||
|
||||
} else if (e.type == Offline::Sensor::GRAVITY) {
|
||||
md.addGravity(ts, fr.getGravity()[e.idx].data);
|
||||
curVec = md.getCurrentMotionAxis();
|
||||
motionAxisAngleRad = md.getMotionChangeInRad();
|
||||
}
|
||||
md.addGravity(ts, fr.getGravity()[e.idx].data);
|
||||
curVec = md.getCurrentMotionAxis();
|
||||
motionAxisAngleRad = md.getMotionChangeInRad();
|
||||
}
|
||||
|
||||
// start with the first available timestamp
|
||||
if (lastTs.isZero()) {lastTs = ts;}
|
||||
// start with the first available timestamp
|
||||
if (lastTs.isZero()) {lastTs = ts;}
|
||||
|
||||
if(ts - lastTs > Timestamp::fromMS(500)) {
|
||||
if(ts - lastTs > Timestamp::fromMS(500)) {
|
||||
|
||||
lastTs = ts;
|
||||
lastTs = ts;
|
||||
|
||||
K::GnuplotPoint2 raw_p1(0, 0);
|
||||
K::GnuplotPoint2 raw_p2(curVec(0,0), curVec(1,0));
|
||||
K::GnuplotPlotElementLines motionLines;
|
||||
motionLines.addSegment(raw_p1, raw_p2);
|
||||
plot.add(&motionLines);
|
||||
K::GnuplotPoint2 raw_p1(0, 0);
|
||||
K::GnuplotPoint2 raw_p2(curVec(0,0), curVec(1,0));
|
||||
K::GnuplotPlotElementLines motionLines;
|
||||
motionLines.addSegment(raw_p1, raw_p2);
|
||||
plot.add(&motionLines);
|
||||
|
||||
gp << "set label 111 ' Angle: " << motionAxisAngleRad * 180 / 3.14159 << "' at screen 0.1,0.1\n";
|
||||
gp << "set label 111 ' Angle: " << motionAxisAngleRad * 180 / 3.14159 << "' at screen 0.1,0.1\n";
|
||||
|
||||
gp.draw(plot);
|
||||
gp.flush();
|
||||
//usleep(5000*33);
|
||||
}
|
||||
}
|
||||
gp.draw(plot);
|
||||
gp.flush();
|
||||
//usleep(5000*33);
|
||||
}
|
||||
}
|
||||
|
||||
//was passiert bei grenzwerten. 90° oder sowas.
|
||||
//wie stabil ist die motion axis eigentlich?
|
||||
//erkenn wir aktuell überhaupt einen turn, wenn wir das telefon drehen?
|
||||
//wie hilft mir die motion achse? über einen faktor? in welchem verhältnis stehen motion axis und heading?
|
||||
//was passiert bei grenzwerten. 90° oder sowas.
|
||||
//wie stabil ist die motion axis eigentlich?
|
||||
//erkenn wir aktuell überhaupt einen turn, wenn wir das telefon drehen?
|
||||
//wie hilft mir die motion achse? über einen faktor? in welchem verhältnis stehen motion axis und heading?
|
||||
|
||||
}
|
||||
|
||||
/** comparing motionAngle and turnAngle */
|
||||
TEST(MotionDetection, motionAngle) {
|
||||
|
||||
MotionDetection md;
|
||||
TurnDetection td;
|
||||
MotionDetection md;
|
||||
TurnDetection td;
|
||||
|
||||
//plot.gp << "set arrow 919 from " << tt.pos.x << "," << tt.pos.y << "," << tt.pos.z << " to "<< tt.pos.x << "," << tt.pos.y << "," << tt.pos.z+1 << "lw 3\n";
|
||||
//plot.gp << "set arrow 919 from " << tt.pos.x << "," << tt.pos.y << "," << tt.pos.z << " to "<< tt.pos.x << "," << tt.pos.y << "," << tt.pos.z+1 << "lw 3\n";
|
||||
|
||||
//Walking with smartphone straight and always parallel to motion axis
|
||||
std::string filename = getDataFile("motion/straight_potrait.csv");
|
||||
//Walking with smartphone straight and always parallel to motion axis
|
||||
std::string filename = getDataFile("motion/straight_potrait.csv");
|
||||
|
||||
//straight_landscape_left/right: walking ~40 sec straight and changing every 5 seconds the mode. started with potrait. landscape routed either to left or right.
|
||||
//std::string filename = getDataFile("motion/straight_landscape_left.csv");
|
||||
//std::string filename = getDataFile("motion/straight_landscape_right.csv");
|
||||
//straight_landscape_left/right: walking ~40 sec straight and changing every 5 seconds the mode. started with potrait. landscape routed either to left or right.
|
||||
//std::string filename = getDataFile("motion/straight_landscape_left.csv");
|
||||
//std::string filename = getDataFile("motion/straight_landscape_right.csv");
|
||||
|
||||
//straight_inturn_landscape: walked straight made a left turn and change the phone to landscape mode during the turn-phase
|
||||
//std::string filename = getDataFile("motion/straight_inturn_landscape.csv");
|
||||
//straight_inturn_landscape: walked straight made a left turn and change the phone to landscape mode during the turn-phase
|
||||
//std::string filename = getDataFile("motion/straight_inturn_landscape.csv");
|
||||
|
||||
//rounds_potrait: walked 3 rounds holding the phone in potrait mode. always making left turns.
|
||||
//std::string filename = getDataFile("motion/rounds_potrait.csv");
|
||||
//rounds_potrait: walked 3 rounds holding the phone in potrait mode. always making left turns.
|
||||
//std::string filename = getDataFile("motion/rounds_potrait.csv");
|
||||
|
||||
//round_landscape: walked 3 rounds holding the phone in landscape mode. always making left turns.
|
||||
//std::string filename = getDataFile("motion/rounds_landscape.csv");
|
||||
//round_landscape: walked 3 rounds holding the phone in landscape mode. always making left turns.
|
||||
//std::string filename = getDataFile("motion/rounds_landscape.csv");
|
||||
|
||||
//round potrait_to_landscape: walked 1 round with potrait, 1 with landscape and again potrait. the mode was change while walking straight not in a turn. always making left turns.
|
||||
//std::string filename = getDataFile("motion/rounds_potrait_to_landscape.csv");
|
||||
//round potrait_to_landscape: walked 1 round with potrait, 1 with landscape and again potrait. the mode was change while walking straight not in a turn. always making left turns.
|
||||
//std::string filename = getDataFile("motion/rounds_potrait_to_landscape.csv");
|
||||
|
||||
//rounds_pocket: had the phone in my jeans pocket screen pointed at my body and the phone was headfirst. pulled it shortly out after 2 rounds and rotated the phone 180° z-wise (screen not showing at me)
|
||||
//std::string filename = getDataFile("motion/rounds_pocket.csv");
|
||||
//rounds_pocket: had the phone in my jeans pocket screen pointed at my body and the phone was headfirst. pulled it shortly out after 2 rounds and rotated the phone 180° z-wise (screen not showing at me)
|
||||
//std::string filename = getDataFile("motion/rounds_pocket.csv");
|
||||
|
||||
//table_flat: phone was flat on the table and moved slowly forward/backward for 60 cm.
|
||||
//std::string filename = getDataFile("motion/table_flat.csv");
|
||||
//table_flat: phone was flat on the table and moved slowly forward/backward for 60 cm.
|
||||
//std::string filename = getDataFile("motion/table_flat.csv");
|
||||
|
||||
Offline::FileReader fr(filename);
|
||||
Timestamp ts;
|
||||
Timestamp ts;
|
||||
|
||||
//save for later plotting
|
||||
std::vector<float> delta_motionAngles;
|
||||
std::vector<float> delta_turnAngles;
|
||||
//save for later plotting
|
||||
std::vector<float> delta_motionAngles;
|
||||
std::vector<float> delta_turnAngles;
|
||||
|
||||
//calc motion axis
|
||||
//calc motion axis
|
||||
for (const Offline::Entry& e : fr.getEntries()) {
|
||||
|
||||
ts = Timestamp::fromMS(e.ts);
|
||||
ts = Timestamp::fromMS(e.ts);
|
||||
|
||||
if (e.type == Offline::Sensor::LIN_ACC) {
|
||||
md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data);
|
||||
md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data);
|
||||
|
||||
} else if (e.type == Offline::Sensor::GRAVITY) {
|
||||
md.addGravity(ts, fr.getGravity()[e.idx].data);
|
||||
delta_motionAngles.push_back(md.getMotionChangeInRad());
|
||||
md.addGravity(ts, fr.getGravity()[e.idx].data);
|
||||
delta_motionAngles.push_back(md.getMotionChangeInRad());
|
||||
|
||||
} else if (e.type == Offline::Sensor::ACC) {
|
||||
const Offline::TS<AccelerometerData>& _acc = fr.getAccelerometer()[e.idx];
|
||||
td.addAccelerometer(ts, _acc.data);
|
||||
td.addAccelerometer(ts, _acc.data);
|
||||
|
||||
} else if (e.type == Offline::Sensor::GYRO) {
|
||||
const Offline::TS<GyroscopeData>& _gyr = fr.getGyroscope()[e.idx];
|
||||
delta_turnAngles.push_back(td.addGyroscope(ts, _gyr.data));
|
||||
}
|
||||
delta_turnAngles.push_back(td.addGyroscope(ts, _gyr.data));
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
//draw motion
|
||||
static K::Gnuplot gpMotion;
|
||||
K::GnuplotPlot plotMotion;
|
||||
K::GnuplotPlotElementLines motionLines;
|
||||
//draw motion
|
||||
static K::Gnuplot gpMotion;
|
||||
K::GnuplotPlot plotMotion;
|
||||
K::GnuplotPlotElementLines motionLines;
|
||||
|
||||
for(int i = 0; i < delta_motionAngles.size() - 1; ++i){
|
||||
for(int i = 0; i < delta_motionAngles.size() - 1; ++i){
|
||||
|
||||
K::GnuplotPoint2 raw_p1(i, delta_motionAngles[i]);
|
||||
K::GnuplotPoint2 raw_p2(i + 1, delta_motionAngles[i+1]);
|
||||
motionLines.addSegment(raw_p1, raw_p2);
|
||||
K::GnuplotPoint2 raw_p1(i, delta_motionAngles[i]);
|
||||
K::GnuplotPoint2 raw_p2(i + 1, delta_motionAngles[i+1]);
|
||||
motionLines.addSegment(raw_p1, raw_p2);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
gpMotion << "set title 'Motion Detection'\n";
|
||||
plotMotion.add(&motionLines);
|
||||
gpMotion.draw(plotMotion);
|
||||
gpMotion.flush();
|
||||
gpMotion << "set title 'Motion Detection'\n";
|
||||
plotMotion.add(&motionLines);
|
||||
gpMotion.draw(plotMotion);
|
||||
gpMotion.flush();
|
||||
|
||||
|
||||
//draw rotation
|
||||
static K::Gnuplot gpTurn;
|
||||
K::GnuplotPlot plotTurn;
|
||||
K::GnuplotPlotElementLines turnLines;
|
||||
//draw rotation
|
||||
static K::Gnuplot gpTurn;
|
||||
K::GnuplotPlot plotTurn;
|
||||
K::GnuplotPlotElementLines turnLines;
|
||||
|
||||
for(int i = 0; i < delta_turnAngles.size() - 1; ++i){
|
||||
for(int i = 0; i < delta_turnAngles.size() - 1; ++i){
|
||||
|
||||
K::GnuplotPoint2 raw_p1(i, delta_turnAngles[i]);
|
||||
K::GnuplotPoint2 raw_p2(i + 1, delta_turnAngles[i+1]);
|
||||
turnLines.addSegment(raw_p1, raw_p2);
|
||||
}
|
||||
K::GnuplotPoint2 raw_p1(i, delta_turnAngles[i]);
|
||||
K::GnuplotPoint2 raw_p2(i + 1, delta_turnAngles[i+1]);
|
||||
turnLines.addSegment(raw_p1, raw_p2);
|
||||
}
|
||||
|
||||
gpTurn << "set title 'Turn Detection'\n";
|
||||
plotTurn.add(&turnLines);
|
||||
gpTurn.draw(plotTurn);
|
||||
gpTurn.flush();
|
||||
gpTurn << "set title 'Turn Detection'\n";
|
||||
plotTurn.add(&turnLines);
|
||||
gpTurn.draw(plotTurn);
|
||||
gpTurn.flush();
|
||||
|
||||
sleep(1);
|
||||
sleep(1);
|
||||
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user