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