FHWS/IPIN2017
Archived
3
0
Fork 0
Code Issues 4 Pull Requests Releases Wiki Activity

added kld with parzen and nv distribution

Browse Source 
added new testfiles
fixed plot
fixed numerical errors
This commit is contained in:
toni
2017-03-23 19:50:33 +01:00
parent 48566b79fc
commit 212573cba2
5 changed files with 215 additions and 84 deletions
Download Patch File Download Diff File Expand all files Collapse all files

73
code/Plotti.h
View File

@@ -35,6 +35,10 @@ struct Plotti {
K::GnuplotSplotElementPoints pAPs;
K::GnuplotSplotElementPoints pInterest;
K::GnuplotSplotElementPoints pParticles;
K::GnuplotSplotElementPoints pNormal1;
K::GnuplotSplotElementPoints pNormal2;
K::GnuplotSplotElementColorPoints pDistributation1;
K::GnuplotSplotElementColorPoints pDistributation2;
K::GnuplotSplotElementColorPoints pColorPoints;
K::GnuplotSplotElementLines gtPath;
K::GnuplotSplotElementLines estPath;
@@ -45,7 +49,11 @@ struct Plotti {
splot.add(&pGrid); pGrid.setPointSize(0.25); pGrid.setColorHex("#888888");
splot.add(&pAPs); pAPs.setPointSize(0.7);
splot.add(&pColorPoints); pColorPoints.setPointSize(0.6);
splot.add(&pDistributation1); pDistributation1.setPointSize(0.6);
splot.add(&pDistributation2); pDistributation2.setPointSize(0.6);
splot.add(&pParticles); pParticles.setColorHex("#0000ff"); pParticles.setPointSize(0.4f);
splot.add(&pNormal1); pNormal1.setColorHex("#ff00ff"); pNormal1.setPointSize(0.4f);
splot.add(&pNormal2); pNormal2.setColorHex("#00aaff"); pNormal2.setPointSize(0.4f);
splot.add(&pFloor);
splot.add(&pOutline); pOutline.setColorHex("#999999");
splot.add(&pStairs); pStairs.setColorHex("#000000");
@@ -100,6 +108,71 @@ struct Plotti {
estPathSmoothed.add(est);
}
void debugDistribution1(std::vector<K::Particle<MyState>> samples){
float min = +9999;
float max = -9999;
pDistributation1.clear();
for (int i = 0; i < samples.size(); ++i) {
//if (i % 10 != 0) {continue;}
double prob = samples[i].weight;
if (prob < min) {min = prob;}
if (prob > max) {max = prob;}
K::GnuplotPoint3 pos(samples[i].state.position.x_cm / 100.0f, samples[i].state.position.y_cm / 100.0f, samples[i].state.position.z_cm / 100.0f);
pDistributation1.add(pos, prob);
}
if (min == max) {min -= 1;}
gp << "set cbrange [" << min << ":" << max << "]\n";
}
void debugDistribution2(std::vector<K::Particle<MyState>> samples){
float min = +9999;
float max = -9999;
pDistributation2.clear();
for (int i = 0; i < samples.size(); ++i) {
if (i % 25 != 0) {continue;}
double prob = samples[i].weight;
if (prob < min) {min = prob;}
if (prob > max) {max = prob;}
K::GnuplotPoint3 pos(samples[i].state.position.x_cm / 100.0f, samples[i].state.position.y_cm / 100.0f, samples[i].state.position.z_cm / 100.0f);
pDistributation2.add(pos, prob);
}
if (min == max) {min -= 1;}
gp << "set cbrange [" << min << ":" << max << "]\n";
}
void drawNormalN1(Distribution::NormalDistributionN normParticle){
pNormal1.clear();
for (int i = 0; i < 100000; ++i) {
if (++i % 25 != 0) {continue;}
Eigen::VectorXd vec = normParticle.draw();
K::GnuplotPoint3 pos(vec.x(), vec.y(), vec.z());
pNormal1.add(pos);
}
}
void drawNormalN2(Distribution::NormalDistributionN normParticle){
pNormal2.clear();
for (int i = 0; i < 100000; ++i) {
if (++i % 25 != 0) {continue;}
Eigen::VectorXd vec = normParticle.draw();
K::GnuplotPoint3 pos(vec.x(), vec.y(), vec.z());
pNormal2.add(pos);
}
}
void debugWiFi(WiFiModelLogDistCeiling& model, const WiFiMeasurements& scan, const Timestamp curTS, const float z) {
WiFiObserverFree wiFiProbability(Settings::WiFiModel::sigma, model);