Point Cloud Library (PCL)
1.7.0
|
00001 #ifndef PCL_TRACKING_IMPL_APPROX_NEAREST_PAIR_POINT_CLOUD_COHERENCE_H_ 00002 #define PCL_TRACKING_IMPL_APPROX_NEAREST_PAIR_POINT_CLOUD_COHERENCE_H_ 00003 00004 #include <pcl/search/octree.h> 00005 #include <pcl/tracking/approx_nearest_pair_point_cloud_coherence.h> 00006 00007 namespace pcl 00008 { 00009 namespace tracking 00010 { 00011 template <typename PointInT> void 00012 ApproxNearestPairPointCloudCoherence<PointInT>::computeCoherence ( 00013 const PointCloudInConstPtr &cloud, const IndicesConstPtr &, float &w) 00014 { 00015 double val = 0.0; 00016 //for (size_t i = 0; i < indices->size (); i++) 00017 for (size_t i = 0; i < cloud->points.size (); i++) 00018 { 00019 int k_index = 0; 00020 float k_distance = 0.0; 00021 //PointInT input_point = cloud->points[(*indices)[i]]; 00022 PointInT input_point = cloud->points[i]; 00023 search_->approxNearestSearch(input_point, k_index, k_distance); 00024 if (k_distance < maximum_distance_ * maximum_distance_) 00025 { 00026 PointInT target_point = target_input_->points[k_index]; 00027 double coherence_val = 1.0; 00028 for (size_t i = 0; i < point_coherences_.size (); i++) 00029 { 00030 PointCoherencePtr coherence = point_coherences_[i]; 00031 double w = coherence->compute (input_point, target_point); 00032 coherence_val *= w; 00033 } 00034 val += coherence_val; 00035 } 00036 } 00037 w = - static_cast<float> (val); 00038 } 00039 00040 template <typename PointInT> bool 00041 ApproxNearestPairPointCloudCoherence<PointInT>::initCompute () 00042 { 00043 if (!PointCloudCoherence<PointInT>::initCompute ()) 00044 { 00045 PCL_ERROR ("[pcl::%s::initCompute] PointCloudCoherence::Init failed.\n", getClassName ().c_str ()); 00046 //deinitCompute (); 00047 return (false); 00048 } 00049 00050 // initialize tree 00051 if (!search_) 00052 search_.reset (new pcl::search::Octree<PointInT> (0.01)); 00053 00054 if (new_target_ && target_input_) 00055 { 00056 search_->setInputCloud (target_input_); 00057 new_target_ = false; 00058 } 00059 00060 return true; 00061 } 00062 00063 } 00064 } 00065 00066 #define PCL_INSTANTIATE_ApproxNearestPairPointCloudCoherence(T) template class PCL_EXPORTS pcl::tracking::ApproxNearestPairPointCloudCoherence<T>; 00067 00068 #endif