particle_filter.hpp
#ifndef PCL_TRACKING_IMPL_PARTICLE_FILTER_H_
#define PCL_TRACKING_IMPL_PARTICLE_FILTER_H_
#include <pcl/common/common.h>
#include <pcl/common/eigen.h>
#include <pcl/common/transforms.h>
#include <pcl/tracking/boost.h>
#include <pcl/tracking/particle_filter.h>
template <typename PointInT, typename StateT> bool
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::initCompute ()
{
if (!Tracker<PointInT, StateT>::initCompute ())
{
PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
return (false);
}
if (transed_reference_vector_.empty ())
{
// only one time allocation
transed_reference_vector_.resize (particle_num_);
for (int i = 0; i < particle_num_; i++)
{
transed_reference_vector_[i] = PointCloudInPtr (new PointCloudIn ());
}
}
coherence_->setTargetCloud (input_);
if (!change_detector_)
change_detector_ = boost::shared_ptr<pcl::octree::OctreePointCloudChangeDetector<PointInT> >(new pcl::octree::OctreePointCloudChangeDetector<PointInT> (change_detector_resolution_));
if (!particles_ || particles_->points.empty ())
initParticles (true);
return (true);
}
template <typename PointInT, typename StateT> int
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::sampleWithReplacement
(const std::vector<int>& a, const std::vector<double>& q)
{
using namespace boost;
static mt19937 gen (static_cast<unsigned int>(time (0)));
uniform_real<> dst (0.0, 1.0);
variate_generator<mt19937&, uniform_real<> > rand (gen, dst);
double rU = rand () * static_cast<double> (particles_->points.size ());
int k = static_cast<int> (rU);
rU -= k; /* rU - [rU] */
if ( rU < q[k] )
return k;
else
return a[k];
}
template <typename PointInT, typename StateT> void
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::genAliasTable (std::vector<int> &a, std::vector<double> &q,
const PointCloudStateConstPtr &particles)
{
/* generate an alias table, a and q */
std::vector<int> HL (particles->points.size ());
std::vector<int>::iterator H = HL.begin ();
std::vector<int>::iterator L = HL.end () - 1;
size_t num = particles->points.size ();
for ( size_t i = 0; i < num; i++ )
q[i] = particles->points[i].weight * static_cast<float> (num);
for ( size_t i = 0; i < num; i++ )
a[i] = static_cast<int> (i);
// setup H and L
for ( size_t i = 0; i < num; i++ )
if ( q[i] >= 1.0 )
*H++ = static_cast<int> (i);
else
*L-- = static_cast<int> (i);
while ( H != HL.begin() && L != HL.end() - 1 )
{
int j = *(L + 1);
int k = *(H - 1);
a[j] = k;
q[k] += q[j] - 1;
L++;
if ( q[k] < 1.0 )
{
*L-- = k;
--H;
}
}
}
template <typename PointInT, typename StateT> void
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::initParticles (bool reset)
{
particles_.reset (new PointCloudState ());
std::vector<double> initial_noise_mean;
if (reset)
{
representative_state_.zero ();
StateT offset = StateT::toState (trans_);
representative_state_ = offset;
representative_state_.weight = 1.0f / static_cast<float> (particle_num_);
}
// sampling...
for ( int i = 0; i < particle_num_; i++ )
{
StateT p;
p.zero ();
p.sample (initial_noise_mean_, initial_noise_covariance_);
p = p + representative_state_;
p.weight = 1.0f / static_cast<float> (particle_num_);
particles_->points.push_back (p); // update
}
}
template <typename PointInT, typename StateT> void
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::normalizeWeight ()
{
// apply exponential function
double w_min = std::numeric_limits<double>::max ();
double w_max = - std::numeric_limits<double>::max ();
for ( size_t i = 0; i < particles_->points.size (); i++ )
{
double weight = particles_->points[i].weight;
if (w_min > weight)
w_min = weight;
if (weight != 0.0 && w_max < weight)
w_max = weight;
}
fit_ratio_ = w_min;
if (w_max != w_min)
{
for ( size_t i = 0; i < particles_->points.size (); i++ )
{
if (particles_->points[i].weight != 0.0)
{
particles_->points[i].weight = static_cast<float> (normalizeParticleWeight (particles_->points[i].weight, w_min, w_max));
}
}
}
else
{
for ( size_t i = 0; i < particles_->points.size (); i++ )
particles_->points[i].weight = 1.0f / static_cast<float> (particles_->points.size ());
}
double sum = 0.0;
for ( size_t i = 0; i < particles_->points.size (); i++ )
{
sum += particles_->points[i].weight;
}
if (sum != 0.0)
{
for ( size_t i = 0; i < particles_->points.size (); i++ )
particles_->points[i].weight = particles_->points[i].weight / static_cast<float> (sum);
}
else
{
for ( size_t i = 0; i < particles_->points.size (); i++ )
particles_->points[i].weight = 1.0f / static_cast<float> (particles_->points.size ());
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename StateT> void
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::cropInputPointCloud (
const PointCloudInConstPtr &, PointCloudIn &output)
{
double x_min, y_min, z_min, x_max, y_max, z_max;
calcBoundingBox (x_min, x_max, y_min, y_max, z_min, z_max);
pass_x_.setFilterLimits (float (x_min), float (x_max));
pass_y_.setFilterLimits (float (y_min), float (y_max));
pass_z_.setFilterLimits (float (z_min), float (z_max));
// x
PointCloudInPtr xcloud (new PointCloudIn);
pass_x_.setInputCloud (input_);
pass_x_.filter (*xcloud);
// y
PointCloudInPtr ycloud (new PointCloudIn);
pass_y_.setInputCloud (xcloud);
pass_y_.filter (*ycloud);
// z
pass_z_.setInputCloud (ycloud);
pass_z_.filter (output);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename StateT> void
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::calcBoundingBox (
double &x_min, double &x_max, double &y_min, double &y_max, double &z_min, double &z_max)
{
x_min = y_min = z_min = std::numeric_limits<double>::max ();
x_max = y_max = z_max = - std::numeric_limits<double>::max ();
for (size_t i = 0; i < transed_reference_vector_.size (); i++)
{
PointCloudInPtr target = transed_reference_vector_[i];
PointInT Pmin, Pmax;
pcl::getMinMax3D (*target, Pmin, Pmax);
if (x_min > Pmin.x)
x_min = Pmin.x;
if (x_max < Pmax.x)
x_max = Pmax.x;
if (y_min > Pmin.y)
y_min = Pmin.y;
if (y_max < Pmax.y)
y_max = Pmax.y;
if (z_min > Pmin.z)
z_min = Pmin.z;
if (z_max < Pmax.z)
z_max = Pmax.z;
}
}
template <typename PointInT, typename StateT> bool
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::testChangeDetection
(const PointCloudInConstPtr &input)
{
change_detector_->setInputCloud (input);
change_detector_->addPointsFromInputCloud ();
std::vector<int> newPointIdxVector;
change_detector_->getPointIndicesFromNewVoxels (newPointIdxVector, change_detector_filter_);
change_detector_->switchBuffers ();
return newPointIdxVector.size () > 0;
}
template <typename PointInT, typename StateT> void
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::weight ()
{
if (!use_normal_)
{
for (size_t i = 0; i < particles_->points.size (); i++)
{
computeTransformedPointCloudWithoutNormal (particles_->points[i], *transed_reference_vector_[i]);
}
PointCloudInPtr coherence_input (new PointCloudIn);
cropInputPointCloud (input_, *coherence_input);
coherence_->setTargetCloud (coherence_input);
coherence_->initCompute ();
for (size_t i = 0; i < particles_->points.size (); i++)
{
IndicesPtr indices;
coherence_->compute (transed_reference_vector_[i], indices, particles_->points[i].weight);
}
}
else
{
for (size_t i = 0; i < particles_->points.size (); i++)
{
IndicesPtr indices (new std::vector<int>);
computeTransformedPointCloudWithNormal (particles_->points[i], *indices, *transed_reference_vector_[i]);
}
PointCloudInPtr coherence_input (new PointCloudIn);
cropInputPointCloud (input_, *coherence_input);
coherence_->setTargetCloud (coherence_input);
coherence_->initCompute ();
for (size_t i = 0; i < particles_->points.size (); i++)
{
IndicesPtr indices (new std::vector<int>);
coherence_->compute (transed_reference_vector_[i], indices, particles_->points[i].weight);
}
}
normalizeWeight ();
}
template <typename PointInT, typename StateT> void
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::computeTransformedPointCloud
(const StateT& hypothesis, std::vector<int>& indices, PointCloudIn &cloud)
{
if (use_normal_)
computeTransformedPointCloudWithNormal (hypothesis, indices, cloud);
else
computeTransformedPointCloudWithoutNormal (hypothesis, cloud);
}
template <typename PointInT, typename StateT> void
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::computeTransformedPointCloudWithoutNormal
(const StateT& hypothesis, PointCloudIn &cloud)
{
const Eigen::Affine3f trans = toEigenMatrix (hypothesis);
// destructively assigns to cloud
pcl::transformPointCloud<PointInT> (*ref_, cloud, trans);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename StateT> void
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::computeTransformedPointCloudWithNormal (
#ifdef PCL_TRACKING_NORMAL_SUPPORTED
const StateT& hypothesis, std::vector<int>& indices, PointCloudIn &cloud)
#else
const StateT&, std::vector<int>&, PointCloudIn &)
#endif
{
#ifdef PCL_TRACKING_NORMAL_SUPPORTED
const Eigen::Affine3f trans = toEigenMatrix (hypothesis);
// destructively assigns to cloud
pcl::transformPointCloudWithNormals<PointInT> (*ref_, cloud, trans);
for ( size_t i = 0; i < cloud.points.size (); i++ )
{
PointInT input_point = cloud.points[i];
if (!pcl_isfinite(input_point.x) || !pcl_isfinite(input_point.y) || !pcl_isfinite(input_point.z))
continue;
// take occlusion into account
Eigen::Vector4f p = input_point.getVector4fMap ();
Eigen::Vector4f n = input_point.getNormalVector4fMap ();
double theta = pcl::getAngle3D (p, n);
if ( theta > occlusion_angle_thr_ )
indices.push_back (i);
}
#else
PCL_WARN ("[pcl::%s::computeTransformedPointCloudWithoutNormal] use_normal_ == true is not supported in this Point Type.",
getClassName ().c_str ());
#endif
}
template <typename PointInT, typename StateT> void
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::resample ()
{
resampleWithReplacement ();
}
template <typename PointInT, typename StateT> void
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::resampleWithReplacement ()
{
std::vector<int> a (particles_->points.size ());
std::vector<double> q (particles_->points.size ());
genAliasTable (a, q, particles_);
const std::vector<double> zero_mean (StateT::stateDimension (), 0.0);
// memoize the original list of particles
PointCloudStatePtr origparticles = particles_;
particles_->points.clear ();
// the first particle, it is a just copy of the maximum result
StateT p = representative_state_;
particles_->points.push_back (p);
// with motion
int motion_num = static_cast<int> (particles_->points.size ()) * static_cast<int> (motion_ratio_);
for ( int i = 1; i < motion_num; i++ )
{
int target_particle_index = sampleWithReplacement (a, q);
StateT p = origparticles->points[target_particle_index];
// add noise using gaussian
p.sample (zero_mean, step_noise_covariance_);
p = p + motion_;
particles_->points.push_back (p);
}
// no motion
for ( int i = motion_num; i < particle_num_; i++ )
{
int target_particle_index = sampleWithReplacement (a, q);
StateT p = origparticles->points[target_particle_index];
// add noise using gaussian
p.sample (zero_mean, step_noise_covariance_);
particles_->points.push_back (p);
}
}
template <typename PointInT, typename StateT> void
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::update ()
{
StateT orig_representative = representative_state_;
representative_state_.zero ();
representative_state_.weight = 0.0;
for ( size_t i = 0; i < particles_->points.size (); i++)
{
StateT p = particles_->points[i];
representative_state_ = representative_state_ + p * p.weight;
}
representative_state_.weight = 1.0f / static_cast<float> (particles_->points.size ());
motion_ = representative_state_ - orig_representative;
}
template <typename PointInT, typename StateT> void
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::computeTracking ()
{
for (int i = 0; i < iteration_num_; i++)
{
if (changed_)
{
resample ();
}
weight (); // likelihood is called in it
if (changed_)
{
update ();
}
}
// if ( getResult ().weight < resample_likelihood_thr_ )
// {
// PCL_WARN ("too small likelihood, re-initializing...\n");
// initParticles (false);
// }
}
#define PCL_INSTANTIATE_ParticleFilterTracker(T,ST) template class PCL_EXPORTS pcl::tracking::ParticleFilterTracker<T,ST>;
#endif
pcl::tracking::ParticleFilterTracker::PointCloudStatePtr
PointCloudState::Ptr PointCloudStatePtr
Definition: particle_filter.h:41
pcl::tracking::ParticleFilterTracker::weight
virtual void weight()
Weighting phase of particle filter method.
Definition: particle_filter.hpp:231
pcl::tracking::ParticleFilterTracker::PointCloudStateConstPtr
PointCloudState::ConstPtr PointCloudStateConstPtr
Definition: particle_filter.h:42
pcl::getAngle3D
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:46
boost
Definition: boost_graph.h:47
pcl::tracking::ParticleFilterTracker::PointCloudInPtr
PointCloudIn::Ptr PointCloudInPtr
Definition: particle_filter.h:37
pcl::IndicesPtr
boost::shared_ptr< std::vector< int > > IndicesPtr
Definition: pcl_base.h:60
pcl::tracking::ParticleFilterTracker::normalizeWeight
virtual void normalizeWeight()
Normalize the weights of all the particels.
Definition: particle_filter.hpp:117
pcl::tracking::ParticleFilterTracker::computeTransformedPointCloudWithoutNormal
void computeTransformedPointCloudWithoutNormal(const StateT &hypothesis, PointCloudIn &cloud)
Compute a reference pointcloud transformed to the pose that hypothesis represents and calculate indic...
Definition: particle_filter.hpp:286
pcl::tracking::ParticleFilterTracker::initCompute
virtual bool initCompute()
This method should get called before starting the actual computation.
Definition: particle_filter.hpp:11
pcl::octree::OctreePointCloudChangeDetector
Octree pointcloud change detector class
Definition: octree_pointcloud_changedetector.h:64
pcl::tracking::ParticleFilterTracker::sampleWithReplacement
int sampleWithReplacement(const std::vector< int > &a, const std::vector< double > &q)
Implementation of "sample with replacement" using Walker&#39;s alias method.
Definition: particle_filter.hpp:41
pcl::tracking::ParticleFilterTracker::genAliasTable
void genAliasTable(std::vector< int > &a, std::vector< double > &q, const PointCloudStateConstPtr &particles)
Generate the tables for walker&#39;s alias method.
Definition: particle_filter.hpp:57
pcl::tracking::ParticleFilterTracker::testChangeDetection
bool testChangeDetection(const PointCloudInConstPtr &input)
Run change detection and return true if there is a change.
Definition: particle_filter.hpp:220
pcl::tracking::ParticleFilterTracker::resample
virtual void resample()
Resampling phase of particle filter method.
Definition: particle_filter.hpp:326
common.h
Define standard C methods and C++ classes that are common to all methods.
pcl::getMinMax3D
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:228
pcl::tracking::ParticleFilterTracker::update
virtual void update()
Calculate the weighted mean of the particles and set it as the result.
Definition: particle_filter.hpp:370
pcl::PointCloud< PointInT >
pcl::tracking::ParticleFilterTracker::computeTransformedPointCloud
void computeTransformedPointCloud(const StateT &hypothesis, std::vector< int > &indices, PointCloudIn &cloud)
Compute a reference pointcloud transformed to the pose that hypothesis represents.
Definition: particle_filter.hpp:276
pcl::tracking::ParticleFilterTracker::cropInputPointCloud
void cropInputPointCloud(const PointCloudInConstPtr &cloud, PointCloudIn &output)
Crop the pointcloud by the bounding box calculated from hypothesis and the reference pointcloud...
Definition: particle_filter.hpp:168
pcl::tracking::ParticleFilterTracker::computeTracking
virtual void computeTracking()
Track the pointcloud using particle filter method.
Definition: particle_filter.hpp:386
pcl::tracking::Tracker
Tracker represents the base tracker class.
Definition: tracker.h:56
pcl::tracking::ParticleFilterTracker::initParticles
void initParticles(bool reset)
Initialize the particles.
Definition: particle_filter.hpp:92
pcl::tracking::ParticleFilterTracker::calcBoundingBox
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.
Definition: particle_filter.hpp:192
pcl::tracking::ParticleFilterTracker::computeTransformedPointCloudWithNormal
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...
Definition: particle_filter.hpp:295
pcl::tracking::ParticleFilterTracker::resampleWithReplacement
void resampleWithReplacement()
Resampling the particle with replacement.
Definition: particle_filter.hpp:332
pcl::tracking::ParticleFilterTracker::PointCloudInConstPtr
PointCloudIn::ConstPtr PointCloudInConstPtr
Definition: particle_filter.h:38