performance enhancements

better 2D drawing

Controller.cpp

@@ -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");
 
 	}
 

Settings.h

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

_android/src/MyActivity.java

@@ -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 {
 

_android/src/StepLoggerClient.java

@@ -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 instance._log(x,y,z);
-		return 1337;		// return a magic-code that is checked within c++
 	}
 
-    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;
+		}
 	}
 
 }

nav/Filter.h

@@ -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;
 
-
+		// sanity check
-
+		Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
-
-		// 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));
 
 		//for (K::Particle<MyState>& p : particles) {
-		#pragma omp parallel for num_threads(2)
+		#pragma omp parallel for num_threads(3)
-		for (int i = 0; i < (int) particles.size(); ++i) {
+		for (int i = 0; i < Settings::numParticles; ++i) {
-			K::Particle<MyState>& p = particles[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;

nav/NavController.h

@@ -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 float avgSum = 0;
-		//static int avgCount = 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;
 

ui/map/2D/Floor2D.h

@@ -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) {
+	void drawElevator(QPainter& qp, const Scaler2D& s, const Floorplan::Elevator* elevator) {
-		const int num = poly->poly.points.size();
+		const QPolygon qpoly = toQPolygon(elevator->getPoints(), s);
-		for (int i = 0; i < num; ++i) {
+		qp.setBrush(Qt::gray);
-			const Point2 pt1 = s.mapToScreen(poly->poly.points[(i+0)]);
+		qp.setPen(Qt::black);
-			const Point2 pt2 = s.mapToScreen(poly->poly.points[(i+1)%num]);
+		qp.drawPolygon(qpoly);
-			qp.drawLine(pt1.x, pt1.y, pt2.x, pt2.y);
+	}
+
+	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;
 	}
 
 };

ui/map/2D/MapView2D.cpp

@@ -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) {
+	for (Renderable2D* r : elementsA) {r->render(qp, scaler, renderParams);}
-		r->render(qp, scaler, renderParams);
+	for (Renderable2D* r : elementsB) {r->render(qp, scaler, renderParams);}
-	}
 
 	qp.end();
 

ui/map/2D/MapView2D.h

@@ -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;

yasmin.pro

@@ -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