Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
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
6namespace pcl {
7namespace tracking {
8template <typename PointInT, typename StateT>
9bool
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
36template <typename PointInT, typename StateT>
37bool
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
49template <typename PointInT, typename StateT>
50void
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.
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 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