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