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

added stuff for bluetooth

Browse Source 
worked on resampling methods
This commit is contained in:
mail@toni-fetzer.de
2019-06-05 18:09:15 +02:00
parent cb61e0fe68
commit 8d37e94647
12 changed files with 472 additions and 320 deletions
Download Patch File Download Diff File Expand all files Collapse all files

110
CMakeLists.txt
View File

@@ -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 "${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_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)
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})

4
main.cpp
View File

@@ -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*";

16
navMesh/meta/NavMeshDijkstra.h
View File

@@ -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<TemporalNode> dijkstra;

2
navMesh/walk/NavMeshWalkEval.h
View File

@@ -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 <typename Tria> class WalkEvalApproachesTarget : public NavMeshWalkEval<Tria> {

3
sensors/beacon/BeaconMeasurement.h
View File

@@ -48,6 +48,9 @@ public:
/** get the rssi */
float getRSSI() const {return rssi;}
/** set another signal strength */
void setRssi(float value){rssi = value;}
};

19
sensors/beacon/BeaconMeasurements.h
View File

@@ -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<BeaconMeasurement>::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());
}
};

103
sensors/beacon/setup/BeaconFingerprint.h Normal file
View File

@@ -0,0 +1,103 @@
#ifndef BEACONFINGERPRINT_H
#define BEACONFINGERPRINT_H
#include "../../../geo/Point3.h"
#include "../BeaconMeasurements.h"
#include <unordered_map>
/**
* 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<MACAddress, BeaconMeasurements> 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<BeaconMeasurement> getAllForMAC(const MACAddress& mac) const {
std::vector<BeaconMeasurement> 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

9
smc/filtering/ParticleFilter.h
View File

@@ -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;

338
smc/filtering/ParticleFilterMixing.h
View File

@@ -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 <typename State, typename Control, typename Observation>
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 <typename State, typename Control, typename Observation>
class ParticleFilterMixing {
private:
private:
/** all used particles */
std::vector<Particle<State>> particles;
/** all used particles */
std::vector<Particle<State>> particles;
/** the current calculated estimation */
State estimation;
/** the current calculated estimation */
State estimation;
/** the resampler to use */
std::shared_ptr<ParticleFilterResampling<State>> resampler;
/** the resampler to use */
std::shared_ptr<ParticleFilterResampling<State>> resampler;
/** the estimation function to use */
std::shared_ptr<ParticleFilterEstimation<State>> estimator;
/** the estimation function to use */
std::shared_ptr<ParticleFilterEstimation<State>> estimator;
/** the transition function to use */
std::shared_ptr<ParticleFilterTransition<State, Control>> transition;
/** the transition function to use */
std::shared_ptr<ParticleFilterTransition<State, Control>> transition;
/** the evaluation function to use */
std::shared_ptr<ParticleFilterEvaluation<State, Observation>> evaluation;
/** the evaluation function to use */
std::shared_ptr<ParticleFilterEvaluation<State, Observation>> evaluation;
/** the initialization function to use */
std::shared_ptr<ParticleFilterInitializer<State>> initializer;
/** the initialization function to use */
std::shared_ptr<ParticleFilterInitializer<State>> 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<ParticleFilterInitializer<State>> 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<ParticleFilterInitializer<State>> 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<Particle<State>>& getParticles() const {
return this->particles;
}
/** access to all particles */
const std::vector<Particle<State>>& getParticles() const {
return this->particles;
}
void setParticles(const std::vector<Particle<State>>& newParticles){
this->particles = newParticles;
}
void setParticles(const std::vector<Particle<State>>& 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<ParticleFilterResampling<State>> 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<ParticleFilterResampling<State>> 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<ParticleFilterEstimation<State>> 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<ParticleFilterEstimation<State>> 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<ParticleFilterTransition<State, Control>> 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<ParticleFilterTransition<State, Control>> transition) {
Assert::isNotNull(transition, "setTransition() MUST not be called with a nullptr!");
this->transition = std::move(transition);
}
/** get the used transition method */
ParticleFilterTransition<State, Control>* getTransition() {
return this->transition.get();
}
/** get the used transition method */
ParticleFilterTransition<State, Control>* getTransition() {
return this->transition.get();
}
/** set the evaluation method to use */
void setEvaluation(std::shared_ptr<ParticleFilterEvaluation<State, Observation>> 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<ParticleFilterEvaluation<State, Observation>> 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<ParticleFilterInitializer<State>> 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<ParticleFilterInitializer<State>> 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;
}
};
}

147
smc/filtering/resampling/ParticleFilterResamplingKDE.h
View File

