555 lines
20 KiB
C++
555 lines
20 KiB
C++
#ifndef FLOGIC_H
|
|
#define FLOGIC_H
|
|
|
|
#include <Indoor/grid/Grid.h>
|
|
|
|
#include <Indoor/grid/walk/v2/GridWalker.h>
|
|
#include <Indoor/grid/walk/v2/GridWalkerMulti.h>
|
|
|
|
#include <Indoor/grid/walk/v2/modules/WalkModuleFollowDestination.h>
|
|
#include <Indoor/grid/walk/v2/modules/WalkModuleHeading.h>
|
|
#include <Indoor/grid/walk/v2/modules/WalkModuleHeadingControl.h>
|
|
#include <Indoor/grid/walk/v2/modules/WalkModuleHeadingVonMises.h>
|
|
#include <Indoor/grid/walk/v2/modules/WalkModuleNodeImportance.h>
|
|
#include <Indoor/grid/walk/v2/modules/WalkModuleSpread.h>
|
|
#include <Indoor/grid/walk/v2/modules/WalkModuleFavorZ.h>
|
|
#include <Indoor/grid/walk/v2/modules/WalkModulePreventVisited.h>
|
|
#include <Indoor/grid/walk/v2/modules/WalkModuleActivityControl.h>
|
|
#include <Indoor/sensors/radio/WiFiQualityAnalyzer.h>
|
|
|
|
#include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h>
|
|
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
|
|
#include <Indoor/sensors/radio/WiFiProbabilityGrid.h>
|
|
|
|
#include <Indoor/sensors/beacon/model/BeaconModelLogDistCeiling.h>
|
|
#include <Indoor/sensors/beacon/BeaconProbabilityFree.h>
|
|
|
|
#include <KLib/math/filter/particles/ParticleFilterMixing.h>
|
|
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
|
|
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h>
|
|
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingKLD.h>
|
|
|
|
#include <KLib/math/filter/smoothing/BackwardFilterTransition.h>
|
|
|
|
#include "Structs.h"
|
|
#include <omp.h>
|
|
#include "../Settings.h"
|
|
|
|
/** particle-filter init randomly distributed within the building*/
|
|
struct PFInit : public K::ParticleFilterInitializer<MyState> {
|
|
|
|
Grid<MyNode>& grid;
|
|
|
|
PFInit(Grid<MyNode>& grid) : grid(grid) {;}
|
|
|
|
virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
|
|
for (K::Particle<MyState>& p : particles) {
|
|
|
|
int idx = rand() % grid.getNumNodes();
|
|
p.state.position = grid[idx]; // random position
|
|
p.state.heading.direction = (rand() % 360) / 180.0 * M_PI; // random heading
|
|
p.state.heading.error = 0;
|
|
p.state.relativePressure = 0; // start with a relative pressure of 0
|
|
p.weight = 1.0 / particles.size(); // equal weight
|
|
|
|
}
|
|
}
|
|
|
|
};
|
|
|
|
/** particle-filter init with fixed position*/
|
|
struct PFInitFixed : public K::ParticleFilterInitializer<MyState> {
|
|
|
|
Grid<MyNode>& grid;
|
|
GridPoint startPos;
|
|
float headingDeg;
|
|
|
|
PFInitFixed(Grid<MyNode>& grid, GridPoint startPos, float headingDeg) :
|
|
grid(grid), startPos(startPos), headingDeg(headingDeg) {;}
|
|
|
|
virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
|
|
|
|
Distribution::Normal<float> norm(0.0f, 1.5f);
|
|
|
|
for (K::Particle<MyState>& p : particles) {
|
|
|
|
GridPoint pos = startPos + GridPoint(norm.draw(),norm.draw(),0.0f);
|
|
|
|
GridPoint startPos = grid.getNodeFor(pos);
|
|
p.state.position = startPos; // scatter arround the start position
|
|
p.state.heading.direction = headingDeg / 180.0 * M_PI; // fixed heading
|
|
p.state.heading.error = 0;
|
|
p.state.relativePressure = 0; // start with a relative pressure of 0
|
|
p.weight = 1.0 / particles.size(); // equal weight
|
|
}
|
|
}
|
|
|
|
};
|
|
|
|
/** very simple transition model, just scatter normal distributed */
|
|
struct PFTransSimple : public K::ParticleFilterTransition<MyState, MyControl>{
|
|
|
|
Grid<MyNode>& grid;
|
|
|
|
// define the noise
|
|
Distribution::Normal<float> noise_cm = Distribution::Normal<float>(0.0, Settings::IMU::stepLength * 2.0 * 100.0);
|
|
Distribution::Normal<float> height_m = Distribution::Normal<float>(0.0, 6.0);
|
|
|
|
// draw randomly from a vector
|
|
//random_selector<> rand;
|
|
|
|
// draw from 0 - 1
|
|
Distribution::Uniform<float> uniRand = Distribution::Uniform<float>(0,1);
|
|
|
|
/** ctor */
|
|
PFTransSimple(Grid<MyNode>& grid) : grid(grid) {}
|
|
|
|
virtual void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {
|
|
|
|
//int noNewPositionCounter = 0;
|
|
|
|
#pragma omp parallel for num_threads(6)
|
|
for (int i = 0; i < particles.size(); ++i) {
|
|
K::Particle<MyState>& p = particles[i];
|
|
|
|
// update the baromter
|
|
float deltaZ_cm = p.state.positionOld.inMeter().z - p.state.position.inMeter().z;
|
|
p.state.relativePressure += deltaZ_cm * 0.105f;
|
|
|
|
double diffHeight = p.state.position.inMeter().z + height_m.draw();
|
|
double newHeight_cm = p.state.position.z_cm;
|
|
if(diffHeight > 9.1){
|
|
newHeight_cm = 10.8 * 100.0;
|
|
} else if (diffHeight < 9.1 && diffHeight > 5.7){
|
|
newHeight_cm = 7.4 * 100.0;
|
|
} else if (diffHeight < 5.7 && diffHeight > 2.0) {
|
|
newHeight_cm = 4.0 * 100.0;
|
|
} else {
|
|
newHeight_cm = 0.0;
|
|
}
|
|
|
|
GridPoint noisePt(noise_cm.draw(), noise_cm.draw(), 0.0);
|
|
GridPoint newPosition = p.state.position + noisePt;
|
|
newPosition.z_cm = newHeight_cm;
|
|
|
|
// p.state.position = grid.getNearestNode(newPosition);
|
|
|
|
if(grid.hasNodeFor(newPosition)){
|
|
p.state.position = newPosition;
|
|
}else{
|
|
//no new position!
|
|
// #pragma omp atomic
|
|
// noNewPositionCounter++;
|
|
}
|
|
|
|
}
|
|
|
|
// std::cout << noNewPositionCounter << std::endl;
|
|
}
|
|
};
|
|
|
|
/** particle-filter transition */
|
|
struct PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
|
|
|
|
Grid<MyNode>& grid;
|
|
|
|
GridWalker<MyNode, MyState> walker;
|
|
|
|
WalkModuleHeading<MyNode, MyState> modHeadUgly; // stupid
|
|
WalkModuleHeadingControl<MyNode, MyState, MyControl> modHead;
|
|
WalkModuleHeadingVonMises<MyNode, MyState, MyControl> modHeadMises;
|
|
WalkModuleNodeImportance<MyNode, MyState> modImportance;
|
|
WalkModuleSpread<MyNode, MyState> modSpread;
|
|
WalkModuleFavorZ<MyNode, MyState> modFavorZ;
|
|
//WalkModulePreventVisited<MyNode, MyState> modPreventVisited;
|
|
|
|
//WalkModuleActivityControl<MyNode, MyState, MyControl> modActivity;
|
|
|
|
std::minstd_rand gen;
|
|
|
|
|
|
PFTrans(Grid<MyNode>& grid, MyControl* ctrl) : grid(grid), modHead(ctrl, Settings::IMU::turnSigma), modHeadMises(ctrl, Settings::IMU::turnSigma) {//, modPressure(ctrl, 0.100) {
|
|
|
|
walker.addModule(&modHead);
|
|
//walker.addModule(&modHeadMises);
|
|
//walker.addModule(&modSpread); // might help in some situations! keep in mind!
|
|
//walker.addModule(&modActivity);
|
|
//walker.addModule(&modHeadUgly);
|
|
walker.addModule(&modImportance);
|
|
//walker.addModule(&modFavorZ);
|
|
//walker.addModule(&modButterAct);
|
|
//walker.addModule(&modWifi);
|
|
//walker.addModule(&modPreventVisited);
|
|
}
|
|
|
|
virtual void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {
|
|
|
|
std::normal_distribution<float> noise(0, Settings::IMU::stepSigma);
|
|
|
|
for (K::Particle<MyState>& p : particles) {
|
|
|
|
//this is just for the smoothing transition... quick and dirty
|
|
p.state.headingChangeMeasured_rad = control->turnSinceLastTransition_rad;
|
|
|
|
// save old position
|
|
p.state.positionOld = p.state.position; //GridPoint(p.state.position.x_cm, p.state.position.y_cm, p.state.position.z_cm);
|
|
|
|
// update steps
|
|
const float dist_m = std::abs(control->numStepsSinceLastTransition * Settings::IMU::stepLength + noise(gen));
|
|
|
|
// update the particle in-place
|
|
p.state = walker.getDestination(grid, p.state, dist_m);
|
|
|
|
// update the baromter
|
|
float deltaZ_cm = p.state.positionOld.inMeter().z - p.state.position.inMeter().z;
|
|
p.state.relativePressure += deltaZ_cm * 0.105f;
|
|
|
|
}
|
|
}
|
|
|
|
};
|
|
|
|
/**
|
|
* particle-filter transition
|
|
* Adapting the Sample Size in Particle Filters Through KLD-Sampling - D. Fox
|
|
*/
|
|
struct PFTransKLDSampling : public K::ParticleFilterTransition<MyState, MyControl> {
|
|
|
|
Grid<MyNode>& grid;
|
|
|
|
GridWalker<MyNode, MyState> walker;
|
|
|
|
WalkModuleHeading<MyNode, MyState> modHeadUgly; // stupid
|
|
WalkModuleHeadingControl<MyNode, MyState, MyControl> modHead;
|
|
WalkModuleHeadingVonMises<MyNode, MyState, MyControl> modHeadMises;
|
|
WalkModuleNodeImportance<MyNode, MyState> modImportance;
|
|
WalkModuleSpread<MyNode, MyState> modSpread;
|
|
WalkModuleFavorZ<MyNode, MyState> modFavorZ;
|
|
//WalkModulePreventVisited<MyNode, MyState> modPreventVisited;
|
|
|
|
//WalkModuleActivityControl<MyNode, MyState, MyControl> modActivity;
|
|
|
|
std::minstd_rand gen;
|
|
|
|
/** upper bound epsilon of the kld distance - the particle size is not allowed to exceed epsilon*/
|
|
double epsilon;
|
|
|
|
/** the upper 1 - delta quantil of the normal distribution. something like 0.01 */
|
|
double delta;
|
|
|
|
/** the bins */
|
|
Binning<MyState> bins;
|
|
|
|
/** max particle size */
|
|
uint32_t N_max;
|
|
|
|
|
|
PFTransKLDSampling(Grid<MyNode>& grid, MyControl* ctrl) : grid(grid), modHead(ctrl, Settings::IMU::turnSigma), modHeadMises(ctrl, Settings::IMU::turnSigma) {//, modPressure(ctrl, 0.100) {
|
|
|
|
walker.addModule(&modHead);
|
|
//walker.addModule(&modHeadMises);
|
|
//walker.addModule(&modSpread); // might help in some situations! keep in mind!
|
|
//walker.addModule(&modActivity);
|
|
//walker.addModule(&modHeadUgly);
|
|
//walker.addModule(&modImportance);
|
|
//walker.addModule(&modFavorZ);
|
|
//walker.addModule(&modButterAct);
|
|
//walker.addModule(&modWifi);
|
|
//walker.addModule(&modPreventVisited);
|
|
|
|
|
|
epsilon = 0.15;
|
|
delta = 0.01;
|
|
N_max = 5000;
|
|
bins.setBinSizes({0.01, 0.01, 0.2, 0.3});
|
|
bins.setRanges({BinningRange(-1,100), BinningRange(-10,60), BinningRange(-1,15), BinningRange(0, 2 * M_PI)});
|
|
}
|
|
|
|
virtual void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {
|
|
|
|
std::normal_distribution<float> noise(0, Settings::IMU::stepSigma);
|
|
Distribution::Uniform<int> getParticle(0, particles.size()-1);
|
|
|
|
//init stuff
|
|
uint32_t n = 0;
|
|
uint32_t k = 1;
|
|
double N = 0;
|
|
|
|
//clear the bins
|
|
bins.clearUsed();
|
|
|
|
//create new particle set
|
|
std::vector<K::Particle<MyState>> particlesNew;
|
|
|
|
do{
|
|
|
|
//draw equally from the particle set
|
|
int particleIdx = getParticle.draw();
|
|
K::Particle<MyState>& p = particles[particleIdx];
|
|
|
|
//sample new particles based on the transition step
|
|
// save old position
|
|
p.state.positionOld = p.state.position; //GridPoint(p.state.position.x_cm, p.state.position.y_cm, p.state.position.z_cm);
|
|
|
|
// update steps
|
|
const float dist_m = std::abs(control->numStepsSinceLastTransition * Settings::IMU::stepLength + noise(gen));
|
|
|
|
// update the particle in-place
|
|
p.state = walker.getDestination(grid, p.state, dist_m);
|
|
|
|
// update the baromter
|
|
float deltaZ_cm = p.state.positionOld.inMeter().z - p.state.position.inMeter().z;
|
|
p.state.relativePressure += deltaZ_cm * 0.105f;
|
|
|
|
//if it falls into an empty bin then draw another particle
|
|
//is bin free?
|
|
if(bins.isFree(p.state)){
|
|
k++;
|
|
bins.markUsed(p.state);
|
|
|
|
//calculate the new N
|
|
double z_delta = K::NormalDistributionCDF::getProbit(1 - delta);
|
|
double front = (k - 1) / (2 * epsilon);
|
|
double back = 1 - (2 / (9 * (k - 1))) + (std::sqrt(2 / (9 * (k - 1))) * z_delta );
|
|
|
|
N = front * std::pow(back, 3.0);
|
|
}
|
|
++n;
|
|
|
|
//add particle to new particleset
|
|
particlesNew.push_back(p);
|
|
|
|
|
|
} while (n < N && n < N_max);
|
|
|
|
|
|
//write new particleset
|
|
particles.clear();
|
|
particles.swap(particlesNew);
|
|
}
|
|
|
|
};
|
|
|
|
|
|
struct BFTrans : public K::BackwardFilterTransition<MyState>{
|
|
|
|
|
|
public:
|
|
|
|
/**
|
|
* ctor
|
|
* @param choice the choice to use for randomly drawing nodes
|
|
* @param fp the underlying floorplan
|
|
*/
|
|
BFTrans()
|
|
{
|
|
//nothin
|
|
}
|
|
|
|
uint64_t ts = 0;
|
|
uint64_t deltaMS = 0;
|
|
|
|
/** set the current time in millisconds */
|
|
void setCurrentTime(const uint64_t ts) {
|
|
if (this->ts == 0) {
|
|
this->ts = ts;
|
|
deltaMS = 0;
|
|
} else {
|
|
deltaMS = this->ts - ts;
|
|
this->ts = ts;
|
|
}
|
|
}
|
|
|
|
|
|
/**
|
|
* smoothing transition starting at T with t, t-1,...0
|
|
* @param particles_new p_t (Forward Filter) p2
|
|
* @param particles_old p_t+1 (Smoothed Particles from Step before) p1
|
|
* q(p1 | p2) is calculated
|
|
*/
|
|
std::vector<std::vector<double>> transition(std::vector<K::Particle<MyState>>const& particles_new,
|
|
std::vector<K::Particle<MyState>>const& particles_old ) override {
|
|
|
|
|
|
// calculate alpha(m,n) = p(q_t+1(m) | q_t(n))
|
|
// this means, predict all possible states q_t+1 with all passible states q_t
|
|
// e.g. p(q_490(1)|q_489(1));p(q_490(1)|q_489(2)) ... p(q_490(1)|q_489(N)) and
|
|
// p(q_490(1)|q_489(1)); p(q_490(2)|q_489(1)) ... p(q_490(M)|q_489(1))
|
|
std::vector<std::vector<double>> predictionProbabilities;
|
|
|
|
omp_set_dynamic(0); // Explicitly disable dynamic teams
|
|
omp_set_num_threads(7);
|
|
#pragma omp parallel for shared(predictionProbabilities)
|
|
for (int i = 0; i < particles_old.size(); ++i) {
|
|
std::vector<double> innerVector;
|
|
auto p1 = &particles_old[i];
|
|
|
|
for(int j = 0; j < particles_new.size(); ++j){
|
|
|
|
auto p2 = &particles_new[j];
|
|
|
|
const double distance_m = p2->state.position.inMeter().getDistance(p1->state.position.inMeter()) / 100.0;
|
|
|
|
//TODO Incorporated Activity - see IPIN16 MySmoothingTransitionExperimental
|
|
|
|
const double distProb = K::NormalDistribution::getProbability(Settings::Smoothing::stepLength, Settings::Smoothing::stepSigma, distance_m);
|
|
|
|
// TODO: FIX THIS CORRECTLY is the heading change similiar to the measurement?
|
|
double diffRad = Angle::getDiffRAD_2PI_PI(p2->state.heading.direction.getRAD(), p1->state.heading.direction.getRAD());
|
|
double diffDeg = Angle::radToDeg(diffRad);
|
|
double measurementRad = Angle::makeSafe_2PI(p1->state.headingChangeMeasured_rad);
|
|
double measurementDeg = Angle::radToDeg(measurementRad);
|
|
const double headingProb = K::NormalDistribution::getProbability(measurementDeg, Settings::Smoothing::headingSigma, diffDeg);
|
|
|
|
// does the angle between two particles positions is similiar to the measurement
|
|
//double angleBetweenParticles = Angle::getDEG_360(p2->state.position.x, p2->state.position.y, p1->state.position.x, p1->state.position.y);
|
|
|
|
//check how near we are to the measurement
|
|
double diffZ = (p2->state.position.inMeter().z - p1->state.position.inMeter().z) / 100.0;
|
|
const double floorProb = K::NormalDistribution::getProbability(Settings::Smoothing::zChange, Settings::Smoothing::zSigma, diffZ);
|
|
|
|
//combine the probabilities
|
|
double prob = distProb;// * floorProb * headingProb;
|
|
innerVector.push_back(prob);
|
|
|
|
}
|
|
#pragma omp critical
|
|
predictionProbabilities.push_back(innerVector);
|
|
}
|
|
|
|
return predictionProbabilities;
|
|
|
|
}
|
|
};
|
|
|
|
|
|
struct PFEval : public K::ParticleFilterEvaluation<MyState, MyObs> {
|
|
|
|
WiFiModel& wifiModel;
|
|
WiFiObserverFree wiFiProbability; // free-calculation
|
|
//WiFiObserverGrid<MyNode> wiFiProbability; // grid-calculation
|
|
WiFiQualityAnalyzer wqa;
|
|
|
|
BeaconModelLogDistCeiling& beaconModel;
|
|
BeaconObserverFree beaconProbability;
|
|
|
|
Grid<MyNode>& grid;
|
|
|
|
PFEval(WiFiModel& wifiModel, BeaconModelLogDistCeiling& beaconModel, Grid<MyNode>& grid) :
|
|
wifiModel(wifiModel),
|
|
beaconModel(beaconModel),
|
|
grid(grid),
|
|
wiFiProbability(Settings::WiFiModel::sigma, wifiModel),
|
|
beaconProbability(Settings::BeaconModel::sigma, beaconModel){
|
|
|
|
}
|
|
|
|
/** probability step-distance */
|
|
//TODO: add number of recognized steps
|
|
inline double getStepDistanceProb(const Point3 particle1, const Point3 particle2){
|
|
double distance = particle1.getDistance(particle2);
|
|
return Distribution::Normal<double>::getProbability(Settings::IMU::stepLength, Settings::IMU::stepSigma + 0.4, distance);
|
|
}
|
|
|
|
//TODO: combinied evaluation heading and distance
|
|
|
|
/** probability for WIFI */
|
|
inline double getWIFI(const MyObs& observation, const WiFiMeasurements& vapWifi, const GridPoint& point) const {
|
|
|
|
const MyNode& node = grid.getNodeFor(point);
|
|
return wiFiProbability.getProbability(node, observation.currentTime, vapWifi);
|
|
}
|
|
|
|
/** probability for BEACONS */
|
|
inline double getBEACON(const MyObs& observation, const GridPoint& point){
|
|
|
|
//consider adding the persons height
|
|
Point3 p = point.inMeter() + Point3(0,0,1.3);
|
|
return beaconProbability.getProbability(p, observation.currentTime, observation.beacons);
|
|
}
|
|
|
|
/** probability for Barometer */
|
|
inline double getBaroPressure(const MyObs& observation, const float hPa) const{
|
|
return Distribution::Normal<double>::getProbability(static_cast<double>(hPa), 0.10, static_cast<double>(observation.relativePressure));
|
|
}
|
|
|
|
double getStairProb(const K::Particle<MyState>& p, const ActivityButterPressure::Activity act) {
|
|
|
|
const float kappa = 0.75;
|
|
|
|
const MyNode& gn = grid.getNodeFor(p.state.position);
|
|
switch (act) {
|
|
|
|
case ActivityButterPressure::Activity::STAY:
|
|
if (gn.getType() == GridNode::TYPE_FLOOR) {return kappa;}
|
|
if (gn.getType() == GridNode::TYPE_DOOR) {return kappa;}
|
|
{return 1-kappa;}
|
|
|
|
case ActivityButterPressure::Activity::UP:
|
|
case ActivityButterPressure::Activity::DOWN:
|
|
if (gn.getType() == GridNode::TYPE_STAIR) {return kappa;}
|
|
if (gn.getType() == GridNode::TYPE_ELEVATOR) {return kappa;}
|
|
{return 1-kappa;}
|
|
|
|
}
|
|
|
|
return 1.0;
|
|
|
|
}
|
|
|
|
virtual double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObs& observation) override {
|
|
|
|
double sum = 0;
|
|
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi);
|
|
|
|
wqa.add(wifiObs);
|
|
float quality = wqa.getQuality();
|
|
|
|
#pragma omp parallel for num_threads(3)
|
|
for (int i = 0; i < particles.size(); ++i) {
|
|
K::Particle<MyState>& p = particles[i];
|
|
|
|
Point3 pos_m = p.state.position.inMeter();
|
|
Point3 posOld_m = p.state.positionOld.inMeter();
|
|
|
|
double pWifi = getWIFI(observation, wifiObs, p.state.position);
|
|
const double pStairProb = getStairProb(p, observation.activity);
|
|
const double pStepDistance = getStepDistanceProb(pos_m, posOld_m);
|
|
const double pBaroPressure = getBaroPressure(observation, p.state.relativePressure);
|
|
//const double pBeacon = getBEACON(observation, p.state.position);
|
|
|
|
//small checks
|
|
_assertNotNAN(pWifi, "Wifi prob is nan");
|
|
_assertNot0(pBaroPressure,"pBaroPressure is null");
|
|
|
|
const bool volatile init = observation.currentTime.sec() < 25;
|
|
//double pWiFiMod = (init) ? (std::pow(pWiFi, 0.1)) : (std::pow(pWiFi, 0.5));
|
|
//double pWiFiMod = (init) ? (std::pow(pWifi, 0.5)) : (std::pow(pWifi, 0.9));
|
|
|
|
// bad wifi? -> we have no idea where we are!
|
|
if (quality < 0.25 && !init) {
|
|
//pWifi = 1;
|
|
//p.weight = std::pow(p.weight, 0.5);
|
|
}
|
|
|
|
const double prob = pWifi * pBaroPressure * pStairProb;
|
|
|
|
p.weight = prob;
|
|
|
|
#pragma omp atomic
|
|
sum += (prob);
|
|
|
|
}
|
|
|
|
if(sum == 0.0){
|
|
return 1.0;
|
|
}
|
|
|
|
return sum;
|
|
|
|
}
|
|
|
|
};
|
|
|
|
#endif // FLOGIC_H
|