FHWS/YASMIN
Archived
3
0
Fork 0
Code Issues 12 Pull Requests Releases Wiki Activity

app geht. optimierung spinnt noch weng

Browse Source
This commit is contained in:
toni
2018-07-17 14:59:39 +02:00
parent e6ede27212
commit 2bb3fb8f82
8 changed files with 90 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files

80
ui/map/2D/ColorPoints2D.h
View File

@@ -11,6 +11,11 @@
#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
*/
@@ -59,38 +64,59 @@ public:
}
/** NOTE: must be called from Qt's main thread! */
template <typename T> void setFromParticles(const std::vector<SMC::Particle<T>>& particles) {
void setFromParticles(const std::vector<SMC::Particle<GridBased::MyState>>& particles){
points.clear();
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;
}
// 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;}
}
// 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));
}
// 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:

4
ui/map/2D/MapView2D.cpp
View File

@@ -115,6 +115,10 @@ void MapView2D::showParticles(const std::vector<SMC::Particle<GridBased::MyState
this->colorPoints->setFromParticles(*particles);
}
void MapView2D::showParticles(const std::vector<SMC::Particle<MeshBased::MyState>>* particles) {
this->colorPoints->setFromParticles(*particles);
}
void MapView2D::setCurrentEstimation(const Point3 pos_m, const Point3 dir) {
(void) dir;
setRenderHeight(pos_m.z);

6
ui/map/2D/MapView2D.h
View File

@@ -13,6 +13,7 @@
#include <Indoor/geo/Point3.h>
#include "nav/grid/State.h"
#include "nav/mesh/State.h"
namespace Floorplan {
class IndoorMap;
@@ -101,13 +102,16 @@ public:
/** NOTE: must be called from Qt's main thread! */
Q_INVOKABLE void showParticles(const void* particles) {
showParticles((const std::vector<SMC::Particle<GridBased::MyState>>*) particles);
showParticles((const std::vector<SMC::Particle<MeshBased::MyState>>*) particles);
}
/** NOTE: must be called from Qt's main thread! */
void showParticles(const std::vector<SMC::Particle<GridBased::MyState>>* particles);
/** NOTE: must be called from Qt's main thread! */
void showParticles(const std::vector<SMC::Particle<MeshBased::MyState>>* particles);
/** set the currently estimated position */
void setCurrentEstimation(const Point3 pos, const Point3 dir);