Loading...
Searching...
No Matches
kld_adaptive_particle_filter.hpp
#ifndef PCL_TRACKING_IMPL_KLD_ADAPTIVE_PARTICLE_FILTER_H_
#define PCL_TRACKING_IMPL_KLD_ADAPTIVE_PARTICLE_FILTER_H_
#include <pcl/tracking/kld_adaptive_particle_filter.h>
namespace pcl {
namespace tracking {
template <typename PointInT, typename StateT>
bool
KLDAdaptiveParticleFilterTracker<PointInT, StateT>::initCompute()
{
if (!Tracker<PointInT, StateT>::initCompute()) {
PCL_ERROR("[pcl::%s::initCompute] Init failed.\n", getClassName().c_str());
return (false);
}
if (transed_reference_vector_.empty()) {
// only one time allocation
transed_reference_vector_.resize(maximum_particle_number_);
for (unsigned int i = 0; i < maximum_particle_number_; i++) {
transed_reference_vector_[i] = PointCloudInPtr(new PointCloudIn());
}
}
coherence_->setTargetCloud(input_);
if (!change_detector_)
change_detector_.reset(new pcl::octree::OctreePointCloudChangeDetector<PointInT>(
change_detector_resolution_));
if (!particles_ || particles_->points.empty())
initParticles(true);
return (true);
}
template <typename PointInT, typename StateT>
bool
KLDAdaptiveParticleFilterTracker<PointInT, StateT>::insertIntoBins(
std::vector<int>&& new_bin, std::vector<std::vector<int>>& bins)
{
for (auto& existing_bin : bins) {
if (equalBin(new_bin, existing_bin))
return false;
}
bins.push_back(std::move(new_bin));
return true;
}
template <typename PointInT, typename StateT>
void
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_->size());
std::vector<double> q(particles_->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_)[j_n];
x_t.sample(zero_mean, step_noise_covariance_);
// motion
if (rand() / static_cast<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_->size());
}
} // namespace tracking
} // namespace pcl
#define PCL_INSTANTIATE_KLDAdaptiveParticleFilterTracker(T, ST) \
template class PCL_EXPORTS pcl::tracking::KLDAdaptiveParticleFilterTracker<T, ST>;
#endif
pcl::PCLBase< PointInT >::input_
PointCloudConstPtr input_
Definition pcl_base.h:147
pcl::octree::OctreePointCloudChangeDetector
Octree pointcloud change detector class
Definition octree_pointcloud_changedetector.h:69
pcl::tracking::KLDAdaptiveParticleFilterTracker::calcKLBound
virtual double calcKLBound(int k)
calculate K-L boundary.
Definition kld_adaptive_particle_filter.h:214
pcl::tracking::KLDAdaptiveParticleFilterTracker::resample
void resample() override
resampling phase of particle filter method.
Definition kld_adaptive_particle_filter.hpp:51
pcl::tracking::KLDAdaptiveParticleFilterTracker::maximum_particle_number_
unsigned int maximum_particle_number_
the maximum number of the particles.
Definition kld_adaptive_particle_filter.h:242
pcl::tracking::KLDAdaptiveParticleFilterTracker::insertIntoBins
virtual bool insertIntoBins(std::vector< int > &&new_bin, std::vector< std::vector< int > > &bins)
insert a bin into the set of the bins.
Definition kld_adaptive_particle_filter.hpp:38
pcl::tracking::KLDAdaptiveParticleFilterTracker::PointCloudIn
typename Tracker< PointInT, StateT >::PointCloudIn PointCloudIn
Definition kld_adaptive_particle_filter.h:48
pcl::tracking::KLDAdaptiveParticleFilterTracker::PointCloudInPtr
typename PointCloudIn::Ptr PointCloudInPtr
Definition kld_adaptive_particle_filter.h:49
pcl::tracking::KLDAdaptiveParticleFilterTracker::bin_size_
StateT bin_size_
the size of a bin.
Definition kld_adaptive_particle_filter.h:252
pcl::tracking::KLDAdaptiveParticleFilterTracker::PointCloudStatePtr
typename PointCloudState::Ptr PointCloudStatePtr
Definition kld_adaptive_particle_filter.h:53
pcl::tracking::KLDAdaptiveParticleFilterTracker::PointCloudState
typename Tracker< PointInT, StateT >::PointCloudState PointCloudState
Definition kld_adaptive_particle_filter.h:52
pcl::tracking::KLDAdaptiveParticleFilterTracker::initCompute
bool initCompute() override
This method should get called before starting the actual computation.
Definition kld_adaptive_particle_filter.hpp:10
pcl::tracking::KLDAdaptiveParticleFilterTracker::equalBin
virtual bool equalBin(const std::vector< int > &a, const std::vector< int > &b)
return true if the two bins are equal.
Definition kld_adaptive_particle_filter.h:141
pcl::tracking::ParticleFilterTracker::genAliasTable
void genAliasTable(std::vector< int > &a, std::vector< double > &q, const PointCloudStateConstPtr &particles)
Generate the tables for walker's alias method.
Definition particle_filter.hpp:58
pcl::tracking::ParticleFilterTracker::motion_ratio_
double motion_ratio_
Ratio of hypothesis to use motion model.
Definition particle_filter.h:595
pcl::tracking::ParticleFilterTracker::initParticles
void initParticles(bool reset)
Initialize the particles.
Definition particle_filter.hpp:94
pcl::tracking::ParticleFilterTracker::coherence_
CloudCoherencePtr coherence_
A pointer to PointCloudCoherence.
Definition particle_filter.h:556
pcl::tracking::ParticleFilterTracker::sampleWithReplacement
int sampleWithReplacement(const std::vector< int > &a, const std::vector< double > &q)
Implementation of "sample with replacement" using Walker's alias method.
Definition particle_filter.hpp:42
pcl::tracking::ParticleFilterTracker::change_detector_
pcl::octree::OctreePointCloudChangeDetector< PointInT >::Ptr change_detector_
Change detector used as a trigger to track.
Definition particle_filter.h:611
pcl::tracking::ParticleFilterTracker::change_detector_resolution_
double change_detector_resolution_
Resolution of change detector.
Definition particle_filter.h:629
pcl::tracking::ParticleFilterTracker::step_noise_covariance_
std::vector< double > step_noise_covariance_
The diagonal elements of covariance matrix of the step noise.
Definition particle_filter.h:561
pcl::tracking::ParticleFilterTracker::transed_reference_vector_
std::vector< PointCloudInPtr > transed_reference_vector_
A list of the pointers to pointclouds.
Definition particle_filter.h:608
pcl::tracking::ParticleFilterTracker::particles_
PointCloudStatePtr particles_
A pointer to the particles.
Definition particle_filter.h:553
pcl::tracking::ParticleFilterTracker::particle_num_
int particle_num_
The number of the particles.
Definition particle_filter.h:541
pcl::tracking::ParticleFilterTracker::motion_
StateT motion_
Difference between the result in t and t-1.
Definition particle_filter.h:592
pcl::tracking::Tracker::getClassName
const std::string & getClassName() const
Get a string representation of the name of this class.
Definition tracker.h:97
pcl::tracking::Tracker::initCompute
virtual bool initCompute()
This method should get called before starting the actual computation.
Definition tracker.hpp:10
pcl::tracking
Definition approx_nearest_pair_point_cloud_coherence.h:7
pcl
Definition convolution.h:46