diff --git a/CMakeLists.txt b/CMakeLists.txt index 18a9b20..51a1e60 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,46 +11,46 @@ SET( CMAKE_BUILD_TYPE "${CMAKE_BUILD_TYPE}" ) PROJECT(Indoor) IF(NOT CMAKE_BUILD_TYPE) - MESSAGE(STATUS "No build type selected. Default to Debug") - SET(CMAKE_BUILD_TYPE "Debug") +MESSAGE(STATUS "No build type selected. Default to Debug") +SET(CMAKE_BUILD_TYPE "Debug") ENDIF() INCLUDE_DIRECTORIES( - ../ + ../ ) FILE(GLOB HEADERS - ./*.h - ./*/*.h - ./*/*/*.h - ./*/*/*/*.h - ./*/*/*/*/*.h - ./*/*/*/*/*/*.h - ./*/*/*/*/*/*/*.h - ./tests/data/* - ./tests/data/*/* - ./tests/data/*/*/* + ./*.h + ./*/*.h + ./*/*/*.h + ./*/*/*/*.h + ./*/*/*/*/*.h + ./*/*/*/*/*/*.h + ./*/*/*/*/*/*/*.h + ./tests/data/* + ./tests/data/*/* + ./tests/data/*/*/* ) FILE(GLOB SOURCES - ./*.cpp - ./*/*.cpp - ./*/*/*.cpp - ./*/*/*/*.cpp - ./*/*/*/*/*.cpp - ./*/*/*/*/*/*.cpp + ./*.cpp + ./*/*.cpp + ./*/*/*.cpp + ./*/*/*/*.cpp + ./*/*/*/*/*.cpp + ./*/*/*/*/*/*.cpp ) FIND_PACKAGE( OpenMP REQUIRED) if(OPENMP_FOUND) - message("OPENMP FOUND") - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") - set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") + message("OPENMP FOUND") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") + set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") endif() if(${CMAKE_GENERATOR} MATCHES "Visual Studio") @@ -61,55 +61,53 @@ if(${CMAKE_GENERATOR} MATCHES "Visual Studio") -D_UNICODE -DNOGDI # -D_X86_ - ) +) - SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /permissive-") - SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /Zc:__cplusplus") - SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /std:c++17") - SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /Zc:twoPhase-") # disable two-phase name lookup due to OpenMP - - - #SET(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} ") - SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} /Zi /Oi /GL /Ot /Ox") - SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} /DEBUG") - SET(CMAKE_EXE_LINKER_FLAGS_RELEASE "${CMAKE_EXE_LINKER_FLAGS_RELEASE} /LTCG /INCREMENTAL:NO") +SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /permissive-") +SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /Zc:__cplusplus") +SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /std:c++17") +SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /Zc:twoPhase-") # disable two-phase name lookup due to OpenMP - set(CMAKE_CONFIGURATION_TYPES Release Debug) + +#SET(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} ") +SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} /Zi /Oi /GL /Ot /Ox") +SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} /DEBUG") +SET(CMAKE_EXE_LINKER_FLAGS_RELEASE "${CMAKE_EXE_LINKER_FLAGS_RELEASE} /LTCG /INCREMENTAL:NO") + +set(CMAKE_CONFIGURATION_TYPES Release Debug) else() # system specific compiler flags ADD_DEFINITIONS( - -std=gnu++17 + -std=gnu++17 - -Wall - -Werror=return-type - -Wextra - -Wpedantic + -Wall + -Werror=return-type + -Wextra + -Wpedantic - -fstack-protector-all - - -g3 - -O0 - #-O2 - -march=native - - -DWITH_TESTS - -DWITH_ASSERTIONS - -DWITH_DEBUG_LOG - -D_GLIBCXX_DEBUG + -fstack-protector-all + -g3 + #-O0 + #-O2 + -march=native + -DWITH_TESTS + -DWITH_ASSERTIONS + -DWITH_DEBUG_LOG + -D_GLIBCXX_DEBUG ) endif() # build a binary file ADD_EXECUTABLE( - ${PROJECT_NAME} - ${HEADERS} - ${SOURCES} + ${PROJECT_NAME} + ${HEADERS} + ${SOURCES} ) #SET(EXTRA_LIBS ${EXTRA_LIBS} nl-genl-3 nl-3) @@ -118,10 +116,10 @@ ADD_EXECUTABLE( # needed external libraries TARGET_LINK_LIBRARIES( - ${PROJECT_NAME} - gtest -# pthread - ${EXTRA_LIBS} + ${PROJECT_NAME} + gtest + pthread + ${EXTRA_LIBS} ) SET(CMAKE_C_COMPILER ${CMAKE_CXX_COMPILER}) diff --git a/main.cpp b/main.cpp index 376e399..739b985 100755 --- a/main.cpp +++ b/main.cpp @@ -98,7 +98,7 @@ int main(int argc, char** argv) { //::testing::GTEST_FLAG(filter) = "*RingBuffer*"; //::testing::GTEST_FLAG(filter) = "*Grid.*"; - //::testing::GTEST_FLAG(filter) = "*Dijkstra.*"; + ::testing::GTEST_FLAG(filter) = "*NavMeshDijkstra*"; //::testing::GTEST_FLAG(filter) = "*LogDistanceCeilingModelBeacon*"; //::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*"; @@ -111,7 +111,7 @@ int main(int argc, char** argv) { //::testing::GTEST_FLAG(filter) = "*Matrix4*"; //::testing::GTEST_FLAG(filter) = "*Sphere3*"; - ::testing::GTEST_FLAG(filter) = "*Quaternion*"; + //::testing::GTEST_FLAG(filter) = "*Quaternion*"; //::testing::GTEST_FLAG(filter) = "Timestamp*"; //::testing::GTEST_FLAG(filter) = "*RayTrace3*"; diff --git a/navMesh/meta/NavMeshDijkstra.h b/navMesh/meta/NavMeshDijkstra.h index 70dba0a..51fc846 100644 --- a/navMesh/meta/NavMeshDijkstra.h +++ b/navMesh/meta/NavMeshDijkstra.h @@ -161,6 +161,19 @@ namespace NM { float getWeightBetween(const TemporalNode& n1, const TemporalNode& n2) const {return n1.pt.getDistance(n2.pt);} }; + struct NodeAccessFavorCenter { + int getNumNeighbors(const TemporalNode& n) const {return n.neighbors.size();} + const TemporalNode* getNeighbor(const TemporalNode& n, const int idx) const {return n.neighbors[idx];} + float getWeightBetween(const TemporalNode& n1, const TemporalNode& n2) const { + + if(n2.pointIndex == 3){ + return n1.pt.getDistance(n2.pt); + } else { + return n1.pt.getDistance(n2.pt) / 0.2f; + } + } + }; + public: /** attach distance/triangle-to-target to the ToTarget struct */ @@ -187,7 +200,8 @@ namespace NM { // Node* end = nodes[0]; // TODO; - NodeAccess acc; + //NodeAccess acc; + NodeAccessFavorCenter acc; // dijkstra Dijkstra dijkstra; diff --git a/navMesh/walk/NavMeshWalkEval.h b/navMesh/walk/NavMeshWalkEval.h index 317a064..f02b530 100644 --- a/navMesh/walk/NavMeshWalkEval.h +++ b/navMesh/walk/NavMeshWalkEval.h @@ -151,7 +151,7 @@ namespace NM { /** - * higher probability for potints that approach the target location + * higher probability for points that approach the target location */ template class WalkEvalApproachesTarget : public NavMeshWalkEval { diff --git a/sensors/beacon/BeaconMeasurement.h b/sensors/beacon/BeaconMeasurement.h index bbb0526..86e7bde 100644 --- a/sensors/beacon/BeaconMeasurement.h +++ b/sensors/beacon/BeaconMeasurement.h @@ -48,6 +48,9 @@ public: /** get the rssi */ float getRSSI() const {return rssi;} + + /** set another signal strength */ + void setRssi(float value){rssi = value;} }; diff --git a/sensors/beacon/BeaconMeasurements.h b/sensors/beacon/BeaconMeasurements.h index 776df87..d25a7b8 100644 --- a/sensors/beacon/BeaconMeasurements.h +++ b/sensors/beacon/BeaconMeasurements.h @@ -24,11 +24,20 @@ struct BeaconMeasurements { /** remove entries older then 3000 ms*/ void removeOld(const Timestamp latestTS) { - auto lambda = [latestTS] (const BeaconMeasurement& e) { - Timestamp age = latestTS - e.getTimestamp(); - return age > Timestamp::fromMS(1000*3); - }; - entries.erase(std::remove_if(entries.begin(), entries.end(), lambda), entries.end()); + + std::vector::iterator it; + for (it = entries.begin(); it != entries.end(); ++it){ + if(latestTS - it->getTimestamp() > Timestamp::fromMS(1000*3)){ + entries.erase(it); + } + } + } + + void add(const BeaconMeasurement& entry){ + entries.push_back(entry); + + //remove entries that are to old (3000ms) + removeOld(entry.getTimestamp()); } }; diff --git a/sensors/beacon/setup/BeaconFingerprint.h b/sensors/beacon/setup/BeaconFingerprint.h new file mode 100644 index 0000000..22188d0 --- /dev/null +++ b/sensors/beacon/setup/BeaconFingerprint.h @@ -0,0 +1,103 @@ +#ifndef BEACONFINGERPRINT_H +#define BEACONFINGERPRINT_H + +#include "../../../geo/Point3.h" +#include "../BeaconMeasurements.h" + +#include + +/** + * denotes a wifi fingerprint: + * known position and 1-n measurements [wifi-scan on all channels] conducted at this position + * + * if more than one measurement is conducted, each AP is contained more than once! + */ +struct WiFiFingerprint { + + + /** real-world-position that was measured */ + Point3 pos_m; + + /** seen APs at the given location */ + BeaconMeasurements measurements; + + /** ctor */ + WiFiFingerprint() {;} + + /** ctor */ + WiFiFingerprint(const Point3 pos_m) : pos_m(pos_m) {;} + + /** ctor */ + WiFiFingerprint(const Point3 pos_m, const BeaconMeasurements& measurements) : pos_m(pos_m), measurements(measurements) {;} + + + /** as each AP might be contained more than once (scanned more than once), group them by MAC and use the average RSSI */ + BeaconMeasurements average() { + + // group scans by MAC (all measurements for one AP) + std::unordered_map group; + for (BeaconMeasurement& bm : measurements.entries) { + group[bm.getBeacon().getMAC()].entries.push_back(bm); + } + + // create the output that contains the AP's average + BeaconMeasurements res; + for (auto& it : group) { + const BeaconMeasurements& apMeasurements = it.second; + BeaconMeasurement avg = apMeasurements.entries.front(); // average starts with a copy of the first entry (to get all data-fields beside the rssi) + for (int i = 1; i < (int)apMeasurements.entries.size(); ++i) { // sum up all other entries [1:end] + avg.setRssi(avg.getRSSI() + apMeasurements.entries[i].getRSSI()); + } + avg.setRssi(avg.getRSSI() / apMeasurements.entries.size()); + res.entries.push_back(avg); // add to output + } + + // done + return res; + + } + + /** get all measurements for the given MAC */ + std::vector getAllForMAC(const MACAddress& mac) const { + std::vector res; + for (const BeaconMeasurement& m : measurements.entries) { + if (m.getBeacon().getMAC() == mac) {res.push_back(m);} + } + return res; + } + + /** serialize */ + void write(std::ostream& out) const { + out << "pos: " << pos_m.x << " " << pos_m.y << " " << pos_m.z << "\n"; + out << "num: " << measurements.entries.size() << "\n"; + for (const BeaconMeasurement& bm : measurements.entries) { + out << bm.getTimestamp().ms() << " " << bm.getBeacon().getMAC().asString() << " " << bm.getRSSI() << "\n"; + } + } + + /** deserialize */ + void read(std::istream& inp) { + std::string tmp; + + // read the position + inp >> tmp; if ("pos:" != tmp) {throw "error";} + inp >> pos_m.x >> pos_m.y >> pos_m.z; + + // number of entries + inp >> tmp; if ("num:" != tmp) {throw "error";} + int numEntries; inp >> numEntries; + + // read the entries + for (int i = 0; i < numEntries; ++i) { + uint64_t ms; inp >> ms; + std::string mac; inp >> mac; + float rssi; inp >> rssi; + BeaconMeasurement bm(Timestamp::fromMS(ms), Beacon(MACAddress(mac)), rssi); + measurements.entries.push_back(bm); + } + } + +}; + + +#endif // BEACONFINGERPRINT_H diff --git a/smc/filtering/ParticleFilter.h b/smc/filtering/ParticleFilter.h index 8d8e824..dae99e5 100644 --- a/smc/filtering/ParticleFilter.h +++ b/smc/filtering/ParticleFilter.h @@ -134,8 +134,7 @@ namespace SMC { Assert::isNotNull(evaluation, "evaluation MUST not be null! call setEvaluation() first!"); Assert::isNotNull(estimation, "estimation MUST not be null! call setEstimation() first!"); - // if the number of efficient particles is too low, perform resampling - if (lastNEff < particles.size() * nEffThresholdPercent) {resampler->resample(particles); } + // perform the transition step transition->transition(particles, control); @@ -143,6 +142,9 @@ namespace SMC { // perform the evaluation step and calculate the sum of all particle weights evaluation->evaluation(particles, observation); + // perform an additional step to prevent impoverishment of the particles + //enrichment->enrichment(particles, observation); + // normalize the particle weights and thereby calculate N_eff lastNEff = normalize(); @@ -151,6 +153,9 @@ namespace SMC { // estimate the current state const State est = estimation->estimate(particles); + // if the number of efficient particles is too low, perform resampling + if (lastNEff < particles.size() * nEffThresholdPercent) {resampler->resample(particles); } + // done return est; diff --git a/smc/filtering/ParticleFilterMixing.h b/smc/filtering/ParticleFilterMixing.h index fa3085c..98419c9 100644 --- a/smc/filtering/ParticleFilterMixing.h +++ b/smc/filtering/ParticleFilterMixing.h @@ -16,225 +16,225 @@ namespace SMC { - /** - * the main-class for the particle filter - * @param State the (user-defined) state for each particle - * @param Observation the observation (sensor) data - */ - template - class ParticleFilterMixing { + /** + * the main-class for the particle filter + * @param State the (user-defined) state for each particle + * @param Observation the observation (sensor) data + */ + template + class ParticleFilterMixing { - private: + private: - /** all used particles */ - std::vector> particles; + /** all used particles */ + std::vector> particles; - /** the current calculated estimation */ - State estimation; + /** the current calculated estimation */ + State estimation; - /** the resampler to use */ - std::shared_ptr> resampler; + /** the resampler to use */ + std::shared_ptr> resampler; - /** the estimation function to use */ - std::shared_ptr> estimator; + /** the estimation function to use */ + std::shared_ptr> estimator; - /** the transition function to use */ - std::shared_ptr> transition; + /** the transition function to use */ + std::shared_ptr> transition; - /** the evaluation function to use */ - std::shared_ptr> evaluation; + /** the evaluation function to use */ + std::shared_ptr> evaluation; - /** the initialization function to use */ - std::shared_ptr> initializer; + /** the initialization function to use */ + std::shared_ptr> initializer; - /** the percentage-of-efficient-particles-threshold for resampling */ - double nEffThresholdPercent = 0.25; + /** the percentage-of-efficient-particles-threshold for resampling */ + double nEffThresholdPercent = 0.25; - /** the current sum of all weights NOT normalized*/ - double weightSum = 1.0; + /** the current sum of all weights NOT normalized*/ + double weightSum = 1.0; - /** the predicted mode probability P(m_t|Z_t-1) */ - double predictedModeProbability = 1.0; + /** the predicted mode probability P(m_t|Z_t-1) */ + double predictedModeProbability = 1.0; - /** the posterior probability of the mode p(m_t | Z_t)*/ - double modePosteriorProbability = 1.0; + /** the posterior probability of the mode p(m_t | Z_t)*/ + double modePosteriorProbability = 1.0; - /** the transition mode probability P(m_t-1 | m_t, Z_t-1)*/ - double transitionModeProbability = 1.0; + /** the transition mode probability P(m_t-1 | m_t, Z_t-1)*/ + double transitionModeProbability = 1.0; - public: + public: - /** ctor - * NOTE: The modePosteriorProbability needs the be normalized depending on the number of filters within the IMMPF!! - */ - ParticleFilterMixing(const uint32_t numParticles, std::shared_ptr> initializer, double modePosteriorProbability) { - this->modePosteriorProbability = modePosteriorProbability; + /** ctor + * NOTE: The modePosteriorProbability needs the be normalized depending on the number of filters within the IMMPF!! + */ + ParticleFilterMixing(const uint32_t numParticles, std::shared_ptr> initializer, double modePosteriorProbability) { + this->modePosteriorProbability = modePosteriorProbability; - particles.resize(numParticles); - setInitializier(std::move(initializer)); - init(); - } + particles.resize(numParticles); + setInitializier(std::move(initializer)); + init(); + } - /** dtor */ - ~ParticleFilterMixing() { - ; - } + /** dtor */ + ~ParticleFilterMixing() { + ; + } - /** access to all particles */ - const std::vector>& getParticles() const { - return this->particles; - } + /** access to all particles */ + const std::vector>& getParticles() const { + return this->particles; + } - void setParticles(const std::vector>& newParticles){ - this->particles = newParticles; - } + void setParticles(const std::vector>& newParticles){ + this->particles = newParticles; + } - /** get the current estimation */ - const State getEstimation() const { - return estimation; - } + /** get the current estimation */ + const State getEstimation() const { + return estimation; + } - /** initialize/re-start the particle filter */ - void init() { - Assert::isNotNull(initializer, "initializer MUST not be null! call setInitializer() first!"); - initializer->initialize(particles); - } + /** initialize/re-start the particle filter */ + void init() { + Assert::isNotNull(initializer, "initializer MUST not be null! call setInitializer() first!"); + initializer->initialize(particles); + } - /** set the resampling method to use */ - void setResampling(std::shared_ptr> resampler) { - Assert::isNotNull(resampler, "setResampling() MUST not be called with a nullptr!"); - this->resampler = std::move(resampler); - } + /** set the resampling method to use */ + void setResampling(std::shared_ptr> resampler) { + Assert::isNotNull(resampler, "setResampling() MUST not be called with a nullptr!"); + this->resampler = std::move(resampler); + } - /** set the estimation method to use */ - void setEstimator(std::shared_ptr> estimator) { - Assert::isNotNull(estimator, "setEstimation() MUST not be called with a nullptr!"); - this->estimator = std::move(estimator); - } + /** set the estimation method to use */ + void setEstimator(std::shared_ptr> estimator) { + Assert::isNotNull(estimator, "setEstimation() MUST not be called with a nullptr!"); + this->estimator = std::move(estimator); + } - /** set the transition method to use */ - void setTransition(std::shared_ptr> transition) { - Assert::isNotNull(transition, "setTransition() MUST not be called with a nullptr!"); - this->transition = std::move(transition); - } + /** set the transition method to use */ + void setTransition(std::shared_ptr> transition) { + Assert::isNotNull(transition, "setTransition() MUST not be called with a nullptr!"); + this->transition = std::move(transition); + } - /** get the used transition method */ - ParticleFilterTransition* getTransition() { - return this->transition.get(); - } + /** get the used transition method */ + ParticleFilterTransition* getTransition() { + return this->transition.get(); + } - /** set the evaluation method to use */ - void setEvaluation(std::shared_ptr> evaluation) { - Assert::isNotNull(evaluation, "setEvaluation() MUST not be called with a nullptr!"); - this->evaluation = std::move(evaluation); - } + /** set the evaluation method to use */ + void setEvaluation(std::shared_ptr> evaluation) { + Assert::isNotNull(evaluation, "setEvaluation() MUST not be called with a nullptr!"); + this->evaluation = std::move(evaluation); + } - /** set the initialization method to use */ - void setInitializier(std::shared_ptr> initializer) { - Assert::isNotNull(initializer, "setInitializer() MUST not be called with a nullptr!"); - this->initializer = std::move(initializer); - } + /** set the initialization method to use */ + void setInitializier(std::shared_ptr> initializer) { + Assert::isNotNull(initializer, "setInitializer() MUST not be called with a nullptr!"); + this->initializer = std::move(initializer); + } - /** set the resampling threshold as the percentage of efficient particles */ - void setNEffThreshold(const double thresholdPercent) { - this->nEffThresholdPercent = thresholdPercent; - } + /** set the resampling threshold as the percentage of efficient particles */ + void setNEffThreshold(const double thresholdPercent) { + this->nEffThresholdPercent = thresholdPercent; + } - /** get the unormalized weight sum of all particles */ - double getWeightSum() const - { - return this->weightSum; - } + /** get the unormalized weight sum of all particles */ + double getWeightSum() const + { + return this->weightSum; + } - /** get the predicted mode probability P(m_t|Z_t-1)*/ - double getPredictedModeProbability() const - { - return this->predictedModeProbability; - } + /** get the predicted mode probability P(m_t|Z_t-1)*/ + double getPredictedModeProbability() const + { + return this->predictedModeProbability; + } - /** set the predicted mode probability P(m_t|Z_t-1)*/ - void setPredictedModeProbability(const double val) { - this->predictedModeProbability = val; - } + /** set the predicted mode probability P(m_t|Z_t-1)*/ + void setPredictedModeProbability(const double val) { + this->predictedModeProbability = val; + } - /** get the posterior mode probability P(m_t|Z_t)*/ - double getModePosteriorProbability() const - { - return this->modePosteriorProbability; - } + /** get the posterior mode probability P(m_t|Z_t)*/ + double getModePosteriorProbability() const + { + return this->modePosteriorProbability; + } - /** set the posterior mode probability P(m_t|Z_t)*/ - void setModePosteriorProbability(const double likelihoodSum) { + /** set the posterior mode probability P(m_t|Z_t)*/ + void setModePosteriorProbability(const double likelihoodSum) { - Assert::isNotNull(likelihoodSum, "likelihoodsum is zero.. thats not possible"); - Assert::isNotNull(this->weightSum, "weightSum is zero.. thats not possible"); - //Assert::isNotNull(this->predictedModeProbability, "predictedModeProbability is zero.. thats not possible"); + Assert::isNotNull(likelihoodSum, "likelihoodsum is zero.. thats not possible"); + Assert::isNotNull(this->weightSum, "weightSum is zero.. thats not possible"); + //Assert::isNotNull(this->predictedModeProbability, "predictedModeProbability is zero.. thats not possible"); - this->modePosteriorProbability = (this->weightSum * this->predictedModeProbability) / likelihoodSum; + this->modePosteriorProbability = (this->weightSum * this->predictedModeProbability) / likelihoodSum; - //Assert::isNotNull(this->modePosteriorProbability, "modePosteriorProbability is zero.. thats not possible"); - } + //Assert::isNotNull(this->modePosteriorProbability, "modePosteriorProbability is zero.. thats not possible"); + } - /** get the transition mode probability P(m_t|Z_t) - * NOTE: Dont use this value! It is only needed for more beatiful mixed sampling! - */ - double getTransitionModeProbability() const - { - return this->transitionModeProbability; - } + /** get the transition mode probability P(m_t|Z_t) + * NOTE: Dont use this value! It is only needed for more beautiful mixed sampling! + */ + double getTransitionModeProbability() const + { + return this->transitionModeProbability; + } - /** set the transition mode probability P(m_t|Z_t)*/ - void setTransitionModeProbability(const double val) { - this->transitionModeProbability = val; - } + /** set the transition mode probability P(m_t|Z_t)*/ + void setTransitionModeProbability(const double val) { + this->transitionModeProbability = val; + } - /** perform resampling -> transition -> evaluation -> estimation */ - void update(const Control* control, const Observation& observation) { + /** perform resampling -> transition -> evaluation -> estimation */ + void update(const Control* control, const Observation& observation) { - // sanity checks (if enabled) - Assert::isNotNull(resampler, "resampler MUST not be null! call setResampler() first!"); - Assert::isNotNull(transition, "transition MUST not be null! call setTransition() first!"); - Assert::isNotNull(evaluation, "evaluation MUST not be null! call setEvaluation() first!"); - Assert::isNotNull(estimator, "estimation MUST not be null! call setEstimation() first!"); + // sanity checks (if enabled) + Assert::isNotNull(resampler, "resampler MUST not be null! call setResampler() first!"); + Assert::isNotNull(transition, "transition MUST not be null! call setTransition() first!"); + Assert::isNotNull(evaluation, "evaluation MUST not be null! call setEvaluation() first!"); + Assert::isNotNull(estimator, "estimation MUST not be null! call setEstimation() first!"); - // perform the transition step - transition->transition(particles, control); + // perform the transition step + transition->transition(particles, control); - // perform the evaluation step and calculate the sum of all particle weights - this->weightSum = evaluation->evaluation(particles, observation); + // perform the evaluation step and calculate the sum of all particle weights + this->weightSum = evaluation->evaluation(particles, observation); - // normalize the particle weights and thereby calculate N_eff - const double neff = normalize(weightSum); + // normalize the particle weights and thereby calculate N_eff + const double neff = normalize(weightSum); - // estimate the current state - this->estimation = estimator->estimate(particles); + // estimate the current state + this->estimation = estimator->estimate(particles); - // if the number of efficient particles is too low, perform resampling - if (neff < particles.size() * nEffThresholdPercent) { resampler->resample(particles); } + // if the number of efficient particles is too low, perform resampling + if (neff < particles.size() * nEffThresholdPercent) { resampler->resample(particles); } - // done - } + // done + } - private: + private: - /** normalize the weight of all particles to one */ - double normalize(const double weightSum) { - double sum = 0.0; - for (auto& p : particles) { - p.weight /= weightSum; - sum += (p.weight * p.weight); - } - return 1.0 / sum; - } + /** normalize the weight of all particles to one */ + double normalize(const double weightSum) { + double sum = 0.0; + for (auto& p : particles) { + p.weight /= weightSum; + sum += (p.weight * p.weight); + } + return 1.0 / sum; + } - /** calculate the number of efficient particles (N_eff) */ - double getNeff() const { - double sum = 0.0; - for (auto& p : particles) {sum += (p.weight * p.weight);} - return 1.0 / sum; - } - }; + /** calculate the number of efficient particles (N_eff) */ + double getNeff() const { + double sum = 0.0; + for (auto& p : particles) {sum += (p.weight * p.weight);} + return 1.0 / sum; + } + }; } diff --git a/smc/filtering/resampling/ParticleFilterResamplingKDE.h b/smc/filtering/resampling/ParticleFilterResamplingKDE.h index 3fc3ed3..0bb263f 100644 --- a/smc/filtering/resampling/ParticleFilterResamplingKDE.h +++ b/smc/filtering/resampling/ParticleFilterResamplingKDE.h @@ -1,4 +1,4 @@ -#ifndef PARTICLEFILTERRESAMPLINGKDE_H +#ifndef PARTICLEFILTERRESAMPLINGKDE_H #define PARTICLEFILTERRESAMPLINGKDE_H #include @@ -19,103 +19,102 @@ namespace SMC { - /** - * Resample based on rapid KDE - */ - template - class ParticleFilterResamplingKDE : public ParticleFilterResampling { +/** + * Resample based on rapid KDE + */ +template +class ParticleFilterResamplingKDE : public ParticleFilterResampling { - private: +private: - /** this is a copy of the particle-set to draw from it */ - std::vector> particlesCopy; + /** this is a copy of the particle-set to draw from it */ + std::vector> particlesCopy; - /** random number generator */ - std::minstd_rand gen; + /** random number generator */ + std::minstd_rand gen; - /** boundingBox for the boxKDE */ - _BBox3 bb; + /** boundingBox for the boxKDE */ + _BBox3 bb; - /** histogram/grid holding the particles*/ - std::unique_ptr> grid; + /** histogram/grid holding the particles*/ + std::unique_ptr> grid; - /** bandwith for KDE */ - Point3 bandwith; + /** bandwith for KDE */ + Point3 bandwith; - /** the current mesh */ - const NM::NavMesh* mesh; + /** the current mesh */ + const NM::NavMesh* mesh; - public: +public: - /** ctor */ - ParticleFilterResamplingKDE(const NM::NavMesh* mesh, const Point3 gridsize_m, const Point3 bandwith) { + /** ctor */ + ParticleFilterResamplingKDE(const NM::NavMesh* mesh, const Point3 gridsize_m, const Point3 bandwith) { - this->mesh = mesh; - this->bandwith = bandwith; - this->bb = mesh->getBBox(); - this->bb.grow(10); + this->mesh = mesh; + this->bandwith = bandwith; + this->bb = mesh->getBBox();//_BBox3(mesh->getBBox().getMin() * 100.f, mesh->getBBox().getMax() * 100.f); + this->bb.grow(10); - // Create histogram - size_t nBinsX = (size_t)((this->bb.getMax().x - this->bb.getMin().x) / gridsize_m.x); - size_t nBinsY = (size_t)((this->bb.getMax().y - this->bb.getMin().y) / gridsize_m.y); - size_t nBinsZ = (size_t)((this->bb.getMax().z - this->bb.getMin().z) / gridsize_m.z); + // Create histogram + size_t nBinsX = (size_t)((this->bb.getMax().x - this->bb.getMin().x) / gridsize_m.x); + size_t nBinsY = (size_t)((this->bb.getMax().y - this->bb.getMin().y) / gridsize_m.y); + size_t nBinsZ = (size_t)((this->bb.getMax().z - this->bb.getMin().z) / gridsize_m.z); - this->grid = std::make_unique>(bb, nBinsX, nBinsY, nBinsZ); + //to centimeter + //bb.add(bb.getMin() * 100.0f); + //bb.add(bb.getMax() * 100.0f); - gen.seed(1234); - } + this->grid = std::make_unique>(bb, nBinsX, nBinsY, nBinsZ); - void resample(std::vector>& particles) override { + gen.seed(1234); + } - // compile-time sanity checks - static_assert( HasOperatorPlusEq::value, "your state needs a += operator!" ); - static_assert( HasOperatorDivEq::value, "your state needs a /= operator!" ); - static_assert( HasOperatorMul::value, "your state needs a * operator!" ); - //static_assert( std::is_constructible::value, "your state needs a constructor with Point3!"); - //todo: static assert for getx, gety, getz, setposition + void resample(std::vector>& particles) override { - grid->clear(); - for (Particle p : particles){ - //grid.add receives position in meter! + // compile-time sanity checks + static_assert( HasOperatorPlusEq::value, "your state needs a += operator!" ); + static_assert( HasOperatorDivEq::value, "your state needs a /= operator!" ); + static_assert( HasOperatorMul::value, "your state needs a * operator!" ); + //static_assert( std::is_constructible::value, "your state needs a constructor with Point3!"); + //todo: static assert for getx, gety, getz, setposition - //if weight is to low, remove. - if((float) p.weight > 0 ){ - grid->add(p.state.getX(), p.state.getY(), p.state.getZ(), p.weight); - } - } + grid->clear(); + for (Particle p : particles){ + //grid.add receives position in meter! - int nFilt = 3; - float sigmaX = bandwith.x / grid->binSizeX; - float sigmaY = bandwith.y / grid->binSizeY; - float sigmaZ = bandwith.z / grid->binSizeZ; + //if weight is to low, remove. + if((float) p.weight > 0 ){ + grid->add(p.state.getX(), p.state.getY(), p.state.getZ(), p.weight); + } + } - BoxGaus3D boxGaus; - boxGaus.approxGaus(grid->image(), Point3(sigmaX, sigmaY, sigmaZ), nFilt); + int nFilt = 3; + float sigmaX = bandwith.x / grid->binSizeX; + float sigmaY = bandwith.y / grid->binSizeY; + float sigmaZ = bandwith.z / grid->binSizeZ; - // fill a drawlist with N equal distributed particles - // assign them a weight based on the KDE density. - DrawList dl; - for (int i = 0; i < 10000; ++i){ - NM::NavMeshLocation tmpPos = mesh->getRandom().draw(); - float weight = grid->fetch(tmpPos.pos.x, tmpPos.pos.y, tmpPos.pos.z); + BoxGaus3D boxGaus; + boxGaus.approxGaus(grid->image(), Point3(sigmaX, sigmaY, sigmaZ), nFilt); - dl.add(tmpPos.pos, weight); - } + // fill a drawlist with N equal distributed particles + // assign them a weight based on the KDE density. + DrawList dl; + for (int i = 0; i < 10000; ++i){ + NM::NavMeshLocation tmpPos = mesh->getRandom().draw(); //TODO: Hier kommen oft seltsame "Randomwerte" raus, die nur innerhalb eines Dreiecks sind. Woran liegt das? + float weight = grid->fetch(tmpPos.pos.x, tmpPos.pos.y, tmpPos.pos.z); - // used the same particles to not lose the heading. + dl.add(tmpPos.pos, weight); + } - // TODO: Summe aller Partikel wird relativ schnell 0! Ich vermute da am Anfang ein einzelner Partikel stark gewichtet ist und alleine - // die Dichte repräsentiert über die KDE. Jetzt wird beim nächsten zufälligen ziehen an dieser Stelle keiner der 10k partikel dort gezogen, d.h. alle - // haben ein Gewicht von 0 und ciao. - // Lösung: erstmal equal weight versuchen. ansonten: warum nehmen wir nochmal diese 10k? und nciht einfach die partikel die wir eh schon haben? - for (int i = 0; i < particles.size(); ++i){ + // used the same particles to not lose the heading. + for (int i = 0; i < particles.size(); ++i){ - double tmpWeight = 1; - particles[i].state.pos = mesh->getLocation(dl.get(tmpWeight)); - particles[i].weight = tmpWeight; - } - } - }; + double tmpWeight = 1; + particles[i].state.pos = mesh->getLocation(dl.get(tmpWeight)); + particles[i].weight = tmpWeight; + } + } +}; } diff --git a/smc/merging/mixing/MixingSamplerDivergency.h b/smc/merging/mixing/MixingSamplerDivergency.h index 88e4663..1edbb2c 100644 --- a/smc/merging/mixing/MixingSamplerDivergency.h +++ b/smc/merging/mixing/MixingSamplerDivergency.h @@ -100,7 +100,7 @@ namespace SMC { auto mode = drawMode(cumTransitionModeProbability); newParticles[k] = drawParticle(mode); - newParticles[k].weight = equalWeight; + newParticles[k].weight = equalWeight; //todo: why equal weight? i guess if the different filters have different representations of probability? } focusedFilter.setParticles(newParticles); @@ -133,7 +133,7 @@ namespace SMC { /** draw one particle according to its weight from the copy vector of a given mode */ const Particle& drawParticle(ParticleFilterMixing& filter) { - double weights = filter.getParticles().back().weight; + //double weights = filter.getParticles().back().weight; // generate random values between [0:cumWeight] std::uniform_real_distribution dist(0, filter.getParticles().back().weight); diff --git a/tests/navMesh/TestNavMeshDijkstra.cpp b/tests/navMesh/TestNavMeshDijkstra.cpp index 625841e..c4d2d03 100644 --- a/tests/navMesh/TestNavMeshDijkstra.cpp +++ b/tests/navMesh/TestNavMeshDijkstra.cpp @@ -79,16 +79,15 @@ TEST(NavMeshDijkstra, build) { } -TEST(NavMeshDijkstra, build2) { +TEST(NavMeshDijkstra, museum) { NavMeshSettings set; NavMesh nm; //Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile("/apps/paper/diss/data/maps/map_stair1.xml"); //Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile("/apps/paper/diss/data/maps/SHL41_nm.xml"); - Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile("/apps/paper/diss/data/maps/museum31.xml"); - - + //Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile("/apps/paper/diss/data/maps/museum31.xml"); + Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile("/home/toni/Documents/programme/localization/maps/museum/map48_ap_path3_smallfixes.xml"); NavMeshFactory fac(&nm, set); fac.build(map); @@ -97,11 +96,32 @@ TEST(NavMeshDijkstra, build2) { dbg.addMesh(nm); dbg.draw(); - NM::NavMeshDijkstra::stamp(nm, Point3(4,4,0)); + NM::NavMeshRandom rnd = nm.getRandom(); + const Point3 dst = Point3(24,24,1.7f); + NM::NavMeshDijkstra::stamp(nm, dst); dbg.addDijkstra(nm); dbg.draw(); + for (int i = 0; i < 1000; ++i) { + + NM::NavMeshLocation start = rnd.draw(); + start.tria->getDistanceToDestination(start.pos); // just a compiler-test + + NM::NavMeshDijkstra::stamp(nm, dst); + + //NM::NavMeshLocation start = start;//nm.getLocation(Point3(0,-6,0)); + std::vector> path = start.tria->getPathToDestination(start.pos); + std::cout << path.size() << std::endl; + + dbg.addDijkstra(nm); + dbg.addDijkstra(path); + dbg.draw(); + + sleep(1); + + } + int xxx = 0; (void) xxx; } @@ -144,7 +164,8 @@ TEST(NavMeshDijkstra, path) { // remove2.poly.points.push_back(Point2(-11,+2)); //Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile("/mnt/vm/paper/diss/data/maps/map_stair1.xml"); - Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile("/apps/paper/diss/data/maps/map_stair1.xml"); + //Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile("/apps/paper/diss/data/maps/map_stair1.xml"); + Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile("/home/toni/Documents/programme/localization/maps/test/map_stair1.xml"); NavMeshFactory fac(&nm, set);