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
00147 inline void
00148 setInitialNoiseCovariance (const std::vector<double> &initial_noise_covariance)
00149 {
00150 initial_noise_covariance_ = initial_noise_covariance;
00151 }
00152
00157 inline void
00158 setInitialNoiseMean (const std::vector<double> &initial_noise_mean)
00159 {
00160 initial_noise_mean_ = initial_noise_mean;
00161 }
00162
00166 inline void
00167 setResampleLikelihoodThr (const double resample_likelihood_thr)
00168 {
00169 resample_likelihood_thr_ = resample_likelihood_thr;
00170 }
00171
00177 inline void
00178 setOcclusionAngleThe (const double occlusion_angle_thr)
00179 {
00180 occlusion_angle_thr_ = occlusion_angle_thr;
00181 }
00182
00188 inline void
00189 setMinIndices (const int min_indices) { min_indices_ = min_indices; }
00190
00194 inline void setTrans (const Eigen::Affine3f &trans) { trans_ = trans; }
00195
00197 inline Eigen::Affine3f getTrans () const { return trans_; }
00198
00200 virtual inline StateT getResult () const { return representative_state_; }
00201
00205 Eigen::Affine3f toEigenMatrix (const StateT& particle)
00206 {
00207 return particle.toEigenMatrix ();
00208 }
00209
00211 inline PointCloudStatePtr getParticles () const { return particles_; }
00212
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
00315 void calcBoundingBox (double &x_min, double &x_max,
00316 double &y_min, double &y_max,
00317 double &z_min, double &z_max);
00318
00324 void cropInputPointCloud (const PointCloudInConstPtr &cloud, PointCloudIn &output);
00325
00326
00327
00335 void computeTransformedPointCloud (const StateT& hypothesis,
00336 std::vector<int>& indices,
00337 PointCloudIn &cloud);
00338
00347 void computeTransformedPointCloudWithNormal (const StateT& hypothesis,
00348 std::vector<int>& indices,
00349 PointCloudIn &cloud);
00350
00358 void computeTransformedPointCloudWithoutNormal (const StateT& hypothesis,
00359 PointCloudIn &cloud);
00360
00361
00363 virtual bool initCompute ();
00364
00368 virtual void weight ();
00369
00374 virtual void resample ();
00375
00377 virtual void update ();
00378
00380 virtual void normalizeWeight ();
00381
00385 void initParticles (bool reset);
00386
00389 virtual void computeTracking ();
00390
00410 int sampleWithReplacement (const std::vector<int>& a, const std::vector<double>& q);
00411
00413 void genAliasTable (std::vector<int> &a, std::vector<double> &q, const PointCloudStateConstPtr &particles);
00414
00416 void
00417 resampleWithReplacement ();
00418
00420 void
00421 resampleDeterministic ();
00422
00426 bool
00427 testChangeDetection (const PointCloudInConstPtr &input);
00428
00430 int iteration_num_;
00431
00433 int particle_num_;
00434
00436 int min_indices_;
00437
00439 double fit_ratio_;
00440
00442 PointCloudInConstPtr ref_;
00443
00445 PointCloudStatePtr particles_;
00446
00448 CloudCoherencePtr coherence_;
00449
00453 std::vector<double> step_noise_covariance_;
00454
00458 std::vector<double> initial_noise_covariance_;
00459
00461 std::vector<double> initial_noise_mean_;
00462
00464 double resample_likelihood_thr_;
00465
00467 double occlusion_angle_thr_;
00468
00471 double alpha_;
00472
00474 StateT representative_state_;
00475
00477 Eigen::Affine3f trans_;
00478
00480 bool use_normal_;
00481
00483 StateT motion_;
00484
00486 double motion_ratio_;
00487
00489 pcl::PassThrough<PointInT> pass_x_;
00491 pcl::PassThrough<PointInT> pass_y_;
00493 pcl::PassThrough<PointInT> pass_z_;
00494
00496 std::vector<PointCloudInPtr> transed_reference_vector_;
00497
00499 boost::shared_ptr<pcl::octree::OctreePointCloudChangeDetector<PointInT> > change_detector_;
00500
00502 bool changed_;
00503
00505 unsigned int change_counter_;
00506
00508 unsigned int change_detector_filter_;
00509
00511 unsigned int change_detector_interval_;
00512
00514 double change_detector_resolution_;
00515
00517 bool use_change_detector_;
00518 };
00519 }
00520 }
00521
00522
00523
00524 #endif //PCL_TRACKING_PARTICLE_FILTER_H_