Point Cloud Library (PCL) 1.13.0
gp3.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of Willow Garage, Inc. nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *
34 * $Id$
35 *
36 */
37
38#ifndef PCL_SURFACE_IMPL_GP3_H_
39#define PCL_SURFACE_IMPL_GP3_H_
40
41#include <pcl/surface/gp3.h>
42
43/////////////////////////////////////////////////////////////////////////////////////////////
44template <typename PointInT> void
46{
47 output.polygons.clear ();
48 output.polygons.reserve (2 * indices_->size ()); /// NOTE: usually the number of triangles is around twice the number of vertices
49 if (!reconstructPolygons (output.polygons))
50 {
51 PCL_ERROR ("[pcl::%s::performReconstruction] Reconstruction failed. Check parameters: search radius (%f) or mu (%f) before continuing.\n", getClassName ().c_str (), search_radius_, mu_);
52 output.cloud.width = output.cloud.height = 0;
53 output.cloud.data.clear ();
54 return;
55 }
56}
57
58/////////////////////////////////////////////////////////////////////////////////////////////
59template <typename PointInT> void
61{
62 polygons.clear ();
63 polygons.reserve (2 * indices_->size ()); /// NOTE: usually the number of triangles is around twice the number of vertices
64 if (!reconstructPolygons (polygons))
65 {
66 PCL_ERROR ("[pcl::%s::performReconstruction] Reconstruction failed. Check parameters: search radius (%f) or mu (%f) before continuing.\n", getClassName ().c_str (), search_radius_, mu_);
67 return;
68 }
69}
70
71/////////////////////////////////////////////////////////////////////////////////////////////
72template <typename PointInT> bool
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] == UNAVAILABLE) /// 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] == UNAVAILABLE) /// 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 const auto& 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 const auto& 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 auto first_gap_after = angleIdx.end ();
789 auto last_gap_before = angleIdx.begin ();
790 int nr_gaps = 0;
791 for (auto 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 (auto 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 (auto 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 (auto 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 (auto 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/////////////////////////////////////////////////////////////////////////////////////////////
1054template <typename PointInT> void
1055pcl::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/////////////////////////////////////////////////////////////////////////////////////////////
1087template <typename PointInT> void
1089 std::vector<pcl::Vertices> &polygons,
1090 const pcl::index_t prev_index, const pcl::index_t next_index, const pcl::index_t 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 auto 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 auto 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/////////////////////////////////////////////////////////////////////////////////////////////
1660template <typename PointInT> std::vector<std::vector<std::size_t> >
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
GreedyProjectionTriangulation is an implementation of a greedy triangulation algorithm for 3D points ...
Definition: gp3.h:131
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
int cp(int from, int to)
Returns field copy operation code.
Definition: repacks.hpp:56
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
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
#define M_PI
Definition: pcl_macros.h:201
std::vector< std::uint8_t > data
std::vector< ::pcl::Vertices > polygons
Definition: PolygonMesh.h:22
::pcl::PCLPointCloud2 cloud
Definition: PolygonMesh.h:20