Point Cloud Library (PCL)
1.9.1
tracking
include
pcl
tracking
impl
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
Po
int
InT>
double
9
pcl::tracking::NormalCoherence<PointInT>::computeCoherence
(PointInT &source, PointInT &target)
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
common.h
pcl::getAngle3D
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
pcl::tracking::NormalCoherence::computeCoherence
double computeCoherence(PointInT &source, PointInT &target)
return the normal coherence between the two points.
Definition:
normal_coherence.hpp:9