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