merged
This commit is contained in:
toni
@@ -23,234 +23,234 @@
|
||||
|
||||
TEST(Butterworth, offlineSinus) {
|
||||
|
||||
//input data
|
||||
std::minstd_rand gen;
|
||||
std::uniform_real_distribution<double> noise (-0.1, +0.1);
|
||||
//input data
|
||||
std::minstd_rand gen;
|
||||
std::uniform_real_distribution<double> noise (-0.1, +0.1);
|
||||
|
||||
int size = 1100; //Fs
|
||||
double* input = new double[size];
|
||||
double* output = new double[size];
|
||||
int size = 1100; //Fs
|
||||
double* input = new double[size];
|
||||
double* output = new double[size];
|
||||
|
||||
// 17.5hz sin signal with random noise [-0.1, 0.1]
|
||||
for( int i=0; i < size; ++i ){
|
||||
input[i] = sin(0.1 * i) + noise(gen);
|
||||
}
|
||||
// 17.5hz sin signal with random noise [-0.1, 0.1]
|
||||
for( int i=0; i < size; ++i ){
|
||||
input[i] = sin(0.1 * i) + noise(gen);
|
||||
}
|
||||
|
||||
//butterworth
|
||||
Filter::ButterworthLP<double> butter(size,20,5);
|
||||
butter.stepInitialization(0);
|
||||
butter.filter(input, output, size, 0, true);
|
||||
//butterworth
|
||||
Filter::ButterworthLP<double> butter(size,20,5);
|
||||
butter.stepInitialization(0);
|
||||
butter.filter(input, output, size, 0, true);
|
||||
|
||||
K::Gnuplot gp;
|
||||
K::GnuplotPlot plot;
|
||||
K::GnuplotPlotElementLines linesInput;
|
||||
K::GnuplotPlotElementLines linesOutput;
|
||||
K::Gnuplot gp;
|
||||
K::GnuplotPlot plot;
|
||||
K::GnuplotPlotElementLines linesInput;
|
||||
K::GnuplotPlotElementLines linesOutput;
|
||||
|
||||
for(int i=0; i < size-1; ++i){
|
||||
for(int i=0; i < size-1; ++i){
|
||||
|
||||
K::GnuplotPoint2 input_p1(i, input[i]);
|
||||
K::GnuplotPoint2 input_p2(i+1, input[i+1]);
|
||||
K::GnuplotPoint2 input_p1(i, input[i]);
|
||||
K::GnuplotPoint2 input_p2(i+1, input[i+1]);
|
||||
|
||||
K::GnuplotPoint2 output_p1(i, output[i]);
|
||||
K::GnuplotPoint2 output_p2(i+1, output[i+1]);
|
||||
K::GnuplotPoint2 output_p1(i, output[i]);
|
||||
K::GnuplotPoint2 output_p2(i+1, output[i+1]);
|
||||
|
||||
linesInput.addSegment(input_p1, input_p2);
|
||||
linesOutput.addSegment(output_p1, output_p2);
|
||||
}
|
||||
linesInput.addSegment(input_p1, input_p2);
|
||||
linesOutput.addSegment(output_p1, output_p2);
|
||||
}
|
||||
linesOutput.getStroke().getColor().setHexStr("#00FF00");
|
||||
|
||||
plot.add(&linesInput);
|
||||
plot.add(&linesOutput);
|
||||
plot.add(&linesInput);
|
||||
plot.add(&linesOutput);
|
||||
|
||||
gp.draw(plot);
|
||||
gp.flush();
|
||||
gp.draw(plot);
|
||||
gp.flush();
|
||||
|
||||
sleep(10);
|
||||
sleep(10);
|
||||
}
|
||||
|
||||
TEST(Butterworth, onlineSinus) {
|
||||
|
||||
int size = 1100; //Fs
|
||||
double* input = new double[size];
|
||||
double* output = new double[size];
|
||||
int size = 1100; //Fs
|
||||
double* input = new double[size];
|
||||
double* output = new double[size];
|
||||
|
||||
Filter::ButterworthLP<double> butter(size,20,5);
|
||||
butter.stepInitialization(0);
|
||||
Filter::ButterworthLP<double> butter(size,20,5);
|
||||
butter.stepInitialization(0);
|
||||
|
||||
//input data
|
||||
std::minstd_rand gen;
|
||||
std::uniform_real_distribution<double> noise (-0.1, +0.1);
|
||||
//input data
|
||||
std::minstd_rand gen;
|
||||
std::uniform_real_distribution<double> noise (-0.1, +0.1);
|
||||
|
||||
// 17.5hz sin signal with random noise [-0.1, 0.1]
|
||||
for( int i=0; i < size; ++i ){
|
||||
input[i] = sin(0.1 * i) + noise(gen);
|
||||
// 17.5hz sin signal with random noise [-0.1, 0.1]
|
||||
for( int i=0; i < size; ++i ){
|
||||
input[i] = sin(0.1 * i) + noise(gen);
|
||||
|
||||
output[i] = butter.process(input[i]);
|
||||
}
|
||||
output[i] = butter.process(input[i]);
|
||||
}
|
||||
|
||||
K::Gnuplot gp;
|
||||
K::GnuplotPlot plot;
|
||||
K::GnuplotPlotElementLines linesInput;
|
||||
K::GnuplotPlotElementLines linesOutput;
|
||||
K::Gnuplot gp;
|
||||
K::GnuplotPlot plot;
|
||||
K::GnuplotPlotElementLines linesInput;
|
||||
K::GnuplotPlotElementLines linesOutput;
|
||||
|
||||
for(int i=0; i < size-1; ++i){
|
||||
for(int i=0; i < size-1; ++i){
|
||||
|
||||
K::GnuplotPoint2 input_p1(i, input[i]);
|
||||
K::GnuplotPoint2 input_p2(i+1, input[i+1]);
|
||||
K::GnuplotPoint2 input_p1(i, input[i]);
|
||||
K::GnuplotPoint2 input_p2(i+1, input[i+1]);
|
||||
|
||||
K::GnuplotPoint2 output_p1(i, output[i]);
|
||||
K::GnuplotPoint2 output_p2(i+1, output[i+1]);
|
||||
K::GnuplotPoint2 output_p1(i, output[i]);
|
||||
K::GnuplotPoint2 output_p2(i+1, output[i+1]);
|
||||
|
||||
linesInput.addSegment(input_p1, input_p2);
|
||||
linesOutput.addSegment(output_p1, output_p2);
|
||||
}
|
||||
linesInput.addSegment(input_p1, input_p2);
|
||||
linesOutput.addSegment(output_p1, output_p2);
|
||||
}
|
||||
linesOutput.getStroke().getColor().setHexStr("#00FF00");
|
||||
|
||||
plot.add(&linesInput);
|
||||
plot.add(&linesOutput);
|
||||
plot.add(&linesInput);
|
||||
plot.add(&linesOutput);
|
||||
|
||||
gp.draw(plot);
|
||||
gp.flush();
|
||||
gp.draw(plot);
|
||||
gp.flush();
|
||||
|
||||
sleep(1);
|
||||
sleep(1);
|
||||
}
|
||||
|
||||
TEST(Butterworth, offlineOctaveBaro) {
|
||||
|
||||
|
||||
double* input = new double[100000];
|
||||
double* output = new double[100000];
|
||||
double* input = new double[100000];
|
||||
double* output = new double[100000];
|
||||
|
||||
Interpolator<int, double> interp;
|
||||
Interpolator<int, double> interp;
|
||||
|
||||
//read file
|
||||
std::string line;
|
||||
std::string filename = getDataFile("baro/logfile_UAH_R1_S4_baro.dat");
|
||||
std::ifstream infile(filename);
|
||||
//read file
|
||||
std::string line;
|
||||
std::string filename = getDataFile("baro/logfile_UAH_R1_S4_baro.dat");
|
||||
std::ifstream infile(filename);
|
||||
|
||||
int counter = 0;
|
||||
while (std::getline(infile, line))
|
||||
{
|
||||
std::istringstream iss(line);
|
||||
int ts;
|
||||
double value;
|
||||
int counter = 0;
|
||||
while (std::getline(infile, line))
|
||||
{
|
||||
std::istringstream iss(line);
|
||||
int ts;
|
||||
double value;
|
||||
|
||||
while (iss >> ts >> value) {
|
||||
while (iss >> ts >> value) {
|
||||
|
||||
interp.add(ts, value);
|
||||
interp.add(ts, value);
|
||||
|
||||
while(interp.getMaxKey() > counter*20 ){
|
||||
double interpValue = interp.get(counter*20);
|
||||
while(interp.getMaxKey() > counter*20 ){
|
||||
double interpValue = interp.get(counter*20);
|
||||
|
||||
input[counter] = interpValue;
|
||||
//std::cout << counter*20 << " " << interpValue << " i" << std::endl;
|
||||
++counter;
|
||||
}
|
||||
input[counter] = interpValue;
|
||||
//std::cout << counter*20 << " " << interpValue << " i" << std::endl;
|
||||
++counter;
|
||||
}
|
||||
|
||||
//std::cout << ts << " " << value << " r" << std::endl;
|
||||
//std::cout << ts << " " << value << " r" << std::endl;
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Filter::ButterworthLP<double> butter(50,0.2,2);
|
||||
butter.filter(input, output, counter, 938.15, true);
|
||||
Filter::ButterworthLP<double> butter(50,0.2,2);
|
||||
butter.filter(input, output, counter, 938.15, true);
|
||||
|
||||
K::Gnuplot gp;
|
||||
K::GnuplotPlot plot;
|
||||
K::GnuplotPlotElementLines linesInput;
|
||||
K::GnuplotPlotElementLines linesOutput;
|
||||
K::Gnuplot gp;
|
||||
K::GnuplotPlot plot;
|
||||
K::GnuplotPlotElementLines linesInput;
|
||||
K::GnuplotPlotElementLines linesOutput;
|
||||
|
||||
for(int i=0; i < counter-1; ++i){
|
||||
for(int i=0; i < counter-1; ++i){
|
||||
|
||||
K::GnuplotPoint2 input_p1(i, input[i]);
|
||||
K::GnuplotPoint2 input_p2(i+1, input[i+1]);
|
||||
K::GnuplotPoint2 input_p1(i, input[i]);
|
||||
K::GnuplotPoint2 input_p2(i+1, input[i+1]);
|
||||
|
||||
K::GnuplotPoint2 output_p1(i, output[i]);
|
||||
K::GnuplotPoint2 output_p2(i+1, output[i+1]);
|
||||
K::GnuplotPoint2 output_p1(i, output[i]);
|
||||
K::GnuplotPoint2 output_p2(i+1, output[i+1]);
|
||||
|
||||
linesInput.addSegment(input_p1, input_p2);
|
||||
linesOutput.addSegment(output_p1, output_p2);
|
||||
}
|
||||
linesInput.addSegment(input_p1, input_p2);
|
||||
linesOutput.addSegment(output_p1, output_p2);
|
||||
}
|
||||
linesOutput.getStroke().getColor().setHexStr("#00FF00");
|
||||
|
||||
plot.add(&linesInput);
|
||||
plot.add(&linesOutput);
|
||||
plot.add(&linesInput);
|
||||
plot.add(&linesOutput);
|
||||
|
||||
gp.draw(plot);
|
||||
gp.flush();
|
||||
gp.draw(plot);
|
||||
gp.flush();
|
||||
|
||||
sleep(1);
|
||||
sleep(1);
|
||||
|
||||
}
|
||||
|
||||
TEST(Butterworth, onlineOctaveBaro) {
|
||||
|
||||
std::vector<double> input;
|
||||
std::vector<double> output;
|
||||
std::vector<double> input;
|
||||
std::vector<double> output;
|
||||
|
||||
Interpolator<int, double> interp;
|
||||
Interpolator<int, double> interp;
|
||||
|
||||
Filter::ButterworthLP<double> butter(50,0.02,2);
|
||||
butter.stepInitialization(938.15);
|
||||
Filter::ButterworthLP<double> butter(50,0.02,2);
|
||||
butter.stepInitialization(938.15);
|
||||
|
||||
//read file
|
||||
std::string line;
|
||||
std::string filename = getDataFile("baro/logfile_UAH_R1_S4_baro.dat");
|
||||
std::ifstream infile(filename);
|
||||
//read file
|
||||
std::string line;
|
||||
std::string filename = getDataFile("baro/logfile_UAH_R1_S4_baro.dat");
|
||||
std::ifstream infile(filename);
|
||||
|
||||
int counter = 1;
|
||||
while (std::getline(infile, line))
|
||||
{
|
||||
std::istringstream iss(line);
|
||||
int ts;
|
||||
double value;
|
||||
int counter = 1;
|
||||
while (std::getline(infile, line))
|
||||
{
|
||||
std::istringstream iss(line);
|
||||
int ts;
|
||||
double value;
|
||||
|
||||
while (iss >> ts >> value) {
|
||||
while (iss >> ts >> value) {
|
||||
|
||||
interp.add(ts, value);
|
||||
interp.add(ts, value);
|
||||
|
||||
while(interp.getMaxKey() > counter*20 ){
|
||||
double interpValue = interp.get(counter*20);
|
||||
while(interp.getMaxKey() > counter*20 ){
|
||||
double interpValue = interp.get(counter*20);
|
||||
|
||||
//std::cout << counter*20 << " " << interpValue << " i" << std::endl;
|
||||
//std::cout << counter*20 << " " << interpValue << " i" << std::endl;
|
||||
|
||||
input.push_back(interpValue);
|
||||
input.push_back(interpValue);
|
||||
|
||||
output.push_back(butter.process(interpValue));
|
||||
output.push_back(butter.process(interpValue));
|
||||
|
||||
++counter;
|
||||
}
|
||||
++counter;
|
||||
}
|
||||
|
||||
//std::cout << ts << " " << value << " r" << std::endl;
|
||||
//std::cout << ts << " " << value << " r" << std::endl;
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
K::Gnuplot gp;
|
||||
K::GnuplotPlot plot;
|
||||
K::GnuplotPlotElementLines linesInput;
|
||||
K::GnuplotPlotElementLines linesOutput;
|
||||
K::Gnuplot gp;
|
||||
K::GnuplotPlot plot;
|
||||
K::GnuplotPlotElementLines linesInput;
|
||||
K::GnuplotPlotElementLines linesOutput;
|
||||
|
||||
for(int i=0; i < input.size()-1; ++i){
|
||||
for(int i=0; i < input.size()-1; ++i){
|
||||
|
||||
K::GnuplotPoint2 input_p1(i, input[i]);
|
||||
K::GnuplotPoint2 input_p2(i+1, input[i+1]);
|
||||
K::GnuplotPoint2 input_p1(i, input[i]);
|
||||
K::GnuplotPoint2 input_p2(i+1, input[i+1]);
|
||||
|
||||
K::GnuplotPoint2 output_p1(i, output[i]);
|
||||
K::GnuplotPoint2 output_p2(i+1, output[i+1]);
|
||||
K::GnuplotPoint2 output_p1(i, output[i]);
|
||||
K::GnuplotPoint2 output_p2(i+1, output[i+1]);
|
||||
|
||||
linesInput.addSegment(input_p1, input_p2);
|
||||
linesOutput.addSegment(output_p1, output_p2);
|
||||
}
|
||||
linesInput.addSegment(input_p1, input_p2);
|
||||
linesOutput.addSegment(output_p1, output_p2);
|
||||
}
|
||||
linesOutput.getStroke().getColor().setHexStr("#00FF00");
|
||||
|
||||
plot.add(&linesInput);
|
||||
plot.add(&linesOutput);
|
||||
plot.add(&linesInput);
|
||||
plot.add(&linesOutput);
|
||||
|
||||
gp.draw(plot);
|
||||
gp.flush();
|
||||
gp.draw(plot);
|
||||
gp.flush();
|
||||
|
||||
sleep(1);
|
||||
sleep(1);
|
||||
}
|
||||
|
||||
|
||||
|
||||
101
tests/navMesh/TestNavMeshBenchmark.cpp
Normal file
101
tests/navMesh/TestNavMeshBenchmark.cpp
Normal file
@@ -0,0 +1,101 @@
|
||||
#ifdef WITH_TESTS
|
||||
|
||||
#include "../Tests.h"
|
||||
|
||||
#include "../../navMesh/NavMeshFactory.h"
|
||||
#include "../../navMesh/walk/NavMeshSub.h"
|
||||
using namespace NM;
|
||||
|
||||
TEST(NavMeshBenchmark, benchDraw) {
|
||||
|
||||
Floorplan::IndoorMap map;
|
||||
Floorplan::Floor floor; map.floors.push_back(&floor); floor.atHeight = 0; floor.height = 3;
|
||||
Floorplan::FloorOutlinePolygon outline; floor.outline.push_back(&outline);
|
||||
|
||||
// circle (many triangles)
|
||||
int i = 0;
|
||||
for (float f = 0; f < M_PI*2; f += 0.1) {
|
||||
const float x = std::cos(f) * 10;
|
||||
const float y = std::sin(f) * 10;
|
||||
outline.poly.points.push_back(Point2(x,y));
|
||||
++i;
|
||||
}
|
||||
|
||||
outline.outdoor = false;
|
||||
outline.method = Floorplan::OutlineMethod::ADD;
|
||||
|
||||
NavMeshSettings set;
|
||||
NavMesh<NM::NavMeshTriangle> nm;
|
||||
NavMeshFactory<NM::NavMeshTriangle> fac(&nm, set);
|
||||
fac.build(&map);
|
||||
|
||||
ASSERT_NEAR(-10, nm.getBBox().getMin().x, 0.5);
|
||||
ASSERT_NEAR(-10, nm.getBBox().getMin().y, 0.5);
|
||||
ASSERT_NEAR( 0, nm.getBBox().getMin().z, 0.5);
|
||||
|
||||
ASSERT_NEAR(+10, nm.getBBox().getMax().x, 0.5);
|
||||
ASSERT_NEAR(+10, nm.getBBox().getMax().y, 0.5);
|
||||
ASSERT_NEAR( 0, nm.getBBox().getMax().z, 0.5);
|
||||
|
||||
ASSERT_EQ(45, nm.getNumTriangles());
|
||||
|
||||
|
||||
NavMeshRandom<NM::NavMeshTriangle> rnd = nm.getRandom();
|
||||
|
||||
for (int i = 0; i < 5000*1000; ++i) {
|
||||
NavMeshLocation<NM::NavMeshTriangle> loc = rnd.draw();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
TEST(NavMeshBenchmark, benchSubRegion) {
|
||||
|
||||
Floorplan::IndoorMap map;
|
||||
Floorplan::Floor floor; map.floors.push_back(&floor); floor.atHeight = 0; floor.height = 3;
|
||||
Floorplan::FloorOutlinePolygon outline; floor.outline.push_back(&outline);
|
||||
|
||||
// circle (many triangles)
|
||||
int i = 0;
|
||||
for (float f = 0; f < M_PI*2; f += 0.1) {
|
||||
const float x = std::cos(f) * 10;
|
||||
const float y = std::sin(f) * 10;
|
||||
outline.poly.points.push_back(Point2(x,y));
|
||||
++i;
|
||||
}
|
||||
|
||||
outline.outdoor = false;
|
||||
outline.method = Floorplan::OutlineMethod::ADD;
|
||||
|
||||
NavMeshSettings set;
|
||||
NavMesh<NM::NavMeshTriangle> nm;
|
||||
NavMeshFactory<NM::NavMeshTriangle> fac(&nm, set);
|
||||
fac.build(&map);
|
||||
|
||||
ASSERT_NEAR(-10, nm.getBBox().getMin().x, 0.5);
|
||||
ASSERT_NEAR(-10, nm.getBBox().getMin().y, 0.5);
|
||||
ASSERT_NEAR( 0, nm.getBBox().getMin().z, 0.5);
|
||||
|
||||
ASSERT_NEAR(+10, nm.getBBox().getMax().x, 0.5);
|
||||
ASSERT_NEAR(+10, nm.getBBox().getMax().y, 0.5);
|
||||
ASSERT_NEAR( 0, nm.getBBox().getMax().z, 0.5);
|
||||
|
||||
ASSERT_EQ(45, nm.getNumTriangles());
|
||||
|
||||
std::minstd_rand gen(1337);
|
||||
std::uniform_real_distribution<float> dist(0, M_PI*2);
|
||||
|
||||
for (int i = 0; i < 50000; ++i) {
|
||||
const float f = dist(gen);
|
||||
const float x = std::cos(f) * 9;
|
||||
const float y = std::sin(f) * 9;
|
||||
NavMeshLocation<NM::NavMeshTriangle> loc = nm.getLocation(Point3(x,y,0));
|
||||
NavMeshSub<NM::NavMeshTriangle>(loc, 5);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
40
tests/navMesh/TestNavMeshFactory.cpp
Normal file
40
tests/navMesh/TestNavMeshFactory.cpp
Normal file
@@ -0,0 +1,40 @@
|
||||
#ifdef WITH_TESTS
|
||||
|
||||
#include "../Tests.h"
|
||||
|
||||
#include "../../navMesh/NavMeshFactory.h"
|
||||
using namespace NM;
|
||||
|
||||
TEST(NavMeshFactory, build1) {
|
||||
|
||||
Floorplan::IndoorMap map;
|
||||
Floorplan::Floor floor; map.floors.push_back(&floor); floor.atHeight = 0; floor.height = 3;
|
||||
Floorplan::FloorOutlinePolygon outline; floor.outline.push_back(&outline);
|
||||
outline.poly.points.push_back(Point2(0,0));
|
||||
outline.poly.points.push_back(Point2(10,0));
|
||||
outline.poly.points.push_back(Point2(10,10));
|
||||
outline.poly.points.push_back(Point2(0,10));
|
||||
outline.outdoor = false;
|
||||
outline.method = Floorplan::OutlineMethod::ADD;
|
||||
|
||||
NavMeshSettings set;
|
||||
NavMesh<NM::NavMeshTriangle> nm;
|
||||
NavMeshFactory<NM::NavMeshTriangle> fac(&nm,set);
|
||||
fac.build(&map);
|
||||
|
||||
ASSERT_NEAR(0, nm.getBBox().getMin().x, 0.5);
|
||||
ASSERT_NEAR(0, nm.getBBox().getMin().y, 0.5);
|
||||
ASSERT_NEAR(0, nm.getBBox().getMin().z, 0.5);
|
||||
|
||||
ASSERT_NEAR(10, nm.getBBox().getMax().x, 0.5);
|
||||
ASSERT_NEAR(10, nm.getBBox().getMax().y, 0.5);
|
||||
ASSERT_NEAR( 0, nm.getBBox().getMax().z, 0.5);
|
||||
|
||||
ASSERT_EQ(2, nm.getNumTriangles());
|
||||
|
||||
// ASSERT_EQ(nm.getNeighbor(0,0), nm[1]);
|
||||
// ASSERT_EQ(nm.getNeighbor(1,0), nm[0]);
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
87
tests/navMesh/TestNavMeshSub.cpp
Normal file
87
tests/navMesh/TestNavMeshSub.cpp
Normal file
@@ -0,0 +1,87 @@
|
||||
#ifdef WITH_TESTS
|
||||
|
||||
#include "../Tests.h"
|
||||
|
||||
#include "../../navMesh/NavMeshFactory.h"
|
||||
#include "../../navMesh/walk/NavMeshSub.h"
|
||||
using namespace NM;
|
||||
|
||||
TEST(NavMeshSub, build1) {
|
||||
|
||||
Floorplan::IndoorMap map;
|
||||
Floorplan::Floor floor; map.floors.push_back(&floor); floor.atHeight = 0; floor.height = 3;
|
||||
Floorplan::FloorOutlinePolygon outline; floor.outline.push_back(&outline);
|
||||
outline.poly.points.push_back(Point2(0,0));
|
||||
outline.poly.points.push_back(Point2(10,0));
|
||||
outline.poly.points.push_back(Point2(10,10));
|
||||
outline.poly.points.push_back(Point2(0,10));
|
||||
outline.outdoor = false;
|
||||
outline.method = Floorplan::OutlineMethod::ADD;
|
||||
|
||||
NavMeshSettings set;
|
||||
NavMesh<NM::NavMeshTriangle> nm;
|
||||
NavMeshFactory<NM::NavMeshTriangle> fac(&nm, set);
|
||||
fac.build(&map);
|
||||
|
||||
nm.getLocation(Point3(1,1,0));
|
||||
nm.getLocation(Point3(8,0.2,0));
|
||||
nm.getLocation(Point3(0.2,8,0));
|
||||
nm.getLocation(Point3(4.5,4.5,0));
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
TEST(NavMeshSub, draw) {
|
||||
|
||||
Floorplan::IndoorMap map;
|
||||
Floorplan::Floor floor; map.floors.push_back(&floor); floor.atHeight = 0; floor.height = 3;
|
||||
Floorplan::FloorOutlinePolygon outline; floor.outline.push_back(&outline);
|
||||
outline.outdoor = false;
|
||||
outline.method = Floorplan::OutlineMethod::ADD;
|
||||
|
||||
// circle (many triangles)
|
||||
int i = 0;
|
||||
for (float f = 0; f < M_PI*2; f += 0.1) {
|
||||
const float x = std::cos(f) * 10;
|
||||
const float y = std::sin(f) * 10;
|
||||
outline.poly.points.push_back(Point2(x,y));
|
||||
++i;
|
||||
}
|
||||
|
||||
|
||||
Floorplan::FloorOutlinePolygon remove; floor.outline.push_back(&remove);
|
||||
remove.outdoor = false;
|
||||
remove.method = Floorplan::OutlineMethod::REMOVE;
|
||||
remove.poly.points.push_back(Point2(-2,-2));
|
||||
remove.poly.points.push_back(Point2(+2,-2));
|
||||
remove.poly.points.push_back(Point2(+2,+2));
|
||||
remove.poly.points.push_back(Point2(-2,+2));
|
||||
|
||||
NavMeshSettings set;
|
||||
NavMesh<NM::NavMeshTriangle> nm;
|
||||
NavMeshFactory<NM::NavMeshTriangle> fac(&nm, set);
|
||||
fac.build(&map);
|
||||
|
||||
NavMeshRandom<NM::NavMeshTriangle> rnd = nm.getRandom();
|
||||
|
||||
for (int i = 0; i < 1000; ++i) {
|
||||
NavMeshLocation<NM::NavMeshTriangle> loc = rnd.draw();
|
||||
ASSERT_TRUE(loc.tria->contains(loc.pos));
|
||||
|
||||
NavMeshSub<NM::NavMeshTriangle> sub2(loc, 5);
|
||||
NavMeshRandom<NM::NavMeshTriangle> rnd2 = sub2.getRandom();
|
||||
for (int j = 0; j < 100; ++j) {
|
||||
NavMeshLocation<NM::NavMeshTriangle> loc2 = rnd2.draw();
|
||||
ASSERT_TRUE(loc2.tria->contains(loc2.pos));
|
||||
ASSERT_TRUE(sub2.contains(loc2.pos));
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
36
tests/navMesh/TestNavMeshTriangle.cpp
Normal file
36
tests/navMesh/TestNavMeshTriangle.cpp
Normal file
@@ -0,0 +1,36 @@
|
||||
#ifdef WITH_TESTS
|
||||
|
||||
#include "../Tests.h"
|
||||
|
||||
#include "../../navMesh/NavMeshTriangle.h"
|
||||
using namespace NM;
|
||||
|
||||
TEST(NavMeshTriangle, contains) {
|
||||
|
||||
NavMeshTriangle t1(Point3(0,0,0), Point3(1,0,0), Point3(0,1,0), 1);
|
||||
|
||||
ASSERT_TRUE(t1.contains(Point3(0,0,0)));
|
||||
ASSERT_TRUE(t1.contains(Point3(1,0,0)));
|
||||
ASSERT_TRUE(t1.contains(Point3(0,1,0)));
|
||||
ASSERT_TRUE(t1.contains(Point3(0.5,0.5,0)));
|
||||
|
||||
ASSERT_FALSE(t1.contains(Point3(0.501,0.5,0)));
|
||||
ASSERT_FALSE(t1.contains(Point3(0.5,0.501,0)));
|
||||
ASSERT_FALSE(t1.contains(Point3(1,1,0)));
|
||||
|
||||
}
|
||||
|
||||
TEST(NavMeshTriangle, area) {
|
||||
|
||||
NavMeshTriangle t1(Point3(0,0,0), Point3(1,0,0), Point3(0,1,0), 1);
|
||||
ASSERT_NEAR(0.5, t1.getArea(), 0.0001);
|
||||
|
||||
NavMeshTriangle t2(Point3(0,0,9), Point3(1,0,9), Point3(0,1,9), 1);
|
||||
ASSERT_NEAR(0.5, t2.getArea(), 0.0001);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
@@ -1,4 +1,4 @@
|
||||
#ifdef WITH_TESTS
|
||||
#ifdef TODO_________WITH_TESTS
|
||||
|
||||
#include "../../Tests.h"
|
||||
|
||||
@@ -9,191 +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;
|
||||
PoseDetection pd;
|
||||
TurnDetection td(&pd);
|
||||
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];
|
||||
pd.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);
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -6,34 +6,33 @@
|
||||
|
||||
TEST(TurnDetection, rotationMatrix) {
|
||||
|
||||
Vector3 dst(0, 0, 1);
|
||||
Vector3 src(1, 1, 0);
|
||||
src = src.normalized();
|
||||
Vector3 dst(0, 0, 1);
|
||||
Vector3 src(1, 1, 0); src = src.normalized();
|
||||
|
||||
// get a matrix that rotates "src" into "dst"
|
||||
Matrix3 rot = PoseDetection::getRotationMatrix(src, dst);
|
||||
Matrix3 rot = PoseDetection::getRotationMatrix(src, dst);
|
||||
|
||||
Vector3 res = rot * src;
|
||||
Vector3 res = rot * src;
|
||||
|
||||
ASSERT_NEAR(dst.x, res.x, 0.01);
|
||||
ASSERT_NEAR(dst.y, res.y, 0.01);
|
||||
ASSERT_NEAR(dst.z, res.z, 0.01);
|
||||
ASSERT_NEAR(dst.x, res.x, 0.01);
|
||||
ASSERT_NEAR(dst.y, res.y, 0.01);
|
||||
ASSERT_NEAR(dst.z, res.z, 0.01);
|
||||
|
||||
}
|
||||
|
||||
TEST(TurnDetection, gyroRotate) {
|
||||
|
||||
|
||||
Vector3 zAxis(0, 0, 1);
|
||||
Vector3 acc(0, 7.0, 7.0);
|
||||
Vector3 zAxis(0, 0, 1);
|
||||
Vector3 acc(0, 7.0, 7.0);
|
||||
|
||||
Matrix3 rot = PoseDetection::getRotationMatrix(acc, zAxis);
|
||||
Matrix3 rot = PoseDetection::getRotationMatrix(acc, zAxis);
|
||||
|
||||
Vector3 gyro(0, 60, 60);
|
||||
Vector3 gyro(0, 60, 60);
|
||||
|
||||
Vector3 gyro2(0, 0, 84);
|
||||
Vector3 gyro2(0, 0, 84);
|
||||
|
||||
Vector3 gyro3 = rot * gyro;
|
||||
Vector3 gyro3 = rot * gyro;
|
||||
|
||||
ASSERT_NEAR(0, (gyro2-gyro3).norm(), 1.0);
|
||||
|
||||
@@ -42,11 +41,10 @@ TEST(TurnDetection, gyroRotate) {
|
||||
|
||||
TEST(TurnDetection, xx) {
|
||||
|
||||
Vector3 dst(0, 0, 1);
|
||||
Vector3 src(0.0, 2.9, -10.0);
|
||||
src = src.normalized(); // sample accelerometer readings
|
||||
Vector3 dst(0, 0, 1);
|
||||
Vector3 src(0.0, 2.9, -10.0); src = src.normalized(); // sample accelerometer readings
|
||||
|
||||
Matrix3 rot = PoseDetection::getRotationMatrix(src, dst);
|
||||
Matrix3 rot = PoseDetection::getRotationMatrix(src, dst);
|
||||
|
||||
// Eigen::Vector3f x; x << 1, 0, 0;
|
||||
// Eigen::Vector3f z = src.normalized();
|
||||
@@ -57,14 +55,14 @@ TEST(TurnDetection, xx) {
|
||||
// rot.row(1) = y;
|
||||
// rot.row(2) = z;
|
||||
|
||||
Vector3 res = rot * src;
|
||||
Vector3 res = rot * src;
|
||||
// ASSERT_NEAR(dst(0), res(0), 0.01);
|
||||
// ASSERT_NEAR(dst(1), res(1), 0.01);
|
||||
// ASSERT_NEAR(dst(2), res(2), 0.01);
|
||||
|
||||
Vector3 gyro(0, 10, 30);
|
||||
Vector3 gyro(0, 10, 30);
|
||||
|
||||
Vector3 gyro2 = rot * gyro;
|
||||
Vector3 gyro2 = rot * gyro;
|
||||
int i = 0; (void) i;
|
||||
|
||||
}
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
#ifdef WITH_TESTS
|
||||
#ifdef TODO_______WITH_TESTS
|
||||
|
||||
#include "../../Tests.h"
|
||||
|
||||
@@ -115,7 +115,7 @@ TEST(Barometer, LIVE_tendence2) {
|
||||
|
||||
}
|
||||
|
||||
sleep(1);
|
||||
sleep(1);
|
||||
|
||||
|
||||
// tendence must be clear and smaller than the sigma
|
||||
@@ -124,120 +124,119 @@ TEST(Barometer, LIVE_tendence2) {
|
||||
}
|
||||
|
||||
TEST(Barometer, Activity) {
|
||||
ActivityButterPressure act;
|
||||
ActivityButterPressure act;
|
||||
|
||||
//read file
|
||||
std::string line;
|
||||
//read file
|
||||
std::string line;
|
||||
std::string filename = getDataFile("barometer/baro1.dat");
|
||||
std::ifstream infile(filename);
|
||||
std::ifstream infile(filename);
|
||||
|
||||
std::vector<ActivityButterPressure::History> actHist;
|
||||
std::vector<ActivityButterPressure::History> rawHist;
|
||||
std::vector<ActivityButterPressure::History> actHist;
|
||||
std::vector<ActivityButterPressure::History> rawHist;
|
||||
|
||||
while (std::getline(infile, line))
|
||||
{
|
||||
std::istringstream iss(line);
|
||||
int ts;
|
||||
double value;
|
||||
while (std::getline(infile, line))
|
||||
{
|
||||
std::istringstream iss(line);
|
||||
int ts;
|
||||
double value;
|
||||
|
||||
while (iss >> ts >> value) {
|
||||
act.add(Timestamp::fromMS(ts), BarometerData(value));
|
||||
Activity currentAct = act.get();
|
||||
rawHist.push_back(ActivityButterPressure::History(Timestamp::fromMS(ts), BarometerData(value)));
|
||||
actHist.push_back(ActivityButterPressure::History(Timestamp::fromMS(ts), BarometerData((int)currentAct)));
|
||||
}
|
||||
}
|
||||
while (iss >> ts >> value) {
|
||||
ActivityButterPressure::Activity currentAct = act.add(Timestamp::fromMS(ts), BarometerData(value));
|
||||
rawHist.push_back(ActivityButterPressure::History(Timestamp::fromMS(ts), BarometerData(value)));
|
||||
actHist.push_back(ActivityButterPressure::History(Timestamp::fromMS(ts), BarometerData(currentAct)));
|
||||
}
|
||||
}
|
||||
|
||||
K::Gnuplot gp;
|
||||
K::Gnuplot gpRaw;
|
||||
K::GnuplotPlot plot;
|
||||
K::GnuplotPlot plotRaw;
|
||||
K::GnuplotPlotElementLines rawLines;
|
||||
K::GnuplotPlotElementLines resultLines;
|
||||
K::Gnuplot gp;
|
||||
K::Gnuplot gpRaw;
|
||||
K::GnuplotPlot plot;
|
||||
K::GnuplotPlot plotRaw;
|
||||
K::GnuplotPlotElementLines rawLines;
|
||||
K::GnuplotPlotElementLines resultLines;
|
||||
|
||||
for(int i=0; i < actHist.size()-1; ++i){
|
||||
for(int i=0; i < actHist.size()-1; ++i){
|
||||
|
||||
//raw
|
||||
K::GnuplotPoint2 raw_p1(rawHist[i].ts.sec(), rawHist[i].data.hPa);
|
||||
K::GnuplotPoint2 raw_p2(rawHist[i+1].ts.sec(), rawHist[i+1].data.hPa);
|
||||
//raw
|
||||
K::GnuplotPoint2 raw_p1(rawHist[i].ts.sec(), rawHist[i].data.hPa);
|
||||
K::GnuplotPoint2 raw_p2(rawHist[i+1].ts.sec(), rawHist[i+1].data.hPa);
|
||||
|
||||
rawLines.addSegment(raw_p1, raw_p2);
|
||||
rawLines.addSegment(raw_p1, raw_p2);
|
||||
|
||||
//results
|
||||
K::GnuplotPoint2 input_p1(actHist[i].ts.sec(), actHist[i].data.hPa);
|
||||
K::GnuplotPoint2 input_p2(actHist[i+1].ts.sec(), actHist[i+1].data.hPa);
|
||||
//results
|
||||
K::GnuplotPoint2 input_p1(actHist[i].ts.sec(), actHist[i].data.hPa);
|
||||
K::GnuplotPoint2 input_p2(actHist[i+1].ts.sec(), actHist[i+1].data.hPa);
|
||||
|
||||
resultLines.addSegment(input_p1, input_p2);
|
||||
}
|
||||
resultLines.addSegment(input_p1, input_p2);
|
||||
}
|
||||
|
||||
plotRaw.add(&rawLines);
|
||||
plot.add(&resultLines);
|
||||
plotRaw.add(&rawLines);
|
||||
plot.add(&resultLines);
|
||||
|
||||
gp.draw(plot);
|
||||
gp.flush();
|
||||
gp.draw(plot);
|
||||
gp.flush();
|
||||
|
||||
gpRaw.draw(plotRaw);
|
||||
gpRaw.flush();
|
||||
gpRaw.draw(plotRaw);
|
||||
gpRaw.flush();
|
||||
|
||||
sleep(5);
|
||||
sleep(5);
|
||||
|
||||
}
|
||||
|
||||
TEST(Barometer, ActivityPercent) {
|
||||
|
||||
ActivityButterPressurePercent act;
|
||||
ActivityButterPressurePercent act;
|
||||
|
||||
//read file
|
||||
std::string line;
|
||||
std::string filename = getDataFile("barometer/baro1.dat");
|
||||
std::ifstream infile(filename);
|
||||
//read file
|
||||
std::string line;
|
||||
std::string filename = getDataFile("barometer/baro1.dat");
|
||||
std::ifstream infile(filename);
|
||||
|
||||
std::vector<ActivityButterPressurePercent::ActivityProbabilities> actHist;
|
||||
std::vector<double> rawHist;
|
||||
std::vector<ActivityButterPressurePercent::ActivityProbabilities> actHist;
|
||||
std::vector<double> rawHist;
|
||||
|
||||
while (std::getline(infile, line))
|
||||
{
|
||||
std::istringstream iss(line);
|
||||
int ts;
|
||||
double value;
|
||||
while (std::getline(infile, line))
|
||||
{
|
||||
std::istringstream iss(line);
|
||||
int ts;
|
||||
double value;
|
||||
|
||||
while (iss >> ts >> value) {
|
||||
ActivityButterPressurePercent::ActivityProbabilities activity = act.add(Timestamp::fromMS(ts), BarometerData(value));
|
||||
rawHist.push_back(value);
|
||||
actHist.push_back(activity);
|
||||
}
|
||||
}
|
||||
while (iss >> ts >> value) {
|
||||
ActivityButterPressurePercent::ActivityProbabilities activity = act.add(Timestamp::fromMS(ts), BarometerData(value));
|
||||
rawHist.push_back(value);
|
||||
actHist.push_back(activity);
|
||||
}
|
||||
}
|
||||
|
||||
K::Gnuplot gp;
|
||||
K::Gnuplot gpRaw;
|
||||
K::GnuplotPlot plot;
|
||||
K::GnuplotPlot plotRaw;
|
||||
K::GnuplotPlotElementLines rawLines;
|
||||
K::GnuplotPlotElementLines resultLines;
|
||||
K::Gnuplot gp;
|
||||
K::Gnuplot gpRaw;
|
||||
K::GnuplotPlot plot;
|
||||
K::GnuplotPlot plotRaw;
|
||||
K::GnuplotPlotElementLines rawLines;
|
||||
K::GnuplotPlotElementLines resultLines;
|
||||
|
||||
for(int i=0; i < actHist.size()-1; ++i){
|
||||
for(int i=0; i < actHist.size()-1; ++i){
|
||||
|
||||
K::GnuplotPoint2 raw_p1(i, rawHist[i]);
|
||||
K::GnuplotPoint2 raw_p2(i+1, rawHist[i+1]);
|
||||
K::GnuplotPoint2 raw_p1(i, rawHist[i]);
|
||||
K::GnuplotPoint2 raw_p2(i+1, rawHist[i+1]);
|
||||
|
||||
rawLines.addSegment(raw_p1, raw_p2);
|
||||
rawLines.addSegment(raw_p1, raw_p2);
|
||||
|
||||
K::GnuplotPoint2 input_p1(i, actHist[i].elevatorDown);
|
||||
K::GnuplotPoint2 input_p2(i+1, actHist[i+1].elevatorDown);
|
||||
K::GnuplotPoint2 input_p1(i, actHist[i].elevatorDown);
|
||||
K::GnuplotPoint2 input_p2(i+1, actHist[i+1].elevatorDown);
|
||||
|
||||
resultLines.addSegment(input_p1, input_p2);
|
||||
}
|
||||
resultLines.addSegment(input_p1, input_p2);
|
||||
}
|
||||
|
||||
plotRaw.add(&rawLines);
|
||||
plot.add(&resultLines);
|
||||
plotRaw.add(&rawLines);
|
||||
plot.add(&resultLines);
|
||||
|
||||
gp.draw(plot);
|
||||
gp.flush();
|
||||
gp.draw(plot);
|
||||
gp.flush();
|
||||
|
||||
gpRaw.draw(plotRaw);
|
||||
gpRaw.flush();
|
||||
gpRaw.draw(plotRaw);
|
||||
gpRaw.flush();
|
||||
|
||||
sleep(5);
|
||||
sleep(5);
|
||||
}
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user