Point Cloud Library (PCL)  1.11.0
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Modules Pages
particle_filter.hpp
1 #ifndef PCL_TRACKING_IMPL_PARTICLE_FILTER_H_
2 #define PCL_TRACKING_IMPL_PARTICLE_FILTER_H_
3 
4 #include <random>
5 
6 #include <pcl/common/common.h>
7 #include <pcl/common/eigen.h>
8 #include <pcl/common/transforms.h>
9 #include <pcl/tracking/particle_filter.h>
10 
11 template <typename PointInT, typename StateT> bool
13 {
15  {
16  PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
17  return (false);
18  }
19 
20  if (transed_reference_vector_.empty ())
21  {
22  // only one time allocation
23  transed_reference_vector_.resize (particle_num_);
24  for (int i = 0; i < particle_num_; i++)
25  {
26  transed_reference_vector_[i] = PointCloudInPtr (new PointCloudIn ());
27  }
28  }
29 
30  coherence_->setTargetCloud (input_);
31 
32  if (!change_detector_)
33  change_detector_.reset (new pcl::octree::OctreePointCloudChangeDetector<PointInT> (change_detector_resolution_));
34 
35  if (!particles_ || particles_->points.empty ())
36  initParticles (true);
37  return (true);
38 }
39 
40 template <typename PointInT, typename StateT> int
42 (const std::vector<int>& a, const std::vector<double>& q)
43 {
44  static std::mt19937 rng([] { std::random_device rd; return rd(); } ());
45  std::uniform_real_distribution<> rd (0.0, 1.0);
46 
47  double rU = rd (rng) * static_cast<double> (particles_->points.size ());
48  int k = static_cast<int> (rU);
49  rU -= k; /* rU - [rU] */
50  if ( rU < q[k] )
51  return k;
52  return a[k];
53 }
54 
55 template <typename PointInT, typename StateT> void
56 pcl::tracking::ParticleFilterTracker<PointInT, StateT>::genAliasTable (std::vector<int> &a, std::vector<double> &q,
57  const PointCloudStateConstPtr &particles)
58 {
59  /* generate an alias table, a and q */
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);
68  // setup H and L
69  for ( std::size_t i = 0; i < num; i++ )
70  if ( q[i] >= 1.0 )
71  *H++ = static_cast<int> (i);
72  else
73  *L-- = static_cast<int> (i);
74 
75  while ( H != HL.begin() && L != HL.end() - 1 )
76  {
77  int j = *(L + 1);
78  int k = *(H - 1);
79  a[j] = k;
80  q[k] += q[j] - 1;
81  ++L;
82  if ( q[k] < 1.0 )
83  {
84  *L-- = k;
85  --H;
86  }
87  }
88 }
89 
90 template <typename PointInT, typename StateT> void
92 {
93  particles_.reset (new PointCloudState ());
94  if (reset)
95  {
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_);
100  }
101 
102  // sampling...
103  for ( int i = 0; i < particle_num_; i++ )
104  {
105  StateT p;
106  p.zero ();
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); // update
111  }
112 }
113 
114 template <typename PointInT, typename StateT> void
116 {
117  // apply exponential function
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++ )
121  {
122  double weight = particles_->points[i].weight;
123  if (w_min > weight)
124  w_min = weight;
125  if (weight != 0.0 && w_max < weight)
126  w_max = weight;
127  }
128 
129  fit_ratio_ = w_min;
130  if (w_max != w_min)
131  {
132  for ( std::size_t i = 0; i < particles_->points.size (); i++ )
133  {
134  if (particles_->points[i].weight != 0.0)
135  {
136  particles_->points[i].weight = static_cast<float> (normalizeParticleWeight (particles_->points[i].weight, w_min, w_max));
137  }
138  }
139  }
140  else
141  {
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 ());
144  }
145 
146  double sum = 0.0;
147  for ( std::size_t i = 0; i < particles_->points.size (); i++ )
148  {
149  sum += particles_->points[i].weight;
150  }
151 
152  if (sum != 0.0)
153  {
154  for ( std::size_t i = 0; i < particles_->points.size (); i++ )
155  particles_->points[i].weight /= static_cast<float> (sum);
156  }
157  else
158  {
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 ());
161  }
162 }
163 
164 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
165 template <typename PointInT, typename StateT> void
167  const PointCloudInConstPtr &, PointCloudIn &output)
168 {
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));
174 
175  // x
176  PointCloudInPtr xcloud (new PointCloudIn);
177  pass_x_.setInputCloud (input_);
178  pass_x_.filter (*xcloud);
179  // y
180  PointCloudInPtr ycloud (new PointCloudIn);
181  pass_y_.setInputCloud (xcloud);
182  pass_y_.filter (*ycloud);
183  // z
184  pass_z_.setInputCloud (ycloud);
185  pass_z_.filter (output);
186 }
187 
188 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
189 template <typename PointInT, typename StateT> void
191  double &x_min, double &x_max, double &y_min, double &y_max, double &z_min, double &z_max)
192 {
193  x_min = y_min = z_min = std::numeric_limits<double>::max ();
194  x_max = y_max = z_max = - std::numeric_limits<double>::max ();
195 
196  for (std::size_t i = 0; i < transed_reference_vector_.size (); i++)
197  {
198  PointCloudInPtr target = transed_reference_vector_[i];
199  PointInT Pmin, Pmax;
200  pcl::getMinMax3D (*target, Pmin, Pmax);
201  if (x_min > Pmin.x)
202  x_min = Pmin.x;
203  if (x_max < Pmax.x)
204  x_max = Pmax.x;
205  if (y_min > Pmin.y)
206  y_min = Pmin.y;
207  if (y_max < Pmax.y)
208  y_max = Pmax.y;
209  if (z_min > Pmin.z)
210  z_min = Pmin.z;
211  if (z_max < Pmax.z)
212  z_max = Pmax.z;
213  }
214 }
215 
216 template <typename PointInT, typename StateT> bool
218 (const PointCloudInConstPtr &input)
219 {
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 ();
226 }
227 
228 template <typename PointInT, typename StateT> void
230 {
231  if (!use_normal_)
232  {
233  for (std::size_t i = 0; i < particles_->points.size (); i++)
234  {
235  computeTransformedPointCloudWithoutNormal (particles_->points[i], *transed_reference_vector_[i]);
236  }
237 
238  PointCloudInPtr coherence_input (new PointCloudIn);
239  cropInputPointCloud (input_, *coherence_input);
240 
241  coherence_->setTargetCloud (coherence_input);
242  coherence_->initCompute ();
243  for (std::size_t i = 0; i < particles_->points.size (); i++)
244  {
245  IndicesPtr indices;
246  coherence_->compute (transed_reference_vector_[i], indices, particles_->points[i].weight);
247  }
248  }
249  else
250  {
251  for (std::size_t i = 0; i < particles_->points.size (); i++)
252  {
253  IndicesPtr indices (new std::vector<int>);
254  computeTransformedPointCloudWithNormal (particles_->points[i], *indices, *transed_reference_vector_[i]);
255  }
256 
257  PointCloudInPtr coherence_input (new PointCloudIn);
258  cropInputPointCloud (input_, *coherence_input);
259 
260  coherence_->setTargetCloud (coherence_input);
261  coherence_->initCompute ();
262  for (std::size_t i = 0; i < particles_->points.size (); i++)
263  {
264  IndicesPtr indices (new std::vector<int>);
265  coherence_->compute (transed_reference_vector_[i], indices, particles_->points[i].weight);
266  }
267  }
268 
269  normalizeWeight ();
270 }
271 
272 template <typename PointInT, typename StateT> void
274 (const StateT& hypothesis, std::vector<int>& indices, PointCloudIn &cloud)
275 {
276  if (use_normal_)
277  computeTransformedPointCloudWithNormal (hypothesis, indices, cloud);
278  else
279  computeTransformedPointCloudWithoutNormal (hypothesis, cloud);
280 }
281 
282 template <typename PointInT, typename StateT> void
284 (const StateT& hypothesis, PointCloudIn &cloud)
285 {
286  const Eigen::Affine3f trans = toEigenMatrix (hypothesis);
287  // destructively assigns to cloud
288  pcl::transformPointCloud<PointInT> (*ref_, cloud, trans);
289 }
290 
291 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
292 template <typename PointInT, typename StateT> void
294 #ifdef PCL_TRACKING_NORMAL_SUPPORTED
295  const StateT& hypothesis, std::vector<int>& indices, PointCloudIn &cloud)
296 #else
297  const StateT&, std::vector<int>&, PointCloudIn &)
298 #endif
299 {
300 #ifdef PCL_TRACKING_NORMAL_SUPPORTED
301  const Eigen::Affine3f trans = toEigenMatrix (hypothesis);
302  // destructively assigns to cloud
303  pcl::transformPointCloudWithNormals<PointInT> (*ref_, cloud, trans);
304  for ( std::size_t i = 0; i < cloud.points.size (); i++ )
305  {
306  PointInT input_point = cloud.points[i];
307 
308  if (!std::isfinite(input_point.x) || !std::isfinite(input_point.y) || !std::isfinite(input_point.z))
309  continue;
310  // take occlusion into account
311  Eigen::Vector4f p = input_point.getVector4fMap ();
312  Eigen::Vector4f n = input_point.getNormalVector4fMap ();
313  double theta = pcl::getAngle3D (p, n);
314  if ( theta > occlusion_angle_thr_ )
315  indices.push_back (i);
316  }
317 #else
318  PCL_WARN ("[pcl::%s::computeTransformedPointCloudWithoutNormal] use_normal_ == true is not supported in this Point Type.",
319  getClassName ().c_str ());
320 #endif
321 }
322 
323 template <typename PointInT, typename StateT> void
325 {
326  resampleWithReplacement ();
327 }
328 
329 template <typename PointInT, typename StateT> void
331 {
332  std::vector<int> a (particles_->points.size ());
333  std::vector<double> q (particles_->points.size ());
334  genAliasTable (a, q, particles_);
335 
336  const std::vector<double> zero_mean (StateT::stateDimension (), 0.0);
337  // memoize the original list of particles
338  PointCloudStatePtr origparticles = particles_;
339  particles_->points.clear ();
340  // the first particle, it is a just copy of the maximum result
341  StateT p = representative_state_;
342  particles_->points.push_back (p);
343 
344  // with motion
345  int motion_num = static_cast<int> (particles_->points.size ()) * static_cast<int> (motion_ratio_);
346  for ( int i = 1; i < motion_num; i++ )
347  {
348  int target_particle_index = sampleWithReplacement (a, q);
349  StateT p = origparticles->points[target_particle_index];
350  // add noise using gaussian
351  p.sample (zero_mean, step_noise_covariance_);
352  p = p + motion_;
353  particles_->points.push_back (p);
354  }
355 
356  // no motion
357  for ( int i = motion_num; i < particle_num_; i++ )
358  {
359  int target_particle_index = sampleWithReplacement (a, q);
360  StateT p = origparticles->points[target_particle_index];
361  // add noise using gaussian
362  p.sample (zero_mean, step_noise_covariance_);
363  particles_->points.push_back (p);
364  }
365 }
366 
367 template <typename PointInT, typename StateT> void
369 {
370 
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++)
375  {
376  StateT p = particles_->points[i];
377  representative_state_ = representative_state_ + p * p.weight;
378  }
379  representative_state_.weight = 1.0f / static_cast<float> (particles_->points.size ());
380  motion_ = representative_state_ - orig_representative;
381 }
382 
383 template <typename PointInT, typename StateT> void
385 {
386 
387  for (int i = 0; i < iteration_num_; i++)
388  {
389  if (changed_)
390  {
391  resample ();
392  }
393 
394  weight (); // likelihood is called in it
395 
396  if (changed_)
397  {
398  update ();
399  }
400  }
401 
402  // if ( getResult ().weight < resample_likelihood_thr_ )
403  // {
404  // PCL_WARN ("too small likelihood, re-initializing...\n");
405  // initParticles (false);
406  // }
407 }
408 
409 #define PCL_INSTANTIATE_ParticleFilterTracker(T,ST) template class PCL_EXPORTS pcl::tracking::ParticleFilterTracker<T,ST>;
410 
411 #endif
virtual void weight()
Weighting phase of particle filter method.
typename PointCloudState::Ptr PointCloudStatePtr
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. ...
Definition: common.hpp:46
typename Tracker< PointInT, StateT >::PointCloudState PointCloudState
virtual void normalizeWeight()
Normalize the weights of all the particels.
shared_ptr< Indices > IndicesPtr
Definition: pcl_base.h:61
void computeTransformedPointCloudWithoutNormal(const StateT &hypothesis, PointCloudIn &cloud)
Compute a reference pointcloud transformed to the pose that hypothesis represents and calculate indic...
int sampleWithReplacement(const std::vector< int > &a, const std::vector< double > &q)
Implementation of "sample with replacement" using Walker's alias method.
typename PointCloudIn::ConstPtr PointCloudInConstPtr
void genAliasTable(std::vector< int > &a, std::vector< double > &q, const PointCloudStateConstPtr &particles)
Generate the tables for walker's alias method.
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.
typename Tracker< PointInT, StateT >::PointCloudIn PointCloudIn
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...
Definition: common.hpp:242
bool initCompute() override
This method should get called before starting the actual computation.
virtual void update()
Calculate the weighted mean of the particles and set it as the result.
void computeTransformedPointCloud(const StateT &hypothesis, std::vector< int > &indices, PointCloudIn &cloud)
Compute a reference pointcloud transformed to the pose that hypothesis represents.
void cropInputPointCloud(const PointCloudInConstPtr &cloud, PointCloudIn &output)
Crop the pointcloud by the bounding box calculated from hypothesis and the reference pointcloud...
typename PointCloudIn::Ptr PointCloudInPtr
void computeTracking() override
Track the pointcloud using particle filter method.
typename PointCloudState::ConstPtr PointCloudStateConstPtr
Tracker represents the base tracker class.
Definition: tracker.h:57
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.
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.