ParticleFilter.h
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 #ifndef PARTICLEFILTER_H_
00029 #define PARTICLEFILTER_H_
00030 
00031 #include <rtabmap/utilite/UMath.h>
00032 #include <rtabmap/utilite/ULogger.h>
00033 
00034 
00035 namespace rtabmap {
00036 
00037 // taken from http://www.developpez.net/forums/d544518/c-cpp/c/equivalent-randn-matlab-c/
00038 #define TWOPI (6.2831853071795864769252867665590057683943387987502) /* 2 * pi */
00039 
00040 /*
00041    RAND is a macro which returns a pseudo-random numbers from a uniform
00042    distribution on the interval [0 1]
00043 */
00044 #define RAND (rand())/((double) RAND_MAX)
00045 
00046 /*
00047    RANDN is a macro which returns a pseudo-random numbers from a normal
00048    distribution with mean zero and standard deviation one. This macro uses Box
00049    Muller's algorithm
00050 */
00051 #define RANDN (sqrt(-2.0*log(RAND))*cos(TWOPI*RAND))
00052 
00053 std::vector<double> cumSum(const std::vector<double> & v)
00054 {
00055         std::vector<double> cum(v.size());
00056         double sum = 0;
00057         for(unsigned int i=0; i<v.size(); ++i)
00058         {
00059                 cum[i] = v[i] + sum;
00060                 sum += v[i];
00061         }
00062         return cum;
00063 }
00064 
00065 std::vector<double> resample(const std::vector<double> & p, // particles
00066                                                          const std::vector<double> & w, // weights
00067                                                          bool normalizeWeights = false)
00068 {
00069         std::vector<double> np; //new particles
00070         if(p.size() != w.size() || p.size() == 0)
00071         {
00072                 UERROR("particles (%d) and weights (%d) are not the same size", p.size(), w.size());
00073                 return np;
00074         }
00075 
00076         std::vector<double> cs;
00077         if(normalizeWeights)
00078         {
00079                 double wSum = uSum(w);
00080                 std::vector<double> wNorm(w.size());
00081                 for(unsigned int i=0; i<w.size(); ++i)
00082                 {
00083                         wNorm[i] = w[i]/wSum;
00084                 }
00085                 cs = cumSum(wNorm); // cumulative sum
00086         }
00087         else
00088         {
00089                 cs = cumSum(w); // cumulative sum
00090         }
00091         for(unsigned int j=0; j<cs.size(); ++j)
00092         {
00093                 cs[j]/=cs.back();
00094         }
00095 
00096         np.resize(p.size());
00097         for(unsigned int i=0; i<np.size(); ++i)
00098         {
00099                 unsigned int index = 0;
00100                 double randnum = RAND;
00101                 for(unsigned int j=0; j<cs.size(); ++j)
00102                 {
00103                         if(randnum < cs[j])
00104                         {
00105                                 index = j;
00106                                 break;
00107                         }
00108                 }
00109                 np[i] = p[index];
00110         }
00111         return np;
00112 }
00113 
00114 
00115 class ParticleFilter
00116 {
00117 public:
00118         ParticleFilter(unsigned int nParticles = 200,
00119                                    double noise = 0.1,
00120                                    double lambda = 10.0,
00121                                    double initValue = 0.0) :
00122                 noise_(noise),
00123                 lambda_(lambda)
00124         {
00125                 particles_.resize(nParticles, initValue);
00126         }
00127 
00128         void init(double initValue = 0.0f)
00129         {
00130                 particles_ = std::vector<double>(particles_.size(), initValue);
00131         }
00132 
00133         double filter(double val)
00134         {
00135                 std::vector<double> weights(particles_.size(), 1);
00136                 double sumWeights = 0;
00137                 for(unsigned int i=0; i<particles_.size(); ++i)
00138                 {
00139                         // add noise to particle
00140                         particles_[i] += noise_ * RANDN;
00141 
00142                         // compute weight
00143                         double dist = fabs(particles_[i] - val);
00144                         //dist = sqrt(dist*dist);
00145                         double w = exp(-lambda_*dist);
00146                         if(uIsFinite(w) && w > 0)
00147                         {
00148                                 weights[i] = w;
00149                         }
00150                         sumWeights += weights[i];
00151                 }
00152 
00153 
00154                 //normalize and compute estimated value
00155                 double value =0.0;
00156                 for(unsigned int i=0; i<weights.size(); ++i)
00157                 {
00158                         weights[i] /= sumWeights;
00159                         value += weights[i] * particles_[i];
00160                 }
00161 
00162                 //resample the particles
00163                 particles_ = resample(particles_, weights, false);
00164 
00165                 return value;
00166         }
00167 
00168 private:
00169         std::vector<double> particles_;
00170         double noise_;
00171         double lambda_;
00172 };
00173 
00174 }
00175 
00176 
00177 #endif /* PARTICLEFILTER_H_ */


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jun 6 2019 21:59:21