pcl::tracking::ParticleFilterTracker< PointInT, StateT > Class Template Reference

ParticleFilterTracker tracks the PointCloud which is given by setReferenceCloud within the measured PointCloud using particle filter method. More...

#include <pcl/tracking/particle_filter.h>

Inheritance diagram for pcl::tracking::ParticleFilterTracker< PointInT, StateT >:
Inheritance graph
[legend]
Collaboration diagram for pcl::tracking::ParticleFilterTracker< PointInT, StateT >:
Collaboration graph
[legend]

List of all members.

Public Types

typedef Tracker< PointInT, StateT > BaseClass
typedef Tracker< PointInT,
StateT >::PointCloudIn 
PointCloudIn
typedef PointCloudIn::Ptr PointCloudInPtr
typedef PointCloudIn::ConstPtr PointCloudInConstPtr
typedef Tracker< PointInT,
StateT >::PointCloudState 
PointCloudState
typedef PointCloudState::Ptr PointCloudStatePtr
typedef PointCloudState::ConstPtr PointCloudStateConstPtr
typedef PointCoherence< PointInT > Coherence
typedef boost::shared_ptr
< Coherence
CoherencePtr
typedef boost::shared_ptr
< const Coherence
CoherenceConstPtr
typedef PointCloudCoherence
< PointInT > 
CloudCoherence
typedef boost::shared_ptr
< CloudCoherence
CloudCoherencePtr
typedef boost::shared_ptr
< const CloudCoherence
CloudCoherenceConstPtr

Public Member Functions

 ParticleFilterTracker ()
 Empty constructor.
void setIterationNum (const int iteration_num)
 Set the number of iteration.
int getIterationNum () const
 Get the number of iteration.
void setParticleNum (const int particle_num)
 Set the number of the particles.
int getParticleNum () const
 Get the number of the particles.
void setReferenceCloud (const PointCloudInConstPtr &ref)
 Set a pointer to a reference dataset to be tracked.
PointCloudInConstPtr const getReferenceCloud ()
 Get a pointer to a reference dataset to be tracked.
void setCloudCoherence (const CloudCoherencePtr &coherence)
 Set the PointCloudCoherence as likelihood.
CloudCoherencePtr getCloudCoherence () const
 Get the PointCloudCoherence to compute likelihood.
void setStepNoiseCovariance (const std::vector< double > &step_noise_covariance)
 Set the covariance of step noise.
void setInitialNoiseCovariance (const std::vector< double > &initial_noise_covariance)
 Set the covariance of the initial noise.
void setInitialNoiseMean (const std::vector< double > &initial_noise_mean)
 Set the mean of the initial noise.
void setResampleLikelihoodThr (const double resample_likelihood_thr)
 Set the threshold to re-initialize the particles.
void setOcclusionAngleThe (const double occlusion_angle_thr)
 Set the threshold of angle to be considered occlusion (default: pi/2).
void setMinIndices (const int min_indices)
 Set the minimum number of indices (default: 1).
void setTrans (const Eigen::Affine3f &trans)
 Set the transformation from the world coordinates to the frame of the particles.
Eigen::Affine3f getTrans () const
 Get the transformation from the world coordinates to the frame of the particles.
virtual StateT getResult () const
 Get an instance of the result of tracking.
Eigen::Affine3f toEigenMatrix (const StateT &particle)
 Convert a state to affine transformation from the world coordinates frame.
PointCloudStatePtr getParticles () const
 Get a pointer to a pointcloud of the particles.
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)) $.
void setAlpha (double alpha)
 Set the value of alpha.
double getAlpha ()
 Get the value of alpha.
void setUseNormal (bool use_normal)
 Set the value of use_normal_.
bool getUseNormal ()
 Get the value of use_normal_.
void setUseChangeDetector (bool use_change_detector)
 Set the value of use_change_detector_.
bool getUseChangeDetector ()
 Get the value of use_change_detector_.
void setMotionRatio (double motion_ratio)
 Set the motion ratio.
double getMotionRatio ()
 Get the motion ratio.
void setIntervalOfChangeDetection (unsigned int change_detector_interval)
 Set the number of interval frames to run change detection.
unsigned int getIntervalOfChangeDetection ()
 Get the number of interval frames to run change detection.
