Point Cloud Library (PCL)  1.15.0-dev
•All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Modules Pages
gp3.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2010, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage, Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  * $Id$
35  *
36  */
37 
38 #ifndef PCL_SURFACE_IMPL_GP3_H_
39 #define PCL_SURFACE_IMPL_GP3_H_
40 
41 #include <pcl/surface/gp3.h>
42 
43 /////////////////////////////////////////////////////////////////////////////////////////////
44 template <typename PointInT> void
45 pcl::GreedyProjectionTriangulation<PointInT>::performReconstruction (pcl::PolygonMesh &output)
46 {
47  output.polygons.clear ();
48  output.polygons.reserve (2 * indices_->size ()); /// NOTE: usually the number of triangles is around twice the number of vertices
49  if (!reconstructPolygons (output.polygons))
50  {
51  PCL_ERROR ("[pcl::%s::performReconstruction] Reconstruction failed. Check parameters: search radius (%f) or mu (%f) before continuing.\n", getClassName ().c_str (), search_radius_, mu_);
52  output.cloud.width = output.cloud.height = 0;
53  output.cloud.data.clear ();
54  return;
55  }
56 }
57 
58 /////////////////////////////////////////////////////////////////////////////////////////////
59 template <typename PointInT> void
60 pcl::GreedyProjectionTriangulation<PointInT>::performReconstruction (std::vector<pcl::Vertices> &polygons)
61 {
62  polygons.clear ();
63  polygons.reserve (2 * indices_->size ()); /// NOTE: usually the number of triangles is around twice the number of vertices
64  if (!reconstructPolygons (polygons))
65  {
66  PCL_ERROR ("[pcl::%s::performReconstruction] Reconstruction failed. Check parameters: search radius (%f) or mu (%f) before continuing.\n", getClassName ().c_str (), search_radius_, mu_);
67  return;
68  }
69 }
70 
71 /////////////////////////////////////////////////////////////////////////////////////////////
72 template <typename PointInT> bool
73 pcl::GreedyProjectionTriangulation<PointInT>::reconstructPolygons (std::vector<pcl::Vertices> &polygons)
74 {
75  if (search_radius_ <= 0 || mu_ <= 0)
76  {
77  polygons.clear ();
78  return (false);
79  }
80  const double sqr_mu = mu_*mu_;
81  const double sqr_max_edge = search_radius_*search_radius_;
82  if (nnn_ > static_cast<int> (indices_->size ()))
83  nnn_ = static_cast<int> (indices_->size ());
84 
85  // Variables to hold the results of nearest neighbor searches
86  pcl::Indices nnIdx (nnn_);
87  std::vector<float> sqrDists (nnn_);
88 
89  // current number of connected components
90  int part_index = 0;
91 
92  // 2D coordinates of points
93  const Eigen::Vector2f uvn_nn_qp_zero = Eigen::Vector2f::Zero();
94 
95  // initializing fields
96  already_connected_ = false; // see declaration for comments :P
97 
98  // initializing states and fringe neighbors
99  part_.clear ();
100  state_.clear ();
101  source_.clear ();
102  ffn_.clear ();
103  sfn_.clear ();
104  part_.resize(indices_->size (), -1); // indices of point's part
105  state_.resize(indices_->size (), FREE);
106  source_.resize(indices_->size (), NONE);
107  ffn_.resize(indices_->size (), NONE);
108  sfn_.resize(indices_->size (), NONE);
109  fringe_queue_.clear ();
110  int fqIdx = 0; // current fringe's index in the queue to be processed
111 
112  // Avoiding NaN coordinates if needed
113  if (!input_->is_dense)
114  {
115  // Skip invalid points from the indices list
116  for (const auto& idx : (*indices_))
117  if (!std::isfinite ((*input_)[idx].x) ||
118  !std::isfinite ((*input_)[idx].y) ||
119  !std::isfinite ((*input_)[idx].z))
120  state_[idx] = NONE;
121  }
122 
123  // Saving coordinates and point to index mapping
124  coords_.clear ();
125  coords_.reserve (indices_->size ());
126  std::vector<int> point2index (input_->size (), -1);
127  for (int cp = 0; cp < static_cast<int> (indices_->size ()); ++cp)
128  {
129  coords_.push_back((*input_)[(*indices_)[cp]].getVector3fMap());
130  point2index[(*indices_)[cp]] = cp;
131  }
132 
133  // Initializing
134  int is_free=0, nr_parts=0, increase_nnn4fn=0, increase_nnn4s=0, increase_dist=0;
135  angles_.resize(nnn_);
136  std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > uvn_nn (nnn_);
137  Eigen::Vector2f uvn_s;
138 
139  // iterating through fringe points and finishing them until everything is done
140  while (is_free != NONE)
141  {
142  R_ = is_free;
143  if (state_[R_] == FREE)
144  {
145  state_[R_] = NONE;
146  part_[R_] = part_index++;
147 
148  // creating starting triangle
149  //searchForNeighbors ((*indices_)[R_], nnIdx, sqrDists);
150  //tree_->nearestKSearch ((*input_)[(*indices_)[R_]], nnn_, nnIdx, sqrDists);
151  tree_->nearestKSearch (indices_->at (R_), nnn_, nnIdx, sqrDists);
152  double sqr_dist_threshold = (std::min)(sqr_max_edge, sqr_mu * sqrDists[1]);
153 
154  // Search tree returns indices into the original cloud, but we are working with indices. TODO: make that optional!
155  for (int i = 1; i < nnn_; i++)
156  {
157  //if (point2index[nnIdx[i]] == -1)
158  // std::cerr << R_ << " [" << indices_->at (R_) << "] " << i << ": " << nnIdx[i] << " / " << point2index[nnIdx[i]] << std::endl;
159  nnIdx[i] = point2index[nnIdx[i]];
160  }
161 
162  // Get the normal estimate at the current point
163  const Eigen::Vector3f nc = (*input_)[(*indices_)[R_]].getNormalVector3fMap ();
164 
165  // Get a coordinate system that lies on a plane defined by its normal
166  v_ = nc.unitOrthogonal ();
167  u_ = nc.cross (v_);
168 
169  // Projecting point onto the surface
170  float dist = nc.dot (coords_[R_]);
171  proj_qp_ = coords_[R_] - dist * nc;
172 
173  // Converting coords, calculating angles and saving the projected near boundary edges
174  int nr_edge = 0;
175  std::vector<doubleEdge> doubleEdges;
176  for (int i = 1; i < nnn_; i++) // nearest neighbor with index 0 is the query point R_ itself
177  {
178  // Transforming coordinates
179  tmp_ = coords_[nnIdx[i]] - proj_qp_;
180  uvn_nn[i][0] = tmp_.dot(u_);
181  uvn_nn[i][1] = tmp_.dot(v_);
182  // Computing the angle between each neighboring point and the query point itself
183  angles_[i].angle = std::atan2(uvn_nn[i][1], uvn_nn[i][0]);
184  // initializing angle descriptors
185  angles_[i].index = nnIdx[i];
186  if (
187  (state_[nnIdx[i]] == COMPLETED) || (state_[nnIdx[i]] == BOUNDARY)
188  || (state_[nnIdx[i]] == NONE) || (nnIdx[i] == UNAVAILABLE) /// NOTE: discarding NaN points and those that are not in indices_
189  || (sqrDists[i] > sqr_dist_threshold)
190  )
191  angles_[i].visible = false;
192  else
193  angles_[i].visible = true;
194  // Saving the edges between nearby boundary points
195  if ((state_[nnIdx[i]] == FRINGE) || (state_[nnIdx[i]] == BOUNDARY))
196  {
197  doubleEdge e;
198  e.index = i;
199  nr_edge++;
200  tmp_ = coords_[ffn_[nnIdx[i]]] - proj_qp_;
201  e.first[0] = tmp_.dot(u_);
202  e.first[1] = tmp_.dot(v_);
203  tmp_ = coords_[sfn_[nnIdx[i]]] - proj_qp_;
204  e.second[0] = tmp_.dot(u_);
205  e.second[1] = tmp_.dot(v_);
206  doubleEdges.push_back(e);
207  }
208  }
209  angles_[0].visible = false;
210 
211  // Verify the visibility of each potential new vertex
212  for (int i = 1; i < nnn_; i++) // nearest neighbor with index 0 is the query point R_ itself
213  if ((angles_[i].visible) && (ffn_[R_] != nnIdx[i]) && (sfn_[R_] != nnIdx[i]))
214  {
215  bool visibility = true;
216  for (int j = 0; j < nr_edge; j++)
217  {
218  if (ffn_[nnIdx[doubleEdges[j].index]] != nnIdx[i])
219  visibility = isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].first, Eigen::Vector2f::Zero());
220  if (!visibility)
221  break;
222  if (sfn_[nnIdx[doubleEdges[j].index]] != nnIdx[i])
223  visibility = isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].second, Eigen::Vector2f::Zero());
224  if (!visibility)
225  break;
226  }
227  angles_[i].visible = visibility;
228  }
229 
230  // Selecting first two visible free neighbors
231  bool not_found = true;
232  int left = 1;
233  do
234  {
235  while ((left < nnn_) && ((!angles_[left].visible) || (state_[nnIdx[left]] > FREE))) left++;
236  if (left >= nnn_)
237  break;
238  int right = left+1;
239  do
240  {
241  while ((right < nnn_) && ((!angles_[right].visible) || (state_[nnIdx[right]] > FREE))) right++;
242  if (right >= nnn_)
243  break;
244  if ((coords_[nnIdx[left]] - coords_[nnIdx[right]]).squaredNorm () > sqr_max_edge)
245  right++;
246  else
247  {
248  addFringePoint (nnIdx[right], R_);
249  addFringePoint (nnIdx[left], nnIdx[right]);
250  addFringePoint (R_, nnIdx[left]);
251  state_[R_] = state_[nnIdx[left]] = state_[nnIdx[right]] = FRINGE;
252  ffn_[R_] = nnIdx[left];
253  sfn_[R_] = nnIdx[right];
254  ffn_[nnIdx[left]] = nnIdx[right];
255  sfn_[nnIdx[left]] = R_;
256  ffn_[nnIdx[right]] = R_;
257  sfn_[nnIdx[right]] = nnIdx[left];
258  addTriangle (R_, nnIdx[left], nnIdx[right], polygons);
259  nr_parts++;
260  not_found = false;
261  break;
262  }
263  }
264  while (true);
265  left++;
266  }
267  while (not_found);
268  }
269 
270  is_free = NONE;
271  for (std::size_t temp = 0; temp < indices_->size (); temp++)
272  {
273  if (state_[temp] == FREE)
274  {
275  is_free = temp;
276  break;
277  }
278  }
279 
280  bool is_fringe = true;
281  while (is_fringe)
282  {
283  is_fringe = false;
284 
285  int fqSize = static_cast<int> (fringe_queue_.size ());
286  while ((fqIdx < fqSize) && (state_[fringe_queue_[fqIdx]] != FRINGE))
287  fqIdx++;
288 
289  // an unfinished fringe point is found
290  if (fqIdx >= fqSize)
291  {
292  continue;
293  }
294 
295  R_ = fringe_queue_[fqIdx];
296  is_fringe = true;
297 
298  if (ffn_[R_] == sfn_[R_])
299  {
300  state_[R_] = COMPLETED;
301  continue;
302  }
303  //searchForNeighbors ((*indices_)[R_], nnIdx, sqrDists);
304  //tree_->nearestKSearch ((*input_)[(*indices_)[R_]], nnn_, nnIdx, sqrDists);
305  tree_->nearestKSearch (indices_->at (R_), nnn_, nnIdx, sqrDists);
306 
307  // Search tree returns indices into the original cloud, but we are working with indices TODO: make that optional!
308  for (int i = 1; i < nnn_; i++)
309  {
310  //if (point2index[nnIdx[i]] == -1)
311  // std::cerr << R_ << " [" << indices_->at (R_) << "] " << i << ": " << nnIdx[i] << " / " << point2index[nnIdx[i]] << std::endl;
312  nnIdx[i] = point2index[nnIdx[i]];
313  }
314 
315  // Locating FFN and SFN to adapt distance threshold
316  double sqr_source_dist = (coords_[R_] - coords_[source_[R_]]).squaredNorm ();
317  double sqr_ffn_dist = (coords_[R_] - coords_[ffn_[R_]]).squaredNorm ();
318  double sqr_sfn_dist = (coords_[R_] - coords_[sfn_[R_]]).squaredNorm ();
319  double max_sqr_fn_dist = (std::max)(sqr_ffn_dist, sqr_sfn_dist);
320  double sqr_dist_threshold = (std::min)(sqr_max_edge, sqr_mu * sqrDists[1]); //sqr_mu * sqr_avg_conn_dist);
321  if (max_sqr_fn_dist > sqrDists[nnn_-1])
322  {
323  if (0 == increase_nnn4fn)
324  PCL_WARN("Not enough neighbors are considered: ffn or sfn out of range! Consider increasing nnn_... Setting R=%d to be BOUNDARY!\n", R_);
325  increase_nnn4fn++;
326  state_[R_] = BOUNDARY;
327  continue;
328  }
329  double max_sqr_fns_dist = (std::max)(sqr_source_dist, max_sqr_fn_dist);
330  if (max_sqr_fns_dist > sqrDists[nnn_-1])
331  {
332  if (0 == increase_nnn4s)
333  PCL_WARN("Not enough neighbors are considered: source of R=%d is out of range! Consider increasing nnn_...\n", R_);
334  increase_nnn4s++;
335  }
336 
337  // Get the normal estimate at the current point
338  const Eigen::Vector3f nc = (*input_)[(*indices_)[R_]].getNormalVector3fMap ();
339 
340  // Get a coordinate system that lies on a plane defined by its normal
341  v_ = nc.unitOrthogonal ();
342  u_ = nc.cross (v_);
343 
344  // Projecting point onto the surface
345  float dist = nc.dot (coords_[R_]);
346  proj_qp_ = coords_[R_] - dist * nc;
347 
348  // Converting coords, calculating angles and saving the projected near boundary edges
349  int nr_edge = 0;
350  std::vector<doubleEdge> doubleEdges;
351  for (int i = 1; i < nnn_; i++) // nearest neighbor with index 0 is the query point R_ itself
352  {
353  tmp_ = coords_[nnIdx[i]] - proj_qp_;
354  uvn_nn[i][0] = tmp_.dot(u_);
355  uvn_nn[i][1] = tmp_.dot(v_);
356 
357  // Computing the angle between each neighboring point and the query point itself
358  angles_[i].angle = std::atan2(uvn_nn[i][1], uvn_nn[i][0]);
359  // initializing angle descriptors
360  angles_[i].index = nnIdx[i];
361  angles_[i].nnIndex = i;
362  if (
363  (state_[nnIdx[i]] == COMPLETED) || (state_[nnIdx[i]] == BOUNDARY)
364  || (state_[nnIdx[i]] == NONE) || (nnIdx[i] == UNAVAILABLE) /// NOTE: discarding NaN points and those that are not in indices_
365  || (sqrDists[i] > sqr_dist_threshold)
366  )
367  angles_[i].visible = false;
368  else
369  angles_[i].visible = true;
370  if ((ffn_[R_] == nnIdx[i]) || (sfn_[R_] == nnIdx[i]))
371  angles_[i].visible = true;
372  bool same_side = true;
373  const Eigen::Vector3f neighbor_normal = (*input_)[(*indices_)[nnIdx[i]]].getNormalVector3fMap (); /// NOTE: nnIdx was reset
374  double cosine = nc.dot (neighbor_normal);
375  if (cosine > 1) cosine = 1;
376  if (cosine < -1) cosine = -1;
377  double angle = std::acos (cosine);
378  if ((!consistent_) && (angle > M_PI/2))
379  angle = M_PI - angle;
380  if (angle > eps_angle_)
381  {
382  angles_[i].visible = false;
383  same_side = false;
384  }
385  // Saving the edges between nearby boundary points
386  if ((i!=0) && (same_side) && ((state_[nnIdx[i]] == FRINGE) || (state_[nnIdx[i]] == BOUNDARY)))
387  {
388  doubleEdge e;
389  e.index = i;
390  nr_edge++;
391  tmp_ = coords_[ffn_[nnIdx[i]]] - proj_qp_;
392  e.first[0] = tmp_.dot(u_);
393  e.first[1] = tmp_.dot(v_);
394  tmp_ = coords_[sfn_[nnIdx[i]]] - proj_qp_;
395  e.second[0] = tmp_.dot(u_);
396  e.second[1] = tmp_.dot(v_);
397  doubleEdges.push_back(e);
398  // Pruning by visibility criterion
399  if ((state_[nnIdx[i]] == FRINGE) && (ffn_[R_] != nnIdx[i]) && (sfn_[R_] != nnIdx[i]))
400  {
401  double angle1 = std::atan2(e.first[1] - uvn_nn[i][1], e.first[0] - uvn_nn[i][0]);
402  double angle2 = std::atan2(e.second[1] - uvn_nn[i][1], e.second[0] - uvn_nn[i][0]);
403  double angleMin, angleMax;
404  if (angle1 < angle2)
405  {
406  angleMin = angle1;
407  angleMax = angle2;
408  }
409  else
410  {
411  angleMin = angle2;
412  angleMax = angle1;
413  }
414  double angleR = angles_[i].angle + M_PI;
415  if (angleR >= M_PI)
416  angleR -= 2*M_PI;
417  if ((source_[nnIdx[i]] == ffn_[nnIdx[i]]) || (source_[nnIdx[i]] == sfn_[nnIdx[i]]))
418  {
419  if ((angleMax - angleMin) < M_PI)
420  {
421  if ((angleMin < angleR) && (angleR < angleMax))
422  angles_[i].visible = false;
423  }
424  else
425  {
426  if ((angleR < angleMin) || (angleMax < angleR))
427  angles_[i].visible = false;
428  }
429  }
430  else
431  {
432  tmp_ = coords_[source_[nnIdx[i]]] - proj_qp_;
433  uvn_s[0] = tmp_.dot(u_);
434  uvn_s[1] = tmp_.dot(v_);
435  double angleS = std::atan2(uvn_s[1] - uvn_nn[i][1], uvn_s[0] - uvn_nn[i][0]);
436  if ((angleMin < angleS) && (angleS < angleMax))
437  {
438  if ((angleMin < angleR) && (angleR < angleMax))
439  angles_[i].visible = false;
440  }
441  else
442  {
443  if ((angleR < angleMin) || (angleMax < angleR))
444  angles_[i].visible = false;
445  }
446  }
447  }
448  }
449  }
450  angles_[0].visible = false;
451 
452  // Verify the visibility of each potential new vertex
453  for (int i = 1; i < nnn_; i++) // nearest neighbor with index 0 is the query point R_ itself
454  if ((angles_[i].visible) && (ffn_[R_] != nnIdx[i]) && (sfn_[R_] != nnIdx[i]))
455  {
456  bool visibility = true;
457  for (int j = 0; j < nr_edge; j++)
458  {
459  if (doubleEdges[j].index != i)
460  {
461  const auto& f = ffn_[nnIdx[doubleEdges[j].index]];
462  if ((f != nnIdx[i]) && (f != R_))
463  visibility = isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].first, Eigen::Vector2f::Zero());
464  if (!visibility)
465  break;
466 
467  const auto& s = sfn_[nnIdx[doubleEdges[j].index]];
468  if ((s != nnIdx[i]) && (s != R_))
469  visibility = isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].second, Eigen::Vector2f::Zero());
470  if (!visibility)
471  break;
472  }
473  }
474  angles_[i].visible = visibility;
475  }
476 
477  // Sorting angles
478  std::sort (angles_.begin (), angles_.end (), GreedyProjectionTriangulation<PointInT>::nnAngleSortAsc);
479 
480  // Triangulating
481  if (angles_[2].visible == false)
482  {
483  if ( !( (angles_[0].index == ffn_[R_] && angles_[1].index == sfn_[R_]) || (angles_[0].index == sfn_[R_] && angles_[1].index == ffn_[R_]) ) )
484  {
485  state_[R_] = BOUNDARY;
486  }
487  else
488  {
489  if ((source_[R_] == angles_[0].index) || (source_[R_] == angles_[1].index))
490  state_[R_] = BOUNDARY;
491  else
492  {
493  if (sqr_max_edge < (coords_[ffn_[R_]] - coords_[sfn_[R_]]).squaredNorm ())
494  {
495  state_[R_] = BOUNDARY;
496  }
497  else
498  {
499  tmp_ = coords_[source_[R_]] - proj_qp_;
500  uvn_s[0] = tmp_.dot(u_);
501  uvn_s[1] = tmp_.dot(v_);
502  double angleS = std::atan2(uvn_s[1], uvn_s[0]);
503  double dif = angles_[1].angle - angles_[0].angle;
504  if ((angles_[0].angle < angleS) && (angleS < angles_[1].angle))
505  {
506  if (dif < 2*M_PI - maximum_angle_)
507  state_[R_] = BOUNDARY;
508  else
509  closeTriangle (polygons);
510  }
511  else
512  {
513  if (dif >= maximum_angle_)
514  state_[R_] = BOUNDARY;
515  else
516  closeTriangle (polygons);
517  }
518  }
519  }
520  }
521  continue;
522  }
523 
524  // Finding the FFN and SFN
525  int start = -1, end = -1;
526  for (int i=0; i<nnn_; i++)
527  {
528  if (ffn_[R_] == angles_[i].index)
529  {
530  start = i;
531  if (sfn_[R_] == angles_[i+1].index)
532  end = i+1;
533  else
534  if (i==0)
535  {
536  for (i = i+2; i < nnn_; i++)
537  if (sfn_[R_] == angles_[i].index)
538  break;
539  end = i;
540  }
541  else
542  {
543  for (i = i+2; i < nnn_; i++)
544  if (sfn_[R_] == angles_[i].index)
545  break;
546  end = i;
547  }
548  break;
549  }
550  if (sfn_[R_] == angles_[i].index)
551  {
552  start = i;
553  if (ffn_[R_] == angles_[i+1].index)
554  end = i+1;
555  else
556  if (i==0)
557  {
558  for (i = i+2; i < nnn_; i++)
559  if (ffn_[R_] == angles_[i].index)
560  break;
561  end = i;
562  }
563  else
564  {
565  for (i = i+2; i < nnn_; i++)
566  if (ffn_[R_] == angles_[i].index)
567  break;
568  end = i;
569  }
570  break;
571  }
572  }
573 
574  // start and end are always set, as we checked if ffn or sfn are out of range before, but let's check anyways if < 0
575  if ((start < 0) || (end < 0) || (end == nnn_) || (!angles_[start].visible) || (!angles_[end].visible))
576  {
577  state_[R_] = BOUNDARY;
578  continue;
579  }
580 
581  // Finding last visible nn
582  int last_visible = end;
583  while ((last_visible+1<nnn_) && (angles_[last_visible+1].visible)) last_visible++;
584 
585  // Finding visibility region of R
586  bool need_invert = false;
587  if ((source_[R_] == ffn_[R_]) || (source_[R_] == sfn_[R_]))
588  {
589  if ((angles_[end].angle - angles_[start].angle) < M_PI)
590  need_invert = true;
591  }
592  else
593  {
594  int sourceIdx;
595  for (sourceIdx=0; sourceIdx<nnn_; sourceIdx++)
596  if (angles_[sourceIdx].index == source_[R_])
597  break;
598  if (sourceIdx == nnn_)
599  {
600  int vis_free = NONE, nnCB = NONE; // any free visible and nearest completed or boundary neighbor of R
601  for (int i = 1; i < nnn_; i++) // nearest neighbor with index 0 is the query point R_ itself
602  {
603  // NOTE: nnCB is an index in nnIdx
604  if ((state_[nnIdx[i]] == COMPLETED) || (state_[nnIdx[i]] == BOUNDARY))
605  {
606  if (nnCB == NONE)
607  {
608  nnCB = i;
609  if (vis_free != NONE)
610  break;
611  }
612  }
613  // NOTE: vis_free is an index in angles
614  if (state_[angles_[i].index] <= FREE)
615  {
616  if (i <= last_visible)
617  {
618  vis_free = i;
619  if (nnCB != NONE)
620  break;
621  }
622  }
623  }
624  // NOTE: nCB is an index in angles
625  int nCB = 0;
626  if (nnCB != NONE)
627  while (angles_[nCB].index != nnIdx[nnCB]) nCB++;
628  else
629  nCB = NONE;
630 
631  if (vis_free != NONE)
632  {
633  if ((vis_free < start) || (vis_free > end))
634  need_invert = true;
635  }
636  else
637  {
638  if (nCB != NONE)
639  {
640  if ((nCB == start) || (nCB == end))
641  {
642  bool inside_CB = false;
643  bool outside_CB = false;
644  for (int i=0; i<nnn_; i++)
645  {
646  if (
647  ((state_[angles_[i].index] == COMPLETED) || (state_[angles_[i].index] == BOUNDARY))
648  && (i != start) && (i != end)
649  )
650  {
651  if ((angles_[start].angle <= angles_[i].angle) && (angles_[i].angle <= angles_[end].angle))
652  {
653  inside_CB = true;
654  if (outside_CB)
655  break;
656  }
657  else
658  {
659  outside_CB = true;
660  if (inside_CB)
661  break;
662  }
663  }
664  }
665  if (inside_CB && !outside_CB)
666  need_invert = true;
667  else if (inside_CB || !outside_CB)
668  {
669  if ((angles_[end].angle - angles_[start].angle) < M_PI)
670  need_invert = true;
671  }
672  }
673  else
674  {
675  if ((angles_[nCB].angle > angles_[start].angle) && (angles_[nCB].angle < angles_[end].angle))
676  need_invert = true;
677  }
678  }
679  else
680  {
681  if (start == end-1)
682  need_invert = true;
683  }
684  }
685  }
686  else if ((angles_[start].angle < angles_[sourceIdx].angle) && (angles_[sourceIdx].angle < angles_[end].angle))
687  need_invert = true;
688  }
689 
690  // switching start and end if necessary
691  if (need_invert)
692  {
693  int tmp = start;
694  start = end;
695  end = tmp;
696  }
697 
698  // Arranging visible nnAngles in the order they need to be connected and
699  // compute the maximal angle difference between two consecutive visible angles
700  bool is_boundary = false, is_skinny = false;
701  std::vector<bool> gaps (nnn_, false);
702  std::vector<bool> skinny (nnn_, false);
703  std::vector<double> dif (nnn_);
704  std::vector<int> angleIdx; angleIdx.reserve (nnn_);
705  if (start > end)
706  {
707  for (int j=start; j<last_visible; j++)
708  {
709  dif[j] = (angles_[j+1].angle - angles_[j].angle);
710  if (dif[j] < minimum_angle_)
711  {
712  skinny[j] = is_skinny = true;
713  }
714  else if (maximum_angle_ <= dif[j])
715  {
716  gaps[j] = is_boundary = true;
717  }
718  if ((!gaps[j]) && (sqr_max_edge < (coords_[angles_[j+1].index] - coords_[angles_[j].index]).squaredNorm ()))
719  {
720  gaps[j] = is_boundary = true;
721  }
722  angleIdx.push_back(j);
723  }
724 
725  dif[last_visible] = (2*M_PI + angles_[0].angle - angles_[last_visible].angle);
726  if (dif[last_visible] < minimum_angle_)
727  {
728  skinny[last_visible] = is_skinny = true;
729  }
730  else if (maximum_angle_ <= dif[last_visible])
731  {
732  gaps[last_visible] = is_boundary = true;
733  }
734  if ((!gaps[last_visible]) && (sqr_max_edge < (coords_[angles_[0].index] - coords_[angles_[last_visible].index]).squaredNorm ()))
735  {
736  gaps[last_visible] = is_boundary = true;
737  }
738  angleIdx.push_back(last_visible);
739 
740  for (int j=0; j<end; j++)
741  {
742  dif[j] = (angles_[j+1].angle - angles_[j].angle);
743  if (dif[j] < minimum_angle_)
744  {
745  skinny[j] = is_skinny = true;
746  }
747  else if (maximum_angle_ <= dif[j])
748  {
749  gaps[j] = is_boundary = true;
750  }
751  if ((!gaps[j]) && (sqr_max_edge < (coords_[angles_[j+1].index] - coords_[angles_[j].index]).squaredNorm ()))
752  {
753  gaps[j] = is_boundary = true;
754  }
755  angleIdx.push_back(j);
756  }
757  angleIdx.push_back(end);
758  }
759  // start < end
760  else
761  {
762  for (int j=start; j<end; j++)
763  {
764  dif[j] = (angles_[j+1].angle - angles_[j].angle);
765  if (dif[j] < minimum_angle_)
766  {
767  skinny[j] = is_skinny = true;
768  }
769  else if (maximum_angle_ <= dif[j])
770  {
771  gaps[j] = is_boundary = true;
772  }
773  if ((!gaps[j]) && (sqr_max_edge < (coords_[angles_[j+1].index] - coords_[angles_[j].index]).squaredNorm ()))
774  {
775  gaps[j] = is_boundary = true;
776  }
777  angleIdx.push_back(j);
778  }
779  angleIdx.push_back(end);
780  }
781 
782  // Set the state of the point
783  state_[R_] = is_boundary ? BOUNDARY : COMPLETED;
784 
785  auto first_gap_after = angleIdx.end ();
786  auto last_gap_before = angleIdx.begin ();
787  int nr_gaps = 0;
788  for (auto it = angleIdx.begin (); it != angleIdx.end () - 1; ++it)
789  {
790  if (gaps[*it])
791  {
792  nr_gaps++;
793  if (first_gap_after == angleIdx.end())
794  first_gap_after = it;
795  last_gap_before = it+1;
796  }
797  }
798  if (nr_gaps > 1)
799  {
800  angleIdx.erase(first_gap_after+1, last_gap_before);
801  }
802 
803  // Neglecting points that would form skinny triangles (if possible)
804  if (is_skinny)
805  {
806  double angle_so_far = 0, angle_would_be;
807  double max_combined_angle = (std::min)(maximum_angle_, M_PI-2*minimum_angle_);
808  Eigen::Vector2f X;
809  Eigen::Vector2f S1;
810  Eigen::Vector2f S2;
811  std::vector<int> to_erase;
812  for (auto it = angleIdx.begin()+1; it != angleIdx.end()-1; ++it)
813  {
814  if (gaps[*(it-1)])
815  angle_so_far = 0;
816  else
817  angle_so_far += dif[*(it-1)];
818  if (gaps[*it])
819  angle_would_be = angle_so_far;
820  else
821  angle_would_be = angle_so_far + dif[*it];
822  if (
823  (skinny[*it] || skinny[*(it-1)]) &&
824  ((state_[angles_[*it].index] <= FREE) || (state_[angles_[*(it-1)].index] <= FREE)) &&
825  ((!gaps[*it]) || (angles_[*it].nnIndex > angles_[*(it-1)].nnIndex)) &&
826  ((!gaps[*(it-1)]) || (angles_[*it].nnIndex > angles_[*(it+1)].nnIndex)) &&
827  (angle_would_be < max_combined_angle)
828  )
829  {
830  if (gaps[*(it-1)])
831  {
832  gaps[*it] = true;
833  to_erase.push_back(*it);
834  }
835  else if (gaps[*it])
836  {
837  gaps[*(it-1)] = true;
838  to_erase.push_back(*it);
839  }
840  else
841  {
842  std::vector<int>::iterator prev_it;
843  int erased_idx = static_cast<int> (to_erase.size ()) -1;
844  for (prev_it = it-1; (erased_idx != -1) && (it != angleIdx.begin()); --it)
845  if (*it == to_erase[erased_idx])
846  erased_idx--;
847  else
848  break;
849  bool can_delete = true;
850  for (auto curr_it = prev_it+1; curr_it != it+1; ++curr_it)
851  {
852  tmp_ = coords_[angles_[*curr_it].index] - proj_qp_;
853  X[0] = tmp_.dot(u_);
854  X[1] = tmp_.dot(v_);
855  tmp_ = coords_[angles_[*prev_it].index] - proj_qp_;
856  S1[0] = tmp_.dot(u_);
857  S1[1] = tmp_.dot(v_);
858  tmp_ = coords_[angles_[*(it+1)].index] - proj_qp_;
859  S2[0] = tmp_.dot(u_);
860  S2[1] = tmp_.dot(v_);
861  // check for inclusions
862  if (isVisible(X,S1,S2))
863  {
864  can_delete = false;
865  angle_so_far = 0;
866  break;
867  }
868  }
869  if (can_delete)
870  {
871  to_erase.push_back(*it);
872  }
873  }
874  }
875  else
876  angle_so_far = 0;
877  }
878  for (const auto &idx : to_erase)
879  {
880  for (auto iter = angleIdx.begin(); iter != angleIdx.end(); ++iter)
881  if (idx == *iter)
882  {
883  angleIdx.erase(iter);
884  break;
885  }
886  }
887  }
888 
889  // Writing edges and updating edge-front
890  changed_1st_fn_ = false;
891  changed_2nd_fn_ = false;
892  new2boundary_ = NONE;
893  for (auto it = angleIdx.begin()+1; it != angleIdx.end()-1; ++it)
894  {
895  current_index_ = angles_[*it].index;
896 
897  is_current_free_ = false;
898  if (state_[current_index_] <= FREE)
899  {
900  state_[current_index_] = FRINGE;
901  is_current_free_ = true;
902  }
903  else if (!already_connected_)
904  {
905  prev_is_ffn_ = (ffn_[current_index_] == angles_[*(it-1)].index) && (!gaps[*(it-1)]);
906  prev_is_sfn_ = (sfn_[current_index_] == angles_[*(it-1)].index) && (!gaps[*(it-1)]);
907  next_is_ffn_ = (ffn_[current_index_] == angles_[*(it+1)].index) && (!gaps[*it]);
908  next_is_sfn_ = (sfn_[current_index_] == angles_[*(it+1)].index) && (!gaps[*it]);
909  }
910 
911  if (gaps[*it])
912  if (gaps[*(it-1)])
913  {
914  if (is_current_free_)
915  state_[current_index_] = NONE; /// TODO: document!
916  }
917 
918  else // (gaps[*it]) && ^(gaps[*(it-1)])
919  {
920  addTriangle (current_index_, angles_[*(it-1)].index, R_, polygons);
921  addFringePoint (current_index_, R_);
922  new2boundary_ = current_index_;
923  if (!already_connected_)
924  connectPoint (polygons, angles_[*(it-1)].index, R_,
925  angles_[*(it+1)].index,
926  uvn_nn[angles_[*it].nnIndex], uvn_nn[angles_[*(it-1)].nnIndex], uvn_nn_qp_zero);
927  else already_connected_ = false;
928  if (ffn_[R_] == angles_[*(angleIdx.begin())].index)
929  {
930  ffn_[R_] = new2boundary_;
931  }
932  else if (sfn_[R_] == angles_[*(angleIdx.begin())].index)
933  {
934  sfn_[R_] = new2boundary_;
935  }
936  }
937 
938  else // ^(gaps[*it])
939  if (gaps[*(it-1)])
940  {
941  addFringePoint (current_index_, R_);
942  new2boundary_ = current_index_;
943  if (!already_connected_) connectPoint (polygons, R_, angles_[*(it+1)].index,
944  (it+2) == angleIdx.end() ? -1 : angles_[*(it+2)].index,
945  uvn_nn[angles_[*it].nnIndex], uvn_nn_qp_zero,
946  uvn_nn[angles_[*(it+1)].nnIndex]);
947  else already_connected_ = false;
948  if (ffn_[R_] == angles_[*(angleIdx.end()-1)].index)
949  {
950  ffn_[R_] = new2boundary_;
951  }
952  else if (sfn_[R_] == angles_[*(angleIdx.end()-1)].index)
953  {
954  sfn_[R_] = new2boundary_;
955  }
956  }
957 
958  else // ^(gaps[*it]) && ^(gaps[*(it-1)])
959  {
960  addTriangle (current_index_, angles_[*(it-1)].index, R_, polygons);
961  addFringePoint (current_index_, R_);
962  if (!already_connected_) connectPoint (polygons, angles_[*(it-1)].index, angles_[*(it+1)].index,
963  (it+2) == angleIdx.end() ? -1 : gaps[*(it+1)] ? R_ : angles_[*(it+2)].index,
964  uvn_nn[angles_[*it].nnIndex],
965  uvn_nn[angles_[*(it-1)].nnIndex],
966  uvn_nn[angles_[*(it+1)].nnIndex]);
967  else already_connected_ = false;
968  }
969  }
970 
971  // Finishing up R_
972  if (ffn_[R_] == sfn_[R_])
973  {
974  state_[R_] = COMPLETED;
975  }
976  if (!gaps[*(angleIdx.end()-2)])
977  {
978  addTriangle (angles_[*(angleIdx.end()-2)].index, angles_[*(angleIdx.end()-1)].index, R_, polygons);
979  addFringePoint (angles_[*(angleIdx.end()-2)].index, R_);
980  if (R_ == ffn_[angles_[*(angleIdx.end()-1)].index])
981  {
982  if (angles_[*(angleIdx.end()-2)].index == sfn_[angles_[*(angleIdx.end()-1)].index])
983  {
984  state_[angles_[*(angleIdx.end()-1)].index] = COMPLETED;
985  }
986  else
987  {
988  ffn_[angles_[*(angleIdx.end()-1)].index] = angles_[*(angleIdx.end()-2)].index;
989  }
990  }
991  else if (R_ == sfn_[angles_[*(angleIdx.end()-1)].index])
992  {
993  if (angles_[*(angleIdx.end()-2)].index == ffn_[angles_[*(angleIdx.end()-1)].index])
994  {
995  state_[angles_[*(angleIdx.end()-1)].index] = COMPLETED;
996  }
997  else
998  {
999  sfn_[angles_[*(angleIdx.end()-1)].index] = angles_[*(angleIdx.end()-2)].index;
1000  }
1001  }
1002  }
1003  if (!gaps[*(angleIdx.begin())])
1004  {
1005  if (R_ == ffn_[angles_[*(angleIdx.begin())].index])
1006  {
1007  if (angles_[*(angleIdx.begin()+1)].index == sfn_[angles_[*(angleIdx.begin())].index])
1008  {
1009  state_[angles_[*(angleIdx.begin())].index] = COMPLETED;
1010  }
1011  else
1012  {
1013  ffn_[angles_[*(angleIdx.begin())].index] = angles_[*(angleIdx.begin()+1)].index;
1014  }
1015  }
1016  else if (R_ == sfn_[angles_[*(angleIdx.begin())].index])
1017  {
1018  if (angles_[*(angleIdx.begin()+1)].index == ffn_[angles_[*(angleIdx.begin())].index])
1019  {
1020  state_[angles_[*(angleIdx.begin())].index] = COMPLETED;
1021  }
1022  else
1023  {
1024  sfn_[angles_[*(angleIdx.begin())].index] = angles_[*(angleIdx.begin()+1)].index;
1025  }
1026  }
1027  }
1028  }
1029  }
1030  PCL_DEBUG ("Number of triangles: %lu\n", polygons.size());
1031  PCL_DEBUG ("Number of unconnected parts: %d\n", nr_parts);
1032  if (increase_nnn4fn > 0)
1033  PCL_WARN ("Number of neighborhood size increase requests for fringe neighbors: %d\n", increase_nnn4fn);
1034  if (increase_nnn4s > 0)
1035  PCL_WARN ("Number of neighborhood size increase requests for source: %d\n", increase_nnn4s);
1036  if (increase_dist > 0)
1037  PCL_WARN ("Number of automatic maximum distance increases: %d\n", increase_dist);
1038 
1039  // sorting and removing doubles from fringe queue
1040  std::sort (fringe_queue_.begin (), fringe_queue_.end ());
1041  fringe_queue_.erase (std::unique (fringe_queue_.begin (), fringe_queue_.end ()), fringe_queue_.end ());
1042  PCL_DEBUG ("Number of processed points: %lu / %lu\n", fringe_queue_.size(), indices_->size ());
1043  return (true);
1044 }
1045 
1046 /////////////////////////////////////////////////////////////////////////////////////////////
1047 template <typename PointInT> void
1048 pcl::GreedyProjectionTriangulation<PointInT>::closeTriangle (std::vector<pcl::Vertices> &polygons)
1049 {
1050  state_[R_] = COMPLETED;
1051  addTriangle (angles_[0].index, angles_[1].index, R_, polygons);
1052  for (int aIdx=0; aIdx<2; aIdx++)
1053  {
1054  if (ffn_[angles_[aIdx].index] == R_)
1055  {
1056  if (sfn_[angles_[aIdx].index] == angles_[(aIdx+1)%2].index)
1057  {
1058  state_[angles_[aIdx].index] = COMPLETED;
1059  }
1060  else
1061  {
1062  ffn_[angles_[aIdx].index] = angles_[(aIdx+1)%2].index;
1063  }
1064  }
1065  else if (sfn_[angles_[aIdx].index] == R_)
1066  {
1067  if (ffn_[angles_[aIdx].index] == angles_[(aIdx+1)%2].index)
1068  {
1069  state_[angles_[aIdx].index] = COMPLETED;
1070  }
1071  else
1072  {
1073  sfn_[angles_[aIdx].index] = angles_[(aIdx+1)%2].index;
1074  }
1075  }
1076  }
1077 }
1078 
1079 /////////////////////////////////////////////////////////////////////////////////////////////
1080 template <typename PointInT> void
1081 pcl::GreedyProjectionTriangulation<PointInT>::connectPoint (
1082  std::vector<pcl::Vertices> &polygons,
1083  const pcl::index_t prev_index, const pcl::index_t next_index, const pcl::index_t next_next_index,
1084  const Eigen::Vector2f &uvn_current,
1085  const Eigen::Vector2f &uvn_prev,
1086  const Eigen::Vector2f &uvn_next)
1087 {
1088  if (is_current_free_)
1089  {
1090  ffn_[current_index_] = prev_index;
1091  sfn_[current_index_] = next_index;
1092  }
1093  else
1094  {
1095  if ((prev_is_ffn_ && next_is_sfn_) || (prev_is_sfn_ && next_is_ffn_))
1096  state_[current_index_] = COMPLETED;
1097  else if (prev_is_ffn_ && !next_is_sfn_)
1098  ffn_[current_index_] = next_index;
1099  else if (next_is_ffn_ && !prev_is_sfn_)
1100  ffn_[current_index_] = prev_index;
1101  else if (prev_is_sfn_ && !next_is_ffn_)
1102  sfn_[current_index_] = next_index;
1103  else if (next_is_sfn_ && !prev_is_ffn_)
1104  sfn_[current_index_] = prev_index;
1105  else
1106  {
1107  bool found_triangle = false;
1108  if ((prev_index != R_) && ((ffn_[current_index_] == ffn_[prev_index]) || (ffn_[current_index_] == sfn_[prev_index])))
1109  {
1110  found_triangle = true;
1111  addTriangle (current_index_, ffn_[current_index_], prev_index, polygons);
1112  state_[prev_index] = COMPLETED;
1113  state_[ffn_[current_index_]] = COMPLETED;
1114  ffn_[current_index_] = next_index;
1115  }
1116  else if ((prev_index != R_) && ((sfn_[current_index_] == ffn_[prev_index]) || (sfn_[current_index_] == sfn_[prev_index])))
1117  {
1118  found_triangle = true;
1119  addTriangle (current_index_, sfn_[current_index_], prev_index, polygons);
1120  state_[prev_index] = COMPLETED;
1121  state_[sfn_[current_index_]] = COMPLETED;
1122  sfn_[current_index_] = next_index;
1123  }
1124  else if (state_[next_index] > FREE)
1125  {
1126  if ((ffn_[current_index_] == ffn_[next_index]) || (ffn_[current_index_] == sfn_[next_index]))
1127  {
1128  found_triangle = true;
1129  addTriangle (current_index_, ffn_[current_index_], next_index, polygons);
1130 
1131  if (ffn_[current_index_] == ffn_[next_index])
1132  {
1133  ffn_[next_index] = current_index_;
1134  }
1135  else
1136  {
1137  sfn_[next_index] = current_index_;
1138  }
1139  state_[ffn_[current_index_]] = COMPLETED;
1140  ffn_[current_index_] = prev_index;
1141  }
1142  else if ((sfn_[current_index_] == ffn_[next_index]) || (sfn_[current_index_] == sfn_[next_index]))
1143  {
1144  found_triangle = true;
1145  addTriangle (current_index_, sfn_[current_index_], next_index, polygons);
1146 
1147  if (sfn_[current_index_] == ffn_[next_index])
1148  {
1149  ffn_[next_index] = current_index_;
1150  }
1151  else
1152  {
1153  sfn_[next_index] = current_index_;
1154  }
1155  state_[sfn_[current_index_]] = COMPLETED;
1156  sfn_[current_index_] = prev_index;
1157  }
1158  }
1159 
1160  if (found_triangle)
1161  {
1162  }
1163  else
1164  {
1165  tmp_ = coords_[ffn_[current_index_]] - proj_qp_;
1166  uvn_ffn_[0] = tmp_.dot(u_);
1167  uvn_ffn_[1] = tmp_.dot(v_);
1168  tmp_ = coords_[sfn_[current_index_]] - proj_qp_;
1169  uvn_sfn_[0] = tmp_.dot(u_);
1170  uvn_sfn_[1] = tmp_.dot(v_);
1171  bool prev_ffn = isVisible(uvn_prev, uvn_next, uvn_current, uvn_ffn_) && isVisible(uvn_prev, uvn_sfn_, uvn_current, uvn_ffn_);
1172  bool prev_sfn = isVisible(uvn_prev, uvn_next, uvn_current, uvn_sfn_) && isVisible(uvn_prev, uvn_ffn_, uvn_current, uvn_sfn_);
1173  bool next_ffn = isVisible(uvn_next, uvn_prev, uvn_current, uvn_ffn_) && isVisible(uvn_next, uvn_sfn_, uvn_current, uvn_ffn_);
1174  bool next_sfn = isVisible(uvn_next, uvn_prev, uvn_current, uvn_sfn_) && isVisible(uvn_next, uvn_ffn_, uvn_current, uvn_sfn_);
1175  int min_dist = -1;
1176  if (prev_ffn && next_sfn && prev_sfn && next_ffn)
1177  {
1178  /* should be never the case */
1179  double prev2f = (coords_[ffn_[current_index_]] - coords_[prev_index]).squaredNorm ();
1180  double next2s = (coords_[sfn_[current_index_]] - coords_[next_index]).squaredNorm ();
1181  double prev2s = (coords_[sfn_[current_index_]] - coords_[prev_index]).squaredNorm ();
1182  double next2f = (coords_[ffn_[current_index_]] - coords_[next_index]).squaredNorm ();
1183  if (prev2f < prev2s)
1184  {
1185  if (prev2f < next2f)
1186  {
1187  if (prev2f < next2s)
1188  min_dist = 0;
1189  else
1190  min_dist = 3;
1191  }
1192  else
1193  {
1194  if (next2f < next2s)
1195  min_dist = 2;
1196  else
1197  min_dist = 3;
1198  }
1199  }
1200  else
1201  {
1202  if (prev2s < next2f)
1203  {
1204  if (prev2s < next2s)
1205  min_dist = 1;
1206  else
1207  min_dist = 3;
1208  }
1209  else
1210  {
1211  if (next2f < next2s)
1212  min_dist = 2;
1213  else
1214  min_dist = 3;
1215  }
1216  }
1217  }
1218  else if (prev_ffn && next_sfn)
1219  {
1220  /* a clear case */
1221  double prev2f = (coords_[ffn_[current_index_]] - coords_[prev_index]).squaredNorm ();
1222  double next2s = (coords_[sfn_[current_index_]] - coords_[next_index]).squaredNorm ();
1223  if (prev2f < next2s)
1224  min_dist = 0;
1225  else
1226  min_dist = 3;
1227  }
1228  else if (prev_sfn && next_ffn)
1229  {
1230  /* a clear case */
1231  double prev2s = (coords_[sfn_[current_index_]] - coords_[prev_index]).squaredNorm ();
1232  double next2f = (coords_[ffn_[current_index_]] - coords_[next_index]).squaredNorm ();
1233  if (prev2s < next2f)
1234  min_dist = 1;
1235  else
1236  min_dist = 2;
1237  }
1238  /* straightforward cases */
1239  else if (prev_ffn && !next_sfn && !prev_sfn && !next_ffn)
1240  min_dist = 0;
1241  else if (!prev_ffn && !next_sfn && prev_sfn && !next_ffn)
1242  min_dist = 1;
1243  else if (!prev_ffn && !next_sfn && !prev_sfn && next_ffn)
1244  min_dist = 2;
1245  else if (!prev_ffn && next_sfn && !prev_sfn && !next_ffn)
1246  min_dist = 3;
1247  /* messed up cases */
1248  else if (prev_ffn)
1249  {
1250  double prev2f = (coords_[ffn_[current_index_]] - coords_[prev_index]).squaredNorm ();
1251  if (prev_sfn)
1252  {
1253  double prev2s = (coords_[sfn_[current_index_]] - coords_[prev_index]).squaredNorm ();
1254  if (prev2s < prev2f)
1255  min_dist = 1;
1256  else
1257  min_dist = 0;
1258  }
1259  else if (next_ffn)
1260  {
1261  double next2f = (coords_[ffn_[current_index_]] - coords_[next_index]).squaredNorm ();
1262  if (next2f < prev2f)
1263  min_dist = 2;
1264  else
1265  min_dist = 0;
1266  }
1267  }
1268  else if (next_sfn)
1269  {
1270  double next2s = (coords_[sfn_[current_index_]] - coords_[next_index]).squaredNorm ();
1271  if (prev_sfn)
1272  {
1273  double prev2s = (coords_[sfn_[current_index_]] - coords_[prev_index]).squaredNorm ();
1274  if (prev2s < next2s)
1275  min_dist = 1;
1276  else
1277  min_dist = 3;
1278  }
1279  else if (next_ffn)
1280  {
1281  double next2f = (coords_[ffn_[current_index_]] - coords_[next_index]).squaredNorm ();
1282  if (next2f < next2s)
1283  min_dist = 2;
1284  else
1285  min_dist = 3;
1286  }
1287  }
1288  switch (min_dist)
1289  {
1290  case 0://prev2f:
1291  {
1292  addTriangle (current_index_, ffn_[current_index_], prev_index, polygons);
1293 
1294  /* updating prev_index */
1295  if (ffn_[prev_index] == current_index_)
1296  {
1297  ffn_[prev_index] = ffn_[current_index_];
1298  }
1299  else if (sfn_[prev_index] == current_index_)
1300  {
1301  sfn_[prev_index] = ffn_[current_index_];
1302  }
1303  else if (ffn_[prev_index] == R_)
1304  {
1305  changed_1st_fn_ = true;
1306  ffn_[prev_index] = ffn_[current_index_];
1307  }
1308  else if (sfn_[prev_index] == R_)
1309  {
1310  changed_1st_fn_ = true;
1311  sfn_[prev_index] = ffn_[current_index_];
1312  }
1313  else if (prev_index == R_)
1314  {
1315  new2boundary_ = ffn_[current_index_];
1316  }
1317 
1318  /* updating ffn */
1319  if (ffn_[ffn_[current_index_]] == current_index_)
1320  {
1321  ffn_[ffn_[current_index_]] = prev_index;
1322  }
1323  else if (sfn_[ffn_[current_index_]] == current_index_)
1324  {
1325  sfn_[ffn_[current_index_]] = prev_index;
1326  }
1327 
1328  /* updating current */
1329  ffn_[current_index_] = next_index;
1330 
1331  break;
1332  }
1333  case 1://prev2s:
1334  {
1335  addTriangle (current_index_, sfn_[current_index_], prev_index, polygons);
1336 
1337  /* updating prev_index */
1338  if (ffn_[prev_index] == current_index_)
1339  {
1340  ffn_[prev_index] = sfn_[current_index_];
1341  }
1342  else if (sfn_[prev_index] == current_index_)
1343  {
1344  sfn_[prev_index] = sfn_[current_index_];
1345  }
1346  else if (ffn_[prev_index] == R_)
1347  {
1348  changed_1st_fn_ = true;
1349  ffn_[prev_index] = sfn_[current_index_];
1350  }
1351  else if (sfn_[prev_index] == R_)
1352  {
1353  changed_1st_fn_ = true;
1354  sfn_[prev_index] = sfn_[current_index_];
1355  }
1356  else if (prev_index == R_)
1357  {
1358  new2boundary_ = sfn_[current_index_];
1359  }
1360 
1361  /* updating sfn */
1362  if (ffn_[sfn_[current_index_]] == current_index_)
1363  {
1364  ffn_[sfn_[current_index_]] = prev_index;
1365  }
1366  else if (sfn_[sfn_[current_index_]] == current_index_)
1367  {
1368  sfn_[sfn_[current_index_]] = prev_index;
1369  }
1370 
1371  /* updating current */
1372  sfn_[current_index_] = next_index;
1373 
1374  break;
1375  }
1376  case 2://next2f:
1377  {
1378  addTriangle (current_index_, ffn_[current_index_], next_index, polygons);
1379  auto neighbor_update = next_index;
1380 
1381  /* updating next_index */
1382  if (state_[next_index] <= FREE)
1383  {
1384  state_[next_index] = FRINGE;
1385  ffn_[next_index] = current_index_;
1386  sfn_[next_index] = ffn_[current_index_];
1387  }
1388  else
1389  {
1390  if (ffn_[next_index] == R_)
1391  {
1392  changed_2nd_fn_ = true;
1393  ffn_[next_index] = ffn_[current_index_];
1394  }
1395  else if (sfn_[next_index] == R_)
1396  {
1397  changed_2nd_fn_ = true;
1398  sfn_[next_index] = ffn_[current_index_];
1399  }
1400  else if (next_index == R_)
1401  {
1402  new2boundary_ = ffn_[current_index_];
1403  if (next_next_index == new2boundary_)
1404  already_connected_ = true;
1405  }
1406  else if (ffn_[next_index] == next_next_index)
1407  {
1408  already_connected_ = true;
1409  ffn_[next_index] = ffn_[current_index_];
1410  }
1411  else if (sfn_[next_index] == next_next_index)
1412  {
1413  already_connected_ = true;
1414  sfn_[next_index] = ffn_[current_index_];
1415  }
1416  else
1417  {
1418  tmp_ = coords_[ffn_[next_index]] - proj_qp_;
1419  uvn_next_ffn_[0] = tmp_.dot(u_);
1420  uvn_next_ffn_[1] = tmp_.dot(v_);
1421  tmp_ = coords_[sfn_[next_index]] - proj_qp_;
1422  uvn_next_sfn_[0] = tmp_.dot(u_);
1423  uvn_next_sfn_[1] = tmp_.dot(v_);
1424 
1425  bool ffn_next_ffn = isVisible(uvn_next_ffn_, uvn_next, uvn_current, uvn_ffn_) && isVisible(uvn_next_ffn_, uvn_next, uvn_next_sfn_, uvn_ffn_);
1426  bool sfn_next_ffn = isVisible(uvn_next_sfn_, uvn_next, uvn_current, uvn_ffn_) && isVisible(uvn_next_sfn_, uvn_next, uvn_next_ffn_, uvn_ffn_);
1427 
1428  int connect2ffn = -1;
1429  if (ffn_next_ffn && sfn_next_ffn)
1430  {
1431  double fn2f = (coords_[ffn_[current_index_]] - coords_[ffn_[next_index]]).squaredNorm ();
1432  double sn2f = (coords_[ffn_[current_index_]] - coords_[sfn_[next_index]]).squaredNorm ();
1433  if (fn2f < sn2f) connect2ffn = 0;
1434  else connect2ffn = 1;
1435  }
1436  else if (ffn_next_ffn) connect2ffn = 0;
1437  else if (sfn_next_ffn) connect2ffn = 1;
1438 
1439  switch (connect2ffn)
1440  {
1441  case 0: // ffn[next]
1442  {
1443  addTriangle (next_index, ffn_[current_index_], ffn_[next_index], polygons);
1444  neighbor_update = ffn_[next_index];
1445 
1446  /* ffn[next_index] */
1447  if ((ffn_[ffn_[next_index]] == ffn_[current_index_]) || (sfn_[ffn_[next_index]] == ffn_[current_index_]))
1448  {
1449  state_[ffn_[next_index]] = COMPLETED;
1450  }
1451  else if (ffn_[ffn_[next_index]] == next_index)
1452  {
1453  ffn_[ffn_[next_index]] = ffn_[current_index_];
1454  }
1455  else if (sfn_[ffn_[next_index]] == next_index)
1456  {
1457  sfn_[ffn_[next_index]] = ffn_[current_index_];
1458  }
1459 
1460  ffn_[next_index] = current_index_;
1461 
1462  break;
1463  }
1464  case 1: // sfn[next]
1465  {
1466  addTriangle (next_index, ffn_[current_index_], sfn_[next_index], polygons);
1467  neighbor_update = sfn_[next_index];
1468 
1469  /* sfn[next_index] */
1470  if ((ffn_[sfn_[next_index]] == ffn_[current_index_]) || (sfn_[sfn_[next_index]] == ffn_[current_index_]))
1471  {
1472  state_[sfn_[next_index]] = COMPLETED;
1473  }
1474  else if (ffn_[sfn_[next_index]] == next_index)
1475  {
1476  ffn_[sfn_[next_index]] = ffn_[current_index_];
1477  }
1478  else if (sfn_[sfn_[next_index]] == next_index)
1479  {
1480  sfn_[sfn_[next_index]] = ffn_[current_index_];
1481  }
1482 
1483  sfn_[next_index] = current_index_;
1484 
1485  break;
1486  }
1487  default:;
1488  }
1489  }
1490  }
1491 
1492  /* updating ffn */
1493  if ((ffn_[ffn_[current_index_]] == neighbor_update) || (sfn_[ffn_[current_index_]] == neighbor_update))
1494  {
1495  state_[ffn_[current_index_]] = COMPLETED;
1496  }
1497  else if (ffn_[ffn_[current_index_]] == current_index_)
1498  {
1499  ffn_[ffn_[current_index_]] = neighbor_update;
1500  }
1501  else if (sfn_[ffn_[current_index_]] == current_index_)
1502  {
1503  sfn_[ffn_[current_index_]] = neighbor_update;
1504  }
1505 
1506  /* updating current */
1507  ffn_[current_index_] = prev_index;
1508 
1509  break;
1510  }
1511  case 3://next2s:
1512  {
1513  addTriangle (current_index_, sfn_[current_index_], next_index, polygons);
1514  auto neighbor_update = next_index;
1515 
1516  /* updating next_index */
1517  if (state_[next_index] <= FREE)
1518  {
1519  state_[next_index] = FRINGE;
1520  ffn_[next_index] = current_index_;
1521  sfn_[next_index] = sfn_[current_index_];
1522  }
1523  else
1524  {
1525  if (ffn_[next_index] == R_)
1526  {
1527  changed_2nd_fn_ = true;
1528  ffn_[next_index] = sfn_[current_index_];
1529  }
1530  else if (sfn_[next_index] == R_)
1531  {
1532  changed_2nd_fn_ = true;
1533  sfn_[next_index] = sfn_[current_index_];
1534  }
1535  else if (next_index == R_)
1536  {
1537  new2boundary_ = sfn_[current_index_];
1538  if (next_next_index == new2boundary_)
1539  already_connected_ = true;
1540  }
1541  else if (ffn_[next_index] == next_next_index)
1542  {
1543  already_connected_ = true;
1544  ffn_[next_index] = sfn_[current_index_];
1545  }
1546  else if (sfn_[next_index] == next_next_index)
1547  {
1548  already_connected_ = true;
1549  sfn_[next_index] = sfn_[current_index_];
1550  }
1551  else
1552  {
1553  tmp_ = coords_[ffn_[next_index]] - proj_qp_;
1554  uvn_next_ffn_[0] = tmp_.dot(u_);
1555  uvn_next_ffn_[1] = tmp_.dot(v_);
1556  tmp_ = coords_[sfn_[next_index]] - proj_qp_;
1557  uvn_next_sfn_[0] = tmp_.dot(u_);
1558  uvn_next_sfn_[1] = tmp_.dot(v_);
1559 
1560  bool ffn_next_sfn = isVisible(uvn_next_ffn_, uvn_next, uvn_current, uvn_sfn_) && isVisible(uvn_next_ffn_, uvn_next, uvn_next_sfn_, uvn_sfn_);
1561  bool sfn_next_sfn = isVisible(uvn_next_sfn_, uvn_next, uvn_current, uvn_sfn_) && isVisible(uvn_next_sfn_, uvn_next, uvn_next_ffn_, uvn_sfn_);
1562 
1563  int connect2sfn = -1;
1564  if (ffn_next_sfn && sfn_next_sfn)
1565  {
1566  double fn2s = (coords_[sfn_[current_index_]] - coords_[ffn_[next_index]]).squaredNorm ();
1567  double sn2s = (coords_[sfn_[current_index_]] - coords_[sfn_[next_index]]).squaredNorm ();
1568  if (fn2s < sn2s) connect2sfn = 0;
1569  else connect2sfn = 1;
1570  }
1571  else if (ffn_next_sfn) connect2sfn = 0;
1572  else if (sfn_next_sfn) connect2sfn = 1;
1573 
1574  switch (connect2sfn)
1575  {
1576  case 0: // ffn[next]
1577  {
1578  addTriangle (next_index, sfn_[current_index_], ffn_[next_index], polygons);
1579  neighbor_update = ffn_[next_index];
1580 
1581  /* ffn[next_index] */
1582  if ((ffn_[ffn_[next_index]] == sfn_[current_index_]) || (sfn_[ffn_[next_index]] == sfn_[current_index_]))
1583  {
1584  state_[ffn_[next_index]] = COMPLETED;
1585  }
1586  else if (ffn_[ffn_[next_index]] == next_index)
1587  {
1588  ffn_[ffn_[next_index]] = sfn_[current_index_];
1589  }
1590  else if (sfn_[ffn_[next_index]] == next_index)
1591  {
1592  sfn_[ffn_[next_index]] = sfn_[current_index_];
1593  }
1594 
1595  ffn_[next_index] = current_index_;
1596 
1597  break;
1598  }
1599  case 1: // sfn[next]
1600  {
1601  addTriangle (next_index, sfn_[current_index_], sfn_[next_index], polygons);
1602  neighbor_update = sfn_[next_index];
1603 
1604  /* sfn[next_index] */
1605  if ((ffn_[sfn_[next_index]] == sfn_[current_index_]) || (sfn_[sfn_[next_index]] == sfn_[current_index_]))
1606  {
1607  state_[sfn_[next_index]] = COMPLETED;
1608  }
1609  else if (ffn_[sfn_[next_index]] == next_index)
1610  {
1611  ffn_[sfn_[next_index]] = sfn_[current_index_];
1612  }
1613  else if (sfn_[sfn_[next_index]] == next_index)
1614  {
1615  sfn_[sfn_[next_index]] = sfn_[current_index_];
1616  }
1617 
1618  sfn_[next_index] = current_index_;
1619 
1620  break;
1621  }
1622  default:;
1623  }
1624  }
1625  }
1626 
1627  /* updating sfn */
1628  if ((ffn_[sfn_[current_index_]] == neighbor_update) || (sfn_[sfn_[current_index_]] == neighbor_update))
1629  {
1630  state_[sfn_[current_index_]] = COMPLETED;
1631  }
1632  else if (ffn_[sfn_[current_index_]] == current_index_)
1633  {
1634  ffn_[sfn_[current_index_]] = neighbor_update;
1635  }
1636  else if (sfn_[sfn_[current_index_]] == current_index_)
1637  {
1638  sfn_[sfn_[current_index_]] = neighbor_update;
1639  }
1640 
1641  sfn_[current_index_] = prev_index;
1642 
1643  break;
1644  }
1645  default:;
1646  }
1647  }
1648  }
1649  }
1650 }
1651 
1652 /////////////////////////////////////////////////////////////////////////////////////////////
1653 template <typename PointInT> std::vector<std::vector<std::size_t> >
1654 pcl::GreedyProjectionTriangulation<PointInT>::getTriangleList (const pcl::PolygonMesh &input)
1655 {
1656  std::vector<std::vector<std::size_t> > triangleList (input.cloud.width * input.cloud.height);
1657 
1658  for (std::size_t i=0; i < input.polygons.size (); ++i)
1659  for (std::size_t j=0; j < input.polygons[i].vertices.size (); ++j)
1660  triangleList[input.polygons[i].vertices[j]].push_back (i);
1661  return (triangleList);
1662 }
1663 
1664 #define PCL_INSTANTIATE_GreedyProjectionTriangulation(T) \
1665  template class PCL_EXPORTS pcl::GreedyProjectionTriangulation<T>;
1666 
1667 #endif // PCL_SURFACE_IMPL_GP3_H_
1668 
1669 
pcl::GreedyProjectionTriangulation
GreedyProjectionTriangulation is an implementation of a greedy triangulation algorithm for 3D points ...
Definition: gp3.h:132
pcl::isVisible
bool isVisible(const Eigen::Vector2f &X, const Eigen::Vector2f &S1, const Eigen::Vector2f &S2, const Eigen::Vector2f &R=Eigen::Vector2f::Zero())
Returns if a point X is visible from point R (or the origin) when taking into account the segment bet...
Definition: gp3.h:64
pcl::gpu::cp
int cp(int from, int to)
Returns field copy operation code.
Definition: repacks.hpp:54
pcl::index_t
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
Definition: types.h:112
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
M_PI
#define M_PI
Definition: pcl_macros.h:203
pcl::PCLPointCloud2::data
std::vector< std::uint8_t > data
Definition: PCLPointCloud2.h:30
pcl::PolygonMesh::polygons
std::vector< ::pcl::Vertices > polygons
Definition: PolygonMesh.h:22
pcl::PolygonMesh::cloud
::pcl::PCLPointCloud2 cloud
Definition: PolygonMesh.h:20