1 #ifndef PCL_TRACKING_KLD_ADAPTIVE_PARTICLE_FILTER_H_
2 #define PCL_TRACKING_KLD_ADAPTIVE_PARTICLE_FILTER_H_
4 #include <pcl/tracking/tracking.h>
5 #include <pcl/tracking/particle_filter.h>
6 #include <pcl/tracking/coherence.h>
19 template <
typename Po
intInT,
typename StateT>
115 int dimension = StateT::stateDimension ();
116 for (
int i = 0; i < dimension; i++)
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};
152 for (i = 1; i < 9; i++)
160 for (i = 1; i < 15; i++)
165 return ((1. - z) / 2.0);
166 return ((1. + z) / 2.0);
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);
186 insertIntoBins (std::vector<int> bin, std::vector<std::vector<int> > &
B);
214 #ifdef PCL_NO_PRECOMPILE
215 #include <pcl/tracking/impl/kld_adaptive_particle_filter.hpp>
KLDAdaptiveParticleFilterTracker tracks the PointCloud which is given by setReferenceCloud within the...
virtual bool initCompute()
This method should get called before starting the actual computation.
ParticleFilterTracker tracks the PointCloud which is given by setReferenceCloud within the measured P...
Tracker represents the base tracker class.
PointCloudCoherence< PointInT > CloudCoherence
StateT bin_size_
the size of a bin.
std::string tracker_name_
The tracker name.
void setEpsilon(double eps)
set epsilon to be used to calc K-L boundary.
Tracker< PointInT, StateT > BaseClass
unsigned int getMaximumParticleNum() const
get the maximum number of the particles.
Tracker< PointInT, StateT >::PointCloudState PointCloudState
StateT getBinSize() const
get the bin size.
double getDelta() const
get delta to be used in chi-squared distribution.
PointCloudState::Ptr PointCloudStatePtr
virtual void resample()
resampling phase of particle filter method.
double getEpsilon() const
get epsilon to be used to calc K-L boundary.
PointCoherence is a base class to compute coherence between the two points.
unsigned int maximum_particle_number_
the maximum number of the particles.
PointCloudState::ConstPtr PointCloudStateConstPtr
boost::shared_ptr< const CloudCoherence > CloudCoherenceConstPtr
virtual double calcKLBound(int k)
calculate K-L boundary.
boost::shared_ptr< PointCloud< PointT > > Ptr
boost::shared_ptr< CloudCoherence > CloudCoherencePtr
PointCloudIn::ConstPtr PointCloudInConstPtr
double normalQuantile(double u)
return upper quantile of standard normal distribution.
void setMaximumParticleNum(unsigned int nr)
set the maximum number of the particles.
double delta_
probability of distance between K-L distance and MLE is less than epsilon_
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
PointCloudCoherence is a base class to compute coherence between the two PointClouds.
boost::shared_ptr< Coherence > CoherencePtr
PointCloudIn::Ptr PointCloudInPtr
boost::shared_ptr< const Coherence > CoherenceConstPtr
void setBinSize(const StateT &bin_size)
set the bin size.
void setDelta(double delta)
set delta to be used in chi-squared distribution.
virtual bool equalBin(std::vector< int > a, std::vector< int > b)
return true if the two bins are equal.
virtual bool insertIntoBins(std::vector< int > bin, std::vector< std::vector< int > > &B)
insert a bin into the set of the bins.
double epsilon_
error between K-L distance and MLE
PointCoherence< PointInT > Coherence
KLDAdaptiveParticleFilterTracker()
Empty constructor.
Tracker< PointInT, StateT >::PointCloudIn PointCloudIn