38 #ifndef PCL_SURFACE_IMPL_GP3_H_
39 #define PCL_SURFACE_IMPL_GP3_H_
41 #include <pcl/surface/gp3.h>
42 #include <pcl/kdtree/impl/kdtree_flann.hpp>
45 template <
typename Po
intInT>
void
49 output.
polygons.reserve (2 * indices_->size ());
50 if (!reconstructPolygons (output.
polygons))
52 PCL_ERROR (
"[pcl::%s::performReconstruction] Reconstruction failed. Check parameters: search radius (%f) or mu (%f) before continuing.\n", getClassName ().c_str (), search_radius_, mu_);
60 template <
typename Po
intInT>
void
64 polygons.reserve (2 * indices_->size ());
65 if (!reconstructPolygons (polygons))
67 PCL_ERROR (
"[pcl::%s::performReconstruction] Reconstruction failed. Check parameters: search radius (%f) or mu (%f) before continuing.\n", getClassName ().c_str (), search_radius_, mu_);
73 template <
typename Po
intInT>
bool
76 if (search_radius_ <= 0 || mu_ <= 0)
81 const double sqr_mu = mu_*mu_;
82 const double sqr_max_edge = search_radius_*search_radius_;
83 if (nnn_ > static_cast<int> (indices_->size ()))
84 nnn_ =
static_cast<int> (indices_->size ());
87 std::vector<int> nnIdx (nnn_);
88 std::vector<float> sqrDists (nnn_);
94 const Eigen::Vector2f uvn_nn_qp_zero = Eigen::Vector2f::Zero();
95 Eigen::Vector2f uvn_current;
96 Eigen::Vector2f uvn_prev;
97 Eigen::Vector2f uvn_next;
100 already_connected_ =
false;
108 part_.resize(indices_->size (), -1);
109 state_.resize(indices_->size (), FREE);
110 source_.resize(indices_->size (), NONE);
111 ffn_.resize(indices_->size (), NONE);
112 sfn_.resize(indices_->size (), NONE);
113 fringe_queue_.clear ();
117 if (!input_->is_dense)
120 for (std::vector<int>::const_iterator it = indices_->begin (); it != indices_->end (); ++it)
121 if (!pcl_isfinite (input_->points[*it].x) ||
122 !pcl_isfinite (input_->points[*it].y) ||
123 !pcl_isfinite (input_->points[*it].z))
129 coords_.reserve (indices_->size ());
130 std::vector<int> point2index (input_->points.size (), -1);
131 for (
int cp = 0; cp < static_cast<int> (indices_->size ()); ++
cp)
133 coords_.push_back(input_->points[(*indices_)[cp]].getVector3fMap());
134 point2index[(*indices_)[
cp]] =
cp;
138 int is_free=0, nr_parts=0, increase_nnn4fn=0, increase_nnn4s=0, increase_dist=0, nr_touched = 0;
140 angles_.resize(nnn_);
141 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > uvn_nn (nnn_);
142 Eigen::Vector2f uvn_s;
145 while (is_free != NONE)
148 if (state_[R_] == FREE)
151 part_[R_] = part_index++;
156 tree_->nearestKSearch (indices_->at (R_), nnn_, nnIdx, sqrDists);
157 double sqr_dist_threshold = (std::min)(sqr_max_edge, sqr_mu * sqrDists[1]);
160 for (
int i = 1; i < nnn_; i++)
164 nnIdx[i] = point2index[nnIdx[i]];
168 const Eigen::Vector3f nc = input_->points[(*indices_)[R_]].getNormalVector3fMap ();
171 v_ = nc.unitOrthogonal ();
175 float dist = nc.dot (coords_[R_]);
176 proj_qp_ = coords_[R_] - dist * nc;
180 std::vector<doubleEdge> doubleEdges;
181 for (
int i = 1; i < nnn_; i++)
184 tmp_ = coords_[nnIdx[i]] - proj_qp_;
185 uvn_nn[i][0] = tmp_.dot(u_);
186 uvn_nn[i][1] = tmp_.dot(v_);
188 angles_[i].angle = atan2(uvn_nn[i][1], uvn_nn[i][0]);
190 angles_[i].index = nnIdx[i];
192 (state_[nnIdx[i]] == COMPLETED) || (state_[nnIdx[i]] == BOUNDARY)
193 || (state_[nnIdx[i]] == NONE) || (nnIdx[i] == -1)
194 || (sqrDists[i] > sqr_dist_threshold)
196 angles_[i].visible =
false;
198 angles_[i].visible =
true;
200 if ((state_[nnIdx[i]] == FRINGE) || (state_[nnIdx[i]] == BOUNDARY))
205 tmp_ = coords_[ffn_[nnIdx[i]]] - proj_qp_;
206 e.first[0] = tmp_.dot(u_);
207 e.first[1] = tmp_.dot(v_);
208 tmp_ = coords_[sfn_[nnIdx[i]]] - proj_qp_;
209 e.second[0] = tmp_.dot(u_);
210 e.second[1] = tmp_.dot(v_);
211 doubleEdges.push_back(e);
214 angles_[0].visible =
false;
217 for (
int i = 1; i < nnn_; i++)
218 if ((angles_[i].visible) && (ffn_[R_] != nnIdx[i]) && (sfn_[R_] != nnIdx[i]))
220 bool visibility =
true;
221 for (
int j = 0; j < nr_edge; j++)
223 if (ffn_[nnIdx[doubleEdges[j].index]] != nnIdx[i])
224 visibility =
isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].first, Eigen::Vector2f::Zero());
227 if (sfn_[nnIdx[doubleEdges[j].index]] != nnIdx[i])
228 visibility =
isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].second, Eigen::Vector2f::Zero());
232 angles_[i].visible = visibility;
236 bool not_found =
true;
240 while ((left < nnn_) && ((!angles_[left].visible) || (state_[nnIdx[left]] > FREE))) left++;
248 while ((right < nnn_) && ((!angles_[right].visible) || (state_[nnIdx[right]] > FREE))) right++;
251 else if ((coords_[nnIdx[left]] - coords_[nnIdx[right]]).squaredNorm () > sqr_max_edge)
255 addFringePoint (nnIdx[right], R_);
256 addFringePoint (nnIdx[left], nnIdx[right]);
257 addFringePoint (R_, nnIdx[left]);
258 state_[R_] = state_[nnIdx[left]] = state_[nnIdx[right]] = FRINGE;
259 ffn_[R_] = nnIdx[left];
260 sfn_[R_] = nnIdx[right];
261 ffn_[nnIdx[left]] = nnIdx[right];
262 sfn_[nnIdx[left]] = R_;
263 ffn_[nnIdx[right]] = R_;
264 sfn_[nnIdx[right]] = nnIdx[left];
265 addTriangle (R_, nnIdx[left], nnIdx[right], polygons);
279 for (
unsigned temp = 0; temp < indices_->size (); temp++)
281 if (state_[temp] == FREE)
293 int fqSize =
static_cast<int> (fringe_queue_.size ());
294 while ((fqIdx < fqSize) && (state_[fringe_queue_[fqIdx]] != FRINGE))
303 R_ = fringe_queue_[fqIdx];
306 if (ffn_[R_] == sfn_[R_])
308 state_[R_] = COMPLETED;
313 tree_->nearestKSearch (indices_->at (R_), nnn_, nnIdx, sqrDists);
316 for (
int i = 1; i < nnn_; i++)
320 nnIdx[i] = point2index[nnIdx[i]];
324 double sqr_source_dist = (coords_[R_] - coords_[source_[R_]]).squaredNorm ();
325 double sqr_ffn_dist = (coords_[R_] - coords_[ffn_[R_]]).squaredNorm ();
326 double sqr_sfn_dist = (coords_[R_] - coords_[sfn_[R_]]).squaredNorm ();
327 double max_sqr_fn_dist = (std::max)(sqr_ffn_dist, sqr_sfn_dist);
328 double sqr_dist_threshold = (std::min)(sqr_max_edge, sqr_mu * sqrDists[1]);
329 if (max_sqr_fn_dist > sqrDists[nnn_-1])
331 if (0 == increase_nnn4fn)
332 PCL_WARN(
"Not enough neighbors are considered: ffn or sfn out of range! Consider increasing nnn_... Setting R=%d to be BOUNDARY!\n", R_);
334 state_[R_] = BOUNDARY;
337 double max_sqr_fns_dist = (std::max)(sqr_source_dist, max_sqr_fn_dist);
338 if (max_sqr_fns_dist > sqrDists[nnn_-1])
340 if (0 == increase_nnn4s)
341 PCL_WARN(
"Not enough neighbors are considered: source of R=%d is out of range! Consider increasing nnn_...\n", R_);
346 const Eigen::Vector3f nc = input_->points[(*indices_)[R_]].getNormalVector3fMap ();
349 v_ = nc.unitOrthogonal ();
353 float dist = nc.dot (coords_[R_]);
354 proj_qp_ = coords_[R_] - dist * nc;
358 std::vector<doubleEdge> doubleEdges;
359 for (
int i = 1; i < nnn_; i++)
361 tmp_ = coords_[nnIdx[i]] - proj_qp_;
362 uvn_nn[i][0] = tmp_.dot(u_);
363 uvn_nn[i][1] = tmp_.dot(v_);
366 angles_[i].angle = atan2(uvn_nn[i][1], uvn_nn[i][0]);
368 angles_[i].index = nnIdx[i];
369 angles_[i].nnIndex = i;
371 (state_[nnIdx[i]] == COMPLETED) || (state_[nnIdx[i]] == BOUNDARY)
372 || (state_[nnIdx[i]] == NONE) || (nnIdx[i] == -1)
373 || (sqrDists[i] > sqr_dist_threshold)
375 angles_[i].visible =
false;
377 angles_[i].visible =
true;
378 if ((ffn_[R_] == nnIdx[i]) || (sfn_[R_] == nnIdx[i]))
379 angles_[i].visible =
true;
380 bool same_side =
true;
381 const Eigen::Vector3f neighbor_normal = input_->points[(*indices_)[nnIdx[i]]].getNormalVector3fMap ();
382 double cosine = nc.dot (neighbor_normal);
383 if (cosine > 1) cosine = 1;
384 if (cosine < -1) cosine = -1;
385 double angle = acos (cosine);
386 if ((!consistent_) && (angle > M_PI/2))
387 angle = M_PI - angle;
388 if (angle > eps_angle_)
390 angles_[i].visible =
false;
394 if ((i!=0) && (same_side) && ((state_[nnIdx[i]] == FRINGE) || (state_[nnIdx[i]] == BOUNDARY)))
399 tmp_ = coords_[ffn_[nnIdx[i]]] - proj_qp_;
400 e.first[0] = tmp_.dot(u_);
401 e.first[1] = tmp_.dot(v_);
402 tmp_ = coords_[sfn_[nnIdx[i]]] - proj_qp_;
403 e.second[0] = tmp_.dot(u_);
404 e.second[1] = tmp_.dot(v_);
405 doubleEdges.push_back(e);
407 if ((state_[nnIdx[i]] == FRINGE) && (ffn_[R_] != nnIdx[i]) && (sfn_[R_] != nnIdx[i]))
409 double angle1 = atan2(e.first[1] - uvn_nn[i][1], e.first[0] - uvn_nn[i][0]);
410 double angle2 = atan2(e.second[1] - uvn_nn[i][1], e.second[0] - uvn_nn[i][0]);
411 double angleMin, angleMax;
422 double angleR = angles_[i].angle + M_PI;
425 if ((source_[nnIdx[i]] == ffn_[nnIdx[i]]) || (source_[nnIdx[i]] == sfn_[nnIdx[i]]))
427 if ((angleMax - angleMin) < M_PI)
429 if ((angleMin < angleR) && (angleR < angleMax))
430 angles_[i].visible =
false;
434 if ((angleR < angleMin) || (angleMax < angleR))
435 angles_[i].visible =
false;
440 tmp_ = coords_[source_[nnIdx[i]]] - proj_qp_;
441 uvn_s[0] = tmp_.dot(u_);
442 uvn_s[1] = tmp_.dot(v_);
443 double angleS = atan2(uvn_s[1] - uvn_nn[i][1], uvn_s[0] - uvn_nn[i][0]);
444 if ((angleMin < angleS) && (angleS < angleMax))
446 if ((angleMin < angleR) && (angleR < angleMax))
447 angles_[i].visible =
false;
451 if ((angleR < angleMin) || (angleMax < angleR))
452 angles_[i].visible =
false;
458 angles_[0].visible =
false;
461 for (
int i = 1; i < nnn_; i++)
462 if ((angles_[i].visible) && (ffn_[R_] != nnIdx[i]) && (sfn_[R_] != nnIdx[i]))
464 bool visibility =
true;
465 for (
int j = 0; j < nr_edge; j++)
467 if (doubleEdges[j].index != i)
469 int f = ffn_[nnIdx[doubleEdges[j].index]];
470 if ((f != nnIdx[i]) && (f != R_))
471 visibility =
isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].first, Eigen::Vector2f::Zero());
472 if (visibility ==
false)
475 int s = sfn_[nnIdx[doubleEdges[j].index]];
476 if ((s != nnIdx[i]) && (s != R_))
477 visibility =
isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].second, Eigen::Vector2f::Zero());
478 if (visibility ==
false)
482 angles_[i].visible = visibility;
486 std::sort (angles_.begin (), angles_.end (), GreedyProjectionTriangulation<PointInT>::nnAngleSortAsc);
489 if (angles_[2].visible ==
false)
491 if ( !( (angles_[0].index == ffn_[R_] && angles_[1].index == sfn_[R_]) || (angles_[0].index == sfn_[R_] && angles_[1].index == ffn_[R_]) ) )
493 state_[R_] = BOUNDARY;
497 if ((source_[R_] == angles_[0].index) || (source_[R_] == angles_[1].index))
498 state_[R_] = BOUNDARY;
501 if (sqr_max_edge < (coords_[ffn_[R_]] - coords_[sfn_[R_]]).squaredNorm ())
503 state_[R_] = BOUNDARY;
507 tmp_ = coords_[source_[R_]] - proj_qp_;
508 uvn_s[0] = tmp_.dot(u_);
509 uvn_s[1] = tmp_.dot(v_);
510 double angleS = atan2(uvn_s[1], uvn_s[0]);
511 double dif = angles_[1].angle - angles_[0].angle;
512 if ((angles_[0].angle < angleS) && (angleS < angles_[1].angle))
514 if (dif < 2*M_PI - maximum_angle_)
515 state_[R_] = BOUNDARY;
517 closeTriangle (polygons);
521 if (dif >= maximum_angle_)
522 state_[R_] = BOUNDARY;
524 closeTriangle (polygons);
533 int start = -1, end = -1;
534 for (
int i=0; i<nnn_; i++)
536 if (ffn_[R_] == angles_[i].index)
539 if (sfn_[R_] == angles_[i+1].index)
544 for (i = i+2; i < nnn_; i++)
545 if (sfn_[R_] == angles_[i].index)
551 for (i = i+2; i < nnn_; i++)
552 if (sfn_[R_] == angles_[i].index)
558 if (sfn_[R_] == angles_[i].index)
561 if (ffn_[R_] == angles_[i+1].index)
566 for (i = i+2; i < nnn_; i++)
567 if (ffn_[R_] == angles_[i].index)
573 for (i = i+2; i < nnn_; i++)
574 if (ffn_[R_] == angles_[i].index)
583 if ((start < 0) || (end < 0) || (end == nnn_) || (!angles_[start].visible) || (!angles_[end].visible))
585 state_[R_] = BOUNDARY;
590 int last_visible = end;
591 while ((last_visible+1<nnn_) && (angles_[last_visible+1].visible)) last_visible++;
594 bool need_invert =
false;
595 int sourceIdx = nnn_;
596 if ((source_[R_] == ffn_[R_]) || (source_[R_] == sfn_[R_]))
598 if ((angles_[end].angle - angles_[start].angle) < M_PI)
603 for (sourceIdx=0; sourceIdx<nnn_; sourceIdx++)
604 if (angles_[sourceIdx].index == source_[R_])
606 if (sourceIdx == nnn_)
608 int vis_free = NONE, nnCB = NONE;
609 for (
int i = 1; i < nnn_; i++)
612 if ((state_[nnIdx[i]] == COMPLETED) || (state_[nnIdx[i]] == BOUNDARY))
617 if (vis_free != NONE)
622 if (state_[angles_[i].index] <= FREE)
624 if (i <= last_visible)
635 while (angles_[nCB].index != nnIdx[nnCB]) nCB++;
639 if (vis_free != NONE)
641 if ((vis_free < start) || (vis_free > end))
648 if ((nCB == start) || (nCB == end))
650 bool inside_CB =
false;
651 bool outside_CB =
false;
652 for (
int i=0; i<nnn_; i++)
655 ((state_[angles_[i].index] == COMPLETED) || (state_[angles_[i].index] == BOUNDARY))
656 && (i != start) && (i != end)
659 if ((angles_[start].angle <= angles_[i].angle) && (angles_[i].angle <= angles_[end].angle))
673 if (inside_CB && !outside_CB)
675 else if (!(!inside_CB && outside_CB))
677 if ((angles_[end].angle - angles_[start].angle) < M_PI)
683 if ((angles_[nCB].angle > angles_[start].angle) && (angles_[nCB].angle < angles_[end].angle))
694 else if ((angles_[start].angle < angles_[sourceIdx].angle) && (angles_[sourceIdx].angle < angles_[end].angle))
708 bool is_boundary =
false, is_skinny =
false;
709 std::vector<bool> gaps (nnn_,
false);
710 std::vector<bool> skinny (nnn_,
false);
711 std::vector<double> dif (nnn_);
712 std::vector<int> angleIdx; angleIdx.reserve (nnn_);
715 for (
int j=start; j<last_visible; j++)
717 dif[j] = (angles_[j+1].angle - angles_[j].angle);
718 if (dif[j] < minimum_angle_)
720 skinny[j] = is_skinny =
true;
722 else if (maximum_angle_ <= dif[j])
724 gaps[j] = is_boundary =
true;
726 if ((!gaps[j]) && (sqr_max_edge < (coords_[angles_[j+1].index] - coords_[angles_[j].index]).squaredNorm ()))
728 gaps[j] = is_boundary =
true;
730 angleIdx.push_back(j);
733 dif[last_visible] = (2*M_PI + angles_[0].angle - angles_[last_visible].angle);
734 if (dif[last_visible] < minimum_angle_)
736 skinny[last_visible] = is_skinny =
true;
738 else if (maximum_angle_ <= dif[last_visible])
740 gaps[last_visible] = is_boundary =
true;
742 if ((!gaps[last_visible]) && (sqr_max_edge < (coords_[angles_[0].index] - coords_[angles_[last_visible].index]).squaredNorm ()))
744 gaps[last_visible] = is_boundary =
true;
746 angleIdx.push_back(last_visible);
748 for (
int j=0; j<end; j++)
750 dif[j] = (angles_[j+1].angle - angles_[j].angle);
751 if (dif[j] < minimum_angle_)
753 skinny[j] = is_skinny =
true;
755 else if (maximum_angle_ <= dif[j])
757 gaps[j] = is_boundary =
true;
759 if ((!gaps[j]) && (sqr_max_edge < (coords_[angles_[j+1].index] - coords_[angles_[j].index]).squaredNorm ()))
761 gaps[j] = is_boundary =
true;
763 angleIdx.push_back(j);
765 angleIdx.push_back(end);
770 for (
int j=start; j<end; j++)
772 dif[j] = (angles_[j+1].angle - angles_[j].angle);
773 if (dif[j] < minimum_angle_)
775 skinny[j] = is_skinny =
true;
777 else if (maximum_angle_ <= dif[j])
779 gaps[j] = is_boundary =
true;
781 if ((!gaps[j]) && (sqr_max_edge < (coords_[angles_[j+1].index] - coords_[angles_[j].index]).squaredNorm ()))
783 gaps[j] = is_boundary =
true;
785 angleIdx.push_back(j);
787 angleIdx.push_back(end);
791 state_[R_] = is_boundary ? BOUNDARY : COMPLETED;
793 std::vector<int>::iterator first_gap_after = angleIdx.end ();
794 std::vector<int>::iterator last_gap_before = angleIdx.begin ();
796 for (std::vector<int>::iterator it = angleIdx.begin (); it != angleIdx.end () - 1; it++)
801 if (first_gap_after == angleIdx.end())
802 first_gap_after = it;
803 last_gap_before = it+1;
808 angleIdx.erase(first_gap_after+1, last_gap_before);
814 double angle_so_far = 0, angle_would_be;
815 double max_combined_angle = (std::min)(maximum_angle_, M_PI-2*minimum_angle_);
819 std::vector<int> to_erase;
820 for (std::vector<int>::iterator it = angleIdx.begin()+1; it != angleIdx.end()-1; it++)
825 angle_so_far += dif[*(it-1)];
827 angle_would_be = angle_so_far;
829 angle_would_be = angle_so_far + dif[*it];
831 (skinny[*it] || skinny[*(it-1)]) &&
832 ((state_[angles_[*it].index] <= FREE) || (state_[angles_[*(it-1)].index] <= FREE)) &&
833 ((!gaps[*it]) || (angles_[*it].nnIndex > angles_[*(it-1)].nnIndex)) &&
834 ((!gaps[*(it-1)]) || (angles_[*it].nnIndex > angles_[*(it+1)].nnIndex)) &&
835 (angle_would_be < max_combined_angle)
841 to_erase.push_back(*it);
845 gaps[*(it-1)] =
true;
846 to_erase.push_back(*it);
850 std::vector<int>::iterator prev_it;
851 int erased_idx =
static_cast<int> (to_erase.size ()) -1;
852 for (prev_it = it-1; (erased_idx != -1) && (it != angleIdx.begin()); it--)
853 if (*it == to_erase[erased_idx])
857 bool can_delete =
true;
858 for (std::vector<int>::iterator curr_it = prev_it+1; curr_it != it+1; curr_it++)
860 tmp_ = coords_[angles_[*curr_it].index] - proj_qp_;
863 tmp_ = coords_[angles_[*prev_it].index] - proj_qp_;
864 S1[0] = tmp_.dot(u_);
865 S1[1] = tmp_.dot(v_);
866 tmp_ = coords_[angles_[*(it+1)].index] - proj_qp_;
867 S2[0] = tmp_.dot(u_);
868 S2[1] = tmp_.dot(v_);
879 to_erase.push_back(*it);
886 for (std::vector<int>::iterator it = to_erase.begin(); it != to_erase.end(); it++)
888 for (std::vector<int>::iterator iter = angleIdx.begin(); iter != angleIdx.end(); iter++)
891 angleIdx.erase(iter);
898 changed_1st_fn_ =
false;
899 changed_2nd_fn_ =
false;
900 new2boundary_ = NONE;
901 for (std::vector<int>::iterator it = angleIdx.begin()+1; it != angleIdx.end()-1; it++)
903 current_index_ = angles_[*it].index;
905 is_current_free_ =
false;
906 if (state_[current_index_] <= FREE)
908 state_[current_index_] = FRINGE;
909 is_current_free_ =
true;
911 else if (!already_connected_)
913 prev_is_ffn_ = (ffn_[current_index_] == angles_[*(it-1)].index) && (!gaps[*(it-1)]);
914 prev_is_sfn_ = (sfn_[current_index_] == angles_[*(it-1)].index) && (!gaps[*(it-1)]);
915 next_is_ffn_ = (ffn_[current_index_] == angles_[*(it+1)].index) && (!gaps[*it]);
916 next_is_sfn_ = (sfn_[current_index_] == angles_[*(it+1)].index) && (!gaps[*it]);
917 if (!prev_is_ffn_ && !next_is_sfn_ && !prev_is_sfn_ && !next_is_ffn_)
926 if (is_current_free_)
927 state_[current_index_] = NONE;
932 addTriangle (current_index_, angles_[*(it-1)].index, R_, polygons);
933 addFringePoint (current_index_, R_);
934 new2boundary_ = current_index_;
935 if (!already_connected_)
936 connectPoint (polygons, angles_[*(it-1)].index, R_,
937 angles_[*(it+1)].index,
938 uvn_nn[angles_[*it].nnIndex], uvn_nn[angles_[*(it-1)].nnIndex], uvn_nn_qp_zero);
939 else already_connected_ =
false;
940 if (ffn_[R_] == angles_[*(angleIdx.begin())].index)
942 ffn_[R_] = new2boundary_;
944 else if (sfn_[R_] == angles_[*(angleIdx.begin())].index)
946 sfn_[R_] = new2boundary_;
953 addFringePoint (current_index_, R_);
954 new2boundary_ = current_index_;
955 if (!already_connected_) connectPoint (polygons, R_, angles_[*(it+1)].index,
956 (it+2) == angleIdx.end() ? -1 : angles_[*(it+2)].index,
957 uvn_nn[angles_[*it].nnIndex], uvn_nn_qp_zero,
958 uvn_nn[angles_[*(it+1)].nnIndex]);
959 else already_connected_ =
false;
960 if (ffn_[R_] == angles_[*(angleIdx.end()-1)].index)
962 ffn_[R_] = new2boundary_;
964 else if (sfn_[R_] == angles_[*(angleIdx.end()-1)].index)
966 sfn_[R_] = new2boundary_;
972 addTriangle (current_index_, angles_[*(it-1)].index, R_, polygons);
973 addFringePoint (current_index_, R_);
974 if (!already_connected_) connectPoint (polygons, angles_[*(it-1)].index, angles_[*(it+1)].index,
975 (it+2) == angleIdx.end() ? -1 : gaps[*(it+1)] ? R_ : angles_[*(it+2)].index,
976 uvn_nn[angles_[*it].nnIndex],
977 uvn_nn[angles_[*(it-1)].nnIndex],
978 uvn_nn[angles_[*(it+1)].nnIndex]);
979 else already_connected_ =
false;
984 if (ffn_[R_] == sfn_[R_])
986 state_[R_] = COMPLETED;
988 if (!gaps[*(angleIdx.end()-2)])
990 addTriangle (angles_[*(angleIdx.end()-2)].index, angles_[*(angleIdx.end()-1)].index, R_, polygons);
991 addFringePoint (angles_[*(angleIdx.end()-2)].index, R_);
992 if (R_ == ffn_[angles_[*(angleIdx.end()-1)].index])
994 if (angles_[*(angleIdx.end()-2)].index == sfn_[angles_[*(angleIdx.end()-1)].index])
996 state_[angles_[*(angleIdx.end()-1)].index] = COMPLETED;
1000 ffn_[angles_[*(angleIdx.end()-1)].index] = angles_[*(angleIdx.end()-2)].index;
1003 else if (R_ == sfn_[angles_[*(angleIdx.end()-1)].index])
1005 if (angles_[*(angleIdx.end()-2)].index == ffn_[angles_[*(angleIdx.end()-1)].index])
1007 state_[angles_[*(angleIdx.end()-1)].index] = COMPLETED;
1011 sfn_[angles_[*(angleIdx.end()-1)].index] = angles_[*(angleIdx.end()-2)].index;
1015 if (!gaps[*(angleIdx.begin())])
1017 if (R_ == ffn_[angles_[*(angleIdx.begin())].index])
1019 if (angles_[*(angleIdx.begin()+1)].index == sfn_[angles_[*(angleIdx.begin())].index])
1021 state_[angles_[*(angleIdx.begin())].index] = COMPLETED;
1025 ffn_[angles_[*(angleIdx.begin())].index] = angles_[*(angleIdx.begin()+1)].index;
1028 else if (R_ == sfn_[angles_[*(angleIdx.begin())].index])
1030 if (angles_[*(angleIdx.begin()+1)].index == ffn_[angles_[*(angleIdx.begin())].index])
1032 state_[angles_[*(angleIdx.begin())].index] = COMPLETED;
1036 sfn_[angles_[*(angleIdx.begin())].index] = angles_[*(angleIdx.begin()+1)].index;
1042 PCL_DEBUG (
"Number of triangles: %lu\n", polygons.size());
1043 PCL_DEBUG (
"Number of unconnected parts: %d\n", nr_parts);
1044 if (increase_nnn4fn > 0)
1045 PCL_WARN (
"Number of neighborhood size increase requests for fringe neighbors: %d\n", increase_nnn4fn);
1046 if (increase_nnn4s > 0)
1047 PCL_WARN (
"Number of neighborhood size increase requests for source: %d\n", increase_nnn4s);
1048 if (increase_dist > 0)
1049 PCL_WARN (
"Number of automatic maximum distance increases: %d\n", increase_dist);
1052 std::sort (fringe_queue_.begin (), fringe_queue_.end ());
1053 fringe_queue_.erase (std::unique (fringe_queue_.begin (), fringe_queue_.end ()), fringe_queue_.end ());
1054 PCL_DEBUG (
"Number of processed points: %lu / %lu\n", fringe_queue_.size(), indices_->size ());
1059 template <
typename Po
intInT>
void
1062 state_[R_] = COMPLETED;
1063 addTriangle (angles_[0].index, angles_[1].index, R_, polygons);
1064 for (
int aIdx=0; aIdx<2; aIdx++)
1066 if (ffn_[angles_[aIdx].index] == R_)
1068 if (sfn_[angles_[aIdx].index] == angles_[(aIdx+1)%2].index)
1070 state_[angles_[aIdx].index] = COMPLETED;
1074 ffn_[angles_[aIdx].index] = angles_[(aIdx+1)%2].index;
1077 else if (sfn_[angles_[aIdx].index] == R_)
1079 if (ffn_[angles_[aIdx].index] == angles_[(aIdx+1)%2].index)
1081 state_[angles_[aIdx].index] = COMPLETED;
1085 sfn_[angles_[aIdx].index] = angles_[(aIdx+1)%2].index;
1092 template <
typename Po
intInT>
void
1094 std::vector<pcl::Vertices> &polygons,
1095 const int prev_index,
const int next_index,
const int next_next_index,
1096 const Eigen::Vector2f &uvn_current,
1097 const Eigen::Vector2f &uvn_prev,
1098 const Eigen::Vector2f &uvn_next)
1100 if (is_current_free_)
1102 ffn_[current_index_] = prev_index;
1103 sfn_[current_index_] = next_index;
1107 if ((prev_is_ffn_ && next_is_sfn_) || (prev_is_sfn_ && next_is_ffn_))
1108 state_[current_index_] = COMPLETED;
1109 else if (prev_is_ffn_ && !next_is_sfn_)
1110 ffn_[current_index_] = next_index;
1111 else if (next_is_ffn_ && !prev_is_sfn_)
1112 ffn_[current_index_] = prev_index;
1113 else if (prev_is_sfn_ && !next_is_ffn_)
1114 sfn_[current_index_] = next_index;
1115 else if (next_is_sfn_ && !prev_is_ffn_)
1116 sfn_[current_index_] = prev_index;
1119 bool found_triangle =
false;
1120 if ((prev_index != R_) && ((ffn_[current_index_] == ffn_[prev_index]) || (ffn_[current_index_] == sfn_[prev_index])))
1122 found_triangle =
true;
1123 addTriangle (current_index_, ffn_[current_index_], prev_index, polygons);
1124 state_[prev_index] = COMPLETED;
1125 state_[ffn_[current_index_]] = COMPLETED;
1126 ffn_[current_index_] = next_index;
1128 else if ((prev_index != R_) && ((sfn_[current_index_] == ffn_[prev_index]) || (sfn_[current_index_] == sfn_[prev_index])))
1130 found_triangle =
true;
1131 addTriangle (current_index_, sfn_[current_index_], prev_index, polygons);
1132 state_[prev_index] = COMPLETED;
1133 state_[sfn_[current_index_]] = COMPLETED;
1134 sfn_[current_index_] = next_index;
1136 else if (state_[next_index] > FREE)
1138 if ((ffn_[current_index_] == ffn_[next_index]) || (ffn_[current_index_] == sfn_[next_index]))
1140 found_triangle =
true;
1141 addTriangle (current_index_, ffn_[current_index_], next_index, polygons);
1143 if (ffn_[current_index_] == ffn_[next_index])
1145 ffn_[next_index] = current_index_;
1149 sfn_[next_index] = current_index_;
1151 state_[ffn_[current_index_]] = COMPLETED;
1152 ffn_[current_index_] = prev_index;
1154 else if ((sfn_[current_index_] == ffn_[next_index]) || (sfn_[current_index_] == sfn_[next_index]))
1156 found_triangle =
true;
1157 addTriangle (current_index_, sfn_[current_index_], next_index, polygons);
1159 if (sfn_[current_index_] == ffn_[next_index])
1161 ffn_[next_index] = current_index_;
1165 sfn_[next_index] = current_index_;
1167 state_[sfn_[current_index_]] = COMPLETED;
1168 sfn_[current_index_] = prev_index;
1177 tmp_ = coords_[ffn_[current_index_]] - proj_qp_;
1178 uvn_ffn_[0] = tmp_.dot(u_);
1179 uvn_ffn_[1] = tmp_.dot(v_);
1180 tmp_ = coords_[sfn_[current_index_]] - proj_qp_;
1181 uvn_sfn_[0] = tmp_.dot(u_);
1182 uvn_sfn_[1] = tmp_.dot(v_);
1183 bool prev_ffn =
isVisible(uvn_prev, uvn_next, uvn_current, uvn_ffn_) &&
isVisible(uvn_prev, uvn_sfn_, uvn_current, uvn_ffn_);
1184 bool prev_sfn =
isVisible(uvn_prev, uvn_next, uvn_current, uvn_sfn_) &&
isVisible(uvn_prev, uvn_ffn_, uvn_current, uvn_sfn_);
1185 bool next_ffn =
isVisible(uvn_next, uvn_prev, uvn_current, uvn_ffn_) &&
isVisible(uvn_next, uvn_sfn_, uvn_current, uvn_ffn_);
1186 bool next_sfn =
isVisible(uvn_next, uvn_prev, uvn_current, uvn_sfn_) &&
isVisible(uvn_next, uvn_ffn_, uvn_current, uvn_sfn_);
1188 if (prev_ffn && next_sfn && prev_sfn && next_ffn)
1191 double prev2f = (coords_[ffn_[current_index_]] - coords_[prev_index]).squaredNorm ();
1192 double next2s = (coords_[sfn_[current_index_]] - coords_[next_index]).squaredNorm ();
1193 double prev2s = (coords_[sfn_[current_index_]] - coords_[prev_index]).squaredNorm ();
1194 double next2f = (coords_[ffn_[current_index_]] - coords_[next_index]).squaredNorm ();
1195 if (prev2f < prev2s)
1197 if (prev2f < next2f)
1199 if (prev2f < next2s)
1206 if (next2f < next2s)
1214 if (prev2s < next2f)
1216 if (prev2s < next2s)
1223 if (next2f < next2s)
1230 else if (prev_ffn && next_sfn)
1233 double prev2f = (coords_[ffn_[current_index_]] - coords_[prev_index]).squaredNorm ();
1234 double next2s = (coords_[sfn_[current_index_]] - coords_[next_index]).squaredNorm ();
1235 if (prev2f < next2s)
1240 else if (prev_sfn && next_ffn)
1243 double prev2s = (coords_[sfn_[current_index_]] - coords_[prev_index]).squaredNorm ();
1244 double next2f = (coords_[ffn_[current_index_]] - coords_[next_index]).squaredNorm ();
1245 if (prev2s < next2f)
1251 else if (prev_ffn && !next_sfn && !prev_sfn && !next_ffn)
1253 else if (!prev_ffn && !next_sfn && prev_sfn && !next_ffn)
1255 else if (!prev_ffn && !next_sfn && !prev_sfn && next_ffn)
1257 else if (!prev_ffn && next_sfn && !prev_sfn && !next_ffn)
1262 double prev2f = (coords_[ffn_[current_index_]] - coords_[prev_index]).squaredNorm ();
1265 double prev2s = (coords_[sfn_[current_index_]] - coords_[prev_index]).squaredNorm ();
1266 if (prev2s < prev2f)
1273 double next2f = (coords_[ffn_[current_index_]] - coords_[next_index]).squaredNorm ();
1274 if (next2f < prev2f)
1282 double next2s = (coords_[sfn_[current_index_]] - coords_[next_index]).squaredNorm ();
1285 double prev2s = (coords_[sfn_[current_index_]] - coords_[prev_index]).squaredNorm ();
1286 if (prev2s < next2s)
1293 double next2f = (coords_[ffn_[current_index_]] - coords_[next_index]).squaredNorm ();
1294 if (next2f < next2s)
1304 addTriangle (current_index_, ffn_[current_index_], prev_index, polygons);
1307 if (ffn_[prev_index] == current_index_)
1309 ffn_[prev_index] = ffn_[current_index_];
1311 else if (sfn_[prev_index] == current_index_)
1313 sfn_[prev_index] = ffn_[current_index_];
1315 else if (ffn_[prev_index] == R_)
1317 changed_1st_fn_ =
true;
1318 ffn_[prev_index] = ffn_[current_index_];
1320 else if (sfn_[prev_index] == R_)
1322 changed_1st_fn_ =
true;
1323 sfn_[prev_index] = ffn_[current_index_];
1325 else if (prev_index == R_)
1327 new2boundary_ = ffn_[current_index_];
1331 if (ffn_[ffn_[current_index_]] == current_index_)
1333 ffn_[ffn_[current_index_]] = prev_index;
1335 else if (sfn_[ffn_[current_index_]] == current_index_)
1337 sfn_[ffn_[current_index_]] = prev_index;
1341 ffn_[current_index_] = next_index;
1347 addTriangle (current_index_, sfn_[current_index_], prev_index, polygons);
1350 if (ffn_[prev_index] == current_index_)
1352 ffn_[prev_index] = sfn_[current_index_];
1354 else if (sfn_[prev_index] == current_index_)
1356 sfn_[prev_index] = sfn_[current_index_];
1358 else if (ffn_[prev_index] == R_)
1360 changed_1st_fn_ =
true;
1361 ffn_[prev_index] = sfn_[current_index_];
1363 else if (sfn_[prev_index] == R_)
1365 changed_1st_fn_ =
true;
1366 sfn_[prev_index] = sfn_[current_index_];
1368 else if (prev_index == R_)
1370 new2boundary_ = sfn_[current_index_];
1374 if (ffn_[sfn_[current_index_]] == current_index_)
1376 ffn_[sfn_[current_index_]] = prev_index;
1378 else if (sfn_[sfn_[current_index_]] == current_index_)
1380 sfn_[sfn_[current_index_]] = prev_index;
1384 sfn_[current_index_] = next_index;
1390 addTriangle (current_index_, ffn_[current_index_], next_index, polygons);
1391 int neighbor_update = next_index;
1394 if (state_[next_index] <= FREE)
1396 state_[next_index] = FRINGE;
1397 ffn_[next_index] = current_index_;
1398 sfn_[next_index] = ffn_[current_index_];
1402 if (ffn_[next_index] == R_)
1404 changed_2nd_fn_ =
true;
1405 ffn_[next_index] = ffn_[current_index_];
1407 else if (sfn_[next_index] == R_)
1409 changed_2nd_fn_ =
true;
1410 sfn_[next_index] = ffn_[current_index_];
1412 else if (next_index == R_)
1414 new2boundary_ = ffn_[current_index_];
1415 if (next_next_index == new2boundary_)
1416 already_connected_ =
true;
1418 else if (ffn_[next_index] == next_next_index)
1420 already_connected_ =
true;
1421 ffn_[next_index] = ffn_[current_index_];
1423 else if (sfn_[next_index] == next_next_index)
1425 already_connected_ =
true;
1426 sfn_[next_index] = ffn_[current_index_];
1430 tmp_ = coords_[ffn_[next_index]] - proj_qp_;
1431 uvn_next_ffn_[0] = tmp_.dot(u_);
1432 uvn_next_ffn_[1] = tmp_.dot(v_);
1433 tmp_ = coords_[sfn_[next_index]] - proj_qp_;
1434 uvn_next_sfn_[0] = tmp_.dot(u_);
1435 uvn_next_sfn_[1] = tmp_.dot(v_);
1437 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_);
1438 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_);
1440 int connect2ffn = -1;
1441 if (ffn_next_ffn && sfn_next_ffn)
1443 double fn2f = (coords_[ffn_[current_index_]] - coords_[ffn_[next_index]]).squaredNorm ();
1444 double sn2f = (coords_[ffn_[current_index_]] - coords_[sfn_[next_index]]).squaredNorm ();
1445 if (fn2f < sn2f) connect2ffn = 0;
1446 else connect2ffn = 1;
1448 else if (ffn_next_ffn) connect2ffn = 0;
1449 else if (sfn_next_ffn) connect2ffn = 1;
1451 switch (connect2ffn)
1455 addTriangle (next_index, ffn_[current_index_], ffn_[next_index], polygons);
1456 neighbor_update = ffn_[next_index];
1459 if ((ffn_[ffn_[next_index]] == ffn_[current_index_]) || (sfn_[ffn_[next_index]] == ffn_[current_index_]))
1461 state_[ffn_[next_index]] = COMPLETED;
1463 else if (ffn_[ffn_[next_index]] == next_index)
1465 ffn_[ffn_[next_index]] = ffn_[current_index_];
1467 else if (sfn_[ffn_[next_index]] == next_index)
1469 sfn_[ffn_[next_index]] = ffn_[current_index_];
1472 ffn_[next_index] = current_index_;
1478 addTriangle (next_index, ffn_[current_index_], sfn_[next_index], polygons);
1479 neighbor_update = sfn_[next_index];
1482 if ((ffn_[sfn_[next_index]] == ffn_[current_index_]) || (sfn_[sfn_[next_index]] == ffn_[current_index_]))
1484 state_[sfn_[next_index]] = COMPLETED;
1486 else if (ffn_[sfn_[next_index]] == next_index)
1488 ffn_[sfn_[next_index]] = ffn_[current_index_];
1490 else if (sfn_[sfn_[next_index]] == next_index)
1492 sfn_[sfn_[next_index]] = ffn_[current_index_];
1495 sfn_[next_index] = current_index_;
1505 if ((ffn_[ffn_[current_index_]] == neighbor_update) || (sfn_[ffn_[current_index_]] == neighbor_update))
1507 state_[ffn_[current_index_]] = COMPLETED;
1509 else if (ffn_[ffn_[current_index_]] == current_index_)
1511 ffn_[ffn_[current_index_]] = neighbor_update;
1513 else if (sfn_[ffn_[current_index_]] == current_index_)
1515 sfn_[ffn_[current_index_]] = neighbor_update;
1519 ffn_[current_index_] = prev_index;
1525 addTriangle (current_index_, sfn_[current_index_], next_index, polygons);
1526 int neighbor_update = next_index;
1529 if (state_[next_index] <= FREE)
1531 state_[next_index] = FRINGE;
1532 ffn_[next_index] = current_index_;
1533 sfn_[next_index] = sfn_[current_index_];
1537 if (ffn_[next_index] == R_)
1539 changed_2nd_fn_ =
true;
1540 ffn_[next_index] = sfn_[current_index_];
1542 else if (sfn_[next_index] == R_)
1544 changed_2nd_fn_ =
true;
1545 sfn_[next_index] = sfn_[current_index_];
1547 else if (next_index == R_)
1549 new2boundary_ = sfn_[current_index_];
1550 if (next_next_index == new2boundary_)
1551 already_connected_ =
true;
1553 else if (ffn_[next_index] == next_next_index)
1555 already_connected_ =
true;
1556 ffn_[next_index] = sfn_[current_index_];
1558 else if (sfn_[next_index] == next_next_index)
1560 already_connected_ =
true;
1561 sfn_[next_index] = sfn_[current_index_];
1565 tmp_ = coords_[ffn_[next_index]] - proj_qp_;
1566 uvn_next_ffn_[0] = tmp_.dot(u_);
1567 uvn_next_ffn_[1] = tmp_.dot(v_);
1568 tmp_ = coords_[sfn_[next_index]] - proj_qp_;
1569 uvn_next_sfn_[0] = tmp_.dot(u_);
1570 uvn_next_sfn_[1] = tmp_.dot(v_);
1572 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_);
1573 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_);
1575 int connect2sfn = -1;
1576 if (ffn_next_sfn && sfn_next_sfn)
1578 double fn2s = (coords_[sfn_[current_index_]] - coords_[ffn_[next_index]]).squaredNorm ();
1579 double sn2s = (coords_[sfn_[current_index_]] - coords_[sfn_[next_index]]).squaredNorm ();
1580 if (fn2s < sn2s) connect2sfn = 0;
1581 else connect2sfn = 1;
1583 else if (ffn_next_sfn) connect2sfn = 0;
1584 else if (sfn_next_sfn) connect2sfn = 1;
1586 switch (connect2sfn)
1590 addTriangle (next_index, sfn_[current_index_], ffn_[next_index], polygons);
1591 neighbor_update = ffn_[next_index];
1594 if ((ffn_[ffn_[next_index]] == sfn_[current_index_]) || (sfn_[ffn_[next_index]] == sfn_[current_index_]))
1596 state_[ffn_[next_index]] = COMPLETED;
1598 else if (ffn_[ffn_[next_index]] == next_index)
1600 ffn_[ffn_[next_index]] = sfn_[current_index_];
1602 else if (sfn_[ffn_[next_index]] == next_index)
1604 sfn_[ffn_[next_index]] = sfn_[current_index_];
1607 ffn_[next_index] = current_index_;
1613 addTriangle (next_index, sfn_[current_index_], sfn_[next_index], polygons);
1614 neighbor_update = sfn_[next_index];
1617 if ((ffn_[sfn_[next_index]] == sfn_[current_index_]) || (sfn_[sfn_[next_index]] == sfn_[current_index_]))
1619 state_[sfn_[next_index]] = COMPLETED;
1621 else if (ffn_[sfn_[next_index]] == next_index)
1623 ffn_[sfn_[next_index]] = sfn_[current_index_];
1625 else if (sfn_[sfn_[next_index]] == next_index)
1627 sfn_[sfn_[next_index]] = sfn_[current_index_];
1630 sfn_[next_index] = current_index_;
1640 if ((ffn_[sfn_[current_index_]] == neighbor_update) || (sfn_[sfn_[current_index_]] == neighbor_update))
1642 state_[sfn_[current_index_]] = COMPLETED;
1644 else if (ffn_[sfn_[current_index_]] == current_index_)
1646 ffn_[sfn_[current_index_]] = neighbor_update;
1648 else if (sfn_[sfn_[current_index_]] == current_index_)
1650 sfn_[sfn_[current_index_]] = neighbor_update;
1653 sfn_[current_index_] = prev_index;
1665 template <
typename Po
intInT> std::vector<std::vector<size_t> >
1670 for (
size_t i=0; i < input.
polygons.size (); ++i)
1671 for (
size_t j=0; j < input.
polygons[i].vertices.size (); ++j)
1672 triangleList[input.
polygons[i].vertices[j]].push_back (i);
1673 return (triangleList);
1676 #define PCL_INSTANTIATE_GreedyProjectionTriangulation(T) \
1677 template class PCL_EXPORTS pcl::GreedyProjectionTriangulation<T>;
1679 #endif // PCL_SURFACE_IMPL_GP3_H_
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...
GreedyProjectionTriangulation is an implementation of a greedy triangulation algorithm for 3D points ...
std::vector< ::pcl::Vertices > polygons
int cp(int from, int to)
Returns field copy operation code.
::pcl::PCLPointCloud2 cloud
std::vector< pcl::uint8_t > data