00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
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
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
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
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
00138
00139
00140
00141
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
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
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
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
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
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
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
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)
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)
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
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
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
00523 output.header = input_->header;
00524
00525
00526
00527
00528
00529 output.width = output.height = 1;
00530 output.is_dense = input_->is_dense;
00531 output.points.resize (1);
00532
00533
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
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