normal_coherence.hpp
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 #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


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:25:51