Point Cloud Library (PCL) 1.14.0
Loading...
Searching...
No Matches
kld_adaptive_particle_filter.h
1#pragma once
2
3#include <pcl/tracking/coherence.h>
4#include <pcl/tracking/particle_filter.h>
5#include <pcl/tracking/tracking.h>
6
7namespace pcl {
8namespace tracking {
9
10/** \brief @b KLDAdaptiveParticleFilterTracker tracks the PointCloud which is given by
11 * setReferenceCloud within the measured PointCloud using particle filter method. The
12 * number of the particles changes adaptively based on KLD sampling [D. Fox, NIPS-01],
13 * [D.Fox, IJRR03].
14 * \author Ryohei Ueda
15 * \ingroup tracking
16 */
17template <typename PointInT, typename StateT>
19: public ParticleFilterTracker<PointInT, StateT> {
20public:
21 using Tracker<PointInT, StateT>::tracker_name_;
22 using Tracker<PointInT, StateT>::search_;
23 using Tracker<PointInT, StateT>::input_;
24 using Tracker<PointInT, StateT>::getClassName;
26 using ParticleFilterTracker<PointInT, StateT>::coherence_;
27 using ParticleFilterTracker<PointInT, StateT>::initParticles;
28 using ParticleFilterTracker<PointInT, StateT>::weight;
29 using ParticleFilterTracker<PointInT, StateT>::update;
30 using ParticleFilterTracker<PointInT, StateT>::iteration_num_;
31 using ParticleFilterTracker<PointInT, StateT>::particle_num_;
32 using ParticleFilterTracker<PointInT, StateT>::particles_;
33 using ParticleFilterTracker<PointInT, StateT>::use_normal_;
34 using ParticleFilterTracker<PointInT, StateT>::use_change_detector_;
36 using ParticleFilterTracker<PointInT, StateT>::change_detector_;
37 using ParticleFilterTracker<PointInT, StateT>::motion_;
38 using ParticleFilterTracker<PointInT, StateT>::motion_ratio_;
42
44
45 using Ptr = shared_ptr<KLDAdaptiveParticleFilterTracker<PointInT, StateT>>;
46 using ConstPtr = shared_ptr<const KLDAdaptiveParticleFilterTracker<PointInT, StateT>>;
47
49 using PointCloudInPtr = typename PointCloudIn::Ptr;
50 using PointCloudInConstPtr = typename PointCloudIn::ConstPtr;
51
53 using PointCloudStatePtr = typename PointCloudState::Ptr;
54 using PointCloudStateConstPtr = typename PointCloudState::ConstPtr;
55
57 using CoherencePtr = typename Coherence::Ptr;
58 using CoherenceConstPtr = typename Coherence::ConstPtr;
59
61 using CloudCoherencePtr = typename CloudCoherence::Ptr;
62 using CloudCoherenceConstPtr = typename CloudCoherence::ConstPtr;
63
64 /** \brief Empty constructor. */
66 : ParticleFilterTracker<PointInT, StateT>(), bin_size_()
67 {
68 tracker_name_ = "KLDAdaptiveParticleFilterTracker";
69 }
70
71 /** \brief set the bin size.
72 * \param bin_size the size of a bin
73 */
74 inline void
75 setBinSize(const StateT& bin_size)
76 {
77 bin_size_ = bin_size;
78 }
79
80 /** \brief get the bin size. */
81 inline StateT
82 getBinSize() const
83 {
84 return (bin_size_);
85 }
86
87 /** \brief set the maximum number of the particles.
88 * \param nr the maximum number of the particles.
89 */
90 inline void
91 setMaximumParticleNum(unsigned int nr)
92 {
94 }
95
96 /** \brief get the maximum number of the particles.*/
97 inline unsigned int
99 {
101 }
102
103 /** \brief set epsilon to be used to calc K-L boundary.
104 * \param eps epsilon
105 */
106 inline void
107 setEpsilon(double eps)
108 {
109 epsilon_ = eps;
110 }
111
112 /** \brief get epsilon to be used to calc K-L boundary. */
113 inline double
115 {
116 return (epsilon_);
117 }
118
119 /** \brief set delta to be used in chi-squared distribution.
120 * \param delta delta of chi-squared distribution.
121 */
122 inline void
123 setDelta(double delta)
124 {
125 delta_ = delta;
126 }
127
128 /** \brief get delta to be used in chi-squared distribution.*/
129 inline double
130 getDelta() const
131 {
132 return (delta_);
133 }
134
135protected:
136 /** \brief return true if the two bins are equal.
137 * \param a index of the bin
138 * \param b index of the bin
139 */
140 virtual bool
141 equalBin(const std::vector<int>& a, const std::vector<int>& b)
142 {
143 int dimension = StateT::stateDimension();
144 for (int i = 0; i < dimension; i++)
145 if (a[i] != b[i])
146 return (false);
147 return (true);
148 }
149
150 /** \brief return upper quantile of standard normal distribution.
151 * \param[in] u ratio of quantile.
152 */
153 double
155 {
156 const double a[9] = {1.24818987e-4,
157 -1.075204047e-3,
158 5.198775019e-3,
159 -0.019198292004,
160 0.059054035642,
161 -0.151968751364,
162 0.319152932694,
163 -0.5319230073,
164 0.797884560593};
165 const double b[15] = {-4.5255659e-5,
166 1.5252929e-4,
167 -1.9538132e-5,
168 -6.76904986e-4,
169 1.390604284e-3,
170 -7.9462082e-4,
171 -2.034254874e-3,
172 6.549791214e-3,
173 -0.010557625006,
174 0.011630447319,
175 -9.279453341e-3,
176 5.353579108e-3,
177 -2.141268741e-3,
178 5.35310549e-4,
179 0.999936657524};
180 double w, y, z;
181
182 if (u == 0.)
183 return (0.5);
184 y = u / 2.0;
185 if (y < -3.)
186 return (0.0);
187 if (y > 3.)
188 return (1.0);
189 if (y < 0.0)
190 y = -y;
191 if (y < 1.0) {
192 w = y * y;
193 z = a[0];
194 for (int i = 1; i < 9; i++)
195 z = z * w + a[i];
196 z *= (y * 2.0);
197 }
198 else {
199 y -= 2.0;
200 z = b[0];
201 for (int i = 1; i < 15; i++)
202 z = z * y + b[i];
203 }
204
205 if (u < 0.0)
206 return ((1. - z) / 2.0);
207 return ((1. + z) / 2.0);
208 }
209
210 /** \brief calculate K-L boundary. K-L boundary follows 1/2e*chi(k-1, 1-d)^2.
211 * \param[in] k the number of bins and the first parameter of chi distribution.
212 */
213 virtual double
215 {
216 double z = normalQuantile(delta_);
217 double chi = 1.0 - 2.0 / (9.0 * (k - 1)) + sqrt(2.0 / (9.0 * (k - 1))) * z;
218 return ((k - 1.0) / (2.0 * epsilon_) * chi * chi * chi);
219 }
220
221 /** \brief insert a bin into the set of the bins. if that bin is already registered,
222 * return false. if not, return true.
223 * \param new_bin a bin to be inserted.
224 * \param bins a set of the bins
225 */
226 virtual bool
227 insertIntoBins(std::vector<int>&& new_bin, std::vector<std::vector<int>>& bins);
228
229 /** \brief This method should get called before starting the actual
230 * computation. */
231 bool
232 initCompute() override;
233
234 /** \brief resampling phase of particle filter method. sampling the particles
235 * according to the weights calculated in weight method. in particular, "sample with
236 * replacement" is achieved by walker's alias method.
237 */
238 void
239 resample() override;
240
241 /** \brief the maximum number of the particles. */
243
244 /** \brief error between K-L distance and MLE*/
245 double epsilon_{0.0};
246
247 /** \brief probability of distance between K-L distance and MLE is less than
248 * epsilon_*/
249 double delta_{0.99};
250
251 /** \brief the size of a bin.*/
252 StateT bin_size_;
253};
254} // namespace tracking
255} // namespace pcl
256
257#ifdef PCL_NO_PRECOMPILE
258#include <pcl/tracking/impl/kld_adaptive_particle_filter.hpp>
259#endif
PointCloudConstPtr input_
The input point cloud dataset.
Definition pcl_base.h:147
KLDAdaptiveParticleFilterTracker tracks the PointCloud which is given by setReferenceCloud within the...
shared_ptr< KLDAdaptiveParticleFilterTracker< PointInT, StateT > > Ptr
void setDelta(double delta)
set delta to be used in chi-squared distribution.
virtual double calcKLBound(int k)
calculate K-L boundary.
double getDelta() const
get delta to be used in chi-squared distribution.
void setEpsilon(double eps)
set epsilon to be used to calc K-L boundary.
unsigned int getMaximumParticleNum() const
get the maximum number of the particles.
double getEpsilon() const
get epsilon to be used to calc K-L boundary.
void resample() override
resampling phase of particle filter method.
unsigned int maximum_particle_number_
the maximum number of the particles.
shared_ptr< const KLDAdaptiveParticleFilterTracker< PointInT, StateT > > ConstPtr
virtual bool insertIntoBins(std::vector< int > &&new_bin, std::vector< std::vector< int > > &bins)
insert a bin into the set of the bins.
typename Tracker< PointInT, StateT >::PointCloudIn PointCloudIn
double normalQuantile(double u)
return upper quantile of standard normal distribution.
double delta_
probability of distance between K-L distance and MLE is less than epsilon_
void setMaximumParticleNum(unsigned int nr)
set the maximum number of the particles.
typename Tracker< PointInT, StateT >::PointCloudState PointCloudState
bool initCompute() override
This method should get called before starting the actual computation.
void setBinSize(const StateT &bin_size)
set the bin size.
virtual bool equalBin(const std::vector< int > &a, const std::vector< int > &b)
return true if the two bins are equal.
ParticleFilterTracker tracks the PointCloud which is given by setReferenceCloud within the measured P...
int iteration_num_
The number of iteration of particlefilter.
double motion_ratio_
Ratio of hypothesis to use motion model.
void initParticles(bool reset)
Initialize the particles.
CloudCoherencePtr coherence_
A pointer to PointCloudCoherence.
int sampleWithReplacement(const std::vector< int > &a, const std::vector< double > &q)
Implementation of "sample with replacement" using Walker's alias method.
StateT representative_state_
The result of tracking.
pcl::octree::OctreePointCloudChangeDetector< PointInT >::Ptr change_detector_
Change detector used as a trigger to track.
double change_detector_resolution_
Resolution of change detector.
std::vector< double > step_noise_covariance_
The diagonal elements of covariance matrix of the step noise.
std::vector< PointCloudInPtr > transed_reference_vector_
A list of the pointers to pointclouds.
bool use_normal_
A flag to use normal or not.
PointCloudStatePtr particles_
A pointer to the particles
virtual void update()
Calculate the weighted mean of the particles and set it as the result.
int particle_num_
The number of the particles.
virtual void weight()
Weighting phase of particle filter method.
StateT motion_
Difference between the result in t and t-1.
bool use_change_detector_
The flag which will be true if using change detection.
PointCloudCoherence is a base class to compute coherence between the two PointClouds.
Definition coherence.h:59
PointCoherence is a base class to compute coherence between the two points.
Definition coherence.h:15
Tracker represents the base tracker class.
Definition tracker.h:55
const std::string & getClassName() const
Get a string representation of the name of this class.
Definition tracker.h:97
SearchPtr search_
A pointer to the spatial search object.
Definition tracker.h:93
std::string tracker_name_
The tracker name.
Definition tracker.h:90
pcl::PointCloud< StateT > PointCloudState
Definition tracker.h:74