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