|
Point Cloud Library (PCL)
1.6.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/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
1.7.6.1