char add_bin(t_bin **head, void (*f)(t_print *))
{
t_bin *tmp;
if (!head || !(*head))
{
if (!(*head = new_bin(f)))
return (1);
return (0);
}
tmp = *head;
while (tmp->next)
tmp = tmp->next;
tmp->next = new_bin(f);
return (0);
}
template <typename PointInT, typename StateT> void
pcl::tracking::KLDAdaptiveParticleFilterTracker<PointInT, StateT>::resample ()
{
unsigned int k = 0;
unsigned int n = 0;
PointCloudStatePtr S (new PointCloudState);
std::vector<std::vector<int> > bins;
// initializing for sampling without replacement
std::vector<int> a (particles_->points.size ());
std::vector<double> q (particles_->points.size ());
this->genAliasTable (a, q, particles_);
const std::vector<double> zero_mean (StateT::stateDimension (), 0.0);
// select the particles with KLD sampling
do
{
int j_n = sampleWithReplacement (a, q);
StateT x_t = particles_->points[j_n];
x_t.sample (zero_mean, step_noise_covariance_);
// motion
if (rand () / double (RAND_MAX) < motion_ratio_)
x_t = x_t + motion_;
S->points.push_back (x_t);
// calc bin
std::vector<int> new_bin (StateT::stateDimension ());
for (int i = 0; i < StateT::stateDimension (); i++)
new_bin[i] = static_cast<int> (x_t[i] / bin_size_[i]);
// calc bin index... how?
if (insertIntoBins (std::move(new_bin), bins))
++k;
++n;
}
while (n < maximum_particle_number_ && (k < 2 || n < calcKLBound (k)));
particles_ = S; // swap
particle_num_ = static_cast<int> (particles_->points.size ());
}