void setMinPointsOfChangeDetection (unsigned int change_detector_filter)
 Set the minimum amount of points required within leaf node to become serialized in change detection.
void setResolutionOfChangeDetection (double resolution)
 Set the resolution of change detection.
double getResolutionOfChangeDetection ()
 Get the resolution of change detection.
unsigned int getMinPointsOfChangeDetection ()
 Get the minimum amount of points required within leaf node to become serialized in change detection.
double getFitRatio () const
 Get the adjustment ratio.
virtual void resetTracking ()
 Reset the particles to restart tracking.

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.
void cropInputPointCloud (const PointCloudInConstPtr &cloud, PointCloudIn &output)
 Crop the pointcloud by the bounding box calculated from hypothesis and the reference pointcloud.
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.
virtual bool initCompute ()
 This method should get called before starting the actual computation.
virtual void weight ()
 Weighting phase of particle filter method.
virtual void resample ()
 Resampling phase of particle filter method.
virtual void update ()
 Calculate the weighted mean of the particles and set it as the result.
virtual void normalizeWeight ()
 Normalize the weights of all the particels.
void initParticles (bool reset)
 Initialize the particles.
virtual void computeTracking ()
 Track the pointcloud using particle filter method.
int sampleWithReplacement (const std::vector< int > &a, const std::vector< double > &q)
 Implementation of "sample with replacement" using Walker's alias method.
void genAliasTable (std::vector< int > &a, std::vector< double > &q, const PointCloudStateConstPtr &particles)
 Generate the tables for walker's alias method.
void resampleWithReplacement ()
 Resampling the particle with replacement.
void resampleDeterministic ()
 Resampling the particle in deterministic way.
bool testChangeDetection (const PointCloudInConstPtr &input)
 Run change detection and return true if there is a change.

Protected Attributes

int iteration_num_
 The number of iteration of particlefilter.
int particle_num_
 The number of the particles.
int min_indices_
 The minimum number of points which the hypothesis should have.
double fit_ratio_
 Adjustment of the particle filter.
PointCloudInConstPtr ref_
 A pointer to reference point cloud.
PointCloudStatePtr particles_
 A pointer to the particles.
CloudCoherencePtr coherence_
 A pointer to PointCloudCoherence.
std::vector< double > step_noise_covariance_
 The diagonal elements of covariance matrix of the step noise.
std::vector< double > initial_noise_covariance_
 The diagonal elements of covariance matrix of the initial noise.
std::vector< double > initial_noise_mean_
 The mean values of initial noise.
double resample_likelihood_thr_
 The threshold for the particles to be re-initialized.
double occlusion_angle_thr_
 The threshold for the points to be considered as occluded.
double alpha_
 The weight to be used in normalization of the weights of the particles.
StateT representative_state_
 The result of tracking.
Eigen::Affine3f trans_
 An affine transformation from the world coordinates frame to the origin of the particles.
bool use_normal_
 A flag to use normal or not.
StateT motion_
 Difference between the result in t and t-1.
double motion_ratio_
 Ratio of hypothesis to use motion model.
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.
std::vector< PointCloudInPtrtransed_reference_vector_
 A list of the pointers to pointclouds.
boost::shared_ptr
< pcl::octree::OctreePointCloudChangeDetector
< PointInT > > 
change_detector_
 Change detector used as a trigger to track.
bool changed_
 A flag to be true when change of pointclouds is detected.
unsigned int change_counter_
 A counter to skip change detection.
unsigned int change_detector_filter_
 Minimum points in a leaf when calling change detector.
unsigned int change_detector_interval_
 The number of interval frame to run change detection.
double change_detector_resolution_
 Resolution of change detector.
bool use_change_detector_
 The flag which will be true if using change detection.

Detailed Description

template<typename PointInT, typename StateT>
class pcl::tracking::ParticleFilterTracker< PointInT, StateT >

ParticleFilterTracker tracks the PointCloud which is given by setReferenceCloud within the measured PointCloud using particle filter method.

Author:
Ryohei Ueda

Definition at line 23 of file particle_filter.h.


Member Typedef Documentation

