Point Cloud Library (PCL)  1.7.1
kld_adaptive_particle_filter.h
1 #ifndef PCL_TRACKING_KLD_ADAPTIVE_PARTICLE_FILTER_H_
2 #define PCL_TRACKING_KLD_ADAPTIVE_PARTICLE_FILTER_H_
3 
4 #include <pcl/tracking/tracking.h>
5 #include <pcl/tracking/particle_filter.h>
6 #include <pcl/tracking/coherence.h>
7 
8 namespace pcl
9 {
10  namespace tracking
11  {
12 
13  /** \brief @b KLDAdaptiveParticleFilterTracker tracks the PointCloud which is given by
14  setReferenceCloud within the measured PointCloud using particle filter method.
15  The number of the particles changes adaptively based on KLD sampling [D. Fox, NIPS-01], [D.Fox, IJRR03].
16  * \author Ryohei Ueda
17  * \ingroup tracking
18  */
19  template <typename PointInT, typename StateT>
21  {
22  public:
44 
46 
50 
54 
56  typedef boost::shared_ptr< Coherence > CoherencePtr;
57  typedef boost::shared_ptr< const Coherence > CoherenceConstPtr;
58 
60  typedef boost::shared_ptr< CloudCoherence > CloudCoherencePtr;
61  typedef boost::shared_ptr< const CloudCoherence > CloudCoherenceConstPtr;
62 
63  /** \brief Empty constructor. */
65  : ParticleFilterTracker<PointInT, StateT> ()
67  , epsilon_ (0)
68  , delta_ (0.99)
69  , bin_size_ ()
70  {
71  tracker_name_ = "KLDAdaptiveParticleFilterTracker";
72  }
73 
74  /** \brief set the bin size.
75  * \param bin_size the size of a bin
76  */
77  inline void setBinSize (const StateT& bin_size) { bin_size_ = bin_size; }
78 
79  /** \brief get the bin size. */
80  inline StateT getBinSize () const { return (bin_size_); }
81 
82  /** \brief set the maximum number of the particles.
83  * \param nr the maximum number of the particles.
84  */
85  inline void setMaximumParticleNum (unsigned int nr) { maximum_particle_number_ = nr; }
86 
87  /** \brief get the maximum number of the particles.*/
88  inline unsigned int getMaximumParticleNum () const { return (maximum_particle_number_); }
89 
90  /** \brief set epsilon to be used to calc K-L boundary.
91  * \param eps epsilon
92  */
93  inline void setEpsilon (double eps) { epsilon_ = eps; }
94 
95  /** \brief get epsilon to be used to calc K-L boundary. */
96  inline double getEpsilon () const { return (epsilon_); }
97 
98  /** \brief set delta to be used in chi-squared distribution.
99  * \param delta delta of chi-squared distribution.
100  */
101  inline void setDelta (double delta) { delta_ = delta; }
102 
103  /** \brief get delta to be used in chi-squared distribution.*/
104  inline double getDelta () const { return (delta_); }
105 
106  protected:
107 
108  /** \brief return true if the two bins are equal.
109  * \param a index of the bin
110  * \param b index of the bin
111  */
112  virtual bool
113  equalBin (std::vector<int> a, std::vector<int> b)
114  {
115  int dimension = StateT::stateDimension ();
116  for (int i = 0; i < dimension; i++)
117  if (a[i] != b[i])
118  return (false);
119  return (true);
120  }
121 
122  /** \brief return upper quantile of standard normal distribution.
123  * \param[in] u ratio of quantile.
124  */
125  double
126  normalQuantile (double u)
127  {
128  const double a[9] = { 1.24818987e-4, -1.075204047e-3, 5.198775019e-3,
129  -0.019198292004, 0.059054035642,-0.151968751364,
130  0.319152932694,-0.5319230073, 0.797884560593};
131  const double b[15] = { -4.5255659e-5, 1.5252929e-4, -1.9538132e-5,
132  -6.76904986e-4, 1.390604284e-3,-7.9462082e-4,
133  -2.034254874e-3, 6.549791214e-3,-0.010557625006,
134  0.011630447319,-9.279453341e-3, 5.353579108e-3,
135  -2.141268741e-3, 5.35310549e-4, 0.999936657524};
136  double w, y, z;
137  int i;
138 
139  if (u == 0.)
140  return (0.5);
141  y = u / 2.0;
142  if (y < -6.)
143  return (0.0);
144  if (y > 6.)
145  return (1.0);
146  if (y < 0.0)
147  y = - y;
148  if (y < 1.0)
149  {
150  w = y * y;
151  z = a[0];
152  for (i = 1; i < 9; i++)
153  z = z * w + a[i];
154  z *= (y * 2.0);
155  }
156  else
157  {
158  y -= 2.0;
159  z = b[0];
160  for (i = 1; i < 15; i++)
161  z = z * y + b[i];
162  }
163 
164  if (u < 0.0)
165  return ((1. - z) / 2.0);
166  return ((1. + z) / 2.0);
167  }
168 
169  /** \brief calculate K-L boundary. K-L boundary follows 1/2e*chi(k-1, 1-d)^2.
170  * \param[in] k the number of bins and the first parameter of chi distribution.
171  */
172  virtual
173  double calcKLBound (int k)
174  {
175  double z = normalQuantile (delta_);
176  double chi = 1.0 - 2.0 / (9.0 * (k - 1)) + sqrt (2.0 / (9.0 * (k - 1))) * z;
177  return ((k - 1.0) / (2.0 * epsilon_) * chi * chi * chi);
178  }
179 
180  /** \brief insert a bin into the set of the bins. if that bin is already registered,
181  return false. if not, return true.
182  * \param bin a bin to be inserted.
183  * \param B a set of the bins
184  */
185  virtual bool
186  insertIntoBins (std::vector<int> bin, std::vector<std::vector<int> > &B);
187 
188  /** \brief This method should get called before starting the actual computation. */
189  virtual bool
190  initCompute ();
191 
192  /** \brief resampling phase of particle filter method.
193  sampling the particles according to the weights calculated in weight method.
194  in particular, "sample with replacement" is archieved by walker's alias method.
195  */
196  virtual void
197  resample ();
198 
199  /** \brief the maximum number of the particles. */
201 
202  /** \brief error between K-L distance and MLE*/
203  double epsilon_;
204 
205  /** \brief probability of distance between K-L distance and MLE is less than epsilon_*/
206  double delta_;
207 
208  /** \brief the size of a bin.*/
209  StateT bin_size_;
210  };
211  }
212 }
213 
214 #ifdef PCL_NO_PRECOMPILE
215 #include <pcl/tracking/impl/kld_adaptive_particle_filter.hpp>
216 #endif
217 
218 #endif