Point Cloud Library (PCL)  1.14.1-dev
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/tracking/nearest_pair_point_cloud_coherence.h>
6 
7 namespace pcl {
8 namespace tracking {
9 template <typename PointInT>
10 void
12  const PointCloudInConstPtr& cloud, const IndicesConstPtr&, float& w)
13 {
14  double val = 0.0;
15  // for (std::size_t i = 0; i < indices->size (); i++)
16  for (std::size_t i = 0; i < cloud->size(); i++) {
17  PointInT input_point = (*cloud)[i];
18  pcl::Indices k_indices(1);
19  std::vector<float> k_distances(1);
20  search_->nearestKSearch(input_point, 1, k_indices, k_distances);
21  int k_index = k_indices[0];
22  float k_distance = k_distances[0];
23  if (k_distance < maximum_distance_ * maximum_distance_) {
24  // nearest_targets.push_back (k_index);
25  // nearest_inputs.push_back (i);
26  PointInT target_point = (*target_input_)[k_index];
27  double coherence_val = 1.0;
28  for (std::size_t i = 0; i < point_coherences_.size(); i++) {
29  PointCoherencePtr coherence = point_coherences_[i];
30  double w = coherence->compute(input_point, target_point);
31  coherence_val *= w;
32  }
33  val += coherence_val;
34  }
35  }
36  w = -static_cast<float>(val);
37 }
38 
39 template <typename PointInT>
40 bool
42 {
44  PCL_ERROR("[pcl::%s::initCompute] PointCloudCoherence::Init failed.\n",
45  getClassName().c_str());
46  // deinitCompute ();
47  return (false);
48  }
49 
50  // initialize tree
51  if (!search_)
52  search_.reset(new pcl::search::KdTree<PointInT>(false));
53 
54  if (new_target_ && target_input_) {
55  search_->setInputCloud(target_input_);
56  new_target_ = false;
57  }
58 
59  return true;
60 }
61 } // namespace tracking
62 } // namespace pcl
63 
64 #define PCL_INSTANTIATE_NearestPairPointCloudCoherence(T) \
65  template class PCL_EXPORTS pcl::tracking::NearestPairPointCloudCoherence<T>;
66 
67 #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:59
typename PointCloudIn::ConstPtr PointCloudInConstPtr
Definition: coherence.h:66
typename PointCoherence< PointInT >::Ptr PointCoherencePtr
Definition: coherence.h:68
shared_ptr< const Indices > IndicesConstPtr
Definition: pcl_base.h:59
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133