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