template<typename PointInT, typename StateT>
typedef Tracker<PointInT, StateT> pcl::tracking::ParticleFilterTracker< PointInT, StateT >::BaseClass
template<typename PointInT, typename StateT>
typedef PointCloudCoherence<PointInT> pcl::tracking::ParticleFilterTracker< PointInT, StateT >::CloudCoherence
template<typename PointInT, typename StateT>
typedef boost::shared_ptr< const CloudCoherence > pcl::tracking::ParticleFilterTracker< PointInT, StateT >::CloudCoherenceConstPtr
template<typename PointInT, typename StateT>
typedef boost::shared_ptr< CloudCoherence > pcl::tracking::ParticleFilterTracker< PointInT, StateT >::CloudCoherencePtr
template<typename PointInT, typename StateT>
typedef PointCoherence<PointInT> pcl::tracking::ParticleFilterTracker< PointInT, StateT >::Coherence
template<typename PointInT, typename StateT>
typedef boost::shared_ptr< const Coherence > pcl::tracking::ParticleFilterTracker< PointInT, StateT >::CoherenceConstPtr
template<typename PointInT, typename StateT>
typedef boost::shared_ptr< Coherence > pcl::tracking::ParticleFilterTracker< PointInT, StateT >::CoherencePtr
template<typename PointInT, typename StateT>
typedef Tracker<PointInT, StateT>::PointCloudIn pcl::tracking::ParticleFilterTracker< PointInT, StateT >::PointCloudIn
template<typename PointInT, typename StateT>
typedef PointCloudIn::ConstPtr pcl::tracking::ParticleFilterTracker< PointInT, StateT >::PointCloudInConstPtr
template<typename PointInT, typename StateT>
typedef PointCloudIn::Ptr pcl::tracking::ParticleFilterTracker< PointInT, StateT >::PointCloudInPtr
template<typename PointInT, typename StateT>
typedef Tracker<PointInT, StateT>::PointCloudState pcl::tracking::ParticleFilterTracker< PointInT, StateT >::PointCloudState
template<typename PointInT, typename StateT>
typedef PointCloudState::ConstPtr pcl::tracking::ParticleFilterTracker< PointInT, StateT >::PointCloudStateConstPtr
template<typename PointInT, typename StateT>
typedef PointCloudState::Ptr pcl::tracking::ParticleFilterTracker< PointInT, StateT >::PointCloudStatePtr

Constructor & Destructor Documentation

template<typename PointInT, typename StateT>
pcl::tracking::ParticleFilterTracker< PointInT, StateT >::ParticleFilterTracker (  )  [inline]

Member Function Documentation

template<typename PointInT , typename StateT >
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 
) [inline, protected]

Compute the parameters for the bounding box of hypothesis pointclouds.

Parameters:
[out] x_min the minimum value of x axis.
[out] x_max the maximum value of x axis.
[out] y_min the minimum value of y axis.
[out] y_max the maximum value of y axis.
[out] z_min the minimum value of z axis.
[out] z_max the maximum value of z axis.

Definition at line 192 of file particle_filter.hpp.

References pcl::getMinMax3D(), and pcl::tracking::ParticleFilterTracker< PointInT, StateT >::transed_reference_vector_.

Referenced by pcl::tracking::ParticleFilterTracker< PointInT, StateT >::cropInputPointCloud().

template<typename PointInT , typename StateT >
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::computeTracking (  )  [inline, protected, virtual]
template<typename PointInT , typename StateT >
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::computeTransformedPointCloud ( const StateT &  hypothesis,
std::vector< int > &  indices,
PointCloudIn cloud 
) [inline, protected]

Compute a reference pointcloud transformed to the pose that hypothesis represents.

Parameters:
[in] hypothesis a particle which represents a hypothesis.
[in] indices the indices which should be taken into account.
[out] cloud the resultant point cloud model dataset which is transformed to hypothesis.

Definition at line 276 of file particle_filter.hpp.

template<typename PointInT , typename StateT >
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::computeTransformedPointCloudWithNormal ( const StateT &  hypothesis,
std::vector< int > &  indices,
PointCloudIn cloud 
) [inline, protected]

Compute a reference pointcloud transformed to the pose that hypothesis represents and calculate indices taking occlusion into account.

