Point Cloud Library (PCL)  1.14.1-dev
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() / static_cast<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
void resample() override
resampling phase of particle filter method.
typename Tracker< PointInT, StateT >::PointCloudIn PointCloudIn
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 >::PointCloudState PointCloudState
bool initCompute() override
This method should get called before starting the actual computation.
Tracker represents the base tracker class.
Definition: tracker.h:55