Point Cloud Library (PCL)
1.7.0
|
00001 #ifndef PCL_TRACKING_IMPL_KLD_ADAPTIVE_PARTICLE_OMP_FILTER_H_ 00002 #define PCL_TRACKING_IMPL_KLD_ADAPTIVE_PARTICLE_OMP_FILTER_H_ 00003 00004 #include <pcl/tracking/kld_adaptive_particle_filter_omp.h> 00005 00006 template <typename PointInT, typename StateT> void 00007 pcl::tracking::KLDAdaptiveParticleFilterOMPTracker<PointInT, StateT>::weight () 00008 { 00009 if (!use_normal_) 00010 { 00011 #ifdef _OPENMP 00012 #pragma omp parallel for num_threads(threads_) 00013 #endif 00014 for (int i = 0; i < particle_num_; i++) 00015 this->computeTransformedPointCloudWithoutNormal (particles_->points[i], *transed_reference_vector_[i]); 00016 00017 PointCloudInPtr coherence_input (new PointCloudIn); 00018 this->cropInputPointCloud (input_, *coherence_input); 00019 if (change_counter_ == 0) 00020 { 00021 // test change detector 00022 if (!use_change_detector_ || this->testChangeDetection (coherence_input)) 00023 { 00024 changed_ = true; 00025 change_counter_ = change_detector_interval_; 00026 coherence_->setTargetCloud (coherence_input); 00027 coherence_->initCompute (); 00028 #ifdef _OPENMP 00029 #pragma omp parallel for num_threads(threads_) 00030 #endif 00031 for (int i = 0; i < particle_num_; i++) 00032 { 00033 IndicesPtr indices; 00034 coherence_->compute (transed_reference_vector_[i], indices, particles_->points[i].weight); 00035 } 00036 } 00037 else 00038 changed_ = false; 00039 } 00040 else 00041 { 00042 --change_counter_; 00043 coherence_->setTargetCloud (coherence_input); 00044 coherence_->initCompute (); 00045 #ifdef _OPENMP 00046 #pragma omp parallel for num_threads(threads_) 00047 #endif 00048 for (int i = 0; i < particle_num_; i++) 00049 { 00050 IndicesPtr indices; 00051 coherence_->compute (transed_reference_vector_[i], indices, particles_->points[i].weight); 00052 } 00053 } 00054 } 00055 else 00056 { 00057 std::vector<IndicesPtr> indices_list (particle_num_); 00058 for (int i = 0; i < particle_num_; i++) 00059 { 00060 indices_list[i] = IndicesPtr (new std::vector<int>); 00061 } 00062 #ifdef _OPENMP 00063 #pragma omp parallel for num_threads(threads_) 00064 #endif 00065 for (int i = 0; i < particle_num_; i++) 00066 { 00067 this->computeTransformedPointCloudWithNormal (particles_->points[i], *indices_list[i], *transed_reference_vector_[i]); 00068 } 00069 00070 PointCloudInPtr coherence_input (new PointCloudIn); 00071 this->cropInputPointCloud (input_, *coherence_input); 00072 00073 coherence_->setTargetCloud (coherence_input); 00074 coherence_->initCompute (); 00075 #ifdef _OPENMP 00076 #pragma omp parallel for num_threads(threads_) 00077 #endif 00078 for (int i = 0; i < particle_num_; i++) 00079 { 00080 coherence_->compute (transed_reference_vector_[i], indices_list[i], particles_->points[i].weight); 00081 } 00082 } 00083 00084 normalizeWeight (); 00085 } 00086 00087 #define PCL_INSTANTIATE_KLDAdaptiveParticleFilterOMPTracker(T,ST) template class PCL_EXPORTS pcl::tracking::KLDAdaptiveParticleFilterOMPTracker<T,ST>; 00088 00089 #endif