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