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/features/pfh_tools.h>
48 #include <pcl/common/transforms.h>
51 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
68 computeFeature (output);
74 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
79 std::vector<pcl::PointIndices> &clusters,
double eps_angle,
80 unsigned int min_pts_per_cluster,
81 unsigned int max_pts_per_cluster)
85 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 ());
90 PCL_ERROR (
"[pcl::extractEuclideanClusters] Number of points in the input point cloud (%lu) different than normals (%lu)!\n", cloud.
points.size (), normals.
points.size ());
95 std::vector<bool> processed (cloud.
points.size (),
false);
97 std::vector<int> nn_indices;
98 std::vector<float> nn_distances;
100 for (std::size_t i = 0; i < cloud.
points.size (); ++i)
105 std::vector<std::size_t> seed_queue;
106 std::size_t sq_idx = 0;
107 seed_queue.push_back (i);
111 while (sq_idx < seed_queue.size ())
114 if (!tree->
radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
120 for (std::size_t j = 1; j < nn_indices.size (); ++j)
122 if (processed[nn_indices[j]])
128 double dot_p = normals.
points[seed_queue[sq_idx]].normal[0] * normals.
points[nn_indices[j]].normal[0]
129 + normals.
points[seed_queue[sq_idx]].normal[1] * normals.
points[nn_indices[j]].normal[1] + normals.
points[seed_queue[sq_idx]].normal[2]
130 * normals.
points[nn_indices[j]].normal[2];
132 if (std::abs (std::acos (dot_p)) < eps_angle)
134 processed[nn_indices[j]] =
true;
135 seed_queue.push_back (nn_indices[j]);
143 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
146 r.
indices.resize (seed_queue.size ());
147 for (std::size_t j = 0; j < seed_queue.size (); ++j)
154 clusters.push_back (r);
160 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
162 std::vector<int> &indices_to_use,
163 std::vector<int> &indices_out, std::vector<int> &indices_in,
166 indices_out.resize (cloud.
points.size ());
167 indices_in.resize (cloud.
points.size ());
172 for (
const int &index : indices_to_use)
174 if (cloud.
points[index].curvature > threshold)
176 indices_out[out] = index;
181 indices_in[in] = index;
186 indices_out.resize (out);
187 indices_in.resize (in);
190 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
bool
192 PointInTPtr & processed, std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > & transformations,
196 Eigen::Vector3f plane_normal;
197 plane_normal[0] = -centroid[0];
198 plane_normal[1] = -centroid[1];
199 plane_normal[2] = -centroid[2];
200 Eigen::Vector3f z_vector = Eigen::Vector3f::UnitZ ();
201 plane_normal.normalize ();
202 Eigen::Vector3f axis = plane_normal.cross (z_vector);
203 double rotation = -asin (axis.norm ());
206 Eigen::Affine3f transformPC (Eigen::AngleAxisf (static_cast<float> (rotation), axis));
208 grid->points.resize (processed->points.size ());
209 for (std::size_t k = 0; k < processed->points.size (); k++)
210 grid->points[k].getVector4fMap () = processed->points[k].getVector4fMap ();
214 Eigen::Vector4f centroid4f (centroid[0], centroid[1], centroid[2], 0);
215 Eigen::Vector4f normal_centroid4f (normal_centroid[0], normal_centroid[1], normal_centroid[2], 0);
217 centroid4f = transformPC * centroid4f;
218 normal_centroid4f = transformPC * normal_centroid4f;
220 Eigen::Vector3f centroid3f (centroid4f[0], centroid4f[1], centroid4f[2]);
222 Eigen::Vector4f farthest_away;
224 farthest_away[3] = 0;
226 float max_dist = (farthest_away - centroid4f).norm ();
230 Eigen::Matrix4f center_mat;
231 center_mat.setIdentity (4, 4);
232 center_mat (0, 3) = -centroid4f[0];
233 center_mat (1, 3) = -centroid4f[1];
234 center_mat (2, 3) = -centroid4f[2];
236 Eigen::Matrix3f scatter;
240 for (
const int &index : indices.
indices)
242 Eigen::Vector3f pvector = grid->points[index].getVector3fMap ();
243 float d_k = (pvector).norm ();
244 float w = (max_dist - d_k);
245 Eigen::Vector3f diff = (pvector);
246 Eigen::Matrix3f mat = diff * diff.transpose ();
253 Eigen::JacobiSVD <Eigen::MatrixXf> svd (scatter, Eigen::ComputeFullV);
254 Eigen::Vector3f evx = svd.matrixV ().col (0);
255 Eigen::Vector3f evy = svd.matrixV ().col (1);
256 Eigen::Vector3f evz = svd.matrixV ().col (2);
257 Eigen::Vector3f evxminus = evx * -1;
258 Eigen::Vector3f evyminus = evy * -1;
259 Eigen::Vector3f evzminus = evz * -1;
261 float s_xplus, s_xminus, s_yplus, s_yminus;
262 s_xplus = s_xminus = s_yplus = s_yminus = 0.f;
265 for (
const auto& point: grid->points)
267 Eigen::Vector3f pvector = point.getVector3fMap ();
268 float dist_x, dist_y;
269 dist_x = std::abs (evx.dot (pvector));
270 dist_y = std::abs (evy.dot (pvector));
272 if ((pvector).dot (evx) >= 0)
277 if ((pvector).dot (evy) >= 0)
284 if (s_xplus < s_xminus)
287 if (s_yplus < s_yminus)
292 float max_x =
static_cast<float> (std::max (s_xplus, s_xminus));
293 float min_x =
static_cast<float> (std::min (s_xplus, s_xminus));
294 float max_y =
static_cast<float> (std::max (s_yplus, s_yminus));
295 float min_y =
static_cast<float> (std::min (s_yplus, s_yminus));
297 fx = (min_x / max_x);
298 fy = (min_y / max_y);
300 Eigen::Vector3f normal3f = Eigen::Vector3f (normal_centroid4f[0], normal_centroid4f[1], normal_centroid4f[2]);
301 if (normal3f.dot (evz) < 0)
307 float max_axis = std::max (fx, fy);
308 float min_axis = std::min (fx, fy);
310 if ((min_axis / max_axis) > axis_ratio_)
312 PCL_WARN (
"Both axes are equally easy/difficult to disambiguate\n");
314 Eigen::Vector3f evy_copy = evy;
315 Eigen::Vector3f evxminus = evx * -1;
316 Eigen::Vector3f evyminus = evy * -1;
318 if (min_axis > min_axis_value_)
321 evy = evx.cross (evz);
322 Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
323 transformations.push_back (trans);
326 evy = evx.cross (evz);
327 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);
344 evy = evx.cross (evz);
345 Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
346 transformations.push_back (trans);
350 evy = evx.cross (evz);
351 trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
352 transformations.push_back (trans);
363 evy = evx.cross (evz);
364 Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
365 transformations.push_back (trans);
373 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
375 std::vector<pcl::PointIndices> & cluster_indices)
379 cluster_axes_.
clear ();
380 cluster_axes_.resize (centroids_dominant_orientations_.size ());
382 for (std::size_t i = 0; i < centroids_dominant_orientations_.size (); i++)
385 std::vector < Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > transformations;
387 sgurf (centroids_dominant_orientations_[i], dominant_normals_[i], processed, transformations, grid, cluster_indices[i]);
390 cluster_axes_[i] = transformations.size ();
392 for (
const auto &transformation : transformations)
396 transforms_.push_back (transformation);
397 valid_transforms_.push_back (
true);
399 std::vector < Eigen::VectorXf > quadrants (8);
402 for (
int k = 0; k < num_hists; k++)
403 quadrants[k].setZero (size_hists);
405 Eigen::Vector4f centroid_p;
406 centroid_p.setZero ();
407 Eigen::Vector4f max_pt;
410 double distance_normalization_factor = (centroid_p - max_pt).norm ();
414 hist_incr = 100.0f /
static_cast<float> (grid->points.size () - 1);
418 float * weights =
new float[num_hists];
420 float sigma_sq = sigma * sigma;
422 for (
const auto& point: grid->points)
424 Eigen::Vector4f p = point.getVector4fMap ();
429 float wx = 1.f - std::exp (-((p[0] * p[0]) / (2.f * sigma_sq)));
430 float wy = 1.f - std::exp (-((p[1] * p[1]) / (2.f * sigma_sq)));
431 float wz = 1.f - std::exp (-((p[2] * p[2]) / (2.f * sigma_sq)));
436 for (std::size_t ii = 0; ii <= 3; ii++)
437 weights[ii] = 0.5f - wx * 0.5f;
439 for (std::size_t ii = 4; ii <= 7; ii++)
440 weights[ii] = 0.5f + wx * 0.5f;
444 for (std::size_t ii = 0; ii <= 3; ii++)
445 weights[ii] = 0.5f + wx * 0.5f;
447 for (std::size_t ii = 4; ii <= 7; ii++)
448 weights[ii] = 0.5f - wx * 0.5f;
454 for (std::size_t ii = 0; ii <= 1; ii++)
455 weights[ii] *= 0.5f - wy * 0.5f;
456 for (std::size_t ii = 4; ii <= 5; ii++)
457 weights[ii] *= 0.5f - wy * 0.5f;
459 for (std::size_t ii = 2; ii <= 3; ii++)
460 weights[ii] *= 0.5f + wy * 0.5f;
462 for (std::size_t ii = 6; ii <= 7; ii++)
463 weights[ii] *= 0.5f + wy * 0.5f;
467 for (std::size_t ii = 0; ii <= 1; ii++)
468 weights[ii] *= 0.5f + wy * 0.5f;
469 for (std::size_t ii = 4; ii <= 5; ii++)
470 weights[ii] *= 0.5f + wy * 0.5f;
472 for (std::size_t ii = 2; ii <= 3; ii++)
473 weights[ii] *= 0.5f - wy * 0.5f;
475 for (std::size_t ii = 6; ii <= 7; ii++)
476 weights[ii] *= 0.5f - wy * 0.5f;
482 for (std::size_t ii = 0; ii <= 7; ii += 2)
483 weights[ii] *= 0.5f - wz * 0.5f;
485 for (std::size_t ii = 1; ii <= 7; ii += 2)
486 weights[ii] *= 0.5f + wz * 0.5f;
491 for (std::size_t ii = 0; ii <= 7; ii += 2)
492 weights[ii] *= 0.5f + wz * 0.5f;
494 for (std::size_t ii = 1; ii <= 7; ii += 2)
495 weights[ii] *= 0.5f - wz * 0.5f;
498 int h_index = (d <= 0) ? 0 : std::ceil (size_hists * (d / distance_normalization_factor)) - 1;
506 for (
int j = 0; j < num_hists; j++)
507 quadrants[j][h_index] += hist_incr * weights[j];
513 vfh_signature.
points.resize (1);
515 for (
int d = 0; d < 308; ++d)
516 vfh_signature.
points[0].histogram[d] = output.
points[i].histogram[d];
519 for (
int k = 0; k < num_hists; k++)
521 for (
int ii = 0; ii < size_hists; ii++, pos++)
523 vfh_signature.
points[0].histogram[pos] = quadrants[k][ii];
527 ourcvfh_output.
points.push_back (vfh_signature.
points[0]);
528 ourcvfh_output.
width = ourcvfh_output.
points.size ();
533 if (!ourcvfh_output.
points.empty ())
535 ourcvfh_output.
height = 1;
537 output = ourcvfh_output;
541 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
544 if (refine_clusters_ <= 0.f)
545 refine_clusters_ = 1.f;
550 PCL_ERROR (
"[pcl::%s::computeFeature] No input dataset containing normals was given!\n", getClassName ().c_str ());
551 output.width = output.height = 0;
552 output.points.clear ();
555 if (normals_->points.size () != surface_->points.size ())
557 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 ());
558 output.width = output.height = 0;
559 output.points.clear ();
563 centroids_dominant_orientations_.clear ();
565 transforms_.clear ();
566 dominant_normals_.clear ();
569 std::vector<int> indices_out;
570 std::vector<int> indices_in;
571 filterNormalsWithHighCurvature (*normals_, *indices_, indices_out, indices_in, curv_threshold_);
574 normals_filtered_cloud->width =
static_cast<std::uint32_t
> (indices_in.size ());
575 normals_filtered_cloud->height = 1;
576 normals_filtered_cloud->points.resize (normals_filtered_cloud->width);
578 std::vector<int> indices_from_nfc_to_indices;
579 indices_from_nfc_to_indices.resize (indices_in.size ());
581 for (std::size_t i = 0; i < indices_in.size (); ++i)
583 normals_filtered_cloud->points[i].x = surface_->points[indices_in[i]].x;
584 normals_filtered_cloud->points[i].y = surface_->points[indices_in[i]].y;
585 normals_filtered_cloud->points[i].z = surface_->points[indices_in[i]].z;
587 indices_from_nfc_to_indices[i] = indices_in[i];
590 std::vector<pcl::PointIndices> clusters;
592 if (normals_filtered_cloud->points.size () >= min_points_)
597 normals_tree_filtered->setInputCloud (normals_filtered_cloud);
602 n3d.
compute (*normals_filtered_cloud);
606 normals_tree->setInputCloud (normals_filtered_cloud);
608 extractEuclideanClustersSmooth (*normals_filtered_cloud, *normals_filtered_cloud, cluster_tolerance_, normals_tree, clusters,
609 eps_angle_threshold_, static_cast<unsigned int> (min_points_));
611 std::vector<pcl::PointIndices> clusters_filtered;
612 int cluster_filtered_idx = 0;
613 for (
const auto &cluster : clusters)
620 clusters_.push_back (pi);
621 clusters_filtered.push_back (pi_filtered);
623 Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
624 Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
626 for (
const auto &index : cluster.indices)
628 avg_normal += normals_filtered_cloud->points[index].getNormalVector4fMap ();
629 avg_centroid += normals_filtered_cloud->points[index].getVector4fMap ();
632 avg_normal /=
static_cast<float> (cluster.indices.size ());
633 avg_centroid /=
static_cast<float> (cluster.indices.size ());
634 avg_normal.normalize ();
636 Eigen::Vector3f avg_norm (avg_normal[0], avg_normal[1], avg_normal[2]);
637 Eigen::Vector3f avg_dominant_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
639 for (
const auto &index : cluster.indices)
642 double dot_p = avg_normal.dot (normals_filtered_cloud->points[index].getNormalVector4fMap ());
643 if (std::abs (std::acos (dot_p)) < (eps_angle_threshold_ * refine_clusters_))
645 clusters_[cluster_filtered_idx].indices.push_back (indices_from_nfc_to_indices[index]);
646 clusters_filtered[cluster_filtered_idx].indices.push_back (index);
651 if (clusters_[cluster_filtered_idx].indices.empty ())
653 clusters_.pop_back ();
654 clusters_filtered.pop_back ();
657 cluster_filtered_idx++;
660 clusters = clusters_filtered;
675 if (!clusters.empty ())
677 for (
const auto &cluster : clusters)
679 Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
680 Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
682 for (
const auto &index : cluster.indices)
684 avg_normal += normals_filtered_cloud->points[index].getNormalVector4fMap ();
685 avg_centroid += normals_filtered_cloud->points[index].getVector4fMap ();
688 avg_normal /=
static_cast<float> (cluster.indices.size ());
689 avg_centroid /=
static_cast<float> (cluster.indices.size ());
690 avg_normal.normalize ();
693 dominant_normals_.emplace_back (avg_normal[0], avg_normal[1], avg_normal[2]);
694 centroids_dominant_orientations_.emplace_back (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
698 output.points.resize (dominant_normals_.size ());
699 output.width =
static_cast<std::uint32_t
> (dominant_normals_.size ());
701 for (std::size_t i = 0; i < dominant_normals_.size (); ++i)
708 output.points[i] = vfh_signature.
points[0];
714 computeRFAndShapeDistribution (cloud_input, output, clusters_);
719 PCL_WARN(
"No clusters were found in the surface... using VFH...\n");
720 Eigen::Vector4f avg_centroid;
722 Eigen::Vector3f cloud_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
723 centroids_dominant_orientations_.push_back (cloud_centroid);
732 output.points.resize (1);
735 output.points[0] = vfh_signature.
points[0];
736 Eigen::Matrix4f
id = Eigen::Matrix4f::Identity ();
737 transforms_.push_back (
id);
738 valid_transforms_.push_back (
false);
742 #define PCL_INSTANTIATE_OURCVFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::OURCVFHEstimation<T,NT,OutT>;
744 #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.
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 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 computeRFAndShapeDistribution(PointInTPtr &processed, PointCloudOut &output, std::vector< pcl::PointIndices > &cluster_indices)
Computes SGURF and the shape distribution based on the selected SGURF.
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.
virtual PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
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 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 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 ...