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
56 output.width = output.height = 0;
57 output.points.clear ();
64 output.width = output.height = 1;
65 output.points.resize (1);
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 (std::size_t i = 0; i < cloud.
points.size (); ++i)
105 std::vector<std::size_t> seed_queue;
106 std::size_t sq_idx = 0;
107 seed_queue.push_back (i);
111 while (sq_idx < seed_queue.size ())
114 if (!tree->
radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
120 for (std::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 (std::abs (std::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 (std::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 (
const int &index : indices_to_use)
174 if (cloud.
points[index].curvature > threshold)
176 indices_out[out] = index;
181 indices_in[in] = index;
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 (std::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;
240 for (
const int &index : indices.
indices)
242 Eigen::Vector3f pvector = grid->points[index].getVector3fMap ();
243 float d_k = (pvector).norm ();
244 float w = (max_dist - d_k);
245 Eigen::Vector3f diff = (pvector);
246 Eigen::Matrix3f mat = diff * diff.transpose ();
253 Eigen::JacobiSVD <Eigen::MatrixXf> svd (scatter, Eigen::ComputeFullV);
254 Eigen::Vector3f evx = svd.matrixV ().col (0);
255 Eigen::Vector3f evy = svd.matrixV ().col (1);
256 Eigen::Vector3f evz = svd.matrixV ().col (2);
257 Eigen::Vector3f evxminus = evx * -1;
258 Eigen::Vector3f evyminus = evy * -1;
259 Eigen::Vector3f evzminus = evz * -1;
261 float s_xplus, s_xminus, s_yplus, s_yminus;
262 s_xplus = s_xminus = s_yplus = s_yminus = 0.f;
265 for (
const auto& point: grid->points)
267 Eigen::Vector3f pvector = point.getVector3fMap ();
268 float dist_x, dist_y;
269 dist_x = std::abs (evx.dot (pvector));
270 dist_y = std::abs (evy.dot (pvector));
272 if ((pvector).dot (evx) >= 0)
277 if ((pvector).dot (evy) >= 0)
284 if (s_xplus < s_xminus)
287 if (s_yplus < s_yminus)
292 float max_x =
static_cast<float> (std::max (s_xplus, s_xminus));
293 float min_x =
static_cast<float> (std::min (s_xplus, s_xminus));
294 float max_y =
static_cast<float> (std::max (s_yplus, s_yminus));
295 float min_y =
static_cast<float> (std::min (s_yplus, s_yminus));
297 fx = (min_x / max_x);
298 fy = (min_y / max_y);
300 Eigen::Vector3f normal3f = Eigen::Vector3f (normal_centroid4f[0], normal_centroid4f[1], normal_centroid4f[2]);
301 if (normal3f.dot (evz) < 0)
307 float max_axis = std::max (fx, fy);
308 float min_axis = std::min (fx, fy);
310 if ((min_axis / max_axis) > axis_ratio_)
312 PCL_WARN (
"Both axes are equally easy/difficult to disambiguate\n");
314 Eigen::Vector3f evy_copy = evy;
315 Eigen::Vector3f evxminus = evx * -1;
316 Eigen::Vector3f evyminus = evy * -1;
318 if (min_axis > min_axis_value_)
321 evy = evx.cross (evz);
322 Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
323 transformations.push_back (trans);
326 evy = evx.cross (evz);
327 trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
328 transformations.push_back (trans);
331 evy = evx.cross (evz);
332 trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
333 transformations.push_back (trans);
336 evy = evx.cross (evz);
337 trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
338 transformations.push_back (trans);
344 evy = evx.cross (evz);
345 Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
346 transformations.push_back (trans);
350 evy = evx.cross (evz);
351 trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
352 transformations.push_back (trans);
363 evy = evx.cross (evz);
364 Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
365 transformations.push_back (trans);
373 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
375 std::vector<pcl::PointIndices> & cluster_indices)
379 cluster_axes_.clear ();
380 cluster_axes_.resize (centroids_dominant_orientations_.size ());
382 for (std::size_t i = 0; i < centroids_dominant_orientations_.size (); i++)
385 std::vector < Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > transformations;
387 sgurf (centroids_dominant_orientations_[i], dominant_normals_[i], processed, transformations, grid, cluster_indices[i]);
390 cluster_axes_[i] = transformations.size ();
392 for (
const auto &transformation : transformations)
396 transforms_.push_back (transformation);
397 valid_transforms_.push_back (
true);
399 std::vector < Eigen::VectorXf > quadrants (8);
402 for (
int k = 0; k < num_hists; k++)
403 quadrants[k].setZero (size_hists);
405 Eigen::Vector4f centroid_p;
406 centroid_p.setZero ();
407 Eigen::Vector4f max_pt;
410 double distance_normalization_factor = (centroid_p - max_pt).norm ();
414 hist_incr = 100.0f /
static_cast<float> (grid->points.size () - 1);
418 float * weights =
new float[num_hists];
420 float sigma_sq = sigma * sigma;
422 for (
const auto& point: grid->points)
424 Eigen::Vector4f p = point.getVector4fMap ();
429 float wx = 1.f - std::exp (-((p[0] * p[0]) / (2.f * sigma_sq)));
430 float wy = 1.f - std::exp (-((p[1] * p[1]) / (2.f * sigma_sq)));
431 float wz = 1.f - std::exp (-((p[2] * p[2]) / (2.f * sigma_sq)));
436 for (std::size_t ii = 0; ii <= 3; ii++)
437 weights[ii] = 0.5f - wx * 0.5f;
439 for (std::size_t ii = 4; ii <= 7; ii++)
440 weights[ii] = 0.5f + wx * 0.5f;
444 for (std::size_t ii = 0; ii <= 3; ii++)
445 weights[ii] = 0.5f + wx * 0.5f;
447 for (std::size_t ii = 4; ii <= 7; ii++)
448 weights[ii] = 0.5f - wx * 0.5f;
454 for (std::size_t ii = 0; ii <= 1; ii++)
455 weights[ii] *= 0.5f - wy * 0.5f;
456 for (std::size_t ii = 4; ii <= 5; ii++)
457 weights[ii] *= 0.5f - wy * 0.5f;
459 for (std::size_t ii = 2; ii <= 3; ii++)
460 weights[ii] *= 0.5f + wy * 0.5f;
462 for (std::size_t ii = 6; ii <= 7; ii++)
463 weights[ii] *= 0.5f + wy * 0.5f;
467 for (std::size_t ii = 0; ii <= 1; ii++)
468 weights[ii] *= 0.5f + wy * 0.5f;
469 for (std::size_t ii = 4; ii <= 5; ii++)
470 weights[ii] *= 0.5f + wy * 0.5f;
472 for (std::size_t ii = 2; ii <= 3; ii++)
473 weights[ii] *= 0.5f - wy * 0.5f;
475 for (std::size_t ii = 6; ii <= 7; ii++)
476 weights[ii] *= 0.5f - wy * 0.5f;
482 for (std::size_t ii = 0; ii <= 7; ii += 2)
483 weights[ii] *= 0.5f - wz * 0.5f;
485 for (std::size_t ii = 1; ii <= 7; ii += 2)
486 weights[ii] *= 0.5f + wz * 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;
498 int h_index = (d <= 0) ? 0 : std::ceil (size_hists * (d / distance_normalization_factor)) - 1;
506 for (
int j = 0; j < num_hists; j++)
507 quadrants[j][h_index] += hist_incr * weights[j];
513 vfh_signature.points.resize (1);
514 vfh_signature.width = vfh_signature.height = 1;
515 for (
int d = 0; d < 308; ++d)
516 vfh_signature.points[0].histogram[d] = output.points[i].histogram[d];
519 for (
int k = 0; k < num_hists; k++)
521 for (
int ii = 0; ii < size_hists; ii++, pos++)
523 vfh_signature.points[0].histogram[pos] = quadrants[k][ii];
527 ourcvfh_output.points.push_back (vfh_signature.points[0]);
528 ourcvfh_output.width = ourcvfh_output.points.size ();
533 if (!ourcvfh_output.points.empty ())
535 ourcvfh_output.height = 1;
537 output = ourcvfh_output;
541 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
544 if (refine_clusters_ <= 0.f)
545 refine_clusters_ = 1.f;
550 PCL_ERROR (
"[pcl::%s::computeFeature] No input dataset containing normals was given!\n", getClassName ().c_str ());
551 output.width = output.height = 0;
552 output.points.clear ();
555 if (normals_->points.size () != surface_->points.size ())
557 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 ());
558 output.width = output.height = 0;
559 output.points.clear ();
563 centroids_dominant_orientations_.clear ();
565 transforms_.clear ();
566 dominant_normals_.clear ();
569 std::vector<int> indices_out;
570 std::vector<int> indices_in;
571 filterNormalsWithHighCurvature (*normals_, *indices_, indices_out, indices_in, curv_threshold_);
574 normals_filtered_cloud->width =
static_cast<std::uint32_t
> (indices_in.size ());
575 normals_filtered_cloud->height = 1;
576 normals_filtered_cloud->points.resize (normals_filtered_cloud->width);
578 std::vector<int> indices_from_nfc_to_indices;
579 indices_from_nfc_to_indices.resize (indices_in.size ());
581 for (std::size_t i = 0; i < indices_in.size (); ++i)
583 normals_filtered_cloud->points[i].x = surface_->points[indices_in[i]].x;
584 normals_filtered_cloud->points[i].y = surface_->points[indices_in[i]].y;
585 normals_filtered_cloud->points[i].z = surface_->points[indices_in[i]].z;
587 indices_from_nfc_to_indices[i] = indices_in[i];
590 std::vector<pcl::PointIndices> clusters;
592 if (normals_filtered_cloud->points.size () >= min_points_)
597 normals_tree_filtered->setInputCloud (normals_filtered_cloud);
602 n3d.
compute (*normals_filtered_cloud);
606 normals_tree->setInputCloud (normals_filtered_cloud);
608 extractEuclideanClustersSmooth (*normals_filtered_cloud, *normals_filtered_cloud, cluster_tolerance_, normals_tree, clusters,
609 eps_angle_threshold_,
static_cast<unsigned int> (min_points_));
611 std::vector<pcl::PointIndices> clusters_filtered;
612 int cluster_filtered_idx = 0;
613 for (
const auto &cluster : clusters)
620 clusters_.push_back (pi);
621 clusters_filtered.push_back (pi_filtered);
623 Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
624 Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
626 for (
const auto &index : cluster.indices)
628 avg_normal += normals_filtered_cloud->points[index].getNormalVector4fMap ();
629 avg_centroid += normals_filtered_cloud->points[index].getVector4fMap ();
632 avg_normal /=
static_cast<float> (cluster.indices.size ());
633 avg_centroid /=
static_cast<float> (cluster.indices.size ());
634 avg_normal.normalize ();
636 Eigen::Vector3f avg_norm (avg_normal[0], avg_normal[1], avg_normal[2]);
637 Eigen::Vector3f avg_dominant_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
639 for (
const auto &index : cluster.indices)
642 double dot_p = avg_normal.dot (normals_filtered_cloud->points[index].getNormalVector4fMap ());
643 if (std::abs (std::acos (dot_p)) < (eps_angle_threshold_ * refine_clusters_))
645 clusters_[cluster_filtered_idx].indices.push_back (indices_from_nfc_to_indices[index]);
646 clusters_filtered[cluster_filtered_idx].indices.push_back (index);
651 if (clusters_[cluster_filtered_idx].indices.empty ())
653 clusters_.pop_back ();
654 clusters_filtered.pop_back ();
657 cluster_filtered_idx++;
660 clusters = clusters_filtered;
675 if (!clusters.empty ())
677 for (
const auto &cluster : clusters)
679 Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
680 Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
682 for (
const auto &index : cluster.indices)
684 avg_normal += normals_filtered_cloud->points[index].getNormalVector4fMap ();
685 avg_centroid += normals_filtered_cloud->points[index].getVector4fMap ();
688 avg_normal /=
static_cast<float> (cluster.indices.size ());
689 avg_centroid /=
static_cast<float> (cluster.indices.size ());
690 avg_normal.normalize ();
693 dominant_normals_.emplace_back (avg_normal[0], avg_normal[1], avg_normal[2]);
694 centroids_dominant_orientations_.emplace_back (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
698 output.points.resize (dominant_normals_.size ());
699 output.width =
static_cast<std::uint32_t
> (dominant_normals_.size ());
701 for (std::size_t i = 0; i < dominant_normals_.size (); ++i)
708 output.points[i] = vfh_signature.
points[0];
714 computeRFAndShapeDistribution (cloud_input, output, clusters_);
719 PCL_WARN(
"No clusters were found in the surface... using VFH...\n");
720 Eigen::Vector4f avg_centroid;
722 Eigen::Vector3f cloud_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
723 centroids_dominant_orientations_.push_back (cloud_centroid);
732 output.points.resize (1);
735 output.points[0] = vfh_signature.
points[0];
736 Eigen::Matrix4f
id = Eigen::Matrix4f::Identity ();
737 transforms_.push_back (
id);
738 valid_transforms_.push_back (
false);
742 #define PCL_INSTANTIATE_OURCVFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::OURCVFHEstimation<T,NT,OutT>;
744 #endif // PCL_FEATURES_IMPL_OURCVFH_H_