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_
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point...
void compute(PointCloudOut &output)
Overloaded computed method from pcl::Feature.
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 setCentroidToUse(const Eigen::Vector3f ¢roid)
Set centroid_to_use_.
boost::shared_ptr< pcl::search::Search< PointT > > Ptr
void compute(PointCloudOut &output)
Overloaded computed method from pcl::Feature.
pcl::PCLHeader header
The point cloud header.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
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.
uint32_t height
The point cloud height (if organized as an image-structure).
boost::shared_ptr< PointCloud< PointT > > Ptr
VFHEstimation estimates the Viewpoint Feature Histogram (VFH) descriptor for a given point cloud data...
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 void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
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 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.
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset...
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.
virtual PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
OURCVFHEstimation estimates the Oriented, Unique and Repetable Clustered Viewpoint Feature Histogram ...
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
void setNormalToUse(const Eigen::Vector3f &normal)
Set the normal to use.
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.
std::vector< int > indices
Feature represents the base feature class.
uint32_t width
The point cloud width (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.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void setUseGivenNormal(bool use)
Set use_given_normal_.
void setNormalizeBins(bool normalize)
set normalize_bins_
void setSearchMethod(const KdTreePtr &tree)
Provide a pointer to the search object.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
pcl::PointCloud< PointInT >::Ptr PointInTPtr
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 computeRFAndShapeDistribution(PointInTPtr &processed, PointCloudOut &output, std::vector< pcl::PointIndices > &cluster_indices)
Computes SGURF and the shape distribution based on the selected SGURF.
void setUseGivenCentroid(bool use)
Set use_given_centroid_.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...