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