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 ());
201 output.width = output.height = 0;
202 output.points.clear ();
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 ());
208 output.width = output.height = 0;
209 output.points.clear ();
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_));
261 vfh.setInputCloud (surface_);
262 vfh.setInputNormals (normals_);
263 vfh.setIndices(indices_);
264 vfh.setSearchMethod (this->tree_);
265 vfh.setUseGivenNormal (
true);
266 vfh.setUseGivenCentroid (
true);
267 vfh.setNormalizeBins (normalize_bins_);
268 vfh.setNormalizeDistance (
true);
269 vfh.setFillSizeComponent (
true);
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)
309 vfh.setNormalToUse (dominant_normals_[i]);
310 vfh.setCentroidToUse (centroids_dominant_orientations_[i]);
312 vfh.compute (vfh_signature);
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);
324 vfh.setCentroidToUse (cloud_centroid);
325 vfh.setUseGivenNormal (
false);
328 vfh.compute (vfh_signature);
333 output.points[0] = vfh_signature.
points[0];
337 #define PCL_INSTANTIATE_CVFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::CVFHEstimation<T,NT,OutT>;
339 #endif // PCL_FEATURES_IMPL_VFH_H_