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 (%zu) than the input cloud (%zu)!\n", tree->
getInputCloud ()->points.size (), cloud.
points.size ());
90 PCL_ERROR (
"[pcl::extractEuclideanClusters] Number of points in the input point cloud (%zu) different than normals (%zu)!\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 axis 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)
379 for (
size_t i = 0; i < centroids_dominant_orientations_.size (); i++)
382 std::vector < Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > transformations;
384 sgurf (centroids_dominant_orientations_[i], dominant_normals_[i], processed, transformations, grid, cluster_indices[i]);
386 for (
size_t t = 0; t < transformations.size (); t++)
390 transforms_.push_back (transformations[t]);
391 valid_transforms_.push_back (
true);
393 std::vector < Eigen::VectorXf > quadrants (8);
396 for (
int k = 0; k < num_hists; k++)
397 quadrants[k].setZero (size_hists);
399 Eigen::Vector4f centroid_p;
400 centroid_p.setZero ();
401 Eigen::Vector4f max_pt;
404 double distance_normalization_factor = (centroid_p - max_pt).norm ();
408 hist_incr = 100.0f /
static_cast<float> (grid->points.size () - 1);
412 float * weights =
new float[num_hists];
414 float sigma_sq = sigma * sigma;
416 for (
int k = 0; k < static_cast<int> (grid->points.size ()); k++)
418 Eigen::Vector4f p = grid->points[k].getVector4fMap ();
423 float wx = 1.f - std::exp (-((p[0] * p[0]) / (2.f * sigma_sq)));
424 float wy = 1.f - std::exp (-((p[1] * p[1]) / (2.f * sigma_sq)));
425 float wz = 1.f - std::exp (-((p[2] * p[2]) / (2.f * sigma_sq)));
430 for (
size_t ii = 0; ii <= 3; ii++)
431 weights[ii] = 0.5f - wx * 0.5f;
433 for (
size_t ii = 4; ii <= 7; ii++)
434 weights[ii] = 0.5f + wx * 0.5f;
438 for (
size_t ii = 0; ii <= 3; ii++)
439 weights[ii] = 0.5f + wx * 0.5f;
441 for (
size_t ii = 4; ii <= 7; ii++)
442 weights[ii] = 0.5f - wx * 0.5f;
448 for (
size_t ii = 0; ii <= 1; ii++)
449 weights[ii] *= 0.5f - wy * 0.5f;
450 for (
size_t ii = 4; ii <= 5; ii++)
451 weights[ii] *= 0.5f - wy * 0.5f;
453 for (
size_t ii = 2; ii <= 3; ii++)
454 weights[ii] *= 0.5f + wy * 0.5f;
456 for (
size_t ii = 6; ii <= 7; ii++)
457 weights[ii] *= 0.5f + wy * 0.5f;
461 for (
size_t ii = 0; ii <= 1; ii++)
462 weights[ii] *= 0.5f + wy * 0.5f;
463 for (
size_t ii = 4; ii <= 5; ii++)
464 weights[ii] *= 0.5f + wy * 0.5f;
466 for (
size_t ii = 2; ii <= 3; ii++)
467 weights[ii] *= 0.5f - wy * 0.5f;
469 for (
size_t ii = 6; ii <= 7; ii++)
470 weights[ii] *= 0.5f - wy * 0.5f;
476 for (
size_t ii = 0; ii <= 7; ii += 2)
477 weights[ii] *= 0.5f - wz * 0.5f;
479 for (
size_t ii = 1; ii <= 7; ii += 2)
480 weights[ii] *= 0.5f + wz * 0.5f;
485 for (
size_t ii = 0; ii <= 7; ii += 2)
486 weights[ii] *= 0.5f + wz * 0.5f;
488 for (
size_t ii = 1; ii <= 7; ii += 2)
489 weights[ii] *= 0.5f - wz * 0.5f;
492 int h_index =
static_cast<int> (std::floor (size_hists * (d / distance_normalization_factor)));
493 for (
int j = 0; j < num_hists; j++)
494 quadrants[j][h_index] += hist_incr * weights[j];
500 vfh_signature.
points.resize (1);
502 for (
int d = 0; d < 308; ++d)
503 vfh_signature.
points[0].histogram[d] = output.
points[i].histogram[d];
506 for (
int k = 0; k < num_hists; k++)
508 for (
int ii = 0; ii < size_hists; ii++, pos++)
510 vfh_signature.
points[0].histogram[pos] = quadrants[k][ii];
514 ourcvfh_output.
points.push_back (vfh_signature.
points[0]);
520 output = ourcvfh_output;
524 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
527 if (refine_clusters_ <= 0.f)
528 refine_clusters_ = 1.f;
533 PCL_ERROR (
"[pcl::%s::computeFeature] No input dataset containing normals was given!\n", getClassName ().c_str ());
534 output.width = output.height = 0;
535 output.points.clear ();
538 if (normals_->points.size () != surface_->points.size ())
540 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 ());
541 output.width = output.height = 0;
542 output.points.clear ();
546 centroids_dominant_orientations_.clear ();
548 transforms_.clear ();
549 dominant_normals_.clear ();
552 std::vector<int> indices_out;
553 std::vector<int> indices_in;
554 filterNormalsWithHighCurvature (*normals_, *indices_, indices_out, indices_in, curv_threshold_);
557 normals_filtered_cloud->width =
static_cast<uint32_t
> (indices_in.size ());
558 normals_filtered_cloud->height = 1;
559 normals_filtered_cloud->points.resize (normals_filtered_cloud->width);
561 std::vector<int> indices_from_nfc_to_indices;
562 indices_from_nfc_to_indices.resize (indices_in.size ());
564 for (
size_t i = 0; i < indices_in.size (); ++i)
566 normals_filtered_cloud->points[i].x = surface_->points[indices_in[i]].x;
567 normals_filtered_cloud->points[i].y = surface_->points[indices_in[i]].y;
568 normals_filtered_cloud->points[i].z = surface_->points[indices_in[i]].z;
570 indices_from_nfc_to_indices[i] = indices_in[i];
573 std::vector<pcl::PointIndices> clusters;
575 if (normals_filtered_cloud->points.size () >= min_points_)
580 normals_tree_filtered->setInputCloud (normals_filtered_cloud);
585 n3d.
compute (*normals_filtered_cloud);
589 normals_tree->setInputCloud (normals_filtered_cloud);
591 extractEuclideanClustersSmooth (*normals_filtered_cloud, *normals_filtered_cloud, cluster_tolerance_, normals_tree, clusters,
592 eps_angle_threshold_, static_cast<unsigned int> (min_points_));
594 std::vector<pcl::PointIndices> clusters_filtered;
595 int cluster_filtered_idx = 0;
596 for (
size_t i = 0; i < clusters.size (); i++)
603 clusters_.push_back (pi);
604 clusters_filtered.push_back (pi_filtered);
606 Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
607 Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
609 for (
size_t j = 0; j < clusters[i].indices.size (); j++)
611 avg_normal += normals_filtered_cloud->points[clusters[i].indices[j]].getNormalVector4fMap ();
612 avg_centroid += normals_filtered_cloud->points[clusters[i].indices[j]].getVector4fMap ();
615 avg_normal /=
static_cast<float> (clusters[i].indices.size ());
616 avg_centroid /=
static_cast<float> (clusters[i].indices.size ());
617 avg_normal.normalize ();
619 Eigen::Vector3f avg_norm (avg_normal[0], avg_normal[1], avg_normal[2]);
620 Eigen::Vector3f avg_dominant_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
622 for (
size_t j = 0; j < clusters[i].indices.size (); j++)
625 double dot_p = avg_normal.dot (normals_filtered_cloud->points[clusters[i].indices[j]].getNormalVector4fMap ());
626 if (fabs (acos (dot_p)) < (eps_angle_threshold_ * refine_clusters_))
628 clusters_[cluster_filtered_idx].indices.push_back (indices_from_nfc_to_indices[clusters[i].indices[j]]);
629 clusters_filtered[cluster_filtered_idx].indices.push_back (clusters[i].indices[j]);
634 if (clusters_[cluster_filtered_idx].indices.size () == 0)
636 clusters_.erase (clusters_.end ());
637 clusters_filtered.erase (clusters_filtered.end ());
640 cluster_filtered_idx++;
643 clusters = clusters_filtered;
658 if (clusters.size () > 0)
661 for (
size_t i = 0; i < clusters.size (); ++i)
664 Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
665 Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
667 for (
size_t j = 0; j < clusters[i].indices.size (); j++)
669 avg_normal += normals_filtered_cloud->points[clusters[i].indices[j]].getNormalVector4fMap ();
670 avg_centroid += normals_filtered_cloud->points[clusters[i].indices[j]].getVector4fMap ();
673 avg_normal /=
static_cast<float> (clusters[i].indices.size ());
674 avg_centroid /=
static_cast<float> (clusters[i].indices.size ());
675 avg_normal.normalize ();
677 Eigen::Vector3f avg_norm (avg_normal[0], avg_normal[1], avg_normal[2]);
678 Eigen::Vector3f avg_dominant_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
681 dominant_normals_.push_back (avg_norm);
682 centroids_dominant_orientations_.push_back (avg_dominant_centroid);
686 output.points.resize (dominant_normals_.size ());
687 output.width =
static_cast<uint32_t
> (dominant_normals_.size ());
689 for (
size_t i = 0; i < dominant_normals_.size (); ++i)
696 output.points[i] = vfh_signature.
points[0];
702 computeRFAndShapeDistribution (cloud_input, output, clusters_);
707 PCL_WARN(
"No clusters were found in the surface... using VFH...\n");
708 Eigen::Vector4f avg_centroid;
710 Eigen::Vector3f cloud_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
711 centroids_dominant_orientations_.push_back (cloud_centroid);
720 output.points.resize (1);
723 output.points[0] = vfh_signature.
points[0];
724 Eigen::Matrix4f
id = Eigen::Matrix4f::Identity ();
725 transforms_.push_back (
id);
726 valid_transforms_.push_back (
false);
730 #define PCL_INSTANTIATE_OURCVFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::OURCVFHEstimation<T,NT,OutT>;
732 #endif // PCL_FEATURES_IMPL_OURCVFH_H_
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.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Feature represents the base feature class.
OURCVFHEstimation estimates the Oriented, Unique and Repetable Clustered Viewpoint Feature Histogram ...
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
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.
pcl::PCLHeader header
The point cloud header.
void compute(PointCloudOut &output)
Overloaded computed method from pcl::Feature.
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.
uint32_t width
The point cloud width (if organized as an image-structure).
void setNormalizeBins(bool normalize)
set normalize_bins_
void computeRFAndShapeDistribution(PointInTPtr &processed, PointCloudOut &output, std::vector< pcl::PointIndices > &cluster_indices)
Computes SGURF and the shape distribution based on the selected SGURF.
void setUseGivenNormal(bool use)
Set use_given_normal_.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
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 setUseGivenCentroid(bool use)
Set use_given_centroid_.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
boost::shared_ptr< pcl::search::Search< PointT > > Ptr
boost::shared_ptr< PointCloud< PointT > > Ptr
uint32_t height
The point cloud height (if organized as an image-structure).
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
void compute(PointCloudOut &output)
Overloaded computed method from pcl::Feature.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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...
pcl::PointCloud< PointInT >::Ptr PointInTPtr
VFHEstimation estimates the Viewpoint Feature Histogram (VFH) descriptor for a given point cloud data...
virtual PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
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.
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 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.
std::vector< int > indices
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point...
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 setCentroidToUse(const Eigen::Vector3f ¢roid)
Set centroid_to_use_.
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset...
void setNormalToUse(const Eigen::Vector3f &normal)
Set the normal to use.