Point Cloud Library (PCL)  1.14.1-dev
distance_coherence.hpp
1 #ifndef PCL_TRACKING_IMPL_DISTANCE_COHERENCE_H_
2 #define PCL_TRACKING_IMPL_DISTANCE_COHERENCE_H_
3 
4 #include <pcl/tracking/distance_coherence.h>
5 
6 namespace pcl {
7 namespace tracking {
8 template <typename PointInT>
9 double
10 DistanceCoherence<PointInT>::computeCoherence(PointInT& source, PointInT& target)
11 {
12  Eigen::Vector4f p = source.getVector4fMap();
13  Eigen::Vector4f p_dash = target.getVector4fMap();
14  double d = (p - p_dash).norm();
15  return 1.0 / (1.0 + d * d * weight_);
16 }
17 } // namespace tracking
18 } // namespace pcl
19 
20 #define PCL_INSTANTIATE_DistanceCoherence(T) \
21  template class PCL_EXPORTS pcl::tracking::DistanceCoherence<T>;
22 
23 #endif
double computeCoherence(PointInT &source, PointInT &target) override
return the distance coherence between the two points.