Point Cloud Library (PCL)  1.14.1-dev
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  auto H = HL.begin();
66  auto 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(static_cast<float>(x_min), static_cast<float>(x_max));
168  pass_y_.setFilterLimits(static_cast<float>(y_min), static_cast<float>(y_max));
169  pass_z_.setFilterLimits(static_cast<float>(z_min), static_cast<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  pcl::Indices 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 pcl::Indices);
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 pcl::Indices);
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, pcl::Indices& 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 template <typename PointT, pcl::traits::HasNormal<PointT>>
297 void
299  const StateT& hypothesis, pcl::Indices& indices, PointCloudIn& cloud)
300 {
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.size(); i++) {
305  PointInT input_point = cloud[i];
306 
307  if (!std::isfinite(input_point.x) || !std::isfinite(input_point.y) ||
308  !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 }
318 
319 template <typename PointInT, typename StateT>
320 void
322 {
323  resampleWithReplacement();
324 }
325 
326 template <typename PointInT, typename StateT>
327 void
329 {
330  std::vector<int> a(particles_->size());
331  std::vector<double> q(particles_->size());
332  genAliasTable(a, q, particles_);
333 
334  const std::vector<double> zero_mean(StateT::stateDimension(), 0.0);
335  // memoize the original list of particles
336  PointCloudStatePtr origparticles = particles_;
337  particles_->points.clear();
338  // the first particle, it is a just copy of the maximum result
339  StateT p = representative_state_;
340  particles_->points.push_back(p);
341 
342  // with motion
343  int motion_num =
344  static_cast<int>(particles_->size()) * static_cast<int>(motion_ratio_);
345  for (int i = 1; i < motion_num; i++) {
346  int target_particle_index = sampleWithReplacement(a, q);
347  StateT p = (*origparticles)[target_particle_index];
348  // add noise using gaussian
349  p.sample(zero_mean, step_noise_covariance_);
350  p = p + motion_;
351  particles_->points.push_back(p);
352  }
353 
354  // no motion
355  for (int i = motion_num; i < particle_num_; i++) {
356  int target_particle_index = sampleWithReplacement(a, q);
357  StateT p = (*origparticles)[target_particle_index];
358  // add noise using gaussian
359  p.sample(zero_mean, step_noise_covariance_);
360  particles_->points.push_back(p);
361  }
362 }
363 
364 template <typename PointInT, typename StateT>
365 void
367 {
368  StateT orig_representative = representative_state_;
369  representative_state_.zero();
370  representative_state_.weight = 0.0;
371  representative_state_ =
372  StateT::weightedAverage(particles_->begin(), particles_->end());
373  representative_state_.weight = 1.0f / static_cast<float>(particles_->size());
374  motion_ = representative_state_ - orig_representative;
375 }
376 
377 template <typename PointInT, typename StateT>
378 void
380 {
381 
382  for (int i = 0; i < iteration_num_; i++) {
383  if (changed_) {
384  resample();
385  }
386 
387  weight(); // likelihood is called in it
388 
389  if (changed_) {
390  update();
391  }
392  }
393 
394  // if ( getResult ().weight < resample_likelihood_thr_ )
395  // {
396  // PCL_WARN ("too small likelihood, re-initializing...\n");
397  // initParticles (false);
398  // }
399 }
400 } // namespace tracking
401 } // namespace pcl
402 
403 #define PCL_INSTANTIATE_ParticleFilterTracker(T, ST) \
404  template class PCL_EXPORTS pcl::tracking::ParticleFilterTracker<T, ST>;
405 
406 #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.
bool testChangeDetection(const PointCloudInConstPtr &input)
Run change detection and return true if there is a change.
typename PointCloudIn::ConstPtr PointCloudInConstPtr
void computeTransformedPointCloud(const StateT &hypothesis, pcl::Indices &indices, PointCloudIn &cloud)
Compute a reference pointcloud transformed to the pose that hypothesis represents.
void computeTransformedPointCloudWithNormal(const StateT &hypothesis, pcl::Indices &indices, PointCloudIn &cloud)
Compute a reference pointcloud transformed to the pose that hypothesis represents and calculate indic...
void resampleWithReplacement()
Resampling the particle with replacement.
typename PointCloudState::Ptr PointCloudStatePtr
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 actual 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:295
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
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
shared_ptr< Indices > IndicesPtr
Definition: pcl_base.h:58