Parameters:
[in] hypothesis a particle which represents a hypothesis.
[in] indices the indices which should be taken into account.
[out] cloud the resultant point cloud model dataset which is transformed to hypothesis.

Definition at line 295 of file particle_filter.hpp.

References pcl::getAngle3D().

Referenced by pcl::tracking::ParticleFilterOMPTracker< PointInT, StateT >::weight(), pcl::tracking::ParticleFilterTracker< PointInT, StateT >::weight(), and pcl::tracking::KLDAdaptiveParticleFilterOMPTracker< PointInT, StateT >::weight().

template<typename PointInT , typename StateT >
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::computeTransformedPointCloudWithoutNormal ( const StateT &  hypothesis,
PointCloudIn cloud 
) [inline, protected]

Compute a reference pointcloud transformed to the pose that hypothesis represents and calculate indices without taking occlusion into account.

Parameters:
[in] hypothesis a particle which represents a hypothesis.
[out] cloud the resultant point cloud model dataset which is transformed to hypothesis.

Definition at line 286 of file particle_filter.hpp.

Referenced by pcl::tracking::ParticleFilterOMPTracker< PointInT, StateT >::weight(), pcl::tracking::ParticleFilterTracker< PointInT, StateT >::weight(), and pcl::tracking::KLDAdaptiveParticleFilterOMPTracker< PointInT, StateT >::weight().

template<typename PointInT , typename StateT >
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::cropInputPointCloud ( const PointCloudInConstPtr cloud,
PointCloudIn output 
) [inline, protected]
template<typename PointInT , typename StateT >
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::genAliasTable ( std::vector< int > &  a,
std::vector< double > &  q,
const PointCloudStateConstPtr particles 
) [inline, protected]
template<typename PointInT, typename StateT>
double pcl::tracking::ParticleFilterTracker< PointInT, StateT >::getAlpha (  )  [inline]

Get the value of alpha.

Definition at line 231 of file particle_filter.h.

References pcl::tracking::ParticleFilterTracker< PointInT, StateT >::alpha_.

template<typename PointInT, typename StateT>
CloudCoherencePtr pcl::tracking::ParticleFilterTracker< PointInT, StateT >::getCloudCoherence (  )  const [inline]

Get the PointCloudCoherence to compute likelihood.

Definition at line 131 of file particle_filter.h.

References pcl::tracking::ParticleFilterTracker< PointInT, StateT >::coherence_.

template<typename PointInT, typename StateT>
double pcl::tracking::ParticleFilterTracker< PointInT, StateT >::getFitRatio (  )  const [inline]

Get the adjustment ratio.

Definition at line 295 of file particle_filter.h.

References pcl::tracking::ParticleFilterTracker< PointInT, StateT >::fit_ratio_.

template<typename PointInT, typename StateT>
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.

References pcl::tracking::ParticleFilterTracker< PointInT, StateT >::change_detector_interval_.

template<typename PointInT, typename StateT>
int pcl::tracking::ParticleFilterTracker< PointInT, StateT >::getIterationNum (  )  const [inline]

Get the number of iteration.

Definition at line 101 of file particle_filter.h.

References pcl::tracking::ParticleFilterTracker< PointInT, StateT >::iteration_num_.

template<typename PointInT, typename StateT>
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.

References pcl::tracking::ParticleFilterTracker< PointInT, StateT >::change_detector_filter_.

template<typename PointInT, typename StateT>
double pcl::tracking::ParticleFilterTracker< PointInT, StateT >::getMotionRatio (  )  [inline]

Get the motion ratio.

Definition at line 255 of file particle_filter.h.

References pcl::tracking::ParticleFilterTracker< PointInT, StateT >::motion_ratio_.

template<typename PointInT, typename StateT>
int pcl::tracking::ParticleFilterTracker< PointInT, StateT >::getParticleNum (  )  const [inline]

Get the number of the particles.

Definition at line 111 of file particle_filter.h.

References pcl::tracking::ParticleFilterTracker< PointInT, StateT >::particle_num_.

template<typename PointInT, typename StateT>
PointCloudStatePtr pcl::tracking::ParticleFilterTracker< PointInT, StateT >::getParticles (  )  const [inline]

Get a pointer to a pointcloud of the particles.

Definition at line 212 of file particle_filter.h.

