FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

merged

Browse Source
This commit is contained in:
toni
2018-01-17 10:26:16 +01:00
parent 74981c6a45 55061ef0da
commit c02d07af51
67 changed files with 16100 additions and 2117 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
smc/smoothing/BackwardSimulation.h
View File

@@ -169,6 +169,7 @@ namespace SMC {
State est = estimation->estimate(smoothedParticles);
estimatedStates.push_back(est);
continue;
}
// compute weights using the transition model

16
smc/smoothing/FastKDESmoothing.h
View File

@@ -76,17 +76,17 @@ namespace SMC {
/** ctor */
FastKDESmoothing(int numParticles, const Floorplan::IndoorMap* map, const int gridsize_cm, const Point2 bandwith) {
this->numParticles = numParticles;
backwardParticles.reserve(numParticles);
firstFunctionCall = true;
//backwardParticles.reserve(numParticles);
this->firstFunctionCall = true;
const Point3 maxBB = FloorplanHelper::getBBox(map).getMax();
const Point3 minBB = FloorplanHelper::getBBox(map).getMin();
bb = BoundingBox<float>(minBB.x, maxBB.x, minBB.y, maxBB.y);
const Point3 maxBB = FloorplanHelper::getBBox(map).getMax() * 100.0;
const Point3 minBB = FloorplanHelper::getBBox(map).getMin() * 100.0;
this->bb = BoundingBox<float>(minBB.x, maxBB.x, minBB.y, maxBB.y);
// Create histogram
size_t nBinsX = static_cast<size_t>((maxBB.x - minBB.x) / gridsize_cm);
size_t nBinsY = static_cast<size_t>((maxBB.y - minBB.y) / gridsize_cm);
grid = Grid2D<float>(bb, nBinsX, nBinsY);
this->grid = Grid2D<float>(bb, nBinsX, nBinsY);
this->bandwith = bandwith;
}
@@ -186,7 +186,7 @@ namespace SMC {
State est = estimation->estimate(smoothedParticles);
estimatedStates.push_back(est);
return est;
continue;
}
// transition p(q_t+1* | q_t): so we are performing again a forward transition step.
@@ -215,7 +215,7 @@ namespace SMC {
smoothedParticles = forwardHistory.getParticleSet(i);
for(Particle<State> p : smoothedParticles){
p.weight = p.weight * grid.fetch(p.state.position.x_cm, p.state.position.y_cm);
Assert::isNot0(p.weight, "smoothed particle has zero weight");
//Assert::isNot0(p.weight, "smoothed particle has zero weight");
}
//normalization