Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
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$
38 *
39 */
40
41#ifndef PCL_FEATURES_IMPL_CVFH_H_
42#define PCL_FEATURES_IMPL_CVFH_H_
43
44#include <pcl/features/cvfh.h>
45#include <pcl/features/normal_3d.h>
46#include <pcl/common/centroid.h>
47#include <pcl/search/kdtree.h> // for KdTree
48
49//////////////////////////////////////////////////////////////////////////////////////////////
50template<typename PointInT, typename PointNT, typename PointOutT> void
52{
54 {
55 output.width = output.height = 0;
56 output.clear ();
57 return;
58 }
59 // Resize the output dataset
60 // Important! We should only allocate precisely how many elements we will need, otherwise
61 // we risk at pre-allocating too much memory which could lead to bad_alloc
62 // (see http://dev.pointclouds.org/issues/657)
63 output.width = output.height = 1;
64 output.resize (1);
65
66 // Perform the actual feature computation
67 computeFeature (output);
68
70}
71
72//////////////////////////////////////////////////////////////////////////////////////////////
73template<typename PointInT, typename PointNT, typename PointOutT> void
77 float tolerance,
79 std::vector<pcl::PointIndices> &clusters,
80 double eps_angle,
81 unsigned int min_pts_per_cluster,
82 unsigned int max_pts_per_cluster)
83{
84 if (tree->getInputCloud ()->size () != cloud.size ())
85 {
86 PCL_ERROR("[pcl::extractEuclideanClusters] Tree built for a different point cloud "
87 "dataset (%zu) than the input cloud (%zu)!\n",
88 static_cast<std::size_t>(tree->getInputCloud()->size()),
89 static_cast<std::size_t>(cloud.size()));
90 return;
91 }
92 if (cloud.size () != normals.size ())
93 {
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()));
98 return;
99 }
100 // If tree gives sorted results, we can skip the first one because it is the query point itself
101 const std::size_t nn_start_idx = tree->getSortedResults () ? 1 : 0;
102
103 // Create a bool vector of processed point indices, and initialize it to false
104 std::vector<bool> processed (cloud.size (), false);
105
106 pcl::Indices nn_indices;
107 std::vector<float> nn_distances;
108 // Process all points in the indices vector
109 for (std::size_t i = 0; i < cloud.size (); ++i)
110 {
111 if (processed[i])
112 continue;
113 processed[i] = true;
114
116 r.header = cloud.header;
117 auto& seed_queue = r.indices;
118
119 seed_queue.push_back (i);
120
121 // loop has an emplace_back, making it difficult to use modern loops
122 for (std::size_t idx = 0; idx != seed_queue.size (); ++idx)
123 {
124 // Search for seed_queue[index]
125 if (!tree->radiusSearch (seed_queue[idx], tolerance, nn_indices, nn_distances))
126 {
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 const double dot_p = normals[seed_queue[idx]].getNormalVector3fMap().dot(
138 normals[nn_indices[j]].getNormalVector3fMap());
139
140 if (std::acos (dot_p) < eps_angle)
141 {
142 processed[nn_indices[j]] = true;
143 seed_queue.emplace_back (nn_indices[j]);
144 }
145 }
146 }
147
148 // If this queue is satisfactory, add to the clusters
149 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
150 {
151 std::sort (r.indices.begin (), r.indices.end ());
152 r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ());
153
154 // Might be better to work directly in the cluster somehow
155 clusters.emplace_back (std::move(r)); // Trying to avoid a copy by moving
156 }
157 }
158}
159
160//////////////////////////////////////////////////////////////////////////////////////////////
161template<typename PointInT, typename PointNT, typename PointOutT> void
163 const pcl::PointCloud<PointNT> & cloud,
164 pcl::Indices &indices_to_use,
165 pcl::Indices &indices_out,
166 pcl::Indices &indices_in,
167 float threshold)
168{
169 indices_out.resize (cloud.size ());
170 indices_in.resize (cloud.size ());
171
172 std::size_t in, out;
173 in = out = 0;
174
175 for (const auto &index : indices_to_use)
176 {
177 if (cloud[index].curvature > threshold)
178 {
179 indices_out[out] = index;
180 out++;
181 }
182 else
183 {
184 indices_in[in] = index;
185 in++;
186 }
187 }
188
189 indices_out.resize (out);
190 indices_in.resize (in);
191}
192
193//////////////////////////////////////////////////////////////////////////////////////////////
194template<typename PointInT, typename PointNT, typename PointOutT> void
196{
197 // Check if input was set
198 if (!normals_)
199 {
200 PCL_ERROR ("[pcl::%s::computeFeature] No input dataset containing normals was given!\n", getClassName ().c_str ());
201 output.width = output.height = 0;
202 output.clear ();
203 return;
204 }
205 if (normals_->size () != surface_->size ())
206 {
207 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 ());
208 output.width = output.height = 0;
209 output.clear ();
210 return;
211 }
212
213 centroids_dominant_orientations_.clear ();
214
215 // ---[ Step 0: remove normals with high curvature
216 pcl::Indices indices_out;
217 pcl::Indices indices_in;
218 filterNormalsWithHighCurvature (*normals_, *indices_, indices_out, indices_in, curv_threshold_);
219
221 normals_filtered_cloud->width = indices_in.size ();
222 normals_filtered_cloud->height = 1;
223 normals_filtered_cloud->points.resize (normals_filtered_cloud->width);
224
225 for (std::size_t i = 0; i < indices_in.size (); ++i)
226 {
227 (*normals_filtered_cloud)[i].x = (*surface_)[indices_in[i]].x;
228 (*normals_filtered_cloud)[i].y = (*surface_)[indices_in[i]].y;
229 (*normals_filtered_cloud)[i].z = (*surface_)[indices_in[i]].z;
230 }
231
232 std::vector<pcl::PointIndices> clusters;
233
234 if(normals_filtered_cloud->size() >= min_points_)
235 {
236 //recompute normals and use them for clustering
237 KdTreePtr normals_tree_filtered (new pcl::search::KdTree<pcl::PointNormal> (false));
238 normals_tree_filtered->setInputCloud (normals_filtered_cloud);
239
240
242 n3d.setRadiusSearch (radius_normals_);
243 n3d.setSearchMethod (normals_tree_filtered);
244 n3d.setInputCloud (normals_filtered_cloud);
245 n3d.compute (*normals_filtered_cloud);
246
247 KdTreePtr normals_tree (new pcl::search::KdTree<pcl::PointNormal> (false));
248 normals_tree->setInputCloud (normals_filtered_cloud);
249
250 extractEuclideanClustersSmooth (*normals_filtered_cloud,
251 *normals_filtered_cloud,
252 cluster_tolerance_,
253 normals_tree,
254 clusters,
255 eps_angle_threshold_,
256 static_cast<unsigned int> (min_points_));
257
258 }
259
260 VFHEstimator vfh;
261 vfh.setInputCloud (surface_);
262 vfh.setInputNormals (normals_);
263 vfh.setIndices(indices_);
264 vfh.setSearchMethod (this->tree_);
265 vfh.setUseGivenNormal (true);
266 vfh.setUseGivenCentroid (true);
267 vfh.setNormalizeBins (normalize_bins_);
268 vfh.setNormalizeDistance (true);
269 vfh.setFillSizeComponent (true);
270 output.height = 1;
271
272 // ---[ Step 1b : check if any dominant cluster was found
273 if (!clusters.empty ())
274 { // ---[ Step 1b.1 : If yes, compute CVFH using the cluster information
275
276 for (const auto &cluster : clusters) //for each cluster
277 {
278 Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
279 Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
280
281 for (const auto &index : cluster.indices)
282 {
283 avg_normal += (*normals_filtered_cloud)[index].getNormalVector4fMap ();
284 avg_centroid += (*normals_filtered_cloud)[index].getVector4fMap ();
285 }
286
287 avg_normal /= static_cast<float> (cluster.indices.size ());
288 avg_centroid /= static_cast<float> (cluster.indices.size ());
289
290 Eigen::Vector4f centroid_test;
291 pcl::compute3DCentroid (*normals_filtered_cloud, centroid_test);
292 avg_normal.normalize ();
293
294 Eigen::Vector3f avg_norm (avg_normal[0], avg_normal[1], avg_normal[2]);
295 Eigen::Vector3f avg_dominant_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
296
297 //append normal and centroid for the clusters
298 dominant_normals_.push_back (avg_norm);
299 centroids_dominant_orientations_.push_back (avg_dominant_centroid);
300 }
301
302 //compute modified VFH for all dominant clusters and add them to the list!
303 output.resize (dominant_normals_.size ());
304 output.width = dominant_normals_.size ();
305
306 for (std::size_t i = 0; i < dominant_normals_.size (); ++i)
307 {
308 //configure VFH computation for CVFH
309 vfh.setNormalToUse (dominant_normals_[i]);
310 vfh.setCentroidToUse (centroids_dominant_orientations_[i]);
312 vfh.compute (vfh_signature);
313 output[i] = vfh_signature[0];
314 }
315 }
316 else
317 { // ---[ Step 1b.1 : If no, compute CVFH using all the object points
318 Eigen::Vector4f avg_centroid;
319 pcl::compute3DCentroid (*surface_, avg_centroid);
320 Eigen::Vector3f cloud_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
321 centroids_dominant_orientations_.push_back (cloud_centroid);
322
323 //configure VFH computation for CVFH using all object points
324 vfh.setCentroidToUse (cloud_centroid);
325 vfh.setUseGivenNormal (false);
326
328 vfh.compute (vfh_signature);
329
330 output.resize (1);
331 output.width = 1;
332
333 output[0] = vfh_signature[0];
334 }
335}
336
337#define PCL_INSTANTIATE_CVFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::CVFHEstimation<T,NT,OutT>;
338
339#endif // PCL_FEATURES_IMPL_VFH_H_
Define methods for centroid estimation and covariance matrix calculus.
CVFHEstimation estimates the Clustered Viewpoint Feature Histogram (CVFH) descriptor for a given poin...
Definition cvfh.h:63
typename Feature< PointInT, PointOutT >::PointCloudOut PointCloudOut
Definition cvfh.h:76
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 cvfh.hpp:162
void compute(PointCloudOut &output)
Overloaded computed method from pcl::Feature.
Definition cvfh.hpp:51
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
PointCloud represents the base class in PCL for storing collections of 3D points.
void resize(std::size_t count)
Resizes the container to contain count elements.
reference emplace_back(Args &&...args)
Emplace 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
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.
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
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
::pcl::PCLHeader header