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/io.h>
49 #include <pcl/common/transforms.h>
52 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
57 output.width = output.height = 0;
65 output.width = output.height = 1;
69 computeFeature (output);
75 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
80 std::vector<pcl::PointIndices> &clusters,
double eps_angle,
81 unsigned int min_pts_per_cluster,
82 unsigned int max_pts_per_cluster)
86 PCL_ERROR(
"[pcl::extractEuclideanClusters] Tree built for a different point cloud "
87 "dataset (%zu) than the input cloud (%zu)!\n",
89 static_cast<std::size_t
>(cloud.
size()));
92 if (cloud.
size () != normals.
size ())
94 PCL_ERROR(
"[pcl::extractEuclideanClusters] Number of points in the input point "
95 "cloud (%zu) different than normals (%zu)!\n",
96 static_cast<std::size_t
>(cloud.
size()),
97 static_cast<std::size_t
>(normals.
size()));
102 std::vector<bool> processed (cloud.
size (),
false);
105 std::vector<float> nn_distances;
107 for (std::size_t i = 0; i < cloud.
size (); ++i)
112 std::vector<std::size_t> seed_queue;
113 std::size_t sq_idx = 0;
114 seed_queue.push_back (i);
118 while (sq_idx < seed_queue.size ())
121 if (!tree->
radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
127 for (std::size_t j = 1; j < nn_indices.size (); ++j)
129 if (processed[nn_indices[j]])
135 double dot_p = normals[seed_queue[sq_idx]].normal[0] * normals[nn_indices[j]].normal[0]
136 + normals[seed_queue[sq_idx]].normal[1] * normals[nn_indices[j]].normal[1] + normals[seed_queue[sq_idx]].normal[2]
137 * normals[nn_indices[j]].normal[2];
139 if (std::abs (std::acos (dot_p)) < eps_angle)
141 processed[nn_indices[j]] =
true;
150 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
153 r.
indices.resize (seed_queue.size ());
154 for (std::size_t j = 0; j < seed_queue.size (); ++j)
161 clusters.push_back (r);
167 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
173 indices_out.resize (cloud.
size ());
174 indices_in.resize (cloud.
size ());
179 for (
const auto &index : indices_to_use)
181 if (cloud[index].curvature > threshold)
183 indices_out[out] = index;
188 indices_in[in] = index;
193 indices_out.resize (out);
194 indices_in.resize (in);
197 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
bool
199 PointInTPtr & processed, std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > & transformations,
203 Eigen::Vector3f plane_normal;
204 plane_normal[0] = -centroid[0];
205 plane_normal[1] = -centroid[1];
206 plane_normal[2] = -centroid[2];
207 Eigen::Vector3f z_vector = Eigen::Vector3f::UnitZ ();
208 plane_normal.normalize ();
209 Eigen::Vector3f axis = plane_normal.cross (z_vector);
210 double rotation = -asin (axis.norm ());
213 Eigen::Affine3f transformPC (Eigen::AngleAxisf (
static_cast<float> (rotation), axis));
215 grid->resize (processed->size ());
216 for (std::size_t k = 0; k < processed->size (); k++)
217 (*grid)[k].getVector4fMap () = (*processed)[k].getVector4fMap ();
221 Eigen::Vector4f centroid4f (centroid[0], centroid[1], centroid[2], 0);
222 Eigen::Vector4f normal_centroid4f (normal_centroid[0], normal_centroid[1], normal_centroid[2], 0);
224 centroid4f = transformPC * centroid4f;
225 normal_centroid4f = transformPC * normal_centroid4f;
227 Eigen::Vector3f centroid3f (centroid4f[0], centroid4f[1], centroid4f[2]);
229 Eigen::Vector4f farthest_away;
231 farthest_away[3] = 0;
233 float max_dist = (farthest_away - centroid4f).norm ();
237 Eigen::Matrix4f center_mat;
238 center_mat.setIdentity (4, 4);
239 center_mat (0, 3) = -centroid4f[0];
240 center_mat (1, 3) = -centroid4f[1];
241 center_mat (2, 3) = -centroid4f[2];
243 Eigen::Matrix3f scatter;
247 for (
const auto &index : indices.
indices)
249 Eigen::Vector3f pvector = (*grid)[index].getVector3fMap ();
250 float d_k = (pvector).norm ();
251 float w = (max_dist - d_k);
252 Eigen::Vector3f diff = (pvector);
253 Eigen::Matrix3f mat = diff * diff.transpose ();
260 Eigen::JacobiSVD <Eigen::MatrixXf> svd (scatter, Eigen::ComputeFullV);
261 Eigen::Vector3f evx = svd.matrixV ().col (0);
262 Eigen::Vector3f evy = svd.matrixV ().col (1);
263 Eigen::Vector3f evz = svd.matrixV ().col (2);
264 Eigen::Vector3f evxminus = evx * -1;
265 Eigen::Vector3f evyminus = evy * -1;
266 Eigen::Vector3f evzminus = evz * -1;
268 float s_xplus, s_xminus, s_yplus, s_yminus;
269 s_xplus = s_xminus = s_yplus = s_yminus = 0.f;
272 for (
const auto& point: grid->points)
274 Eigen::Vector3f pvector = point.getVector3fMap ();
275 float dist_x, dist_y;
276 dist_x = std::abs (evx.dot (pvector));
277 dist_y = std::abs (evy.dot (pvector));
279 if ((pvector).dot (evx) >= 0)
284 if ((pvector).dot (evy) >= 0)
291 if (s_xplus < s_xminus)
294 if (s_yplus < s_yminus)
299 float max_x =
static_cast<float> (std::max (s_xplus, s_xminus));
300 float min_x =
static_cast<float> (std::min (s_xplus, s_xminus));
301 float max_y =
static_cast<float> (std::max (s_yplus, s_yminus));
302 float min_y =
static_cast<float> (std::min (s_yplus, s_yminus));
304 fx = (min_x / max_x);
305 fy = (min_y / max_y);
307 Eigen::Vector3f normal3f = Eigen::Vector3f (normal_centroid4f[0], normal_centroid4f[1], normal_centroid4f[2]);
308 if (normal3f.dot (evz) < 0)
314 float max_axis = std::max (fx, fy);
315 float min_axis = std::min (fx, fy);
317 if ((min_axis / max_axis) > axis_ratio_)
319 PCL_WARN (
"Both axes are equally easy/difficult to disambiguate\n");
321 Eigen::Vector3f evy_copy = evy;
322 Eigen::Vector3f evxminus = evx * -1;
323 Eigen::Vector3f evyminus = evy * -1;
325 if (min_axis > min_axis_value_)
328 evy = evx.cross (evz);
329 Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
330 transformations.push_back (trans);
333 evy = evx.cross (evz);
334 trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
335 transformations.push_back (trans);
338 evy = evx.cross (evz);
339 trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
340 transformations.push_back (trans);
343 evy = evx.cross (evz);
344 trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
345 transformations.push_back (trans);
351 evy = evx.cross (evz);
352 Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
353 transformations.push_back (trans);
357 evy = evx.cross (evz);
358 trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
359 transformations.push_back (trans);
370 evy = evx.cross (evz);
371 Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
372 transformations.push_back (trans);
380 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
382 std::vector<pcl::PointIndices> & cluster_indices)
386 cluster_axes_.clear ();
387 cluster_axes_.resize (centroids_dominant_orientations_.size ());
389 for (std::size_t i = 0; i < centroids_dominant_orientations_.size (); i++)
392 std::vector < Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > transformations;
394 sgurf (centroids_dominant_orientations_[i], dominant_normals_[i], processed, transformations, grid, cluster_indices[i]);
397 cluster_axes_[i] = transformations.size ();
399 for (
const auto &transformation : transformations)
403 transforms_.push_back (transformation);
404 valid_transforms_.push_back (
true);
406 std::vector < Eigen::VectorXf > quadrants (8);
409 for (
int k = 0; k < num_hists; k++)
410 quadrants[k].setZero (size_hists);
412 Eigen::Vector4f centroid_p;
413 centroid_p.setZero ();
414 Eigen::Vector4f max_pt;
417 double distance_normalization_factor = (centroid_p - max_pt).norm ();
421 hist_incr = 100.0f /
static_cast<float> (grid->size () - 1);
425 float * weights =
new float[num_hists];
427 float sigma_sq = sigma * sigma;
429 for (
const auto& point: grid->points)
431 Eigen::Vector4f p = point.getVector4fMap ();
436 float wx = 1.f - std::exp (-((p[0] * p[0]) / (2.f * sigma_sq)));
437 float wy = 1.f - std::exp (-((p[1] * p[1]) / (2.f * sigma_sq)));
438 float wz = 1.f - std::exp (-((p[2] * p[2]) / (2.f * sigma_sq)));
443 for (std::size_t ii = 0; ii <= 3; ii++)
444 weights[ii] = 0.5f - wx * 0.5f;
446 for (std::size_t ii = 4; ii <= 7; ii++)
447 weights[ii] = 0.5f + wx * 0.5f;
451 for (std::size_t ii = 0; ii <= 3; ii++)
452 weights[ii] = 0.5f + wx * 0.5f;
454 for (std::size_t ii = 4; ii <= 7; ii++)
455 weights[ii] = 0.5f - wx * 0.5f;
461 for (std::size_t ii = 0; ii <= 1; ii++)
462 weights[ii] *= 0.5f - wy * 0.5f;
463 for (std::size_t ii = 4; ii <= 5; ii++)
464 weights[ii] *= 0.5f - wy * 0.5f;
466 for (std::size_t ii = 2; ii <= 3; ii++)
467 weights[ii] *= 0.5f + wy * 0.5f;
469 for (std::size_t ii = 6; ii <= 7; ii++)
470 weights[ii] *= 0.5f + wy * 0.5f;
474 for (std::size_t ii = 0; ii <= 1; ii++)
475 weights[ii] *= 0.5f + wy * 0.5f;
476 for (std::size_t ii = 4; ii <= 5; ii++)
477 weights[ii] *= 0.5f + wy * 0.5f;
479 for (std::size_t ii = 2; ii <= 3; ii++)
480 weights[ii] *= 0.5f - wy * 0.5f;
482 for (std::size_t ii = 6; ii <= 7; ii++)
483 weights[ii] *= 0.5f - wy * 0.5f;
489 for (std::size_t ii = 0; ii <= 7; ii += 2)
490 weights[ii] *= 0.5f - wz * 0.5f;
492 for (std::size_t ii = 1; ii <= 7; ii += 2)
493 weights[ii] *= 0.5f + wz * 0.5f;
498 for (std::size_t ii = 0; ii <= 7; ii += 2)
499 weights[ii] *= 0.5f + wz * 0.5f;
501 for (std::size_t ii = 1; ii <= 7; ii += 2)
502 weights[ii] *= 0.5f - wz * 0.5f;
505 int h_index = (d <= 0) ? 0 : std::ceil (size_hists * (d / distance_normalization_factor)) - 1;
513 for (
int j = 0; j < num_hists; j++)
514 quadrants[j][h_index] += hist_incr * weights[j];
520 vfh_signature.resize (1);
521 vfh_signature.width = vfh_signature.height = 1;
522 for (
int d = 0; d < 308; ++d)
523 vfh_signature[0].histogram[d] = output[i].histogram[d];
526 for (
int k = 0; k < num_hists; k++)
528 for (
int ii = 0; ii < size_hists; ii++, pos++)
530 vfh_signature[0].histogram[pos] = quadrants[k][ii];
534 ourcvfh_output.push_back (vfh_signature[0]);
535 ourcvfh_output.width = ourcvfh_output.size ();
540 if (!ourcvfh_output.empty ())
542 ourcvfh_output.height = 1;
544 output = ourcvfh_output;
548 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
551 if (refine_clusters_ <= 0.f)
552 refine_clusters_ = 1.f;
557 PCL_ERROR (
"[pcl::%s::computeFeature] No input dataset containing normals was given!\n", getClassName ().c_str ());
558 output.width = output.height = 0;
562 if (normals_->size () != surface_->size ())
564 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 ());
565 output.width = output.height = 0;
570 centroids_dominant_orientations_.clear ();
572 transforms_.clear ();
573 dominant_normals_.clear ();
578 filterNormalsWithHighCurvature (*normals_, *indices_, indices_out, indices_in, curv_threshold_);
581 normals_filtered_cloud->width = indices_in.size ();
582 normals_filtered_cloud->height = 1;
583 normals_filtered_cloud->points.resize (normals_filtered_cloud->width);
586 indices_from_nfc_to_indices.resize (indices_in.size ());
588 for (std::size_t i = 0; i < indices_in.size (); ++i)
590 (*normals_filtered_cloud)[i].x = (*surface_)[indices_in[i]].x;
591 (*normals_filtered_cloud)[i].y = (*surface_)[indices_in[i]].y;
592 (*normals_filtered_cloud)[i].z = (*surface_)[indices_in[i]].z;
594 indices_from_nfc_to_indices[i] = indices_in[i];
597 std::vector<pcl::PointIndices> clusters;
599 if (normals_filtered_cloud->size () >= min_points_)
604 normals_tree_filtered->setInputCloud (normals_filtered_cloud);
609 n3d.
compute (*normals_filtered_cloud);
613 normals_tree->setInputCloud (normals_filtered_cloud);
615 extractEuclideanClustersSmooth (*normals_filtered_cloud, *normals_filtered_cloud, cluster_tolerance_, normals_tree, clusters,
616 eps_angle_threshold_,
static_cast<unsigned int> (min_points_));
618 std::vector<pcl::PointIndices> clusters_filtered;
619 int cluster_filtered_idx = 0;
620 for (
const auto &cluster : clusters)
627 clusters_.push_back (pi);
628 clusters_filtered.push_back (pi_filtered);
630 Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
631 Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
633 for (
const auto &index : cluster.indices)
635 avg_normal += (*normals_filtered_cloud)[index].getNormalVector4fMap ();
636 avg_centroid += (*normals_filtered_cloud)[index].getVector4fMap ();
639 avg_normal /=
static_cast<float> (cluster.indices.size ());
640 avg_centroid /=
static_cast<float> (cluster.indices.size ());
641 avg_normal.normalize ();
643 Eigen::Vector3f avg_norm (avg_normal[0], avg_normal[1], avg_normal[2]);
644 Eigen::Vector3f avg_dominant_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
646 for (
const auto &index : cluster.indices)
649 double dot_p = avg_normal.dot ((*normals_filtered_cloud)[index].getNormalVector4fMap ());
650 if (std::abs (std::acos (dot_p)) < (eps_angle_threshold_ * refine_clusters_))
652 clusters_[cluster_filtered_idx].indices.push_back (indices_from_nfc_to_indices[index]);
653 clusters_filtered[cluster_filtered_idx].indices.push_back (index);
658 if (clusters_[cluster_filtered_idx].indices.empty ())
660 clusters_.pop_back ();
661 clusters_filtered.pop_back ();
664 cluster_filtered_idx++;
667 clusters = clusters_filtered;
682 if (!clusters.empty ())
684 for (
const auto &cluster : clusters)
686 Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
687 Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
689 for (
const auto &index : cluster.indices)
691 avg_normal += (*normals_filtered_cloud)[index].getNormalVector4fMap ();
692 avg_centroid += (*normals_filtered_cloud)[index].getVector4fMap ();
695 avg_normal /=
static_cast<float> (cluster.indices.size ());
696 avg_centroid /=
static_cast<float> (cluster.indices.size ());
697 avg_normal.normalize ();
700 dominant_normals_.emplace_back (avg_normal[0], avg_normal[1], avg_normal[2]);
701 centroids_dominant_orientations_.emplace_back (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
705 output.resize (dominant_normals_.size ());
706 output.width = dominant_normals_.size ();
708 for (std::size_t i = 0; i < dominant_normals_.size (); ++i)
715 output[i] = vfh_signature[0];
721 computeRFAndShapeDistribution (cloud_input, output, clusters_);
726 PCL_WARN(
"No clusters were found in the surface... using VFH...\n");
727 Eigen::Vector4f avg_centroid;
729 Eigen::Vector3f cloud_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
730 centroids_dominant_orientations_.push_back (cloud_centroid);
742 output[0] = vfh_signature[0];
743 Eigen::Matrix4f
id = Eigen::Matrix4f::Identity ();
744 transforms_.push_back (
id);
745 valid_transforms_.push_back (
false);
749 #define PCL_INSTANTIATE_OURCVFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::OURCVFHEstimation<T,NT,OutT>;
751 #endif // PCL_FEATURES_IMPL_OURCVFH_H_