154 lines
4.7 KiB
C++
154 lines
4.7 KiB
C++
#ifndef NAVMESHWALKWIFIREGIONAL_H
|
|
#define NAVMESHWALKWIFIREGIONAL_H
|
|
|
|
#include <vector>
|
|
|
|
#include "../NavMesh.h"
|
|
#include "../NavMeshLocation.h"
|
|
#include "../../geo/Heading.h"
|
|
|
|
#include "../../sensors/radio/WiFiMeasurements.h"
|
|
#include <Indoor/sensors/radio/setup/WiFiOptimizerLogDistCeiling.h>
|
|
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
|
|
|
|
#include "NavMeshSub.h"
|
|
#include "NavMeshWalkParams.h"
|
|
#include "NavMeshWalkEval.h"
|
|
|
|
namespace NM {
|
|
|
|
/** simple walker extended by a wifi anti impoverishment method
|
|
* draw 10000 random particles within a specific region arround
|
|
* the last estimation equaly.
|
|
* evaluate them with wifi and then draw cumulative from a list
|
|
* instead of killing the particles who walk against walls
|
|
* the number of new particles is restricted by ??? */
|
|
template <typename Tria> class NavMeshWalkWifiRegional {
|
|
|
|
public:
|
|
/** single result */
|
|
struct ResultEntry {
|
|
NavMeshLocation<Tria> location;
|
|
Heading heading;
|
|
double probability;
|
|
ResultEntry() : heading(0) {;}
|
|
};
|
|
|
|
private:
|
|
|
|
const NavMesh<Tria>& mesh;
|
|
|
|
std::vector<NavMeshWalkEval<Tria>*> evals;
|
|
|
|
std::vector<ResultEntry> wifiSamples;
|
|
WiFiModel& wifiModel;
|
|
WiFiObserverFree wifiProbability;
|
|
double wifiCumWeight;
|
|
DrawList<NavMeshLocation<Tria>> wifiSamplesDrawList;
|
|
|
|
int hits = 0;
|
|
int misses = 0;
|
|
|
|
public:
|
|
|
|
/** list of results */
|
|
using ResultList = std::vector<ResultEntry>;
|
|
|
|
/** ctor */
|
|
NavMeshWalkWifiRegional(const NavMesh<Tria>& mesh, WiFiModel& wifiModel) : mesh(mesh),
|
|
wifiModel(wifiModel),
|
|
wifiProbability(Settings::WiFiModel::sigma, wifiModel){
|
|
|
|
}
|
|
|
|
/** add a new evaluator to the walker */
|
|
void addEvaluator(NavMeshWalkEval<Tria>* eval) {
|
|
this->evals.push_back(eval);
|
|
}
|
|
|
|
/** update every transition step the WiFi */
|
|
void updateWiFi(const WiFiMeasurements& wifiObs, const Timestamp currentTime, const Point3 lastEst){
|
|
|
|
this->wifiSamplesDrawList.reset();
|
|
|
|
//todo: restrict this to specific region
|
|
NavMeshRandom<Tria> rnd = mesh.getRandom();
|
|
wifiCumWeight = 0;
|
|
for(int i = 0; i < 10000; ++i){
|
|
|
|
NavMeshLocation<Tria> tmpLocation = rnd.drawWithin(lastEst, 10.0);
|
|
double weight = wifiProbability.getProbability(tmpLocation.pos, currentTime, wifiObs);
|
|
|
|
this->wifiSamplesDrawList.add(tmpLocation, weight);
|
|
}
|
|
}
|
|
|
|
ResultEntry getOne(const NavMeshWalkParams<Tria>& params) {
|
|
|
|
// sanity checks
|
|
params.check();
|
|
|
|
ResultEntry re;
|
|
|
|
// to-be-walked distance;
|
|
const float toBeWalkedDist = params.getToBeWalkedDistance();
|
|
const float toBeWalkedDistSafe = 0.75 + toBeWalkedDist * 1.1;
|
|
|
|
// construct reachable region
|
|
NavMeshSub<Tria> reachable(params.start, toBeWalkedDistSafe);
|
|
|
|
// get the to-be-reached destination's position (using start+distance+heading)
|
|
const Point2 dir = params.heading.asVector();
|
|
const Point2 dst = params.start.pos.xy() + (dir * toBeWalkedDist);
|
|
|
|
const Tria* dstTria = reachable.getContainingTriangle(dst);
|
|
|
|
// is above destination reachable?
|
|
if (dstTria) {
|
|
|
|
re.heading = params.heading; // heading was OK -> keep
|
|
re.location.pos = dstTria->toPoint3(dst); // new destination position
|
|
re.location.tria = dstTria; // new destination triangle
|
|
re.probability = 1.0;
|
|
++hits;
|
|
|
|
} else {
|
|
re.location = wifiSamplesDrawList.get();
|
|
re.heading = Heading(params.start.pos.xy(), re.location.pos.xy());
|
|
re.probability = 0.1;
|
|
++misses;
|
|
}
|
|
|
|
const int total = (hits + misses);
|
|
if (total % 10000 == 0) {
|
|
//std::cout << "hits: " << (hits*100/total) << "%" << std::endl;
|
|
}
|
|
|
|
// calculate probability
|
|
/*
|
|
const NavMeshPotentialWalk<Tria> pwalk(params, re.location);
|
|
re.probability = 1.0;
|
|
for (const NavMeshWalkEval<Tria>* eval : evals) {
|
|
const double p1 = eval->getProbability(pwalk);
|
|
re.probability *= p1;
|
|
}
|
|
*/
|
|
|
|
// done
|
|
return re;
|
|
|
|
}
|
|
|
|
ResultList getMany(const NavMeshWalkParams<Tria>& params) {
|
|
|
|
// sanity checks
|
|
params.check();
|
|
|
|
return {getOne(params)};
|
|
}
|
|
};
|
|
|
|
}
|
|
|
|
#endif // NAVMESHWALKWIFIREGIONAL_H
|