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 (%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 (
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 axes 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)
380 cluster_axes_.
clear ();
381 cluster_axes_.resize (centroids_dominant_orientations_.size ());
383 for (
size_t i = 0; i < centroids_dominant_orientations_.size (); i++)
386 std::vector < Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > transformations;
388 sgurf (centroids_dominant_orientations_[i], dominant_normals_[i], processed, transformations, grid, cluster_indices[i]);
391 cluster_axes_[i] = transformations.size ();
393 for (
size_t t = 0; t < transformations.size (); t++)
397 transforms_.push_back (transformations[t]);
398 valid_transforms_.push_back (
true);
400 std::vector < Eigen::VectorXf > quadrants (8);
403 for (
int k = 0; k < num_hists; k++)
404 quadrants[k].setZero (size_hists);
406 Eigen::Vector4f centroid_p;
407 centroid_p.setZero ();
408 Eigen::Vector4f max_pt;
411 double distance_normalization_factor = (centroid_p - max_pt).norm ();
415 hist_incr = 100.0f /
static_cast<float> (grid->points.size () - 1);
419 float * weights =
new float[num_hists];
421 float sigma_sq = sigma * sigma;
423 for (
int k = 0; k < static_cast<int> (grid->points.size ()); k++)
425 Eigen::Vector4f p = grid->points[k].getVector4fMap ();
430 float wx = 1.f - std::exp (-((p[0] * p[0]) / (2.f * sigma_sq)));
431 float wy = 1.f - std::exp (-((p[1] * p[1]) / (2.f * sigma_sq)));
432 float wz = 1.f - std::exp (-((p[2] * p[2]) / (2.f * sigma_sq)));
437 for (
size_t ii = 0; ii <= 3; ii++)
438 weights[ii] = 0.5f - wx * 0.5f;
440 for (
size_t ii = 4; ii <= 7; ii++)
441 weights[ii] = 0.5f + wx * 0.5f;
445 for (
size_t ii = 0; ii <= 3; ii++)
446 weights[ii] = 0.5f + wx * 0.5f;
448 for (
size_t ii = 4; ii <= 7; ii++)
449 weights[ii] = 0.5f - wx * 0.5f;
455 for (
size_t ii = 0; ii <= 1; ii++)
456 weights[ii] *= 0.5f - wy * 0.5f;
457 for (
size_t ii = 4; ii <= 5; ii++)
458 weights[ii] *= 0.5f - wy * 0.5f;
460 for (
size_t ii = 2; ii <= 3; ii++)
461 weights[ii] *= 0.5f + wy * 0.5f;
463 for (
size_t ii = 6; ii <= 7; ii++)
464 weights[ii] *= 0.5f + wy * 0.5f;
468 for (
size_t ii = 0; ii <= 1; ii++)
469 weights[ii] *= 0.5f + wy * 0.5f;
470 for (
size_t ii = 4; ii <= 5; ii++)
471 weights[ii] *= 0.5f + wy * 0.5f;
473 for (
size_t ii = 2; ii <= 3; ii++)
474 weights[ii] *= 0.5f - wy * 0.5f;
476 for (
size_t ii = 6; ii <= 7; ii++)
477 weights[ii] *= 0.5f - wy * 0.5f;
483 for (
size_t ii = 0; ii <= 7; ii += 2)
484 weights[ii] *= 0.5f - wz * 0.5f;
486 for (
size_t ii = 1; ii <= 7; ii += 2)
487 weights[ii] *= 0.5f + wz * 0.5f;
492 for (
size_t ii = 0; ii <= 7; ii += 2)
493 weights[ii] *= 0.5f + wz * 0.5f;
495 for (
size_t ii = 1; ii <= 7; ii += 2)
496 weights[ii] *= 0.5f - wz * 0.5f;
499 int h_index = (d <= 0) ? 0 : std::ceil (size_hists * (d / distance_normalization_factor)) - 1;
507 for (
int j = 0; j < num_hists; j++)
508 quadrants[j][h_index] += hist_incr * weights[j];
514 vfh_signature.
points.resize (1);
516 for (
int d = 0; d < 308; ++d)
517 vfh_signature.
points[0].histogram[d] = output.
points[i].histogram[d];
520 for (
int k = 0; k < num_hists; k++)
522 for (
int ii = 0; ii < size_hists; ii++, pos++)
524 vfh_signature.
points[0].histogram[pos] = quadrants[k][ii];
528 ourcvfh_output.
points.push_back (vfh_signature.
points[0]);
529 ourcvfh_output.
width = ourcvfh_output.
points.size ();
534 if (ourcvfh_output.
points.size ())
536 ourcvfh_output.
height = 1;
538 output = ourcvfh_output;
542 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
545 if (refine_clusters_ <= 0.f)
546 refine_clusters_ = 1.f;
551 PCL_ERROR (
"[pcl::%s::computeFeature] No input dataset containing normals was given!\n", getClassName ().c_str ());
552 output.width = output.height = 0;
553 output.points.clear ();
556 if (normals_->points.size () != surface_->points.size ())
558 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 ());
559 output.width = output.height = 0;
560 output.points.clear ();
564 centroids_dominant_orientations_.clear ();
566 transforms_.clear ();
567 dominant_normals_.clear ();
570 std::vector<int> indices_out;
571 std::vector<int> indices_in;
572 filterNormalsWithHighCurvature (*normals_, *indices_, indices_out, indices_in, curv_threshold_);
575 normals_filtered_cloud->width =
static_cast<uint32_t
> (indices_in.size ());
576 normals_filtered_cloud->height = 1;
577 normals_filtered_cloud->points.resize (normals_filtered_cloud->width);
579 std::vector<int> indices_from_nfc_to_indices;
580 indices_from_nfc_to_indices.resize (indices_in.size ());
582 for (
size_t i = 0; i < indices_in.size (); ++i)
584 normals_filtered_cloud->points[i].x = surface_->points[indices_in[i]].x;
585 normals_filtered_cloud->points[i].y = surface_->points[indices_in[i]].y;
586 normals_filtered_cloud->points[i].z = surface_->points[indices_in[i]].z;
588 indices_from_nfc_to_indices[i] = indices_in[i];
591 std::vector<pcl::PointIndices> clusters;
593 if (normals_filtered_cloud->points.size () >= min_points_)
598 normals_tree_filtered->setInputCloud (normals_filtered_cloud);
603 n3d.
compute (*normals_filtered_cloud);
607 normals_tree->setInputCloud (normals_filtered_cloud);
609 extractEuclideanClustersSmooth (*normals_filtered_cloud, *normals_filtered_cloud, cluster_tolerance_, normals_tree, clusters,
610 eps_angle_threshold_,
static_cast<unsigned int> (min_points_));
612 std::vector<pcl::PointIndices> clusters_filtered;
613 int cluster_filtered_idx = 0;
614 for (
size_t i = 0; i < clusters.size (); i++)
621 clusters_.push_back (pi);
622 clusters_filtered.push_back (pi_filtered);
624 Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
625 Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
627 for (
size_t j = 0; j < clusters[i].indices.size (); j++)
629 avg_normal += normals_filtered_cloud->points[clusters[i].indices[j]].getNormalVector4fMap ();
630 avg_centroid += normals_filtered_cloud->points[clusters[i].indices[j]].getVector4fMap ();
633 avg_normal /=
static_cast<float> (clusters[i].indices.size ());
634 avg_centroid /=
static_cast<float> (clusters[i].indices.size ());
635 avg_normal.normalize ();
637 Eigen::Vector3f avg_norm (avg_normal[0], avg_normal[1], avg_normal[2]);
638 Eigen::Vector3f avg_dominant_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
640 for (
size_t j = 0; j < clusters[i].indices.size (); j++)
643 double dot_p = avg_normal.dot (normals_filtered_cloud->points[clusters[i].indices[j]].getNormalVector4fMap ());
644 if (fabs (acos (dot_p)) < (eps_angle_threshold_ * refine_clusters_))
646 clusters_[cluster_filtered_idx].indices.push_back (indices_from_nfc_to_indices[clusters[i].indices[j]]);
647 clusters_filtered[cluster_filtered_idx].indices.push_back (clusters[i].indices[j]);
652 if (clusters_[cluster_filtered_idx].indices.size () == 0)
654 clusters_.pop_back ();
655 clusters_filtered.pop_back ();
658 cluster_filtered_idx++;
661 clusters = clusters_filtered;
676 if (clusters.size () > 0)
679 for (
size_t i = 0; i < clusters.size (); ++i)
682 Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
683 Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
685 for (
size_t j = 0; j < clusters[i].indices.size (); j++)
687 avg_normal += normals_filtered_cloud->points[clusters[i].indices[j]].getNormalVector4fMap ();
688 avg_centroid += normals_filtered_cloud->points[clusters[i].indices[j]].getVector4fMap ();
691 avg_normal /=
static_cast<float> (clusters[i].indices.size ());
692 avg_centroid /=
static_cast<float> (clusters[i].indices.size ());
693 avg_normal.normalize ();
695 Eigen::Vector3f avg_norm (avg_normal[0], avg_normal[1], avg_normal[2]);
696 Eigen::Vector3f avg_dominant_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
699 dominant_normals_.push_back (avg_norm);
700 centroids_dominant_orientations_.push_back (avg_dominant_centroid);
704 output.points.resize (dominant_normals_.size ());
705 output.width =
static_cast<uint32_t
> (dominant_normals_.size ());
707 for (
size_t i = 0; i < dominant_normals_.size (); ++i)
714 output.points[i] = vfh_signature.
points[0];
720 computeRFAndShapeDistribution (cloud_input, output, clusters_);
725 PCL_WARN(
"No clusters were found in the surface... using VFH...\n");
726 Eigen::Vector4f avg_centroid;
728 Eigen::Vector3f cloud_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
729 centroids_dominant_orientations_.push_back (cloud_centroid);
738 output.points.resize (1);
741 output.points[0] = vfh_signature.
points[0];
742 Eigen::Matrix4f
id = Eigen::Matrix4f::Identity ();
743 transforms_.push_back (
id);
744 valid_transforms_.push_back (
false);
748 #define PCL_INSTANTIATE_OURCVFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::OURCVFHEstimation<T,NT,OutT>;
750 #endif // PCL_FEATURES_IMPL_OURCVFH_H_