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