kld_adaptive_particle_filter.h
#ifndef PCL_TRACKING_KLD_ADAPTIVE_PARTICLE_FILTER_H_
#define PCL_TRACKING_KLD_ADAPTIVE_PARTICLE_FILTER_H_
#include <pcl/tracking/tracking.h>
#include <pcl/tracking/particle_filter.h>
#include <pcl/tracking/coherence.h>
namespace pcl
{
namespace tracking
{
/** \brief @b KLDAdaptiveParticleFilterTracker tracks the PointCloud which is given by
setReferenceCloud within the measured PointCloud using particle filter method.
The number of the particles changes adaptively based on KLD sampling [D. Fox, NIPS-01], [D.Fox, IJRR03].
* \author Ryohei Ueda
* \ingroup tracking
*/
template <typename PointInT, typename StateT>
class KLDAdaptiveParticleFilterTracker: public ParticleFilterTracker<PointInT, StateT>
{
public:
using Tracker<PointInT, StateT>::tracker_name_;
using Tracker<PointInT, StateT>::search_;
using Tracker<PointInT, StateT>::input_;
using Tracker<PointInT, StateT>::getClassName;
using ParticleFilterTracker<PointInT, StateT>::transed_reference_vector_;
using ParticleFilterTracker<PointInT, StateT>::coherence_;
using ParticleFilterTracker<PointInT, StateT>::initParticles;
using ParticleFilterTracker<PointInT, StateT>::weight;
using ParticleFilterTracker<PointInT, StateT>::update;
using ParticleFilterTracker<PointInT, StateT>::iteration_num_;
using ParticleFilterTracker<PointInT, StateT>::particle_num_;
using ParticleFilterTracker<PointInT, StateT>::particles_;
using ParticleFilterTracker<PointInT, StateT>::use_normal_;
using ParticleFilterTracker<PointInT, StateT>::use_change_detector_;
using ParticleFilterTracker<PointInT, StateT>::change_detector_resolution_;
using ParticleFilterTracker<PointInT, StateT>::change_detector_;
using ParticleFilterTracker<PointInT, StateT>::motion_;
using ParticleFilterTracker<PointInT, StateT>::motion_ratio_;
using ParticleFilterTracker<PointInT, StateT>::step_noise_covariance_;
using ParticleFilterTracker<PointInT, StateT>::representative_state_;
using ParticleFilterTracker<PointInT, StateT>::sampleWithReplacement;
typedef Tracker<PointInT, StateT> BaseClass;
typedef typename Tracker<PointInT, StateT>::PointCloudIn PointCloudIn;
typedef typename PointCloudIn::Ptr PointCloudInPtr;
typedef typename PointCloudIn::ConstPtr PointCloudInConstPtr;
typedef typename Tracker<PointInT, StateT>::PointCloudState PointCloudState;
typedef typename PointCloudState::Ptr PointCloudStatePtr;
typedef typename PointCloudState::ConstPtr PointCloudStateConstPtr;
typedef PointCoherence<PointInT> Coherence;
typedef boost::shared_ptr< Coherence > CoherencePtr;
typedef boost::shared_ptr< const Coherence > CoherenceConstPtr;
typedef PointCloudCoherence<PointInT> CloudCoherence;
typedef boost::shared_ptr< CloudCoherence > CloudCoherencePtr;
typedef boost::shared_ptr< const CloudCoherence > CloudCoherenceConstPtr;
/** \brief Empty constructor. */
KLDAdaptiveParticleFilterTracker ()
: ParticleFilterTracker<PointInT, StateT> ()
, maximum_particle_number_ ()
, epsilon_ (0)
, delta_ (0.99)
, bin_size_ ()
{
tracker_name_ = "KLDAdaptiveParticleFilterTracker";
}
/** \brief set the bin size.
* \param bin_size the size of a bin
*/
inline void setBinSize (const StateT& bin_size) { bin_size_ = bin_size; }
/** \brief get the bin size. */
inline StateT getBinSize () const { return (bin_size_); }
/** \brief set the maximum number of the particles.
* \param nr the maximum number of the particles.
*/
inline void setMaximumParticleNum (unsigned int nr) { maximum_particle_number_ = nr; }
/** \brief get the maximum number of the particles.*/
inline unsigned int getMaximumParticleNum () const { return (maximum_particle_number_); }
/** \brief set epsilon to be used to calc K-L boundary.
* \param eps epsilon
*/
inline void setEpsilon (double eps) { epsilon_ = eps; }
/** \brief get epsilon to be used to calc K-L boundary. */
inline double getEpsilon () const { return (epsilon_); }
/** \brief set delta to be used in chi-squared distribution.
* \param delta delta of chi-squared distribution.
*/
inline void setDelta (double delta) { delta_ = delta; }
/** \brief get delta to be used in chi-squared distribution.*/
inline double getDelta () const { return (delta_); }
protected:
/** \brief return true if the two bins are equal.
* \param a index of the bin
* \param b index of the bin
*/
virtual bool
equalBin (std::vector<int> a, std::vector<int> b)
{
int dimension = StateT::stateDimension ();
for (int i = 0; i < dimension; i++)
if (a[i] != b[i])
return (false);
return (true);
}
/** \brief return upper quantile of standard normal distribution.
* \param[in] u ratio of quantile.
*/
double
normalQuantile (double u)
{
const double a[9] = { 1.24818987e-4, -1.075204047e-3, 5.198775019e-3,
-0.019198292004, 0.059054035642,-0.151968751364,
0.319152932694,-0.5319230073, 0.797884560593};
const double b[15] = { -4.5255659e-5, 1.5252929e-4, -1.9538132e-5,
-6.76904986e-4, 1.390604284e-3,-7.9462082e-4,
-2.034254874e-3, 6.549791214e-3,-0.010557625006,
0.011630447319,-9.279453341e-3, 5.353579108e-3,
-2.141268741e-3, 5.35310549e-4, 0.999936657524};
double w, y, z;
int i;
if (u == 0.)
return (0.5);
y = u / 2.0;
if (y < -6.)
return (0.0);
if (y > 6.)
return (1.0);
if (y < 0.0)
y = - y;
if (y < 1.0)
{
w = y * y;
z = a[0];
for (i = 1; i < 9; i++)
z = z * w + a[i];
z *= (y * 2.0);
}
else
{
y -= 2.0;
z = b[0];
for (i = 1; i < 15; i++)
z = z * y + b[i];
}
if (u < 0.0)
return ((1. - z) / 2.0);
return ((1. + z) / 2.0);
}
/** \brief calculate K-L boundary. K-L boundary follows 1/2e*chi(k-1, 1-d)^2.
* \param[in] k the number of bins and the first parameter of chi distribution.
*/
virtual
double calcKLBound (int k)
{
double z = normalQuantile (delta_);
double chi = 1.0 - 2.0 / (9.0 * (k - 1)) + sqrt (2.0 / (9.0 * (k - 1))) * z;
return ((k - 1.0) / (2.0 * epsilon_) * chi * chi * chi);
}
/** \brief insert a bin into the set of the bins. if that bin is already registered,
return false. if not, return true.
* \param bin a bin to be inserted.
* \param B a set of the bins
*/
virtual bool
insertIntoBins (std::vector<int> bin, std::vector<std::vector<int> > &B);
/** \brief This method should get called before starting the actual computation. */
virtual bool
initCompute ();
/** \brief resampling phase of particle filter method.
sampling the particles according to the weights calculated in weight method.
in particular, "sample with replacement" is archieved by walker's alias method.
*/
virtual void
resample ();
/** \brief the maximum number of the particles. */
unsigned int maximum_particle_number_;
/** \brief error between K-L distance and MLE*/
double epsilon_;
/** \brief probability of distance between K-L distance and MLE is less than epsilon_*/
double delta_;
/** \brief the size of a bin.*/
StateT bin_size_;
};
}
}
#ifdef PCL_NO_PRECOMPILE
#include <pcl/tracking/impl/kld_adaptive_particle_filter.hpp>
#endif
#endif
pcl::tracking::KLDAdaptiveParticleFilterTracker::maximum_particle_number_
unsigned int maximum_particle_number_
the maximum number of the particles.
Definition: kld_adaptive_particle_filter.h:200
pcl::tracking::KLDAdaptiveParticleFilterTracker::resample
virtual void resample()
resampling phase of particle filter method.
Definition: kld_adaptive_particle_filter.hpp:49
pcl::tracking::KLDAdaptiveParticleFilterTracker::getEpsilon
double getEpsilon() const
get epsilon to be used to calc K-L boundary.
Definition: kld_adaptive_particle_filter.h:96
pcl::tracking::KLDAdaptiveParticleFilterTracker::PointCloudState
Tracker< PointInT, StateT >::PointCloudState PointCloudState
Definition: kld_adaptive_particle_filter.h:51
pcl::tracking::KLDAdaptiveParticleFilterTracker::PointCloudInConstPtr
PointCloudIn::ConstPtr PointCloudInConstPtr
Definition: kld_adaptive_particle_filter.h:49
pcl::tracking::KLDAdaptiveParticleFilterTracker::setDelta
void setDelta(double delta)
set delta to be used in chi-squared distribution.
Definition: kld_adaptive_particle_filter.h:101
pcl::tracking::KLDAdaptiveParticleFilterTracker::PointCloudStatePtr
PointCloudState::Ptr PointCloudStatePtr
Definition: kld_adaptive_particle_filter.h:52
pcl::tracking::KLDAdaptiveParticleFilterTracker
KLDAdaptiveParticleFilterTracker tracks the PointCloud which is given by setReferenceCloud within the...
Definition: kld_adaptive_particle_filter.h:20
pcl::tracking::KLDAdaptiveParticleFilterTracker::PointCloudIn
Tracker< PointInT, StateT >::PointCloudIn PointCloudIn
Definition: kld_adaptive_particle_filter.h:47
pcl
Definition: convolution.h:45
pcl::tracking::KLDAdaptiveParticleFilterTracker::setBinSize
void setBinSize(const StateT &bin_size)
set the bin size.
Definition: kld_adaptive_particle_filter.h:77
pcl::tracking::KLDAdaptiveParticleFilterTracker::CoherenceConstPtr
boost::shared_ptr< const Coherence > CoherenceConstPtr
Definition: kld_adaptive_particle_filter.h:57
pcl::tracking::KLDAdaptiveParticleFilterTracker::bin_size_
StateT bin_size_
the size of a bin.
Definition: kld_adaptive_particle_filter.h:209
pcl::tracking::KLDAdaptiveParticleFilterTracker::initCompute
virtual bool initCompute()
This method should get called before starting the actual computation.
Definition: kld_adaptive_particle_filter.hpp:7
pcl::tracking::KLDAdaptiveParticleFilterTracker::normalQuantile
double normalQuantile(double u)
return upper quantile of standard normal distribution.
Definition: kld_adaptive_particle_filter.h:126
pcl::tracking::KLDAdaptiveParticleFilterTracker::CoherencePtr
boost::shared_ptr< Coherence > CoherencePtr
Definition: kld_adaptive_particle_filter.h:56
pcl::tracking::ParticleFilterTracker
ParticleFilterTracker tracks the PointCloud which is given by setReferenceCloud within the measured P...
Definition: particle_filter.h:22
pcl::tracking::KLDAdaptiveParticleFilterTracker::PointCloudStateConstPtr
PointCloudState::ConstPtr PointCloudStateConstPtr
Definition: kld_adaptive_particle_filter.h:53
pcl::PointCloud< PointInT >::Ptr
boost::shared_ptr< PointCloud< PointInT > > Ptr
Definition: point_cloud.h:428
pcl::tracking::KLDAdaptiveParticleFilterTracker::equalBin
virtual bool equalBin(std::vector< int > a, std::vector< int > b)
return true if the two bins are equal.
Definition: kld_adaptive_particle_filter.h:113
pcl::tracking::KLDAdaptiveParticleFilterTracker::calcKLBound
virtual double calcKLBound(int k)
calculate K-L boundary.
Definition: kld_adaptive_particle_filter.h:173
pcl::tracking::KLDAdaptiveParticleFilterTracker::KLDAdaptiveParticleFilterTracker
KLDAdaptiveParticleFilterTracker()
Empty constructor.
Definition: kld_adaptive_particle_filter.h:64
pcl::tracking::KLDAdaptiveParticleFilterTracker::BaseClass
Tracker< PointInT, StateT > BaseClass
Definition: kld_adaptive_particle_filter.h:45
pcl::tracking::KLDAdaptiveParticleFilterTracker::getBinSize
StateT getBinSize() const
get the bin size.
Definition: kld_adaptive_particle_filter.h:80
pcl::tracking::KLDAdaptiveParticleFilterTracker::setMaximumParticleNum
void setMaximumParticleNum(unsigned int nr)
set the maximum number of the particles.
Definition: kld_adaptive_particle_filter.h:85
pcl::tracking::PointCoherence
PointCoherence is a base class to compute coherence between the two points.
Definition: coherence.h:17
pcl::tracking::KLDAdaptiveParticleFilterTracker::delta_
double delta_
probability of distance between K-L distance and MLE is less than epsilon_
Definition: kld_adaptive_particle_filter.h:206
pcl::PointCloud< PointInT >::ConstPtr
boost::shared_ptr< const PointCloud< PointInT > > ConstPtr
Definition: point_cloud.h:429
pcl::tracking::KLDAdaptiveParticleFilterTracker::insertIntoBins
virtual bool insertIntoBins(std::vector< int > bin, std::vector< std::vector< int > > &B)
insert a bin into the set of the bins.
Definition: kld_adaptive_particle_filter.hpp:37
pcl::tracking::KLDAdaptiveParticleFilterTracker::getMaximumParticleNum
unsigned int getMaximumParticleNum() const
get the maximum number of the particles.
Definition: kld_adaptive_particle_filter.h:88
pcl::PointCloud< PointInT >
pcl::tracking::KLDAdaptiveParticleFilterTracker::CloudCoherenceConstPtr
boost::shared_ptr< const CloudCoherence > CloudCoherenceConstPtr
Definition: kld_adaptive_particle_filter.h:61
pcl::tracking::PointCloudCoherence
PointCloudCoherence is a base class to compute coherence between the two PointClouds.
Definition: coherence.h:60
pcl::tracking::KLDAdaptiveParticleFilterTracker::setEpsilon
void setEpsilon(double eps)
set epsilon to be used to calc K-L boundary.
Definition: kld_adaptive_particle_filter.h:93
pcl::tracking::Tracker
Tracker represents the base tracker class.
Definition: tracker.h:56
pcl::tracking::KLDAdaptiveParticleFilterTracker::CloudCoherence
PointCloudCoherence< PointInT > CloudCoherence
Definition: kld_adaptive_particle_filter.h:59
pcl::tracking::KLDAdaptiveParticleFilterTracker::epsilon_
double epsilon_
error between K-L distance and MLE
Definition: kld_adaptive_particle_filter.h:203
pcl::tracking::KLDAdaptiveParticleFilterTracker::Coherence
PointCoherence< PointInT > Coherence
Definition: kld_adaptive_particle_filter.h:55
pcl::tracking::KLDAdaptiveParticleFilterTracker::getDelta
double getDelta() const
get delta to be used in chi-squared distribution.
Definition: kld_adaptive_particle_filter.h:104
pcl::tracking::KLDAdaptiveParticleFilterTracker::CloudCoherencePtr
boost::shared_ptr< CloudCoherence > CloudCoherencePtr
Definition: kld_adaptive_particle_filter.h:60
pcl::B
Definition: norms.h:55
pcl::tracking::KLDAdaptiveParticleFilterTracker::PointCloudInPtr
PointCloudIn::Ptr PointCloudInPtr
Definition: kld_adaptive_particle_filter.h:48
pcl::tracking::Tracker::tracker_name_
std::string tracker_name_
The tracker name.
Definition: tracker.h:92