Point Cloud Library (PCL)
1.7.0
|
00001 #ifndef PCL_TRACKING_IMPL_KLD_ADAPTIVE_PARTICLE_FILTER_H_ 00002 #define PCL_TRACKING_IMPL_KLD_ADAPTIVE_PARTICLE_FILTER_H_ 00003 00004 #include <pcl/tracking/kld_adaptive_particle_filter.h> 00005 00006 template <typename PointInT, typename StateT> bool 00007 pcl::tracking::KLDAdaptiveParticleFilterTracker<PointInT, StateT>::initCompute () 00008 { 00009 if (!Tracker<PointInT, StateT>::initCompute ()) 00010 { 00011 PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ()); 00012 return (false); 00013 } 00014 00015 if (transed_reference_vector_.empty ()) 00016 { 00017 // only one time allocation 00018 transed_reference_vector_.resize (maximum_particle_number_); 00019 for (unsigned int i = 0; i < maximum_particle_number_; i++) 00020 { 00021 transed_reference_vector_[i] = PointCloudInPtr (new PointCloudIn ()); 00022 } 00023 } 00024 00025 coherence_->setTargetCloud (input_); 00026 00027 if (!change_detector_) 00028 change_detector_ = boost::shared_ptr<pcl::octree::OctreePointCloudChangeDetector<PointInT> >(new pcl::octree::OctreePointCloudChangeDetector<PointInT> (change_detector_resolution_)); 00029 00030 if (!particles_ || particles_->points.empty ()) 00031 initParticles (true); 00032 return (true); 00033 } 00034 00035 template <typename PointInT, typename StateT> bool 00036 pcl::tracking::KLDAdaptiveParticleFilterTracker<PointInT, StateT>::insertIntoBins 00037 (std::vector<int> bin, std::vector<std::vector<int> > &B) 00038 { 00039 for (size_t i = 0; i < B.size (); i++) 00040 { 00041 if (equalBin (bin, B[i])) 00042 return false; 00043 } 00044 B.push_back (bin); 00045 return true; 00046 } 00047 00048 template <typename PointInT, typename StateT> void 00049 pcl::tracking::KLDAdaptiveParticleFilterTracker<PointInT, StateT>::resample () 00050 { 00051 unsigned int k = 0; 00052 unsigned int n = 0; 00053 PointCloudStatePtr S (new PointCloudState); 00054 std::vector<std::vector<int> > B; // bins 00055 00056 // initializing for sampling without replacement 00057 std::vector<int> a (particles_->points.size ()); 00058 std::vector<double> q (particles_->points.size ()); 00059 this->genAliasTable (a, q, particles_); 00060 00061 const std::vector<double> zero_mean (StateT::stateDimension (), 0.0); 00062 00063 // select the particles with KLD sampling 00064 do 00065 { 00066 int j_n = sampleWithReplacement (a, q); 00067 StateT x_t = particles_->points[j_n]; 00068 x_t.sample (zero_mean, step_noise_covariance_); 00069 00070 // motion 00071 if (rand () / double (RAND_MAX) < motion_ratio_) 00072 x_t = x_t + motion_; 00073 00074 S->points.push_back (x_t); 00075 // calc bin 00076 std::vector<int> bin (StateT::stateDimension ()); 00077 for (int i = 0; i < StateT::stateDimension (); i++) 00078 bin[i] = static_cast<int> (x_t[i] / bin_size_[i]); 00079 00080 // calc bin index... how? 00081 if (insertIntoBins (bin, B)) 00082 ++k; 00083 ++n; 00084 } 00085 while (k < 2 || (n < maximum_particle_number_ && n < calcKLBound (k))); 00086 00087 particles_ = S; // swap 00088 particle_num_ = static_cast<int> (particles_->points.size ()); 00089 } 00090 00091 00092 #define PCL_INSTANTIATE_KLDAdaptiveParticleFilterTracker(T,ST) template class PCL_EXPORTS pcl::tracking::KLDAdaptiveParticleFilterTracker<T,ST>; 00093 00094 #endif