1 #ifndef PCL_TRACKING_IMPL_KLD_ADAPTIVE_PARTICLE_FILTER_H_
2 #define PCL_TRACKING_IMPL_KLD_ADAPTIVE_PARTICLE_FILTER_H_
4 #include <pcl/tracking/kld_adaptive_particle_filter.h>
6 template <
typename Po
intInT,
typename StateT>
bool
11 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
15 if (transed_reference_vector_.empty ())
18 transed_reference_vector_.resize (maximum_particle_number_);
19 for (
unsigned int i = 0; i < maximum_particle_number_; i++)
25 coherence_->setTargetCloud (input_);
27 if (!change_detector_)
30 if (!particles_ || particles_->points.empty ())
35 template <
typename Po
intInT,
typename StateT>
bool
37 (std::vector<int> bin, std::vector<std::vector<int> > &
B)
39 for (
size_t i = 0; i <
B.size (); i++)
41 if (equalBin (bin,
B[i]))
48 template <
typename Po
intInT,
typename StateT>
void
54 std::vector<std::vector<int> >
B;
57 std::vector<int> a (particles_->points.size ());
58 std::vector<double> q (particles_->points.size ());
59 this->genAliasTable (a, q, particles_);
61 const std::vector<double> zero_mean (StateT::stateDimension (), 0.0);
66 int j_n = sampleWithReplacement (a, q);
67 StateT x_t = particles_->points[j_n];
68 x_t.sample (zero_mean, step_noise_covariance_);
71 if (rand () /
double (RAND_MAX) < motion_ratio_)
74 S->points.push_back (x_t);
76 std::vector<int> bin (StateT::stateDimension ());
77 for (
int i = 0; i < StateT::stateDimension (); i++)
78 bin[i] = static_cast<int> (x_t[i] / bin_size_[i]);
81 if (insertIntoBins (bin, B))
85 while (k < 2 || (n < maximum_particle_number_ && n < calcKLBound (k)));
88 particle_num_ =
static_cast<int> (particles_->points.size ());
92 #define PCL_INSTANTIATE_KLDAdaptiveParticleFilterTracker(T,ST) template class PCL_EXPORTS pcl::tracking::KLDAdaptiveParticleFilterTracker<T,ST>;
virtual bool insertIntoBins(std::vector< int > bin, std::vector< std::vector< int > > &B)
insert a bin into the set of the bins.
virtual bool initCompute()
This method should get called before starting the actual computation.
Octree pointcloud change detector class
virtual void resample()
resampling phase of particle filter method.
PointCloudIn::Ptr PointCloudInPtr
PointCloudState::Ptr PointCloudStatePtr
Tracker represents the base tracker class.