1 #ifndef PCL_TRACKING_PARTICLE_FILTER_H_
2 #define PCL_TRACKING_PARTICLE_FILTER_H_
4 #include <pcl/tracking/tracking.h>
5 #include <pcl/tracking/tracker.h>
6 #include <pcl/tracking/coherence.h>
7 #include <pcl/filters/passthrough.h>
8 #include <pcl/octree/octree.h>
10 #include <Eigen/Dense>
22 template <
typename Po
intInT,
typename StateT>
208 return particle.toEigenMatrix ();
222 return exp (1.0 -
alpha_ * (w - w_min) / (w_max - w_min));
315 double &y_min,
double &y_max,
316 double &z_min,
double &z_max);
332 std::vector<int>& indices,
342 std::vector<int>& indices,
484 boost::shared_ptr<pcl::octree::OctreePointCloudChangeDetector<PointInT> >
change_detector_;
508 #ifdef PCL_NO_PRECOMPILE
509 #include <pcl/tracking/impl/particle_filter.hpp>
512 #endif //PCL_TRACKING_PARTICLE_FILTER_H_
Eigen::Affine3f toEigenMatrix(const StateT &particle)
Convert a state to affine transformation from the world coordinates frame.
void cropInputPointCloud(const PointCloudInConstPtr &cloud, PointCloudIn &output)
Crop the pointcloud by the bounding box calculated from hypothesis and the reference pointcloud...
double motion_ratio_
Ratio of hypothesis to use motion model.
boost::shared_ptr< const Coherence > CoherenceConstPtr
std::vector< double > initial_noise_covariance_
The diagonal elements of covariance matrix of the initial noise.
int min_indices_
The minimum number of points which the hypothesis should have.
void setReferenceCloud(const PointCloudInConstPtr &ref)
Set a pointer to a reference dataset to be tracked.
double getResolutionOfChangeDetection()
Get the resolution of change detection.
void computeTransformedPointCloud(const StateT &hypothesis, std::vector< int > &indices, PointCloudIn &cloud)
Compute a reference pointcloud transformed to the pose that hypothesis represents.
double change_detector_resolution_
Resolution of change detector.
ParticleFilterTracker tracks the PointCloud which is given by setReferenceCloud within the measured P...
bool changed_
A flag to be true when change of pointclouds is detected.
Eigen::Affine3f trans_
An affine transformation from the world coordinates frame to the origin of the particles.
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.
double fit_ratio_
Adjustment of the particle filter.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Tracker< PointInT, StateT > BaseClass
PointCoherence< PointInT > Coherence
void initParticles(bool reset)
Initialize the particles.
pcl::PassThrough< PointInT > pass_y_
Pass through filter to crop the pointclouds within the hypothesis bounding box.
Tracker< PointInT, StateT >::PointCloudIn PointCloudIn
void setResampleLikelihoodThr(const double resample_likelihood_thr)
Set the threshold to re-initialize the particles.
boost::shared_ptr< Coherence > CoherencePtr
bool testChangeDetection(const PointCloudInConstPtr &input)
Run change detection and return true if there is a change.
boost::shared_ptr< PointCloud< PointT > > Ptr
std::string tracker_name_
The tracker name.
void setAlpha(double alpha)
Set the value of alpha.
void resampleDeterministic()
Resampling the particle in deterministic way.
void computeTransformedPointCloudWithoutNormal(const StateT &hypothesis, PointCloudIn &cloud)
Compute a reference pointcloud transformed to the pose that hypothesis represents and calculate indic...
void setFilterFieldName(const std::string &field_name)
Provide the name of the field to be used for filtering data.
void setOcclusionAngleThe(const double occlusion_angle_thr)
Set the threshold of angle to be considered occlusion (default: pi/2).
void setMinPointsOfChangeDetection(unsigned int change_detector_filter)
Set the minimum amount of points required within leaf node to become serialized in change detection...
virtual void resetTracking()
Reset the particles to restart tracking.
unsigned int getIntervalOfChangeDetection()
Get the number of interval frames to run change detection.
pcl::PassThrough< PointInT > pass_x_
Pass through filter to crop the pointclouds within the hypothesis bounding box.
virtual void update()
Calculate the weighted mean of the particles and set it as the result.
bool use_normal_
A flag to use normal or not.
void resampleWithReplacement()
Resampling the particle with replacement.
CloudCoherencePtr coherence_
A pointer to PointCloudCoherence.
void setResolutionOfChangeDetection(double resolution)
Set the resolution of change detection.
std::vector< double > step_noise_covariance_
The diagonal elements of covariance matrix of the step noise.
void setInitialNoiseCovariance(const std::vector< double > &initial_noise_covariance)
Set the covariance of the initial noise.
std::vector< PointCloudInPtr > transed_reference_vector_
A list of the pointers to pointclouds.
double occlusion_angle_thr_
The threshold for the points to be considered as occluded.
PointCloudCoherence< PointInT > CloudCoherence
void setStepNoiseCovariance(const std::vector< double > &step_noise_covariance)
Set the covariance of step noise.
Eigen::Affine3f getTrans() const
Get the transformation from the world coordinates to the frame of the particles.
boost::shared_ptr< pcl::octree::OctreePointCloudChangeDetector< PointInT > > change_detector_
Change detector used as a trigger to track.
virtual StateT getResult() const
Get an instance of the result of tracking.
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...
boost::shared_ptr< const CloudCoherence > CloudCoherenceConstPtr
PointCloudInConstPtr const getReferenceCloud()
Get a pointer to a reference dataset to be tracked.
void setCloudCoherence(const CloudCoherencePtr &coherence)
Set the PointCloudCoherence as likelihood.
pcl::PassThrough< PointInT > pass_z_
Pass through filter to crop the pointclouds within the hypothesis bounding box.
void setInitialNoiseMean(const std::vector< double > &initial_noise_mean)
Set the mean of the initial noise.
void setUseChangeDetector(bool use_change_detector)
Set the value of use_change_detector_.
virtual bool initCompute()
This method should get called before starting the actual computation.
int sampleWithReplacement(const std::vector< int > &a, const std::vector< double > &q)
Implementation of "sample with replacement" using Walker's alias method.
PointCloudCoherence is a base class to compute coherence between the two PointClouds.
bool getUseNormal()
Get the value of use_normal_.
StateT motion_
Difference between the result in t and t-1.
void setIterationNum(const int iteration_num)
Set the number of iteration.
virtual void computeTracking()
Track the pointcloud using particle filter method.
PointCloudState::Ptr PointCloudStatePtr
PointCloudIn::ConstPtr PointCloudInConstPtr
double resample_likelihood_thr_
The threshold for the particles to be re-initialized.
PointCloudIn::Ptr PointCloudInPtr
double getFitRatio() const
Get the adjustment ratio.
void setIntervalOfChangeDetection(unsigned int change_detector_interval)
Set the number of interval frames to run change detection.
PointCoherence is a base class to compute coherence between the two points.
void setKeepOrganized(bool keep_organized)
Set whether the filtered points should be kept and set to the value given through setUserFilterValue ...
int iteration_num_
The number of iteration of particlefilter.
void setMinIndices(const int min_indices)
Set the minimum number of indices (default: 1).
virtual void weight()
Weighting phase of particle filter method.
CloudCoherencePtr getCloudCoherence() const
Get the PointCloudCoherence to compute likelihood.
void setParticleNum(const int particle_num)
Set the number of the particles.
PointCloudInConstPtr ref_
A pointer to reference point cloud.
virtual void resample()
Resampling phase of particle filter method.
double getAlpha()
Get the value of alpha.
boost::shared_ptr< CloudCoherence > CloudCoherencePtr
std::vector< double > initial_noise_mean_
The mean values of initial noise.
bool getUseChangeDetector()
Get the value of use_change_detector_.
Tracker< PointInT, StateT >::PointCloudState PointCloudState
void setUseNormal(bool use_normal)
Set the value of use_normal_.
void setTrans(const Eigen::Affine3f &trans)
Set the transformation from the world coordinates to the frame of the particles.
unsigned int getMinPointsOfChangeDetection()
Get the minimum amount of points required within leaf node to become serialized in change detection...
unsigned int change_detector_interval_
The number of interval frame to run change detection.
double normalizeParticleWeight(double w, double w_min, double w_max)
Normalize the weight of a particle using .
void genAliasTable(std::vector< int > &a, std::vector< double > &q, const PointCloudStateConstPtr &particles)
Generate the tables for walker's alias method.
unsigned int change_counter_
A counter to skip change detection.
double getMotionRatio()
Get the motion ratio.
unsigned int change_detector_filter_
Minimum points in a leaf when calling change detector.
double alpha_
The weight to be used in normalization of the weights of the particles.
PointCloudStatePtr particles_
A pointer to the particles.
ParticleFilterTracker()
Empty constructor.
int getIterationNum() const
Get the number of iteration.
StateT representative_state_
The result of tracking.
PointCloudStatePtr getParticles() const
Get a pointer to a pointcloud of the particles.
void setMotionRatio(double motion_ratio)
Set the motion ratio.
virtual void normalizeWeight()
Normalize the weights of all the particels.
int getParticleNum() const
Get the number of the particles.
PointCloudState::ConstPtr PointCloudStateConstPtr
bool use_change_detector_
The flag which will be true if using change detection.
Tracker represents the base tracker class.
int particle_num_
The number of the particles.