38#ifndef PCL_SURFACE_IMPL_SURFEL_SMOOTHING_H_
39#define PCL_SURFACE_IMPL_SURFEL_SMOOTHING_H_
41#include <pcl/search/organized.h>
42#include <pcl/search/kdtree.h>
43#include <pcl/surface/surfel_smoothing.h>
45#include <pcl/console/print.h>
48template <
typename Po
intT,
typename Po
intNT>
bool
56 PCL_ERROR (
"SurfelSmoothing: normal cloud not set\n");
60 if (input_->size () != normals_->size ())
62 PCL_ERROR (
"SurfelSmoothing: number of input points different from the number of given normals\n");
69 if (input_->isOrganized ())
84template <
typename Po
intT,
typename Po
intNT>
float
98 std::vector<float>
diffs (interm_cloud_->size ());
101 for (std::size_t i = 0; i < interm_cloud_->size (); ++i)
115 float theta_i = std::exp ( (-1) *
dist / scale_squared_);
151 (*output_normals)[i].getNormalVector4fMap () = (*normals_)[i].getNormalVector4fMap ();
160template <
typename Po
intT,
typename Po
intNT>
void
193 float theta_i = std::exp ( (-1) *
dist / scale_squared_);
241 PCL_DEBUG (
"[pcl::SurfelSmoothing::smoothPoint] Passed the number of BIG iterations: %d\n",
big_iterations);
246template <
typename Po
intT,
typename Po
intNT>
void
252 PCL_ERROR (
"[pcl::SurfelSmoothing::computeSmoothedCloud]: SurfelSmoothing not initialized properly, skipping computeSmoothedCloud ().\n");
256 tree_->setInputCloud (input_);
268 for (std::size_t i = 0; i < input_->size (); ++i)
275template <
typename Po
intT,
typename Po
intNT>
void
280 if (interm_cloud_->size () !=
cloud2->
size () ||
283 PCL_ERROR (
"[pcl::SurfelSmoothing::extractSalientFeaturesBetweenScales]: Number of points in the clouds does not match.\n");
288 for (std::size_t i = 0; i <
cloud2->
size (); ++i)
318#define PCL_INSTANTIATE_SurfelSmoothing(PointT,PointNT) template class PCL_EXPORTS pcl::SurfelSmoothing<PointT, PointNT>;
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
typename pcl::PointCloud< PointNT >::Ptr NormalCloudPtr
void extractSalientFeaturesBetweenScales(PointCloudInPtr &cloud2, NormalCloudPtr &cloud2_normals, pcl::IndicesPtr &output_features)
float smoothCloudIteration(PointCloudInPtr &output_positions, NormalCloudPtr &output_normals)
void smoothPoint(std::size_t &point_index, PointT &output_point, PointNT &output_normal)
void computeSmoothedCloud(PointCloudInPtr &output_positions, NormalCloudPtr &output_normals)
typename pcl::PointCloud< PointT >::Ptr PointCloudInPtr
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds.
Define standard C methods to do distance calculations.
float squaredEuclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the squared euclidean distance between the two given points.
shared_ptr< Indices > IndicesPtr
A point structure representing Euclidean xyz coordinates, and the RGB color.