Point Cloud Library (PCL) 1.13.0
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
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 (const auto& point : *cloud) {
17 pcl::index_t k_index = 0;
18 float k_distance = 0.0;
19 // PointInT input_point = cloud->points[(*indices)[i]];
20 PointInT input_point = point;
21 search_->approxNearestSearch(input_point, k_index, k_distance);
22 if (k_distance < maximum_distance_ * maximum_distance_) {
23 PointInT target_point = (*target_input_)[k_index];
24 double coherence_val = 1.0;
25 for (std::size_t i = 0; i < point_coherences_.size(); i++) {
26 PointCoherencePtr coherence = point_coherences_[i];
27 double w = coherence->compute(input_point, target_point);
28 coherence_val *= w;
29 }
30 val += coherence_val;
31 }
32 }
33 w = -static_cast<float>(val);
34}
35
36template <typename PointInT>
37bool
39{
41 PCL_ERROR("[pcl::%s::initCompute] PointCloudCoherence::Init failed.\n",
42 getClassName().c_str());
43 // deinitCompute ();
44 return (false);
45 }
46
47 // initialize tree
48 if (!search_)
49 search_.reset(new pcl::search::Octree<PointInT>(0.01));
50
51 if (new_target_ && target_input_) {
52 search_->setInputCloud(target_input_);
53 new_target_ = false;
54 }
55
56 return true;
57}
58
59} // namespace tracking
60} // namespace pcl
61
62#define PCL_INSTANTIATE_ApproxNearestPairPointCloudCoherence(T) \
63 template class PCL_EXPORTS pcl::tracking::ApproxNearestPairPointCloudCoherence<T>;
64
65#endif
bool initCompute() override
This method should get called before starting the actual computation.
typename NearestPairPointCloudCoherence< PointInT >::PointCloudInConstPtr PointCloudInConstPtr
void computeCoherence(const PointCloudInConstPtr &cloud, const IndicesConstPtr &indices, float &w_j) override
compute the nearest pairs and compute coherence using point_coherences_
typename NearestPairPointCloudCoherence< PointInT >::PointCoherencePtr PointCoherencePtr
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
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
Definition: types.h:112