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>;
void resample() override
resampling phase of particle filter method.
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 PointCloudIn::Ptr PointCloudInPtr
typename PointCloudState::Ptr PointCloudStatePtr
typename Tracker< PointInT, StateT >::PointCloudState PointCloudState
bool initCompute() override
This method should get called before starting the actual computation.
Tracker represents the base tracker class.