Point Cloud Library (PCL) 1.14.0
Loading...
Searching...
No Matches
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
10namespace pcl {
11namespace tracking {
12template <typename PointInT, typename StateT>
13bool
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
40template <typename PointInT, typename StateT>
41int
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
56template <typename PointInT, typename StateT>
57void
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
92template <typename PointInT, typename StateT>
93void
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
115template <typename PointInT, typename StateT>
116void
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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
160template <typename PointInT, typename StateT>
161void
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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
185template <typename PointInT, typename StateT>
186void
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
216template <typename PointInT, typename StateT>
217bool
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
230template <typename PointInT, typename StateT>
231void
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
273template <typename PointInT, typename StateT>
274void
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
284template <typename PointInT, typename StateT>
285void
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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
295template <typename PointInT, typename StateT>
296template <typename PointT, pcl::traits::HasNormal<PointT>>
297void
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
319template <typename PointInT, typename StateT>
320void
322{
323 resampleWithReplacement();
324}
325
326template <typename PointInT, typename StateT>
327void
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
364template <typename PointInT, typename StateT>
365void
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
377template <typename PointInT, typename StateT>
378void
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