Point Cloud Library (PCL)
1.11.1-dev
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
namespace
pcl
{
9
namespace
tracking {
10
template
<
typename
Po
int
InT>
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(
"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
pcl
Definition:
convolution.h:46
common.h
pcl::tracking::NormalCoherence::computeCoherence
double computeCoherence(PointInT &source, PointInT &target) override
return the normal coherence between the two points.
Definition:
normal_coherence.hpp:12
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:47