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