1 #ifndef PCL_TRACKING_IMPL_PARTICLE_FILTER_H_
2 #define PCL_TRACKING_IMPL_PARTICLE_FILTER_H_
7 #include <pcl/common/eigen.h>
8 #include <pcl/common/transforms.h>
9 #include <pcl/tracking/particle_filter.h>
11 template <
typename Po
intInT,
typename StateT>
bool
16 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
20 if (transed_reference_vector_.empty ())
23 transed_reference_vector_.resize (particle_num_);
24 for (
int i = 0; i < particle_num_; i++)
30 coherence_->setTargetCloud (input_);
32 if (!change_detector_)
35 if (!particles_ || particles_->points.empty ())
40 template <
typename Po
intInT,
typename StateT>
int
42 (
const std::vector<int>& a,
const std::vector<double>& q)
44 static std::mt19937 rng([] { std::random_device rd;
return rd(); } ());
45 std::uniform_real_distribution<> rd (0.0, 1.0);
47 double rU = rd (rng) *
static_cast<double> (particles_->points.size ());
48 int k =
static_cast<int> (rU);
55 template <
typename Po
intInT,
typename StateT>
void
60 std::vector<int> HL (particles->points.size ());
61 std::vector<int>::iterator H = HL.begin ();
62 std::vector<int>::iterator L = HL.end () - 1;
63 std::size_t num = particles->points.size ();
64 for ( std::size_t i = 0; i < num; i++ )
65 q[i] = particles->points[i].weight *
static_cast<float> (num);
66 for ( std::size_t i = 0; i < num; i++ )
67 a[i] =
static_cast<int> (i);
69 for ( std::size_t i = 0; i < num; i++ )
71 *H++ =
static_cast<int> (i);
73 *L-- =
static_cast<int> (i);
75 while ( H != HL.begin() && L != HL.end() - 1 )
90 template <
typename Po
intInT,
typename StateT>
void
96 representative_state_.zero ();
97 StateT offset = StateT::toState (trans_);
98 representative_state_ = offset;
99 representative_state_.weight = 1.0f /
static_cast<float> (particle_num_);
103 for (
int i = 0; i < particle_num_; i++ )
107 p.sample (initial_noise_mean_, initial_noise_covariance_);
108 p = p + representative_state_;
109 p.weight = 1.0f /
static_cast<float> (particle_num_);
110 particles_->points.push_back (p);
114 template <
typename Po
intInT,
typename StateT>
void
118 double w_min = std::numeric_limits<double>::max ();
119 double w_max = - std::numeric_limits<double>::max ();
120 for ( std::size_t i = 0; i < particles_->points.size (); i++ )
122 double weight = particles_->points[i].weight;
125 if (weight != 0.0 && w_max < weight)
132 for ( std::size_t i = 0; i < particles_->points.size (); i++ )
134 if (particles_->points[i].weight != 0.0)
136 particles_->points[i].weight =
static_cast<float> (normalizeParticleWeight (particles_->points[i].weight, w_min, w_max));
142 for ( std::size_t i = 0; i < particles_->points.size (); i++ )
143 particles_->points[i].weight = 1.0f /
static_cast<float> (particles_->points.size ());
147 for ( std::size_t i = 0; i < particles_->points.size (); i++ )
149 sum += particles_->points[i].weight;
154 for ( std::size_t i = 0; i < particles_->points.size (); i++ )
155 particles_->points[i].weight /=
static_cast<float> (sum);
159 for ( std::size_t i = 0; i < particles_->points.size (); i++ )
160 particles_->points[i].weight = 1.0f /
static_cast<float> (particles_->points.size ());
165 template <
typename Po
intInT,
typename StateT>
void
169 double x_min, y_min, z_min, x_max, y_max, z_max;
170 calcBoundingBox (x_min, x_max, y_min, y_max, z_min, z_max);
171 pass_x_.setFilterLimits (
float (x_min),
float (x_max));
172 pass_y_.setFilterLimits (
float (y_min),
float (y_max));
173 pass_z_.setFilterLimits (
float (z_min),
float (z_max));
177 pass_x_.setInputCloud (input_);
178 pass_x_.filter (*xcloud);
181 pass_y_.setInputCloud (xcloud);
182 pass_y_.filter (*ycloud);
184 pass_z_.setInputCloud (ycloud);
185 pass_z_.filter (output);
189 template <
typename Po
intInT,
typename StateT>
void
191 double &x_min,
double &x_max,
double &y_min,
double &y_max,
double &z_min,
double &z_max)
193 x_min = y_min = z_min = std::numeric_limits<double>::max ();
194 x_max = y_max = z_max = - std::numeric_limits<double>::max ();
196 for (std::size_t i = 0; i < transed_reference_vector_.size (); i++)
216 template <
typename Po
intInT,
typename StateT>
bool
220 change_detector_->setInputCloud (input);
221 change_detector_->addPointsFromInputCloud ();
222 std::vector<int> newPointIdxVector;
223 change_detector_->getPointIndicesFromNewVoxels (newPointIdxVector, change_detector_filter_);
224 change_detector_->switchBuffers ();
225 return !newPointIdxVector.empty ();
228 template <
typename Po
intInT,
typename StateT>
void
233 for (std::size_t i = 0; i < particles_->points.size (); i++)
235 computeTransformedPointCloudWithoutNormal (particles_->points[i], *transed_reference_vector_[i]);
239 cropInputPointCloud (input_, *coherence_input);
241 coherence_->setTargetCloud (coherence_input);
242 coherence_->initCompute ();
243 for (std::size_t i = 0; i < particles_->points.size (); i++)
246 coherence_->compute (transed_reference_vector_[i], indices, particles_->points[i].weight);
251 for (std::size_t i = 0; i < particles_->points.size (); i++)
254 computeTransformedPointCloudWithNormal (particles_->points[i], *indices, *transed_reference_vector_[i]);
258 cropInputPointCloud (input_, *coherence_input);
260 coherence_->setTargetCloud (coherence_input);
261 coherence_->initCompute ();
262 for (std::size_t i = 0; i < particles_->points.size (); i++)
265 coherence_->compute (transed_reference_vector_[i], indices, particles_->points[i].weight);
272 template <
typename Po
intInT,
typename StateT>
void
274 (
const StateT& hypothesis, std::vector<int>& indices,
PointCloudIn &cloud)
277 computeTransformedPointCloudWithNormal (hypothesis, indices, cloud);
279 computeTransformedPointCloudWithoutNormal (hypothesis, cloud);
282 template <
typename Po
intInT,
typename StateT>
void
286 const Eigen::Affine3f trans = toEigenMatrix (hypothesis);
288 pcl::transformPointCloud<PointInT> (*ref_, cloud, trans);
292 template <
typename Po
intInT,
typename StateT>
void
294 #ifdef PCL_TRACKING_NORMAL_SUPPORTED
295 const StateT& hypothesis, std::vector<int>& indices,
PointCloudIn &cloud)
300 #ifdef PCL_TRACKING_NORMAL_SUPPORTED
301 const Eigen::Affine3f trans = toEigenMatrix (hypothesis);
303 pcl::transformPointCloudWithNormals<PointInT> (*ref_, cloud, trans);
304 for ( std::size_t i = 0; i < cloud.points.size (); i++ )
306 PointInT input_point = cloud.points[i];
308 if (!std::isfinite(input_point.x) || !std::isfinite(input_point.y) || !std::isfinite(input_point.z))
311 Eigen::Vector4f p = input_point.getVector4fMap ();
312 Eigen::Vector4f n = input_point.getNormalVector4fMap ();
314 if ( theta > occlusion_angle_thr_ )
315 indices.push_back (i);
318 PCL_WARN (
"[pcl::%s::computeTransformedPointCloudWithoutNormal] use_normal_ == true is not supported in this Point Type.",
319 getClassName ().c_str ());
323 template <
typename Po
intInT,
typename StateT>
void
326 resampleWithReplacement ();
329 template <
typename Po
intInT,
typename StateT>
void
332 std::vector<int> a (particles_->points.size ());
333 std::vector<double> q (particles_->points.size ());
334 genAliasTable (a, q, particles_);
336 const std::vector<double> zero_mean (StateT::stateDimension (), 0.0);
339 particles_->points.clear ();
341 StateT p = representative_state_;
342 particles_->points.push_back (p);
345 int motion_num =
static_cast<int> (particles_->points.size ()) *
static_cast<int> (motion_ratio_);
346 for (
int i = 1; i < motion_num; i++ )
348 int target_particle_index = sampleWithReplacement (a, q);
349 StateT p = origparticles->points[target_particle_index];
351 p.sample (zero_mean, step_noise_covariance_);
353 particles_->points.push_back (p);
357 for (
int i = motion_num; i < particle_num_; i++ )
359 int target_particle_index = sampleWithReplacement (a, q);
360 StateT p = origparticles->points[target_particle_index];
362 p.sample (zero_mean, step_noise_covariance_);
363 particles_->points.push_back (p);
367 template <
typename Po
intInT,
typename StateT>
void
371 StateT orig_representative = representative_state_;
372 representative_state_.zero ();
373 representative_state_.
weight = 0.0;
374 for ( std::size_t i = 0; i < particles_->points.size (); i++)
376 StateT p = particles_->points[i];
377 representative_state_ = representative_state_ + p * p.
weight;
379 representative_state_.weight = 1.0f /
static_cast<float> (particles_->points.size ());
380 motion_ = representative_state_ - orig_representative;
383 template <
typename Po
intInT,
typename StateT>
void
387 for (
int i = 0; i < iteration_num_; i++)
409 #define PCL_INSTANTIATE_ParticleFilterTracker(T,ST) template class PCL_EXPORTS pcl::tracking::ParticleFilterTracker<T,ST>;