Point Cloud Library (PCL)  1.11.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  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  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 
369  StateT orig_representative = representative_state_;
370  representative_state_.zero();
371  representative_state_.weight = 0.0;
372  for (const auto& p : *particles_) {
373  representative_state_ = representative_state_ + p * p.weight;
374  }
375  representative_state_.weight = 1.0f / static_cast<float>(particles_->size());
376  motion_ = representative_state_ - orig_representative;
377 }
378 
379 template <typename PointInT, typename StateT>
380 void
382 {
383 
384  for (int i = 0; i < iteration_num_; i++) {
385  if (changed_) {
386  resample();
387  }
388 
389  weight(); // likelihood is called in it
390 
391  if (changed_) {
392  update();
393  }
394  }
395 
396  // if ( getResult ().weight < resample_likelihood_thr_ )
397  // {
398  // PCL_WARN ("too small likelihood, re-initializing...\n");
399  // initParticles (false);
400  // }
401 }
402 } // namespace tracking
403 } // namespace pcl
404 
405 #define PCL_INSTANTIATE_ParticleFilterTracker(T, ST) \
406  template class PCL_EXPORTS pcl::tracking::ParticleFilterTracker<T, ST>;
407 
408 #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:366
pcl
Definition: convolution.h:46
pcl::IndicesPtr
shared_ptr< Indices > IndicesPtr
Definition: pcl_base.h:58
common.h
pcl::tracking::ParticleFilterTracker::computeTracking
void computeTracking() override
Track the pointcloud using particle filter method.
Definition: particle_filter.hpp:381
pcl::tracking::ParticleFilterTracker::resampleWithReplacement
void resampleWithReplacement()
Resampling the particle with replacement.
Definition: particle_filter.hpp:328
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:58
pcl::tracking::ParticleFilterTracker::PointCloudIn
typename Tracker< PointInT, StateT >::PointCloudIn PointCloudIn
Definition: particle_filter.h:35
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:47
pcl::tracking::ParticleFilterTracker::resample
virtual void resample()
Resampling phase of particle filter method.
Definition: particle_filter.hpp:321
pcl::tracking::ParticleFilterTracker::computeTransformedPointCloudWithNormal
void computeTransformedPointCloudWithNormal(const StateT &hypothesis, pcl::Indices &indices, PointCloudIn &cloud)
Compute a reference pointcloud transformed to the pose that hypothesis represents and calculate indic...
Definition: particle_filter.hpp:298
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:162
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
pcl::tracking::ParticleFilterTracker::weight
virtual void weight()
Weighting phase of particle filter method.
Definition: particle_filter.hpp:232
pcl::tracking::ParticleFilterTracker::initParticles
void initParticles(bool reset)
Initialize the particles.
Definition: particle_filter.hpp:94
pcl::tracking::ParticleFilterTracker::PointCloudInConstPtr
typename PointCloudIn::ConstPtr PointCloudInConstPtr
Definition: particle_filter.h:37
pcl::tracking::ParticleFilterTracker::computeTransformedPointCloud
void computeTransformedPointCloud(const StateT &hypothesis, pcl::Indices &indices, PointCloudIn &cloud)
Compute a reference pointcloud transformed to the pose that hypothesis represents.
Definition: particle_filter.hpp:275
pcl::tracking::ParticleFilterTracker::PointCloudState
typename Tracker< PointInT, StateT >::PointCloudState PointCloudState
Definition: particle_filter.h:39
pcl::tracking::ParticleFilterTracker::PointCloudInPtr
typename PointCloudIn::Ptr PointCloudInPtr
Definition: particle_filter.h:36
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:295
pcl::octree::OctreePointCloudChangeDetector< PointInT >
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:187
pcl::tracking::ParticleFilterTracker::PointCloudStatePtr
typename PointCloudState::Ptr PointCloudStatePtr
Definition: particle_filter.h:40
pcl::tracking::ParticleFilterTracker::PointCloudStateConstPtr
typename PointCloudState::ConstPtr PointCloudStateConstPtr
Definition: particle_filter.h:41
pcl::tracking::Tracker
Tracker represents the base tracker class.
Definition: tracker.h:55
pcl::tracking::ParticleFilterTracker::initCompute
bool initCompute() override
This method should get called before starting the actua computation.
Definition: particle_filter.hpp:14