Point Cloud Library (PCL)  1.11.0
nearest_pair_point_cloud_coherence.hpp
1 #ifndef PCL_TRACKING_IMPL_NEAREST_PAIR_POINT_CLOUD_COHERENCE_H_
2 #define PCL_TRACKING_IMPL_NEAREST_PAIR_POINT_CLOUD_COHERENCE_H_
3 
4 #include <pcl/search/kdtree.h>
5 #include <pcl/search/organized.h>
6 #include <pcl/tracking/nearest_pair_point_cloud_coherence.h>
7 
8 namespace pcl
9 {
10  namespace tracking
11  {
12  template <typename PointInT> void
14  const PointCloudInConstPtr &cloud, const IndicesConstPtr &, float &w)
15  {
16  double val = 0.0;
17  //for (std::size_t i = 0; i < indices->size (); i++)
18  for (std::size_t i = 0; i < cloud->points.size (); i++)
19  {
20  PointInT input_point = cloud->points[i];
21  std::vector<int> k_indices(1);
22  std::vector<float> k_distances(1);
23  search_->nearestKSearch (input_point, 1, k_indices, k_distances);
24  int k_index = k_indices[0];
25  float k_distance = k_distances[0];
26  if (k_distance < maximum_distance_ * maximum_distance_)
27  {
28  // nearest_targets.push_back (k_index);
29  // nearest_inputs.push_back (i);
30  PointInT target_point = target_input_->points[k_index];
31  double coherence_val = 1.0;
32  for (std::size_t i = 0; i < point_coherences_.size (); i++)
33  {
34  PointCoherencePtr coherence = point_coherences_[i];
35  double w = coherence->compute (input_point, target_point);
36  coherence_val *= w;
37  }
38  val += coherence_val;
39  }
40  }
41  w = - static_cast<float> (val);
42  }
43 
44  template <typename PointInT> bool
46  {
48  {
49  PCL_ERROR ("[pcl::%s::initCompute] PointCloudCoherence::Init failed.\n", getClassName ().c_str ());
50  //deinitCompute ();
51  return (false);
52  }
53 
54  // initialize tree
55  if (!search_)
56  search_.reset (new pcl::search::KdTree<PointInT> (false));
57 
58  if (new_target_ && target_input_)
59  {
60  search_->setInputCloud (target_input_);
61  new_target_ = false;
62  }
63 
64  return true;
65  }
66  }
67 }
68 
69 #define PCL_INSTANTIATE_NearestPairPointCloudCoherence(T) template class PCL_EXPORTS pcl::tracking::NearestPairPointCloudCoherence<T>;
70 
71 #endif
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:62
bool initCompute() override
This method should get called before starting the actual computation.
void computeCoherence(const PointCloudInConstPtr &cloud, const IndicesConstPtr &indices, float &w_j) override
compute the nearest pairs and compute coherence using point_coherences_
PointCloudCoherence is a base class to compute coherence between the two PointClouds.
Definition: coherence.h:60
typename PointCloudIn::ConstPtr PointCloudInConstPtr
Definition: coherence.h:67
typename PointCoherence< PointInT >::Ptr PointCoherencePtr
Definition: coherence.h:69
shared_ptr< const Indices > IndicesConstPtr
Definition: pcl_base.h:62