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

114
CMakeLists.txt
View File

@@ -11,46 +11,46 @@ SET( CMAKE_BUILD_TYPE "${CMAKE_BUILD_TYPE}" )
PROJECT(Indoor) PROJECT(Indoor)
IF(NOT CMAKE_BUILD_TYPE) IF(NOT CMAKE_BUILD_TYPE)
MESSAGE(STATUS "No build type selected. Default to Debug") MESSAGE(STATUS "No build type selected. Default to Debug")
SET(CMAKE_BUILD_TYPE "Debug") SET(CMAKE_BUILD_TYPE "Debug")
ENDIF() ENDIF()
INCLUDE_DIRECTORIES( INCLUDE_DIRECTORIES(
../ ../
) )
FILE(GLOB HEADERS FILE(GLOB HEADERS
./*.h ./*.h
./*/*.h ./*/*.h
./*/*/*.h ./*/*/*.h
./*/*/*/*.h ./*/*/*/*.h
./*/*/*/*/*.h ./*/*/*/*/*.h
./*/*/*/*/*/*.h ./*/*/*/*/*/*.h
./*/*/*/*/*/*/*.h ./*/*/*/*/*/*/*.h
./tests/data/* ./tests/data/*
./tests/data/*/* ./tests/data/*/*
./tests/data/*/*/* ./tests/data/*/*/*
) )
FILE(GLOB SOURCES FILE(GLOB SOURCES
./*.cpp ./*.cpp
./*/*.cpp ./*/*.cpp
./*/*/*.cpp ./*/*/*.cpp
./*/*/*/*.cpp ./*/*/*/*.cpp
./*/*/*/*/*.cpp ./*/*/*/*/*.cpp
./*/*/*/*/*/*.cpp ./*/*/*/*/*/*.cpp
) )
FIND_PACKAGE( OpenMP REQUIRED) FIND_PACKAGE( OpenMP REQUIRED)
if(OPENMP_FOUND) if(OPENMP_FOUND)
message("OPENMP FOUND") message("OPENMP FOUND")
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
endif() endif()
if(${CMAKE_GENERATOR} MATCHES "Visual Studio") if(${CMAKE_GENERATOR} MATCHES "Visual Studio")
@@ -61,55 +61,53 @@ if(${CMAKE_GENERATOR} MATCHES "Visual Studio")
-D_UNICODE -D_UNICODE
-DNOGDI -DNOGDI
# -D_X86_ # -D_X86_
) )
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /permissive-") SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /permissive-")
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /Zc:__cplusplus") 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} /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} /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_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() else()
# system specific compiler flags # system specific compiler flags
ADD_DEFINITIONS( ADD_DEFINITIONS(
-std=gnu++17 -std=gnu++17
-Wall -Wall
-Werror=return-type -Werror=return-type
-Wextra -Wextra
-Wpedantic -Wpedantic
-fstack-protector-all -fstack-protector-all
-g3
-O0
#-O2
-march=native
-DWITH_TESTS
-DWITH_ASSERTIONS
-DWITH_DEBUG_LOG
-D_GLIBCXX_DEBUG
-g3
#-O0
#-O2
-march=native
-DWITH_TESTS
-DWITH_ASSERTIONS
-DWITH_DEBUG_LOG
-D_GLIBCXX_DEBUG
) )
endif() endif()
# build a binary file # build a binary file
ADD_EXECUTABLE( ADD_EXECUTABLE(
${PROJECT_NAME} ${PROJECT_NAME}
${HEADERS} ${HEADERS}
${SOURCES} ${SOURCES}
) )
#SET(EXTRA_LIBS ${EXTRA_LIBS} nl-genl-3 nl-3) #SET(EXTRA_LIBS ${EXTRA_LIBS} nl-genl-3 nl-3)
@@ -118,10 +116,10 @@ ADD_EXECUTABLE(
# needed external libraries # needed external libraries
TARGET_LINK_LIBRARIES( TARGET_LINK_LIBRARIES(
${PROJECT_NAME} ${PROJECT_NAME}
gtest gtest
# pthread pthread
${EXTRA_LIBS} ${EXTRA_LIBS}
) )
SET(CMAKE_C_COMPILER ${CMAKE_CXX_COMPILER}) 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) = "*RingBuffer*";
//::testing::GTEST_FLAG(filter) = "*Grid.*"; //::testing::GTEST_FLAG(filter) = "*Grid.*";
//::testing::GTEST_FLAG(filter) = "*Dijkstra.*"; ::testing::GTEST_FLAG(filter) = "*NavMeshDijkstra*";
//::testing::GTEST_FLAG(filter) = "*LogDistanceCeilingModelBeacon*"; //::testing::GTEST_FLAG(filter) = "*LogDistanceCeilingModelBeacon*";
//::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*"; //::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*";
@@ -111,7 +111,7 @@ int main(int argc, char** argv) {
//::testing::GTEST_FLAG(filter) = "*Matrix4*"; //::testing::GTEST_FLAG(filter) = "*Matrix4*";
//::testing::GTEST_FLAG(filter) = "*Sphere3*"; //::testing::GTEST_FLAG(filter) = "*Sphere3*";
::testing::GTEST_FLAG(filter) = "*Quaternion*"; //::testing::GTEST_FLAG(filter) = "*Quaternion*";
//::testing::GTEST_FLAG(filter) = "Timestamp*"; //::testing::GTEST_FLAG(filter) = "Timestamp*";
//::testing::GTEST_FLAG(filter) = "*RayTrace3*"; //::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);} 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: public:
/** attach distance/triangle-to-target to the ToTarget struct */ /** attach distance/triangle-to-target to the ToTarget struct */
@@ -187,7 +200,8 @@ namespace NM {
// Node* end = nodes[0]; // TODO; // Node* end = nodes[0]; // TODO;
NodeAccess acc; //NodeAccess acc;
NodeAccessFavorCenter acc;
// dijkstra // dijkstra
Dijkstra<TemporalNode> 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> { template <typename Tria> class WalkEvalApproachesTarget : public NavMeshWalkEval<Tria> {

3
sensors/beacon/BeaconMeasurement.h
View File

@@ -48,6 +48,9 @@ public:
/** get the rssi */ /** get the rssi */
float getRSSI() const {return 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*/ /** remove entries older then 3000 ms*/
void removeOld(const Timestamp latestTS) { void removeOld(const Timestamp latestTS) {
auto lambda = [latestTS] (const BeaconMeasurement& e) {
Timestamp age = latestTS - e.getTimestamp(); std::vector<BeaconMeasurement>::iterator it;
return age > Timestamp::fromMS(1000*3); for (it = entries.begin(); it != entries.end(); ++it){
}; if(latestTS - it->getTimestamp() > Timestamp::fromMS(1000*3)){
entries.erase(std::remove_if(entries.begin(), entries.end(), lambda), entries.end()); 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(evaluation, "evaluation MUST not be null! call setEvaluation() first!");
Assert::isNotNull(estimation, "estimation MUST not be null! call setEstimation() first!"); Assert::isNotNull(estimation, "estimation MUST not be null! call setEstimation() first!");
// if the number of efficient particles is too low, perform resampling
if (lastNEff < particles.size() * nEffThresholdPercent) {resampler->resample(particles); }
// perform the transition step // perform the transition step
transition->transition(particles, control); transition->transition(particles, control);
@@ -143,6 +142,9 @@ namespace SMC {
// perform the evaluation step and calculate the sum of all particle weights // perform the evaluation step and calculate the sum of all particle weights
evaluation->evaluation(particles, observation); evaluation->evaluation(particles, observation);
// perform an additional step to prevent impoverishment of the particles
//enrichment->enrichment(particles, observation);
// normalize the particle weights and thereby calculate N_eff // normalize the particle weights and thereby calculate N_eff
lastNEff = normalize(); lastNEff = normalize();
@@ -151,6 +153,9 @@ namespace SMC {
// estimate the current state // estimate the current state
const State est = estimation->estimate(particles); 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 // done
return est; return est;

338
smc/filtering/ParticleFilterMixing.h
View File

@@ -16,225 +16,225 @@
namespace SMC { namespace SMC {
/** /**
* the main-class for the particle filter * the main-class for the particle filter
* @param State the (user-defined) state for each particle * @param State the (user-defined) state for each particle
* @param Observation the observation (sensor) data * @param Observation the observation (sensor) data
*/ */
template <typename State, typename Control, typename Observation> template <typename State, typename Control, typename Observation>
class ParticleFilterMixing { class ParticleFilterMixing {
private: private:
/** all used particles */ /** all used particles */
std::vector<Particle<State>> particles; std::vector<Particle<State>> particles;
/** the current calculated estimation */ /** the current calculated estimation */
State estimation; State estimation;
/** the resampler to use */ /** the resampler to use */
std::shared_ptr<ParticleFilterResampling<State>> resampler; std::shared_ptr<ParticleFilterResampling<State>> resampler;
/** the estimation function to use */ /** the estimation function to use */
std::shared_ptr<ParticleFilterEstimation<State>> estimator; std::shared_ptr<ParticleFilterEstimation<State>> estimator;
/** the transition function to use */ /** the transition function to use */
std::shared_ptr<ParticleFilterTransition<State, Control>> transition; std::shared_ptr<ParticleFilterTransition<State, Control>> transition;
/** the evaluation function to use */ /** the evaluation function to use */
std::shared_ptr<ParticleFilterEvaluation<State, Observation>> evaluation; std::shared_ptr<ParticleFilterEvaluation<State, Observation>> evaluation;
/** the initialization function to use */ /** the initialization function to use */
std::shared_ptr<ParticleFilterInitializer<State>> initializer; std::shared_ptr<ParticleFilterInitializer<State>> initializer;
/** the percentage-of-efficient-particles-threshold for resampling */ /** the percentage-of-efficient-particles-threshold for resampling */
double nEffThresholdPercent = 0.25; double nEffThresholdPercent = 0.25;
/** the current sum of all weights NOT normalized*/ /** the current sum of all weights NOT normalized*/
double weightSum = 1.0; double weightSum = 1.0;
/** the predicted mode probability P(m_t|Z_t-1) */ /** the predicted mode probability P(m_t|Z_t-1) */
double predictedModeProbability = 1.0; double predictedModeProbability = 1.0;
/** the posterior probability of the mode p(m_t | Z_t)*/ /** the posterior probability of the mode p(m_t | Z_t)*/
double modePosteriorProbability = 1.0; double modePosteriorProbability = 1.0;
/** the transition mode probability P(m_t-1 | m_t, Z_t-1)*/ /** the transition mode probability P(m_t-1 | m_t, Z_t-1)*/
double transitionModeProbability = 1.0; double transitionModeProbability = 1.0;
public: public:
/** ctor /** ctor
* NOTE: The modePosteriorProbability needs the be normalized depending on the number of filters within the IMMPF!! * 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) { ParticleFilterMixing(const uint32_t numParticles, std::shared_ptr<ParticleFilterInitializer<State>> initializer, double modePosteriorProbability) {
this->modePosteriorProbability = modePosteriorProbability; this->modePosteriorProbability = modePosteriorProbability;
particles.resize(numParticles); particles.resize(numParticles);
setInitializier(std::move(initializer)); setInitializier(std::move(initializer));
init(); init();
} }
/** dtor */ /** dtor */
~ParticleFilterMixing() { ~ParticleFilterMixing() {
; ;
} }
/** access to all particles */ /** access to all particles */
const std::vector<Particle<State>>& getParticles() const { const std::vector<Particle<State>>& getParticles() const {
return this->particles; return this->particles;
} }
void setParticles(const std::vector<Particle<State>>& newParticles){ void setParticles(const std::vector<Particle<State>>& newParticles){
this->particles = newParticles; this->particles = newParticles;
} }
/** get the current estimation */ /** get the current estimation */
const State getEstimation() const { const State getEstimation() const {
return estimation; return estimation;
} }
/** initialize/re-start the particle filter */ /** initialize/re-start the particle filter */
void init() { void init() {
Assert::isNotNull(initializer, "initializer MUST not be null! call setInitializer() first!"); Assert::isNotNull(initializer, "initializer MUST not be null! call setInitializer() first!");
initializer->initialize(particles); initializer->initialize(particles);
} }
/** set the resampling method to use */ /** set the resampling method to use */
void setResampling(std::shared_ptr<ParticleFilterResampling<State>> resampler) { void setResampling(std::shared_ptr<ParticleFilterResampling<State>> resampler) {
Assert::isNotNull(resampler, "setResampling() MUST not be called with a nullptr!"); Assert::isNotNull(resampler, "setResampling() MUST not be called with a nullptr!");
this->resampler = std::move(resampler); this->resampler = std::move(resampler);
} }
/** set the estimation method to use */ /** set the estimation method to use */
void setEstimator(std::shared_ptr<ParticleFilterEstimation<State>> estimator) { void setEstimator(std::shared_ptr<ParticleFilterEstimation<State>> estimator) {
Assert::isNotNull(estimator, "setEstimation() MUST not be called with a nullptr!"); Assert::isNotNull(estimator, "setEstimation() MUST not be called with a nullptr!");
this->estimator = std::move(estimator); this->estimator = std::move(estimator);
} }
/** set the transition method to use */ /** set the transition method to use */
void setTransition(std::shared_ptr<ParticleFilterTransition<State, Control>> transition) { void setTransition(std::shared_ptr<ParticleFilterTransition<State, Control>> transition) {
Assert::isNotNull(transition, "setTransition() MUST not be called with a nullptr!"); Assert::isNotNull(transition, "setTransition() MUST not be called with a nullptr!");
this->transition = std::move(transition); this->transition = std::move(transition);
} }
/** get the used transition method */ /** get the used transition method */
ParticleFilterTransition<State, Control>* getTransition() { ParticleFilterTransition<State, Control>* getTransition() {
return this->transition.get(); return this->transition.get();
} }
/** set the evaluation method to use */ /** set the evaluation method to use */
void setEvaluation(std::shared_ptr<ParticleFilterEvaluation<State, Observation>> evaluation) { void setEvaluation(std::shared_ptr<ParticleFilterEvaluation<State, Observation>> evaluation) {
Assert::isNotNull(evaluation, "setEvaluation() MUST not be called with a nullptr!"); Assert::isNotNull(evaluation, "setEvaluation() MUST not be called with a nullptr!");
this->evaluation = std::move(evaluation); this->evaluation = std::move(evaluation);
} }
/** set the initialization method to use */ /** set the initialization method to use */
void setInitializier(std::shared_ptr<ParticleFilterInitializer<State>> initializer) { void setInitializier(std::shared_ptr<ParticleFilterInitializer<State>> initializer) {
Assert::isNotNull(initializer, "setInitializer() MUST not be called with a nullptr!"); Assert::isNotNull(initializer, "setInitializer() MUST not be called with a nullptr!");
this->initializer = std::move(initializer); this->initializer = std::move(initializer);
} }
/** set the resampling threshold as the percentage of efficient particles */ /** set the resampling threshold as the percentage of efficient particles */
void setNEffThreshold(const double thresholdPercent) { void setNEffThreshold(const double thresholdPercent) {
this->nEffThresholdPercent = thresholdPercent; this->nEffThresholdPercent = thresholdPercent;
} }
/** get the unormalized weight sum of all particles */ /** get the unormalized weight sum of all particles */
double getWeightSum() const double getWeightSum() const
{ {
return this->weightSum; return this->weightSum;
} }
/** get the predicted mode probability P(m_t|Z_t-1)*/ /** get the predicted mode probability P(m_t|Z_t-1)*/
double getPredictedModeProbability() const double getPredictedModeProbability() const
{ {
return this->predictedModeProbability; return this->predictedModeProbability;
} }
/** set the predicted mode probability P(m_t|Z_t-1)*/ /** set the predicted mode probability P(m_t|Z_t-1)*/
void setPredictedModeProbability(const double val) { void setPredictedModeProbability(const double val) {
this->predictedModeProbability = val; this->predictedModeProbability = val;
} }
/** get the posterior mode probability P(m_t|Z_t)*/ /** get the posterior mode probability P(m_t|Z_t)*/
double getModePosteriorProbability() const double getModePosteriorProbability() const
{ {
return this->modePosteriorProbability; return this->modePosteriorProbability;
} }
/** set the posterior mode probability P(m_t|Z_t)*/ /** set the posterior mode probability P(m_t|Z_t)*/
void setModePosteriorProbability(const double likelihoodSum) { void setModePosteriorProbability(const double likelihoodSum) {
Assert::isNotNull(likelihoodSum, "likelihoodsum 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->weightSum, "weightSum is zero.. thats not possible");
//Assert::isNotNull(this->predictedModeProbability, "predictedModeProbability 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) /** get the transition mode probability P(m_t|Z_t)
* NOTE: Dont use this value! It is only needed for more beatiful mixed sampling! * NOTE: Dont use this value! It is only needed for more beautiful mixed sampling!
*/ */
double getTransitionModeProbability() const double getTransitionModeProbability() const
{ {
return this->transitionModeProbability; return this->transitionModeProbability;
} }
/** set the transition mode probability P(m_t|Z_t)*/ /** set the transition mode probability P(m_t|Z_t)*/
void setTransitionModeProbability(const double val) { void setTransitionModeProbability(const double val) {
this->transitionModeProbability = val; this->transitionModeProbability = val;
} }
/** perform resampling -> transition -> evaluation -> estimation */ /** perform resampling -> transition -> evaluation -> estimation */
void update(const Control* control, const Observation& observation) { void update(const Control* control, const Observation& observation) {
// sanity checks (if enabled) // sanity checks (if enabled)
Assert::isNotNull(resampler, "resampler MUST not be null! call setResampler() first!"); Assert::isNotNull(resampler, "resampler MUST not be null! call setResampler() first!");
Assert::isNotNull(transition, "transition MUST not be null! call setTransition() 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(evaluation, "evaluation MUST not be null! call setEvaluation() first!");
Assert::isNotNull(estimator, "estimation MUST not be null! call setEstimation() first!"); Assert::isNotNull(estimator, "estimation MUST not be null! call setEstimation() first!");
// perform the transition step // perform the transition step
transition->transition(particles, control); transition->transition(particles, control);
// perform the evaluation step and calculate the sum of all particle weights // perform the evaluation step and calculate the sum of all particle weights
this->weightSum = evaluation->evaluation(particles, observation); this->weightSum = evaluation->evaluation(particles, observation);
// normalize the particle weights and thereby calculate N_eff // normalize the particle weights and thereby calculate N_eff
const double neff = normalize(weightSum); const double neff = normalize(weightSum);
// estimate the current state // estimate the current state
this->estimation = estimator->estimate(particles); this->estimation = estimator->estimate(particles);
// if the number of efficient particles is too low, perform resampling // if the number of efficient particles is too low, perform resampling
if (neff < particles.size() * nEffThresholdPercent) { resampler->resample(particles); } if (neff < particles.size() * nEffThresholdPercent) { resampler->resample(particles); }
// done // done
} }
private: private:
/** normalize the weight of all particles to one */ /** normalize the weight of all particles to one */
double normalize(const double weightSum) { double normalize(const double weightSum) {
double sum = 0.0; double sum = 0.0;
for (auto& p : particles) { for (auto& p : particles) {
p.weight /= weightSum; p.weight /= weightSum;
sum += (p.weight * p.weight); sum += (p.weight * p.weight);
} }
return 1.0 / sum; return 1.0 / sum;
} }
/** calculate the number of efficient particles (N_eff) */ /** calculate the number of efficient particles (N_eff) */
double getNeff() const { double getNeff() const {
double sum = 0.0; double sum = 0.0;
for (auto& p : particles) {sum += (p.weight * p.weight);} for (auto& p : particles) {sum += (p.weight * p.weight);}
return 1.0 / sum; 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 #define PARTICLEFILTERRESAMPLINGKDE_H
#include <algorithm> #include <algorithm>
@@ -19,103 +19,102 @@
namespace SMC { namespace SMC {
/** /**
* Resample based on rapid KDE * Resample based on rapid KDE
*/ */
template <typename State, typename Tria> template <typename State, typename Tria>
class ParticleFilterResamplingKDE : public ParticleFilterResampling<State> { class ParticleFilterResamplingKDE : public ParticleFilterResampling<State> {
private: private:
/** this is a copy of the particle-set to draw from it */ /** this is a copy of the particle-set to draw from it */
std::vector<Particle<State>> particlesCopy; std::vector<Particle<State>> particlesCopy;
/** random number generator */ /** random number generator */
std::minstd_rand gen; std::minstd_rand gen;
/** boundingBox for the boxKDE */ /** boundingBox for the boxKDE */
_BBox3<float> bb; _BBox3<float> bb;
/** histogram/grid holding the particles*/ /** histogram/grid holding the particles*/
std::unique_ptr<Grid3D<float>> grid; std::unique_ptr<Grid3D<float>> grid;
/** bandwith for KDE */ /** bandwith for KDE */
Point3 bandwith; Point3 bandwith;
/** the current mesh */ /** the current mesh */
const NM::NavMesh<Tria>* mesh; const NM::NavMesh<Tria>* mesh;
public: public:
/** ctor */ /** ctor */
ParticleFilterResamplingKDE(const NM::NavMesh<Tria>* mesh, const Point3 gridsize_m, const Point3 bandwith) { ParticleFilterResamplingKDE(const NM::NavMesh<Tria>* mesh, const Point3 gridsize_m, const Point3 bandwith) {
this->mesh = mesh; this->mesh = mesh;
this->bandwith = bandwith; this->bandwith = bandwith;
this->bb = mesh->getBBox(); this->bb = mesh->getBBox();//_BBox3<float>(mesh->getBBox().getMin() * 100.f, mesh->getBBox().getMax() * 100.f);
this->bb.grow(10); this->bb.grow(10);
// Create histogram // Create histogram
size_t nBinsX = (size_t)((this->bb.getMax().x - this->bb.getMin().x) / gridsize_m.x); 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 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); 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 void resample(std::vector<Particle<State>>& particles) override {
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
grid->clear(); // compile-time sanity checks
for (Particle<State> p : particles){ static_assert( HasOperatorPlusEq<State>::value, "your state needs a += operator!" );
//grid.add receives position in meter! 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. grid->clear();
if((float) p.weight > 0 ){ for (Particle<State> p : particles){
grid->add(p.state.getX(), p.state.getY(), p.state.getZ(), p.weight); //grid.add receives position in meter!
}
}
int nFilt = 3; //if weight is to low, remove.
float sigmaX = bandwith.x / grid->binSizeX; if((float) p.weight > 0 ){
float sigmaY = bandwith.y / grid->binSizeY; grid->add(p.state.getX(), p.state.getY(), p.state.getZ(), p.weight);
float sigmaZ = bandwith.z / grid->binSizeZ; }
}
BoxGaus3D<float> boxGaus; int nFilt = 3;
boxGaus.approxGaus(grid->image(), Point3(sigmaX, sigmaY, sigmaZ), nFilt); 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 BoxGaus3D<float> boxGaus;
// assign them a weight based on the KDE density. boxGaus.approxGaus(grid->image(), Point3(sigmaX, sigmaY, sigmaZ), nFilt);
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);
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 // used the same particles to not lose the heading.
// 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 for (int i = 0; i < particles.size(); ++i){
// 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){
double tmpWeight = 1; double tmpWeight = 1;
particles[i].state.pos = mesh->getLocation(dl.get(tmpWeight)); particles[i].state.pos = mesh->getLocation(dl.get(tmpWeight));
particles[i].weight = tmpWeight; particles[i].weight = tmpWeight;
} }
} }
}; };
} }

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

@@ -100,7 +100,7 @@ namespace SMC {
auto mode = drawMode(cumTransitionModeProbability); auto mode = drawMode(cumTransitionModeProbability);
newParticles[k] = drawParticle(mode); 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); focusedFilter.setParticles(newParticles);
@@ -133,7 +133,7 @@ namespace SMC {
/** draw one particle according to its weight from the copy vector of a given mode */ /** draw one particle according to its weight from the copy vector of a given mode */
const Particle<State>& drawParticle(ParticleFilterMixing<State, Control, Observation>& filter) { 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] // generate random values between [0:cumWeight]
std::uniform_real_distribution<float> dist(0, filter.getParticles().back().weight); 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; NavMeshSettings set;
NavMesh<MyNMT1231902345> nm; 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/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/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); NavMeshFactory<MyNMT1231902345> fac(&nm, set);
fac.build(map); fac.build(map);
@@ -97,11 +96,32 @@ TEST(NavMeshDijkstra, build2) {
dbg.addMesh(nm); dbg.addMesh(nm);
dbg.draw(); 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.addDijkstra(nm);
dbg.draw(); 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; int xxx = 0; (void) xxx;
} }
@@ -144,7 +164,8 @@ TEST(NavMeshDijkstra, path) {
// remove2.poly.points.push_back(Point2(-11,+2)); // 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("/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); NavMeshFactory<MyNMT1231902345> fac(&nm, set);