13 PCL_ERROR(
"[pcl::%s::initCompute] Init failed.\n", getClassName().c_str());
17 if (transed_reference_vector_.empty()) {
19 transed_reference_vector_.resize(maximum_particle_number_);
20 for (
unsigned int i = 0; i < maximum_particle_number_; i++) {
25 coherence_->setTargetCloud(input_);
27 if (!change_detector_)
29 change_detector_resolution_));
31 if (!particles_ || particles_->points.empty())
56 std::vector<std::vector<int>> bins;
59 std::vector<int> a(particles_->size());
60 std::vector<double> q(particles_->size());
61 this->genAliasTable(a, q, particles_);
63 const std::vector<double> zero_mean(StateT::stateDimension(), 0.0);
67 int j_n = sampleWithReplacement(a, q);
68 StateT x_t = (*particles_)[j_n];
69 x_t.sample(zero_mean, step_noise_covariance_);
72 if (rand() /
static_cast<double>(RAND_MAX) < motion_ratio_)
75 S->points.push_back(x_t);
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]);
82 if (insertIntoBins(std::move(new_bin), bins))
85 }
while (n < maximum_particle_number_ && (k < 2 || n < calcKLBound(k)));
88 particle_num_ =
static_cast<int>(particles_->size());
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 PointCloudIn::Ptr PointCloudInPtr
typename PointCloudState::Ptr PointCloudStatePtr
typename Tracker< PointInT, StateT >::PointCloudState PointCloudState
bool initCompute() override
This method should get called before starting the actual computation.