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

updated sensors and filter to current code version

Browse Source 
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
This commit is contained in:
toni
2018-07-12 18:39:27 +02:00
parent b4a1a3d969
commit 625f5fe04d
22 changed files with 325 additions and 261 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
nav/grid/RegionalResampling.h
View File

@@ -1,12 +1,12 @@
#ifndef REGIONALRESAMPLING_H
#define REGIONALRESAMPLING_H
#include <KLib/math/filter/particles/ParticleFilter.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResampling.h>
#include "State.h"
namespace GridBased {
class RegionalResampling : public K::ParticleFilterResampling<MyState> {
class RegionalResampling : public SMC::ParticleFilterResampling<MyState> {
public:
@@ -14,25 +14,25 @@ namespace GridBased {
RegionalResampling() {;}
void resample(std::vector<K::Particle<MyState>>& particles) override {
void resample(std::vector<SMC::Particle<MyState>>& particles) override {
Point3 sum;
for (const K::Particle<MyState>& p : particles) {
for (const SMC::Particle<MyState>& p : particles) {
sum += p.state.position.inMeter();
}
const Point3 avg = sum / particles.size();
std::vector<K::Particle<MyState>> next;
for (const K::Particle<MyState>& p : particles) {
std::vector<SMC::Particle<MyState>> next;
for (const SMC::Particle<MyState>& p : particles) {
const float dist = p.state.position.inMeter().getDistance(avg);
if (rand() % 6 != 0) {continue;}
if (dist < maxDist) {next.push_back(p);}
}
// cumulate
std::vector<K::Particle<MyState>> copy = particles;
std::vector<SMC::Particle<MyState>> copy = particles;
double cumWeight = 0;
for ( K::Particle<MyState>& p : copy) {
for ( SMC::Particle<MyState>& p : copy) {
cumWeight += p.weight;
p.weight = cumWeight;
}
@@ -50,7 +50,7 @@ namespace GridBased {
std::minstd_rand gen;
/** draw one particle according to its weight from the copy vector */
const K::Particle<MyState>& draw(std::vector<K::Particle<MyState>>& copy, const double cumWeight) {
const SMC::Particle<MyState>& draw(std::vector<SMC::Particle<MyState>>& copy, const double cumWeight) {
// generate random values between [0:cumWeight]
std::uniform_real_distribution<float> dist(0, cumWeight);
@@ -59,7 +59,7 @@ namespace GridBased {
const float rand = dist(gen);
// search comparator (cumWeight is ordered -> use binary search)
auto comp = [] (const K::Particle<MyState>& s, const float d) {return s.weight < d;};
auto comp = [] (const SMC::Particle<MyState>& s, const float d) {return s.weight < d;};
auto it = std::lower_bound(copy.begin(), copy.end(), rand, comp);
return *it;