41 #ifndef PCL_FEATURES_IMPL_OURCVFH_H_ 42 #define PCL_FEATURES_IMPL_OURCVFH_H_ 44 #include <pcl/features/our_cvfh.h> 45 #include <pcl/features/vfh.h> 46 #include <pcl/features/normal_3d.h> 47 #include <pcl/common/transforms.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 78 std::vector<pcl::PointIndices> &clusters,
double eps_angle,
79 unsigned int min_pts_per_cluster,
80 unsigned int max_pts_per_cluster)
84 PCL_ERROR(
"[pcl::extractEuclideanClusters] Tree built for a different point cloud " 85 "dataset (%zu) than the input cloud (%zu)!\n",
87 static_cast<std::size_t>(cloud.
size()));
90 if (cloud.
size () != normals.
size ())
92 PCL_ERROR(
"[pcl::extractEuclideanClusters] Number of points in the input point " 93 "cloud (%zu) different than normals (%zu)!\n",
94 static_cast<std::size_t>(cloud.
size()),
95 static_cast<std::size_t>(normals.
size()));
100 std::vector<bool> processed (cloud.
size (),
false);
102 std::vector<int> nn_indices;
103 std::vector<float> nn_distances;
105 for (std::size_t i = 0; i < cloud.
size (); ++i)
110 std::vector<std::size_t> seed_queue;
111 std::size_t sq_idx = 0;
112 seed_queue.push_back (i);
116 while (sq_idx < seed_queue.size ())
119 if (!tree->
radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
125 for (std::size_t j = 1; j < nn_indices.size (); ++j)
127 if (processed[nn_indices[j]])
133 double dot_p = normals[seed_queue[sq_idx]].normal[0] * normals[nn_indices[j]].normal[0]
134 + normals[seed_queue[sq_idx]].normal[1] * normals[nn_indices[j]].normal[1] + normals[seed_queue[sq_idx]].normal[2]
135 * normals[nn_indices[j]].normal[2];
137 if (std::abs (std::acos (dot_p)) < eps_angle)
139 processed[nn_indices[j]] =
true;
148 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
151 r.
indices.resize (seed_queue.size ());
152 for (std::size_t j = 0; j < seed_queue.size (); ++j)
159 clusters.push_back (r);
165 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void 167 std::vector<int> &indices_to_use,
168 std::vector<int> &indices_out, std::vector<int> &indices_in,
171 indices_out.resize (cloud.
size ());
172 indices_in.resize (cloud.
size ());
177 for (
const int &index : indices_to_use)
179 if (cloud[index].curvature > threshold)
181 indices_out[out] = index;
186 indices_in[in] = index;
191 indices_out.resize (out);
192 indices_in.resize (in);
195 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
bool 197 PointInTPtr & processed, std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > & transformations,
201 Eigen::Vector3f plane_normal;
202 plane_normal[0] = -centroid[0];
203 plane_normal[1] = -centroid[1];
204 plane_normal[2] = -centroid[2];
205 Eigen::Vector3f z_vector = Eigen::Vector3f::UnitZ ();
206 plane_normal.normalize ();
207 Eigen::Vector3f axis = plane_normal.cross (z_vector);
208 double rotation = -asin (axis.norm ());
211 Eigen::Affine3f transformPC (Eigen::AngleAxisf (static_cast<float> (rotation), axis));
213 grid->resize (processed->size ());
214 for (std::size_t k = 0; k < processed->size (); k++)
215 (*grid)[k].getVector4fMap () = (*processed)[k].getVector4fMap ();
219 Eigen::Vector4f centroid4f (centroid[0], centroid[1], centroid[2], 0);
220 Eigen::Vector4f normal_centroid4f (normal_centroid[0], normal_centroid[1], normal_centroid[2], 0);
222 centroid4f = transformPC * centroid4f;
223 normal_centroid4f = transformPC * normal_centroid4f;
225 Eigen::Vector3f centroid3f (centroid4f[0], centroid4f[1], centroid4f[2]);
227 Eigen::Vector4f farthest_away;
229 farthest_away[3] = 0;
231 float max_dist = (farthest_away - centroid4f).norm ();
235 Eigen::Matrix4f center_mat;
236 center_mat.setIdentity (4, 4);
237 center_mat (0, 3) = -centroid4f[0];
238 center_mat (1, 3) = -centroid4f[1];
239 center_mat (2, 3) = -centroid4f[2];
241 Eigen::Matrix3f scatter;
245 for (
const int &index : indices.
indices)
247 Eigen::Vector3f pvector = (*grid)[index].getVector3fMap ();
248 float d_k = (pvector).norm ();
249 float w = (max_dist - d_k);
250 Eigen::Vector3f diff = (pvector);
251 Eigen::Matrix3f mat = diff * diff.transpose ();
258 Eigen::JacobiSVD <Eigen::MatrixXf> svd (scatter, Eigen::ComputeFullV);
259 Eigen::Vector3f evx = svd.matrixV ().col (0);
260 Eigen::Vector3f evy = svd.matrixV ().col (1);
261 Eigen::Vector3f evz = svd.matrixV ().col (2);
262 Eigen::Vector3f evxminus = evx * -1;
263 Eigen::Vector3f evyminus = evy * -1;
264 Eigen::Vector3f evzminus = evz * -1;
266 float s_xplus, s_xminus, s_yplus, s_yminus;
267 s_xplus = s_xminus = s_yplus = s_yminus = 0.f;
270 for (
const auto& point: grid->points)
272 Eigen::Vector3f pvector = point.getVector3fMap ();
273 float dist_x, dist_y;
274 dist_x = std::abs (evx.dot (pvector));
275 dist_y = std::abs (evy.dot (pvector));
277 if ((pvector).dot (evx) >= 0)
282 if ((pvector).dot (evy) >= 0)
289 if (s_xplus < s_xminus)
292 if (s_yplus < s_yminus)
297 float max_x =
static_cast<float> (std::max (s_xplus, s_xminus));
298 float min_x =
static_cast<float> (std::min (s_xplus, s_xminus));
299 float max_y =
static_cast<float> (std::max (s_yplus, s_yminus));
300 float min_y =
static_cast<float> (std::min (s_yplus, s_yminus));
302 fx = (min_x / max_x);
303 fy = (min_y / max_y);
305 Eigen::Vector3f normal3f = Eigen::Vector3f (normal_centroid4f[0], normal_centroid4f[1], normal_centroid4f[2]);
306 if (normal3f.dot (evz) < 0)
312 float max_axis = std::max (fx, fy);
313 float min_axis = std::min (fx, fy);
315 if ((min_axis / max_axis) > axis_ratio_)
317 PCL_WARN (
"Both axes are equally easy/difficult to disambiguate\n");
319 Eigen::Vector3f evy_copy = evy;
320 Eigen::Vector3f evxminus = evx * -1;
321 Eigen::Vector3f evyminus = evy * -1;
323 if (min_axis > min_axis_value_)
326 evy = evx.cross (evz);
327 Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
328 transformations.push_back (trans);
331 evy = evx.cross (evz);
332 trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
333 transformations.push_back (trans);
336 evy = evx.cross (evz);
337 trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
338 transformations.push_back (trans);
341 evy = evx.cross (evz);
342 trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
343 transformations.push_back (trans);
349 evy = evx.cross (evz);
350 Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
351 transformations.push_back (trans);
355 evy = evx.cross (evz);
356 trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
357 transformations.push_back (trans);
368 evy = evx.cross (evz);
369 Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
370 transformations.push_back (trans);
378 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void 380 std::vector<pcl::PointIndices> & cluster_indices)
384 cluster_axes_.
clear ();
385 cluster_axes_.resize (centroids_dominant_orientations_.size ());
387 for (std::size_t i = 0; i < centroids_dominant_orientations_.size (); i++)
390 std::vector < Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > transformations;
392 sgurf (centroids_dominant_orientations_[i], dominant_normals_[i], processed, transformations, grid, cluster_indices[i]);
395 cluster_axes_[i] = transformations.size ();
397 for (
const auto &transformation : transformations)
401 transforms_.push_back (transformation);
402 valid_transforms_.push_back (
true);
404 std::vector < Eigen::VectorXf > quadrants (8);
407 for (
int k = 0; k < num_hists; k++)
408 quadrants[k].setZero (size_hists);
410 Eigen::Vector4f centroid_p;
411 centroid_p.setZero ();
412 Eigen::Vector4f max_pt;
415 double distance_normalization_factor = (centroid_p - max_pt).norm ();
419 hist_incr = 100.0f /
static_cast<float> (grid->size () - 1);
423 float * weights =
new float[num_hists];
425 float sigma_sq = sigma * sigma;
427 for (
const auto& point: grid->points)
429 Eigen::Vector4f p = point.getVector4fMap ();
434 float wx = 1.f - std::exp (-((p[0] * p[0]) / (2.f * sigma_sq)));
435 float wy = 1.f - std::exp (-((p[1] * p[1]) / (2.f * sigma_sq)));
436 float wz = 1.f - std::exp (-((p[2] * p[2]) / (2.f * sigma_sq)));
441 for (std::size_t ii = 0; ii <= 3; ii++)
442 weights[ii] = 0.5f - wx * 0.5f;
444 for (std::size_t ii = 4; ii <= 7; ii++)
445 weights[ii] = 0.5f + wx * 0.5f;
449 for (std::size_t ii = 0; ii <= 3; ii++)
450 weights[ii] = 0.5f + wx * 0.5f;
452 for (std::size_t ii = 4; ii <= 7; ii++)
453 weights[ii] = 0.5f - wx * 0.5f;
459 for (std::size_t ii = 0; ii <= 1; ii++)
460 weights[ii] *= 0.5f - wy * 0.5f;
461 for (std::size_t ii = 4; ii <= 5; ii++)
462 weights[ii] *= 0.5f - wy * 0.5f;
464 for (std::size_t ii = 2; ii <= 3; ii++)
465 weights[ii] *= 0.5f + wy * 0.5f;
467 for (std::size_t ii = 6; ii <= 7; ii++)
468 weights[ii] *= 0.5f + wy * 0.5f;
472 for (std::size_t ii = 0; ii <= 1; ii++)
473 weights[ii] *= 0.5f + wy * 0.5f;
474 for (std::size_t ii = 4; ii <= 5; ii++)
475 weights[ii] *= 0.5f + wy * 0.5f;
477 for (std::size_t ii = 2; ii <= 3; ii++)
478 weights[ii] *= 0.5f - wy * 0.5f;
480 for (std::size_t ii = 6; ii <= 7; ii++)
481 weights[ii] *= 0.5f - wy * 0.5f;
487 for (std::size_t ii = 0; ii <= 7; ii += 2)
488 weights[ii] *= 0.5f - wz * 0.5f;
490 for (std::size_t ii = 1; ii <= 7; ii += 2)
491 weights[ii] *= 0.5f + wz * 0.5f;
496 for (std::size_t ii = 0; ii <= 7; ii += 2)
497 weights[ii] *= 0.5f + wz * 0.5f;
499 for (std::size_t ii = 1; ii <= 7; ii += 2)
500 weights[ii] *= 0.5f - wz * 0.5f;
503 int h_index = (d <= 0) ? 0 : std::ceil (size_hists * (d / distance_normalization_factor)) - 1;
511 for (
int j = 0; j < num_hists; j++)
512 quadrants[j][h_index] += hist_incr * weights[j];
518 vfh_signature.
points.resize (1);
520 for (
int d = 0; d < 308; ++d)
521 vfh_signature[0].histogram[d] = output[i].histogram[d];
524 for (
int k = 0; k < num_hists; k++)
526 for (
int ii = 0; ii < size_hists; ii++, pos++)
528 vfh_signature[0].histogram[pos] = quadrants[k][ii];
532 ourcvfh_output.
push_back (vfh_signature[0]);
533 ourcvfh_output.
width = ourcvfh_output.
size ();
538 if (!ourcvfh_output.
points.empty ())
540 ourcvfh_output.
height = 1;
542 output = ourcvfh_output;
546 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void 549 if (refine_clusters_ <= 0.f)
550 refine_clusters_ = 1.f;
555 PCL_ERROR (
"[pcl::%s::computeFeature] No input dataset containing normals was given!\n", getClassName ().c_str ());
556 output.width = output.height = 0;
557 output.points.clear ();
560 if (normals_->size () != surface_->size ())
562 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 ());
563 output.width = output.height = 0;
564 output.points.clear ();
568 centroids_dominant_orientations_.clear ();
570 transforms_.clear ();
571 dominant_normals_.clear ();
574 std::vector<int> indices_out;
575 std::vector<int> indices_in;
576 filterNormalsWithHighCurvature (*normals_, *indices_, indices_out, indices_in, curv_threshold_);
579 normals_filtered_cloud->width = indices_in.size ();
580 normals_filtered_cloud->height = 1;
581 normals_filtered_cloud->points.resize (normals_filtered_cloud->width);
583 std::vector<int> indices_from_nfc_to_indices;
584 indices_from_nfc_to_indices.resize (indices_in.size ());
586 for (std::size_t i = 0; i < indices_in.size (); ++i)
588 (*normals_filtered_cloud)[i].x = (*surface_)[indices_in[i]].x;
589 (*normals_filtered_cloud)[i].y = (*surface_)[indices_in[i]].y;
590 (*normals_filtered_cloud)[i].z = (*surface_)[indices_in[i]].z;
592 indices_from_nfc_to_indices[i] = indices_in[i];
595 std::vector<pcl::PointIndices> clusters;
597 if (normals_filtered_cloud->size () >= min_points_)
602 normals_tree_filtered->setInputCloud (normals_filtered_cloud);
607 n3d.
compute (*normals_filtered_cloud);
611 normals_tree->setInputCloud (normals_filtered_cloud);
613 extractEuclideanClustersSmooth (*normals_filtered_cloud, *normals_filtered_cloud, cluster_tolerance_, normals_tree, clusters,
614 eps_angle_threshold_, static_cast<unsigned int> (min_points_));
616 std::vector<pcl::PointIndices> clusters_filtered;
617 int cluster_filtered_idx = 0;
618 for (
const auto &cluster : clusters)
625 clusters_.push_back (pi);
626 clusters_filtered.push_back (pi_filtered);
628 Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
629 Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
631 for (
const auto &index : cluster.indices)
633 avg_normal += (*normals_filtered_cloud)[index].getNormalVector4fMap ();
634 avg_centroid += (*normals_filtered_cloud)[index].getVector4fMap ();
637 avg_normal /=
static_cast<float> (cluster.indices.size ());
638 avg_centroid /=
static_cast<float> (cluster.indices.size ());
639 avg_normal.normalize ();
641 Eigen::Vector3f avg_norm (avg_normal[0], avg_normal[1], avg_normal[2]);
642 Eigen::Vector3f avg_dominant_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
644 for (
const auto &index : cluster.indices)
647 double dot_p = avg_normal.dot ((*normals_filtered_cloud)[index].getNormalVector4fMap ());
648 if (std::abs (std::acos (dot_p)) < (eps_angle_threshold_ * refine_clusters_))
650 clusters_[cluster_filtered_idx].indices.push_back (indices_from_nfc_to_indices[index]);
651 clusters_filtered[cluster_filtered_idx].indices.push_back (index);
656 if (clusters_[cluster_filtered_idx].indices.empty ())
658 clusters_.pop_back ();
659 clusters_filtered.pop_back ();
662 cluster_filtered_idx++;
665 clusters = clusters_filtered;
680 if (!clusters.empty ())
682 for (
const auto &cluster : clusters)
684 Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
685 Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
687 for (
const auto &index : cluster.indices)
689 avg_normal += (*normals_filtered_cloud)[index].getNormalVector4fMap ();
690 avg_centroid += (*normals_filtered_cloud)[index].getVector4fMap ();
693 avg_normal /=
static_cast<float> (cluster.indices.size ());
694 avg_centroid /=
static_cast<float> (cluster.indices.size ());
695 avg_normal.normalize ();
698 dominant_normals_.emplace_back (avg_normal[0], avg_normal[1], avg_normal[2]);
699 centroids_dominant_orientations_.emplace_back (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
703 output.points.resize (dominant_normals_.size ());
704 output.width = dominant_normals_.size ();
706 for (std::size_t i = 0; i < dominant_normals_.size (); ++i)
713 output[i] = vfh_signature[0];
719 computeRFAndShapeDistribution (cloud_input, output, clusters_);
724 PCL_WARN(
"No clusters were found in the surface... using VFH...\n");
725 Eigen::Vector4f avg_centroid;
727 Eigen::Vector3f cloud_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
728 centroids_dominant_orientations_.push_back (cloud_centroid);
737 output.points.resize (1);
740 output[0] = vfh_signature[0];
741 Eigen::Matrix4f
id = Eigen::Matrix4f::Identity ();
742 transforms_.push_back (
id);
743 valid_transforms_.push_back (
false);
747 #define PCL_INSTANTIATE_OURCVFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::OURCVFHEstimation<T,NT,OutT>; 749 #endif // PCL_FEATURES_IMPL_OURCVFH_H_ void setUseGivenCentroid(bool use)
Set use_given_centroid_.
void compute(PointCloudOut &output)
Overloaded computed method from pcl::Feature.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
shared_ptr< PointCloud< PointT > > Ptr
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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.
typename pcl::PointCloud< PointInT >::Ptr PointInTPtr
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
void computeRFAndShapeDistribution(PointInTPtr &processed, PointCloudOut &output, std::vector< pcl::PointIndices > &cluster_indices)
Computes SGURF and the shape distribution based on the selected SGURF.
virtual int radiusSearch(const PointT &point, double radius, Indices &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 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 getMaxDistance(const pcl::PointCloud< PointT > &cloud, const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt)
Get the point at maximum distance from a given point and a given pointcloud.
void setUseGivenNormal(bool use)
Set use_given_normal_.
std::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_.
void copyPointCloud(const pcl::PointCloud< PointInT > &cloud_in, pcl::PointCloud< PointOutT > &cloud_out)
Copy all the fields from a given point cloud into a new point cloud.
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform, bool copy_all_fields)
Apply an affine transform defined by an Eigen Transform.
void demeanPointCloud(ConstCloudIterator< PointT > &cloud_iterator, const Eigen::Matrix< Scalar, 4, 1 > ¢roid, pcl::PointCloud< PointT > &cloud_out, int npts=0)
Subtract a centroid from a point cloud and return the de-meaned representation.
void clear()
Removes all points in a cloud and sets the width and height to 0.
VFHEstimation estimates the Viewpoint Feature Histogram (VFH) descriptor for a given point cloud data...
std::uint32_t height
The point cloud height (if organized as an image-structure).
void setNormalizeBins(bool normalize)
set normalize_bins_
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.
bool sgurf(Eigen::Vector3f ¢roid, Eigen::Vector3f &normal_centroid, PointInTPtr &processed, std::vector< Eigen::Matrix4f, Eigen::aligned_allocator< Eigen::Matrix4f > > &transformations, PointInTPtr &grid, pcl::PointIndices &indices)
Computes SGURF.
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.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
shared_ptr< pcl::search::Search< PointT > > Ptr
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 setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset...
OURCVFHEstimation estimates the Oriented, Unique and Repetable Clustered Viewpoint Feature Histogram ...