Point Cloud Library (PCL)  1.11.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  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
pcl::tracking::ParticleFilterTracker::update
virtual void update()
Calculate the weighted mean of the particles and set it as the result.
Definition: particle_filter.hpp:368
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:384
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:274
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:293
pcl::tracking::ParticleFilterTracker::resampleWithReplacement
void resampleWithReplacement()
Resampling the particle with replacement.
Definition: particle_filter.hpp:330
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:324
pcl::tracking::ParticleFilterTracker::testChangeDetection
bool testChangeDetection(const PointCloudInConstPtr &input)
Run change detection and return true if there is a change.
Definition: particle_filter.hpp:218
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:166
pcl::tracking::ParticleFilterTracker::weight
virtual void weight()
Weighting phase of particle filter method.
Definition: particle_filter.hpp:229
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:115
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:284
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:190
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:57
pcl::tracking::ParticleFilterTracker::initCompute
bool initCompute() override
This method should get called before starting the actual computation.
Definition: particle_filter.hpp:12