625f5fe04d
removed KLib stuff added new activity filter is uncommand! at the moment, the app is not able to load new maps and breaks using old maps
121 lines
2.6 KiB
C++
121 lines
2.6 KiB
C++
#ifndef COLORPOINTS2D_H
|
|
#define COLORPOINTS2D_H
|
|
|
|
#include <Indoor/floorplan/v2/Floorplan.h>
|
|
#include "Renderable2D.h"
|
|
#include <Indoor/geo/Point3.h>
|
|
|
|
#include <Indoor/grid/Grid.h>
|
|
|
|
#include <Indoor/smc/Particle.h>
|
|
#include "../nav/grid/Node.h"
|
|
#include "../nav/Observation.h"
|
|
|
|
/**
|
|
* debug color points
|
|
*/
|
|
class ColorPoints2D : public Renderable2D {
|
|
|
|
private:
|
|
|
|
struct PT {
|
|
Point3 pos;
|
|
QColor color;
|
|
PT(const Point3 pos, const QColor color) : pos(pos), color(color) {;}
|
|
};
|
|
|
|
std::vector<PT> points;
|
|
|
|
public:
|
|
|
|
/** ctor */
|
|
ColorPoints2D() {
|
|
|
|
}
|
|
|
|
void showGridImportance(Grid<MyGridNode>* grid) {
|
|
|
|
float min = +INFINITY;
|
|
float max = -INFINITY;
|
|
|
|
for (const MyGridNode& n : *grid) {
|
|
const float f = n.getWalkImportance();
|
|
if (f < min) {min = f;}
|
|
if (f > max) {max = f;}
|
|
}
|
|
|
|
max = 1.2;
|
|
|
|
for (const MyGridNode& n : *grid) {
|
|
const Point3 pt(n.x_cm/100.0f, n.y_cm/100.0f + 0.1f, n.z_cm/100.0f);
|
|
const float f = n.getWalkImportance();
|
|
float h = 0.66 - ((f-min)/(max-min)) * 0.66; // 0.66 is blue on the HSV-scale
|
|
if (h < 0) {h = 0;}
|
|
if (h > 1) {h = 1;}
|
|
const QColor color = QColor::fromHsvF(h, 1, 1);
|
|
points.push_back(PT(pt, color));
|
|
}
|
|
|
|
}
|
|
|
|
/** NOTE: must be called from Qt's main thread! */
|
|
template <typename T> void setFromParticles(const std::vector<SMC::Particle<T>>& particles) {
|
|
|
|
points.clear();
|
|
|
|
// group particles by grid-point
|
|
std::unordered_map<GridPoint, float> weights;
|
|
for (const SMC::Particle<T>& p : particles) {
|
|
const GridPoint gp = p.state.position;
|
|
if (weights.find(gp) != weights.end()) {continue;}
|
|
weights[gp] += p.weight;
|
|
}
|
|
|
|
// find min/max
|
|
float min = +INFINITY;
|
|
float max = -INFINITY;
|
|
for (auto it : weights) {
|
|
if (it.second > max) {max = it.second;}
|
|
if (it.second < min) {min = it.second;}
|
|
}
|
|
|
|
// draw colored
|
|
for (auto it : weights) {
|
|
const GridPoint gp = it.first;
|
|
const float w = it.second;
|
|
const float p = (w-min) / (max-min); // [0:1]
|
|
const Point3 pt(gp.x_cm/100.0f, gp.y_cm/100.0f + 0.1f, gp.z_cm/100.0f);
|
|
float h = 0.66 - (p*0.66); // 0.66 is blue on the HSV-scale
|
|
const QColor color = QColor::fromHsvF(h, 1, 1);
|
|
points.push_back(PT(pt, color));
|
|
}
|
|
|
|
}
|
|
|
|
protected:
|
|
|
|
void doRender(QPainter& qp, const Scaler2D& s, const RenderParams2D& r) override {
|
|
|
|
QPen pen;
|
|
pen.setWidth(4);
|
|
|
|
for (const PT& pt : points) {
|
|
|
|
if (pt.pos.z < r.clip.belowHeight_m) {continue;}
|
|
if (pt.pos.z > r.clip.aboveHeight_m) {continue;}
|
|
|
|
const Point2 p2 = s.mapToScreen(pt.pos.xy());
|
|
|
|
pen.setColor(pt.color);
|
|
qp.setPen(pen);
|
|
qp.drawPoint(p2.x, p2.y);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
};
|
|
|
|
#endif // COLORPOINTS2D_H
|