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> &&new_bin, std::vector<std::vector<int> > &bins)
39 for (
auto &existing_bin : bins)
41 if (equalBin (new_bin, existing_bin))
44 bins.push_back (std::move(new_bin));
48 template <
typename Po
intInT,
typename StateT>
void
54 std::vector<std::vector<int> > bins;
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> new_bin (StateT::stateDimension ());
77 for (
int i = 0; i < StateT::stateDimension (); i++)
78 new_bin[i] = static_cast<int> (x_t[i] / bin_size_[i]);
81 if (insertIntoBins (std::move(new_bin), bins))
85 while (n < maximum_particle_number_ && (k < 2 || 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>;
typename Tracker< PointInT, StateT >::PointCloudIn PointCloudIn
typename PointCloudState::Ptr PointCloudStatePtr
typename PointCloudIn::Ptr PointCloudInPtr
typename Tracker< PointInT, StateT >::PointCloudState PointCloudState
bool initCompute() override
This method should get called before starting the actual computation.
void resample() override
resampling phase of particle filter method.
Tracker represents the base tracker class.
virtual bool insertIntoBins(std::vector< int > &&new_bin, std::vector< std::vector< int > > &bins)
insert a bin into the set of the bins.