Point Cloud Library (PCL) 1.12.1
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
8namespace pcl {
9namespace tracking {
10template <typename PointInT>
11double
12NormalCoherence<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("norm might be ZERO!\n");
18 std::cout << "source: " << source << std::endl;
19 std::cout << "target: " << target << std::endl;
20 exit(1);
21 return 0.0;
22 }
23 n.normalize();
24 n_dash.normalize();
25 double theta = pcl::getAngle3D(n, n_dash);
26 if (!std::isnan(theta))
27 return 1.0 / (1.0 + weight_ * theta * theta);
28 return 0.0;
29}
30} // namespace tracking
31} // namespace pcl
32
33#define PCL_INSTANTIATE_NormalCoherence(T) \
34 template class PCL_EXPORTS pcl::tracking::NormalCoherence<T>;
35
36#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