1 #ifndef PCL_TRACKING_IMPL_PARTICLE_FILTER_H_
2 #define PCL_TRACKING_IMPL_PARTICLE_FILTER_H_
7 #include <pcl/common/eigen.h>
8 #include <pcl/common/transforms.h>
9 #include <pcl/tracking/particle_filter.h>
11 template <
typename Po
intInT,
typename StateT>
bool
16 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
20 if (transed_reference_vector_.empty ())
23 transed_reference_vector_.resize (particle_num_);
24 for (
int i = 0; i < particle_num_; i++)
30 coherence_->setTargetCloud (input_);
32 if (!change_detector_)
35 if (!particles_ || particles_->points.empty ())
40 template <
typename Po
intInT,
typename StateT>
int
42 (
const std::vector<int>& a,
const std::vector<double>& q)
44 static std::mt19937 rng([] { std::random_device rd;
return rd(); } ());
45 std::uniform_real_distribution<> rd (0.0, 1.0);
47 double rU = rd (rng) *
static_cast<double> (particles_->points.size ());
48 int k =
static_cast<int> (rU);
55 template <
typename Po
intInT,
typename StateT>
void
60 std::vector<int> HL (particles->points.size ());
61 std::vector<int>::iterator H = HL.begin ();
62 std::vector<int>::iterator L = HL.end () - 1;
63 std::size_t num = particles->points.size ();
64 for ( std::size_t i = 0; i < num; i++ )
65 q[i] = particles->points[i].weight *
static_cast<float> (num);
66 for ( std::size_t i = 0; i < num; i++ )
67 a[i] =
static_cast<int> (i);
69 for ( std::size_t i = 0; i < num; i++ )
71 *H++ =
static_cast<int> (i);
73 *L-- =
static_cast<int> (i);
75 while ( H != HL.begin() && L != HL.end() - 1 )
90 template <
typename Po
intInT,
typename StateT>
void
96 representative_state_.zero ();
97 StateT offset = StateT::toState (trans_);
98 representative_state_ = offset;
99 representative_state_.weight = 1.0f /
static_cast<float> (particle_num_);
103 for (
int i = 0; i < particle_num_; i++ )
107 p.sample (initial_noise_mean_, initial_noise_covariance_);
108 p = p + representative_state_;
109 p.weight = 1.0f /
static_cast<float> (particle_num_);
110 particles_->points.push_back (p);
114 template <
typename Po
intInT,
typename StateT>
void
118 double w_min = std::numeric_limits<double>::max ();
119 double w_max = - std::numeric_limits<double>::max ();
120 for ( std::size_t i = 0; i < particles_->points.size (); i++ )
122 double weight = particles_->points[i].weight;
125 if (weight != 0.0 && w_max < weight)
132 for ( std::size_t i = 0; i < particles_->points.size (); i++ )
134 if (particles_->points[i].weight != 0.0)
136 particles_->points[i].weight =
static_cast<float> (normalizeParticleWeight (particles_->points[i].weight, w_min, w_max));
142 for ( std::size_t i = 0; i < particles_->points.size (); i++ )
143 particles_->points[i].weight = 1.0f /
static_cast<float> (particles_->points.size ());
147 for ( std::size_t i = 0; i < particles_->points.size (); i++ )
149 sum += particles_->points[i].weight;
154 for ( std::size_t i = 0; i < particles_->points.size (); i++ )
155 particles_->points[i].weight /=
static_cast<float> (sum);
159 for ( std::size_t i = 0; i < particles_->points.size (); i++ )
160 particles_->points[i].weight = 1.0f /
static_cast<float> (particles_->points.size ());
165 template <
typename Po
intInT,
typename StateT>
void
169 double x_min, y_min, z_min, x_max, y_max, z_max;
170 calcBoundingBox (x_min, x_max, y_min, y_max, z_min, z_max);
171 pass_x_.setFilterLimits (
float (x_min),
float (x_max));
172 pass_y_.setFilterLimits (
float (y_min),
float (y_max));
173 pass_z_.setFilterLimits (
float (z_min),
float (z_max));
177 pass_x_.setInputCloud (input_);
178 pass_x_.filter (*xcloud);
181 pass_y_.setInputCloud (xcloud);
182 pass_y_.filter (*ycloud);
184 pass_z_.setInputCloud (ycloud);
185 pass_z_.filter (output);
189 template <
typename Po
intInT,
typename StateT>
void
191 double &x_min,
double &x_max,
double &y_min,
double &y_max,
double &z_min,
double &z_max)
193 x_min = y_min = z_min = std::numeric_limits<double>::max ();
194 x_max = y_max = z_max = - std::numeric_limits<double>::max ();
196 for (std::size_t i = 0; i < transed_reference_vector_.size (); i++)
216 template <
typename Po
intInT,
typename StateT>
bool
220 change_detector_->setInputCloud (input);
221 change_detector_->addPointsFromInputCloud ();
222 std::vector<int> newPointIdxVector;
223 change_detector_->getPointIndicesFromNewVoxels (newPointIdxVector, change_detector_filter_);
224 change_detector_->switchBuffers ();
225 return !newPointIdxVector.empty ();
228 template <
typename Po
intInT,
typename StateT>
void
233 for (std::size_t i = 0; i < particles_->points.size (); i++)
235 computeTransformedPointCloudWithoutNormal (particles_->points[i], *transed_reference_vector_[i]);
239 cropInputPointCloud (input_, *coherence_input);
241 coherence_->setTargetCloud (coherence_input);
242 coherence_->initCompute ();
243 for (std::size_t i = 0; i < particles_->points.size (); i++)
246 coherence_->compute (transed_reference_vector_[i], indices, particles_->points[i].weight);
251 for (std::size_t i = 0; i < particles_->points.size (); i++)
254 computeTransformedPointCloudWithNormal (particles_->points[i], *indices, *transed_reference_vector_[i]);
258 cropInputPointCloud (input_, *coherence_input);
260 coherence_->setTargetCloud (coherence_input);
261 coherence_->initCompute ();
262 for (std::size_t i = 0; i < particles_->points.size (); i++)
265 coherence_->compute (transed_reference_vector_[i], indices, particles_->points[i].weight);
272 template <
typename Po
intInT,
typename StateT>
void
274 (
const StateT& hypothesis, std::vector<int>& indices,
PointCloudIn &cloud)
277 computeTransformedPointCloudWithNormal (hypothesis, indices, cloud);
279 computeTransformedPointCloudWithoutNormal (hypothesis, cloud);
282 template <
typename Po
intInT,
typename StateT>
void
286 const Eigen::Affine3f trans = toEigenMatrix (hypothesis);
288 pcl::transformPointCloud<PointInT> (*ref_, cloud, trans);
292 template <
typename Po
intInT,
typename StateT>
void
294 #ifdef PCL_TRACKING_NORMAL_SUPPORTED
295 const StateT& hypothesis, std::vector<int>& indices,
PointCloudIn &cloud)
300 #ifdef PCL_TRACKING_NORMAL_SUPPORTED
301 const Eigen::Affine3f trans = toEigenMatrix (hypothesis);
303 pcl::transformPointCloudWithNormals<PointInT> (*ref_, cloud, trans);
304 for ( std::size_t i = 0; i < cloud.points.size (); i++ )
306 PointInT input_point = cloud.points[i];
308 if (!std::isfinite(input_point.x) || !std::isfinite(input_point.y) || !std::isfinite(input_point.z))
311 Eigen::Vector4f p = input_point.getVector4fMap ();
312 Eigen::Vector4f n = input_point.getNormalVector4fMap ();
314 if ( theta > occlusion_angle_thr_ )
315 indices.push_back (i);
318 PCL_WARN (
"[pcl::%s::computeTransformedPointCloudWithoutNormal] use_normal_ == true is not supported in this Point Type.",
319 getClassName ().c_str ());
323 template <
typename Po
intInT,
typename StateT>
void
326 resampleWithReplacement ();
329 template <
typename Po
intInT,
typename StateT>
void
332 std::vector<int> a (particles_->points.size ());
333 std::vector<double> q (particles_->points.size ());
334 genAliasTable (a, q, particles_);
336 const std::vector<double> zero_mean (StateT::stateDimension (), 0.0);
339 particles_->points.clear ();
341 StateT p = representative_state_;
342 particles_->points.push_back (p);
345 int motion_num =
static_cast<int> (particles_->points.size ()) *
static_cast<int> (motion_ratio_);
346 for (
int i = 1; i < motion_num; i++ )
348 int target_particle_index = sampleWithReplacement (a, q);
349 StateT p = origparticles->points[target_particle_index];
351 p.sample (zero_mean, step_noise_covariance_);
353 particles_->points.push_back (p);
357 for (
int i = motion_num; i < particle_num_; i++ )
359 int target_particle_index = sampleWithReplacement (a, q);
360 StateT p = origparticles->points[target_particle_index];
362 p.sample (zero_mean, step_noise_covariance_);
363 particles_->points.push_back (p);
367 template <
typename Po
intInT,
typename StateT>
void
371 StateT orig_representative = representative_state_;
372 representative_state_.zero ();
373 representative_state_.
weight = 0.0;
374 for ( std::size_t i = 0; i < particles_->points.size (); i++)
376 StateT p = particles_->points[i];
377 representative_state_ = representative_state_ + p * p.
weight;
379 representative_state_.weight = 1.0f /
static_cast<float> (particles_->points.size ());
380 motion_ = representative_state_ - orig_representative;
383 template <
typename Po
intInT,
typename StateT>
void
387 for (
int i = 0; i < iteration_num_; i++)
409 #define PCL_INSTANTIATE_ParticleFilterTracker(T,ST) template class PCL_EXPORTS pcl::tracking::ParticleFilterTracker<T,ST>;
void genAliasTable(std::vector< int > &a, std::vector< double > &q, const PointCloudStateConstPtr &particles)
Generate the tables for walker's alias method.
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.
void initParticles(bool reset)
Initialize the particles.
int sampleWithReplacement(const std::vector< int > &a, const std::vector< double > &q)
Implementation of "sample with replacement" using Walker's alias method.
void computeTransformedPointCloud(const StateT &hypothesis, std::vector< int > &indices, PointCloudIn &cloud)
Compute a reference pointcloud transformed to the pose that hypothesis represents.
bool testChangeDetection(const PointCloudInConstPtr &input)
Run change detection and return true if there is a change.
typename PointCloudIn::ConstPtr PointCloudInConstPtr
void resampleWithReplacement()
Resampling the particle with replacement.
typename PointCloudState::Ptr PointCloudStatePtr
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 computeTransformedPointCloudWithoutNormal(const StateT &hypothesis, PointCloudIn &cloud)
Compute a reference pointcloud transformed to the pose that hypothesis represents and calculate indic...
virtual void resample()
Resampling phase of particle filter method.
typename PointCloudState::ConstPtr PointCloudStateConstPtr
void computeTracking() override
Track the pointcloud using particle filter method.
void cropInputPointCloud(const PointCloudInConstPtr &cloud, PointCloudIn &output)
Crop the pointcloud by the bounding box calculated from hypothesis and the reference pointcloud.
virtual void update()
Calculate the weighted mean of the particles and set it as the result.
virtual void weight()
Weighting phase of particle filter method.
bool initCompute() override
This method should get called before starting the actual computation.
typename PointCloudIn::Ptr PointCloudInPtr
typename Tracker< PointInT, StateT >::PointCloudState PointCloudState
typename Tracker< PointInT, StateT >::PointCloudIn PointCloudIn
virtual void normalizeWeight()
Normalize the weights of all the particels.
Tracker represents the base tracker class.
Define standard C methods and C++ classes that are common to all methods.
void getMinMax3D(const pcl::PointCloud< PointT > &cloud, PointT &min_pt, PointT &max_pt)
Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud.
double getAngle3D(const Eigen::Vector4f &v1, const Eigen::Vector4f &v2, const bool in_degree=false)
Compute the smallest angle between two 3D vectors in radians (default) or degree.
shared_ptr< Indices > IndicesPtr