References pcl::tracking::ParticleFilterTracker< PointInT, StateT >::particles_.

template<typename PointInT, typename StateT>
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.

References pcl::tracking::ParticleFilterTracker< PointInT, StateT >::ref_.

template<typename PointInT, typename StateT>
double pcl::tracking::ParticleFilterTracker< PointInT, StateT >::getResolutionOfChangeDetection (  )  [inline]

Get the resolution of change detection.

Definition at line 285 of file particle_filter.h.

References pcl::tracking::ParticleFilterTracker< PointInT, StateT >::change_detector_resolution_.

template<typename PointInT, typename StateT>
virtual StateT pcl::tracking::ParticleFilterTracker< PointInT, StateT >::getResult (  )  const [inline, virtual]

Get an instance of the result of tracking.

This function returns the particle that represents the transform between the reference point cloud at the beginning and the best guess about its location in the most recent frame.

Implements pcl::tracking::Tracker< PointInT, StateT >.

Definition at line 201 of file particle_filter.h.

References pcl::tracking::ParticleFilterTracker< PointInT, StateT >::representative_state_.

template<typename PointInT, typename StateT>
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 195 of file particle_filter.h.

References pcl::tracking::ParticleFilterTracker< PointInT, StateT >::trans_.

template<typename PointInT, typename StateT>
bool pcl::tracking::ParticleFilterTracker< PointInT, StateT >::getUseChangeDetector (  )  [inline]

Get the value of use_change_detector_.

Definition at line 247 of file particle_filter.h.

References pcl::tracking::ParticleFilterTracker< PointInT, StateT >::use_change_detector_.

template<typename PointInT, typename StateT>
bool pcl::tracking::ParticleFilterTracker< PointInT, StateT >::getUseNormal (  )  [inline]

Get the value of use_normal_.

Definition at line 239 of file particle_filter.h.

References pcl::tracking::ParticleFilterTracker< PointInT, StateT >::use_normal_.

template<typename PointInT , typename StateT >
bool pcl::tracking::ParticleFilterTracker< PointInT, StateT >::initCompute (  )  [inline, protected, virtual]
template<typename PointInT , typename StateT >
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::initParticles ( bool  reset  )  [inline, protected]
template<typename PointInT, typename StateT>
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)) $.

Note:
This method is described in [P.Azad et. al, ICRA11].
Parameters:
[in] w the weight to be normalized
[in] w_min the minimum weight of the particles
[in] w_max the maximum weight of the particles

Definition at line 220 of file particle_filter.h.

References pcl::tracking::ParticleFilterTracker< PointInT, StateT >::alpha_.

Referenced by pcl::tracking::ParticleFilterTracker< PointInT, StateT >::normalizeWeight().

template<typename PointInT , typename StateT >
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::normalizeWeight (  )  [inline, protected, virtual]
template<typename PointInT , typename StateT >
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::resample (  )  [inline, 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.

References pcl::tracking::ParticleFilterTracker< PointInT, StateT >::resampleWithReplacement().

Referenced by pcl::tracking::ParticleFilterTracker< PointInT, StateT >::computeTracking().

template<typename PointInT, typename StateT>
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::resampleDeterministic (  )  [protected]

Resampling the particle in deterministic way.

template<typename PointInT , typename StateT >
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::resampleWithReplacement (  )  [inline, protected]
template<typename PointInT, typename StateT>
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.

References pcl::tracking::ParticleFilterTracker< PointInT, StateT >::particles_.

template<typename PointInT , typename StateT >
int pcl::tracking::ParticleFilterTracker< PointInT, StateT >::sampleWithReplacement ( const std::vector< int > &  a,
const std::vector< double > &  q 
) [inline, 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}, }

Parameters:
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.

Referenced by pcl::tracking::KLDAdaptiveParticleFilterTracker< PointInT, StateT >::resample(), and pcl::tracking::ParticleFilterTracker< PointInT, StateT >::resampleWithReplacement().

template<typename PointInT, typename StateT>
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::setAlpha ( double  alpha  )  [inline]

Set the value of alpha.

Parameters:
[in] alpha the value of alpha

Definition at line 228 of file particle_filter.h.

References pcl::tracking::ParticleFilterTracker< PointInT, StateT >::alpha_.

