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