075d8bb633
added main menu added debug display many debug widgets for plotting live data worked on android live sensors added offline-data sensor feeding some dummy data sensors worked on the map display added ui debug for grid-points, particles and weights added a cool dude to display the estimation added real filtering based on the Indoor components c++11 fixes for android compilation online and offline filtering support new resampling technique for testing map loading via dialog
115 lines
2.6 KiB
C++
115 lines
2.6 KiB
C++
#ifndef GL_PARTICLES_H
|
|
#define GL_PARTICLES_H
|
|
|
|
|
|
#include <Indoor/floorplan/v2/Floorplan.h>
|
|
#include <KLib/math/filter/particles/ParticleFilter.h>
|
|
|
|
#include "../gl/GLHelper.h"
|
|
#include "../gl/GLPoints.h"
|
|
#include "../Renderable.h"
|
|
|
|
#include "../../../nav/Node.h"
|
|
|
|
class ColorPoints : public Renderable {
|
|
|
|
private:
|
|
|
|
GLPoints points;
|
|
float size = 3.0f;
|
|
|
|
public:
|
|
|
|
/** ctor */
|
|
ColorPoints() {
|
|
|
|
}
|
|
|
|
/** NOTE: must be called from Qt's main thread! */
|
|
void setFromGridImportance(Grid<MyGridNode>* grid) {
|
|
|
|
points.clear();
|
|
|
|
for (const MyGridNode& n : *grid) {
|
|
const QVector3D pt(n.x_cm/100.0f, n.z_cm/100.0f + 0.1f, n.y_cm/100.0f); // swap z and y
|
|
const float f = n.getNavImportance();
|
|
float h = 0.66 - (f*0.20); // 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.addPoint(pt, color);
|
|
}
|
|
|
|
size = 3.0f;
|
|
points.rebuild();
|
|
|
|
}
|
|
|
|
/** NOTE: must be called from Qt's main thread! */
|
|
template <typename T> void setFromParticles(const std::vector<K::Particle<T>>& particles) {
|
|
|
|
points.clear();
|
|
|
|
// group particles by grid-point
|
|
std::unordered_map<GridPoint, float> weights;
|
|
for (const K::Particle<T>& p : particles) {
|
|
const GridPoint gp = p.state.position;
|
|
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 QVector3D pt(gp.x_cm/100.0f, gp.z_cm/100.0f + 0.1f, gp.y_cm/100.0f); // swap z and y
|
|
float h = 0.66 - (p*0.66); // 0.66 is blue on the HSV-scale
|
|
const QColor color = QColor::fromHsvF(h, 1, 1);
|
|
points.addPoint(pt, color);
|
|
}
|
|
|
|
|
|
// for (const K::Particle<T>& p : particles) {
|
|
// const GridPoint gp = p.state.position;
|
|
// const QVector3D pt(gp.x_cm/100.0f, gp.z_cm/100.0f + 0.1f, gp.y_cm/100.0f); // swap z and y
|
|
// const QColor color = Qt::blue;
|
|
// points.addPoint(pt, color);
|
|
// }
|
|
|
|
size = 6.0f;
|
|
points.rebuild();
|
|
|
|
}
|
|
|
|
|
|
void initGL() override {
|
|
loadShader(":/res/gl/vertex1.glsl", ":/res/gl/fragmentColorPoint.glsl");
|
|
//program.setUniformValue("color", QVector4D(0.5, 0.5, 0.5, 1.0));
|
|
points.initGL();
|
|
}
|
|
|
|
/** render the floor */
|
|
void _render() override {
|
|
//glDisable(GL_DEPTH_TEST);
|
|
//glPointSize()
|
|
#ifndef ANDROID
|
|
glPointSize(size);
|
|
#endif
|
|
points.render(&program);
|
|
//glEnable(GL_DEPTH_TEST);
|
|
}
|
|
|
|
|
|
};
|
|
|
|
|
|
#endif // GL_PARTICLES_H
|