Point Cloud Library (PCL)  1.9.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 < -3.)
143  return (0.0);
144  if (y > 3.)
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
pcl
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
pcl::tracking::KLDAdaptiveParticleFilterTracker::resample
virtual void resample()
resampling phase of particle filter method.
Definition: kld_adaptive_particle_filter.hpp:49
pcl::PointCloud< PointInT >::Ptr
boost::shared_ptr< PointCloud< PointInT > > Ptr
Definition: point_cloud.h:428
pcl::tracking::ParticleFilterTracker
ParticleFilterTracker tracks the PointCloud which is given by setReferenceCloud within the measured P...
Definition: particle_filter.h:22
pcl::tracking::KLDAdaptiveParticleFilterTracker::epsilon_
double epsilon_
error between K-L distance and MLE
Definition: kld_adaptive_particle_filter.h:203
pcl::tracking::KLDAdaptiveParticleFilterTracker::CloudCoherencePtr
boost::shared_ptr< CloudCoherence > CloudCoherencePtr
Definition: kld_adaptive_particle_filter.h:60
pcl::tracking::KLDAdaptiveParticleFilterTracker::PointCloudState
Tracker< PointInT, StateT >::PointCloudState PointCloudState
Definition: kld_adaptive_particle_filter.h:51
pcl::tracking::KLDAdaptiveParticleFilterTracker::setDelta
void setDelta(double delta)
set delta to be used in chi-squared distribution.
Definition: kld_adaptive_particle_filter.h:101
pcl::tracking::KLDAdaptiveParticleFilterTracker::bin_size_
StateT bin_size_
the size of a bin.
Definition: kld_adaptive_particle_filter.h:209
pcl::PointCloud< PointInT >
pcl::tracking::KLDAdaptiveParticleFilterTracker::PointCloudStatePtr
PointCloudState::Ptr PointCloudStatePtr
Definition: kld_adaptive_particle_filter.h:52
pcl::tracking::KLDAdaptiveParticleFilterTracker::PointCloudIn
Tracker< PointInT, StateT >::PointCloudIn PointCloudIn
Definition: kld_adaptive_particle_filter.h:47
pcl::tracking::KLDAdaptiveParticleFilterTracker::CoherenceConstPtr
boost::shared_ptr< const Coherence > CoherenceConstPtr
Definition: kld_adaptive_particle_filter.h:57
pcl::tracking::KLDAdaptiveParticleFilterTracker::CoherencePtr
boost::shared_ptr< Coherence > CoherencePtr
Definition: kld_adaptive_particle_filter.h:56
pcl::tracking::Tracker::tracker_name_
std::string tracker_name_
The tracker name.
Definition: tracker.h:92
pcl::tracking::KLDAdaptiveParticleFilterTracker::getBinSize
StateT getBinSize() const
get the bin size.
Definition: kld_adaptive_particle_filter.h:80
pcl::tracking::KLDAdaptiveParticleFilterTracker::calcKLBound
virtual double calcKLBound(int k)
calculate K-L boundary.
Definition: kld_adaptive_particle_filter.h:173
pcl::tracking::KLDAdaptiveParticleFilterTracker::initCompute
virtual bool initCompute()
This method should get called before starting the actual computation.
Definition: kld_adaptive_particle_filter.hpp:7
pcl::tracking::KLDAdaptiveParticleFilterTracker::getMaximumParticleNum
unsigned int getMaximumParticleNum() const
get the maximum number of the particles.
Definition: kld_adaptive_particle_filter.h:88
pcl::tracking::KLDAdaptiveParticleFilterTracker::insertIntoBins
virtual bool insertIntoBins(std::vector< int > bin, std::vector< std::vector< int > > &B)
insert a bin into the set of the bins.
Definition: kld_adaptive_particle_filter.hpp:37
pcl::tracking::KLDAdaptiveParticleFilterTracker::normalQuantile
double normalQuantile(double u)
return upper quantile of standard normal distribution.
Definition: kld_adaptive_particle_filter.h:126
pcl::tracking::KLDAdaptiveParticleFilterTracker::KLDAdaptiveParticleFilterTracker
KLDAdaptiveParticleFilterTracker()
Empty constructor.
Definition: kld_adaptive_particle_filter.h:64
pcl::tracking::KLDAdaptiveParticleFilterTracker::equalBin
virtual bool equalBin(std::vector< int > a, std::vector< int > b)
return true if the two bins are equal.
Definition: kld_adaptive_particle_filter.h:113
pcl::tracking::KLDAdaptiveParticleFilterTracker::CloudCoherenceConstPtr
boost::shared_ptr< const CloudCoherence > CloudCoherenceConstPtr
Definition: kld_adaptive_particle_filter.h:61
pcl::tracking::KLDAdaptiveParticleFilterTracker::setMaximumParticleNum
void setMaximumParticleNum(unsigned int nr)
set the maximum number of the particles.
Definition: kld_adaptive_particle_filter.h:85
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:206
pcl::tracking::KLDAdaptiveParticleFilterTracker::PointCloudStateConstPtr
PointCloudState::ConstPtr PointCloudStateConstPtr
Definition: kld_adaptive_particle_filter.h:53
pcl::tracking::KLDAdaptiveParticleFilterTracker::CloudCoherence
PointCloudCoherence< PointInT > CloudCoherence
Definition: kld_adaptive_particle_filter.h:59
pcl::PointCloud< PointInT >::ConstPtr
boost::shared_ptr< const PointCloud< PointInT > > ConstPtr
Definition: point_cloud.h:429
pcl::tracking::KLDAdaptiveParticleFilterTracker::PointCloudInPtr
PointCloudIn::Ptr PointCloudInPtr
Definition: kld_adaptive_particle_filter.h:48
pcl::tracking::PointCloudCoherence
PointCloudCoherence is a base class to compute coherence between the two PointClouds.
Definition: coherence.h:60
pcl::tracking::PointCoherence
PointCoherence is a base class to compute coherence between the two points.
Definition: coherence.h:17
pcl::B
@ B
Definition: norms.h:55
pcl::tracking::KLDAdaptiveParticleFilterTracker::BaseClass
Tracker< PointInT, StateT > BaseClass
Definition: kld_adaptive_particle_filter.h:45
pcl::tracking::KLDAdaptiveParticleFilterTracker::PointCloudInConstPtr
PointCloudIn::ConstPtr PointCloudInConstPtr
Definition: kld_adaptive_particle_filter.h:49
pcl::tracking::KLDAdaptiveParticleFilterTracker::getDelta
double getDelta() const
get delta to be used in chi-squared distribution.
Definition: kld_adaptive_particle_filter.h:104
pcl::tracking::KLDAdaptiveParticleFilterTracker::Coherence
PointCoherence< PointInT > Coherence
Definition: kld_adaptive_particle_filter.h:55
pcl::tracking::KLDAdaptiveParticleFilterTracker::maximum_particle_number_
unsigned int maximum_particle_number_
the maximum number of the particles.
Definition: kld_adaptive_particle_filter.h:200
pcl::tracking::KLDAdaptiveParticleFilterTracker::setBinSize
void setBinSize(const StateT &bin_size)
set the bin size.
Definition: kld_adaptive_particle_filter.h:77
pcl::tracking::KLDAdaptiveParticleFilterTracker::getEpsilon
double getEpsilon() const
get epsilon to be used to calc K-L boundary.
Definition: kld_adaptive_particle_filter.h:96
pcl::tracking::Tracker
Tracker represents the base tracker class.
Definition: tracker.h:56
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:93