template<typename PointInT, typename StateT>
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::setCloudCoherence ( const CloudCoherencePtr coherence  )  [inline]

Set the PointCloudCoherence as likelihood.

Parameters:
[in] coherence a pointer to PointCloudCoherence.

Definition at line 127 of file particle_filter.h.

References pcl::tracking::ParticleFilterTracker< PointInT, StateT >::coherence_.

template<typename PointInT, typename StateT>
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.

Parameters:
[in] initial_noise_covariance the diagonal elements of covariance matrix of initial noise.

Definition at line 147 of file particle_filter.h.

References pcl::tracking::ParticleFilterTracker< PointInT, StateT >::initial_noise_covariance_.

template<typename PointInT, typename StateT>
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.

Parameters:
[in] initial_noise_mean the mean values of initial noise.

Definition at line 156 of file particle_filter.h.

References pcl::tracking::ParticleFilterTracker< PointInT, StateT >::initial_noise_mean_.

template<typename PointInT, typename StateT>
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::setIntervalOfChangeDetection ( unsigned int  change_detector_interval  )  [inline]

Set the number of interval frames to run change detection.

Parameters:
[in] change_detector_interval the number of interval frames.

Definition at line 260 of file particle_filter.h.

References pcl::tracking::ParticleFilterTracker< PointInT, StateT >::change_detector_interval_.

template<typename PointInT, typename StateT>
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::setIterationNum ( const int  iteration_num  )  [inline]

Set the number of iteration.

Parameters:
[in] iteration_num the number of iteration.

Definition at line 97 of file particle_filter.h.

References pcl::tracking::ParticleFilterTracker< PointInT, StateT >::iteration_num_.

template<typename PointInT, typename StateT>
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.

Parameters:
[in] min_indices the minimum number of indices.

Definition at line 187 of file particle_filter.h.

References pcl::tracking::ParticleFilterTracker< PointInT, StateT >::min_indices_.

template<typename PointInT, typename StateT>
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.

Parameters:
[in] change_detector_filter the minimum amount of points required within leaf node

Definition at line 274 of file particle_filter.h.

References pcl::tracking::ParticleFilterTracker< PointInT, StateT >::change_detector_filter_.

template<typename PointInT, typename StateT>
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::setMotionRatio ( double  motion_ratio  )  [inline]

Set the motion ratio.

Parameters:
[in] motion_ratio the ratio of hypothesis to use motion model.

Definition at line 252 of file particle_filter.h.

References pcl::tracking::ParticleFilterTracker< PointInT, StateT >::motion_ratio_.

template<typename PointInT, typename StateT>
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.

Parameters:
[in] occlusion_angle_thr threshold of angle to be considered occlusion.

Definition at line 176 of file particle_filter.h.

References pcl::tracking::ParticleFilterTracker< PointInT, StateT >::occlusion_angle_thr_.

template<typename PointInT, typename StateT>
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::setParticleNum ( const int  particle_num  )  [inline]

Set the number of the particles.

Parameters:
[in] particle_num the number of the particles.

Definition at line 107 of file particle_filter.h.

References pcl::tracking::ParticleFilterTracker< PointInT, StateT >::particle_num_.

template<typename PointInT, typename StateT>
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::setReferenceCloud ( const PointCloudInConstPtr ref  )  [inline]

Set a pointer to a reference dataset to be tracked.

Parameters:
[in] ref a pointer to a PointCloud message

Definition at line 117 of file particle_filter.h.

References pcl::tracking::ParticleFilterTracker< PointInT, StateT >::ref_.

template<typename PointInT, typename StateT>
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::setResampleLikelihoodThr ( const double  resample_likelihood_thr  )  [inline]

Set the threshold to re-initialize the particles.

Parameters:
[in] resample_likelihood_thr threshold to re-initialize.

Definition at line 165 of file particle_filter.h.

References pcl::tracking::ParticleFilterTracker< PointInT, StateT >::resample_likelihood_thr_.

template<typename PointInT, typename StateT>
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::setResolutionOfChangeDetection ( double  resolution  )  [inline]

Set the resolution of change detection.

Parameters:
[in] resolution resolution of change detection octree

Definition at line 282 of file particle_filter.h.

