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> unsigned int maximum_particle_number_
the maximum number of the particles.
virtual void resample()
resampling phase of particle filter method.
Tracker< PointInT, StateT >::PointCloudState PointCloudState
PointCloudIn::ConstPtr PointCloudInConstPtr
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.
PointCloudState::Ptr PointCloudStatePtr
KLDAdaptiveParticleFilterTracker tracks the PointCloud which is given by setReferenceCloud within the...
Tracker< PointInT, StateT >::PointCloudIn PointCloudIn
void setBinSize(const StateT &bin_size)
set the bin size.
boost::shared_ptr< const Coherence > CoherenceConstPtr
StateT bin_size_
the size of a bin.
virtual bool initCompute()
This method should get called before starting the actual computation.
double normalQuantile(double u)
return upper quantile of standard normal distribution.
boost::shared_ptr< Coherence > CoherencePtr
ParticleFilterTracker tracks the PointCloud which is given by setReferenceCloud within the measured P...
PointCloudState::ConstPtr PointCloudStateConstPtr
boost::shared_ptr< PointCloud< PointInT > > Ptr
virtual bool equalBin(std::vector< int > a, std::vector< int > b)
return true if the two bins are equal.
virtual double calcKLBound(int k)
calculate K-L boundary.
KLDAdaptiveParticleFilterTracker()
Empty constructor.
Tracker< PointInT, StateT > BaseClass
void setMaximumParticleNum(unsigned int nr)
set the maximum number of the particles.
PointCoherence is a base class to compute coherence between the two points.
double delta_
probability of distance between K-L distance and MLE is less than epsilon_
boost::shared_ptr< const PointCloud< PointInT > > ConstPtr
virtual bool insertIntoBins(std::vector< int > bin, std::vector< std::vector< int > > &B)
insert a bin into the set of the bins.
boost::shared_ptr< const CloudCoherence > CloudCoherenceConstPtr
double getDelta() const
get delta to be used in chi-squared distribution.
PointCloudCoherence is a base class to compute coherence between the two PointClouds.
StateT getBinSize() const
get the bin size.
void setEpsilon(double eps)
set epsilon to be used to calc K-L boundary.
Tracker represents the base tracker class.
PointCloudCoherence< PointInT > CloudCoherence
double epsilon_
error between K-L distance and MLE
PointCoherence< PointInT > Coherence
boost::shared_ptr< CloudCoherence > CloudCoherencePtr
unsigned int getMaximumParticleNum() const
get the maximum number of the particles.
PointCloudIn::Ptr PointCloudInPtr
std::string tracker_name_
The tracker name.