├── .gitignore ├── CMakeLists.txt ├── README.rst ├── lib ├── CMakeLists.txt ├── include │ └── libPF │ │ ├── CRandomNumberGenerator.h │ │ ├── CompareParticleWeights.h │ │ ├── ImportanceResampling.h │ │ ├── MovementModel.h │ │ ├── ObservationModel.h │ │ ├── Particle.h │ │ ├── ParticleFilter.h │ │ ├── ParticleFilter.hxx │ │ ├── RandomNumberGenerationStrategy.h │ │ ├── ResamplingStrategy.h │ │ └── StateDistribution.h └── src │ └── CRandomNumberGenerator.cpp ├── sample1 ├── CMakeLists.txt ├── MyMovementModel.cpp ├── MyMovementModel.h ├── MyObservationModel.cpp ├── MyObservationModel.h ├── MyStateDistribution.cpp ├── MyStateDistribution.h └── main.cpp └── sample2 ├── CMakeLists.txt ├── CarMovementModel.cpp ├── CarMovementModel.h ├── CarObservationModel.cpp ├── CarObservationModel.h ├── CarState.cpp ├── CarState.h ├── CarStateDistribution.cpp ├── CarStateDistribution.h ├── MainWindow.cpp ├── MainWindow.h ├── MainWindowForm.ui ├── RenderWidget.cpp ├── RenderWidget.h └── main.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | 3 | add_subdirectory(lib) 4 | add_subdirectory(sample1) 5 | add_subdirectory(sample2) 6 | -------------------------------------------------------------------------------- /README.rst: -------------------------------------------------------------------------------- 1 | libPF 2 | ==== 3 | libPF is a simple, easy to use, C++ template library for particle filters. 4 | All you have to do to implement a particle filter using this library is: 5 | 6 | - Create a state 7 | - Create a movement model for your state 8 | - Create a measurement model for your state 9 | 10 | The library does all the rest. 11 | 12 | Please see the documentation in the source files for more information. 13 | -------------------------------------------------------------------------------- /lib/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(libPF) 2 | cmake_minimum_required(VERSION 2.8) 3 | 4 | include_directories(include) 5 | 6 | add_library(PF 7 | src/CRandomNumberGenerator.cpp) 8 | 9 | -------------------------------------------------------------------------------- /lib/include/libPF/CRandomNumberGenerator.h: -------------------------------------------------------------------------------- 1 | #ifndef RANDOMNUMBERGENERATOR_H 2 | #define RANDOMNUMBERGENERATOR_H 3 | 4 | #include "libPF/RandomNumberGenerationStrategy.h" 5 | 6 | namespace libPF 7 | { 8 | 9 | /** 10 | * @class CRandomNumberGenerator 11 | * 12 | * @brief Class for the generation of random numbers. 13 | * 14 | * This class can generate randomly generated numbers from uniform and 15 | * gaussian distributions. 16 | * Note: this is a very simple PRNG, using the C-function rand(). 17 | * 18 | * @author Stephan Wirth 19 | */ 20 | class CRandomNumberGenerator : public RandomNumberGenerationStrategy { 21 | 22 | public: 23 | 24 | /** 25 | * The constructor calls init(). 26 | */ 27 | CRandomNumberGenerator(); 28 | 29 | /** 30 | * Empty destructor. 31 | */ 32 | ~CRandomNumberGenerator(); 33 | 34 | /** 35 | * This method creates gaussian distributed random numbers (Box-Müller method). 36 | * @param standardDeviation Standard deviation d of the random number to generate. 37 | * @return N(0, d*d)-distributed random number 38 | */ 39 | double getGaussian(double standardDeviation) const; 40 | 41 | /** 42 | * Generates a uniform distributed random number between min and max. 43 | * @param min the minimum value, default is 0.0 44 | * @param max the maximum value, default is 1.0 45 | * @return random number between min and max, uniform distributed. 46 | */ 47 | double getUniform(double min = 0.0, double max = 1.0) const; 48 | 49 | protected: 50 | 51 | /** 52 | * Initializes the seed by calling srand(time(0)) 53 | */ 54 | void init(); 55 | 56 | private: 57 | 58 | /// stores if there is a buffered gaussian variable or not 59 | mutable bool m_GaussianBufferFilled; 60 | 61 | /// buffer for a gaussian distributed variable, the Box-Müller method always 62 | /// creates two variables, we return one and store the other here. 63 | mutable double m_GaussianBufferVariable; 64 | 65 | }; 66 | 67 | } // end of namespace 68 | 69 | #endif // RANDOMNUMBERGENERATOR_H 70 | -------------------------------------------------------------------------------- /lib/include/libPF/CompareParticleWeights.h: -------------------------------------------------------------------------------- 1 | #ifndef COMPAREPARTICLEWEIGHTS_H 2 | #define COMPAREPARTICLEWEIGHTS_H 3 | 4 | #include "libPF/Particle.h" 5 | 6 | namespace libPF 7 | { 8 | 9 | /** 10 | * @class CompareParticleWeights 11 | * 12 | * @author Stephan Wirth 13 | * 14 | * @brief Class with one operator to compare two pointers of particles according 15 | * to the weight of the particles. 16 | * 17 | * With this class as compare function, std::sort() can be used on arrays of 18 | * pointers to Particle. After sorting the array with this function, the particle 19 | * with the smallest weight will be at the last position. 20 | * 21 | * @see Particle 22 | */ 23 | template 24 | class CompareParticleWeights { 25 | 26 | public: 27 | 28 | /** 29 | * @return true if the weight of the particle p1 is higher than the weight of particle p2. 30 | */ 31 | bool operator() (const libPF::Particle* p1, 32 | const libPF::Particle* p2) const 33 | { 34 | return p1->getWeight() > p2->getWeight(); 35 | } 36 | }; 37 | 38 | } // end of namespace 39 | 40 | #endif 41 | 42 | 43 | -------------------------------------------------------------------------------- /lib/include/libPF/ImportanceResampling.h: -------------------------------------------------------------------------------- 1 | #ifndef IMPORTANCERESAMPLING_H 2 | #define IMPORTANCERESAMPLING_H 3 | 4 | #include "libPF/CRandomNumberGenerator.h" 5 | 6 | namespace libPF 7 | { 8 | 9 | /** 10 | * @class ImportanceResampling 11 | * 12 | * @brief A resampling strategy that performs importance resampling 13 | * 14 | * The resampling strategy defines how the resampling is performed in the resample step 15 | * of a particle filter. 16 | * 17 | * @author Stephan Wirth 18 | * 19 | * @see ResamplingStrategy 20 | */ 21 | 22 | template 23 | class ImportanceResampling : public ResamplingStrategy{ 24 | 25 | /** 26 | * A ParticleList is an array of pointers to Particles. 27 | */ 28 | typedef std::vector< Particle* > ParticleList; 29 | 30 | public: 31 | /** 32 | * The constructor of this base class inits some members. 33 | */ 34 | ImportanceResampling(); 35 | 36 | /** 37 | * The destructor is empty. 38 | */ 39 | virtual ~ImportanceResampling(); 40 | 41 | /** 42 | * This is the main method of ImportanceResampling. It takes two references to 43 | * particle lists. The first reference refers to the old particle list, the 44 | * second to the new one. 45 | * @param source the source list to draw new particles from. 46 | * @param destination the destination list where to put the copies. 47 | */ 48 | void resample(const ParticleList& source, const ParticleList& destination) const; 49 | 50 | /** 51 | * Sets the Random Number Generator to use in resample() to generate uniformly distributed random numbers. 52 | */ 53 | void setRNG(RandomNumberGenerationStrategy* rng); 54 | 55 | private: 56 | 57 | // Stores a pointer to the random number generator. 58 | const RandomNumberGenerationStrategy* m_RNG; 59 | 60 | // The default random number generator 61 | CRandomNumberGenerator m_DefaultRNG; 62 | 63 | }; 64 | 65 | 66 | template 67 | ImportanceResampling::ImportanceResampling() : 68 | m_RNG(&m_DefaultRNG) { 69 | } 70 | 71 | template 72 | ImportanceResampling::~ImportanceResampling() { 73 | } 74 | 75 | 76 | // resampling based on the cumulative distribution function (CDF) 77 | template 78 | void ImportanceResampling::resample(const ParticleList& sourceList, const ParticleList& destinationList) const { 79 | 80 | double inverseNum = 1.0f / sourceList.size(); 81 | double start = m_RNG->getUniform() * inverseNum; // random start in CDF 82 | double cumulativeWeight = 0.0f; 83 | unsigned int sourceIndex = 0; // index to draw from 84 | cumulativeWeight += sourceList[sourceIndex]->getWeight(); 85 | for (unsigned int destIndex = 0; destIndex < destinationList.size(); destIndex++) { 86 | double probSum = start + inverseNum * destIndex; // amount of cumulative weight to reach 87 | while (probSum > cumulativeWeight) { // sum weights until 88 | sourceIndex++; 89 | if (sourceIndex >= sourceList.size()) { 90 | sourceIndex = sourceList.size() - 1; 91 | break; 92 | } 93 | cumulativeWeight += sourceList[sourceIndex]->getWeight(); // target sum reached 94 | } 95 | *(destinationList[destIndex]) = *(sourceList[sourceIndex]); // copy particle (via assignment operator) 96 | } 97 | } 98 | 99 | 100 | template 101 | void ImportanceResampling::setRNG(RandomNumberGenerationStrategy* rng) 102 | { 103 | m_RNG = rng; 104 | } 105 | 106 | } // end of namespace 107 | #endif // IMPORTANCERESAMPLING_H 108 | 109 | -------------------------------------------------------------------------------- /lib/include/libPF/MovementModel.h: -------------------------------------------------------------------------------- 1 | #ifndef MOVEMENTSTRATEGY_H 2 | #define MOVEMENTSTRATEGY_H 3 | 4 | #include 5 | #include 6 | 7 | namespace libPF 8 | { 9 | 10 | /** 11 | * @class MovementModel 12 | * 13 | * @brief Templated interface for movement models for particle filters. 14 | * 15 | * The movement model in a particle filter defines how a particle's state changes 16 | * over time. 17 | * It is used in the drift and diffuse step of libPF::ParticleFilter (strategy pattern). 18 | * To define a movement model, create a sub-class of this class and 19 | * implement the drift() method. A particle filter with this movement model 20 | * applies the drift method for each particle in each filter step. Also you have 21 | * to implement the function diffuse() to define a jitter that is added to 22 | * a state after drift() (which may be empty of course). You can use the function 23 | * randomGauss() to obtain Gaussian-distributed random variables. 24 | * 25 | * @author Stephan Wirth 26 | * 27 | * @see ParticleFilter 28 | * @see Particle 29 | */ 30 | 31 | template 32 | class MovementModel { 33 | 34 | public: 35 | 36 | /** 37 | * The destructor is empty. 38 | */ 39 | virtual ~MovementModel(); 40 | 41 | /** 42 | * This is the main method of MovementModel. It takes a state reference as 43 | * argument and is supposed to extract the state's variables and manipulate 44 | * them. dt means delta t and defines the time in seconds that has passed 45 | * since the last filter update. 46 | * Define this function in your sub-class! 47 | * @param state Reference to the state that has to be manipulated. 48 | * @param dt time that has passed since the last filter update in seconds. 49 | */ 50 | virtual void drift(StateType& state, double dt) const = 0; 51 | 52 | /** 53 | * This method will be applied in a ParticleFilter after drift(). It can be 54 | * used to add a small jitter to the state. 55 | * @param state Reference to the state that has to be manipulated. 56 | * @param dt time that has passed since the last filter update in seconds. 57 | */ 58 | virtual void diffuse(StateType& state, double dt) const = 0; 59 | 60 | private: 61 | 62 | }; 63 | 64 | template 65 | MovementModel::~MovementModel() { 66 | } 67 | 68 | } // end of namespace 69 | #endif 70 | 71 | -------------------------------------------------------------------------------- /lib/include/libPF/ObservationModel.h: -------------------------------------------------------------------------------- 1 | #ifndef OBSERVATIONSTRATEGY_H 2 | #define OBSERVATIONSTRATEGY_H 3 | 4 | #include 5 | 6 | namespace libPF 7 | { 8 | 9 | /** 10 | * @class ObservationModel 11 | * 12 | * @brief Templated interface for observation models for particle filters. 13 | * 14 | * The observation model in a particle filter defines the measurement process 15 | * that is used to weighten the particles according to their state. 16 | * It is used in the measurement step of the particle filter libPF::ParticleFilter::measure() 17 | * (strategy pattern). 18 | * To define an observation model, create a sub-class of this class, specializing the 19 | * measurement method and the state type to use (with specializing the 20 | * template). The measurement method takes a reference to a state as an argument. 21 | * Use this reference to extract the state's variables and use your measurement 22 | * function to compute a state-dependent weight. The weight has to be a positive, 23 | * non-zero value. 24 | * 25 | * @author Stephan Wirth 26 | * @see ParticleFilter 27 | * @see Particle 28 | */ 29 | 30 | template 31 | class ObservationModel { 32 | 33 | public: 34 | 35 | /** 36 | * The destructor is empty. 37 | */ 38 | virtual ~ObservationModel(); 39 | 40 | /** 41 | * This is the main method of ObservationModel. It takes a state reference 42 | * as argument and is supposed to extract the state's variables to compute 43 | * an importance weight for the state. 44 | * Define this method in your sub-class! 45 | * @param state Reference to the state that has to be weightened. 46 | * @return importance weight for the given state (positive, non-zero value). 47 | */ 48 | virtual double measure(const StateType& state) const = 0; 49 | 50 | private: 51 | 52 | }; 53 | 54 | template 55 | ObservationModel::~ObservationModel() { 56 | } 57 | 58 | } // end of namespace 59 | #endif 60 | 61 | -------------------------------------------------------------------------------- /lib/include/libPF/Particle.h: -------------------------------------------------------------------------------- 1 | #ifndef PARTICLE_H 2 | #define PARTICLE_H 3 | 4 | #include "libPF/ParticleFilter.h" 5 | 6 | namespace libPF 7 | { 8 | /** 9 | * @class Particle 10 | * @brief Class that represents a particle for a particle filter. 11 | * 12 | * A particle as it is used in particle filters is a set of one state 13 | * (m_State) and one importance factor (m_Weight). 14 | * A set of Particles is a discrete representation of a probability 15 | * distribution. Normally the user of the class ParticleFilter has 16 | * not to care about this class, as it is used only internally by 17 | * ParticleFilter. 18 | * @author Stephan Wirth 19 | * @see ParticleFilter 20 | */ 21 | template 22 | class Particle 23 | { 24 | public: 25 | 26 | /** 27 | * This constructor assigns the given state to the member m_State 28 | * and the given weight to the member m_Weight. 29 | * @param state The initial state of the particle 30 | * @param weight The initial weight of the particle 31 | */ 32 | Particle(const StateType& state, double weight); 33 | 34 | /** 35 | * The destructor is empty. 36 | */ 37 | virtual ~Particle(); 38 | 39 | /** 40 | * @return reference to the state of the particle 41 | */ 42 | inline const StateType& getState() const; 43 | 44 | /** 45 | * Sets a new state (assignment operator is used, be sure it works 46 | * for StateType!). 47 | * @param newState a new state for the particle. 48 | */ 49 | inline void setState(const StateType& newState); 50 | 51 | /** 52 | * @return the weight 53 | */ 54 | inline double getWeight() const; 55 | 56 | /** 57 | * Sets a new weight 58 | * @param newWeight the new weight 59 | */ 60 | inline void setWeight(double newWeight); 61 | 62 | private: 63 | // make ParticleFilter a friend that can have non-const access 64 | // to m_State 65 | template friend class ParticleFilter; 66 | 67 | // Stores the state of the particle. 68 | StateType m_State; 69 | 70 | // Stores the importance factor (=weight) of the particle. 71 | double m_Weight; 72 | }; 73 | 74 | template 75 | Particle::Particle(const StateType& state, double weight) : 76 | m_State(state), 77 | m_Weight(weight) 78 | { 79 | } 80 | 81 | template 82 | Particle::~Particle() { 83 | } 84 | 85 | template 86 | const StateType& Particle::getState() const 87 | { 88 | return m_State; 89 | } 90 | 91 | template 92 | void Particle::setState(const StateType& newState) 93 | { 94 | m_State = newState; 95 | } 96 | 97 | template 98 | double Particle::getWeight() const 99 | { 100 | return m_Weight; 101 | } 102 | 103 | template 104 | void Particle::setWeight(double newWeight) 105 | { 106 | m_Weight = newWeight; 107 | } 108 | } 109 | 110 | #endif 111 | -------------------------------------------------------------------------------- /lib/include/libPF/ParticleFilter.h: -------------------------------------------------------------------------------- 1 | #ifndef PARTICLEFILTER_H 2 | #define PARTICLEFILTER_H 3 | #include 4 | #include // for time measurement 5 | #include 6 | #include 7 | 8 | #include "libPF/ObservationModel.h" 9 | #include "libPF/MovementModel.h" 10 | #include "libPF/ResamplingStrategy.h" 11 | #include "libPF/ImportanceResampling.h" 12 | #include "libPF/CompareParticleWeights.h" 13 | #include "libPF/Particle.h" 14 | #include "libPF/StateDistribution.h" 15 | 16 | namespace libPF 17 | { 18 | 19 | /** 20 | * @class ParticleFilter 21 | * 22 | * @author Stephan Wirth 23 | * 24 | * @brief (Templated) class that defines a particle filter. 25 | * 26 | * A particle filter is a descrete method to describe and compute with a probability distribution. 27 | * In other words, it is an implementation of a recursive Bayesian filter by Monte Carlo Methods. 28 | * The sequential importance sampling (SIS) used is also known as bootstrap filtering, the 29 | * condensation algorithm, particle filtering, interacting particle approximations and survival 30 | * of the fittest. 31 | * This template class implements the basic methods for a particle filter. 32 | * The changeable parts of the particle filter are implemented using the strategy pattern. If you 33 | * don't know what it is, you should read something about it, as it is used in many methods here. 34 | * 35 | * The following strategies are used by ParticleFilter, all of them can be switched at runtime. 36 | * @li ObservationModel defines how a state can be evaluated (weighted) 37 | * @li MovementModel defines how a state will be propagated during time 38 | * @li ResamplingStrategy defines how resampling occurs (see ImportanceResampling for the default implementation) 39 | * 40 | * 41 | * You must do the following to use the particle filter: 42 | * @li Create a class for the state that you want to track with the ParticleFilter. 43 | * Let's name your state MyState. The state has to implement the operator 44 | * @code 45 | * operator=(const MyState& other); 46 | * @endcode 47 | * because a particle filter copies the "fittest" states in a resampling step. 48 | * If you want to use the methods getBestXPercentEstimate() or getMmseEstimate(), 49 | * your state has to implement two more operators: 50 | * @code 51 | * MyState operator*(float factor) const; 52 | * @endcode 53 | * and 54 | * @code 55 | * MyState& operator+=(const MyState& other); 56 | * @endcode 57 | * These operators allow the ParticleFilter to compute the weighted average 58 | * of a set of states. 59 | * Instead of creating your own state, you may use a basic datatype as state, 60 | * for example to create a ParticleFilter is perfectly possible. 61 | * @li The next step is to create an observation strategy MyObservationModel for your state, deriving 62 | * it from ObservationModel and implementing the method 63 | * ObservationModel::measure(). 64 | * @li Create a movement strategy MyMovementModel for your state, deriving it from 65 | * MovementModel and implement the methods MovementModel::diffuse() 66 | * and MovementModel::drift(). 67 | * 68 | * After that you can use the particle filter this way: 69 | * @code 70 | * int numOfParticles = 500; 71 | * MyMovementModel mm; // create movement strategy 72 | * MyObservationModel om; // create observation strategy 73 | * ParticleFilter pf(numOfParticles, &om, &mm); // create filter 74 | * 75 | * // run the filter loop 76 | * bool doFilter = true; 77 | * while (doFilter) { 78 | * // update your observation model here 79 | * // ... 80 | * // update your movement model here (if necessary) 81 | * // ... 82 | * 83 | * // run one filter step, the filter uses the 84 | * // observation model and the movement model 85 | * // that were given in the constructor 86 | * pf->filter(); 87 | * 88 | * // retrieve the best state and 89 | * // do something with the result 90 | * std::cout << pf->getBestState().getVariable1() << std::endl; 91 | * 92 | * } 93 | * @endcode 94 | * 95 | * The ParticleFilter has the following resampling modes: 96 | * @li RESAMPLE_NEVER skip resampling, 97 | * @li RESAMPLE_ALWAYS does a resampling in every filter step whenever 98 | * filter() is called, 99 | * @li RESAMPLE_NEFF does a resampling in filter() only if the number of 100 | * effective particles falls below the half of the total number of 101 | * particles (see getNumEffectiveParticles() for details). 102 | * 103 | * The default is RESAMPLE_NEFF. 104 | * You can switch the mode via setResamplingMode(). 105 | * 106 | * You have two options to influence the states that are used internally by 107 | * the particle filter. The first one is to set a prior state: 108 | * @code 109 | * // initialize the filter's states by setting a prior state 110 | * MyState priorState; 111 | * priorState.setPosition(20, 30); 112 | * priorState.setVelocity(1.2, 0); 113 | * pf.setPriorState(priorState); 114 | * @endcode 115 | * The second option is to use a state distribution that is derived from 116 | * StateDistribution: 117 | * @code 118 | * // create a distribution 119 | * MyStateDistribution distribution; 120 | * // draw all states from this distribution 121 | * pf.drawAllFromDistribution(distribution); 122 | * @endcode 123 | * 124 | * To traverse the particle list, you may use particleListBegin() and particleListEnd() 125 | * which return iterators to the beginning and to the end of the list respectively. 126 | * 127 | * @see Particle 128 | * @see ObservationModel 129 | * @see MovementModel 130 | * @see ResamplingStrategy 131 | */ 132 | 133 | /** 134 | * Resampling modes. 135 | */ 136 | enum ResamplingMode 137 | { 138 | /// never resample, 139 | RESAMPLE_NEVER, 140 | /// always resample 141 | RESAMPLE_ALWAYS, 142 | /// only resample if Neff < numParticles / 2 143 | RESAMPLE_NEFF 144 | }; 145 | 146 | template 147 | class ParticleFilter { 148 | 149 | public: 150 | 151 | /** 152 | * A ParticleList is an array of pointers to Particles. 153 | */ 154 | typedef std::vector< Particle* > ParticleList; 155 | 156 | /** 157 | * Typedef for an iterator over particles 158 | */ 159 | typedef typename ParticleList::iterator ParticleIterator; 160 | 161 | /** 162 | * Typedef for a const iterator over particles 163 | */ 164 | typedef typename ParticleList::const_iterator ConstParticleIterator; 165 | 166 | /** 167 | * The constructor allocates the memory for the particle lists and saves 168 | * pointers to ObservationModel and MovementModel in member variables. 169 | * Be sure that these objects are valid through the lifetime of ParticleFilter! 170 | * The default constructor of StateType will be used to create the 171 | * initial particles. 172 | * The particle lists will have @a numParticles elements of type StateType. 173 | * @param numParticles Number of particles for the filter. Has to be greater 174 | * than zero. 175 | * @param os ObservationModel to use for weightening particles 176 | * @param ms MovementModel to use for propagation of particles 177 | */ 178 | ParticleFilter(unsigned int numParticles, ObservationModel* os, MovementModel* ms); 179 | 180 | /** 181 | * The destructor releases the particle lists. 182 | */ 183 | virtual ~ParticleFilter(); 184 | 185 | /** 186 | * @return Number of particles used in this filter 187 | */ 188 | unsigned int numParticles() const; 189 | 190 | /** 191 | * @param os new observation model 192 | */ 193 | void setObservationModel(ObservationModel* os); 194 | 195 | /** 196 | * @return the observation model the particle filter currently uses 197 | */ 198 | ObservationModel* getObservationModel() const; 199 | 200 | /** 201 | * @param ms new movement model 202 | */ 203 | void setMovementModel(MovementModel* ms); 204 | 205 | /** 206 | * @return the movement model the particle filter currently uses 207 | */ 208 | MovementModel* getMovementModel() const; 209 | 210 | /** 211 | * @param rs new resampling strategy 212 | */ 213 | void setResamplingStrategy(ResamplingStrategy* rs); 214 | 215 | /** 216 | * @return the resampling strategy the particle filter currently uses 217 | */ 218 | ResamplingStrategy* getResamplingStrategy() const; 219 | 220 | /** 221 | * Changes the resampling mode 222 | * @param mode new resampling mode. 223 | */ 224 | void setResamplingMode(ResamplingMode mode); 225 | 226 | /** 227 | * @return the currently set resampling mode 228 | */ 229 | ResamplingMode getResamplingMode() const; 230 | 231 | /** 232 | * Computes and returns the number of effective particles. 233 | * @return The estimated number of effective particles according to the formula: 234 | * \f[ 235 | * N_{eff} = \frac{1}{\sum_{i=1}^{N_s} (w_k^i)^2} 236 | * \f] 237 | */ 238 | unsigned int getNumEffectiveParticles() const; 239 | 240 | /** 241 | * Sets all particle states to the given state. Useful for integrating a 242 | * known prior state to begin with tracking. 243 | * @param priorState State that will be copied to all particles. 244 | */ 245 | void setPriorState(const StateType& priorState); 246 | 247 | /** 248 | * Draws all particle states from the given distribution. 249 | * @param distribution The state distribution to draw the states from. 250 | */ 251 | void drawAllFromDistribution(const StateDistribution& distribution); 252 | 253 | /** 254 | * Resets the filter timer. Call this function after pausing the filter 255 | * to avoid a drift step with a high delta t. 256 | */ 257 | void resetTimer(); 258 | 259 | /** 260 | * @return Pointer to the particle that has the highest weight. 261 | */ 262 | const Particle* getBestParticle() const; 263 | 264 | /** 265 | * @return State that is carried by the particle with heighest weight. 266 | */ 267 | const StateType& getBestState() const; 268 | 269 | /** 270 | * @param i Particle index 271 | * @return weight of Particle i. 272 | */ 273 | double getWeight(unsigned int i) const; 274 | 275 | /** 276 | * Performs an entire filter procedure. 277 | * filter() also saves the last filter time to be able to compute the 278 | * appropriate dt for the drift step. To reset the time manually, call 279 | * resetTimer(). 280 | * The functions resample(), 281 | * drift(), diffuse() and measure() are called. 282 | * @param dt time interval to use for filtering. If negative, the time interval 283 | * will be calculated (time since last call of filter()) 284 | */ 285 | void filter(double dt = -1.0); 286 | 287 | /** 288 | * Returns a pointer to a particle with a given index. 289 | * @param particleNo Index of requested particle 290 | * @return Pointer to particle with index particleNo 291 | */ 292 | const Particle* getParticle(unsigned int particleNo) const; 293 | 294 | /** 295 | * Returns a const reference to the state of particle with given index. 296 | * @param particleNo Index of particle 297 | * @return Pointer to the state of particle at index particleNo. 298 | */ 299 | const StateType& getState(unsigned int particleNo) const; 300 | 301 | /** 302 | * Returns the "mean" state, i.e. the sum of the weighted states. You can use this only if you implemented operator*(double) and 303 | * operator+=(MyState) in your derived State MyState. 304 | * @return "mean" state. Best estimation. 305 | */ 306 | StateType getMmseEstimate() const; 307 | 308 | /** 309 | * Same as getMmseEstimate(), but uses only the best x% of the particles. 310 | * @param x percentage of particles to use. Has to be positive and greater 311 | * than zero. 312 | * @return "mean" state of the best x% particles. If x <= 0, the state 313 | * with the highest weight is returned. 314 | */ 315 | StateType getBestXPercentEstimate(float x) const; 316 | 317 | /** 318 | * This method selects a new set of particles out of an old set according to their weight 319 | * (importance resampling). The particles from the list m_CurrentList points to are used as source, 320 | * m_LastList points to the destination list. The pointers m_CurrentList and m_LastList are switched. 321 | * The higher the weight of a particle, the more particles are drawn (copied) from this particle. 322 | * The weight remains untouched, because measure() will be called afterwards. 323 | * This method only works on a sorted m_CurrentList, therefore sort() is called first. 324 | */ 325 | void resample(); 326 | 327 | /** 328 | * This method drifts the particles (second step of a filter process) using 329 | * the movement model of the particle filter. dt defines the time interval 330 | * that has to be used in drifting (in seconds). 331 | */ 332 | void drift(double dt); 333 | 334 | /** 335 | * This method "diffuses" the particles using the movement model of the particle filter to add a small jitter 336 | * to the particle states. dt defines the time interval that has to be used in diffusion (in seconds). 337 | */ 338 | void diffuse(double dt); 339 | 340 | /** 341 | * This method assigns weights to the particles using the observation model of the particle filter. 342 | */ 343 | virtual void measure(); 344 | 345 | /** 346 | * Returns an iterator to the particle list's beginning. 347 | */ 348 | ConstParticleIterator particleListBegin(); 349 | 350 | /** 351 | * Returns an iterator to the end of the particle list. 352 | */ 353 | ConstParticleIterator particleListEnd(); 354 | 355 | 356 | protected: 357 | 358 | /** 359 | * This method sorts the particles according to their weight. STL's std::sort() is used together with the 360 | * custom compare function CompareParticleWeights(). 361 | * The particle with the highest weight is at position 0 after calling this function. 362 | */ 363 | void sort(); 364 | 365 | /** 366 | * This method normalizes the weights of the particles. After calling this function, the sum of the weights of 367 | * all particles in the current particle list equals 1.0. 368 | * In this function the sum of all weights of the particles of the current particle list is computed and each 369 | * weight of each particle is devided through this sum. 370 | */ 371 | void normalize(); 372 | 373 | private: 374 | 375 | // Particle lists. 376 | // The particles are drawn from m_LastList to m_CurrentList to avoid new and delete commands. 377 | // In each run, the pointers m_CurrentList and m_LastList are switched in resample(). 378 | ParticleList m_CurrentList; 379 | ParticleList m_LastList; 380 | 381 | // Stores the number of particles. 382 | unsigned int m_NumParticles; 383 | 384 | // Holds a pointer to the observation strategy (used for weighting) 385 | ObservationModel* m_ObservationModel; 386 | 387 | // Holds a pointer to the movement strategy (used for moving and diffusing) 388 | MovementModel* m_MovementModel; 389 | 390 | // Stores a pointer to the resampling strategy. 391 | ResamplingStrategy* m_ResamplingStrategy; 392 | 393 | // The default resampling strategy. 394 | ImportanceResampling m_DefaultResamplingStrategy; 395 | 396 | // Stores the last filter time to have the right dt value for drift. 397 | clock_t m_LastDriftTime; 398 | 399 | // Flag that stores if the filter has run once or not) 400 | bool m_FirstRun; 401 | 402 | // Stores which resampling mode is set, default is ResamplingMode::RESAMPLE_NEFF 403 | ResamplingMode m_ResamplingMode; 404 | 405 | 406 | }; 407 | 408 | } // end of namespace 409 | 410 | // include template implementation 411 | #include "ParticleFilter.hxx" 412 | 413 | #endif 414 | 415 | -------------------------------------------------------------------------------- /lib/include/libPF/ParticleFilter.hxx: -------------------------------------------------------------------------------- 1 | namespace libPF 2 | { 3 | 4 | 5 | template 6 | ParticleFilter::ParticleFilter(unsigned int numParticles, ObservationModel* os, MovementModel* ms) : 7 | m_NumParticles(numParticles), 8 | m_ObservationModel(os), 9 | m_MovementModel(ms), 10 | m_ResamplingStrategy(&m_DefaultResamplingStrategy), 11 | m_FirstRun(true), 12 | m_ResamplingMode(RESAMPLE_NEFF) 13 | { 14 | 15 | assert(numParticles > 0); 16 | 17 | // allocate memory for particle lists 18 | m_CurrentList.resize(numParticles); 19 | m_LastList.resize(numParticles); 20 | 21 | double initialWeight = 1.0 / numParticles; 22 | // fill particle lists 23 | for (unsigned int i = 0; i < numParticles; i++) { 24 | m_CurrentList[i] = new Particle(StateType(), initialWeight); 25 | m_LastList[i] = new Particle(StateType(), initialWeight); 26 | } 27 | } 28 | 29 | 30 | template 31 | ParticleFilter::~ParticleFilter() { 32 | // release particles 33 | ConstParticleIterator iter; 34 | for (iter = m_CurrentList.begin(); iter != m_CurrentList.end(); ++iter) 35 | { 36 | delete *iter; 37 | } 38 | for (iter = m_LastList.begin(); iter != m_LastList.end(); ++iter) 39 | { 40 | delete *iter; 41 | } 42 | } 43 | 44 | 45 | template 46 | unsigned int ParticleFilter::numParticles() const { 47 | return m_NumParticles; 48 | } 49 | 50 | template 51 | void ParticleFilter::setObservationModel(ObservationModel* os) { 52 | m_ObservationModel = os; 53 | } 54 | 55 | template 56 | ObservationModel* ParticleFilter::getObservationModel() const { 57 | return m_ObservationModel; 58 | } 59 | 60 | template 61 | void ParticleFilter::setMovementModel(MovementModel* ms) { 62 | m_MovementModel = ms; 63 | } 64 | 65 | template 66 | MovementModel* ParticleFilter::getMovementModel() const { 67 | return m_MovementModel; 68 | } 69 | 70 | template 71 | void ParticleFilter::setResamplingStrategy(ResamplingStrategy* rs) { 72 | m_ResamplingStrategy = rs; 73 | } 74 | 75 | template 76 | ResamplingStrategy* ParticleFilter::getResamplingStrategy() const { 77 | return m_ResamplingStrategy; 78 | } 79 | 80 | template 81 | void ParticleFilter::setResamplingMode(ResamplingMode mode) { 82 | m_ResamplingMode = mode; 83 | } 84 | 85 | template 86 | ResamplingMode ParticleFilter::getResamplingMode() const { 87 | return m_ResamplingMode; 88 | } 89 | 90 | template 91 | void ParticleFilter::setPriorState(const StateType& priorState) { 92 | ConstParticleIterator iter; 93 | for (iter = m_CurrentList.begin(); iter != m_CurrentList.end(); ++iter) 94 | { 95 | (*iter)->setState(priorState); 96 | } 97 | } 98 | 99 | template 100 | void ParticleFilter::drawAllFromDistribution(const StateDistribution& distribution) { 101 | ConstParticleIterator iter; 102 | for (iter = m_CurrentList.begin(); iter != m_CurrentList.end(); ++iter) 103 | { 104 | (*iter)->setState(distribution.draw()); 105 | } 106 | } 107 | 108 | template 109 | void ParticleFilter::resetTimer() { 110 | m_FirstRun = true; 111 | } 112 | 113 | template 114 | void ParticleFilter::filter(double dt) { 115 | if (m_ResamplingMode == RESAMPLE_NEFF) { 116 | if (getNumEffectiveParticles() < m_NumParticles / 2) { 117 | resample(); 118 | } 119 | } else if (m_ResamplingMode == RESAMPLE_ALWAYS) { 120 | resample(); 121 | } // else do not resample 122 | 123 | if (dt < 0.0) // use internal time measurement 124 | { 125 | // for the first run, we have no information about the time interval 126 | if (m_FirstRun) { 127 | m_FirstRun = false; 128 | m_LastDriftTime = clock(); 129 | } 130 | clock_t currentTime = clock(); 131 | dt = ((double)currentTime - (double)m_LastDriftTime) / CLOCKS_PER_SEC; 132 | m_LastDriftTime = currentTime; 133 | } 134 | drift(dt); 135 | diffuse(dt); 136 | measure(); 137 | } 138 | 139 | template 140 | const Particle* ParticleFilter::getParticle(unsigned int particleNo) const { 141 | assert(particleNo < m_NumParticles); 142 | return m_CurrentList[particleNo]; 143 | } 144 | 145 | template 146 | const StateType& ParticleFilter::getState(unsigned int particleNo) const { 147 | assert(particleNo < m_NumParticles); 148 | return m_CurrentList[particleNo]->getState(); 149 | } 150 | 151 | template 152 | double ParticleFilter::getWeight(unsigned int particleNo) const { 153 | assert(particleNo < m_NumParticles); 154 | return m_CurrentList[particleNo]->getWeight(); 155 | } 156 | 157 | 158 | template 159 | void ParticleFilter::sort() { 160 | std::sort(m_CurrentList.begin(), m_CurrentList.end(), CompareParticleWeights()); 161 | } 162 | 163 | template 164 | void ParticleFilter::normalize() { 165 | double weightSum = 0.0; 166 | ConstParticleIterator iter; 167 | for (iter = m_CurrentList.begin(); iter != m_CurrentList.end(); ++iter) { 168 | weightSum += (*iter)->getWeight(); 169 | } 170 | // only normalize if weightSum is big enough to devide 171 | if (weightSum > m_NumParticles * std::numeric_limits::epsilon()) { 172 | double factor = 1.0 / weightSum; 173 | for (iter = m_CurrentList.begin(); iter != m_CurrentList.end(); ++iter) { 174 | double newWeight = (*iter)->getWeight() * factor; 175 | (*iter)->setWeight(newWeight); 176 | } 177 | } else { 178 | std::cerr << "WARNING: ParticleFilter::normalize(): Particle weights *very* small!" << std::endl; 179 | } 180 | } 181 | 182 | template 183 | void ParticleFilter::resample() { 184 | // swap lists 185 | m_CurrentList.swap(m_LastList); 186 | // call resampling strategy 187 | m_ResamplingStrategy->resample(m_LastList, m_CurrentList); 188 | } 189 | 190 | 191 | template 192 | void ParticleFilter::drift(double dt) { 193 | for (unsigned int i = 0; i < m_NumParticles; i++) { 194 | m_MovementModel->drift(m_CurrentList[i]->m_State, dt); 195 | } 196 | } 197 | 198 | template 199 | void ParticleFilter::diffuse(double dt) { 200 | for (unsigned int i = 0; i < m_NumParticles; i++) { 201 | m_MovementModel->diffuse(m_CurrentList[i]->m_State, dt); 202 | } 203 | } 204 | 205 | template 206 | void ParticleFilter::measure() { 207 | for (unsigned int i = 0; i < m_NumParticles; i++) { 208 | // apply observation model 209 | double weight = m_ObservationModel->measure(m_CurrentList[i]->getState()); 210 | m_CurrentList[i]->setWeight(weight); 211 | } 212 | // after measurement we have to re-sort and normalize the particles 213 | sort(); 214 | normalize(); 215 | } 216 | 217 | template 218 | unsigned int ParticleFilter::getNumEffectiveParticles() const { 219 | double squareSum = 0; 220 | for (unsigned int i = 0; i < m_NumParticles; i++) { 221 | double weight = m_CurrentList[i]->getWeight(); 222 | squareSum += weight * weight; 223 | } 224 | return static_cast(1.0f / squareSum); 225 | } 226 | 227 | 228 | template 229 | const Particle* ParticleFilter::getBestParticle() const { 230 | return m_CurrentList[0]; 231 | } 232 | 233 | template 234 | const StateType& ParticleFilter::getBestState() const { 235 | return m_CurrentList[0]->getState(); 236 | } 237 | 238 | template 239 | StateType ParticleFilter::getMmseEstimate() const { 240 | StateType estimate = m_CurrentList[0]->getState() * m_CurrentList[0]->getWeight(); 241 | for (unsigned int i = 1; i < m_NumParticles; i++) { 242 | estimate += m_CurrentList[i]->getState() * m_CurrentList[i]->getWeight(); 243 | } 244 | return estimate; 245 | } 246 | 247 | template 248 | StateType ParticleFilter::getBestXPercentEstimate(float percentage) const { 249 | StateType estimate = m_CurrentList[0]->getState() * m_CurrentList[0]->getWeight(); 250 | double weightSum = m_CurrentList[0]->getWeight(); 251 | unsigned int numToConsider = m_NumParticles / 100.0f * percentage; 252 | for (unsigned int i = 1; i < numToConsider; i++) { 253 | estimate += m_CurrentList[i]->getState() * m_CurrentList[i]->getWeight(); 254 | weightSum += m_CurrentList[i]->getWeight(); 255 | } 256 | estimate = estimate * (1.0 / weightSum); 257 | return estimate; 258 | } 259 | 260 | template 261 | typename ParticleFilter::ConstParticleIterator ParticleFilter::particleListBegin() 262 | { 263 | return m_CurrentList.begin(); 264 | } 265 | 266 | template 267 | typename ParticleFilter::ConstParticleIterator ParticleFilter::particleListEnd() 268 | { 269 | return m_CurrentList.end(); 270 | } 271 | 272 | } // end of namespace 273 | -------------------------------------------------------------------------------- /lib/include/libPF/RandomNumberGenerationStrategy.h: -------------------------------------------------------------------------------- 1 | #ifndef RANDOMNUMBERGENERATIONSTRATEGY_H 2 | #define RANDOMNUMBERGENERATIONSTRATEGY_H 3 | 4 | namespace libPF 5 | { 6 | 7 | /** 8 | * @class RandomNumberGenerationStrategy 9 | * 10 | * @brief Interface for a strategy to create random numbers 11 | * 12 | * This class defines the interface for random number generators. To create your 13 | * own random number generation strategy, sub-class from this class. 14 | * 15 | * @author Stephan Wirth 16 | */ 17 | class RandomNumberGenerationStrategy { 18 | 19 | public: 20 | 21 | /** 22 | * Empty constructor. 23 | */ 24 | RandomNumberGenerationStrategy() {}; 25 | 26 | /** 27 | * Empty destructor. 28 | */ 29 | virtual ~RandomNumberGenerationStrategy() {}; 30 | 31 | /** 32 | * Interface for the generation function of gaussian distributed numbers. 33 | * @param standardDeviation Standard deviation d of the random number to generate. 34 | * @return N(0, d*d)-distributed random number 35 | */ 36 | virtual double getGaussian(double standardDeviation) const = 0; 37 | 38 | /** 39 | * Interface for the generation of a uniform distributed random number between min and max. 40 | * @param min the minimum value, default is 0.0 41 | * @param max the maximum value, default is 1.0 42 | * @return random number between min and max, uniform distributed. 43 | */ 44 | virtual double getUniform(double min = 0.0, double max = 1.0) const = 0; 45 | 46 | protected: 47 | 48 | private: 49 | 50 | }; 51 | 52 | } // end of namespace 53 | 54 | #endif // RANDOMNUMBERGENERATIONSTRATEGY_H 55 | -------------------------------------------------------------------------------- /lib/include/libPF/ResamplingStrategy.h: -------------------------------------------------------------------------------- 1 | #ifndef RESAMPLINGSTRATEGY_H 2 | #define RESAMPLINGSTRATEGY_H 3 | 4 | #include 5 | 6 | #include "libPF/Particle.h" 7 | 8 | namespace libPF 9 | { 10 | 11 | /** 12 | * @class ResamplingStrategy 13 | * 14 | * @brief Templated interface for resampling strategies 15 | * 16 | * The resampling strategy defines how the resampling is performed in the resample step 17 | * of a particle filter. One implementation of this strategy is @see ImportanceResampling. 18 | * 19 | * @author Stephan Wirth 20 | * 21 | * @see ParticleFilter 22 | */ 23 | 24 | template 25 | class ResamplingStrategy { 26 | 27 | /** 28 | * A ParticleList is an array of pointers to Particles. 29 | */ 30 | typedef std::vector< Particle* > ParticleList; 31 | 32 | public: 33 | 34 | /** 35 | * The destructor is empty. 36 | */ 37 | virtual ~ResamplingStrategy(); 38 | 39 | /** 40 | * This is the main method of ResamplingStrategy. It takes two references to 41 | * particle lists. The first reference refers to the old particle list, the 42 | * second to the new one. The strategy has to define which particles have to be copied to the new list. 43 | * Use the assignment operator to copy a particle. Be careful that you don't copy 44 | * the pointer to the particle! Never change the size of the lists! 45 | * Define this function in your sub-class! 46 | * @param source the source list to draw new particles from. 47 | * @param destination the destination list where to put the copies. 48 | */ 49 | virtual void resample(const ParticleList& source, const ParticleList& destination) const = 0; 50 | 51 | private: 52 | 53 | }; 54 | 55 | template 56 | ResamplingStrategy::~ResamplingStrategy() { 57 | } 58 | 59 | } // end of namespace 60 | #endif // RESAMPLINGSTRATEGY_H 61 | 62 | -------------------------------------------------------------------------------- /lib/include/libPF/StateDistribution.h: -------------------------------------------------------------------------------- 1 | #ifndef STATEDISTRIBUTION_H 2 | #define STATEDISTRIBUTION_H 3 | namespace libPF 4 | { 5 | 6 | /** 7 | * @class StateDistribution 8 | * 9 | * @brief Templated base class for state distributions. 10 | * 11 | * To initialize or reset a particle filter, you may need to use a state 12 | * distribution to draw your states from. For example, if you do tracking of a 13 | * position (x, y) in an image a StateDistribution may be used to draw random 14 | * positions of valid pixels from {(x, y) | x in {0;width-1}, y in {0, height-1}} 15 | * 16 | * Use this class as base class for your state distribution and you can use 17 | * the method ParticleFilter::drawAllFromDistribution(). 18 | * 19 | * @author Stephan Wirth 20 | * @see ParticleFilter 21 | */ 22 | 23 | template 24 | class StateDistribution { 25 | 26 | public: 27 | /** 28 | * The constructor of this base class is empty. 29 | */ 30 | StateDistribution(); 31 | 32 | /** 33 | * The destructor is empty. 34 | */ 35 | virtual ~StateDistribution(); 36 | 37 | /** 38 | * This is the main method of StateDistribution. It has to return a (random) 39 | * state, drawn from your distribution. ParticleFilter will copy the state 40 | * using the assignment operator, so be sure your state supports it! 41 | * Define this method in your sub-class! 42 | * @return Drawn state from the distribution. 43 | */ 44 | virtual const StateType draw() const = 0; 45 | 46 | private: 47 | 48 | }; 49 | 50 | 51 | template 52 | StateDistribution::StateDistribution() { 53 | } 54 | 55 | template 56 | StateDistribution::~StateDistribution() { 57 | } 58 | 59 | } // end of namespace 60 | 61 | #endif // STATEDISTRIBUTION_H 62 | -------------------------------------------------------------------------------- /lib/src/CRandomNumberGenerator.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | 6 | #include "libPF/CRandomNumberGenerator.h" 7 | 8 | using namespace libPF; 9 | 10 | CRandomNumberGenerator::CRandomNumberGenerator() 11 | { 12 | init(); 13 | } 14 | 15 | CRandomNumberGenerator::~CRandomNumberGenerator() 16 | { 17 | } 18 | 19 | void CRandomNumberGenerator::init() 20 | { 21 | srand(time(0)); 22 | } 23 | 24 | double CRandomNumberGenerator::getGaussian(double standardDeviation) const 25 | { 26 | 27 | if (standardDeviation < 0) { 28 | standardDeviation = -standardDeviation; 29 | } 30 | 31 | if (m_GaussianBufferFilled == true) 32 | { 33 | m_GaussianBufferFilled = false; 34 | return standardDeviation * m_GaussianBufferVariable; 35 | } 36 | double x1, x2, w, y1, y2; 37 | do { 38 | x1 = getUniform(-1.0, 1.0); 39 | x2 = getUniform(-1.0, 1.0); 40 | w = x1 * x1 + x2 * x2; 41 | } while ( w >= 1.0 ); 42 | 43 | w = sqrt((-2.0 * log(w)) / w); 44 | y1 = x1 * w; 45 | y2 = x2 * w; 46 | // now y1 and y2 are N(0,1) distributed 47 | // we use only one, so we store the other 48 | m_GaussianBufferVariable = y2; 49 | m_GaussianBufferFilled = true; 50 | return standardDeviation * y1; 51 | } 52 | 53 | double CRandomNumberGenerator::getUniform(double min, double max) const 54 | { 55 | double range = max - min; 56 | return 1.0 * rand() / RAND_MAX * range + min; 57 | } 58 | -------------------------------------------------------------------------------- /sample1/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | include_directories(../lib/include) 3 | 4 | add_executable(sample1 5 | main.cpp 6 | MyMovementModel.cpp 7 | MyObservationModel.cpp 8 | MyStateDistribution.cpp) 9 | 10 | target_link_libraries(sample1 PF) 11 | -------------------------------------------------------------------------------- /sample1/MyMovementModel.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "MyMovementModel.h" 4 | 5 | MyMovementModel::MyMovementModel() : libPF::MovementModel() { 6 | } 7 | 8 | MyMovementModel::~MyMovementModel() { 9 | } 10 | 11 | void MyMovementModel::drift(float& /*state*/, double /*dt*/) const { 12 | } 13 | 14 | void MyMovementModel::diffuse(float& state, double /*dt*/) const { 15 | state = state + m_RNG.getGaussian(0.0001); 16 | } 17 | -------------------------------------------------------------------------------- /sample1/MyMovementModel.h: -------------------------------------------------------------------------------- 1 | #ifndef MYMOVEMENTMODEL_H 2 | #define MYMOVEMENTMODEL_H 3 | 4 | 5 | #include 6 | #include 7 | 8 | /** 9 | * @class MyMovementModel 10 | * 11 | * @brief Test class for ParticleFilter. 12 | * 13 | * This simple movement model only adds a small jitter in diffuse() and does 14 | * nothing in drift(). 15 | * @author Stephan Wirth 16 | */ 17 | class MyMovementModel : public libPF::MovementModel { 18 | 19 | public: 20 | /** 21 | * empty 22 | */ 23 | MyMovementModel(); 24 | 25 | /** 26 | * empty 27 | */ 28 | ~MyMovementModel(); 29 | 30 | /** 31 | * The drift method is empty in this example. 32 | * @param state Pointer to the state that has to be manipulated. 33 | */ 34 | void drift(float& state, double dt) const; 35 | 36 | /** 37 | * The diffusion consists of a very small gaussian jitter on the 38 | * state's variable. 39 | * @param state Pointer to the state that has to be manipulated. 40 | */ 41 | void diffuse(float& state, double dt) const; 42 | 43 | protected: 44 | 45 | private: 46 | 47 | /// The random number generator 48 | libPF::CRandomNumberGenerator m_RNG; 49 | 50 | }; 51 | 52 | #endif 53 | 54 | -------------------------------------------------------------------------------- /sample1/MyObservationModel.cpp: -------------------------------------------------------------------------------- 1 | #include "MyObservationModel.h" 2 | 3 | MyObservationModel::MyObservationModel() : libPF::ObservationModel() { 4 | } 5 | 6 | MyObservationModel::~MyObservationModel() { 7 | } 8 | 9 | double MyObservationModel::measure(const float& state) const { 10 | return 1.0/(fabs(state * state - 2)); 11 | } 12 | 13 | 14 | -------------------------------------------------------------------------------- /sample1/MyObservationModel.h: -------------------------------------------------------------------------------- 1 | #ifndef MYOBSERVATIONMODEL_H 2 | #define MYOBSERVATIONMODEL_H 3 | 4 | #include 5 | 6 | #include 7 | 8 | /** 9 | * @class MyObservationModel 10 | * 11 | * @brief Observation model that measures a MyState. 12 | * 13 | * The measurement is made according to the formula 14 | * \f[ 15 | * w = \frac{1}{|x^2 - 2|} 16 | * \f] 17 | * where x is the variable of MyState and w is the weight that is returned. 18 | * This is a measure for the distance from x to the squareroot of two. If 19 | * The distance is low, the returned weight is high. 20 | * 21 | * @author Stephan Wirth 22 | * 23 | * @brief Test class for ParticleFilter. 24 | * 25 | */ 26 | class MyObservationModel : public libPF::ObservationModel { 27 | 28 | public: 29 | 30 | /** 31 | * empty 32 | */ 33 | MyObservationModel(); 34 | 35 | /** 36 | * empty 37 | */ 38 | ~MyObservationModel(); 39 | 40 | /** 41 | * 42 | * @param state Reference to the state that has to be weightened. 43 | * @return weight for the given state. 44 | */ 45 | double measure(const float& state) const; 46 | 47 | protected: 48 | 49 | private: 50 | 51 | }; 52 | 53 | #endif 54 | -------------------------------------------------------------------------------- /sample1/MyStateDistribution.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "MyStateDistribution.h" 3 | 4 | MyStateDistribution::MyStateDistribution() 5 | { 6 | } 7 | 8 | MyStateDistribution::~MyStateDistribution() 9 | { 10 | } 11 | 12 | const float MyStateDistribution::draw() const 13 | { 14 | return m_RNG.getUniform(0.0, 2.0); 15 | } 16 | -------------------------------------------------------------------------------- /sample1/MyStateDistribution.h: -------------------------------------------------------------------------------- 1 | #ifndef MYSTATEDISTRIBUTION_H 2 | #define MYSTATEDISTRIBUTION_H 3 | 4 | #include 5 | #include 6 | 7 | /** 8 | * @class MyStateDistribution 9 | * 10 | * @brief Distribution to draw a random state from 11 | * 12 | * As the state in this example is a simple float that tries 13 | * to "track" the squareroot of two, the distribution returns 14 | * a uniformly distributed number between zero and two. 15 | * 16 | * @author Stephan Wirth 17 | * 18 | */ 19 | class MyStateDistribution : public libPF::StateDistribution 20 | { 21 | public: 22 | /** 23 | * Empty constructor 24 | */ 25 | MyStateDistribution(); 26 | 27 | /** 28 | * Empty destructor 29 | */ 30 | ~MyStateDistribution(); 31 | 32 | /** 33 | * @return a random number between 0 and 2 34 | */ 35 | const float draw() const; 36 | 37 | private: 38 | 39 | /// The random number generator 40 | libPF::CRandomNumberGenerator m_RNG; 41 | }; 42 | 43 | #endif // MYSTATEDISTRIBUTION_H 44 | -------------------------------------------------------------------------------- /sample1/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | #include "MyObservationModel.h" 7 | #include "MyMovementModel.h" 8 | #include "MyStateDistribution.h" 9 | 10 | using namespace std; 11 | int main(int, char**) { 12 | 13 | MyObservationModel om; 14 | MyMovementModel mm; 15 | 16 | typedef libPF::ParticleFilter ParticleFilterType; 17 | 18 | ParticleFilterType pf(500, &om, &mm); 19 | 20 | cout << "Particle Filter constructed. There are " << pf.numParticles() << " particles in the filter." << endl; 21 | 22 | // getters of models 23 | libPF::ObservationModel* currentObservationModel = pf.getObservationModel(); 24 | cout << (currentObservationModel == &om) << endl; // should be "1" 25 | libPF::MovementModel* currentMovementModel = pf.getMovementModel(); 26 | cout << (currentMovementModel == &mm) << endl; // should be "1" 27 | 28 | // setters of models 29 | MyObservationModel om2; 30 | MyMovementModel mm2; 31 | pf.setObservationModel(&om2); 32 | pf.setMovementModel(&mm2); 33 | 34 | // change the resampling mode 35 | if (pf.getResamplingMode() == libPF::RESAMPLE_NEFF) 36 | { 37 | pf.setResamplingMode(libPF::RESAMPLE_ALWAYS); 38 | } 39 | 40 | // getter for resampling strategy 41 | libPF::ResamplingStrategy* resamplingStrategy = pf.getResamplingStrategy(); 42 | 43 | // setter for resampling strategy (not implemented here) 44 | // MyResamplingStrategy myResamplingStrategy; 45 | // pf.setResamplingStrategy(&myResamplingStrategy); 46 | 47 | // to integrate a known prior state use 48 | pf.setPriorState(1.4); 49 | 50 | // or use a distribution to draw from 51 | MyStateDistribution distribution; 52 | pf.drawAllFromDistribution(distribution); 53 | 54 | for (int i = 0; i < 10000; i++) { 55 | pf.filter(); 56 | float best = pf.getBestState(); 57 | float mmse = pf.getMmseEstimate(); 58 | float best5percent = pf.getBestXPercentEstimate(5.0); 59 | if (i % 1000 == 0) 60 | cout << " best: " << setiosflags(ios::fixed) << setprecision(10) << best << " (" << setiosflags(ios::fixed) << setprecision(10) << best*best << ") " << 61 | " mmse: " << setiosflags(ios::fixed) << setprecision(10) << mmse << " (" << setiosflags(ios::fixed) << setprecision(10) << mmse*mmse << ") " << 62 | " best 5%: " << setiosflags(ios::fixed) << setprecision(10) << best5percent << " (" << setiosflags(ios::fixed) << setprecision(10) << best5percent*best5percent << ") " << 63 | endl; 64 | } 65 | 66 | // to reset the timer call 67 | pf.resetTimer(); 68 | 69 | // you can call the filter steps by hand 70 | double dt = 0.001; 71 | pf.resample(); 72 | pf.drift(dt); 73 | pf.diffuse(dt); 74 | pf.measure(); 75 | 76 | // iteration over the particle list 77 | ParticleFilterType::ConstParticleIterator iter; 78 | unsigned int count = 0; 79 | for (iter = pf.particleListBegin(); iter != pf.particleListEnd(); ++iter) 80 | { 81 | libPF::Particle* particle = *iter; 82 | cout << "Particle [" << count++ << "]: weight = " << particle->getWeight() << ", value = " << particle->getState() << endl; 83 | } 84 | 85 | cout << endl; 86 | 87 | // you can iterate over the list with indices as well and request weight and state from the particle filter 88 | for (unsigned int index = 0; index < pf.numParticles(); index += 100) 89 | { 90 | cout << "Particle [" << index << "]: weight = " << pf.getWeight(index) << ", value = " << pf.getState(index) << endl; 91 | } 92 | 93 | // number of effective particles 94 | cout << "Number of effective particles: " << pf.getNumEffectiveParticles() << endl; 95 | 96 | return 0; 97 | } 98 | 99 | -------------------------------------------------------------------------------- /sample2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | set(QT_USE_QTOPENGL TRUE) 3 | 4 | find_package(Qt4 REQUIRED) 5 | 6 | include(${QT_USE_FILE}) 7 | add_definitions(${QT_DEFINITIONS}) 8 | 9 | set(sample2_SOURCES main.cpp 10 | CarMovementModel.cpp 11 | CarObservationModel.cpp 12 | CarState.cpp 13 | CarStateDistribution.cpp 14 | MainWindow.cpp 15 | RenderWidget.cpp) 16 | 17 | # headers that nead to be moc'ed 18 | set(sample2_HEADERS 19 | MainWindow.h 20 | RenderWidget.h) 21 | 22 | # ui files 23 | set(sample2_FORMS MainWindowForm.ui) 24 | 25 | QT4_WRAP_CPP(sample2_HEADERS_MOC ${sample2_HEADERS}) 26 | QT4_WRAP_UI(sample2_HEADERS_FORMS ${sample2_FORMS}) 27 | 28 | include_directories(${CMAKE_CURRENT_BINARY_DIR}) 29 | include_directories(./ ../lib/include) 30 | 31 | add_executable(sample2 ${sample2_SOURCES} ${sample2_HEADERS_MOC} ${sample2_HEADERS_FORMS}) 32 | target_link_libraries(sample2 ${QT_LIBRARIES} PF) 33 | 34 | -------------------------------------------------------------------------------- /sample2/CarMovementModel.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "CarMovementModel.h" 3 | 4 | using namespace std; 5 | 6 | CarMovementModel::CarMovementModel() : libPF::MovementModel() , 7 | m_XStdDev(10.0), 8 | m_YStdDev(10.0), 9 | m_ThetaStdDev(0.0), 10 | m_SpeedStdDev(400.0), 11 | m_RotSpeedStdDev(1000.0 / 180.0 * M_PI) 12 | { 13 | m_RNG = new libPF::CRandomNumberGenerator(); 14 | } 15 | 16 | CarMovementModel::~CarMovementModel() { 17 | delete m_RNG; 18 | } 19 | 20 | void CarMovementModel::drift(CarState& state, double dt) const { 21 | double speed = state.getSpeed(); 22 | double orientation = state.getTheta(); 23 | state.setXPos(state.getXPos() + cos(orientation) * dt * speed); 24 | state.setYPos(state.getYPos() + sin(orientation) * dt * speed); 25 | state.setTheta(state.getTheta() + dt * state.getRotationSpeed()); 26 | } 27 | 28 | void CarMovementModel::diffuse(CarState& state, double dt) const { 29 | state.setXPos(state.getXPos() + m_RNG->getGaussian(m_XStdDev) * dt); 30 | state.setYPos(state.getYPos() + m_RNG->getGaussian(m_YStdDev) * dt); 31 | state.setSpeed(state.getSpeed() + m_RNG->getGaussian(m_SpeedStdDev) * dt); 32 | state.setTheta(state.getTheta() + m_RNG->getGaussian(m_ThetaStdDev) * dt); 33 | state.setRotationSpeed(state.getRotationSpeed() + m_RNG->getGaussian(m_RotSpeedStdDev) * dt); 34 | } 35 | 36 | void CarMovementModel::setXStdDev(double d) 37 | { 38 | m_XStdDev = d; 39 | } 40 | 41 | double CarMovementModel::getXStdDev() const 42 | { 43 | return m_XStdDev; 44 | } 45 | 46 | void CarMovementModel::setYStdDev(double d) 47 | { 48 | m_YStdDev = d; 49 | } 50 | 51 | double CarMovementModel::getYStdDev() const 52 | { 53 | return m_YStdDev; 54 | } 55 | 56 | void CarMovementModel::setThetaStdDev(double d) 57 | { 58 | m_ThetaStdDev = d; 59 | } 60 | 61 | double CarMovementModel::getThetaStdDev() const 62 | { 63 | return m_ThetaStdDev; 64 | } 65 | 66 | void CarMovementModel::setSpeedStdDev(double d) 67 | { 68 | m_SpeedStdDev = d; 69 | } 70 | 71 | double CarMovementModel::getSpeedStdDev() const 72 | { 73 | return m_SpeedStdDev; 74 | } 75 | 76 | void CarMovementModel::setRotationSpeedStdDev(double d) 77 | { 78 | m_RotSpeedStdDev = d; 79 | } 80 | 81 | double CarMovementModel::getRotationSpeedStdDev() const 82 | { 83 | return m_RotSpeedStdDev; 84 | } 85 | -------------------------------------------------------------------------------- /sample2/CarMovementModel.h: -------------------------------------------------------------------------------- 1 | #ifndef CARMOVEMENTMODEL_H 2 | #define CARMOVEMENTMODEL_H 3 | 4 | #include 5 | 6 | #include "CarState.h" 7 | 8 | class RandomNumberGenerator; 9 | 10 | /** 11 | * @class CarMovementModel 12 | * 13 | * @brief Test class for ParticleFilter. 14 | * 15 | * This movement model propagates a car state according to the translation and 16 | * rotation speed. 17 | * 18 | * @author Stephan Wirth 19 | */ 20 | class CarMovementModel : public libPF::MovementModel { 21 | 22 | public: 23 | /** 24 | * Constructor 25 | */ 26 | CarMovementModel(); 27 | 28 | /** 29 | * Destructor 30 | */ 31 | ~CarMovementModel(); 32 | 33 | /** 34 | * The drift method propagates the car using its speed. 35 | * @param state Pointer to the state that has to be manipulated. 36 | */ 37 | void drift(CarState& state, double dt) const; 38 | 39 | /** 40 | * The diffusion consists of a very small gaussian jitter on the 41 | * state's variable. 42 | * @param state Pointer to the state that has to be manipulated. 43 | */ 44 | void diffuse(CarState& state, double dt) const; 45 | 46 | /** 47 | * @param d new standard deviation for the diffusion of x 48 | */ 49 | void setXStdDev(double d); 50 | 51 | /** 52 | * @return the standard deviation for the diffusion of x 53 | */ 54 | double getXStdDev() const; 55 | 56 | /** 57 | * @param d new standard deviation for the diffusion of y 58 | */ 59 | void setYStdDev(double d); 60 | 61 | /** 62 | * @return the standard deviation for the diffusion of y 63 | */ 64 | double getYStdDev() const; 65 | 66 | /** 67 | * @param d new standard deviation for the diffusion of theta 68 | */ 69 | void setThetaStdDev(double d); 70 | 71 | /** 72 | * @return the standard deviation for the diffusion of theta 73 | */ 74 | double getThetaStdDev() const; 75 | 76 | /** 77 | * @param d new standard deviation for the diffusion of speed 78 | */ 79 | void setSpeedStdDev(double d); 80 | 81 | /** 82 | * @return the standard deviation for the diffusion of speed 83 | */ 84 | double getSpeedStdDev() const; 85 | 86 | /** 87 | * @param d new standard deviation for the diffusion of rotation speed 88 | */ 89 | void setRotationSpeedStdDev(double d); 90 | 91 | /** 92 | * @return the standard deviation for the diffusion of rotation speed 93 | */ 94 | double getRotationSpeedStdDev() const; 95 | 96 | protected: 97 | 98 | private: 99 | 100 | /// Stores the random number generator 101 | libPF::RandomNumberGenerationStrategy* m_RNG; 102 | 103 | /// Store the standard deviations of the model 104 | double m_XStdDev; 105 | double m_YStdDev; 106 | double m_ThetaStdDev; 107 | double m_SpeedStdDev; 108 | double m_RotSpeedStdDev; 109 | 110 | 111 | }; 112 | 113 | #endif 114 | 115 | -------------------------------------------------------------------------------- /sample2/CarObservationModel.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "CarObservationModel.h" 3 | 4 | CarObservationModel::CarObservationModel() : libPF::ObservationModel() { 5 | } 6 | 7 | CarObservationModel::~CarObservationModel() { 8 | } 9 | 10 | double CarObservationModel::measure(const CarState& state) const { 11 | double xdist = (state.getXPos() - m_TrueCarState.getXPos()); 12 | double ydist = (state.getYPos() - m_TrueCarState.getYPos()); 13 | double dist2 = (xdist*xdist + ydist*ydist); 14 | if (dist2 < 0.5) dist2 = 0.5; // avoid division by zero 15 | return 1.0/dist2; 16 | } 17 | 18 | void CarObservationModel::setTrueCarState(const CarState& state) 19 | { 20 | m_TrueCarState = state; 21 | } 22 | -------------------------------------------------------------------------------- /sample2/CarObservationModel.h: -------------------------------------------------------------------------------- 1 | #ifndef CAROBSERVATIONMODEL_H 2 | #define CAROBSERVATIONMODEL_H 3 | 4 | #include 5 | #include 6 | 7 | #include "CarState.h" 8 | 9 | /** 10 | * @class CarObservationModel 11 | * 12 | * @brief Observation model that measures the position of a car. 13 | * 14 | * @author Stephan Wirth 15 | * 16 | * @brief Test class for ParticleFilter. 17 | * 18 | */ 19 | class CarObservationModel : public libPF::ObservationModel { 20 | 21 | public: 22 | 23 | /** 24 | * empty 25 | */ 26 | CarObservationModel(); 27 | 28 | /** 29 | * empty 30 | */ 31 | ~CarObservationModel(); 32 | 33 | /** 34 | * 35 | * @param state Reference to the state that has to be weightened. 36 | * @return weight for the given state. 37 | */ 38 | double measure(const CarState& state) const; 39 | 40 | void setTrueCarState(const CarState& state); 41 | 42 | protected: 43 | 44 | private: 45 | 46 | CarState m_TrueCarState; 47 | 48 | }; 49 | 50 | #endif 51 | -------------------------------------------------------------------------------- /sample2/CarState.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "CarState.h" 4 | 5 | CarState::CarState() : 6 | m_XPos(0.0), 7 | m_YPos(0.0), 8 | m_SinTheta(0.0), 9 | m_CosTheta(1.0), 10 | m_Speed(0.0), 11 | m_RotationSpeed(0.0) 12 | { 13 | } 14 | 15 | CarState::~CarState() 16 | { 17 | } 18 | 19 | CarState CarState::operator*(float factor) const 20 | { 21 | CarState newState; 22 | newState.m_XPos = m_XPos * factor; 23 | newState.m_YPos = m_YPos * factor; 24 | newState.m_Speed = m_Speed * factor; 25 | newState.m_RotationSpeed = m_RotationSpeed * factor; 26 | newState.m_SinTheta = m_SinTheta * factor; 27 | newState.m_CosTheta = m_CosTheta * factor; 28 | return newState; 29 | } 30 | 31 | CarState& CarState::operator+=(const CarState& other) 32 | { 33 | m_XPos += other.m_XPos; 34 | m_YPos += other.m_YPos; 35 | m_Speed += other.m_Speed; 36 | m_SinTheta += other.m_SinTheta; 37 | m_CosTheta += other.m_CosTheta; 38 | m_RotationSpeed += other.m_RotationSpeed; 39 | return *this; 40 | } 41 | 42 | float CarState::getXPos() const 43 | { 44 | return m_XPos; 45 | } 46 | 47 | void CarState::setXPos(float x) 48 | { 49 | m_XPos = x; 50 | } 51 | 52 | float CarState::getYPos() const 53 | { 54 | return m_YPos; 55 | } 56 | 57 | void CarState::setYPos(float y) 58 | { 59 | m_YPos = y; 60 | } 61 | 62 | float CarState::getTheta() const 63 | { 64 | return atan2(m_SinTheta, m_CosTheta); 65 | } 66 | 67 | void CarState::setTheta(float t) 68 | { 69 | m_SinTheta = sin(t); 70 | m_CosTheta = cos(t); 71 | } 72 | 73 | float CarState::getSpeed() const 74 | { 75 | return m_Speed; 76 | } 77 | 78 | void CarState::setSpeed(float s) 79 | { 80 | m_Speed = s; 81 | } 82 | 83 | float CarState::getRotationSpeed() const 84 | { 85 | return m_RotationSpeed; 86 | } 87 | 88 | void CarState::setRotationSpeed(float s) 89 | { 90 | m_RotationSpeed = s; 91 | } 92 | -------------------------------------------------------------------------------- /sample2/CarState.h: -------------------------------------------------------------------------------- 1 | #ifndef CARSTATE_H 2 | #define CARSTATE_H 3 | 4 | 5 | /** 6 | * @class CarState 7 | * @brief Sample state for a particle filter that simulates a car. 8 | * 9 | * This state has the following parameters: 10 | * @li xpos the x-Position of the car 11 | * @li ypos the y-Position of the car 12 | * @li theta the orientation of the car (in radiants) 13 | * @li speed the forward speed of the car 14 | * @li rotationSpeed the speed with which the car rotates. 15 | */ 16 | class CarState 17 | { 18 | public: 19 | CarState(); 20 | ~CarState(); 21 | 22 | CarState operator*(float factor) const; 23 | 24 | CarState& operator+=(const CarState& other); 25 | 26 | 27 | float getXPos() const; 28 | 29 | void setXPos(float x); 30 | 31 | float getYPos() const; 32 | 33 | void setYPos(float y); 34 | 35 | float getTheta() const; 36 | 37 | void setTheta(float t); 38 | 39 | float getSpeed() const; 40 | 41 | void setSpeed(float s); 42 | 43 | float getRotationSpeed() const; 44 | 45 | void setRotationSpeed(float s); 46 | 47 | 48 | private: 49 | 50 | float m_XPos; 51 | float m_YPos; 52 | float m_SinTheta; 53 | float m_CosTheta; 54 | float m_Speed; 55 | float m_RotationSpeed; 56 | }; 57 | 58 | #endif // CARSTATE_H 59 | -------------------------------------------------------------------------------- /sample2/CarStateDistribution.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "CarStateDistribution.h" 5 | 6 | CarStateDistribution::CarStateDistribution(float xmin, float xmax, float ymin, float ymax) : 7 | m_XMin(xmin), 8 | m_XMax(xmax), 9 | m_YMin(ymin), 10 | m_YMax(ymax) 11 | { 12 | m_RNG = new libPF::CRandomNumberGenerator(); 13 | } 14 | 15 | CarStateDistribution::~CarStateDistribution() 16 | { 17 | delete m_RNG; 18 | } 19 | 20 | const CarState CarStateDistribution::draw() const 21 | { 22 | CarState state; 23 | state.setXPos(m_RNG->getUniform(m_XMin, m_XMax)); 24 | state.setYPos(m_RNG->getUniform(m_YMin, m_YMax)); 25 | state.setTheta(m_RNG->getUniform(-M_PI, M_PI)); 26 | return state; 27 | } 28 | -------------------------------------------------------------------------------- /sample2/CarStateDistribution.h: -------------------------------------------------------------------------------- 1 | #ifndef CARSTATEDISTRIBUTION_H 2 | #define CARSTATEDISTRIBUTION_H 3 | 4 | #include 5 | 6 | #include "CarState.h" 7 | 8 | namespace libPF 9 | { 10 | class RandomNumberGenerationStrategy; 11 | } 12 | 13 | class CarStateDistribution : public libPF::StateDistribution 14 | { 15 | public: 16 | CarStateDistribution(float xmin, float xmax, float ymin, float ymax); 17 | ~CarStateDistribution(); 18 | 19 | const CarState draw() const; 20 | 21 | private: 22 | float m_XMin, m_XMax, m_YMin, m_YMax; 23 | 24 | libPF::RandomNumberGenerationStrategy* m_RNG; 25 | }; 26 | 27 | #endif // CARSTATEDISTRIBUTION_H 28 | -------------------------------------------------------------------------------- /sample2/MainWindow.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include "MainWindow.h" 11 | 12 | #include "CarMovementModel.h" 13 | #include "CarObservationModel.h" 14 | #include "CarStateDistribution.h" 15 | 16 | MainWindow::MainWindow() : QMainWindow(), 17 | m_ParticleFilter(250, &m_ObservationModel, &m_MovementModel), 18 | m_RunFilter(true) 19 | { 20 | setupUi(this); 21 | 22 | this->grabKeyboard(); 23 | 24 | m_frontDown = m_backDown = m_rightDown = m_leftDown = 0; 25 | m_RenderWidget->setParticleFilter(&m_ParticleFilter); 26 | 27 | connect(m_ParticleWeightPlotCheckBox, SIGNAL(stateChanged(int)), this, SLOT(changeDisplaySettings())); 28 | connect(m_LoopModeRadioButton, SIGNAL(toggled(bool)), this, SLOT(changeRunMode())); 29 | connect(m_ResampleButton, SIGNAL(clicked()), this, SLOT(resample())); 30 | connect(m_DriftButton, SIGNAL(clicked()), this, SLOT(drift())); 31 | connect(m_DiffuseButton, SIGNAL(clicked()), this, SLOT(diffuse())); 32 | connect(m_MeasureButton, SIGNAL(clicked()), this, SLOT(measure())); 33 | connect(m_RedrawParticlesButton, SIGNAL(clicked()), this, SLOT(redrawParticles())); 34 | connect(m_ResamplingModeComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(setResamplingMode())); 35 | 36 | // read standard values into spin boxes 37 | m_XSpinBox->setValue(m_MovementModel.getXStdDev()); 38 | m_YSpinBox->setValue(m_MovementModel.getYStdDev()); 39 | m_ThetaSpinBox->setValue(m_MovementModel.getThetaStdDev() / M_PI * 180.0); 40 | m_SpeedSpinBox->setValue(m_MovementModel.getSpeedStdDev()); 41 | m_RotationSpeedSpinBox->setValue(m_MovementModel.getRotationSpeedStdDev() / M_PI * 180.0); 42 | 43 | // connect spin boxes 44 | connect(m_XSpinBox, SIGNAL(valueChanged(double)), this, SLOT(setMovementModelParameters())); 45 | connect(m_YSpinBox, SIGNAL(valueChanged(double)), this, SLOT(setMovementModelParameters())); 46 | connect(m_ThetaSpinBox, SIGNAL(valueChanged(double)), this, SLOT(setMovementModelParameters())); 47 | connect(m_SpeedSpinBox, SIGNAL(valueChanged(double)), this, SLOT(setMovementModelParameters())); 48 | connect(m_RotationSpeedSpinBox, SIGNAL(valueChanged(double)), this, SLOT(setMovementModelParameters())); 49 | 50 | QTimer::singleShot(50, this, SLOT(initStates())); 51 | // start the loop 52 | QTimer::singleShot(100, this, SLOT(gameLoop())); 53 | } 54 | 55 | MainWindow::~MainWindow() 56 | { 57 | } 58 | 59 | void MainWindow::initStates() 60 | { 61 | m_TrueCarState.setXPos(m_RenderWidget->width()/2); // x-position 62 | m_TrueCarState.setYPos(m_RenderWidget->height()/2); // y-position 63 | m_TrueCarState.setTheta(0.0); 64 | m_TrueCarState.setSpeed(0.0); 65 | m_TrueCarState.setRotationSpeed(0.0); 66 | m_ParticleFilter.setPriorState(m_TrueCarState); 67 | } 68 | 69 | void MainWindow::simulateMovement() 70 | { 71 | if (m_LoopTimer.isNull()) 72 | { 73 | m_LoopTimer.start(); 74 | return; 75 | } 76 | double dt = m_LoopTimer.restart() / 1000.0; 77 | 78 | double acceleration = m_AccelerationSpinBox->value(); 79 | double maxSpeed = m_MaxSpeedSpinBox->value(); 80 | double newSpeed = m_TrueCarState.getSpeed(); 81 | if (m_frontDown) 82 | newSpeed += acceleration * dt; // accelerate 83 | else if (m_backDown) 84 | newSpeed -= acceleration * dt; // break 85 | else 86 | newSpeed *= 0.99; // rolling 87 | // clamp speed 88 | if (newSpeed > maxSpeed) newSpeed = maxSpeed; 89 | if (newSpeed < -maxSpeed) newSpeed = -maxSpeed; 90 | m_TrueCarState.setSpeed(newSpeed); 91 | 92 | double torque = m_TorqueSpinBox->value() / 180.0 * M_PI; 93 | double maxRotSpeed = m_MaxRotationSpeedSpinBox->value() / 180.0 * M_PI; 94 | double newRotSpeed = m_TrueCarState.getRotationSpeed(); 95 | if (m_leftDown) 96 | newRotSpeed -= torque * dt; 97 | else if (m_rightDown) 98 | newRotSpeed += torque * dt; 99 | else 100 | newRotSpeed *= 0.95; // rolling 101 | // clamp rotation speed 102 | if (newRotSpeed > maxRotSpeed) newRotSpeed = maxRotSpeed; 103 | if (newRotSpeed < -maxRotSpeed) newRotSpeed = -maxRotSpeed; 104 | m_TrueCarState.setRotationSpeed(newRotSpeed); 105 | 106 | // propagate state (we use our own movement model with fixed dt) 107 | m_MovementModel.drift(m_TrueCarState, dt); 108 | m_RenderWidget->setTrueCarState(m_TrueCarState); 109 | } 110 | 111 | void MainWindow::gameLoop() 112 | { 113 | simulateMovement(); 114 | // update the observation model 115 | m_ObservationModel.setTrueCarState(m_TrueCarState); 116 | if (m_RunFilter) 117 | { 118 | m_ParticleFilter.filter(); 119 | } 120 | m_RenderWidget->repaint(); 121 | updateLabels(); 122 | QTimer::singleShot(10, this, SLOT(gameLoop())); 123 | } 124 | 125 | void MainWindow::changeDisplaySettings() 126 | { 127 | if (m_ParticleWeightPlotCheckBox->isChecked()) 128 | { 129 | m_RenderWidget->setDrawParticleWeights(true); 130 | } else 131 | { 132 | m_RenderWidget->setDrawParticleWeights(false); 133 | } 134 | } 135 | 136 | void MainWindow::changeRunMode() 137 | { 138 | if (m_RunFilter == false) 139 | { 140 | m_LoopModeRadioButton->setChecked(true); 141 | m_SingleStepModeGroupBox->setEnabled(false); 142 | m_RunFilter = true; 143 | m_ParticleFilter.resetTimer(); 144 | } else { 145 | m_SingleStepModeRadioButton->setChecked(true); 146 | m_SingleStepModeGroupBox->setEnabled(true); 147 | m_RunFilter = false; 148 | } 149 | } 150 | 151 | void MainWindow::resample() 152 | { 153 | m_ParticleFilter.resample(); 154 | m_RenderWidget->repaint(); 155 | updateLabels(); 156 | } 157 | void MainWindow::drift() 158 | { 159 | float timestep = 1.0/60.0; 160 | m_ParticleFilter.drift(timestep); 161 | m_RenderWidget->repaint(); 162 | updateLabels(); 163 | } 164 | void MainWindow::diffuse() 165 | { 166 | float timestep = 1.0/60.0; 167 | m_ParticleFilter.diffuse(timestep); 168 | m_RenderWidget->repaint(); 169 | updateLabels(); 170 | } 171 | void MainWindow::measure() 172 | { 173 | m_ParticleFilter.measure(); 174 | m_RenderWidget->repaint(); 175 | updateLabels(); 176 | } 177 | 178 | void MainWindow::redrawParticles() 179 | { 180 | float xmin = 0; 181 | float xmax = m_RenderWidget->width(); 182 | float ymin = 0; 183 | float ymax = m_RenderWidget->height(); 184 | CarStateDistribution distribution(xmin, xmax, ymin, ymax); 185 | m_ParticleFilter.drawAllFromDistribution(distribution); 186 | } 187 | 188 | void MainWindow::setResamplingMode() 189 | { 190 | QString text = m_ResamplingModeComboBox->currentText(); 191 | if (text == "Always") 192 | { 193 | m_ParticleFilter.setResamplingMode(libPF::RESAMPLE_ALWAYS); 194 | } else if (text == "NEFF") 195 | { 196 | m_ParticleFilter.setResamplingMode(libPF::RESAMPLE_NEFF); 197 | } else if (text == "Never") 198 | { 199 | m_ParticleFilter.setResamplingMode(libPF::RESAMPLE_NEVER); 200 | } 201 | } 202 | 203 | void MainWindow::setMovementModelParameters() 204 | { 205 | m_MovementModel.setXStdDev(m_XSpinBox->value()); 206 | m_MovementModel.setYStdDev(m_YSpinBox->value()); 207 | m_MovementModel.setThetaStdDev(m_ThetaSpinBox->value() / 180.0 * M_PI); 208 | m_MovementModel.setSpeedStdDev(m_SpeedSpinBox->value()); 209 | m_MovementModel.setRotationSpeedStdDev(m_RotationSpeedSpinBox->value() / 180.0 * M_PI); 210 | } 211 | 212 | void MainWindow::updateLabels() 213 | { 214 | int prec = 4; 215 | m_TrueXLabel->setText(QString::number(m_TrueCarState.getXPos(), 'f', prec)); 216 | m_TrueYLabel->setText(QString::number(m_TrueCarState.getYPos(), 'f', prec)); 217 | m_TrueThetaLabel->setText(QString::number(m_TrueCarState.getTheta() / M_PI * 180.0, 'f', prec)); 218 | m_TrueSpeedLabel->setText(QString::number(m_TrueCarState.getSpeed(), 'f', prec)); 219 | m_TrueRotationSpeedLabel->setText(QString::number(m_TrueCarState.getRotationSpeed() / M_PI * 180.0, 'f', prec)); 220 | 221 | const CarState& estimatedCarState = m_ParticleFilter.getMmseEstimate(); 222 | 223 | m_EstimatedXLabel->setText(QString::number(estimatedCarState.getXPos(), 'f', prec)); 224 | m_EstimatedYLabel->setText(QString::number(estimatedCarState.getYPos(), 'f', prec)); 225 | m_EstimatedThetaLabel->setText(QString::number(estimatedCarState.getTheta() / M_PI * 180.0, 'f', prec)); 226 | m_EstimatedSpeedLabel->setText(QString::number(estimatedCarState.getSpeed(), 'f', prec)); 227 | m_EstimatedRotationSpeedLabel->setText(QString::number(estimatedCarState.getRotationSpeed() / M_PI * 180.0, 'f', prec)); 228 | 229 | } 230 | 231 | //----------------------------------------------------------------------------- 232 | void MainWindow::keyPressEvent(QKeyEvent* e) 233 | { 234 | if ((e->key() == Qt::Key_W) || (e->key()==Qt::Key_Up)) 235 | m_frontDown=true; 236 | else if ((e->key() == Qt::Key_S) || (e->key()==Qt::Key_Down)) 237 | m_backDown=true; 238 | else if ((e->key() == Qt::Key_A) || (e->key()==Qt::Key_Left)) 239 | m_leftDown=true; 240 | else if ((e->key() == Qt::Key_D) || (e->key()==Qt::Key_Right)) 241 | m_rightDown=true; 242 | else if (e->key() == Qt::Key_R) 243 | { 244 | initStates(); 245 | } 246 | else if (e->key() == Qt::Key_1) 247 | resample(); 248 | else if (e->key() == Qt::Key_2) 249 | drift(); 250 | else if (e->key() == Qt::Key_3) 251 | diffuse(); 252 | else if (e->key() == Qt::Key_4) 253 | measure(); 254 | else if (e->key() == Qt::Key_Tab) 255 | changeRunMode(); 256 | else 257 | e->ignore(); 258 | } 259 | //----------------------------------------------------------------------------- 260 | void MainWindow::keyReleaseEvent(QKeyEvent* e) 261 | { 262 | if ((e->key() == Qt::Key_W) || (e->key()==Qt::Key_Up)) 263 | m_frontDown=false; 264 | else if ((e->key() == Qt::Key_S) || (e->key()==Qt::Key_Down)) 265 | m_backDown=false; 266 | else if ((e->key() == Qt::Key_A) || (e->key()==Qt::Key_Left)) 267 | m_leftDown=false; 268 | else if ((e->key() == Qt::Key_D) || (e->key()==Qt::Key_Right)) 269 | m_rightDown=false; 270 | } 271 | //----------------------------------------------------------------------------- 272 | -------------------------------------------------------------------------------- /sample2/MainWindow.h: -------------------------------------------------------------------------------- 1 | #ifndef MAINWINDOW_H 2 | #define MAINWINDOW_H 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include "CarObservationModel.h" 9 | #include "CarMovementModel.h" 10 | 11 | #include "ui_MainWindowForm.h" 12 | #include "CarState.h" 13 | 14 | 15 | 16 | class MainWindow : public QMainWindow, private Ui::MainWindowForm 17 | { 18 | Q_OBJECT 19 | 20 | public: 21 | MainWindow(); 22 | ~MainWindow(); 23 | 24 | public slots: 25 | 26 | void gameLoop(); 27 | 28 | protected: 29 | void keyPressEvent(QKeyEvent* e); 30 | void keyReleaseEvent(QKeyEvent* e); 31 | void simulateMovement(); 32 | 33 | protected slots: 34 | void initStates(); 35 | 36 | void changeDisplaySettings(); 37 | void changeRunMode(); 38 | 39 | void resample(); 40 | void drift(); 41 | void diffuse(); 42 | void measure(); 43 | 44 | void redrawParticles(); 45 | void setResamplingMode(); 46 | 47 | void setMovementModelParameters(); 48 | 49 | void updateLabels(); 50 | 51 | private: 52 | void switchToSingleStepMode(); 53 | 54 | CarMovementModel m_MovementModel; 55 | CarObservationModel m_ObservationModel; 56 | libPF::ParticleFilter m_ParticleFilter; 57 | 58 | 59 | CarState m_TrueCarState; 60 | 61 | bool m_leftDown; 62 | bool m_rightDown; 63 | bool m_frontDown; 64 | bool m_backDown; 65 | 66 | bool m_RunFilter; 67 | 68 | QTime m_LoopTimer; 69 | 70 | }; 71 | 72 | #endif // MAINWINDOW_H 73 | -------------------------------------------------------------------------------- /sample2/MainWindowForm.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | MainWindowForm 4 | 5 | 6 | 7 | 0 8 | 0 9 | 1053 10 | 738 11 | 12 | 13 | 14 | Particle Filter Example 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 0 25 | 0 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | Qt::Horizontal 36 | 37 | 38 | 39 | 40 40 | 20 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | Car Properties 49 | 50 | 51 | 52 | 53 | 54 | Acceleration: 55 | 56 | 57 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 58 | 59 | 60 | 61 | 62 | 63 | 64 | 999.990000000000009 65 | 66 | 67 | 100.000000000000000 68 | 69 | 70 | 71 | 72 | 73 | 74 | Max. Speed: 75 | 76 | 77 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 78 | 79 | 80 | 81 | 82 | 83 | 84 | 999.990000000000009 85 | 86 | 87 | 200.000000000000000 88 | 89 | 90 | 91 | 92 | 93 | 94 | Torque: 95 | 96 | 97 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 98 | 99 | 100 | 101 | 102 | 103 | 104 | 999.990000000000009 105 | 106 | 107 | 120.000000000000000 108 | 109 | 110 | 111 | 112 | 113 | 114 | Max. Rotation Speed: 115 | 116 | 117 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 118 | 119 | 120 | 121 | 122 | 123 | 124 | 999.990000000000009 125 | 126 | 127 | 180.000000000000000 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | True Car State 138 | 139 | 140 | 141 | 142 | 143 | X: 144 | 145 | 146 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 80 155 | 0 156 | 157 | 158 | 159 | 000.000 160 | 161 | 162 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 163 | 164 | 165 | 166 | 167 | 168 | 169 | Y: 170 | 171 | 172 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 80 181 | 0 182 | 183 | 184 | 185 | 000.000 186 | 187 | 188 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 189 | 190 | 191 | 192 | 193 | 194 | 195 | Theta: 196 | 197 | 198 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 80 207 | 0 208 | 209 | 210 | 211 | 000.000 212 | 213 | 214 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 215 | 216 | 217 | 218 | 219 | 220 | 221 | Speed: 222 | 223 | 224 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 80 233 | 0 234 | 235 | 236 | 237 | 000.000 238 | 239 | 240 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 241 | 242 | 243 | 244 | 245 | 246 | 247 | Rotation Speed: 248 | 249 | 250 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 80 259 | 0 260 | 261 | 262 | 263 | 000.000 264 | 265 | 266 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | Estimated Car State 277 | 278 | 279 | 280 | 281 | 282 | X: 283 | 284 | 285 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 80 294 | 0 295 | 296 | 297 | 298 | 000.000 299 | 300 | 301 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 302 | 303 | 304 | 305 | 306 | 307 | 308 | Y: 309 | 310 | 311 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 312 | 313 | 314 | 315 | 316 | 317 | 318 | 319 | 80 320 | 0 321 | 322 | 323 | 324 | 000.000 325 | 326 | 327 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 328 | 329 | 330 | 331 | 332 | 333 | 334 | Theta: 335 | 336 | 337 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 338 | 339 | 340 | 341 | 342 | 343 | 344 | 345 | 80 346 | 0 347 | 348 | 349 | 350 | 000.000 351 | 352 | 353 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 354 | 355 | 356 | 357 | 358 | 359 | 360 | Speed: 361 | 362 | 363 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 364 | 365 | 366 | 367 | 368 | 369 | 370 | 371 | 80 372 | 0 373 | 374 | 375 | 376 | 000.000 377 | 378 | 379 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 380 | 381 | 382 | 383 | 384 | 385 | 386 | Rotation Speed: 387 | 388 | 389 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 390 | 391 | 392 | 393 | 394 | 395 | 396 | 397 | 80 398 | 0 399 | 400 | 401 | 402 | 000.000 403 | 404 | 405 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 406 | 407 | 408 | 409 | 410 | 411 | 412 | 413 | 414 | 415 | 416 | 417 | 418 | 419 | 420 | 421 | Filter Control 422 | 423 | 424 | 425 | 426 | 427 | Loop Mode 428 | 429 | 430 | true 431 | 432 | 433 | 434 | 435 | 436 | 437 | Single Step Mode 438 | 439 | 440 | 441 | 442 | 443 | 444 | false 445 | 446 | 447 | false 448 | 449 | 450 | 451 | 452 | 453 | Resample (1) 454 | 455 | 456 | 457 | 458 | 459 | 460 | Drift (2) 461 | 462 | 463 | 464 | 465 | 466 | 467 | Diffuse (3) 468 | 469 | 470 | 471 | 472 | 473 | 474 | Measure (4) 475 | 476 | 477 | 478 | 479 | 480 | 481 | 482 | 483 | 484 | Resampling Mode 485 | 486 | 487 | 488 | 489 | 490 | 491 | NEFF 492 | 493 | 494 | 495 | 496 | Always 497 | 498 | 499 | 500 | 501 | Never 502 | 503 | 504 | 505 | 506 | 507 | 508 | 509 | 510 | 511 | 512 | Redraws the particles from a random distribution 513 | 514 | 515 | Redraw particles 516 | 517 | 518 | 519 | 520 | 521 | 522 | 523 | 524 | 525 | Movement Model: Standard deviations 526 | 527 | 528 | 529 | 530 | 531 | X: 532 | 533 | 534 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 535 | 536 | 537 | 538 | 539 | 540 | 541 | 3 542 | 543 | 544 | 100000.000000000000000 545 | 546 | 547 | 10.000000000000000 548 | 549 | 550 | 551 | 552 | 553 | 554 | Y: 555 | 556 | 557 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 558 | 559 | 560 | 561 | 562 | 563 | 564 | 3 565 | 566 | 567 | 100000.000000000000000 568 | 569 | 570 | 10.000000000000000 571 | 572 | 573 | 574 | 575 | 576 | 577 | Theta: 578 | 579 | 580 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 581 | 582 | 583 | 584 | 585 | 586 | 587 | 3 588 | 589 | 590 | 100000.000000000000000 591 | 592 | 593 | 10.000000000000000 594 | 595 | 596 | 597 | 598 | 599 | 600 | Speed: 601 | 602 | 603 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 604 | 605 | 606 | 607 | 608 | 609 | 610 | 3 611 | 612 | 613 | 100000.000000000000000 614 | 615 | 616 | 10.000000000000000 617 | 618 | 619 | 620 | 621 | 622 | 623 | Rotation Speed: 624 | 625 | 626 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 627 | 628 | 629 | 630 | 631 | 632 | 633 | 3 634 | 635 | 636 | 100000.000000000000000 637 | 638 | 639 | 10.000000000000000 640 | 641 | 642 | 643 | 644 | 645 | 646 | 647 | 648 | 649 | Display Control 650 | 651 | 652 | 653 | 654 | 655 | Particle Weight Plot 656 | 657 | 658 | true 659 | 660 | 661 | 662 | 663 | 664 | 665 | 666 | 667 | 668 | Qt::Vertical 669 | 670 | 671 | 672 | 20 673 | 18 674 | 675 | 676 | 677 | 678 | 679 | 680 | 681 | 682 | 683 | 684 | 685 | 0 686 | 0 687 | 1053 688 | 22 689 | 690 | 691 | 692 | 693 | 694 | 695 | 696 | RenderWidget 697 | QWidget 698 |
RenderWidget.h
699 | 1 700 | 701 | 702 | 703 | 704 | 705 | -------------------------------------------------------------------------------- /sample2/RenderWidget.cpp: -------------------------------------------------------------------------------- 1 | #include "RenderWidget.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include "CarMovementModel.h" 11 | #include "CarObservationModel.h" 12 | 13 | 14 | using namespace libPF; 15 | 16 | //----------------------------------------------------------------------------- 17 | RenderWidget::RenderWidget(QWidget *parent /*= 0*/) : QGLWidget(parent), 18 | m_ParticleFilter(NULL), 19 | m_DrawParticleWeights(true) 20 | { 21 | m_LastDrawTime = clock(); 22 | setMouseTracking(true); 23 | } 24 | //----------------------------------------------------------------------------- 25 | RenderWidget::~RenderWidget() 26 | { 27 | } 28 | 29 | void RenderWidget::setParticleFilter(ParticleFilter* filter) 30 | { 31 | m_ParticleFilter = filter; 32 | } 33 | 34 | void RenderWidget::setTrueCarState(CarState& state) 35 | { 36 | m_TrueCarState = state; 37 | } 38 | 39 | void RenderWidget::setDrawParticleWeights(bool b) 40 | { 41 | m_DrawParticleWeights = b; 42 | } 43 | 44 | //----------------------------------------------------------------------------- 45 | void RenderWidget::paintEvent(QPaintEvent* /*event*/) 46 | { 47 | QPainter painter; 48 | painter.begin(this); 49 | painter.setPen(Qt::black); 50 | painter.setBrush(Qt::black); 51 | painter.drawRect(QRect(0, 0, width() - 1, height() - 1)); 52 | 53 | QColor color; 54 | unsigned int nearestToMouseIndex = m_ParticleFilter->numParticles(); 55 | double minMouseDistance = width() + height(); // some high value 56 | for (unsigned int i = 0; i < m_ParticleFilter->numParticles(); ++i) 57 | { 58 | const CarState& state = m_ParticleFilter->getState(i); 59 | double weight = m_ParticleFilter->getWeight(i); 60 | float hue = pow(weight, 0.2); 61 | float value = pow(weight, 0.05); 62 | color.setHsvF(hue, 1.0, value); 63 | painter.setPen(color); 64 | drawCarState(painter, state); 65 | double xDist = pow(state.getXPos() - m_MousePosition.x(), 2); 66 | double yDist = pow(state.getYPos() - m_MousePosition.y(), 2); 67 | double distanceToMouse = xDist + yDist; 68 | if (distanceToMouse < minMouseDistance) 69 | { 70 | minMouseDistance = distanceToMouse; 71 | nearestToMouseIndex = i; 72 | } 73 | } 74 | if (nearestToMouseIndex < m_ParticleFilter->numParticles()) 75 | { 76 | double x = m_ParticleFilter->getState(nearestToMouseIndex).getXPos(); 77 | double y = m_ParticleFilter->getState(nearestToMouseIndex).getYPos(); 78 | double weight = m_ParticleFilter->getWeight(nearestToMouseIndex); 79 | painter.setPen(Qt::white); 80 | painter.drawText(m_MousePosition.x(), m_MousePosition.y(), QString::number(weight, 'f')); 81 | painter.drawLine(x, y, m_MousePosition.x(), m_MousePosition.y()); 82 | } 83 | 84 | painter.setPen(Qt::red); 85 | drawCarState(painter, m_ParticleFilter->getBestState()); 86 | 87 | painter.setPen(Qt::yellow); 88 | drawCarState(painter, m_ParticleFilter->getBestXPercentEstimate(20.0)); 89 | 90 | painter.setPen(Qt::magenta); 91 | drawCarState(painter, m_ParticleFilter->getMmseEstimate()); 92 | 93 | painter.setPen(Qt::white); 94 | drawCarState(painter, m_TrueCarState); 95 | 96 | if (m_DrawParticleWeights) 97 | { 98 | painter.setPen(Qt::green); 99 | // draw particle weights 100 | unsigned int num = m_ParticleFilter->numParticles(); 101 | float ystart = height() - 1; 102 | for (unsigned int i = 0; i < num - 1; ++i) 103 | { 104 | float x1 = 1.0 * width() / num * i; 105 | float x2 = 1.0 * width() / num * (i+1); 106 | float scaledWeight1 = 1.0 / pow(2, -log(m_ParticleFilter->getWeight(i))); // log scale 107 | float scaledWeight2 = 1.0 / pow(2, -log(m_ParticleFilter->getWeight(i+1))); 108 | float y1 = ystart - scaledWeight1 * ystart; 109 | float y2 = ystart - scaledWeight2 * ystart; 110 | painter.drawLine(QPointF(x1, y1), QPointF(x2, y2)); 111 | } 112 | } 113 | 114 | 115 | clock_t currentTime = clock(); 116 | double dt = ((double)currentTime - (double)m_LastDrawTime) / CLOCKS_PER_SEC; 117 | m_LastDrawTime = currentTime; 118 | float thisFps = dt; 119 | float alpha = 0.99; 120 | m_Fps = alpha * m_Fps + (1 - alpha) * thisFps; 121 | painter.drawText(10, 20, QString::number(1.0/m_Fps)); 122 | painter.end(); 123 | 124 | 125 | } 126 | //----------------------------------------------------------------------------- 127 | 128 | void RenderWidget::mouseMoveEvent(QMouseEvent* event) 129 | { 130 | m_MousePosition = event->pos(); 131 | } 132 | 133 | //----------------------------------------------------------------------------- 134 | 135 | void RenderWidget::drawCarState(QPainter& painter, const CarState& state) 136 | { 137 | float x = state.getXPos(); 138 | float y = state.getYPos(); 139 | QRectF rect(x-4, y-4, 8, 8); 140 | painter.drawEllipse(rect); 141 | float xoff = 8.0 * cos(state.getTheta()); 142 | float yoff = 8.0 * sin(state.getTheta()); 143 | QLineF line(x, y, x+xoff, y+yoff); 144 | painter.drawLine(line); 145 | } 146 | -------------------------------------------------------------------------------- /sample2/RenderWidget.h: -------------------------------------------------------------------------------- 1 | #ifndef RENDERWIDGET_H 2 | #define RENDERWIDGET_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include "CarMovementModel.h" 11 | #include "CarObservationModel.h" 12 | #include "CarState.h" 13 | 14 | class QTimer; 15 | 16 | class RenderWidget : public QGLWidget 17 | { 18 | Q_OBJECT 19 | public: 20 | RenderWidget(QWidget *parent = 0); 21 | ~RenderWidget(); 22 | 23 | void setParticleFilter(libPF::ParticleFilter* pf); 24 | void setTrueCarState(CarState& state); 25 | 26 | void setDrawParticleWeights(bool b); 27 | protected: 28 | void paintEvent(QPaintEvent *event); 29 | 30 | void mouseMoveEvent(QMouseEvent* event); 31 | 32 | private: 33 | void drawCarState(QPainter& painter, const CarState& state); 34 | 35 | private: 36 | libPF::ParticleFilter* m_ParticleFilter; 37 | CarState m_TrueCarState; 38 | 39 | bool m_DrawParticleWeights; 40 | 41 | float m_Fps; 42 | 43 | clock_t m_LastDrawTime; 44 | 45 | QPoint m_MousePosition; 46 | }; 47 | 48 | #endif 49 | -------------------------------------------------------------------------------- /sample2/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "MainWindow.h" 3 | 4 | 5 | int main(int argc, char *argv[]) 6 | { 7 | QApplication app( argc, argv ); 8 | MainWindow mainWindow; 9 | mainWindow.show(); 10 | return app.exec(); 11 | } 12 | 13 | --------------------------------------------------------------------------------