1 #ifndef PCL_TRACKING_IMPL_PARTICLE_FILTER_H_
2 #define PCL_TRACKING_IMPL_PARTICLE_FILTER_H_
5 #include <pcl/common/eigen.h>
6 #include <pcl/common/transforms.h>
7 #include <pcl/tracking/boost.h>
8 #include <pcl/tracking/particle_filter.h>
10 template <
typename Po
intInT,
typename StateT>
bool
15 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
19 if (transed_reference_vector_.empty ())
22 transed_reference_vector_.resize (particle_num_);
23 for (
int i = 0; i < particle_num_; i++)
29 coherence_->setTargetCloud (input_);
31 if (!change_detector_)
34 if (!particles_ || particles_->points.empty ())
39 template <
typename Po
intInT,
typename StateT>
int
41 (
const std::vector<int>& a,
const std::vector<double>& q)
43 using namespace boost;
44 static mt19937 gen (
static_cast<unsigned int>(time (0)));
45 uniform_real<> dst (0.0, 1.0);
46 variate_generator<mt19937&, uniform_real<> > rand (gen, dst);
47 double rU = rand () *
static_cast<double> (particles_->points.size ());
48 int k =
static_cast<int> (rU);
56 template <
typename Po
intInT,
typename StateT>
void
61 std::vector<int> HL (particles->points.size ());
62 std::vector<int>::iterator H = HL.begin ();
63 std::vector<int>::iterator L = HL.end () - 1;
64 size_t num = particles->points.size ();
65 for (
size_t i = 0; i < num; i++ )
66 q[i] = particles->points[i].weight *
static_cast<float> (num);
67 for (
size_t i = 0; i < num; i++ )
68 a[i] =
static_cast<int> (i);
70 for (
size_t i = 0; i < num; i++ )
72 *H++ =
static_cast<int> (i);
74 *L-- =
static_cast<int> (i);
76 while ( H != HL.begin() && L != HL.end() - 1 )
91 template <
typename Po
intInT,
typename StateT>
void
95 std::vector<double> initial_noise_mean;
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_);
105 for (
int i = 0; i < particle_num_; i++ )
109 p.sample (initial_noise_mean_, initial_noise_covariance_);
110 p = p + representative_state_;
111 p.weight = 1.0f /
static_cast<float> (particle_num_);
112 particles_->points.push_back (p);
116 template <
typename Po
intInT,
typename StateT>
void
120 double w_min = std::numeric_limits<double>::max ();
121 double w_max = - std::numeric_limits<double>::max ();
122 for (
size_t i = 0; i < particles_->points.size (); i++ )
124 double weight = particles_->points[i].weight;
127 if (weight != 0.0 && w_max < weight)
134 for (
size_t i = 0; i < particles_->points.size (); i++ )
136 if (particles_->points[i].weight != 0.0)
138 particles_->points[i].weight =
static_cast<float> (normalizeParticleWeight (particles_->points[i].weight, w_min, w_max));
144 for (
size_t i = 0; i < particles_->points.size (); i++ )
145 particles_->points[i].weight = 1.0f /
static_cast<float> (particles_->points.size ());
149 for (
size_t i = 0; i < particles_->points.size (); i++ )
151 sum += particles_->points[i].weight;
156 for (
size_t i = 0; i < particles_->points.size (); i++ )
157 particles_->points[i].weight = particles_->points[i].weight /
static_cast<float> (sum);
161 for (
size_t i = 0; i < particles_->points.size (); i++ )
162 particles_->points[i].weight = 1.0f /
static_cast<float> (particles_->points.size ());
167 template <
typename Po
intInT,
typename StateT>
void
171 double x_min, y_min, z_min, x_max, y_max, z_max;
172 calcBoundingBox (x_min, x_max, y_min, y_max, z_min, z_max);
173 pass_x_.setFilterLimits (
float (x_min),
float (x_max));
174 pass_y_.setFilterLimits (
float (y_min),
float (y_max));
175 pass_z_.setFilterLimits (
float (z_min),
float (z_max));
179 pass_x_.setInputCloud (input_);
180 pass_x_.filter (*xcloud);
183 pass_y_.setInputCloud (xcloud);
184 pass_y_.filter (*ycloud);
186 pass_z_.setInputCloud (ycloud);
187 pass_z_.filter (output);
191 template <
typename Po
intInT,
typename StateT>
void
193 double &x_min,
double &x_max,
double &y_min,
double &y_max,
double &z_min,
double &z_max)
195 x_min = y_min = z_min = std::numeric_limits<double>::max ();
196 x_max = y_max = z_max = - std::numeric_limits<double>::max ();
198 for (
size_t i = 0; i < transed_reference_vector_.size (); i++)
218 template <
typename Po
intInT,
typename StateT>
bool
222 change_detector_->setInputCloud (input);
223 change_detector_->addPointsFromInputCloud ();
224 std::vector<int> newPointIdxVector;
225 change_detector_->getPointIndicesFromNewVoxels (newPointIdxVector, change_detector_filter_);
226 change_detector_->switchBuffers ();
227 return newPointIdxVector.size () > 0;
230 template <
typename Po
intInT,
typename StateT>
void
235 for (
size_t i = 0; i < particles_->points.size (); i++)
237 computeTransformedPointCloudWithoutNormal (particles_->points[i], *transed_reference_vector_[i]);
241 cropInputPointCloud (input_, *coherence_input);
243 coherence_->setTargetCloud (coherence_input);
244 coherence_->initCompute ();
245 for (
size_t i = 0; i < particles_->points.size (); i++)
248 coherence_->compute (transed_reference_vector_[i], indices, particles_->points[i].weight);
253 for (
size_t i = 0; i < particles_->points.size (); i++)
256 computeTransformedPointCloudWithNormal (particles_->points[i], *indices, *transed_reference_vector_[i]);
260 cropInputPointCloud (input_, *coherence_input);
262 coherence_->setTargetCloud (coherence_input);
263 coherence_->initCompute ();
264 for (
size_t i = 0; i < particles_->points.size (); i++)
267 coherence_->compute (transed_reference_vector_[i], indices, particles_->points[i].weight);
274 template <
typename Po
intInT,
typename StateT>
void
276 (
const StateT& hypothesis, std::vector<int>& indices,
PointCloudIn &cloud)
279 computeTransformedPointCloudWithNormal (hypothesis, indices, cloud);
281 computeTransformedPointCloudWithoutNormal (hypothesis, cloud);
284 template <
typename Po
intInT,
typename StateT>
void
288 const Eigen::Affine3f trans = toEigenMatrix (hypothesis);
290 pcl::transformPointCloud<PointInT> (*ref_, cloud, trans);
294 template <
typename Po
intInT,
typename StateT>
void
296 #ifdef PCL_TRACKING_NORMAL_SUPPORTED
297 const StateT& hypothesis, std::vector<int>& indices,
PointCloudIn &cloud)
302 #ifdef PCL_TRACKING_NORMAL_SUPPORTED
303 const Eigen::Affine3f trans = toEigenMatrix (hypothesis);
305 pcl::transformPointCloudWithNormals<PointInT> (*ref_, cloud, trans);
306 for (
size_t i = 0; i < cloud.
points.size (); i++ )
308 PointInT input_point = cloud.
points[i];
310 if (!pcl_isfinite(input_point.x) || !pcl_isfinite(input_point.y) || !pcl_isfinite(input_point.z))
313 Eigen::Vector4f p = input_point.getVector4fMap ();
314 Eigen::Vector4f n = input_point.getNormalVector4fMap ();
316 if ( theta > occlusion_angle_thr_ )
317 indices.push_back (i);
320 PCL_WARN (
"[pcl::%s::computeTransformedPointCloudWithoutNormal] use_normal_ == true is not supported in this Point Type.",
321 getClassName ().c_str ());
325 template <
typename Po
intInT,
typename StateT>
void
328 resampleWithReplacement ();
331 template <
typename Po
intInT,
typename StateT>
void
334 std::vector<int> a (particles_->points.size ());
335 std::vector<double> q (particles_->points.size ());
336 genAliasTable (a, q, particles_);
338 const std::vector<double> zero_mean (StateT::stateDimension (), 0.0);
341 particles_->points.clear ();
343 StateT p = representative_state_;
344 particles_->points.push_back (p);
347 int motion_num =
static_cast<int> (particles_->points.size ()) *
static_cast<int> (motion_ratio_);
348 for (
int i = 1; i < motion_num; i++ )
350 int target_particle_index = sampleWithReplacement (a, q);
351 StateT p = origparticles->points[target_particle_index];
353 p.sample (zero_mean, step_noise_covariance_);
355 particles_->points.push_back (p);
359 for (
int i = motion_num; i < particle_num_; i++ )
361 int target_particle_index = sampleWithReplacement (a, q);
362 StateT p = origparticles->points[target_particle_index];
364 p.sample (zero_mean, step_noise_covariance_);
365 particles_->points.push_back (p);
369 template <
typename Po
intInT,
typename StateT>
void
373 StateT orig_representative = representative_state_;
374 representative_state_.zero ();
375 representative_state_.
weight = 0.0;
376 for (
size_t i = 0; i < particles_->points.size (); i++)
378 StateT p = particles_->points[i];
379 representative_state_ = representative_state_ + p * p.
weight;
381 representative_state_.weight = 1.0f /
static_cast<float> (particles_->points.size ());
382 motion_ = representative_state_ - orig_representative;
385 template <
typename Po
intInT,
typename StateT>
void
389 for (
int i = 0; i < iteration_num_; i++)
411 #define PCL_INSTANTIATE_ParticleFilterTracker(T,ST) template class PCL_EXPORTS pcl::tracking::ParticleFilterTracker<T,ST>;