kld_adaptive_particle_filter_omp.hpp
Go to the documentation of this file.
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


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:25:13