41 #ifndef PCL_FEATURES_IMPL_CVFH_H_ 42 #define PCL_FEATURES_IMPL_CVFH_H_ 44 #include <pcl/features/cvfh.h> 45 #include <pcl/features/normal_3d.h> 46 #include <pcl/features/pfh_tools.h> 50 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void 67 computeFeature (output);
73 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void 79 std::vector<pcl::PointIndices> &clusters,
81 unsigned int min_pts_per_cluster,
82 unsigned int max_pts_per_cluster)
86 PCL_ERROR (
"[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->
getInputCloud ()->points.size (), cloud.
points.size ());
91 PCL_ERROR (
"[pcl::extractEuclideanClusters] Number of points in the input point cloud (%lu) different than normals (%lu)!\n", cloud.
points.size (), normals.
points.size ());
96 std::vector<bool> processed (cloud.
points.size (),
false);
98 std::vector<int> nn_indices;
99 std::vector<float> nn_distances;
101 for (
int i = 0; i < static_cast<int> (cloud.
points.size ()); ++i)
106 std::vector<unsigned int> seed_queue;
108 seed_queue.push_back (i);
112 while (sq_idx < static_cast<int> (seed_queue.size ()))
115 if (!tree->
radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
121 for (
size_t j = 1; j < nn_indices.size (); ++j)
123 if (processed[nn_indices[j]])
129 double dot_p = normals.
points[seed_queue[sq_idx]].normal[0] * normals.
points[nn_indices[j]].normal[0]
130 + normals.
points[seed_queue[sq_idx]].normal[1] * normals.
points[nn_indices[j]].normal[1]
131 + normals.
points[seed_queue[sq_idx]].normal[2] * normals.
points[nn_indices[j]].normal[2];
133 if (fabs (acos (dot_p)) < eps_angle)
135 processed[nn_indices[j]] =
true;
136 seed_queue.push_back (nn_indices[j]);
144 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
147 r.
indices.resize (seed_queue.size ());
148 for (
size_t j = 0; j < seed_queue.size (); ++j)
155 clusters.push_back (r);
161 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void 164 std::vector<int> &indices_to_use,
165 std::vector<int> &indices_out,
166 std::vector<int> &indices_in,
169 indices_out.resize (cloud.
points.size ());
170 indices_in.resize (cloud.
points.size ());
175 for (
int i = 0; i < static_cast<int> (indices_to_use.size ()); i++)
177 if (cloud.
points[indices_to_use[i]].curvature > threshold)
179 indices_out[out] = indices_to_use[i];
184 indices_in[in] = indices_to_use[i];
189 indices_out.resize (out);
190 indices_in.resize (in);
194 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void 200 PCL_ERROR (
"[pcl::%s::computeFeature] No input dataset containing normals was given!\n", getClassName ().c_str ());
205 if (normals_->points.size () != surface_->points.size ())
207 PCL_ERROR (
"[pcl::%s::computeFeature] The number of points in the input dataset differs from the number of points in the dataset containing the normals!\n", getClassName ().c_str ());
213 centroids_dominant_orientations_.clear ();
216 std::vector<int> indices_out;
217 std::vector<int> indices_in;
218 filterNormalsWithHighCurvature (*normals_, *indices_, indices_out, indices_in, curv_threshold_);
221 normals_filtered_cloud->
width =
static_cast<uint32_t
> (indices_in.size ());
222 normals_filtered_cloud->
height = 1;
223 normals_filtered_cloud->
points.resize (normals_filtered_cloud->
width);
225 for (
size_t i = 0; i < indices_in.size (); ++i)
227 normals_filtered_cloud->
points[i].x = surface_->points[indices_in[i]].x;
228 normals_filtered_cloud->
points[i].y = surface_->points[indices_in[i]].y;
229 normals_filtered_cloud->
points[i].z = surface_->points[indices_in[i]].z;
232 std::vector<pcl::PointIndices> clusters;
234 if(normals_filtered_cloud->
points.size() >= min_points_)
238 normals_tree_filtered->setInputCloud (normals_filtered_cloud);
245 n3d.
compute (*normals_filtered_cloud);
248 normals_tree->setInputCloud (normals_filtered_cloud);
250 extractEuclideanClustersSmooth (*normals_filtered_cloud,
251 *normals_filtered_cloud,
255 eps_angle_threshold_,
256 static_cast<unsigned int> (min_points_));
273 if (clusters.size () > 0)
276 for (
size_t i = 0; i < clusters.size (); ++i)
278 Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
279 Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
281 for (
size_t j = 0; j < clusters[i].indices.size (); j++)
283 avg_normal += normals_filtered_cloud->
points[clusters[i].indices[j]].getNormalVector4fMap ();
284 avg_centroid += normals_filtered_cloud->
points[clusters[i].indices[j]].getVector4fMap ();
287 avg_normal /=
static_cast<float> (clusters[i].indices.size ());
288 avg_centroid /=
static_cast<float> (clusters[i].indices.size ());
290 Eigen::Vector4f centroid_test;
292 avg_normal.normalize ();
294 Eigen::Vector3f avg_norm (avg_normal[0], avg_normal[1], avg_normal[2]);
295 Eigen::Vector3f avg_dominant_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
298 dominant_normals_.push_back (avg_norm);
299 centroids_dominant_orientations_.push_back (avg_dominant_centroid);
303 output.
points.resize (dominant_normals_.size ());
304 output.
width =
static_cast<uint32_t
> (dominant_normals_.size ());
306 for (
size_t i = 0; i < dominant_normals_.size (); ++i)
318 Eigen::Vector4f avg_centroid;
320 Eigen::Vector3f cloud_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
321 centroids_dominant_orientations_.push_back (cloud_centroid);
337 #define PCL_INSTANTIATE_CVFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::CVFHEstimation<T,NT,OutT>; 339 #endif // PCL_FEATURES_IMPL_VFH_H_ void setUseGivenCentroid(bool use)
Set use_given_centroid_.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
CVFHEstimation estimates the Clustered Viewpoint Feature Histogram (CVFH) descriptor for a given poin...
virtual int radiusSearch(const PointT &point, double radius, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const =0
Search for all the nearest neighbors of the query point in a given radius.
void filterNormalsWithHighCurvature(const pcl::PointCloud< PointNT > &cloud, std::vector< int > &indices_to_use, std::vector< int > &indices_out, std::vector< int > &indices_in, float threshold)
Removes normals with high curvature caused by real edges or noisy data.
std::vector< int > indices
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point...
void setUseGivenNormal(bool use)
Set use_given_normal_.
uint32_t height
The point cloud height (if organized as an image-structure).
boost::shared_ptr< PointCloud< PointT > > Ptr
uint32_t width
The point cloud width (if organized as an image-structure).
void setNormalToUse(const Eigen::Vector3f &normal)
Set the normal to use.
void setCentroidToUse(const Eigen::Vector3f ¢roid)
Set centroid_to_use_.
boost::shared_ptr< pcl::search::Search< PointT > > Ptr
VFHEstimation estimates the Viewpoint Feature Histogram (VFH) descriptor for a given point cloud data...
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
void setNormalizeBins(bool normalize)
set normalize_bins_
PointCloud represents the base class in PCL for storing collections of 3D points. ...
pcl::PCLHeader header
The point cloud header.
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
void compute(PointCloudOut &output)
Overloaded computed method from pcl::Feature.
void setSearchMethod(const KdTreePtr &tree)
Provide a pointer to the search object.
virtual PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
pcl::search::Search< PointNormal >::Ptr KdTreePtr
void compute(PointCloudOut &output)
Overloaded computed method from pcl::Feature.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Feature represents the base feature class.
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
unsigned int compute3DCentroid(ConstCloudIterator< PointT > &cloud_iterator, Eigen::Matrix< Scalar, 4, 1 > ¢roid)
Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset...
void setFillSizeComponent(bool fill_size)
set size_component_
Define methods for centroid estimation and covariance matrix calculus.
void setNormalizeDistance(bool normalize)
set normalize_distances_