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()));
102 std::vector<bool> processed (cloud.
size (),
false);
105 std::vector<float> nn_distances;
107 for (std::size_t i = 0; i < cloud.
size (); ++i)
112 std::vector<std::size_t> seed_queue;
113 std::size_t sq_idx = 0;
114 seed_queue.push_back (i);
118 while (sq_idx < seed_queue.size ())
121 if (!tree->
radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
127 for (std::size_t j = 1; j < nn_indices.size (); ++j)
129 if (processed[nn_indices[j]])
135 double dot_p = normals[seed_queue[sq_idx]].normal[0] * normals[nn_indices[j]].normal[0]
136 + normals[seed_queue[sq_idx]].normal[1] * normals[nn_indices[j]].normal[1] + normals[seed_queue[sq_idx]].normal[2]
137 * normals[nn_indices[j]].normal[2];
139 if (std::abs (std::acos (dot_p)) < eps_angle)
141 processed[nn_indices[j]] =
true;
150 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
153 r.
indices.resize (seed_queue.size ());
154 for (std::size_t j = 0; j < seed_queue.size (); ++j)
161 clusters.push_back (r);
167 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
173 indices_out.resize (cloud.
size ());
174 indices_in.resize (cloud.
size ());
179 for (
const auto &index : indices_to_use)
181 if (cloud[index].curvature > threshold)
183 indices_out[out] = index;
188 indices_in[in] = index;
193 indices_out.resize (out);
194 indices_in.resize (in);
197 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
bool
199 PointInTPtr & processed, std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > & transformations,
203 Eigen::Vector3f plane_normal;
204 plane_normal[0] = -centroid[0];
205 plane_normal[1] = -centroid[1];
206 plane_normal[2] = -centroid[2];
207 Eigen::Vector3f z_vector = Eigen::Vector3f::UnitZ ();
208 plane_normal.normalize ();
209 Eigen::Vector3f axis = plane_normal.cross (z_vector);
210 double rotation = -asin (axis.norm ());
213 Eigen::Affine3f transformPC (Eigen::AngleAxisf (
static_cast<float> (rotation), axis));
215 grid->resize (processed->size ());
216 for (std::size_t k = 0; k < processed->size (); k++)
217 (*grid)[k].getVector4fMap () = (*processed)[k].getVector4fMap ();
221 Eigen::Vector4f centroid4f (centroid[0], centroid[1], centroid[2], 0);
222 Eigen::Vector4f normal_centroid4f (normal_centroid[0], normal_centroid[1], normal_centroid[2], 0);
224 centroid4f = transformPC * centroid4f;
225 normal_centroid4f = transformPC * normal_centroid4f;
227 Eigen::Vector3f centroid3f (centroid4f[0], centroid4f[1], centroid4f[2]);
229 Eigen::Vector4f farthest_away;
231 farthest_away[3] = 0;
233 float max_dist = (farthest_away - centroid4f).norm ();
237 Eigen::Matrix4f center_mat;
238 center_mat.setIdentity (4, 4);
239 center_mat (0, 3) = -centroid4f[0];
240 center_mat (1, 3) = -centroid4f[1];
241 center_mat (2, 3) = -centroid4f[2];
243 Eigen::Matrix3f scatter;
247 for (
const auto &index : indices.
indices)
249 Eigen::Vector3f pvector = (*grid)[index].getVector3fMap ();
250 float d_k = (pvector).norm ();
251 float w = (max_dist - d_k);
252 Eigen::Vector3f diff = (pvector);
253 Eigen::Matrix3f mat = diff * diff.transpose ();
260 Eigen::JacobiSVD <Eigen::MatrixXf> svd (scatter, Eigen::ComputeFullV);
261 Eigen::Vector3f evx = svd.matrixV ().col (0);
262 Eigen::Vector3f evy = svd.matrixV ().col (1);
263 Eigen::Vector3f evz = svd.matrixV ().col (2);
264 Eigen::Vector3f evxminus = evx * -1;
265 Eigen::Vector3f evyminus = evy * -1;
266 Eigen::Vector3f evzminus = evz * -1;
268 float s_xplus, s_xminus, s_yplus, s_yminus;
269 s_xplus = s_xminus = s_yplus = s_yminus = 0.f;
272 for (
const auto& point: grid->points)
274 Eigen::Vector3f pvector = point.getVector3fMap ();
275 float dist_x, dist_y;
276 dist_x = std::abs (evx.dot (pvector));
277 dist_y = std::abs (evy.dot (pvector));
279 if ((pvector).dot (evx) >= 0)
284 if ((pvector).dot (evy) >= 0)
291 if (s_xplus < s_xminus)
294 if (s_yplus < s_yminus)
299 float max_x =
static_cast<float> (std::max (s_xplus, s_xminus));
300 float min_x =
static_cast<float> (std::min (s_xplus, s_xminus));
301 float max_y =
static_cast<float> (std::max (s_yplus, s_yminus));
302 float min_y =
static_cast<float> (std::min (s_yplus, s_yminus));
304 fx = (min_x / max_x);
305 fy = (min_y / max_y);
307 Eigen::Vector3f normal3f = Eigen::Vector3f (normal_centroid4f[0], normal_centroid4f[1], normal_centroid4f[2]);
308 if (normal3f.dot (evz) < 0)
314 float max_axis = std::max (fx, fy);
315 float min_axis = std::min (fx, fy);
317 if ((min_axis / max_axis) > axis_ratio_)
319 PCL_WARN (
"Both axes are equally easy/difficult to disambiguate\n");
321 Eigen::Vector3f evy_copy = evy;
322 Eigen::Vector3f evxminus = evx * -1;
323 Eigen::Vector3f evyminus = evy * -1;
325 if (min_axis > min_axis_value_)
328 evy = evx.cross (evz);
329 Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
330 transformations.push_back (trans);
333 evy = evx.cross (evz);
334 trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
335 transformations.push_back (trans);
338 evy = evx.cross (evz);
339 trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
340 transformations.push_back (trans);
343 evy = evx.cross (evz);
344 trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
345 transformations.push_back (trans);
351 evy = evx.cross (evz);
352 Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
353 transformations.push_back (trans);
357 evy = evx.cross (evz);
358 trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
359 transformations.push_back (trans);
370 evy = evx.cross (evz);
371 Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
372 transformations.push_back (trans);
380 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
382 std::vector<pcl::PointIndices> & cluster_indices)
386 cluster_axes_.
clear ();
387 cluster_axes_.resize (centroids_dominant_orientations_.size ());
389 for (std::size_t i = 0; i < centroids_dominant_orientations_.size (); i++)
392 std::vector < Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > transformations;
394 sgurf (centroids_dominant_orientations_[i], dominant_normals_[i], processed, transformations, grid, cluster_indices[i]);
397 cluster_axes_[i] = transformations.size ();
399 for (
const auto &transformation : transformations)
403 transforms_.push_back (transformation);
404 valid_transforms_.push_back (
true);
406 std::vector < Eigen::VectorXf > quadrants (8);
409 for (
int k = 0; k < num_hists; k++)
410 quadrants[k].setZero (size_hists);
412 Eigen::Vector4f centroid_p;
413 centroid_p.setZero ();
414 Eigen::Vector4f max_pt;
417 double distance_normalization_factor = (centroid_p - max_pt).norm ();
421 hist_incr = 100.0f /
static_cast<float> (grid->size () - 1);
425 float * weights =
new float[num_hists];
427 float sigma_sq = sigma * sigma;
429 for (
const auto& point: grid->points)
431 Eigen::Vector4f p = point.getVector4fMap ();
436 float wx = 1.f - std::exp (-((p[0] * p[0]) / (2.f * sigma_sq)));
437 float wy = 1.f - std::exp (-((p[1] * p[1]) / (2.f * sigma_sq)));
438 float wz = 1.f - std::exp (-((p[2] * p[2]) / (2.f * sigma_sq)));
443 for (std::size_t ii = 0; ii <= 3; ii++)
444 weights[ii] = 0.5f - wx * 0.5f;
446 for (std::size_t ii = 4; ii <= 7; ii++)
447 weights[ii] = 0.5f + wx * 0.5f;
451 for (std::size_t ii = 0; ii <= 3; ii++)
452 weights[ii] = 0.5f + wx * 0.5f;
454 for (std::size_t ii = 4; ii <= 7; ii++)
455 weights[ii] = 0.5f - wx * 0.5f;
461 for (std::size_t ii = 0; ii <= 1; ii++)
462 weights[ii] *= 0.5f - wy * 0.5f;
463 for (std::size_t ii = 4; ii <= 5; ii++)
464 weights[ii] *= 0.5f - wy * 0.5f;
466 for (std::size_t ii = 2; ii <= 3; ii++)
467 weights[ii] *= 0.5f + wy * 0.5f;
469 for (std::size_t ii = 6; ii <= 7; ii++)
470 weights[ii] *= 0.5f + wy * 0.5f;
474 for (std::size_t ii = 0; ii <= 1; ii++)
475 weights[ii] *= 0.5f + wy * 0.5f;
476 for (std::size_t ii = 4; ii <= 5; ii++)
477 weights[ii] *= 0.5f + wy * 0.5f;
479 for (std::size_t ii = 2; ii <= 3; ii++)
480 weights[ii] *= 0.5f - wy * 0.5f;
482 for (std::size_t ii = 6; ii <= 7; ii++)
483 weights[ii] *= 0.5f - wy * 0.5f;
489 for (std::size_t ii = 0; ii <= 7; ii += 2)
490 weights[ii] *= 0.5f - wz * 0.5f;
492 for (std::size_t ii = 1; ii <= 7; ii += 2)
493 weights[ii] *= 0.5f + wz * 0.5f;
498 for (std::size_t ii = 0; ii <= 7; ii += 2)
499 weights[ii] *= 0.5f + wz * 0.5f;
501 for (std::size_t ii = 1; ii <= 7; ii += 2)
502 weights[ii] *= 0.5f - wz * 0.5f;
505 int h_index = (d <= 0) ? 0 : std::ceil (size_hists * (d / distance_normalization_factor)) - 1;
513 for (
int j = 0; j < num_hists; j++)
514 quadrants[j][h_index] += hist_incr * weights[j];
522 for (
int d = 0; d < 308; ++d)
523 vfh_signature[0].histogram[d] = output[i].histogram[d];
526 for (
int k = 0; k < num_hists; k++)
528 for (
int ii = 0; ii < size_hists; ii++, pos++)
530 vfh_signature[0].histogram[pos] = quadrants[k][ii];
534 ourcvfh_output.
push_back (vfh_signature[0]);
535 ourcvfh_output.
width = ourcvfh_output.
size ();
540 if (!ourcvfh_output.
empty ())
542 ourcvfh_output.
height = 1;
544 output = ourcvfh_output;
548 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
551 if (refine_clusters_ <= 0.f)
552 refine_clusters_ = 1.f;
557 PCL_ERROR (
"[pcl::%s::computeFeature] No input dataset containing normals was given!\n", getClassName ().c_str ());
558 output.width = output.height = 0;
562 if (normals_->size () != surface_->size ())
564 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 ());
565 output.width = output.height = 0;
570 centroids_dominant_orientations_.clear ();
572 transforms_.clear ();
573 dominant_normals_.clear ();
578 filterNormalsWithHighCurvature (*normals_, *indices_, indices_out, indices_in, curv_threshold_);
581 normals_filtered_cloud->width = indices_in.size ();
582 normals_filtered_cloud->height = 1;
583 normals_filtered_cloud->points.resize (normals_filtered_cloud->width);
586 indices_from_nfc_to_indices.resize (indices_in.size ());
588 for (std::size_t i = 0; i < indices_in.size (); ++i)
590 (*normals_filtered_cloud)[i].x = (*surface_)[indices_in[i]].x;
591 (*normals_filtered_cloud)[i].y = (*surface_)[indices_in[i]].y;
592 (*normals_filtered_cloud)[i].z = (*surface_)[indices_in[i]].z;
594 indices_from_nfc_to_indices[i] = indices_in[i];
597 std::vector<pcl::PointIndices> clusters;
599 if (normals_filtered_cloud->size () >= min_points_)
604 normals_tree_filtered->setInputCloud (normals_filtered_cloud);
609 n3d.
compute (*normals_filtered_cloud);
613 normals_tree->setInputCloud (normals_filtered_cloud);
615 extractEuclideanClustersSmooth (*normals_filtered_cloud, *normals_filtered_cloud, cluster_tolerance_, normals_tree, clusters,
616 eps_angle_threshold_,
static_cast<unsigned int> (min_points_));
618 std::vector<pcl::PointIndices> clusters_filtered;
619 int cluster_filtered_idx = 0;
620 for (
const auto &cluster : clusters)
627 clusters_.push_back (pi);
628 clusters_filtered.push_back (pi_filtered);
630 Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
631 Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
633 for (
const auto &index : cluster.indices)
635 avg_normal += (*normals_filtered_cloud)[index].getNormalVector4fMap ();
636 avg_centroid += (*normals_filtered_cloud)[index].getVector4fMap ();
639 avg_normal /=
static_cast<float> (cluster.indices.size ());
640 avg_centroid /=
static_cast<float> (cluster.indices.size ());
641 avg_normal.normalize ();
643 Eigen::Vector3f avg_norm (avg_normal[0], avg_normal[1], avg_normal[2]);
644 Eigen::Vector3f avg_dominant_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
646 for (
const auto &index : cluster.indices)
649 double dot_p = avg_normal.dot ((*normals_filtered_cloud)[index].getNormalVector4fMap ());
650 if (std::abs (std::acos (dot_p)) < (eps_angle_threshold_ * refine_clusters_))
652 clusters_[cluster_filtered_idx].indices.push_back (indices_from_nfc_to_indices[index]);
653 clusters_filtered[cluster_filtered_idx].indices.push_back (index);
658 if (clusters_[cluster_filtered_idx].indices.empty ())
660 clusters_.pop_back ();
661 clusters_filtered.pop_back ();
664 cluster_filtered_idx++;
667 clusters = clusters_filtered;
682 if (!clusters.empty ())
684 for (
const auto &cluster : clusters)
686 Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
687 Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
689 for (
const auto &index : cluster.indices)
691 avg_normal += (*normals_filtered_cloud)[index].getNormalVector4fMap ();
692 avg_centroid += (*normals_filtered_cloud)[index].getVector4fMap ();
695 avg_normal /=
static_cast<float> (cluster.indices.size ());
696 avg_centroid /=
static_cast<float> (cluster.indices.size ());
697 avg_normal.normalize ();
700 dominant_normals_.emplace_back (avg_normal[0], avg_normal[1], avg_normal[2]);
701 centroids_dominant_orientations_.emplace_back (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
705 output.resize (dominant_normals_.size ());
706 output.width = dominant_normals_.size ();
708 for (std::size_t i = 0; i < dominant_normals_.size (); ++i)
715 output[i] = vfh_signature[0];
721 computeRFAndShapeDistribution (cloud_input, output, clusters_);
726 PCL_WARN(
"No clusters were found in the surface... using VFH...\n");
727 Eigen::Vector4f avg_centroid;
729 Eigen::Vector3f cloud_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
730 centroids_dominant_orientations_.push_back (cloud_centroid);
742 output[0] = vfh_signature[0];
743 Eigen::Matrix4f
id = Eigen::Matrix4f::Identity ();
744 transforms_.push_back (
id);
745 valid_transforms_.push_back (
false);
749 #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...
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.