FHWS/YASMIN
Archived
3
0
Fork 0
Code Issues 12 Pull Requests Releases Wiki Activity

performance enhancements

Browse Source 
better 2D drawing
This commit is contained in:
FrankE
2016-09-29 21:02:19 +02:00
parent 5ddc455bee
commit 833327bafd
10 changed files with 185 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
Controller.cpp
View File

@@ -51,9 +51,9 @@ Controller::Controller() : sl(scaler) {
//SensorFactory::set(new SensorFactoryOffline(Settings::Data::getOfflineDir() + "bergwerk/path3/nexus/vor/1454782562231.csv"));
//SensorFactory::set(new SensorFactoryOffline(Settings::Data::getOfflineDir() + "/bergwerk/path4/nexus/rueck/1454776724285_rueck.csv"));
//SensorFactory::set(new SensorFactoryOffline(Settings::Data::getOfflineDir() + "/bergwerk/path4/nexus/vor/1454776525797.csv"));
SensorFactory::set(new SensorFactoryOffline(Settings::Data::getOfflineDir() + "/bergwerk/path4/nexus/vor/1454776525797.csv"));
SensorFactory::set(new SensorFactoryAndroid());
//SensorFactory::set(new SensorFactoryAndroid());
// SensorFactory::get().getAccelerometer().start();
// SensorFactory::get().getGyroscope().start();
// SensorFactory::get().getBarometer().start();
@@ -137,6 +137,7 @@ void buildGridOnce(Grid<MyGridNode>* grid, Floorplan::IndoorMap* map, const std:
// load all APs from the floorplan and use same TXP/EXP/WAF for all of them
wifiModel.loadAPs(map, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF);
Assert::isFalse(wifiModel.getAllAPs().empty(), "no AccessPoints stored within the map.xml");
}

2
Settings.h
View File

@@ -8,7 +8,7 @@
namespace Settings {
const int numParticles = 2000;
const int numParticles = 5000;
namespace IMU {
const float turnSigma = 1.5; // 3.5

1
_android/src/MyActivity.java
View File

@@ -3,6 +3,7 @@ package indoor.java;
import android.os.Bundle;
import android.view.WindowManager;
import org.qtproject.qt5.android.bindings.QtActivity;
import android.widget.Toast;
public class MyActivity extends QtActivity {

12
_android/src/StepLoggerClient.java
View File

@@ -64,16 +64,18 @@ public class StepLoggerClient {
/** static access from C++ */
public static int log(double x, double y, double z) {
Log.d("me", "got values");
instance._log(x,y,z);
return 1337; // return a magic-code that is checked within c++
return instance._log(x,y,z);
}
private void _log(double x, double y, double z) {
private int _log(double x, double y, double z) {
Log.d("me", mService + "");
if (mService == null) {return;}
if (mService == null) {return 0;}
try {
mService.logPosition(System.currentTimeMillis(), x, y, z);
} catch (final Exception e) {;}
return 1337;
} catch (final Exception e) {
return 0;
}
}
}

49
nav/Filter.h
View File

@@ -26,6 +26,7 @@
#include "../Settings.h"
#include <omp.h>
#include <future>
class PFInit : public K::ParticleFilterInitializer<MyState> {
@@ -113,25 +114,19 @@ public:
((MyControl*)_ctrl)->resetAfterTransition();
std::normal_distribution<float> noise(0, Settings::IMU::stepSigma);
double probSum = 0;
// seems OK
// float sum = 0;
// for (int i = 0; i < 1000; ++i) {
// float val = noise(gen);
// sum += std::abs(val);
// }
//Log::add("123", "sum: " + std::to_string(sum));
//Log::add("123", std::to_string(Timestamp::fromRunningTime().ms()) + ": " + std::to_string(ctrl.numStepsSinceLastTransition));
// sanity check
Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
//for (K::Particle<MyState>& p : particles) {
#pragma omp parallel for num_threads(2)
for (int i = 0; i < (int) particles.size(); ++i) {
K::Particle<MyState>& p = particles[i];
#pragma omp parallel for num_threads(3)
for (int i = 0; i < Settings::numParticles; ++i) {
//#pragma omp atomic
const float dist_m = std::abs(ctrl.numStepsSinceLastTransition * Settings::IMU::stepLength + noise(gen));
K::Particle<MyState>& p = particles[i];
double prob;
p.state = walker.getDestination(*grid, p.state, dist_m, prob);
//p.weight *= prob;//(prob > 0.01) ? (1.0) : (0.15);
@@ -141,17 +136,8 @@ public:
p.weight = std::pow(p.weight, 0.1); // make all particles a little more equal [less strict]
p.weight *= std::pow(prob, 0.1); // add grid-walk-probability
if (p.weight != p.weight) {throw Exception("nan");}
probSum += prob;
//p.weight = Distribution::Exponential<double>::getProbability(5.0, prob);
}
// const double avgProb = probSum / particles.size();
// const double threshold = avgProb * 0.15;
// for (int i = 0; i < (int) particles.size(); ++i) {
// K::Particle<MyState>& p = particles[i];
// p.weight = (p.weight > threshold) ? (1.0) : (0.01); // downvote all transitions below the threshold
// //p.weight = 1;
// }
}
}
@@ -221,7 +207,13 @@ public:
Log::add("Filter", "VAP: " + std::to_string(numAP1) + " -> " + std::to_string(numAP2));
for (K::Particle<MyState>& p : particles) {
// sanity check
Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
#pragma omp parallel for num_threads(3)
for (int i = 0; i < Settings::numParticles; ++i) {
K::Particle<MyState>& p = particles[i];
// WiFi free
//const double pWiFi = wiFiProbability.getProbability(p.state.position.inMeter()+person, observation.currentTime, vg.group(observation.wifi));
@@ -238,10 +230,11 @@ public:
p.weight *= prob; // NOTE: keeps the weight returned by the transition step!
//p.weight = prob; // does NOT keep the weights returned by the transition step
sum += p.weight;
if (p.weight != p.weight) {throw Exception("nan");}
#pragma omp atomic
sum += p.weight;
}
return sum;

7
nav/NavController.h
View File

@@ -270,8 +270,8 @@ private:
/** check whether its time for a filter update, and if so, execute the update and return true */
bool filterUpdateIfNeeded() {
//static float avgSum = 0;
//static int avgCount = 0;
static float avgSum = 0;
static int avgCount = 0;
// fixed update rate based on incoming sensor data
// allows working with live data and faster for offline data
@@ -287,7 +287,8 @@ private:
const Timestamp ts2 = Timestamp::fromUnixTime();
const Timestamp tsDiff = ts2-ts1;
const QString filterTime = QString::number(tsDiff.ms());
//avgSum += tsDiff.ms(); ++avgCount; std::cout << "ts:" << curObs.currentTime << " avg:" << (avgSum/avgCount) << std::endl;
avgSum += tsDiff.ms(); ++avgCount;
Log::add("xxx", "ts:" + std::to_string(curObs.currentTime.ms()) + " avg:" + std::to_string(avgSum/avgCount));
QMetaObject::invokeMethod(mainController->getInfoWidget(), "showFilterTime", Qt::QueuedConnection, Q_ARG(const QString&, filterTime));
return true;

132
ui/map/2D/Floor2D.h
View File

@@ -3,6 +3,7 @@
#include <Indoor/floorplan/v2/Floorplan.h>
#include "Renderable2D.h"
#include <QLinearGradient>
/**
* draw the floor itself (outline, obstacles)
@@ -27,34 +28,145 @@ protected:
if (floor->atHeight < r.clip.belowHeight_m) {return;}
if (floor->atHeight > r.clip.aboveHeight_m) {return;}
qp.setPen(Qt::black);
for (const Floorplan::FloorObstacle* obs : floor->obstacles) {
const Floorplan::FloorObstacleLine* line = dynamic_cast<const Floorplan::FloorObstacleLine*>(obs);
if (line) {drawLine(qp, s, line);}
if (line) {drawObstacle(qp, s, line);}
}
qp.setPen(Qt::gray);
for (const Floorplan::FloorOutlinePolygon* poly : floor->outline) {
drawOutline(qp, s, poly);
}
for (const Floorplan::Stair* stair : floor->stairs) {
const Floorplan::StairFreeform* sf = dynamic_cast<const Floorplan::StairFreeform*>(stair);
if (sf) {drawStair(qp, s, sf);}
}
for (const Floorplan::Elevator* elevator : floor->elevators) {
drawElevator(qp, s, elevator);
}
}
private:
void drawLine(QPainter& qp, const Scaler2D& s, const Floorplan::FloorObstacleLine* line) {
static inline QPen getPen(Floorplan::Material mat, Floorplan::ObstacleType type) {
using namespace Floorplan;
QPen pen; pen.setColor(Qt::darkGray);
if (mat == Material::CONCRETE) {pen.setWidth(3);}
if (mat == Material::GLASS) {pen.setStyle(Qt::PenStyle::DotLine);}
if (type == ObstacleType::HANDRAIL) {pen.setStyle(Qt::PenStyle::DashLine);}
if (type == ObstacleType::UNKNOWN) {pen.setColor(Qt::red); pen.setWidth(5);}
if (type == ObstacleType::PILLAR) {pen.setColor(Qt::red); pen.setWidth(5);}
return pen;
}
void drawObstacle(QPainter& qp, const Scaler2D& s, const Floorplan::FloorObstacleLine* line) {
const Point2 pt1 = s.mapToScreen(line->from);
const Point2 pt2 = s.mapToScreen(line->to);
qp.setPen(getPen(line->material, line->type));
qp.drawLine(pt1.x, pt1.y, pt2.x, pt2.y);
}
void drawOutline(QPainter& qp, const Scaler2D& s, const Floorplan::FloorOutlinePolygon* poly) {
const int num = poly->poly.points.size();
for (int i = 0; i < num; ++i) {
const Point2 pt1 = s.mapToScreen(poly->poly.points[(i+0)]);
const Point2 pt2 = s.mapToScreen(poly->poly.points[(i+1)%num]);
qp.drawLine(pt1.x, pt1.y, pt2.x, pt2.y);
void drawElevator(QPainter& qp, const Scaler2D& s, const Floorplan::Elevator* elevator) {
const QPolygon qpoly = toQPolygon(elevator->getPoints(), s);
qp.setBrush(Qt::gray);
qp.setPen(Qt::black);
qp.drawPolygon(qpoly);
}
void drawStair(QPainter& qp, const Scaler2D& s, const Floorplan::StairFreeform* stair) {
std::vector<Floorplan::StairPart> parts = stair->getParts();
std::vector<Floorplan::Quad3> quads = Floorplan::getQuads(parts, floor);
for (int i = 0; i < (int) parts.size(); ++i) {
const Floorplan::StairPart& part = parts[i];
const Floorplan::Quad3& quad = quads[i];
const Point2 start = s.mapToScreen(part.start.xy());
const Point2 end = s.mapToScreen(part.end.xy());
// fill the polygon with a gradient corresponding with the stair's height relative to the floor's height
QLinearGradient gradient(start.x, start.y, end.x, end.y);
const float p1 = 0.1 + clamp01( part.start.z / floor->height) * 0.8;
const float p2 = 0.1 + clamp01( part.end.z / floor->height) * 0.8;
gradient.setColorAt(0, QColor(p1*255, p1*255, p1*255));
gradient.setColorAt(1, QColor(p2*255, p2*255, p2*255));
qp.setBrush(gradient);
qp.setPen(QColor(0,0,0,128));
// polygon-construction
QPolygon poly;
poly.push_back(toQPoint(s.mapToScreen(quad.p1.xy())));
poly.push_back(toQPoint(s.mapToScreen(quad.p2.xy())));
poly.push_back(toQPoint(s.mapToScreen(quad.p3.xy())));
poly.push_back(toQPoint(s.mapToScreen(quad.p4.xy())));
qp.drawPolygon(poly);
}
}
/** draw the given outline polygon */
void drawOutline(QPainter& qp, const Scaler2D& s, const Floorplan::FloorOutlinePolygon* poly) {
// configure the drawing
if (poly->method == Floorplan::OutlineMethod::ADD) {
qp.setPen(Qt::gray);
qp.setBrush(Qt::NoBrush);
} else if (poly->method == Floorplan::OutlineMethod::REMOVE) {
QBrush brush;
brush.setStyle(Qt::BrushStyle::DiagCrossPattern);
brush.setColor(QColor(0,0,0));
qp.setPen(Qt::gray);
qp.setBrush(brush);
}
// construct the polygon
const int num = poly->poly.points.size();
QPolygon qpoly;
for (int i = 0; i < num; ++i) {
qpoly.push_back(toQPoint(s.mapToScreen(poly->poly.points[(i+0)])));
}
// draw the polygon
qp.drawPolygon(qpoly);
}
/** convert floorplan polygon to QPolygon */
static inline QPolygon toQPolygon(const Floorplan::Polygon2& poly) {
QPolygon qpoly;
for (const Point2 p : poly.points) {
qpoly.push_back(QPoint(p.x, p.y));
}
return qpoly;
}
/** convert floorplan polygon to QPolygon */
static inline QPolygon toQPolygon(const Floorplan::Polygon2& poly, const Scaler2D& s) {
QPolygon qpoly;
for (const Point2 _p : poly.points) {
const Point2 p = s.mapToScreen(_p);
qpoly.push_back(QPoint(p.x, p.y));
}
return qpoly;
}
/** convert Point2 to QPoint */
static inline QPoint toQPoint(const Point2 p) {
return QPoint(p.x, p.y);
}
/** helper method. limit val to [0:1] */
static inline float clamp01(const float val) {
if (val < 0) {return 0;}
if (val > 1) {return 1;}
return val;
}
};

17
ui/map/2D/MapView2D.cpp
View File

@@ -21,19 +21,19 @@ MapView2D::MapView2D(QWidget *parent) : QWidget(parent) {
setFocusPolicy(Qt::StrongFocus);
setRenderHeight(0);
colorPoints = new ColorPoints2D();
elements.push_back(colorPoints);
elementsB.push_back(colorPoints);
pathToDest = new Path2D();
pathToDest->setColor(Qt::blue);
pathToDest->setWidth(10);
elements.push_back(pathToDest);
elementsB.push_back(pathToDest);
pathWalked = new Path2D();
pathToDest->setColor(Qt::black);
pathToDest->setWidth(5);
elements.push_back(pathWalked);
elementsB.push_back(pathWalked);
// buttons
//menu = new QWidget(this);
@@ -90,11 +90,11 @@ void MapView2D::setMap(WiFiCalibrationDataModel* mdl, Floorplan::IndoorMap* map)
for (Floorplan::Floor* floor : map->floors) {
Floor2D* f = new Floor2D(floor);
elements.push_back(f);
elementsA.push_back(f);
}
wifiCalib = new WiFiCalibTool(mdl, map);
elements.push_back(wifiCalib);
elementsB.push_back(wifiCalib);
scaler.setCenterM(Point2(70, 35));
@@ -177,9 +177,8 @@ void MapView2D::paintEvent(QPaintEvent*) {
qp.fillRect(0, 0, width(), height(), Qt::white);
// render elements
for (Renderable2D* r : elements) {
r->render(qp, scaler, renderParams);
}
for (Renderable2D* r : elementsA) {r->render(qp, scaler, renderParams);}
for (Renderable2D* r : elementsB) {r->render(qp, scaler, renderParams);}
qp.end();

4
ui/map/2D/MapView2D.h
View File

@@ -39,7 +39,9 @@ class MapView2D : public QWidget {
private:
std::vector<Renderable2D*> elements;
std::vector<Renderable2D*> elementsA;
std::vector<Renderable2D*> elementsB;
ColorPoints2D* colorPoints = nullptr;
Path2D* pathToDest = nullptr;
Path2D* pathWalked = nullptr;

15
yasmin.pro
View File

@@ -9,6 +9,21 @@ QT += qml opengl svg
ANDROID {
QT += androidextras
QT += sensors
#http://stackoverflow.com/questions/28391685/opencv-with-hard-float-support-for-android#28393545
# wasn't faster... and seems dangerous :P
#QMAKE_CXXFLAGS -= -mfpu=vfp
#QMAKE_CFLAGS -= -mfpu=vfp
#QMAKE_CXXFLAGS += -mfpu=neon -funsafe-math-optimizations
#QMAKE_CFLAGS += -mfpu=neon -funsafe-math-optimizations
# wasn't faster..
#QMAKE_CXXFLAGS += -mtune=cortex-a57
#QMAKE_CFLAGS += -mtune=cortex-a57
#QMAKE_CXXFLAGS += -O3
#QMAKE_CFLAGS += -O3
}
# openMP