Point Cloud Library (PCL)
1.7.0
|
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/distances.h> 00047 #include <pcl/common/transforms.h> 00048 #include <vector> 00049 00050 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00051 template <typename PointInT, typename PointOutT> void 00052 pcl::ESFEstimation<PointInT, PointOutT>::computeESF ( 00053 PointCloudIn &pc, std::vector<float> &hist) 00054 { 00055 const int binsize = 64; 00056 unsigned int sample_size = 20000; 00057 srand (static_cast<unsigned int> (time (0))); 00058 int maxindex = static_cast<int> (pc.points.size ()); 00059 00060 int index1, index2, index3; 00061 std::vector<float> d2v, d1v, d3v, wt_d3; 00062 std::vector<int> wt_d2; 00063 d1v.reserve (sample_size); 00064 d2v.reserve (sample_size * 3); 00065 d3v.reserve (sample_size); 00066 wt_d2.reserve (sample_size * 3); 00067 wt_d3.reserve (sample_size); 00068 00069 float h_in[binsize] = {0}; 00070 float h_out[binsize] = {0}; 00071 float h_mix[binsize] = {0}; 00072 float h_mix_ratio[binsize] = {0}; 00073 00074 float h_a3_in[binsize] = {0}; 00075 float h_a3_out[binsize] = {0}; 00076 float h_a3_mix[binsize] = {0}; 00077 float h_d1[binsize] = {0}; 00078 00079 float h_d3_in[binsize] = {0}; 00080 float h_d3_out[binsize] = {0}; 00081 float h_d3_mix[binsize] = {0}; 00082 00083 float ratio=0.0; 00084 float pih = static_cast<float>(M_PI) / 2.0f; 00085 float a,b,c,s; 00086 int th1,th2,th3; 00087 int vxlcnt = 0; 00088 int pcnt1,pcnt2,pcnt3; 00089 for (size_t nn_idx = 0; nn_idx < sample_size; ++nn_idx) 00090 { 00091 // get a new random point 00092 index1 = rand()%maxindex; 00093 index2 = rand()%maxindex; 00094 index3 = rand()%maxindex; 00095 00096 if (index1==index2 || index1 == index3 || index2 == index3) 00097 { 00098 nn_idx--; 00099 continue; 00100 } 00101 00102 Eigen::Vector4f p1 = pc.points[index1].getVector4fMap (); 00103 Eigen::Vector4f p2 = pc.points[index2].getVector4fMap (); 00104 Eigen::Vector4f p3 = pc.points[index3].getVector4fMap (); 00105 00106 // A3 00107 Eigen::Vector4f v21 (p2 - p1); 00108 Eigen::Vector4f v31 (p3 - p1); 00109 Eigen::Vector4f v23 (p2 - p3); 00110 a = v21.norm (); b = v31.norm (); c = v23.norm (); s = (a+b+c) * 0.5f; 00111 if (s * (s-a) * (s-b) * (s-c) <= 0.001f) 00112 continue; 00113 00114 v21.normalize (); 00115 v31.normalize (); 00116 v23.normalize (); 00117 00118 //TODO: .dot gives nan's 00119 th1 = static_cast<int> (pcl_round (acos (fabs (v21.dot (v31))) / pih * (binsize-1))); 00120 th2 = static_cast<int> (pcl_round (acos (fabs (v23.dot (v31))) / pih * (binsize-1))); 00121 th3 = static_cast<int> (pcl_round (acos (fabs (v23.dot (v21))) / pih * (binsize-1))); 00122 if (th1 < 0 || th1 >= binsize) 00123 { 00124 nn_idx--; 00125 continue; 00126 } 00127 if (th2 < 0 || th2 >= binsize) 00128 { 00129 nn_idx--; 00130 continue; 00131 } 00132 if (th3 < 0 || th3 >= binsize) 00133 { 00134 nn_idx--; 00135 continue; 00136 } 00137 00138 // D2 00139 d2v.push_back (pcl::euclideanDistance (pc.points[index1], pc.points[index2])); 00140 d2v.push_back (pcl::euclideanDistance (pc.points[index1], pc.points[index3])); 00141 d2v.push_back (pcl::euclideanDistance (pc.points[index2], pc.points[index3])); 00142 00143 int vxlcnt_sum = 0; 00144 int p_cnt = 0; 00145 // IN, OUT, MIXED, Ratio line tracing, index1->index2 00146 { 00147 const int xs = p1[0] < 0.0? static_cast<int>(floor(p1[0])+GRIDSIZE_H): static_cast<int>(ceil(p1[0])+GRIDSIZE_H-1); 00148 const int ys = p1[1] < 0.0? static_cast<int>(floor(p1[1])+GRIDSIZE_H): static_cast<int>(ceil(p1[1])+GRIDSIZE_H-1); 00149 const int zs = p1[2] < 0.0? static_cast<int>(floor(p1[2])+GRIDSIZE_H): static_cast<int>(ceil(p1[2])+GRIDSIZE_H-1); 00150 const int xt = p2[0] < 0.0? static_cast<int>(floor(p2[0])+GRIDSIZE_H): static_cast<int>(ceil(p2[0])+GRIDSIZE_H-1); 00151 const int yt = p2[1] < 0.0? static_cast<int>(floor(p2[1])+GRIDSIZE_H): static_cast<int>(ceil(p2[1])+GRIDSIZE_H-1); 00152 const int zt = p2[2] < 0.0? static_cast<int>(floor(p2[2])+GRIDSIZE_H): static_cast<int>(ceil(p2[2])+GRIDSIZE_H-1); 00153 wt_d2.push_back (this->lci (xs, ys, zs, xt, yt, zt, ratio, vxlcnt, pcnt1)); 00154 if (wt_d2.back () == 2) 00155 h_mix_ratio[static_cast<int> (pcl_round (ratio * (binsize-1)))]++; 00156 vxlcnt_sum += vxlcnt; 00157 p_cnt += pcnt1; 00158 } 00159 // IN, OUT, MIXED, Ratio line tracing, index1->index3 00160 { 00161 const int xs = p1[0] < 0.0? static_cast<int>(floor(p1[0])+GRIDSIZE_H): static_cast<int>(ceil(p1[0])+GRIDSIZE_H-1); 00162 const int ys = p1[1] < 0.0? static_cast<int>(floor(p1[1])+GRIDSIZE_H): static_cast<int>(ceil(p1[1])+GRIDSIZE_H-1); 00163 const int zs = p1[2] < 0.0? static_cast<int>(floor(p1[2])+GRIDSIZE_H): static_cast<int>(ceil(p1[2])+GRIDSIZE_H-1); 00164 const int xt = p3[0] < 0.0? static_cast<int>(floor(p3[0])+GRIDSIZE_H): static_cast<int>(ceil(p3[0])+GRIDSIZE_H-1); 00165 const int yt = p3[1] < 0.0? static_cast<int>(floor(p3[1])+GRIDSIZE_H): static_cast<int>(ceil(p3[1])+GRIDSIZE_H-1); 00166 const int zt = p3[2] < 0.0? static_cast<int>(floor(p3[2])+GRIDSIZE_H): static_cast<int>(ceil(p3[2])+GRIDSIZE_H-1); 00167 wt_d2.push_back (this->lci (xs, ys, zs, xt, yt, zt, ratio, vxlcnt, pcnt2)); 00168 if (wt_d2.back () == 2) 00169 h_mix_ratio[static_cast<int>(pcl_round (ratio * (binsize-1)))]++; 00170 vxlcnt_sum += vxlcnt; 00171 p_cnt += pcnt2; 00172 } 00173 // IN, OUT, MIXED, Ratio line tracing, index2->index3 00174 { 00175 const int xs = p2[0] < 0.0? static_cast<int>(floor(p2[0])+GRIDSIZE_H): static_cast<int>(ceil(p2[0])+GRIDSIZE_H-1); 00176 const int ys = p2[1] < 0.0? static_cast<int>(floor(p2[1])+GRIDSIZE_H): static_cast<int>(ceil(p2[1])+GRIDSIZE_H-1); 00177 const int zs = p2[2] < 0.0? static_cast<int>(floor(p2[2])+GRIDSIZE_H): static_cast<int>(ceil(p2[2])+GRIDSIZE_H-1); 00178 const int xt = p3[0] < 0.0? static_cast<int>(floor(p3[0])+GRIDSIZE_H): static_cast<int>(ceil(p3[0])+GRIDSIZE_H-1); 00179 const int yt = p3[1] < 0.0? static_cast<int>(floor(p3[1])+GRIDSIZE_H): static_cast<int>(ceil(p3[1])+GRIDSIZE_H-1); 00180 const int zt = p3[2] < 0.0? static_cast<int>(floor(p3[2])+GRIDSIZE_H): static_cast<int>(ceil(p3[2])+GRIDSIZE_H-1); 00181 wt_d2.push_back (this->lci (xs,ys,zs,xt,yt,zt,ratio,vxlcnt,pcnt3)); 00182 if (wt_d2.back () == 2) 00183 h_mix_ratio[static_cast<int>(pcl_round(ratio * (binsize-1)))]++; 00184 vxlcnt_sum += vxlcnt; 00185 p_cnt += pcnt3; 00186 } 00187 00188 // D3 ( herons formula ) 00189 d3v.push_back (sqrtf (sqrtf (s * (s-a) * (s-b) * (s-c)))); 00190 if (vxlcnt_sum <= 21) 00191 { 00192 wt_d3.push_back (0); 00193 h_a3_out[th1] += static_cast<float> (pcnt3) / 32.0f; 00194 h_a3_out[th2] += static_cast<float> (pcnt1) / 32.0f; 00195 h_a3_out[th3] += static_cast<float> (pcnt2) / 32.0f; 00196 } 00197 else 00198 if (p_cnt - vxlcnt_sum < 4) 00199 { 00200 h_a3_in[th1] += static_cast<float> (pcnt3) / 32.0f; 00201 h_a3_in[th2] += static_cast<float> (pcnt1) / 32.0f; 00202 h_a3_in[th3] += static_cast<float> (pcnt2) / 32.0f; 00203 wt_d3.push_back (1); 00204 } 00205 else 00206 { 00207 h_a3_mix[th1] += static_cast<float> (pcnt3) / 32.0f; 00208 h_a3_mix[th2] += static_cast<float> (pcnt1) / 32.0f; 00209 h_a3_mix[th3] += static_cast<float> (pcnt2) / 32.0f; 00210 wt_d3.push_back (static_cast<float> (vxlcnt_sum) / static_cast<float> (p_cnt)); 00211 } 00212 } 00213 // Normalizing, get max 00214 float maxd2 = 0; 00215 float maxd3 = 0; 00216 00217 for (size_t nn_idx = 0; nn_idx < sample_size; ++nn_idx) 00218 { 00219 // get max of Dx 00220 if (d2v[nn_idx] > maxd2) 00221 maxd2 = d2v[nn_idx]; 00222 if (d2v[sample_size + nn_idx] > maxd2) 00223 maxd2 = d2v[sample_size + nn_idx]; 00224 if (d2v[sample_size*2 +nn_idx] > maxd2) 00225 maxd2 = d2v[sample_size*2 +nn_idx]; 00226 if (d3v[nn_idx] > maxd3) 00227 maxd3 = d3v[nn_idx]; 00228 } 00229 00230 // Normalize and create histogram 00231 int index; 00232 for (size_t nn_idx = 0; nn_idx < sample_size; ++nn_idx) 00233 { 00234 if (wt_d3[nn_idx] >= 0.999) // IN 00235 { 00236 index = static_cast<int>(pcl_round (d3v[nn_idx] / maxd3 * (binsize-1))); 00237 if (index >= 0 && index < binsize) 00238 h_d3_in[index]++; 00239 } 00240 else 00241 { 00242 if (wt_d3[nn_idx] <= 0.001) // OUT 00243 { 00244 index = static_cast<int>(pcl_round (d3v[nn_idx] / maxd3 * (binsize-1))); 00245 if (index >= 0 && index < binsize) 00246 h_d3_out[index]++ ; 00247 } 00248 else 00249 { 00250 index = static_cast<int>(pcl_round (d3v[nn_idx] / maxd3 * (binsize-1))); 00251 if (index >= 0 && index < binsize) 00252 h_d3_mix[index]++; 00253 } 00254 } 00255 } 00256 //normalize and create histogram 00257 for (size_t nn_idx = 0; nn_idx < d2v.size(); ++nn_idx ) 00258 { 00259 if (wt_d2[nn_idx] == 0) 00260 h_in[static_cast<int>(pcl_round (d2v[nn_idx] / maxd2 * (binsize-1)))]++ ; 00261 if (wt_d2[nn_idx] == 1) 00262 h_out[static_cast<int>(pcl_round (d2v[nn_idx] / maxd2 * (binsize-1)))]++; 00263 if (wt_d2[nn_idx] == 2) 00264 h_mix[static_cast<int>(pcl_round (d2v[nn_idx] / maxd2 * (binsize-1)))]++ ; 00265 } 00266 00267 //float weights[10] = {1, 1, 1, 1, 1, 1, 1, 1 , 1 , 1}; 00268 float weights[10] = {0.5f, 0.5f, 0.5f, 0.5f, 0.5f, 1.0f, 1.0f, 2.0f, 2.0f, 2.0f}; 00269 00270 hist.reserve (binsize * 10); 00271 for (int i = 0; i < binsize; i++) 00272 hist.push_back (h_a3_in[i] * weights[0]); 00273 for (int i = 0; i < binsize; i++) 00274 hist.push_back (h_a3_out[i] * weights[1]); 00275 for (int i = 0; i < binsize; i++) 00276 hist.push_back (h_a3_mix[i] * weights[2]); 00277 00278 for (int i = 0; i < binsize; i++) 00279 hist.push_back (h_d3_in[i] * weights[3]); 00280 for (int i = 0; i < binsize; i++) 00281 hist.push_back (h_d3_out[i] * weights[4]); 00282 for (int i = 0; i < binsize; i++) 00283 hist.push_back (h_d3_mix[i] * weights[5]); 00284 00285 for (int i = 0; i < binsize; i++) 00286 hist.push_back (h_in[i]*0.5f * weights[6]); 00287 for (int i = 0; i < binsize; i++) 00288 hist.push_back (h_out[i] * weights[7]); 00289 for (int i = 0; i < binsize; i++) 00290 hist.push_back (h_mix[i] * weights[8]); 00291 for (int i = 0; i < binsize; i++) 00292 hist.push_back (h_mix_ratio[i]*0.5f * weights[9]); 00293 00294 float sm = 0; 00295 for (size_t i = 0; i < hist.size (); i++) 00296 sm += hist[i]; 00297 00298 for (size_t i = 0; i < hist.size (); i++) 00299 hist[i] /= sm; 00300 } 00301 00302 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00303 template <typename PointInT, typename PointOutT> int 00304 pcl::ESFEstimation<PointInT, PointOutT>::lci ( 00305 const int x1, const int y1, const int z1, 00306 const int x2, const int y2, const int z2, 00307 float &ratio, int &incnt, int &pointcount) 00308 { 00309 int voxelcount = 0; 00310 int voxel_in = 0; 00311 int act_voxel[3]; 00312 act_voxel[0] = x1; 00313 act_voxel[1] = y1; 00314 act_voxel[2] = z1; 00315 int x_inc, y_inc, z_inc; 00316 int dx = x2 - x1; 00317 int dy = y2 - y1; 00318 int dz = z2 - z1; 00319 if (dx < 0) 00320 x_inc = -1; 00321 else 00322 x_inc = 1; 00323 int l = abs (dx); 00324 if (dy < 0) 00325 y_inc = -1 ; 00326 else 00327 y_inc = 1; 00328 int m = abs (dy); 00329 if (dz < 0) 00330 z_inc = -1 ; 00331 else 00332 z_inc = 1; 00333 int n = abs (dz); 00334 int dx2 = 2 * l; 00335 int dy2 = 2 * m; 00336 int dz2 = 2 * n; 00337 if ((l >= m) & (l >= n)) 00338 { 00339 int err_1 = dy2 - l; 00340 int err_2 = dz2 - l; 00341 for (int i = 1; i<l; i++) 00342 { 00343 voxelcount++;; 00344 voxel_in += static_cast<int>(lut_[act_voxel[0]][act_voxel[1]][act_voxel[2]] == 1); 00345 if (err_1 > 0) 00346 { 00347 act_voxel[1] += y_inc; 00348 err_1 -= dx2; 00349 } 00350 if (err_2 > 0) 00351 { 00352 act_voxel[2] += z_inc; 00353 err_2 -= dx2; 00354 } 00355 err_1 += dy2; 00356 err_2 += dz2; 00357 act_voxel[0] += x_inc; 00358 } 00359 } 00360 else if ((m >= l) & (m >= n)) 00361 { 00362 int err_1 = dx2 - m; 00363 int err_2 = dz2 - m; 00364 for (int i=1; i<m; i++) 00365 { 00366 voxelcount++; 00367 voxel_in += static_cast<int>(lut_[act_voxel[0]][act_voxel[1]][act_voxel[2]] == 1); 00368 if (err_1 > 0) 00369 { 00370 act_voxel[0] += x_inc; 00371 err_1 -= dy2; 00372 } 00373 if (err_2 > 0) 00374 { 00375 act_voxel[2] += z_inc; 00376 err_2 -= dy2; 00377 } 00378 err_1 += dx2; 00379 err_2 += dz2; 00380 act_voxel[1] += y_inc; 00381 } 00382 } 00383 else 00384 { 00385 int err_1 = dy2 - n; 00386 int err_2 = dx2 - n; 00387 for (int i=1; i<n; i++) 00388 { 00389 voxelcount++; 00390 voxel_in += static_cast<int>(lut_[act_voxel[0]][act_voxel[1]][act_voxel[2]] == 1); 00391 if (err_1 > 0) 00392 { 00393 act_voxel[1] += y_inc; 00394 err_1 -= dz2; 00395 } 00396 if (err_2 > 0) 00397 { 00398 act_voxel[0] += x_inc; 00399 err_2 -= dz2; 00400 } 00401 err_1 += dy2; 00402 err_2 += dx2; 00403 act_voxel[2] += z_inc; 00404 } 00405 } 00406 voxelcount++; 00407 voxel_in += static_cast<int>(lut_[act_voxel[0]][act_voxel[1]][act_voxel[2]] == 1); 00408 incnt = voxel_in; 00409 pointcount = voxelcount; 00410 00411 if (voxel_in >= voxelcount-1) 00412 return (0); 00413 00414 if (voxel_in <= 7) 00415 return (1); 00416 00417 ratio = static_cast<float>(voxel_in) / static_cast<float>(voxelcount); 00418 return (2); 00419 } 00420 00421 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00422 template <typename PointInT, typename PointOutT> void 00423 pcl::ESFEstimation<PointInT, PointOutT>::voxelize9 (PointCloudIn &cluster) 00424 { 00425 int xi,yi,zi,xx,yy,zz; 00426 for (size_t i = 0; i < cluster.points.size (); ++i) 00427 { 00428 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); 00429 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); 00430 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); 00431 00432 for (int x = -1; x < 2; x++) 00433 for (int y = -1; y < 2; y++) 00434 for (int z = -1; z < 2; z++) 00435 { 00436 xi = xx + x; 00437 yi = yy + y; 00438 zi = zz + z; 00439 00440 if (yi >= GRIDSIZE || xi >= GRIDSIZE || zi>=GRIDSIZE || yi < 0 || xi < 0 || zi < 0) 00441 { 00442 ; 00443 } 00444 else 00445 this->lut_[xi][yi][zi] = 1; 00446 } 00447 } 00448 } 00449 00450 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00451 template <typename PointInT, typename PointOutT> void 00452 pcl::ESFEstimation<PointInT, PointOutT>::cleanup9 (PointCloudIn &cluster) 00453 { 00454 int xi,yi,zi,xx,yy,zz; 00455 for (size_t i = 0; i < cluster.points.size (); ++i) 00456 { 00457 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); 00458 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); 00459 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); 00460 00461 for (int x = -1; x < 2; x++) 00462 for (int y = -1; y < 2; y++) 00463 for (int z = -1; z < 2; z++) 00464 { 00465 xi = xx + x; 00466 yi = yy + y; 00467 zi = zz + z; 00468 00469 if (yi >= GRIDSIZE || xi >= GRIDSIZE || zi>=GRIDSIZE || yi < 0 || xi < 0 || zi < 0) 00470 { 00471 ; 00472 } 00473 else 00474 this->lut_[xi][yi][zi] = 0; 00475 } 00476 } 00477 } 00478 00479 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00480 template <typename PointInT, typename PointOutT> void 00481 pcl::ESFEstimation<PointInT, PointOutT>::scale_points_unit_sphere ( 00482 const pcl::PointCloud<PointInT> &pc, float scalefactor, Eigen::Vector4f& centroid) 00483 { 00484 pcl::compute3DCentroid (pc, centroid); 00485 pcl::demeanPointCloud (pc, centroid, local_cloud_); 00486 00487 float max_distance = 0, d; 00488 pcl::PointXYZ cog (0, 0, 0); 00489 00490 for (size_t i = 0; i < local_cloud_.points.size (); ++i) 00491 { 00492 d = pcl::euclideanDistance(cog,local_cloud_.points[i]); 00493 if (d > max_distance) 00494 max_distance = d; 00495 } 00496 00497 float scale_factor = 1.0f / max_distance * scalefactor; 00498 00499 Eigen::Affine3f matrix = Eigen::Affine3f::Identity(); 00500 matrix.scale (scale_factor); 00501 pcl::transformPointCloud (local_cloud_, local_cloud_, matrix); 00502 } 00503 00504 ////////////////////////////////////////////////////////////////////////////////////////////// 00505 template<typename PointInT, typename PointOutT> void 00506 pcl::ESFEstimation<PointInT, PointOutT>::compute (PointCloudOut &output) 00507 { 00508 if (!Feature<PointInT, PointOutT>::initCompute ()) 00509 { 00510 output.width = output.height = 0; 00511 output.points.clear (); 00512 return; 00513 } 00514 // Copy the header 00515 output.header = input_->header; 00516 00517 // Resize the output dataset 00518 // Important! We should only allocate precisely how many elements we will need, otherwise 00519 // we risk at pre-allocating too much memory which could lead to bad_alloc 00520 // (see http://dev.pointclouds.org/issues/657) 00521 output.width = output.height = 1; 00522 output.is_dense = input_->is_dense; 00523 output.points.resize (1); 00524 00525 // Perform the actual feature computation 00526 computeFeature (output); 00527 00528 Feature<PointInT, PointOutT>::deinitCompute (); 00529 } 00530 00531 00532 ////////////////////////////////////////////////////////////////////////////////////////////// 00533 template <typename PointInT, typename PointOutT> void 00534 pcl::ESFEstimation<PointInT, PointOutT>::computeFeature (PointCloudOut &output) 00535 { 00536 Eigen::Vector4f xyz_centroid; 00537 std::vector<float> hist; 00538 scale_points_unit_sphere (*surface_, static_cast<float>(GRIDSIZE_H), xyz_centroid); 00539 this->voxelize9 (local_cloud_); 00540 this->computeESF (local_cloud_, hist); 00541 this->cleanup9 (local_cloud_); 00542 00543 // We only output _1_ signature 00544 output.points.resize (1); 00545 output.width = 1; 00546 output.height = 1; 00547 00548 for (size_t d = 0; d < hist.size (); ++d) 00549 output.points[0].histogram[d] = hist[d]; 00550 } 00551 00552 #define PCL_INSTANTIATE_ESFEstimation(T,OutT) template class PCL_EXPORTS pcl::ESFEstimation<T,OutT>; 00553 00554 #endif // PCL_FEATURES_IMPL_ESF_H_ 00555