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/io.h>
49 #include <pcl/common/transforms.h>
52 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
69 computeFeature (output);
75 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
80 std::vector<pcl::PointIndices> &clusters,
double eps_angle,
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 "
87 "dataset (%zu) than the input cloud (%zu)!\n",
89 static_cast<std::size_t
>(cloud.
size()));
92 if (cloud.
size () != normals.
size ())
94 PCL_ERROR(
"[pcl::extractEuclideanClusters] Number of points in the input point "
95 "cloud (%zu) different than normals (%zu)!\n",
96 static_cast<std::size_t
>(cloud.
size()),
97 static_cast<std::size_t
>(normals.
size()));
104 std::vector<bool> processed (cloud.
size (),
false);
107 std::vector<float> nn_distances;
109 for (std::size_t i = 0; i < cloud.
size (); ++i)
114 std::vector<std::size_t> seed_queue;
115 std::size_t sq_idx = 0;
116 seed_queue.push_back (i);
120 while (sq_idx < seed_queue.size ())
123 if (!tree->
radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
129 for (std::size_t j = nn_start_idx; j < nn_indices.size (); ++j)
131 if (processed[nn_indices[j]])
137 double dot_p = normals[seed_queue[sq_idx]].normal[0] * normals[nn_indices[j]].normal[0]
138 + normals[seed_queue[sq_idx]].normal[1] * normals[nn_indices[j]].normal[1] + normals[seed_queue[sq_idx]].normal[2]
139 * normals[nn_indices[j]].normal[2];
141 if (std::abs (std::acos (dot_p)) < eps_angle)
143 processed[nn_indices[j]] =
true;
152 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
155 r.
indices.resize (seed_queue.size ());
156 for (std::size_t j = 0; j < seed_queue.size (); ++j)
163 clusters.push_back (r);
169 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
175 indices_out.resize (cloud.
size ());
176 indices_in.resize (cloud.
size ());
181 for (
const auto &index : indices_to_use)
183 if (cloud[index].curvature > threshold)
185 indices_out[out] = index;
190 indices_in[in] = index;
195 indices_out.resize (out);
196 indices_in.resize (in);
199 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
bool
201 PointInTPtr & processed, std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > & transformations,
205 Eigen::Vector3f plane_normal;
206 plane_normal[0] = -centroid[0];
207 plane_normal[1] = -centroid[1];
208 plane_normal[2] = -centroid[2];
209 Eigen::Vector3f z_vector = Eigen::Vector3f::UnitZ ();
210 plane_normal.normalize ();
211 Eigen::Vector3f axis = plane_normal.cross (z_vector);
212 double rotation = -asin (axis.norm ());
215 Eigen::Affine3f transformPC (Eigen::AngleAxisf (
static_cast<float> (rotation), axis));
217 grid->resize (processed->size ());
218 for (std::size_t k = 0; k < processed->size (); k++)
219 (*grid)[k].getVector4fMap () = (*processed)[k].getVector4fMap ();
223 Eigen::Vector4f centroid4f (centroid[0], centroid[1], centroid[2], 0);
224 Eigen::Vector4f normal_centroid4f (normal_centroid[0], normal_centroid[1], normal_centroid[2], 0);
226 centroid4f = transformPC * centroid4f;
227 normal_centroid4f = transformPC * normal_centroid4f;
229 Eigen::Vector3f centroid3f (centroid4f[0], centroid4f[1], centroid4f[2]);
231 Eigen::Vector4f farthest_away;
233 farthest_away[3] = 0;
235 float max_dist = (farthest_away - centroid4f).norm ();
239 Eigen::Matrix4f center_mat;
240 center_mat.setIdentity (4, 4);
241 center_mat (0, 3) = -centroid4f[0];
242 center_mat (1, 3) = -centroid4f[1];
243 center_mat (2, 3) = -centroid4f[2];
245 Eigen::Matrix3f scatter;
249 for (
const auto &index : indices.
indices)
251 Eigen::Vector3f pvector = (*grid)[index].getVector3fMap ();
252 float d_k = (pvector).norm ();
253 float w = (max_dist - d_k);
254 Eigen::Vector3f diff = (pvector);
255 Eigen::Matrix3f mat = diff * diff.transpose ();
262 Eigen::JacobiSVD <Eigen::MatrixXf> svd (scatter, Eigen::ComputeFullV);
263 Eigen::Vector3f evx = svd.matrixV ().col (0);
264 Eigen::Vector3f evy = svd.matrixV ().col (1);
265 Eigen::Vector3f evz = svd.matrixV ().col (2);
266 Eigen::Vector3f evxminus = evx * -1;
267 Eigen::Vector3f evyminus = evy * -1;
268 Eigen::Vector3f evzminus = evz * -1;
270 float s_xplus, s_xminus, s_yplus, s_yminus;
271 s_xplus = s_xminus = s_yplus = s_yminus = 0.f;
274 for (
const auto& point: grid->points)
276 Eigen::Vector3f pvector = point.getVector3fMap ();
277 float dist_x, dist_y;
278 dist_x = std::abs (evx.dot (pvector));
279 dist_y = std::abs (evy.dot (pvector));
281 if ((pvector).dot (evx) >= 0)
286 if ((pvector).dot (evy) >= 0)
293 if (s_xplus < s_xminus)
296 if (s_yplus < s_yminus)
301 float max_x =
static_cast<float> (std::max (s_xplus, s_xminus));
302 float min_x =
static_cast<float> (std::min (s_xplus, s_xminus));
303 float max_y =
static_cast<float> (std::max (s_yplus, s_yminus));
304 float min_y =
static_cast<float> (std::min (s_yplus, s_yminus));
306 fx = (min_x / max_x);
307 fy = (min_y / max_y);
309 Eigen::Vector3f normal3f = Eigen::Vector3f (normal_centroid4f[0], normal_centroid4f[1], normal_centroid4f[2]);
310 if (normal3f.dot (evz) < 0)
316 float max_axis = std::max (fx, fy);
317 float min_axis = std::min (fx, fy);
319 if ((min_axis / max_axis) > axis_ratio_)
321 PCL_WARN (
"Both axes are equally easy/difficult to disambiguate\n");
323 Eigen::Vector3f evy_copy = evy;
324 Eigen::Vector3f evxminus = evx * -1;
325 Eigen::Vector3f evyminus = evy * -1;
327 if (min_axis > min_axis_value_)
330 evy = evx.cross (evz);
331 Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
332 transformations.push_back (trans);
335 evy = evx.cross (evz);
336 trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
337 transformations.push_back (trans);
340 evy = evx.cross (evz);
341 trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
342 transformations.push_back (trans);
345 evy = evx.cross (evz);
346 trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
347 transformations.push_back (trans);
353 evy = evx.cross (evz);
354 Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
355 transformations.push_back (trans);
359 evy = evx.cross (evz);
360 trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
361 transformations.push_back (trans);
372 evy = evx.cross (evz);
373 Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
374 transformations.push_back (trans);
382 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
384 std::vector<pcl::PointIndices> & cluster_indices)
388 cluster_axes_.
clear ();
389 cluster_axes_.resize (centroids_dominant_orientations_.size ());
391 for (std::size_t i = 0; i < centroids_dominant_orientations_.size (); i++)
394 std::vector < Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > transformations;
396 sgurf (centroids_dominant_orientations_[i], dominant_normals_[i], processed, transformations, grid, cluster_indices[i]);
399 cluster_axes_[i] = transformations.size ();
401 for (
const auto &transformation : transformations)
405 transforms_.push_back (transformation);
406 valid_transforms_.push_back (
true);
408 std::vector < Eigen::VectorXf > quadrants (8);
411 for (
int k = 0; k < num_hists; k++)
412 quadrants[k].setZero (size_hists);
414 Eigen::Vector4f centroid_p;
415 centroid_p.setZero ();
416 Eigen::Vector4f max_pt;
419 double distance_normalization_factor = (centroid_p - max_pt).norm ();
423 hist_incr = 100.0f /
static_cast<float> (grid->size () - 1);
427 float * weights =
new float[num_hists];
429 float sigma_sq = sigma * sigma;
431 for (
const auto& point: grid->points)
433 Eigen::Vector4f p = point.getVector4fMap ();
438 float wx = 1.f - std::exp (-((p[0] * p[0]) / (2.f * sigma_sq)));
439 float wy = 1.f - std::exp (-((p[1] * p[1]) / (2.f * sigma_sq)));
440 float wz = 1.f - std::exp (-((p[2] * p[2]) / (2.f * sigma_sq)));
445 for (std::size_t ii = 0; ii <= 3; ii++)
446 weights[ii] = 0.5f - wx * 0.5f;
448 for (std::size_t ii = 4; ii <= 7; ii++)
449 weights[ii] = 0.5f + wx * 0.5f;
453 for (std::size_t ii = 0; ii <= 3; ii++)
454 weights[ii] = 0.5f + wx * 0.5f;
456 for (std::size_t ii = 4; ii <= 7; ii++)
457 weights[ii] = 0.5f - wx * 0.5f;
463 for (std::size_t ii = 0; ii <= 1; ii++)
464 weights[ii] *= 0.5f - wy * 0.5f;
465 for (std::size_t ii = 4; ii <= 5; ii++)
466 weights[ii] *= 0.5f - wy * 0.5f;
468 for (std::size_t ii = 2; ii <= 3; ii++)
469 weights[ii] *= 0.5f + wy * 0.5f;
471 for (std::size_t ii = 6; ii <= 7; ii++)
472 weights[ii] *= 0.5f + wy * 0.5f;
476 for (std::size_t ii = 0; ii <= 1; ii++)
477 weights[ii] *= 0.5f + wy * 0.5f;
478 for (std::size_t ii = 4; ii <= 5; ii++)
479 weights[ii] *= 0.5f + wy * 0.5f;
481 for (std::size_t ii = 2; ii <= 3; ii++)
482 weights[ii] *= 0.5f - wy * 0.5f;
484 for (std::size_t ii = 6; ii <= 7; ii++)
485 weights[ii] *= 0.5f - wy * 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;
500 for (std::size_t ii = 0; ii <= 7; ii += 2)
501 weights[ii] *= 0.5f + wz * 0.5f;
503 for (std::size_t ii = 1; ii <= 7; ii += 2)
504 weights[ii] *= 0.5f - wz * 0.5f;
507 int h_index = (d <= 0) ? 0 : std::ceil (size_hists * (d / distance_normalization_factor)) - 1;
515 for (
int j = 0; j < num_hists; j++)
516 quadrants[j][h_index] += hist_incr * weights[j];
524 for (
int d = 0; d < 308; ++d)
525 vfh_signature[0].histogram[d] = output[i].histogram[d];
528 for (
int k = 0; k < num_hists; k++)
530 for (
int ii = 0; ii < size_hists; ii++, pos++)
532 vfh_signature[0].histogram[pos] = quadrants[k][ii];
536 ourcvfh_output.
push_back (vfh_signature[0]);
537 ourcvfh_output.
width = ourcvfh_output.
size ();
542 if (!ourcvfh_output.
empty ())
544 ourcvfh_output.
height = 1;
546 output = ourcvfh_output;
550 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
553 if (refine_clusters_ <= 0.f)
554 refine_clusters_ = 1.f;
559 PCL_ERROR (
"[pcl::%s::computeFeature] No input dataset containing normals was given!\n", getClassName ().c_str ());
560 output.width = output.height = 0;
564 if (normals_->size () != surface_->size ())
566 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 ());
567 output.width = output.height = 0;
572 centroids_dominant_orientations_.clear ();
574 transforms_.clear ();
575 dominant_normals_.clear ();
580 filterNormalsWithHighCurvature (*normals_, *indices_, indices_out, indices_in, curv_threshold_);
583 normals_filtered_cloud->width = indices_in.size ();
584 normals_filtered_cloud->height = 1;
585 normals_filtered_cloud->points.resize (normals_filtered_cloud->width);
588 indices_from_nfc_to_indices.resize (indices_in.size ());
590 for (std::size_t i = 0; i < indices_in.size (); ++i)
592 (*normals_filtered_cloud)[i].x = (*surface_)[indices_in[i]].x;
593 (*normals_filtered_cloud)[i].y = (*surface_)[indices_in[i]].y;
594 (*normals_filtered_cloud)[i].z = (*surface_)[indices_in[i]].z;
596 indices_from_nfc_to_indices[i] = indices_in[i];
599 std::vector<pcl::PointIndices> clusters;
601 if (normals_filtered_cloud->size () >= min_points_)
606 normals_tree_filtered->setInputCloud (normals_filtered_cloud);
611 n3d.
compute (*normals_filtered_cloud);
615 normals_tree->setInputCloud (normals_filtered_cloud);
617 extractEuclideanClustersSmooth (*normals_filtered_cloud, *normals_filtered_cloud, cluster_tolerance_, normals_tree, clusters,
618 eps_angle_threshold_,
static_cast<unsigned int> (min_points_));
620 std::vector<pcl::PointIndices> clusters_filtered;
621 int cluster_filtered_idx = 0;
622 for (
const auto &cluster : clusters)
629 clusters_.push_back (pi);
630 clusters_filtered.push_back (pi_filtered);
632 Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
633 Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
635 for (
const auto &index : cluster.indices)
637 avg_normal += (*normals_filtered_cloud)[index].getNormalVector4fMap ();
638 avg_centroid += (*normals_filtered_cloud)[index].getVector4fMap ();
641 avg_normal /=
static_cast<float> (cluster.indices.size ());
642 avg_centroid /=
static_cast<float> (cluster.indices.size ());
643 avg_normal.normalize ();
645 Eigen::Vector3f avg_norm (avg_normal[0], avg_normal[1], avg_normal[2]);
646 Eigen::Vector3f avg_dominant_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
648 for (
const auto &index : cluster.indices)
651 double dot_p = avg_normal.dot ((*normals_filtered_cloud)[index].getNormalVector4fMap ());
652 if (std::abs (std::acos (dot_p)) < (eps_angle_threshold_ * refine_clusters_))
654 clusters_[cluster_filtered_idx].indices.push_back (indices_from_nfc_to_indices[index]);
655 clusters_filtered[cluster_filtered_idx].indices.push_back (index);
660 if (clusters_[cluster_filtered_idx].indices.empty ())
662 clusters_.pop_back ();
663 clusters_filtered.pop_back ();
666 cluster_filtered_idx++;
669 clusters = clusters_filtered;
684 if (!clusters.empty ())
686 for (
const auto &cluster : clusters)
688 Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
689 Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
691 for (
const auto &index : cluster.indices)
693 avg_normal += (*normals_filtered_cloud)[index].getNormalVector4fMap ();
694 avg_centroid += (*normals_filtered_cloud)[index].getVector4fMap ();
697 avg_normal /=
static_cast<float> (cluster.indices.size ());
698 avg_centroid /=
static_cast<float> (cluster.indices.size ());
699 avg_normal.normalize ();
702 dominant_normals_.emplace_back (avg_normal[0], avg_normal[1], avg_normal[2]);
703 centroids_dominant_orientations_.emplace_back (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
707 output.resize (dominant_normals_.size ());
708 output.width = dominant_normals_.size ();
710 for (std::size_t i = 0; i < dominant_normals_.size (); ++i)
717 output[i] = vfh_signature[0];
723 computeRFAndShapeDistribution (cloud_input, output, clusters_);
728 PCL_WARN(
"No clusters were found in the surface... using VFH...\n");
729 Eigen::Vector4f avg_centroid;
731 Eigen::Vector3f cloud_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
732 centroids_dominant_orientations_.push_back (cloud_centroid);
744 output[0] = vfh_signature[0];
745 Eigen::Matrix4f
id = Eigen::Matrix4f::Identity ();
746 transforms_.push_back (
id);
747 valid_transforms_.push_back (
false);
751 #define PCL_INSTANTIATE_OURCVFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::OURCVFHEstimation<T,NT,OutT>;
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset.
Feature represents the base feature class.
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
void setSearchMethod(const KdTreePtr &tree)
Provide a pointer to the search object.
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point.
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
OURCVFHEstimation estimates the Oriented, Unique and Repetable Clustered Viewpoint Feature Histogram ...
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 filterNormalsWithHighCurvature(const pcl::PointCloud< PointNT > &cloud, pcl::Indices &indices_to_use, pcl::Indices &indices_out, pcl::Indices &indices_in, float threshold)
Removes normals with high curvature caused by real edges or noisy data.
void compute(PointCloudOut &output)
Overloaded computed method from pcl::Feature.
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.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
void resize(std::size_t count)
Resizes the container to contain count elements.
std::uint32_t width
The point cloud width (if organized as an image-structure).
pcl::PCLHeader header
The point cloud header.
std::uint32_t height
The point cloud height (if organized as an image-structure).
void clear()
Removes all points in a cloud and sets the width and height to 0.
shared_ptr< PointCloud< PointT > > Ptr
VFHEstimation estimates the Viewpoint Feature Histogram (VFH) descriptor for a given point cloud data...
void setUseGivenNormal(bool use)
Set use_given_normal_.
void setCentroidToUse(const Eigen::Vector3f ¢roid)
Set centroid_to_use_.
void compute(PointCloudOut &output)
Overloaded computed method from pcl::Feature.
void setNormalToUse(const Eigen::Vector3f &normal)
Set the normal to use.
void setNormalizeBins(bool normalize)
set normalize_bins_
void setUseGivenCentroid(bool use)
Set use_given_centroid_.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
virtual bool getSortedResults()
Gets whether the results should be sorted (ascending in the distance) or not Otherwise the results ma...
shared_ptr< pcl::search::Search< PointT > > Ptr
virtual PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
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.
Define standard C methods and C++ classes that are common to all methods.
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 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 transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Matrix< Scalar, 4, 4 > &transform, bool copy_all_fields)
Apply a rigid transform defined by a 4x4 matrix.
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 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.
IndicesAllocator<> Indices
Type used for indices in PCL.