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