58 srand (
static_cast<unsigned int> (time (
nullptr)));
62 std::vector<int>
wt_d2;
83 float pih =
static_cast<float>(
M_PI) / 2.0f;
101 Eigen::Vector4f p1 =
pc[
index1].getVector4fMap ();
102 Eigen::Vector4f p2 =
pc[
index2].getVector4fMap ();
103 Eigen::Vector4f p3 =
pc[
index3].getVector4fMap ();
106 Eigen::Vector4f
v21 (p2 - p1);
107 Eigen::Vector4f
v31 (p3 - p1);
108 Eigen::Vector4f
v23 (p2 - p3);
109 a =
v21.norm (); b =
v31.norm ();
c =
v23.norm (); s = (a+b+
c) * 0.5f;
110 if (s * (s-a) * (s-b) * (s-
c) <= 0.001f)
149 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);
150 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);
151 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);
152 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);
153 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);
154 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 if (
wt_d2.back () == 2)
163 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);
164 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);
165 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);
166 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);
167 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);
168 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 if (
wt_d2.back () == 2)
177 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);
178 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);
179 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);
180 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);
181 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);
182 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 if (
wt_d2.back () == 2)
191 d3v.push_back (std::sqrt (std::sqrt (s * (s-a) * (s-b) * (s-
c))));
239 if (index >= 0 && index <
binsize)
247 if (index >= 0 && index <
binsize)
253 if (index >= 0 && index <
binsize)
270 float weights[10] = {0.5f, 0.5f, 0.5f, 0.5f, 0.5f, 1.0f, 1.0f, 2.0f, 2.0f, 2.0f};
274 hist.push_back (i * weights[0]);
276 hist.push_back (i * weights[1]);
278 hist.push_back (i * weights[2]);
281 hist.push_back (i * weights[3]);
283 hist.push_back (i * weights[4]);
285 hist.push_back (i * weights[5]);
287 for (
const float &i :
h_in)
288 hist.push_back (i*0.5f * weights[6]);
289 for (
const float &i :
h_out)
290 hist.push_back (i * weights[7]);
291 for (
const float &i :
h_mix)
292 hist.push_back (i * weights[8]);
294 hist.push_back (i*0.5f * weights[9]);
297 for (
const float &i : hist)
300 for (
float &i : hist)
427 for (
const auto& point: cluster)
429 int xx = point.x<0.0?
static_cast<int>(std::floor(point.x)+GRIDSIZE_H) :
static_cast<int>(std::ceil(point.x)+GRIDSIZE_H-1);
430 int yy = point.y<0.0?
static_cast<int>(std::floor(point.y)+GRIDSIZE_H) :
static_cast<int>(std::ceil(point.y)+GRIDSIZE_H-1);
431 int zz = point.z<0.0?
static_cast<int>(std::floor(point.z)+GRIDSIZE_H) :
static_cast<int>(std::ceil(point.z)+GRIDSIZE_H-1);
433 for (
int x = -1; x < 2; x++)
434 for (
int y = -1; y < 2; y++)
435 for (
int z = -1; z < 2; z++)
441 if (
yi >= GRIDSIZE ||
xi >= GRIDSIZE ||
zi>=GRIDSIZE ||
yi < 0 ||
xi < 0 ||
zi < 0)
446 this->lut_[
xi][
yi][
zi] = 1;
455 for (
const auto& point: cluster)
457 int xx = point.x<0.0?
static_cast<int>(std::floor(point.x)+GRIDSIZE_H) :
static_cast<int>(std::ceil(point.x)+GRIDSIZE_H-1);
458 int yy = point.y<0.0?
static_cast<int>(std::floor(point.y)+GRIDSIZE_H) :
static_cast<int>(std::ceil(point.y)+GRIDSIZE_H-1);
459 int zz = point.z<0.0?
static_cast<int>(std::floor(point.z)+GRIDSIZE_H) :
static_cast<int>(std::ceil(point.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;
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 (std::size_t d = 0; d < hist.size (); ++d)
549 output[0].histogram[d] = hist[d];
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 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.
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Matrix< Scalar, 4, 4 > &transform, bool copy_all_fields)
Apply a rigid transform defined by a 4x4 matrix.
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.