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
94 std::vector<double> initial_noise_mean;
97 representative_state_.zero ();
98 StateT offset = StateT::toState (trans_);
99 representative_state_ = offset;
100 representative_state_.weight = 1.0f / static_cast<float> (particle_num_);
104 for (
int i = 0; i < particle_num_; i++ )
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);
115 template <
typename Po
intInT,
typename StateT>
void
119 double w_min = std::numeric_limits<double>::max ();
120 double w_max = - std::numeric_limits<double>::max ();
121 for ( std::size_t i = 0; i < particles_->points.size (); i++ )
123 double weight = particles_->points[i].weight;
126 if (weight != 0.0 && w_max < weight)
133 for ( std::size_t i = 0; i < particles_->points.size (); i++ )
135 if (particles_->points[i].weight != 0.0)
137 particles_->points[i].weight = static_cast<float> (normalizeParticleWeight (particles_->points[i].weight, w_min, w_max));
143 for ( std::size_t i = 0; i < particles_->points.size (); i++ )
144 particles_->points[i].weight = 1.0f / static_cast<float> (particles_->points.size ());
148 for ( std::size_t i = 0; i < particles_->points.size (); i++ )
150 sum += particles_->points[i].weight;
155 for ( std::size_t i = 0; i < particles_->points.size (); i++ )
156 particles_->points[i].weight /= static_cast<float> (sum);
160 for ( std::size_t i = 0; i < particles_->points.size (); i++ )
161 particles_->points[i].weight = 1.0f / static_cast<float> (particles_->points.size ());
166 template <
typename Po
intInT,
typename StateT>
void
170 double x_min, y_min, z_min, x_max, y_max, z_max;
171 calcBoundingBox (x_min, x_max, y_min, y_max, z_min, z_max);
172 pass_x_.setFilterLimits (
float (x_min),
float (x_max));
173 pass_y_.setFilterLimits (
float (y_min),
float (y_max));
174 pass_z_.setFilterLimits (
float (z_min),
float (z_max));
178 pass_x_.setInputCloud (input_);
179 pass_x_.filter (*xcloud);
182 pass_y_.setInputCloud (xcloud);
183 pass_y_.filter (*ycloud);
185 pass_z_.setInputCloud (ycloud);
186 pass_z_.filter (output);
190 template <
typename Po
intInT,
typename StateT>
void
192 double &x_min,
double &x_max,
double &y_min,
double &y_max,
double &z_min,
double &z_max)
194 x_min = y_min = z_min = std::numeric_limits<double>::max ();
195 x_max = y_max = z_max = - std::numeric_limits<double>::max ();
197 for (std::size_t i = 0; i < transed_reference_vector_.size (); i++)
217 template <
typename Po
intInT,
typename StateT>
bool
221 change_detector_->setInputCloud (input);
222 change_detector_->addPointsFromInputCloud ();
223 std::vector<int> newPointIdxVector;
224 change_detector_->getPointIndicesFromNewVoxels (newPointIdxVector, change_detector_filter_);
225 change_detector_->switchBuffers ();
226 return !newPointIdxVector.empty ();
229 template <
typename Po
intInT,
typename StateT>
void
234 for (std::size_t i = 0; i < particles_->points.size (); i++)
236 computeTransformedPointCloudWithoutNormal (particles_->points[i], *transed_reference_vector_[i]);
240 cropInputPointCloud (input_, *coherence_input);
242 coherence_->setTargetCloud (coherence_input);
243 coherence_->initCompute ();
244 for (std::size_t i = 0; i < particles_->points.size (); i++)
247 coherence_->compute (transed_reference_vector_[i], indices, particles_->points[i].weight);
252 for (std::size_t i = 0; i < particles_->points.size (); i++)
255 computeTransformedPointCloudWithNormal (particles_->points[i], *indices, *transed_reference_vector_[i]);
259 cropInputPointCloud (input_, *coherence_input);
261 coherence_->setTargetCloud (coherence_input);
262 coherence_->initCompute ();
263 for (std::size_t i = 0; i < particles_->points.size (); i++)
266 coherence_->compute (transed_reference_vector_[i], indices, particles_->points[i].weight);
273 template <
typename Po
intInT,
typename StateT>
void
275 (
const StateT& hypothesis, std::vector<int>& indices,
PointCloudIn &cloud)
278 computeTransformedPointCloudWithNormal (hypothesis, indices, cloud);
280 computeTransformedPointCloudWithoutNormal (hypothesis, cloud);
283 template <
typename Po
intInT,
typename StateT>
void
287 const Eigen::Affine3f trans = toEigenMatrix (hypothesis);
289 pcl::transformPointCloud<PointInT> (*ref_, cloud, trans);
293 template <
typename Po
intInT,
typename StateT>
void
295 #ifdef PCL_TRACKING_NORMAL_SUPPORTED
296 const StateT& hypothesis, std::vector<int>& indices,
PointCloudIn &cloud)
301 #ifdef PCL_TRACKING_NORMAL_SUPPORTED
302 const Eigen::Affine3f trans = toEigenMatrix (hypothesis);
304 pcl::transformPointCloudWithNormals<PointInT> (*ref_, cloud, trans);
305 for ( std::size_t i = 0; i < cloud.points.size (); i++ )
307 PointInT input_point = cloud.points[i];
309 if (!std::isfinite(input_point.x) || !std::isfinite(input_point.y) || !std::isfinite(input_point.z))
312 Eigen::Vector4f p = input_point.getVector4fMap ();
313 Eigen::Vector4f n = input_point.getNormalVector4fMap ();
315 if ( theta > occlusion_angle_thr_ )
316 indices.push_back (i);
319 PCL_WARN (
"[pcl::%s::computeTransformedPointCloudWithoutNormal] use_normal_ == true is not supported in this Point Type.",
320 getClassName ().c_str ());
324 template <
typename Po
intInT,
typename StateT>
void
327 resampleWithReplacement ();
330 template <
typename Po
intInT,
typename StateT>
void
333 std::vector<int> a (particles_->points.size ());
334 std::vector<double> q (particles_->points.size ());
335 genAliasTable (a, q, particles_);
337 const std::vector<double> zero_mean (StateT::stateDimension (), 0.0);
340 particles_->points.clear ();
342 StateT p = representative_state_;
343 particles_->points.push_back (p);
346 int motion_num = static_cast<int> (particles_->points.size ()) * static_cast<int> (motion_ratio_);
347 for (
int i = 1; i < motion_num; i++ )
349 int target_particle_index = sampleWithReplacement (a, q);
350 StateT p = origparticles->points[target_particle_index];
352 p.sample (zero_mean, step_noise_covariance_);
354 particles_->points.push_back (p);
358 for (
int i = motion_num; i < particle_num_; i++ )
360 int target_particle_index = sampleWithReplacement (a, q);
361 StateT p = origparticles->points[target_particle_index];
363 p.sample (zero_mean, step_noise_covariance_);
364 particles_->points.push_back (p);
368 template <
typename Po
intInT,
typename StateT>
void
372 StateT orig_representative = representative_state_;
373 representative_state_.zero ();
374 representative_state_.
weight = 0.0;
375 for ( std::size_t i = 0; i < particles_->points.size (); i++)
377 StateT p = particles_->points[i];
378 representative_state_ = representative_state_ + p * p.
weight;
380 representative_state_.weight = 1.0f / static_cast<float> (particles_->points.size ());
381 motion_ = representative_state_ - orig_representative;
384 template <
typename Po
intInT,
typename StateT>
void
388 for (
int i = 0; i < iteration_num_; i++)
410 #define PCL_INSTANTIATE_ParticleFilterTracker(T,ST) template class PCL_EXPORTS pcl::tracking::ParticleFilterTracker<T,ST>;