Point Cloud Library (PCL) 1.12.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>()
68 , epsilon_(0)
69 , delta_(0.99)
70 , bin_size_()
71 {
72 tracker_name_ = "KLDAdaptiveParticleFilterTracker";
73 }
74
75 /** \brief set the bin size.
76 * \param bin_size the size of a bin
77 */
78 inline void
79 setBinSize(const StateT& bin_size)
80 {
81 bin_size_ = bin_size;
82 }
83
84 /** \brief get the bin size. */
85 inline StateT
86 getBinSize() const
87 {
88 return (bin_size_);
89 }
90
91 /** \brief set the maximum number of the particles.
92 * \param nr the maximum number of the particles.
93 */
94 inline void
95 setMaximumParticleNum(unsigned int nr)
96 {
98 }
99
100 /** \brief get the maximum number of the particles.*/
101 inline unsigned int
103 {
105 }
106
107 /** \brief set epsilon to be used to calc K-L boundary.
108 * \param eps epsilon
109 */
110 inline void
111 setEpsilon(double eps)
112 {
113 epsilon_ = eps;
114 }
115
116 /** \brief get epsilon to be used to calc K-L boundary. */
117 inline double
119 {
120 return (epsilon_);
121 }
122
123 /** \brief set delta to be used in chi-squared distribution.
124 * \param delta delta of chi-squared distribution.
125 */
126 inline void
127 setDelta(double delta)
128 {
129 delta_ = delta;
130 }
131
132 /** \brief get delta to be used in chi-squared distribution.*/
133 inline double
134 getDelta() const
135 {
136 return (delta_);
137 }
138
139protected:
140 /** \brief return true if the two bins are equal.
141 * \param a index of the bin
142 * \param b index of the bin
143 */
144 virtual bool
145 equalBin(const std::vector<int>& a, const std::vector<int>& b)
146 {
147 int dimension = StateT::stateDimension();
148 for (int i = 0; i < dimension; i++)
149 if (a[i] != b[i])
150 return (false);
151 return (true);
152 }
153
154 /** \brief return upper quantile of standard normal distribution.
155 * \param[in] u ratio of quantile.
156 */
157 double
159 {
160 const double a[9] = {1.24818987e-4,
161 -1.075204047e-3,
162 5.198775019e-3,
163 -0.019198292004,
164 0.059054035642,
165 -0.151968751364,
166 0.319152932694,
167 -0.5319230073,
168 0.797884560593};
169 const double b[15] = {-4.5255659e-5,
170 1.5252929e-4,
171 -1.9538132e-5,
172 -6.76904986e-4,
173 1.390604284e-3,
174 -7.9462082e-4,
175 -2.034254874e-3,
176 6.549791214e-3,
177 -0.010557625006,
178 0.011630447319,
179 -9.279453341e-3,
180 5.353579108e-3,
181 -2.141268741e-3,
182 5.35310549e-4,
183 0.999936657524};
184 double w, y, z;
185
186 if (u == 0.)
187 return (0.5);
188 y = u / 2.0;
189 if (y < -3.)
190 return (0.0);
191 if (y > 3.)
192 return (1.0);
193 if (y < 0.0)
194 y = -y;
195 if (y < 1.0) {
196 w = y * y;
197 z = a[0];
198 for (int i = 1; i < 9; i++)
199 z = z * w + a[i];
200 z *= (y * 2.0);
201 }
202 else {
203 y -= 2.0;
204 z = b[0];
205 for (int i = 1; i < 15; i++)
206 z = z * y + b[i];
207 }
208
209 if (u < 0.0)
210 return ((1. - z) / 2.0);
211 return ((1. + z) / 2.0);
212 }
213
214 /** \brief calculate K-L boundary. K-L boundary follows 1/2e*chi(k-1, 1-d)^2.
215 * \param[in] k the number of bins and the first parameter of chi distribution.
216 */
217 virtual double
219 {
220 double z = normalQuantile(delta_);
221 double chi = 1.0 - 2.0 / (9.0 * (k - 1)) + sqrt(2.0 / (9.0 * (k - 1))) * z;
222 return ((k - 1.0) / (2.0 * epsilon_) * chi * chi * chi);
223 }
224
225 /** \brief insert a bin into the set of the bins. if that bin is already registered,
226 * return false. if not, return true.
227 * \param new_bin a bin to be inserted.
228 * \param bins a set of the bins
229 */
230 virtual bool
231 insertIntoBins(std::vector<int>&& new_bin, std::vector<std::vector<int>>& bins);
232
233 /** \brief This method should get called before starting the actual
234 * computation. */
235 bool
236 initCompute() override;
237
238 /** \brief resampling phase of particle filter method. sampling the particles
239 * according to the weights calculated in weight method. in particular, "sample with
240 * replacement" is archieved by walker's alias method.
241 */
242 void
243 resample() override;
244
245 /** \brief the maximum number of the particles. */
247
248 /** \brief error between K-L distance and MLE*/
249 double epsilon_;
250
251 /** \brief probability of distance between K-L distance and MLE is less than
252 * epsilon_*/
253 double delta_;
254
255 /** \brief the size of a bin.*/
256 StateT bin_size_;
257};
258} // namespace tracking
259} // namespace pcl
260
261#ifdef PCL_NO_PRECOMPILE
262#include <pcl/tracking/impl/kld_adaptive_particle_filter.hpp>
263#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