Point Cloud Library (PCL)  1.11.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/transforms.h>
6 #include <pcl/tracking/particle_filter.h>
7 
8 #include <random>
9 
10 namespace pcl {
11 namespace tracking {
12 template <typename PointInT, typename StateT>
13 bool
15 {
17  PCL_ERROR("[pcl::%s::initCompute] Init failed.\n", getClassName().c_str());
18  return (false);
19  }
20 
21  if (transed_reference_vector_.empty()) {
22  // only one time allocation
23  transed_reference_vector_.resize(particle_num_);
24  for (int i = 0; i < particle_num_; i++) {
25  transed_reference_vector_[i] = PointCloudInPtr(new PointCloudIn());
26  }
27  }
28 
29  coherence_->setTargetCloud(input_);
30 
31  if (!change_detector_)
33  change_detector_resolution_));
34 
35  if (!particles_ || particles_->points.empty())
36  initParticles(true);
37  return (true);
38 }
39 
40 template <typename PointInT, typename StateT>
41 int
43  const std::vector<int>& a, const std::vector<double>& q)
44 {
45  static std::mt19937 rng{std::random_device{}()};
46  std::uniform_real_distribution<> rd(0.0, 1.0);
47 
48  double rU = rd(rng) * static_cast<double>(particles_->size());
49  int k = static_cast<int>(rU);
50  rU -= k; /* rU - [rU] */
51  if (rU < q[k])
52  return k;
53  return a[k];
54 }
55 
56 template <typename PointInT, typename StateT>
57 void
59  std::vector<int>& a,
60  std::vector<double>& q,
61  const PointCloudStateConstPtr& particles)
62 {
63  /* generate an alias table, a and q */
64  std::vector<int> HL(particles->size());
65  std::vector<int>::iterator H = HL.begin();
66  std::vector<int>::iterator L = HL.end() - 1;
67  const auto num = particles->size();
68  for (std::size_t i = 0; i < num; i++)
69  q[i] = (*particles)[i].weight * static_cast<float>(num);
70  for (std::size_t i = 0; i < num; i++)
71  a[i] = static_cast<int>(i);
72  // setup H and L
73  for (std::size_t i = 0; i < num; i++)
74  if (q[i] >= 1.0)
75  *H++ = static_cast<int>(i);
76  else
77  *L-- = static_cast<int>(i);
78 
79  while (H != HL.begin() && L != HL.end() - 1) {
80  int j = *(L + 1);
81  int k = *(H - 1);
82  a[j] = k;
83  q[k] += q[j] - 1;
84  ++L;
85  if (q[k] < 1.0) {
86  *L-- = k;
87  --H;
88  }
89  }
90 }
91 
92 template <typename PointInT, typename StateT>
93 void
95 {
96  particles_.reset(new PointCloudState());
97  if (reset) {
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  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>
116 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 (const auto& point : *particles_) {
123  double weight = point.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  for (auto& point : *particles_) {
133  if (point.weight != 0.0) {
134  point.weight =
135  static_cast<float>(normalizeParticleWeight(point.weight, w_min, w_max));
136  }
137  }
138  }
139  else {
140  for (auto& point : *particles_)
141  point.weight = 1.0f / static_cast<float>(particles_->size());
142  }
143 
144  double sum = 0.0;
145  for (const auto& point : *particles_) {
146  sum += point.weight;
147  }
148 
149  if (sum != 0.0) {
150  for (auto& point : *particles_)
151  point.weight /= static_cast<float>(sum);
152  }
153  else {
154  for (auto& point : *particles_)
155  point.weight = 1.0f / static_cast<float>(particles_->size());
156  }
157 }
158 
159 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
160 template <typename PointInT, typename StateT>
161 void
163  const PointCloudInConstPtr&, PointCloudIn& output)
164 {
165  double x_min, y_min, z_min, x_max, y_max, z_max;
166  calcBoundingBox(x_min, x_max, y_min, y_max, z_min, z_max);
167  pass_x_.setFilterLimits(float(x_min), float(x_max));
168  pass_y_.setFilterLimits(float(y_min), float(y_max));
169  pass_z_.setFilterLimits(float(z_min), float(z_max));
170 
171  // x
172  PointCloudInPtr xcloud(new PointCloudIn);
173  pass_x_.setInputCloud(input_);
174  pass_x_.filter(*xcloud);
175  // y
176  PointCloudInPtr ycloud(new PointCloudIn);
177  pass_y_.setInputCloud(xcloud);
178  pass_y_.filter(*ycloud);
179  // z
180  pass_z_.setInputCloud(ycloud);
181  pass_z_.filter(output);
182 }
183 
184 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
185 template <typename PointInT, typename StateT>
186 void
188  double& x_max,
189  double& y_min,
190  double& y_max,
191  double& z_min,
192  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  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>
217 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,
225  change_detector_filter_);
226  change_detector_->switchBuffers();
227  return !newPointIdxVector.empty();
228 }
229 
230 template <typename PointInT, typename StateT>
231 void
233 {
234  if (!use_normal_) {
235  for (std::size_t i = 0; i < particles_->size(); i++) {
236  computeTransformedPointCloudWithoutNormal((*particles_)[i],
237  *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 (std::size_t i = 0; i < particles_->size(); i++) {
246  IndicesPtr indices;
247  coherence_->compute(
248  transed_reference_vector_[i], indices, (*particles_)[i].weight);
249  }
250  }
251  else {
252  for (std::size_t i = 0; i < particles_->size(); i++) {
253  IndicesPtr indices(new std::vector<int>);
254  computeTransformedPointCloudWithNormal(
255  (*particles_)[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_->size(); i++) {
264  IndicesPtr indices(new std::vector<int>);
265  coherence_->compute(
266  transed_reference_vector_[i], indices, (*particles_)[i].weight);
267  }
268  }
269 
270  normalizeWeight();
271 }
272 
273 template <typename PointInT, typename StateT>
274 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>
285 void
287  const StateT& hypothesis, PointCloudIn& cloud)
288 {
289  const Eigen::Affine3f trans = toEigenMatrix(hypothesis);
290  // destructively assigns to cloud
291  pcl::transformPointCloud<PointInT>(*ref_, cloud, trans);
292 }
293 
294 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
295 template <typename PointInT, typename StateT>
296 void
298 #ifdef PCL_TRACKING_NORMAL_SUPPORTED
299  const StateT& hypothesis, std::vector<int>& indices, PointCloudIn& cloud)
300 #else
301  const StateT&, std::vector<int>&, PointCloudIn&)
302 #endif
303 {
304 #ifdef PCL_TRACKING_NORMAL_SUPPORTED
305  const Eigen::Affine3f trans = toEigenMatrix(hypothesis);
306  // destructively assigns to cloud
307  pcl::transformPointCloudWithNormals<PointInT>(*ref_, cloud, trans);
308  for (std::size_t i = 0; i < cloud.size(); i++) {
309  PointInT input_point = cloud[i];
310 
311  if (!std::isfinite(input_point.x) || !std::isfinite(input_point.y) ||
312  !std::isfinite(input_point.z))
313  continue;
314  // take occlusion into account
315  Eigen::Vector4f p = input_point.getVector4fMap();
316  Eigen::Vector4f n = input_point.getNormalVector4fMap();
317  double theta = pcl::getAngle3D(p, n);
318  if (theta > occlusion_angle_thr_)
319  indices.push_back(i);
320  }
321 #else
322  PCL_WARN("[pcl::%s::computeTransformedPointCloudWithoutNormal] "
323  "use_normal_ == true is not supported in this Point Type.",
324  getClassName().c_str());
325 #endif
326 }
327 
328 template <typename PointInT, typename StateT>
329 void
331 {
332  resampleWithReplacement();
333 }
334 
335 template <typename PointInT, typename StateT>
336 void
338 {
339  std::vector<int> a(particles_->size());
340  std::vector<double> q(particles_->size());
341  genAliasTable(a, q, particles_);
342 
343  const std::vector<double> zero_mean(StateT::stateDimension(), 0.0);
344  // memoize the original list of particles
345  PointCloudStatePtr origparticles = particles_;
346  particles_->points.clear();
347  // the first particle, it is a just copy of the maximum result
348  StateT p = representative_state_;
349  particles_->points.push_back(p);
350 
351  // with motion
352  int motion_num =
353  static_cast<int>(particles_->size()) * static_cast<int>(motion_ratio_);
354  for (int i = 1; i < motion_num; i++) {
355  int target_particle_index = sampleWithReplacement(a, q);
356  StateT p = (*origparticles)[target_particle_index];
357  // add noise using gaussian
358  p.sample(zero_mean, step_noise_covariance_);
359  p = p + motion_;
360  particles_->points.push_back(p);
361  }
362 
363  // no motion
364  for (int i = motion_num; i < particle_num_; i++) {
365  int target_particle_index = sampleWithReplacement(a, q);
366  StateT p = (*origparticles)[target_particle_index];
367  // add noise using gaussian
368  p.sample(zero_mean, step_noise_covariance_);
369  particles_->points.push_back(p);
370  }
371 }
372 
373 template <typename PointInT, typename StateT>
374 void
376 {
377 
378  StateT orig_representative = representative_state_;
379  representative_state_.zero();
380  representative_state_.weight = 0.0;
381  for (const auto& p : *particles_) {
382  representative_state_ = representative_state_ + p * p.weight;
383  }
384  representative_state_.weight = 1.0f / static_cast<float>(particles_->size());
385  motion_ = representative_state_ - orig_representative;
386 }
387 
388 template <typename PointInT, typename StateT>
389 void
391 {
392 
393  for (int i = 0; i < iteration_num_; i++) {
394  if (changed_) {
395  resample();
396  }
397 
398  weight(); // likelihood is called in it
399 
400  if (changed_) {
401  update();
402  }
403  }
404 
405  // if ( getResult ().weight < resample_likelihood_thr_ )
406  // {
407  // PCL_WARN ("too small likelihood, re-initializing...\n");
408  // initParticles (false);
409  // }
410 }
411 } // namespace tracking
412 } // namespace pcl
413 
414 #define PCL_INSTANTIATE_ParticleFilterTracker(T, ST) \
415  template class PCL_EXPORTS pcl::tracking::ParticleFilterTracker<T, ST>;
416 
417 #endif
void genAliasTable(std::vector< int > &a, std::vector< double > &q, const PointCloudStateConstPtr &particles)
Generate the tables for walker's alias method.
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 initParticles(bool reset)
Initialize the particles.
int sampleWithReplacement(const std::vector< int > &a, const std::vector< double > &q)
Implementation of "sample with replacement" using Walker's alias method.
void computeTransformedPointCloud(const StateT &hypothesis, std::vector< int > &indices, PointCloudIn &cloud)
Compute a reference pointcloud transformed to the pose that hypothesis represents.
bool testChangeDetection(const PointCloudInConstPtr &input)
Run change detection and return true if there is a change.
typename PointCloudIn::ConstPtr PointCloudInConstPtr
void resampleWithReplacement()
Resampling the particle with replacement.
typename PointCloudState::Ptr PointCloudStatePtr
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 computeTransformedPointCloudWithoutNormal(const StateT &hypothesis, PointCloudIn &cloud)
Compute a reference pointcloud transformed to the pose that hypothesis represents and calculate indic...
virtual void resample()
Resampling phase of particle filter method.
typename PointCloudState::ConstPtr PointCloudStateConstPtr
void computeTracking() override
Track the pointcloud using particle filter method.
void cropInputPointCloud(const PointCloudInConstPtr &cloud, PointCloudIn &output)
Crop the pointcloud by the bounding box calculated from hypothesis and the reference pointcloud.
virtual void update()
Calculate the weighted mean of the particles and set it as the result.
virtual void weight()
Weighting phase of particle filter method.
bool initCompute() override
This method should get called before starting the actua computation.
typename PointCloudIn::Ptr PointCloudInPtr
typename Tracker< PointInT, StateT >::PointCloudState PointCloudState
typename Tracker< PointInT, StateT >::PointCloudIn PointCloudIn
virtual void normalizeWeight()
Normalize the weights of all the particels.
Tracker represents the base tracker class.
Definition: tracker.h:55
Define standard C methods and C++ classes that are common to all methods.
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:243
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:47
shared_ptr< Indices > IndicesPtr
Definition: pcl_base.h:61