38 #ifndef PCL_SURFACE_IMPL_GP3_H_
39 #define PCL_SURFACE_IMPL_GP3_H_
41 #include <pcl/surface/gp3.h>
44 template <
typename Po
intInT>
void
48 output.
polygons.reserve (2 * indices_->size ());
49 if (!reconstructPolygons (output.
polygons))
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_);
59 template <
typename Po
intInT>
void
63 polygons.reserve (2 * indices_->size ());
64 if (!reconstructPolygons (polygons))
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_);
72 template <
typename Po
intInT>
bool
75 if (search_radius_ <= 0 || mu_ <= 0)
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 ());
86 std::vector<int> nnIdx (nnn_);
87 std::vector<float> sqrDists (nnn_);
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;
99 already_connected_ =
false;
107 part_.resize(indices_->size (), -1);
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 ();
116 if (!input_->is_dense)
119 for (std::vector<int>::const_iterator it = indices_->begin (); it != indices_->end (); ++it)
120 if (!std::isfinite (input_->points[*it].x) ||
121 !std::isfinite (input_->points[*it].y) ||
122 !std::isfinite (input_->points[*it].z))
128 coords_.reserve (indices_->size ());
129 std::vector<int> point2index (input_->points.size (), -1);
130 for (
int cp = 0; cp < static_cast<int> (indices_->size ()); ++
cp)
132 coords_.push_back(input_->points[(*indices_)[
cp]].getVector3fMap());
133 point2index[(*indices_)[
cp]] =
cp;
137 int is_free=0, nr_parts=0, increase_nnn4fn=0, increase_nnn4s=0, increase_dist=0, nr_touched = 0;
139 angles_.resize(nnn_);
140 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > uvn_nn (nnn_);
141 Eigen::Vector2f uvn_s;
144 while (is_free != NONE)
147 if (state_[R_] == FREE)
150 part_[R_] = part_index++;
155 tree_->nearestKSearch (indices_->at (R_), nnn_, nnIdx, sqrDists);
156 double sqr_dist_threshold = (std::min)(sqr_max_edge, sqr_mu * sqrDists[1]);
159 for (
int i = 1; i < nnn_; i++)
163 nnIdx[i] = point2index[nnIdx[i]];
167 const Eigen::Vector3f nc = input_->points[(*indices_)[R_]].getNormalVector3fMap ();
170 v_ = nc.unitOrthogonal ();
174 float dist = nc.dot (coords_[R_]);
175 proj_qp_ = coords_[R_] - dist * nc;
179 std::vector<doubleEdge> doubleEdges;
180 for (
int i = 1; i < nnn_; i++)
183 tmp_ = coords_[nnIdx[i]] - proj_qp_;
184 uvn_nn[i][0] = tmp_.dot(u_);
185 uvn_nn[i][1] = tmp_.dot(v_);
187 angles_[i].angle = std::atan2(uvn_nn[i][1], uvn_nn[i][0]);
189 angles_[i].index = nnIdx[i];
191 (state_[nnIdx[i]] == COMPLETED) || (state_[nnIdx[i]] == BOUNDARY)
192 || (state_[nnIdx[i]] == NONE) || (nnIdx[i] == -1)
193 || (sqrDists[i] > sqr_dist_threshold)
195 angles_[i].visible =
false;
197 angles_[i].visible =
true;
199 if ((state_[nnIdx[i]] == FRINGE) || (state_[nnIdx[i]] == BOUNDARY))
204 tmp_ = coords_[ffn_[nnIdx[i]]] - proj_qp_;
205 e.first[0] = tmp_.dot(u_);
206 e.first[1] = tmp_.dot(v_);
207 tmp_ = coords_[sfn_[nnIdx[i]]] - proj_qp_;
208 e.second[0] = tmp_.dot(u_);
209 e.second[1] = tmp_.dot(v_);
210 doubleEdges.push_back(e);
213 angles_[0].visible =
false;
216 for (
int i = 1; i < nnn_; i++)
217 if ((angles_[i].visible) && (ffn_[R_] != nnIdx[i]) && (sfn_[R_] != nnIdx[i]))
219 bool visibility =
true;
220 for (
int j = 0; j < nr_edge; j++)
222 if (ffn_[nnIdx[doubleEdges[j].index]] != nnIdx[i])
223 visibility =
isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].first, Eigen::Vector2f::Zero());
226 if (sfn_[nnIdx[doubleEdges[j].index]] != nnIdx[i])
227 visibility =
isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].second, Eigen::Vector2f::Zero());
231 angles_[i].visible = visibility;
235 bool not_found =
true;
239 while ((left < nnn_) && ((!angles_[left].visible) || (state_[nnIdx[left]] > FREE))) left++;
245 while ((right < nnn_) && ((!angles_[right].visible) || (state_[nnIdx[right]] > FREE))) right++;
248 if ((coords_[nnIdx[left]] - coords_[nnIdx[right]]).squaredNorm () > sqr_max_edge)
252 addFringePoint (nnIdx[right], R_);
253 addFringePoint (nnIdx[left], nnIdx[right]);
254 addFringePoint (R_, nnIdx[left]);
255 state_[R_] = state_[nnIdx[left]] = state_[nnIdx[right]] = FRINGE;
256 ffn_[R_] = nnIdx[left];
257 sfn_[R_] = nnIdx[right];
258 ffn_[nnIdx[left]] = nnIdx[right];
259 sfn_[nnIdx[left]] = R_;
260 ffn_[nnIdx[right]] = R_;
261 sfn_[nnIdx[right]] = nnIdx[left];
262 addTriangle (R_, nnIdx[left], nnIdx[right], polygons);
275 for (std::size_t temp = 0; temp < indices_->size (); temp++)
277 if (state_[temp] == FREE)
289 int fqSize =
static_cast<int> (fringe_queue_.size ());
290 while ((fqIdx < fqSize) && (state_[fringe_queue_[fqIdx]] != FRINGE))
299 R_ = fringe_queue_[fqIdx];
302 if (ffn_[R_] == sfn_[R_])
304 state_[R_] = COMPLETED;
309 tree_->nearestKSearch (indices_->at (R_), nnn_, nnIdx, sqrDists);
312 for (
int i = 1; i < nnn_; i++)
316 nnIdx[i] = point2index[nnIdx[i]];
320 double sqr_source_dist = (coords_[R_] - coords_[source_[R_]]).squaredNorm ();
321 double sqr_ffn_dist = (coords_[R_] - coords_[ffn_[R_]]).squaredNorm ();
322 double sqr_sfn_dist = (coords_[R_] - coords_[sfn_[R_]]).squaredNorm ();
323 double max_sqr_fn_dist = (std::max)(sqr_ffn_dist, sqr_sfn_dist);
324 double sqr_dist_threshold = (std::min)(sqr_max_edge, sqr_mu * sqrDists[1]);
325 if (max_sqr_fn_dist > sqrDists[nnn_-1])
327 if (0 == increase_nnn4fn)
328 PCL_WARN(
"Not enough neighbors are considered: ffn or sfn out of range! Consider increasing nnn_... Setting R=%d to be BOUNDARY!\n", R_);
330 state_[R_] = BOUNDARY;
333 double max_sqr_fns_dist = (std::max)(sqr_source_dist, max_sqr_fn_dist);
334 if (max_sqr_fns_dist > sqrDists[nnn_-1])
336 if (0 == increase_nnn4s)
337 PCL_WARN(
"Not enough neighbors are considered: source of R=%d is out of range! Consider increasing nnn_...\n", R_);
342 const Eigen::Vector3f nc = input_->points[(*indices_)[R_]].getNormalVector3fMap ();
345 v_ = nc.unitOrthogonal ();
349 float dist = nc.dot (coords_[R_]);
350 proj_qp_ = coords_[R_] - dist * nc;
354 std::vector<doubleEdge> doubleEdges;
355 for (
int i = 1; i < nnn_; i++)
357 tmp_ = coords_[nnIdx[i]] - proj_qp_;
358 uvn_nn[i][0] = tmp_.dot(u_);
359 uvn_nn[i][1] = tmp_.dot(v_);
362 angles_[i].angle = std::atan2(uvn_nn[i][1], uvn_nn[i][0]);
364 angles_[i].index = nnIdx[i];
365 angles_[i].nnIndex = i;
367 (state_[nnIdx[i]] == COMPLETED) || (state_[nnIdx[i]] == BOUNDARY)
368 || (state_[nnIdx[i]] == NONE) || (nnIdx[i] == -1)
369 || (sqrDists[i] > sqr_dist_threshold)
371 angles_[i].visible =
false;
373 angles_[i].visible =
true;
374 if ((ffn_[R_] == nnIdx[i]) || (sfn_[R_] == nnIdx[i]))
375 angles_[i].visible =
true;
376 bool same_side =
true;
377 const Eigen::Vector3f neighbor_normal = input_->points[(*indices_)[nnIdx[i]]].getNormalVector3fMap ();
378 double cosine = nc.dot (neighbor_normal);
379 if (cosine > 1) cosine = 1;
380 if (cosine < -1) cosine = -1;
381 double angle = std::acos (cosine);
382 if ((!consistent_) && (angle > M_PI/2))
383 angle = M_PI - angle;
384 if (angle > eps_angle_)
386 angles_[i].visible =
false;
390 if ((i!=0) && (same_side) && ((state_[nnIdx[i]] == FRINGE) || (state_[nnIdx[i]] == BOUNDARY)))
395 tmp_ = coords_[ffn_[nnIdx[i]]] - proj_qp_;
396 e.first[0] = tmp_.dot(u_);
397 e.first[1] = tmp_.dot(v_);
398 tmp_ = coords_[sfn_[nnIdx[i]]] - proj_qp_;
399 e.second[0] = tmp_.dot(u_);
400 e.second[1] = tmp_.dot(v_);
401 doubleEdges.push_back(e);
403 if ((state_[nnIdx[i]] == FRINGE) && (ffn_[R_] != nnIdx[i]) && (sfn_[R_] != nnIdx[i]))
405 double angle1 = std::atan2(e.first[1] - uvn_nn[i][1], e.first[0] - uvn_nn[i][0]);
406 double angle2 = std::atan2(e.second[1] - uvn_nn[i][1], e.second[0] - uvn_nn[i][0]);
407 double angleMin, angleMax;
418 double angleR = angles_[i].angle + M_PI;
421 if ((source_[nnIdx[i]] == ffn_[nnIdx[i]]) || (source_[nnIdx[i]] == sfn_[nnIdx[i]]))
423 if ((angleMax - angleMin) < M_PI)
425 if ((angleMin < angleR) && (angleR < angleMax))
426 angles_[i].visible =
false;
430 if ((angleR < angleMin) || (angleMax < angleR))
431 angles_[i].visible =
false;
436 tmp_ = coords_[source_[nnIdx[i]]] - proj_qp_;
437 uvn_s[0] = tmp_.dot(u_);
438 uvn_s[1] = tmp_.dot(v_);
439 double angleS = std::atan2(uvn_s[1] - uvn_nn[i][1], uvn_s[0] - uvn_nn[i][0]);
440 if ((angleMin < angleS) && (angleS < angleMax))
442 if ((angleMin < angleR) && (angleR < angleMax))
443 angles_[i].visible =
false;
447 if ((angleR < angleMin) || (angleMax < angleR))
448 angles_[i].visible =
false;
454 angles_[0].visible =
false;
457 for (
int i = 1; i < nnn_; i++)
458 if ((angles_[i].visible) && (ffn_[R_] != nnIdx[i]) && (sfn_[R_] != nnIdx[i]))
460 bool visibility =
true;
461 for (
int j = 0; j < nr_edge; j++)
463 if (doubleEdges[j].index != i)
465 int f = ffn_[nnIdx[doubleEdges[j].index]];
466 if ((f != nnIdx[i]) && (f != R_))
467 visibility =
isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].first, Eigen::Vector2f::Zero());
471 int s = sfn_[nnIdx[doubleEdges[j].index]];
472 if ((s != nnIdx[i]) && (s != R_))
473 visibility =
isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].second, Eigen::Vector2f::Zero());
478 angles_[i].visible = visibility;
482 std::sort (angles_.begin (), angles_.end (), GreedyProjectionTriangulation<PointInT>::nnAngleSortAsc);
485 if (angles_[2].visible ==
false)
487 if ( !( (angles_[0].index == ffn_[R_] && angles_[1].index == sfn_[R_]) || (angles_[0].index == sfn_[R_] && angles_[1].index == ffn_[R_]) ) )
489 state_[R_] = BOUNDARY;
493 if ((source_[R_] == angles_[0].index) || (source_[R_] == angles_[1].index))
494 state_[R_] = BOUNDARY;
497 if (sqr_max_edge < (coords_[ffn_[R_]] - coords_[sfn_[R_]]).squaredNorm ())
499 state_[R_] = BOUNDARY;
503 tmp_ = coords_[source_[R_]] - proj_qp_;
504 uvn_s[0] = tmp_.dot(u_);
505 uvn_s[1] = tmp_.dot(v_);
506 double angleS = std::atan2(uvn_s[1], uvn_s[0]);
507 double dif = angles_[1].angle - angles_[0].angle;
508 if ((angles_[0].angle < angleS) && (angleS < angles_[1].angle))
510 if (dif < 2*M_PI - maximum_angle_)
511 state_[R_] = BOUNDARY;
513 closeTriangle (polygons);
517 if (dif >= maximum_angle_)
518 state_[R_] = BOUNDARY;
520 closeTriangle (polygons);
529 int start = -1, end = -1;
530 for (
int i=0; i<nnn_; i++)
532 if (ffn_[R_] == angles_[i].index)
535 if (sfn_[R_] == angles_[i+1].index)
540 for (i = i+2; i < nnn_; i++)
541 if (sfn_[R_] == angles_[i].index)
547 for (i = i+2; i < nnn_; i++)
548 if (sfn_[R_] == angles_[i].index)
554 if (sfn_[R_] == angles_[i].index)
557 if (ffn_[R_] == angles_[i+1].index)
562 for (i = i+2; i < nnn_; i++)
563 if (ffn_[R_] == angles_[i].index)
569 for (i = i+2; i < nnn_; i++)
570 if (ffn_[R_] == angles_[i].index)
579 if ((start < 0) || (end < 0) || (end == nnn_) || (!angles_[start].visible) || (!angles_[end].visible))
581 state_[R_] = BOUNDARY;
586 int last_visible = end;
587 while ((last_visible+1<nnn_) && (angles_[last_visible+1].visible)) last_visible++;
590 bool need_invert =
false;
591 if ((source_[R_] == ffn_[R_]) || (source_[R_] == sfn_[R_]))
593 if ((angles_[end].angle - angles_[start].angle) < M_PI)
599 for (sourceIdx=0; sourceIdx<nnn_; sourceIdx++)
600 if (angles_[sourceIdx].index == source_[R_])
602 if (sourceIdx == nnn_)
604 int vis_free = NONE, nnCB = NONE;
605 for (
int i = 1; i < nnn_; i++)
608 if ((state_[nnIdx[i]] == COMPLETED) || (state_[nnIdx[i]] == BOUNDARY))
613 if (vis_free != NONE)
618 if (state_[angles_[i].index] <= FREE)
620 if (i <= last_visible)
631 while (angles_[nCB].index != nnIdx[nnCB]) nCB++;
635 if (vis_free != NONE)
637 if ((vis_free < start) || (vis_free > end))
644 if ((nCB == start) || (nCB == end))
646 bool inside_CB =
false;
647 bool outside_CB =
false;
648 for (
int i=0; i<nnn_; i++)
651 ((state_[angles_[i].index] == COMPLETED) || (state_[angles_[i].index] == BOUNDARY))
652 && (i != start) && (i != end)
655 if ((angles_[start].angle <= angles_[i].angle) && (angles_[i].angle <= angles_[end].angle))
669 if (inside_CB && !outside_CB)
671 else if (!(!inside_CB && outside_CB))
673 if ((angles_[end].angle - angles_[start].angle) < M_PI)
679 if ((angles_[nCB].angle > angles_[start].angle) && (angles_[nCB].angle < angles_[end].angle))
690 else if ((angles_[start].angle < angles_[sourceIdx].angle) && (angles_[sourceIdx].angle < angles_[end].angle))
704 bool is_boundary =
false, is_skinny =
false;
705 std::vector<bool> gaps (nnn_,
false);
706 std::vector<bool> skinny (nnn_,
false);
707 std::vector<double> dif (nnn_);
708 std::vector<int> angleIdx; angleIdx.reserve (nnn_);
711 for (
int j=start; j<last_visible; j++)
713 dif[j] = (angles_[j+1].angle - angles_[j].angle);
714 if (dif[j] < minimum_angle_)
716 skinny[j] = is_skinny =
true;
718 else if (maximum_angle_ <= dif[j])
720 gaps[j] = is_boundary =
true;
722 if ((!gaps[j]) && (sqr_max_edge < (coords_[angles_[j+1].index] - coords_[angles_[j].index]).squaredNorm ()))
724 gaps[j] = is_boundary =
true;
726 angleIdx.push_back(j);
729 dif[last_visible] = (2*M_PI + angles_[0].angle - angles_[last_visible].angle);
730 if (dif[last_visible] < minimum_angle_)
732 skinny[last_visible] = is_skinny =
true;
734 else if (maximum_angle_ <= dif[last_visible])
736 gaps[last_visible] = is_boundary =
true;
738 if ((!gaps[last_visible]) && (sqr_max_edge < (coords_[angles_[0].index] - coords_[angles_[last_visible].index]).squaredNorm ()))
740 gaps[last_visible] = is_boundary =
true;
742 angleIdx.push_back(last_visible);
744 for (
int j=0; j<end; j++)
746 dif[j] = (angles_[j+1].angle - angles_[j].angle);
747 if (dif[j] < minimum_angle_)
749 skinny[j] = is_skinny =
true;
751 else if (maximum_angle_ <= dif[j])
753 gaps[j] = is_boundary =
true;
755 if ((!gaps[j]) && (sqr_max_edge < (coords_[angles_[j+1].index] - coords_[angles_[j].index]).squaredNorm ()))
757 gaps[j] = is_boundary =
true;
759 angleIdx.push_back(j);
761 angleIdx.push_back(end);
766 for (
int j=start; j<end; j++)
768 dif[j] = (angles_[j+1].angle - angles_[j].angle);
769 if (dif[j] < minimum_angle_)
771 skinny[j] = is_skinny =
true;
773 else if (maximum_angle_ <= dif[j])
775 gaps[j] = is_boundary =
true;
777 if ((!gaps[j]) && (sqr_max_edge < (coords_[angles_[j+1].index] - coords_[angles_[j].index]).squaredNorm ()))
779 gaps[j] = is_boundary =
true;
781 angleIdx.push_back(j);
783 angleIdx.push_back(end);
787 state_[R_] = is_boundary ? BOUNDARY : COMPLETED;
789 std::vector<int>::iterator first_gap_after = angleIdx.end ();
790 std::vector<int>::iterator last_gap_before = angleIdx.begin ();
792 for (std::vector<int>::iterator it = angleIdx.begin (); it != angleIdx.end () - 1; it++)
797 if (first_gap_after == angleIdx.end())
798 first_gap_after = it;
799 last_gap_before = it+1;
804 angleIdx.erase(first_gap_after+1, last_gap_before);
810 double angle_so_far = 0, angle_would_be;
811 double max_combined_angle = (std::min)(maximum_angle_, M_PI-2*minimum_angle_);
815 std::vector<int> to_erase;
816 for (std::vector<int>::iterator it = angleIdx.begin()+1; it != angleIdx.end()-1; it++)
821 angle_so_far += dif[*(it-1)];
823 angle_would_be = angle_so_far;
825 angle_would_be = angle_so_far + dif[*it];
827 (skinny[*it] || skinny[*(it-1)]) &&
828 ((state_[angles_[*it].index] <= FREE) || (state_[angles_[*(it-1)].index] <= FREE)) &&
829 ((!gaps[*it]) || (angles_[*it].nnIndex > angles_[*(it-1)].nnIndex)) &&
830 ((!gaps[*(it-1)]) || (angles_[*it].nnIndex > angles_[*(it+1)].nnIndex)) &&
831 (angle_would_be < max_combined_angle)
837 to_erase.push_back(*it);
841 gaps[*(it-1)] =
true;
842 to_erase.push_back(*it);
846 std::vector<int>::iterator prev_it;
847 int erased_idx =
static_cast<int> (to_erase.size ()) -1;
848 for (prev_it = it-1; (erased_idx != -1) && (it != angleIdx.begin()); it--)
849 if (*it == to_erase[erased_idx])
853 bool can_delete =
true;
854 for (std::vector<int>::iterator curr_it = prev_it+1; curr_it != it+1; curr_it++)
856 tmp_ = coords_[angles_[*curr_it].index] - proj_qp_;
859 tmp_ = coords_[angles_[*prev_it].index] - proj_qp_;
860 S1[0] = tmp_.dot(u_);
861 S1[1] = tmp_.dot(v_);
862 tmp_ = coords_[angles_[*(it+1)].index] - proj_qp_;
863 S2[0] = tmp_.dot(u_);
864 S2[1] = tmp_.dot(v_);
875 to_erase.push_back(*it);
882 for (
const int &it : to_erase)
884 for (std::vector<int>::iterator iter = angleIdx.begin(); iter != angleIdx.end(); iter++)
887 angleIdx.erase(iter);
894 changed_1st_fn_ =
false;
895 changed_2nd_fn_ =
false;
896 new2boundary_ = NONE;
897 for (std::vector<int>::iterator it = angleIdx.begin()+1; it != angleIdx.end()-1; it++)
899 current_index_ = angles_[*it].index;
901 is_current_free_ =
false;
902 if (state_[current_index_] <= FREE)
904 state_[current_index_] = FRINGE;
905 is_current_free_ =
true;
907 else if (!already_connected_)
909 prev_is_ffn_ = (ffn_[current_index_] == angles_[*(it-1)].index) && (!gaps[*(it-1)]);
910 prev_is_sfn_ = (sfn_[current_index_] == angles_[*(it-1)].index) && (!gaps[*(it-1)]);
911 next_is_ffn_ = (ffn_[current_index_] == angles_[*(it+1)].index) && (!gaps[*it]);
912 next_is_sfn_ = (sfn_[current_index_] == angles_[*(it+1)].index) && (!gaps[*it]);
913 if (!prev_is_ffn_ && !next_is_sfn_ && !prev_is_sfn_ && !next_is_ffn_)
922 if (is_current_free_)
923 state_[current_index_] = NONE;
928 addTriangle (current_index_, angles_[*(it-1)].index, R_, polygons);
929 addFringePoint (current_index_, R_);
930 new2boundary_ = current_index_;
931 if (!already_connected_)
932 connectPoint (polygons, angles_[*(it-1)].index, R_,
933 angles_[*(it+1)].index,
934 uvn_nn[angles_[*it].nnIndex], uvn_nn[angles_[*(it-1)].nnIndex], uvn_nn_qp_zero);
935 else already_connected_ =
false;
936 if (ffn_[R_] == angles_[*(angleIdx.begin())].index)
938 ffn_[R_] = new2boundary_;
940 else if (sfn_[R_] == angles_[*(angleIdx.begin())].index)
942 sfn_[R_] = new2boundary_;
949 addFringePoint (current_index_, R_);
950 new2boundary_ = current_index_;
951 if (!already_connected_) connectPoint (polygons, R_, angles_[*(it+1)].index,
952 (it+2) == angleIdx.end() ? -1 : angles_[*(it+2)].index,
953 uvn_nn[angles_[*it].nnIndex], uvn_nn_qp_zero,
954 uvn_nn[angles_[*(it+1)].nnIndex]);
955 else already_connected_ =
false;
956 if (ffn_[R_] == angles_[*(angleIdx.end()-1)].index)
958 ffn_[R_] = new2boundary_;
960 else if (sfn_[R_] == angles_[*(angleIdx.end()-1)].index)
962 sfn_[R_] = new2boundary_;
968 addTriangle (current_index_, angles_[*(it-1)].index, R_, polygons);
969 addFringePoint (current_index_, R_);
970 if (!already_connected_) connectPoint (polygons, angles_[*(it-1)].index, angles_[*(it+1)].index,
971 (it+2) == angleIdx.end() ? -1 : gaps[*(it+1)] ? R_ : angles_[*(it+2)].index,
972 uvn_nn[angles_[*it].nnIndex],
973 uvn_nn[angles_[*(it-1)].nnIndex],
974 uvn_nn[angles_[*(it+1)].nnIndex]);
975 else already_connected_ =
false;
980 if (ffn_[R_] == sfn_[R_])
982 state_[R_] = COMPLETED;
984 if (!gaps[*(angleIdx.end()-2)])
986 addTriangle (angles_[*(angleIdx.end()-2)].index, angles_[*(angleIdx.end()-1)].index, R_, polygons);
987 addFringePoint (angles_[*(angleIdx.end()-2)].index, R_);
988 if (R_ == ffn_[angles_[*(angleIdx.end()-1)].index])
990 if (angles_[*(angleIdx.end()-2)].index == sfn_[angles_[*(angleIdx.end()-1)].index])
992 state_[angles_[*(angleIdx.end()-1)].index] = COMPLETED;
996 ffn_[angles_[*(angleIdx.end()-1)].index] = angles_[*(angleIdx.end()-2)].index;
999 else if (R_ == sfn_[angles_[*(angleIdx.end()-1)].index])
1001 if (angles_[*(angleIdx.end()-2)].index == ffn_[angles_[*(angleIdx.end()-1)].index])
1003 state_[angles_[*(angleIdx.end()-1)].index] = COMPLETED;
1007 sfn_[angles_[*(angleIdx.end()-1)].index] = angles_[*(angleIdx.end()-2)].index;
1011 if (!gaps[*(angleIdx.begin())])
1013 if (R_ == ffn_[angles_[*(angleIdx.begin())].index])
1015 if (angles_[*(angleIdx.begin()+1)].index == sfn_[angles_[*(angleIdx.begin())].index])
1017 state_[angles_[*(angleIdx.begin())].index] = COMPLETED;
1021 ffn_[angles_[*(angleIdx.begin())].index] = angles_[*(angleIdx.begin()+1)].index;
1024 else if (R_ == sfn_[angles_[*(angleIdx.begin())].index])
1026 if (angles_[*(angleIdx.begin()+1)].index == ffn_[angles_[*(angleIdx.begin())].index])
1028 state_[angles_[*(angleIdx.begin())].index] = COMPLETED;
1032 sfn_[angles_[*(angleIdx.begin())].index] = angles_[*(angleIdx.begin()+1)].index;
1038 PCL_DEBUG (
"Number of triangles: %lu\n", polygons.size());
1039 PCL_DEBUG (
"Number of unconnected parts: %d\n", nr_parts);
1040 if (increase_nnn4fn > 0)
1041 PCL_WARN (
"Number of neighborhood size increase requests for fringe neighbors: %d\n", increase_nnn4fn);
1042 if (increase_nnn4s > 0)
1043 PCL_WARN (
"Number of neighborhood size increase requests for source: %d\n", increase_nnn4s);
1044 if (increase_dist > 0)
1045 PCL_WARN (
"Number of automatic maximum distance increases: %d\n", increase_dist);
1048 std::sort (fringe_queue_.begin (), fringe_queue_.end ());
1049 fringe_queue_.erase (std::unique (fringe_queue_.begin (), fringe_queue_.end ()), fringe_queue_.end ());
1050 PCL_DEBUG (
"Number of processed points: %lu / %lu\n", fringe_queue_.size(), indices_->size ());
1055 template <
typename Po
intInT>
void
1058 state_[R_] = COMPLETED;
1059 addTriangle (angles_[0].index, angles_[1].index, R_, polygons);
1060 for (
int aIdx=0; aIdx<2; aIdx++)
1062 if (ffn_[angles_[aIdx].index] == R_)
1064 if (sfn_[angles_[aIdx].index] == angles_[(aIdx+1)%2].index)
1066 state_[angles_[aIdx].index] = COMPLETED;
1070 ffn_[angles_[aIdx].index] = angles_[(aIdx+1)%2].index;
1073 else if (sfn_[angles_[aIdx].index] == R_)
1075 if (ffn_[angles_[aIdx].index] == angles_[(aIdx+1)%2].index)
1077 state_[angles_[aIdx].index] = COMPLETED;
1081 sfn_[angles_[aIdx].index] = angles_[(aIdx+1)%2].index;
1088 template <
typename Po
intInT>
void
1090 std::vector<pcl::Vertices> &polygons,
1091 const int prev_index,
const int next_index,
const int next_next_index,
1092 const Eigen::Vector2f &uvn_current,
1093 const Eigen::Vector2f &uvn_prev,
1094 const Eigen::Vector2f &uvn_next)
1096 if (is_current_free_)
1098 ffn_[current_index_] = prev_index;
1099 sfn_[current_index_] = next_index;
1103 if ((prev_is_ffn_ && next_is_sfn_) || (prev_is_sfn_ && next_is_ffn_))
1104 state_[current_index_] = COMPLETED;
1105 else if (prev_is_ffn_ && !next_is_sfn_)
1106 ffn_[current_index_] = next_index;
1107 else if (next_is_ffn_ && !prev_is_sfn_)
1108 ffn_[current_index_] = prev_index;
1109 else if (prev_is_sfn_ && !next_is_ffn_)
1110 sfn_[current_index_] = next_index;
1111 else if (next_is_sfn_ && !prev_is_ffn_)
1112 sfn_[current_index_] = prev_index;
1115 bool found_triangle =
false;
1116 if ((prev_index != R_) && ((ffn_[current_index_] == ffn_[prev_index]) || (ffn_[current_index_] == sfn_[prev_index])))
1118 found_triangle =
true;
1119 addTriangle (current_index_, ffn_[current_index_], prev_index, polygons);
1120 state_[prev_index] = COMPLETED;
1121 state_[ffn_[current_index_]] = COMPLETED;
1122 ffn_[current_index_] = next_index;
1124 else if ((prev_index != R_) && ((sfn_[current_index_] == ffn_[prev_index]) || (sfn_[current_index_] == sfn_[prev_index])))
1126 found_triangle =
true;
1127 addTriangle (current_index_, sfn_[current_index_], prev_index, polygons);
1128 state_[prev_index] = COMPLETED;
1129 state_[sfn_[current_index_]] = COMPLETED;
1130 sfn_[current_index_] = next_index;
1132 else if (state_[next_index] > FREE)
1134 if ((ffn_[current_index_] == ffn_[next_index]) || (ffn_[current_index_] == sfn_[next_index]))
1136 found_triangle =
true;
1137 addTriangle (current_index_, ffn_[current_index_], next_index, polygons);
1139 if (ffn_[current_index_] == ffn_[next_index])
1141 ffn_[next_index] = current_index_;
1145 sfn_[next_index] = current_index_;
1147 state_[ffn_[current_index_]] = COMPLETED;
1148 ffn_[current_index_] = prev_index;
1150 else if ((sfn_[current_index_] == ffn_[next_index]) || (sfn_[current_index_] == sfn_[next_index]))
1152 found_triangle =
true;
1153 addTriangle (current_index_, sfn_[current_index_], next_index, polygons);
1155 if (sfn_[current_index_] == ffn_[next_index])
1157 ffn_[next_index] = current_index_;
1161 sfn_[next_index] = current_index_;
1163 state_[sfn_[current_index_]] = COMPLETED;
1164 sfn_[current_index_] = prev_index;
1173 tmp_ = coords_[ffn_[current_index_]] - proj_qp_;
1174 uvn_ffn_[0] = tmp_.dot(u_);
1175 uvn_ffn_[1] = tmp_.dot(v_);
1176 tmp_ = coords_[sfn_[current_index_]] - proj_qp_;
1177 uvn_sfn_[0] = tmp_.dot(u_);
1178 uvn_sfn_[1] = tmp_.dot(v_);
1179 bool prev_ffn =
isVisible(uvn_prev, uvn_next, uvn_current, uvn_ffn_) &&
isVisible(uvn_prev, uvn_sfn_, uvn_current, uvn_ffn_);
1180 bool prev_sfn =
isVisible(uvn_prev, uvn_next, uvn_current, uvn_sfn_) &&
isVisible(uvn_prev, uvn_ffn_, uvn_current, uvn_sfn_);
1181 bool next_ffn =
isVisible(uvn_next, uvn_prev, uvn_current, uvn_ffn_) &&
isVisible(uvn_next, uvn_sfn_, uvn_current, uvn_ffn_);
1182 bool next_sfn =
isVisible(uvn_next, uvn_prev, uvn_current, uvn_sfn_) &&
isVisible(uvn_next, uvn_ffn_, uvn_current, uvn_sfn_);
1184 if (prev_ffn && next_sfn && prev_sfn && next_ffn)
1187 double prev2f = (coords_[ffn_[current_index_]] - coords_[prev_index]).squaredNorm ();
1188 double next2s = (coords_[sfn_[current_index_]] - coords_[next_index]).squaredNorm ();
1189 double prev2s = (coords_[sfn_[current_index_]] - coords_[prev_index]).squaredNorm ();
1190 double next2f = (coords_[ffn_[current_index_]] - coords_[next_index]).squaredNorm ();
1191 if (prev2f < prev2s)
1193 if (prev2f < next2f)
1195 if (prev2f < next2s)
1202 if (next2f < next2s)
1210 if (prev2s < next2f)
1212 if (prev2s < next2s)
1219 if (next2f < next2s)
1226 else if (prev_ffn && next_sfn)
1229 double prev2f = (coords_[ffn_[current_index_]] - coords_[prev_index]).squaredNorm ();
1230 double next2s = (coords_[sfn_[current_index_]] - coords_[next_index]).squaredNorm ();
1231 if (prev2f < next2s)
1236 else if (prev_sfn && next_ffn)
1239 double prev2s = (coords_[sfn_[current_index_]] - coords_[prev_index]).squaredNorm ();
1240 double next2f = (coords_[ffn_[current_index_]] - coords_[next_index]).squaredNorm ();
1241 if (prev2s < next2f)
1247 else if (prev_ffn && !next_sfn && !prev_sfn && !next_ffn)
1249 else if (!prev_ffn && !next_sfn && prev_sfn && !next_ffn)
1251 else if (!prev_ffn && !next_sfn && !prev_sfn && next_ffn)
1253 else if (!prev_ffn && next_sfn && !prev_sfn && !next_ffn)
1258 double prev2f = (coords_[ffn_[current_index_]] - coords_[prev_index]).squaredNorm ();
1261 double prev2s = (coords_[sfn_[current_index_]] - coords_[prev_index]).squaredNorm ();
1262 if (prev2s < prev2f)
1269 double next2f = (coords_[ffn_[current_index_]] - coords_[next_index]).squaredNorm ();
1270 if (next2f < prev2f)
1278 double next2s = (coords_[sfn_[current_index_]] - coords_[next_index]).squaredNorm ();
1281 double prev2s = (coords_[sfn_[current_index_]] - coords_[prev_index]).squaredNorm ();
1282 if (prev2s < next2s)
1289 double next2f = (coords_[ffn_[current_index_]] - coords_[next_index]).squaredNorm ();
1290 if (next2f < next2s)
1300 addTriangle (current_index_, ffn_[current_index_], prev_index, polygons);
1303 if (ffn_[prev_index] == current_index_)
1305 ffn_[prev_index] = ffn_[current_index_];
1307 else if (sfn_[prev_index] == current_index_)
1309 sfn_[prev_index] = ffn_[current_index_];
1311 else if (ffn_[prev_index] == R_)
1313 changed_1st_fn_ =
true;
1314 ffn_[prev_index] = ffn_[current_index_];
1316 else if (sfn_[prev_index] == R_)
1318 changed_1st_fn_ =
true;
1319 sfn_[prev_index] = ffn_[current_index_];
1321 else if (prev_index == R_)
1323 new2boundary_ = ffn_[current_index_];
1327 if (ffn_[ffn_[current_index_]] == current_index_)
1329 ffn_[ffn_[current_index_]] = prev_index;
1331 else if (sfn_[ffn_[current_index_]] == current_index_)
1333 sfn_[ffn_[current_index_]] = prev_index;
1337 ffn_[current_index_] = next_index;
1343 addTriangle (current_index_, sfn_[current_index_], prev_index, polygons);
1346 if (ffn_[prev_index] == current_index_)
1348 ffn_[prev_index] = sfn_[current_index_];
1350 else if (sfn_[prev_index] == current_index_)
1352 sfn_[prev_index] = sfn_[current_index_];
1354 else if (ffn_[prev_index] == R_)
1356 changed_1st_fn_ =
true;
1357 ffn_[prev_index] = sfn_[current_index_];
1359 else if (sfn_[prev_index] == R_)
1361 changed_1st_fn_ =
true;
1362 sfn_[prev_index] = sfn_[current_index_];
1364 else if (prev_index == R_)
1366 new2boundary_ = sfn_[current_index_];
1370 if (ffn_[sfn_[current_index_]] == current_index_)
1372 ffn_[sfn_[current_index_]] = prev_index;
1374 else if (sfn_[sfn_[current_index_]] == current_index_)
1376 sfn_[sfn_[current_index_]] = prev_index;
1380 sfn_[current_index_] = next_index;
1386 addTriangle (current_index_, ffn_[current_index_], next_index, polygons);
1387 int neighbor_update = next_index;
1390 if (state_[next_index] <= FREE)
1392 state_[next_index] = FRINGE;
1393 ffn_[next_index] = current_index_;
1394 sfn_[next_index] = ffn_[current_index_];
1398 if (ffn_[next_index] == R_)
1400 changed_2nd_fn_ =
true;
1401 ffn_[next_index] = ffn_[current_index_];
1403 else if (sfn_[next_index] == R_)
1405 changed_2nd_fn_ =
true;
1406 sfn_[next_index] = ffn_[current_index_];
1408 else if (next_index == R_)
1410 new2boundary_ = ffn_[current_index_];
1411 if (next_next_index == new2boundary_)
1412 already_connected_ =
true;
1414 else if (ffn_[next_index] == next_next_index)
1416 already_connected_ =
true;
1417 ffn_[next_index] = ffn_[current_index_];
1419 else if (sfn_[next_index] == next_next_index)
1421 already_connected_ =
true;
1422 sfn_[next_index] = ffn_[current_index_];
1426 tmp_ = coords_[ffn_[next_index]] - proj_qp_;
1427 uvn_next_ffn_[0] = tmp_.dot(u_);
1428 uvn_next_ffn_[1] = tmp_.dot(v_);
1429 tmp_ = coords_[sfn_[next_index]] - proj_qp_;
1430 uvn_next_sfn_[0] = tmp_.dot(u_);
1431 uvn_next_sfn_[1] = tmp_.dot(v_);
1433 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_);
1434 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_);
1436 int connect2ffn = -1;
1437 if (ffn_next_ffn && sfn_next_ffn)
1439 double fn2f = (coords_[ffn_[current_index_]] - coords_[ffn_[next_index]]).squaredNorm ();
1440 double sn2f = (coords_[ffn_[current_index_]] - coords_[sfn_[next_index]]).squaredNorm ();
1441 if (fn2f < sn2f) connect2ffn = 0;
1442 else connect2ffn = 1;
1444 else if (ffn_next_ffn) connect2ffn = 0;
1445 else if (sfn_next_ffn) connect2ffn = 1;
1447 switch (connect2ffn)
1451 addTriangle (next_index, ffn_[current_index_], ffn_[next_index], polygons);
1452 neighbor_update = ffn_[next_index];
1455 if ((ffn_[ffn_[next_index]] == ffn_[current_index_]) || (sfn_[ffn_[next_index]] == ffn_[current_index_]))
1457 state_[ffn_[next_index]] = COMPLETED;
1459 else if (ffn_[ffn_[next_index]] == next_index)
1461 ffn_[ffn_[next_index]] = ffn_[current_index_];
1463 else if (sfn_[ffn_[next_index]] == next_index)
1465 sfn_[ffn_[next_index]] = ffn_[current_index_];
1468 ffn_[next_index] = current_index_;
1474 addTriangle (next_index, ffn_[current_index_], sfn_[next_index], polygons);
1475 neighbor_update = sfn_[next_index];
1478 if ((ffn_[sfn_[next_index]] == ffn_[current_index_]) || (sfn_[sfn_[next_index]] == ffn_[current_index_]))
1480 state_[sfn_[next_index]] = COMPLETED;
1482 else if (ffn_[sfn_[next_index]] == next_index)
1484 ffn_[sfn_[next_index]] = ffn_[current_index_];
1486 else if (sfn_[sfn_[next_index]] == next_index)
1488 sfn_[sfn_[next_index]] = ffn_[current_index_];
1491 sfn_[next_index] = current_index_;
1501 if ((ffn_[ffn_[current_index_]] == neighbor_update) || (sfn_[ffn_[current_index_]] == neighbor_update))
1503 state_[ffn_[current_index_]] = COMPLETED;
1505 else if (ffn_[ffn_[current_index_]] == current_index_)
1507 ffn_[ffn_[current_index_]] = neighbor_update;
1509 else if (sfn_[ffn_[current_index_]] == current_index_)
1511 sfn_[ffn_[current_index_]] = neighbor_update;
1515 ffn_[current_index_] = prev_index;
1521 addTriangle (current_index_, sfn_[current_index_], next_index, polygons);
1522 int neighbor_update = next_index;
1525 if (state_[next_index] <= FREE)
1527 state_[next_index] = FRINGE;
1528 ffn_[next_index] = current_index_;
1529 sfn_[next_index] = sfn_[current_index_];
1533 if (ffn_[next_index] == R_)
1535 changed_2nd_fn_ =
true;
1536 ffn_[next_index] = sfn_[current_index_];
1538 else if (sfn_[next_index] == R_)
1540 changed_2nd_fn_ =
true;
1541 sfn_[next_index] = sfn_[current_index_];
1543 else if (next_index == R_)
1545 new2boundary_ = sfn_[current_index_];
1546 if (next_next_index == new2boundary_)
1547 already_connected_ =
true;
1549 else if (ffn_[next_index] == next_next_index)
1551 already_connected_ =
true;
1552 ffn_[next_index] = sfn_[current_index_];
1554 else if (sfn_[next_index] == next_next_index)
1556 already_connected_ =
true;
1557 sfn_[next_index] = sfn_[current_index_];
1561 tmp_ = coords_[ffn_[next_index]] - proj_qp_;
1562 uvn_next_ffn_[0] = tmp_.dot(u_);
1563 uvn_next_ffn_[1] = tmp_.dot(v_);
1564 tmp_ = coords_[sfn_[next_index]] - proj_qp_;
1565 uvn_next_sfn_[0] = tmp_.dot(u_);
1566 uvn_next_sfn_[1] = tmp_.dot(v_);
1568 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_);
1569 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_);
1571 int connect2sfn = -1;
1572 if (ffn_next_sfn && sfn_next_sfn)
1574 double fn2s = (coords_[sfn_[current_index_]] - coords_[ffn_[next_index]]).squaredNorm ();
1575 double sn2s = (coords_[sfn_[current_index_]] - coords_[sfn_[next_index]]).squaredNorm ();
1576 if (fn2s < sn2s) connect2sfn = 0;
1577 else connect2sfn = 1;
1579 else if (ffn_next_sfn) connect2sfn = 0;
1580 else if (sfn_next_sfn) connect2sfn = 1;
1582 switch (connect2sfn)
1586 addTriangle (next_index, sfn_[current_index_], ffn_[next_index], polygons);
1587 neighbor_update = ffn_[next_index];
1590 if ((ffn_[ffn_[next_index]] == sfn_[current_index_]) || (sfn_[ffn_[next_index]] == sfn_[current_index_]))
1592 state_[ffn_[next_index]] = COMPLETED;
1594 else if (ffn_[ffn_[next_index]] == next_index)
1596 ffn_[ffn_[next_index]] = sfn_[current_index_];
1598 else if (sfn_[ffn_[next_index]] == next_index)
1600 sfn_[ffn_[next_index]] = sfn_[current_index_];
1603 ffn_[next_index] = current_index_;
1609 addTriangle (next_index, sfn_[current_index_], sfn_[next_index], polygons);
1610 neighbor_update = sfn_[next_index];
1613 if ((ffn_[sfn_[next_index]] == sfn_[current_index_]) || (sfn_[sfn_[next_index]] == sfn_[current_index_]))
1615 state_[sfn_[next_index]] = COMPLETED;
1617 else if (ffn_[sfn_[next_index]] == next_index)
1619 ffn_[sfn_[next_index]] = sfn_[current_index_];
1621 else if (sfn_[sfn_[next_index]] == next_index)
1623 sfn_[sfn_[next_index]] = sfn_[current_index_];
1626 sfn_[next_index] = current_index_;
1636 if ((ffn_[sfn_[current_index_]] == neighbor_update) || (sfn_[sfn_[current_index_]] == neighbor_update))
1638 state_[sfn_[current_index_]] = COMPLETED;
1640 else if (ffn_[sfn_[current_index_]] == current_index_)
1642 ffn_[sfn_[current_index_]] = neighbor_update;
1644 else if (sfn_[sfn_[current_index_]] == current_index_)
1646 sfn_[sfn_[current_index_]] = neighbor_update;
1649 sfn_[current_index_] = prev_index;
1661 template <
typename Po
intInT> std::vector<std::vector<std::size_t> >
1666 for (std::size_t i=0; i < input.
polygons.size (); ++i)
1667 for (std::size_t j=0; j < input.
polygons[i].vertices.size (); ++j)
1668 triangleList[input.
polygons[i].vertices[j]].push_back (i);
1669 return (triangleList);
1672 #define PCL_INSTANTIATE_GreedyProjectionTriangulation(T) \
1673 template class PCL_EXPORTS pcl::GreedyProjectionTriangulation<T>;
1675 #endif // PCL_SURFACE_IMPL_GP3_H_