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 (
int i = 0; i < static_cast<int> (cloud.
points.size ()); ++i)
105 std::vector<unsigned int> seed_queue;
107 seed_queue.push_back (i);
111 while (sq_idx < static_cast<int> (seed_queue.size ()))
114 if (!tree->
radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
120 for (
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 (fabs (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 (
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 (
int i = 0; i < static_cast<int> (indices_to_use.size ()); i++)
174 if (cloud.
points[indices_to_use[i]].curvature > threshold)
176 indices_out[out] = indices_to_use[i];
181 indices_in[in] = indices_to_use[i];
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 (
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;
241 for (
int k = 0; k < static_cast<int> (indices.
indices.size ()); k++)
243 Eigen::Vector3f pvector = grid->points[indices.
indices[k]].getVector3fMap ();
244 float d_k = (pvector).norm ();
245 float w = (max_dist - d_k);
246 Eigen::Vector3f diff = (pvector);
247 Eigen::Matrix3f mat = diff * diff.transpose ();
248 scatter = scatter + mat * w;
254 Eigen::JacobiSVD <Eigen::MatrixXf> svd (scatter, Eigen::ComputeFullV);
255 Eigen::Vector3f evx = svd.matrixV ().col (0);
256 Eigen::Vector3f evy = svd.matrixV ().col (1);
257 Eigen::Vector3f evz = svd.matrixV ().col (2);
258 Eigen::Vector3f evxminus = evx * -1;
259 Eigen::Vector3f evyminus = evy * -1;
260 Eigen::Vector3f evzminus = evz * -1;
262 float s_xplus, s_xminus, s_yplus, s_yminus;
263 s_xplus = s_xminus = s_yplus = s_yminus = 0.f;
266 for (
int k = 0; k < static_cast<int> (grid->points.size ()); k++)
268 Eigen::Vector3f pvector = grid->points[k].getVector3fMap ();
269 float dist_x, dist_y;
270 dist_x = std::abs (evx.dot (pvector));
271 dist_y = std::abs (evy.dot (pvector));
273 if ((pvector).dot (evx) >= 0)
278 if ((pvector).dot (evy) >= 0)
285 if (s_xplus < s_xminus)
288 if (s_yplus < s_yminus)
293 float max_x =
static_cast<float> (std::max (s_xplus, s_xminus));
294 float min_x =
static_cast<float> (std::min (s_xplus, s_xminus));
295 float max_y =
static_cast<float> (std::max (s_yplus, s_yminus));
296 float min_y =
static_cast<float> (std::min (s_yplus, s_yminus));
298 fx = (min_x / max_x);
299 fy = (min_y / max_y);
301 Eigen::Vector3f normal3f = Eigen::Vector3f (normal_centroid4f[0], normal_centroid4f[1], normal_centroid4f[2]);
302 if (normal3f.dot (evz) < 0)
308 float max_axis = std::max (fx, fy);
309 float min_axis = std::min (fx, fy);
311 if ((min_axis / max_axis) > axis_ratio_)
313 PCL_WARN (
"Both axes are equally easy/difficult to disambiguate\n");
315 Eigen::Vector3f evy_copy = evy;
316 Eigen::Vector3f evxminus = evx * -1;
317 Eigen::Vector3f evyminus = evy * -1;
319 if (min_axis > min_axis_value_)
322 evy = evx.cross (evz);
323 Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
324 transformations.push_back (trans);
327 evy = evx.cross (evz);
328 trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
329 transformations.push_back (trans);
332 evy = evx.cross (evz);
333 trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
334 transformations.push_back (trans);
337 evy = evx.cross (evz);
338 trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
339 transformations.push_back (trans);
345 evy = evx.cross (evz);
346 Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
347 transformations.push_back (trans);
351 evy = evx.cross (evz);
352 trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
353 transformations.push_back (trans);
364 evy = evx.cross (evz);
365 Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
366 transformations.push_back (trans);
374 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
376 std::vector<pcl::PointIndices> & cluster_indices)
380 cluster_axes_.
clear ();
381 cluster_axes_.resize (centroids_dominant_orientations_.size ());
383 for (
size_t i = 0; i < centroids_dominant_orientations_.size (); i++)
386 std::vector < Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > transformations;
388 sgurf (centroids_dominant_orientations_[i], dominant_normals_[i], processed, transformations, grid, cluster_indices[i]);
391 cluster_axes_[i] = transformations.size ();
393 for (
size_t t = 0; t < transformations.size (); t++)
397 transforms_.push_back (transformations[t]);
398 valid_transforms_.push_back (
true);
400 std::vector < Eigen::VectorXf > quadrants (8);
403 for (
int k = 0; k < num_hists; k++)
404 quadrants[k].setZero (size_hists);
406 Eigen::Vector4f centroid_p;
407 centroid_p.setZero ();
408 Eigen::Vector4f max_pt;
411 double distance_normalization_factor = (centroid_p - max_pt).norm ();
415 hist_incr = 100.0f /
static_cast<float> (grid->points.size () - 1);
419 float * weights =
new float[num_hists];
421 float sigma_sq = sigma * sigma;
423 for (
int k = 0; k < static_cast<int> (grid->points.size ()); k++)
425 Eigen::Vector4f p = grid->points[k].getVector4fMap ();
430 float wx = 1.f - std::exp (-((p[0] * p[0]) / (2.f * sigma_sq)));
431 float wy = 1.f - std::exp (-((p[1] * p[1]) / (2.f * sigma_sq)));
432 float wz = 1.f - std::exp (-((p[2] * p[2]) / (2.f * sigma_sq)));
437 for (
size_t ii = 0; ii <= 3; ii++)
438 weights[ii] = 0.5f - wx * 0.5f;
440 for (
size_t ii = 4; ii <= 7; ii++)
441 weights[ii] = 0.5f + wx * 0.5f;
445 for (
size_t ii = 0; ii <= 3; ii++)
446 weights[ii] = 0.5f + wx * 0.5f;
448 for (
size_t ii = 4; ii <= 7; ii++)
449 weights[ii] = 0.5f - wx * 0.5f;
455 for (
size_t ii = 0; ii <= 1; ii++)
456 weights[ii] *= 0.5f - wy * 0.5f;
457 for (
size_t ii = 4; ii <= 5; ii++)
458 weights[ii] *= 0.5f - wy * 0.5f;
460 for (
size_t ii = 2; ii <= 3; ii++)
461 weights[ii] *= 0.5f + wy * 0.5f;
463 for (
size_t ii = 6; ii <= 7; ii++)
464 weights[ii] *= 0.5f + wy * 0.5f;
468 for (
size_t ii = 0; ii <= 1; ii++)
469 weights[ii] *= 0.5f + wy * 0.5f;
470 for (
size_t ii = 4; ii <= 5; ii++)
471 weights[ii] *= 0.5f + wy * 0.5f;
473 for (
size_t ii = 2; ii <= 3; ii++)
474 weights[ii] *= 0.5f - wy * 0.5f;
476 for (
size_t ii = 6; ii <= 7; ii++)
477 weights[ii] *= 0.5f - wy * 0.5f;
483 for (
size_t ii = 0; ii <= 7; ii += 2)
484 weights[ii] *= 0.5f - wz * 0.5f;
486 for (
size_t ii = 1; ii <= 7; ii += 2)
487 weights[ii] *= 0.5f + wz * 0.5f;
492 for (
size_t ii = 0; ii <= 7; ii += 2)
493 weights[ii] *= 0.5f + wz * 0.5f;
495 for (
size_t ii = 1; ii <= 7; ii += 2)
496 weights[ii] *= 0.5f - wz * 0.5f;
499 int h_index = (d <= 0) ? 0 : std::ceil (size_hists * (d / distance_normalization_factor)) - 1;
500 for (
int j = 0; j < num_hists; j++)
501 quadrants[j][h_index] += hist_incr * weights[j];
507 vfh_signature.
points.resize (1);
509 for (
int d = 0; d < 308; ++d)
510 vfh_signature.
points[0].histogram[d] = output.
points[i].histogram[d];
513 for (
int k = 0; k < num_hists; k++)
515 for (
int ii = 0; ii < size_hists; ii++, pos++)
517 vfh_signature.
points[0].histogram[pos] = quadrants[k][ii];
521 ourcvfh_output.
points.push_back (vfh_signature.
points[0]);
522 ourcvfh_output.
width = ourcvfh_output.
points.size ();
527 if (ourcvfh_output.
points.size ())
529 ourcvfh_output.
height = 1;
531 output = ourcvfh_output;
535 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
538 if (refine_clusters_ <= 0.f)
539 refine_clusters_ = 1.f;
544 PCL_ERROR (
"[pcl::%s::computeFeature] No input dataset containing normals was given!\n", getClassName ().c_str ());
545 output.width = output.height = 0;
546 output.points.clear ();
549 if (normals_->points.size () != surface_->points.size ())
551 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 ());
552 output.width = output.height = 0;
553 output.points.clear ();
557 centroids_dominant_orientations_.clear ();
559 transforms_.clear ();
560 dominant_normals_.clear ();
563 std::vector<int> indices_out;
564 std::vector<int> indices_in;
565 filterNormalsWithHighCurvature (*normals_, *indices_, indices_out, indices_in, curv_threshold_);
568 normals_filtered_cloud->width =
static_cast<uint32_t
> (indices_in.size ());
569 normals_filtered_cloud->height = 1;
570 normals_filtered_cloud->points.resize (normals_filtered_cloud->width);
572 std::vector<int> indices_from_nfc_to_indices;
573 indices_from_nfc_to_indices.resize (indices_in.size ());
575 for (
size_t i = 0; i < indices_in.size (); ++i)
577 normals_filtered_cloud->points[i].x = surface_->points[indices_in[i]].x;
578 normals_filtered_cloud->points[i].y = surface_->points[indices_in[i]].y;
579 normals_filtered_cloud->points[i].z = surface_->points[indices_in[i]].z;
581 indices_from_nfc_to_indices[i] = indices_in[i];
584 std::vector<pcl::PointIndices> clusters;
586 if (normals_filtered_cloud->points.size () >= min_points_)
591 normals_tree_filtered->setInputCloud (normals_filtered_cloud);
596 n3d.
compute (*normals_filtered_cloud);
600 normals_tree->setInputCloud (normals_filtered_cloud);
602 extractEuclideanClustersSmooth (*normals_filtered_cloud, *normals_filtered_cloud, cluster_tolerance_, normals_tree, clusters,
603 eps_angle_threshold_, static_cast<unsigned int> (min_points_));
605 std::vector<pcl::PointIndices> clusters_filtered;
606 int cluster_filtered_idx = 0;
607 for (
size_t i = 0; i < clusters.size (); i++)
614 clusters_.push_back (pi);
615 clusters_filtered.push_back (pi_filtered);
617 Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
618 Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
620 for (
size_t j = 0; j < clusters[i].indices.size (); j++)
622 avg_normal += normals_filtered_cloud->points[clusters[i].indices[j]].getNormalVector4fMap ();
623 avg_centroid += normals_filtered_cloud->points[clusters[i].indices[j]].getVector4fMap ();
626 avg_normal /=
static_cast<float> (clusters[i].indices.size ());
627 avg_centroid /=
static_cast<float> (clusters[i].indices.size ());
628 avg_normal.normalize ();
630 Eigen::Vector3f avg_norm (avg_normal[0], avg_normal[1], avg_normal[2]);
631 Eigen::Vector3f avg_dominant_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
633 for (
size_t j = 0; j < clusters[i].indices.size (); j++)
636 double dot_p = avg_normal.dot (normals_filtered_cloud->points[clusters[i].indices[j]].getNormalVector4fMap ());
637 if (fabs (acos (dot_p)) < (eps_angle_threshold_ * refine_clusters_))
639 clusters_[cluster_filtered_idx].indices.push_back (indices_from_nfc_to_indices[clusters[i].indices[j]]);
640 clusters_filtered[cluster_filtered_idx].indices.push_back (clusters[i].indices[j]);
645 if (clusters_[cluster_filtered_idx].indices.size () == 0)
647 clusters_.pop_back ();
648 clusters_filtered.pop_back ();
651 cluster_filtered_idx++;
654 clusters = clusters_filtered;
669 if (clusters.size () > 0)
672 for (
size_t i = 0; i < clusters.size (); ++i)
675 Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
676 Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
678 for (
size_t j = 0; j < clusters[i].indices.size (); j++)
680 avg_normal += normals_filtered_cloud->points[clusters[i].indices[j]].getNormalVector4fMap ();
681 avg_centroid += normals_filtered_cloud->points[clusters[i].indices[j]].getVector4fMap ();
684 avg_normal /=
static_cast<float> (clusters[i].indices.size ());
685 avg_centroid /=
static_cast<float> (clusters[i].indices.size ());
686 avg_normal.normalize ();
688 Eigen::Vector3f avg_norm (avg_normal[0], avg_normal[1], avg_normal[2]);
689 Eigen::Vector3f avg_dominant_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
692 dominant_normals_.push_back (avg_norm);
693 centroids_dominant_orientations_.push_back (avg_dominant_centroid);
697 output.points.resize (dominant_normals_.size ());
698 output.width =
static_cast<uint32_t
> (dominant_normals_.size ());
700 for (
size_t i = 0; i < dominant_normals_.size (); ++i)
707 output.points[i] = vfh_signature.
points[0];
713 computeRFAndShapeDistribution (cloud_input, output, clusters_);
718 PCL_WARN(
"No clusters were found in the surface... using VFH...\n");
719 Eigen::Vector4f avg_centroid;
721 Eigen::Vector3f cloud_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
722 centroids_dominant_orientations_.push_back (cloud_centroid);
731 output.points.resize (1);
734 output.points[0] = vfh_signature.
points[0];
735 Eigen::Matrix4f
id = Eigen::Matrix4f::Identity ();
736 transforms_.push_back (
id);
737 valid_transforms_.push_back (
false);
741 #define PCL_INSTANTIATE_OURCVFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::OURCVFHEstimation<T,NT,OutT>;
743 #endif // PCL_FEATURES_IMPL_OURCVFH_H_
void setUseGivenCentroid(bool use)
Set use_given_centroid_.
pcl::PointCloud< PointInT >::Ptr PointInTPtr
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...
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
virtual int radiusSearch(const PointT &point, double radius, std::vector< int > &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.
std::vector< int > indices
void computeRFAndShapeDistribution(PointInTPtr &processed, PointCloudOut &output, std::vector< pcl::PointIndices > &cluster_indices)
Computes SGURF and the shape distribution based on the selected SGURF.
PCL_EXPORTS void copyPointCloud(const pcl::PCLPointCloud2 &cloud_in, const std::vector< int > &indices, pcl::PCLPointCloud2 &cloud_out)
Extract the indices of a given point cloud as a new point cloud.
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 transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform)
Apply an affine transform defined by an Eigen Transform.
void setUseGivenNormal(bool use)
Set use_given_normal_.
uint32_t height
The point cloud height (if organized as an image-structure).
boost::shared_ptr< PointCloud< PointT > > Ptr
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_.
boost::shared_ptr< pcl::search::Search< PointT > > Ptr
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...
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
void setNormalizeBins(bool normalize)
set normalize_bins_
PointCloud represents the base class in PCL for storing collections of 3D points. ...
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.
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 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 ...