Point Cloud Library (PCL)  1.9.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Modules Pages
normal_coherence.hpp
1 #ifndef PCL_TRACKING_IMPL_NORMAL_COHERENCE_H_
2 #define PCL_TRACKING_IMPL_NORMAL_COHERENCE_H_
3 
4 #include <pcl/common/common.h>
5 #include <pcl/console/print.h>
6 #include <pcl/tracking/normal_coherence.h>
7 
8 template <typename PointInT> double
10 {
11  Eigen::Vector4f n = source.getNormalVector4fMap ();
12  Eigen::Vector4f n_dash = target.getNormalVector4fMap ();
13  if ( n.norm () <= 1e-5 || n_dash.norm () <= 1e-5 )
14  {
15  PCL_ERROR("norm might be ZERO!\n");
16  std::cout << "source: " << source << std::endl;
17  std::cout << "target: " << target << std::endl;
18  exit (1);
19  return 0.0;
20  }
21  else
22  {
23  n.normalize ();
24  n_dash.normalize ();
25  double theta = pcl::getAngle3D (n, n_dash);
26  if (!pcl_isnan (theta))
27  return 1.0 / (1.0 + weight_ * theta * theta);
28  else
29  return 0.0;
30  }
31 }
32 
33 
34 #define PCL_INSTANTIATE_NormalCoherence(T) template class PCL_EXPORTS pcl::tracking::NormalCoherence<T>;
35 
36 #endif
double getAngle3D(const Eigen::Vector4f &v1, const Eigen::Vector4f &v2, const bool in_degree=false)
Compute the smallest angle between two 3D vectors in radians (default) or degree. ...
Definition: common.hpp:46
double computeCoherence(PointInT &source, PointInT &target)
return the normal coherence between the two points.