diff --git a/Controller.cpp b/Controller.cpp index b33a728..22e54ae 100644 --- a/Controller.cpp +++ b/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* 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"); } diff --git a/Settings.h b/Settings.h index c403a35..082e351 100644 --- a/Settings.h +++ b/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 diff --git a/_android/src/MyActivity.java b/_android/src/MyActivity.java index 85e3b79..a72a799 100644 --- a/_android/src/MyActivity.java +++ b/_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 { diff --git a/_android/src/StepLoggerClient.java b/_android/src/StepLoggerClient.java index 9bca174..8cf55b5 100644 --- a/_android/src/StepLoggerClient.java +++ b/_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 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; + } } } diff --git a/nav/Filter.h b/nav/Filter.h index 49df991..f30007e 100644 --- a/nav/Filter.h +++ b/nav/Filter.h @@ -26,6 +26,7 @@ #include "../Settings.h" #include +#include class PFInit : public K::ParticleFilterInitializer { @@ -113,25 +114,19 @@ public: ((MyControl*)_ctrl)->resetAfterTransition(); std::normal_distribution 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& p : particles) { - #pragma omp parallel for num_threads(2) - for (int i = 0; i < (int) particles.size(); ++i) { - K::Particle& 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& 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::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& 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& 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& 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; diff --git a/nav/NavController.h b/nav/NavController.h index 741552d..faafebf 100644 --- a/nav/NavController.h +++ b/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 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; diff --git a/ui/map/2D/Floor2D.h b/ui/map/2D/Floor2D.h index 62df59f..9bdca9d 100644 --- a/ui/map/2D/Floor2D.h +++ b/ui/map/2D/Floor2D.h @@ -3,6 +3,7 @@ #include #include "Renderable2D.h" +#include /** * 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(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(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 parts = stair->getParts(); + std::vector 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; } }; diff --git a/ui/map/2D/MapView2D.cpp b/ui/map/2D/MapView2D.cpp index d183f10..76ae493 100644 --- a/ui/map/2D/MapView2D.cpp +++ b/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) { - 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(); diff --git a/ui/map/2D/MapView2D.h b/ui/map/2D/MapView2D.h index 8b8c4ae..db10bb2 100644 --- a/ui/map/2D/MapView2D.h +++ b/ui/map/2D/MapView2D.h @@ -39,7 +39,9 @@ class MapView2D : public QWidget { private: - std::vector elements; + std::vector elementsA; + std::vector elementsB; + ColorPoints2D* colorPoints = nullptr; Path2D* pathToDest = nullptr; Path2D* pathWalked = nullptr; diff --git a/yasmin.pro b/yasmin.pro index c49b9fe..0b71485 100644 --- a/yasmin.pro +++ b/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