ACloudViewer  3.9.4
A Modern Library for 3D Data Processing
CGAL_normEst.h
Go to the documentation of this file.
1 /* License Information
2  *
3  * Copyright (C) 2012 Boulch Alexandre, Ecole Nationale des Ponts et Chaussees -
4  * Ecole des Ponts ParisTech
5  *
6  * This program is free software: you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation, either version 3 of the License, or
9  * (at your option) any later version.
10  *
11  * This program is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with this program. If not, see <http://www.gnu.org/licenses/>.
18  *
19  * Note that this library relies on external libraries subject to their own
20  * license. To use this software, you are subject to the dependencies license,
21  * these licenses applies to the dependency ONLY and NOT this code. Please
22  * refer below to the web sites for license informations.
23  *
24  * OPENMP (http://openmp.org/)
25  * CGAL (http://www.cgal.org/) see CGAL Licence Term
26  */
27 
28 #ifndef NORM_EST_CGAL_OMP_H
29 #define NORM_EST_CGAL_OMP_H
30 
31 #if defined(_OPENMP) && defined(USE_OPENMP_FOR_NORMEST)
32 #include <omp.h>
33 #endif
34 
35 #include <CGAL/Aff_transformation_3.h>
36 #include <CGAL/Fuzzy_sphere.h>
37 #include <CGAL/Kd_tree.h>
38 #include <CGAL/Orthogonal_k_neighbor_search.h>
39 #include <CGAL/Point_3.h>
40 #include <CGAL/Search_traits_3.h>
41 #include <CGAL/Simple_cartesian.h>
42 #include <CGAL/Vector_3.h>
43 #include <time.h>
44 
45 #include <iostream>
46 #include <map>
47 #include <vector>
48 
63 class CGAL_Normal_Estimator {
64 private:
65 public:
70  template <typename T>
71  class My_Triplet {
72  private:
73  T data[3];
74 
75  public:
76  My_Triplet() {};
77  My_Triplet(T a, T b, T c) {
78  data[0] = a;
79  data[1] = b;
80  data[2] = c;
81  }
82  T &operator()(int i) { return data[i]; }
83  T operator()(int i) const { return data[i]; }
84  };
85 
86  typedef CGAL::Simple_cartesian<float> Kernel;
87  typedef typename CGAL::Point_3<Kernel> Point3;
88  typedef typename CGAL::Vector_3<Kernel> Vector3;
89  typedef typename CGAL::Aff_transformation_3<Kernel> Matrix3;
90  typedef typename CGAL::Search_traits_3<Kernel> TreeTraits;
91  typedef My_Triplet<Point3> Triplet;
92  typedef typename CGAL::Orthogonal_k_neighbor_search<TreeTraits>
93  Neighbor_search;
94  typedef typename CGAL::Kd_tree<TreeTraits> Tree;
95  typedef typename CGAL::Fuzzy_sphere<TreeTraits> Fuzzy_sphere;
96  typedef typename std::vector<Point3>::iterator vecPt3Iterator;
97  typedef typename std::vector<Vector3>::iterator vecVec3Iterator;
98  typedef typename std::vector<Triplet>::iterator vecTripIterator;
99  typedef typename Neighbor_search::iterator Neighbor_search_iterator;
100 
101  enum {
102  MEAN = 0,
103  BEST = 1,
105  CLUSTER = 2,
107  POINTS = 100, /*<POINTS value 100, used for method choice, triplets by
108  random selection in the neighborhood*/
109  UNIF = 101,
111  CUBES = 102,
113  KNN = 200,
115  RADIUS = 201
117  };
118 
124  CGAL_Normal_Estimator(std::vector<Point3> &points,
125  std::vector<Vector3> &normals)
126  : pts(points), nls(normals) {
127  set_default_parameters();
128  }
129 
130  int &number_of_planes() { return n_planes; }
131  int number_of_planes() const { return n_planes; }
132  int &accum_slices() { return n_phi; }
133  int accum_slices() const { return n_phi; }
134  int &rotation_number() { return n_rot; }
135  int rotation_number() const { return n_rot; }
136  int &normal_selection_mode() { return selection_type; }
137  int normal_selection_mode() const { return selection_type; }
138  float &cluster_angle_rad() { return tol_angle_rad; }
139  float cluster_angle_rad() const { return tol_angle_rad; }
140 
141  int &minimal_neighbor_number_for_range_search() {
142  return lower_neighbor_bound_neighbors;
143  }
144  int minimal_neighbor_number_for_range_search() const {
145  return lower_neighbor_bound_neighbors;
146  }
147 
148  float &small_radius_fact() { return small_radius_factor; }
149  float small_radius_fact() const { return small_radius_factor; }
150  int &number_of_cubes() { return n_cubes; }
151  int number_of_cubes() const { return n_cubes; }
152 
153  std::vector<Point3> &point_cloud() { return pts; }
154  std::vector<Point3> point_cloud() const { return pts; }
155  std::vector<Vector3> &normal_cloud() { return nls; }
156  std::vector<Vector3> normal_cloud() const { return nls; }
157 
158  void estimate(int method = POINTS,
159  int neighborhood_type = KNN,
160  float neighborhood_size = 200) {
161  std::cout << "Normal_Estimation ";
162  switch (method) {
163  case POINTS:
164  std::cout << "Points ";
165  switch (neighborhood_type) {
166  case KNN:
167  std::cout << "Knn=";
168  std::cout << (int)neighborhood_size << std::endl;
169  points_knn((int)neighborhood_size);
170  break;
171  case RADIUS:
172  std::cout << "radius=";
173  std::cout << neighborhood_size << std::endl;
174  points_radius(neighborhood_size);
175  break;
176  default:
177  std::cout << "Parameter Error : bad neighborhood type"
178  << std::endl;
179  break;
180  }
181  break;
182  case UNIF:
183  std::cout << "Unif ";
184  switch (neighborhood_type) {
185  case KNN:
186  std::cout << "Knn=";
187  std::cout << (int)neighborhood_size << std::endl;
188  unif_knn((int)neighborhood_size);
189  break;
190  case RADIUS:
191  std::cout << "radius=";
192  std::cout << neighborhood_size << std::endl;
193  unif_radius(neighborhood_size);
194  break;
195  default:
196  std::cout << "Parameter Error : bad neighborhood type"
197  << std::endl;
198  break;
199  }
200  break;
201  case CUBES:
202  std::cout << "Cubes ";
203  switch (neighborhood_type) {
204  case KNN:
205  std::cout << "Knn=";
206  std::cout << (int)neighborhood_size << std::endl;
207  cubes_knn((int)neighborhood_size);
208  break;
209  case RADIUS:
210  std::cout << "radius=";
211  std::cout << neighborhood_size << std::endl;
212  cubes_radius(neighborhood_size);
213  break;
214  default:
215  std::cout << "Parameter Error : bad neighborhood type"
216  << std::endl;
217  break;
218  }
219  break;
220  default:
221  std::cout << "Parameter Error : bad method" << std::endl;
222  break;
223  }
224  }
225 
226 private:
227  float PI;
228  int lower_neighbor_bound_neighbors;
232  int n_planes;
233  int n_phi;
234  int n_rot;
235  float tol_angle_rad;
236  float small_radius_factor;
238  int n_cubes;
239  int selection_type;
242  std::vector<Point3> &pts;
243  std::vector<Vector3> &nls;
248  void set_default_parameters() {
249  PI = 3.14159265f;
250  n_planes = 700;
251  n_rot = 5;
252  n_phi = 15;
253  tol_angle_rad = 0.79;
254  small_radius_factor = 4;
255  n_cubes = 4;
256  lower_neighbor_bound_neighbors = 10;
257  selection_type = CLUSTER;
258  }
259 
266  inline void generate_rotation_matrix(std::vector<Matrix3> &rotMat,
267  std::vector<Matrix3> &rotMatInv,
268  int rotations) {
269  rotMat.clear();
270  rotMatInv.clear();
271 
272  if (rotations == 0) {
273  Matrix3 rMat(1, 0, 0, 0, 1, 0, 0, 0, 1);
274  rotMat.push_back(rMat);
275  rotMatInv.push_back(rMat);
276  } else {
277  for (int i = 0; i < rotations; i++) {
278  float theta = (rand() + 0.f) / RAND_MAX * 2 * 3.14159265f;
279  float phi = (rand() + 0.f) / RAND_MAX * 2 * 3.14159265f;
280  float psi = (rand() + 0.f) / RAND_MAX * 2 * 3.14159265f;
281  Matrix3 Rt(1, 0, 0, 0, cos(theta), -sin(theta), 0, sin(theta),
282  cos(theta));
283  Matrix3 Rph(cos(phi), 0, sin(phi), 0, 1, 0, -sin(phi), 0,
284  cos(phi));
285  Matrix3 Rps(cos(psi), -sin(psi), 0, sin(psi), cos(psi), 0, 0, 0,
286  1);
287  Matrix3 Rtinv(1, 0, 0, 0, cos(theta), sin(theta), 0,
288  -sin(theta), cos(theta));
289  Matrix3 Rphinv(cos(phi), 0, -sin(phi), 0, 1, 0, sin(phi), 0,
290  cos(phi));
291  Matrix3 Rpsinv(cos(psi), sin(psi), 0, -sin(psi), cos(psi), 0, 0,
292  0, 1);
293 
294  Matrix3 rMat = Rt * Rph * Rps;
295  Matrix3 rMatInv = Rpsinv * Rphinv * Rtinv;
296  rotMat.push_back(rMat);
297  rotMatInv.push_back(rMatInv);
298  }
299  }
300  }
301 
307  inline void generate_random_points_vector(std::vector<Point3> &points,
308  int point_number) {
309  points.resize(point_number);
310  for (int i = 0; i < point_number; i++) {
311  float x, y, z;
312  do {
313  x = ((rand() + 0.f) / RAND_MAX) * 2 - 1;
314  y = ((rand() + 0.f) / RAND_MAX) * 2 - 1;
315  z = ((rand() + 0.f) / RAND_MAX) * 2 - 1;
316  } while (x * x + y * y + z * z > 1);
317 
318  points[i] = Point3(x, y, z);
319  }
320  }
321 
327  inline void generate_random_int_vector(std::vector<int> &vecInt,
328  int point_number) {
329  vecInt.resize(point_number);
330  for (int i = 0; i < point_number; i++) {
331  vecInt[i] = rand();
332  }
333  }
334 
344  inline void list_of_triplets(std::vector<Vector3> &triplets,
345  const int &number_of_points,
346  const unsigned int &plane_number,
347  std::vector<int> &vecInt) {
348  /*
349  * Here we take care of not using twice the same plane
350  * For that we use the combinatorial number system
351  */
352 
353  // computing the number of permutations
354  unsigned long long total_comb = number_of_points;
355  total_comb *= (number_of_points - 1);
356  total_comb *= (number_of_points - 2);
357  total_comb /= 6;
358 
359  std::vector<unsigned long long> tab_binome_3(number_of_points + 1);
360  std::vector<unsigned long long> tab_binome_2(number_of_points + 1);
361 
362  for (int i = 0; i < number_of_points + 1; i++) {
363  if (i > 3) {
364  tab_binome_3[i] = tab_binome_3[i - 1] * i / (i - 3);
365  } else if (i == 3) {
366  tab_binome_3[i] = 1;
367  } else {
368  tab_binome_3[i] = 0;
369  }
370  if (i > 2) {
371  tab_binome_2[i] = tab_binome_2[i - 1] * i / (i - 2);
372  } else if (i == 2) {
373  tab_binome_2[i] = 1;
374  } else {
375  tab_binome_2[i] = 0;
376  }
377  }
378 
379  std::vector<unsigned long long> comb_idx(plane_number);
380  for (int i = 0; i < plane_number; i++) {
381  comb_idx[i] = i % total_comb;
382  }
383  if (total_comb < RAND_MAX) {
384  std::map<int, int> table_next;
385  for (int i = 0; i < plane_number; i++) {
386  int temp_idx = vecInt[i % vecInt.size()] % total_comb;
387  if (temp_idx < plane_number) {
388  int temp = comb_idx[i];
389  comb_idx[i] = comb_idx[temp_idx];
390  comb_idx[temp_idx] = temp;
391  } else {
392  std::map<int, int>::iterator itmap =
393  table_next.find(temp_idx);
394  if (itmap != table_next.end()) {
395  int temp = comb_idx[i];
396  comb_idx[i] = itmap->second;
397  itmap->second = temp;
398  } else {
399  comb_idx[i] = temp_idx;
400  table_next.insert(std::pair<int, int>(temp_idx, i));
401  }
402  }
403  }
404  } else {
405  unsigned long long ref_RAND_MAX = RAND_MAX;
406  int size_test = 0;
407  while (ref_RAND_MAX < total_comb) {
408  size_test++;
409  ref_RAND_MAX *= RAND_MAX;
410  }
411  std::map<unsigned long long, unsigned long long> table_next;
412  int pos = 0;
413  for (int i = 0; i < plane_number; i++) {
414  // generating a random int
415  unsigned long long random_int = vecInt[pos % vecInt.size()];
416  pos++;
417  for (int j = 0; j < size_test; j++) {
418  random_int +=
419  ((unsigned long long)vecInt[pos % vecInt.size()]) *
420  RAND_MAX * (j + 1);
421  pos++;
422  }
423 
424  random_int = random_int % total_comb;
425  if (random_int < plane_number) {
426  int temp = comb_idx[i];
427  comb_idx[i] = comb_idx[random_int];
428  comb_idx[random_int] = temp;
429  } else {
430  std::map<unsigned long long, unsigned long long>::iterator
431  itmap = table_next.find(random_int);
432  if (itmap != table_next.end()) {
433  int temp = comb_idx[i];
434  comb_idx[i] = itmap->second;
435  itmap->second = temp;
436  } else {
437  comb_idx[i] = random_int;
438  table_next.insert(
439  std::pair<unsigned long long,
440  unsigned long long>(random_int, i));
441  }
442  }
443  }
444  }
445 
446  // getting the triplets from the numbers
447  triplets.resize(plane_number);
448  for (int pos = 0; pos < plane_number; pos++) {
449  int comb[3];
450  unsigned long long idx = comb_idx[pos];
451  int pos_temp = 0;
452  while (tab_binome_3[pos_temp] <= idx) {
453  pos_temp++;
454  }
455  pos_temp--;
456  comb[0] = pos_temp;
457  idx -= tab_binome_3[pos_temp];
458  if (idx == 0) {
459  comb[1] = 1;
460  comb[2] = 0;
461  triplets[pos] = Vector3(comb[0], comb[1], comb[2]);
462  continue;
463  }
464 
465  pos_temp = 0;
466  while (tab_binome_2[pos_temp] <= idx) {
467  pos_temp++;
468  }
469  pos_temp--;
470  comb[1] = pos_temp;
471  idx -= tab_binome_2[pos_temp];
472  if (idx == 0) {
473  comb[2] = 0;
474  triplets[pos] = Vector3(comb[0], comb[1], comb[2]);
475  continue;
476  }
477 
478  pos_temp = 0;
479  while (pos_temp != idx) {
480  pos_temp++;
481  }
482  comb[2] = pos_temp;
483  triplets[pos] = Vector3(comb[0], comb[1], comb[2]);
484  }
485  }
486 
493  inline void generate_cube_vector(std::vector<int> &cubes_idx, int n) {
494  // probabilities of picking a cube (ponderates by an approximation of
495  // its volume intersecting the sphere)
496  std::vector<float> probas(n_cubes * n_cubes * n_cubes);
497  float step = 2.f / n_cubes;
498  float xmin = -(n_cubes / 2.f) * step;
499  float ymin = -(n_cubes / 2.f) * step;
500  float zmin = -(n_cubes / 2.f) * step;
501  float sum_prob = 0;
502  for (int k = 0; k < n_cubes; k++) {
503  for (int j = 0; j < n_cubes; j++) {
504  for (int i = 0; i < n_cubes; i++) {
505  float prob = 0;
506 
507  float x1 = xmin + i * step;
508  float y1 = ymin + j * step;
509  float z1 = zmin + k * step;
510  float x2 = x1 + step;
511  float y2 = y1 + step;
512  float z2 = z1 + step;
513 
514  Vector3 pt(x1, y1, z1);
515  if (pt * pt <= 1) {
516  prob += 0.125;
517  }
518  pt = Vector3(x2, y1, z1);
519  if (pt * pt <= 1) {
520  prob += 0.125;
521  }
522  pt = Vector3(x1, y2, z1);
523  if (pt * pt <= 1) {
524  prob += 0.125;
525  }
526  pt = Vector3(x2, y2, z1);
527  if (pt * pt <= 1) {
528  prob += 0.125;
529  }
530  pt = Vector3(x1, y1, z2);
531  if (pt * pt <= 1) {
532  prob += 0.125;
533  }
534  pt = Vector3(x2, y1, z2);
535  if (pt * pt <= 1) {
536  prob += 0.125;
537  }
538  pt = Vector3(x1, y2, z2);
539  if (pt * pt <= 1) {
540  prob += 0.125;
541  }
542  pt = Vector3(x2, y2, z2);
543  if (pt * pt <= 1) {
544  prob += 0.125;
545  }
546  probas[i + j * n_cubes + k * n_cubes * n_cubes] = prob;
547  sum_prob += prob;
548  }
549  }
550  }
551  // cumulative proba sum
552  probas[0] /= sum_prob;
553  for (int i = 1; i < n_cubes * n_cubes * n_cubes; i++) {
554  probas[i] /= sum_prob;
555  probas[i] += probas[i - 1];
556  }
557 
558  // getting the cubes according to the probas
559  cubes_idx.resize(n);
560  for (int i = 0; i < n; i++) {
561  float pos = (rand() + 0.f) / RAND_MAX;
562  int begin = 0;
563  int end = n_cubes * n_cubes * n_cubes - 1;
564  int temp = (begin + end) / 2;
565  while (temp != begin) {
566  if (probas[temp] < pos) {
567  begin = temp;
568  } else {
569  end = temp;
570  }
571  temp = (begin + end) / 2;
572  }
573  cubes_idx[i] = end;
574  }
575  }
576 
586  inline void assign_points_to_cubes(std::vector<int> cubes[],
587  float radius,
588  Point3 &refPoint,
589  std::vector<Point3> &points,
590  std::vector<Point3> &points2,
591  Matrix3 &rotMat) {
592  float step = 2.f / n_cubes * radius;
593  for (unsigned int i = 0; i < points2.size(); i++) {
594  points[i] = points2[i];
595  points[i] = points[i].transform(rotMat);
596  Point3 refPoint2 = refPoint;
597  refPoint2 = refPoint2.transform(rotMat);
598  int x = std::max(
599  0, std::min(int((points[i].x() - refPoint2.x()) / step +
600  (n_cubes / 2.f)),
601  n_cubes - 1));
602  int y = std::max(
603  0, std::min(int((points[i].y() - refPoint2.y()) / step +
604  (n_cubes / 2.f)),
605  n_cubes - 1));
606  int z = std::max(
607  0, std::min(int((points[i].z() - refPoint2.z()) / step +
608  (n_cubes / 2.f)),
609  n_cubes - 1));
610  cubes[x + y * n_cubes + z * n_cubes * n_cubes].push_back(i);
611  }
612  }
613 
621  inline void generate_cubes_triplets(std::vector<Vector3> &triplets,
622  std::vector<int> cubes[],
623  std::vector<int> &cubes_idx,
624  std::vector<int> &vecInt) {
625  triplets.resize(n_planes);
626 
627  vecVec3Iterator ittrip = triplets.begin();
628  std::vector<int>::iterator itcube = cubes_idx.begin();
629  std::vector<int>::iterator itint = vecInt.begin();
630 
631  int idx = 0;
632  int coord[3];
633  while (ittrip != triplets.end() && itcube != cubes_idx.end()) {
634  if (cubes[*itcube].size() != 0) {
635  int new_idx = cubes[*itcube][(*itint) % cubes[*itcube].size()];
636  bool is_valid = true;
637  for (int i = 0; i < idx; i++) {
638  if (new_idx == coord[i]) {
639  is_valid = false;
640  }
641  }
642  if (is_valid) {
643  coord[idx] = new_idx;
644  idx++;
645  }
646  if (idx == 3) {
647  idx = 0;
648  *ittrip = Vector3(coord[0], coord[1], coord[2]);
649  ittrip++;
650  }
651  }
652  itcube++;
653  itint++;
654  }
655 
656  while (ittrip != triplets.end()) {
657  int picked_points = 0;
658  while (picked_points < 3) {
659  int pos = rand() % (n_cubes * n_cubes * n_cubes);
660  if (cubes[pos].size() == 0) {
661  continue;
662  }
663  int idx = rand() % cubes[pos].size();
664  bool is_valid = true;
665  for (int i = 0; i < picked_points; i++) {
666  if (cubes[pos][idx] == coord[i]) {
667  is_valid = false;
668  }
669  }
670  if (is_valid) {
671  coord[picked_points] = cubes[pos][idx];
672  picked_points++;
673  }
674  }
675  (*ittrip) = Vector3(coord[0], coord[1], coord[2]);
676  ittrip++;
677  }
678  }
679 
687  inline void find_a_triplet(Tree &tree,
688  float radius1,
689  float radius2,
690  Triplet &triplet,
691  Point3 &pt) {
692  int picked_points = 0;
693 
694  while (picked_points < 3) {
695  // picking point in the unit balls
696  /*
697  *For fast results we pick them in the cube and discard bad points
698  */
699  float x, y, z;
700  do {
701  x = ((rand() + 0.f) / RAND_MAX) * 2 - 1;
702  y = ((rand() + 0.f) / RAND_MAX) * 2 - 1;
703  z = ((rand() + 0.f) / RAND_MAX) * 2 - 1;
704  } while (x * x + y * y + z * z > 1);
705 
706  x *= radius1;
707  y *= radius1;
708  z *= radius1;
709 
710  x += pt.x();
711  y += pt.y();
712  z += pt.z();
713 
714  Point3 pt(x, y, z);
715 
716  Fuzzy_sphere s_query(pt, radius2);
717  std::vector<Point3> points_search;
718  tree.search(std::back_inserter(points_search), s_query);
719 
720  if (points_search.size() != 0) {
721  Point3 new_Point = points_search[rand() % points_search.size()];
722  bool is_valid = true;
723  for (int i = 0; i < picked_points; i++) {
724  if (new_Point == triplet(i)) {
725  is_valid = false;
726  }
727  }
728  if (is_valid) {
729  triplet(picked_points) = new_Point;
730  picked_points++;
731  }
732  }
733  }
734  }
735 
747  inline void generate_list_of_triplets(std::vector<Triplet> &triplets,
748  int plane_number,
749  float radius1,
750  float radius2,
751  Tree &tree,
752  Point3 &pt,
753  std::vector<Point3> &points,
754  std::vector<int> &vecInt) {
755  triplets.resize(plane_number);
756 
757  vecTripIterator ittrip = triplets.begin();
758  vecPt3Iterator itpoints = points.begin();
759  std::vector<int>::iterator itint = vecInt.begin();
760 
761  int idx = 0;
762  while (ittrip != triplets.end() && itpoints != points.end()) {
763  // getting coordinates
764  float x, y, z;
765  x = (*itpoints).x() * radius1 + pt.x();
766  y = (*itpoints).y() * radius1 + pt.y();
767  z = (*itpoints).z() * radius1 + pt.z();
768 
769  // searching neighbors of the point
770  Point3 refPoint(x, y, z);
771  Fuzzy_sphere s_query(refPoint, radius2);
772  std::vector<Point3> points_search;
773  tree.search(std::back_inserter(points_search), s_query);
774 
775  // testing the validity
776  if (points_search.size() != 0) {
777  Point3 new_point =
778  points_search[(*itint) % points_search.size()];
779  bool is_valid = true;
780  for (int i = 0; i < idx; i++) {
781  if (new_point == (*ittrip)(i)) {
782  is_valid = false;
783  }
784  }
785  if (is_valid) {
786  (*ittrip)(idx) = new_point;
787  idx++;
788  }
789  if (idx == 3) {
790  idx = 0;
791  ittrip++;
792  }
793  }
794 
795  itpoints++;
796  itint++;
797  }
798 
799  while (ittrip != triplets.end()) {
800  find_a_triplet(tree, radius1, radius2, *ittrip, pt);
801  ittrip++;
802  }
803  }
804 
814  float normal_at_point(const int d1,
815  const int d2,
816  std::vector<Point3> &points,
817  int n,
818  std::vector<Vector3> &triplets,
819  std::vector<float> &conf_interv) {
820  if (points.size() < 3) {
821  nls[n] = Vector3(0, 0, 0);
822  return 0;
823  }
824 
825  // creation and initialization accumulators
826  float *votes = new float[d1 * d2];
827  Vector3 *votesV = new Vector3[d1 * d2];
828  for (int i = 0; i < d1; i++) {
829  for (int j = 0; j < d2; j++) {
830  votes[i + j * d1] = 0;
831  votesV[i + j * d1] = Vector3(0, 0, 0);
832  }
833  }
834 
835  float max1 = 0, max2 = 0;
836  int i1 = 0, i2 = 0;
837  int j1 = 0, j2 = 0;
838  float votes_val;
839 
840  for (int n_try = 0; n_try < n_planes; n_try++) {
841  int p0 = triplets[n_try][0];
842  int p1 = triplets[n_try][1];
843  int p2 = triplets[n_try][2];
844 
845  Vector3 v1 = points[p1] - points[p0];
846  Vector3 v2 = points[p2] - points[p0];
847 
848  Vector3 Pn = CGAL::cross_product(v1, v2);
849  Pn = Pn / sqrt(Pn * Pn);
850  if (Pn * (points[p0] - CGAL::ORIGIN) > 0) {
851  Pn = -Pn;
852  }
853 
854  float phi;
855  phi = acos((float)Pn[2]);
856  float dphi = PI / n_phi;
857  int posp, post;
858  posp = int(floor((phi + dphi / 2.) * n_phi / PI));
859 
860  if (posp == 0 || posp == n_phi) {
861  post = 0;
862  } else {
863  float theta = acos((float)Pn[0] /
864  sqrt(float(Pn[0] * Pn[0] + Pn[1] * Pn[1])));
865  if (Pn[1] < 0) {
866  theta *= -1;
867  theta += 2 * PI;
868  }
869  float dtheta = PI / (n_phi * sin(posp * dphi));
870  post = (int)(floor((theta + dtheta / 2) / dtheta)) %
871  (2 * n_phi);
872  }
873 
874  post = std::max(0, std::min(2 * n_phi - 1, post));
875  posp = std::max(0, std::min(n_phi, posp));
876 
877  votes[post + posp * d1] += 1.;
878  votesV[post + posp * d1] = Pn + votesV[post + posp * d1];
879 
880  max1 = votes[i1 + j1 * d1] / (n_try + 1);
881  max2 = votes[i2 + j2 * d1] / (n_try + 1);
882  votes_val = votes[post + posp * d1] / (n_try + 1);
883 
884  if (votes_val > max1) {
885  max2 = max1;
886  i2 = i1;
887  j2 = j1;
888  max1 = votes_val;
889  i1 = post;
890  j1 = posp;
891  } else if (votes_val > max2 && post != i1 && posp != j1) {
892  max2 = votes_val;
893  i2 = post;
894  j2 = posp;
895  }
896 
897  if (max1 - conf_interv[n_try] > max2) {
898  break;
899  }
900  }
901 
902  nls[n] = votesV[i1 + j1 * d1] /
903  sqrt(votesV[i1 + j1 * d1] * votesV[i1 + j1 * d1]);
904  delete[] votes;
905  delete[] votesV;
906 
907  return max1;
908  }
909 
919  float normal_at_point(const int d1,
920  const int d2,
921  int points_size,
922  int n,
923  std::vector<Triplet> &triplets,
924  std::vector<float> &conf_interv) {
925  if (points_size < 3) {
926  nls[n] = Vector3(0, 0, 0);
927  return 0;
928  }
929 
930  // creation and initialization accumulators
931  float *votes = new float[d1 * d2];
932  Vector3 *votesV = new Vector3[d1 * d2];
933  for (int i = 0; i < d1; i++) {
934  for (int j = 0; j < d2; j++) {
935  votes[i + j * d1] = 0;
936  votesV[i + j * d1] = Vector3(0, 0, 0);
937  }
938  }
939 
940  float max1 = 0, max2 = 0;
941  int i1 = 0, i2 = 0;
942  int j1 = 0, j2 = 0;
943  float votes_val;
944 
945  for (int n_try = 0; n_try < n_planes; n_try++) {
946  Point3 p0 = triplets[n_try](0);
947  Point3 p1 = triplets[n_try](1);
948  Point3 p2 = triplets[n_try](2);
949  Vector3 v1 = p1 - p0;
950  Vector3 v2 = p2 - p0;
951 
952  Vector3 Pn = CGAL::cross_product(v1, v2);
953  Pn = Pn / sqrt(Pn * Pn);
954  if (Pn * (p0 - CGAL::ORIGIN) > 0) {
955  Pn = -Pn;
956  }
957 
958  float phi;
959  phi = acos((float)Pn[2]);
960  float dphi = PI / n_phi;
961  int posp, post;
962  posp = int(floor((phi + dphi / 2.) * n_phi / PI));
963 
964  if (posp == 0 || posp == n_phi) {
965  post = 0;
966  } else {
967  float theta = acos((float)Pn[0] /
968  sqrt(float(Pn[0] * Pn[0] + Pn[1] * Pn[1])));
969  if (Pn[1] < 0) {
970  theta *= -1;
971  theta += 2 * PI;
972  }
973  float dtheta = PI / (n_phi * sin(posp * dphi));
974  post = (int)(floor((theta + dtheta / 2) / dtheta)) %
975  (2 * n_phi);
976  }
977 
978  post = std::max(0, std::min(2 * n_phi - 1, post));
979  posp = std::max(0, std::min(n_phi, posp));
980 
981  votes[post + posp * d1] += 1.;
982  votesV[post + posp * d1] = Pn + votesV[post + posp * d1];
983 
984  max1 = votes[i1 + j1 * d1] / (n_try + 1);
985  max2 = votes[i2 + j2 * d1] / (n_try + 1);
986  votes_val = votes[post + posp * d1] / (n_try + 1);
987 
988  if (votes_val > max1) {
989  max2 = max1;
990  i2 = i1;
991  j2 = j1;
992  max1 = votes_val;
993  i1 = post;
994  j1 = posp;
995  } else if (votes_val > max2 && post != i1 && posp != j1) {
996  max2 = votes_val;
997  i2 = post;
998  j2 = posp;
999  }
1000 
1001  if (max1 - conf_interv[n_try] > max2) {
1002  break;
1003  }
1004  }
1005 
1006  nls[n] = votesV[i1 + j1 * d1] /
1007  sqrt(votesV[i1 + j1 * d1] * votesV[i1 + j1 * d1]);
1008 
1009  delete[] votes;
1010  delete[] votesV;
1011 
1012  return max1;
1013  }
1014 
1022  inline Vector3 normal_selection(int &rotations,
1023  std::vector<Vector3> &normals_vec,
1024  std::vector<float> &normals_conf) {
1025  std::vector<bool> normals_use(rotations);
1026 
1027  // init normals_use and reorient normals
1028  normals_use[0] = true;
1029  for (int i = 1; i < rotations; i++) {
1030  normals_use[i] = true;
1031  if (normals_vec[0] * normals_vec[i] < 0) {
1032  normals_vec[i] = -normals_vec[i];
1033  }
1034  }
1035 
1036  Vector3 normal_final;
1037  switch (selection_type) {
1038  case 1: // best
1039  {
1040  float confidence_final = 0;
1041  for (int i = 0; i < rotations; i++) {
1042  if (normals_conf[i] > confidence_final) {
1043  confidence_final = normals_conf[i];
1044  normal_final = normals_vec[i];
1045  }
1046  }
1047  } break;
1048  case 2: // mb
1049  {
1050  std::vector<std::pair<Vector3, float>> normals_fin;
1051  int number_to_test = rotations;
1052  while (number_to_test > 0) {
1053  // getting the max
1054  float max_conf = 0;
1055  int idx = 0;
1056  for (int i = 0; i < rotations; i++) {
1057  if (normals_use[i] && normals_conf[i] > max_conf) {
1058  max_conf = normals_conf[i];
1059  idx = i;
1060  }
1061  }
1062 
1063  normals_fin.push_back(std::pair<Vector3, float>(
1064  normals_vec[idx] * normals_conf[idx],
1065  normals_conf[idx]));
1066  normals_use[idx] = false;
1067  number_to_test--;
1068 
1069  for (int i = 0; i < rotations; i++) {
1070  if (normals_use[i] &&
1071  acos(normals_vec[idx] * normals_vec[i]) <
1072  tol_angle_rad) {
1073  normals_use[i] = false;
1074  number_to_test--;
1075  normals_fin.back().first =
1076  normals_fin.back().first +
1077  normals_vec[i] * normals_conf[i];
1078  normals_fin.back().second += normals_conf[i];
1079  }
1080  }
1081  }
1082 
1083  normal_final = normals_fin[0].first;
1084  float conf_fin = normals_fin[0].second;
1085  for (unsigned int i = 1; i < normals_fin.size(); i++) {
1086  if (normals_fin[i].second > conf_fin) {
1087  conf_fin = normals_fin[i].second;
1088  normal_final = normals_fin[i].first;
1089  }
1090  }
1091  } break;
1092  default: // mean
1093  {
1094  normal_final = normals_conf[0] * normals_vec[0];
1095  for (int i = 1; i < rotations; i++) {
1096  normal_final =
1097  normal_final + normals_conf[i] * normals_vec[i];
1098  }
1099  } break;
1100  }
1101 
1102  return normal_final / sqrt(normal_final * normal_final);
1103  }
1104 
1111  void points_knn(int neighbor_number) {
1112  // initialize the random number generator
1113  srand((unsigned int)time(NULL));
1114 
1115  nls.resize(pts.size());
1116 
1117  // dimensions of the accumulator
1118  const int d1 = 2 * n_phi;
1119  const int d2 = n_phi + 1;
1120 
1121  // creation of the rotation matrices and their inverses
1122  std::vector<Matrix3> rotMat;
1123  std::vector<Matrix3> rotMatInv;
1124  generate_rotation_matrix(rotMat, rotMatInv, n_rot * 200);
1125 
1126  int rotations;
1127  if (n_rot == 0) {
1128  rotations = 1;
1129  } else {
1130  rotations = n_rot;
1131  }
1132 
1133  // confidence intervals (2 intervals length)
1134  std::vector<float> conf_interv(n_planes);
1135  for (int i = 0; i < n_planes; i++) {
1136  conf_interv[i] = 2.f / std::sqrt(i + 1.f);
1137  }
1138 
1139  std::vector<int> vecInt;
1140  generate_random_int_vector(vecInt, 1000000);
1141 
1142  // creating the list of triplets
1143  std::vector<Vector3> trip;
1144  if (rotations <= 1) {
1145  list_of_triplets(trip, neighbor_number, n_planes, vecInt);
1146 
1147  } else {
1148  list_of_triplets(trip, neighbor_number, rotations * n_planes,
1149  vecInt);
1150  }
1151 
1152 #if defined(_OPENMP) && defined(USE_OPENMP_FOR_NORMEST)
1153 #pragma omp parallel
1154  {
1155  Tree tree(pts.begin(), pts.end());
1156 #pragma omp for schedule(guided)
1157 #else
1158  Tree tree(pts.begin(), pts.end());
1159 #endif
1160  for (int n = 0; n < (int)pts.size(); n++) {
1161  std::vector<Point3> points(neighbor_number);
1162  std::vector<Point3> points2(neighbor_number);
1163  int points_size = 0;
1164 
1165  // getting the list of neighbors
1166  Neighbor_search search(tree, pts[n], neighbor_number);
1167 
1168  for (Neighbor_search_iterator it = search.begin();
1169  it != search.end() && points_size < neighbor_number;
1170  ++it) {
1171  points[points_size] = it->first;
1172  points2[points_size] = it->first;
1173  points_size++;
1174  }
1175 
1176  std::vector<Vector3> normals_vec(rotations);
1177  std::vector<float> normals_conf(rotations);
1178 
1179  for (int i = 0; i < rotations; i++) {
1180  vecVec3Iterator first = trip.begin() + i * n_planes;
1181  vecVec3Iterator last = trip.begin() + (i + 1) * n_planes;
1182  std::vector<Vector3> triplets(first, last);
1183 
1184  for (unsigned int pt = 0; pt < points_size; pt++) {
1185  points[pt] = points2[pt].transform(
1186  rotMat[(n + i) % rotMat.size()]);
1187  }
1188  normals_conf[i] = normal_at_point(d1, d2, points, n,
1189  triplets, conf_interv);
1190 
1191  normals_vec[i] = nls[n].transform(
1192  rotMatInv[(n + i) % rotMat.size()]);
1193  }
1194  nls[n] = normal_selection(rotations, normals_vec, normals_conf);
1195  }
1196 #if defined(_OPENMP) && defined(USE_OPENMP_FOR_NORMEST)
1197  }
1198 #endif
1199  }
1200 
1207  void points_radius(float radius) {
1208  // initialize the random number generator
1209  srand((unsigned int)time(NULL));
1210 
1211  nls.resize(pts.size());
1212 
1213  // dimensions of the accumulator
1214  const int d1 = 2 * n_phi;
1215  const int d2 = n_phi + 1;
1216 
1217  // creation of the rotation matrices and their inverses
1218  std::vector<Matrix3> rotMat;
1219  std::vector<Matrix3> rotMatInv;
1220  generate_rotation_matrix(rotMat, rotMatInv, n_rot * 200);
1221 
1222  int rotations;
1223  if (n_rot == 0) {
1224  rotations = 1;
1225  } else {
1226  rotations = n_rot;
1227  }
1228 
1229  // confidence intervals (2 intervals length)
1230  std::vector<float> conf_interv(n_planes);
1231  for (int i = 0; i < n_planes; i++) {
1232  conf_interv[i] = 2.f / std::sqrt(i + 1.f);
1233  }
1234 
1235  std::vector<int> vecInt;
1236  generate_random_int_vector(vecInt, 1000000);
1237 
1238 #if defined(_OPENMP) && defined(USE_OPENMP_FOR_NORMEST)
1239 #pragma omp parallel
1240  {
1241  Tree tree(pts.begin(), pts.end());
1242 #pragma omp for schedule(guided)
1243 #else
1244  Tree tree(pts.begin(), pts.end());
1245 #endif
1246  for (int n = 0; n < (int)pts.size(); n++) {
1247  // std::cout << n<<std::endl;
1248  std::vector<Point3> points2;
1249  Fuzzy_sphere s_query(pts[n], radius);
1250 
1251  // getting the list of neighbors
1252  tree.search(std::back_inserter(points2), s_query);
1253 
1254  int points_size = (int)points2.size();
1255 
1256  if (points_size < lower_neighbor_bound_neighbors) {
1257  points2.clear();
1258 
1259  // getting the list of neighbors
1260  Neighbor_search search(tree, pts[n],
1261  lower_neighbor_bound_neighbors);
1262 
1263  for (Neighbor_search_iterator it = search.begin();
1264  it != search.end() &&
1265  points_size < lower_neighbor_bound_neighbors;
1266  ++it) {
1267  points2.push_back(it->first);
1268  points_size++;
1269  }
1270  points_size = points2.size();
1271  }
1272 
1273  unsigned long int max_number_comb = points_size;
1274  max_number_comb *= points_size - 1;
1275  max_number_comb *= points_size - 2;
1276  if (max_number_comb < n_planes) {
1277  continue;
1278  }
1279  // creating the list of triplets
1280  std::vector<Vector3> trip;
1281 
1282  if (rotations <= 1) {
1283  list_of_triplets(trip, points_size, n_planes, vecInt);
1284  } else {
1285  list_of_triplets(trip, points_size, rotations * n_planes,
1286  vecInt);
1287  }
1288 
1289  std::vector<Vector3> normals_vec(rotations);
1290  std::vector<float> normals_conf(rotations);
1291  std::vector<Point3> points(points_size);
1292 
1293  for (int i = 0; i < rotations; i++) {
1294  vecVec3Iterator first = trip.begin() + i * n_planes;
1295  vecVec3Iterator last = trip.begin() + (i + 1) * n_planes;
1296  std::vector<Vector3> triplets(first, last);
1297 
1298  for (unsigned int pt = 0; pt < points_size; pt++) {
1299  points[pt] = points2[pt].transform(
1300  rotMat[(n + i) % rotMat.size()]);
1301  }
1302  normals_conf[i] = normal_at_point(d1, d2, points, n,
1303  triplets, conf_interv);
1304 
1305  normals_vec[i] = nls[n].transform(
1306  rotMatInv[(n + i) % rotMat.size()]);
1307  }
1308 
1309  nls[n] = normal_selection(rotations, normals_vec, normals_conf);
1310  }
1311 #if defined(_OPENMP) && defined(USE_OPENMP_FOR_NORMEST)
1312  }
1313 #endif
1314  }
1315 
1322  void unif_knn(int neighbor_number) {
1323  // initialize the random number generator
1324  srand((unsigned int)time(NULL));
1325  // resizing the normal point cloud
1326  nls.resize(pts.size());
1327 
1328  Tree tree(pts.begin(), pts.end());
1329 
1330  // dimensions of the accumulator
1331  const int d1 = 2 * n_phi;
1332  const int d2 = n_phi + 1;
1333 
1334  // confidence intervals (2 intervals length)
1335  std::vector<float> conf_interv(n_planes);
1336  for (int i = 0; i < n_planes; i++) {
1337  conf_interv[i] = 2.f / std::sqrt(i + 1.f);
1338  }
1339 
1340  // creation of the rotation matrices and their inverses
1341  std::vector<Matrix3> rotMat;
1342  std::vector<Matrix3> rotMatInv;
1343  generate_rotation_matrix(rotMat, rotMatInv, n_rot * 200);
1344 
1345  int rotations;
1346  if (n_rot == 0) {
1347  rotations = 1;
1348  } else {
1349  rotations = n_rot;
1350  }
1351 
1352  // creation of vector of int and points
1353  std::vector<Point3> points_rand;
1354  std::vector<int> vecInt;
1355  generate_random_points_vector(points_rand, 1000000);
1356  generate_random_int_vector(vecInt, 1000000);
1357 
1358 #if defined(_OPENMP) && defined(USE_OPENMP_FOR_NORMEST)
1359 #pragma omp parallel for schedule(guided)
1360 #endif
1361  for (int n = 0; n < (int)pts.size(); n++) {
1362  std::vector<Point3> points(neighbor_number);
1363  int points_size = 0;
1364  float radius = 0;
1365 #if defined(_OPENMP) && defined(USE_OPENMP_FOR_NORMEST)
1366 #pragma omp critical
1367 #endif
1368  {
1369  // getting the list of neighbors
1370  Neighbor_search search(tree, pts[n], neighbor_number);
1371 
1372  for (Neighbor_search_iterator it = search.begin();
1373  it != search.end() && points_size < neighbor_number;
1374  ++it) {
1375  points[points_size] = it->first;
1376  points_size++;
1377  if (radius < it->second) {
1378  radius = it->second;
1379  }
1380  }
1381  }
1382 
1383  if (points_size != neighbor_number) {
1384  continue;
1385  }
1386  radius = sqrt(radius);
1387 
1388  float s_radius = radius / small_radius_factor;
1389 
1390  // point cloud of neighbors and kdtree creation
1391  Tree tree_neighbors(points.begin(), points.end());
1392 
1393  // creating the list of triplets
1394  std::vector<Triplet> trip;
1395  generate_list_of_triplets(trip, rotations * n_planes, radius,
1396  s_radius, tree_neighbors, pts[n],
1397  points_rand, vecInt);
1398 
1399  std::vector<Vector3> normals_vec(rotations);
1400  std::vector<float> normals_conf(rotations);
1401 
1402  for (int i = 0; i < rotations; i++) {
1403  vecTripIterator first = trip.begin() + i * n_planes;
1404  vecTripIterator last = trip.begin() + (i + 1) * n_planes;
1405  std::vector<Triplet> triplets(first, last);
1406 
1407  for (unsigned int tr = 0; tr < triplets.size(); tr++) {
1408  triplets[tr](0) = triplets[tr](0).transform(
1409  rotMat[(n + i) % rotMat.size()]);
1410  triplets[tr](1) = triplets[tr](1).transform(
1411  rotMat[(n + i) % rotMat.size()]);
1412  triplets[tr](2) = triplets[tr](2).transform(
1413  rotMat[(n + i) % rotMat.size()]);
1414  }
1415 
1416  normals_conf[i] = normal_at_point(d1, d2, points_size, n,
1417  triplets, conf_interv);
1418 
1419  normals_vec[i] =
1420  nls[n].transform(rotMatInv[(n + i) % rotMat.size()]);
1421  }
1422 
1423  nls[n] = normal_selection(rotations, normals_vec, normals_conf);
1424  }
1425  }
1426 
1433  void unif_radius(float radius) {
1434  // initialize the random number generator
1435  srand((unsigned int)time(NULL));
1436  // resizing the normal point cloud
1437  nls.resize(pts.size());
1438 
1439  Tree tree(pts.begin(), pts.end());
1440 
1441  // dimensions of the accumulator
1442  const int d1 = 2 * n_phi;
1443  const int d2 = n_phi + 1;
1444 
1445  // confidence intervals (2 intervals length)
1446  std::vector<float> conf_interv(n_planes);
1447  for (int i = 0; i < n_planes; i++) {
1448  conf_interv[i] = 2.f / std::sqrt(i + 1.f);
1449  }
1450 
1451  // creation of the rotation matrices and their inverses
1452  std::vector<Matrix3> rotMat;
1453  std::vector<Matrix3> rotMatInv;
1454  generate_rotation_matrix(rotMat, rotMatInv, n_rot * 200);
1455 
1456  int rotations;
1457  if (n_rot == 0) {
1458  rotations = 1;
1459  } else {
1460  rotations = n_rot;
1461  }
1462 
1463  // creation of vector of int and points
1464  std::vector<Point3> points_rand;
1465  std::vector<int> vecInt;
1466  generate_random_points_vector(points_rand, 1000000);
1467  generate_random_int_vector(vecInt, 1000000);
1468 
1469 #if defined(_OPENMP) && defined(USE_OPENMP_FOR_NORMEST)
1470 #pragma omp parallel for schedule(guided)
1471 #endif
1472  for (int n = 0; n < (int)pts.size(); n++) {
1473  std::vector<Point3> points;
1474  Fuzzy_sphere s_query(pts[n], radius);
1475 #if defined(_OPENMP) && defined(USE_OPENMP_FOR_NORMEST)
1476 #pragma omp critical
1477 #endif
1478  { tree.search(std::back_inserter(points), s_query); }
1479 
1480  int points_size = points.size();
1481  float radius2 = radius;
1482 
1483  if (points_size < lower_neighbor_bound_neighbors) {
1484  radius2 = 0;
1485  points.clear();
1486 #if defined(_OPENMP) && defined(USE_OPENMP_FOR_NORMEST)
1487 #pragma omp critical
1488 #endif
1489  {
1490  // getting the list of neighbors
1491  Neighbor_search search(tree, pts[n],
1492  lower_neighbor_bound_neighbors);
1493 
1494  for (Neighbor_search_iterator it = search.begin();
1495  it != search.end() &&
1496  points_size < lower_neighbor_bound_neighbors;
1497  ++it) {
1498  points.push_back(it->first);
1499  points_size++;
1500  if (radius2 < it->second) {
1501  radius2 = it->second;
1502  }
1503  }
1504  }
1505  points_size = points.size();
1506  }
1507 
1508  float s_radius = radius2 / small_radius_factor;
1509 
1510  // point cloud of neighbors and kdtree creation
1511  Tree tree_neighbors(points.begin(), points.end());
1512 
1513  // creating the list of triplets
1514  std::vector<Triplet> trip;
1515  generate_list_of_triplets(trip, rotations * n_planes, radius2,
1516  s_radius, tree_neighbors, pts[n],
1517  points_rand, vecInt);
1518 
1519  std::vector<Vector3> normals_vec(rotations);
1520  std::vector<float> normals_conf(rotations);
1521 
1522  for (int i = 0; i < rotations; i++) {
1523  vecTripIterator first = trip.begin() + i * n_planes;
1524  vecTripIterator last = trip.begin() + (i + 1) * n_planes;
1525  std::vector<Triplet> triplets(first, last);
1526 
1527  for (unsigned int tr = 0; tr < triplets.size(); tr++) {
1528  triplets[tr](0) = triplets[tr](0).transform(
1529  rotMat[(n + i) % rotMat.size()]);
1530  triplets[tr](1) = triplets[tr](1).transform(
1531  rotMat[(n + i) % rotMat.size()]);
1532  triplets[tr](2) = triplets[tr](2).transform(
1533  rotMat[(n + i) % rotMat.size()]);
1534  }
1535 
1536  normals_conf[i] = normal_at_point(d1, d2, points_size, n,
1537  triplets, conf_interv);
1538 
1539  normals_vec[i] =
1540  nls[n].transform(rotMatInv[(n + i) % rotMat.size()]);
1541  }
1542  nls[n] = normal_selection(rotations, normals_vec, normals_conf);
1543  }
1544  }
1545 
1552  void cubes_knn(int neighbor_number) {
1553  // resizing the normal point cloud
1554  nls.resize(pts.size());
1555 
1556  // initialize the random number generator
1557  srand((unsigned int)time(NULL));
1558 
1559  // dimensions of the accumulator
1560  const int d1 = 2 * n_phi;
1561  const int d2 = n_phi + 1;
1562 
1563  // confidence intervals (2 intervals length)
1564  std::vector<float> conf_interv(n_planes);
1565  for (int i = 0; i < n_planes; i++) {
1566  conf_interv[i] = 2.f / std::sqrt(i + 1.f);
1567  }
1568 
1569  // creation of the rotation matrices and their inverses
1570  std::vector<Matrix3> rotMat;
1571  std::vector<Matrix3> rotMatInv;
1572  generate_rotation_matrix(rotMat, rotMatInv, n_rot * 200);
1573 
1574  int rotations;
1575  if (n_rot == 0) {
1576  rotations = 1;
1577  } else {
1578  rotations = n_rot;
1579  }
1580 
1581  // creation of vector of cubes and int
1582  std::vector<int> cubes_idx;
1583  std::vector<int> vecInt;
1584  generate_cube_vector(cubes_idx, 1000000);
1585  generate_random_int_vector(vecInt, 1000000);
1586 
1587 #if defined(_OPENMP) && defined(USE_OPENMP_FOR_NORMEST)
1588 #pragma omp parallel
1589  {
1590  Tree tree(pts.begin(), pts.end());
1591 #pragma omp for schedule(guided)
1592 #else
1593  Tree tree(pts.begin(), pts.end());
1594 #endif
1595  for (int n = 0; n < (int)pts.size(); n++) {
1596  std::vector<Point3> points2(neighbor_number);
1597  std::vector<Point3> points(neighbor_number);
1598  int points_size = 0;
1599  float radius = 0;
1600 
1601  // getting the list of neighbors
1602  Neighbor_search search(tree, pts[n], neighbor_number);
1603 
1604  for (Neighbor_search_iterator it = search.begin();
1605  it != search.end() && points_size < neighbor_number;
1606  ++it) {
1607  points2[points_size] = it->first;
1608  points[points_size] = it->first;
1609  points_size++;
1610  if (radius < it->second) {
1611  radius = it->second;
1612  }
1613  }
1614 
1615  if (points_size != neighbor_number) {
1616  continue;
1617  }
1618  radius = sqrt(radius);
1619 
1620  std::vector<int> *cubes =
1621  new std::vector<int>[n_cubes * n_cubes * n_cubes];
1622  std::vector<Vector3> triplets;
1623  std::vector<Vector3> normals_vec(rotations);
1624  std::vector<float> normals_conf(rotations);
1625 
1626  for (int i = 0; i < rotations; i++) {
1627  assign_points_to_cubes(cubes, radius, pts[n], points,
1628  points2,
1629  rotMat[(n + i) % rotMat.size()]);
1630 
1631  generate_cubes_triplets(triplets, cubes, cubes_idx, vecInt);
1632 
1633  // cout << "1" << endl;
1634  normals_conf[i] = normal_at_point(d1, d2, points, n,
1635  triplets, conf_interv);
1636  // cout << "2" << endl;
1637 
1638  for (unsigned int pt = 0; pt < points_size; pt++) {
1639  points[pt] = points2[pt];
1640  }
1641  normals_vec[i] = nls[n].transform(
1642  rotMatInv[(n + i) % rotMat.size()]);
1643  }
1644 
1645  nls[n] = normal_selection(rotations, normals_vec, normals_conf);
1646 
1647  delete[] cubes;
1648  }
1649 #if defined(_OPENMP) && defined(USE_OPENMP_FOR_NORMEST)
1650  }
1651 #endif
1652  }
1653 
1660  void cubes_radius(float radius) {
1661  // resizing the normal point cloud
1662  nls.resize(pts.size());
1663 
1664  // initialize the random number generator
1665  srand((unsigned int)time(NULL));
1666 
1667  // dimensions of the accumulator
1668  const int d1 = 2 * n_phi;
1669  const int d2 = n_phi + 1;
1670 
1671  // confidence intervals (2 intervals length)
1672  std::vector<float> conf_interv(n_planes);
1673  for (int i = 0; i < n_planes; i++) {
1674  conf_interv[i] = 2.f / std::sqrt(i + 1.f);
1675  }
1676 
1677  // creation of the rotation matrices and their inverses
1678  std::vector<Matrix3> rotMat;
1679  std::vector<Matrix3> rotMatInv;
1680  generate_rotation_matrix(rotMat, rotMatInv, n_rot * 200);
1681 
1682  int rotations;
1683  if (n_rot == 0) {
1684  rotations = 1;
1685  } else {
1686  rotations = n_rot;
1687  }
1688 
1689  // creation of vector of cubes and int
1690  std::vector<int> cubes_idx;
1691  std::vector<int> vecInt;
1692  generate_cube_vector(cubes_idx, 1000000);
1693  generate_random_int_vector(vecInt, 1000000);
1694 
1695 #if defined(_OPENMP) && defined(USE_OPENMP_FOR_NORMEST)
1696 #pragma omp parallel
1697  {
1698  Tree tree(pts.begin(), pts.end());
1699 #pragma omp for schedule(guided)
1700 #else
1701  Tree tree(pts.begin(), pts.end());
1702 #endif
1703  for (int n = 0; n < (int)pts.size(); n++) {
1704  std::vector<Point3> points2;
1705  Fuzzy_sphere s_query(pts[n], radius);
1706  tree.search(std::back_inserter(points2), s_query);
1707 
1708  int points_size = points2.size();
1709  float radius2 = radius;
1710  if (points_size < lower_neighbor_bound_neighbors) {
1711  radius2 = 0;
1712  points2.clear();
1713  // getting the list of neighbors
1714  Neighbor_search search(tree, pts[n],
1715  lower_neighbor_bound_neighbors);
1716 
1717  for (Neighbor_search_iterator it = search.begin();
1718  it != search.end() &&
1719  points_size < lower_neighbor_bound_neighbors;
1720  ++it) {
1721  points2.push_back(it->first);
1722  points_size++;
1723  if (radius2 < it->second) {
1724  radius2 = it->second;
1725  }
1726  }
1727  points_size = points2.size();
1728  }
1729 
1730  std::vector<int> *cubes =
1731  new std::vector<int>[n_cubes * n_cubes * n_cubes];
1732  std::vector<Vector3> triplets;
1733  std::vector<Vector3> normals_vec(rotations);
1734  std::vector<float> normals_conf(rotations);
1735  std::vector<Point3> points(points_size);
1736 
1737  for (int i = 0; i < rotations; i++) {
1738  assign_points_to_cubes(cubes, radius2, pts[n], points,
1739  points2,
1740  rotMat[(n + i) % rotMat.size()]);
1741 
1742  generate_cubes_triplets(triplets, cubes, cubes_idx, vecInt);
1743 
1744  // cout << "1" << endl;
1745  normals_conf[i] = normal_at_point(d1, d2, points, n,
1746  triplets, conf_interv);
1747  // cout << "2" << endl;
1748 
1749  for (unsigned int pt = 0; pt < points_size; pt++) {
1750  points[pt] = points2[pt];
1751  }
1752  normals_vec[i] = nls[n].transform(
1753  rotMatInv[(n + i) % rotMat.size()]);
1754  }
1755 
1756  nls[n] = normal_selection(rotations, normals_vec, normals_conf);
1757 
1758  delete[] cubes;
1759  }
1760 #if defined(_OPENMP) && defined(USE_OPENMP_FOR_NORMEST)
1761  }
1762 #endif
1763  }
1764 };
1765 #endif
PI
#define PI
Definition: Factor.h:32
size
int size
Definition: FileIOFactory.cpp:130
points
int points
Definition: FileIOFactory.cpp:144
NULL
#define NULL
Definition: UVUnwrapping.cpp:40
CGAL_Normal_Estimator::My_Triplet
simple class for triplet of points
Definition: CGAL_normEst.h:71
CGAL_Normal_Estimator::My_Triplet::operator()
T & operator()(int i)
Definition: CGAL_normEst.h:82
CGAL_Normal_Estimator::My_Triplet::My_Triplet
My_Triplet(T a, T b, T c)
Definition: CGAL_normEst.h:77
CGAL_Normal_Estimator::My_Triplet::operator()
T operator()(int i) const
Definition: CGAL_normEst.h:83
CGAL_Normal_Estimator::My_Triplet::My_Triplet
My_Triplet()
Definition: CGAL_normEst.h:76
CGAL_Normal_Estimator
Class grouping different variant of the algorithm.
Definition: CGAL_normEst.h:63
CGAL_Normal_Estimator::estimate
void estimate(int method=POINTS, int neighborhood_type=KNN, float neighborhood_size=200)
Definition: CGAL_normEst.h:158
CGAL_Normal_Estimator::KNN
@ KNN
Definition: CGAL_normEst.h:113
CGAL_Normal_Estimator::CLUSTER
@ CLUSTER
Definition: CGAL_normEst.h:105
CGAL_Normal_Estimator::CUBES
@ CUBES
Definition: CGAL_normEst.h:111
CGAL_Normal_Estimator::MEAN
@ MEAN
Definition: CGAL_normEst.h:102
CGAL_Normal_Estimator::POINTS
@ POINTS
Definition: CGAL_normEst.h:107
CGAL_Normal_Estimator::RADIUS
@ RADIUS
Definition: CGAL_normEst.h:115
CGAL_Normal_Estimator::BEST
@ BEST
Definition: CGAL_normEst.h:103
CGAL_Normal_Estimator::UNIF
@ UNIF
Definition: CGAL_normEst.h:109
CGAL_Normal_Estimator::minimal_neighbor_number_for_range_search
int & minimal_neighbor_number_for_range_search()
Definition: CGAL_normEst.h:141
CGAL_Normal_Estimator::vecPt3Iterator
std::vector< Point3 >::iterator vecPt3Iterator
Definition: CGAL_normEst.h:96
CGAL_Normal_Estimator::normal_cloud
std::vector< Vector3 > normal_cloud() const
Definition: CGAL_normEst.h:156
CGAL_Normal_Estimator::CGAL_Normal_Estimator
CGAL_Normal_Estimator(std::vector< Point3 > &points, std::vector< Vector3 > &normals)
Constructor.
Definition: CGAL_normEst.h:124
CGAL_Normal_Estimator::Vector3
CGAL::Vector_3< Kernel > Vector3
Definition: CGAL_normEst.h:88
CGAL_Normal_Estimator::normal_selection_mode
int & normal_selection_mode()
Definition: CGAL_normEst.h:136
CGAL_Normal_Estimator::point_cloud
std::vector< Point3 > & point_cloud()
Definition: CGAL_normEst.h:153
CGAL_Normal_Estimator::minimal_neighbor_number_for_range_search
int minimal_neighbor_number_for_range_search() const
Definition: CGAL_normEst.h:144
CGAL_Normal_Estimator::Kernel
CGAL::Simple_cartesian< float > Kernel
Definition: CGAL_normEst.h:86
CGAL_Normal_Estimator::cluster_angle_rad
float cluster_angle_rad() const
Definition: CGAL_normEst.h:139
CGAL_Normal_Estimator::number_of_planes
int & number_of_planes()
Definition: CGAL_normEst.h:130
CGAL_Normal_Estimator::Fuzzy_sphere
CGAL::Fuzzy_sphere< TreeTraits > Fuzzy_sphere
Definition: CGAL_normEst.h:95
CGAL_Normal_Estimator::Triplet
My_Triplet< Point3 > Triplet
Definition: CGAL_normEst.h:91
CGAL_Normal_Estimator::number_of_cubes
int number_of_cubes() const
Definition: CGAL_normEst.h:151
CGAL_Normal_Estimator::point_cloud
std::vector< Point3 > point_cloud() const
Definition: CGAL_normEst.h:154
CGAL_Normal_Estimator::small_radius_fact
float small_radius_fact() const
Definition: CGAL_normEst.h:149
CGAL_Normal_Estimator::Neighbor_search
CGAL::Orthogonal_k_neighbor_search< TreeTraits > Neighbor_search
Definition: CGAL_normEst.h:93
CGAL_Normal_Estimator::accum_slices
int & accum_slices()
Definition: CGAL_normEst.h:132
CGAL_Normal_Estimator::vecTripIterator
std::vector< Triplet >::iterator vecTripIterator
Definition: CGAL_normEst.h:98
CGAL_Normal_Estimator::normal_cloud
std::vector< Vector3 > & normal_cloud()
Definition: CGAL_normEst.h:155
CGAL_Normal_Estimator::rotation_number
int & rotation_number()
Definition: CGAL_normEst.h:134
CGAL_Normal_Estimator::normal_selection_mode
int normal_selection_mode() const
Definition: CGAL_normEst.h:137
CGAL_Normal_Estimator::TreeTraits
CGAL::Search_traits_3< Kernel > TreeTraits
Definition: CGAL_normEst.h:90
CGAL_Normal_Estimator::rotation_number
int rotation_number() const
Definition: CGAL_normEst.h:135
CGAL_Normal_Estimator::Tree
CGAL::Kd_tree< TreeTraits > Tree
Definition: CGAL_normEst.h:94
CGAL_Normal_Estimator::number_of_cubes
int & number_of_cubes()
Definition: CGAL_normEst.h:150
CGAL_Normal_Estimator::Matrix3
CGAL::Aff_transformation_3< Kernel > Matrix3
Definition: CGAL_normEst.h:89
CGAL_Normal_Estimator::cluster_angle_rad
float & cluster_angle_rad()
Definition: CGAL_normEst.h:138
CGAL_Normal_Estimator::number_of_planes
int number_of_planes() const
Definition: CGAL_normEst.h:131
CGAL_Normal_Estimator::small_radius_fact
float & small_radius_fact()
Definition: CGAL_normEst.h:148
CGAL_Normal_Estimator::accum_slices
int accum_slices() const
Definition: CGAL_normEst.h:133
CGAL_Normal_Estimator::vecVec3Iterator
std::vector< Vector3 >::iterator vecVec3Iterator
Definition: CGAL_normEst.h:97
CGAL_Normal_Estimator::Neighbor_search_iterator
Neighbor_search::iterator Neighbor_search_iterator
Definition: CGAL_normEst.h:99
CGAL_Normal_Estimator::Point3
CGAL::Point_3< Kernel > Point3
Definition: CGAL_normEst.h:87
normals
double normals[3]
Definition: TriangleMeshIO.cpp:54
min
int min(int a, int b)
Definition: cutil_math.h:53
max
int max(int a, int b)
Definition: cutil_math.h:48
QtCompat::endl
QTextStream & endl(QTextStream &stream)
Definition: QtCompat.h:718
cloudViewer::utility::floor
MiniVec< float, N > floor(const MiniVec< float, N > &a)
Definition: MiniVec.h:75