Point Cloud Library (PCL)  1.7.1
esf.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, 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: pfh.hpp 5027 2012-03-12 03:10:45Z rusu $
38  *
39  */
40 
41 #ifndef PCL_FEATURES_IMPL_ESF_H_
42 #define PCL_FEATURES_IMPL_ESF_H_
43 
44 #include <pcl/features/esf.h>
45 #include <pcl/common/common.h>
46 #include <pcl/common/distances.h>
47 #include <pcl/common/transforms.h>
48 #include <vector>
49 
50 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
51 template <typename PointInT, typename PointOutT> void
53  PointCloudIn &pc, std::vector<float> &hist)
54 {
55  const int binsize = 64;
56  unsigned int sample_size = 20000;
57  srand (static_cast<unsigned int> (time (0)));
58  int maxindex = static_cast<int> (pc.points.size ());
59 
60  int index1, index2, index3;
61  std::vector<float> d2v, d1v, d3v, wt_d3;
62  std::vector<int> wt_d2;
63  d1v.reserve (sample_size);
64  d2v.reserve (sample_size * 3);
65  d3v.reserve (sample_size);
66  wt_d2.reserve (sample_size * 3);
67  wt_d3.reserve (sample_size);
68 
69  float h_in[binsize] = {0};
70  float h_out[binsize] = {0};
71  float h_mix[binsize] = {0};
72  float h_mix_ratio[binsize] = {0};
73 
74  float h_a3_in[binsize] = {0};
75  float h_a3_out[binsize] = {0};
76  float h_a3_mix[binsize] = {0};
77  float h_d1[binsize] = {0};
78 
79  float h_d3_in[binsize] = {0};
80  float h_d3_out[binsize] = {0};
81  float h_d3_mix[binsize] = {0};
82 
83  float ratio=0.0;
84  float pih = static_cast<float>(M_PI) / 2.0f;
85  float a,b,c,s;
86  int th1,th2,th3;
87  int vxlcnt = 0;
88  int pcnt1,pcnt2,pcnt3;
89  for (size_t nn_idx = 0; nn_idx < sample_size; ++nn_idx)
90  {
91  // get a new random point
92  index1 = rand()%maxindex;
93  index2 = rand()%maxindex;
94  index3 = rand()%maxindex;
95 
96  if (index1==index2 || index1 == index3 || index2 == index3)
97  {
98  nn_idx--;
99  continue;
100  }
101 
102  Eigen::Vector4f p1 = pc.points[index1].getVector4fMap ();
103  Eigen::Vector4f p2 = pc.points[index2].getVector4fMap ();
104  Eigen::Vector4f p3 = pc.points[index3].getVector4fMap ();
105 
106  // A3
107  Eigen::Vector4f v21 (p2 - p1);
108  Eigen::Vector4f v31 (p3 - p1);
109  Eigen::Vector4f v23 (p2 - p3);
110  a = v21.norm (); b = v31.norm (); c = v23.norm (); s = (a+b+c) * 0.5f;
111  if (s * (s-a) * (s-b) * (s-c) <= 0.001f)
112  continue;
113 
114  v21.normalize ();
115  v31.normalize ();
116  v23.normalize ();
117 
118  //TODO: .dot gives nan's
119  th1 = static_cast<int> (pcl_round (acos (fabs (v21.dot (v31))) / pih * (binsize-1)));
120  th2 = static_cast<int> (pcl_round (acos (fabs (v23.dot (v31))) / pih * (binsize-1)));
121  th3 = static_cast<int> (pcl_round (acos (fabs (v23.dot (v21))) / pih * (binsize-1)));
122  if (th1 < 0 || th1 >= binsize)
123  {
124  nn_idx--;
125  continue;
126  }
127  if (th2 < 0 || th2 >= binsize)
128  {
129  nn_idx--;
130  continue;
131  }
132  if (th3 < 0 || th3 >= binsize)
133  {
134  nn_idx--;
135  continue;
136  }
137 
138  // D2
139  d2v.push_back (pcl::euclideanDistance (pc.points[index1], pc.points[index2]));
140  d2v.push_back (pcl::euclideanDistance (pc.points[index1], pc.points[index3]));
141  d2v.push_back (pcl::euclideanDistance (pc.points[index2], pc.points[index3]));
142 
143  int vxlcnt_sum = 0;
144  int p_cnt = 0;
145  // IN, OUT, MIXED, Ratio line tracing, index1->index2
146  {
147  const int xs = p1[0] < 0.0? static_cast<int>(floor(p1[0])+GRIDSIZE_H): static_cast<int>(ceil(p1[0])+GRIDSIZE_H-1);
148  const int ys = p1[1] < 0.0? static_cast<int>(floor(p1[1])+GRIDSIZE_H): static_cast<int>(ceil(p1[1])+GRIDSIZE_H-1);
149  const int zs = p1[2] < 0.0? static_cast<int>(floor(p1[2])+GRIDSIZE_H): static_cast<int>(ceil(p1[2])+GRIDSIZE_H-1);
150  const int xt = p2[0] < 0.0? static_cast<int>(floor(p2[0])+GRIDSIZE_H): static_cast<int>(ceil(p2[0])+GRIDSIZE_H-1);
151  const int yt = p2[1] < 0.0? static_cast<int>(floor(p2[1])+GRIDSIZE_H): static_cast<int>(ceil(p2[1])+GRIDSIZE_H-1);
152  const int zt = p2[2] < 0.0? static_cast<int>(floor(p2[2])+GRIDSIZE_H): static_cast<int>(ceil(p2[2])+GRIDSIZE_H-1);
153  wt_d2.push_back (this->lci (xs, ys, zs, xt, yt, zt, ratio, vxlcnt, pcnt1));
154  if (wt_d2.back () == 2)
155  h_mix_ratio[static_cast<int> (pcl_round (ratio * (binsize-1)))]++;
156  vxlcnt_sum += vxlcnt;
157  p_cnt += pcnt1;
158  }
159  // IN, OUT, MIXED, Ratio line tracing, index1->index3
160  {
161  const int xs = p1[0] < 0.0? static_cast<int>(floor(p1[0])+GRIDSIZE_H): static_cast<int>(ceil(p1[0])+GRIDSIZE_H-1);
162  const int ys = p1[1] < 0.0? static_cast<int>(floor(p1[1])+GRIDSIZE_H): static_cast<int>(ceil(p1[1])+GRIDSIZE_H-1);
163  const int zs = p1[2] < 0.0? static_cast<int>(floor(p1[2])+GRIDSIZE_H): static_cast<int>(ceil(p1[2])+GRIDSIZE_H-1);
164  const int xt = p3[0] < 0.0? static_cast<int>(floor(p3[0])+GRIDSIZE_H): static_cast<int>(ceil(p3[0])+GRIDSIZE_H-1);
165  const int yt = p3[1] < 0.0? static_cast<int>(floor(p3[1])+GRIDSIZE_H): static_cast<int>(ceil(p3[1])+GRIDSIZE_H-1);
166  const int zt = p3[2] < 0.0? static_cast<int>(floor(p3[2])+GRIDSIZE_H): static_cast<int>(ceil(p3[2])+GRIDSIZE_H-1);
167  wt_d2.push_back (this->lci (xs, ys, zs, xt, yt, zt, ratio, vxlcnt, pcnt2));
168  if (wt_d2.back () == 2)
169  h_mix_ratio[static_cast<int>(pcl_round (ratio * (binsize-1)))]++;
170  vxlcnt_sum += vxlcnt;
171  p_cnt += pcnt2;
172  }
173  // IN, OUT, MIXED, Ratio line tracing, index2->index3
174  {
175  const int xs = p2[0] < 0.0? static_cast<int>(floor(p2[0])+GRIDSIZE_H): static_cast<int>(ceil(p2[0])+GRIDSIZE_H-1);
176  const int ys = p2[1] < 0.0? static_cast<int>(floor(p2[1])+GRIDSIZE_H): static_cast<int>(ceil(p2[1])+GRIDSIZE_H-1);
177  const int zs = p2[2] < 0.0? static_cast<int>(floor(p2[2])+GRIDSIZE_H): static_cast<int>(ceil(p2[2])+GRIDSIZE_H-1);
178  const int xt = p3[0] < 0.0? static_cast<int>(floor(p3[0])+GRIDSIZE_H): static_cast<int>(ceil(p3[0])+GRIDSIZE_H-1);
179  const int yt = p3[1] < 0.0? static_cast<int>(floor(p3[1])+GRIDSIZE_H): static_cast<int>(ceil(p3[1])+GRIDSIZE_H-1);
180  const int zt = p3[2] < 0.0? static_cast<int>(floor(p3[2])+GRIDSIZE_H): static_cast<int>(ceil(p3[2])+GRIDSIZE_H-1);
181  wt_d2.push_back (this->lci (xs,ys,zs,xt,yt,zt,ratio,vxlcnt,pcnt3));
182  if (wt_d2.back () == 2)
183  h_mix_ratio[static_cast<int>(pcl_round(ratio * (binsize-1)))]++;
184  vxlcnt_sum += vxlcnt;
185  p_cnt += pcnt3;
186  }
187 
188  // D3 ( herons formula )
189  d3v.push_back (sqrtf (sqrtf (s * (s-a) * (s-b) * (s-c))));
190  if (vxlcnt_sum <= 21)
191  {
192  wt_d3.push_back (0);
193  h_a3_out[th1] += static_cast<float> (pcnt3) / 32.0f;
194  h_a3_out[th2] += static_cast<float> (pcnt1) / 32.0f;
195  h_a3_out[th3] += static_cast<float> (pcnt2) / 32.0f;
196  }
197  else
198  if (p_cnt - vxlcnt_sum < 4)
199  {
200  h_a3_in[th1] += static_cast<float> (pcnt3) / 32.0f;
201  h_a3_in[th2] += static_cast<float> (pcnt1) / 32.0f;
202  h_a3_in[th3] += static_cast<float> (pcnt2) / 32.0f;
203  wt_d3.push_back (1);
204  }
205  else
206  {
207  h_a3_mix[th1] += static_cast<float> (pcnt3) / 32.0f;
208  h_a3_mix[th2] += static_cast<float> (pcnt1) / 32.0f;
209  h_a3_mix[th3] += static_cast<float> (pcnt2) / 32.0f;
210  wt_d3.push_back (static_cast<float> (vxlcnt_sum) / static_cast<float> (p_cnt));
211  }
212  }
213  // Normalizing, get max
214  float maxd2 = 0;
215  float maxd3 = 0;
216 
217  for (size_t nn_idx = 0; nn_idx < sample_size; ++nn_idx)
218  {
219  // get max of Dx
220  if (d2v[nn_idx] > maxd2)
221  maxd2 = d2v[nn_idx];
222  if (d2v[sample_size + nn_idx] > maxd2)
223  maxd2 = d2v[sample_size + nn_idx];
224  if (d2v[sample_size*2 +nn_idx] > maxd2)
225  maxd2 = d2v[sample_size*2 +nn_idx];
226  if (d3v[nn_idx] > maxd3)
227  maxd3 = d3v[nn_idx];
228  }
229 
230  // Normalize and create histogram
231  int index;
232  for (size_t nn_idx = 0; nn_idx < sample_size; ++nn_idx)
233  {
234  if (wt_d3[nn_idx] >= 0.999) // IN
235  {
236  index = static_cast<int>(pcl_round (d3v[nn_idx] / maxd3 * (binsize-1)));
237  if (index >= 0 && index < binsize)
238  h_d3_in[index]++;
239  }
240  else
241  {
242  if (wt_d3[nn_idx] <= 0.001) // OUT
243  {
244  index = static_cast<int>(pcl_round (d3v[nn_idx] / maxd3 * (binsize-1)));
245  if (index >= 0 && index < binsize)
246  h_d3_out[index]++ ;
247  }
248  else
249  {
250  index = static_cast<int>(pcl_round (d3v[nn_idx] / maxd3 * (binsize-1)));
251  if (index >= 0 && index < binsize)
252  h_d3_mix[index]++;
253  }
254  }
255  }
256  //normalize and create histogram
257  for (size_t nn_idx = 0; nn_idx < d2v.size(); ++nn_idx )
258  {
259  if (wt_d2[nn_idx] == 0)
260  h_in[static_cast<int>(pcl_round (d2v[nn_idx] / maxd2 * (binsize-1)))]++ ;
261  if (wt_d2[nn_idx] == 1)
262  h_out[static_cast<int>(pcl_round (d2v[nn_idx] / maxd2 * (binsize-1)))]++;
263  if (wt_d2[nn_idx] == 2)
264  h_mix[static_cast<int>(pcl_round (d2v[nn_idx] / maxd2 * (binsize-1)))]++ ;
265  }
266 
267  //float weights[10] = {1, 1, 1, 1, 1, 1, 1, 1 , 1 , 1};
268  float weights[10] = {0.5f, 0.5f, 0.5f, 0.5f, 0.5f, 1.0f, 1.0f, 2.0f, 2.0f, 2.0f};
269 
270  hist.reserve (binsize * 10);
271  for (int i = 0; i < binsize; i++)
272  hist.push_back (h_a3_in[i] * weights[0]);
273  for (int i = 0; i < binsize; i++)
274  hist.push_back (h_a3_out[i] * weights[1]);
275  for (int i = 0; i < binsize; i++)
276  hist.push_back (h_a3_mix[i] * weights[2]);
277 
278  for (int i = 0; i < binsize; i++)
279  hist.push_back (h_d3_in[i] * weights[3]);
280  for (int i = 0; i < binsize; i++)
281  hist.push_back (h_d3_out[i] * weights[4]);
282  for (int i = 0; i < binsize; i++)
283  hist.push_back (h_d3_mix[i] * weights[5]);
284 
285  for (int i = 0; i < binsize; i++)
286  hist.push_back (h_in[i]*0.5f * weights[6]);
287  for (int i = 0; i < binsize; i++)
288  hist.push_back (h_out[i] * weights[7]);
289  for (int i = 0; i < binsize; i++)
290  hist.push_back (h_mix[i] * weights[8]);
291  for (int i = 0; i < binsize; i++)
292  hist.push_back (h_mix_ratio[i]*0.5f * weights[9]);
293 
294  float sm = 0;
295  for (size_t i = 0; i < hist.size (); i++)
296  sm += hist[i];
297 
298  for (size_t i = 0; i < hist.size (); i++)
299  hist[i] /= sm;
300 }
301 
302 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
303 template <typename PointInT, typename PointOutT> int
305  const int x1, const int y1, const int z1,
306  const int x2, const int y2, const int z2,
307  float &ratio, int &incnt, int &pointcount)
308 {
309  int voxelcount = 0;
310  int voxel_in = 0;
311  int act_voxel[3];
312  act_voxel[0] = x1;
313  act_voxel[1] = y1;
314  act_voxel[2] = z1;
315  int x_inc, y_inc, z_inc;
316  int dx = x2 - x1;
317  int dy = y2 - y1;
318  int dz = z2 - z1;
319  if (dx < 0)
320  x_inc = -1;
321  else
322  x_inc = 1;
323  int l = abs (dx);
324  if (dy < 0)
325  y_inc = -1 ;
326  else
327  y_inc = 1;
328  int m = abs (dy);
329  if (dz < 0)
330  z_inc = -1 ;
331  else
332  z_inc = 1;
333  int n = abs (dz);
334  int dx2 = 2 * l;
335  int dy2 = 2 * m;
336  int dz2 = 2 * n;
337  if ((l >= m) & (l >= n))
338  {
339  int err_1 = dy2 - l;
340  int err_2 = dz2 - l;
341  for (int i = 1; i<l; i++)
342  {
343  voxelcount++;;
344  voxel_in += static_cast<int>(lut_[act_voxel[0]][act_voxel[1]][act_voxel[2]] == 1);
345  if (err_1 > 0)
346  {
347  act_voxel[1] += y_inc;
348  err_1 -= dx2;
349  }
350  if (err_2 > 0)
351  {
352  act_voxel[2] += z_inc;
353  err_2 -= dx2;
354  }
355  err_1 += dy2;
356  err_2 += dz2;
357  act_voxel[0] += x_inc;
358  }
359  }
360  else if ((m >= l) & (m >= n))
361  {
362  int err_1 = dx2 - m;
363  int err_2 = dz2 - m;
364  for (int i=1; i<m; i++)
365  {
366  voxelcount++;
367  voxel_in += static_cast<int>(lut_[act_voxel[0]][act_voxel[1]][act_voxel[2]] == 1);
368  if (err_1 > 0)
369  {
370  act_voxel[0] += x_inc;
371  err_1 -= dy2;
372  }
373  if (err_2 > 0)
374  {
375  act_voxel[2] += z_inc;
376  err_2 -= dy2;
377  }
378  err_1 += dx2;
379  err_2 += dz2;
380  act_voxel[1] += y_inc;
381  }
382  }
383  else
384  {
385  int err_1 = dy2 - n;
386  int err_2 = dx2 - n;
387  for (int i=1; i<n; i++)
388  {
389  voxelcount++;
390  voxel_in += static_cast<int>(lut_[act_voxel[0]][act_voxel[1]][act_voxel[2]] == 1);
391  if (err_1 > 0)
392  {
393  act_voxel[1] += y_inc;
394  err_1 -= dz2;
395  }
396  if (err_2 > 0)
397  {
398  act_voxel[0] += x_inc;
399  err_2 -= dz2;
400  }
401  err_1 += dy2;
402  err_2 += dx2;
403  act_voxel[2] += z_inc;
404  }
405  }
406  voxelcount++;
407  voxel_in += static_cast<int>(lut_[act_voxel[0]][act_voxel[1]][act_voxel[2]] == 1);
408  incnt = voxel_in;
409  pointcount = voxelcount;
410 
411  if (voxel_in >= voxelcount-1)
412  return (0);
413 
414  if (voxel_in <= 7)
415  return (1);
416 
417  ratio = static_cast<float>(voxel_in) / static_cast<float>(voxelcount);
418  return (2);
419 }
420 
421 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
422 template <typename PointInT, typename PointOutT> void
424 {
425  int xi,yi,zi,xx,yy,zz;
426  for (size_t i = 0; i < cluster.points.size (); ++i)
427  {
428  xx = cluster.points[i].x<0.0? static_cast<int>(floor(cluster.points[i].x)+GRIDSIZE_H) : static_cast<int>(ceil(cluster.points[i].x)+GRIDSIZE_H-1);
429  yy = cluster.points[i].y<0.0? static_cast<int>(floor(cluster.points[i].y)+GRIDSIZE_H) : static_cast<int>(ceil(cluster.points[i].y)+GRIDSIZE_H-1);
430  zz = cluster.points[i].z<0.0? static_cast<int>(floor(cluster.points[i].z)+GRIDSIZE_H) : static_cast<int>(ceil(cluster.points[i].z)+GRIDSIZE_H-1);
431 
432  for (int x = -1; x < 2; x++)
433  for (int y = -1; y < 2; y++)
434  for (int z = -1; z < 2; z++)
435  {
436  xi = xx + x;
437  yi = yy + y;
438  zi = zz + z;
439 
440  if (yi >= GRIDSIZE || xi >= GRIDSIZE || zi>=GRIDSIZE || yi < 0 || xi < 0 || zi < 0)
441  {
442  ;
443  }
444  else
445  this->lut_[xi][yi][zi] = 1;
446  }
447  }
448 }
449 
450 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
451 template <typename PointInT, typename PointOutT> void
453 {
454  int xi,yi,zi,xx,yy,zz;
455  for (size_t i = 0; i < cluster.points.size (); ++i)
456  {
457  xx = cluster.points[i].x<0.0? static_cast<int>(floor(cluster.points[i].x)+GRIDSIZE_H) : static_cast<int>(ceil(cluster.points[i].x)+GRIDSIZE_H-1);
458  yy = cluster.points[i].y<0.0? static_cast<int>(floor(cluster.points[i].y)+GRIDSIZE_H) : static_cast<int>(ceil(cluster.points[i].y)+GRIDSIZE_H-1);
459  zz = cluster.points[i].z<0.0? static_cast<int>(floor(cluster.points[i].z)+GRIDSIZE_H) : static_cast<int>(ceil(cluster.points[i].z)+GRIDSIZE_H-1);
460 
461  for (int x = -1; x < 2; x++)
462  for (int y = -1; y < 2; y++)
463  for (int z = -1; z < 2; z++)
464  {
465  xi = xx + x;
466  yi = yy + y;
467  zi = zz + z;
468 
469  if (yi >= GRIDSIZE || xi >= GRIDSIZE || zi>=GRIDSIZE || yi < 0 || xi < 0 || zi < 0)
470  {
471  ;
472  }
473  else
474  this->lut_[xi][yi][zi] = 0;
475  }
476  }
477 }
478 
479 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
480 template <typename PointInT, typename PointOutT> void
482  const pcl::PointCloud<PointInT> &pc, float scalefactor, Eigen::Vector4f& centroid)
483 {
484  pcl::compute3DCentroid (pc, centroid);
485  pcl::demeanPointCloud (pc, centroid, local_cloud_);
486 
487  float max_distance = 0, d;
488  pcl::PointXYZ cog (0, 0, 0);
489 
490  for (size_t i = 0; i < local_cloud_.points.size (); ++i)
491  {
492  d = pcl::euclideanDistance(cog,local_cloud_.points[i]);
493  if (d > max_distance)
494  max_distance = d;
495  }
496 
497  float scale_factor = 1.0f / max_distance * scalefactor;
498 
499  Eigen::Affine3f matrix = Eigen::Affine3f::Identity();
500  matrix.scale (scale_factor);
501  pcl::transformPointCloud (local_cloud_, local_cloud_, matrix);
502 }
503 
504 //////////////////////////////////////////////////////////////////////////////////////////////
505 template<typename PointInT, typename PointOutT> void
507 {
509  {
510  output.width = output.height = 0;
511  output.points.clear ();
512  return;
513  }
514  // Copy the header
515  output.header = input_->header;
516 
517  // Resize the output dataset
518  // Important! We should only allocate precisely how many elements we will need, otherwise
519  // we risk at pre-allocating too much memory which could lead to bad_alloc
520  // (see http://dev.pointclouds.org/issues/657)
521  output.width = output.height = 1;
522  output.is_dense = input_->is_dense;
523  output.points.resize (1);
524 
525  // Perform the actual feature computation
526  computeFeature (output);
527 
529 }
530 
531 
532 //////////////////////////////////////////////////////////////////////////////////////////////
533 template <typename PointInT, typename PointOutT> void
535 {
536  Eigen::Vector4f xyz_centroid;
537  std::vector<float> hist;
538  scale_points_unit_sphere (*surface_, static_cast<float>(GRIDSIZE_H), xyz_centroid);
539  this->voxelize9 (local_cloud_);
540  this->computeESF (local_cloud_, hist);
541  this->cleanup9 (local_cloud_);
542 
543  // We only output _1_ signature
544  output.points.resize (1);
545  output.width = 1;
546  output.height = 1;
547 
548  for (size_t d = 0; d < hist.size (); ++d)
549  output.points[0].histogram[d] = hist[d];
550 }
551 
552 #define PCL_INSTANTIATE_ESFEstimation(T,OutT) template class PCL_EXPORTS pcl::ESFEstimation<T,OutT>;
553 
554 #endif // PCL_FEATURES_IMPL_ESF_H_
555