1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of Willow Garage, Inc. nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *
34 * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
35 */
36
37 #include <pcl/gpu/containers/initialization.h>
38 #include <pcl/gpu/features/features.hpp>
39 #include <pcl/gpu/utils/device/static_check.hpp>
40 #include "internal.hpp"
41
42 #include <pcl/exceptions.h>
43 #include <pcl/console/print.h>
44
45 using namespace pcl::device;
46
47 /////////////////////////////////////////////////////////////////////////
48 /// Feature
49
Feature()50 pcl::gpu::Feature::Feature() { radius_ = 0.f, max_results_ = 0; }
setInputCloud(const PointCloud & cloud)51 void pcl::gpu::Feature::setInputCloud(const PointCloud& cloud) { cloud_ = cloud; }
setSearchSurface(const PointCloud & surface)52 void pcl::gpu::Feature::setSearchSurface(const PointCloud& surface) { surface_ = surface; }
setIndices(const Indices & indices)53 void pcl::gpu::Feature::setIndices(const Indices& indices) { indices_ = indices; }
setRadiusSearch(float radius,int max_results)54 void pcl::gpu::Feature::setRadiusSearch(float radius, int max_results) { radius_ = radius; max_results_ = max_results; }
55
56 /////////////////////////////////////////////////////////////////////////
57 /// FeatureFromNormals
setInputNormals(const Normals & normals)58 void pcl::gpu::FeatureFromNormals::setInputNormals(const Normals& normals) { normals_ = normals; }
59
60
61 /////////////////////////////////////////////////////////////////////////
62 /// NormalEstimation
NormalEstimation()63 pcl::gpu::NormalEstimation::NormalEstimation() : vpx_(0), vpy_(0), vpz_(0) {}
64
computeNormals(const PointCloud & cloud,const NeighborIndices & nn_indices,Normals & normals)65 void pcl::gpu::NormalEstimation::computeNormals(const PointCloud& cloud, const NeighborIndices& nn_indices, Normals& normals)
66 {
67 normals.create(nn_indices.neighboors_size());
68
69 const device::PointCloud& c = (const device::PointCloud&)cloud;
70 device::Normals& n = (device::Normals&)normals;
71
72 device::computeNormals(c, nn_indices, n);
73 }
74
flipNormalTowardsViewpoint(const PointCloud & cloud,float vp_x,float vp_y,float vp_z,Normals & normals)75 void pcl::gpu::NormalEstimation::flipNormalTowardsViewpoint(const PointCloud& cloud, float vp_x, float vp_y, float vp_z, Normals& normals)
76 {
77 const device::PointCloud& c = (const device::PointCloud&)cloud;
78 device::Normals& n = (device::Normals&)normals;
79
80 device::flipNormalTowardsViewpoint(c, make_float3(vp_x, vp_y, vp_z), n);
81 }
82
flipNormalTowardsViewpoint(const PointCloud & cloud,const Indices & indices,float vp_x,float vp_y,float vp_z,Normals & normals)83 void pcl::gpu::NormalEstimation::flipNormalTowardsViewpoint(const PointCloud& cloud, const Indices& indices, float vp_x, float vp_y, float vp_z, Normals& normals)
84 {
85 const device::PointCloud& c = (const device::PointCloud&)cloud;
86 device::Normals& n = (device::Normals&)normals;
87
88 device::flipNormalTowardsViewpoint(c, indices, make_float3(vp_x, vp_y, vp_z), n);
89 }
90
91
setViewPoint(float vpx,float vpy,float vpz)92 void pcl::gpu::NormalEstimation::setViewPoint (float vpx, float vpy, float vpz)
93 {
94 vpx_ = vpx; vpy_ = vpy; vpz_ = vpz;
95 }
96
getViewPoint(float & vpx,float & vpy,float & vpz) const97 void pcl::gpu::NormalEstimation::getViewPoint (float &vpx, float &vpy, float &vpz) const
98 {
99 vpx = vpx_; vpy = vpy_; vpz = vpz_;
100 }
101
compute(Normals & normals)102 void pcl::gpu::NormalEstimation::compute(Normals& normals)
103 {
104 assert(!cloud_.empty());
105
106 PointCloud& surface = surface_.empty() ? cloud_ : surface_;
107
108 octree_.setCloud(surface);
109 octree_.build();
110
111 if (indices_.empty() || (!indices_.empty() && indices_.size() == cloud_.size()))
112 {
113 octree_.radiusSearch(cloud_, radius_, max_results_, nn_indices_);
114 computeNormals(surface, nn_indices_, normals);
115 flipNormalTowardsViewpoint(cloud_, vpx_, vpy_, vpz_, normals);
116 }
117 else
118 {
119 octree_.radiusSearch(cloud_, indices_, radius_, max_results_, nn_indices_);
120 computeNormals(surface, nn_indices_, normals);
121 flipNormalTowardsViewpoint(cloud_, indices_, vpx_, vpy_, vpz_, normals);
122 }
123 }
124
125
126 /////////////////////////////////////////////////////////////////////////
127 /// PFHEstimation
128
compute(const PointCloud & cloud,const Normals & normals,const NeighborIndices & neighbours,DeviceArray2D<PFHSignature125> & features)129 void pcl::gpu::PFHEstimation::compute(const PointCloud& cloud, const Normals& normals, const NeighborIndices& neighbours, DeviceArray2D<PFHSignature125>& features)
130 {
131 assert( cloud.size() == normals.size() );
132 assert( neighbours.validate(cloud.size()) );
133
134 const device::PointCloud& c = (const device::PointCloud&)cloud;
135 const device::Normals& n = (const device::Normals&)normals;
136
137 features.create (static_cast<int> (neighbours.sizes.size ()), 1);
138
139 DeviceArray2D<device::PFHSignature125>& f = (DeviceArray2D<device::PFHSignature125>&)features;
140
141 repackToAosForPfh(c, n, neighbours, data_rpk, max_elems_rpk);
142 computePfh125(data_rpk, max_elems_rpk, neighbours, f);
143 }
144
compute(DeviceArray2D<PFHSignature125> & features)145 void pcl::gpu::PFHEstimation::compute(DeviceArray2D<PFHSignature125>& features)
146 {
147 PointCloud& surface = surface_.empty() ? cloud_ : surface_;
148
149 octree_.setCloud(surface);
150 octree_.build();
151
152 assert( cloud_.size() == normals_.size());
153
154 if (indices_.empty() || (!indices_.empty() && indices_.size() == cloud_.size()))
155 {
156 octree_.radiusSearch(cloud_, radius_, max_results_, nn_indices_);
157 compute(surface, normals_, nn_indices_, features);
158 }
159 else
160 {
161 octree_.radiusSearch(cloud_, indices_, radius_, max_results_, nn_indices_);
162 compute(surface, normals_, nn_indices_, features);
163 }
164
165 }
166
compute(const PointCloud & cloud,const Normals & normals,const NeighborIndices & neighbours,DeviceArray2D<PFHRGBSignature250> & features)167 void pcl::gpu::PFHRGBEstimation::compute(const PointCloud& cloud, const Normals& normals, const NeighborIndices& neighbours, DeviceArray2D<PFHRGBSignature250>& features)
168 {
169 assert( cloud.size() == normals.size() );
170 assert( neighbours.validate(cloud.size()) );
171
172 const device::PointCloud& c = (const device::PointCloud&)cloud;
173 const device::Normals& n = (const device::Normals&)normals;
174
175 features.create (static_cast<int> (neighbours.sizes.size ()), 1);
176
177 DeviceArray2D<device::PFHRGBSignature250>& f = (DeviceArray2D<device::PFHRGBSignature250>&)features;
178
179 repackToAosForPfhRgb(c, n, neighbours, data_rpk, max_elems_rpk);
180 computePfhRgb250(data_rpk, max_elems_rpk, neighbours, f);
181 }
182
compute(DeviceArray2D<PFHRGBSignature250> & features)183 void pcl::gpu::PFHRGBEstimation::compute(DeviceArray2D<PFHRGBSignature250>& features)
184 {
185 PointCloud& surface = surface_.empty() ? cloud_ : surface_;
186
187 octree_.setCloud(surface);
188 octree_.build();
189
190 assert( cloud_.size() == normals_.size());
191
192 if (indices_.empty() || (!indices_.empty() && indices_.size() == cloud_.size()))
193 {
194 octree_.radiusSearch(cloud_, radius_, max_results_, nn_indices_);
195 compute(surface, normals_, nn_indices_, features);
196 }
197 else
198 {
199 octree_.radiusSearch(cloud_, indices_, radius_, max_results_, nn_indices_);
200 compute(surface, normals_, nn_indices_, features);
201 }
202 }
203
204 /////////////////////////////////////////////////////////////////////////
205 /// FPFHEstimation
206
FPFHEstimation()207 pcl::gpu::FPFHEstimation::FPFHEstimation()
208 {
209 Static<sizeof(FPFHEstimation:: PointType) == sizeof(device:: PointType)>::check();
210 Static<sizeof(FPFHEstimation::NormalType) == sizeof(device::NormalType)>::check();
211 }
212
~FPFHEstimation()213 pcl::gpu::FPFHEstimation::~FPFHEstimation() {}
214
215
compute(const PointCloud & cloud,const Normals & normals,const NeighborIndices & neighbours,DeviceArray2D<FPFHSignature33> & features)216 void pcl::gpu::FPFHEstimation::compute(const PointCloud& cloud, const Normals& normals, const NeighborIndices& neighbours, DeviceArray2D<FPFHSignature33>& features)
217 {
218 assert( cloud.size() == normals.size() );
219 assert( neighbours.validate(cloud.size()) );
220
221 const device::PointCloud& c = (const device::PointCloud&)cloud;
222 const device::Normals& n = (const device::Normals&)normals;
223
224 features.create (static_cast<int> (cloud.size ()), 1);
225 spfh.create (static_cast<int> (cloud.size ()), 1);
226
227 DeviceArray2D<device::FPFHSignature33>& s = (DeviceArray2D<device::FPFHSignature33>&)spfh;
228 DeviceArray2D<device::FPFHSignature33>& f = (DeviceArray2D<device::FPFHSignature33>&)features;
229
230 device::computeSPFH(c, n, device::Indices(), neighbours, s);
231 device::computeFPFH(c, neighbours, s, f);
232 }
233
compute(DeviceArray2D<FPFHSignature33> & features)234 void pcl::gpu::FPFHEstimation::compute(DeviceArray2D<FPFHSignature33>& features)
235 {
236 bool hasInds = !indices_.empty() && indices_.size() != cloud_.size();
237 bool hasSurf = !surface_.empty();
238
239 features.create (static_cast<int> (hasInds ? indices_.size () : cloud_.size ()), 1);
240
241 if (!hasInds && !hasSurf)
242 {
243 features.create (static_cast<int> (cloud_.size ()), 1);
244 octree_.setCloud(cloud_);
245 octree_.build();
246 assert( cloud_.size() == normals_.size());
247 octree_.radiusSearch(cloud_, radius_, max_results_, nn_indices_);
248 compute(cloud_, normals_, nn_indices_, features);
249 return;
250 }
251
252 PointCloud& surface = surface_.empty() ? cloud_ : surface_;
253
254 octree_.setCloud(surface);
255 octree_.build();
256
257 if (hasInds)
258 octree_.radiusSearch(cloud_, indices_, radius_, max_results_, nn_indices_);
259 else
260 octree_.radiusSearch(cloud_, radius_, max_results_, nn_indices_);
261
262 int total = computeUniqueIndices(surface.size(), nn_indices_, unique_indices_storage, lookup);
263
264 DeviceArray<int> unique_indices(unique_indices_storage.ptr(), total);
265 octree_.radiusSearch(surface, unique_indices, radius_, max_results_, nn_indices2_);
266
267 DeviceArray2D<device::FPFHSignature33>& spfh33 = (DeviceArray2D<device::FPFHSignature33>&)spfh;
268 const device::PointCloud& c = (const device::PointCloud&)cloud_;
269 const device::PointCloud& s = (const device::PointCloud&)surface;
270 const device::Normals& n = (const device::Normals&)normals_;
271 device::computeSPFH(s, n, unique_indices, nn_indices2_, spfh33);
272
273 DeviceArray2D<device::FPFHSignature33>& f = (DeviceArray2D<device::FPFHSignature33>&)features;
274 device::computeFPFH(c, indices_, s, nn_indices_, lookup, spfh33, f);
275 }
276
277
278 /////////////////////////////////////////////////////////////////////////
279 /// PPFEstimation
280
compute(DeviceArray<PPFSignature> & features)281 void pcl::gpu::PPFEstimation::compute(DeviceArray<PPFSignature>& features)
282 {
283 Static<sizeof(PPFEstimation:: PointType) == sizeof(device:: PointType)>::check();
284 Static<sizeof(PPFEstimation::NormalType) == sizeof(device::NormalType)>::check();
285
286 assert(this->surface_.empty() && !indices_.empty() && !cloud_.empty() && normals_.size() == cloud_.size());
287 features.create(indices_.size () * cloud_.size ());
288
289 const device::PointCloud& c = (const device::PointCloud&)cloud_;
290 const device::Normals& n = (const device::Normals&)normals_;
291
292 DeviceArray<device::PPFSignature>& f = (DeviceArray<device::PPFSignature>&)features;
293 device::computePPF(c, n, indices_, f);
294 }
295
296 /////////////////////////////////////////////////////////////////////////
297 /// PPFRGBEstimation
298
compute(DeviceArray<PPFRGBSignature> & features)299 void pcl::gpu::PPFRGBEstimation::compute(DeviceArray<PPFRGBSignature>& features)
300 {
301 Static<sizeof(PPFEstimation:: PointType) == sizeof(device:: PointType)>::check();
302 Static<sizeof(PPFEstimation::NormalType) == sizeof(device::NormalType)>::check();
303
304 assert(this->surface_.empty() && !indices_.empty() && !cloud_.empty() && normals_.size() == cloud_.size());
305 features.create(indices_.size () * cloud_.size ());
306
307 const device::PointCloud& c = (const device::PointCloud&)cloud_;
308 const device::Normals& n = (const device::Normals&)normals_;
309
310 DeviceArray<device::PPFRGBSignature>& f = (DeviceArray<device::PPFRGBSignature>&)features;
311 device::computePPFRGB(c, n, indices_, f);
312 }
313
314 /////////////////////////////////////////////////////////////////////////
315 /// PPFRGBRegionEstimation
316
compute(DeviceArray<PPFRGBSignature> & features)317 void pcl::gpu::PPFRGBRegionEstimation::compute(DeviceArray<PPFRGBSignature>& features)
318 {
319 Static<sizeof(PPFRGBRegionEstimation:: PointType) == sizeof(device:: PointType)>::check();
320 Static<sizeof(PPFRGBRegionEstimation::NormalType) == sizeof(device::NormalType)>::check();
321
322 assert(this->surface_.empty() && !indices_.empty() && !cloud_.empty() && normals_.size() == cloud_.size());
323
324 features.create(indices_.size());
325
326 octree_.setCloud(cloud_);
327 octree_.build();
328
329 octree_.radiusSearch(cloud_, indices_, radius_, max_results_, nn_indices_);
330
331 const device::PointCloud& c = (const device::PointCloud&)cloud_;
332 const device::Normals& n = (const device::Normals&)normals_;
333
334 DeviceArray<device::PPFRGBSignature>& f = (DeviceArray<device::PPFRGBSignature>&)features;
335
336 device::computePPFRGBRegion(c, n, indices_, nn_indices_, f);
337 }
338
339 /////////////////////////////////////////////////////////////////////////
340 /// PrincipalCurvaturesEstimation
341
compute(DeviceArray<PrincipalCurvatures> & features)342 void pcl::gpu::PrincipalCurvaturesEstimation::compute(DeviceArray<PrincipalCurvatures>& features)
343 {
344 Static<sizeof(PPFRGBRegionEstimation:: PointType) == sizeof(device:: PointType)>::check();
345 Static<sizeof(PPFRGBRegionEstimation::NormalType) == sizeof(device::NormalType)>::check();
346
347 assert(/*!indices_.empty() && */!cloud_.empty() && max_results_ > 0 && radius_ > 0.f);
348 assert(surface_.empty() ? normals_.size() == cloud_.size() : normals_.size() == surface_.size());
349
350 PointCloud& surface = surface_.empty() ? cloud_ : surface_;
351
352 octree_.setCloud(surface);
353 octree_.build();
354
355 if(indices_.empty())
356 octree_.radiusSearch(cloud_, radius_, max_results_, nn_indices_);
357 else
358 octree_.radiusSearch(cloud_, indices_, radius_, max_results_, nn_indices_);
359
360 const device::Normals& n = (const device::Normals&)normals_;
361
362 features.create(normals_.size());
363
364 DeviceArray<device::PrincipalCurvatures>& f = (DeviceArray<device::PrincipalCurvatures>&)features;
365
366 device::computePointPrincipalCurvatures(n, indices_, nn_indices_, f, proj_normals_buf);
367 }
368
369
370 /////////////////////////////////////////////////////////////////////////
371 /// VFHEstimation
372
VFHEstimation()373 pcl::gpu::VFHEstimation::VFHEstimation()
374 {
375 vpx_ = vpy_ = vpz_ = 0.f;
376
377 //default parameters to compute VFH
378 use_given_normal_ = false;
379 use_given_centroid_ = false;
380 normalize_bins_ = true;
381 normalize_distances_ = false;
382 size_component_ = false;
383 }
384
setViewPoint(float vpx,float vpy,float vpz)385 void pcl::gpu::VFHEstimation::setViewPoint(float vpx, float vpy, float vpz) { vpx_ = vpx; vpy_ = vpy; vpz_ = vpz; }
getViewPoint(float & vpx,float & vpy,float & vpz) const386 void pcl::gpu::VFHEstimation::getViewPoint(float& vpx, float& vpy, float& vpz) const { vpx = vpx_; vpy = vpy_; vpz = vpz_; }
387
setUseGivenNormal(bool use)388 void pcl::gpu::VFHEstimation::setUseGivenNormal (bool use) { use_given_normal_ = use; }
setNormalToUse(const NormalType & normal)389 void pcl::gpu::VFHEstimation::setNormalToUse (const NormalType& normal) { normal_to_use_ = normal; }
setUseGivenCentroid(bool use)390 void pcl::gpu::VFHEstimation::setUseGivenCentroid (bool use) { use_given_centroid_ = use; }
setCentroidToUse(const PointType & centroid)391 void pcl::gpu::VFHEstimation::setCentroidToUse (const PointType& centroid) { centroid_to_use_ = centroid; }
392
setNormalizeBins(bool normalize)393 void pcl::gpu::VFHEstimation::setNormalizeBins (bool normalize) { normalize_bins_ = normalize; }
setNormalizeDistance(bool normalize)394 void pcl::gpu::VFHEstimation::setNormalizeDistance (bool normalize) { normalize_distances_ = normalize; }
setFillSizeComponent(bool fill_size)395 void pcl::gpu::VFHEstimation::setFillSizeComponent (bool fill_size) { size_component_ = fill_size; }
396
397 ////////////////////////////////////////////////////////////////////////////////////////////
398
compute(DeviceArray<VFHSignature308> & feature)399 void pcl::gpu::VFHEstimation::compute(DeviceArray<VFHSignature308>& feature)
400 {
401 assert(!surface_.empty() && normals_.size() == surface_.size() && cloud_.empty());
402
403 Static<sizeof(VFHEstimation:: PointType) == sizeof(device:: PointType)>::check();
404 Static<sizeof(VFHEstimation::NormalType) == sizeof(device::NormalType)>::check();
405
406 feature.create(1);
407
408 VFHEstimationImpl impl;
409
410 const device::PointCloud& s = (const device::PointCloud&)surface_;
411 const device::Normals& n = (const device::Normals&)normals_;
412
413 if (use_given_centroid_)
414 {
415 impl.xyz_centroid.x = centroid_to_use_.x;
416 impl.xyz_centroid.y = centroid_to_use_.y;
417 impl.xyz_centroid.z = centroid_to_use_.z;
418 }
419 else
420 {
421 compute3DCentroid(s, indices_, impl.xyz_centroid);
422
423 }
424 if (use_given_normal_)
425 {
426 impl.normal_centroid.x = normal_to_use_.x;
427 impl.normal_centroid.y = normal_to_use_.y;
428 impl.normal_centroid.z = normal_to_use_.z;
429 }
430 else
431 compute3DCentroid (n, indices_, impl.normal_centroid);
432
433 impl.viewpoint = make_float3(vpx_, vpy_, vpz_);
434
435
436 impl.indices = indices_;
437 impl.points = s;
438 impl.normals = n;
439
440 impl.normalize_distances = normalize_distances_;
441 impl.size_component = size_component_;
442 impl.normalize_bins = normalize_bins_;
443
444 DeviceArray<device::VFHSignature308>& f = (DeviceArray<device::VFHSignature308>&)feature;
445 impl.compute(f);
446 }
447
448 /////////////////////////////////////////////////////////////////////////
449 /// SpinImageEstimation
450
setImageWidth(unsigned int bin_count)451 void pcl::gpu::SpinImageEstimation::setImageWidth (unsigned int bin_count) { image_width_ = bin_count; }
setSupportAngle(float support_angle_cos)452 void pcl::gpu::SpinImageEstimation::setSupportAngle (float support_angle_cos)
453 {
454 if (0.f > support_angle_cos || support_angle_cos > 1.f) // may be permit negative cosine?
455 pcl::gpu::error("Cosine of support angle should be between 0 and 1", __FILE__, __LINE__);
456 support_angle_cos_ = support_angle_cos;
457 }
458
setMinPointCountInNeighbourhood(unsigned int min_pts_neighb)459 void pcl::gpu::SpinImageEstimation::setMinPointCountInNeighbourhood (unsigned int min_pts_neighb) { min_pts_neighb_ = min_pts_neighb; }
setInputWithNormals(const PointCloud & input,const Normals & normals)460 void pcl::gpu::SpinImageEstimation::setInputWithNormals(const PointCloud& input, const Normals& normals)
461 {
462 setInputCloud (input);
463 input_normals_ = normals;
464 }
465
setSearchSurfaceWithNormals(const PointCloud & surface,const Normals & normals)466 void pcl::gpu::SpinImageEstimation::setSearchSurfaceWithNormals (const PointCloud& surface, const Normals& normals)
467 {
468 setSearchSurface (surface);
469 setInputNormals (normals);
470 }
471
setRotationAxis(const NormalType & axis)472 void pcl::gpu::SpinImageEstimation::setRotationAxis (const NormalType& axis)
473 {
474 rotation_axis_ = axis;
475 use_custom_axis_ = true;
476 use_custom_axes_cloud_ = false;
477 }
478
setInputRotationAxes(const Normals & axes)479 void pcl::gpu::SpinImageEstimation::setInputRotationAxes (const Normals& axes)
480 {
481 rotation_axes_cloud_ = axes;
482 use_custom_axes_cloud_ = true;
483 use_custom_axis_ = false;
484 }
485
useNormalsAsRotationAxis()486 void pcl::gpu::SpinImageEstimation::useNormalsAsRotationAxis () { use_custom_axis_ = false; use_custom_axes_cloud_ = false; }
setAngularDomain(bool is_angular)487 void pcl::gpu::SpinImageEstimation::setAngularDomain (bool is_angular) { is_angular_ = is_angular; }
setRadialStructure(bool is_radial)488 void pcl::gpu::SpinImageEstimation::setRadialStructure (bool is_radial) { is_radial_ = is_radial; }
489
490 //////////////////////////////////////////////////////////////////////////////////////////////
491
SpinImageEstimation(unsigned int image_width,double support_angle_cos,unsigned int min_pts_neighb)492 pcl::gpu::SpinImageEstimation::SpinImageEstimation (unsigned int image_width, double support_angle_cos, unsigned int min_pts_neighb)
493 : is_angular_(false), use_custom_axis_(false), use_custom_axes_cloud_(false), is_radial_(false),
494 image_width_(image_width), support_angle_cos_(support_angle_cos), min_pts_neighb_(min_pts_neighb)
495 {
496 assert (support_angle_cos_ <= 1.0 && support_angle_cos_ >= 0.0);
497 }
498
499 ////////////////////////////////////////////////////////////////////////////////////////////
compute(DeviceArray2D<SpinImage> & features,DeviceArray<unsigned char> & mask)500 void pcl::gpu::SpinImageEstimation::compute(DeviceArray2D<SpinImage>& features, DeviceArray<unsigned char>& mask)
501 {
502 assert(!indices_.empty());
503
504 if (image_width_ != 8)
505 pcl::gpu::error("Currently only image_width = 8 is supported (less is possible right now, more - need to allocate more memory)", __FILE__, __LINE__);
506
507 Static<sizeof(SpinImageEstimation:: PointType) == sizeof(device:: PointType)>::check();
508 Static<sizeof(SpinImageEstimation::NormalType) == sizeof(device::NormalType)>::check();
509
510 features.create (static_cast<int> (indices_.size ()), 1);
511 mask.create(indices_.size());
512
513 //////////////////////////////
514 if (!surface_)
515 {
516 surface_ = cloud_;
517 normals_ = input_normals_;
518 fake_surface_ = true;
519 }
520
521 assert(!(use_custom_axis_ && use_custom_axes_cloud_));
522
523 if (!use_custom_axis_ && !use_custom_axes_cloud_ && !input_normals_)
524 pcl::gpu::error("No normals for input cloud were given!", __FILE__, __LINE__);
525
526 if ((is_angular_ || support_angle_cos_ > 0.0) && !input_normals_)
527 pcl::gpu::error("No normals for input cloud were given!", __FILE__, __LINE__);
528
529 if (use_custom_axes_cloud_ && rotation_axes_cloud_.size () != cloud_.size ())
530 pcl::gpu::error("Rotation axis cloud have different size from input!", __FILE__, __LINE__);
531
532 ///////////////////////////////////////////////
533 octree_.setCloud(surface_);
534 octree_.build();
535 octree_.radiusSearch(cloud_, indices_, radius_, max_results_, nn_indices_);
536
537 // OK, we are interested in the points of the cylinder of height 2*r and base radius r, where r = m_dBinSize * in_iImageWidth
538 // it can be embedded to the sphere of radius sqrt(2) * m_dBinSize * in_iImageWidth
539 // suppose that points are uniformly distributed, so we lose ~40% // according to the volumes ratio
540 float bin_size = radius_ / image_width_;
541 if (!is_radial_)
542 bin_size /= ::sqrt(2.f);
543
544
545 const device::PointCloud& s = (const device::PointCloud&)surface_;
546 const device::PointCloud& c = (const device::PointCloud&)cloud_;
547 const device::Normals& in = (const device::Normals&)input_normals_;
548 const device::Normals& n = (const device::Normals&)normals_;
549
550
551 if (use_custom_axis_)
552 {
553 float3 axis = make_float3(rotation_axis_.x, rotation_axis_.y, rotation_axis_.z);
554 computeSpinImagesCustomAxes(is_radial_, is_angular_, support_angle_cos_, indices_, c, in,
555 s, n, nn_indices_, min_pts_neighb_, image_width_, bin_size, axis, features);
556 }
557 else if (use_custom_axes_cloud_)
558 {
559 const device::Normals& axes = (const device::Normals&)rotation_axes_cloud_;
560
561 computeSpinImagesCustomAxesCloud(is_radial_, is_angular_, support_angle_cos_, indices_, c, in,
562 s, n, nn_indices_, min_pts_neighb_, image_width_, bin_size, axes, features);
563 }
564 else
565 {
566 computeSpinImagesOrigigNormal(is_radial_, is_angular_, support_angle_cos_, indices_, c, in,
567 s, n, nn_indices_, min_pts_neighb_, image_width_, bin_size, features);
568 }
569
570 computeMask(nn_indices_, min_pts_neighb_, mask);
571 }
572