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