ParticleFilterTracker tracks the PointCloud which is given by setReferenceCloud within the measured PointCloud using particle filter method. More...
#include <particle_filter.h>
Public Types | |
typedef Tracker< PointInT, StateT > | BaseClass |
typedef PointCloudCoherence < PointInT > | CloudCoherence |
typedef boost::shared_ptr < const CloudCoherence > | CloudCoherenceConstPtr |
typedef boost::shared_ptr < CloudCoherence > | CloudCoherencePtr |
typedef PointCoherence< PointInT > | Coherence |
typedef boost::shared_ptr < const Coherence > | CoherenceConstPtr |
typedef boost::shared_ptr < Coherence > | CoherencePtr |
typedef Tracker< PointInT, StateT >::PointCloudIn | PointCloudIn |
typedef PointCloudIn::ConstPtr | PointCloudInConstPtr |
typedef PointCloudIn::Ptr | PointCloudInPtr |
typedef Tracker< PointInT, StateT >::PointCloudState | PointCloudState |
typedef PointCloudState::ConstPtr | PointCloudStateConstPtr |
typedef PointCloudState::Ptr | PointCloudStatePtr |
Public Member Functions | |
double | getAlpha () |
get the value of alpha. | |
CloudCoherencePtr | getCloudCoherence () const |
get the PointCloudCoherence to compute likelihood. | |
double | getFitRatio () const |
get the adjustment ratio. | |
unsigned int | getIntervalOfChangeDetection () |
get the number of interval frames to run change detection. | |
int | getIterationNum () const |
get the number of iteration. | |
unsigned int | getMinPointsOfChangeDetection () |
get the minimum amount of points required within leaf node to become serialized in change detection | |
double | getMotionRatio () |
get the motion ratio | |
int | getParticleNum () const |
get the number of the particles. | |
PointCloudStatePtr | getParticles () const |
get a pointer to a pointcloud of the particles. | |
PointCloudInConstPtr const | getReferenceCloud () |
get a pointer to a reference dataset to be tracked. | |
double | getResolutionOfChangeDetection () |
get the resolution of change detection. | |
virtual StateT | getResult () const |
Get an instance of the result of tracking. | |
Eigen::Affine3f | getTrans () const |
get the transformation from the world coordinates to the frame of the particles. | |
bool | getUseChangeDetector () |
get the value of use_change_detector_. | |
bool | getUseNormal () |
get the value of use_normal_. | |
double | normalizeParticleWeight (double w, double w_min, double w_max) |
normalize the weight of a particle using exp(1- alpha ( w - w_{min}) / (w_max - w_min)). this method is described in [P.Azad et. al, ICRA11]. | |
ParticleFilterTracker () | |
Empty constructor. | |
virtual void | resetTracking () |
reset the particles to restart tracking | |
void | setAlpha (double alpha) |
set the value of alpha. | |
void | setCloudCoherence (const CloudCoherencePtr &coherence) |
set the PointCloudCoherence as likelihood. | |
void | setInitialNoiseCovariance (const std::vector< double > &initial_noise_covariance) |
set the covariance of the initial noise. it will be used when initializing the particles. | |
void | setInitialNoiseMean (const std::vector< double > &initial_noise_mean) |
set the mean of the initial noise. it will be used when initializing the particles. | |
void | setIntervalOfChangeDetection (unsigned int change_detector_interval) |
set the number of interval frames to run change detection. | |
void | setIterationNum (const int iteration_num) |
set the number of iteration. | |
void | setMinIndices (const int min_indices) |
set the minimum number of indices (default: 1). ParticleFilterTracker does not take into account the hypothesis whose the number of points is smaller than the minimum indices. | |
void | setMinPointsOfChangeDetection (unsigned int change_detector_filter) |
set the minimum amount of points required within leaf node to become serialized in change detection | |
void | setMotionRatio (double motion_ratio) |
set the motion ratio | |
void | setOcclusionAngleThe (const double occlusion_angle_thr) |
set the threshold of angle to be considered occlusion (default: pi/2). ParticleFilterTracker does not take the occluded points into account according to the angle between the normal and the position. | |
void | setParticleNum (const int particle_num) |
set the number of the particles. | |
void | setReferenceCloud (const PointCloudInConstPtr &ref) |
set a pointer to a reference dataset to be tracked. | |
void | setResampleLikelihoodThr (const double resample_likelihood_thr) |
set the threshold to re-initialize the particles. | |
void | setResolutionOfChangeDetection (double resolution) |
set the resolution of change detection. | |
void | setStepNoiseCovariance (const std::vector< double > &step_noise_covariance) |
set the covariance of step noise. | |
void | setTrans (const Eigen::Affine3f &trans) |
set the transformation from the world coordinates to the frame of the particles. | |
void | setUseChangeDetector (bool use_change_detector) |
set the value of use_change_detector_. | |
void | setUseNormal (bool use_normal) |
set the value of use_normal_. | |
Eigen::Affine3f | toEigenMatrix (const StateT &particle) |
convert a state to affine transformation from the world coordinates frame. | |
Protected Member Functions | |
void | calcBoundingBox (double &x_min, double &x_max, double &y_min, double &y_max, double &z_min, double &z_max) |
compute the parameters for the bounding box of hypothesis pointclouds. | |
virtual void | computeTracking () |
track the pointcloud using particle filter method. | |
void | computeTransformedPointCloud (const StateT &hypothesis, std::vector< int > &indices, PointCloudIn &cloud) |
compute a reference pointcloud transformed to the pose that hypothesis represents. | |
void | computeTransformedPointCloudWithNormal (const StateT &hypothesis, std::vector< int > &indices, PointCloudIn &cloud) |
compute a reference pointcloud transformed to the pose that hypothesis represents and calculate indices taking occlusion into \ account. | |
void | computeTransformedPointCloudWithoutNormal (const StateT &hypothesis, PointCloudIn &cloud) |
compute a reference pointcloud transformed to the pose that hypothesis represents and calculate indices without taking occlusion into account. | |
void | cropInputPointCloud (const PointCloudInConstPtr &cloud, PointCloudIn &output) |
crop the pointcloud by the bounding box calculated from hypothesis and the reference pointcloud. | |
void | genAliasTable (std::vector< int > &a, std::vector< double > &q, const PointCloudStateConstPtr &particles) |
generate the tables for walker's alias method | |
virtual bool | initCompute () |
This method should get called before starting the actual computation. | |
void | initParticles (bool reset) |
initialize the particles. initial_noise_covariance_ and initial_noise_mean_ are used for gausiaan sampling. | |
virtual void | normalizeWeight () |
normalize the weights of all the particels. | |
virtual void | resample () |
resampling phase of particle filter method. sampling the particles according to the weights calculated in weight method. in particular, "sample with replacement" is archieved by walker's alias method. | |
void | resampleDeterministic () |
resampling the particle in deterministic way | |
void | resampleWithReplacement () |
resampling the particle with replacement | |
int | sampleWithReplacement (const std::vector< int > &a, const std::vector< double > &q) |
implementation of "sample with replacement" using Walker's alias method. about Walker's alias method, you can check the paper below: {355749, author = {Walker, Alastair J.}, title = {An Efficient Method for Generating Discrete Random Variables with General Distributions}, journal = {ACM Trans. Math. Softw.}, volume = {3}, number = {3}, year = {1977}, issn = {0098-3500}, pages = {253--256}, doi = {http://doi.acm.org/10.1145/355744.355749}, publisher = {ACM}, address = {New York, NY, USA}, } | |
bool | testChangeDetection (const PointCloudInConstPtr &input) |
run change detection and return true if there is a change. | |
virtual void | update () |
calculate the weighted mean of the particles and set it as the result | |
virtual void | weight () |
weighting phase of particle filter method. calculate the likelihood of all of the particles and set the weights. | |
Protected Attributes | |
double | alpha_ |
the weight to be used in normalization of the weights of the particles | |
unsigned int | change_counter_ |
a counter to skip change detection | |
boost::shared_ptr < pcl::octree::OctreePointCloudChangeDetector < PointInT > > | change_detector_ |
change detector used as a trigger to track | |
unsigned int | change_detector_filter_ |
minimum points in a leaf when calling change detector. defaults to 10 | |
unsigned int | change_detector_interval_ |
the number of interval frame to run change detection. defaults to 10. | |
double | change_detector_resolution_ |
resolution of change detector. defaults to 0.01. | |
bool | changed_ |
a flag to be true when change of pointclouds is detected | |
CloudCoherencePtr | coherence_ |
a pointer to PointCloudCoherence. | |
double | fit_ratio_ |
adjustment of the particle filter. | |
std::vector< double > | initial_noise_covariance_ |
the diagonal elements of covariance matrix of the initial noise. the covariance matrix is used when initialize the particles. | |
std::vector< double > | initial_noise_mean_ |
the mean values of initial noise. | |
int | iteration_num_ |
the number of iteration of particlefilter. | |
int | min_indices_ |
the minimum number of points which the hypothesis should have. | |
StateT | motion_ |
difference between the result in t and t-1 | |
double | motion_ratio_ |
ratio of hypothesis to use motion model | |
double | occlusion_angle_thr_ |
the threshold for the points to be considered as occluded | |
int | particle_num_ |
the number of the particles. | |
PointCloudStatePtr | particles_ |
a pointer to the particles | |
pcl::PassThrough< PointInT > | pass_x_ |
pass through filter to crop the pointclouds within the hypothesis bounding box | |
pcl::PassThrough< PointInT > | pass_y_ |
pass through filter to crop the pointclouds within the hypothesis bounding box | |
pcl::PassThrough< PointInT > | pass_z_ |
pass through filter to crop the pointclouds within the hypothesis bounding box | |
PointCloudInConstPtr | ref_ |
a pointer to reference point cloud. | |
StateT | representative_state_ |
the result of tracking. | |
double | resample_likelihood_thr_ |
the threshold for the particles to be re-initialized | |
std::vector< double > | step_noise_covariance_ |
the diagonal elements of covariance matrix of the step noise. the covariance matrix is used at every resample method. | |
Eigen::Affine3f | trans_ |
an affine transformation from the world coordinates frame to the origin of the particles | |
std::vector< PointCloudInPtr > | transed_reference_vector_ |
a list of the pointers to pointclouds | |
bool | use_change_detector_ |
the flag which will be true if using change detection | |
bool | use_normal_ |
a flag to use normal or not. defaults to false |
ParticleFilterTracker tracks the PointCloud which is given by setReferenceCloud within the measured PointCloud using particle filter method.
Definition at line 23 of file particle_filter.h.
typedef Tracker<PointInT, StateT> pcl::tracking::ParticleFilterTracker< PointInT, StateT >::BaseClass |
Reimplemented from pcl::tracking::Tracker< PointInT, StateT >.
Reimplemented in pcl::tracking::KLDAdaptiveParticleFilterOMPTracker< PointInT, StateT >, pcl::tracking::KLDAdaptiveParticleFilterTracker< PointInT, StateT >, and pcl::tracking::ParticleFilterOMPTracker< PointInT, StateT >.
Definition at line 35 of file particle_filter.h.
typedef PointCloudCoherence<PointInT> pcl::tracking::ParticleFilterTracker< PointInT, StateT >::CloudCoherence |
Reimplemented in pcl::tracking::KLDAdaptiveParticleFilterOMPTracker< PointInT, StateT >, pcl::tracking::KLDAdaptiveParticleFilterTracker< PointInT, StateT >, and pcl::tracking::ParticleFilterOMPTracker< PointInT, StateT >.
Definition at line 49 of file particle_filter.h.
typedef boost::shared_ptr< const CloudCoherence > pcl::tracking::ParticleFilterTracker< PointInT, StateT >::CloudCoherenceConstPtr |
Reimplemented in pcl::tracking::KLDAdaptiveParticleFilterOMPTracker< PointInT, StateT >, pcl::tracking::KLDAdaptiveParticleFilterTracker< PointInT, StateT >, and pcl::tracking::ParticleFilterOMPTracker< PointInT, StateT >.
Definition at line 51 of file particle_filter.h.
typedef boost::shared_ptr< CloudCoherence > pcl::tracking::ParticleFilterTracker< PointInT, StateT >::CloudCoherencePtr |
Reimplemented in pcl::tracking::KLDAdaptiveParticleFilterOMPTracker< PointInT, StateT >, pcl::tracking::KLDAdaptiveParticleFilterTracker< PointInT, StateT >, and pcl::tracking::ParticleFilterOMPTracker< PointInT, StateT >.
Definition at line 50 of file particle_filter.h.
typedef PointCoherence<PointInT> pcl::tracking::ParticleFilterTracker< PointInT, StateT >::Coherence |
Reimplemented in pcl::tracking::KLDAdaptiveParticleFilterOMPTracker< PointInT, StateT >, pcl::tracking::KLDAdaptiveParticleFilterTracker< PointInT, StateT >, and pcl::tracking::ParticleFilterOMPTracker< PointInT, StateT >.
Definition at line 45 of file particle_filter.h.
typedef boost::shared_ptr< const Coherence > pcl::tracking::ParticleFilterTracker< PointInT, StateT >::CoherenceConstPtr |
Reimplemented in pcl::tracking::KLDAdaptiveParticleFilterOMPTracker< PointInT, StateT >, pcl::tracking::KLDAdaptiveParticleFilterTracker< PointInT, StateT >, and pcl::tracking::ParticleFilterOMPTracker< PointInT, StateT >.
Definition at line 47 of file particle_filter.h.
typedef boost::shared_ptr< Coherence > pcl::tracking::ParticleFilterTracker< PointInT, StateT >::CoherencePtr |
Reimplemented in pcl::tracking::KLDAdaptiveParticleFilterOMPTracker< PointInT, StateT >, pcl::tracking::KLDAdaptiveParticleFilterTracker< PointInT, StateT >, and pcl::tracking::ParticleFilterOMPTracker< PointInT, StateT >.
Definition at line 46 of file particle_filter.h.
typedef Tracker<PointInT, StateT>::PointCloudIn pcl::tracking::ParticleFilterTracker< PointInT, StateT >::PointCloudIn |
Reimplemented from pcl::tracking::Tracker< PointInT, StateT >.
Reimplemented in pcl::tracking::KLDAdaptiveParticleFilterOMPTracker< PointInT, StateT >, pcl::tracking::KLDAdaptiveParticleFilterTracker< PointInT, StateT >, and pcl::tracking::ParticleFilterOMPTracker< PointInT, StateT >.
Definition at line 37 of file particle_filter.h.
typedef PointCloudIn::ConstPtr pcl::tracking::ParticleFilterTracker< PointInT, StateT >::PointCloudInConstPtr |
Reimplemented from pcl::tracking::Tracker< PointInT, StateT >.
Reimplemented in pcl::tracking::KLDAdaptiveParticleFilterOMPTracker< PointInT, StateT >, pcl::tracking::KLDAdaptiveParticleFilterTracker< PointInT, StateT >, and pcl::tracking::ParticleFilterOMPTracker< PointInT, StateT >.
Definition at line 39 of file particle_filter.h.
typedef PointCloudIn::Ptr pcl::tracking::ParticleFilterTracker< PointInT, StateT >::PointCloudInPtr |
Reimplemented from pcl::tracking::Tracker< PointInT, StateT >.
Reimplemented in pcl::tracking::KLDAdaptiveParticleFilterOMPTracker< PointInT, StateT >, pcl::tracking::KLDAdaptiveParticleFilterTracker< PointInT, StateT >, and pcl::tracking::ParticleFilterOMPTracker< PointInT, StateT >.
Definition at line 38 of file particle_filter.h.
typedef Tracker<PointInT, StateT>::PointCloudState pcl::tracking::ParticleFilterTracker< PointInT, StateT >::PointCloudState |
Reimplemented from pcl::tracking::Tracker< PointInT, StateT >.
Reimplemented in pcl::tracking::KLDAdaptiveParticleFilterOMPTracker< PointInT, StateT >, pcl::tracking::KLDAdaptiveParticleFilterTracker< PointInT, StateT >, and pcl::tracking::ParticleFilterOMPTracker< PointInT, StateT >.
Definition at line 41 of file particle_filter.h.
typedef PointCloudState::ConstPtr pcl::tracking::ParticleFilterTracker< PointInT, StateT >::PointCloudStateConstPtr |
Reimplemented from pcl::tracking::Tracker< PointInT, StateT >.
Reimplemented in pcl::tracking::KLDAdaptiveParticleFilterOMPTracker< PointInT, StateT >, pcl::tracking::KLDAdaptiveParticleFilterTracker< PointInT, StateT >, and pcl::tracking::ParticleFilterOMPTracker< PointInT, StateT >.
Definition at line 43 of file particle_filter.h.
typedef PointCloudState::Ptr pcl::tracking::ParticleFilterTracker< PointInT, StateT >::PointCloudStatePtr |
Reimplemented from pcl::tracking::Tracker< PointInT, StateT >.
Reimplemented in pcl::tracking::KLDAdaptiveParticleFilterOMPTracker< PointInT, StateT >, pcl::tracking::KLDAdaptiveParticleFilterTracker< PointInT, StateT >, and pcl::tracking::ParticleFilterOMPTracker< PointInT, StateT >.
Definition at line 42 of file particle_filter.h.
pcl::tracking::ParticleFilterTracker< PointInT, StateT >::ParticleFilterTracker | ( | ) | [inline] |
Empty constructor.
Definition at line 54 of file particle_filter.h.
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::calcBoundingBox | ( | double & | x_min, |
double & | x_max, | ||
double & | y_min, | ||
double & | y_max, | ||
double & | z_min, | ||
double & | z_max | ||
) | [protected] |
compute the parameters for the bounding box of hypothesis pointclouds.
x_min | the minimum value of x axis. |
x_max | the maximum value of x axis. |
y_min | the minimum value of y axis. |
y_max | the maximum value of y axis. |
z_min | the minimum value of z axis. |
z_max | the maximum value of z axis. |
Definition at line 192 of file particle_filter.hpp.
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::computeTracking | ( | ) | [protected, virtual] |
track the pointcloud using particle filter method.
Implements pcl::tracking::Tracker< PointInT, StateT >.
Definition at line 386 of file particle_filter.hpp.
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::computeTransformedPointCloud | ( | const StateT & | hypothesis, |
std::vector< int > & | indices, | ||
PointCloudIn & | cloud | ||
) | [protected] |
compute a reference pointcloud transformed to the pose that hypothesis represents.
hypothesis | a particle which represents a hypothesis. |
indices | the indices which should be taken into account. |
cloud | the resultant point cloud model dataset which is transformed to hypothesis. |
Definition at line 276 of file particle_filter.hpp.
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::computeTransformedPointCloudWithNormal | ( | const StateT & | hypothesis, |
std::vector< int > & | indices, | ||
PointCloudIn & | cloud | ||
) | [protected] |
compute a reference pointcloud transformed to the pose that hypothesis represents and calculate indices taking occlusion into \ account.
hypothesis | a particle which represents a hypothesis. |
indices | the indices which should be taken into account. |
cloud | the resultant point cloud model dataset which is transformed to hypothesis. |
Definition at line 295 of file particle_filter.hpp.
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::computeTransformedPointCloudWithoutNormal | ( | const StateT & | hypothesis, |
PointCloudIn & | cloud | ||
) | [protected] |
compute a reference pointcloud transformed to the pose that hypothesis represents and calculate indices without taking occlusion into account.
hypothesis | a particle which represents a hypothesis. |
cloud | the resultant point cloud model dataset which is transformed to hypothesis. |
Definition at line 286 of file particle_filter.hpp.
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::cropInputPointCloud | ( | const PointCloudInConstPtr & | cloud, |
PointCloudIn & | output | ||
) | [protected] |
crop the pointcloud by the bounding box calculated from hypothesis and the reference pointcloud.
cloud | a pointer to pointcloud to be cropped. |
output | a pointer to be assigned the cropped pointcloud. |
Definition at line 168 of file particle_filter.hpp.
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::genAliasTable | ( | std::vector< int > & | a, |
std::vector< double > & | q, | ||
const PointCloudStateConstPtr & | particles | ||
) | [protected] |
generate the tables for walker's alias method
Definition at line 57 of file particle_filter.hpp.
double pcl::tracking::ParticleFilterTracker< PointInT, StateT >::getAlpha | ( | ) | [inline] |
get the value of alpha.
Definition at line 231 of file particle_filter.h.
CloudCoherencePtr pcl::tracking::ParticleFilterTracker< PointInT, StateT >::getCloudCoherence | ( | ) | const [inline] |
get the PointCloudCoherence to compute likelihood.
Definition at line 131 of file particle_filter.h.
double pcl::tracking::ParticleFilterTracker< PointInT, StateT >::getFitRatio | ( | ) | const [inline] |
get the adjustment ratio.
Definition at line 295 of file particle_filter.h.
unsigned int pcl::tracking::ParticleFilterTracker< PointInT, StateT >::getIntervalOfChangeDetection | ( | ) | [inline] |
get the number of interval frames to run change detection.
Definition at line 266 of file particle_filter.h.
int pcl::tracking::ParticleFilterTracker< PointInT, StateT >::getIterationNum | ( | ) | const [inline] |
get the number of iteration.
Definition at line 101 of file particle_filter.h.
unsigned int pcl::tracking::ParticleFilterTracker< PointInT, StateT >::getMinPointsOfChangeDetection | ( | ) | [inline] |
get the minimum amount of points required within leaf node to become serialized in change detection
Definition at line 288 of file particle_filter.h.
double pcl::tracking::ParticleFilterTracker< PointInT, StateT >::getMotionRatio | ( | ) | [inline] |
get the motion ratio
Definition at line 255 of file particle_filter.h.
int pcl::tracking::ParticleFilterTracker< PointInT, StateT >::getParticleNum | ( | ) | const [inline] |
get the number of the particles.
Definition at line 111 of file particle_filter.h.
PointCloudStatePtr pcl::tracking::ParticleFilterTracker< PointInT, StateT >::getParticles | ( | ) | const [inline] |
get a pointer to a pointcloud of the particles.
Definition at line 211 of file particle_filter.h.
PointCloudInConstPtr const pcl::tracking::ParticleFilterTracker< PointInT, StateT >::getReferenceCloud | ( | ) | [inline] |
get a pointer to a reference dataset to be tracked.
Definition at line 121 of file particle_filter.h.
double pcl::tracking::ParticleFilterTracker< PointInT, StateT >::getResolutionOfChangeDetection | ( | ) | [inline] |
get the resolution of change detection.
Definition at line 285 of file particle_filter.h.
virtual StateT pcl::tracking::ParticleFilterTracker< PointInT, StateT >::getResult | ( | ) | const [inline, virtual] |
Get an instance of the result of tracking.
Implements pcl::tracking::Tracker< PointInT, StateT >.
Definition at line 200 of file particle_filter.h.
Eigen::Affine3f pcl::tracking::ParticleFilterTracker< PointInT, StateT >::getTrans | ( | ) | const [inline] |
get the transformation from the world coordinates to the frame of the particles.
Definition at line 197 of file particle_filter.h.
bool pcl::tracking::ParticleFilterTracker< PointInT, StateT >::getUseChangeDetector | ( | ) | [inline] |
get the value of use_change_detector_.
Definition at line 247 of file particle_filter.h.
bool pcl::tracking::ParticleFilterTracker< PointInT, StateT >::getUseNormal | ( | ) | [inline] |
get the value of use_normal_.
Definition at line 239 of file particle_filter.h.
bool pcl::tracking::ParticleFilterTracker< PointInT, StateT >::initCompute | ( | ) | [protected, virtual] |
This method should get called before starting the actual computation.
Reimplemented from pcl::tracking::Tracker< PointInT, StateT >.
Reimplemented in pcl::tracking::KLDAdaptiveParticleFilterTracker< PointInT, StateT >.
Definition at line 11 of file particle_filter.hpp.
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::initParticles | ( | bool | reset | ) | [protected] |
initialize the particles. initial_noise_covariance_ and initial_noise_mean_ are used for gausiaan sampling.
Definition at line 92 of file particle_filter.hpp.
double pcl::tracking::ParticleFilterTracker< PointInT, StateT >::normalizeParticleWeight | ( | double | w, |
double | w_min, | ||
double | w_max | ||
) | [inline] |
normalize the weight of a particle using exp(1- alpha ( w - w_{min}) / (w_max - w_min)). this method is described in [P.Azad et. al, ICRA11].
w | the weight to be normalized |
w_min | the minimum weight of the particles |
w_max | the maximum weight of the particles |
Definition at line 220 of file particle_filter.h.
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::normalizeWeight | ( | ) | [protected, virtual] |
normalize the weights of all the particels.
Definition at line 117 of file particle_filter.hpp.
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::resample | ( | ) | [protected, virtual] |
resampling phase of particle filter method. sampling the particles according to the weights calculated in weight method. in particular, "sample with replacement" is archieved by walker's alias method.
Reimplemented in pcl::tracking::KLDAdaptiveParticleFilterTracker< PointInT, StateT >.
Definition at line 326 of file particle_filter.hpp.
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::resampleDeterministic | ( | ) | [protected] |
resampling the particle in deterministic way
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::resampleWithReplacement | ( | ) | [protected] |
resampling the particle with replacement
Definition at line 332 of file particle_filter.hpp.
virtual void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::resetTracking | ( | ) | [inline, virtual] |
reset the particles to restart tracking
Definition at line 298 of file particle_filter.h.
int pcl::tracking::ParticleFilterTracker< PointInT, StateT >::sampleWithReplacement | ( | const std::vector< int > & | a, |
const std::vector< double > & | q | ||
) | [protected] |
implementation of "sample with replacement" using Walker's alias method. about Walker's alias method, you can check the paper below: {355749, author = {Walker, Alastair J.}, title = {An Efficient Method for Generating Discrete Random Variables with General Distributions}, journal = {ACM Trans. Math. Softw.}, volume = {3}, number = {3}, year = {1977}, issn = {0098-3500}, pages = {253--256}, doi = {http://doi.acm.org/10.1145/355744.355749}, publisher = {ACM}, address = {New York, NY, USA}, }
a | an alias table, which generated by genAliasTable. |
q | a table of weight, which generated by genAliasTable. |
Definition at line 41 of file particle_filter.hpp.
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::setAlpha | ( | double | alpha | ) | [inline] |
set the value of alpha.
alpha | the value of alpha |
Definition at line 228 of file particle_filter.h.
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::setCloudCoherence | ( | const CloudCoherencePtr & | coherence | ) | [inline] |
set the PointCloudCoherence as likelihood.
coherence | a pointer to PointCloudCoherence. |
Definition at line 127 of file particle_filter.h.
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::setInitialNoiseCovariance | ( | const std::vector< double > & | initial_noise_covariance | ) | [inline] |
set the covariance of the initial noise. it will be used when initializing the particles.
initial_noise_covariance | the diagonal elements of covariance matrix of initial noise. |
Definition at line 148 of file particle_filter.h.
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::setInitialNoiseMean | ( | const std::vector< double > & | initial_noise_mean | ) | [inline] |
set the mean of the initial noise. it will be used when initializing the particles.
initial_noise_mean | the mean values of initial noise. |
Definition at line 158 of file particle_filter.h.
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::setIntervalOfChangeDetection | ( | unsigned int | change_detector_interval | ) | [inline] |
set the number of interval frames to run change detection.
change_detector_interval | the number of interval frames. |
Definition at line 260 of file particle_filter.h.
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::setIterationNum | ( | const int | iteration_num | ) | [inline] |
set the number of iteration.
iteration_num | the number of iteration. |
Definition at line 97 of file particle_filter.h.
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::setMinIndices | ( | const int | min_indices | ) | [inline] |
set the minimum number of indices (default: 1). ParticleFilterTracker does not take into account the hypothesis whose the number of points is smaller than the minimum indices.
min_indices | the minimum number of indices. |
Definition at line 189 of file particle_filter.h.
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::setMinPointsOfChangeDetection | ( | unsigned int | change_detector_filter | ) | [inline] |
set the minimum amount of points required within leaf node to become serialized in change detection
change_detector_filter | the minimum amount of points required within leaf node |
Definition at line 274 of file particle_filter.h.
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::setMotionRatio | ( | double | motion_ratio | ) | [inline] |
set the motion ratio
motion_ratio | the ratio of hypothesis to use motion model. |
Definition at line 252 of file particle_filter.h.
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::setOcclusionAngleThe | ( | const double | occlusion_angle_thr | ) | [inline] |
set the threshold of angle to be considered occlusion (default: pi/2). ParticleFilterTracker does not take the occluded points into account according to the angle between the normal and the position.
occlusion_angle_thr | threshold of angle to be considered occlusion. |
Definition at line 178 of file particle_filter.h.
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::setParticleNum | ( | const int | particle_num | ) | [inline] |
set the number of the particles.
particle_num | the number of the particles. |
Definition at line 107 of file particle_filter.h.
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::setReferenceCloud | ( | const PointCloudInConstPtr & | ref | ) | [inline] |
set a pointer to a reference dataset to be tracked.
cloud | a pointer to a PointCloud message |
Definition at line 117 of file particle_filter.h.
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::setResampleLikelihoodThr | ( | const double | resample_likelihood_thr | ) | [inline] |
set the threshold to re-initialize the particles.
resample_likelihood_thr | threshold to re-initialize. |
Definition at line 167 of file particle_filter.h.
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::setResolutionOfChangeDetection | ( | double | resolution | ) | [inline] |
set the resolution of change detection.
resolution | resolution of change detection octree |
Definition at line 282 of file particle_filter.h.
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::setStepNoiseCovariance | ( | const std::vector< double > & | step_noise_covariance | ) | [inline] |
set the covariance of step noise.
step_noise_covariance | the diagonal elements of covariance matrix of step noise. |
Definition at line 138 of file particle_filter.h.
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::setTrans | ( | const Eigen::Affine3f & | trans | ) | [inline] |
set the transformation from the world coordinates to the frame of the particles.
trans | Affine transformation from the worldcoordinates to the frame of the particles. |
Definition at line 194 of file particle_filter.h.
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::setUseChangeDetector | ( | bool | use_change_detector | ) | [inline] |
set the value of use_change_detector_.
use_normal | the value of use_change_detector_. |
Definition at line 244 of file particle_filter.h.
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::setUseNormal | ( | bool | use_normal | ) | [inline] |
set the value of use_normal_.
use_normal | the value of use_normal_. |
Definition at line 236 of file particle_filter.h.
bool pcl::tracking::ParticleFilterTracker< PointInT, StateT >::testChangeDetection | ( | const PointCloudInConstPtr & | input | ) | [protected] |
run change detection and return true if there is a change.
input | a pointer to the input pointcloud. |
Definition at line 220 of file particle_filter.hpp.
Eigen::Affine3f pcl::tracking::ParticleFilterTracker< PointInT, StateT >::toEigenMatrix | ( | const StateT & | particle | ) | [inline] |
convert a state to affine transformation from the world coordinates frame.
particle | an instance of StateT. |
Definition at line 205 of file particle_filter.h.
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::update | ( | ) | [protected, virtual] |
calculate the weighted mean of the particles and set it as the result
Definition at line 370 of file particle_filter.hpp.
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::weight | ( | ) | [protected, virtual] |
weighting phase of particle filter method. calculate the likelihood of all of the particles and set the weights.
Reimplemented in pcl::tracking::KLDAdaptiveParticleFilterOMPTracker< PointInT, StateT >, and pcl::tracking::ParticleFilterOMPTracker< PointInT, StateT >.
Definition at line 231 of file particle_filter.hpp.
double pcl::tracking::ParticleFilterTracker< PointInT, StateT >::alpha_ [protected] |
the weight to be used in normalization of the weights of the particles
Definition at line 471 of file particle_filter.h.
unsigned int pcl::tracking::ParticleFilterTracker< PointInT, StateT >::change_counter_ [protected] |
a counter to skip change detection
Definition at line 505 of file particle_filter.h.
boost::shared_ptr<pcl::octree::OctreePointCloudChangeDetector<PointInT> > pcl::tracking::ParticleFilterTracker< PointInT, StateT >::change_detector_ [protected] |
change detector used as a trigger to track
Definition at line 499 of file particle_filter.h.
unsigned int pcl::tracking::ParticleFilterTracker< PointInT, StateT >::change_detector_filter_ [protected] |
minimum points in a leaf when calling change detector. defaults to 10
Definition at line 508 of file particle_filter.h.
unsigned int pcl::tracking::ParticleFilterTracker< PointInT, StateT >::change_detector_interval_ [protected] |
the number of interval frame to run change detection. defaults to 10.
Definition at line 511 of file particle_filter.h.
double pcl::tracking::ParticleFilterTracker< PointInT, StateT >::change_detector_resolution_ [protected] |
resolution of change detector. defaults to 0.01.
Definition at line 514 of file particle_filter.h.
bool pcl::tracking::ParticleFilterTracker< PointInT, StateT >::changed_ [protected] |
a flag to be true when change of pointclouds is detected
Definition at line 502 of file particle_filter.h.
CloudCoherencePtr pcl::tracking::ParticleFilterTracker< PointInT, StateT >::coherence_ [protected] |
a pointer to PointCloudCoherence.
Definition at line 448 of file particle_filter.h.
double pcl::tracking::ParticleFilterTracker< PointInT, StateT >::fit_ratio_ [protected] |
adjustment of the particle filter.
Definition at line 439 of file particle_filter.h.
std::vector<double> pcl::tracking::ParticleFilterTracker< PointInT, StateT >::initial_noise_covariance_ [protected] |
the diagonal elements of covariance matrix of the initial noise. the covariance matrix is used when initialize the particles.
Definition at line 458 of file particle_filter.h.
std::vector<double> pcl::tracking::ParticleFilterTracker< PointInT, StateT >::initial_noise_mean_ [protected] |
the mean values of initial noise.
Definition at line 461 of file particle_filter.h.
int pcl::tracking::ParticleFilterTracker< PointInT, StateT >::iteration_num_ [protected] |
the number of iteration of particlefilter.
Definition at line 430 of file particle_filter.h.
int pcl::tracking::ParticleFilterTracker< PointInT, StateT >::min_indices_ [protected] |
the minimum number of points which the hypothesis should have.
Definition at line 436 of file particle_filter.h.
StateT pcl::tracking::ParticleFilterTracker< PointInT, StateT >::motion_ [protected] |
difference between the result in t and t-1
Definition at line 483 of file particle_filter.h.
double pcl::tracking::ParticleFilterTracker< PointInT, StateT >::motion_ratio_ [protected] |
ratio of hypothesis to use motion model
Definition at line 486 of file particle_filter.h.
double pcl::tracking::ParticleFilterTracker< PointInT, StateT >::occlusion_angle_thr_ [protected] |
the threshold for the points to be considered as occluded
Definition at line 467 of file particle_filter.h.
int pcl::tracking::ParticleFilterTracker< PointInT, StateT >::particle_num_ [protected] |
the number of the particles.
Definition at line 433 of file particle_filter.h.
PointCloudStatePtr pcl::tracking::ParticleFilterTracker< PointInT, StateT >::particles_ [protected] |
a pointer to the particles
Definition at line 445 of file particle_filter.h.
pcl::PassThrough<PointInT> pcl::tracking::ParticleFilterTracker< PointInT, StateT >::pass_x_ [protected] |
pass through filter to crop the pointclouds within the hypothesis bounding box
Definition at line 489 of file particle_filter.h.
pcl::PassThrough<PointInT> pcl::tracking::ParticleFilterTracker< PointInT, StateT >::pass_y_ [protected] |
pass through filter to crop the pointclouds within the hypothesis bounding box
Definition at line 491 of file particle_filter.h.
pcl::PassThrough<PointInT> pcl::tracking::ParticleFilterTracker< PointInT, StateT >::pass_z_ [protected] |
pass through filter to crop the pointclouds within the hypothesis bounding box
Definition at line 493 of file particle_filter.h.
PointCloudInConstPtr pcl::tracking::ParticleFilterTracker< PointInT, StateT >::ref_ [protected] |
a pointer to reference point cloud.
Definition at line 442 of file particle_filter.h.
StateT pcl::tracking::ParticleFilterTracker< PointInT, StateT >::representative_state_ [protected] |
the result of tracking.
Definition at line 474 of file particle_filter.h.
double pcl::tracking::ParticleFilterTracker< PointInT, StateT >::resample_likelihood_thr_ [protected] |
the threshold for the particles to be re-initialized
Definition at line 464 of file particle_filter.h.
std::vector<double> pcl::tracking::ParticleFilterTracker< PointInT, StateT >::step_noise_covariance_ [protected] |
the diagonal elements of covariance matrix of the step noise. the covariance matrix is used at every resample method.
Definition at line 453 of file particle_filter.h.
Eigen::Affine3f pcl::tracking::ParticleFilterTracker< PointInT, StateT >::trans_ [protected] |
an affine transformation from the world coordinates frame to the origin of the particles
Definition at line 477 of file particle_filter.h.
std::vector<PointCloudInPtr> pcl::tracking::ParticleFilterTracker< PointInT, StateT >::transed_reference_vector_ [protected] |
a list of the pointers to pointclouds
Definition at line 496 of file particle_filter.h.
bool pcl::tracking::ParticleFilterTracker< PointInT, StateT >::use_change_detector_ [protected] |
the flag which will be true if using change detection
Definition at line 517 of file particle_filter.h.
bool pcl::tracking::ParticleFilterTracker< PointInT, StateT >::use_normal_ [protected] |
a flag to use normal or not. defaults to false
Definition at line 480 of file particle_filter.h.