Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
our_cvfh.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2011, Willow Garage, Inc.
6 * Copyright (c) 2012-, Open Perception, Inc.
7 *
8 * All rights reserved.
9 *
10 * Redistribution and use in source and binary forms, with or without
11 * modification, are permitted provided that the following conditions
12 * are met:
13 *
14 * * Redistributions of source code must retain the above copyright
15 * notice, this list of conditions and the following disclaimer.
16 * * Redistributions in binary form must reproduce the above
17 * copyright notice, this list of conditions and the following
18 * disclaimer in the documentation and/or other materials provided
19 * with the distribution.
20 * * Neither the name of the copyright holder(s) nor the names of its
21 * contributors may be used to endorse or promote products derived
22 * from this software without specific prior written permission.
23 *
24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 * POSSIBILITY OF SUCH DAMAGE.
36 *
37 * $Id: cvfh.hpp 5311 2012-03-26 22:02:04Z aaldoma $
38 *
39 */
40
41#ifndef PCL_FEATURES_IMPL_OURCVFH_H_
42#define PCL_FEATURES_IMPL_OURCVFH_H_
43
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> // for copyPointCloud
48#include <pcl/common/common.h> // for getMaxDistance
49#include <pcl/common/transforms.h>
50#include <pcl/search/kdtree.h> // for KdTree
51
52//////////////////////////////////////////////////////////////////////////////////////////////
53template<typename PointInT, typename PointNT, typename PointOutT> void
55{
57 {
58 output.width = output.height = 0;
59 output.clear ();
60 return;
61 }
62 // Resize the output dataset
63 // Important! We should only allocate precisely how many elements we will need, otherwise
64 // we risk at pre-allocating too much memory which could lead to bad_alloc
65 // (see http://dev.pointclouds.org/issues/657)
66 output.width = output.height = 1;
67 output.resize (1);
68
69 // Perform the actual feature computation
70 computeFeature (output);
71
73}
74
75//////////////////////////////////////////////////////////////////////////////////////////////
76template<typename PointInT, typename PointNT, typename PointOutT> void
79 float tolerance,
81 std::vector<pcl::PointIndices> &clusters, double eps_angle,
82 unsigned int min_pts_per_cluster,
83 unsigned int max_pts_per_cluster)
84{
85 if (tree->getInputCloud ()->size () != cloud.size ())
86 {
87 PCL_ERROR("[pcl::extractEuclideanClusters] Tree built for a different point cloud "
88 "dataset (%zu) than the input cloud (%zu)!\n",
89 static_cast<std::size_t>(tree->getInputCloud()->size()),
90 static_cast<std::size_t>(cloud.size()));
91 return;
92 }
93 if (cloud.size () != normals.size ())
94 {
95 PCL_ERROR("[pcl::extractEuclideanClusters] Number of points in the input point "
96 "cloud (%zu) different than normals (%zu)!\n",
97 static_cast<std::size_t>(cloud.size()),
98 static_cast<std::size_t>(normals.size()));
99 return;
100 }
101 // If tree gives sorted results, we can skip the first one because it is the query point itself
102 const std::size_t nn_start_idx = tree->getSortedResults () ? 1 : 0;
103
104 // Create a bool vector of processed point indices, and initialize it to false
105 std::vector<bool> processed (cloud.size (), false);
106
107 pcl::Indices nn_indices;
108 std::vector<float> nn_distances;
109 // Process all points in the indices vector
110 for (std::size_t i = 0; i < cloud.size (); ++i)
111 {
112 if (processed[i])
113 continue;
114
115 std::vector<std::size_t> seed_queue;
116 std::size_t sq_idx = 0;
117 seed_queue.push_back (i);
118
119 processed[i] = true;
120
121 while (sq_idx < seed_queue.size ())
122 {
123 // Search for sq_idx
124 if (!tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
125 {
126 sq_idx++;
127 continue;
128 }
129
130 for (std::size_t j = nn_start_idx; j < nn_indices.size (); ++j)
131 {
132 if (processed[nn_indices[j]]) // Has this point been processed before ?
133 continue;
134
135 //processed[nn_indices[j]] = true;
136 // [-1;1]
137
138 double dot_p = normals[seed_queue[sq_idx]].normal[0] * normals[nn_indices[j]].normal[0]
139 + normals[seed_queue[sq_idx]].normal[1] * normals[nn_indices[j]].normal[1] + normals[seed_queue[sq_idx]].normal[2]
140 * normals[nn_indices[j]].normal[2];
141
142 if (std::abs (std::acos (dot_p)) < eps_angle)
143 {
144 processed[nn_indices[j]] = true;
145 seed_queue.push_back (nn_indices[j]);
146 }
147 }
148
149 sq_idx++;
150 }
151
152 // If this queue is satisfactory, add to the clusters
153 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
154 {
156 r.indices.resize (seed_queue.size ());
157 for (std::size_t j = 0; j < seed_queue.size (); ++j)
158 r.indices[j] = seed_queue[j];
159
160 std::sort (r.indices.begin (), r.indices.end ());
161 r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ());
162
163 r.header = cloud.header;
164 clusters.push_back (r); // We could avoid a copy by working directly in the vector
165 }
166 }
167}
168
169//////////////////////////////////////////////////////////////////////////////////////////////
170template<typename PointInT, typename PointNT, typename PointOutT> void
172 pcl::Indices &indices_to_use,
173 pcl::Indices &indices_out, pcl::Indices &indices_in,
174 float threshold)
175{
176 indices_out.resize (cloud.size ());
177 indices_in.resize (cloud.size ());
178
179 std::size_t in, out;
180 in = out = 0;
181
182 for (const auto &index : indices_to_use)
183 {
184 if (cloud[index].curvature > threshold)
185 {
186 indices_out[out] = index;
187 out++;
188 }
189 else
190 {
191 indices_in[in] = index;
192 in++;
193 }
194 }
195
196 indices_out.resize (out);
197 indices_in.resize (in);
198}
199
200template<typename PointInT, typename PointNT, typename PointOutT> bool
201pcl::OURCVFHEstimation<PointInT, PointNT, PointOutT>::sgurf (Eigen::Vector3f & centroid, Eigen::Vector3f & normal_centroid,
202 PointInTPtr & processed, std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > & transformations,
203 PointInTPtr & grid, pcl::PointIndices & indices)
204{
205
206 Eigen::Vector3f plane_normal;
207 plane_normal[0] = -centroid[0];
208 plane_normal[1] = -centroid[1];
209 plane_normal[2] = -centroid[2];
210 Eigen::Vector3f z_vector = Eigen::Vector3f::UnitZ ();
211 plane_normal.normalize ();
212 Eigen::Vector3f axis = plane_normal.cross (z_vector);
213 double rotation = -asin (axis.norm ());
214 axis.normalize ();
215
216 Eigen::Affine3f transformPC (Eigen::AngleAxisf (static_cast<float> (rotation), axis));
217
218 grid->resize (processed->size ());
219 for (std::size_t k = 0; k < processed->size (); k++)
220 (*grid)[k].getVector4fMap () = (*processed)[k].getVector4fMap ();
221
222 pcl::transformPointCloud (*grid, *grid, transformPC);
223
224 Eigen::Vector4f centroid4f (centroid[0], centroid[1], centroid[2], 0);
225 Eigen::Vector4f normal_centroid4f (normal_centroid[0], normal_centroid[1], normal_centroid[2], 0);
226
227 centroid4f = transformPC * centroid4f;
228 normal_centroid4f = transformPC * normal_centroid4f;
229
230 Eigen::Vector3f centroid3f (centroid4f[0], centroid4f[1], centroid4f[2]);
231
232 Eigen::Vector4f farthest_away;
233 pcl::getMaxDistance (*grid, indices.indices, centroid4f, farthest_away);
234 farthest_away[3] = 0;
235
236 float max_dist = (farthest_away - centroid4f).norm ();
237
238 pcl::demeanPointCloud (*grid, centroid4f, *grid);
239
240 Eigen::Matrix4f center_mat;
241 center_mat.setIdentity (4, 4);
242 center_mat (0, 3) = -centroid4f[0];
243 center_mat (1, 3) = -centroid4f[1];
244 center_mat (2, 3) = -centroid4f[2];
245
246 Eigen::Matrix3f scatter;
247 scatter.setZero ();
248 float sum_w = 0.f;
249
250 for (const auto &index : indices.indices)
251 {
252 Eigen::Vector3f pvector = (*grid)[index].getVector3fMap ();
253 float d_k = (pvector).norm ();
254 float w = (max_dist - d_k);
255 Eigen::Vector3f diff = (pvector);
256 Eigen::Matrix3f mat = diff * diff.transpose ();
257 scatter += mat * w;
258 sum_w += w;
259 }
260
261 scatter /= sum_w;
262
263 Eigen::JacobiSVD <Eigen::MatrixXf> svd (scatter, Eigen::ComputeFullV);
264 Eigen::Vector3f evx = svd.matrixV ().col (0);
265 Eigen::Vector3f evy = svd.matrixV ().col (1);
266 Eigen::Vector3f evz = svd.matrixV ().col (2);
267 Eigen::Vector3f evxminus = evx * -1;
268 Eigen::Vector3f evyminus = evy * -1;
269 Eigen::Vector3f evzminus = evz * -1;
270
271 float s_xplus, s_xminus, s_yplus, s_yminus;
272 s_xplus = s_xminus = s_yplus = s_yminus = 0.f;
273
274 //disambiguate rf using all points
275 for (const auto& point: grid->points)
276 {
277 Eigen::Vector3f pvector = point.getVector3fMap ();
278 float dist_x, dist_y;
279 dist_x = std::abs (evx.dot (pvector));
280 dist_y = std::abs (evy.dot (pvector));
281
282 if ((pvector).dot (evx) >= 0)
283 s_xplus += dist_x;
284 else
285 s_xminus += dist_x;
286
287 if ((pvector).dot (evy) >= 0)
288 s_yplus += dist_y;
289 else
290 s_yminus += dist_y;
291
292 }
293
294 if (s_xplus < s_xminus)
295 evx = evxminus;
296
297 if (s_yplus < s_yminus)
298 evy = evyminus;
299
300 //select the axis that could be disambiguated more easily
301 float fx, fy;
302 float max_x = static_cast<float> (std::max (s_xplus, s_xminus));
303 float min_x = static_cast<float> (std::min (s_xplus, s_xminus));
304 float max_y = static_cast<float> (std::max (s_yplus, s_yminus));
305 float min_y = static_cast<float> (std::min (s_yplus, s_yminus));
306
307 fx = (min_x / max_x);
308 fy = (min_y / max_y);
309
310 Eigen::Vector3f normal3f = Eigen::Vector3f (normal_centroid4f[0], normal_centroid4f[1], normal_centroid4f[2]);
311 if (normal3f.dot (evz) < 0)
312 evz = evzminus;
313
314 //if fx/y close to 1, it was hard to disambiguate
315 //what if both are equally easy or difficult to disambiguate, namely fy == fx or very close
316
317 float max_axis = std::max (fx, fy);
318 float min_axis = std::min (fx, fy);
319
320 if ((min_axis / max_axis) > axis_ratio_)
321 {
322 PCL_WARN ("Both axes are equally easy/difficult to disambiguate\n");
323
324 Eigen::Vector3f evy_copy = evy;
325 Eigen::Vector3f evxminus = evx * -1;
326 Eigen::Vector3f evyminus = evy * -1;
327
328 if (min_axis > min_axis_value_)
329 {
330 //combination of all possibilities
331 evy = evx.cross (evz);
332 Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
333 transformations.push_back (trans);
334
335 evx = evxminus;
336 evy = evx.cross (evz);
337 trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
338 transformations.push_back (trans);
339
340 evx = evy_copy;
341 evy = evx.cross (evz);
342 trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
343 transformations.push_back (trans);
344
345 evx = evyminus;
346 evy = evx.cross (evz);
347 trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
348 transformations.push_back (trans);
349
350 }
351 else
352 {
353 //1-st case (evx selected)
354 evy = evx.cross (evz);
355 Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
356 transformations.push_back (trans);
357
358 //2-nd case (evy selected)
359 evx = evy_copy;
360 evy = evx.cross (evz);
361 trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
362 transformations.push_back (trans);
363 }
364 }
365 else
366 {
367 if (fy < fx)
368 {
369 evx = evy;
370 fx = fy;
371 }
372
373 evy = evx.cross (evz);
374 Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
375 transformations.push_back (trans);
376
377 }
378
379 return true;
380}
381
382//////////////////////////////////////////////////////////////////////////////////////////////
383template<typename PointInT, typename PointNT, typename PointOutT> void
385 std::vector<pcl::PointIndices> & cluster_indices)
386{
387 PointCloudOut ourcvfh_output;
388
389 cluster_axes_.clear ();
390 cluster_axes_.resize (centroids_dominant_orientations_.size ());
391
392 for (std::size_t i = 0; i < centroids_dominant_orientations_.size (); i++)
393 {
394
395 std::vector < Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > transformations;
397 sgurf (centroids_dominant_orientations_[i], dominant_normals_[i], processed, transformations, grid, cluster_indices[i]);
398
399 // Make a note of how many transformations correspond to each cluster
400 cluster_axes_[i] = transformations.size ();
401
402 for (const auto &transformation : transformations)
403 {
404
405 pcl::transformPointCloud (*processed, *grid, transformation);
406 transforms_.push_back (transformation);
407 valid_transforms_.push_back (true);
408
409 std::vector < Eigen::VectorXf > quadrants (8);
410 int size_hists = 13;
411 int num_hists = 8;
412 for (int k = 0; k < num_hists; k++)
413 quadrants[k].setZero (size_hists);
414
415 Eigen::Vector4f centroid_p;
416 centroid_p.setZero ();
417 Eigen::Vector4f max_pt;
418 pcl::getMaxDistance (*grid, centroid_p, max_pt);
419 max_pt[3] = 0;
420 double distance_normalization_factor = (centroid_p - max_pt).norm ();
421
422 float hist_incr;
423 if (normalize_bins_)
424 hist_incr = 100.0f / static_cast<float> (grid->size () - 1);
425 else
426 hist_incr = 1.0f;
427
428 float * weights = new float[num_hists];
429 float sigma = 0.01f; //1cm
430 float sigma_sq = sigma * sigma;
431
432 for (const auto& point: grid->points)
433 {
434 Eigen::Vector4f p = point.getVector4fMap ();
435 p[3] = 0.f;
436 float d = p.norm ();
437
438 //compute weight for all octants
439 float wx = 1.f - std::exp (-((p[0] * p[0]) / (2.f * sigma_sq))); //how is the weight distributed among two semi-cubes
440 float wy = 1.f - std::exp (-((p[1] * p[1]) / (2.f * sigma_sq)));
441 float wz = 1.f - std::exp (-((p[2] * p[2]) / (2.f * sigma_sq)));
442
443 //distribute the weights using the x-coordinate
444 if (p[0] >= 0)
445 {
446 for (std::size_t ii = 0; ii <= 3; ii++)
447 weights[ii] = 0.5f - wx * 0.5f;
448
449 for (std::size_t ii = 4; ii <= 7; ii++)
450 weights[ii] = 0.5f + wx * 0.5f;
451 }
452 else
453 {
454 for (std::size_t ii = 0; ii <= 3; ii++)
455 weights[ii] = 0.5f + wx * 0.5f;
456
457 for (std::size_t ii = 4; ii <= 7; ii++)
458 weights[ii] = 0.5f - wx * 0.5f;
459 }
460
461 //distribute the weights using the y-coordinate
462 if (p[1] >= 0)
463 {
464 for (std::size_t ii = 0; ii <= 1; ii++)
465 weights[ii] *= 0.5f - wy * 0.5f;
466 for (std::size_t ii = 4; ii <= 5; ii++)
467 weights[ii] *= 0.5f - wy * 0.5f;
468
469 for (std::size_t ii = 2; ii <= 3; ii++)
470 weights[ii] *= 0.5f + wy * 0.5f;
471
472 for (std::size_t ii = 6; ii <= 7; ii++)
473 weights[ii] *= 0.5f + wy * 0.5f;
474 }
475 else
476 {
477 for (std::size_t ii = 0; ii <= 1; ii++)
478 weights[ii] *= 0.5f + wy * 0.5f;
479 for (std::size_t ii = 4; ii <= 5; ii++)
480 weights[ii] *= 0.5f + wy * 0.5f;
481
482 for (std::size_t ii = 2; ii <= 3; ii++)
483 weights[ii] *= 0.5f - wy * 0.5f;
484
485 for (std::size_t ii = 6; ii <= 7; ii++)
486 weights[ii] *= 0.5f - wy * 0.5f;
487 }
488
489 //distribute the weights using the z-coordinate
490 if (p[2] >= 0)
491 {
492 for (std::size_t ii = 0; ii <= 7; ii += 2)
493 weights[ii] *= 0.5f - wz * 0.5f;
494
495 for (std::size_t ii = 1; ii <= 7; ii += 2)
496 weights[ii] *= 0.5f + wz * 0.5f;
497
498 }
499 else
500 {
501 for (std::size_t ii = 0; ii <= 7; ii += 2)
502 weights[ii] *= 0.5f + wz * 0.5f;
503
504 for (std::size_t ii = 1; ii <= 7; ii += 2)
505 weights[ii] *= 0.5f - wz * 0.5f;
506 }
507
508 int h_index = (d <= 0) ? 0 : std::ceil (size_hists * (d / distance_normalization_factor)) - 1;
509 /* from http://www.pcl-users.org/OUR-CVFH-problem-td4028436.html
510 h_index will be 13 when d is computed on the farthest away point.
511
512 adding the following after computing h_index fixes the problem:
513 */
514 if(h_index > 12)
515 h_index = 12;
516 for (int j = 0; j < num_hists; j++)
517 quadrants[j][h_index] += hist_incr * weights[j];
518
519 }
520
521 //copy to the cvfh signature
522 PointCloudOut vfh_signature;
523 vfh_signature.resize (1);
524 vfh_signature.width = vfh_signature.height = 1;
525 for (int d = 0; d < 308; ++d)
526 vfh_signature[0].histogram[d] = output[i].histogram[d];
527
528 int pos = 45 * 3;
529 for (int k = 0; k < num_hists; k++)
530 {
531 for (int ii = 0; ii < size_hists; ii++, pos++)
532 {
533 vfh_signature[0].histogram[pos] = quadrants[k][ii];
534 }
535 }
536
537 ourcvfh_output.push_back (vfh_signature[0]);
538 ourcvfh_output.width = ourcvfh_output.size ();
539 delete[] weights;
540 }
541 }
542
543 if (!ourcvfh_output.empty ())
544 {
545 ourcvfh_output.height = 1;
546 }
547 output = ourcvfh_output;
548}
549
550//////////////////////////////////////////////////////////////////////////////////////////////
551template<typename PointInT, typename PointNT, typename PointOutT> void
553{
554 if (refine_clusters_ <= 0.f)
555 refine_clusters_ = 1.f;
556
557 // Check if input was set
558 if (!normals_)
559 {
560 PCL_ERROR ("[pcl::%s::computeFeature] No input dataset containing normals was given!\n", getClassName ().c_str ());
561 output.width = output.height = 0;
562 output.clear ();
563 return;
564 }
565 if (normals_->size () != surface_->size ())
566 {
567 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 ());
568 output.width = output.height = 0;
569 output.clear ();
570 return;
571 }
572
573 centroids_dominant_orientations_.clear ();
574 clusters_.clear ();
575 transforms_.clear ();
576 dominant_normals_.clear ();
577
578 // ---[ Step 0: remove normals with high curvature
579 pcl::Indices indices_out;
580 pcl::Indices indices_in;
581 filterNormalsWithHighCurvature (*normals_, *indices_, indices_out, indices_in, curv_threshold_);
582
584 normals_filtered_cloud->width = indices_in.size ();
585 normals_filtered_cloud->height = 1;
586 normals_filtered_cloud->points.resize (normals_filtered_cloud->width);
587
588 pcl::Indices indices_from_nfc_to_indices;
589 indices_from_nfc_to_indices.resize (indices_in.size ());
590
591 for (std::size_t i = 0; i < indices_in.size (); ++i)
592 {
593 (*normals_filtered_cloud)[i].x = (*surface_)[indices_in[i]].x;
594 (*normals_filtered_cloud)[i].y = (*surface_)[indices_in[i]].y;
595 (*normals_filtered_cloud)[i].z = (*surface_)[indices_in[i]].z;
596 //(*normals_filtered_cloud)[i].getNormalVector4fMap() = (*normals_)[indices_in[i]].getNormalVector4fMap();
597 indices_from_nfc_to_indices[i] = indices_in[i];
598 }
599
600 std::vector<pcl::PointIndices> clusters;
601
602 if (normals_filtered_cloud->size () >= min_points_)
603 {
604 //recompute normals and use them for clustering
605 {
606 KdTreePtr normals_tree_filtered (new pcl::search::KdTree<pcl::PointNormal> (false));
607 normals_tree_filtered->setInputCloud (normals_filtered_cloud);
609 n3d.setRadiusSearch (radius_normals_);
610 n3d.setSearchMethod (normals_tree_filtered);
611 n3d.setInputCloud (normals_filtered_cloud);
612 n3d.compute (*normals_filtered_cloud);
613 }
614
615 KdTreePtr normals_tree (new pcl::search::KdTree<pcl::PointNormal> (false));
616 normals_tree->setInputCloud (normals_filtered_cloud);
617
618 extractEuclideanClustersSmooth (*normals_filtered_cloud, *normals_filtered_cloud, cluster_tolerance_, normals_tree, clusters,
619 eps_angle_threshold_, static_cast<unsigned int> (min_points_));
620
621 std::vector<pcl::PointIndices> clusters_filtered;
622 int cluster_filtered_idx = 0;
623 for (const auto &cluster : clusters)
624 {
625
627 pcl::PointIndices pi_filtered;
628
629 clusters_.push_back (pi);
630 clusters_filtered.push_back (pi_filtered);
631
632 Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
633 Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
634
635 for (const auto &index : cluster.indices)
636 {
637 avg_normal += (*normals_filtered_cloud)[index].getNormalVector4fMap ();
638 avg_centroid += (*normals_filtered_cloud)[index].getVector4fMap ();
639 }
640
641 avg_normal /= static_cast<float> (cluster.indices.size ());
642 avg_centroid /= static_cast<float> (cluster.indices.size ());
643 avg_normal.normalize ();
644
645 Eigen::Vector3f avg_norm (avg_normal[0], avg_normal[1], avg_normal[2]);
646 Eigen::Vector3f avg_dominant_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
647
648 for (const auto &index : cluster.indices)
649 {
650 //decide if normal should be added
651 double dot_p = avg_normal.dot ((*normals_filtered_cloud)[index].getNormalVector4fMap ());
652 if (std::abs (std::acos (dot_p)) < (eps_angle_threshold_ * refine_clusters_))
653 {
654 clusters_[cluster_filtered_idx].indices.push_back (indices_from_nfc_to_indices[index]);
655 clusters_filtered[cluster_filtered_idx].indices.push_back (index);
656 }
657 }
658
659 //remove last cluster if no points found...
660 if (clusters_[cluster_filtered_idx].indices.empty ())
661 {
662 clusters_.pop_back ();
663 clusters_filtered.pop_back ();
664 }
665 else
666 cluster_filtered_idx++;
667 }
668
669 clusters = clusters_filtered;
670
671 }
672
674 vfh.setInputCloud (surface_);
675 vfh.setInputNormals (normals_);
676 vfh.setIndices (indices_);
677 vfh.setSearchMethod (this->tree_);
678 vfh.setUseGivenNormal (true);
679 vfh.setUseGivenCentroid (true);
680 vfh.setNormalizeBins (normalize_bins_);
681 output.height = 1;
682
683 // ---[ Step 1b : check if any dominant cluster was found
684 if (!clusters.empty ())
685 { // ---[ Step 1b.1 : If yes, compute CVFH using the cluster information
686 for (const auto &cluster : clusters) //for each cluster
687 {
688 Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
689 Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
690
691 for (const auto &index : cluster.indices)
692 {
693 avg_normal += (*normals_filtered_cloud)[index].getNormalVector4fMap ();
694 avg_centroid += (*normals_filtered_cloud)[index].getVector4fMap ();
695 }
696
697 avg_normal /= static_cast<float> (cluster.indices.size ());
698 avg_centroid /= static_cast<float> (cluster.indices.size ());
699 avg_normal.normalize ();
700
701 //append normal and centroid for the clusters
702 dominant_normals_.emplace_back (avg_normal[0], avg_normal[1], avg_normal[2]);
703 centroids_dominant_orientations_.emplace_back (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
704 }
705
706 //compute modified VFH for all dominant clusters and add them to the list!
707 output.resize (dominant_normals_.size ());
708 output.width = dominant_normals_.size ();
709
710 for (std::size_t i = 0; i < dominant_normals_.size (); ++i)
711 {
712 //configure VFH computation for CVFH
713 vfh.setNormalToUse (dominant_normals_[i]);
714 vfh.setCentroidToUse (centroids_dominant_orientations_[i]);
716 vfh.compute (vfh_signature);
717 output[i] = vfh_signature[0];
718 }
719
720 //finish filling the descriptor with the shape distribution
721 PointInTPtr cloud_input (new pcl::PointCloud<PointInT>);
722 pcl::copyPointCloud (*surface_, *indices_, *cloud_input);
723 computeRFAndShapeDistribution (cloud_input, output, clusters_); //this will set transforms_
724 }
725 else
726 { // ---[ Step 1b.1 : If no, compute a VFH using all the object points
727
728 PCL_WARN("No clusters were found in the surface... using VFH...\n");
729 Eigen::Vector4f avg_centroid;
730 pcl::compute3DCentroid (*surface_, avg_centroid);
731 Eigen::Vector3f cloud_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
732 centroids_dominant_orientations_.push_back (cloud_centroid);
733
734 //configure VFH computation using all object points
735 vfh.setCentroidToUse (cloud_centroid);
736 vfh.setUseGivenNormal (false);
737
739 vfh.compute (vfh_signature);
740
741 output.resize (1);
742 output.width = 1;
743
744 output[0] = vfh_signature[0];
745 Eigen::Matrix4f id = Eigen::Matrix4f::Identity ();
746 transforms_.push_back (id);
747 valid_transforms_.push_back (false);
748 }
749}
750
751#define PCL_INSTANTIATE_OURCVFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::OURCVFHEstimation<T,NT,OutT>;
752
753#endif // PCL_FEATURES_IMPL_OURCVFH_H_
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset.
Definition feature.h:339
Feature represents the base feature class.
Definition feature.h:107
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
Definition feature.h:198
void setSearchMethod(const KdTreePtr &tree)
Provide a pointer to the search object.
Definition feature.h:164
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Definition feature.hpp:194
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point.
Definition normal_3d.h:244
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
Definition normal_3d.h:328
OURCVFHEstimation estimates the Oriented, Unique and Repetable Clustered Viewpoint Feature Histogram ...
Definition our_cvfh.h:60
bool sgurf(Eigen::Vector3f &centroid, Eigen::Vector3f &normal_centroid, PointInTPtr &processed, std::vector< Eigen::Matrix4f, Eigen::aligned_allocator< Eigen::Matrix4f > > &transformations, PointInTPtr &grid, pcl::PointIndices &indices)
Computes SGURF.
Definition our_cvfh.hpp:201
typename Feature< PointInT, PointOutT >::PointCloudOut PointCloudOut
Definition our_cvfh.h:72
void filterNormalsWithHighCurvature(const pcl::PointCloud< PointNT > &cloud, pcl::Indices &indices_to_use, pcl::Indices &indices_out, pcl::Indices &indices_in, float threshold)
Removes normals with high curvature caused by real edges or noisy data.
Definition our_cvfh.hpp:171
void compute(PointCloudOut &output)
Overloaded computed method from pcl::Feature.
Definition our_cvfh.hpp:54
typename pcl::PointCloud< PointInT >::Ptr PointInTPtr
Definition our_cvfh.h:74
void computeRFAndShapeDistribution(PointInTPtr &processed, PointCloudOut &output, std::vector< pcl::PointIndices > &cluster_indices)
Computes SGURF and the shape distribution based on the selected SGURF.
Definition our_cvfh.hpp:384
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition pcl_base.hpp:65
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
Definition pcl_base.hpp:72
PointCloud represents the base class in PCL for storing collections of 3D points.
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
pcl::PCLHeader header
The point cloud header.
std::size_t size() const
shared_ptr< PointCloud< PointT > > Ptr
VFHEstimation estimates the Viewpoint Feature Histogram (VFH) descriptor for a given point cloud data...
Definition vfh.h:73
void setUseGivenNormal(bool use)
Set use_given_normal_.
Definition vfh.h:142
void setCentroidToUse(const Eigen::Vector3f &centroid)
Set centroid_to_use_.
Definition vfh.h:171
void compute(PointCloudOut &output)
Overloaded computed method from pcl::Feature.
Definition vfh.hpp:65
void setNormalToUse(const Eigen::Vector3f &normal)
Set the normal to use.
Definition vfh.h:152
void setNormalizeBins(bool normalize)
set normalize_bins_
Definition vfh.h:180
void setUseGivenCentroid(bool use)
Set use_given_centroid_.
Definition vfh.h:161
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition kdtree.h:62
virtual bool getSortedResults()
Gets whether the results should be sorted (ascending in the distance) or not Otherwise the results ma...
Definition search.hpp:68
shared_ptr< pcl::search::Search< PointT > > Ptr
Definition search.h:81
virtual PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
Definition search.h:124
virtual int radiusSearch(const PointT &point, double radius, Indices &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.
Define standard C methods and C++ classes that are common to all methods.
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.
Definition common.hpp:197
void demeanPointCloud(ConstCloudIterator< PointT > &cloud_iterator, const Eigen::Matrix< Scalar, 4, 1 > &centroid, pcl::PointCloud< PointT > &cloud_out, int npts=0)
Subtract a centroid from a point cloud and return the de-meaned representation.
Definition centroid.hpp:933
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Matrix< Scalar, 4, 4 > &transform, bool copy_all_fields)
Apply a rigid transform defined by a 4x4 matrix.
unsigned int compute3DCentroid(ConstCloudIterator< PointT > &cloud_iterator, Eigen::Matrix< Scalar, 4, 1 > &centroid)
Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
Definition centroid.hpp:57
void copyPointCloud(const pcl::PointCloud< PointInT > &cloud_in, pcl::PointCloud< PointOutT > &cloud_out)
Copy all the fields from a given point cloud into a new point cloud.
Definition io.hpp:142
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
::pcl::PCLHeader header