00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #ifndef PCL_SURFACE_IMPL_GP3_H_
00039 #define PCL_SURFACE_IMPL_GP3_H_
00040
00041 #include <pcl/surface/gp3.h>
00042 #include <pcl/kdtree/impl/kdtree_flann.hpp>
00043
00045 template <typename PointInT> void
00046 pcl::GreedyProjectionTriangulation<PointInT>::performReconstruction (pcl::PolygonMesh &output)
00047 {
00048 output.polygons.clear ();
00049 output.polygons.reserve (2 * indices_->size ());
00050 if (!reconstructPolygons (output.polygons))
00051 {
00052 PCL_ERROR ("[pcl::%s::performReconstruction] Reconstruction failed. Check parameters: search radius (%f) or mu (%f) before continuing.\n", getClassName ().c_str (), search_radius_, mu_);
00053 output.cloud.width = output.cloud.height = 0;
00054 output.cloud.data.clear ();
00055 return;
00056 }
00057 }
00058
00060 template <typename PointInT> void
00061 pcl::GreedyProjectionTriangulation<PointInT>::performReconstruction (std::vector<pcl::Vertices> &polygons)
00062 {
00063 polygons.clear ();
00064 polygons.reserve (2 * indices_->size ());
00065 if (!reconstructPolygons (polygons))
00066 {
00067 PCL_ERROR ("[pcl::%s::performReconstruction] Reconstruction failed. Check parameters: search radius (%f) or mu (%f) before continuing.\n", getClassName ().c_str (), search_radius_, mu_);
00068 return;
00069 }
00070 }
00071
00073 template <typename PointInT> bool
00074 pcl::GreedyProjectionTriangulation<PointInT>::reconstructPolygons (std::vector<pcl::Vertices> &polygons)
00075 {
00076 if (search_radius_ <= 0 || mu_ <= 0)
00077 {
00078 polygons.clear ();
00079 return (false);
00080 }
00081 const double sqr_mu = mu_*mu_;
00082 const double sqr_max_edge = search_radius_*search_radius_;
00083 if (nnn_ > static_cast<int> (indices_->size ()))
00084 nnn_ = static_cast<int> (indices_->size ());
00085
00086
00087 std::vector<int> nnIdx (nnn_);
00088 std::vector<float> sqrDists (nnn_);
00089
00090
00091 int part_index = 0;
00092
00093
00094 const Eigen::Vector2f uvn_nn_qp_zero = Eigen::Vector2f::Zero();
00095 Eigen::Vector2f uvn_current;
00096 Eigen::Vector2f uvn_prev;
00097 Eigen::Vector2f uvn_next;
00098
00099
00100 already_connected_ = false;
00101
00102
00103 part_.clear ();
00104 state_.clear ();
00105 source_.clear ();
00106 ffn_.clear ();
00107 sfn_.clear ();
00108 part_.resize(indices_->size (), -1);
00109 state_.resize(indices_->size (), FREE);
00110 source_.resize(indices_->size (), NONE);
00111 ffn_.resize(indices_->size (), NONE);
00112 sfn_.resize(indices_->size (), NONE);
00113 fringe_queue_.clear ();
00114 int fqIdx = 0;
00115
00116
00117 if (!input_->is_dense)
00118 {
00119
00120 for (std::vector<int>::const_iterator it = indices_->begin (); it != indices_->end (); ++it)
00121 if (!pcl_isfinite (input_->points[*it].x) ||
00122 !pcl_isfinite (input_->points[*it].y) ||
00123 !pcl_isfinite (input_->points[*it].z))
00124 state_[*it] = NONE;
00125 }
00126
00127
00128 coords_.clear ();
00129 coords_.reserve (indices_->size ());
00130 std::vector<int> point2index (input_->points.size (), -1);
00131 for (int cp = 0; cp < static_cast<int> (indices_->size ()); ++cp)
00132 {
00133 coords_.push_back(input_->points[(*indices_)[cp]].getVector3fMap());
00134 point2index[(*indices_)[cp]] = cp;
00135 }
00136
00137
00138 int is_free=0, nr_parts=0, increase_nnn4fn=0, increase_nnn4s=0, increase_dist=0, nr_touched = 0;
00139 bool is_fringe;
00140 angles_.resize(nnn_);
00141 std::vector<Eigen::Vector2f> uvn_nn (nnn_);
00142 Eigen::Vector2f uvn_s;
00143
00144
00145 while (is_free != NONE)
00146 {
00147 R_ = is_free;
00148 if (state_[R_] == FREE)
00149 {
00150 state_[R_] = NONE;
00151 part_[R_] = part_index++;
00152
00153
00154
00155
00156 tree_->nearestKSearch (indices_->at (R_), nnn_, nnIdx, sqrDists);
00157 double sqr_dist_threshold = (std::min)(sqr_max_edge, sqr_mu * sqrDists[1]);
00158
00159
00160 for (int i = 1; i < nnn_; i++)
00161 {
00162
00163
00164 nnIdx[i] = point2index[nnIdx[i]];
00165 }
00166
00167
00168 const Eigen::Vector3f nc = input_->points[(*indices_)[R_]].getNormalVector3fMap ();
00169
00170
00171 v_ = nc.unitOrthogonal ();
00172 u_ = nc.cross (v_);
00173
00174
00175 float dist = nc.dot (coords_[R_]);
00176 proj_qp_ = coords_[R_] - dist * nc;
00177
00178
00179 int nr_edge = 0;
00180 std::vector<doubleEdge> doubleEdges;
00181 for (int i = 1; i < nnn_; i++)
00182 {
00183
00184 tmp_ = coords_[nnIdx[i]] - proj_qp_;
00185 uvn_nn[i][0] = tmp_.dot(u_);
00186 uvn_nn[i][1] = tmp_.dot(v_);
00187
00188 angles_[i].angle = atan2(uvn_nn[i][1], uvn_nn[i][0]);
00189
00190 angles_[i].index = nnIdx[i];
00191 if (
00192 (state_[nnIdx[i]] == COMPLETED) || (state_[nnIdx[i]] == BOUNDARY)
00193 || (state_[nnIdx[i]] == NONE) || (nnIdx[i] == -1)
00194 || (sqrDists[i] > sqr_dist_threshold)
00195 )
00196 angles_[i].visible = false;
00197 else
00198 angles_[i].visible = true;
00199
00200 if ((state_[nnIdx[i]] == FRINGE) || (state_[nnIdx[i]] == BOUNDARY))
00201 {
00202 doubleEdge e;
00203 e.index = i;
00204 nr_edge++;
00205 tmp_ = coords_[ffn_[nnIdx[i]]] - proj_qp_;
00206 e.first[0] = tmp_.dot(u_);
00207 e.first[1] = tmp_.dot(v_);
00208 tmp_ = coords_[sfn_[nnIdx[i]]] - proj_qp_;
00209 e.second[0] = tmp_.dot(u_);
00210 e.second[1] = tmp_.dot(v_);
00211 doubleEdges.push_back(e);
00212 }
00213 }
00214 angles_[0].visible = false;
00215
00216
00217 for (int i = 1; i < nnn_; i++)
00218 if ((angles_[i].visible) && (ffn_[R_] != nnIdx[i]) && (sfn_[R_] != nnIdx[i]))
00219 {
00220 bool visibility = true;
00221 for (int j = 0; j < nr_edge; j++)
00222 {
00223 if (ffn_[nnIdx[doubleEdges[j].index]] != nnIdx[i])
00224 visibility = isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].first, Eigen::Vector2f::Zero());
00225 if (!visibility)
00226 break;
00227 if (sfn_[nnIdx[doubleEdges[j].index]] != nnIdx[i])
00228 visibility = isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].second, Eigen::Vector2f::Zero());
00229 if (!visibility == false)
00230 break;
00231 }
00232 angles_[i].visible = visibility;
00233 }
00234
00235
00236 bool not_found = true;
00237 int left = 1;
00238 do
00239 {
00240 while ((left < nnn_) && ((!angles_[left].visible) || (state_[nnIdx[left]] > FREE))) left++;
00241 if (left >= nnn_)
00242 break;
00243 else
00244 {
00245 int right = left+1;
00246 do
00247 {
00248 while ((right < nnn_) && ((!angles_[right].visible) || (state_[nnIdx[right]] > FREE))) right++;
00249 if (right >= nnn_)
00250 break;
00251 else if ((coords_[nnIdx[left]] - coords_[nnIdx[right]]).squaredNorm () > sqr_max_edge)
00252 right++;
00253 else
00254 {
00255 addFringePoint (nnIdx[right], R_);
00256 addFringePoint (nnIdx[left], nnIdx[right]);
00257 addFringePoint (R_, nnIdx[left]);
00258 state_[R_] = state_[nnIdx[left]] = state_[nnIdx[right]] = FRINGE;
00259 ffn_[R_] = nnIdx[left];
00260 sfn_[R_] = nnIdx[right];
00261 ffn_[nnIdx[left]] = nnIdx[right];
00262 sfn_[nnIdx[left]] = R_;
00263 ffn_[nnIdx[right]] = R_;
00264 sfn_[nnIdx[right]] = nnIdx[left];
00265 addTriangle (R_, nnIdx[left], nnIdx[right], polygons);
00266 nr_parts++;
00267 not_found = false;
00268 break;
00269 }
00270 }
00271 while (true);
00272 left++;
00273 }
00274 }
00275 while (not_found);
00276 }
00277
00278 is_free = NONE;
00279 for (unsigned temp = 0; temp < indices_->size (); temp++)
00280 {
00281 if (state_[temp] == FREE)
00282 {
00283 is_free = temp;
00284 break;
00285 }
00286 }
00287
00288 is_fringe = true;
00289 while (is_fringe)
00290 {
00291 is_fringe = false;
00292
00293 int fqSize = static_cast<int> (fringe_queue_.size ());
00294 while ((fqIdx < fqSize) && (state_[fringe_queue_[fqIdx]] != FRINGE))
00295 fqIdx++;
00296
00297
00298 if (fqIdx >= fqSize)
00299 {
00300 continue;
00301 }
00302
00303 R_ = fringe_queue_[fqIdx];
00304 is_fringe = true;
00305
00306 if (ffn_[R_] == sfn_[R_])
00307 {
00308 state_[R_] = COMPLETED;
00309 continue;
00310 }
00311
00312
00313 tree_->nearestKSearch (indices_->at (R_), nnn_, nnIdx, sqrDists);
00314
00315
00316 for (int i = 1; i < nnn_; i++)
00317 {
00318
00319
00320 nnIdx[i] = point2index[nnIdx[i]];
00321 }
00322
00323
00324 double sqr_source_dist = (coords_[R_] - coords_[source_[R_]]).squaredNorm ();
00325 double sqr_ffn_dist = (coords_[R_] - coords_[ffn_[R_]]).squaredNorm ();
00326 double sqr_sfn_dist = (coords_[R_] - coords_[sfn_[R_]]).squaredNorm ();
00327 double max_sqr_fn_dist = (std::max)(sqr_ffn_dist, sqr_sfn_dist);
00328 double sqr_dist_threshold = (std::min)(sqr_max_edge, sqr_mu * sqrDists[1]);
00329 if (max_sqr_fn_dist > sqrDists[nnn_-1])
00330 {
00331 if (0 == increase_nnn4fn)
00332 PCL_WARN("Not enough neighbors are considered: ffn or sfn out of range! Consider increasing nnn_... Setting R=%d to be BOUNDARY!\n", R_);
00333 increase_nnn4fn++;
00334 state_[R_] = BOUNDARY;
00335 continue;
00336 }
00337 double max_sqr_fns_dist = (std::max)(sqr_source_dist, max_sqr_fn_dist);
00338 if (max_sqr_fns_dist > sqrDists[nnn_-1])
00339 {
00340 if (0 == increase_nnn4s)
00341 PCL_WARN("Not enough neighbors are considered: source of R=%d is out of range! Consider increasing nnn_...\n", R_);
00342 increase_nnn4s++;
00343 }
00344
00345
00346 const Eigen::Vector3f nc = input_->points[(*indices_)[R_]].getNormalVector3fMap ();
00347
00348
00349 v_ = nc.unitOrthogonal ();
00350 u_ = nc.cross (v_);
00351
00352
00353 float dist = nc.dot (coords_[R_]);
00354 proj_qp_ = coords_[R_] - dist * nc;
00355
00356
00357 int nr_edge = 0;
00358 std::vector<doubleEdge> doubleEdges;
00359 for (int i = 1; i < nnn_; i++)
00360 {
00361 tmp_ = coords_[nnIdx[i]] - proj_qp_;
00362 uvn_nn[i][0] = tmp_.dot(u_);
00363 uvn_nn[i][1] = tmp_.dot(v_);
00364
00365
00366 angles_[i].angle = atan2(uvn_nn[i][1], uvn_nn[i][0]);
00367
00368 angles_[i].index = nnIdx[i];
00369 angles_[i].nnIndex = i;
00370 if (
00371 (state_[nnIdx[i]] == COMPLETED) || (state_[nnIdx[i]] == BOUNDARY)
00372 || (state_[nnIdx[i]] == NONE) || (nnIdx[i] == -1)
00373 || (sqrDists[i] > sqr_dist_threshold)
00374 )
00375 angles_[i].visible = false;
00376 else
00377 angles_[i].visible = true;
00378 if ((ffn_[R_] == nnIdx[i]) || (sfn_[R_] == nnIdx[i]))
00379 angles_[i].visible = true;
00380 bool same_side = true;
00381 const Eigen::Vector3f neighbor_normal = input_->points[(*indices_)[nnIdx[i]]].getNormalVector3fMap ();
00382 double cosine = nc.dot (neighbor_normal);
00383 if (cosine > 1) cosine = 1;
00384 if (cosine < -1) cosine = -1;
00385 double angle = acos (cosine);
00386 if ((!consistent_) && (angle > M_PI/2))
00387 angle = M_PI - angle;
00388 if (angle > eps_angle_)
00389 {
00390 angles_[i].visible = false;
00391 same_side = false;
00392 }
00393
00394 if ((i!=0) && (same_side) && ((state_[nnIdx[i]] == FRINGE) || (state_[nnIdx[i]] == BOUNDARY)))
00395 {
00396 doubleEdge e;
00397 e.index = i;
00398 nr_edge++;
00399 tmp_ = coords_[ffn_[nnIdx[i]]] - proj_qp_;
00400 e.first[0] = tmp_.dot(u_);
00401 e.first[1] = tmp_.dot(v_);
00402 tmp_ = coords_[sfn_[nnIdx[i]]] - proj_qp_;
00403 e.second[0] = tmp_.dot(u_);
00404 e.second[1] = tmp_.dot(v_);
00405 doubleEdges.push_back(e);
00406
00407 if ((state_[nnIdx[i]] == FRINGE) && (ffn_[R_] != nnIdx[i]) && (sfn_[R_] != nnIdx[i]))
00408 {
00409 double angle1 = atan2(e.first[1] - uvn_nn[i][1], e.first[0] - uvn_nn[i][0]);
00410 double angle2 = atan2(e.second[1] - uvn_nn[i][1], e.second[0] - uvn_nn[i][0]);
00411 double angleMin, angleMax;
00412 if (angle1 < angle2)
00413 {
00414 angleMin = angle1;
00415 angleMax = angle2;
00416 }
00417 else
00418 {
00419 angleMin = angle2;
00420 angleMax = angle1;
00421 }
00422 double angleR = angles_[i].angle + M_PI;
00423 if (angleR >= 2*M_PI)
00424 angleR -= 2*M_PI;
00425 if ((source_[nnIdx[i]] == ffn_[nnIdx[i]]) || (source_[nnIdx[i]] == sfn_[nnIdx[i]]))
00426 {
00427 if ((angleMax - angleMin) < M_PI)
00428 {
00429 if ((angleMin < angleR) && (angleR < angleMax))
00430 angles_[i].visible = false;
00431 }
00432 else
00433 {
00434 if ((angleR < angleMin) || (angleMax < angleR))
00435 angles_[i].visible = false;
00436 }
00437 }
00438 else
00439 {
00440 tmp_ = coords_[source_[nnIdx[i]]] - proj_qp_;
00441 uvn_s[0] = tmp_.dot(u_);
00442 uvn_s[1] = tmp_.dot(v_);
00443 double angleS = atan2(uvn_s[1] - uvn_nn[i][1], uvn_s[0] - uvn_nn[i][0]);
00444 if ((angleMin < angleS) && (angleS < angleMax))
00445 {
00446 if ((angleMin < angleR) && (angleR < angleMax))
00447 angles_[i].visible = false;
00448 }
00449 else
00450 {
00451 if ((angleR < angleMin) || (angleMax < angleR))
00452 angles_[i].visible = false;
00453 }
00454 }
00455 }
00456 }
00457 }
00458 angles_[0].visible = false;
00459
00460
00461 for (int i = 1; i < nnn_; i++)
00462 if ((angles_[i].visible) && (ffn_[R_] != nnIdx[i]) && (sfn_[R_] != nnIdx[i]))
00463 {
00464 bool visibility = true;
00465 for (int j = 0; j < nr_edge; j++)
00466 {
00467 if (doubleEdges[j].index != i)
00468 {
00469 int f = ffn_[nnIdx[doubleEdges[j].index]];
00470 if ((f != nnIdx[i]) && (f != R_))
00471 visibility = isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].first, Eigen::Vector2f::Zero());
00472 if (visibility == false)
00473 break;
00474
00475 int s = sfn_[nnIdx[doubleEdges[j].index]];
00476 if ((s != nnIdx[i]) && (s != R_))
00477 visibility = isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].second, Eigen::Vector2f::Zero());
00478 if (visibility == false)
00479 break;
00480 }
00481 }
00482 angles_[i].visible = visibility;
00483 }
00484
00485
00486 std::sort (angles_.begin (), angles_.end (), GreedyProjectionTriangulation<PointInT>::nnAngleSortAsc);
00487
00488
00489 if (angles_[2].visible == false)
00490 {
00491 if ( !( (angles_[0].index == ffn_[R_] && angles_[1].index == sfn_[R_]) || (angles_[0].index == sfn_[R_] && angles_[1].index == ffn_[R_]) ) )
00492 {
00493 state_[R_] = BOUNDARY;
00494 }
00495 else
00496 {
00497 if ((source_[R_] == angles_[0].index) || (source_[R_] == angles_[1].index))
00498 state_[R_] = BOUNDARY;
00499 else
00500 {
00501 if (sqr_max_edge < (coords_[ffn_[R_]] - coords_[sfn_[R_]]).squaredNorm ())
00502 {
00503 state_[R_] = BOUNDARY;
00504 }
00505 else
00506 {
00507 tmp_ = coords_[source_[R_]] - proj_qp_;
00508 uvn_s[0] = tmp_.dot(u_);
00509 uvn_s[1] = tmp_.dot(v_);
00510 double angleS = atan2(uvn_s[1], uvn_s[0]);
00511 double dif = angles_[1].angle - angles_[0].angle;
00512 if ((angles_[0].angle < angleS) && (angleS < angles_[1].angle))
00513 {
00514 if (dif < 2*M_PI - maximum_angle_)
00515 state_[R_] = BOUNDARY;
00516 else
00517 closeTriangle (polygons);
00518 }
00519 else
00520 {
00521 if (dif >= maximum_angle_)
00522 state_[R_] = BOUNDARY;
00523 else
00524 closeTriangle (polygons);
00525 }
00526 }
00527 }
00528 }
00529 continue;
00530 }
00531
00532
00533 int start = -1, end = -1;
00534 for (int i=0; i<nnn_; i++)
00535 {
00536 if (ffn_[R_] == angles_[i].index)
00537 {
00538 start = i;
00539 if (sfn_[R_] == angles_[i+1].index)
00540 end = i+1;
00541 else
00542 if (i==0)
00543 {
00544 for (i = i+2; i < nnn_; i++)
00545 if (sfn_[R_] == angles_[i].index)
00546 break;
00547 end = i;
00548 }
00549 else
00550 {
00551 for (i = i+2; i < nnn_; i++)
00552 if (sfn_[R_] == angles_[i].index)
00553 break;
00554 end = i;
00555 }
00556 break;
00557 }
00558 if (sfn_[R_] == angles_[i].index)
00559 {
00560 start = i;
00561 if (ffn_[R_] == angles_[i+1].index)
00562 end = i+1;
00563 else
00564 if (i==0)
00565 {
00566 for (i = i+2; i < nnn_; i++)
00567 if (ffn_[R_] == angles_[i].index)
00568 break;
00569 end = i;
00570 }
00571 else
00572 {
00573 for (i = i+2; i < nnn_; i++)
00574 if (ffn_[R_] == angles_[i].index)
00575 break;
00576 end = i;
00577 }
00578 break;
00579 }
00580 }
00581
00582
00583 if ((start < 0) || (end < 0) || (end == nnn_) || (!angles_[start].visible) || (!angles_[end].visible))
00584 {
00585 state_[R_] = BOUNDARY;
00586 continue;
00587 }
00588
00589
00590 int last_visible = end;
00591 while ((last_visible+1<nnn_) && (angles_[last_visible+1].visible)) last_visible++;
00592
00593
00594 bool need_invert = false;
00595 int sourceIdx = nnn_;
00596 if ((source_[R_] == ffn_[R_]) || (source_[R_] == sfn_[R_]))
00597 {
00598 if ((angles_[end].angle - angles_[start].angle) < M_PI)
00599 need_invert = true;
00600 }
00601 else
00602 {
00603 for (sourceIdx=0; sourceIdx<nnn_; sourceIdx++)
00604 if (angles_[sourceIdx].index == source_[R_])
00605 break;
00606 if (sourceIdx == nnn_)
00607 {
00608 int vis_free = NONE, nnCB = NONE;
00609 for (int i = 1; i < nnn_; i++)
00610 {
00611
00612 if ((state_[nnIdx[i]] == COMPLETED) || (state_[nnIdx[i]] == BOUNDARY))
00613 {
00614 if (nnCB == NONE)
00615 {
00616 nnCB = i;
00617 if (vis_free != NONE)
00618 break;
00619 }
00620 }
00621
00622 if (state_[angles_[i].index] <= FREE)
00623 {
00624 if (i <= last_visible)
00625 {
00626 vis_free = i;
00627 if (nnCB != NONE)
00628 break;
00629 }
00630 }
00631 }
00632
00633 int nCB = 0;
00634 if (nnCB != NONE)
00635 while (angles_[nCB].index != nnIdx[nnCB]) nCB++;
00636 else
00637 nCB = NONE;
00638
00639 if (vis_free != NONE)
00640 {
00641 if ((vis_free < start) || (vis_free > end))
00642 need_invert = true;
00643 }
00644 else
00645 {
00646 if (nCB != NONE)
00647 {
00648 if ((nCB == start) || (nCB == end))
00649 {
00650 bool inside_CB = false;
00651 bool outside_CB = false;
00652 for (int i=0; i<nnn_; i++)
00653 {
00654 if (
00655 ((state_[angles_[i].index] == COMPLETED) || (state_[angles_[i].index] == BOUNDARY))
00656 && (i != start) && (i != end)
00657 )
00658 {
00659 if ((angles_[start].angle <= angles_[i].angle) && (angles_[i].angle <= angles_[end].angle))
00660 {
00661 inside_CB = true;
00662 if (outside_CB)
00663 break;
00664 }
00665 else
00666 {
00667 outside_CB = true;
00668 if (inside_CB)
00669 break;
00670 }
00671 }
00672 }
00673 if (inside_CB && !outside_CB)
00674 need_invert = true;
00675 else if (!(!inside_CB && outside_CB))
00676 {
00677 if ((angles_[end].angle - angles_[start].angle) < M_PI)
00678 need_invert = true;
00679 }
00680 }
00681 else
00682 {
00683 if ((angles_[nCB].angle > angles_[start].angle) && (angles_[nCB].angle < angles_[end].angle))
00684 need_invert = true;
00685 }
00686 }
00687 else
00688 {
00689 if (start == end-1)
00690 need_invert = true;
00691 }
00692 }
00693 }
00694 else if ((angles_[start].angle < angles_[sourceIdx].angle) && (angles_[sourceIdx].angle < angles_[end].angle))
00695 need_invert = true;
00696 }
00697
00698
00699 if (need_invert)
00700 {
00701 int tmp = start;
00702 start = end;
00703 end = tmp;
00704 }
00705
00706
00707
00708 bool is_boundary = false, is_skinny = false;
00709 std::vector<bool> gaps (nnn_, false);
00710 std::vector<bool> skinny (nnn_, false);
00711 std::vector<double> dif (nnn_);
00712 std::vector<int> angleIdx; angleIdx.reserve (nnn_);
00713 if (start > end)
00714 {
00715 for (int j=start; j<last_visible; j++)
00716 {
00717 dif[j] = (angles_[j+1].angle - angles_[j].angle);
00718 if (dif[j] < minimum_angle_)
00719 {
00720 skinny[j] = is_skinny = true;
00721 }
00722 else if (maximum_angle_ <= dif[j])
00723 {
00724 gaps[j] = is_boundary = true;
00725 }
00726 if ((!gaps[j]) && (sqr_max_edge < (coords_[angles_[j+1].index] - coords_[angles_[j].index]).squaredNorm ()))
00727 {
00728 gaps[j] = is_boundary = true;
00729 }
00730 angleIdx.push_back(j);
00731 }
00732
00733 dif[last_visible] = (2*M_PI + angles_[0].angle - angles_[last_visible].angle);
00734 if (dif[last_visible] < minimum_angle_)
00735 {
00736 skinny[last_visible] = is_skinny = true;
00737 }
00738 else if (maximum_angle_ <= dif[last_visible])
00739 {
00740 gaps[last_visible] = is_boundary = true;
00741 }
00742 if ((!gaps[last_visible]) && (sqr_max_edge < (coords_[angles_[0].index] - coords_[angles_[last_visible].index]).squaredNorm ()))
00743 {
00744 gaps[last_visible] = is_boundary = true;
00745 }
00746 angleIdx.push_back(last_visible);
00747
00748 for (int j=0; j<end; j++)
00749 {
00750 dif[j] = (angles_[j+1].angle - angles_[j].angle);
00751 if (dif[j] < minimum_angle_)
00752 {
00753 skinny[j] = is_skinny = true;
00754 }
00755 else if (maximum_angle_ <= dif[j])
00756 {
00757 gaps[j] = is_boundary = true;
00758 }
00759 if ((!gaps[j]) && (sqr_max_edge < (coords_[angles_[j+1].index] - coords_[angles_[j].index]).squaredNorm ()))
00760 {
00761 gaps[j] = is_boundary = true;
00762 }
00763 angleIdx.push_back(j);
00764 }
00765 angleIdx.push_back(end);
00766 }
00767
00768 else
00769 {
00770 for (int j=start; j<end; j++)
00771 {
00772 dif[j] = (angles_[j+1].angle - angles_[j].angle);
00773 if (dif[j] < minimum_angle_)
00774 {
00775 skinny[j] = is_skinny = true;
00776 }
00777 else if (maximum_angle_ <= dif[j])
00778 {
00779 gaps[j] = is_boundary = true;
00780 }
00781 if ((!gaps[j]) && (sqr_max_edge < (coords_[angles_[j+1].index] - coords_[angles_[j].index]).squaredNorm ()))
00782 {
00783 gaps[j] = is_boundary = true;
00784 }
00785 angleIdx.push_back(j);
00786 }
00787 angleIdx.push_back(end);
00788 }
00789
00790
00791 state_[R_] = is_boundary ? BOUNDARY : COMPLETED;
00792
00793 std::vector<int>::iterator first_gap_after = angleIdx.end ();
00794 std::vector<int>::iterator last_gap_before = angleIdx.begin ();
00795 int nr_gaps = 0;
00796 for (std::vector<int>::iterator it = angleIdx.begin (); it != angleIdx.end () - 1; it++)
00797 {
00798 if (gaps[*it])
00799 {
00800 nr_gaps++;
00801 if (first_gap_after == angleIdx.end())
00802 first_gap_after = it;
00803 last_gap_before = it+1;
00804 }
00805 }
00806 if (nr_gaps > 1)
00807 {
00808 angleIdx.erase(first_gap_after+1, last_gap_before);
00809 }
00810
00811
00812 if (is_skinny)
00813 {
00814 double angle_so_far = 0, angle_would_be;
00815 double max_combined_angle = (std::min)(maximum_angle_, M_PI-2*minimum_angle_);
00816 Eigen::Vector2f X;
00817 Eigen::Vector2f S1;
00818 Eigen::Vector2f S2;
00819 std::vector<int> to_erase;
00820 for (std::vector<int>::iterator it = angleIdx.begin()+1; it != angleIdx.end()-1; it++)
00821 {
00822 if (gaps[*(it-1)])
00823 angle_so_far = 0;
00824 else
00825 angle_so_far += dif[*(it-1)];
00826 if (gaps[*it])
00827 angle_would_be = angle_so_far;
00828 else
00829 angle_would_be = angle_so_far + dif[*it];
00830 if (
00831 (skinny[*it] || skinny[*(it-1)]) &&
00832 ((state_[angles_[*it].index] <= FREE) || (state_[angles_[*(it-1)].index] <= FREE)) &&
00833 ((!gaps[*it]) || (angles_[*it].nnIndex > angles_[*(it-1)].nnIndex)) &&
00834 ((!gaps[*(it-1)]) || (angles_[*it].nnIndex > angles_[*(it+1)].nnIndex)) &&
00835 (angle_would_be < max_combined_angle)
00836 )
00837 {
00838 if (gaps[*(it-1)])
00839 {
00840 gaps[*it] = true;
00841 to_erase.push_back(*it);
00842 }
00843 else if (gaps[*it])
00844 {
00845 gaps[*(it-1)] = true;
00846 to_erase.push_back(*it);
00847 }
00848 else
00849 {
00850 std::vector<int>::iterator prev_it;
00851 int erased_idx = static_cast<int> (to_erase.size ()) -1;
00852 for (prev_it = it-1; (erased_idx != -1) && (it != angleIdx.begin()); it--)
00853 if (*it == to_erase[erased_idx])
00854 erased_idx--;
00855 else
00856 break;
00857 bool can_delete = true;
00858 for (std::vector<int>::iterator curr_it = prev_it+1; curr_it != it+1; curr_it++)
00859 {
00860 tmp_ = coords_[angles_[*curr_it].index] - proj_qp_;
00861 X[0] = tmp_.dot(u_);
00862 X[1] = tmp_.dot(v_);
00863 tmp_ = coords_[angles_[*prev_it].index] - proj_qp_;
00864 S1[0] = tmp_.dot(u_);
00865 S1[1] = tmp_.dot(v_);
00866 tmp_ = coords_[angles_[*(it+1)].index] - proj_qp_;
00867 S2[0] = tmp_.dot(u_);
00868 S2[1] = tmp_.dot(v_);
00869
00870 if (isVisible(X,S1,S2))
00871 {
00872 can_delete = false;
00873 angle_so_far = 0;
00874 break;
00875 }
00876 }
00877 if (can_delete)
00878 {
00879 to_erase.push_back(*it);
00880 }
00881 }
00882 }
00883 else
00884 angle_so_far = 0;
00885 }
00886 for (std::vector<int>::iterator it = to_erase.begin(); it != to_erase.end(); it++)
00887 {
00888 for (std::vector<int>::iterator iter = angleIdx.begin(); iter != angleIdx.end(); iter++)
00889 if (*it == *iter)
00890 {
00891 angleIdx.erase(iter);
00892 break;
00893 }
00894 }
00895 }
00896
00897
00898 changed_1st_fn_ = false;
00899 changed_2nd_fn_ = false;
00900 new2boundary_ = NONE;
00901 for (std::vector<int>::iterator it = angleIdx.begin()+1; it != angleIdx.end()-1; it++)
00902 {
00903 current_index_ = angles_[*it].index;
00904
00905 is_current_free_ = false;
00906 if (state_[current_index_] <= FREE)
00907 {
00908 state_[current_index_] = FRINGE;
00909 is_current_free_ = true;
00910 }
00911 else if (!already_connected_)
00912 {
00913 prev_is_ffn_ = (ffn_[current_index_] == angles_[*(it-1)].index) && (!gaps[*(it-1)]);
00914 prev_is_sfn_ = (sfn_[current_index_] == angles_[*(it-1)].index) && (!gaps[*(it-1)]);
00915 next_is_ffn_ = (ffn_[current_index_] == angles_[*(it+1)].index) && (!gaps[*it]);
00916 next_is_sfn_ = (sfn_[current_index_] == angles_[*(it+1)].index) && (!gaps[*it]);
00917 if (!prev_is_ffn_ && !next_is_sfn_ && !prev_is_sfn_ && !next_is_ffn_)
00918 {
00919 nr_touched++;
00920 }
00921 }
00922
00923 if (gaps[*it])
00924 if (gaps[*(it-1)])
00925 {
00926 if (is_current_free_)
00927 state_[current_index_] = NONE;
00928 }
00929
00930 else
00931 {
00932 addTriangle (current_index_, angles_[*(it-1)].index, R_, polygons);
00933 addFringePoint (current_index_, R_);
00934 new2boundary_ = current_index_;
00935 if (!already_connected_)
00936 connectPoint (polygons, angles_[*(it-1)].index, R_,
00937 angles_[*(it+1)].index,
00938 uvn_nn[angles_[*it].nnIndex], uvn_nn[angles_[*(it-1)].nnIndex], uvn_nn_qp_zero);
00939 else already_connected_ = false;
00940 if (ffn_[R_] == angles_[*(angleIdx.begin())].index)
00941 {
00942 ffn_[R_] = new2boundary_;
00943 }
00944 else if (sfn_[R_] == angles_[*(angleIdx.begin())].index)
00945 {
00946 sfn_[R_] = new2boundary_;
00947 }
00948 }
00949
00950 else
00951 if (gaps[*(it-1)])
00952 {
00953 addFringePoint (current_index_, R_);
00954 new2boundary_ = current_index_;
00955 if (!already_connected_) connectPoint (polygons, R_, angles_[*(it+1)].index,
00956 (it+2) == angleIdx.end() ? -1 : angles_[*(it+2)].index,
00957 uvn_nn[angles_[*it].nnIndex], uvn_nn_qp_zero,
00958 uvn_nn[angles_[*(it+1)].nnIndex]);
00959 else already_connected_ = false;
00960 if (ffn_[R_] == angles_[*(angleIdx.end()-1)].index)
00961 {
00962 ffn_[R_] = new2boundary_;
00963 }
00964 else if (sfn_[R_] == angles_[*(angleIdx.end()-1)].index)
00965 {
00966 sfn_[R_] = new2boundary_;
00967 }
00968 }
00969
00970 else
00971 {
00972 addTriangle (current_index_, angles_[*(it-1)].index, R_, polygons);
00973 addFringePoint (current_index_, R_);
00974 if (!already_connected_) connectPoint (polygons, angles_[*(it-1)].index, angles_[*(it+1)].index,
00975 (it+2) == angleIdx.end() ? -1 : gaps[*(it+1)] ? R_ : angles_[*(it+2)].index,
00976 uvn_nn[angles_[*it].nnIndex],
00977 uvn_nn[angles_[*(it-1)].nnIndex],
00978 uvn_nn[angles_[*(it+1)].nnIndex]);
00979 else already_connected_ = false;
00980 }
00981 }
00982
00983
00984 if (ffn_[R_] == sfn_[R_])
00985 {
00986 state_[R_] = COMPLETED;
00987 }
00988 if (!gaps[*(angleIdx.end()-2)])
00989 {
00990 addTriangle (angles_[*(angleIdx.end()-2)].index, angles_[*(angleIdx.end()-1)].index, R_, polygons);
00991 addFringePoint (angles_[*(angleIdx.end()-2)].index, R_);
00992 if (R_ == ffn_[angles_[*(angleIdx.end()-1)].index])
00993 {
00994 if (angles_[*(angleIdx.end()-2)].index == sfn_[angles_[*(angleIdx.end()-1)].index])
00995 {
00996 state_[angles_[*(angleIdx.end()-1)].index] = COMPLETED;
00997 }
00998 else
00999 {
01000 ffn_[angles_[*(angleIdx.end()-1)].index] = angles_[*(angleIdx.end()-2)].index;
01001 }
01002 }
01003 else if (R_ == sfn_[angles_[*(angleIdx.end()-1)].index])
01004 {
01005 if (angles_[*(angleIdx.end()-2)].index == ffn_[angles_[*(angleIdx.end()-1)].index])
01006 {
01007 state_[angles_[*(angleIdx.end()-1)].index] = COMPLETED;
01008 }
01009 else
01010 {
01011 sfn_[angles_[*(angleIdx.end()-1)].index] = angles_[*(angleIdx.end()-2)].index;
01012 }
01013 }
01014 }
01015 if (!gaps[*(angleIdx.begin())])
01016 {
01017 if (R_ == ffn_[angles_[*(angleIdx.begin())].index])
01018 {
01019 if (angles_[*(angleIdx.begin()+1)].index == sfn_[angles_[*(angleIdx.begin())].index])
01020 {
01021 state_[angles_[*(angleIdx.begin())].index] = COMPLETED;
01022 }
01023 else
01024 {
01025 ffn_[angles_[*(angleIdx.begin())].index] = angles_[*(angleIdx.begin()+1)].index;
01026 }
01027 }
01028 else if (R_ == sfn_[angles_[*(angleIdx.begin())].index])
01029 {
01030 if (angles_[*(angleIdx.begin()+1)].index == ffn_[angles_[*(angleIdx.begin())].index])
01031 {
01032 state_[angles_[*(angleIdx.begin())].index] = COMPLETED;
01033 }
01034 else
01035 {
01036 sfn_[angles_[*(angleIdx.begin())].index] = angles_[*(angleIdx.begin()+1)].index;
01037 }
01038 }
01039 }
01040 }
01041 }
01042 PCL_DEBUG ("Number of triangles: %zu\n", polygons.size());
01043 PCL_DEBUG ("Number of unconnected parts: %d\n", nr_parts);
01044 if (increase_nnn4fn > 0)
01045 PCL_WARN ("Number of neighborhood size increase requests for fringe neighbors: %d\n", increase_nnn4fn);
01046 if (increase_nnn4s > 0)
01047 PCL_WARN ("Number of neighborhood size increase requests for source: %d\n", increase_nnn4s);
01048 if (increase_dist > 0)
01049 PCL_WARN ("Number of automatic maximum distance increases: %d\n", increase_dist);
01050
01051
01052 std::sort (fringe_queue_.begin (), fringe_queue_.end ());
01053 fringe_queue_.erase (std::unique (fringe_queue_.begin (), fringe_queue_.end ()), fringe_queue_.end ());
01054 PCL_DEBUG ("Number of processed points: %zu / %zu\n", fringe_queue_.size(), indices_->size ());
01055 return (true);
01056 }
01057
01059 template <typename PointInT> void
01060 pcl::GreedyProjectionTriangulation<PointInT>::closeTriangle (std::vector<pcl::Vertices> &polygons)
01061 {
01062 state_[R_] = COMPLETED;
01063 addTriangle (angles_[0].index, angles_[1].index, R_, polygons);
01064 for (int aIdx=0; aIdx<2; aIdx++)
01065 {
01066 if (ffn_[angles_[aIdx].index] == R_)
01067 {
01068 if (sfn_[angles_[aIdx].index] == angles_[(aIdx+1)%2].index)
01069 {
01070 state_[angles_[aIdx].index] = COMPLETED;
01071 }
01072 else
01073 {
01074 ffn_[angles_[aIdx].index] = angles_[(aIdx+1)%2].index;
01075 }
01076 }
01077 else if (sfn_[angles_[aIdx].index] == R_)
01078 {
01079 if (ffn_[angles_[aIdx].index] == angles_[(aIdx+1)%2].index)
01080 {
01081 state_[angles_[aIdx].index] = COMPLETED;
01082 }
01083 else
01084 {
01085 sfn_[angles_[aIdx].index] = angles_[(aIdx+1)%2].index;
01086 }
01087 }
01088 }
01089 }
01090
01092 template <typename PointInT> void
01093 pcl::GreedyProjectionTriangulation<PointInT>::connectPoint (
01094 std::vector<pcl::Vertices> &polygons,
01095 const int prev_index, const int next_index, const int next_next_index,
01096 const Eigen::Vector2f &uvn_current,
01097 const Eigen::Vector2f &uvn_prev,
01098 const Eigen::Vector2f &uvn_next)
01099 {
01100 if (is_current_free_)
01101 {
01102 ffn_[current_index_] = prev_index;
01103 sfn_[current_index_] = next_index;
01104 }
01105 else
01106 {
01107 if ((prev_is_ffn_ && next_is_sfn_) || (prev_is_sfn_ && next_is_ffn_))
01108 state_[current_index_] = COMPLETED;
01109 else if (prev_is_ffn_ && !next_is_sfn_)
01110 ffn_[current_index_] = next_index;
01111 else if (next_is_ffn_ && !prev_is_sfn_)
01112 ffn_[current_index_] = prev_index;
01113 else if (prev_is_sfn_ && !next_is_ffn_)
01114 sfn_[current_index_] = next_index;
01115 else if (next_is_sfn_ && !prev_is_ffn_)
01116 sfn_[current_index_] = prev_index;
01117 else
01118 {
01119 bool found_triangle = false;
01120 if ((prev_index != R_) && ((ffn_[current_index_] == ffn_[prev_index]) || (ffn_[current_index_] == sfn_[prev_index])))
01121 {
01122 found_triangle = true;
01123 addTriangle (current_index_, ffn_[current_index_], prev_index, polygons);
01124 state_[prev_index] = COMPLETED;
01125 state_[ffn_[current_index_]] = COMPLETED;
01126 ffn_[current_index_] = next_index;
01127 }
01128 else if ((prev_index != R_) && ((sfn_[current_index_] == ffn_[prev_index]) || (sfn_[current_index_] == sfn_[prev_index])))
01129 {
01130 found_triangle = true;
01131 addTriangle (current_index_, sfn_[current_index_], prev_index, polygons);
01132 state_[prev_index] = COMPLETED;
01133 state_[sfn_[current_index_]] = COMPLETED;
01134 sfn_[current_index_] = next_index;
01135 }
01136 else if (state_[next_index] > FREE)
01137 {
01138 if ((ffn_[current_index_] == ffn_[next_index]) || (ffn_[current_index_] == sfn_[next_index]))
01139 {
01140 found_triangle = true;
01141 addTriangle (current_index_, ffn_[current_index_], next_index, polygons);
01142
01143 if (ffn_[current_index_] == ffn_[next_index])
01144 {
01145 ffn_[next_index] = current_index_;
01146 }
01147 else
01148 {
01149 sfn_[next_index] = current_index_;
01150 }
01151 state_[ffn_[current_index_]] = COMPLETED;
01152 ffn_[current_index_] = prev_index;
01153 }
01154 else if ((sfn_[current_index_] == ffn_[next_index]) || (sfn_[current_index_] == sfn_[next_index]))
01155 {
01156 found_triangle = true;
01157 addTriangle (current_index_, sfn_[current_index_], next_index, polygons);
01158
01159 if (sfn_[current_index_] == ffn_[next_index])
01160 {
01161 ffn_[next_index] = current_index_;
01162 }
01163 else
01164 {
01165 sfn_[next_index] = current_index_;
01166 }
01167 state_[sfn_[current_index_]] = COMPLETED;
01168 sfn_[current_index_] = prev_index;
01169 }
01170 }
01171
01172 if (found_triangle)
01173 {
01174 }
01175 else
01176 {
01177 tmp_ = coords_[ffn_[current_index_]] - proj_qp_;
01178 uvn_ffn_[0] = tmp_.dot(u_);
01179 uvn_ffn_[1] = tmp_.dot(v_);
01180 tmp_ = coords_[sfn_[current_index_]] - proj_qp_;
01181 uvn_sfn_[0] = tmp_.dot(u_);
01182 uvn_sfn_[1] = tmp_.dot(v_);
01183 bool prev_ffn = isVisible(uvn_prev, uvn_next, uvn_current, uvn_ffn_) && isVisible(uvn_prev, uvn_sfn_, uvn_current, uvn_ffn_);
01184 bool prev_sfn = isVisible(uvn_prev, uvn_next, uvn_current, uvn_sfn_) && isVisible(uvn_prev, uvn_ffn_, uvn_current, uvn_sfn_);
01185 bool next_ffn = isVisible(uvn_next, uvn_prev, uvn_current, uvn_ffn_) && isVisible(uvn_next, uvn_sfn_, uvn_current, uvn_ffn_);
01186 bool next_sfn = isVisible(uvn_next, uvn_prev, uvn_current, uvn_sfn_) && isVisible(uvn_next, uvn_ffn_, uvn_current, uvn_sfn_);
01187 int min_dist = -1;
01188 if (prev_ffn && next_sfn && prev_sfn && next_ffn)
01189 {
01190
01191 double prev2f = (coords_[ffn_[current_index_]] - coords_[prev_index]).squaredNorm ();
01192 double next2s = (coords_[sfn_[current_index_]] - coords_[next_index]).squaredNorm ();
01193 double prev2s = (coords_[sfn_[current_index_]] - coords_[prev_index]).squaredNorm ();
01194 double next2f = (coords_[ffn_[current_index_]] - coords_[next_index]).squaredNorm ();
01195 if (prev2f < prev2s)
01196 {
01197 if (prev2f < next2f)
01198 {
01199 if (prev2f < next2s)
01200 min_dist = 0;
01201 else
01202 min_dist = 3;
01203 }
01204 else
01205 {
01206 if (next2f < next2s)
01207 min_dist = 2;
01208 else
01209 min_dist = 3;
01210 }
01211 }
01212 else
01213 {
01214 if (prev2s < next2f)
01215 {
01216 if (prev2s < next2s)
01217 min_dist = 1;
01218 else
01219 min_dist = 3;
01220 }
01221 else
01222 {
01223 if (next2f < next2s)
01224 min_dist = 2;
01225 else
01226 min_dist = 3;
01227 }
01228 }
01229 }
01230 else if (prev_ffn && next_sfn)
01231 {
01232
01233 double prev2f = (coords_[ffn_[current_index_]] - coords_[prev_index]).squaredNorm ();
01234 double next2s = (coords_[sfn_[current_index_]] - coords_[next_index]).squaredNorm ();
01235 if (prev2f < next2s)
01236 min_dist = 0;
01237 else
01238 min_dist = 3;
01239 }
01240 else if (prev_sfn && next_ffn)
01241 {
01242
01243 double prev2s = (coords_[sfn_[current_index_]] - coords_[prev_index]).squaredNorm ();
01244 double next2f = (coords_[ffn_[current_index_]] - coords_[next_index]).squaredNorm ();
01245 if (prev2s < next2f)
01246 min_dist = 1;
01247 else
01248 min_dist = 2;
01249 }
01250
01251 else if (prev_ffn && !next_sfn && !prev_sfn && !next_ffn)
01252 min_dist = 0;
01253 else if (!prev_ffn && !next_sfn && prev_sfn && !next_ffn)
01254 min_dist = 1;
01255 else if (!prev_ffn && !next_sfn && !prev_sfn && next_ffn)
01256 min_dist = 2;
01257 else if (!prev_ffn && next_sfn && !prev_sfn && !next_ffn)
01258 min_dist = 3;
01259
01260 else if (prev_ffn)
01261 {
01262 double prev2f = (coords_[ffn_[current_index_]] - coords_[prev_index]).squaredNorm ();
01263 if (prev_sfn)
01264 {
01265 double prev2s = (coords_[sfn_[current_index_]] - coords_[prev_index]).squaredNorm ();
01266 if (prev2s < prev2f)
01267 min_dist = 1;
01268 else
01269 min_dist = 0;
01270 }
01271 else if (next_ffn)
01272 {
01273 double next2f = (coords_[ffn_[current_index_]] - coords_[next_index]).squaredNorm ();
01274 if (next2f < prev2f)
01275 min_dist = 2;
01276 else
01277 min_dist = 0;
01278 }
01279 }
01280 else if (next_sfn)
01281 {
01282 double next2s = (coords_[sfn_[current_index_]] - coords_[next_index]).squaredNorm ();
01283 if (prev_sfn)
01284 {
01285 double prev2s = (coords_[sfn_[current_index_]] - coords_[prev_index]).squaredNorm ();
01286 if (prev2s < next2s)
01287 min_dist = 1;
01288 else
01289 min_dist = 3;
01290 }
01291 else if (next_ffn)
01292 {
01293 double next2f = (coords_[ffn_[current_index_]] - coords_[next_index]).squaredNorm ();
01294 if (next2f < next2s)
01295 min_dist = 2;
01296 else
01297 min_dist = 3;
01298 }
01299 }
01300 switch (min_dist)
01301 {
01302 case 0:
01303 {
01304 addTriangle (current_index_, ffn_[current_index_], prev_index, polygons);
01305
01306
01307 if (ffn_[prev_index] == current_index_)
01308 {
01309 ffn_[prev_index] = ffn_[current_index_];
01310 }
01311 else if (sfn_[prev_index] == current_index_)
01312 {
01313 sfn_[prev_index] = ffn_[current_index_];
01314 }
01315 else if (ffn_[prev_index] == R_)
01316 {
01317 changed_1st_fn_ = true;
01318 ffn_[prev_index] = ffn_[current_index_];
01319 }
01320 else if (sfn_[prev_index] == R_)
01321 {
01322 changed_1st_fn_ = true;
01323 sfn_[prev_index] = ffn_[current_index_];
01324 }
01325 else if (prev_index == R_)
01326 {
01327 new2boundary_ = ffn_[current_index_];
01328 }
01329
01330
01331 if (ffn_[ffn_[current_index_]] == current_index_)
01332 {
01333 ffn_[ffn_[current_index_]] = prev_index;
01334 }
01335 else if (sfn_[ffn_[current_index_]] == current_index_)
01336 {
01337 sfn_[ffn_[current_index_]] = prev_index;
01338 }
01339
01340
01341 ffn_[current_index_] = next_index;
01342
01343 break;
01344 }
01345 case 1:
01346 {
01347 addTriangle (current_index_, sfn_[current_index_], prev_index, polygons);
01348
01349
01350 if (ffn_[prev_index] == current_index_)
01351 {
01352 ffn_[prev_index] = sfn_[current_index_];
01353 }
01354 else if (sfn_[prev_index] == current_index_)
01355 {
01356 sfn_[prev_index] = sfn_[current_index_];
01357 }
01358 else if (ffn_[prev_index] == R_)
01359 {
01360 changed_1st_fn_ = true;
01361 ffn_[prev_index] = sfn_[current_index_];
01362 }
01363 else if (sfn_[prev_index] == R_)
01364 {
01365 changed_1st_fn_ = true;
01366 sfn_[prev_index] = sfn_[current_index_];
01367 }
01368 else if (prev_index == R_)
01369 {
01370 new2boundary_ = sfn_[current_index_];
01371 }
01372
01373
01374 if (ffn_[sfn_[current_index_]] == current_index_)
01375 {
01376 ffn_[sfn_[current_index_]] = prev_index;
01377 }
01378 else if (sfn_[sfn_[current_index_]] == current_index_)
01379 {
01380 sfn_[sfn_[current_index_]] = prev_index;
01381 }
01382
01383
01384 sfn_[current_index_] = next_index;
01385
01386 break;
01387 }
01388 case 2:
01389 {
01390 addTriangle (current_index_, ffn_[current_index_], next_index, polygons);
01391 int neighbor_update = next_index;
01392
01393
01394 if (state_[next_index] <= FREE)
01395 {
01396 state_[next_index] = FRINGE;
01397 ffn_[next_index] = current_index_;
01398 sfn_[next_index] = ffn_[current_index_];
01399 }
01400 else
01401 {
01402 if (ffn_[next_index] == R_)
01403 {
01404 changed_2nd_fn_ = true;
01405 ffn_[next_index] = ffn_[current_index_];
01406 }
01407 else if (sfn_[next_index] == R_)
01408 {
01409 changed_2nd_fn_ = true;
01410 sfn_[next_index] = ffn_[current_index_];
01411 }
01412 else if (next_index == R_)
01413 {
01414 new2boundary_ = ffn_[current_index_];
01415 if (next_next_index == new2boundary_)
01416 already_connected_ = true;
01417 }
01418 else if (ffn_[next_index] == next_next_index)
01419 {
01420 already_connected_ = true;
01421 ffn_[next_index] = ffn_[current_index_];
01422 }
01423 else if (sfn_[next_index] == next_next_index)
01424 {
01425 already_connected_ = true;
01426 sfn_[next_index] = ffn_[current_index_];
01427 }
01428 else
01429 {
01430 tmp_ = coords_[ffn_[next_index]] - proj_qp_;
01431 uvn_next_ffn_[0] = tmp_.dot(u_);
01432 uvn_next_ffn_[1] = tmp_.dot(v_);
01433 tmp_ = coords_[sfn_[next_index]] - proj_qp_;
01434 uvn_next_sfn_[0] = tmp_.dot(u_);
01435 uvn_next_sfn_[1] = tmp_.dot(v_);
01436
01437 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_);
01438 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_);
01439
01440 int connect2ffn = -1;
01441 if (ffn_next_ffn && sfn_next_ffn)
01442 {
01443 double fn2f = (coords_[ffn_[current_index_]] - coords_[ffn_[next_index]]).squaredNorm ();
01444 double sn2f = (coords_[ffn_[current_index_]] - coords_[sfn_[next_index]]).squaredNorm ();
01445 if (fn2f < sn2f) connect2ffn = 0;
01446 else connect2ffn = 1;
01447 }
01448 else if (ffn_next_ffn) connect2ffn = 0;
01449 else if (sfn_next_ffn) connect2ffn = 1;
01450
01451 switch (connect2ffn)
01452 {
01453 case 0:
01454 {
01455 addTriangle (next_index, ffn_[current_index_], ffn_[next_index], polygons);
01456 neighbor_update = ffn_[next_index];
01457
01458
01459 if ((ffn_[ffn_[next_index]] == ffn_[current_index_]) || (sfn_[ffn_[next_index]] == ffn_[current_index_]))
01460 {
01461 state_[ffn_[next_index]] = COMPLETED;
01462 }
01463 else if (ffn_[ffn_[next_index]] == next_index)
01464 {
01465 ffn_[ffn_[next_index]] = ffn_[current_index_];
01466 }
01467 else if (sfn_[ffn_[next_index]] == next_index)
01468 {
01469 sfn_[ffn_[next_index]] = ffn_[current_index_];
01470 }
01471
01472 ffn_[next_index] = current_index_;
01473
01474 break;
01475 }
01476 case 1:
01477 {
01478 addTriangle (next_index, ffn_[current_index_], sfn_[next_index], polygons);
01479 neighbor_update = sfn_[next_index];
01480
01481
01482 if ((ffn_[sfn_[next_index]] = ffn_[current_index_]) || (sfn_[sfn_[next_index]] == ffn_[current_index_]))
01483 {
01484 state_[sfn_[next_index]] = COMPLETED;
01485 }
01486 else if (ffn_[sfn_[next_index]] == next_index)
01487 {
01488 ffn_[sfn_[next_index]] = ffn_[current_index_];
01489 }
01490 else if (sfn_[sfn_[next_index]] == next_index)
01491 {
01492 sfn_[sfn_[next_index]] = ffn_[current_index_];
01493 }
01494
01495 sfn_[next_index] = current_index_;
01496
01497 break;
01498 }
01499 default:;
01500 }
01501 }
01502 }
01503
01504
01505 if ((ffn_[ffn_[current_index_]] == neighbor_update) || (sfn_[ffn_[current_index_]] == neighbor_update))
01506 {
01507 state_[ffn_[current_index_]] = COMPLETED;
01508 }
01509 else if (ffn_[ffn_[current_index_]] == current_index_)
01510 {
01511 ffn_[ffn_[current_index_]] = neighbor_update;
01512 }
01513 else if (sfn_[ffn_[current_index_]] == current_index_)
01514 {
01515 sfn_[ffn_[current_index_]] = neighbor_update;
01516 }
01517
01518
01519 ffn_[current_index_] = prev_index;
01520
01521 break;
01522 }
01523 case 3:
01524 {
01525 addTriangle (current_index_, sfn_[current_index_], next_index, polygons);
01526 int neighbor_update = next_index;
01527
01528
01529 if (state_[next_index] <= FREE)
01530 {
01531 state_[next_index] = FRINGE;
01532 ffn_[next_index] = current_index_;
01533 sfn_[next_index] = sfn_[current_index_];
01534 }
01535 else
01536 {
01537 if (ffn_[next_index] == R_)
01538 {
01539 changed_2nd_fn_ = true;
01540 ffn_[next_index] = sfn_[current_index_];
01541 }
01542 else if (sfn_[next_index] == R_)
01543 {
01544 changed_2nd_fn_ = true;
01545 sfn_[next_index] = sfn_[current_index_];
01546 }
01547 else if (next_index == R_)
01548 {
01549 new2boundary_ = sfn_[current_index_];
01550 if (next_next_index == new2boundary_)
01551 already_connected_ = true;
01552 }
01553 else if (ffn_[next_index] == next_next_index)
01554 {
01555 already_connected_ = true;
01556 ffn_[next_index] = sfn_[current_index_];
01557 }
01558 else if (sfn_[next_index] == next_next_index)
01559 {
01560 already_connected_ = true;
01561 sfn_[next_index] = sfn_[current_index_];
01562 }
01563 else
01564 {
01565 tmp_ = coords_[ffn_[next_index]] - proj_qp_;
01566 uvn_next_ffn_[0] = tmp_.dot(u_);
01567 uvn_next_ffn_[1] = tmp_.dot(v_);
01568 tmp_ = coords_[sfn_[next_index]] - proj_qp_;
01569 uvn_next_sfn_[0] = tmp_.dot(u_);
01570 uvn_next_sfn_[1] = tmp_.dot(v_);
01571
01572 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_);
01573 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_);
01574
01575 int connect2sfn = -1;
01576 if (ffn_next_sfn && sfn_next_sfn)
01577 {
01578 double fn2s = (coords_[sfn_[current_index_]] - coords_[ffn_[next_index]]).squaredNorm ();
01579 double sn2s = (coords_[sfn_[current_index_]] - coords_[sfn_[next_index]]).squaredNorm ();
01580 if (fn2s < sn2s) connect2sfn = 0;
01581 else connect2sfn = 1;
01582 }
01583 else if (ffn_next_sfn) connect2sfn = 0;
01584 else if (sfn_next_sfn) connect2sfn = 1;
01585
01586 switch (connect2sfn)
01587 {
01588 case 0:
01589 {
01590 addTriangle (next_index, sfn_[current_index_], ffn_[next_index], polygons);
01591 neighbor_update = ffn_[next_index];
01592
01593
01594 if ((ffn_[ffn_[next_index]] == sfn_[current_index_]) || (sfn_[ffn_[next_index]] == sfn_[current_index_]))
01595 {
01596 state_[ffn_[next_index]] = COMPLETED;
01597 }
01598 else if (ffn_[ffn_[next_index]] == next_index)
01599 {
01600 ffn_[ffn_[next_index]] = sfn_[current_index_];
01601 }
01602 else if (sfn_[ffn_[next_index]] == next_index)
01603 {
01604 sfn_[ffn_[next_index]] = sfn_[current_index_];
01605 }
01606
01607 ffn_[next_index] = current_index_;
01608
01609 break;
01610 }
01611 case 1:
01612 {
01613 addTriangle (next_index, sfn_[current_index_], sfn_[next_index], polygons);
01614 neighbor_update = sfn_[next_index];
01615
01616
01617 if ((ffn_[sfn_[next_index]] == sfn_[current_index_]) || (sfn_[sfn_[next_index]] == sfn_[current_index_]))
01618 {
01619 state_[sfn_[next_index]] = COMPLETED;
01620 }
01621 else if (ffn_[sfn_[next_index]] == next_index)
01622 {
01623 ffn_[sfn_[next_index]] = sfn_[current_index_];
01624 }
01625 else if (sfn_[sfn_[next_index]] == next_index)
01626 {
01627 sfn_[sfn_[next_index]] = sfn_[current_index_];
01628 }
01629
01630 sfn_[next_index] = current_index_;
01631
01632 break;
01633 }
01634 default:;
01635 }
01636 }
01637 }
01638
01639
01640 if ((ffn_[sfn_[current_index_]] == neighbor_update) || (sfn_[sfn_[current_index_]] == neighbor_update))
01641 {
01642 state_[sfn_[current_index_]] = COMPLETED;
01643 }
01644 else if (ffn_[sfn_[current_index_]] == current_index_)
01645 {
01646 ffn_[sfn_[current_index_]] = neighbor_update;
01647 }
01648 else if (sfn_[sfn_[current_index_]] == current_index_)
01649 {
01650 sfn_[sfn_[current_index_]] = neighbor_update;
01651 }
01652
01653 sfn_[current_index_] = prev_index;
01654
01655 break;
01656 }
01657 default:;
01658 }
01659 }
01660 }
01661 }
01662 }
01663
01665 template <typename PointInT> std::vector<std::vector<size_t> >
01666 pcl::GreedyProjectionTriangulation<PointInT>::getTriangleList (const pcl::PolygonMesh &input)
01667 {
01668 std::vector<std::vector<size_t> > triangleList (input.cloud.width * input.cloud.height);
01669
01670 for (size_t i=0; i < input.polygons.size (); ++i)
01671 for (size_t j=0; j < input.polygons[i].vertices.size (); ++j)
01672 triangleList[j].push_back (i);
01673 return (triangleList);
01674 }
01675
01676 #define PCL_INSTANTIATE_GreedyProjectionTriangulation(T) \
01677 template class PCL_EXPORTS pcl::GreedyProjectionTriangulation<T>;
01678
01679 #endif // PCL_SURFACE_IMPL_GP3_H_
01680
01681