Point Cloud Library (PCL) 1.15.0
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
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;
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 }
913
914 if (gaps[*it])
915 if (gaps[*(it-1)])
916 {
917 if (is_current_free_)
918 state_[current_index_] = NONE; /// TODO: document!
919 }
920
921 else // (gaps[*it]) && ^(gaps[*(it-1)])
922 {
923 addTriangle (current_index_, angles_[*(it-1)].index, R_, polygons);
924 addFringePoint (current_index_, R_);
925 new2boundary_ = current_index_;
926 if (!already_connected_)
927 connectPoint (polygons, angles_[*(it-1)].index, R_,
928 angles_[*(it+1)].index,
929 uvn_nn[angles_[*it].nnIndex], uvn_nn[angles_[*(it-1)].nnIndex], uvn_nn_qp_zero);
930 else already_connected_ = false;
931 if (ffn_[R_] == angles_[*(angleIdx.begin())].index)
932 {
933 ffn_[R_] = new2boundary_;
934 }
935 else if (sfn_[R_] == angles_[*(angleIdx.begin())].index)
936 {
937 sfn_[R_] = new2boundary_;
938 }
939 }
940
941 else // ^(gaps[*it])
942 if (gaps[*(it-1)])
943 {
944 addFringePoint (current_index_, R_);
945 new2boundary_ = current_index_;
946 if (!already_connected_) connectPoint (polygons, R_, angles_[*(it+1)].index,
947 (it+2) == angleIdx.end() ? -1 : angles_[*(it+2)].index,
948 uvn_nn[angles_[*it].nnIndex], uvn_nn_qp_zero,
949 uvn_nn[angles_[*(it+1)].nnIndex]);
950 else already_connected_ = false;
951 if (ffn_[R_] == angles_[*(angleIdx.end()-1)].index)
952 {
953 ffn_[R_] = new2boundary_;
954 }
955 else if (sfn_[R_] == angles_[*(angleIdx.end()-1)].index)
956 {
957 sfn_[R_] = new2boundary_;
958 }
959 }
960
961 else // ^(gaps[*it]) && ^(gaps[*(it-1)])
962 {
963 addTriangle (current_index_, angles_[*(it-1)].index, R_, polygons);
964 addFringePoint (current_index_, R_);
965 if (!already_connected_) connectPoint (polygons, angles_[*(it-1)].index, angles_[*(it+1)].index,
966 (it+2) == angleIdx.end() ? -1 : gaps[*(it+1)] ? R_ : angles_[*(it+2)].index,
967 uvn_nn[angles_[*it].nnIndex],
968 uvn_nn[angles_[*(it-1)].nnIndex],
969 uvn_nn[angles_[*(it+1)].nnIndex]);
970 else already_connected_ = false;
971 }
972 }
973
974 // Finishing up R_
975 if (ffn_[R_] == sfn_[R_])
976 {
977 state_[R_] = COMPLETED;
978 }
979 if (!gaps[*(angleIdx.end()-2)])
980 {
981 addTriangle (angles_[*(angleIdx.end()-2)].index, angles_[*(angleIdx.end()-1)].index, R_, polygons);
982 addFringePoint (angles_[*(angleIdx.end()-2)].index, R_);
983 if (R_ == ffn_[angles_[*(angleIdx.end()-1)].index])
984 {
985 if (angles_[*(angleIdx.end()-2)].index == sfn_[angles_[*(angleIdx.end()-1)].index])
986 {
987 state_[angles_[*(angleIdx.end()-1)].index] = COMPLETED;
988 }
989 else
990 {
991 ffn_[angles_[*(angleIdx.end()-1)].index] = angles_[*(angleIdx.end()-2)].index;
992 }
993 }
994 else if (R_ == sfn_[angles_[*(angleIdx.end()-1)].index])
995 {
996 if (angles_[*(angleIdx.end()-2)].index == ffn_[angles_[*(angleIdx.end()-1)].index])
997 {
998 state_[angles_[*(angleIdx.end()-1)].index] = COMPLETED;
999 }
1000 else
1001 {
1002 sfn_[angles_[*(angleIdx.end()-1)].index] = angles_[*(angleIdx.end()-2)].index;
1003 }
1004 }
1005 }
1006 if (!gaps[*(angleIdx.begin())])
1007 {
1008 if (R_ == ffn_[angles_[*(angleIdx.begin())].index])
1009 {
1010 if (angles_[*(angleIdx.begin()+1)].index == sfn_[angles_[*(angleIdx.begin())].index])
1011 {
1012 state_[angles_[*(angleIdx.begin())].index] = COMPLETED;
1013 }
1014 else
1015 {
1016 ffn_[angles_[*(angleIdx.begin())].index] = angles_[*(angleIdx.begin()+1)].index;
1017 }
1018 }
1019 else if (R_ == sfn_[angles_[*(angleIdx.begin())].index])
1020 {
1021 if (angles_[*(angleIdx.begin()+1)].index == ffn_[angles_[*(angleIdx.begin())].index])
1022 {
1023 state_[angles_[*(angleIdx.begin())].index] = COMPLETED;
1024 }
1025 else
1026 {
1027 sfn_[angles_[*(angleIdx.begin())].index] = angles_[*(angleIdx.begin()+1)].index;
1028 }
1029 }
1030 }
1031 }
1032 }
1033 PCL_DEBUG ("Number of triangles: %lu\n", polygons.size());
1034 PCL_DEBUG ("Number of unconnected parts: %d\n", nr_parts);
1035 if (increase_nnn4fn > 0)
1036 PCL_WARN ("Number of neighborhood size increase requests for fringe neighbors: %d\n", increase_nnn4fn);
1037 if (increase_nnn4s > 0)
1038 PCL_WARN ("Number of neighborhood size increase requests for source: %d\n", increase_nnn4s);
1039 if (increase_dist > 0)
1040 PCL_WARN ("Number of automatic maximum distance increases: %d\n", increase_dist);
1041
1042 // sorting and removing doubles from fringe queue
1043 std::sort (fringe_queue_.begin (), fringe_queue_.end ());
1044 fringe_queue_.erase (std::unique (fringe_queue_.begin (), fringe_queue_.end ()), fringe_queue_.end ());
1045 PCL_DEBUG ("Number of processed points: %lu / %lu\n", fringe_queue_.size(), indices_->size ());
1046 return (true);
1047}
1048
1049/////////////////////////////////////////////////////////////////////////////////////////////
1050template <typename PointInT> void
1051pcl::GreedyProjectionTriangulation<PointInT>::closeTriangle (std::vector<pcl::Vertices> &polygons)
1052{
1053 state_[R_] = COMPLETED;
1054 addTriangle (angles_[0].index, angles_[1].index, R_, polygons);
1055 for (int aIdx=0; aIdx<2; aIdx++)
1056 {
1057 if (ffn_[angles_[aIdx].index] == R_)
1058 {
1059 if (sfn_[angles_[aIdx].index] == angles_[(aIdx+1)%2].index)
1060 {
1061 state_[angles_[aIdx].index] = COMPLETED;
1062 }
1063 else
1064 {
1065 ffn_[angles_[aIdx].index] = angles_[(aIdx+1)%2].index;
1066 }
1067 }
1068 else if (sfn_[angles_[aIdx].index] == R_)
1069 {
1070 if (ffn_[angles_[aIdx].index] == angles_[(aIdx+1)%2].index)
1071 {
1072 state_[angles_[aIdx].index] = COMPLETED;
1073 }
1074 else
1075 {
1076 sfn_[angles_[aIdx].index] = angles_[(aIdx+1)%2].index;
1077 }
1078 }
1079 }
1080}
1081
1082/////////////////////////////////////////////////////////////////////////////////////////////
1083template <typename PointInT> void
1085 std::vector<pcl::Vertices> &polygons,
1086 const pcl::index_t prev_index, const pcl::index_t next_index, const pcl::index_t next_next_index,
1087 const Eigen::Vector2f &uvn_current,
1088 const Eigen::Vector2f &uvn_prev,
1089 const Eigen::Vector2f &uvn_next)
1090{
1091 if (is_current_free_)
1092 {
1093 ffn_[current_index_] = prev_index;
1094 sfn_[current_index_] = next_index;
1095 }
1096 else
1097 {
1098 if ((prev_is_ffn_ && next_is_sfn_) || (prev_is_sfn_ && next_is_ffn_))
1099 state_[current_index_] = COMPLETED;
1100 else if (prev_is_ffn_ && !next_is_sfn_)
1101 ffn_[current_index_] = next_index;
1102 else if (next_is_ffn_ && !prev_is_sfn_)
1103 ffn_[current_index_] = prev_index;
1104 else if (prev_is_sfn_ && !next_is_ffn_)
1105 sfn_[current_index_] = next_index;
1106 else if (next_is_sfn_ && !prev_is_ffn_)
1107 sfn_[current_index_] = prev_index;
1108 else
1109 {
1110 bool found_triangle = false;
1111 if ((prev_index != R_) && ((ffn_[current_index_] == ffn_[prev_index]) || (ffn_[current_index_] == sfn_[prev_index])))
1112 {
1113 found_triangle = true;
1114 addTriangle (current_index_, ffn_[current_index_], prev_index, polygons);
1115 state_[prev_index] = COMPLETED;
1116 state_[ffn_[current_index_]] = COMPLETED;
1117 ffn_[current_index_] = next_index;
1118 }
1119 else if ((prev_index != R_) && ((sfn_[current_index_] == ffn_[prev_index]) || (sfn_[current_index_] == sfn_[prev_index])))
1120 {
1121 found_triangle = true;
1122 addTriangle (current_index_, sfn_[current_index_], prev_index, polygons);
1123 state_[prev_index] = COMPLETED;
1124 state_[sfn_[current_index_]] = COMPLETED;
1125 sfn_[current_index_] = next_index;
1126 }
1127 else if (state_[next_index] > FREE)
1128 {
1129 if ((ffn_[current_index_] == ffn_[next_index]) || (ffn_[current_index_] == sfn_[next_index]))
1130 {
1131 found_triangle = true;
1132 addTriangle (current_index_, ffn_[current_index_], next_index, polygons);
1133
1134 if (ffn_[current_index_] == ffn_[next_index])
1135 {
1136 ffn_[next_index] = current_index_;
1137 }
1138 else
1139 {
1140 sfn_[next_index] = current_index_;
1141 }
1142 state_[ffn_[current_index_]] = COMPLETED;
1143 ffn_[current_index_] = prev_index;
1144 }
1145 else if ((sfn_[current_index_] == ffn_[next_index]) || (sfn_[current_index_] == sfn_[next_index]))
1146 {
1147 found_triangle = true;
1148 addTriangle (current_index_, sfn_[current_index_], next_index, polygons);
1149
1150 if (sfn_[current_index_] == ffn_[next_index])
1151 {
1152 ffn_[next_index] = current_index_;
1153 }
1154 else
1155 {
1156 sfn_[next_index] = current_index_;
1157 }
1158 state_[sfn_[current_index_]] = COMPLETED;
1159 sfn_[current_index_] = prev_index;
1160 }
1161 }
1162
1163 if (found_triangle)
1164 {
1165 }
1166 else
1167 {
1168 tmp_ = coords_[ffn_[current_index_]] - proj_qp_;
1169 uvn_ffn_[0] = tmp_.dot(u_);
1170 uvn_ffn_[1] = tmp_.dot(v_);
1171 tmp_ = coords_[sfn_[current_index_]] - proj_qp_;
1172 uvn_sfn_[0] = tmp_.dot(u_);
1173 uvn_sfn_[1] = tmp_.dot(v_);
1174 bool prev_ffn = isVisible(uvn_prev, uvn_next, uvn_current, uvn_ffn_) && isVisible(uvn_prev, uvn_sfn_, uvn_current, uvn_ffn_);
1175 bool prev_sfn = isVisible(uvn_prev, uvn_next, uvn_current, uvn_sfn_) && isVisible(uvn_prev, uvn_ffn_, uvn_current, uvn_sfn_);
1176 bool next_ffn = isVisible(uvn_next, uvn_prev, uvn_current, uvn_ffn_) && isVisible(uvn_next, uvn_sfn_, uvn_current, uvn_ffn_);
1177 bool next_sfn = isVisible(uvn_next, uvn_prev, uvn_current, uvn_sfn_) && isVisible(uvn_next, uvn_ffn_, uvn_current, uvn_sfn_);
1178 int min_dist = -1;
1179 if (prev_ffn && next_sfn && prev_sfn && next_ffn)
1180 {
1181 /* should be never the case */
1182 double prev2f = (coords_[ffn_[current_index_]] - coords_[prev_index]).squaredNorm ();
1183 double next2s = (coords_[sfn_[current_index_]] - coords_[next_index]).squaredNorm ();
1184 double prev2s = (coords_[sfn_[current_index_]] - coords_[prev_index]).squaredNorm ();
1185 double next2f = (coords_[ffn_[current_index_]] - coords_[next_index]).squaredNorm ();
1186 if (prev2f < prev2s)
1187 {
1188 if (prev2f < next2f)
1189 {
1190 if (prev2f < next2s)
1191 min_dist = 0;
1192 else
1193 min_dist = 3;
1194 }
1195 else
1196 {
1197 if (next2f < next2s)
1198 min_dist = 2;
1199 else
1200 min_dist = 3;
1201 }
1202 }
1203 else
1204 {
1205 if (prev2s < next2f)
1206 {
1207 if (prev2s < next2s)
1208 min_dist = 1;
1209 else
1210 min_dist = 3;
1211 }
1212 else
1213 {
1214 if (next2f < next2s)
1215 min_dist = 2;
1216 else
1217 min_dist = 3;
1218 }
1219 }
1220 }
1221 else if (prev_ffn && next_sfn)
1222 {
1223 /* a clear case */
1224 double prev2f = (coords_[ffn_[current_index_]] - coords_[prev_index]).squaredNorm ();
1225 double next2s = (coords_[sfn_[current_index_]] - coords_[next_index]).squaredNorm ();
1226 if (prev2f < next2s)
1227 min_dist = 0;
1228 else
1229 min_dist = 3;
1230 }
1231 else if (prev_sfn && next_ffn)
1232 {
1233 /* a clear case */
1234 double prev2s = (coords_[sfn_[current_index_]] - coords_[prev_index]).squaredNorm ();
1235 double next2f = (coords_[ffn_[current_index_]] - coords_[next_index]).squaredNorm ();
1236 if (prev2s < next2f)
1237 min_dist = 1;
1238 else
1239 min_dist = 2;
1240 }
1241 /* straightforward cases */
1242 else if (prev_ffn && !next_sfn && !prev_sfn && !next_ffn)
1243 min_dist = 0;
1244 else if (!prev_ffn && !next_sfn && prev_sfn && !next_ffn)
1245 min_dist = 1;
1246 else if (!prev_ffn && !next_sfn && !prev_sfn && next_ffn)
1247 min_dist = 2;
1248 else if (!prev_ffn && next_sfn && !prev_sfn && !next_ffn)
1249 min_dist = 3;
1250 /* messed up cases */
1251 else if (prev_ffn)
1252 {
1253 double prev2f = (coords_[ffn_[current_index_]] - coords_[prev_index]).squaredNorm ();
1254 if (prev_sfn)
1255 {
1256 double prev2s = (coords_[sfn_[current_index_]] - coords_[prev_index]).squaredNorm ();
1257 if (prev2s < prev2f)
1258 min_dist = 1;
1259 else
1260 min_dist = 0;
1261 }
1262 else if (next_ffn)
1263 {
1264 double next2f = (coords_[ffn_[current_index_]] - coords_[next_index]).squaredNorm ();
1265 if (next2f < prev2f)
1266 min_dist = 2;
1267 else
1268 min_dist = 0;
1269 }
1270 }
1271 else if (next_sfn)
1272 {
1273 double next2s = (coords_[sfn_[current_index_]] - coords_[next_index]).squaredNorm ();
1274 if (prev_sfn)
1275 {
1276 double prev2s = (coords_[sfn_[current_index_]] - coords_[prev_index]).squaredNorm ();
1277 if (prev2s < next2s)
1278 min_dist = 1;
1279 else
1280 min_dist = 3;
1281 }
1282 else if (next_ffn)
1283 {
1284 double next2f = (coords_[ffn_[current_index_]] - coords_[next_index]).squaredNorm ();
1285 if (next2f < next2s)
1286 min_dist = 2;
1287 else
1288 min_dist = 3;
1289 }
1290 }
1291 switch (min_dist)
1292 {
1293 case 0://prev2f:
1294 {
1295 addTriangle (current_index_, ffn_[current_index_], prev_index, polygons);
1296
1297 /* updating prev_index */
1298 if (ffn_[prev_index] == current_index_)
1299 {
1300 ffn_[prev_index] = ffn_[current_index_];
1301 }
1302 else if (sfn_[prev_index] == current_index_)
1303 {
1304 sfn_[prev_index] = ffn_[current_index_];
1305 }
1306 else if (ffn_[prev_index] == R_)
1307 {
1308 changed_1st_fn_ = true;
1309 ffn_[prev_index] = ffn_[current_index_];
1310 }
1311 else if (sfn_[prev_index] == R_)
1312 {
1313 changed_1st_fn_ = true;
1314 sfn_[prev_index] = ffn_[current_index_];
1315 }
1316 else if (prev_index == R_)
1317 {
1318 new2boundary_ = ffn_[current_index_];
1319 }
1320
1321 /* updating ffn */
1322 if (ffn_[ffn_[current_index_]] == current_index_)
1323 {
1324 ffn_[ffn_[current_index_]] = prev_index;
1325 }
1326 else if (sfn_[ffn_[current_index_]] == current_index_)
1327 {
1328 sfn_[ffn_[current_index_]] = prev_index;
1329 }
1330
1331 /* updating current */
1332 ffn_[current_index_] = next_index;
1333
1334 break;
1335 }
1336 case 1://prev2s:
1337 {
1338 addTriangle (current_index_, sfn_[current_index_], prev_index, polygons);
1339
1340 /* updating prev_index */
1341 if (ffn_[prev_index] == current_index_)
1342 {
1343 ffn_[prev_index] = sfn_[current_index_];
1344 }
1345 else if (sfn_[prev_index] == current_index_)
1346 {
1347 sfn_[prev_index] = sfn_[current_index_];
1348 }
1349 else if (ffn_[prev_index] == R_)
1350 {
1351 changed_1st_fn_ = true;
1352 ffn_[prev_index] = sfn_[current_index_];
1353 }
1354 else if (sfn_[prev_index] == R_)
1355 {
1356 changed_1st_fn_ = true;
1357 sfn_[prev_index] = sfn_[current_index_];
1358 }
1359 else if (prev_index == R_)
1360 {
1361 new2boundary_ = sfn_[current_index_];
1362 }
1363
1364 /* updating sfn */
1365 if (ffn_[sfn_[current_index_]] == current_index_)
1366 {
1367 ffn_[sfn_[current_index_]] = prev_index;
1368 }
1369 else if (sfn_[sfn_[current_index_]] == current_index_)
1370 {
1371 sfn_[sfn_[current_index_]] = prev_index;
1372 }
1373
1374 /* updating current */
1375 sfn_[current_index_] = next_index;
1376
1377 break;
1378 }
1379 case 2://next2f:
1380 {
1381 addTriangle (current_index_, ffn_[current_index_], next_index, polygons);
1382 auto neighbor_update = next_index;
1383
1384 /* updating next_index */
1385 if (state_[next_index] <= FREE)
1386 {
1387 state_[next_index] = FRINGE;
1388 ffn_[next_index] = current_index_;
1389 sfn_[next_index] = ffn_[current_index_];
1390 }
1391 else
1392 {
1393 if (ffn_[next_index] == R_)
1394 {
1395 changed_2nd_fn_ = true;
1396 ffn_[next_index] = ffn_[current_index_];
1397 }
1398 else if (sfn_[next_index] == R_)
1399 {
1400 changed_2nd_fn_ = true;
1401 sfn_[next_index] = ffn_[current_index_];
1402 }
1403 else if (next_index == R_)
1404 {
1405 new2boundary_ = ffn_[current_index_];
1406 if (next_next_index == new2boundary_)
1407 already_connected_ = true;
1408 }
1409 else if (ffn_[next_index] == next_next_index)
1410 {
1411 already_connected_ = true;
1412 ffn_[next_index] = ffn_[current_index_];
1413 }
1414 else if (sfn_[next_index] == next_next_index)
1415 {
1416 already_connected_ = true;
1417 sfn_[next_index] = ffn_[current_index_];
1418 }
1419 else
1420 {
1421 tmp_ = coords_[ffn_[next_index]] - proj_qp_;
1422 uvn_next_ffn_[0] = tmp_.dot(u_);
1423 uvn_next_ffn_[1] = tmp_.dot(v_);
1424 tmp_ = coords_[sfn_[next_index]] - proj_qp_;
1425 uvn_next_sfn_[0] = tmp_.dot(u_);
1426 uvn_next_sfn_[1] = tmp_.dot(v_);
1427
1428 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_);
1429 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_);
1430
1431 int connect2ffn = -1;
1432 if (ffn_next_ffn && sfn_next_ffn)
1433 {
1434 double fn2f = (coords_[ffn_[current_index_]] - coords_[ffn_[next_index]]).squaredNorm ();
1435 double sn2f = (coords_[ffn_[current_index_]] - coords_[sfn_[next_index]]).squaredNorm ();
1436 if (fn2f < sn2f) connect2ffn = 0;
1437 else connect2ffn = 1;
1438 }
1439 else if (ffn_next_ffn) connect2ffn = 0;
1440 else if (sfn_next_ffn) connect2ffn = 1;
1441
1442 switch (connect2ffn)
1443 {
1444 case 0: // ffn[next]
1445 {
1446 addTriangle (next_index, ffn_[current_index_], ffn_[next_index], polygons);
1447 neighbor_update = ffn_[next_index];
1448
1449 /* ffn[next_index] */
1450 if ((ffn_[ffn_[next_index]] == ffn_[current_index_]) || (sfn_[ffn_[next_index]] == ffn_[current_index_]))
1451 {
1452 state_[ffn_[next_index]] = COMPLETED;
1453 }
1454 else if (ffn_[ffn_[next_index]] == next_index)
1455 {
1456 ffn_[ffn_[next_index]] = ffn_[current_index_];
1457 }
1458 else if (sfn_[ffn_[next_index]] == next_index)
1459 {
1460 sfn_[ffn_[next_index]] = ffn_[current_index_];
1461 }
1462
1463 ffn_[next_index] = current_index_;
1464
1465 break;
1466 }
1467 case 1: // sfn[next]
1468 {
1469 addTriangle (next_index, ffn_[current_index_], sfn_[next_index], polygons);
1470 neighbor_update = sfn_[next_index];
1471
1472 /* sfn[next_index] */
1473 if ((ffn_[sfn_[next_index]] == ffn_[current_index_]) || (sfn_[sfn_[next_index]] == ffn_[current_index_]))
1474 {
1475 state_[sfn_[next_index]] = COMPLETED;
1476 }
1477 else if (ffn_[sfn_[next_index]] == next_index)
1478 {
1479 ffn_[sfn_[next_index]] = ffn_[current_index_];
1480 }
1481 else if (sfn_[sfn_[next_index]] == next_index)
1482 {
1483 sfn_[sfn_[next_index]] = ffn_[current_index_];
1484 }
1485
1486 sfn_[next_index] = current_index_;
1487
1488 break;
1489 }
1490 default:;
1491 }
1492 }
1493 }
1494
1495 /* updating ffn */
1496 if ((ffn_[ffn_[current_index_]] == neighbor_update) || (sfn_[ffn_[current_index_]] == neighbor_update))
1497 {
1498 state_[ffn_[current_index_]] = COMPLETED;
1499 }
1500 else if (ffn_[ffn_[current_index_]] == current_index_)
1501 {
1502 ffn_[ffn_[current_index_]] = neighbor_update;
1503 }
1504 else if (sfn_[ffn_[current_index_]] == current_index_)
1505 {
1506 sfn_[ffn_[current_index_]] = neighbor_update;
1507 }
1508
1509 /* updating current */
1510 ffn_[current_index_] = prev_index;
1511
1512 break;
1513 }
1514 case 3://next2s:
1515 {
1516 addTriangle (current_index_, sfn_[current_index_], next_index, polygons);
1517 auto neighbor_update = next_index;
1518
1519 /* updating next_index */
1520 if (state_[next_index] <= FREE)
1521 {
1522 state_[next_index] = FRINGE;
1523 ffn_[next_index] = current_index_;
1524 sfn_[next_index] = sfn_[current_index_];
1525 }
1526 else
1527 {
1528 if (ffn_[next_index] == R_)
1529 {
1530 changed_2nd_fn_ = true;
1531 ffn_[next_index] = sfn_[current_index_];
1532 }
1533 else if (sfn_[next_index] == R_)
1534 {
1535 changed_2nd_fn_ = true;
1536 sfn_[next_index] = sfn_[current_index_];
1537 }
1538 else if (next_index == R_)
1539 {
1540 new2boundary_ = sfn_[current_index_];
1541 if (next_next_index == new2boundary_)
1542 already_connected_ = true;
1543 }
1544 else if (ffn_[next_index] == next_next_index)
1545 {
1546 already_connected_ = true;
1547 ffn_[next_index] = sfn_[current_index_];
1548 }
1549 else if (sfn_[next_index] == next_next_index)
1550 {
1551 already_connected_ = true;
1552 sfn_[next_index] = sfn_[current_index_];
1553 }
1554 else
1555 {
1556 tmp_ = coords_[ffn_[next_index]] - proj_qp_;
1557 uvn_next_ffn_[0] = tmp_.dot(u_);
1558 uvn_next_ffn_[1] = tmp_.dot(v_);
1559 tmp_ = coords_[sfn_[next_index]] - proj_qp_;
1560 uvn_next_sfn_[0] = tmp_.dot(u_);
1561 uvn_next_sfn_[1] = tmp_.dot(v_);
1562
1563 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_);
1564 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_);
1565
1566 int connect2sfn = -1;
1567 if (ffn_next_sfn && sfn_next_sfn)
1568 {
1569 double fn2s = (coords_[sfn_[current_index_]] - coords_[ffn_[next_index]]).squaredNorm ();
1570 double sn2s = (coords_[sfn_[current_index_]] - coords_[sfn_[next_index]]).squaredNorm ();
1571 if (fn2s < sn2s) connect2sfn = 0;
1572 else connect2sfn = 1;
1573 }
1574 else if (ffn_next_sfn) connect2sfn = 0;
1575 else if (sfn_next_sfn) connect2sfn = 1;
1576
1577 switch (connect2sfn)
1578 {
1579 case 0: // ffn[next]
1580 {
1581 addTriangle (next_index, sfn_[current_index_], ffn_[next_index], polygons);
1582 neighbor_update = ffn_[next_index];
1583
1584 /* ffn[next_index] */
1585 if ((ffn_[ffn_[next_index]] == sfn_[current_index_]) || (sfn_[ffn_[next_index]] == sfn_[current_index_]))
1586 {
1587 state_[ffn_[next_index]] = COMPLETED;
1588 }
1589 else if (ffn_[ffn_[next_index]] == next_index)
1590 {
1591 ffn_[ffn_[next_index]] = sfn_[current_index_];
1592 }
1593 else if (sfn_[ffn_[next_index]] == next_index)
1594 {
1595 sfn_[ffn_[next_index]] = sfn_[current_index_];
1596 }
1597
1598 ffn_[next_index] = current_index_;
1599
1600 break;
1601 }
1602 case 1: // sfn[next]
1603 {
1604 addTriangle (next_index, sfn_[current_index_], sfn_[next_index], polygons);
1605 neighbor_update = sfn_[next_index];
1606
1607 /* sfn[next_index] */
1608 if ((ffn_[sfn_[next_index]] == sfn_[current_index_]) || (sfn_[sfn_[next_index]] == sfn_[current_index_]))
1609 {
1610 state_[sfn_[next_index]] = COMPLETED;
1611 }
1612 else if (ffn_[sfn_[next_index]] == next_index)
1613 {
1614 ffn_[sfn_[next_index]] = sfn_[current_index_];
1615 }
1616 else if (sfn_[sfn_[next_index]] == next_index)
1617 {
1618 sfn_[sfn_[next_index]] = sfn_[current_index_];
1619 }
1620
1621 sfn_[next_index] = current_index_;
1622
1623 break;
1624 }
1625 default:;
1626 }
1627 }
1628 }
1629
1630 /* updating sfn */
1631 if ((ffn_[sfn_[current_index_]] == neighbor_update) || (sfn_[sfn_[current_index_]] == neighbor_update))
1632 {
1633 state_[sfn_[current_index_]] = COMPLETED;
1634 }
1635 else if (ffn_[sfn_[current_index_]] == current_index_)
1636 {
1637 ffn_[sfn_[current_index_]] = neighbor_update;
1638 }
1639 else if (sfn_[sfn_[current_index_]] == current_index_)
1640 {
1641 sfn_[sfn_[current_index_]] = neighbor_update;
1642 }
1643
1644 sfn_[current_index_] = prev_index;
1645
1646 break;
1647 }
1648 default:;
1649 }
1650 }
1651 }
1652 }
1653}
1654
1655/////////////////////////////////////////////////////////////////////////////////////////////
1656template <typename PointInT> std::vector<std::vector<std::size_t> >
1658{
1659 std::vector<std::vector<std::size_t> > triangleList (input.cloud.width * input.cloud.height);
1660
1661 for (std::size_t i=0; i < input.polygons.size (); ++i)
1662 for (std::size_t j=0; j < input.polygons[i].vertices.size (); ++j)
1663 triangleList[input.polygons[i].vertices[j]].push_back (i);
1664 return (triangleList);
1665}
1666
1667#define PCL_INSTANTIATE_GreedyProjectionTriangulation(T) \
1668 template class PCL_EXPORTS pcl::GreedyProjectionTriangulation<T>;
1669
1670#endif // PCL_SURFACE_IMPL_GP3_H_
1671
1672
GreedyProjectionTriangulation is an implementation of a greedy triangulation algorithm for 3D points ...
Definition gp3.h:132
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:54
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:203
std::vector< std::uint8_t > data
std::vector< ::pcl::Vertices > polygons
Definition PolygonMesh.h:22
::pcl::PCLPointCloud2 cloud
Definition PolygonMesh.h:20