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