3#include <pcl/tracking/coherence.h>
4#include <pcl/tracking/particle_filter.h>
5#include <pcl/tracking/tracking.h>
17template <
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>
PointCloudConstPtr input_
KLDAdaptiveParticleFilterTracker tracks the PointCloud which is given by setReferenceCloud within the...
shared_ptr< const KLDAdaptiveParticleFilterTracker< PointInT, StateT > > ConstPtr
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.
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.
shared_ptr< KLDAdaptiveParticleFilterTracker< PointInT, StateT > > Ptr
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...
int iteration_num_
The number of iteration of particlefilter.
double motion_ratio_
Ratio of hypothesis to use motion model.
void initParticles(bool reset)
Initialize the particles.
CloudCoherencePtr coherence_
A pointer to PointCloudCoherence.
int sampleWithReplacement(const std::vector< int > &a, const std::vector< double > &q)
Implementation of "sample with replacement" using Walker's alias method.
StateT representative_state_
The result of tracking.
pcl::octree::OctreePointCloudChangeDetector< PointInT >::Ptr change_detector_
Change detector used as a trigger to track.
double change_detector_resolution_
Resolution of change detector.
std::vector< double > step_noise_covariance_
The diagonal elements of covariance matrix of the step noise.
std::vector< PointCloudInPtr > transed_reference_vector_
A list of the pointers to pointclouds.
bool use_normal_
A flag to use normal or not.
PointCloudStatePtr particles_
A pointer to the particles
virtual void update()
Calculate the weighted mean of the particles and set it as the result.
int particle_num_
The number of the particles.
virtual void weight()
Weighting phase of particle filter method.
StateT motion_
Difference between the result in t and t-1.
bool use_change_detector_
The flag which will be true if using change detection.
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< PointCoherence< PointInT > > Ptr
shared_ptr< const PointCoherence< PointInT > > ConstPtr
Tracker represents the base tracker class.
const std::string & getClassName() const
Get a string representation of the name of this class.
SearchPtr search_
A pointer to the spatial search object.
std::string tracker_name_
The tracker name.