Point Cloud Library (PCL)
1.7.0
|
00001 #ifndef PCL_TRACKING_IMPL_NORMAL_COHERENCE_H_ 00002 #define PCL_TRACKING_IMPL_NORMAL_COHERENCE_H_ 00003 00004 #include <pcl/common/common.h> 00005 #include <pcl/console/print.h> 00006 #include <pcl/tracking/normal_coherence.h> 00007 00008 template <typename PointInT> double 00009 pcl::tracking::NormalCoherence<PointInT>::computeCoherence (PointInT &source, PointInT &target) 00010 { 00011 Eigen::Vector4f n = source.getNormalVector4fMap (); 00012 Eigen::Vector4f n_dash = target.getNormalVector4fMap (); 00013 if ( n.norm () <= 1e-5 || n_dash.norm () <= 1e-5 ) 00014 { 00015 PCL_ERROR("norm might be ZERO!\n"); 00016 std::cout << "source: " << source << std::endl; 00017 std::cout << "target: " << target << std::endl; 00018 exit (1); 00019 return 0.0; 00020 } 00021 else 00022 { 00023 n.normalize (); 00024 n_dash.normalize (); 00025 double theta = pcl::getAngle3D (n, n_dash); 00026 if (!pcl_isnan (theta)) 00027 return 1.0 / (1.0 + weight_ * theta * theta); 00028 else 00029 return 0.0; 00030 } 00031 } 00032 00033 00034 #define PCL_INSTANTIATE_NormalCoherence(T) template class PCL_EXPORTS pcl::tracking::NormalCoherence<T>; 00035 00036 #endif