00001 #ifndef PCL_TRACKING_PARTICLE_FILTER_H_
00002 #define PCL_TRACKING_PARTICLE_FILTER_H_
00003
00004 #include <pcl/tracking/tracking.h>
00005 #include <pcl/tracking/tracker.h>
00006 #include <pcl/tracking/coherence.h>
00007 #include <pcl/filters/passthrough.h>
00008 #include <pcl/octree/octree.h>
00009
00010 #include <Eigen/Dense>
00011
00012 namespace pcl
00013 {
00014
00015 namespace tracking
00016 {
00022 template <typename PointInT, typename StateT>
00023 class ParticleFilterTracker: public Tracker<PointInT, StateT>
00024 {
00025 protected:
00026 using Tracker<PointInT, StateT>::deinitCompute;
00027
00028 public:
00029 using Tracker<PointInT, StateT>::tracker_name_;
00030 using Tracker<PointInT, StateT>::search_;
00031 using Tracker<PointInT, StateT>::input_;
00032 using Tracker<PointInT, StateT>::indices_;
00033 using Tracker<PointInT, StateT>::getClassName;
00034
00035 typedef Tracker<PointInT, StateT> BaseClass;
00036
00037 typedef typename Tracker<PointInT, StateT>::PointCloudIn PointCloudIn;
00038 typedef typename PointCloudIn::Ptr PointCloudInPtr;
00039 typedef typename PointCloudIn::ConstPtr PointCloudInConstPtr;
00040
00041 typedef typename Tracker<PointInT, StateT>::PointCloudState PointCloudState;
00042 typedef typename PointCloudState::Ptr PointCloudStatePtr;
00043 typedef typename PointCloudState::ConstPtr PointCloudStateConstPtr;
00044
00045 typedef PointCoherence<PointInT> Coherence;
00046 typedef boost::shared_ptr< Coherence > CoherencePtr;
00047 typedef boost::shared_ptr< const Coherence > CoherenceConstPtr;
00048
00049 typedef PointCloudCoherence<PointInT> CloudCoherence;
00050 typedef boost::shared_ptr< CloudCoherence > CloudCoherencePtr;
00051 typedef boost::shared_ptr< const CloudCoherence > CloudCoherenceConstPtr;
00052
00054 ParticleFilterTracker ()
00055 : iteration_num_ (1)
00056 , particle_num_ ()
00057 , min_indices_ (1)
00058 , ref_ ()
00059 , particles_ ()
00060 , coherence_ ()
00061 , step_noise_covariance_ ()
00062 , initial_noise_covariance_ ()
00063 , initial_noise_mean_ ()
00064 , resample_likelihood_thr_ (0.0)
00065 , occlusion_angle_thr_ (M_PI / 2.0)
00066 , alpha_ (15.0)
00067 , representative_state_ ()
00068 , trans_ ()
00069 , use_normal_ (false)
00070 , motion_ ()
00071 , motion_ratio_ (0.25)
00072 , pass_x_ ()
00073 , pass_y_ ()
00074 , pass_z_ ()
00075 , transed_reference_vector_ ()
00076 , change_detector_ ()
00077 , changed_ (false)
00078 , change_counter_ (0)
00079 , change_detector_filter_ (10)
00080 , change_detector_interval_ (10)
00081 , change_detector_resolution_ (0.01)
00082 , use_change_detector_ (false)
00083 {
00084 tracker_name_ = "ParticleFilterTracker";
00085 pass_x_.setFilterFieldName ("x");
00086 pass_y_.setFilterFieldName ("y");
00087 pass_z_.setFilterFieldName ("z");
00088 pass_x_.setKeepOrganized (false);
00089 pass_y_.setKeepOrganized (false);
00090 pass_z_.setKeepOrganized (false);
00091 }
00092
00096 inline void
00097 setIterationNum (const int iteration_num) { iteration_num_ = iteration_num; }
00098
00100 inline int
00101 getIterationNum () const { return iteration_num_; }
00102
00106 inline void
00107 setParticleNum (const int particle_num) { particle_num_ = particle_num; }
00108
00110 inline int
00111 getParticleNum () const { return particle_num_; }
00112
00116 inline void
00117 setReferenceCloud (const PointCloudInConstPtr &ref) { ref_ = ref; }
00118
00120 inline PointCloudInConstPtr const
00121 getReferenceCloud () { return ref_; }
00122
00126 inline void
00127 setCloudCoherence (const CloudCoherencePtr &coherence) { coherence_ = coherence; }
00128
00130 inline CloudCoherencePtr
00131 getCloudCoherence () const { return coherence_; }
00132
00133
00137 inline void
00138 setStepNoiseCovariance (const std::vector<double> &step_noise_covariance)
00139 {
00140 step_noise_covariance_ = step_noise_covariance;
00141 }
00142
00146 inline void
00147 setInitialNoiseCovariance (const std::vector<double> &initial_noise_covariance)
00148 {
00149 initial_noise_covariance_ = initial_noise_covariance;
00150 }
00151
00155 inline void
00156 setInitialNoiseMean (const std::vector<double> &initial_noise_mean)
00157 {
00158 initial_noise_mean_ = initial_noise_mean;
00159 }
00160
00164 inline void
00165 setResampleLikelihoodThr (const double resample_likelihood_thr)
00166 {
00167 resample_likelihood_thr_ = resample_likelihood_thr;
00168 }
00169
00175 inline void
00176 setOcclusionAngleThe (const double occlusion_angle_thr)
00177 {
00178 occlusion_angle_thr_ = occlusion_angle_thr;
00179 }
00180
00186 inline void
00187 setMinIndices (const int min_indices) { min_indices_ = min_indices; }
00188
00192 inline void setTrans (const Eigen::Affine3f &trans) { trans_ = trans; }
00193
00195 inline Eigen::Affine3f getTrans () const { return trans_; }
00196
00201 virtual inline StateT getResult () const { return representative_state_; }
00202
00206 Eigen::Affine3f toEigenMatrix (const StateT& particle)
00207 {
00208 return particle.toEigenMatrix ();
00209 }
00210
00212 inline PointCloudStatePtr getParticles () const { return particles_; }
00213
00220 inline double normalizeParticleWeight (double w, double w_min, double w_max)
00221 {
00222 return exp (1.0 - alpha_ * (w - w_min) / (w_max - w_min));
00223 }
00224
00228 inline void setAlpha (double alpha) { alpha_ = alpha; }
00229
00231 inline double getAlpha () { return alpha_; }
00232
00236 inline void setUseNormal (bool use_normal) { use_normal_ = use_normal; }
00237
00239 inline bool getUseNormal () { return use_normal_; }
00240
00244 inline void setUseChangeDetector (bool use_change_detector) { use_change_detector_ = use_change_detector; }
00245
00247 inline bool getUseChangeDetector () { return use_change_detector_; }
00248
00252 inline void setMotionRatio (double motion_ratio) { motion_ratio_ = motion_ratio; }
00253
00255 inline double getMotionRatio () { return motion_ratio_;}
00256
00260 inline void setIntervalOfChangeDetection (unsigned int change_detector_interval)
00261 {
00262 change_detector_interval_ = change_detector_interval;
00263 }
00264
00266 inline unsigned int getIntervalOfChangeDetection ()
00267 {
00268 return change_detector_interval_;
00269 }
00270
00274 inline void setMinPointsOfChangeDetection (unsigned int change_detector_filter)
00275 {
00276 change_detector_filter_ = change_detector_filter;
00277 }
00278
00282 inline void setResolutionOfChangeDetection (double resolution) { change_detector_resolution_ = resolution; }
00283
00285 inline double getResolutionOfChangeDetection () { return change_detector_resolution_; }
00286
00288 inline unsigned int getMinPointsOfChangeDetection ()
00289 {
00290 return change_detector_filter_;
00291 }
00292
00294 inline double
00295 getFitRatio() const { return fit_ratio_; }
00296
00298 virtual inline void resetTracking ()
00299 {
00300 if (particles_)
00301 particles_->points.clear ();
00302 }
00303
00304 protected:
00305
00314 void calcBoundingBox (double &x_min, double &x_max,
00315 double &y_min, double &y_max,
00316 double &z_min, double &z_max);
00317
00322 void cropInputPointCloud (const PointCloudInConstPtr &cloud, PointCloudIn &output);
00323
00324
00325
00331 void computeTransformedPointCloud (const StateT& hypothesis,
00332 std::vector<int>& indices,
00333 PointCloudIn &cloud);
00334
00341 void computeTransformedPointCloudWithNormal (const StateT& hypothesis,
00342 std::vector<int>& indices,
00343 PointCloudIn &cloud);
00344
00350 void computeTransformedPointCloudWithoutNormal (const StateT& hypothesis,
00351 PointCloudIn &cloud);
00352
00353
00355 virtual bool initCompute ();
00356
00358 virtual void weight ();
00359
00363 virtual void resample ();
00364
00366 virtual void update ();
00367
00369 virtual void normalizeWeight ();
00370
00372 void initParticles (bool reset);
00373
00375 virtual void computeTracking ();
00376
00396 int sampleWithReplacement (const std::vector<int>& a, const std::vector<double>& q);
00397
00399 void genAliasTable (std::vector<int> &a, std::vector<double> &q, const PointCloudStateConstPtr &particles);
00400
00402 void
00403 resampleWithReplacement ();
00404
00406 void
00407 resampleDeterministic ();
00408
00412 bool
00413 testChangeDetection (const PointCloudInConstPtr &input);
00414
00416 int iteration_num_;
00417
00419 int particle_num_;
00420
00422 int min_indices_;
00423
00425 double fit_ratio_;
00426
00428 PointCloudInConstPtr ref_;
00429
00431 PointCloudStatePtr particles_;
00432
00434 CloudCoherencePtr coherence_;
00435
00439 std::vector<double> step_noise_covariance_;
00440
00444 std::vector<double> initial_noise_covariance_;
00445
00447 std::vector<double> initial_noise_mean_;
00448
00450 double resample_likelihood_thr_;
00451
00453 double occlusion_angle_thr_;
00454
00456 double alpha_;
00457
00459 StateT representative_state_;
00460
00462 Eigen::Affine3f trans_;
00463
00465 bool use_normal_;
00466
00468 StateT motion_;
00469
00471 double motion_ratio_;
00472
00474 pcl::PassThrough<PointInT> pass_x_;
00476 pcl::PassThrough<PointInT> pass_y_;
00478 pcl::PassThrough<PointInT> pass_z_;
00479
00481 std::vector<PointCloudInPtr> transed_reference_vector_;
00482
00484 boost::shared_ptr<pcl::octree::OctreePointCloudChangeDetector<PointInT> > change_detector_;
00485
00487 bool changed_;
00488
00490 unsigned int change_counter_;
00491
00493 unsigned int change_detector_filter_;
00494
00496 unsigned int change_detector_interval_;
00497
00499 double change_detector_resolution_;
00500
00502 bool use_change_detector_;
00503 };
00504 }
00505 }
00506
00507
00508 #ifdef PCL_NO_PRECOMPILE
00509 #include <pcl/tracking/impl/particle_filter.hpp>
00510 #endif
00511
00512 #endif //PCL_TRACKING_PARTICLE_FILTER_H_