Loading...
Searching...
No Matches
particle_filter.h
#pragma once
#include <pcl/filters/passthrough.h>
#include <pcl/octree/octree_pointcloud_changedetector.h>
#include <pcl/tracking/coherence.h>
#include <pcl/tracking/tracker.h>
#include <pcl/tracking/tracking.h>
#include <pcl/memory.h>
#include <pcl/point_types.h>
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()
: representative_state_(), motion_(), pass_x_(), pass_y_(), pass_z_()
{
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)
{
if (traits::has_normal_v<PointInT> || !use_normal) {
use_normal_ = use_normal;
return;
}
PCL_WARN("[pcl::%s::setUseNormal] "
"use_normal_ == true is not supported in this Point Type.\n",
getClassName().c_str());
use_normal_ = false;
}
/** \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,
pcl::Indices& indices,
PointCloudIn& cloud);
#ifdef DOXYGEN_ONLY
/** \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,
pcl::Indices& indices,
PointCloudIn& cloud);
#else
template <typename PointT = PointInT, traits::HasNormal<PointT> = true>
void
computeTransformedPointCloudWithNormal(const StateT& hypothesis,
pcl::Indices& indices,
PointCloudIn& cloud);
template <typename PointT = PointInT, traits::HasNoNormal<PointT> = true>
void
computeTransformedPointCloudWithNormal(const StateT&, pcl::Indices&, PointCloudIn&)
{
PCL_WARN("[pcl::%s::computeTransformedPointCloudWithNormal] "
"use_normal_ == true is not supported in this Point Type.\n",
getClassName().c_str());
}
#endif
/** \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 achieved 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_{1};
/** \brief The number of the particles. */
int particle_num_{0};
/** \brief The minimum number of points which the hypothesis should have. */
int min_indices_{1};
/** \brief Adjustment of the particle filter. */
double fit_ratio_{0.0};
/** \brief A pointer to reference point cloud. */
PointCloudInConstPtr ref_{nullptr};
/** \brief A pointer to the particles */
PointCloudStatePtr particles_{nullptr};
/** \brief A pointer to PointCloudCoherence. */
CloudCoherencePtr coherence_{nullptr};
/** \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_{0.0};
/** \brief The threshold for the points to be considered as occluded. */
double occlusion_angle_thr_{M_PI / 2.0};
/** \brief The weight to be used in normalization of the weights of the
* particles. */
double alpha_{15.0};
/** \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_{false};
/** \brief Difference between the result in t and t-1. */
StateT motion_;
/** \brief Ratio of hypothesis to use motion model. */
double motion_ratio_{0.25};
/** \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_{
nullptr};
/** \brief A flag to be true when change of pointclouds is detected. */
bool changed_{false};
/** \brief A counter to skip change detection. */
unsigned int change_counter_{0};
/** \brief Minimum points in a leaf when calling change detector. defaults
* to 10. */
unsigned int change_detector_filter_{10};
/** \brief The number of interval frame to run change detection. defaults
* to 10. */
unsigned int change_detector_interval_{10};
/** \brief Resolution of change detector. defaults to 0.01. */
double change_detector_resolution_{0.01};
/** \brief The flag which will be true if using change detection. */
bool use_change_detector_{false};
};
} // namespace tracking
} // namespace pcl
// #include <pcl/tracking/impl/particle_filter.hpp>
#ifdef PCL_NO_PRECOMPILE
#include <pcl/tracking/impl/particle_filter.hpp>
#endif
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:134
pcl::PCLBase< PointInT >::input_
PointCloudConstPtr input_
The input point cloud dataset.
Definition pcl_base.h:147
pcl::PCLBase< PointInT >::indices_
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition pcl_base.h:150
pcl::PCLBase< PointInT >::deinitCompute
bool deinitCompute()
This method should get called after finishing the actual computation.
Definition pcl_base.hpp:175
pcl::PassThrough
PassThrough passes points in a cloud based on constraints for one particular field of the point type.
Definition passthrough.h:82
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:112
pcl::PointCloud< PointInT >
pcl::octree::OctreePointCloudChangeDetector::Ptr
shared_ptr< OctreePointCloudChangeDetector< PointT, LeafContainerT, BranchContainerT > > Ptr
Definition octree_pointcloud_changedetector.h:73
pcl::tracking::ParticleFilterTracker
ParticleFilterTracker tracks the PointCloud which is given by setReferenceCloud within the measured P...
Definition particle_filter.h:19
pcl::tracking::ParticleFilterTracker::setResampleLikelihoodThr
void setResampleLikelihoodThr(const double resample_likelihood_thr)
Set the threshold to re-initialize the particles.
Definition particle_filter.h:163
pcl::tracking::ParticleFilterTracker::Coherence
PointCoherence< PointInT > Coherence
Definition particle_filter.h:43
pcl::tracking::ParticleFilterTracker::getParticleNum
int getParticleNum() const
Get the number of the particles.
Definition particle_filter.h:91
pcl::tracking::ParticleFilterTracker::getUseChangeDetector
bool getUseChangeDetector()
Get the value of use_change_detector_.
Definition particle_filter.h:301
pcl::tracking::ParticleFilterTracker::genAliasTable
void genAliasTable(std::vector< int > &a, std::vector< double > &q, const PointCloudStateConstPtr &particles)
Generate the tables for walker's alias method.
Definition particle_filter.hpp:58
pcl::tracking::ParticleFilterTracker::BaseClass
Tracker< PointInT, StateT > BaseClass
Definition particle_filter.h:33
pcl::tracking::ParticleFilterTracker::getIntervalOfChangeDetection
unsigned int getIntervalOfChangeDetection()
Get the number of interval frames to run change detection.
Definition particle_filter.h:333
pcl::tracking::ParticleFilterTracker::iteration_num_
int iteration_num_
The number of iteration of particlefilter.
Definition particle_filter.h:538
pcl::tracking::ParticleFilterTracker::getFitRatio
double getFitRatio() const
Get the adjustment ratio.
Definition particle_filter.h:375
pcl::tracking::ParticleFilterTracker::motion_ratio_
double motion_ratio_
Ratio of hypothesis to use motion model.
Definition particle_filter.h:595
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:187
pcl::tracking::ParticleFilterTracker::initParticles
void initParticles(bool reset)
Initialize the particles.
Definition particle_filter.hpp:94
pcl::tracking::ParticleFilterTracker::coherence_
CloudCoherencePtr coherence_
A pointer to PointCloudCoherence.
Definition particle_filter.h:556
pcl::tracking::ParticleFilterTracker::sampleWithReplacement
int sampleWithReplacement(const std::vector< int > &a, const std::vector< double > &q)
Implementation of "sample with replacement" using Walker's alias method.
Definition particle_filter.hpp:42
pcl::tracking::ParticleFilterTracker::setUseChangeDetector
void setUseChangeDetector(bool use_change_detector)
Set the value of use_change_detector_.
Definition particle_filter.h:294
pcl::tracking::ParticleFilterTracker::CoherencePtr
typename Coherence::Ptr CoherencePtr
Definition particle_filter.h:44
pcl::tracking::ParticleFilterTracker::getIterationNum
int getIterationNum() const
Get the number of iteration.
Definition particle_filter.h:75
pcl::tracking::ParticleFilterTracker::testChangeDetection
bool testChangeDetection(const PointCloudInConstPtr &input)
Run change detection and return true if there is a change.
Definition particle_filter.hpp:218
pcl::tracking::ParticleFilterTracker::ref_
PointCloudInConstPtr ref_
A pointer to reference point cloud.
Definition particle_filter.h:550
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:344
pcl::tracking::ParticleFilterTracker::PointCloudInConstPtr
typename PointCloudIn::ConstPtr PointCloudInConstPtr
Definition particle_filter.h:37
pcl::tracking::ParticleFilterTracker::computeTransformedPointCloud
void computeTransformedPointCloud(const StateT &hypothesis, pcl::Indices &indices, PointCloudIn &cloud)
Compute a reference pointcloud transformed to the pose that hypothesis represents.
Definition particle_filter.hpp:275
pcl::tracking::ParticleFilterTracker::setStepNoiseCovariance
void setStepNoiseCovariance(const std::vector< double > &step_noise_covariance)
Set the covariance of step noise.
Definition particle_filter.h:133
pcl::tracking::ParticleFilterTracker::CoherenceConstPtr
typename Coherence::ConstPtr CoherenceConstPtr
Definition particle_filter.h:45
pcl::tracking::ParticleFilterTracker::CloudCoherencePtr
typename CloudCoherence::Ptr CloudCoherencePtr
Definition particle_filter.h:48
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:204
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:566
pcl::tracking::ParticleFilterTracker::computeTransformedPointCloudWithNormal
void computeTransformedPointCloudWithNormal(const StateT &hypothesis, pcl::Indices &indices, PointCloudIn &cloud)
Compute a reference pointcloud transformed to the pose that hypothesis represents and calculate indic...
Definition particle_filter.hpp:298
pcl::tracking::ParticleFilterTracker::resampleWithReplacement
void resampleWithReplacement()
Resampling the particle with replacement.
Definition particle_filter.hpp:328
pcl::tracking::ParticleFilterTracker::Ptr
shared_ptr< ParticleFilterTracker< PointInT, StateT > > Ptr
Definition particle_filter.h:30
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:225
pcl::tracking::ParticleFilterTracker::setMinIndices
void setMinIndices(const int min_indices)
Set the minimum number of indices (default: 1).
Definition particle_filter.h:185
pcl::tracking::ParticleFilterTracker::getCloudCoherence
CloudCoherencePtr getCloudCoherence() const
Get the PointCloudCoherence to compute likelihood.
Definition particle_filter.h:123
pcl::tracking::ParticleFilterTracker::changed_
bool changed_
A flag to be true when change of pointclouds is detected.
Definition particle_filter.h:615
pcl::tracking::ParticleFilterTracker::representative_state_
StateT representative_state_
The result of tracking.
Definition particle_filter.h:582
pcl::tracking::ParticleFilterTracker::change_detector_filter_
unsigned int change_detector_filter_
Minimum points in a leaf when calling change detector.
Definition particle_filter.h:622
pcl::tracking::ParticleFilterTracker::setParticleNum
void setParticleNum(const int particle_num)
Set the number of the particles.
Definition particle_filter.h:84
pcl::tracking::ParticleFilterTracker::change_detector_
pcl::octree::OctreePointCloudChangeDetector< PointInT >::Ptr change_detector_
Change detector used as a trigger to track.
Definition particle_filter.h:611
pcl::tracking::ParticleFilterTracker::getParticles
PointCloudStatePtr getParticles() const
Get a pointer to a pointcloud of the particles.
Definition particle_filter.h:232
pcl::tracking::ParticleFilterTracker::PointCloudStatePtr
typename PointCloudState::Ptr PointCloudStatePtr
Definition particle_filter.h:40
pcl::tracking::ParticleFilterTracker::getReferenceCloud
PointCloudInConstPtr const getReferenceCloud()
Get a pointer to a reference dataset to be tracked.
Definition particle_filter.h:107
pcl::tracking::ParticleFilterTracker::change_detector_resolution_
double change_detector_resolution_
Resolution of change detector.
Definition particle_filter.h:629
pcl::tracking::ParticleFilterTracker::step_noise_covariance_
std::vector< double > step_noise_covariance_
The diagonal elements of covariance matrix of the step noise.
Definition particle_filter.h:561
pcl::tracking::ParticleFilterTracker::pass_z_
pcl::PassThrough< PointInT > pass_z_
Pass through filter to crop the pointclouds within the hypothesis bounding box.
Definition particle_filter.h:605
pcl::tracking::ParticleFilterTracker::setResolutionOfChangeDetection
void setResolutionOfChangeDetection(double resolution)
Set the resolution of change detection.
Definition particle_filter.h:353
pcl::tracking::ParticleFilterTracker::setCloudCoherence
void setCloudCoherence(const CloudCoherencePtr &coherence)
Set the PointCloudCoherence as likelihood.
Definition particle_filter.h:116
pcl::tracking::ParticleFilterTracker::transed_reference_vector_
std::vector< PointCloudInPtr > transed_reference_vector_
A list of the pointers to pointclouds.
Definition particle_filter.h:608
pcl::tracking::ParticleFilterTracker::resetTracking
virtual void resetTracking()
Reset the particles to restart tracking.
Definition particle_filter.h:382
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:286
pcl::tracking::ParticleFilterTracker::setUseNormal
void setUseNormal(bool use_normal)
Set the value of use_normal_.
Definition particle_filter.h:271
pcl::tracking::ParticleFilterTracker::resample
virtual void resample()
Resampling phase of particle filter method.
Definition particle_filter.hpp:321
pcl::tracking::ParticleFilterTracker::PointCloudStateConstPtr
typename PointCloudState::ConstPtr PointCloudStateConstPtr
Definition particle_filter.h:41
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:602
pcl::tracking::ParticleFilterTracker::ParticleFilterTracker
ParticleFilterTracker()
Empty constructor.
Definition particle_filter.h:52
pcl::tracking::ParticleFilterTracker::change_counter_
unsigned int change_counter_
A counter to skip change detection.
Definition particle_filter.h:618
pcl::tracking::ParticleFilterTracker::getMotionRatio
double getMotionRatio()
Get the motion ratio.
Definition particle_filter.h:317
pcl::tracking::ParticleFilterTracker::initial_noise_mean_
std::vector< double > initial_noise_mean_
The mean values of initial noise.
Definition particle_filter.h:569
pcl::tracking::ParticleFilterTracker::computeTracking
void computeTracking() override
Track the pointcloud using particle filter method.
Definition particle_filter.hpp:379
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:326
pcl::tracking::ParticleFilterTracker::setReferenceCloud
void setReferenceCloud(const PointCloudInConstPtr &ref)
Set a pointer to a reference dataset to be tracked.
Definition particle_filter.h:100
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:174
pcl::tracking::ParticleFilterTracker::alpha_
double alpha_
The weight to be used in normalization of the weights of the particles.
Definition particle_filter.h:579
pcl::tracking::ParticleFilterTracker::setIterationNum
void setIterationNum(const int iteration_num)
Set the number of iteration.
Definition particle_filter.h:68
pcl::tracking::ParticleFilterTracker::min_indices_
int min_indices_
The minimum number of points which the hypothesis should have.
Definition particle_filter.h:544
pcl::tracking::ParticleFilterTracker::use_normal_
bool use_normal_
A flag to use normal or not.
Definition particle_filter.h:589
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:162
pcl::tracking::ParticleFilterTracker::resampleDeterministic
void resampleDeterministic()
Resampling the particle in deterministic way.
pcl::tracking::ParticleFilterTracker::getResult
StateT getResult() const override
Get an instance of the result of tracking.
Definition particle_filter.h:215
pcl::tracking::ParticleFilterTracker::particles_
PointCloudStatePtr particles_
A pointer to the particles
Definition particle_filter.h:553
pcl::tracking::ParticleFilterTracker::change_detector_interval_
unsigned int change_detector_interval_
The number of interval frame to run change detection.
Definition particle_filter.h:626
pcl::tracking::ParticleFilterTracker::fit_ratio_
double fit_ratio_
Adjustment of the particle filter.
Definition particle_filter.h:547
pcl::tracking::ParticleFilterTracker::getResolutionOfChangeDetection
double getResolutionOfChangeDetection()
Get the resolution of change detection.
Definition particle_filter.h:360
pcl::tracking::ParticleFilterTracker::getAlpha
double getAlpha()
Get the value of alpha.
Definition particle_filter.h:262
pcl::tracking::ParticleFilterTracker::setAlpha
void setAlpha(double alpha)
Set the value of alpha.
Definition particle_filter.h:255
pcl::tracking::ParticleFilterTracker::CloudCoherence
PointCloudCoherence< PointInT > CloudCoherence
Definition particle_filter.h:47
pcl::tracking::ParticleFilterTracker::ConstPtr
shared_ptr< const ParticleFilterTracker< PointInT, StateT > > ConstPtr
Definition particle_filter.h:31
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:246
pcl::tracking::ParticleFilterTracker::update
virtual void update()
Calculate the weighted mean of the particles and set it as the result.
Definition particle_filter.hpp:366
pcl::tracking::ParticleFilterTracker::resample_likelihood_thr_
double resample_likelihood_thr_
The threshold for the particles to be re-initialized.
Definition particle_filter.h:572
pcl::tracking::ParticleFilterTracker::particle_num_
int particle_num_
The number of the particles.
Definition particle_filter.h:541
pcl::tracking::ParticleFilterTracker::weight
virtual void weight()
Weighting phase of particle filter method.
Definition particle_filter.hpp:232
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:599
pcl::tracking::ParticleFilterTracker::setInitialNoiseMean
void setInitialNoiseMean(const std::vector< double > &initial_noise_mean)
Set the mean of the initial noise.
Definition particle_filter.h:154
pcl::tracking::ParticleFilterTracker::initCompute
bool initCompute() override
This method should get called before starting the actual computation.
Definition particle_filter.hpp:14
pcl::tracking::ParticleFilterTracker::getUseNormal
bool getUseNormal()
Get the value of use_normal_.
Definition particle_filter.h:285
pcl::tracking::ParticleFilterTracker::PointCloudInPtr
typename PointCloudIn::Ptr PointCloudInPtr
Definition particle_filter.h:36
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:368
pcl::tracking::ParticleFilterTracker::PointCloudState
typename Tracker< PointInT, StateT >::PointCloudState PointCloudState
Definition particle_filter.h:39
pcl::tracking::ParticleFilterTracker::motion_
StateT motion_
Difference between the result in t and t-1.
Definition particle_filter.h:592
pcl::tracking::ParticleFilterTracker::use_change_detector_
bool use_change_detector_
The flag which will be true if using change detection.
Definition particle_filter.h:632
pcl::tracking::ParticleFilterTracker::CloudCoherenceConstPtr
typename CloudCoherence::ConstPtr CloudCoherenceConstPtr
Definition particle_filter.h:49
pcl::tracking::ParticleFilterTracker::PointCloudIn
typename Tracker< PointInT, StateT >::PointCloudIn PointCloudIn
Definition particle_filter.h:35
pcl::tracking::ParticleFilterTracker::setMotionRatio
void setMotionRatio(double motion_ratio)
Set the motion ratio.
Definition particle_filter.h:310
pcl::tracking::ParticleFilterTracker::setInitialNoiseCovariance
void setInitialNoiseCovariance(const std::vector< double > &initial_noise_covariance)
Set the covariance of the initial noise.
Definition particle_filter.h:144
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:586
pcl::tracking::ParticleFilterTracker::normalizeWeight
virtual void normalizeWeight()
Normalize the weights of all the particels.
Definition particle_filter.hpp:117
pcl::tracking::ParticleFilterTracker::occlusion_angle_thr_
double occlusion_angle_thr_
The threshold for the points to be considered as occluded.
Definition particle_filter.h:575
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:196
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:15
pcl::tracking::Tracker
Tracker represents the base tracker class.
Definition tracker.h:55
pcl::tracking::Tracker::getClassName
const std::string & getClassName() const
Get a string representation of the name of this class.
Definition tracker.h:97
pcl::tracking::Tracker::search_
SearchPtr search_
A pointer to the spatial search object.
Definition tracker.h:93
pcl::tracking::Tracker::tracker_name_
std::string tracker_name_
The tracker name.
Definition tracker.h:90
pcl::tracking::Tracker::PointCloudState
pcl::PointCloud< StateT > PointCloudState
Definition tracker.h:74
point_types.h
Defines all the PCL implemented PointT point type structures.
memory.h
Defines functions, macros and traits for allocating and using memory.
pcl
Definition convolution.h:46
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
M_PI
#define M_PI
Definition pcl_macros.h:203