Point Cloud Library (PCL)  1.14.1-dev
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 namespace pcl {
9 namespace tracking {
10 template <typename PointInT>
11 double
12 NormalCoherence<PointInT>::computeCoherence(PointInT& source, PointInT& target)
13 {
14  Eigen::Vector4f n = source.getNormalVector4fMap();
15  Eigen::Vector4f n_dash = target.getNormalVector4fMap();
16  if (n.norm() <= 1e-5 || n_dash.norm() <= 1e-5) {
17  PCL_ERROR_STREAM("[NormalCoherence::computeCoherence] normal of source and/or "
18  "target is zero! source: "
19  << source << "target: " << target << std::endl);
20  return 0.0;
21  }
22  n.normalize();
23  n_dash.normalize();
24  double theta = pcl::getAngle3D(n, n_dash);
25  if (!std::isnan(theta))
26  return 1.0 / (1.0 + weight_ * theta * theta);
27  return 0.0;
28 }
29 } // namespace tracking
30 } // namespace pcl
31 
32 #define PCL_INSTANTIATE_NormalCoherence(T) \
33  template class PCL_EXPORTS pcl::tracking::NormalCoherence<T>;
34 
35 #endif
double computeCoherence(PointInT &source, PointInT &target) override
return the normal coherence between the two points.
Define standard C methods and C++ classes that are common to all methods.
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:47