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