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/transforms.h>
50 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
55 output.width = output.height = 0;
56 output.points.clear ();
63 output.width = output.height = 1;
64 output.points.resize (1);
67 computeFeature (output);
73 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
78 std::vector<pcl::PointIndices> &clusters,
double eps_angle,
79 unsigned int min_pts_per_cluster,
80 unsigned int max_pts_per_cluster)
84 PCL_ERROR(
"[pcl::extractEuclideanClusters] Tree built for a different point cloud "
85 "dataset (%zu) than the input cloud (%zu)!\n",
87 static_cast<std::size_t
>(cloud.
size()));
90 if (cloud.
size () != normals.
size ())
92 PCL_ERROR(
"[pcl::extractEuclideanClusters] Number of points in the input point "
93 "cloud (%zu) different than normals (%zu)!\n",
94 static_cast<std::size_t
>(cloud.
size()),
95 static_cast<std::size_t
>(normals.
size()));
100 std::vector<bool> processed (cloud.
size (),
false);
102 std::vector<int> nn_indices;
103 std::vector<float> nn_distances;
105 for (std::size_t i = 0; i < cloud.
size (); ++i)
110 std::vector<std::size_t> seed_queue;
111 std::size_t sq_idx = 0;
112 seed_queue.push_back (i);
116 while (sq_idx < seed_queue.size ())
119 if (!tree->
radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
125 for (std::size_t j = 1; j < nn_indices.size (); ++j)
127 if (processed[nn_indices[j]])
133 double dot_p = normals[seed_queue[sq_idx]].normal[0] * normals[nn_indices[j]].normal[0]
134 + normals[seed_queue[sq_idx]].normal[1] * normals[nn_indices[j]].normal[1] + normals[seed_queue[sq_idx]].normal[2]
135 * normals[nn_indices[j]].normal[2];
137 if (std::abs (std::acos (dot_p)) < eps_angle)
139 processed[nn_indices[j]] =
true;
148 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
151 r.
indices.resize (seed_queue.size ());
152 for (std::size_t j = 0; j < seed_queue.size (); ++j)
159 clusters.push_back (r);
165 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
167 std::vector<int> &indices_to_use,
168 std::vector<int> &indices_out, std::vector<int> &indices_in,
171 indices_out.resize (cloud.
size ());
172 indices_in.resize (cloud.
size ());
177 for (
const int &index : indices_to_use)
179 if (cloud[index].curvature > threshold)
181 indices_out[out] = index;
186 indices_in[in] = index;
191 indices_out.resize (out);
192 indices_in.resize (in);
195 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
bool
197 PointInTPtr & processed, std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > & transformations,
201 Eigen::Vector3f plane_normal;
202 plane_normal[0] = -centroid[0];
203 plane_normal[1] = -centroid[1];
204 plane_normal[2] = -centroid[2];
205 Eigen::Vector3f z_vector = Eigen::Vector3f::UnitZ ();
206 plane_normal.normalize ();
207 Eigen::Vector3f axis = plane_normal.cross (z_vector);
208 double rotation = -asin (axis.norm ());
211 Eigen::Affine3f transformPC (Eigen::AngleAxisf (
static_cast<float> (rotation), axis));
213 grid->resize (processed->size ());
214 for (std::size_t k = 0; k < processed->size (); k++)
215 (*grid)[k].getVector4fMap () = (*processed)[k].getVector4fMap ();
219 Eigen::Vector4f centroid4f (centroid[0], centroid[1], centroid[2], 0);
220 Eigen::Vector4f normal_centroid4f (normal_centroid[0], normal_centroid[1], normal_centroid[2], 0);
222 centroid4f = transformPC * centroid4f;
223 normal_centroid4f = transformPC * normal_centroid4f;
225 Eigen::Vector3f centroid3f (centroid4f[0], centroid4f[1], centroid4f[2]);
227 Eigen::Vector4f farthest_away;
229 farthest_away[3] = 0;
231 float max_dist = (farthest_away - centroid4f).norm ();
235 Eigen::Matrix4f center_mat;
236 center_mat.setIdentity (4, 4);
237 center_mat (0, 3) = -centroid4f[0];
238 center_mat (1, 3) = -centroid4f[1];
239 center_mat (2, 3) = -centroid4f[2];
241 Eigen::Matrix3f scatter;
245 for (
const int &index : indices.
indices)
247 Eigen::Vector3f pvector = (*grid)[index].getVector3fMap ();
248 float d_k = (pvector).norm ();
249 float w = (max_dist - d_k);
250 Eigen::Vector3f diff = (pvector);
251 Eigen::Matrix3f mat = diff * diff.transpose ();
258 Eigen::JacobiSVD <Eigen::MatrixXf> svd (scatter, Eigen::ComputeFullV);
259 Eigen::Vector3f evx = svd.matrixV ().col (0);
260 Eigen::Vector3f evy = svd.matrixV ().col (1);
261 Eigen::Vector3f evz = svd.matrixV ().col (2);
262 Eigen::Vector3f evxminus = evx * -1;
263 Eigen::Vector3f evyminus = evy * -1;
264 Eigen::Vector3f evzminus = evz * -1;
266 float s_xplus, s_xminus, s_yplus, s_yminus;
267 s_xplus = s_xminus = s_yplus = s_yminus = 0.f;
270 for (
const auto& point: grid->points)
272 Eigen::Vector3f pvector = point.getVector3fMap ();
273 float dist_x, dist_y;
274 dist_x = std::abs (evx.dot (pvector));
275 dist_y = std::abs (evy.dot (pvector));
277 if ((pvector).dot (evx) >= 0)
282 if ((pvector).dot (evy) >= 0)
289 if (s_xplus < s_xminus)
292 if (s_yplus < s_yminus)
297 float max_x =
static_cast<float> (std::max (s_xplus, s_xminus));
298 float min_x =
static_cast<float> (std::min (s_xplus, s_xminus));
299 float max_y =
static_cast<float> (std::max (s_yplus, s_yminus));
300 float min_y =
static_cast<float> (std::min (s_yplus, s_yminus));
302 fx = (min_x / max_x);
303 fy = (min_y / max_y);
305 Eigen::Vector3f normal3f = Eigen::Vector3f (normal_centroid4f[0], normal_centroid4f[1], normal_centroid4f[2]);
306 if (normal3f.dot (evz) < 0)
312 float max_axis = std::max (fx, fy);
313 float min_axis = std::min (fx, fy);
315 if ((min_axis / max_axis) > axis_ratio_)
317 PCL_WARN (
"Both axes are equally easy/difficult to disambiguate\n");
319 Eigen::Vector3f evy_copy = evy;
320 Eigen::Vector3f evxminus = evx * -1;
321 Eigen::Vector3f evyminus = evy * -1;
323 if (min_axis > min_axis_value_)
326 evy = evx.cross (evz);
327 Eigen::Matrix4f 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);
341 evy = evx.cross (evz);
342 trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
343 transformations.push_back (trans);
349 evy = evx.cross (evz);
350 Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
351 transformations.push_back (trans);
355 evy = evx.cross (evz);
356 trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
357 transformations.push_back (trans);
368 evy = evx.cross (evz);
369 Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
370 transformations.push_back (trans);
378 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
380 std::vector<pcl::PointIndices> & cluster_indices)
384 cluster_axes_.clear ();
385 cluster_axes_.resize (centroids_dominant_orientations_.size ());
387 for (std::size_t i = 0; i < centroids_dominant_orientations_.size (); i++)
390 std::vector < Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > transformations;
392 sgurf (centroids_dominant_orientations_[i], dominant_normals_[i], processed, transformations, grid, cluster_indices[i]);
395 cluster_axes_[i] = transformations.size ();
397 for (
const auto &transformation : transformations)
401 transforms_.push_back (transformation);
402 valid_transforms_.push_back (
true);
404 std::vector < Eigen::VectorXf > quadrants (8);
407 for (
int k = 0; k < num_hists; k++)
408 quadrants[k].setZero (size_hists);
410 Eigen::Vector4f centroid_p;
411 centroid_p.setZero ();
412 Eigen::Vector4f max_pt;
415 double distance_normalization_factor = (centroid_p - max_pt).norm ();
419 hist_incr = 100.0f /
static_cast<float> (grid->size () - 1);
423 float * weights =
new float[num_hists];
425 float sigma_sq = sigma * sigma;
427 for (
const auto& point: grid->points)
429 Eigen::Vector4f p = point.getVector4fMap ();
434 float wx = 1.f - std::exp (-((p[0] * p[0]) / (2.f * sigma_sq)));
435 float wy = 1.f - std::exp (-((p[1] * p[1]) / (2.f * sigma_sq)));
436 float wz = 1.f - std::exp (-((p[2] * p[2]) / (2.f * sigma_sq)));
441 for (std::size_t ii = 0; ii <= 3; ii++)
442 weights[ii] = 0.5f - wx * 0.5f;
444 for (std::size_t ii = 4; ii <= 7; ii++)
445 weights[ii] = 0.5f + wx * 0.5f;
449 for (std::size_t ii = 0; ii <= 3; ii++)
450 weights[ii] = 0.5f + wx * 0.5f;
452 for (std::size_t ii = 4; ii <= 7; ii++)
453 weights[ii] = 0.5f - wx * 0.5f;
459 for (std::size_t ii = 0; ii <= 1; ii++)
460 weights[ii] *= 0.5f - wy * 0.5f;
461 for (std::size_t ii = 4; ii <= 5; ii++)
462 weights[ii] *= 0.5f - wy * 0.5f;
464 for (std::size_t ii = 2; ii <= 3; ii++)
465 weights[ii] *= 0.5f + wy * 0.5f;
467 for (std::size_t ii = 6; ii <= 7; ii++)
468 weights[ii] *= 0.5f + wy * 0.5f;
472 for (std::size_t ii = 0; ii <= 1; ii++)
473 weights[ii] *= 0.5f + wy * 0.5f;
474 for (std::size_t ii = 4; ii <= 5; ii++)
475 weights[ii] *= 0.5f + wy * 0.5f;
477 for (std::size_t ii = 2; ii <= 3; ii++)
478 weights[ii] *= 0.5f - wy * 0.5f;
480 for (std::size_t ii = 6; ii <= 7; ii++)
481 weights[ii] *= 0.5f - wy * 0.5f;
487 for (std::size_t ii = 0; ii <= 7; ii += 2)
488 weights[ii] *= 0.5f - wz * 0.5f;
490 for (std::size_t ii = 1; ii <= 7; ii += 2)
491 weights[ii] *= 0.5f + wz * 0.5f;
496 for (std::size_t ii = 0; ii <= 7; ii += 2)
497 weights[ii] *= 0.5f + wz * 0.5f;
499 for (std::size_t ii = 1; ii <= 7; ii += 2)
500 weights[ii] *= 0.5f - wz * 0.5f;
503 int h_index = (d <= 0) ? 0 : std::ceil (size_hists * (d / distance_normalization_factor)) - 1;
511 for (
int j = 0; j < num_hists; j++)
512 quadrants[j][h_index] += hist_incr * weights[j];
518 vfh_signature.points.resize (1);
519 vfh_signature.width = vfh_signature.height = 1;
520 for (
int d = 0; d < 308; ++d)
521 vfh_signature[0].histogram[d] = output[i].histogram[d];
524 for (
int k = 0; k < num_hists; k++)
526 for (
int ii = 0; ii < size_hists; ii++, pos++)
528 vfh_signature[0].histogram[pos] = quadrants[k][ii];
532 ourcvfh_output.push_back (vfh_signature[0]);
533 ourcvfh_output.width = ourcvfh_output.size ();
538 if (!ourcvfh_output.points.empty ())
540 ourcvfh_output.height = 1;
542 output = ourcvfh_output;
546 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
549 if (refine_clusters_ <= 0.f)
550 refine_clusters_ = 1.f;
555 PCL_ERROR (
"[pcl::%s::computeFeature] No input dataset containing normals was given!\n", getClassName ().c_str ());
556 output.width = output.height = 0;
557 output.points.clear ();
560 if (normals_->size () != surface_->size ())
562 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 ());
563 output.width = output.height = 0;
564 output.points.clear ();
568 centroids_dominant_orientations_.clear ();
570 transforms_.clear ();
571 dominant_normals_.clear ();
574 std::vector<int> indices_out;
575 std::vector<int> indices_in;
576 filterNormalsWithHighCurvature (*normals_, *indices_, indices_out, indices_in, curv_threshold_);
579 normals_filtered_cloud->width = indices_in.size ();
580 normals_filtered_cloud->height = 1;
581 normals_filtered_cloud->points.resize (normals_filtered_cloud->width);
583 std::vector<int> indices_from_nfc_to_indices;
584 indices_from_nfc_to_indices.resize (indices_in.size ());
586 for (std::size_t i = 0; i < indices_in.size (); ++i)
588 (*normals_filtered_cloud)[i].x = (*surface_)[indices_in[i]].x;
589 (*normals_filtered_cloud)[i].y = (*surface_)[indices_in[i]].y;
590 (*normals_filtered_cloud)[i].z = (*surface_)[indices_in[i]].z;
592 indices_from_nfc_to_indices[i] = indices_in[i];
595 std::vector<pcl::PointIndices> clusters;
597 if (normals_filtered_cloud->size () >= min_points_)
602 normals_tree_filtered->setInputCloud (normals_filtered_cloud);
607 n3d.
compute (*normals_filtered_cloud);
611 normals_tree->setInputCloud (normals_filtered_cloud);
613 extractEuclideanClustersSmooth (*normals_filtered_cloud, *normals_filtered_cloud, cluster_tolerance_, normals_tree, clusters,
614 eps_angle_threshold_,
static_cast<unsigned int> (min_points_));
616 std::vector<pcl::PointIndices> clusters_filtered;
617 int cluster_filtered_idx = 0;
618 for (
const auto &cluster : clusters)
625 clusters_.push_back (pi);
626 clusters_filtered.push_back (pi_filtered);
628 Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
629 Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
631 for (
const auto &index : cluster.indices)
633 avg_normal += (*normals_filtered_cloud)[index].getNormalVector4fMap ();
634 avg_centroid += (*normals_filtered_cloud)[index].getVector4fMap ();
637 avg_normal /=
static_cast<float> (cluster.indices.size ());
638 avg_centroid /=
static_cast<float> (cluster.indices.size ());
639 avg_normal.normalize ();
641 Eigen::Vector3f avg_norm (avg_normal[0], avg_normal[1], avg_normal[2]);
642 Eigen::Vector3f avg_dominant_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
644 for (
const auto &index : cluster.indices)
647 double dot_p = avg_normal.dot ((*normals_filtered_cloud)[index].getNormalVector4fMap ());
648 if (std::abs (std::acos (dot_p)) < (eps_angle_threshold_ * refine_clusters_))
650 clusters_[cluster_filtered_idx].indices.push_back (indices_from_nfc_to_indices[index]);
651 clusters_filtered[cluster_filtered_idx].indices.push_back (index);
656 if (clusters_[cluster_filtered_idx].indices.empty ())
658 clusters_.pop_back ();
659 clusters_filtered.pop_back ();
662 cluster_filtered_idx++;
665 clusters = clusters_filtered;
680 if (!clusters.empty ())
682 for (
const auto &cluster : clusters)
684 Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
685 Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
687 for (
const auto &index : cluster.indices)
689 avg_normal += (*normals_filtered_cloud)[index].getNormalVector4fMap ();
690 avg_centroid += (*normals_filtered_cloud)[index].getVector4fMap ();
693 avg_normal /=
static_cast<float> (cluster.indices.size ());
694 avg_centroid /=
static_cast<float> (cluster.indices.size ());
695 avg_normal.normalize ();
698 dominant_normals_.emplace_back (avg_normal[0], avg_normal[1], avg_normal[2]);
699 centroids_dominant_orientations_.emplace_back (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
703 output.points.resize (dominant_normals_.size ());
704 output.width = dominant_normals_.size ();
706 for (std::size_t i = 0; i < dominant_normals_.size (); ++i)
713 output[i] = vfh_signature[0];
719 computeRFAndShapeDistribution (cloud_input, output, clusters_);
724 PCL_WARN(
"No clusters were found in the surface... using VFH...\n");
725 Eigen::Vector4f avg_centroid;
727 Eigen::Vector3f cloud_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
728 centroids_dominant_orientations_.push_back (cloud_centroid);
737 output.points.resize (1);
740 output[0] = vfh_signature[0];
741 Eigen::Matrix4f
id = Eigen::Matrix4f::Identity ();
742 transforms_.push_back (
id);
743 valid_transforms_.push_back (
false);
747 #define PCL_INSTANTIATE_OURCVFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::OURCVFHEstimation<T,NT,OutT>;
749 #endif // PCL_FEATURES_IMPL_OURCVFH_H_