Point Cloud Library (PCL)  1.7.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_ = boost::shared_ptr<pcl::octree::OctreePointCloudChangeDetector<PointInT> >(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> bin, std::vector<std::vector<int> > &B)
38 {
39  for (size_t i = 0; i < B.size (); i++)
40  {
41  if (equalBin (bin, B[i]))
42  return false;
43  }
44  B.push_back (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> > B; // 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> bin (StateT::stateDimension ());
77  for (int i = 0; i < StateT::stateDimension (); i++)
78  bin[i] = static_cast<int> (x_t[i] / bin_size_[i]);
79 
80  // calc bin index... how?
81  if (insertIntoBins (bin, B))
82  ++k;
83  ++n;
84  }
85  while (k < 2 || (n < maximum_particle_number_ && 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