Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
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
7namespace pcl {
8namespace tracking {
9template <typename PointInT>
10void
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
39template <typename PointInT>
40bool
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.
typename PointCloudCoherence< PointInT >::PointCloudInConstPtr PointCloudInConstPtr
typename PointCloudCoherence< PointInT >::PointCoherencePtr PointCoherencePtr
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
shared_ptr< const Indices > IndicesConstPtr
Definition pcl_base.h:59
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133