3 #include <pcl/tracking/coherence.h> 4 #include <pcl/tracking/particle_filter.h> 5 #include <pcl/tracking/tracking.h> 17 template <
typename Po
intInT,
typename StateT>
45 using Ptr = shared_ptr<KLDAdaptiveParticleFilterTracker<PointInT, StateT>>;
46 using ConstPtr = shared_ptr<const KLDAdaptiveParticleFilterTracker<PointInT, StateT>>;
145 equalBin(
const std::vector<int>& a,
const std::vector<int>& b)
147 int dimension = StateT::stateDimension();
148 for (
int i = 0; i < dimension; i++)
160 const double a[9] = {1.24818987e-4,
169 const double b[15] = {-4.5255659e-5,
198 for (
int i = 1; i < 9; i++)
205 for (
int i = 1; i < 15; i++)
210 return ((1. - z) / 2.0);
211 return ((1. + z) / 2.0);
221 double chi = 1.0 - 2.0 / (9.0 * (k - 1)) + sqrt(2.0 / (9.0 * (k - 1))) * z;
222 return ((k - 1.0) / (2.0 *
epsilon_) * chi * chi * chi);
231 insertIntoBins(std::vector<int>&& new_bin, std::vector<std::vector<int>>& bins);
261 #ifdef PCL_NO_PRECOMPILE 262 #include <pcl/tracking/impl/kld_adaptive_particle_filter.hpp> unsigned int maximum_particle_number_
the maximum number of the particles.
double getEpsilon() const
get epsilon to be used to calc K-L boundary.
void setDelta(double delta)
set delta to be used in chi-squared distribution.
KLDAdaptiveParticleFilterTracker tracks the PointCloud which is given by setReferenceCloud within the...
typename PointCloudState::ConstPtr PointCloudStateConstPtr
void setBinSize(const StateT &bin_size)
set the bin size.
typename Tracker< PointInT, StateT >::PointCloudIn PointCloudIn
StateT bin_size_
the size of a bin.
typename PointCloudState::Ptr PointCloudStatePtr
typename PointCloudIn::Ptr PointCloudInPtr
typename CloudCoherence::Ptr CloudCoherencePtr
double normalQuantile(double u)
return upper quantile of standard normal distribution.
shared_ptr< const KLDAdaptiveParticleFilterTracker< PointInT, StateT > > ConstPtr
ParticleFilterTracker tracks the PointCloud which is given by setReferenceCloud within the measured P...
typename Tracker< PointInT, StateT >::PointCloudState PointCloudState
typename CloudCoherence::ConstPtr CloudCoherenceConstPtr
shared_ptr< PointCoherence< PointInT > > Ptr
shared_ptr< const PointCoherence< PointInT > > ConstPtr
virtual double calcKLBound(int k)
calculate K-L boundary.
typename Coherence::Ptr CoherencePtr
KLDAdaptiveParticleFilterTracker()
Empty constructor.
typename PointCloudIn::ConstPtr PointCloudInConstPtr
StateT getBinSize() const
get the bin size.
void setMaximumParticleNum(unsigned int nr)
set the maximum number of the particles.
bool initCompute() override
This method should get called before starting the actual computation.
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_
void resample() override
resampling phase of particle filter method.
unsigned int getMaximumParticleNum() const
get the maximum number of the particles.
shared_ptr< PointCloudCoherence< PointInT > > Ptr
PointCloudCoherence is a base class to compute coherence between the two PointClouds.
void setEpsilon(double eps)
set epsilon to be used to calc K-L boundary.
shared_ptr< KLDAdaptiveParticleFilterTracker< PointInT, StateT > > Ptr
Tracker represents the base tracker class.
double epsilon_
error between K-L distance and MLE
double getDelta() const
get delta to be used in chi-squared distribution.
virtual bool insertIntoBins(std::vector< int > &&new_bin, std::vector< std::vector< int >> &bins)
insert a bin into the set of the bins.
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
shared_ptr< const PointCloudCoherence< PointInT > > ConstPtr
std::string tracker_name_
The tracker name.