Point Cloud Library (PCL)
1.7.0
|
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 { 00017 /** \brief @b ParticleFilterTracker tracks the PointCloud which is given by 00018 setReferenceCloud within the measured PointCloud using particle filter method. 00019 * \author Ryohei Ueda 00020 * \ingroup tracking 00021 */ 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 00053 /** \brief Empty constructor. */ 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 00093 /** \brief Set the number of iteration. 00094 * \param[in] iteration_num the number of iteration. 00095 */ 00096 inline void 00097 setIterationNum (const int iteration_num) { iteration_num_ = iteration_num; } 00098 00099 /** \brief Get the number of iteration. */ 00100 inline int 00101 getIterationNum () const { return iteration_num_; } 00102 00103 /** \brief Set the number of the particles. 00104 * \param[in] particle_num the number of the particles. 00105 */ 00106 inline void 00107 setParticleNum (const int particle_num) { particle_num_ = particle_num; } 00108 00109 /** \brief Get the number of the particles. */ 00110 inline int 00111 getParticleNum () const { return particle_num_; } 00112 00113 /** \brief Set a pointer to a reference dataset to be tracked. 00114 * \param[in] ref a pointer to a PointCloud message 00115 */ 00116 inline void 00117 setReferenceCloud (const PointCloudInConstPtr &ref) { ref_ = ref; } 00118 00119 /** \brief Get a pointer to a reference dataset to be tracked. */ 00120 inline PointCloudInConstPtr const 00121 getReferenceCloud () { return ref_; } 00122 00123 /** \brief Set the PointCloudCoherence as likelihood. 00124 * \param[in] coherence a pointer to PointCloudCoherence. 00125 */ 00126 inline void 00127 setCloudCoherence (const CloudCoherencePtr &coherence) { coherence_ = coherence; } 00128 00129 /** \brief Get the PointCloudCoherence to compute likelihood. */ 00130 inline CloudCoherencePtr 00131 getCloudCoherence () const { return coherence_; } 00132 00133 00134 /** \brief Set the covariance of step noise. 00135 * \param[in] step_noise_covariance the diagonal elements of covariance matrix of step noise. 00136 */ 00137 inline void 00138 setStepNoiseCovariance (const std::vector<double> &step_noise_covariance) 00139 { 00140 step_noise_covariance_ = step_noise_covariance; 00141 } 00142 00143 /** \brief Set the covariance of the initial noise. It will be used when initializing the particles. 00144 * \param[in] initial_noise_covariance the diagonal elements of covariance matrix of initial noise. 00145 */ 00146 inline void 00147 setInitialNoiseCovariance (const std::vector<double> &initial_noise_covariance) 00148 { 00149 initial_noise_covariance_ = initial_noise_covariance; 00150 } 00151 00152 /** \brief Set the mean of the initial noise. It will be used when initializing the particles. 00153 * \param[in] initial_noise_mean the mean values of initial noise. 00154 */ 00155 inline void 00156 setInitialNoiseMean (const std::vector<double> &initial_noise_mean) 00157 { 00158 initial_noise_mean_ = initial_noise_mean; 00159 } 00160 00161 /** \brief Set the threshold to re-initialize the particles. 00162 * \param[in] resample_likelihood_thr threshold to re-initialize. 00163 */ 00164 inline void 00165 setResampleLikelihoodThr (const double resample_likelihood_thr) 00166 { 00167 resample_likelihood_thr_ = resample_likelihood_thr; 00168 } 00169 00170 /** \brief Set the threshold of angle to be considered occlusion (default: pi/2). 00171 * ParticleFilterTracker does not take the occluded points into account according to the angle 00172 * between the normal and the position. 00173 * \param[in] occlusion_angle_thr threshold of angle to be considered occlusion. 00174 */ 00175 inline void 00176 setOcclusionAngleThe (const double occlusion_angle_thr) 00177 { 00178 occlusion_angle_thr_ = occlusion_angle_thr; 00179 } 00180 00181 /** \brief Set the minimum number of indices (default: 1). 00182 * ParticleFilterTracker does not take into account the hypothesis 00183 * whose the number of points is smaller than the minimum indices. 00184 * \param[in] min_indices the minimum number of indices. 00185 */ 00186 inline void 00187 setMinIndices (const int min_indices) { min_indices_ = min_indices; } 00188 00189 /** \brief Set the transformation from the world coordinates to the frame of the particles. 00190 * \param[in] trans Affine transformation from the worldcoordinates to the frame of the particles. 00191 */ 00192 inline void setTrans (const Eigen::Affine3f &trans) { trans_ = trans; } 00193 00194 /** \brief Get the transformation from the world coordinates to the frame of the particles. */ 00195 inline Eigen::Affine3f getTrans () const { return trans_; } 00196 00197 /** \brief Get an instance of the result of tracking. 00198 * This function returns the particle that represents the transform between the reference point cloud at the 00199 * beginning and the best guess about its location in the most recent frame. 00200 */ 00201 virtual inline StateT getResult () const { return representative_state_; } 00202 00203 /** \brief Convert a state to affine transformation from the world coordinates frame. 00204 * \param[in] particle an instance of StateT. 00205 */ 00206 Eigen::Affine3f toEigenMatrix (const StateT& particle) 00207 { 00208 return particle.toEigenMatrix (); 00209 } 00210 00211 /** \brief Get a pointer to a pointcloud of the particles. */ 00212 inline PointCloudStatePtr getParticles () const { return particles_; } 00213 00214 /** \brief Normalize the weight of a particle using \f$ exp(1- alpha ( w - w_{min}) / (w_max - w_min)) \f$ 00215 * \note This method is described in [P.Azad et. al, ICRA11]. 00216 * \param[in] w the weight to be normalized 00217 * \param[in] w_min the minimum weight of the particles 00218 * \param[in] w_max the maximum weight of the particles 00219 */ 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 00225 /** \brief Set the value of alpha. 00226 * \param[in] alpha the value of alpha 00227 */ 00228 inline void setAlpha (double alpha) { alpha_ = alpha; } 00229 00230 /** \brief Get the value of alpha. */ 00231 inline double getAlpha () { return alpha_; } 00232 00233 /** \brief Set the value of use_normal_. 00234 * \param[in] use_normal the value of use_normal_. 00235 */ 00236 inline void setUseNormal (bool use_normal) { use_normal_ = use_normal; } 00237 00238 /** \brief Get the value of use_normal_. */ 00239 inline bool getUseNormal () { return use_normal_; } 00240 00241 /** \brief Set the value of use_change_detector_. 00242 * \param[in] use_change_detector the value of use_change_detector_. 00243 */ 00244 inline void setUseChangeDetector (bool use_change_detector) { use_change_detector_ = use_change_detector; } 00245 00246 /** \brief Get the value of use_change_detector_. */ 00247 inline bool getUseChangeDetector () { return use_change_detector_; } 00248 00249 /** \brief Set the motion ratio 00250 * \param[in] motion_ratio the ratio of hypothesis to use motion model. 00251 */ 00252 inline void setMotionRatio (double motion_ratio) { motion_ratio_ = motion_ratio; } 00253 00254 /** \brief Get the motion ratio. */ 00255 inline double getMotionRatio () { return motion_ratio_;} 00256 00257 /** \brief Set the number of interval frames to run change detection. 00258 * \param[in] change_detector_interval the number of interval frames. 00259 */ 00260 inline void setIntervalOfChangeDetection (unsigned int change_detector_interval) 00261 { 00262 change_detector_interval_ = change_detector_interval; 00263 } 00264 00265 /** \brief Get the number of interval frames to run change detection. */ 00266 inline unsigned int getIntervalOfChangeDetection () 00267 { 00268 return change_detector_interval_; 00269 } 00270 00271 /** \brief Set the minimum amount of points required within leaf node to become serialized in change detection 00272 * \param[in] change_detector_filter the minimum amount of points required within leaf node 00273 */ 00274 inline void setMinPointsOfChangeDetection (unsigned int change_detector_filter) 00275 { 00276 change_detector_filter_ = change_detector_filter; 00277 } 00278 00279 /** \brief Set the resolution of change detection. 00280 * \param[in] resolution resolution of change detection octree 00281 */ 00282 inline void setResolutionOfChangeDetection (double resolution) { change_detector_resolution_ = resolution; } 00283 00284 /** \brief Get the resolution of change detection. */ 00285 inline double getResolutionOfChangeDetection () { return change_detector_resolution_; } 00286 00287 /** \brief Get the minimum amount of points required within leaf node to become serialized in change detection. */ 00288 inline unsigned int getMinPointsOfChangeDetection () 00289 { 00290 return change_detector_filter_; 00291 } 00292 00293 /** \brief Get the adjustment ratio. */ 00294 inline double 00295 getFitRatio() const { return fit_ratio_; } 00296 00297 /** \brief Reset the particles to restart tracking*/ 00298 virtual inline void resetTracking () 00299 { 00300 if (particles_) 00301 particles_->points.clear (); 00302 } 00303 00304 protected: 00305 00306 /** \brief Compute the parameters for the bounding box of hypothesis pointclouds. 00307 * \param[out] x_min the minimum value of x axis. 00308 * \param[out] x_max the maximum value of x axis. 00309 * \param[out] y_min the minimum value of y axis. 00310 * \param[out] y_max the maximum value of y axis. 00311 * \param[out] z_min the minimum value of z axis. 00312 * \param[out] z_max the maximum value of z axis. 00313 */ 00314 void calcBoundingBox (double &x_min, double &x_max, 00315 double &y_min, double &y_max, 00316 double &z_min, double &z_max); 00317 00318 /** \brief Crop the pointcloud by the bounding box calculated from hypothesis and the reference pointcloud. 00319 * \param[in] cloud a pointer to pointcloud to be cropped. 00320 * \param[out] output a pointer to be assigned the cropped pointcloud. 00321 */ 00322 void cropInputPointCloud (const PointCloudInConstPtr &cloud, PointCloudIn &output); 00323 00324 00325 00326 /** \brief Compute a reference pointcloud transformed to the pose that hypothesis represents. 00327 * \param[in] hypothesis a particle which represents a hypothesis. 00328 * \param[in] indices the indices which should be taken into account. 00329 * \param[out] cloud the resultant point cloud model dataset which is transformed to hypothesis. 00330 **/ 00331 void computeTransformedPointCloud (const StateT& hypothesis, 00332 std::vector<int>& indices, 00333 PointCloudIn &cloud); 00334 00335 /** \brief Compute a reference pointcloud transformed to the pose that hypothesis represents and calculate 00336 * indices taking occlusion into account. 00337 * \param[in] hypothesis a particle which represents a hypothesis. 00338 * \param[in] indices the indices which should be taken into account. 00339 * \param[out] cloud the resultant point cloud model dataset which is transformed to hypothesis. 00340 **/ 00341 void computeTransformedPointCloudWithNormal (const StateT& hypothesis, 00342 std::vector<int>& indices, 00343 PointCloudIn &cloud); 00344 00345 /** \brief Compute a reference pointcloud transformed to the pose that hypothesis represents and calculate 00346 * indices without taking occlusion into account. 00347 * \param[in] hypothesis a particle which represents a hypothesis. 00348 * \param[out] cloud the resultant point cloud model dataset which is transformed to hypothesis. 00349 **/ 00350 void computeTransformedPointCloudWithoutNormal (const StateT& hypothesis, 00351 PointCloudIn &cloud); 00352 00353 00354 /** \brief This method should get called before starting the actual computation. */ 00355 virtual bool initCompute (); 00356 00357 /** \brief Weighting phase of particle filter method. Calculate the likelihood of all of the particles and set the weights. */ 00358 virtual void weight (); 00359 00360 /** \brief Resampling phase of particle filter method. Sampling the particles according to the weights calculated 00361 * in weight method. In particular, "sample with replacement" is archieved by walker's alias method. 00362 */ 00363 virtual void resample (); 00364 00365 /** \brief Calculate the weighted mean of the particles and set it as the result. */ 00366 virtual void update (); 00367 00368 /** \brief Normalize the weights of all the particels. */ 00369 virtual void normalizeWeight (); 00370 00371 /** \brief Initialize the particles. initial_noise_covariance_ and initial_noise_mean_ are used for Gaussian sampling. */ 00372 void initParticles (bool reset); 00373 00374 /** \brief Track the pointcloud using particle filter method. */ 00375 virtual void computeTracking (); 00376 00377 /** \brief Implementation of "sample with replacement" using Walker's alias method. 00378 about Walker's alias method, you can check the paper below: 00379 @article{355749, 00380 author = {Walker, Alastair J.}, 00381 title = {An Efficient Method for Generating Discrete 00382 Random Variables with General Distributions}, 00383 journal = {ACM Trans. Math. Softw.}, 00384 volume = {3}, 00385 number = {3}, 00386 year = {1977}, 00387 issn = {0098-3500}, 00388 pages = {253--256}, 00389 doi = {http://doi.acm.org/10.1145/355744.355749}, 00390 publisher = {ACM}, 00391 address = {New York, NY, USA}, 00392 } 00393 \param a an alias table, which generated by genAliasTable. 00394 \param q a table of weight, which generated by genAliasTable. 00395 */ 00396 int sampleWithReplacement (const std::vector<int>& a, const std::vector<double>& q); 00397 00398 /** \brief Generate the tables for walker's alias method. */ 00399 void genAliasTable (std::vector<int> &a, std::vector<double> &q, const PointCloudStateConstPtr &particles); 00400 00401 /** \brief Resampling the particle with replacement. */ 00402 void 00403 resampleWithReplacement (); 00404 00405 /** \brief Resampling the particle in deterministic way. */ 00406 void 00407 resampleDeterministic (); 00408 00409 /** \brief Run change detection and return true if there is a change. 00410 * \param[in] input a pointer to the input pointcloud. 00411 */ 00412 bool 00413 testChangeDetection (const PointCloudInConstPtr &input); 00414 00415 /** \brief The number of iteration of particlefilter. */ 00416 int iteration_num_; 00417 00418 /** \brief The number of the particles. */ 00419 int particle_num_; 00420 00421 /** \brief The minimum number of points which the hypothesis should have. */ 00422 int min_indices_; 00423 00424 /** \brief Adjustment of the particle filter. */ 00425 double fit_ratio_; 00426 00427 /** \brief A pointer to reference point cloud. */ 00428 PointCloudInConstPtr ref_; 00429 00430 /** \brief A pointer to the particles */ 00431 PointCloudStatePtr particles_; 00432 00433 /** \brief A pointer to PointCloudCoherence. */ 00434 CloudCoherencePtr coherence_; 00435 00436 /** \brief The diagonal elements of covariance matrix of the step noise. the covariance matrix is used 00437 * at every resample method. 00438 */ 00439 std::vector<double> step_noise_covariance_; 00440 00441 /** \brief The diagonal elements of covariance matrix of the initial noise. the covariance matrix is used 00442 * when initialize the particles. 00443 */ 00444 std::vector<double> initial_noise_covariance_; 00445 00446 /** \brief The mean values of initial noise. */ 00447 std::vector<double> initial_noise_mean_; 00448 00449 /** \brief The threshold for the particles to be re-initialized. */ 00450 double resample_likelihood_thr_; 00451 00452 /** \brief The threshold for the points to be considered as occluded. */ 00453 double occlusion_angle_thr_; 00454 00455 /** \brief The weight to be used in normalization of the weights of the particles. */ 00456 double alpha_; 00457 00458 /** \brief The result of tracking. */ 00459 StateT representative_state_; 00460 00461 /** \brief An affine transformation from the world coordinates frame to the origin of the particles. */ 00462 Eigen::Affine3f trans_; 00463 00464 /** \brief A flag to use normal or not. defaults to false. */ 00465 bool use_normal_; 00466 00467 /** \brief Difference between the result in t and t-1. */ 00468 StateT motion_; 00469 00470 /** \brief Ratio of hypothesis to use motion model. */ 00471 double motion_ratio_; 00472 00473 /** \brief Pass through filter to crop the pointclouds within the hypothesis bounding box. */ 00474 pcl::PassThrough<PointInT> pass_x_; 00475 /** \brief Pass through filter to crop the pointclouds within the hypothesis bounding box. */ 00476 pcl::PassThrough<PointInT> pass_y_; 00477 /** \brief Pass through filter to crop the pointclouds within the hypothesis bounding box. */ 00478 pcl::PassThrough<PointInT> pass_z_; 00479 00480 /** \brief A list of the pointers to pointclouds. */ 00481 std::vector<PointCloudInPtr> transed_reference_vector_; 00482 00483 /** \brief Change detector used as a trigger to track. */ 00484 boost::shared_ptr<pcl::octree::OctreePointCloudChangeDetector<PointInT> > change_detector_; 00485 00486 /** \brief A flag to be true when change of pointclouds is detected. */ 00487 bool changed_; 00488 00489 /** \brief A counter to skip change detection. */ 00490 unsigned int change_counter_; 00491 00492 /** \brief Minimum points in a leaf when calling change detector. defaults to 10. */ 00493 unsigned int change_detector_filter_; 00494 00495 /** \brief The number of interval frame to run change detection. defaults to 10. */ 00496 unsigned int change_detector_interval_; 00497 00498 /** \brief Resolution of change detector. defaults to 0.01. */ 00499 double change_detector_resolution_; 00500 00501 /** \brief The flag which will be true if using change detection. */ 00502 bool use_change_detector_; 00503 }; 00504 } 00505 } 00506 00507 // #include <pcl/tracking/impl/particle_filter.hpp> 00508 #ifdef PCL_NO_PRECOMPILE 00509 #include <pcl/tracking/impl/particle_filter.hpp> 00510 #endif 00511 00512 #endif //PCL_TRACKING_PARTICLE_FILTER_H_