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