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 ()); }