147 lines
3.6 KiB
C++
147 lines
3.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"
|
|
|
|
#include "../nav/grid/State.h"
|
|
#include "../nav/mesh/State.h"
|
|
|
|
#include <type_traits>
|
|
|
|
/**
|
|
* 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! */
|
|
void setFromParticles(const std::vector<SMC::Particle<GridBased::MyState>>& particles){
|
|
|
|
points.clear();
|
|
|
|
// group particles by grid-point
|
|
std::unordered_map<GridPoint, float> weights;
|
|
for (const SMC::Particle<GridBased::MyState>& 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));
|
|
}
|
|
|
|
}
|
|
|
|
void setFromParticles(const std::vector<SMC::Particle<MeshBased::MyState>>& particles){
|
|
|
|
points.clear();
|
|
|
|
float min = +INFINITY;
|
|
float max = -INFINITY;
|
|
for (const auto p : particles){
|
|
if (p.weight > max) {max = p.weight;}
|
|
if (p.weight < min) {min = p.weight;}
|
|
}
|
|
|
|
for (const auto p : particles) {
|
|
const Point3 pt(p.state.loc.pos.x, p.state.loc.pos.y, p.state.loc.pos.z);
|
|
const float prob = (p.weight-min) / (max-min);
|
|
float h = 0.66 - (prob*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
|