Point Cloud Library (PCL)  1.11.0
kld_adaptive_particle_filter.hpp
1 #ifndef PCL_TRACKING_IMPL_KLD_ADAPTIVE_PARTICLE_FILTER_H_
2 #define PCL_TRACKING_IMPL_KLD_ADAPTIVE_PARTICLE_FILTER_H_
3 
4 #include <pcl/tracking/kld_adaptive_particle_filter.h>
5 
6 template <typename PointInT, typename StateT> bool
8 {
10  {
11  PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
12  return (false);
13  }
14 
15  if (transed_reference_vector_.empty ())
16  {
17  // only one time allocation
18  transed_reference_vector_.resize (maximum_particle_number_);
19  for (unsigned int i = 0; i < maximum_particle_number_; i++)
20  {
21  transed_reference_vector_[i] = PointCloudInPtr (new PointCloudIn ());
22  }
23  }
24 
25  coherence_->setTargetCloud (input_);
26 
27  if (!change_detector_)
28  change_detector_.reset (new pcl::octree::OctreePointCloudChangeDetector<PointInT> (change_detector_resolution_));
29 
30  if (!particles_ || particles_->points.empty ())
31  initParticles (true);
32  return (true);
33 }
34 
35 template <typename PointInT, typename StateT> bool
37 (std::vector<int> &&new_bin, std::vector<std::vector<int> > &bins)
38 {
39  for (auto &existing_bin : bins)
40  {
41  if (equalBin (new_bin, existing_bin))
42  return false;
43  }
44  bins.push_back (std::move(new_bin));
45  return true;
46 }
47 
48 template <typename PointInT, typename StateT> void
50 {
51  unsigned int k = 0;
52  unsigned int n = 0;
54  std::vector<std::vector<int> > bins;
55 
56  // initializing for sampling without replacement
57  std::vector<int> a (particles_->points.size ());
58  std::vector<double> q (particles_->points.size ());
59  this->genAliasTable (a, q, particles_);
60 
61  const std::vector<double> zero_mean (StateT::stateDimension (), 0.0);
62 
63  // select the particles with KLD sampling
64  do
65  {
66  int j_n = sampleWithReplacement (a, q);
67  StateT x_t = particles_->points[j_n];
68  x_t.sample (zero_mean, step_noise_covariance_);
69 
70  // motion
71  if (rand () / double (RAND_MAX) < motion_ratio_)
72  x_t = x_t + motion_;
73 
74  S->points.push_back (x_t);
75  // calc bin
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]);
79 
80  // calc bin index... how?
81  if (insertIntoBins (std::move(new_bin), bins))
82  ++k;
83  ++n;
84  }
85  while (n < maximum_particle_number_ && (k < 2 || n < calcKLBound (k)));
86 
87  particles_ = S; // swap
88  particle_num_ = static_cast<int> (particles_->points.size ());
89 }
90 
91 
92 #define PCL_INSTANTIATE_KLDAdaptiveParticleFilterTracker(T,ST) template class PCL_EXPORTS pcl::tracking::KLDAdaptiveParticleFilterTracker<T,ST>;
93 
94 #endif
typename Tracker< PointInT, StateT >::PointCloudIn PointCloudIn
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.
Definition: tracker.h:57
virtual bool insertIntoBins(std::vector< int > &&new_bin, std::vector< std::vector< int > > &bins)
insert a bin into the set of the bins.