particle_filter.h
#pragma once
#include <boost/shared_ptr.hpp>
#include <pcl/tracking/tracking.h>
#include <pcl/tracking/tracker.h>
#include <pcl/tracking/coherence.h>
#include <pcl/filters/passthrough.h>
#include <pcl/octree/octree_pointcloud_changedetector.h>
#include <Eigen/Dense>
namespace pcl
{
namespace tracking
{
/** \brief @b ParticleFilterTracker tracks the PointCloud which is given by
setReferenceCloud within the measured PointCloud using particle filter method.
* \author Ryohei Ueda
* \ingroup tracking
*/
template <typename PointInT, typename StateT>
class ParticleFilterTracker: public Tracker<PointInT, StateT>
{
protected:
using Tracker<PointInT, StateT>::deinitCompute;
public:
using Tracker<PointInT, StateT>::tracker_name_;
using Tracker<PointInT, StateT>::search_;
using Tracker<PointInT, StateT>::input_;
using Tracker<PointInT, StateT>::indices_;
using Tracker<PointInT, StateT>::getClassName;
using Ptr = shared_ptr<ParticleFilterTracker<PointInT, StateT>>;
using ConstPtr = shared_ptr<const ParticleFilterTracker<PointInT, StateT>>;
using BaseClass = Tracker<PointInT, StateT>;
using PointCloudIn = typename Tracker<PointInT, StateT>::PointCloudIn;
using PointCloudInPtr = typename PointCloudIn::Ptr;
using PointCloudInConstPtr = typename PointCloudIn::ConstPtr;
using PointCloudState = typename Tracker<PointInT, StateT>::PointCloudState;
using PointCloudStatePtr = typename PointCloudState::Ptr;
using PointCloudStateConstPtr = typename PointCloudState::ConstPtr;
using Coherence = PointCoherence<PointInT>;
using CoherencePtr = typename Coherence::Ptr;
using CoherenceConstPtr = typename Coherence::ConstPtr;
using CloudCoherence = PointCloudCoherence<PointInT>;
using CloudCoherencePtr = typename CloudCoherence::Ptr;
using CloudCoherenceConstPtr = typename CloudCoherence::ConstPtr;
/** \brief Empty constructor. */
ParticleFilterTracker ()
: iteration_num_ (1)
, particle_num_ ()
, min_indices_ (1)
, ref_ ()
, particles_ ()
, coherence_ ()
, resample_likelihood_thr_ (0.0)
, occlusion_angle_thr_ (M_PI / 2.0)
, alpha_ (15.0)
, representative_state_ ()
, use_normal_ (false)
, motion_ ()
, motion_ratio_ (0.25)
, pass_x_ ()
, pass_y_ ()
, pass_z_ ()
, transed_reference_vector_ ()
, change_detector_ ()
, changed_ (false)
, change_counter_ (0)
, change_detector_filter_ (10)
, change_detector_interval_ (10)
, change_detector_resolution_ (0.01)
, use_change_detector_ (false)
{
tracker_name_ = "ParticleFilterTracker";
pass_x_.setFilterFieldName ("x");
pass_y_.setFilterFieldName ("y");
pass_z_.setFilterFieldName ("z");
pass_x_.setKeepOrganized (false);
pass_y_.setKeepOrganized (false);
pass_z_.setKeepOrganized (false);
}
/** \brief Set the number of iteration.
* \param[in] iteration_num the number of iteration.
*/
inline void
setIterationNum (const int iteration_num) { iteration_num_ = iteration_num; }
/** \brief Get the number of iteration. */
inline int
getIterationNum () const { return iteration_num_; }
/** \brief Set the number of the particles.
* \param[in] particle_num the number of the particles.
*/
inline void
setParticleNum (const int particle_num) { particle_num_ = particle_num; }
/** \brief Get the number of the particles. */
inline int
getParticleNum () const { return particle_num_; }
/** \brief Set a pointer to a reference dataset to be tracked.
* \param[in] ref a pointer to a PointCloud message
*/
inline void
setReferenceCloud (const PointCloudInConstPtr &ref) { ref_ = ref; }
/** \brief Get a pointer to a reference dataset to be tracked. */
inline PointCloudInConstPtr const
getReferenceCloud () { return ref_; }
/** \brief Set the PointCloudCoherence as likelihood.
* \param[in] coherence a pointer to PointCloudCoherence.
*/
inline void
setCloudCoherence (const CloudCoherencePtr &coherence) { coherence_ = coherence; }
/** \brief Get the PointCloudCoherence to compute likelihood. */
inline CloudCoherencePtr
getCloudCoherence () const { return coherence_; }
/** \brief Set the covariance of step noise.
* \param[in] step_noise_covariance the diagonal elements of covariance matrix of step noise.
*/
inline void
setStepNoiseCovariance (const std::vector<double> &step_noise_covariance)
{
step_noise_covariance_ = step_noise_covariance;
}
/** \brief Set the covariance of the initial noise. It will be used when initializing the particles.
* \param[in] initial_noise_covariance the diagonal elements of covariance matrix of initial noise.
*/
inline void
setInitialNoiseCovariance (const std::vector<double> &initial_noise_covariance)
{
initial_noise_covariance_ = initial_noise_covariance;
}
/** \brief Set the mean of the initial noise. It will be used when initializing the particles.
* \param[in] initial_noise_mean the mean values of initial noise.
*/
inline void
setInitialNoiseMean (const std::vector<double> &initial_noise_mean)
{
initial_noise_mean_ = initial_noise_mean;
}
/** \brief Set the threshold to re-initialize the particles.
* \param[in] resample_likelihood_thr threshold to re-initialize.
*/
inline void
setResampleLikelihoodThr (const double resample_likelihood_thr)
{
resample_likelihood_thr_ = resample_likelihood_thr;
}
/** \brief 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.
* \param[in] occlusion_angle_thr threshold of angle to be considered occlusion.
*/
inline void
setOcclusionAngleThe (const double occlusion_angle_thr)
{
occlusion_angle_thr_ = occlusion_angle_thr;
}
/** \brief 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.
* \param[in] min_indices the minimum number of indices.
*/
inline void
setMinIndices (const int min_indices) { min_indices_ = min_indices; }
/** \brief Set the transformation from the world coordinates to the frame of the particles.
* \param[in] trans Affine transformation from the worldcoordinates to the frame of the particles.
*/
inline void setTrans (const Eigen::Affine3f &trans) { trans_ = trans; }
/** \brief Get the transformation from the world coordinates to the frame of the particles. */
inline Eigen::Affine3f getTrans () const { return trans_; }
/** \brief 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.
*/
inline StateT getResult () const override { return representative_state_; }
/** \brief Convert a state to affine transformation from the world coordinates frame.
* \param[in] particle an instance of StateT.
*/
Eigen::Affine3f toEigenMatrix (const StateT& particle)
{
return particle.toEigenMatrix ();
}
/** \brief Get a pointer to a pointcloud of the particles. */
inline PointCloudStatePtr getParticles () const { return particles_; }
/** \brief Normalize the weight of a particle using \f$ std::exp(1- alpha ( w - w_{min}) / (w_max - w_min)) \f$
* \note This method is described in [P.Azad et. al, ICRA11].
* \param[in] w the weight to be normalized
* \param[in] w_min the minimum weight of the particles
* \param[in] w_max the maximum weight of the particles
*/
inline double normalizeParticleWeight (double w, double w_min, double w_max)
{
return std::exp (1.0 - alpha_ * (w - w_min) / (w_max - w_min));
}
/** \brief Set the value of alpha.
* \param[in] alpha the value of alpha
*/
inline void setAlpha (double alpha) { alpha_ = alpha; }
/** \brief Get the value of alpha. */
inline double getAlpha () { return alpha_; }
/** \brief Set the value of use_normal_.
* \param[in] use_normal the value of use_normal_.
*/
inline void setUseNormal (bool use_normal) { use_normal_ = use_normal; }
/** \brief Get the value of use_normal_. */
inline bool getUseNormal () { return use_normal_; }
/** \brief Set the value of use_change_detector_.
* \param[in] use_change_detector the value of use_change_detector_.
*/
inline void setUseChangeDetector (bool use_change_detector) { use_change_detector_ = use_change_detector; }
/** \brief Get the value of use_change_detector_. */
inline bool getUseChangeDetector () { return use_change_detector_; }
/** \brief Set the motion ratio
* \param[in] motion_ratio the ratio of hypothesis to use motion model.
*/
inline void setMotionRatio (double motion_ratio) { motion_ratio_ = motion_ratio; }
/** \brief Get the motion ratio. */
inline double getMotionRatio () { return motion_ratio_;}
/** \brief Set the number of interval frames to run change detection.
* \param[in] change_detector_interval the number of interval frames.
*/
inline void setIntervalOfChangeDetection (unsigned int change_detector_interval)
{
change_detector_interval_ = change_detector_interval;
}
/** \brief Get the number of interval frames to run change detection. */
inline unsigned int getIntervalOfChangeDetection ()
{
return change_detector_interval_;
}
/** \brief Set the minimum amount of points required within leaf node to become serialized in change detection
* \param[in] change_detector_filter the minimum amount of points required within leaf node
*/
inline void setMinPointsOfChangeDetection (unsigned int change_detector_filter)
{
change_detector_filter_ = change_detector_filter;
}
/** \brief Set the resolution of change detection.
* \param[in] resolution resolution of change detection octree
*/
inline void setResolutionOfChangeDetection (double resolution) { change_detector_resolution_ = resolution; }
/** \brief Get the resolution of change detection. */
inline double getResolutionOfChangeDetection () { return change_detector_resolution_; }
/** \brief Get the minimum amount of points required within leaf node to become serialized in change detection. */
inline unsigned int getMinPointsOfChangeDetection ()
{
return change_detector_filter_;
}
/** \brief Get the adjustment ratio. */
inline double
getFitRatio() const { return fit_ratio_; }
/** \brief Reset the particles to restart tracking*/
virtual inline void resetTracking ()
{
if (particles_)
particles_->points.clear ();
}
protected:
/** \brief Compute the parameters for the bounding box of hypothesis pointclouds.
* \param[out] x_min the minimum value of x axis.
* \param[out] x_max the maximum value of x axis.
* \param[out] y_min the minimum value of y axis.
* \param[out] y_max the maximum value of y axis.
* \param[out] z_min the minimum value of z axis.
* \param[out] z_max the maximum value of z axis.
*/
void calcBoundingBox (double &x_min, double &x_max,
double &y_min, double &y_max,
double &z_min, double &z_max);
/** \brief Crop the pointcloud by the bounding box calculated from hypothesis and the reference pointcloud.
* \param[in] cloud a pointer to pointcloud to be cropped.
* \param[out] output a pointer to be assigned the cropped pointcloud.
*/
void cropInputPointCloud (const PointCloudInConstPtr &cloud, PointCloudIn &output);
/** \brief Compute a reference pointcloud transformed to the pose that hypothesis represents.
* \param[in] hypothesis a particle which represents a hypothesis.
* \param[in] indices the indices which should be taken into account.
* \param[out] cloud the resultant point cloud model dataset which is transformed to hypothesis.
**/
void computeTransformedPointCloud (const StateT& hypothesis,
std::vector<int>& indices,
PointCloudIn &cloud);
/** \brief Compute a reference pointcloud transformed to the pose that hypothesis represents and calculate
* indices taking occlusion into account.
* \param[in] hypothesis a particle which represents a hypothesis.
* \param[in] indices the indices which should be taken into account.
* \param[out] cloud the resultant point cloud model dataset which is transformed to hypothesis.
**/
void computeTransformedPointCloudWithNormal (const StateT& hypothesis,
std::vector<int>& indices,
PointCloudIn &cloud);
/** \brief Compute a reference pointcloud transformed to the pose that hypothesis represents and calculate
* indices without taking occlusion into account.
* \param[in] hypothesis a particle which represents a hypothesis.
* \param[out] cloud the resultant point cloud model dataset which is transformed to hypothesis.
**/
void computeTransformedPointCloudWithoutNormal (const StateT& hypothesis,
PointCloudIn &cloud);
/** \brief This method should get called before starting the actual computation. */
bool initCompute () override;
/** \brief Weighting phase of particle filter method. Calculate the likelihood of all of the particles and set the weights. */
virtual void weight ();
/** \brief 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.
*/
virtual void resample ();
/** \brief Calculate the weighted mean of the particles and set it as the result. */
virtual void update ();
/** \brief Normalize the weights of all the particels. */
virtual void normalizeWeight ();
/** \brief Initialize the particles. initial_noise_covariance_ and initial_noise_mean_ are used for Gaussian sampling. */
void initParticles (bool reset);
/** \brief Track the pointcloud using particle filter method. */
void computeTracking () override;
/** \brief Implementation of "sample with replacement" using Walker's alias method.
about Walker's alias method, you can check the paper below:
article{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},
}
\param a an alias table, which generated by genAliasTable.
\param q a table of weight, which generated by genAliasTable.
*/
int sampleWithReplacement (const std::vector<int>& a, const std::vector<double>& q);
/** \brief Generate the tables for walker's alias method. */
void genAliasTable (std::vector<int> &a, std::vector<double> &q, const PointCloudStateConstPtr &particles);
/** \brief Resampling the particle with replacement. */
void
resampleWithReplacement ();
/** \brief Resampling the particle in deterministic way. */
void
resampleDeterministic ();
/** \brief Run change detection and return true if there is a change.
* \param[in] input a pointer to the input pointcloud.
*/
bool
testChangeDetection (const PointCloudInConstPtr &input);
/** \brief The number of iteration of particlefilter. */
int iteration_num_;
/** \brief The number of the particles. */
int particle_num_;
/** \brief The minimum number of points which the hypothesis should have. */
int min_indices_;
/** \brief Adjustment of the particle filter. */
double fit_ratio_;
/** \brief A pointer to reference point cloud. */
PointCloudInConstPtr ref_;
/** \brief A pointer to the particles */
PointCloudStatePtr particles_;
/** \brief A pointer to PointCloudCoherence. */
CloudCoherencePtr coherence_;
/** \brief The diagonal elements of covariance matrix of the step noise. the covariance matrix is used
* at every resample method.
*/
std::vector<double> step_noise_covariance_;
/** \brief The diagonal elements of covariance matrix of the initial noise. the covariance matrix is used
* when initialize the particles.
*/
std::vector<double> initial_noise_covariance_;
/** \brief The mean values of initial noise. */
std::vector<double> initial_noise_mean_;
/** \brief The threshold for the particles to be re-initialized. */
double resample_likelihood_thr_;
/** \brief The threshold for the points to be considered as occluded. */
double occlusion_angle_thr_;
/** \brief The weight to be used in normalization of the weights of the particles. */
double alpha_;
/** \brief The result of tracking. */
StateT representative_state_;
/** \brief An affine transformation from the world coordinates frame to the origin of the particles. */
Eigen::Affine3f trans_;
/** \brief A flag to use normal or not. defaults to false. */
bool use_normal_;
/** \brief Difference between the result in t and t-1. */
StateT motion_;
/** \brief Ratio of hypothesis to use motion model. */
double motion_ratio_;
/** \brief Pass through filter to crop the pointclouds within the hypothesis bounding box. */
pcl::PassThrough<PointInT> pass_x_;
/** \brief Pass through filter to crop the pointclouds within the hypothesis bounding box. */
pcl::PassThrough<PointInT> pass_y_;
/** \brief Pass through filter to crop the pointclouds within the hypothesis bounding box. */
pcl::PassThrough<PointInT> pass_z_;
/** \brief A list of the pointers to pointclouds. */
std::vector<PointCloudInPtr> transed_reference_vector_;
/** \brief Change detector used as a trigger to track. */
typename pcl::octree::OctreePointCloudChangeDetector<PointInT>::Ptr change_detector_;
/** \brief A flag to be true when change of pointclouds is detected. */
bool changed_;
/** \brief A counter to skip change detection. */
unsigned int change_counter_;
/** \brief Minimum points in a leaf when calling change detector. defaults to 10. */
unsigned int change_detector_filter_;
/** \brief The number of interval frame to run change detection. defaults to 10. */
unsigned int change_detector_interval_;
/** \brief Resolution of change detector. defaults to 0.01. */
double change_detector_resolution_;
/** \brief The flag which will be true if using change detection. */
bool use_change_detector_;
};
}
}
// #include <pcl/tracking/impl/particle_filter.hpp>
#ifdef PCL_NO_PRECOMPILE
#include <pcl/tracking/impl/particle_filter.hpp>
#endif
pcl::octree::OctreePointCloudChangeDetector::Ptr
shared_ptr< OctreePointCloudChangeDetector< PointT, LeafContainerT, BranchContainerT > > Ptr
Definition: octree_pointcloud_changedetector.h:72
pcl::tracking::ParticleFilterTracker::setOcclusionAngleThe
void setOcclusionAngleThe(const double occlusion_angle_thr)
Set the threshold of angle to be considered occlusion (default: pi/2).
Definition: particle_filter.h:175
pcl::tracking::ParticleFilterTracker::update
virtual void update()
Calculate the weighted mean of the particles and set it as the result.
Definition: particle_filter.hpp:369
pcl
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
pcl::tracking::ParticleFilterTracker::pass_z_
pcl::PassThrough< PointInT > pass_z_
Pass through filter to crop the pointclouds within the hypothesis bounding box.
Definition: particle_filter.h:477
pcl::tracking::ParticleFilterTracker::setResolutionOfChangeDetection
void setResolutionOfChangeDetection(double resolution)
Set the resolution of change detection.
Definition: particle_filter.h:281
pcl::tracking::ParticleFilterTracker::getUseNormal
bool getUseNormal()
Get the value of use_normal_.
Definition: particle_filter.h:238
pcl::tracking::ParticleFilterTracker
ParticleFilterTracker tracks the PointCloud which is given by setReferenceCloud within the measured P...
Definition: particle_filter.h:23
pcl::tracking::ParticleFilterTracker::getIterationNum
int getIterationNum() const
Get the number of iteration.
Definition: particle_filter.h:100
pcl::tracking::ParticleFilterTracker::setInitialNoiseCovariance
void setInitialNoiseCovariance(const std::vector< double > &initial_noise_covariance)
Set the covariance of the initial noise.
Definition: particle_filter.h:146
pcl::tracking::ParticleFilterTracker::resample_likelihood_thr_
double resample_likelihood_thr_
The threshold for the particles to be re-initialized.
Definition: particle_filter.h:449
pcl::PassThrough::setFilterFieldName
void setFilterFieldName(const std::string &field_name)
Provide the name of the field to be used for filtering data.
Definition: passthrough.h:110
pcl::tracking::ParticleFilterTracker::setMinPointsOfChangeDetection
void setMinPointsOfChangeDetection(unsigned int change_detector_filter)
Set the minimum amount of points required within leaf node to become serialized in change detection.
Definition: particle_filter.h:273
pcl::tracking::ParticleFilterTracker::ref_
PointCloudInConstPtr ref_
A pointer to reference point cloud.
Definition: particle_filter.h:427
pcl::tracking::ParticleFilterTracker::use_normal_
bool use_normal_
A flag to use normal or not.
Definition: particle_filter.h:464
pcl::tracking::PointCloudCoherence::ConstPtr
shared_ptr< const PointCloudCoherence< PointInT > > ConstPtr
Definition: coherence.h:63
pcl::tracking::ParticleFilterTracker::particles_
PointCloudStatePtr particles_
A pointer to the particles
Definition: particle_filter.h:430
pcl::tracking::ParticleFilterTracker::motion_ratio_
double motion_ratio_
Ratio of hypothesis to use motion model.
Definition: particle_filter.h:470
pcl::tracking::ParticleFilterTracker::computeTracking
void computeTracking() override
Track the pointcloud using particle filter method.
Definition: particle_filter.hpp:385
pcl::tracking::ParticleFilterTracker::setInitialNoiseMean
void setInitialNoiseMean(const std::vector< double > &initial_noise_mean)
Set the mean of the initial noise.
Definition: particle_filter.h:155
pcl::tracking::ParticleFilterTracker::setIterationNum
void setIterationNum(const int iteration_num)
Set the number of iteration.
Definition: particle_filter.h:96
pcl::tracking::ParticleFilterTracker::initial_noise_mean_
std::vector< double > initial_noise_mean_
The mean values of initial noise.
Definition: particle_filter.h:446
pcl::tracking::ParticleFilterTracker::pass_x_
pcl::PassThrough< PointInT > pass_x_
Pass through filter to crop the pointclouds within the hypothesis bounding box.
Definition: particle_filter.h:473
pcl::tracking::ParticleFilterTracker::CloudCoherenceConstPtr
typename CloudCoherence::ConstPtr CloudCoherenceConstPtr
Definition: particle_filter.h:54
pcl::tracking::ParticleFilterTracker::ParticleFilterTracker
ParticleFilterTracker()
Empty constructor.
Definition: particle_filter.h:57
pcl::tracking::ParticleFilterTracker::getMotionRatio
double getMotionRatio()
Get the motion ratio.
Definition: particle_filter.h:254
pcl::tracking::ParticleFilterTracker::setTrans
void setTrans(const Eigen::Affine3f &trans)
Set the transformation from the world coordinates to the frame of the particles.
Definition: particle_filter.h:191
pcl::tracking::ParticleFilterTracker::Ptr
shared_ptr< ParticleFilterTracker< PointInT, StateT > > Ptr
Definition: particle_filter.h:35
pcl::tracking::PointCoherence::Ptr
shared_ptr< PointCoherence< PointInT > > Ptr
Definition: coherence.h:19
pcl::tracking::ParticleFilterTracker::getIntervalOfChangeDetection
unsigned int getIntervalOfChangeDetection()
Get the number of interval frames to run change detection.
Definition: particle_filter.h:265
pcl::tracking::ParticleFilterTracker::computeTransformedPointCloud
void computeTransformedPointCloud(const StateT &hypothesis, std::vector< int > &indices, PointCloudIn &cloud)
Compute a reference pointcloud transformed to the pose that hypothesis represents.
Definition: particle_filter.hpp:275
pcl::PointCloud< PointInT >
pcl::tracking::ParticleFilterTracker::computeTransformedPointCloudWithNormal
void computeTransformedPointCloudWithNormal(const StateT &hypothesis, std::vector< int > &indices, PointCloudIn &cloud)
Compute a reference pointcloud transformed to the pose that hypothesis represents and calculate indic...
Definition: particle_filter.hpp:294
pcl::tracking::ParticleFilterTracker::change_counter_
unsigned int change_counter_
A counter to skip change detection.
Definition: particle_filter.h:489
pcl::tracking::ParticleFilterTracker::resampleWithReplacement
void resampleWithReplacement()
Resampling the particle with replacement.
Definition: particle_filter.hpp:331
pcl::tracking::ParticleFilterTracker::getResult
StateT getResult() const override
Get an instance of the result of tracking.
Definition: particle_filter.h:200
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:56
pcl::tracking::ParticleFilterTracker::getTrans
Eigen::Affine3f getTrans() const
Get the transformation from the world coordinates to the frame of the particles.
Definition: particle_filter.h:194
pcl::FilterIndices::setKeepOrganized
void setKeepOrganized(bool keep_organized)
Set whether the filtered points should be kept and set to the value given through setUserFilterValue ...
Definition: filter_indices.h:145
pcl::tracking::ParticleFilterTracker::toEigenMatrix
Eigen::Affine3f toEigenMatrix(const StateT &particle)
Convert a state to affine transformation from the world coordinates frame.
Definition: particle_filter.h:205
pcl::tracking::Tracker::tracker_name_
std::string tracker_name_
The tracker name.
Definition: tracker.h:92
pcl::tracking::ParticleFilterTracker::PointCloudIn
typename Tracker< PointInT, StateT >::PointCloudIn PointCloudIn
Definition: particle_filter.h:40
pcl::tracking::ParticleFilterTracker::getMinPointsOfChangeDetection
unsigned int getMinPointsOfChangeDetection()
Get the minimum amount of points required within leaf node to become serialized in change detection.
Definition: particle_filter.h:287
pcl::tracking::ParticleFilterTracker::ConstPtr
shared_ptr< const ParticleFilterTracker< PointInT, StateT > > ConstPtr
Definition: particle_filter.h:36
pcl::tracking::ParticleFilterTracker::setMinIndices
void setMinIndices(const int min_indices)
Set the minimum number of indices (default: 1).
Definition: particle_filter.h:186
pcl::tracking::ParticleFilterTracker::use_change_detector_
bool use_change_detector_
The flag which will be true if using change detection.
Definition: particle_filter.h:501
pcl::tracking::ParticleFilterTracker::getFitRatio
double getFitRatio() const
Get the adjustment ratio.
Definition: particle_filter.h:294
pcl::tracking::ParticleFilterTracker::getCloudCoherence
CloudCoherencePtr getCloudCoherence() const
Get the PointCloudCoherence to compute likelihood.
Definition: particle_filter.h:130
pcl::tracking::ParticleFilterTracker::coherence_
CloudCoherencePtr coherence_
A pointer to PointCloudCoherence.
Definition: particle_filter.h:433
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::getReferenceCloud
const PointCloudInConstPtr getReferenceCloud()
Get a pointer to a reference dataset to be tracked.
Definition: particle_filter.h:120
pcl::tracking::ParticleFilterTracker::change_detector_resolution_
double change_detector_resolution_
Resolution of change detector.
Definition: particle_filter.h:498
pcl::tracking::ParticleFilterTracker::getResolutionOfChangeDetection
double getResolutionOfChangeDetection()
Get the resolution of change detection.
Definition: particle_filter.h:284
pcl::tracking::ParticleFilterTracker::setUseChangeDetector
void setUseChangeDetector(bool use_change_detector)
Set the value of use_change_detector_.
Definition: particle_filter.h:243
pcl::tracking::ParticleFilterTracker::occlusion_angle_thr_
double occlusion_angle_thr_
The threshold for the points to be considered as occluded.
Definition: particle_filter.h:452
pcl::tracking::ParticleFilterTracker::resetTracking
virtual void resetTracking()
Reset the particles to restart tracking.
Definition: particle_filter.h:297
pcl::tracking::ParticleFilterTracker::setParticleNum
void setParticleNum(const int particle_num)
Set the number of the particles.
Definition: particle_filter.h:106
pcl::tracking::ParticleFilterTracker::changed_
bool changed_
A flag to be true when change of pointclouds is detected.
Definition: particle_filter.h:486
pcl::tracking::ParticleFilterTracker::setStepNoiseCovariance
void setStepNoiseCovariance(const std::vector< double > &step_noise_covariance)
Set the covariance of step noise.
Definition: particle_filter.h:137
pcl::tracking::ParticleFilterTracker::getAlpha
double getAlpha()
Get the value of alpha.
Definition: particle_filter.h:230
pcl::tracking::ParticleFilterTracker::resample
virtual void resample()
Resampling phase of particle filter method.
Definition: particle_filter.hpp:325
pcl::tracking::PointCoherence::ConstPtr
shared_ptr< const PointCoherence< PointInT > > ConstPtr
Definition: coherence.h:20
pcl::tracking::ParticleFilterTracker::transed_reference_vector_
std::vector< PointCloudInPtr > transed_reference_vector_
A list of the pointers to pointclouds.
Definition: particle_filter.h:480
pcl::tracking::ParticleFilterTracker::getUseChangeDetector
bool getUseChangeDetector()
Get the value of use_change_detector_.
Definition: particle_filter.h:246
pcl::tracking::ParticleFilterTracker::CloudCoherencePtr
typename CloudCoherence::Ptr CloudCoherencePtr
Definition: particle_filter.h:53
pcl::tracking::ParticleFilterTracker::min_indices_
int min_indices_
The minimum number of points which the hypothesis should have.
Definition: particle_filter.h:421
pcl::tracking::ParticleFilterTracker::particle_num_
int particle_num_
The number of the particles.
Definition: particle_filter.h:418
pcl::tracking::ParticleFilterTracker::change_detector_filter_
unsigned int change_detector_filter_
Minimum points in a leaf when calling change detector.
Definition: particle_filter.h:492
pcl::tracking::ParticleFilterTracker::testChangeDetection
bool testChangeDetection(const PointCloudInConstPtr &input)
Run change detection and return true if there is a change.
Definition: particle_filter.hpp:219
pcl::tracking::ParticleFilterTracker::cropInputPointCloud
void cropInputPointCloud(const PointCloudInConstPtr &cloud, PointCloudIn &output)
Crop the pointcloud by the bounding box calculated from hypothesis and the reference pointcloud.
Definition: particle_filter.hpp:167
pcl::tracking::ParticleFilterTracker::setCloudCoherence
void setCloudCoherence(const CloudCoherencePtr &coherence)
Set the PointCloudCoherence as likelihood.
Definition: particle_filter.h:126
pcl::tracking::ParticleFilterTracker::change_detector_
pcl::octree::OctreePointCloudChangeDetector< PointInT >::Ptr change_detector_
Change detector used as a trigger to track.
Definition: particle_filter.h:483
pcl::tracking::ParticleFilterTracker::resampleDeterministic
void resampleDeterministic()
Resampling the particle in deterministic way.
pcl::tracking::ParticleFilterTracker::fit_ratio_
double fit_ratio_
Adjustment of the particle filter.
Definition: particle_filter.h:424
pcl::PassThrough< PointInT >
pcl::tracking::ParticleFilterTracker::setAlpha
void setAlpha(double alpha)
Set the value of alpha.
Definition: particle_filter.h:227
pcl::tracking::ParticleFilterTracker::weight
virtual void weight()
Weighting phase of particle filter method.
Definition: particle_filter.hpp:230
pcl::tracking::ParticleFilterTracker::initParticles
void initParticles(bool reset)
Initialize the particles.
Definition: particle_filter.hpp:91
pcl::tracking::ParticleFilterTracker::PointCloudInConstPtr
typename PointCloudIn::ConstPtr PointCloudInConstPtr
Definition: particle_filter.h:42
pcl::tracking::ParticleFilterTracker::alpha_
double alpha_
The weight to be used in normalization of the weights of the particles.
Definition: particle_filter.h:455
pcl::tracking::ParticleFilterTracker::PointCloudState
typename Tracker< PointInT, StateT >::PointCloudState PointCloudState
Definition: particle_filter.h:44
pcl::tracking::ParticleFilterTracker::PointCloudInPtr
typename PointCloudIn::Ptr PointCloudInPtr
Definition: particle_filter.h:41
pcl::tracking::ParticleFilterTracker::CoherencePtr
typename Coherence::Ptr CoherencePtr
Definition: particle_filter.h:49
pcl::tracking::ParticleFilterTracker::motion_
StateT motion_
Difference between the result in t and t-1.
Definition: particle_filter.h:467
pcl::tracking::ParticleFilterTracker::change_detector_interval_
unsigned int change_detector_interval_
The number of interval frame to run change detection.
Definition: particle_filter.h:495
pcl::tracking::ParticleFilterTracker::normalizeWeight
virtual void normalizeWeight()
Normalize the weights of all the particels.
Definition: particle_filter.hpp:116
pcl::tracking::PointCloudCoherence
PointCloudCoherence is a base class to compute coherence between the two PointClouds.
Definition: coherence.h:59
pcl::tracking::PointCoherence
PointCoherence is a base class to compute coherence between the two points.
Definition: coherence.h:16
pcl::tracking::ParticleFilterTracker::initial_noise_covariance_
std::vector< double > initial_noise_covariance_
The diagonal elements of covariance matrix of the initial noise.
Definition: particle_filter.h:443
pcl::tracking::ParticleFilterTracker::getParticleNum
int getParticleNum() const
Get the number of the particles.
Definition: particle_filter.h:110
pcl::tracking::ParticleFilterTracker::setResampleLikelihoodThr
void setResampleLikelihoodThr(const double resample_likelihood_thr)
Set the threshold to re-initialize the particles.
Definition: particle_filter.h:164
pcl::tracking::ParticleFilterTracker::CoherenceConstPtr
typename Coherence::ConstPtr CoherenceConstPtr
Definition: particle_filter.h:50
pcl::tracking::ParticleFilterTracker::setMotionRatio
void setMotionRatio(double motion_ratio)
Set the motion ratio.
Definition: particle_filter.h:251
pcl::tracking::ParticleFilterTracker::computeTransformedPointCloudWithoutNormal
void computeTransformedPointCloudWithoutNormal(const StateT &hypothesis, PointCloudIn &cloud)
Compute a reference pointcloud transformed to the pose that hypothesis represents and calculate indic...
Definition: particle_filter.hpp:285
pcl::tracking::ParticleFilterTracker::iteration_num_
int iteration_num_
The number of iteration of particlefilter.
Definition: particle_filter.h:415
pcl::tracking::PointCloudCoherence::Ptr
shared_ptr< PointCloudCoherence< PointInT > > Ptr
Definition: coherence.h:62
pcl::tracking::ParticleFilterTracker::normalizeParticleWeight
double normalizeParticleWeight(double w, double w_min, double w_max)
Normalize the weight of a particle using .
Definition: particle_filter.h:219
pcl::tracking::ParticleFilterTracker::getParticles
PointCloudStatePtr getParticles() const
Get a pointer to a pointcloud of the particles.
Definition: particle_filter.h:211
pcl::tracking::ParticleFilterTracker::trans_
Eigen::Affine3f trans_
An affine transformation from the world coordinates frame to the origin of the particles.
Definition: particle_filter.h:461
pcl::tracking::ParticleFilterTracker::calcBoundingBox
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.
Definition: particle_filter.hpp:191
pcl::tracking::ParticleFilterTracker::PointCloudStatePtr
typename PointCloudState::Ptr PointCloudStatePtr
Definition: particle_filter.h:45
pcl::tracking::ParticleFilterTracker::PointCloudStateConstPtr
typename PointCloudState::ConstPtr PointCloudStateConstPtr
Definition: particle_filter.h:46
pcl::tracking::ParticleFilterTracker::representative_state_
StateT representative_state_
The result of tracking.
Definition: particle_filter.h:458
pcl::tracking::ParticleFilterTracker::setIntervalOfChangeDetection
void setIntervalOfChangeDetection(unsigned int change_detector_interval)
Set the number of interval frames to run change detection.
Definition: particle_filter.h:259
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:438
pcl::tracking::Tracker
Tracker represents the base tracker class.
Definition: tracker.h:56
pcl::tracking::ParticleFilterTracker::setUseNormal
void setUseNormal(bool use_normal)
Set the value of use_normal_.
Definition: particle_filter.h:235
pcl::shared_ptr
boost::shared_ptr< T > shared_ptr
Alias for boost::shared_ptr.
Definition: pcl_macros.h:90
pcl::tracking::ParticleFilterTracker::initCompute
bool initCompute() override
This method should get called before starting the actual computation.
Definition: particle_filter.hpp:12
pcl::tracking::ParticleFilterTracker::setReferenceCloud
void setReferenceCloud(const PointCloudInConstPtr &ref)
Set a pointer to a reference dataset to be tracked.
Definition: particle_filter.h:116
pcl::tracking::ParticleFilterTracker::pass_y_
pcl::PassThrough< PointInT > pass_y_
Pass through filter to crop the pointclouds within the hypothesis bounding box.
Definition: particle_filter.h:475