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
pcl::tracking::KLDAdaptiveParticleFilterTracker::ConstPtr
shared_ptr< const KLDAdaptiveParticleFilterTracker< PointInT, StateT > > ConstPtr
Definition: kld_adaptive_particle_filter.h:47
pcl::tracking::KLDAdaptiveParticleFilterTracker::CoherenceConstPtr
typename Coherence::ConstPtr CoherenceConstPtr
Definition: kld_adaptive_particle_filter.h:59
pcl
Definition: convolution.h:46
pcl::tracking::ParticleFilterTracker
ParticleFilterTracker tracks the PointCloud which is given by setReferenceCloud within the measured P...
Definition: particle_filter.h:24
pcl::tracking::KLDAdaptiveParticleFilterTracker::epsilon_
double epsilon_
error between K-L distance and MLE
Definition: kld_adaptive_particle_filter.h:204
pcl::tracking::KLDAdaptiveParticleFilterTracker::equalBin
virtual bool equalBin(const std::vector< int > &a, const std::vector< int > &b)
return true if the two bins are equal.
Definition: kld_adaptive_particle_filter.h:115
pcl::tracking::PointCloudCoherence::ConstPtr
shared_ptr< const PointCloudCoherence< PointInT > > ConstPtr
Definition: coherence.h:63
pcl::tracking::KLDAdaptiveParticleFilterTracker::PointCloudInPtr
typename PointCloudIn::Ptr PointCloudInPtr
Definition: kld_adaptive_particle_filter.h:50
pcl::tracking::KLDAdaptiveParticleFilterTracker::PointCloudIn
typename Tracker< PointInT, StateT >::PointCloudIn PointCloudIn
Definition: kld_adaptive_particle_filter.h:49
pcl::tracking::PointCoherence::Ptr
shared_ptr< PointCoherence< PointInT > > Ptr
Definition: coherence.h:19
pcl::tracking::KLDAdaptiveParticleFilterTracker::setDelta
void setDelta(double delta)
set delta to be used in chi-squared distribution.
Definition: kld_adaptive_particle_filter.h:103
pcl::tracking::KLDAdaptiveParticleFilterTracker::CloudCoherenceConstPtr
typename CloudCoherence::ConstPtr CloudCoherenceConstPtr
Definition: kld_adaptive_particle_filter.h:63
pcl::tracking::KLDAdaptiveParticleFilterTracker::CloudCoherencePtr
typename CloudCoherence::Ptr CloudCoherencePtr
Definition: kld_adaptive_particle_filter.h:62
pcl::tracking::KLDAdaptiveParticleFilterTracker::bin_size_
StateT bin_size_
the size of a bin.
Definition: kld_adaptive_particle_filter.h:210
pcl::PointCloud< PointInT >
pcl::tracking::KLDAdaptiveParticleFilterTracker::PointCloudStatePtr
typename PointCloudState::Ptr PointCloudStatePtr
Definition: kld_adaptive_particle_filter.h:54
pcl::tracking::KLDAdaptiveParticleFilterTracker::initCompute
bool initCompute() override
This method should get called before starting the actual computation.
Definition: kld_adaptive_particle_filter.hpp:7
pcl::tracking::Tracker::tracker_name_
std::string tracker_name_
The tracker name.
Definition: tracker.h:93
pcl::tracking::KLDAdaptiveParticleFilterTracker::getBinSize
StateT getBinSize() const
get the bin size.
Definition: kld_adaptive_particle_filter.h:82
pcl::tracking::KLDAdaptiveParticleFilterTracker::calcKLBound
virtual double calcKLBound(int k)
calculate K-L boundary.
Definition: kld_adaptive_particle_filter.h:174
pcl::tracking::KLDAdaptiveParticleFilterTracker::CoherencePtr
typename Coherence::Ptr CoherencePtr
Definition: kld_adaptive_particle_filter.h:58
pcl::tracking::KLDAdaptiveParticleFilterTracker::PointCloudState
typename Tracker< PointInT, StateT >::PointCloudState PointCloudState
Definition: kld_adaptive_particle_filter.h:53
pcl::tracking::KLDAdaptiveParticleFilterTracker::getMaximumParticleNum
unsigned int getMaximumParticleNum() const
get the maximum number of the particles.
Definition: kld_adaptive_particle_filter.h:90
pcl::tracking::KLDAdaptiveParticleFilterTracker::normalQuantile
double normalQuantile(double u)
return upper quantile of standard normal distribution.
Definition: kld_adaptive_particle_filter.h:128
pcl::tracking::PointCoherence::ConstPtr
shared_ptr< const PointCoherence< PointInT > > ConstPtr
Definition: coherence.h:20
pcl::tracking::KLDAdaptiveParticleFilterTracker::KLDAdaptiveParticleFilterTracker
KLDAdaptiveParticleFilterTracker()
Empty constructor.
Definition: kld_adaptive_particle_filter.h:66
pcl::tracking::KLDAdaptiveParticleFilterTracker::setMaximumParticleNum
void setMaximumParticleNum(unsigned int nr)
set the maximum number of the particles.
Definition: kld_adaptive_particle_filter.h:87
pcl::tracking::KLDAdaptiveParticleFilterTracker::PointCloudInConstPtr
typename PointCloudIn::ConstPtr PointCloudInConstPtr
Definition: kld_adaptive_particle_filter.h:51
pcl::tracking::KLDAdaptiveParticleFilterTracker::delta_
double delta_
probability of distance between K-L distance and MLE is less than epsilon_
Definition: kld_adaptive_particle_filter.h:207
pcl::tracking::PointCloudCoherence
PointCloudCoherence is a base class to compute coherence between the two PointClouds.
Definition: coherence.h:60
pcl::tracking::KLDAdaptiveParticleFilterTracker::Ptr
shared_ptr< KLDAdaptiveParticleFilterTracker< PointInT, StateT > > Ptr
Definition: kld_adaptive_particle_filter.h:46
pcl::tracking::PointCoherence
PointCoherence is a base class to compute coherence between the two points.
Definition: coherence.h:17
pcl::tracking::KLDAdaptiveParticleFilterTracker::resample
void resample() override
resampling phase of particle filter method.
Definition: kld_adaptive_particle_filter.hpp:49
pcl::tracking::KLDAdaptiveParticleFilterTracker::getDelta
double getDelta() const
get delta to be used in chi-squared distribution.
Definition: kld_adaptive_particle_filter.h:106
pcl::tracking::PointCloudCoherence::Ptr
shared_ptr< PointCloudCoherence< PointInT > > Ptr
Definition: coherence.h:62
pcl::tracking::KLDAdaptiveParticleFilterTracker::maximum_particle_number_
unsigned int maximum_particle_number_
the maximum number of the particles.
Definition: kld_adaptive_particle_filter.h:201
pcl::tracking::KLDAdaptiveParticleFilterTracker::setBinSize
void setBinSize(const StateT &bin_size)
set the bin size.
Definition: kld_adaptive_particle_filter.h:79
pcl::tracking::KLDAdaptiveParticleFilterTracker::insertIntoBins
virtual bool insertIntoBins(std::vector< int > &&new_bin, std::vector< std::vector< int > > &bins)
insert a bin into the set of the bins.
Definition: kld_adaptive_particle_filter.hpp:37
pcl::tracking::KLDAdaptiveParticleFilterTracker::getEpsilon
double getEpsilon() const
get epsilon to be used to calc K-L boundary.
Definition: kld_adaptive_particle_filter.h:98
pcl::tracking::Tracker
Tracker represents the base tracker class.
Definition: tracker.h:58
pcl::tracking::KLDAdaptiveParticleFilterTracker
KLDAdaptiveParticleFilterTracker tracks the PointCloud which is given by setReferenceCloud within the...
Definition: kld_adaptive_particle_filter.h:20
pcl::tracking::KLDAdaptiveParticleFilterTracker::setEpsilon
void setEpsilon(double eps)
set epsilon to be used to calc K-L boundary.
Definition: kld_adaptive_particle_filter.h:95
pcl::tracking::KLDAdaptiveParticleFilterTracker::PointCloudStateConstPtr
typename PointCloudState::ConstPtr PointCloudStateConstPtr
Definition: kld_adaptive_particle_filter.h:55