1 #ifndef PCL_TRACKING_IMPL_PARTICLE_FILTER_H_ 2 #define PCL_TRACKING_IMPL_PARTICLE_FILTER_H_ 4 #include <pcl/common/common.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>; double getAngle3D(const Eigen::Vector4f &v1, const Eigen::Vector4f &v2)
Compute the smallest angle between two vectors in the [ 0, PI ) interval in 3D.
PointCloudState::Ptr PointCloudStatePtr
virtual void weight()
Weighting phase of particle filter method.
PointCloudState::ConstPtr PointCloudStateConstPtr
PointCloudIn::Ptr PointCloudInPtr
boost::shared_ptr< std::vector< int > > IndicesPtr
virtual void normalizeWeight()
Normalize the weights of all the particels.
void computeTransformedPointCloudWithoutNormal(const StateT &hypothesis, PointCloudIn &cloud)
Compute a reference pointcloud transformed to the pose that hypothesis represents and calculate indic...
virtual bool initCompute()
This method should get called before starting the actual computation.
Octree pointcloud change detector class
int sampleWithReplacement(const std::vector< int > &a, const std::vector< double > &q)
Implementation of "sample with replacement" using Walker's alias method.
void genAliasTable(std::vector< int > &a, std::vector< double > &q, const PointCloudStateConstPtr &particles)
Generate the tables for walker's alias method.
bool testChangeDetection(const PointCloudInConstPtr &input)
Run change detection and return true if there is a change.
virtual void resample()
Resampling phase of particle filter method.
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...
virtual void update()
Calculate the weighted mean of the particles and set it as the result.
void computeTransformedPointCloud(const StateT &hypothesis, std::vector< int > &indices, PointCloudIn &cloud)
Compute a reference pointcloud transformed to the pose that hypothesis represents.
void cropInputPointCloud(const PointCloudInConstPtr &cloud, PointCloudIn &output)
Crop the pointcloud by the bounding box calculated from hypothesis and the reference pointcloud...
virtual void computeTracking()
Track the pointcloud using particle filter method.
Tracker represents the base tracker class.
void initParticles(bool reset)
Initialize the particles.
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 computeTransformedPointCloudWithNormal(const StateT &hypothesis, std::vector< int > &indices, PointCloudIn &cloud)
Compute a reference pointcloud transformed to the pose that hypothesis represents and calculate indic...
void resampleWithReplacement()
Resampling the particle with replacement.
PointCloudIn::ConstPtr PointCloudInConstPtr