Point Cloud Library (PCL)  1.7.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
PointCloudState::ConstPtr PointCloudStateConstPtr
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:212
void genAliasTable(std::vector< int > &a, std::vector< double > &q, const PointCloudStateConstPtr &particles)
Generate the tables for walker's alias method.
void cropInputPointCloud(const PointCloudInConstPtr &cloud, PointCloudIn &output)
Crop the pointcloud by the bounding box calculated from hypothesis and the reference pointcloud...
PointCloudIn::ConstPtr PointCloudInConstPtr
void initParticles(bool reset)
Initialize the particles.
virtual void normalizeWeight()
Normalize the weights of all the particels.
virtual void computeTracking()
Track the pointcloud using particle filter method.
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...
Tracker represents the base tracker class.
Definition: tracker.h:56
void computeTransformedPointCloudWithoutNormal(const StateT &hypothesis, PointCloudIn &cloud)
Compute a reference pointcloud transformed to the pose that hypothesis represents and calculate indic...
virtual void weight()
Weighting phase of particle filter method.
bool testChangeDetection(const PointCloudInConstPtr &input)
Run change detection and return true if there is a change.
void computeTransformedPointCloud(const StateT &hypothesis, std::vector< int > &indices, PointCloudIn &cloud)
Compute a reference pointcloud transformed to the pose that hypothesis represents.
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.
virtual void resample()
Resampling phase of particle filter method.
virtual void update()
Calculate the weighted mean of the particles and set it as the result.
boost::shared_ptr< std::vector< int > > IndicesPtr
Definition: pcl_base.h:60
void resampleWithReplacement()
Resampling the particle with replacement.
PointCloudState::Ptr PointCloudStatePtr
int sampleWithReplacement(const std::vector< int > &a, const std::vector< double > &q)
Implementation of "sample with replacement" using Walker's alias method.
double getAngle3D(const Eigen::Vector4f &v1, const Eigen::Vector4f &v2)
Compute the smallest angle between two vectors in the [ 0, PI ) interval in 3D.
Definition: common.hpp:46
virtual bool initCompute()
This method should get called before starting the actual computation.