References pcl::tracking::ParticleFilterTracker< PointInT, StateT >::change_detector_resolution_.

template<typename PointInT, typename StateT>
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::setStepNoiseCovariance ( const std::vector< double > &  step_noise_covariance  )  [inline]

Set the covariance of step noise.

Parameters:
[in] step_noise_covariance the diagonal elements of covariance matrix of step noise.

Definition at line 138 of file particle_filter.h.

References pcl::tracking::ParticleFilterTracker< PointInT, StateT >::step_noise_covariance_.

template<typename PointInT, typename StateT>
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.

Parameters:
[in] trans Affine transformation from the worldcoordinates to the frame of the particles.

Definition at line 192 of file particle_filter.h.

References pcl::tracking::ParticleFilterTracker< PointInT, StateT >::trans_.

template<typename PointInT, typename StateT>
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::setUseChangeDetector ( bool  use_change_detector  )  [inline]

Set the value of use_change_detector_.

Parameters:
[in] use_change_detector the value of use_change_detector_.

Definition at line 244 of file particle_filter.h.

References pcl::tracking::ParticleFilterTracker< PointInT, StateT >::use_change_detector_.

template<typename PointInT, typename StateT>
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::setUseNormal ( bool  use_normal  )  [inline]

Set the value of use_normal_.

Parameters:
[in] use_normal the value of use_normal_.

Definition at line 236 of file particle_filter.h.

References pcl::tracking::ParticleFilterTracker< PointInT, StateT >::use_normal_.

template<typename PointInT , typename StateT >
bool pcl::tracking::ParticleFilterTracker< PointInT, StateT >::testChangeDetection ( const PointCloudInConstPtr input  )  [inline, protected]

Run change detection and return true if there is a change.

Parameters:
[in] input a pointer to the input pointcloud.

Definition at line 220 of file particle_filter.hpp.

Referenced by pcl::tracking::ParticleFilterOMPTracker< PointInT, StateT >::weight(), and pcl::tracking::KLDAdaptiveParticleFilterOMPTracker< PointInT, StateT >::weight().

template<typename PointInT, typename StateT>
Eigen::Affine3f pcl::tracking::ParticleFilterTracker< PointInT, StateT >::toEigenMatrix ( const StateT &  particle  )  [inline]

Convert a state to affine transformation from the world coordinates frame.

Parameters:
[in] particle an instance of StateT.

Definition at line 206 of file particle_filter.h.

template<typename PointInT , typename StateT >
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::update (  )  [inline, protected, virtual]
template<typename PointInT , typename StateT >
void pcl::tracking::ParticleFilterTracker< PointInT, StateT >::weight (  )  [inline, protected, virtual]

Member Data Documentation

template<typename PointInT, typename StateT>
double pcl::tracking::ParticleFilterTracker< PointInT, StateT >::alpha_ [protected]
template<typename PointInT, typename StateT>
unsigned int pcl::tracking::ParticleFilterTracker< PointInT, StateT >::change_counter_ [protected]
template<typename PointInT, typename StateT>
boost::shared_ptr<pcl::octree::OctreePointCloudChangeDetector<PointInT> > pcl::tracking::ParticleFilterTracker< PointInT, StateT >::change_detector_ [protected]
template<typename PointInT, typename StateT>
unsigned int pcl::tracking::ParticleFilterTracker< PointInT, StateT >::change_detector_filter_ [protected]
template<typename PointInT, typename StateT>
unsigned int pcl::tracking::ParticleFilterTracker< PointInT, StateT >::change_detector_interval_ [protected]
template<typename PointInT, typename StateT>
double pcl::tracking::ParticleFilterTracker< PointInT, StateT >::change_detector_resolution_ [protected]
template<typename PointInT, typename StateT>
bool pcl::tracking::ParticleFilterTracker< PointInT, StateT >::changed_ [protected]
template<typename PointInT, typename StateT>
CloudCoherencePtr pcl::tracking::ParticleFilterTracker< PointInT, StateT >::coherence_ [protected]
template<typename PointInT, typename StateT>
double pcl::tracking::ParticleFilterTracker< PointInT, StateT >::fit_ratio_ [protected]
template<typename PointInT, typename StateT>
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 444 of file particle_filter.h.

