41 #ifndef PCL_FEATURES_IMPL_ESF_H_
42 #define PCL_FEATURES_IMPL_ESF_H_
44 #include <pcl/features/esf.h>
47 #include <pcl/common/transforms.h>
51 template <
typename Po
intInT,
typename Po
intOutT>
void
55 const int binsize = 64;
56 unsigned int sample_size = 20000;
58 srand (
static_cast<unsigned int> (time (
nullptr)));
59 int maxindex =
static_cast<int> (pc.
points.size ());
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);
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};
75 float h_a3_in[binsize] = {0};
76 float h_a3_out[binsize] = {0};
77 float h_a3_mix[binsize] = {0};
79 float h_d3_in[binsize] = {0};
80 float h_d3_out[binsize] = {0};
81 float h_d3_mix[binsize] = {0};
84 float pih =
static_cast<float>(M_PI) / 2.0f;
88 int pcnt1,pcnt2,pcnt3;
89 for (std::size_t nn_idx = 0; nn_idx < sample_size; ++nn_idx)
92 index1 = rand()%maxindex;
93 index2 = rand()%maxindex;
94 index3 = rand()%maxindex;
96 if (index1==index2 || index1 == index3 || index2 == index3)
102 Eigen::Vector4f p1 = pc.
points[index1].getVector4fMap ();
103 Eigen::Vector4f p2 = pc.
points[index2].getVector4fMap ();
104 Eigen::Vector4f p3 = pc.
points[index3].getVector4fMap ();
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)
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)
130 if (th2 < 0 || th2 >= binsize)
135 if (th3 < 0 || th3 >= binsize)
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;
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;
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;
192 d3v.push_back (std::sqrt (std::sqrt (s * (s-a) * (s-b) * (s-c))));
193 if (vxlcnt_sum <= 21)
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;
201 if (p_cnt - vxlcnt_sum < 4)
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;
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));
220 for (std::size_t nn_idx = 0; nn_idx < sample_size; ++nn_idx)
223 if (d2v[nn_idx] > maxd2)
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)
235 for (std::size_t nn_idx = 0; nn_idx < sample_size; ++nn_idx)
237 if (wt_d3[nn_idx] >= 0.999)
239 index =
static_cast<int>(
pcl_round (d3v[nn_idx] / maxd3 * (binsize-1)));
240 if (index >= 0 && index < binsize)
245 if (wt_d3[nn_idx] <= 0.001)
247 index =
static_cast<int>(
pcl_round (d3v[nn_idx] / maxd3 * (binsize-1)));
248 if (index >= 0 && index < binsize)
253 index =
static_cast<int>(
pcl_round (d3v[nn_idx] / maxd3 * (binsize-1)));
254 if (index >= 0 && index < binsize)
260 for (std::size_t nn_idx = 0; nn_idx < d2v.size(); ++nn_idx )
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)))]++ ;
271 float weights[10] = {0.5f, 0.5f, 0.5f, 0.5f, 0.5f, 1.0f, 1.0f, 2.0f, 2.0f, 2.0f};
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]);
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]);
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]);
298 for (
const float &i : hist)
301 for (
float &i : hist)
306 template <
typename Po
intInT,
typename Po
intOutT>
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)
318 int x_inc, y_inc, z_inc;
326 int l = std::abs (dx);
331 int m = std::abs (dy);
336 int n = std::abs (dz);
340 if ((l >= m) & (l >= n))
344 for (
int i = 1; i<l; i++)
347 voxel_in +=
static_cast<int>(lut_[act_voxel[0]][act_voxel[1]][act_voxel[2]] == 1);
350 act_voxel[1] += y_inc;
355 act_voxel[2] += z_inc;
360 act_voxel[0] += x_inc;
363 else if ((m >= l) & (m >= n))
367 for (
int i=1; i<m; i++)
370 voxel_in +=
static_cast<int>(lut_[act_voxel[0]][act_voxel[1]][act_voxel[2]] == 1);
373 act_voxel[0] += x_inc;
378 act_voxel[2] += z_inc;
383 act_voxel[1] += y_inc;
390 for (
int i=1; i<n; i++)
393 voxel_in +=
static_cast<int>(lut_[act_voxel[0]][act_voxel[1]][act_voxel[2]] == 1);
396 act_voxel[1] += y_inc;
401 act_voxel[0] += x_inc;
406 act_voxel[2] += z_inc;
410 voxel_in +=
static_cast<int>(lut_[act_voxel[0]][act_voxel[1]][act_voxel[2]] == 1);
412 pointcount = voxelcount;
414 if (voxel_in >= voxelcount-1)
420 ratio =
static_cast<float>(voxel_in) /
static_cast<float>(voxelcount);
425 template <
typename Po
intInT,
typename Po
intOutT>
void
428 int xi,yi,zi,xx,yy,zz;
429 for (std::size_t i = 0; i < cluster.
points.size (); ++i)
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);
435 for (
int x = -1; x < 2; x++)
436 for (
int y = -1; y < 2; y++)
437 for (
int z = -1; z < 2; z++)
443 if (yi >= GRIDSIZE || xi >= GRIDSIZE || zi>=GRIDSIZE || yi < 0 || xi < 0 || zi < 0)
448 this->lut_[xi][yi][zi] = 1;
454 template <
typename Po
intInT,
typename Po
intOutT>
void
457 int xi,yi,zi,xx,yy,zz;
458 for (std::size_t i = 0; i < cluster.
points.size (); ++i)
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);
464 for (
int x = -1; x < 2; x++)
465 for (
int y = -1; y < 2; y++)
466 for (
int z = -1; z < 2; z++)
472 if (yi >= GRIDSIZE || xi >= GRIDSIZE || zi>=GRIDSIZE || yi < 0 || xi < 0 || zi < 0)
477 this->lut_[xi][yi][zi] = 0;
483 template <
typename Po
intInT,
typename Po
intOutT>
void
490 float max_distance = 0, d;
493 for (std::size_t i = 0; i < local_cloud_.points.size (); ++i)
496 if (d > max_distance)
500 float scale_factor = 1.0f / max_distance * scalefactor;
502 Eigen::Affine3f matrix = Eigen::Affine3f::Identity();
503 matrix.scale (scale_factor);
508 template<
typename Po
intInT,
typename Po
intOutT>
void
513 output.width = output.height = 0;
514 output.points.clear ();
518 output.header = input_->header;
524 output.width = output.height = 1;
525 output.is_dense = input_->is_dense;
526 output.points.resize (1);
529 computeFeature (output);
536 template <
typename Po
intInT,
typename Po
intOutT>
void
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_);
547 output.points.resize (1);
551 for (std::size_t d = 0; d < hist.size (); ++d)
552 output.points[0].histogram[d] = hist[d];
555 #define PCL_INSTANTIATE_ESFEstimation(T,OutT) template class PCL_EXPORTS pcl::ESFEstimation<T,OutT>;
557 #endif // PCL_FEATURES_IMPL_ESF_H_