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