Point Cloud Library (PCL)  1.7.1
approx_nearest_pair_point_cloud_coherence.hpp
1 #ifndef PCL_TRACKING_IMPL_APPROX_NEAREST_PAIR_POINT_CLOUD_COHERENCE_H_
2 #define PCL_TRACKING_IMPL_APPROX_NEAREST_PAIR_POINT_CLOUD_COHERENCE_H_
3 
4 #include <pcl/search/octree.h>
5 #include <pcl/tracking/approx_nearest_pair_point_cloud_coherence.h>
6 
7 namespace pcl
8 {
9  namespace tracking
10  {
11  template <typename PointInT> void
13  const PointCloudInConstPtr &cloud, const IndicesConstPtr &, float &w)
14  {
15  double val = 0.0;
16  //for (size_t i = 0; i < indices->size (); i++)
17  for (size_t i = 0; i < cloud->points.size (); i++)
18  {
19  int k_index = 0;
20  float k_distance = 0.0;
21  //PointInT input_point = cloud->points[(*indices)[i]];
22  PointInT input_point = cloud->points[i];
23  search_->approxNearestSearch(input_point, k_index, k_distance);
24  if (k_distance < maximum_distance_ * maximum_distance_)
25  {
26  PointInT target_point = target_input_->points[k_index];
27  double coherence_val = 1.0;
28  for (size_t i = 0; i < point_coherences_.size (); i++)
29  {
30  PointCoherencePtr coherence = point_coherences_[i];
31  double w = coherence->compute (input_point, target_point);
32  coherence_val *= w;
33  }
34  val += coherence_val;
35  }
36  }
37  w = - static_cast<float> (val);
38  }
39 
40  template <typename PointInT> bool
42  {
44  {
45  PCL_ERROR ("[pcl::%s::initCompute] PointCloudCoherence::Init failed.\n", getClassName ().c_str ());
46  //deinitCompute ();
47  return (false);
48  }
49 
50  // initialize tree
51  if (!search_)
52  search_.reset (new pcl::search::Octree<PointInT> (0.01));
53 
54  if (new_target_ && target_input_)
55  {
56  search_->setInputCloud (target_input_);
57  new_target_ = false;
58  }
59 
60  return true;
61  }
62 
63  }
64 }
65 
66 #define PCL_INSTANTIATE_ApproxNearestPairPointCloudCoherence(T) template class PCL_EXPORTS pcl::tracking::ApproxNearestPairPointCloudCoherence<T>;
67 
68 #endif