Referenced by pcl::tracking::ParticleFilterTracker< PointInT, StateT >::initParticles(), and pcl::tracking::ParticleFilterTracker< PointInT, StateT >::setInitialNoiseCovariance().

template<typename PointInT, typename StateT>
std::vector<double> pcl::tracking::ParticleFilterTracker< PointInT, StateT >::initial_noise_mean_ [protected]
template<typename PointInT, typename StateT>
int pcl::tracking::ParticleFilterTracker< PointInT, StateT >::iteration_num_ [protected]
template<typename PointInT, typename StateT>
int pcl::tracking::ParticleFilterTracker< PointInT, StateT >::min_indices_ [protected]

The minimum number of points which the hypothesis should have.

Definition at line 422 of file particle_filter.h.

Referenced by pcl::tracking::ParticleFilterTracker< PointInT, StateT >::setMinIndices().

template<typename PointInT, typename StateT>
StateT pcl::tracking::ParticleFilterTracker< PointInT, StateT >::motion_ [protected]
template<typename PointInT, typename StateT>
double pcl::tracking::ParticleFilterTracker< PointInT, StateT >::motion_ratio_ [protected]
template<typename PointInT, typename StateT>
double pcl::tracking::ParticleFilterTracker< PointInT, StateT >::occlusion_angle_thr_ [protected]

The threshold for the points to be considered as occluded.

Definition at line 453 of file particle_filter.h.

Referenced by pcl::tracking::ParticleFilterTracker< PointInT, StateT >::setOcclusionAngleThe().

template<typename PointInT, typename StateT>
int pcl::tracking::ParticleFilterTracker< PointInT, StateT >::particle_num_ [protected]
template<typename PointInT, typename StateT>
PointCloudStatePtr pcl::tracking::ParticleFilterTracker< PointInT, StateT >::particles_ [protected]
template<typename PointInT, typename StateT>
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 474 of file particle_filter.h.

Referenced by pcl::tracking::ParticleFilterTracker< PointInT, StateT >::cropInputPointCloud(), and pcl::tracking::ParticleFilterTracker< PointInT, StateT >::ParticleFilterTracker().

template<typename PointInT, typename StateT>
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 476 of file particle_filter.h.

Referenced by pcl::tracking::ParticleFilterTracker< PointInT, StateT >::cropInputPointCloud(), and pcl::tracking::ParticleFilterTracker< PointInT, StateT >::ParticleFilterTracker().

template<typename PointInT, typename StateT>
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 478 of file particle_filter.h.

Referenced by pcl::tracking::ParticleFilterTracker< PointInT, StateT >::cropInputPointCloud(), and pcl::tracking::ParticleFilterTracker< PointInT, StateT >::ParticleFilterTracker().

template<typename PointInT, typename StateT>
PointCloudInConstPtr pcl::tracking::ParticleFilterTracker< PointInT, StateT >::ref_ [protected]
template<typename PointInT, typename StateT>
StateT pcl::tracking::ParticleFilterTracker< PointInT, StateT >::representative_state_ [protected]
template<typename PointInT, typename StateT>
double pcl::tracking::ParticleFilterTracker< PointInT, StateT >::resample_likelihood_thr_ [protected]

The threshold for the particles to be re-initialized.

Definition at line 450 of file particle_filter.h.

Referenced by pcl::tracking::ParticleFilterTracker< PointInT, StateT >::setResampleLikelihoodThr().

template<typename PointInT, typename StateT>
std::vector<double> pcl::tracking::ParticleFilterTracker< PointInT, StateT >::step_noise_covariance_ [protected]
template<typename PointInT, typename StateT>
Eigen::Affine3f pcl::tracking::ParticleFilterTracker< PointInT, StateT >::trans_ [protected]
template<typename PointInT, typename StateT>
std::vector<PointCloudInPtr> pcl::tracking::ParticleFilterTracker< PointInT, StateT >::transed_reference_vector_ [protected]
template<typename PointInT, typename StateT>
bool pcl::tracking::ParticleFilterTracker< PointInT, StateT >::use_change_detector_ [protected]
template<typename PointInT, typename StateT>
bool pcl::tracking::ParticleFilterTracker< PointInT, StateT >::use_normal_ [protected]

The documentation for this class was generated from the following files:
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends