41 #ifndef PCL_FEATURES_IMPL_ESF_H_
42 #define PCL_FEATURES_IMPL_ESF_H_
44 #include <pcl/features/esf.h>
45 #include <pcl/common/common.h>
46 #include <pcl/common/distances.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;
57 srand (static_cast<unsigned int> (time (0)));
58 int maxindex =
static_cast<int> (pc.
points.size ());
60 int index1, index2, index3;
61 std::vector<float> d2v, d1v, d3v, wt_d3;
62 std::vector<int> wt_d2;
63 d1v.reserve (sample_size);
64 d2v.reserve (sample_size * 3);
65 d3v.reserve (sample_size);
66 wt_d2.reserve (sample_size * 3);
67 wt_d3.reserve (sample_size);
69 float h_in[binsize] = {0};
70 float h_out[binsize] = {0};
71 float h_mix[binsize] = {0};
72 float h_mix_ratio[binsize] = {0};
74 float h_a3_in[binsize] = {0};
75 float h_a3_out[binsize] = {0};
76 float h_a3_mix[binsize] = {0};
77 float h_d1[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 (
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)
119 th1 =
static_cast<int> (pcl_round (acos (fabs (v21.dot (v31))) / pih * (binsize-1)));
120 th2 =
static_cast<int> (pcl_round (acos (fabs (v23.dot (v31))) / pih * (binsize-1)));
121 th3 =
static_cast<int> (pcl_round (acos (fabs (v23.dot (v21))) / pih * (binsize-1)));
122 if (th1 < 0 || th1 >= binsize)
127 if (th2 < 0 || th2 >= binsize)
132 if (th3 < 0 || th3 >= binsize)
147 const int xs = p1[0] < 0.0?
static_cast<int>(floor(p1[0])+GRIDSIZE_H): static_cast<int>(ceil(p1[0])+GRIDSIZE_H-1);
148 const int ys = p1[1] < 0.0?
static_cast<int>(floor(p1[1])+GRIDSIZE_H): static_cast<int>(ceil(p1[1])+GRIDSIZE_H-1);
149 const int zs = p1[2] < 0.0?
static_cast<int>(floor(p1[2])+GRIDSIZE_H): static_cast<int>(ceil(p1[2])+GRIDSIZE_H-1);
150 const int xt = p2[0] < 0.0?
static_cast<int>(floor(p2[0])+GRIDSIZE_H): static_cast<int>(ceil(p2[0])+GRIDSIZE_H-1);
151 const int yt = p2[1] < 0.0?
static_cast<int>(floor(p2[1])+GRIDSIZE_H): static_cast<int>(ceil(p2[1])+GRIDSIZE_H-1);
152 const int zt = p2[2] < 0.0?
static_cast<int>(floor(p2[2])+GRIDSIZE_H): static_cast<int>(ceil(p2[2])+GRIDSIZE_H-1);
153 wt_d2.push_back (this->lci (xs, ys, zs, xt, yt, zt, ratio, vxlcnt, pcnt1));
154 if (wt_d2.back () == 2)
155 h_mix_ratio[static_cast<int> (pcl_round (ratio * (binsize-1)))]++;
156 vxlcnt_sum += vxlcnt;
161 const int xs = p1[0] < 0.0?
static_cast<int>(floor(p1[0])+GRIDSIZE_H): static_cast<int>(ceil(p1[0])+GRIDSIZE_H-1);
162 const int ys = p1[1] < 0.0?
static_cast<int>(floor(p1[1])+GRIDSIZE_H): static_cast<int>(ceil(p1[1])+GRIDSIZE_H-1);
163 const int zs = p1[2] < 0.0?
static_cast<int>(floor(p1[2])+GRIDSIZE_H): static_cast<int>(ceil(p1[2])+GRIDSIZE_H-1);
164 const int xt = p3[0] < 0.0?
static_cast<int>(floor(p3[0])+GRIDSIZE_H): static_cast<int>(ceil(p3[0])+GRIDSIZE_H-1);
165 const int yt = p3[1] < 0.0?
static_cast<int>(floor(p3[1])+GRIDSIZE_H): static_cast<int>(ceil(p3[1])+GRIDSIZE_H-1);
166 const int zt = p3[2] < 0.0?
static_cast<int>(floor(p3[2])+GRIDSIZE_H): static_cast<int>(ceil(p3[2])+GRIDSIZE_H-1);
167 wt_d2.push_back (this->lci (xs, ys, zs, xt, yt, zt, ratio, vxlcnt, pcnt2));
168 if (wt_d2.back () == 2)
169 h_mix_ratio[static_cast<int>(pcl_round (ratio * (binsize-1)))]++;
170 vxlcnt_sum += vxlcnt;
175 const int xs = p2[0] < 0.0?
static_cast<int>(floor(p2[0])+GRIDSIZE_H): static_cast<int>(ceil(p2[0])+GRIDSIZE_H-1);
176 const int ys = p2[1] < 0.0?
static_cast<int>(floor(p2[1])+GRIDSIZE_H): static_cast<int>(ceil(p2[1])+GRIDSIZE_H-1);
177 const int zs = p2[2] < 0.0?
static_cast<int>(floor(p2[2])+GRIDSIZE_H): static_cast<int>(ceil(p2[2])+GRIDSIZE_H-1);
178 const int xt = p3[0] < 0.0?
static_cast<int>(floor(p3[0])+GRIDSIZE_H): static_cast<int>(ceil(p3[0])+GRIDSIZE_H-1);
179 const int yt = p3[1] < 0.0?
static_cast<int>(floor(p3[1])+GRIDSIZE_H): static_cast<int>(ceil(p3[1])+GRIDSIZE_H-1);
180 const int zt = p3[2] < 0.0?
static_cast<int>(floor(p3[2])+GRIDSIZE_H): static_cast<int>(ceil(p3[2])+GRIDSIZE_H-1);
181 wt_d2.push_back (this->lci (xs,ys,zs,xt,yt,zt,ratio,vxlcnt,pcnt3));
182 if (wt_d2.back () == 2)
183 h_mix_ratio[static_cast<int>(pcl_round(ratio * (binsize-1)))]++;
184 vxlcnt_sum += vxlcnt;
189 d3v.push_back (sqrtf (sqrtf (s * (s-a) * (s-b) * (s-c))));
190 if (vxlcnt_sum <= 21)
193 h_a3_out[th1] +=
static_cast<float> (pcnt3) / 32.0f;
194 h_a3_out[th2] +=
static_cast<float> (pcnt1) / 32.0f;
195 h_a3_out[th3] +=
static_cast<float> (pcnt2) / 32.0f;
198 if (p_cnt - vxlcnt_sum < 4)
200 h_a3_in[th1] +=
static_cast<float> (pcnt3) / 32.0f;
201 h_a3_in[th2] +=
static_cast<float> (pcnt1) / 32.0f;
202 h_a3_in[th3] +=
static_cast<float> (pcnt2) / 32.0f;
207 h_a3_mix[th1] +=
static_cast<float> (pcnt3) / 32.0f;
208 h_a3_mix[th2] +=
static_cast<float> (pcnt1) / 32.0f;
209 h_a3_mix[th3] +=
static_cast<float> (pcnt2) / 32.0f;
210 wt_d3.push_back (static_cast<float> (vxlcnt_sum) / static_cast<float> (p_cnt));
217 for (
size_t nn_idx = 0; nn_idx < sample_size; ++nn_idx)
220 if (d2v[nn_idx] > maxd2)
222 if (d2v[sample_size + nn_idx] > maxd2)
223 maxd2 = d2v[sample_size + nn_idx];
224 if (d2v[sample_size*2 +nn_idx] > maxd2)
225 maxd2 = d2v[sample_size*2 +nn_idx];
226 if (d3v[nn_idx] > maxd3)
232 for (
size_t nn_idx = 0; nn_idx < sample_size; ++nn_idx)
234 if (wt_d3[nn_idx] >= 0.999)
236 index =
static_cast<int>(pcl_round (d3v[nn_idx] / maxd3 * (binsize-1)));
237 if (index >= 0 && index < binsize)
242 if (wt_d3[nn_idx] <= 0.001)
244 index =
static_cast<int>(pcl_round (d3v[nn_idx] / maxd3 * (binsize-1)));
245 if (index >= 0 && index < binsize)
250 index =
static_cast<int>(pcl_round (d3v[nn_idx] / maxd3 * (binsize-1)));
251 if (index >= 0 && index < binsize)
257 for (
size_t nn_idx = 0; nn_idx < d2v.size(); ++nn_idx )
259 if (wt_d2[nn_idx] == 0)
260 h_in[
static_cast<int>(pcl_round (d2v[nn_idx] / maxd2 * (binsize-1)))]++ ;
261 if (wt_d2[nn_idx] == 1)
262 h_out[
static_cast<int>(pcl_round (d2v[nn_idx] / maxd2 * (binsize-1)))]++;
263 if (wt_d2[nn_idx] == 2)
264 h_mix[
static_cast<int>(pcl_round (d2v[nn_idx] / maxd2 * (binsize-1)))]++ ;
268 float weights[10] = {0.5f, 0.5f, 0.5f, 0.5f, 0.5f, 1.0f, 1.0f, 2.0f, 2.0f, 2.0f};
270 hist.reserve (binsize * 10);
271 for (
int i = 0; i < binsize; i++)
272 hist.push_back (h_a3_in[i] * weights[0]);
273 for (
int i = 0; i < binsize; i++)
274 hist.push_back (h_a3_out[i] * weights[1]);
275 for (
int i = 0; i < binsize; i++)
276 hist.push_back (h_a3_mix[i] * weights[2]);
278 for (
int i = 0; i < binsize; i++)
279 hist.push_back (h_d3_in[i] * weights[3]);
280 for (
int i = 0; i < binsize; i++)
281 hist.push_back (h_d3_out[i] * weights[4]);
282 for (
int i = 0; i < binsize; i++)
283 hist.push_back (h_d3_mix[i] * weights[5]);
285 for (
int i = 0; i < binsize; i++)
286 hist.push_back (h_in[i]*0.5f * weights[6]);
287 for (
int i = 0; i < binsize; i++)
288 hist.push_back (h_out[i] * weights[7]);
289 for (
int i = 0; i < binsize; i++)
290 hist.push_back (h_mix[i] * weights[8]);
291 for (
int i = 0; i < binsize; i++)
292 hist.push_back (h_mix_ratio[i]*0.5f * weights[9]);
295 for (
size_t i = 0; i < hist.size (); i++)
298 for (
size_t i = 0; i < hist.size (); i++)
303 template <
typename Po
intInT,
typename Po
intOutT>
int
305 const int x1,
const int y1,
const int z1,
306 const int x2,
const int y2,
const int z2,
307 float &ratio,
int &incnt,
int &pointcount)
315 int x_inc, y_inc, z_inc;
337 if ((l >= m) & (l >= n))
341 for (
int i = 1; i<l; i++)
344 voxel_in +=
static_cast<int>(lut_[act_voxel[0]][act_voxel[1]][act_voxel[2]] == 1);
347 act_voxel[1] += y_inc;
352 act_voxel[2] += z_inc;
357 act_voxel[0] += x_inc;
360 else if ((m >= l) & (m >= n))
364 for (
int i=1; i<m; i++)
367 voxel_in +=
static_cast<int>(lut_[act_voxel[0]][act_voxel[1]][act_voxel[2]] == 1);
370 act_voxel[0] += x_inc;
375 act_voxel[2] += z_inc;
380 act_voxel[1] += y_inc;
387 for (
int i=1; i<n; i++)
390 voxel_in +=
static_cast<int>(lut_[act_voxel[0]][act_voxel[1]][act_voxel[2]] == 1);
393 act_voxel[1] += y_inc;
398 act_voxel[0] += x_inc;
403 act_voxel[2] += z_inc;
407 voxel_in +=
static_cast<int>(lut_[act_voxel[0]][act_voxel[1]][act_voxel[2]] == 1);
409 pointcount = voxelcount;
411 if (voxel_in >= voxelcount-1)
417 ratio =
static_cast<float>(voxel_in) / static_cast<float>(voxelcount);
422 template <
typename Po
intInT,
typename Po
intOutT>
void
425 int xi,yi,zi,xx,yy,zz;
426 for (
size_t i = 0; i < cluster.
points.size (); ++i)
428 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);
429 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);
430 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);
432 for (
int x = -1; x < 2; x++)
433 for (
int y = -1; y < 2; y++)
434 for (
int z = -1; z < 2; z++)
440 if (yi >= GRIDSIZE || xi >= GRIDSIZE || zi>=GRIDSIZE || yi < 0 || xi < 0 || zi < 0)
445 this->lut_[xi][yi][zi] = 1;
451 template <
typename Po
intInT,
typename Po
intOutT>
void
454 int xi,yi,zi,xx,yy,zz;
455 for (
size_t i = 0; i < cluster.
points.size (); ++i)
457 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);
458 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);
459 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);
461 for (
int x = -1; x < 2; x++)
462 for (
int y = -1; y < 2; y++)
463 for (
int z = -1; z < 2; z++)
469 if (yi >= GRIDSIZE || xi >= GRIDSIZE || zi>=GRIDSIZE || yi < 0 || xi < 0 || zi < 0)
474 this->lut_[xi][yi][zi] = 0;
480 template <
typename Po
intInT,
typename Po
intOutT>
void
487 float max_distance = 0, d;
490 for (
size_t i = 0; i < local_cloud_.points.size (); ++i)
493 if (d > max_distance)
497 float scale_factor = 1.0f / max_distance * scalefactor;
499 Eigen::Affine3f matrix = Eigen::Affine3f::Identity();
500 matrix.scale (scale_factor);
505 template<
typename Po
intInT,
typename Po
intOutT>
void
515 output.
header = input_->header;
526 computeFeature (output);
533 template <
typename Po
intInT,
typename Po
intOutT>
void
536 Eigen::Vector4f xyz_centroid;
537 std::vector<float> hist;
538 scale_points_unit_sphere (*surface_, static_cast<float>(GRIDSIZE_H), xyz_centroid);
539 this->voxelize9 (local_cloud_);
540 this->computeESF (local_cloud_, hist);
541 this->cleanup9 (local_cloud_);
548 for (
size_t d = 0; d < hist.size (); ++d)
549 output.
points[0].histogram[d] = hist[d];
552 #define PCL_INSTANTIATE_ESFEstimation(T,OutT) template class PCL_EXPORTS pcl::ESFEstimation<T,OutT>;
554 #endif // PCL_FEATURES_IMPL_ESF_H_
void demeanPointCloud(ConstCloudIterator< PointT > &cloud_iterator, const Eigen::Matrix< Scalar, 4, 1 > ¢roid, pcl::PointCloud< PointT > &cloud_out, int npts=0)
Subtract a centroid from a point cloud and return the de-meaned representation.
Feature represents the base feature class.
void cleanup9(PointCloudIn &cluster)
...
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform)
Apply an affine transform defined by an Eigen Transform.
pcl::PCLHeader header
The point cloud header.
uint32_t width
The point cloud width (if organized as an image-structure).
float euclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the euclidean distance between the two given points.
void computeESF(PointCloudIn &pc, std::vector< float > &hist)
...
int lci(const int x1, const int y1, const int z1, const int x2, const int y2, const int z2, float &ratio, int &incnt, int &pointcount)
...
void voxelize9(PointCloudIn &cluster)
...
uint32_t height
The point cloud height (if organized as an image-structure).
void scale_points_unit_sphere(const pcl::PointCloud< PointInT > &pc, float scalefactor, Eigen::Vector4f ¢roid)
...
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
void computeFeature(PointCloudOut &output)
Estimate the Ensebmel of Shape Function (ESF) descriptors at a set of points given by <setInputCloud ...
A point structure representing Euclidean xyz coordinates.
unsigned int compute3DCentroid(ConstCloudIterator< PointT > &cloud_iterator, Eigen::Matrix< Scalar, 4, 1 > ¢roid)
Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
void compute(PointCloudOut &output)
Overloaded computed method from pcl::Feature.