55 const int binsize = 64;
56 unsigned int sample_size = 20000;
58 srand (
static_cast<unsigned int> (time (
nullptr)));
59 const auto maxindex = pc.
size ();
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};
78 float h_d3_in[binsize] = {0};
79 float h_d3_out[binsize] = {0};
80 float h_d3_mix[binsize] = {0};
83 float pih =
static_cast<float>(
M_PI) / 2.0f;
87 int pcnt1,pcnt2,pcnt3;
88 for (std::size_t nn_idx = 0; nn_idx < sample_size; ++nn_idx)
91 int index1 = rand()%maxindex;
92 int index2 = rand()%maxindex;
93 int index3 = rand()%maxindex;
95 if (index1==index2 || index1 == index3 || index2 == index3)
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)
121 th1 =
static_cast<int> (
pcl_round (std::acos (std::abs (v21.dot (v31))) / pih * (binsize-1)));
122 th2 =
static_cast<int> (
pcl_round (std::acos (std::abs (v23.dot (v31))) / pih * (binsize-1)));
123 th3 =
static_cast<int> (
pcl_round (std::acos (std::abs (v23.dot (v21))) / pih * (binsize-1)));
124 if (th1 < 0 || th1 >= binsize)
129 if (th2 < 0 || th2 >= binsize)
134 if (th3 < 0 || th3 >= binsize)
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);
155 wt_d2.push_back (this->lci (xs, ys, zs, xt, yt, zt, ratio, vxlcnt, pcnt1));
156 if (wt_d2.back () == 2)
157 h_mix_ratio[
static_cast<int> (
pcl_round (ratio * (binsize-1)))]++;
158 vxlcnt_sum += vxlcnt;
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);
169 wt_d2.push_back (this->lci (xs, ys, zs, xt, yt, zt, ratio, vxlcnt, pcnt2));
170 if (wt_d2.back () == 2)
171 h_mix_ratio[
static_cast<int>(
pcl_round (ratio * (binsize-1)))]++;
172 vxlcnt_sum += vxlcnt;
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);
183 wt_d2.push_back (this->lci (xs,ys,zs,xt,yt,zt,ratio,vxlcnt,pcnt3));
184 if (wt_d2.back () == 2)
185 h_mix_ratio[
static_cast<int>(
pcl_round(ratio * (binsize-1)))]++;
186 vxlcnt_sum += vxlcnt;
191 d3v.push_back (std::sqrt (std::sqrt (s * (s-a) * (s-b) * (s-c))));
192 if (vxlcnt_sum <= 21)
195 h_a3_out[th1] +=
static_cast<float> (pcnt3) / 32.0f;
196 h_a3_out[th2] +=
static_cast<float> (pcnt1) / 32.0f;
197 h_a3_out[th3] +=
static_cast<float> (pcnt2) / 32.0f;
200 if (p_cnt - vxlcnt_sum < 4)
202 h_a3_in[th1] +=
static_cast<float> (pcnt3) / 32.0f;
203 h_a3_in[th2] +=
static_cast<float> (pcnt1) / 32.0f;
204 h_a3_in[th3] +=
static_cast<float> (pcnt2) / 32.0f;
209 h_a3_mix[th1] +=
static_cast<float> (pcnt3) / 32.0f;
210 h_a3_mix[th2] +=
static_cast<float> (pcnt1) / 32.0f;
211 h_a3_mix[th3] +=
static_cast<float> (pcnt2) / 32.0f;
212 wt_d3.push_back (
static_cast<float> (vxlcnt_sum) /
static_cast<float> (p_cnt));
219 for (std::size_t nn_idx = 0; nn_idx < sample_size; ++nn_idx)
222 if (d2v[nn_idx] > maxd2)
224 if (d2v[sample_size + nn_idx] > maxd2)
225 maxd2 = d2v[sample_size + nn_idx];
226 if (d2v[sample_size*2 +nn_idx] > maxd2)
227 maxd2 = d2v[sample_size*2 +nn_idx];
228 if (d3v[nn_idx] > maxd3)
234 for (std::size_t nn_idx = 0; nn_idx < sample_size; ++nn_idx)
236 if (wt_d3[nn_idx] >= 0.999)
238 index =
static_cast<int>(
pcl_round (d3v[nn_idx] / maxd3 * (binsize-1)));
239 if (index >= 0 && index < binsize)
244 if (wt_d3[nn_idx] <= 0.001)
246 index =
static_cast<int>(
pcl_round (d3v[nn_idx] / maxd3 * (binsize-1)));
247 if (index >= 0 && index < binsize)
252 index =
static_cast<int>(
pcl_round (d3v[nn_idx] / maxd3 * (binsize-1)));
253 if (index >= 0 && index < binsize)
259 for (std::size_t nn_idx = 0; nn_idx < d2v.size(); ++nn_idx )
261 if (wt_d2[nn_idx] == 0)
262 h_in[
static_cast<int>(
pcl_round (d2v[nn_idx] / maxd2 * (binsize-1)))]++ ;
263 if (wt_d2[nn_idx] == 1)
264 h_out[
static_cast<int>(
pcl_round (d2v[nn_idx] / maxd2 * (binsize-1)))]++;
265 if (wt_d2[nn_idx] == 2)
266 h_mix[
static_cast<int>(
pcl_round (d2v[nn_idx] / maxd2 * (binsize-1)))]++ ;
270 float weights[10] = {0.5f, 0.5f, 0.5f, 0.5f, 0.5f, 1.0f, 1.0f, 2.0f, 2.0f, 2.0f};
272 hist.reserve (binsize * 10);
273 for (
const float &i : h_a3_in)
274 hist.push_back (i * weights[0]);
275 for (
const float &i : h_a3_out)
276 hist.push_back (i * weights[1]);
277 for (
const float &i : h_a3_mix)
278 hist.push_back (i * weights[2]);
280 for (
const float &i : h_d3_in)
281 hist.push_back (i * weights[3]);
282 for (
const float &i : h_d3_out)
283 hist.push_back (i * weights[4]);
284 for (
const float &i : h_d3_mix)
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]);
293 for (
const float &i : h_mix_ratio)
294 hist.push_back (i*0.5f * weights[9]);
297 for (
const float &i : hist)
300 for (
float &i : hist)