@@ -1,4 +1,4 @@
#ifndef PARTICLEFILTERRESAMPLINGKDE_H
#ifndef PARTICLEFILTERRESAMPLINGKDE_H
#define PARTICLEFILTERRESAMPLINGKDE_H
#include <algorithm>
@@ -19,103 +19,102 @@
namespace SMC {
/**
* Resample based on rapid KDE
*/
template <typename State, typename Tria>
class ParticleFilterResamplingKDE : public ParticleFilterResampling<State> {
/**
* Resample based on rapid KDE
*/
template <typename State, typename Tria>
class ParticleFilterResamplingKDE : public ParticleFilterResampling<State> {
private:
private:
/** this is a copy of the particle-set to draw from it */
std::vector<Particle<State>> particlesCopy;
/** this is a copy of the particle-set to draw from it */
std::vector<Particle<State>> particlesCopy;
/** random number generator */
std::minstd_rand gen;
/** random number generator */
std::minstd_rand gen;
/** boundingBox for the boxKDE */
_BBox3<float> bb;
/** boundingBox for the boxKDE */
_BBox3<float> bb;
/** histogram/grid holding the particles*/
std::unique_ptr<Grid3D<float>> grid;
/** histogram/grid holding the particles*/
std::unique_ptr<Grid3D<float>> grid;
/** bandwith for KDE */
Point3 bandwith;
/** bandwith for KDE */
Point3 bandwith;
/** the current mesh */
const NM::NavMesh<Tria>* mesh;
/** the current mesh */
const NM::NavMesh<Tria>* mesh;
public:
public:
/** ctor */
ParticleFilterResamplingKDE(const NM::NavMesh<Tria>* mesh, const Point3 gridsize_m, const Point3 bandwith) {
/** ctor */
ParticleFilterResamplingKDE(const NM::NavMesh<Tria>* 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<float>(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<Grid3D<float>>(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<Grid3D<float>>(bb, nBinsX, nBinsY, nBinsZ);
void resample(std::vector<Particle<State>>& particles) override {
gen.seed(1234);
}
// compile-time sanity checks
static_assert( HasOperatorPlusEq<State>::value, "your state needs a += operator!" );
static_assert( HasOperatorDivEq<State>::value, "your state needs a /= operator!" );
static_assert( HasOperatorMul<State>::value, "your state needs a * operator!" );
//static_assert( std::is_constructible<State, Point3>::value, "your state needs a constructor with Point3!");
//todo: static assert for getx, gety, getz, setposition
void resample(std::vector<Particle<State>>& particles) override {
grid->clear();
for (Particle<State> p : particles){
//grid.add receives position in meter!
// compile-time sanity checks
static_assert( HasOperatorPlusEq<State>::value, "your state needs a += operator!" );
static_assert( HasOperatorDivEq<State>::value, "your state needs a /= operator!" );
static_assert( HasOperatorMul<State>::value, "your state needs a * operator!" );
//static_assert( std::is_constructible<State, Point3>::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<State> 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<float> 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<Point3> dl;
for (int i = 0; i < 10000; ++i){
NM::NavMeshLocation<Tria> tmpPos = mesh->getRandom().draw();
float weight = grid->fetch(tmpPos.pos.x, tmpPos.pos.y, tmpPos.pos.z);
BoxGaus3D<float> 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<Point3> dl;
for (int i = 0; i < 10000; ++i){
NM::NavMeshLocation<Tria> 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;
}
}
};
}

4
smc/merging/mixing/MixingSamplerDivergency.h
View File

@@ -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<State>& drawParticle(ParticleFilterMixing<State, Control, Observation>& filter) {
double weights = filter.getParticles().back().weight;
//double weights = filter.getParticles().back().weight;
// generate random values between [0:cumWeight]
std::uniform_real_distribution<float> dist(0, filter.getParticles().back().weight);

33
tests/navMesh/TestNavMeshDijkstra.cpp
View File

@@ -79,16 +79,15 @@ TEST(NavMeshDijkstra, build) {
}
TEST(NavMeshDijkstra, build2) {
TEST(NavMeshDijkstra, museum) {
NavMeshSettings set;
NavMesh<MyNMT1231902345> 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<MyNMT1231902345> 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<MyNMT1231902345> 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<MyNMT1231902345> start = rnd.draw();
start.tria->getDistanceToDestination<MyNMT1231902345>(start.pos); // just a compiler-test
NM::NavMeshDijkstra::stamp(nm, dst);
//NM::NavMeshLocation<MyNMT1231902345> start = start;//nm.getLocation(Point3(0,-6,0));
std::vector<NM::NavMeshLocation<MyNMT1231902345>> path = start.tria->getPathToDestination<MyNMT1231902345>(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<MyNMT1231902345> fac(&nm, set);