ACloudViewer  3.9.4
A Modern Library for 3D Data Processing
PCL_normEst.h
Go to the documentation of this file.
1 /* License Information
2  *
3  * Copyright (C) 2013 Boulch Alexandre, Ecole Nationale des Ponts et Chaussees
4  * Permission is hereby granted, free of charge, to any person obtaining a copy
5  * of this software and associated documentation files (the "Software"), to deal
6  * in the Software without restriction, including without limitation the rights
7  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
8  * copies of the Software, and to permit persons to whom the Software is
9  * furnished to do so, subject to the following conditions:
10  *
11  * The above copyright notice and this permission notice shall be included in
12  * all copies or substantial portions of the Software.
13  *
14  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
15  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
16  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
17  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
18  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
19  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
20  * SOFTWARE.
21  *
22  * Note that this library relies on external libraries subject to their own
23  * license. To use this software, you are subject to the dependencies license,
24  * these licenses applies to the dependency ONLY and NOT this code. Please
25  * refer below to the web sites for license informations.
26  *
27  * OPENMP (http://openmp.org/)
28  * POINT CLOUD LIBRARIES (http://pointclouds.org/)
29  */
30 
31 #ifndef NORM_EST_PCL_OMP_H
32 #define NORM_EST_PCL_OMP_H
33 
34 #include <pcl/kdtree/kdtree_flann.h>
35 #include <pcl/point_cloud.h>
36 #include <pcl/point_types.h>
37 
38 #if defined(_OPENMP) && defined(USE_OPENMP_FOR_NORMEST)
39 #include <omp.h>
40 #endif
41 
42 #include <time.h>
43 
44 #include <iostream>
45 #include <map>
46 #include <set>
47 #include <vector>
48 
65 template <class Point, class Normal>
66 class PCL_Normal_Estimator {
67 public:
68  enum {
69  MEAN = 0,
70  BEST = 1,
72  CLUSTER = 2,
74  POINTS = 100, /*<POINTS value 100, used for method choice, triplets by
75  random selection in the neighborhood*/
76  UNIF = 101,
78  CUBES = 102,
80  KNN = 200,
82  RADIUS = 201
84  };
85 
91  PCL_Normal_Estimator(typename pcl::PointCloud<Point>::Ptr points,
92  typename pcl::PointCloud<Normal>::Ptr normals)
93  : pts(points), nls(normals) {
94  set_default_parameters();
95  }
96 
97  int &number_of_planes() { return n_planes; }
98  int number_of_planes() const { return n_planes; }
99  int &accum_slices() { return n_phi; }
100  int accum_slices() const { return n_phi; }
101  int &rotation_number() { return n_rot; }
102  int rotation_number() const { return n_rot; }
103  int &normal_selection_mode() { return selection_type; }
104  int normal_selection_mode() const { return selection_type; }
105  float &cluster_angle_rad() { return tol_angle_rad; }
106  float cluster_angle_rad() const { return tol_angle_rad; }
107 
108  int &minimal_neighbor_number_for_range_search() {
109  return lower_neighbor_bound_neighbors;
110  }
111  int minimal_neighbor_number_for_range_search() const {
112  return lower_neighbor_bound_neighbors;
113  }
114  float &small_radius_fact() { return small_radius_factor; }
115  float small_radius_fact() const { return small_radius_factor; }
116  int &number_of_cubes() { return n_cubes; }
117  int number_of_cubes() const { return n_cubes; }
118 
119  typename pcl::PointCloud<Point>::Ptr &point_cloud() { return pts; }
120  typename pcl::PointCloud<Point>::Ptr point_cloud() const { return pts; }
121  typename pcl::PointCloud<Normal>::Ptr &normal_cloud() { return nls; }
122  typename pcl::PointCloud<Normal>::Ptr normal_cloud() const { return nls; }
123 
124  void estimate(int method = POINTS,
125  int neighborhood_type = KNN,
126  float neighborhood_size = 200) {
127  std::cout << "Normal_Estimation ";
128  switch (method) {
129  case POINTS:
130  std::cout << "Points ";
131  switch (neighborhood_type) {
132  case KNN:
133  std::cout << "Knn=";
134  std::cout << (int)neighborhood_size << std::endl;
135  points_knn((int)neighborhood_size);
136  break;
137  case RADIUS:
138  std::cout << "radius=";
139  std::cout << neighborhood_size << std::endl;
140  points_radius(neighborhood_size);
141  break;
142  default:
143  std::cout << "Parameter Error : bad neighborhood type"
144  << std::endl;
145  break;
146  }
147  break;
148  case UNIF:
149  std::cout << "Unif ";
150  switch (neighborhood_type) {
151  case KNN:
152  std::cout << "Knn=";
153  std::cout << (int)neighborhood_size << std::endl;
154  unif_knn((int)neighborhood_size);
155  break;
156  case RADIUS:
157  std::cout << "radius=";
158  std::cout << neighborhood_size << std::endl;
159  unif_radius(neighborhood_size);
160  break;
161  default:
162  std::cout << "Parameter Error : bad neighborhood type"
163  << std::endl;
164  break;
165  }
166  break;
167  case CUBES:
168  std::cout << "Cubes ";
169  switch (neighborhood_type) {
170  case KNN:
171  std::cout << "Knn=";
172  std::cout << (int)neighborhood_size << std::endl;
173  cubes_knn((int)neighborhood_size);
174  break;
175  case RADIUS:
176  std::cout << "radius=";
177  std::cout << neighborhood_size << std::endl;
178  cubes_radius(neighborhood_size);
179  break;
180  default:
181  std::cout << "Parameter Error : bad neighborhood type"
182  << std::endl;
183  break;
184  }
185  break;
186  default:
187  std::cout << "Parameter Error : bad method" << std::endl;
188  break;
189  }
190  }
191 
192 private:
193  float PI;
194  int lower_neighbor_bound_neighbors;
198  int n_planes;
199  int n_phi;
200  int n_rot;
201  float tol_angle_rad;
202  float small_radius_factor;
204  int n_cubes;
206  int selection_type;
208  typename pcl::PointCloud<Point>::Ptr pts;
209  typename pcl::PointCloud<Normal>::Ptr nls;
214  void set_default_parameters() {
215  PI = 3.14159265f;
216  n_planes = 700;
217  n_rot = 5;
218  n_phi = 15;
219  tol_angle_rad = 0.79f;
220  small_radius_factor = 4;
221  n_cubes = 4;
222  lower_neighbor_bound_neighbors = 10;
223  selection_type = CLUSTER;
224  }
225 
232  inline void generate_rotation_matrix(
233  std::vector<Eigen::Matrix3f> &rotMat,
234  std::vector<Eigen::Matrix3f> &rotMatInv,
235  int rotations) {
236  rotMat.clear();
237  rotMatInv.clear();
238 
239  if (rotations == 0) {
240  Eigen::Matrix3f rMat;
241  rMat << 1, 0, 0, 0, 1, 0, 0, 0, 1;
242  rotMat.push_back(rMat);
243  rotMatInv.push_back(rMat);
244  } else {
245  for (int i = 0; i < rotations; i++) {
246  float theta = (rand() + 0.f) / RAND_MAX * 2 * 3.14159265f;
247  float phi = (rand() + 0.f) / RAND_MAX * 2 * 3.14159265f;
248  float psi = (rand() + 0.f) / RAND_MAX * 2 * 3.14159265f;
249  Eigen::Matrix3f Rt;
250  Eigen::Matrix3f Rph;
251  Eigen::Matrix3f Rps;
252  Rt << 1, 0, 0, 0, cos(theta), -sin(theta), 0, sin(theta),
253  cos(theta);
254  Rph << cos(phi), 0, sin(phi), 0, 1, 0, -sin(phi), 0, cos(phi);
255  Rps << cos(psi), -sin(psi), 0, sin(psi), cos(psi), 0, 0, 0, 1;
256  Eigen::Matrix3f Rtinv;
257  Eigen::Matrix3f Rphinv;
258  Eigen::Matrix3f Rpsinv;
259  Rtinv << 1, 0, 0, 0, cos(theta), sin(theta), 0, -sin(theta),
260  cos(theta);
261  Rphinv << cos(phi), 0, -sin(phi), 0, 1, 0, sin(phi), 0,
262  cos(phi);
263  Rpsinv << cos(psi), sin(psi), 0, -sin(psi), cos(psi), 0, 0, 0,
264  1;
265 
266  Eigen::Matrix3f rMat = Rt * Rph * Rps;
267  Eigen::Matrix3f rMatInv = Rpsinv * Rphinv * Rtinv;
268  rotMat.push_back(rMat);
269  rotMatInv.push_back(rMatInv);
270  }
271  }
272  }
273 
279  inline void generate_random_points_vector(
280  std::vector<Eigen::Vector3f> &points, int point_number) {
281  points.resize(point_number);
282  for (int i = 0; i < point_number; i++) {
283  float x, y, z;
284  do {
285  x = ((rand() + 0.f) / RAND_MAX) * 2 - 1;
286  y = ((rand() + 0.f) / RAND_MAX) * 2 - 1;
287  z = ((rand() + 0.f) / RAND_MAX) * 2 - 1;
288  } while (x * x + y * y + z * z > 1);
289 
290  points[i][0] = x;
291  points[i][1] = y;
292  points[i][2] = z;
293  }
294  }
295 
301  inline void generate_random_int_vector(std::vector<int> &vecInt,
302  int point_number) {
303  vecInt.resize(point_number);
304  for (int i = 0; i < point_number; i++) {
305  vecInt[i] = rand();
306  }
307  }
308 
318  inline void list_of_triplets(std::vector<Eigen::Vector3i> &triplets,
319  const int &number_of_points,
320  const unsigned int &plane_number,
321  std::vector<int> &vecInt) {
322  /*
323  * Here we take care of not using twice the same plane
324  * For that we use the combinatorial number system
325  */
326 
327  // computing the number of permutations
328  unsigned long long total_comb = number_of_points;
329  total_comb *= (number_of_points - 1);
330  total_comb *= (number_of_points - 2);
331  total_comb /= 6;
332 
333  std::vector<unsigned long long> tab_binome_3(number_of_points + 1);
334  std::vector<unsigned long long> tab_binome_2(number_of_points + 1);
335 
336  for (int i = 0; i < number_of_points + 1; i++) {
337  if (i > 3) {
338  tab_binome_3[i] = tab_binome_3[i - 1] * i / (i - 3);
339  } else if (i == 3) {
340  tab_binome_3[i] = 1;
341  } else {
342  tab_binome_3[i] = 0;
343  }
344  if (i > 2) {
345  tab_binome_2[i] = tab_binome_2[i - 1] * i / (i - 2);
346  } else if (i == 2) {
347  tab_binome_2[i] = 1;
348  } else {
349  tab_binome_2[i] = 0;
350  }
351  }
352 
353  std::vector<unsigned long long> comb_idx(plane_number);
354  for (int i = 0; i < plane_number; i++) {
355  comb_idx[i] = i % total_comb;
356  }
357  if (total_comb < RAND_MAX) {
358  std::map<int, int> table_next;
359  for (int i = 0; i < plane_number; i++) {
360  int temp_idx = vecInt[i % vecInt.size()] % total_comb;
361  if (temp_idx < plane_number) {
362  int temp = comb_idx[i];
363  comb_idx[i] = comb_idx[temp_idx];
364  comb_idx[temp_idx] = temp;
365  } else {
366  std::map<int, int>::iterator itmap =
367  table_next.find(temp_idx);
368  if (itmap != table_next.end()) {
369  int temp = comb_idx[i];
370  comb_idx[i] = itmap->second;
371  itmap->second = temp;
372  } else {
373  comb_idx[i] = temp_idx;
374  table_next.insert(std::pair<int, int>(temp_idx, i));
375  }
376  }
377  }
378  } else {
379  unsigned long long ref_RAND_MAX = RAND_MAX;
380  int size_test = 0;
381  while (ref_RAND_MAX < total_comb) {
382  size_test++;
383  ref_RAND_MAX *= RAND_MAX;
384  }
385  std::map<unsigned long long, unsigned long long> table_next;
386  int pos = 0;
387  for (int i = 0; i < plane_number; i++) {
388  // generating a random int
389  unsigned long long random_int = vecInt[pos % vecInt.size()];
390  pos++;
391  for (int j = 0; j < size_test; j++) {
392  random_int +=
393  ((unsigned long long)vecInt[pos % vecInt.size()]) *
394  RAND_MAX * (j + 1);
395  pos++;
396  }
397 
398  random_int = random_int % total_comb;
399  if (random_int < plane_number) {
400  int temp = comb_idx[i];
401  comb_idx[i] = comb_idx[random_int];
402  comb_idx[random_int] = temp;
403  } else {
404  std::map<unsigned long long, unsigned long long>::iterator
405  itmap = table_next.find(random_int);
406  if (itmap != table_next.end()) {
407  int temp = comb_idx[i];
408  comb_idx[i] = itmap->second;
409  itmap->second = temp;
410  } else {
411  comb_idx[i] = random_int;
412  table_next.insert(
413  std::pair<unsigned long long,
414  unsigned long long>(random_int, i));
415  }
416  }
417  }
418  }
419 
420  // getting the triplets from the numbers
421  triplets.resize(plane_number);
422  for (int pos = 0; pos < plane_number; pos++) {
423  int comb[3];
424  unsigned long long idx = comb_idx[pos];
425  int pos_temp = 0;
426  while (tab_binome_3[pos_temp] <= idx) {
427  pos_temp++;
428  }
429  pos_temp--;
430  comb[0] = pos_temp;
431  idx -= tab_binome_3[pos_temp];
432  if (idx == 0) {
433  comb[1] = 1;
434  comb[2] = 0;
435  triplets[pos] = Eigen::Vector3i(comb[0], comb[1], comb[2]);
436  continue;
437  }
438 
439  pos_temp = 0;
440  while (tab_binome_2[pos_temp] <= idx) {
441  pos_temp++;
442  }
443  pos_temp--;
444  comb[1] = pos_temp;
445  idx -= tab_binome_2[pos_temp];
446  if (idx == 0) {
447  comb[2] = 0;
448  triplets[pos] = Eigen::Vector3i(comb[0], comb[1], comb[2]);
449  continue;
450  }
451 
452  pos_temp = 0;
453  while (pos_temp != idx) {
454  pos_temp++;
455  }
456  comb[2] = pos_temp;
457  triplets[pos] = Eigen::Vector3i(comb[0], comb[1], comb[2]);
458  }
459  }
460 
467  inline void generate_cube_vector(std::vector<int> &cubes_idx, int n) {
468  // probabilities of picking a cube (ponderates by an approximation of
469  // its volume intersecting the sphere)
470  std::vector<float> probas(n_cubes * n_cubes * n_cubes);
471  float step = 2.f / n_cubes;
472  float xmin = -(n_cubes / 2.f) * step;
473  float ymin = -(n_cubes / 2.f) * step;
474  float zmin = -(n_cubes / 2.f) * step;
475  float sum_prob = 0;
476  for (int k = 0; k < n_cubes; k++) {
477  for (int j = 0; j < n_cubes; j++) {
478  for (int i = 0; i < n_cubes; i++) {
479  float prob = 0;
480 
481  float x1 = xmin + i * step;
482  float y1 = ymin + j * step;
483  float z1 = zmin + k * step;
484  float x2 = x1 + step;
485  float y2 = y1 + step;
486  float z2 = z1 + step;
487 
488  Eigen::Vector3f pt(x1, y1, z1);
489  if (pt.squaredNorm() <= 1) {
490  prob += 0.125;
491  }
492  pt = Eigen::Vector3f(x2, y1, z1);
493  if (pt.squaredNorm() <= 1) {
494  prob += 0.125;
495  }
496  pt = Eigen::Vector3f(x1, y2, z1);
497  if (pt.squaredNorm() <= 1) {
498  prob += 0.125;
499  }
500  pt = Eigen::Vector3f(x2, y2, z1);
501  if (pt.squaredNorm() <= 1) {
502  prob += 0.125;
503  }
504  pt = Eigen::Vector3f(x1, y1, z2);
505  if (pt.squaredNorm() <= 1) {
506  prob += 0.125;
507  }
508  pt = Eigen::Vector3f(x2, y1, z2);
509  if (pt.squaredNorm() <= 1) {
510  prob += 0.125;
511  }
512  pt = Eigen::Vector3f(x1, y2, z2);
513  if (pt.squaredNorm() <= 1) {
514  prob += 0.125;
515  }
516  pt = Eigen::Vector3f(x2, y2, z2);
517  if (pt.squaredNorm() <= 1) {
518  prob += 0.125;
519  }
520 
521  probas[i + j * n_cubes + k * n_cubes * n_cubes] = prob;
522  sum_prob += prob;
523  }
524  }
525  }
526  // cumulative proba sum
527  probas[0] /= sum_prob;
528  for (int i = 1; i < n_cubes * n_cubes * n_cubes; i++) {
529  probas[i] /= sum_prob;
530  probas[i] += probas[i - 1];
531  }
532 
533  // getting the cubes according to the probas
534  cubes_idx.resize(n);
535  for (int i = 0; i < n; i++) {
536  float pos = (rand() + 0.f) / RAND_MAX;
537  int begin = 0;
538  int end = n_cubes * n_cubes * n_cubes - 1;
539  int temp = (begin + end) / 2;
540  while (temp != begin) {
541  if (probas[temp] < pos) {
542  begin = temp;
543  } else {
544  end = temp;
545  }
546  temp = (begin + end) / 2;
547  }
548  cubes_idx[i] = end;
549  }
550  }
551 
562  inline void assign_points_to_cubes(std::vector<int> &pointIdxRadiusSearch,
563  std::vector<int> cubes[],
564  float radius,
565  Point &refPoint,
566  Point points[],
567  Eigen::Matrix3f &rotMat) {
568  float step = 2.f / n_cubes * radius;
569  for (unsigned int i = 0; i < pointIdxRadiusSearch.size(); i++) {
570  int idx = pointIdxRadiusSearch[i];
571  points[i] = pts->points[idx];
572  points[i].getVector3fMap() = rotMat * points[i].getVector3fMap();
573  Point refPoint2 = refPoint;
574  refPoint2.getVector3fMap() = rotMat * refPoint2.getVector3fMap();
575  int x = std::max(0,
576  std::min(int((points[i].x - refPoint2.x) / step +
577  (n_cubes / 2.f)),
578  n_cubes - 1));
579  int y = std::max(0,
580  std::min(int((points[i].y - refPoint2.y) / step +
581  (n_cubes / 2.f)),
582  n_cubes - 1));
583  int z = std::max(0,
584  std::min(int((points[i].z - refPoint2.z) / step +
585  (n_cubes / 2.f)),
586  n_cubes - 1));
587  cubes[x + y * n_cubes + z * n_cubes * n_cubes].push_back(i);
588  }
589  }
590 
598  inline void generate_cubes_triplets(std::vector<Eigen::Vector3i> &triplets,
599  std::vector<int> cubes[],
600  std::vector<int> &cubes_idx,
601  std::vector<int> &vecInt) {
602  triplets.resize(n_planes);
603 
604  std::vector<Eigen::Vector3i>::iterator ittrip = triplets.begin();
605  std::vector<int>::iterator itcube = cubes_idx.begin();
606  std::vector<int>::iterator itint = vecInt.begin();
607 
608  int idx = 0;
609  while (ittrip != triplets.end() && itcube != cubes_idx.end()) {
610  if (cubes[*itcube].size() != 0) {
611  int new_idx = cubes[*itcube][(*itint) % cubes[*itcube].size()];
612  bool is_valid = true;
613  for (int i = 0; i < idx; i++) {
614  if (new_idx == (*ittrip)[i]) {
615  is_valid = false;
616  }
617  }
618  if (is_valid) {
619  (*ittrip)[idx] = new_idx;
620  idx++;
621  }
622  if (idx == 3) {
623  idx = 0;
624  ittrip++;
625  }
626  }
627  itcube++;
628  itint++;
629  }
630 
631  while (ittrip != triplets.end()) {
632  int picked_points = 0;
633  while (picked_points < 3) {
634  int pos = rand() % (n_cubes * n_cubes * n_cubes);
635  if (cubes[pos].size() == 0) {
636  continue;
637  }
638  int idx = rand() % cubes[pos].size();
639  bool is_valid = true;
640  for (int i = 0; i < picked_points; i++) {
641  if (cubes[pos][idx] == (*ittrip)[i]) {
642  is_valid = false;
643  }
644  }
645  if (is_valid) {
646  (*ittrip)[picked_points] = cubes[pos][idx];
647  picked_points++;
648  }
649  }
650  ittrip++;
651  }
652  }
653 
661  inline void find_a_triplet(pcl::KdTreeFLANN<Point> &tree,
662  float radius1,
663  float radius2,
664  Eigen::Vector3i &triplet,
665  Point &pt) {
666  int picked_points = 0;
667 
668  while (picked_points < 3) {
669  // picking point in the unit balls
670  /*
671  *For fast results we pick them in the cube and discard bad points
672  */
673  float x, y, z;
674  do {
675  x = ((rand() + 0.f) / RAND_MAX) * 2 - 1;
676  y = ((rand() + 0.f) / RAND_MAX) * 2 - 1;
677  z = ((rand() + 0.f) / RAND_MAX) * 2 - 1;
678  } while (x * x + y * y + z * z > 1);
679 
680  x *= radius1;
681  y *= radius1;
682  z *= radius1;
683 
684  x += pt.x;
685  y += pt.y;
686  z += pt.z;
687 
688  Point pt;
689  pt.x = x;
690  pt.y = y;
691  pt.z = z;
692 
693  std::vector<int> pointIdxRadiusSearch;
694  std::vector<float> pointRadiusSquaredDistance;
695  tree.radiusSearch(pt, radius2, pointIdxRadiusSearch,
696  pointRadiusSquaredDistance);
697 
698  if (pointIdxRadiusSearch.size() != 0) {
699  int new_index =
700  pointIdxRadiusSearch[rand() %
701  pointIdxRadiusSearch.size()];
702  bool is_valid = true;
703  for (int i = 0; i < picked_points; i++) {
704  if (new_index == triplet[i]) {
705  is_valid = false;
706  }
707  }
708  if (is_valid) {
709  triplet[picked_points] = new_index;
710  picked_points++;
711  }
712  }
713  }
714  }
715 
727  inline void generate_list_of_triplets(
728  std::vector<Eigen::Vector3i> &triplets,
729  int plane_number,
730  float radius1,
731  float radius2,
732  pcl::KdTreeFLANN<Point> &tree,
733  Point &pt,
734  std::vector<Eigen::Vector3f> &points,
735  std::vector<int> &vecInt) {
736  triplets.resize(plane_number);
737 
738  std::vector<Eigen::Vector3i>::iterator ittrip = triplets.begin();
739  std::vector<Eigen::Vector3f>::iterator itpoints = points.begin();
740  std::vector<int>::iterator itint = vecInt.begin();
741 
742  int idx = 0;
743  while (ittrip != triplets.end() && itpoints != points.end()) {
744  // getting coordinates
745  float x, y, z;
746  x = (*itpoints)[0] * radius1 + pt.x;
747  y = (*itpoints)[1] * radius1 + pt.y;
748  z = (*itpoints)[2] * radius1 + pt.z;
749 
750  // searching neighbors of the point
751  Point refPoint;
752  refPoint.x = x;
753  refPoint.y = y;
754  refPoint.z = z;
755  std::vector<int> pointIdxRadiusSearch;
756  std::vector<float> pointRadiusSquaredDistance;
757  tree.radiusSearch(refPoint, radius2, pointIdxRadiusSearch,
758  pointRadiusSquaredDistance);
759 
760  // testing the validity
761  if (pointIdxRadiusSearch.size() != 0) {
762  int new_index =
763  pointIdxRadiusSearch[(*itint) %
764  pointIdxRadiusSearch.size()];
765  bool is_valid = true;
766  for (int i = 0; i < idx; i++) {
767  if (new_index == (*ittrip)[i]) {
768  is_valid = false;
769  }
770  }
771  if (is_valid) {
772  (*ittrip)[idx] = new_index;
773  idx++;
774  }
775  if (idx == 3) {
776  idx = 0;
777  ittrip++;
778  }
779  }
780 
781  itpoints++;
782  itint++;
783  }
784 
785  while (ittrip != triplets.end()) {
786  find_a_triplet(tree, radius1, radius2, *ittrip, pt);
787  ittrip++;
788  }
789  }
790 
801  float normal_at_point(const int d1,
802  const int d2,
803  Point points[],
804  int points_size,
805  int n,
806  std::vector<Eigen::Vector3i> &triplets,
807  std::vector<float> &conf_interv) {
808  if (points_size < 3) {
809  nls->points[n].normal_x = 0;
810  nls->points[n].normal_y = 0;
811  nls->points[n].normal_z = 0;
812  return 0;
813  }
814 
815  // creation and initialization accumulators
816  float *votes = new float[d1 * d2];
817  Eigen::Vector3f *votesV = new Eigen::Vector3f[d1 * d2];
818  for (int i = 0; i < d1; i++) {
819  for (int j = 0; j < d2; j++) {
820  votes[i + j * d1] = 0;
821  votesV[i + j * d1] = Eigen::Vector3f(0, 0, 0);
822  }
823  }
824 
825  float max1 = 0, max2 = 0;
826  int i1 = 0, i2 = 0;
827  int j1 = 0, j2 = 0;
828  float votes_val;
829 
830  // bool cont = true;
831  // int icomp = -1;
832  // int jcomp = -1;
833 
834  for (int n_try = 0; n_try < n_planes; n_try++) {
835  int p0 = triplets[n_try][0];
836  int p1 = triplets[n_try][1];
837  int p2 = triplets[n_try][2];
838 
839  Eigen::Vector3f v1 =
840  points[p1].getVector3fMap() - points[p0].getVector3fMap();
841  Eigen::Vector3f v2 =
842  points[p2].getVector3fMap() - points[p0].getVector3fMap();
843 
844  Eigen::Vector3f Pn = v1.cross(v2);
845  Pn.normalize();
846  if (Pn.dot(points[p0].getVector3fMap()) > 0) {
847  Pn = -Pn;
848  }
849 
850  float phi;
851  phi = acos((float)Pn[2]);
852  float dphi = PI / n_phi;
853  int posp, post;
854  posp = int(floor((phi + dphi / 2.) * n_phi / PI));
855 
856  if (posp == 0 || posp == n_phi) {
857  post = 0;
858  } else {
859  float theta = acos((float)Pn[0] /
860  sqrt(float(Pn[0] * Pn[0] + Pn[1] * Pn[1])));
861  if (Pn[1] < 0) {
862  theta *= -1;
863  theta += 2 * PI;
864  }
865  float dtheta = PI / (n_phi * sin(posp * dphi));
866  post = (int)(floor((theta + dtheta / 2) / dtheta)) %
867  (2 * n_phi);
868  }
869 
870  post = std::max(0, std::min(2 * n_phi - 1, post));
871  posp = std::max(0, std::min(n_phi, posp));
872 
873  votes[post + posp * d1] += 1.;
874  votesV[post + posp * d1] += Pn;
875 
876  max1 = votes[i1 + j1 * d1] / (n_try + 1);
877  max2 = votes[i2 + j2 * d1] / (n_try + 1);
878  votes_val = votes[post + posp * d1] / (n_try + 1);
879 
880  if (votes_val > max1) {
881  max2 = max1;
882  i2 = i1;
883  j2 = j1;
884  max1 = votes_val;
885  i1 = post;
886  j1 = posp;
887  } else if (votes_val > max2 && post != i1 && posp != j1) {
888  max2 = votes_val;
889  i2 = post;
890  j2 = posp;
891  }
892 
893  if (max1 - conf_interv[n_try] > max2) {
894  break;
895  }
896  }
897  votesV[i1 + j1 * d1].normalize();
898  nls->points[n].getNormalVector3fMap() = votesV[i1 + j1 * d1];
899 
900  delete[] votes;
901  delete[] votesV;
902 
903  return max1;
904  }
905 
913  inline Eigen::Vector3f normal_selection(
914  int &rotations,
915  std::vector<Eigen::Vector3f> &normals_vec,
916  std::vector<float> &normals_conf) {
917  std::vector<bool> normals_use(rotations);
918  // alignement of normals
919  normals_use[0] = true;
920  for (int i = 1; i < rotations; i++) {
921  normals_use[i] = true;
922  if (normals_vec[0].dot(normals_vec[i]) < 0) {
923  normals_vec[i] *= -1;
924  }
925  }
926 
927  Eigen::Vector3f normal_final;
928  switch (selection_type) {
929  case 1: // best
930  {
931  float confidence_final = 0;
932  for (int i = 0; i < rotations; i++) {
933  if (normals_conf[i] > confidence_final) {
934  confidence_final = normals_conf[i];
935  normal_final = normals_vec[i];
936  }
937  }
938  } break;
939  case 2: // mb
940  {
941  std::vector<std::pair<Eigen::Vector3f, float>> normals_fin;
942  int number_to_test = rotations;
943  while (number_to_test > 0) {
944  // getting the max
945  float max_conf = 0;
946  int idx = 0;
947  for (int i = 0; i < rotations; i++) {
948  if (normals_use[i] && normals_conf[i] > max_conf) {
949  max_conf = normals_conf[i];
950  idx = i;
951  }
952  }
953 
954  normals_fin.push_back(std::pair<Eigen::Vector3f, float>(
955  normals_vec[idx] * normals_conf[idx],
956  normals_conf[idx]));
957  normals_use[idx] = false;
958  number_to_test--;
959 
960  for (int i = 0; i < rotations; i++) {
961  if (normals_use[i] &&
962  acos(normals_vec[idx].dot(normals_vec[i])) <
963  tol_angle_rad) {
964  normals_use[i] = false;
965  number_to_test--;
966  normals_fin.back().first +=
967  normals_vec[i] * normals_conf[i];
968  normals_fin.back().second += normals_conf[i];
969  }
970  }
971  }
972 
973  normal_final = normals_fin[0].first;
974  float conf_fin = normals_fin[0].second;
975  for (unsigned int i = 1; i < normals_fin.size(); i++) {
976  if (normals_fin[i].second > conf_fin) {
977  conf_fin = normals_fin[i].second;
978  normal_final = normals_fin[i].first;
979  }
980  }
981  } break;
982  default: // mean
983  {
984  normal_final = normals_conf[0] * normals_vec[0];
985  for (int i = 1; i < rotations; i++) {
986  normal_final += normals_conf[i] * normals_vec[i];
987  }
988  } break;
989  }
990 
991  normal_final.normalize();
992  return normal_final;
993  }
994 
1001  void points_knn(int neighbor_number) {
1002  // resizing the normal point cloud
1003  nls->resize(pts->size());
1004 
1005  // initialize the random number generator
1006  srand((unsigned int)time(NULL));
1007 
1008  // kd tree creation
1009  pcl::KdTreeFLANN<Point> tree;
1010  tree.setInputCloud(pts);
1011 
1012  // dimensions of the accumulator
1013  const int d1 = 2 * n_phi;
1014  const int d2 = n_phi + 1;
1015 
1016  // creation of the rotation matrices and their inverses
1017  std::vector<Eigen::Matrix3f> rotMat;
1018  std::vector<Eigen::Matrix3f> rotMatInv;
1019  generate_rotation_matrix(rotMat, rotMatInv, n_rot * 200);
1020 
1021  int rotations;
1022  if (n_rot == 0) {
1023  rotations = 1;
1024  } else {
1025  rotations = n_rot;
1026  }
1027 
1028  // creating the list of triplets
1029  std::vector<Eigen::Vector3i> trip;
1030  std::vector<int> vecInt;
1031  generate_random_int_vector(vecInt, 1000000);
1032  if (rotations <= 1) {
1033  list_of_triplets(trip, neighbor_number, n_planes, vecInt);
1034 
1035  } else {
1036  list_of_triplets(trip, neighbor_number, rotations * n_planes,
1037  vecInt);
1038  }
1039 
1040  // confidence intervals (2 intervals length)
1041  std::vector<float> conf_interv(n_planes);
1042  for (int i = 0; i < n_planes; i++) {
1043  conf_interv[i] = 2.f / std::sqrt(i + 1.f);
1044  }
1045 
1046  // random permutation of the points (avoid thread difficult block)
1047  std::vector<int> permutation(pts->size());
1048  for (int i = 0; i < pts->size(); i++) {
1049  permutation[i] = i;
1050  }
1051  for (int i = 0; i < pts->size(); i++) {
1052  int j = rand() % pts->size();
1053  int temp = permutation[i];
1054  permutation[i] = permutation[j];
1055  permutation[j] = temp;
1056  }
1057 
1058 #if defined(_OPENMP) && defined(USE_OPENMP_FOR_NORMEST)
1059 #pragma omp parallel for schedule(guided)
1060 #endif
1061  for (int per = 0; per < (int)pts->size(); per++) {
1062  int n = permutation[per];
1063 
1064  // if the point is at the origin do not compute the normal
1065  // useful for laser point cloud
1066  if (pts->points[n].getVector3fMap() == Eigen::Vector3f(0, 0, 0)) {
1067  continue;
1068  }
1069 
1070  // getting the list of neighbors
1071  std::vector<int> pointIdxSearch;
1072  std::vector<float> pointSquaredDistance;
1073  tree.nearestKSearch(pts->points[n], neighbor_number, pointIdxSearch,
1074  pointSquaredDistance);
1075 
1076  unsigned int points_size = (unsigned int)pointIdxSearch.size();
1077  Point *points = new Point[points_size];
1078  for (unsigned int pt = 0; pt < pointIdxSearch.size(); pt++) {
1079  points[pt] = pts->points[pointIdxSearch[pt]];
1080  }
1081 
1082  std::vector<Eigen::Vector3f> normals_vec(rotations);
1083  std::vector<float> normals_conf(rotations);
1084 
1085  for (int i = 0; i < rotations; i++) {
1086  std::vector<Eigen::Vector3i>::const_iterator first =
1087  trip.begin() + i * n_planes;
1088  std::vector<Eigen::Vector3i>::const_iterator last =
1089  trip.begin() + (i + 1) * n_planes;
1090  std::vector<Eigen::Vector3i> triplets(first, last);
1091 
1092  for (unsigned int pt = 0; pt < points_size; pt++) {
1093  points[pt].getVector3fMap() =
1094  rotMat[(n + i) % rotMat.size()] *
1095  points[pt].getVector3fMap();
1096  }
1097  normals_conf[i] = normal_at_point(d1, d2, points, points_size,
1098  n, triplets, conf_interv);
1099 
1100  for (unsigned int pt = 0; pt < points_size; pt++) {
1101  points[pt] = pts->points[pointIdxSearch[pt]];
1102  }
1103  normals_vec[i] = rotMatInv[(n + i) % rotMat.size()] *
1104  nls->points[n].getNormalVector3fMap();
1105  }
1106 
1107  nls->points[n].getNormalVector3fMap() =
1108  normal_selection(rotations, normals_vec, normals_conf);
1109 
1110  delete[] points;
1111  }
1112  }
1113 
1120  void points_radius(float radius) {
1121  // resizing the normal point cloud
1122  if (nls != pts) {
1123  nls->resize(pts->size());
1124  }
1125 
1126  // initialize the random number generator
1127  srand((unsigned int)time(NULL));
1128 
1129  // kd tree creation
1130  pcl::KdTreeFLANN<Point> tree;
1131  tree.setInputCloud(pts);
1132 
1133  // dimensions of the accumulator
1134  const int d1 = 2 * n_phi;
1135  const int d2 = n_phi + 1;
1136 
1137  // creation of the rotation matrices and their inverses
1138  std::vector<Eigen::Matrix3f> rotMat;
1139  std::vector<Eigen::Matrix3f> rotMatInv;
1140  generate_rotation_matrix(rotMat, rotMatInv, n_rot * 200);
1141 
1142  int rotations;
1143  if (n_rot == 0) {
1144  rotations = 1;
1145  } else {
1146  rotations = n_rot;
1147  }
1148 
1149  // confidence intervals (2 intervals length)
1150  std::vector<float> conf_interv(n_planes);
1151  for (int i = 0; i < n_planes; i++) {
1152  conf_interv[i] = 2.f / std::sqrt(i + 1.f);
1153  }
1154 
1155  std::vector<int> vecInt;
1156  generate_random_int_vector(vecInt, 1000000);
1157 
1158  // random permutation of the points (avoid thread difficult block)
1159  std::vector<int> permutation(pts->size());
1160  for (int i = 0; i < pts->size(); i++) {
1161  permutation[i] = i;
1162  }
1163  for (int i = 0; i < pts->size(); i++) {
1164  int j = rand() % pts->size();
1165  int temp = permutation[i];
1166  permutation[i] = permutation[j];
1167  permutation[j] = temp;
1168  }
1169 
1170 #if defined(_OPENMP) && defined(USE_OPENMP_FOR_NORMEST)
1171 #pragma omp parallel for schedule(guided)
1172 #endif
1173  for (int per = 0; per < (int)pts->size(); per++) {
1174  int n = permutation[per];
1175  // if the point is at the origin do not compute the normal
1176  // useful for laser point cloud
1177  if (pts->points[n].getVector3fMap() == Eigen::Vector3f(0, 0, 0)) {
1178  continue;
1179  }
1180 
1181  // getting the list of neighbors
1182  std::vector<int> pointIdxSearch;
1183  std::vector<float> pointSquaredDistance;
1184  tree.radiusSearch(pts->points[n], radius, pointIdxSearch,
1185  pointSquaredDistance);
1186 
1187  if (pointIdxSearch.size() < lower_neighbor_bound_neighbors) {
1188  tree.nearestKSearch(pts->points[n],
1189  lower_neighbor_bound_neighbors,
1190  pointIdxSearch, pointSquaredDistance);
1191  }
1192 
1193  // creating the list of triplets
1194  std::vector<Eigen::Vector3i> trip;
1195  if (rotations <= 1) {
1196  list_of_triplets(trip, (int)pointIdxSearch.size(), n_planes,
1197  vecInt);
1198  } else {
1199  list_of_triplets(trip, (int)pointIdxSearch.size(),
1200  rotations * n_planes, vecInt);
1201  }
1202 
1203  unsigned int points_size = (unsigned int)pointIdxSearch.size();
1204  Point *points = new Point[points_size];
1205  for (unsigned int pt = 0; pt < pointIdxSearch.size(); pt++) {
1206  points[pt] = pts->points[pointIdxSearch[pt]];
1207  }
1208 
1209  std::vector<Eigen::Vector3f> normals_vec(rotations);
1210  std::vector<float> normals_conf(rotations);
1211 
1212  for (int i = 0; i < rotations; i++) {
1213  std::vector<Eigen::Vector3i>::const_iterator first =
1214  trip.begin() + i * n_planes;
1215  std::vector<Eigen::Vector3i>::const_iterator last =
1216  trip.begin() + (i + 1) * n_planes;
1217  std::vector<Eigen::Vector3i> triplets(first, last);
1218 
1219  for (unsigned int pt = 0; pt < points_size; pt++) {
1220  points[pt].getVector3fMap() =
1221  rotMat[(n + i) % rotMat.size()] *
1222  points[pt].getVector3fMap();
1223  }
1224  normals_conf[i] = normal_at_point(d1, d2, points, points_size,
1225  n, triplets, conf_interv);
1226 
1227  for (unsigned int pt = 0; pt < points_size; pt++) {
1228  points[pt] = pts->points[pointIdxSearch[pt]];
1229  }
1230  normals_vec[i] = rotMatInv[(n + i) % rotMat.size()] *
1231  nls->points[n].getNormalVector3fMap();
1232  }
1233  nls->points[n].getNormalVector3fMap() =
1234  normal_selection(rotations, normals_vec, normals_conf);
1235 
1236  delete[] points;
1237  }
1238  }
1239 
1246  void unif_knn(int neighbor_number) {
1247  // resizing the normal point cloud
1248  if (nls != pts) {
1249  nls->resize(pts->size());
1250  }
1251 
1252  // initialize the random number generator
1253  srand((unsigned int)time(NULL));
1254 
1255  // kd tree creation
1256  pcl::KdTreeFLANN<Point> tree;
1257  tree.setInputCloud(pts);
1258 
1259  // dimensions of the accumulator
1260  const int d1 = 2 * n_phi;
1261  const int d2 = n_phi + 1;
1262 
1263  // confidence intervals (2 intervals length)
1264  std::vector<float> conf_interv(n_planes);
1265  for (int i = 0; i < n_planes; i++) {
1266  conf_interv[i] = 2.f / std::sqrt(i + 1.f);
1267  }
1268 
1269  // creation of the rotation matrices and their inverses
1270  std::vector<Eigen::Matrix3f> rotMat;
1271  std::vector<Eigen::Matrix3f> rotMatInv;
1272  generate_rotation_matrix(rotMat, rotMatInv, n_rot * 200);
1273 
1274  int rotations;
1275  if (n_rot == 0) {
1276  rotations = 1;
1277  } else {
1278  rotations = n_rot;
1279  }
1280 
1281  // creation of vector of int and points
1282  std::vector<Eigen::Vector3f> points_rand;
1283  std::vector<int> vecInt;
1284  generate_random_int_vector(vecInt, 1000000);
1285  generate_random_points_vector(points_rand, 1000000);
1286 
1287  // random permutation of the points (avoid thread difficult block)
1288  std::vector<int> permutation(pts->size());
1289  for (int i = 0; i < pts->size(); i++) {
1290  permutation[i] = i;
1291  }
1292  for (int i = 0; i < pts->size(); i++) {
1293  int j = rand() % pts->size();
1294  int temp = permutation[i];
1295  permutation[i] = permutation[j];
1296  permutation[j] = temp;
1297  }
1298 
1299 #if defined(_OPENMP) && defined(USE_OPENMP_FOR_NORMEST)
1300 #pragma omp parallel for schedule(guided)
1301 #endif
1302  for (int per = 0; per < (int)pts->size(); per++) {
1303  int n = permutation[per];
1304  // cout << n << endl;
1305  // if the point is at the origin do not compute the normal
1306  // useful for laser point cloud
1307  if (pts->points[n].getVector3fMap() == Eigen::Vector3f(0, 0, 0)) {
1308  continue;
1309  }
1310 
1311  // getting the list of neighbors
1312  std::vector<int> pointIdxRadiusSearch;
1313  std::vector<float> pointRadiusSquaredDistance;
1314  tree.nearestKSearch(pts->points[n], neighbor_number,
1315  pointIdxRadiusSearch,
1316  pointRadiusSquaredDistance);
1317 
1318  float radius = 0;
1319  for (unsigned int i = 0; i < pointRadiusSquaredDistance.size();
1320  i++) {
1321  if (pointRadiusSquaredDistance[i] > radius) {
1322  radius = pointRadiusSquaredDistance[i];
1323  }
1324  }
1325  radius = sqrt(radius);
1326 
1327  float s_radius = radius / small_radius_factor;
1328 
1329  // point cloud of neighbors and kdtree creation
1330  typename pcl::PointCloud<Point>::Ptr cloud_neighbors(
1331  new pcl::PointCloud<Point>);
1332  cloud_neighbors->resize(pointIdxRadiusSearch.size());
1333  for (unsigned int i = 0; i < pointIdxRadiusSearch.size(); i++) {
1334  cloud_neighbors->points[i] =
1335  pts->points[pointIdxRadiusSearch[i]];
1336  }
1337  pcl::KdTreeFLANN<Point> tree_neighbors;
1338  tree_neighbors.setInputCloud(cloud_neighbors);
1339 
1340  Point *points = new Point[neighbor_number];
1341 
1342  // creating the list of triplets
1343  std::vector<Eigen::Vector3i> trip;
1344  generate_list_of_triplets(trip, rotations * n_planes, radius,
1345  s_radius, tree_neighbors, pts->points[n],
1346  points_rand, vecInt);
1347 
1348  std::vector<Eigen::Vector3f> normals_vec(rotations);
1349  std::vector<float> normals_conf(rotations);
1350 
1351  for (int i = 0; i < rotations; i++) {
1352  std::vector<Eigen::Vector3i>::const_iterator first =
1353  trip.begin() + i * n_planes;
1354  std::vector<Eigen::Vector3i>::const_iterator last =
1355  trip.begin() + (i + 1) * n_planes;
1356  std::vector<Eigen::Vector3i> triplets(first, last);
1357 
1358  for (unsigned int pt = 0; pt < neighbor_number; pt++) {
1359  points[pt] = pts->points[pointIdxRadiusSearch[pt]];
1360  points[pt].getVector3fMap() =
1361  rotMat[(n + i) % rotMat.size()] *
1362  points[pt].getVector3fMap();
1363  }
1364 
1365  normals_conf[i] =
1366  normal_at_point(d1, d2, points, neighbor_number, n,
1367  triplets, conf_interv);
1368  normals_vec[i] = rotMatInv[(n + i) % rotMat.size()] *
1369  nls->points[n].getNormalVector3fMap();
1370  }
1371 
1372  nls->points[n].getNormalVector3fMap() =
1373  normal_selection(rotations, normals_vec, normals_conf);
1374 
1375  delete[] points;
1376  }
1377  }
1378 
1385  void unif_radius(float radius) {
1386  // resizing the normal point cloud
1387  if (nls != pts) {
1388  nls->resize(pts->size());
1389  }
1390 
1391  // initialize the random number generator
1392  srand((unsigned int)time(NULL));
1393 
1394  // kd tree creation
1395  pcl::KdTreeFLANN<Point> tree;
1396  tree.setInputCloud(pts);
1397 
1398  // dimensions of the accumulator
1399  const int d1 = 2 * n_phi;
1400  const int d2 = n_phi + 1;
1401 
1402  // confidence intervals (2 intervals length)
1403  std::vector<float> conf_interv(n_planes);
1404  for (int i = 0; i < n_planes; i++) {
1405  conf_interv[i] = 2.f / std::sqrt(i + 1.f);
1406  }
1407 
1408  // creation of the rotation matrices and their inverses
1409  std::vector<Eigen::Matrix3f> rotMat;
1410  std::vector<Eigen::Matrix3f> rotMatInv;
1411  generate_rotation_matrix(rotMat, rotMatInv, n_rot * 200);
1412 
1413  int rotations;
1414  if (n_rot == 0) {
1415  rotations = 1;
1416  } else {
1417  rotations = n_rot;
1418  }
1419 
1420  // creation of vector of int and points
1421  std::vector<Eigen::Vector3f> points_rand;
1422  std::vector<int> vecInt;
1423  generate_random_points_vector(points_rand, 1000000);
1424  generate_random_int_vector(vecInt, 1000000);
1425 
1426  // random permutation of the points (avoid thread difficult block)
1427  std::vector<int> permutation(pts->size());
1428  for (int i = 0; i < pts->size(); i++) {
1429  permutation[i] = i;
1430  }
1431  for (int i = 0; i < pts->size(); i++) {
1432  int j = rand() % pts->size();
1433  int temp = permutation[i];
1434  permutation[i] = permutation[j];
1435  permutation[j] = temp;
1436  }
1437 
1438 #if defined(_OPENMP) && defined(USE_OPENMP_FOR_NORMEST)
1439 #pragma omp parallel for schedule(guided)
1440 #endif
1441  for (int per = 0; per < (int)pts->size(); per++) {
1442  int n = permutation[per];
1443  // cout << n << endl;
1444  // if the point is at the origin do not compute the normal
1445  // useful for laser point cloud
1446  if (pts->points[n].getVector3fMap() == Eigen::Vector3f(0, 0, 0)) {
1447  continue;
1448  }
1449 
1450  // getting the list of neighbors
1451  std::vector<int> pointIdxRadiusSearch;
1452  std::vector<float> pointRadiusSquaredDistance;
1453  tree.radiusSearch(pts->points[n], radius, pointIdxRadiusSearch,
1454  pointRadiusSquaredDistance);
1455 
1456  float radius2 = radius;
1457  if (pointIdxRadiusSearch.size() < lower_neighbor_bound_neighbors) {
1458  radius2 = 0;
1459  tree.nearestKSearch(
1460  pts->points[n], lower_neighbor_bound_neighbors,
1461  pointIdxRadiusSearch, pointRadiusSquaredDistance);
1462  for (unsigned int i = 0; i < pointRadiusSquaredDistance.size();
1463  i++) {
1464  if (pointRadiusSquaredDistance[i] > radius2) {
1465  radius2 = pointRadiusSquaredDistance[i];
1466  }
1467  }
1468  }
1469  float s_radius = radius2 / small_radius_factor;
1470 
1471  // point cloud of neighbors and kdtree creation
1472  typename pcl::PointCloud<Point>::Ptr cloud_neighbors(
1473  new pcl::PointCloud<Point>);
1474  cloud_neighbors->resize(pointIdxRadiusSearch.size());
1475  for (unsigned int i = 0; i < pointIdxRadiusSearch.size(); i++) {
1476  cloud_neighbors->points[i] =
1477  pts->points[pointIdxRadiusSearch[i]];
1478  }
1479  pcl::KdTreeFLANN<Point> tree_neighbors;
1480  tree_neighbors.setInputCloud(cloud_neighbors);
1481 
1482  unsigned int points_size =
1483  (unsigned int)pointIdxRadiusSearch.size();
1484  Point *points = new Point[points_size];
1485 
1486  // creating the list of triplets
1487  std::vector<Eigen::Vector3i> trip;
1488  generate_list_of_triplets(trip, rotations * n_planes, radius2,
1489  s_radius, tree_neighbors, pts->points[n],
1490  points_rand, vecInt);
1491 
1492  std::vector<Eigen::Vector3f> normals_vec(rotations);
1493  std::vector<float> normals_conf(rotations);
1494 
1495  for (int i = 0; i < rotations; i++) {
1496  std::vector<Eigen::Vector3i>::const_iterator first =
1497  trip.begin() + i * n_planes;
1498  std::vector<Eigen::Vector3i>::const_iterator last =
1499  trip.begin() + (i + 1) * n_planes;
1500  std::vector<Eigen::Vector3i> triplets(first, last);
1501 
1502  for (unsigned int pt = 0; pt < points_size; pt++) {
1503  points[pt] = pts->points[pointIdxRadiusSearch[pt]];
1504  points[pt].getVector3fMap() =
1505  rotMat[(n + i) % rotMat.size()] *
1506  points[pt].getVector3fMap();
1507  }
1508 
1509  normals_conf[i] = normal_at_point(d1, d2, points, points_size,
1510  n, triplets, conf_interv);
1511 
1512  nls->points[n].getNormalVector3fMap() =
1513  rotMatInv[(n + i) % rotMat.size()] *
1514  nls->points[n].getNormalVector3fMap();
1515 
1516  normals_vec[i] = nls->points[n].getNormalVector3fMap();
1517  }
1518 
1519  nls->points[n].getNormalVector3fMap() =
1520  normal_selection(rotations, normals_vec, normals_conf);
1521 
1522  delete[] points;
1523  }
1524  }
1525 
1532  void cubes_knn(int neighbor_number) {
1533  // resizing the normal point cloud
1534  if (nls != pts) {
1535  nls->resize(pts->size());
1536  }
1537 
1538  // initialize the random number generator
1539  srand((unsigned int)time(NULL));
1540 
1541  // kd tree creation
1542  pcl::KdTreeFLANN<Point> tree;
1543  tree.setInputCloud(pts);
1544 
1545  // dimensions of the accumulator
1546  const int d1 = 2 * n_phi;
1547  const int d2 = n_phi + 1;
1548 
1549  // confidence intervals (2 intervals length)
1550  std::vector<float> conf_interv(n_planes);
1551  for (int i = 0; i < n_planes; i++) {
1552  conf_interv[i] = 2.f / std::sqrt(i + 1.f);
1553  }
1554 
1555  // creation of the rotation matrices and their inverses
1556  std::vector<Eigen::Matrix3f> rotMat;
1557  std::vector<Eigen::Matrix3f> rotMatInv;
1558  generate_rotation_matrix(rotMat, rotMatInv, n_rot * 200);
1559 
1560  int rotations;
1561  if (n_rot == 0) {
1562  rotations = 1;
1563  } else {
1564  rotations = n_rot;
1565  }
1566 
1567  // creation of vector of cubes and int
1568  std::vector<int> cubes_idx;
1569  std::vector<int> vecInt;
1570  generate_cube_vector(cubes_idx, 1000000);
1571  generate_random_int_vector(vecInt, 1000000);
1572 
1573  // random permutation of the points (avoid thread difficult block)
1574  std::vector<int> permutation(pts->size());
1575  for (int i = 0; i < pts->size(); i++) {
1576  permutation[i] = i;
1577  }
1578  for (int i = 0; i < pts->size(); i++) {
1579  int j = rand() % pts->size();
1580  int temp = permutation[i];
1581  permutation[i] = permutation[j];
1582  permutation[j] = temp;
1583  }
1584 
1585 #if defined(_OPENMP) && defined(USE_OPENMP_FOR_NORMEST)
1586 #pragma omp parallel for schedule(guided)
1587 #endif
1588  for (int per = 0; per < (int)pts->size(); per++) {
1589  int n = permutation[per];
1590  // cout << n << endl;
1591  // if the point is at the origin do not compute the normal
1592  // useful for laser point cloud
1593  if (pts->points[n].getVector3fMap() == Eigen::Vector3f(0, 0, 0)) {
1594  continue;
1595  }
1596 
1597  // getting the list of neighbors
1598  std::vector<int> pointIdxRadiusSearch;
1599  std::vector<float> pointRadiusSquaredDistance;
1600  tree.nearestKSearch(pts->points[n], neighbor_number,
1601  pointIdxRadiusSearch,
1602  pointRadiusSquaredDistance);
1603 
1604  float radius = 0;
1605  for (unsigned int i = 0; i < pointRadiusSquaredDistance.size();
1606  i++) {
1607  if (pointRadiusSquaredDistance[i] > radius) {
1608  radius = pointRadiusSquaredDistance[i];
1609  }
1610  }
1611  radius = sqrt(radius);
1612 
1613  std::vector<int> *cubes =
1614  new std::vector<int>[n_cubes * n_cubes * n_cubes];
1615  Point *points = new Point[neighbor_number];
1616  std::vector<Eigen::Vector3i> triplets;
1617 
1618  std::vector<Eigen::Vector3f> normals_vec(rotations);
1619  std::vector<float> normals_conf(rotations);
1620 
1621  for (int i = 0; i < rotations; i++) {
1622  assign_points_to_cubes(pointIdxRadiusSearch, cubes, radius,
1623  pts->points[n], points,
1624  rotMat[(n + i) % rotMat.size()]);
1625 
1626  generate_cubes_triplets(triplets, cubes, cubes_idx, vecInt);
1627 
1628  // cout << "1" << endl;
1629  normals_conf[i] =
1630  normal_at_point(d1, d2, points, neighbor_number, n,
1631  triplets, conf_interv);
1632  // cout << "2" << endl;
1633 
1634  for (unsigned int pt = 0; pt < neighbor_number; pt++) {
1635  points[pt] = pts->points[pointIdxRadiusSearch[pt]];
1636  }
1637  normals_vec[i] = rotMatInv[(n + i) % rotMat.size()] *
1638  nls->points[n].getNormalVector3fMap();
1639  }
1640 
1641  nls->points[n].getNormalVector3fMap() =
1642  normal_selection(rotations, normals_vec, normals_conf);
1643 
1644  delete[] points;
1645  delete[] cubes;
1646  }
1647  }
1648 
1655  void cubes_radius(float radius) {
1656  // resizing the normal point cloud
1657  if (nls != pts) {
1658  nls->resize(pts->size());
1659  }
1660 
1661  // initialize the random number generator
1662  srand((unsigned int)time(NULL));
1663 
1664  // kd tree creation
1665  pcl::KdTreeFLANN<Point> tree;
1666  tree.setInputCloud(pts);
1667 
1668  // dimensions of the accumulator
1669  const int d1 = 2 * n_phi;
1670  const int d2 = n_phi + 1;
1671 
1672  // confidence intervals (2 intervals length)
1673  std::vector<float> conf_interv(n_planes);
1674  for (int i = 0; i < n_planes; i++) {
1675  conf_interv[i] = 2.f / std::sqrt(i + 1.f);
1676  }
1677 
1678  // creation of the rotation matrices and their inverses
1679  std::vector<Eigen::Matrix3f> rotMat;
1680  std::vector<Eigen::Matrix3f> rotMatInv;
1681  generate_rotation_matrix(rotMat, rotMatInv, n_rot * 200);
1682 
1683  int rotations;
1684  if (n_rot == 0) {
1685  rotations = 1;
1686  } else {
1687  rotations = n_rot;
1688  }
1689 
1690  // creation of vector of cubes and int
1691  std::vector<int> cubes_idx;
1692  std::vector<int> vecInt;
1693  generate_cube_vector(cubes_idx, 1000000);
1694  generate_random_int_vector(vecInt, 1000000);
1695 
1696  // random permutation of the points (avoid thread difficult block)
1697  std::vector<int> permutation(pts->size());
1698  for (int i = 0; i < pts->size(); i++) {
1699  permutation[i] = i;
1700  }
1701  for (int i = 0; i < pts->size(); i++) {
1702  int j = rand() % pts->size();
1703  int temp = permutation[i];
1704  permutation[i] = permutation[j];
1705  permutation[j] = temp;
1706  }
1707 
1708 #if defined(_OPENMP) && defined(USE_OPENMP_FOR_NORMEST)
1709 #pragma omp parallel for schedule(guided)
1710 #endif
1711  for (int per = 0; per < (int)pts->size(); per++) {
1712  int n = permutation[per];
1713  // std::cout << n << std::endl;
1714  // if the point is at the origin do not compute the normal
1715  // useful for laser point cloud
1716  if (pts->points[n].getVector3fMap() == Eigen::Vector3f(0, 0, 0)) {
1717  continue;
1718  }
1719 
1720  float radius2 = radius;
1721 
1722  // getting the list of neighbors
1723  std::vector<int> pointIdxRadiusSearch;
1724  std::vector<float> pointRadiusSquaredDistance;
1725  tree.radiusSearch(pts->points[n], radius, pointIdxRadiusSearch,
1726  pointRadiusSquaredDistance);
1727 
1728  if (pointIdxRadiusSearch.size() < lower_neighbor_bound_neighbors) {
1729  radius2 = 0;
1730  tree.nearestKSearch(
1731  pts->points[n], lower_neighbor_bound_neighbors,
1732  pointIdxRadiusSearch, pointRadiusSquaredDistance);
1733  for (unsigned int i = 0; i < pointRadiusSquaredDistance.size();
1734  i++) {
1735  if (pointRadiusSquaredDistance[i] > radius2) {
1736  radius2 = pointRadiusSquaredDistance[i];
1737  }
1738  }
1739  }
1740 
1741  std::vector<int> *cubes =
1742  new std::vector<int>[n_cubes * n_cubes * n_cubes];
1743  unsigned int points_size =
1744  (unsigned int)pointIdxRadiusSearch.size();
1745  Point *points = new Point[points_size];
1746 
1747  std::vector<Eigen::Vector3i> triplets;
1748 
1749  std::vector<Eigen::Vector3f> normals_vec(rotations);
1750  std::vector<float> normals_conf(rotations);
1751 
1752  for (int i = 0; i < rotations; i++) {
1753  assign_points_to_cubes(pointIdxRadiusSearch, cubes, radius2,
1754  pts->points[n], points,
1755  rotMat[(i + n) % rotMat.size()]);
1756  generate_cubes_triplets(triplets, cubes, cubes_idx, vecInt);
1757 
1758  normals_conf[i] = normal_at_point(d1, d2, points, points_size,
1759  n, triplets, conf_interv);
1760  for (unsigned int pt = 0; pt < points_size; pt++) {
1761  points[pt] = pts->points[pointIdxRadiusSearch[pt]];
1762  }
1763  normals_vec[i] = rotMatInv[(i + n) % rotMat.size()] *
1764  nls->points[n].getNormalVector3fMap();
1765  }
1766 
1767  nls->points[n].getNormalVector3fMap() =
1768  normal_selection(rotations, normals_vec, normals_conf);
1769 
1770  delete[] points;
1771  delete[] cubes;
1772  }
1773  }
1774 };
1775 
1776 #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
PCL_Normal_Estimator
Class grouping different variants of the algorithm Class using a dependency to Point Cloud Library.
Definition: PCL_normEst.h:66
PCL_Normal_Estimator::normal_selection_mode
int normal_selection_mode() const
Definition: PCL_normEst.h:104
PCL_Normal_Estimator::normal_cloud
pcl::PointCloud< Normal >::Ptr normal_cloud() const
Definition: PCL_normEst.h:122
PCL_Normal_Estimator::accum_slices
int & accum_slices()
Definition: PCL_normEst.h:99
PCL_Normal_Estimator::normal_selection_mode
int & normal_selection_mode()
Definition: PCL_normEst.h:103
PCL_Normal_Estimator::point_cloud
pcl::PointCloud< Point >::Ptr point_cloud() const
Definition: PCL_normEst.h:120
PCL_Normal_Estimator::number_of_planes
int number_of_planes() const
Definition: PCL_normEst.h:98
PCL_Normal_Estimator::cluster_angle_rad
float & cluster_angle_rad()
Definition: PCL_normEst.h:105
PCL_Normal_Estimator::accum_slices
int accum_slices() const
Definition: PCL_normEst.h:100
PCL_Normal_Estimator::number_of_cubes
int & number_of_cubes()
Definition: PCL_normEst.h:116
PCL_Normal_Estimator::estimate
void estimate(int method=POINTS, int neighborhood_type=KNN, float neighborhood_size=200)
Definition: PCL_normEst.h:124
PCL_Normal_Estimator::minimal_neighbor_number_for_range_search
int & minimal_neighbor_number_for_range_search()
Definition: PCL_normEst.h:108
PCL_Normal_Estimator::RADIUS
@ RADIUS
Definition: PCL_normEst.h:82
PCL_Normal_Estimator::CUBES
@ CUBES
Definition: PCL_normEst.h:78
PCL_Normal_Estimator::UNIF
@ UNIF
Definition: PCL_normEst.h:76
PCL_Normal_Estimator::MEAN
@ MEAN
Definition: PCL_normEst.h:69
PCL_Normal_Estimator::POINTS
@ POINTS
Definition: PCL_normEst.h:74
PCL_Normal_Estimator::KNN
@ KNN
Definition: PCL_normEst.h:80
PCL_Normal_Estimator::BEST
@ BEST
Definition: PCL_normEst.h:70
PCL_Normal_Estimator::CLUSTER
@ CLUSTER
Definition: PCL_normEst.h:72
PCL_Normal_Estimator::PCL_Normal_Estimator
PCL_Normal_Estimator(typename pcl::PointCloud< Point >::Ptr points, typename pcl::PointCloud< Normal >::Ptr normals)
Constructor.
Definition: PCL_normEst.h:91
PCL_Normal_Estimator::rotation_number
int rotation_number() const
Definition: PCL_normEst.h:102
PCL_Normal_Estimator::number_of_planes
int & number_of_planes()
Definition: PCL_normEst.h:97
PCL_Normal_Estimator::small_radius_fact
float small_radius_fact() const
Definition: PCL_normEst.h:115
PCL_Normal_Estimator::point_cloud
pcl::PointCloud< Point >::Ptr & point_cloud()
Definition: PCL_normEst.h:119
PCL_Normal_Estimator::minimal_neighbor_number_for_range_search
int minimal_neighbor_number_for_range_search() const
Definition: PCL_normEst.h:111
PCL_Normal_Estimator::cluster_angle_rad
float cluster_angle_rad() const
Definition: PCL_normEst.h:106
PCL_Normal_Estimator::number_of_cubes
int number_of_cubes() const
Definition: PCL_normEst.h:117
PCL_Normal_Estimator::rotation_number
int & rotation_number()
Definition: PCL_normEst.h:101
PCL_Normal_Estimator::small_radius_fact
float & small_radius_fact()
Definition: PCL_normEst.h:114
PCL_Normal_Estimator::normal_cloud
pcl::PointCloud< Normal >::Ptr & normal_cloud()
Definition: PCL_normEst.h:121
normals
double normals[3]
Definition: TriangleMeshIO.cpp:54
dot
__host__ __device__ float dot(float2 a, float2 b)
Definition: cutil_math.h:1119
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
knncpp::Vector3i
Eigen::Matrix< Index, 3, 1 > Vector3i
Definition: knncpp.h:30