esf.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2012, Willow Garage, Inc.
00006  *  Copyright (c) 2012-, Open Perception, Inc.
00007  *
00008  *  All rights reserved.
00009  *
00010  *  Redistribution and use in source and binary forms, with or without
00011  *  modification, are permitted provided that the following conditions
00012  *  are met:
00013  *
00014  *   * Redistributions of source code must retain the above copyright
00015  *     notice, this list of conditions and the following disclaimer.
00016  *   * Redistributions in binary form must reproduce the above
00017  *     copyright notice, this list of conditions and the following
00018  *     disclaimer in the documentation and/or other materials provided
00019  *     with the distribution.
00020  *   * Neither the name of the copyright holder(s) nor the names of its
00021  *     contributors may be used to endorse or promote products derived
00022  *     from this software without specific prior written permission.
00023  *
00024  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00025  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00026  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00027  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00028  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00029  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00030  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00031  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00033  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00034  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035  *  POSSIBILITY OF SUCH DAMAGE.
00036  *
00037  * $Id: pfh.hpp 5027 2012-03-12 03:10:45Z rusu $
00038  *
00039  */
00040 
00041 #ifndef PCL_FEATURES_IMPL_ESF_H_
00042 #define PCL_FEATURES_IMPL_ESF_H_
00043 
00044 #include <pcl/features/esf.h>
00045 #include <pcl/common/common.h>
00046 #include <pcl/common/transforms.h>
00047 #include <vector>
00048 
00050 template <typename PointInT, typename PointOutT> void
00051 pcl::ESFEstimation<PointInT, PointOutT>::computeESF (
00052     PointCloudIn &pc, std::vector<float> &hist)
00053 {
00054   const int binsize = 64;
00055   unsigned int sample_size = 20000;
00056   srand (static_cast<unsigned int> (time (0)));
00057   int maxindex = static_cast<int> (pc.points.size ());
00058 
00059   int index1, index2, index3;
00060   std::vector<float> d2v, d1v, d3v, wt_d3;
00061   std::vector<int> wt_d2;
00062   d1v.reserve (sample_size);
00063   d2v.reserve (sample_size * 3);
00064   d3v.reserve (sample_size);
00065   wt_d2.reserve (sample_size * 3);
00066   wt_d3.reserve (sample_size);
00067 
00068   float h_in[binsize] = {0};
00069   float h_out[binsize] = {0};
00070   float h_mix[binsize] = {0};
00071   float h_mix_ratio[binsize] = {0};
00072 
00073   float h_a3_in[binsize] = {0};
00074   float h_a3_out[binsize] = {0};
00075   float h_a3_mix[binsize] = {0};
00076   float h_d1[binsize] = {0};
00077 
00078   float h_d3_in[binsize] = {0};
00079   float h_d3_out[binsize] = {0};
00080   float h_d3_mix[binsize] = {0};
00081 
00082   float ratio=0.0;
00083   float pih = static_cast<float>(M_PI) / 2.0f;
00084   float a,b,c,s;
00085   int th1,th2,th3;
00086   int vxlcnt = 0;
00087   int pcnt1,pcnt2,pcnt3;
00088   for (size_t nn_idx = 0; nn_idx < sample_size; ++nn_idx)
00089   {
00090     // get a new random point
00091     index1 = rand()%maxindex;
00092     index2 = rand()%maxindex;
00093     index3 = rand()%maxindex;
00094 
00095     if (index1==index2 || index1 == index3 || index2 == index3)
00096     {
00097       nn_idx--;
00098       continue;
00099     }
00100 
00101     Eigen::Vector4f p1 = pc.points[index1].getVector4fMap ();
00102     Eigen::Vector4f p2 = pc.points[index2].getVector4fMap ();
00103     Eigen::Vector4f p3 = pc.points[index3].getVector4fMap ();
00104 
00105     // A3
00106     Eigen::Vector4f v21 (p2 - p1);
00107     Eigen::Vector4f v31 (p3 - p1);
00108     Eigen::Vector4f v23 (p2 - p3);
00109     a = v21.norm (); b = v31.norm (); c = v23.norm (); s = (a+b+c) * 0.5f;
00110     if (s * (s-a) * (s-b) * (s-c) <= 0.001f)
00111       continue;
00112 
00113     v21.normalize ();
00114     v31.normalize ();
00115     v23.normalize ();
00116 
00117     //TODO: .dot gives nan's
00118     th1 = static_cast<int> (pcl_round (acos (fabs (v21.dot (v31))) / pih * (binsize-1)));
00119     th2 = static_cast<int> (pcl_round (acos (fabs (v23.dot (v31))) / pih * (binsize-1)));
00120     th3 = static_cast<int> (pcl_round (acos (fabs (v23.dot (v21))) / pih * (binsize-1)));
00121     if (th1 < 0 || th1 >= binsize)
00122     {
00123       nn_idx--;
00124       continue;
00125     }
00126     if (th2 < 0 || th2 >= binsize)
00127     {
00128       nn_idx--;
00129       continue;
00130     }
00131     if (th3 < 0 || th3 >= binsize)
00132     {
00133       nn_idx--;
00134       continue;
00135     }
00136 
00137     //pcl::PointXYZ cog(((rand()%100)-50.0f) / 100.0f,((rand()%100)-50.0f) / 100.0f,((rand()%100)-50.0f) / 100.0f);
00138     // D1
00139     //                      d1v.push_back( pcl::euclideanDistance(cog, pc.points[index1]) );
00140 
00141     // D2
00142     d2v.push_back (pcl::euclideanDistance (pc.points[index1], pc.points[index2]));
00143     d2v.push_back (pcl::euclideanDistance (pc.points[index1], pc.points[index3]));
00144     d2v.push_back (pcl::euclideanDistance (pc.points[index2], pc.points[index3]));
00145 
00146     int vxlcnt_sum = 0;
00147     int p_cnt = 0;
00148     // IN, OUT, MIXED, Ratio line tracing, index1->index2
00149     {
00150       const int xs = p1[0] < 0.0? static_cast<int>(floor(p1[0])+GRIDSIZE_H): static_cast<int>(ceil(p1[0])+GRIDSIZE_H-1);
00151       const int ys = p1[1] < 0.0? static_cast<int>(floor(p1[1])+GRIDSIZE_H): static_cast<int>(ceil(p1[1])+GRIDSIZE_H-1);
00152       const int zs = p1[2] < 0.0? static_cast<int>(floor(p1[2])+GRIDSIZE_H): static_cast<int>(ceil(p1[2])+GRIDSIZE_H-1);
00153       const int xt = p2[0] < 0.0? static_cast<int>(floor(p2[0])+GRIDSIZE_H): static_cast<int>(ceil(p2[0])+GRIDSIZE_H-1);
00154       const int yt = p2[1] < 0.0? static_cast<int>(floor(p2[1])+GRIDSIZE_H): static_cast<int>(ceil(p2[1])+GRIDSIZE_H-1);
00155       const int zt = p2[2] < 0.0? static_cast<int>(floor(p2[2])+GRIDSIZE_H): static_cast<int>(ceil(p2[2])+GRIDSIZE_H-1);
00156       wt_d2.push_back (this->lci (xs, ys, zs, xt, yt, zt, ratio, vxlcnt, pcnt1));
00157       if (wt_d2.back () == 2)
00158         h_mix_ratio[static_cast<int> (pcl_round (ratio * (binsize-1)))]++;
00159       vxlcnt_sum += vxlcnt;
00160       p_cnt += pcnt1;
00161     }
00162     // IN, OUT, MIXED, Ratio line tracing, index1->index3
00163     {
00164       const int xs = p1[0] < 0.0? static_cast<int>(floor(p1[0])+GRIDSIZE_H): static_cast<int>(ceil(p1[0])+GRIDSIZE_H-1);
00165       const int ys = p1[1] < 0.0? static_cast<int>(floor(p1[1])+GRIDSIZE_H): static_cast<int>(ceil(p1[1])+GRIDSIZE_H-1);
00166       const int zs = p1[2] < 0.0? static_cast<int>(floor(p1[2])+GRIDSIZE_H): static_cast<int>(ceil(p1[2])+GRIDSIZE_H-1);
00167       const int xt = p3[0] < 0.0? static_cast<int>(floor(p3[0])+GRIDSIZE_H): static_cast<int>(ceil(p3[0])+GRIDSIZE_H-1);
00168       const int yt = p3[1] < 0.0? static_cast<int>(floor(p3[1])+GRIDSIZE_H): static_cast<int>(ceil(p3[1])+GRIDSIZE_H-1);
00169       const int zt = p3[2] < 0.0? static_cast<int>(floor(p3[2])+GRIDSIZE_H): static_cast<int>(ceil(p3[2])+GRIDSIZE_H-1);
00170       wt_d2.push_back (this->lci (xs, ys, zs, xt, yt, zt, ratio, vxlcnt, pcnt2));
00171       if (wt_d2.back () == 2)
00172         h_mix_ratio[static_cast<int>(pcl_round (ratio * (binsize-1)))]++;
00173       vxlcnt_sum += vxlcnt;
00174       p_cnt += pcnt2;
00175     }
00176     // IN, OUT, MIXED, Ratio line tracing, index2->index3
00177     {
00178       const int xs = p2[0] < 0.0? static_cast<int>(floor(p2[0])+GRIDSIZE_H): static_cast<int>(ceil(p2[0])+GRIDSIZE_H-1);
00179       const int ys = p2[1] < 0.0? static_cast<int>(floor(p2[1])+GRIDSIZE_H): static_cast<int>(ceil(p2[1])+GRIDSIZE_H-1);
00180       const int zs = p2[2] < 0.0? static_cast<int>(floor(p2[2])+GRIDSIZE_H): static_cast<int>(ceil(p2[2])+GRIDSIZE_H-1);
00181       const int xt = p3[0] < 0.0? static_cast<int>(floor(p3[0])+GRIDSIZE_H): static_cast<int>(ceil(p3[0])+GRIDSIZE_H-1);
00182       const int yt = p3[1] < 0.0? static_cast<int>(floor(p3[1])+GRIDSIZE_H): static_cast<int>(ceil(p3[1])+GRIDSIZE_H-1);
00183       const int zt = p3[2] < 0.0? static_cast<int>(floor(p3[2])+GRIDSIZE_H): static_cast<int>(ceil(p3[2])+GRIDSIZE_H-1);
00184       wt_d2.push_back (this->lci (xs,ys,zs,xt,yt,zt,ratio,vxlcnt,pcnt3));
00185       if (wt_d2.back () == 2)
00186         h_mix_ratio[static_cast<int>(pcl_round(ratio * (binsize-1)))]++;
00187       vxlcnt_sum += vxlcnt;
00188       p_cnt += pcnt3;
00189     }
00190 
00191     // D3 ( herons formula )
00192     d3v.push_back (sqrt (sqrt (s * (s-a) * (s-b) * (s-c))));
00193     if (vxlcnt_sum <= 21)
00194     {
00195       wt_d3.push_back (0);
00196       h_a3_out[th1] += static_cast<float> (pcnt3) / 32.0f;
00197       h_a3_out[th2] += static_cast<float> (pcnt1) / 32.0f;
00198       h_a3_out[th3] += static_cast<float> (pcnt2) / 32.0f;
00199     }
00200     else
00201       if (p_cnt - vxlcnt_sum < 4)
00202       {
00203         h_a3_in[th1] += static_cast<float> (pcnt3) / 32.0f;
00204         h_a3_in[th2] += static_cast<float> (pcnt1) / 32.0f;
00205         h_a3_in[th3] += static_cast<float> (pcnt2) / 32.0f;
00206         wt_d3.push_back (1);
00207       }
00208       else
00209       {
00210         h_a3_mix[th1] += static_cast<float> (pcnt3) / 32.0f;
00211         h_a3_mix[th2] += static_cast<float> (pcnt1) / 32.0f;
00212         h_a3_mix[th3] += static_cast<float> (pcnt2) / 32.0f;
00213         wt_d3.push_back (static_cast<float> (vxlcnt_sum) / static_cast<float> (p_cnt));
00214       }
00215   }
00216   // Normalizing, get max
00217   float maxd1 = 0;
00218   float maxd2 = 0;
00219   float maxd3 = 0;
00220 
00221   for (size_t nn_idx = 0; nn_idx < sample_size; ++nn_idx)
00222   {
00223     // get max of Dx
00224     if (d1v[nn_idx] > maxd1)
00225       maxd1 = d1v[nn_idx];
00226     if (d2v[nn_idx] > maxd2)
00227       maxd2 = d2v[nn_idx];
00228     if (d2v[sample_size + nn_idx] > maxd2)
00229       maxd2 = d2v[sample_size + nn_idx];
00230     if (d2v[sample_size*2 +nn_idx] > maxd2)
00231       maxd2 = d2v[sample_size*2 +nn_idx];
00232     if (d3v[nn_idx] > maxd3)
00233       maxd3 = d3v[nn_idx];
00234   }
00235 
00236   // Normalize and create histogram
00237   int index;
00238   for (size_t nn_idx = 0; nn_idx < sample_size; ++nn_idx)
00239   {
00240     h_d1[static_cast<int>(pcl_round (d1v[nn_idx] / maxd1 * (binsize-1)))]++ ;
00241 
00242     if (wt_d3[nn_idx] >= 0.999) // IN
00243     {
00244       index = static_cast<int>(pcl_round (d3v[nn_idx] / maxd3 * (binsize-1)));
00245       if (index >= 0 && index < binsize)
00246         h_d3_in[index]++;
00247     }
00248     else
00249     {
00250       if (wt_d3[nn_idx] <= 0.001) // OUT
00251       {
00252         index = static_cast<int>(pcl_round (d3v[nn_idx] / maxd3 * (binsize-1)));
00253         if (index >= 0 && index < binsize)
00254           h_d3_out[index]++ ;
00255       }
00256       else
00257       {
00258         index = static_cast<int>(pcl_round (d3v[nn_idx] / maxd3 * (binsize-1)));
00259         if (index >= 0 && index < binsize)
00260           h_d3_mix[index]++;
00261       }
00262     }
00263   }
00264   //normalize and create histogram
00265   for (size_t nn_idx = 0; nn_idx < d2v.size(); ++nn_idx )
00266   {
00267     if (wt_d2[nn_idx] == 0)
00268       h_in[static_cast<int>(pcl_round (d2v[nn_idx] / maxd2 * (binsize-1)))]++ ;
00269     if (wt_d2[nn_idx] == 1)
00270       h_out[static_cast<int>(pcl_round (d2v[nn_idx] / maxd2 * (binsize-1)))]++;
00271     if (wt_d2[nn_idx] == 2)
00272       h_mix[static_cast<int>(pcl_round (d2v[nn_idx] / maxd2 * (binsize-1)))]++ ;
00273   }
00274 
00275   //float weights[10] = {1,  1,  1,  1,  1,  1,  1,  1 , 1 ,  1};
00276   float weights[10] = {0.5f, 0.5f, 0.5f, 0.5f, 0.5f, 1.0f,  1.0f, 2.0f, 2.0f, 2.0f};
00277 
00278   hist.reserve (binsize * 10);
00279   for (int i = 0; i < binsize; i++)
00280     hist.push_back (h_a3_in[i] * weights[0]);
00281   for (int i = 0; i < binsize; i++)
00282     hist.push_back (h_a3_out[i] * weights[1]);
00283   for (int i = 0; i < binsize; i++)
00284     hist.push_back (h_a3_mix[i] * weights[2]);
00285 
00286   for (int i = 0; i < binsize; i++)
00287     hist.push_back (h_d3_in[i] * weights[3]);
00288   for (int i = 0; i < binsize; i++)
00289     hist.push_back (h_d3_out[i] * weights[4]);
00290   for (int i = 0; i < binsize; i++)
00291     hist.push_back (h_d3_mix[i] * weights[5]);
00292 
00293   for (int i = 0; i < binsize; i++)
00294     hist.push_back (h_in[i]*0.5f * weights[6]);
00295   for (int i = 0; i < binsize; i++)
00296     hist.push_back (h_out[i] * weights[7]);
00297   for (int i = 0; i < binsize; i++)
00298     hist.push_back (h_mix[i] * weights[8]);
00299   for (int i = 0; i < binsize; i++)
00300     hist.push_back (h_mix_ratio[i]*0.5f * weights[9]);
00301 
00302   float sm = 0;
00303   for (size_t i = 0; i < hist.size (); i++)
00304     sm += hist[i];
00305 
00306   for (size_t i = 0; i < hist.size (); i++)
00307     hist[i] /= sm;
00308 }
00309 
00311 template <typename PointInT, typename PointOutT> int
00312 pcl::ESFEstimation<PointInT, PointOutT>::lci (
00313     const int x1, const int y1, const int z1, 
00314     const int x2, const int y2, const int z2, 
00315     float &ratio, int &incnt, int &pointcount)
00316 {
00317   int voxelcount = 0;
00318   int voxel_in = 0;
00319   int act_voxel[3];
00320   act_voxel[0] = x1;
00321   act_voxel[1] = y1;
00322   act_voxel[2] = z1;
00323   int x_inc, y_inc, z_inc;
00324   int dx = x2 - x1;
00325   int dy = y2 - y1;
00326   int dz = z2 - z1;
00327   if (dx < 0)
00328     x_inc = -1;
00329   else
00330     x_inc = 1;
00331   int l = abs (dx);
00332   if (dy < 0)
00333     y_inc = -1 ;
00334   else
00335     y_inc = 1;
00336   int m = abs (dy);
00337   if (dz < 0)
00338     z_inc = -1 ;
00339   else
00340     z_inc = 1;
00341   int n = abs (dz);
00342   int dx2 = 2 * l;
00343   int dy2 = 2 * m;
00344   int dz2 = 2 * n;
00345   if ((l >= m) & (l >= n))
00346   {
00347     int err_1 = dy2 - l;
00348     int err_2 = dz2 - l;
00349     for (int i = 1; i<l; i++)
00350     {
00351       voxelcount++;;
00352       voxel_in +=  static_cast<int>(lut_[act_voxel[0]][act_voxel[1]][act_voxel[2]] == 1);
00353       if (err_1 > 0)
00354       {
00355         act_voxel[1] += y_inc;
00356         err_1 -=  dx2;
00357       }
00358       if (err_2 > 0)
00359       {
00360         act_voxel[2] += z_inc;
00361         err_2 -= dx2;
00362       }
00363       err_1 += dy2;
00364       err_2 += dz2;
00365       act_voxel[0] += x_inc;
00366     }
00367   }
00368   else if ((m >= l) & (m >= n))
00369   {
00370     int err_1 = dx2 - m;
00371     int err_2 = dz2 - m;
00372     for (int i=1; i<m; i++)
00373     {
00374       voxelcount++;
00375       voxel_in +=  static_cast<int>(lut_[act_voxel[0]][act_voxel[1]][act_voxel[2]] == 1);
00376       if (err_1 > 0)
00377       {
00378         act_voxel[0] +=  x_inc;
00379         err_1 -= dy2;
00380       }
00381       if (err_2 > 0)
00382       {
00383         act_voxel[2] += z_inc;
00384         err_2 -= dy2;
00385       }
00386       err_1 += dx2;
00387       err_2 += dz2;
00388       act_voxel[1] += y_inc;
00389     }
00390   }
00391   else
00392   {
00393     int err_1 = dy2 - n;
00394     int err_2 = dx2 - n;
00395     for (int i=1; i<n; i++)
00396     {
00397       voxelcount++;
00398       voxel_in +=  static_cast<int>(lut_[act_voxel[0]][act_voxel[1]][act_voxel[2]] == 1);
00399       if (err_1 > 0)
00400       {
00401         act_voxel[1] += y_inc;
00402         err_1 -= dz2;
00403       }
00404       if (err_2 > 0)
00405       {
00406         act_voxel[0] += x_inc;
00407         err_2 -= dz2;
00408       }
00409       err_1 += dy2;
00410       err_2 += dx2;
00411       act_voxel[2] += z_inc;
00412     }
00413   }
00414   voxelcount++;
00415   voxel_in +=  static_cast<int>(lut_[act_voxel[0]][act_voxel[1]][act_voxel[2]] == 1);
00416   incnt = voxel_in;
00417   pointcount = voxelcount;
00418 
00419   if (voxel_in >=  voxelcount-1)
00420     return (0);
00421 
00422   if (voxel_in <= 7)
00423     return (1);
00424 
00425   ratio = static_cast<float>(voxel_in) / static_cast<float>(voxelcount);
00426   return (2);
00427 }
00428 
00430 template <typename PointInT, typename PointOutT> void
00431 pcl::ESFEstimation<PointInT, PointOutT>::voxelize9 (PointCloudIn &cluster)
00432 {
00433   int xi,yi,zi,xx,yy,zz;
00434   for (size_t i = 0; i < cluster.points.size (); ++i)
00435   {
00436     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);
00437     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);
00438     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);
00439 
00440     for (int x = -1; x < 2; x++)
00441       for (int y = -1; y < 2; y++)
00442         for (int z = -1; z < 2; z++)
00443         {
00444           xi = xx + x;
00445           yi = yy + y;
00446           zi = zz + z;
00447 
00448           if (yi >= GRIDSIZE || xi >= GRIDSIZE || zi>=GRIDSIZE || yi < 0 || xi < 0 || zi < 0)
00449           {
00450             ;
00451           }
00452           else
00453             this->lut_[xi][yi][zi] = 1;
00454         }
00455   }
00456 }
00457 
00459 template <typename PointInT, typename PointOutT> void
00460 pcl::ESFEstimation<PointInT, PointOutT>::cleanup9 (PointCloudIn &cluster)
00461 {
00462   int xi,yi,zi,xx,yy,zz;
00463   for (size_t i = 0; i < cluster.points.size (); ++i)
00464   {
00465     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);
00466     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);
00467     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);
00468 
00469     for (int x = -1; x < 2; x++)
00470       for (int y = -1; y < 2; y++)
00471         for (int z = -1; z < 2; z++)
00472         {
00473           xi = xx + x;
00474           yi = yy + y;
00475           zi = zz + z;
00476 
00477           if (yi >= GRIDSIZE || xi >= GRIDSIZE || zi>=GRIDSIZE || yi < 0 || xi < 0 || zi < 0)
00478           {
00479             ;
00480           }
00481           else
00482             this->lut_[xi][yi][zi] = 0;
00483         }
00484   }
00485 }
00486 
00488 template <typename PointInT, typename PointOutT> void
00489 pcl::ESFEstimation<PointInT, PointOutT>::scale_points_unit_sphere (
00490     const pcl::PointCloud<PointInT> &pc, float scalefactor, Eigen::Vector4f& centroid)
00491 {
00492   pcl::compute3DCentroid (pc, centroid);
00493   pcl::demeanPointCloud (pc, centroid, local_cloud_);
00494 
00495   float max_distance = 0, d;
00496   pcl::PointXYZ cog (0, 0, 0);
00497 
00498   for (size_t i = 0; i < local_cloud_.points.size (); ++i)
00499   {
00500     d = pcl::euclideanDistance(cog,local_cloud_.points[i]);
00501     if (d > max_distance)
00502       max_distance = d;
00503   }
00504 
00505   float scale_factor = 1.0f / max_distance * scalefactor;
00506 
00507   Eigen::Affine3f matrix = Eigen::Affine3f::Identity();
00508   matrix.scale (scale_factor);
00509   pcl::transformPointCloud (local_cloud_, local_cloud_, matrix);
00510 }
00511 
00513 template<typename PointInT, typename PointOutT> void
00514 pcl::ESFEstimation<PointInT, PointOutT>::compute (PointCloudOut &output)
00515 {
00516   if (!Feature<PointInT, PointOutT>::initCompute ())
00517   {
00518     output.width = output.height = 0;
00519     output.points.clear ();
00520     return;
00521   }
00522   // Copy the header
00523   output.header = input_->header;
00524 
00525   // Resize the output dataset
00526   // Important! We should only allocate precisely how many elements we will need, otherwise
00527   // we risk at pre-allocating too much memory which could lead to bad_alloc 
00528   // (see http://dev.pointclouds.org/issues/657)
00529   output.width = output.height = 1;
00530   output.is_dense = input_->is_dense;
00531   output.points.resize (1);
00532 
00533   // Perform the actual feature computation
00534   computeFeature (output);
00535 
00536   Feature<PointInT, PointOutT>::deinitCompute ();
00537 }
00538 
00539 
00541 template <typename PointInT, typename PointOutT> void
00542 pcl::ESFEstimation<PointInT, PointOutT>::computeFeature (PointCloudOut &output)
00543 {
00544   Eigen::Vector4f xyz_centroid;
00545   std::vector<float> hist;
00546   scale_points_unit_sphere (*surface_, static_cast<float>(GRIDSIZE_H), xyz_centroid);
00547   this->voxelize9 (local_cloud_);
00548   this->computeESF (local_cloud_, hist);
00549   this->cleanup9 (local_cloud_);
00550 
00551   // We only output _1_ signature
00552   output.points.resize (1);
00553   output.width = 1;
00554   output.height = 1;
00555 
00556   for (size_t d = 0; d < hist.size (); ++d)
00557     output.points[0].histogram[d] = hist[d];
00558 }
00559 
00560 #define PCL_INSTANTIATE_ESFEstimation(T,OutT) template class PCL_EXPORTS pcl::ESFEstimation<T,OutT>;
00561 
00562 #endif    // PCL_FEATURES_IMPL_ESF_H_
00563 


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:14:51