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

tabbing...

Browse Source
This commit is contained in:
toni
2018-11-24 17:21:49 +01:00
parent 9de1e7142b
commit cb61e0fe68
3 changed files with 19 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
navMesh/walk/NavMeshWalkSimple.h
View File

@@ -70,7 +70,7 @@ namespace NM {
// to-be-walked distance; // to-be-walked distance;
const float toBeWalkedDist = params.getToBeWalkedDistance(); const float toBeWalkedDist = params.getToBeWalkedDistance();
const float toBeWalkedDistSafe = 0.75 + toBeWalkedDist * 1.1; const float toBeWalkedDistSafe = 0.75 + toBeWalkedDist * 1.1;
// construct reachable region // construct reachable region
NavMeshSub<Tria> reachable(params.start, toBeWalkedDistSafe); NavMeshSub<Tria> reachable(params.start, toBeWalkedDistSafe);
@@ -84,23 +84,23 @@ namespace NM {
// is above destination reachable? // is above destination reachable?
if (dstTria) { if (dstTria) {
re.heading = params.heading; // heading was OK -> keep re.heading = params.heading; // heading was OK -> keep
re.location.pos = dstTria->toPoint3(dst); // new destination position re.location.pos = dstTria->toPoint3(dst); // new destination position
re.location.tria = dstTria; // new destination triangle re.location.tria = dstTria; // new destination triangle
++hits; ++hits;
} else { } else {
NavMeshRandom<Tria> rnd = reachable.getRandom(); // random-helper NavMeshRandom<Tria> rnd = reachable.getRandom(); // random-helper
re.location = rnd.draw(); // get a random destianation re.location = rnd.draw(); // get a random destianation
re.heading = Heading(params.start.pos.xy(), re.location.pos.xy()); // update the heading re.heading = Heading(params.start.pos.xy(), re.location.pos.xy()); // update the heading
++misses; ++misses;
} }
const int total = (hits + misses); const int total = (hits + misses);
if (total % 10000 == 0) { if (total % 10000 == 0) {
//std::cout << "hits: " << (hits*100/total) << "%" << std::endl; //std::cout << "hits: " << (hits*100/total) << "%" << std::endl;
} }
// calculate probability // calculate probability

2
smc/Particle.h
View File

@@ -29,7 +29,7 @@ namespace SMC {
State state; State state;
/** the (current) probability for this state */ /** the (current) probability for this state */
double weight; double weight;
/** empty ctor */ /** empty ctor */

20
smc/filtering/ParticleFilter.h
View File

@@ -138,13 +138,13 @@ namespace SMC {
if (lastNEff < particles.size() * nEffThresholdPercent) {resampler->resample(particles); } if (lastNEff < particles.size() * nEffThresholdPercent) {resampler->resample(particles); }
// perform the transition step // perform the transition step
transition->transition(particles, control); transition->transition(particles, control);
// perform the evaluation step and calculate the sum of all particle weights // perform the evaluation step and calculate the sum of all particle weights
evaluation->evaluation(particles, observation); evaluation->evaluation(particles, observation);
// normalize the particle weights and thereby calculate N_eff // normalize the particle weights and thereby calculate N_eff
lastNEff = normalize(); lastNEff = normalize();
//std::cout << "normalized. n_eff is " << lastNEff << std::endl; //std::cout << "normalized. n_eff is " << lastNEff << std::endl;
@@ -160,7 +160,7 @@ namespace SMC {
void updateTransitionOnly(const Control* control) { void updateTransitionOnly(const Control* control) {
// sanity checks (if enabled) // sanity checks (if enabled)
Assert::isNotNull(transition, "transition MUST not be null! call setTransition() first!"); Assert::isNotNull(transition, "transition MUST not be null! call setTransition() first!");
// perform the transition step // perform the transition step
transition->transition(particles, control); transition->transition(particles, control);
@@ -171,13 +171,13 @@ namespace SMC {
State updateEvaluationOnly(const Observation& observation) { State updateEvaluationOnly(const Observation& observation) {
// sanity checks (if enabled) // sanity checks (if enabled)
Assert::isNotNull(resampler, "resampler MUST not be null! call setResampler() first!"); Assert::isNotNull(resampler, "resampler MUST not be null! call setResampler() first!");
Assert::isNotNull(evaluation, "evaluation MUST not be null! call setEvaluation() first!"); Assert::isNotNull(evaluation, "evaluation MUST not be null! call setEvaluation() first!");
Assert::isNotNull(estimation, "estimation MUST not be null! call setEstimation() first!"); Assert::isNotNull(estimation, "estimation MUST not be null! call setEstimation() first!");
// if the number of efficient particles is too low, perform resampling // if the number of efficient particles is too low, perform resampling
if (lastNEff < particles.size() * nEffThresholdPercent) { resampler->resample(particles); } if (lastNEff < particles.size() * nEffThresholdPercent) { resampler->resample(particles); }
//resampler->resample(particles); //resampler->resample(particles);
// perform the evaluation step and calculate the sum of all particle weights // perform the evaluation step and calculate the sum of all particle weights
evaluation->evaluation(particles, observation); evaluation->evaluation(particles, observation);
@@ -188,7 +188,7 @@ namespace SMC {
//Assert::isNotNull(weightSum, "sum of all particle weights (returned from eval) is 0.0!"); //Assert::isNotNull(weightSum, "sum of all particle weights (returned from eval) is 0.0!");
// normalize the particle weights and thereby calculate N_eff // normalize the particle weights and thereby calculate N_eff
lastNEff = normalize(); lastNEff = normalize();
// estimate the current state // estimate the current state
const State est = estimation->estimate(particles); const State est = estimation->estimate(particles);