Point Cloud Library (PCL)
1.7.0
|
00001 #ifndef PCL_TRACKING_IMPL_COHERENCE_H_ 00002 #define PCL_TRACKING_IMPL_COHERENCE_H_ 00003 00004 #include <pcl/console/print.h> 00005 #include <pcl/tracking/coherence.h> 00006 00007 namespace pcl 00008 { 00009 namespace tracking 00010 { 00011 00012 template <typename PointInT> double 00013 PointCoherence<PointInT>::compute (PointInT &source, PointInT &target) 00014 { 00015 return computeCoherence (source, target); 00016 } 00017 00018 template <typename PointInT> double 00019 PointCloudCoherence<PointInT>::calcPointCoherence (PointInT &source, PointInT &target) 00020 { 00021 double val = 0.0; 00022 for (size_t i = 0; i < point_coherences_.size (); i++) 00023 { 00024 PointCoherencePtr coherence = point_coherences_[i]; 00025 double d = log(coherence->compute (source, target)); 00026 //double d = coherence->compute (source, target); 00027 if (! pcl_isnan(d)) 00028 val += d; 00029 else 00030 PCL_WARN ("nan!\n"); 00031 } 00032 return val; 00033 } 00034 00035 template <typename PointInT> bool 00036 PointCloudCoherence<PointInT>::initCompute () 00037 { 00038 if (!target_input_ || target_input_->points.empty ()) 00039 { 00040 PCL_ERROR ("[pcl::%s::compute] target_input_ is empty!\n", getClassName ().c_str ()); 00041 return false; 00042 } 00043 00044 return true; 00045 00046 } 00047 00048 template <typename PointInT> void 00049 PointCloudCoherence<PointInT>::compute (const PointCloudInConstPtr &cloud, const IndicesConstPtr &indices, float &w) 00050 { 00051 if (!initCompute ()) 00052 { 00053 PCL_ERROR ("[pcl::%s::compute] Init failed.\n", getClassName ().c_str ()); 00054 return; 00055 } 00056 computeCoherence (cloud, indices, w); 00057 } 00058 } 00059 } 00060 00061 #endif