Point Cloud Library (PCL)  1.10.0
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  std::vector<double> initial_noise_mean;
95  if (reset)
96  {
97  representative_state_.zero ();
98  StateT offset = StateT::toState (trans_);
99  representative_state_ = offset;
100  representative_state_.weight = 1.0f / static_cast<float> (particle_num_);
101  }
102 
103  // sampling...
104  for ( int i = 0; i < particle_num_; i++ )
105  {
106  StateT p;
107  p.zero ();
108  p.sample (initial_noise_mean_, initial_noise_covariance_);
109  p = p + representative_state_;
110  p.weight = 1.0f / static_cast<float> (particle_num_);
111  particles_->points.push_back (p); // update
112  }
113 }
114 
115 template <typename PointInT, typename StateT> void
117 {
118  // apply exponential function
119  double w_min = std::numeric_limits<double>::max ();
120  double w_max = - std::numeric_limits<double>::max ();
121  for ( std::size_t i = 0; i < particles_->points.size (); i++ )
122  {
123  double weight = particles_->points[i].weight;
124  if (w_min > weight)
125  w_min = weight;
126  if (weight != 0.0 && w_max < weight)
127  w_max = weight;
128  }
129 
130  fit_ratio_ = w_min;
131  if (w_max != w_min)
132  {
133  for ( std::size_t i = 0; i < particles_->points.size (); i++ )
134  {
135  if (particles_->points[i].weight != 0.0)
136  {
137  particles_->points[i].weight = static_cast<float> (normalizeParticleWeight (particles_->points[i].weight, w_min, w_max));
138  }
139  }
140  }
141  else
142  {
143  for ( std::size_t i = 0; i < particles_->points.size (); i++ )
144  particles_->points[i].weight = 1.0f / static_cast<float> (particles_->points.size ());
145  }
146 
147  double sum = 0.0;
148  for ( std::size_t i = 0; i < particles_->points.size (); i++ )
149  {
150  sum += particles_->points[i].weight;
151  }
152 
153  if (sum != 0.0)
154  {
155  for ( std::size_t i = 0; i < particles_->points.size (); i++ )
156  particles_->points[i].weight /= static_cast<float> (sum);
157  }
158  else
159  {
160  for ( std::size_t i = 0; i < particles_->points.size (); i++ )
161  particles_->points[i].weight = 1.0f / static_cast<float> (particles_->points.size ());
162  }
163 }
164 
165 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
166 template <typename PointInT, typename StateT> void
168  const PointCloudInConstPtr &, PointCloudIn &output)
169 {
170  double x_min, y_min, z_min, x_max, y_max, z_max;
171  calcBoundingBox (x_min, x_max, y_min, y_max, z_min, z_max);
172  pass_x_.setFilterLimits (float (x_min), float (x_max));
173  pass_y_.setFilterLimits (float (y_min), float (y_max));
174  pass_z_.setFilterLimits (float (z_min), float (z_max));
175 
176  // x
177  PointCloudInPtr xcloud (new PointCloudIn);
178  pass_x_.setInputCloud (input_);
179  pass_x_.filter (*xcloud);
180  // y
181  PointCloudInPtr ycloud (new PointCloudIn);
182  pass_y_.setInputCloud (xcloud);
183  pass_y_.filter (*ycloud);
184  // z
185  pass_z_.setInputCloud (ycloud);
186  pass_z_.filter (output);
187 }
188 
189 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
190 template <typename PointInT, typename StateT> void
192  double &x_min, double &x_max, double &y_min, double &y_max, double &z_min, double &z_max)
193 {
194  x_min = y_min = z_min = std::numeric_limits<double>::max ();
195  x_max = y_max = z_max = - std::numeric_limits<double>::max ();
196 
197  for (std::size_t i = 0; i < transed_reference_vector_.size (); i++)
198  {
199  PointCloudInPtr target = transed_reference_vector_[i];
200  PointInT Pmin, Pmax;
201  pcl::getMinMax3D (*target, Pmin, Pmax);
202  if (x_min > Pmin.x)
203  x_min = Pmin.x;
204  if (x_max < Pmax.x)
205  x_max = Pmax.x;
206  if (y_min > Pmin.y)
207  y_min = Pmin.y;
208  if (y_max < Pmax.y)
209  y_max = Pmax.y;
210  if (z_min > Pmin.z)
211  z_min = Pmin.z;
212  if (z_max < Pmax.z)
213  z_max = Pmax.z;
214  }
215 }
216 
217 template <typename PointInT, typename StateT> bool
219 (const PointCloudInConstPtr &input)
220 {
221  change_detector_->setInputCloud (input);
222  change_detector_->addPointsFromInputCloud ();
223  std::vector<int> newPointIdxVector;
224  change_detector_->getPointIndicesFromNewVoxels (newPointIdxVector, change_detector_filter_);
225  change_detector_->switchBuffers ();
226  return !newPointIdxVector.empty ();
227 }
228 
229 template <typename PointInT, typename StateT> void
231 {
232  if (!use_normal_)
233  {
234  for (std::size_t i = 0; i < particles_->points.size (); i++)
235  {
236  computeTransformedPointCloudWithoutNormal (particles_->points[i], *transed_reference_vector_[i]);
237  }
238 
239  PointCloudInPtr coherence_input (new PointCloudIn);
240  cropInputPointCloud (input_, *coherence_input);
241 
242  coherence_->setTargetCloud (coherence_input);
243  coherence_->initCompute ();
244  for (std::size_t i = 0; i < particles_->points.size (); i++)
245  {
246  IndicesPtr indices;
247  coherence_->compute (transed_reference_vector_[i], indices, particles_->points[i].weight);
248  }
249  }
250  else
251  {
252  for (std::size_t i = 0; i < particles_->points.size (); i++)
253  {
254  IndicesPtr indices (new std::vector<int>);
255  computeTransformedPointCloudWithNormal (particles_->points[i], *indices, *transed_reference_vector_[i]);
256  }
257 
258  PointCloudInPtr coherence_input (new PointCloudIn);
259  cropInputPointCloud (input_, *coherence_input);
260 
261  coherence_->setTargetCloud (coherence_input);
262  coherence_->initCompute ();
263  for (std::size_t i = 0; i < particles_->points.size (); i++)
264  {
265  IndicesPtr indices (new std::vector<int>);
266  coherence_->compute (transed_reference_vector_[i], indices, particles_->points[i].weight);
267  }
268  }
269 
270  normalizeWeight ();
271 }
272 
273 template <typename PointInT, typename StateT> void
275 (const StateT& hypothesis, std::vector<int>& indices, PointCloudIn &cloud)
276 {
277  if (use_normal_)
278  computeTransformedPointCloudWithNormal (hypothesis, indices, cloud);
279  else
280  computeTransformedPointCloudWithoutNormal (hypothesis, cloud);
281 }
282 
283 template <typename PointInT, typename StateT> void
285 (const StateT& hypothesis, PointCloudIn &cloud)
286 {
287  const Eigen::Affine3f trans = toEigenMatrix (hypothesis);
288  // destructively assigns to cloud
289  pcl::transformPointCloud<PointInT> (*ref_, cloud, trans);
290 }
291 
292 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
293 template <typename PointInT, typename StateT> void
295 #ifdef PCL_TRACKING_NORMAL_SUPPORTED
296  const StateT& hypothesis, std::vector<int>& indices, PointCloudIn &cloud)
297 #else
298  const StateT&, std::vector<int>&, PointCloudIn &)
299 #endif
300 {
301 #ifdef PCL_TRACKING_NORMAL_SUPPORTED
302  const Eigen::Affine3f trans = toEigenMatrix (hypothesis);
303  // destructively assigns to cloud
304  pcl::transformPointCloudWithNormals<PointInT> (*ref_, cloud, trans);
305  for ( std::size_t i = 0; i < cloud.points.size (); i++ )
306  {
307  PointInT input_point = cloud.points[i];
308 
309  if (!std::isfinite(input_point.x) || !std::isfinite(input_point.y) || !std::isfinite(input_point.z))
310  continue;
311  // take occlusion into account
312  Eigen::Vector4f p = input_point.getVector4fMap ();
313  Eigen::Vector4f n = input_point.getNormalVector4fMap ();
314  double theta = pcl::getAngle3D (p, n);
315  if ( theta > occlusion_angle_thr_ )
316  indices.push_back (i);
317  }
318 #else
319  PCL_WARN ("[pcl::%s::computeTransformedPointCloudWithoutNormal] use_normal_ == true is not supported in this Point Type.",
320  getClassName ().c_str ());
321 #endif
322 }
323 
324 template <typename PointInT, typename StateT> void
326 {
327  resampleWithReplacement ();
328 }
329 
330 template <typename PointInT, typename StateT> void
332 {
333  std::vector<int> a (particles_->points.size ());
334  std::vector<double> q (particles_->points.size ());
335  genAliasTable (a, q, particles_);
336 
337  const std::vector<double> zero_mean (StateT::stateDimension (), 0.0);
338  // memoize the original list of particles
339  PointCloudStatePtr origparticles = particles_;
340  particles_->points.clear ();
341  // the first particle, it is a just copy of the maximum result
342  StateT p = representative_state_;
343  particles_->points.push_back (p);
344 
345  // with motion
346  int motion_num = static_cast<int> (particles_->points.size ()) * static_cast<int> (motion_ratio_);
347  for ( int i = 1; i < motion_num; i++ )
348  {
349  int target_particle_index = sampleWithReplacement (a, q);
350  StateT p = origparticles->points[target_particle_index];
351  // add noise using gaussian
352  p.sample (zero_mean, step_noise_covariance_);
353  p = p + motion_;
354  particles_->points.push_back (p);
355  }
356 
357  // no motion
358  for ( int i = motion_num; i < particle_num_; i++ )
359  {
360  int target_particle_index = sampleWithReplacement (a, q);
361  StateT p = origparticles->points[target_particle_index];
362  // add noise using gaussian
363  p.sample (zero_mean, step_noise_covariance_);
364  particles_->points.push_back (p);
365  }
366 }
367 
368 template <typename PointInT, typename StateT> void
370 {
371 
372  StateT orig_representative = representative_state_;
373  representative_state_.zero ();
374  representative_state_.weight = 0.0;
375  for ( std::size_t i = 0; i < particles_->points.size (); i++)
376  {
377  StateT p = particles_->points[i];
378  representative_state_ = representative_state_ + p * p.weight;
379  }
380  representative_state_.weight = 1.0f / static_cast<float> (particles_->points.size ());
381  motion_ = representative_state_ - orig_representative;
382 }
383 
384 template <typename PointInT, typename StateT> void
386 {
387 
388  for (int i = 0; i < iteration_num_; i++)
389  {
390  if (changed_)
391  {
392  resample ();
393  }
394 
395  weight (); // likelihood is called in it
396 
397  if (changed_)
398  {
399  update ();
400  }
401  }
402 
403  // if ( getResult ().weight < resample_likelihood_thr_ )
404  // {
405  // PCL_WARN ("too small likelihood, re-initializing...\n");
406  // initParticles (false);
407  // }
408 }
409 
410 #define PCL_INSTANTIATE_ParticleFilterTracker(T,ST) template class PCL_EXPORTS pcl::tracking::ParticleFilterTracker<T,ST>;
411 
412 #endif
pcl::tracking::ParticleFilterTracker::update
virtual void update()
Calculate the weighted mean of the particles and set it as the result.
Definition: particle_filter.hpp:369
pcl::IndicesPtr
shared_ptr< Indices > IndicesPtr
Definition: pcl_base.h:61
common.h
pcl::tracking::ParticleFilterTracker::computeTracking
void computeTracking() override
Track the pointcloud using particle filter method.
Definition: particle_filter.hpp:385
pcl::tracking::ParticleFilterTracker::computeTransformedPointCloud
void computeTransformedPointCloud(const StateT &hypothesis, std::vector< int > &indices, PointCloudIn &cloud)
Compute a reference pointcloud transformed to the pose that hypothesis represents.
Definition: particle_filter.hpp:275
pcl::tracking::ParticleFilterTracker::computeTransformedPointCloudWithNormal
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...
Definition: particle_filter.hpp:294
pcl::tracking::ParticleFilterTracker::resampleWithReplacement
void resampleWithReplacement()
Resampling the particle with replacement.
Definition: particle_filter.hpp:331
pcl::tracking::ParticleFilterTracker::genAliasTable
void genAliasTable(std::vector< int > &a, std::vector< double > &q, const PointCloudStateConstPtr &particles)
Generate the tables for walker's alias method.
Definition: particle_filter.hpp:56
pcl::tracking::ParticleFilterTracker::PointCloudIn
typename Tracker< PointInT, StateT >::PointCloudIn PointCloudIn
Definition: particle_filter.h:40
pcl::tracking::ParticleFilterTracker::sampleWithReplacement
int sampleWithReplacement(const std::vector< int > &a, const std::vector< double > &q)
Implementation of "sample with replacement" using Walker's alias method.
Definition: particle_filter.hpp:42
pcl::getAngle3D
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
pcl::tracking::ParticleFilterTracker::resample
virtual void resample()
Resampling phase of particle filter method.
Definition: particle_filter.hpp:325
pcl::tracking::ParticleFilterTracker::testChangeDetection
bool testChangeDetection(const PointCloudInConstPtr &input)
Run change detection and return true if there is a change.
Definition: particle_filter.hpp:219
pcl::tracking::ParticleFilterTracker::cropInputPointCloud
void cropInputPointCloud(const PointCloudInConstPtr &cloud, PointCloudIn &output)
Crop the pointcloud by the bounding box calculated from hypothesis and the reference pointcloud.
Definition: particle_filter.hpp:167
pcl::tracking::ParticleFilterTracker::weight
virtual void weight()
Weighting phase of particle filter method.
Definition: particle_filter.hpp:230
pcl::tracking::ParticleFilterTracker::initParticles
void initParticles(bool reset)
Initialize the particles.
Definition: particle_filter.hpp:91
pcl::tracking::ParticleFilterTracker::PointCloudInConstPtr
typename PointCloudIn::ConstPtr PointCloudInConstPtr
Definition: particle_filter.h:42
pcl::tracking::ParticleFilterTracker::PointCloudState
typename Tracker< PointInT, StateT >::PointCloudState PointCloudState
Definition: particle_filter.h:44
pcl::tracking::ParticleFilterTracker::PointCloudInPtr
typename PointCloudIn::Ptr PointCloudInPtr
Definition: particle_filter.h:41
pcl::getMinMax3D
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
pcl::octree::OctreePointCloudChangeDetector< PointInT >
pcl::tracking::ParticleFilterTracker::normalizeWeight
virtual void normalizeWeight()
Normalize the weights of all the particels.
Definition: particle_filter.hpp:116
pcl::tracking::ParticleFilterTracker::computeTransformedPointCloudWithoutNormal
void computeTransformedPointCloudWithoutNormal(const StateT &hypothesis, PointCloudIn &cloud)
Compute a reference pointcloud transformed to the pose that hypothesis represents and calculate indic...
Definition: particle_filter.hpp:285
pcl::tracking::ParticleFilterTracker::calcBoundingBox
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.
Definition: particle_filter.hpp:191
pcl::tracking::ParticleFilterTracker::PointCloudStatePtr
typename PointCloudState::Ptr PointCloudStatePtr
Definition: particle_filter.h:45
pcl::tracking::ParticleFilterTracker::PointCloudStateConstPtr
typename PointCloudState::ConstPtr PointCloudStateConstPtr
Definition: particle_filter.h:46
pcl::tracking::Tracker
Tracker represents the base tracker class.
Definition: tracker.h:56
pcl::tracking::ParticleFilterTracker::initCompute
bool initCompute() override
This method should get called before starting the actual computation.
Definition: particle_filter.hpp:12