Loading...
Searching...
No Matches
nearest_pair_point_cloud_coherence.hpp
#ifndef PCL_TRACKING_IMPL_NEAREST_PAIR_POINT_CLOUD_COHERENCE_H_
#define PCL_TRACKING_IMPL_NEAREST_PAIR_POINT_CLOUD_COHERENCE_H_
#include <pcl/search/kdtree.h>
#include <pcl/tracking/nearest_pair_point_cloud_coherence.h>
namespace pcl {
namespace tracking {
template <typename PointInT>
void
NearestPairPointCloudCoherence<PointInT>::computeCoherence(
const PointCloudInConstPtr& cloud, const IndicesConstPtr&, float& w)
{
double val = 0.0;
// for (std::size_t i = 0; i < indices->size (); i++)
for (std::size_t i = 0; i < cloud->size(); i++) {
PointInT input_point = (*cloud)[i];
pcl::Indices k_indices(1);
std::vector<float> k_distances(1);
search_->nearestKSearch(input_point, 1, k_indices, k_distances);
int k_index = k_indices[0];
float k_distance = k_distances[0];
if (k_distance < maximum_distance_ * maximum_distance_) {
// nearest_targets.push_back (k_index);
// nearest_inputs.push_back (i);
PointInT target_point = (*target_input_)[k_index];
double coherence_val = 1.0;
for (std::size_t i = 0; i < point_coherences_.size(); i++) {
PointCoherencePtr coherence = point_coherences_[i];
double w = coherence->compute(input_point, target_point);
coherence_val *= w;
}
val += coherence_val;
}
}
w = -static_cast<float>(val);
}
template <typename PointInT>
bool
NearestPairPointCloudCoherence<PointInT>::initCompute()
{
if (!PointCloudCoherence<PointInT>::initCompute()) {
PCL_ERROR("[pcl::%s::initCompute] PointCloudCoherence::Init failed.\n",
getClassName().c_str());
// deinitCompute ();
return (false);
}
// initialize tree
if (!search_)
search_.reset(new pcl::search::KdTree<PointInT>(false));
if (new_target_ && target_input_) {
search_->setInputCloud(target_input_);
new_target_ = false;
}
return true;
}
} // namespace tracking
} // namespace pcl
#define PCL_INSTANTIATE_NearestPairPointCloudCoherence(T) \
template class PCL_EXPORTS pcl::tracking::NearestPairPointCloudCoherence<T>;
#endif
pcl::search::KdTree
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition kdtree.h:62
pcl::tracking::NearestPairPointCloudCoherence::new_target_
bool new_target_
A flag which is true if target_input_ is updated.
Definition nearest_pair_point_cloud_coherence.h:85
pcl::tracking::NearestPairPointCloudCoherence::initCompute
bool initCompute() override
This method should get called before starting the actual computation.
Definition nearest_pair_point_cloud_coherence.hpp:41
pcl::tracking::NearestPairPointCloudCoherence::PointCoherencePtr
typename PointCloudCoherence< PointInT >::PointCoherencePtr PointCoherencePtr
Definition nearest_pair_point_cloud_coherence.h:21
pcl::tracking::NearestPairPointCloudCoherence::PointCloudInConstPtr
typename PointCloudCoherence< PointInT >::PointCloudInConstPtr PointCloudInConstPtr
Definition nearest_pair_point_cloud_coherence.h:22
pcl::tracking::NearestPairPointCloudCoherence::maximum_distance_
double maximum_distance_
max of distance for points to be taken into account
Definition nearest_pair_point_cloud_coherence.h:91
pcl::tracking::NearestPairPointCloudCoherence::computeCoherence
void computeCoherence(const PointCloudInConstPtr &cloud, const IndicesConstPtr &indices, float &w_j) override
compute the nearest pairs and compute coherence using point_coherences_
Definition nearest_pair_point_cloud_coherence.hpp:11
pcl::tracking::NearestPairPointCloudCoherence::search_
SearchPtr search_
A pointer to the spatial search object.
Definition nearest_pair_point_cloud_coherence.h:88
pcl::tracking::PointCloudCoherence::target_input_
PointCloudInConstPtr target_input_
a pointer to target point cloud
Definition coherence.h:141
pcl::tracking::PointCloudCoherence::getClassName
const std::string & getClassName() const
Get a string representation of the name of this class.
Definition coherence.h:132
pcl::tracking::PointCloudCoherence::initCompute
virtual bool initCompute()
This method should get called before starting the actual computation.
Definition coherence.hpp:36
pcl::tracking::PointCloudCoherence::point_coherences_
std::vector< PointCoherencePtr > point_coherences_
a list of pointers to PointCoherence.
Definition coherence.h:144
pcl::tracking
Definition approx_nearest_pair_point_cloud_coherence.h:7
pcl
Definition convolution.h:46
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
pcl::IndicesConstPtr
shared_ptr< const Indices > IndicesConstPtr
Definition pcl_base.h:59