Point Cloud Library (PCL)
1.3.1
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of Willow Garage, Inc. nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 * 00034 * $Id: gp3.hpp 3031 2011-11-01 04:25:15Z rusu $ 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 if (search_radius_ <= 0 || mu_ <= 0) 00049 { 00050 PCL_ERROR ("[pcl::%s::performReconstruction] Invalid search radius (%f) or mu parameter (%f)!\n", getClassName ().c_str (), search_radius_, mu_); 00051 output.cloud.width = output.cloud.height = 0; 00052 output.cloud.data.clear (); 00053 output.polygons.clear (); 00054 return; 00055 } 00056 const double sqr_mu = mu_*mu_; 00057 const double sqr_max_edge = search_radius_*search_radius_; 00058 if (nnn_ > (int)indices_->size ()) 00059 nnn_ = indices_->size (); 00060 00061 // Variables to hold the results of nearest neighbor searches 00062 std::vector<int> nnIdx (nnn_); 00063 std::vector<float> sqrDists (nnn_); 00064 00065 // current number of connected components 00066 int part_index = 0; 00067 00068 // 2D coordinates of points 00069 const Eigen::Vector2f uvn_nn_qp_zero = Eigen::Vector2f::Zero(); 00070 Eigen::Vector2f uvn_current; 00071 Eigen::Vector2f uvn_prev; 00072 Eigen::Vector2f uvn_next; 00073 00074 // initializing fields 00075 already_connected_ = false; // see declaration for comments :P 00076 00077 // initializing states and fringe neighbors 00078 part_.clear (); 00079 state_.clear (); 00080 source_.clear (); 00081 ffn_.clear (); 00082 sfn_.clear (); 00083 part_.resize(indices_->size (), -1); // indices of point's part 00084 state_.resize(indices_->size (), FREE); 00085 source_.resize(indices_->size (), NONE); 00086 ffn_.resize(indices_->size (), NONE); 00087 sfn_.resize(indices_->size (), NONE); 00088 fringe_queue_.clear (); 00089 int fqIdx = 0; // current fringe's index in the queue to be processed 00090 00091 // Saving coordinates 00092 coords_.clear (); 00093 coords_.reserve (indices_->size ()); 00094 for (size_t cp = 0; cp < indices_->size (); ++cp) 00095 { 00096 coords_.push_back(input_->points[(*indices_)[cp]].getVector3fMap()); 00097 } 00098 00099 // Initializing 00100 int is_free=0, nr_parts=0, increase_nnn4fn=0, increase_nnn4s=0, increase_dist=0, nr_touched = 0; 00101 bool is_fringe; 00102 angles_.resize(nnn_); 00103 std::vector<Eigen::Vector2f> uvn_nn (nnn_); 00104 Eigen::Vector2f uvn_s; 00105 00106 // iterating through fringe points and finishing them until everything is done 00107 while (is_free != NONE) 00108 { 00109 R_ = is_free; 00110 if (state_[R_] == FREE) 00111 { 00112 state_[R_] = NONE; 00113 part_[R_] = part_index++; 00114 00115 // creating starting triangle 00116 //searchForNeighbors ((*indices_)[R_], nnIdx, sqrDists); 00117 tree_->nearestKSearch ((*indices_)[R_], nnn_, nnIdx, sqrDists); 00118 double sqr_dist_threshold = (std::min)(sqr_max_edge, sqr_mu * sqrDists[1]); 00119 00120 // Get the normal estimate at the current point 00121 const Eigen::Vector3f nc = input_->points[(*indices_)[R_]].getNormalVector3fMap (); 00122 00123 // Get a coordinate system that lies on a plane defined by its normal 00124 v_ = nc.unitOrthogonal (); 00125 u_ = nc.cross (v_); 00126 00127 // Projecting point onto the surface 00128 double dist = nc.dot(coords_[R_]); 00129 proj_qp_ = coords_[R_] - dist * nc; 00130 00131 // Converting coords, calculating angles and saving the projected near boundary edges 00132 int nr_edge = 0; 00133 std::vector<doubleEdge> doubleEdges; 00134 for (int i = 1; i < nnn_; i++) // nearest neighbor with index 0 is the query point R_ itself 00135 { 00136 // Transforming coordinates 00137 tmp_ = coords_[nnIdx[i]] - proj_qp_; 00138 uvn_nn[i][0] = tmp_.dot(u_); 00139 uvn_nn[i][1] = tmp_.dot(v_); 00140 // Computing the angle between each neighboring point and the query point itself 00141 angles_[i].angle = atan2(uvn_nn[i][1], uvn_nn[i][0]); 00142 // initializing angle descriptors 00143 angles_[i].index = nnIdx[i]; 00144 if ( 00145 (state_[nnIdx[i]] == COMPLETED) || (state_[nnIdx[i]] == BOUNDARY) 00146 || (sqrDists[i] > sqr_dist_threshold) 00147 ) 00148 angles_[i].visible = false; 00149 else 00150 angles_[i].visible = true; 00151 // Saving the edges between nearby boundary points 00152 if ((state_[nnIdx[i]] == FRINGE) || (state_[nnIdx[i]] == BOUNDARY)) 00153 { 00154 doubleEdge e; 00155 e.index = i; 00156 nr_edge++; 00157 tmp_ = coords_[ffn_[nnIdx[i]]] - proj_qp_; 00158 e.first[0] = tmp_.dot(u_); 00159 e.first[1] = tmp_.dot(v_); 00160 tmp_ = coords_[sfn_[nnIdx[i]]] - proj_qp_; 00161 e.second[0] = tmp_.dot(u_); 00162 e.second[1] = tmp_.dot(v_); 00163 doubleEdges.push_back(e); 00164 } 00165 } 00166 angles_[0].visible = false; 00167 00168 // Verify the visibility of each potential new vertex 00169 for (int i = 1; i < nnn_; i++) // nearest neighbor with index 0 is the query point R_ itself 00170 if ((angles_[i].visible) && (ffn_[R_] != nnIdx[i]) && (sfn_[R_] != nnIdx[i])) 00171 { 00172 bool visibility = true; 00173 for (int j = 0; j < nr_edge; j++) 00174 { 00175 if (ffn_[nnIdx[doubleEdges[j].index]] != nnIdx[i]) 00176 visibility = isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].first, Eigen::Vector2f::Zero()); 00177 if (!visibility) 00178 break; 00179 if (sfn_[nnIdx[doubleEdges[j].index]] != nnIdx[i]) 00180 visibility = isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].second, Eigen::Vector2f::Zero()); 00181 if (!visibility == false) 00182 break; 00183 } 00184 angles_[i].visible = visibility; 00185 } 00186 00187 // Selecting first two visible free neighbors 00188 bool not_found = true; 00189 int left = 1; 00190 do 00191 { 00192 while ((left < nnn_) && ((!angles_[left].visible) || (state_[nnIdx[left]] > FREE))) left++; 00193 if (left >= nnn_) 00194 break; 00195 else 00196 { 00197 int right = left+1; 00198 do 00199 { 00200 while ((right < nnn_) && ((!angles_[right].visible) || (state_[nnIdx[right]] > FREE))) right++; 00201 if (right >= nnn_) 00202 break; 00203 else if ((coords_[nnIdx[left]] - coords_[nnIdx[right]]).squaredNorm () > sqr_max_edge) 00204 right++; 00205 else 00206 { 00207 addFringePoint (nnIdx[right], R_); 00208 addFringePoint (nnIdx[left], nnIdx[right]); 00209 addFringePoint (R_, nnIdx[left]); 00210 state_[R_] = state_[nnIdx[left]] = state_[nnIdx[right]] = FRINGE; 00211 ffn_[R_] = nnIdx[left]; 00212 sfn_[R_] = nnIdx[right]; 00213 ffn_[nnIdx[left]] = nnIdx[right]; 00214 sfn_[nnIdx[left]] = R_; 00215 ffn_[nnIdx[right]] = R_; 00216 sfn_[nnIdx[right]] = nnIdx[left]; 00217 addTriangle (R_, nnIdx[left], nnIdx[right], output); 00218 nr_parts++; 00219 not_found = false; 00220 break; 00221 } 00222 } 00223 while (true); 00224 left++; 00225 } 00226 } 00227 while (not_found); 00228 } 00229 00230 is_free = NONE; 00231 for (unsigned temp = 0; temp < indices_->size (); temp++) 00232 { 00233 if (state_[temp] == FREE) 00234 { 00235 is_free = temp; 00236 break; 00237 } 00238 } 00239 00240 is_fringe = true; 00241 while (is_fringe) 00242 { 00243 is_fringe = false; 00244 00245 int fqSize = fringe_queue_.size(); 00246 while ((fqIdx < fqSize) && (state_[fringe_queue_[fqIdx]] != FRINGE)) 00247 fqIdx++; 00248 00249 // an unfinished fringe point is found 00250 if (fqIdx >= fqSize) 00251 { 00252 continue; 00253 } 00254 00255 R_ = fringe_queue_[fqIdx]; 00256 is_fringe = true; 00257 00258 if (ffn_[R_] == sfn_[R_]) 00259 { 00260 state_[R_] = COMPLETED; 00261 continue; 00262 } 00263 //searchForNeighbors ((*indices_)[R_], nnIdx, sqrDists); 00264 tree_->nearestKSearch ((*indices_)[R_], nnn_, nnIdx, sqrDists); 00265 00266 // Locating FFN and SFN to adapt distance threshold 00267 double sqr_source_dist = (coords_[R_] - coords_[source_[R_]]).squaredNorm (); 00268 double sqr_ffn_dist = (coords_[R_] - coords_[ffn_[R_]]).squaredNorm (); 00269 double sqr_sfn_dist = (coords_[R_] - coords_[sfn_[R_]]).squaredNorm (); 00270 double max_sqr_fn_dist = (std::max)(sqr_ffn_dist, sqr_sfn_dist); 00271 double sqr_dist_threshold = (std::min)(sqr_max_edge, sqr_mu * sqrDists[1]); //sqr_mu * sqr_avg_conn_dist); 00272 if (max_sqr_fn_dist > sqrDists[nnn_-1]) 00273 { 00274 if (0 == increase_nnn4fn) 00275 PCL_WARN("Not enough neighbors are considered: ffn or sfn out of range! Consider increasing nnn_... Setting R=%d to be BOUNDARY!\n", R_); 00276 increase_nnn4fn++; 00277 state_[R_] = BOUNDARY; 00278 continue; 00279 } 00280 double max_sqr_fns_dist = (std::max)(sqr_source_dist, max_sqr_fn_dist); 00281 if (max_sqr_fns_dist > sqrDists[nnn_-1]) 00282 { 00283 if (0 == increase_nnn4s) 00284 PCL_WARN("Not enough neighbors are considered: source of R=%d is out of range! Consider increasing nnn_...\n", R_); 00285 increase_nnn4s++; 00286 } 00287 00288 // Get the normal estimate at the current point 00289 const Eigen::Vector3f nc = input_->points[(*indices_)[R_]].getNormalVector3fMap (); 00290 00291 // Get a coordinate system that lies on a plane defined by its normal 00292 v_ = nc.unitOrthogonal (); 00293 u_ = nc.cross (v_); 00294 00295 // Projecting point onto the surface 00296 double dist = nc.dot(coords_[R_]); 00297 proj_qp_ = coords_[R_] - dist * nc; 00298 00299 // Converting coords, calculating angles and saving the projected near boundary edges 00300 int nr_edge = 0; 00301 std::vector<doubleEdge> doubleEdges; 00302 for (int i = 1; i < nnn_; i++) // nearest neighbor with index 0 is the query point R_ itself 00303 { 00304 tmp_ = coords_[nnIdx[i]] - proj_qp_; 00305 uvn_nn[i][0] = tmp_.dot(u_); 00306 uvn_nn[i][1] = tmp_.dot(v_); 00307 00308 // Computing the angle between each neighboring point and the query point itself 00309 angles_[i].angle = atan2(uvn_nn[i][1], uvn_nn[i][0]); 00310 // initializing angle descriptors 00311 angles_[i].index = nnIdx[i]; 00312 angles_[i].nnIndex = i; 00313 if ( 00314 (state_[nnIdx[i]] == COMPLETED) || (state_[nnIdx[i]] == BOUNDARY) 00315 || (sqrDists[i] > sqr_dist_threshold) 00316 ) 00317 angles_[i].visible = false; 00318 else 00319 angles_[i].visible = true; 00320 if ((ffn_[R_] == nnIdx[i]) || (sfn_[R_] == nnIdx[i])) 00321 angles_[i].visible = true; 00322 bool same_side = true; 00323 const Eigen::Vector3f neighbor_normal = input_->points[(*indices_)[nnIdx[i]]].getNormalVector3fMap (); 00324 double cosine = nc.dot (neighbor_normal); 00325 if (cosine > 1) cosine = 1; 00326 if (cosine < -1) cosine = -1; 00327 double angle = acos (cosine); 00328 if ((!consistent_) && (angle > M_PI/2)) 00329 angle = M_PI - angle; 00330 if (angle > eps_angle_) 00331 { 00332 angles_[i].visible = false; 00333 same_side = false; 00334 } 00335 // Saving the edges between nearby boundary points 00336 if ((i!=0) && (same_side) && ((state_[nnIdx[i]] == FRINGE) || (state_[nnIdx[i]] == BOUNDARY))) 00337 { 00338 doubleEdge e; 00339 e.index = i; 00340 nr_edge++; 00341 tmp_ = coords_[ffn_[nnIdx[i]]] - proj_qp_; 00342 e.first[0] = tmp_.dot(u_); 00343 e.first[1] = tmp_.dot(v_); 00344 tmp_ = coords_[sfn_[nnIdx[i]]] - proj_qp_; 00345 e.second[0] = tmp_.dot(u_); 00346 e.second[1] = tmp_.dot(v_); 00347 doubleEdges.push_back(e); 00348 // Pruning by visibility criterion 00349 if ((state_[nnIdx[i]] == FRINGE) && (ffn_[R_] != nnIdx[i]) && (sfn_[R_] != nnIdx[i])) 00350 { 00351 double angle1 = atan2(e.first[1] - uvn_nn[i][1], e.first[0] - uvn_nn[i][0]); 00352 double angle2 = atan2(e.second[1] - uvn_nn[i][1], e.second[0] - uvn_nn[i][0]); 00353 double angleMin, angleMax; 00354 if (angle1 < angle2) 00355 { 00356 angleMin = angle1; 00357 angleMax = angle2; 00358 } 00359 else 00360 { 00361 angleMin = angle2; 00362 angleMax = angle1; 00363 } 00364 double angleR = angles_[i].angle + M_PI; 00365 if (angleR >= 2*M_PI) 00366 angleR -= 2*M_PI; 00367 if ((source_[nnIdx[i]] == ffn_[nnIdx[i]]) || (source_[nnIdx[i]] == sfn_[nnIdx[i]])) 00368 { 00369 if ((angleMax - angleMin) < M_PI) 00370 { 00371 if ((angleMin < angleR) && (angleR < angleMax)) 00372 angles_[i].visible = false; 00373 } 00374 else 00375 { 00376 if ((angleR < angleMin) || (angleMax < angleR)) 00377 angles_[i].visible = false; 00378 } 00379 } 00380 else 00381 { 00382 tmp_ = coords_[source_[nnIdx[i]]] - proj_qp_; 00383 uvn_s[0] = tmp_.dot(u_); 00384 uvn_s[1] = tmp_.dot(v_); 00385 double angleS = atan2(uvn_s[1] - uvn_nn[i][1], uvn_s[0] - uvn_nn[i][0]); 00386 if ((angleMin < angleS) && (angleS < angleMax)) 00387 { 00388 if ((angleMin < angleR) && (angleR < angleMax)) 00389 angles_[i].visible = false; 00390 } 00391 else 00392 { 00393 if ((angleR < angleMin) || (angleMax < angleR)) 00394 angles_[i].visible = false; 00395 } 00396 } 00397 } 00398 } 00399 } 00400 angles_[0].visible = false; 00401 00402 // Verify the visibility of each potential new vertex 00403 for (int i = 1; i < nnn_; i++) // nearest neighbor with index 0 is the query point R_ itself 00404 if ((angles_[i].visible) && (ffn_[R_] != nnIdx[i]) && (sfn_[R_] != nnIdx[i])) 00405 { 00406 bool visibility = true; 00407 for (int j = 0; j < nr_edge; j++) 00408 { 00409 if (doubleEdges[j].index != i) 00410 { 00411 int f = ffn_[nnIdx[doubleEdges[j].index]]; 00412 if ((f != nnIdx[i]) && (f != R_)) 00413 visibility = isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].first, Eigen::Vector2f::Zero()); 00414 if (visibility == false) 00415 break; 00416 00417 int s = sfn_[nnIdx[doubleEdges[j].index]]; 00418 if ((s != nnIdx[i]) && (s != R_)) 00419 visibility = isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].second, Eigen::Vector2f::Zero()); 00420 if (visibility == false) 00421 break; 00422 } 00423 } 00424 angles_[i].visible = visibility; 00425 } 00426 00427 // Sorting angles 00428 std::sort (angles_.begin (), angles_.end (), GreedyProjectionTriangulation<PointInT>::nnAngleSortAsc); 00429 00430 // Triangulating 00431 if (angles_[2].visible == false) 00432 { 00433 if ( !( (angles_[0].index == ffn_[R_] && angles_[1].index == sfn_[R_]) || (angles_[0].index == sfn_[R_] && angles_[1].index == ffn_[R_]) ) ) 00434 { 00435 state_[R_] = BOUNDARY; 00436 } 00437 else 00438 { 00439 if ((source_[R_] == angles_[0].index) || (source_[R_] == angles_[1].index)) 00440 state_[R_] = BOUNDARY; 00441 else 00442 { 00443 if (sqr_max_edge < (coords_[ffn_[R_]] - coords_[sfn_[R_]]).squaredNorm ()) 00444 { 00445 state_[R_] = BOUNDARY; 00446 } 00447 else 00448 { 00449 tmp_ = coords_[source_[R_]] - proj_qp_; 00450 uvn_s[0] = tmp_.dot(u_); 00451 uvn_s[1] = tmp_.dot(v_); 00452 double angleS = atan2(uvn_s[1], uvn_s[0]); 00453 double dif = angles_[1].angle - angles_[0].angle; 00454 if ((angles_[0].angle < angleS) && (angleS < angles_[1].angle)) 00455 { 00456 if (dif < 2*M_PI - maximum_angle_) 00457 state_[R_] = BOUNDARY; 00458 else 00459 closeTriangle (output); 00460 } 00461 else 00462 { 00463 if (dif >= maximum_angle_) 00464 state_[R_] = BOUNDARY; 00465 else 00466 closeTriangle (output); 00467 } 00468 } 00469 } 00470 } 00471 continue; 00472 } 00473 00474 // Finding the FFN and SFN 00475 int start = -1, end = -1; 00476 for (int i=0; i<nnn_; i++) 00477 { 00478 if (ffn_[R_] == angles_[i].index) 00479 { 00480 start = i; 00481 if (sfn_[R_] == angles_[i+1].index) 00482 end = i+1; 00483 else 00484 if (i==0) 00485 { 00486 for (i = i+2; i < nnn_; i++) 00487 if (sfn_[R_] == angles_[i].index) 00488 break; 00489 end = i; 00490 } 00491 else 00492 { 00493 for (i = i+2; i < nnn_; i++) 00494 if (sfn_[R_] == angles_[i].index) 00495 break; 00496 end = i; 00497 } 00498 break; 00499 } 00500 if (sfn_[R_] == angles_[i].index) 00501 { 00502 start = i; 00503 if (ffn_[R_] == angles_[i+1].index) 00504 end = i+1; 00505 else 00506 if (i==0) 00507 { 00508 for (i = i+2; i < nnn_; i++) 00509 if (ffn_[R_] == angles_[i].index) 00510 break; 00511 end = i; 00512 } 00513 else 00514 { 00515 for (i = i+2; i < nnn_; i++) 00516 if (ffn_[R_] == angles_[i].index) 00517 break; 00518 end = i; 00519 } 00520 break; 00521 } 00522 } 00523 00524 // start and end are always set, as we checked if ffn or sfn are out of range before, but let's check anyways if < 0 00525 if ((start < 0) || (end < 0) || (end == nnn_) || (!angles_[start].visible) || (!angles_[end].visible)) 00526 { 00527 state_[R_] = BOUNDARY; 00528 continue; 00529 } 00530 00531 // Finding last visible nn 00532 int last_visible = end; 00533 while ((last_visible+1<nnn_) && (angles_[last_visible+1].visible)) last_visible++; 00534 00535 // Finding visibility region of R 00536 bool need_invert = false; 00537 int sourceIdx = nnn_; 00538 if ((source_[R_] == ffn_[R_]) || (source_[R_] == sfn_[R_])) 00539 { 00540 if ((angles_[end].angle - angles_[start].angle) < M_PI) 00541 need_invert = true; 00542 } 00543 else 00544 { 00545 for (sourceIdx=0; sourceIdx<nnn_; sourceIdx++) 00546 if (angles_[sourceIdx].index == source_[R_]) 00547 break; 00548 if (sourceIdx == nnn_) 00549 { 00550 int vis_free = NONE, nnCB = NONE; // any free visible and nearest completed or boundary neighbor of R 00551 for (int i = 1; i < nnn_; i++) // nearest neighbor with index 0 is the query point R_ itself 00552 { 00553 // NOTE: nnCB is an index in nnIdx 00554 if ((state_[nnIdx[i]] == COMPLETED) || (state_[nnIdx[i]] == BOUNDARY)) 00555 { 00556 if (nnCB == NONE) 00557 { 00558 nnCB = i; 00559 if (vis_free != NONE) 00560 break; 00561 } 00562 } 00563 // NOTE: vis_free is an index in angles 00564 if (state_[angles_[i].index] <= FREE) 00565 { 00566 if (i <= last_visible) 00567 { 00568 vis_free = i; 00569 if (nnCB != NONE) 00570 break; 00571 } 00572 } 00573 } 00574 // NOTE: nCB is an index in angles 00575 int nCB = 0; 00576 if (nnCB != NONE) 00577 while (angles_[nCB].index != nnIdx[nnCB]) nCB++; 00578 else 00579 nCB = NONE; 00580 00581 if (vis_free != NONE) 00582 { 00583 if ((vis_free < start) || (vis_free > end)) 00584 need_invert = true; 00585 } 00586 else 00587 { 00588 if (nCB != NONE) 00589 { 00590 if ((nCB == start) || (nCB == end)) 00591 { 00592 bool inside_CB = false; 00593 bool outside_CB = false; 00594 for (int i=0; i<nnn_; i++) 00595 { 00596 if ( 00597 ((state_[angles_[i].index] == COMPLETED) || (state_[angles_[i].index] == BOUNDARY)) 00598 && (i != start) && (i != end) 00599 ) 00600 { 00601 if ((angles_[start].angle <= angles_[i].angle) && (angles_[i].angle <= angles_[end].angle)) 00602 { 00603 inside_CB = true; 00604 if (outside_CB) 00605 break; 00606 } 00607 else 00608 { 00609 outside_CB = true; 00610 if (inside_CB) 00611 break; 00612 } 00613 } 00614 } 00615 if (inside_CB && !outside_CB) 00616 need_invert = true; 00617 else if (!(!inside_CB && outside_CB)) 00618 { 00619 if ((angles_[end].angle - angles_[start].angle) < M_PI) 00620 need_invert = true; 00621 } 00622 } 00623 else 00624 { 00625 if ((angles_[nCB].angle > angles_[start].angle) && (angles_[nCB].angle < angles_[end].angle)) 00626 need_invert = true; 00627 } 00628 } 00629 else 00630 { 00631 if (start == end-1) 00632 need_invert = true; 00633 } 00634 } 00635 } 00636 else if ((angles_[start].angle < angles_[sourceIdx].angle) && (angles_[sourceIdx].angle < angles_[end].angle)) 00637 need_invert = true; 00638 } 00639 00640 // switching start and end if necessary 00641 if (need_invert) 00642 { 00643 int tmp = start; 00644 start = end; 00645 end = tmp; 00646 } 00647 00648 // Arranging visible nnAngles in the order they need to be connected and 00649 // compute the maximal angle difference between two consecutive visible angles 00650 bool is_boundary = false, is_skinny = false; 00651 std::vector<bool> gaps (nnn_, false); 00652 std::vector<bool> skinny (nnn_, false); 00653 std::vector<double> dif (nnn_); 00654 std::vector<int> angleIdx; angleIdx.reserve (nnn_); 00655 if (start > end) 00656 { 00657 for (int j=start; j<last_visible; j++) 00658 { 00659 dif[j] = (angles_[j+1].angle - angles_[j].angle); 00660 if (dif[j] < minimum_angle_) 00661 { 00662 skinny[j] = is_skinny = true; 00663 } 00664 else if (maximum_angle_ <= dif[j]) 00665 { 00666 gaps[j] = is_boundary = true; 00667 } 00668 if ((!gaps[j]) && (sqr_max_edge < (coords_[angles_[j+1].index] - coords_[angles_[j].index]).squaredNorm ())) 00669 { 00670 gaps[j] = is_boundary = true; 00671 } 00672 angleIdx.push_back(j); 00673 } 00674 00675 dif[last_visible] = (2*M_PI + angles_[0].angle - angles_[last_visible].angle); 00676 if (dif[last_visible] < minimum_angle_) 00677 { 00678 skinny[last_visible] = is_skinny = true; 00679 } 00680 else if (maximum_angle_ <= dif[last_visible]) 00681 { 00682 gaps[last_visible] = is_boundary = true; 00683 } 00684 if ((!gaps[last_visible]) && (sqr_max_edge < (coords_[angles_[0].index] - coords_[angles_[last_visible].index]).squaredNorm ())) 00685 { 00686 gaps[last_visible] = is_boundary = true; 00687 } 00688 angleIdx.push_back(last_visible); 00689 00690 for (int j=0; j<end; j++) 00691 { 00692 dif[j] = (angles_[j+1].angle - angles_[j].angle); 00693 if (dif[j] < minimum_angle_) 00694 { 00695 skinny[j] = is_skinny = true; 00696 } 00697 else if (maximum_angle_ <= dif[j]) 00698 { 00699 gaps[j] = is_boundary = true; 00700 } 00701 if ((!gaps[j]) && (sqr_max_edge < (coords_[angles_[j+1].index] - coords_[angles_[j].index]).squaredNorm ())) 00702 { 00703 gaps[j] = is_boundary = true; 00704 } 00705 angleIdx.push_back(j); 00706 } 00707 angleIdx.push_back(end); 00708 } 00709 // start < end 00710 else 00711 { 00712 for (int j=start; j<end; j++) 00713 { 00714 dif[j] = (angles_[j+1].angle - angles_[j].angle); 00715 if (dif[j] < minimum_angle_) 00716 { 00717 skinny[j] = is_skinny = true; 00718 } 00719 else if (maximum_angle_ <= dif[j]) 00720 { 00721 gaps[j] = is_boundary = true; 00722 } 00723 if ((!gaps[j]) && (sqr_max_edge < (coords_[angles_[j+1].index] - coords_[angles_[j].index]).squaredNorm ())) 00724 { 00725 gaps[j] = is_boundary = true; 00726 } 00727 angleIdx.push_back(j); 00728 } 00729 angleIdx.push_back(end); 00730 } 00731 00732 // Set the state of the point 00733 state_[R_] = is_boundary ? BOUNDARY : COMPLETED; 00734 00735 std::vector<int>::iterator first_gap_after = angleIdx.end (); 00736 std::vector<int>::iterator last_gap_before = angleIdx.begin (); 00737 int nr_gaps = 0; 00738 for (std::vector<int>::iterator it = angleIdx.begin (); it != angleIdx.end () - 1; it++) 00739 { 00740 if (gaps[*it]) 00741 { 00742 nr_gaps++; 00743 if (first_gap_after == angleIdx.end()) 00744 first_gap_after = it; 00745 last_gap_before = it+1; 00746 } 00747 } 00748 if (nr_gaps > 1) 00749 { 00750 angleIdx.erase(first_gap_after+1, last_gap_before); 00751 } 00752 00753 // Neglecting points that would form skinny triangles (if possible) 00754 if (is_skinny) 00755 { 00756 double angle_so_far = 0, angle_would_be; 00757 double max_combined_angle = (std::min)(maximum_angle_, M_PI-2*minimum_angle_); 00758 Eigen::Vector2f X; 00759 Eigen::Vector2f S1; 00760 Eigen::Vector2f S2; 00761 std::vector<int> to_erase; 00762 for (std::vector<int>::iterator it = angleIdx.begin()+1; it != angleIdx.end()-1; it++) 00763 { 00764 if (gaps[*(it-1)]) 00765 angle_so_far = 0; 00766 else 00767 angle_so_far += dif[*(it-1)]; 00768 if (gaps[*it]) 00769 angle_would_be = angle_so_far; 00770 else 00771 angle_would_be = angle_so_far + dif[*it]; 00772 if ( 00773 (skinny[*it] || skinny[*(it-1)]) && 00774 ((state_[angles_[*it].index] <= FREE) || (state_[angles_[*(it-1)].index] <= FREE)) && 00775 ((!gaps[*it]) || (angles_[*it].nnIndex > angles_[*(it-1)].nnIndex)) && 00776 ((!gaps[*(it-1)]) || (angles_[*it].nnIndex > angles_[*(it+1)].nnIndex)) && 00777 (angle_would_be < max_combined_angle) 00778 ) 00779 { 00780 if (gaps[*(it-1)]) 00781 { 00782 gaps[*it] = true; 00783 to_erase.push_back(*it); 00784 } 00785 else if (gaps[*it]) 00786 { 00787 gaps[*(it-1)] = true; 00788 to_erase.push_back(*it); 00789 } 00790 else 00791 { 00792 std::vector<int>::iterator prev_it; 00793 int erased_idx = to_erase.size()-1; 00794 for (prev_it = it-1; (erased_idx != -1) && (it != angleIdx.begin()); it--) 00795 if (*it == to_erase[erased_idx]) 00796 erased_idx--; 00797 else 00798 break; 00799 bool can_delete = true; 00800 for (std::vector<int>::iterator curr_it = prev_it+1; curr_it != it+1; curr_it++) 00801 { 00802 tmp_ = coords_[angles_[*curr_it].index] - proj_qp_; 00803 X[0] = tmp_.dot(u_); 00804 X[1] = tmp_.dot(v_); 00805 tmp_ = coords_[angles_[*prev_it].index] - proj_qp_; 00806 S1[0] = tmp_.dot(u_); 00807 S1[1] = tmp_.dot(v_); 00808 tmp_ = coords_[angles_[*(it+1)].index] - proj_qp_; 00809 S2[0] = tmp_.dot(u_); 00810 S2[1] = tmp_.dot(v_); 00811 // check for inclusions 00812 if (isVisible(X,S1,S2)) 00813 { 00814 can_delete = false; 00815 angle_so_far = 0; 00816 break; 00817 } 00818 } 00819 if (can_delete) 00820 { 00821 to_erase.push_back(*it); 00822 } 00823 } 00824 } 00825 else 00826 angle_so_far = 0; 00827 } 00828 for (std::vector<int>::iterator it = to_erase.begin(); it != to_erase.end(); it++) 00829 { 00830 for (std::vector<int>::iterator iter = angleIdx.begin(); iter != angleIdx.end(); iter++) 00831 if (*it == *iter) 00832 { 00833 angleIdx.erase(iter); 00834 break; 00835 } 00836 } 00837 } 00838 00839 // Writing edges and updating edge-front 00840 changed_1st_fn_ = false; 00841 changed_2nd_fn_ = false; 00842 new2boundary_ = NONE; 00843 for (std::vector<int>::iterator it = angleIdx.begin()+1; it != angleIdx.end()-1; it++) 00844 { 00845 current_index_ = angles_[*it].index; 00846 00847 is_current_free_ = false; 00848 if (state_[current_index_] <= FREE) 00849 { 00850 state_[current_index_] = FRINGE; 00851 is_current_free_ = true; 00852 } 00853 else if (!already_connected_) 00854 { 00855 prev_is_ffn_ = (ffn_[current_index_] == angles_[*(it-1)].index) && (!gaps[*(it-1)]); 00856 prev_is_sfn_ = (sfn_[current_index_] == angles_[*(it-1)].index) && (!gaps[*(it-1)]); 00857 next_is_ffn_ = (ffn_[current_index_] == angles_[*(it+1)].index) && (!gaps[*it]); 00858 next_is_sfn_ = (sfn_[current_index_] == angles_[*(it+1)].index) && (!gaps[*it]); 00859 if (!prev_is_ffn_ && !next_is_sfn_ && !prev_is_sfn_ && !next_is_ffn_) 00860 { 00861 nr_touched++; 00862 } 00863 } 00864 00865 if (gaps[*it]) 00866 if (gaps[*(it-1)]) 00867 { 00868 if (is_current_free_) 00869 state_[current_index_] = NONE; 00870 } 00871 00872 else // (gaps[*it]) && ^(gaps[*(it-1)]) 00873 { 00874 addTriangle (current_index_, angles_[*(it-1)].index, R_, output); 00875 addFringePoint (current_index_, R_); 00876 new2boundary_ = current_index_; 00877 if (!already_connected_) 00878 connectPoint (output, angles_[*(it-1)].index, R_, 00879 angles_[*(it+1)].index, 00880 uvn_nn[angles_[*it].nnIndex], uvn_nn[angles_[*(it-1)].nnIndex], uvn_nn_qp_zero); 00881 else already_connected_ = false; 00882 if (ffn_[R_] == angles_[*(angleIdx.begin())].index) 00883 { 00884 ffn_[R_] = new2boundary_; 00885 } 00886 else if (sfn_[R_] == angles_[*(angleIdx.begin())].index) 00887 { 00888 sfn_[R_] = new2boundary_; 00889 } 00890 } 00891 00892 else // ^(gaps[*it]) 00893 if (gaps[*(it-1)]) 00894 { 00895 addFringePoint (current_index_, R_); 00896 new2boundary_ = current_index_; 00897 if (!already_connected_) connectPoint(output, R_, angles_[*(it+1)].index, 00898 (it+2) == angleIdx.end() ? -1 : angles_[*(it+2)].index, 00899 uvn_nn[angles_[*it].nnIndex], uvn_nn_qp_zero, uvn_nn[angles_[*(it+1)].nnIndex]); 00900 else already_connected_ = false; 00901 if (ffn_[R_] == angles_[*(angleIdx.end()-1)].index) 00902 { 00903 ffn_[R_] = new2boundary_; 00904 } 00905 else if (sfn_[R_] == angles_[*(angleIdx.end()-1)].index) 00906 { 00907 sfn_[R_] = new2boundary_; 00908 } 00909 } 00910 00911 else // ^(gaps[*it]) && ^(gaps[*(it-1)]) 00912 { 00913 addTriangle (current_index_, angles_[*(it-1)].index, R_, output); 00914 addFringePoint (current_index_, R_); 00915 if (!already_connected_) connectPoint(output, angles_[*(it-1)].index, angles_[*(it+1)].index, 00916 (it+2) == angleIdx.end() ? -1 : gaps[*(it+1)] ? R_ : angles_[*(it+2)].index, 00917 uvn_nn[angles_[*it].nnIndex], uvn_nn[angles_[*(it-1)].nnIndex], uvn_nn[angles_[*(it+1)].nnIndex]); 00918 else already_connected_ = false; 00919 } 00920 } 00921 00922 // Finishing up R_ 00923 if (ffn_[R_] == sfn_[R_]) 00924 { 00925 state_[R_] = COMPLETED; 00926 } 00927 if (!gaps[*(angleIdx.end()-2)]) 00928 { 00929 addTriangle (angles_[*(angleIdx.end()-2)].index, angles_[*(angleIdx.end()-1)].index, R_, output); 00930 addFringePoint (angles_[*(angleIdx.end()-2)].index, R_); 00931 if (R_ == ffn_[angles_[*(angleIdx.end()-1)].index]) 00932 { 00933 if (angles_[*(angleIdx.end()-2)].index == sfn_[angles_[*(angleIdx.end()-1)].index]) 00934 { 00935 state_[angles_[*(angleIdx.end()-1)].index] = COMPLETED; 00936 } 00937 else 00938 { 00939 ffn_[angles_[*(angleIdx.end()-1)].index] = angles_[*(angleIdx.end()-2)].index; 00940 } 00941 } 00942 else if (R_ == sfn_[angles_[*(angleIdx.end()-1)].index]) 00943 { 00944 if (angles_[*(angleIdx.end()-2)].index == ffn_[angles_[*(angleIdx.end()-1)].index]) 00945 { 00946 state_[angles_[*(angleIdx.end()-1)].index] = COMPLETED; 00947 } 00948 else 00949 { 00950 sfn_[angles_[*(angleIdx.end()-1)].index] = angles_[*(angleIdx.end()-2)].index; 00951 } 00952 } 00953 } 00954 if (!gaps[*(angleIdx.begin())]) 00955 { 00956 if (R_ == ffn_[angles_[*(angleIdx.begin())].index]) 00957 { 00958 if (angles_[*(angleIdx.begin()+1)].index == sfn_[angles_[*(angleIdx.begin())].index]) 00959 { 00960 state_[angles_[*(angleIdx.begin())].index] = COMPLETED; 00961 } 00962 else 00963 { 00964 ffn_[angles_[*(angleIdx.begin())].index] = angles_[*(angleIdx.begin()+1)].index; 00965 } 00966 } 00967 else if (R_ == sfn_[angles_[*(angleIdx.begin())].index]) 00968 { 00969 if (angles_[*(angleIdx.begin()+1)].index == ffn_[angles_[*(angleIdx.begin())].index]) 00970 { 00971 state_[angles_[*(angleIdx.begin())].index] = COMPLETED; 00972 } 00973 else 00974 { 00975 sfn_[angles_[*(angleIdx.begin())].index] = angles_[*(angleIdx.begin()+1)].index; 00976 } 00977 } 00978 } 00979 } 00980 } 00981 PCL_DEBUG ("Number of triangles: %d\n", (int)output.polygons.size()); 00982 PCL_DEBUG ("Number of unconnected parts: %d\n", nr_parts); 00983 if (increase_nnn4fn > 0) 00984 PCL_WARN ("Number of neighborhood size increase requests for fringe neighbors: %d\n", increase_nnn4fn); 00985 if (increase_nnn4s > 0) 00986 PCL_WARN ("Number of neighborhood size increase requests for source: %d\n", increase_nnn4s); 00987 if (increase_dist > 0) 00988 PCL_WARN ("Number of automatic maximum distance increases: %d\n", increase_dist); 00989 00990 // sorting and removing doubles from fringe queue 00991 std::sort (fringe_queue_.begin (), fringe_queue_.end ()); 00992 fringe_queue_.erase (std::unique (fringe_queue_.begin (), fringe_queue_.end ()), fringe_queue_.end ()); 00993 PCL_DEBUG ("Number of processed points: %d / %d\n", (int)fringe_queue_.size(), (int)indices_->size ()); 00994 } 00995 00997 template <typename PointInT> void 00998 pcl::GreedyProjectionTriangulation<PointInT>::closeTriangle (pcl::PolygonMesh &output) 00999 { 01000 state_[R_] = COMPLETED; 01001 addTriangle (angles_[0].index, angles_[1].index, R_, output); 01002 for (int aIdx=0; aIdx<2; aIdx++) 01003 { 01004 if (ffn_[angles_[aIdx].index] == R_) 01005 { 01006 if (sfn_[angles_[aIdx].index] == angles_[(aIdx+1)%2].index) 01007 { 01008 state_[angles_[aIdx].index] = COMPLETED; 01009 } 01010 else 01011 { 01012 ffn_[angles_[aIdx].index] = angles_[(aIdx+1)%2].index; 01013 } 01014 } 01015 else if (sfn_[angles_[aIdx].index] == R_) 01016 { 01017 if (ffn_[angles_[aIdx].index] == angles_[(aIdx+1)%2].index) 01018 { 01019 state_[angles_[aIdx].index] = COMPLETED; 01020 } 01021 else 01022 { 01023 sfn_[angles_[aIdx].index] = angles_[(aIdx+1)%2].index; 01024 } 01025 } 01026 } 01027 } 01028 01030 template <typename PointInT> void 01031 pcl::GreedyProjectionTriangulation<PointInT>::connectPoint ( 01032 pcl::PolygonMesh &output, 01033 const int prev_index, const int next_index, const int next_next_index, 01034 const Eigen::Vector2f &uvn_current, 01035 const Eigen::Vector2f &uvn_prev, 01036 const Eigen::Vector2f &uvn_next) 01037 { 01038 if (is_current_free_) 01039 { 01040 ffn_[current_index_] = prev_index; 01041 sfn_[current_index_] = next_index; 01042 } 01043 else 01044 { 01045 if ((prev_is_ffn_ && next_is_sfn_) || (prev_is_sfn_ && next_is_ffn_)) 01046 { 01047 state_[current_index_] = COMPLETED; 01048 } 01049 else if (prev_is_ffn_ && !next_is_sfn_) 01050 { 01051 ffn_[current_index_] = next_index; 01052 } 01053 else if (next_is_ffn_ && !prev_is_sfn_) 01054 { 01055 ffn_[current_index_] = prev_index; 01056 } 01057 else if (prev_is_sfn_ && !next_is_ffn_) 01058 { 01059 sfn_[current_index_] = next_index; 01060 } 01061 else if (next_is_sfn_ && !prev_is_ffn_) 01062 { 01063 sfn_[current_index_] = prev_index; 01064 } 01065 else 01066 { 01067 bool found_triangle = false; 01068 if ((prev_index != R_) && ((ffn_[current_index_] == ffn_[prev_index]) || (ffn_[current_index_] == sfn_[prev_index]))) 01069 { 01070 found_triangle = true; 01071 addTriangle (current_index_, ffn_[current_index_], prev_index, output); 01072 state_[prev_index] = COMPLETED; 01073 state_[ffn_[current_index_]] = COMPLETED; 01074 ffn_[current_index_] = next_index; 01075 } 01076 else if ((prev_index != R_) && ((sfn_[current_index_] == ffn_[prev_index]) || (sfn_[current_index_] == sfn_[prev_index]))) 01077 { 01078 found_triangle = true; 01079 addTriangle (current_index_, sfn_[current_index_], prev_index, output); 01080 state_[prev_index] = COMPLETED; 01081 state_[sfn_[current_index_]] = COMPLETED; 01082 sfn_[current_index_] = next_index; 01083 } 01084 else if (state_[next_index] > FREE) 01085 { 01086 if ((ffn_[current_index_] == ffn_[next_index]) || (ffn_[current_index_] == sfn_[next_index])) 01087 { 01088 found_triangle = true; 01089 addTriangle (current_index_, ffn_[current_index_], next_index, output); 01090 01091 if (ffn_[current_index_] == ffn_[next_index]) 01092 { 01093 ffn_[next_index] = current_index_; 01094 } 01095 else 01096 { 01097 sfn_[next_index] = current_index_; 01098 } 01099 state_[ffn_[current_index_]] = COMPLETED; 01100 ffn_[current_index_] = prev_index; 01101 } 01102 else if ((sfn_[current_index_] == ffn_[next_index]) || (sfn_[current_index_] == sfn_[next_index])) 01103 { 01104 found_triangle = true; 01105 addTriangle (current_index_, sfn_[current_index_], next_index, output); 01106 01107 if (sfn_[current_index_] == ffn_[next_index]) 01108 { 01109 ffn_[next_index] = current_index_; 01110 } 01111 else 01112 { 01113 sfn_[next_index] = current_index_; 01114 } 01115 state_[sfn_[current_index_]] = COMPLETED; 01116 sfn_[current_index_] = prev_index; 01117 } 01118 } 01119 01120 if (found_triangle) 01121 { 01122 } 01123 else 01124 { 01125 tmp_ = coords_[ffn_[current_index_]] - proj_qp_; 01126 uvn_ffn_[0] = tmp_.dot(u_); 01127 uvn_ffn_[1] = tmp_.dot(v_); 01128 tmp_ = coords_[sfn_[current_index_]] - proj_qp_; 01129 uvn_sfn_[0] = tmp_.dot(u_); 01130 uvn_sfn_[1] = tmp_.dot(v_); 01131 bool prev_ffn = isVisible(uvn_prev, uvn_next, uvn_current, uvn_ffn_) && isVisible(uvn_prev, uvn_sfn_, uvn_current, uvn_ffn_); 01132 bool prev_sfn = isVisible(uvn_prev, uvn_next, uvn_current, uvn_sfn_) && isVisible(uvn_prev, uvn_ffn_, uvn_current, uvn_sfn_); 01133 bool next_ffn = isVisible(uvn_next, uvn_prev, uvn_current, uvn_ffn_) && isVisible(uvn_next, uvn_sfn_, uvn_current, uvn_ffn_); 01134 bool next_sfn = isVisible(uvn_next, uvn_prev, uvn_current, uvn_sfn_) && isVisible(uvn_next, uvn_ffn_, uvn_current, uvn_sfn_); 01135 int min_dist = -1; 01136 if (prev_ffn && next_sfn && prev_sfn && next_ffn) 01137 { 01138 /* should be never the case */ 01139 double prev2f = (coords_[ffn_[current_index_]] - coords_[prev_index]).squaredNorm (); 01140 double next2s = (coords_[sfn_[current_index_]] - coords_[next_index]).squaredNorm (); 01141 double prev2s = (coords_[sfn_[current_index_]] - coords_[prev_index]).squaredNorm (); 01142 double next2f = (coords_[ffn_[current_index_]] - coords_[next_index]).squaredNorm (); 01143 if (prev2f < prev2s) 01144 { 01145 if (prev2f < next2f) 01146 { 01147 if (prev2f < next2s) 01148 min_dist = 0; 01149 else 01150 min_dist = 3; 01151 } 01152 else 01153 { 01154 if (next2f < next2s) 01155 min_dist = 2; 01156 else 01157 min_dist = 3; 01158 } 01159 } 01160 else 01161 { 01162 if (prev2s < next2f) 01163 { 01164 if (prev2s < next2s) 01165 min_dist = 1; 01166 else 01167 min_dist = 3; 01168 } 01169 else 01170 { 01171 if (next2f < next2s) 01172 min_dist = 2; 01173 else 01174 min_dist = 3; 01175 } 01176 } 01177 } 01178 else if (prev_ffn && next_sfn) 01179 { 01180 /* a clear case */ 01181 double prev2f = (coords_[ffn_[current_index_]] - coords_[prev_index]).squaredNorm (); 01182 double next2s = (coords_[sfn_[current_index_]] - coords_[next_index]).squaredNorm (); 01183 if (prev2f < next2s) 01184 min_dist = 0; 01185 else 01186 min_dist = 3; 01187 } 01188 else if (prev_sfn && next_ffn) 01189 { 01190 /* a clear case */ 01191 double prev2s = (coords_[sfn_[current_index_]] - coords_[prev_index]).squaredNorm (); 01192 double next2f = (coords_[ffn_[current_index_]] - coords_[next_index]).squaredNorm (); 01193 if (prev2s < next2f) 01194 min_dist = 1; 01195 else 01196 min_dist = 2; 01197 } 01198 /* straightforward cases */ 01199 else if (prev_ffn && !next_sfn && !prev_sfn && !next_ffn) 01200 min_dist = 0; 01201 else if (!prev_ffn && !next_sfn && prev_sfn && !next_ffn) 01202 min_dist = 1; 01203 else if (!prev_ffn && !next_sfn && !prev_sfn && next_ffn) 01204 min_dist = 2; 01205 else if (!prev_ffn && next_sfn && !prev_sfn && !next_ffn) 01206 min_dist = 3; 01207 /* messed up cases */ 01208 else if (prev_ffn) 01209 { 01210 double prev2f = (coords_[ffn_[current_index_]] - coords_[prev_index]).squaredNorm (); 01211 if (prev_sfn) 01212 { 01213 double prev2s = (coords_[sfn_[current_index_]] - coords_[prev_index]).squaredNorm (); 01214 if (prev2s < prev2f) 01215 min_dist = 1; 01216 else 01217 min_dist = 0; 01218 } 01219 else if (next_ffn) 01220 { 01221 double next2f = (coords_[ffn_[current_index_]] - coords_[next_index]).squaredNorm (); 01222 if (next2f < prev2f) 01223 min_dist = 2; 01224 else 01225 min_dist = 0; 01226 } 01227 } 01228 else if (next_sfn) 01229 { 01230 double next2s = (coords_[sfn_[current_index_]] - coords_[next_index]).squaredNorm (); 01231 if (prev_sfn) 01232 { 01233 double prev2s = (coords_[sfn_[current_index_]] - coords_[prev_index]).squaredNorm (); 01234 if (prev2s < next2s) 01235 min_dist = 1; 01236 else 01237 min_dist = 3; 01238 } 01239 else if (next_ffn) 01240 { 01241 double next2f = (coords_[ffn_[current_index_]] - coords_[next_index]).squaredNorm (); 01242 if (next2f < next2s) 01243 min_dist = 2; 01244 else 01245 min_dist = 3; 01246 } 01247 } 01248 switch (min_dist) 01249 { 01250 case 0://prev2f: 01251 { 01252 addTriangle (current_index_, ffn_[current_index_], prev_index, output); 01253 01254 /* updating prev_index */ 01255 if (ffn_[prev_index] == current_index_) 01256 { 01257 ffn_[prev_index] = ffn_[current_index_]; 01258 } 01259 else if (sfn_[prev_index] == current_index_) 01260 { 01261 sfn_[prev_index] = ffn_[current_index_]; 01262 } 01263 else if (ffn_[prev_index] == R_) 01264 { 01265 changed_1st_fn_ = true; 01266 ffn_[prev_index] = ffn_[current_index_]; 01267 } 01268 else if (sfn_[prev_index] == R_) 01269 { 01270 changed_1st_fn_ = true; 01271 sfn_[prev_index] = ffn_[current_index_]; 01272 } 01273 else if (prev_index == R_) 01274 { 01275 new2boundary_ = ffn_[current_index_]; 01276 } 01277 01278 /* updating ffn */ 01279 if (ffn_[ffn_[current_index_]] == current_index_) 01280 { 01281 ffn_[ffn_[current_index_]] = prev_index; 01282 } 01283 else if (sfn_[ffn_[current_index_]] == current_index_) 01284 { 01285 sfn_[ffn_[current_index_]] = prev_index; 01286 } 01287 01288 /* updating current */ 01289 ffn_[current_index_] = next_index; 01290 01291 break; 01292 } 01293 case 1://prev2s: 01294 { 01295 addTriangle (current_index_, sfn_[current_index_], prev_index, output); 01296 01297 /* updating prev_index */ 01298 if (ffn_[prev_index] == current_index_) 01299 { 01300 ffn_[prev_index] = sfn_[current_index_]; 01301 } 01302 else if (sfn_[prev_index] == current_index_) 01303 { 01304 sfn_[prev_index] = sfn_[current_index_]; 01305 } 01306 else if (ffn_[prev_index] == R_) 01307 { 01308 changed_1st_fn_ = true; 01309 ffn_[prev_index] = sfn_[current_index_]; 01310 } 01311 else if (sfn_[prev_index] == R_) 01312 { 01313 changed_1st_fn_ = true; 01314 sfn_[prev_index] = sfn_[current_index_]; 01315 } 01316 else if (prev_index == R_) 01317 { 01318 new2boundary_ = sfn_[current_index_]; 01319 } 01320 01321 /* updating sfn */ 01322 if (ffn_[sfn_[current_index_]] == current_index_) 01323 { 01324 ffn_[sfn_[current_index_]] = prev_index; 01325 } 01326 else if (sfn_[sfn_[current_index_]] == current_index_) 01327 { 01328 sfn_[sfn_[current_index_]] = prev_index; 01329 } 01330 01331 /* updating current */ 01332 sfn_[current_index_] = next_index; 01333 01334 break; 01335 } 01336 case 2://next2f: 01337 { 01338 addTriangle (current_index_, ffn_[current_index_], next_index, output); 01339 int neighbor_update = next_index; 01340 01341 /* updating next_index */ 01342 if (state_[next_index] <= FREE) 01343 { 01344 state_[next_index] = FRINGE; 01345 ffn_[next_index] = current_index_; 01346 sfn_[next_index] = ffn_[current_index_]; 01347 } 01348 else 01349 { 01350 if (ffn_[next_index] == R_) 01351 { 01352 changed_2nd_fn_ = true; 01353 ffn_[next_index] = ffn_[current_index_]; 01354 } 01355 else if (sfn_[next_index] == R_) 01356 { 01357 changed_2nd_fn_ = true; 01358 sfn_[next_index] = ffn_[current_index_]; 01359 } 01360 else if (next_index == R_) 01361 { 01362 new2boundary_ = ffn_[current_index_]; 01363 if (next_next_index == new2boundary_) 01364 already_connected_ = true; 01365 } 01366 else if (ffn_[next_index] == next_next_index) 01367 { 01368 already_connected_ = true; 01369 ffn_[next_index] = ffn_[current_index_]; 01370 } 01371 else if (sfn_[next_index] == next_next_index) 01372 { 01373 already_connected_ = true; 01374 sfn_[next_index] = ffn_[current_index_]; 01375 } 01376 else 01377 { 01378 tmp_ = coords_[ffn_[next_index]] - proj_qp_; 01379 uvn_next_ffn_[0] = tmp_.dot(u_); 01380 uvn_next_ffn_[1] = tmp_.dot(v_); 01381 tmp_ = coords_[sfn_[next_index]] - proj_qp_; 01382 uvn_next_sfn_[0] = tmp_.dot(u_); 01383 uvn_next_sfn_[1] = tmp_.dot(v_); 01384 01385 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_); 01386 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_); 01387 01388 int connect2ffn = -1; 01389 if (ffn_next_ffn && sfn_next_ffn) 01390 { 01391 double fn2f = (coords_[ffn_[current_index_]] - coords_[ffn_[next_index]]).squaredNorm (); 01392 double sn2f = (coords_[ffn_[current_index_]] - coords_[sfn_[next_index]]).squaredNorm (); 01393 if (fn2f < sn2f) connect2ffn = 0; 01394 else connect2ffn = 1; 01395 } 01396 else if (ffn_next_ffn) connect2ffn = 0; 01397 else if (sfn_next_ffn) connect2ffn = 1; 01398 01399 switch (connect2ffn) 01400 { 01401 case 0: // ffn[next] 01402 { 01403 addTriangle (next_index, ffn_[current_index_], ffn_[next_index], output); 01404 neighbor_update = ffn_[next_index]; 01405 01406 /* ffn[next_index] */ 01407 if ((ffn_[ffn_[next_index]] == ffn_[current_index_]) || (sfn_[ffn_[next_index]] == ffn_[current_index_])) 01408 { 01409 state_[ffn_[next_index]] = COMPLETED; 01410 } 01411 else if (ffn_[ffn_[next_index]] == next_index) 01412 { 01413 ffn_[ffn_[next_index]] = ffn_[current_index_]; 01414 } 01415 else if (sfn_[ffn_[next_index]] == next_index) 01416 { 01417 sfn_[ffn_[next_index]] = ffn_[current_index_]; 01418 } 01419 01420 ffn_[next_index] = current_index_; 01421 01422 break; 01423 } 01424 case 1: // sfn[next] 01425 { 01426 addTriangle (next_index, ffn_[current_index_], sfn_[next_index], output); 01427 neighbor_update = sfn_[next_index]; 01428 01429 /* sfn[next_index] */ 01430 if ((ffn_[sfn_[next_index]] = ffn_[current_index_]) || (sfn_[sfn_[next_index]] == ffn_[current_index_])) 01431 { 01432 state_[sfn_[next_index]] = COMPLETED; 01433 } 01434 else if (ffn_[sfn_[next_index]] == next_index) 01435 { 01436 ffn_[sfn_[next_index]] = ffn_[current_index_]; 01437 } 01438 else if (sfn_[sfn_[next_index]] == next_index) 01439 { 01440 sfn_[sfn_[next_index]] = ffn_[current_index_]; 01441 } 01442 01443 sfn_[next_index] = current_index_; 01444 01445 break; 01446 } 01447 default:; 01448 } 01449 } 01450 } 01451 01452 /* updating ffn */ 01453 if ((ffn_[ffn_[current_index_]] == neighbor_update) || (sfn_[ffn_[current_index_]] == neighbor_update)) 01454 { 01455 state_[ffn_[current_index_]] = COMPLETED; 01456 } 01457 else if (ffn_[ffn_[current_index_]] == current_index_) 01458 { 01459 ffn_[ffn_[current_index_]] = neighbor_update; 01460 } 01461 else if (sfn_[ffn_[current_index_]] == current_index_) 01462 { 01463 sfn_[ffn_[current_index_]] = neighbor_update; 01464 } 01465 01466 /* updating current */ 01467 ffn_[current_index_] = prev_index; 01468 01469 break; 01470 } 01471 case 3://next2s: 01472 { 01473 addTriangle (current_index_, sfn_[current_index_], next_index, output); 01474 int neighbor_update = next_index; 01475 01476 /* updating next_index */ 01477 if (state_[next_index] <= FREE) 01478 { 01479 state_[next_index] = FRINGE; 01480 ffn_[next_index] = current_index_; 01481 sfn_[next_index] = sfn_[current_index_]; 01482 } 01483 else 01484 { 01485 if (ffn_[next_index] == R_) 01486 { 01487 changed_2nd_fn_ = true; 01488 ffn_[next_index] = sfn_[current_index_]; 01489 } 01490 else if (sfn_[next_index] == R_) 01491 { 01492 changed_2nd_fn_ = true; 01493 sfn_[next_index] = sfn_[current_index_]; 01494 } 01495 else if (next_index == R_) 01496 { 01497 new2boundary_ = sfn_[current_index_]; 01498 if (next_next_index == new2boundary_) 01499 already_connected_ = true; 01500 } 01501 else if (ffn_[next_index] == next_next_index) 01502 { 01503 already_connected_ = true; 01504 ffn_[next_index] = sfn_[current_index_]; 01505 } 01506 else if (sfn_[next_index] == next_next_index) 01507 { 01508 already_connected_ = true; 01509 sfn_[next_index] = sfn_[current_index_]; 01510 } 01511 else 01512 { 01513 tmp_ = coords_[ffn_[next_index]] - proj_qp_; 01514 uvn_next_ffn_[0] = tmp_.dot(u_); 01515 uvn_next_ffn_[1] = tmp_.dot(v_); 01516 tmp_ = coords_[sfn_[next_index]] - proj_qp_; 01517 uvn_next_sfn_[0] = tmp_.dot(u_); 01518 uvn_next_sfn_[1] = tmp_.dot(v_); 01519 01520 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_); 01521 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_); 01522 01523 int connect2sfn = -1; 01524 if (ffn_next_sfn && sfn_next_sfn) 01525 { 01526 double fn2s = (coords_[sfn_[current_index_]] - coords_[ffn_[next_index]]).squaredNorm (); 01527 double sn2s = (coords_[sfn_[current_index_]] - coords_[sfn_[next_index]]).squaredNorm (); 01528 if (fn2s < sn2s) connect2sfn = 0; 01529 else connect2sfn = 1; 01530 } 01531 else if (ffn_next_sfn) connect2sfn = 0; 01532 else if (sfn_next_sfn) connect2sfn = 1; 01533 01534 switch (connect2sfn) 01535 { 01536 case 0: // ffn[next] 01537 { 01538 addTriangle (next_index, sfn_[current_index_], ffn_[next_index], output); 01539 neighbor_update = ffn_[next_index]; 01540 01541 /* ffn[next_index] */ 01542 if ((ffn_[ffn_[next_index]] == sfn_[current_index_]) || (sfn_[ffn_[next_index]] == sfn_[current_index_])) 01543 { 01544 state_[ffn_[next_index]] = COMPLETED; 01545 } 01546 else if (ffn_[ffn_[next_index]] == next_index) 01547 { 01548 ffn_[ffn_[next_index]] = sfn_[current_index_]; 01549 } 01550 else if (sfn_[ffn_[next_index]] == next_index) 01551 { 01552 sfn_[ffn_[next_index]] = sfn_[current_index_]; 01553 } 01554 01555 ffn_[next_index] = current_index_; 01556 01557 break; 01558 } 01559 case 1: // sfn[next] 01560 { 01561 addTriangle (next_index, sfn_[current_index_], sfn_[next_index], output); 01562 neighbor_update = sfn_[next_index]; 01563 01564 /* sfn[next_index] */ 01565 if ((ffn_[sfn_[next_index]] == sfn_[current_index_]) || (sfn_[sfn_[next_index]] == sfn_[current_index_])) 01566 { 01567 state_[sfn_[next_index]] = COMPLETED; 01568 } 01569 else if (ffn_[sfn_[next_index]] == next_index) 01570 { 01571 ffn_[sfn_[next_index]] = sfn_[current_index_]; 01572 } 01573 else if (sfn_[sfn_[next_index]] == next_index) 01574 { 01575 sfn_[sfn_[next_index]] = sfn_[current_index_]; 01576 } 01577 01578 sfn_[next_index] = current_index_; 01579 01580 break; 01581 } 01582 default:; 01583 } 01584 } 01585 } 01586 01587 /* updating sfn */ 01588 if ((ffn_[sfn_[current_index_]] == neighbor_update) || (sfn_[sfn_[current_index_]] == neighbor_update)) 01589 { 01590 state_[sfn_[current_index_]] = COMPLETED; 01591 } 01592 else if (ffn_[sfn_[current_index_]] == current_index_) 01593 { 01594 ffn_[sfn_[current_index_]] = neighbor_update; 01595 } 01596 else if (sfn_[sfn_[current_index_]] == current_index_) 01597 { 01598 sfn_[sfn_[current_index_]] = neighbor_update; 01599 } 01600 01601 sfn_[current_index_] = prev_index; 01602 01603 break; 01604 } 01605 default:; 01606 } 01607 } 01608 } 01609 } 01610 } 01611 01613 template <typename PointInT> std::vector<std::vector<size_t> > 01614 pcl::GreedyProjectionTriangulation<PointInT>::getTriangleList (pcl::PolygonMesh input) 01615 { 01616 std::vector< std::vector<size_t> > triangleList; 01617 for (size_t i=0; i < input.cloud.width * input.cloud.height; ++i) 01618 { 01619 std::vector<size_t> temp; 01620 triangleList.push_back(temp); 01621 } 01622 01623 for (size_t i=0; i < input.polygons.size(); ++i) 01624 for (size_t j=0; j < input.polygons[i].vertices.size(); ++j) 01625 triangleList[j].push_back(i); 01626 return triangleList; 01627 } 01628 01630 template <typename PointInT> void 01631 pcl::GreedyProjectionTriangulation<PointInT>::removeOverlapTriangles ( 01632 pcl::PolygonMesh &mesh1, 01633 pcl::PolygonMesh &mesh2) 01634 { 01635 size_t point_size1 = mesh1.cloud.width * mesh1.cloud.height; 01636 01637 // create new cloud 01638 pcl::PointCloud<PointInT> newcloud; 01639 pcl::PointCloud<PointInT> cloud2; 01640 01641 pcl::fromROSMsg(mesh1.cloud, newcloud); 01642 pcl::fromROSMsg(mesh2.cloud, cloud2); 01643 newcloud += cloud2; 01644 01645 std::vector<std::vector<size_t> > triangleList1 = getTriangleList(mesh1); 01646 std::vector<std::vector<size_t> > triangleList2 = getTriangleList(mesh1); 01647 01648 // Variables to hold the results of nearest neighbor searches 01649 std::vector<int> nnIdx (1); 01650 std::vector<float> sqrDists (1); 01651 01652 // for searching 01653 KdTreeFLANN<SearchPoint> kdtree; 01654 01655 pcl::PointCloud<SearchPoint>::Ptr mycloud (new pcl::PointCloud<SearchPoint> ()); 01656 01657 Eigen::Vector3f tmp; 01658 for(size_t i=0; i< newcloud.points.size (); ++i) 01659 { 01660 tmp = newcloud.points[i].getVector3fMap(); 01661 mycloud->points.push_back (SearchPoint (tmp(0), tmp(1), tmp(2))); 01662 } 01663 01664 kdtree.setInputCloud (mycloud); 01665 01666 Eigen::Vector3f center(0,0,0); 01667 01668 // verties of triangle 01669 int idx[3]; 01670 01671 for (size_t i=0; i < mesh1.polygons.size(); ++i) 01672 { 01673 for (size_t j=0; j < mesh1.polygons[i].vertices.size(); ++j) 01674 { 01675 idx[j] = mesh1.polygons[i].vertices[j]; 01676 center = center + input_->points[idx[j]].getVector3fMap(); 01677 } 01678 center = center/3; 01679 SearchPoint center_point(center(0), center(1), center(2)); 01680 01681 kdtree.nearestKSearch (center_point, 1, nnIdx, sqrDists); 01682 01683 // remove the triangle if we can 01684 if (nnIdx[0] >= (int)point_size1 && triangleList2[nnIdx[0]].size() > 0) 01685 { // remove triangle 01686 for (int j = 0; j < 3; ++j) 01687 { 01688 // set vertex 1 to FREE if the triangle to be deleted was the only triangle it was in 01689 if (triangleList1[idx[j]].size() == 1) 01690 { 01691 state_[idx[j]] = FREE; 01692 sfn_[idx[j]] = -1; 01693 ffn_[idx[j]] = -1; 01694 01695 for (int k = 0; k < 3; ++k) 01696 { 01697 // update sfn and ffn of other 2 vertices 01698 if (k != j) 01699 { 01700 if (sfn_[idx[k]] == idx[j]) 01701 sfn_[idx[k]] = idx[3- k-j]; 01702 if (ffn_[idx[k]] == idx[j]) 01703 ffn_[idx[k]] = idx[3- k-j]; 01704 } 01705 }// end for k 01706 } 01707 else 01708 { // set to be FRINGE 01709 state_[idx[j]] = FRINGE; 01710 // scanning other triangle that has this vertex 01711 size_t both_share_2triangles = 0; 01712 size_t last_k = 0; 01713 for (int k = 0; k < 3; ++k) 01714 { 01715 size_t share_2triangles = 0; 01716 if (k == j) 01717 continue; 01718 01719 for (size_t p = 0; p < triangleList1[idx[j]].size(); ++p) 01720 for (size_t q =0; q < triangleList1[idx[k]].size(); ++q) 01721 if (triangleList1[idx[j]][p] == triangleList1[idx[j]][q]) 01722 share_2triangles++; 01723 01724 // if 2 vertex share 2 triangles 01725 if(share_2triangles == 2) 01726 { 01727 sfn_[idx[j]] = idx[k]; 01728 ffn_[idx[j]] = idx[k]; 01729 both_share_2triangles++; 01730 last_k = k; 01731 } 01732 } 01733 // if both share 2 triangles 01734 if (both_share_2triangles == 2) 01735 { 01736 // change ffn or sfn 01737 ffn_[idx[j]] = idx[3-j-last_k]; 01738 } 01739 } 01740 01741 }// end for j 01742 }// end if 01743 }// end fo i 01744 01745 } 01746 01748 template <typename PointInT> void 01749 pcl::GreedyProjectionTriangulation<PointInT>::merge2Meshes ( 01750 pcl::PolygonMesh &output, pcl::PolygonMesh &mesh2, 01751 std::vector<int> state2, std::vector<int> sfn2, 01752 std::vector<int> ffn2) 01753 { 01754 // store old information 01755 size_t point_size1 = input_->points.size (); 01756 01757 // create new cloud 01758 pcl::PointCloud<PointInT> newcloud; 01759 pcl::PointCloud<PointInT> cloud2; 01760 newcloud = *input_; 01761 01762 pcl::fromROSMsg(mesh2.cloud, cloud2); 01763 newcloud += cloud2; 01764 01765 // update cloud 01766 input_ = PointCloudInConstPtr (new pcl::PointCloud<PointInT>(newcloud)); 01767 01768 // change header 01769 output.header = input_->header; 01770 01771 // change cloud of output 01772 pcl::toROSMsg (*input_, output.cloud); 01773 indices_->resize (input_->points.size ()); 01774 01775 // update indices 01776 for (size_t i = point_size1; i < indices_->size (); ++i) 01777 (*indices_)[i] = i; 01778 01779 // update tree 01780 tree_->setInputCloud (input_, indices_); 01781 01782 // initializing states and fringe neighbors 01783 01784 part_.resize(indices_->size ()); // indices of point's part 01785 for (size_t i = point_size1; i < indices_->size (); ++i) part_[i] = -1; 01786 01787 state_.resize(indices_->size ()); 01788 for (size_t i = point_size1; i < indices_->size (); ++i) state_[i] = state2[indices_->size () - point_size1]; 01789 01790 source_.resize(indices_->size ()); 01791 for (size_t i = point_size1; i < indices_->size (); ++i) source_[i] = NONE; 01792 01793 ffn_.resize(indices_->size ()); 01794 for (size_t i = point_size1; i < indices_->size (); ++i) ffn_[i] = ffn2[indices_->size () - point_size1]; 01795 01796 sfn_.resize(indices_->size (), NONE); 01797 for (size_t i = point_size1; i < indices_->size (); ++i) sfn_[i] = sfn2[indices_->size () - point_size1]; 01798 01799 // merge and update 2 meshes 01800 // for (size_t i = 0; i < indices_->size (); ++i) { 01801 // if(state_[i] == BOUNDARY) state_[i] = FRINGE; 01802 // } 01803 01804 // restart triangulating 01805 const double sqr_mu = mu_*mu_; 01806 const double sqr_max_edge = search_radius_*search_radius_; 01807 if (nnn_ > (int)indices_->size ()) 01808 nnn_ = indices_->size (); 01809 01810 // Variables to hold the results of nearest neighbor searches 01811 std::vector<int> nnIdx (nnn_); 01812 std::vector<float> sqrDists (nnn_); 01813 01814 // current number of connected components 01815 int part_index = 0; // need to consider 01816 01817 // 2D coordinates of points 01818 const Eigen::Vector2f uvn_nn_qp_zero = Eigen::Vector2f::Zero(); 01819 Eigen::Vector2f uvn_current; 01820 Eigen::Vector2f uvn_prev; 01821 Eigen::Vector2f uvn_next; 01822 01823 // initializing fields 01824 already_connected_ = false; // see declaration for comments :P 01825 01826 // initializing states and fringe neighbors 01827 01828 fringe_queue_.clear (); 01829 // need to be considered 01830 int fqIdx = 0; // current fringe's index in the queue to be processed 01831 01832 // Saving coordinates 01833 coords_.reserve (indices_->size ()); 01834 for (size_t cp = point_size1; cp < indices_->size (); ++cp) 01835 { 01836 coords_.push_back(input_->points[(*indices_)[cp]].getVector3fMap()); 01837 } 01838 01839 // Initializing 01840 int is_free = 0; 01841 int nr_parts=0, increase_nnn4fn=0, increase_nnn4s=0, increase_dist=0, nr_touched = 0; 01842 bool is_fringe; 01843 angles_.resize(nnn_); 01844 std::vector<Eigen::Vector2f> uvn_nn (nnn_); 01845 Eigen::Vector2f uvn_s; 01846 01847 // iterating through fringe points and finishing them until everything is done 01848 while (is_free != NONE) 01849 { 01850 R_ = is_free; 01851 if (state_[R_] == FREE) 01852 { 01853 state_[R_] = NONE; 01854 part_[R_] = part_index++; 01855 01856 // creating starting triangle 01857 //searchForNeighbors ((*indices_)[R_], nnIdx, sqrDists); 01858 tree_->nearestKSearch ((*indices_)[R_], nnn_, nnIdx, sqrDists); 01859 double sqr_dist_threshold = (std::min)(sqr_max_edge, sqr_mu * sqrDists[1]); 01860 01861 // Get the normal estimate at the current point 01862 const Eigen::Vector3f nc = input_->points[(*indices_)[R_]].getNormalVector3fMap (); 01863 01864 // Get a coordinate system that lies on a plane defined by its normal 01865 v_ = nc.unitOrthogonal (); 01866 u_ = nc.cross (v_); 01867 01868 // Projecting point onto the surface 01869 double dist = nc.dot(coords_[R_]); 01870 proj_qp_ = coords_[R_] - dist * nc; 01871 01872 // Converting coords, calculating angles and saving the projected near boundary edges 01873 int nr_edge = 0; 01874 std::vector<doubleEdge> doubleEdges; 01875 for (int i = 1; i < nnn_; i++) // nearest neighbor with index 0 is the query point R_ itself 01876 { 01877 // Transforming coordinates 01878 tmp_ = coords_[nnIdx[i]] - proj_qp_; 01879 uvn_nn[i][0] = tmp_.dot(u_); 01880 uvn_nn[i][1] = tmp_.dot(v_); 01881 // Computing the angle between each neighboring point and the query point itself 01882 angles_[i].angle = atan2(uvn_nn[i][1], uvn_nn[i][0]); 01883 // initializing angle descriptors 01884 angles_[i].index = nnIdx[i]; 01885 01886 if ( 01887 (state_[nnIdx[i]] == COMPLETED) || (state_[nnIdx[i]] == BOUNDARY) 01888 || (sqrDists[i] > sqr_dist_threshold) 01889 ) 01890 angles_[i].visible = false; 01891 else 01892 angles_[i].visible = true; 01893 // Saving the edges between nearby boundary points 01894 if ((state_[nnIdx[i]] == FRINGE) || (state_[nnIdx[i]] == BOUNDARY)) 01895 { 01896 doubleEdge e; 01897 e.index = i; 01898 nr_edge++; 01899 tmp_ = coords_[ffn_[nnIdx[i]]] - proj_qp_; 01900 e.first[0] = tmp_.dot(u_); 01901 e.first[1] = tmp_.dot(v_); 01902 tmp_ = coords_[sfn_[nnIdx[i]]] - proj_qp_; 01903 e.second[0] = tmp_.dot(u_); 01904 e.second[1] = tmp_.dot(v_); 01905 doubleEdges.push_back(e); 01906 } 01907 } 01908 angles_[0].visible = false; 01909 01910 // Verify the visibility of each potential new vertex 01911 for (int i = 1; i < nnn_; i++) // nearest neighbor with index 0 is the query point R_ itself 01912 if ((angles_[i].visible) && (ffn_[R_] != nnIdx[i]) && (sfn_[R_] != nnIdx[i])) 01913 { 01914 bool visibility = true; 01915 for (int j = 0; j < nr_edge; j++) 01916 { 01917 if (ffn_[nnIdx[doubleEdges[j].index]] != nnIdx[i]) 01918 visibility = isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].first, Eigen::Vector2f::Zero()); 01919 if (!visibility) 01920 break; 01921 if (sfn_[nnIdx[doubleEdges[j].index]] != nnIdx[i]) 01922 visibility = isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].second, Eigen::Vector2f::Zero()); 01923 if (!visibility == false) 01924 break; 01925 } 01926 angles_[i].visible = visibility; 01927 } 01928 01929 // Selecting first two visible free neighbors 01930 bool not_found = true; 01931 int left = 1; 01932 do 01933 { 01934 while ((left < nnn_) && ((!angles_[left].visible) || (state_[nnIdx[left]] > FREE))) left++; 01935 if (left >= nnn_) 01936 break; 01937 else 01938 { 01939 int right = left+1; 01940 do 01941 { 01942 while ((right < nnn_) && ((!angles_[right].visible) || (state_[nnIdx[right]] > FREE))) right++; 01943 if (right >= nnn_) 01944 break; 01945 else if ((coords_[nnIdx[left]] - coords_[nnIdx[right]]).squaredNorm () > sqr_max_edge) 01946 right++; 01947 else 01948 { 01949 addFringePoint (nnIdx[right], R_); 01950 addFringePoint (nnIdx[left], nnIdx[right]); 01951 addFringePoint (R_, nnIdx[left]); 01952 state_[R_] = state_[nnIdx[left]] = state_[nnIdx[right]] = FRINGE; 01953 ffn_[R_] = nnIdx[left]; 01954 sfn_[R_] = nnIdx[right]; 01955 ffn_[nnIdx[left]] = nnIdx[right]; 01956 sfn_[nnIdx[left]] = R_; 01957 ffn_[nnIdx[right]] = R_; 01958 sfn_[nnIdx[right]] = nnIdx[left]; 01959 addTriangle (R_, nnIdx[left], nnIdx[right], output); 01960 nr_parts++; 01961 not_found = false; 01962 break; 01963 } 01964 } 01965 while (true); 01966 left++; 01967 } 01968 } 01969 while (not_found); 01970 } 01971 01972 is_free = NONE; 01973 for (unsigned temp = 0; temp < indices_->size (); temp++) 01974 { 01975 if (state_[temp] == FREE) 01976 { 01977 is_free = temp; 01978 break; 01979 } 01980 } 01981 01982 is_fringe = true; 01983 while (is_fringe) 01984 { 01985 is_fringe = false; 01986 01987 int fqSize = fringe_queue_.size(); 01988 while ((fqIdx < fqSize) && (state_[fringe_queue_[fqIdx]] != FRINGE)) 01989 fqIdx++; 01990 01991 // an unfinished fringe point is found 01992 if (fqIdx >= fqSize) 01993 { 01994 continue; 01995 } 01996 01997 R_ = fringe_queue_[fqIdx]; 01998 is_fringe = true; 01999 02000 if (ffn_[R_] == sfn_[R_]) 02001 { 02002 state_[R_] = COMPLETED; 02003 continue; 02004 } 02005 //searchForNeighbors ((*indices_)[R_], nnIdx, sqrDists); 02006 tree_->nearestKSearch ((*indices_)[R_], nnn_, nnIdx, sqrDists); 02007 02008 // Locating FFN and SFN to adapt distance threshold 02009 double sqr_source_dist = (coords_[R_] - coords_[source_[R_]]).squaredNorm (); 02010 double sqr_ffn_dist = (coords_[R_] - coords_[ffn_[R_]]).squaredNorm (); 02011 double sqr_sfn_dist = (coords_[R_] - coords_[sfn_[R_]]).squaredNorm (); 02012 double max_sqr_fn_dist = (std::max)(sqr_ffn_dist, sqr_sfn_dist); 02013 double sqr_dist_threshold = (std::min)(sqr_max_edge, sqr_mu * sqrDists[1]); //sqr_mu * sqr_avg_conn_dist); 02014 if (max_sqr_fn_dist > sqrDists[nnn_-1]) 02015 { 02016 if (0 == increase_nnn4fn) 02017 PCL_WARN("Not enough neighbors are considered: ffn or sfn out of range! Consider increasing nnn_... Setting R=%d to be BOUNDARY!\n", R_); 02018 increase_nnn4fn++; 02019 state_[R_] = BOUNDARY; 02020 continue; 02021 } 02022 double max_sqr_fns_dist = (std::max)(sqr_source_dist, max_sqr_fn_dist); 02023 if (max_sqr_fns_dist > sqrDists[nnn_-1]) 02024 { 02025 if (0 == increase_nnn4s) 02026 PCL_WARN("Not enough neighbors are considered: source of R=%d is out of range! Consider increasing nnn_...\n", R_); 02027 increase_nnn4s++; 02028 } 02029 02030 // Get the normal estimate at the current point 02031 const Eigen::Vector3f nc = input_->points[(*indices_)[R_]].getNormalVector3fMap (); 02032 02033 // Get a coordinate system that lies on a plane defined by its normal 02034 v_ = nc.unitOrthogonal (); 02035 u_ = nc.cross (v_); 02036 02037 // Projecting point onto the surface 02038 double dist = nc.dot(coords_[R_]); 02039 proj_qp_ = coords_[R_] - dist * nc; 02040 02041 // Converting coords, calculating angles and saving the projected near boundary edges 02042 int nr_edge = 0; 02043 std::vector<doubleEdge> doubleEdges; 02044 for (int i = 1; i < nnn_; i++) // nearest neighbor with index 0 is the query point R_ itself 02045 { 02046 tmp_ = coords_[nnIdx[i]] - proj_qp_; 02047 uvn_nn[i][0] = tmp_.dot(u_); 02048 uvn_nn[i][1] = tmp_.dot(v_); 02049 02050 // Computing the angle between each neighboring point and the query point itself 02051 angles_[i].angle = atan2(uvn_nn[i][1], uvn_nn[i][0]); 02052 // initializing angle descriptors 02053 angles_[i].index = nnIdx[i]; 02054 angles_[i].nnIndex = i; 02055 if ( 02056 (state_[nnIdx[i]] == COMPLETED) || (state_[nnIdx[i]] == BOUNDARY) 02057 || (sqrDists[i] > sqr_dist_threshold) 02058 ) 02059 angles_[i].visible = false; 02060 else 02061 angles_[i].visible = true; 02062 if ((ffn_[R_] == nnIdx[i]) || (sfn_[R_] == nnIdx[i])) 02063 angles_[i].visible = true; 02064 bool same_side = true; 02065 const Eigen::Vector3f neighbor_normal = input_->points[(*indices_)[nnIdx[i]]].getNormalVector3fMap (); 02066 double cosine = nc.dot (neighbor_normal); 02067 if (cosine > 1) cosine = 1; 02068 if (cosine < -1) cosine = -1; 02069 double angle = acos (cosine); 02070 if ((!consistent_) && (angle > M_PI/2)) 02071 angle = M_PI - angle; 02072 if (angle > eps_angle_) 02073 { 02074 angles_[i].visible = false; 02075 same_side = false; 02076 } 02077 // Saving the edges between nearby boundary points 02078 if ((i!=0) && (same_side) && ((state_[nnIdx[i]] == FRINGE) || (state_[nnIdx[i]] == BOUNDARY))) 02079 { 02080 doubleEdge e; 02081 e.index = i; 02082 nr_edge++; 02083 tmp_ = coords_[ffn_[nnIdx[i]]] - proj_qp_; 02084 e.first[0] = tmp_.dot(u_); 02085 e.first[1] = tmp_.dot(v_); 02086 tmp_ = coords_[sfn_[nnIdx[i]]] - proj_qp_; 02087 e.second[0] = tmp_.dot(u_); 02088 e.second[1] = tmp_.dot(v_); 02089 doubleEdges.push_back(e); 02090 // Pruning by visibility criterion 02091 if ((state_[nnIdx[i]] == FRINGE) && (ffn_[R_] != nnIdx[i]) && (sfn_[R_] != nnIdx[i])) 02092 { 02093 double angle1 = atan2(e.first[1] - uvn_nn[i][1], e.first[0] - uvn_nn[i][0]); 02094 double angle2 = atan2(e.second[1] - uvn_nn[i][1], e.second[0] - uvn_nn[i][0]); 02095 double angleMin, angleMax; 02096 if (angle1 < angle2) 02097 { 02098 angleMin = angle1; 02099 angleMax = angle2; 02100 } 02101 else 02102 { 02103 angleMin = angle2; 02104 angleMax = angle1; 02105 } 02106 double angleR = angles_[i].angle + M_PI; 02107 if (angleR >= 2*M_PI) 02108 angleR -= 2*M_PI; 02109 if ((source_[nnIdx[i]] == ffn_[nnIdx[i]]) || (source_[nnIdx[i]] == sfn_[nnIdx[i]])) 02110 { 02111 if ((angleMax - angleMin) < M_PI) 02112 { 02113 if ((angleMin < angleR) && (angleR < angleMax)) 02114 angles_[i].visible = false; 02115 } 02116 else 02117 { 02118 if ((angleR < angleMin) || (angleMax < angleR)) 02119 angles_[i].visible = false; 02120 } 02121 } 02122 else 02123 { 02124 tmp_ = coords_[source_[nnIdx[i]]] - proj_qp_; 02125 uvn_s[0] = tmp_.dot(u_); 02126 uvn_s[1] = tmp_.dot(v_); 02127 double angleS = atan2(uvn_s[1] - uvn_nn[i][1], uvn_s[0] - uvn_nn[i][0]); 02128 if ((angleMin < angleS) && (angleS < angleMax)) 02129 { 02130 if ((angleMin < angleR) && (angleR < angleMax)) 02131 angles_[i].visible = false; 02132 } 02133 else 02134 { 02135 if ((angleR < angleMin) || (angleMax < angleR)) 02136 angles_[i].visible = false; 02137 } 02138 } 02139 } 02140 } 02141 } 02142 angles_[0].visible = false; 02143 02144 // Verify the visibility of each potential new vertex 02145 for (int i = 1; i < nnn_; i++) // nearest neighbor with index 0 is the query point R_ itself 02146 if ((angles_[i].visible) && (ffn_[R_] != nnIdx[i]) && (sfn_[R_] != nnIdx[i])) 02147 { 02148 bool visibility = true; 02149 for (int j = 0; j < nr_edge; j++) 02150 { 02151 if (doubleEdges[j].index != i) 02152 { 02153 int f = ffn_[nnIdx[doubleEdges[j].index]]; 02154 if ((f != nnIdx[i]) && (f != R_)) 02155 visibility = isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].first, Eigen::Vector2f::Zero()); 02156 if (visibility == false) 02157 break; 02158 02159 int s = sfn_[nnIdx[doubleEdges[j].index]]; 02160 if ((s != nnIdx[i]) && (s != R_)) 02161 visibility = isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].second, Eigen::Vector2f::Zero()); 02162 if (visibility == false) 02163 break; 02164 } 02165 } 02166 angles_[i].visible = visibility; 02167 } 02168 02169 // Sorting angles 02170 std::sort (angles_.begin (), angles_.end (), GreedyProjectionTriangulation<PointInT>::nnAngleSortAsc); 02171 02172 // Triangulating 02173 if (angles_[2].visible == false) 02174 { 02175 if ( !( (angles_[0].index == ffn_[R_] && angles_[1].index == sfn_[R_]) || (angles_[0].index == sfn_[R_] && angles_[1].index == ffn_[R_]) ) ) 02176 { 02177 state_[R_] = BOUNDARY; 02178 } 02179 else 02180 { 02181 if ((source_[R_] == angles_[0].index) || (source_[R_] == angles_[1].index)) 02182 state_[R_] = BOUNDARY; 02183 else 02184 { 02185 if (sqr_max_edge < (coords_[ffn_[R_]] - coords_[sfn_[R_]]).squaredNorm ()) 02186 { 02187 state_[R_] = BOUNDARY; 02188 } 02189 else 02190 { 02191 tmp_ = coords_[source_[R_]] - proj_qp_; 02192 uvn_s[0] = tmp_.dot(u_); 02193 uvn_s[1] = tmp_.dot(v_); 02194 double angleS = atan2(uvn_s[1], uvn_s[0]); 02195 double dif = angles_[1].angle - angles_[0].angle; 02196 if ((angles_[0].angle < angleS) && (angleS < angles_[1].angle)) 02197 { 02198 if (dif < 2*M_PI - maximum_angle_) 02199 state_[R_] = BOUNDARY; 02200 else 02201 closeTriangle (output); 02202 } 02203 else 02204 { 02205 if (dif >= maximum_angle_) 02206 state_[R_] = BOUNDARY; 02207 else 02208 closeTriangle (output); 02209 } 02210 } 02211 } 02212 } 02213 continue; 02214 } 02215 02216 // Finding the FFN and SFN 02217 int start = -1, end = -1; 02218 for (int i=0; i<nnn_; i++) 02219 { 02220 if (ffn_[R_] == angles_[i].index) 02221 { 02222 start = i; 02223 if (sfn_[R_] == angles_[i+1].index) 02224 end = i+1; 02225 else 02226 if (i==0) 02227 { 02228 for (i = i+2; i < nnn_; i++) 02229 if (sfn_[R_] == angles_[i].index) 02230 break; 02231 end = i; 02232 } 02233 else 02234 { 02235 for (i = i+2; i < nnn_; i++) 02236 if (sfn_[R_] == angles_[i].index) 02237 break; 02238 end = i; 02239 } 02240 break; 02241 } 02242 if (sfn_[R_] == angles_[i].index) 02243 { 02244 start = i; 02245 if (ffn_[R_] == angles_[i+1].index) 02246 end = i+1; 02247 else 02248 if (i==0) 02249 { 02250 for (i = i+2; i < nnn_; i++) 02251 if (ffn_[R_] == angles_[i].index) 02252 break; 02253 end = i; 02254 } 02255 else 02256 { 02257 for (i = i+2; i < nnn_; i++) 02258 if (ffn_[R_] == angles_[i].index) 02259 break; 02260 end = i; 02261 } 02262 break; 02263 } 02264 } 02265 02266 // start and end are always set, as we checked if ffn or sfn are out of range before, but let's check anyways if < 0 02267 if ((start < 0) || (end < 0) || (end == nnn_) || (!angles_[start].visible) || (!angles_[end].visible)) 02268 { 02269 state_[R_] = BOUNDARY; 02270 continue; 02271 } 02272 02273 // Finding last visible nn 02274 int last_visible = end; 02275 while ((last_visible+1<nnn_) && (angles_[last_visible+1].visible)) last_visible++; 02276 02277 // Finding visibility region of R 02278 bool need_invert = false; 02279 int sourceIdx = nnn_; 02280 if ((source_[R_] == ffn_[R_]) || (source_[R_] == sfn_[R_])) 02281 { 02282 if ((angles_[end].angle - angles_[start].angle) < M_PI) 02283 need_invert = true; 02284 } 02285 else 02286 { 02287 for (sourceIdx=0; sourceIdx<nnn_; sourceIdx++) 02288 if (angles_[sourceIdx].index == source_[R_]) 02289 break; 02290 if (sourceIdx == nnn_) 02291 { 02292 int vis_free = NONE, nnCB = NONE; // any free visible and nearest completed or boundary neighbor of R 02293 for (int i = 1; i < nnn_; i++) // nearest neighbor with index 0 is the query point R_ itself 02294 { 02295 // NOTE: nnCB is an index in nnIdx 02296 if ((state_[nnIdx[i]] == COMPLETED) || (state_[nnIdx[i]] == BOUNDARY)) 02297 { 02298 if (nnCB == NONE) 02299 { 02300 nnCB = i; 02301 if (vis_free != NONE) 02302 break; 02303 } 02304 } 02305 // NOTE: vis_free is an index in angles 02306 if (state_[angles_[i].index] <= FREE) 02307 { 02308 if (i <= last_visible) 02309 { 02310 vis_free = i; 02311 if (nnCB != NONE) 02312 break; 02313 } 02314 } 02315 } 02316 // NOTE: nCB is an index in angles 02317 int nCB = 0; 02318 if (nnCB != NONE) 02319 while (angles_[nCB].index != nnIdx[nnCB]) nCB++; 02320 else 02321 nCB = NONE; 02322 02323 if (vis_free != NONE) 02324 { 02325 if ((vis_free < start) || (vis_free > end)) 02326 need_invert = true; 02327 } 02328 else 02329 { 02330 if (nCB != NONE) 02331 { 02332 if ((nCB == start) || (nCB == end)) 02333 { 02334 bool inside_CB = false; 02335 bool outside_CB = false; 02336 for (int i=0; i<nnn_; i++) 02337 { 02338 if ( 02339 ((state_[angles_[i].index] == COMPLETED) || (state_[angles_[i].index] == BOUNDARY)) 02340 && (i != start) && (i != end) 02341 ) 02342 { 02343 if ((angles_[start].angle <= angles_[i].angle) && (angles_[i].angle <= angles_[end].angle)) 02344 { 02345 inside_CB = true; 02346 if (outside_CB) 02347 break; 02348 } 02349 else 02350 { 02351 outside_CB = true; 02352 if (inside_CB) 02353 break; 02354 } 02355 } 02356 } 02357 if (inside_CB && !outside_CB) 02358 need_invert = true; 02359 else if (!(!inside_CB && outside_CB)) 02360 { 02361 if ((angles_[end].angle - angles_[start].angle) < M_PI) 02362 need_invert = true; 02363 } 02364 } 02365 else 02366 { 02367 if ((angles_[nCB].angle > angles_[start].angle) && (angles_[nCB].angle < angles_[end].angle)) 02368 need_invert = true; 02369 } 02370 } 02371 else 02372 { 02373 if (start == end-1) 02374 need_invert = true; 02375 } 02376 } 02377 } 02378 else if ((angles_[start].angle < angles_[sourceIdx].angle) && (angles_[sourceIdx].angle < angles_[end].angle)) 02379 need_invert = true; 02380 } 02381 02382 // switching start and end if necessary 02383 if (need_invert) 02384 { 02385 int tmp = start; 02386 start = end; 02387 end = tmp; 02388 } 02389 02390 // Arranging visible nnAngles in the order they need to be connected and 02391 // compute the maximal angle difference between two consecutive visible angles 02392 bool is_boundary = false, is_skinny = false; 02393 std::vector<bool> gaps (nnn_, false); 02394 std::vector<bool> skinny (nnn_, false); 02395 std::vector<double> dif (nnn_); 02396 std::vector<int> angleIdx; angleIdx.reserve (nnn_); 02397 if (start > end) 02398 { 02399 for (int j=start; j<last_visible; j++) 02400 { 02401 dif[j] = (angles_[j+1].angle - angles_[j].angle); 02402 if (dif[j] < minimum_angle_) 02403 { 02404 skinny[j] = is_skinny = true; 02405 } 02406 else if (maximum_angle_ <= dif[j]) 02407 { 02408 gaps[j] = is_boundary = true; 02409 } 02410 if ((!gaps[j]) && (sqr_max_edge < (coords_[angles_[j+1].index] - coords_[angles_[j].index]).squaredNorm ())) 02411 { 02412 gaps[j] = is_boundary = true; 02413 } 02414 angleIdx.push_back(j); 02415 } 02416 02417 dif[last_visible] = (2*M_PI + angles_[0].angle - angles_[last_visible].angle); 02418 if (dif[last_visible] < minimum_angle_) 02419 { 02420 skinny[last_visible] = is_skinny = true; 02421 } 02422 else if (maximum_angle_ <= dif[last_visible]) 02423 { 02424 gaps[last_visible] = is_boundary = true; 02425 } 02426 if ((!gaps[last_visible]) && (sqr_max_edge < (coords_[angles_[0].index] - coords_[angles_[last_visible].index]).squaredNorm ())) 02427 { 02428 gaps[last_visible] = is_boundary = true; 02429 } 02430 angleIdx.push_back(last_visible); 02431 02432 for (int j=0; j<end; j++) 02433 { 02434 dif[j] = (angles_[j+1].angle - angles_[j].angle); 02435 if (dif[j] < minimum_angle_) 02436 { 02437 skinny[j] = is_skinny = true; 02438 } 02439 else if (maximum_angle_ <= dif[j]) 02440 { 02441 gaps[j] = is_boundary = true; 02442 } 02443 if ((!gaps[j]) && (sqr_max_edge < (coords_[angles_[j+1].index] - coords_[angles_[j].index]).squaredNorm ())) 02444 { 02445 gaps[j] = is_boundary = true; 02446 } 02447 angleIdx.push_back(j); 02448 } 02449 angleIdx.push_back(end); 02450 } 02451 // start < end 02452 else 02453 { 02454 for (int j=start; j<end; j++) 02455 { 02456 dif[j] = (angles_[j+1].angle - angles_[j].angle); 02457 if (dif[j] < minimum_angle_) 02458 { 02459 skinny[j] = is_skinny = true; 02460 } 02461 else if (maximum_angle_ <= dif[j]) 02462 { 02463 gaps[j] = is_boundary = true; 02464 } 02465 if ((!gaps[j]) && (sqr_max_edge < (coords_[angles_[j+1].index] - coords_[angles_[j].index]).squaredNorm ())) 02466 { 02467 gaps[j] = is_boundary = true; 02468 } 02469 angleIdx.push_back(j); 02470 } 02471 angleIdx.push_back(end); 02472 } 02473 02474 // Set the state of the point 02475 state_[R_] = is_boundary ? BOUNDARY : COMPLETED; 02476 02477 std::vector<int>::iterator first_gap_after = angleIdx.end (); 02478 std::vector<int>::iterator last_gap_before = angleIdx.begin (); 02479 int nr_gaps = 0; 02480 for (std::vector<int>::iterator it = angleIdx.begin (); it != angleIdx.end () - 1; it++) 02481 { 02482 if (gaps[*it]) 02483 { 02484 nr_gaps++; 02485 if (first_gap_after == angleIdx.end()) 02486 first_gap_after = it; 02487 last_gap_before = it+1; 02488 } 02489 } 02490 if (nr_gaps > 1) 02491 { 02492 angleIdx.erase(first_gap_after+1, last_gap_before); 02493 } 02494 02495 // Neglecting points that would form skinny triangles (if possible) 02496 if (is_skinny) 02497 { 02498 double angle_so_far = 0, angle_would_be; 02499 double max_combined_angle = (std::min)(maximum_angle_, M_PI-2*minimum_angle_); 02500 Eigen::Vector2f X; 02501 Eigen::Vector2f S1; 02502 Eigen::Vector2f S2; 02503 std::vector<int> to_erase; 02504 for (std::vector<int>::iterator it = angleIdx.begin()+1; it != angleIdx.end()-1; it++) 02505 { 02506 if (gaps[*(it-1)]) 02507 angle_so_far = 0; 02508 else 02509 angle_so_far += dif[*(it-1)]; 02510 if (gaps[*it]) 02511 angle_would_be = angle_so_far; 02512 else 02513 angle_would_be = angle_so_far + dif[*it]; 02514 if ( 02515 (skinny[*it] || skinny[*(it-1)]) && 02516 ((state_[angles_[*it].index] <= FREE) || (state_[angles_[*(it-1)].index] <= FREE)) && 02517 ((!gaps[*it]) || (angles_[*it].nnIndex > angles_[*(it-1)].nnIndex)) && 02518 ((!gaps[*(it-1)]) || (angles_[*it].nnIndex > angles_[*(it+1)].nnIndex)) && 02519 (angle_would_be < max_combined_angle) 02520 ) 02521 { 02522 if (gaps[*(it-1)]) 02523 { 02524 gaps[*it] = true; 02525 to_erase.push_back(*it); 02526 } 02527 else if (gaps[*it]) 02528 { 02529 gaps[*(it-1)] = true; 02530 to_erase.push_back(*it); 02531 } 02532 else 02533 { 02534 std::vector<int>::iterator prev_it; 02535 int erased_idx = to_erase.size()-1; 02536 for (prev_it = it-1; (erased_idx != -1) && (it != angleIdx.begin()); it--) 02537 if (*it == to_erase[erased_idx]) 02538 erased_idx--; 02539 else 02540 break; 02541 bool can_delete = true; 02542 for (std::vector<int>::iterator curr_it = prev_it+1; curr_it != it+1; curr_it++) 02543 { 02544 tmp_ = coords_[angles_[*curr_it].index] - proj_qp_; 02545 X[0] = tmp_.dot(u_); 02546 X[1] = tmp_.dot(v_); 02547 tmp_ = coords_[angles_[*prev_it].index] - proj_qp_; 02548 S1[0] = tmp_.dot(u_); 02549 S1[1] = tmp_.dot(v_); 02550 tmp_ = coords_[angles_[*(it+1)].index] - proj_qp_; 02551 S2[0] = tmp_.dot(u_); 02552 S2[1] = tmp_.dot(v_); 02553 // check for inclusions 02554 if (isVisible(X,S1,S2)) 02555 { 02556 can_delete = false; 02557 angle_so_far = 0; 02558 break; 02559 } 02560 } 02561 if (can_delete) 02562 { 02563 to_erase.push_back(*it); 02564 } 02565 } 02566 } 02567 else 02568 angle_so_far = 0; 02569 } 02570 for (std::vector<int>::iterator it = to_erase.begin(); it != to_erase.end(); it++) 02571 { 02572 for (std::vector<int>::iterator iter = angleIdx.begin(); iter != angleIdx.end(); iter++) 02573 if (*it == *iter) 02574 { 02575 angleIdx.erase(iter); 02576 break; 02577 } 02578 } 02579 } 02580 02581 // Writing edges and updating edge-front 02582 changed_1st_fn_ = false; 02583 changed_2nd_fn_ = false; 02584 new2boundary_ = NONE; 02585 for (std::vector<int>::iterator it = angleIdx.begin()+1; it != angleIdx.end()-1; it++) 02586 { 02587 current_index_ = angles_[*it].index; 02588 02589 is_current_free_ = false; 02590 if (state_[current_index_] <= FREE) 02591 { 02592 state_[current_index_] = FRINGE; 02593 is_current_free_ = true; 02594 } 02595 else if (!already_connected_) 02596 { 02597 prev_is_ffn_ = (ffn_[current_index_] == angles_[*(it-1)].index) && (!gaps[*(it-1)]); 02598 prev_is_sfn_ = (sfn_[current_index_] == angles_[*(it-1)].index) && (!gaps[*(it-1)]); 02599 next_is_ffn_ = (ffn_[current_index_] == angles_[*(it+1)].index) && (!gaps[*it]); 02600 next_is_sfn_ = (sfn_[current_index_] == angles_[*(it+1)].index) && (!gaps[*it]); 02601 if (!prev_is_ffn_ && !next_is_sfn_ && !prev_is_sfn_ && !next_is_ffn_) 02602 { 02603 nr_touched++; 02604 } 02605 } 02606 02607 if (gaps[*it]) 02608 if (gaps[*(it-1)]) 02609 { 02610 if (is_current_free_) 02611 state_[current_index_] = NONE; 02612 } 02613 02614 else // (gaps[*it]) && ^(gaps[*(it-1)]) 02615 { 02616 addTriangle (current_index_, angles_[*(it-1)].index, R_, output); 02617 addFringePoint (current_index_, R_); 02618 new2boundary_ = current_index_; 02619 if (!already_connected_) 02620 connectPoint (output, angles_[*(it-1)].index, R_, 02621 angles_[*(it+1)].index, 02622 uvn_nn[angles_[*it].nnIndex], uvn_nn[angles_[*(it-1)].nnIndex], uvn_nn_qp_zero); 02623 else already_connected_ = false; 02624 if (ffn_[R_] == angles_[*(angleIdx.begin())].index) 02625 { 02626 ffn_[R_] = new2boundary_; 02627 } 02628 else if (sfn_[R_] == angles_[*(angleIdx.begin())].index) 02629 { 02630 sfn_[R_] = new2boundary_; 02631 } 02632 } 02633 02634 else // ^(gaps[*it]) 02635 if (gaps[*(it-1)]) 02636 { 02637 addFringePoint (current_index_, R_); 02638 new2boundary_ = current_index_; 02639 if (!already_connected_) connectPoint(output, R_, angles_[*(it+1)].index, 02640 (it+2) == angleIdx.end() ? -1 : angles_[*(it+2)].index, 02641 uvn_nn[angles_[*it].nnIndex], uvn_nn_qp_zero, uvn_nn[angles_[*(it+1)].nnIndex]); 02642 else already_connected_ = false; 02643 if (ffn_[R_] == angles_[*(angleIdx.end()-1)].index) 02644 { 02645 ffn_[R_] = new2boundary_; 02646 } 02647 else if (sfn_[R_] == angles_[*(angleIdx.end()-1)].index) 02648 { 02649 sfn_[R_] = new2boundary_; 02650 } 02651 } 02652 02653 else // ^(gaps[*it]) && ^(gaps[*(it-1)]) 02654 { 02655 addTriangle (current_index_, angles_[*(it-1)].index, R_, output); 02656 addFringePoint (current_index_, R_); 02657 if (!already_connected_) connectPoint(output, angles_[*(it-1)].index, angles_[*(it+1)].index, 02658 (it+2) == angleIdx.end() ? -1 : gaps[*(it+1)] ? R_ : angles_[*(it+2)].index, 02659 uvn_nn[angles_[*it].nnIndex], uvn_nn[angles_[*(it-1)].nnIndex], uvn_nn[angles_[*(it+1)].nnIndex]); 02660 else already_connected_ = false; 02661 } 02662 } 02663 02664 // Finishing up R_ 02665 if (ffn_[R_] == sfn_[R_]) 02666 { 02667 state_[R_] = COMPLETED; 02668 } 02669 if (!gaps[*(angleIdx.end()-2)]) 02670 { 02671 addTriangle (angles_[*(angleIdx.end()-2)].index, angles_[*(angleIdx.end()-1)].index, R_, output); 02672 addFringePoint (angles_[*(angleIdx.end()-2)].index, R_); 02673 if (R_ == ffn_[angles_[*(angleIdx.end()-1)].index]) 02674 { 02675 if (angles_[*(angleIdx.end()-2)].index == sfn_[angles_[*(angleIdx.end()-1)].index]) 02676 { 02677 state_[angles_[*(angleIdx.end()-1)].index] = COMPLETED; 02678 } 02679 else 02680 { 02681 ffn_[angles_[*(angleIdx.end()-1)].index] = angles_[*(angleIdx.end()-2)].index; 02682 } 02683 } 02684 else if (R_ == sfn_[angles_[*(angleIdx.end()-1)].index]) 02685 { 02686 if (angles_[*(angleIdx.end()-2)].index == ffn_[angles_[*(angleIdx.end()-1)].index]) 02687 { 02688 state_[angles_[*(angleIdx.end()-1)].index] = COMPLETED; 02689 } 02690 else 02691 { 02692 sfn_[angles_[*(angleIdx.end()-1)].index] = angles_[*(angleIdx.end()-2)].index; 02693 } 02694 } 02695 } 02696 if (!gaps[*(angleIdx.begin())]) 02697 { 02698 if (R_ == ffn_[angles_[*(angleIdx.begin())].index]) 02699 { 02700 if (angles_[*(angleIdx.begin()+1)].index == sfn_[angles_[*(angleIdx.begin())].index]) 02701 { 02702 state_[angles_[*(angleIdx.begin())].index] = COMPLETED; 02703 } 02704 else 02705 { 02706 ffn_[angles_[*(angleIdx.begin())].index] = angles_[*(angleIdx.begin()+1)].index; 02707 } 02708 } 02709 else if (R_ == sfn_[angles_[*(angleIdx.begin())].index]) 02710 { 02711 if (angles_[*(angleIdx.begin()+1)].index == ffn_[angles_[*(angleIdx.begin())].index]) 02712 { 02713 state_[angles_[*(angleIdx.begin())].index] = COMPLETED; 02714 } 02715 else 02716 { 02717 sfn_[angles_[*(angleIdx.begin())].index] = angles_[*(angleIdx.begin()+1)].index; 02718 } 02719 } 02720 } 02721 } 02722 } 02723 PCL_DEBUG ("Number of triangles: %d\n", (int)output.polygons.size()); 02724 PCL_DEBUG ("Number of unconnected parts: %d\n", nr_parts); 02725 if (increase_nnn4fn > 0) 02726 PCL_WARN ("Number of neighborhood size increase requests for fringe neighbors: %d\n", increase_nnn4fn); 02727 if (increase_nnn4s > 0) 02728 PCL_WARN ("Number of neighborhood size increase requests for source: %d\n", increase_nnn4s); 02729 if (increase_dist > 0) 02730 PCL_WARN ("Number of automatic maximum distance increases: %d\n", increase_dist); 02731 02732 // sorting and removing doubles from fringe queue 02733 std::sort (fringe_queue_.begin (), fringe_queue_.end ()); 02734 fringe_queue_.erase (std::unique (fringe_queue_.begin (), fringe_queue_.end ()), fringe_queue_.end ()); 02735 PCL_DEBUG ("Number of processed points: %d / %d\n", (int)fringe_queue_.size(), (int)indices_->size ()); 02736 } 02737 02739 template <typename PointInT> void 02740 pcl::GreedyProjectionTriangulation<PointInT>::updateMesh ( 02741 const PointCloudInConstPtr &update, 02742 pcl::PolygonMesh &output) 02743 { 02744 // store old information 02745 size_t point_size_old = input_->points.size (); 02746 02747 // create new cloud 02748 pcl::PointCloud<PointInT> newcloud; 02749 newcloud = *input_; 02750 newcloud += *update; 02751 02752 // update cloud 02753 input_ = PointCloudInConstPtr (new pcl::PointCloud<PointInT>(newcloud)); 02754 02755 // change header 02756 output.header = input_->header; 02757 02758 // change cloud of output 02759 pcl::toROSMsg (*input_, output.cloud); 02760 indices_->resize (input_->points.size ()); 02761 02762 // update indices 02763 for (size_t i = point_size_old; i < indices_->size (); ++i) 02764 (*indices_)[i] = i; 02765 02766 // update tree 02767 tree_->setInputCloud (input_, indices_); 02768 02769 // initializing states and fringe neighbors 02770 02771 part_.resize(indices_->size ()); // indices of point's part 02772 for (size_t i = point_size_old; i < indices_->size (); ++i) part_[i] = -1; 02773 02774 // change the state set BOUNDARY to be FRINGE and all new point to be FREE 02775 state_.resize(indices_->size ()); 02776 for (size_t i = 0; i < point_size_old; ++i) 02777 if (state_[i] == BOUNDARY) state_[i] = FRINGE; 02778 02779 for (size_t i = point_size_old; i < indices_->size (); ++i) state_[i] = FREE; 02780 02781 source_.resize(indices_->size ()); 02782 for (size_t i = point_size_old; i < indices_->size (); ++i) source_[i] = NONE; 02783 02784 ffn_.resize(indices_->size ()); 02785 for (size_t i = point_size_old; i < indices_->size (); ++i) ffn_[i] = NONE; 02786 02787 sfn_.resize(indices_->size (), NONE); 02788 for (size_t i = point_size_old; i < indices_->size (); ++i) sfn_[i] = NONE; 02789 02790 fringe_queue_.clear (); 02791 02792 int fqIdx = 0; // current fringe's index in the queue to be processed 02793 02794 // Saving coordinates 02795 coords_.reserve (indices_->size ()); 02796 for (size_t cp = point_size_old; cp < indices_->size (); ++cp) 02797 { 02798 coords_.push_back(input_->points[(*indices_)[cp]].getVector3fMap()); 02799 } 02800 02801 const double sqr_mu = mu_*mu_; 02802 const double sqr_max_edge = search_radius_*search_radius_; 02803 if (nnn_ > (int)indices_->size ()) 02804 nnn_ = indices_->size (); 02805 02806 // Variables to hold the results of nearest neighbor searches 02807 std::vector<int> nnIdx (nnn_); 02808 std::vector<float> sqrDists (nnn_); 02809 02810 // current number of connected components 02811 int part_index = 0; // need to consider 02812 02813 // 2D coordinates of points 02814 const Eigen::Vector2f uvn_nn_qp_zero = Eigen::Vector2f::Zero(); 02815 Eigen::Vector2f uvn_current; 02816 Eigen::Vector2f uvn_prev; 02817 Eigen::Vector2f uvn_next; 02818 02819 // initializing fields 02820 already_connected_ = false; // see declaration for comments :P 02821 02822 // Initializing 02823 int is_free = point_size_old; 02824 int nr_parts=0, increase_nnn4fn=0, increase_nnn4s=0, increase_dist=0, nr_touched = 0; 02825 bool is_fringe; 02826 angles_.resize(nnn_); 02827 std::vector<Eigen::Vector2f> uvn_nn (nnn_); 02828 Eigen::Vector2f uvn_s; 02829 02830 // iterating through fringe points and finishing them until everything is done 02831 while (is_free != NONE) 02832 { 02833 R_ = is_free; 02834 if (state_[R_] == FREE) 02835 { 02836 state_[R_] = NONE; 02837 part_[R_] = part_index++; 02838 02839 // creating starting triangle 02840 //searchForNeighbors ((*indices_)[R_], nnIdx, sqrDists); 02841 tree_->nearestKSearch ((*indices_)[R_], nnn_, nnIdx, sqrDists); 02842 double sqr_dist_threshold = (std::min)(sqr_max_edge, sqr_mu * sqrDists[1]); 02843 02844 // Get the normal estimate at the current point 02845 const Eigen::Vector3f nc = input_->points[(*indices_)[R_]].getNormalVector3fMap (); 02846 02847 // Get a coordinate system that lies on a plane defined by its normal 02848 v_ = nc.unitOrthogonal (); 02849 u_ = nc.cross (v_); 02850 02851 // Projecting point onto the surface 02852 double dist = nc.dot(coords_[R_]); 02853 proj_qp_ = coords_[R_] - dist * nc; 02854 02855 // Converting coords, calculating angles and saving the projected near boundary edges 02856 int nr_edge = 0; 02857 std::vector<doubleEdge> doubleEdges; 02858 for (int i = 1; i < nnn_; i++) // nearest neighbor with index 0 is the query point R_ itself 02859 { 02860 // Transforming coordinates 02861 tmp_ = coords_[nnIdx[i]] - proj_qp_; 02862 uvn_nn[i][0] = tmp_.dot(u_); 02863 uvn_nn[i][1] = tmp_.dot(v_); 02864 // Computing the angle between each neighboring point and the query point itself 02865 angles_[i].angle = atan2(uvn_nn[i][1], uvn_nn[i][0]); 02866 // initializing angle descriptors 02867 angles_[i].index = nnIdx[i]; 02868 02869 if ( 02870 (state_[nnIdx[i]] == COMPLETED) || (state_[nnIdx[i]] == BOUNDARY) 02871 || (sqrDists[i] > sqr_dist_threshold) 02872 ) 02873 angles_[i].visible = false; 02874 else 02875 angles_[i].visible = true; 02876 // Saving the edges between nearby boundary points 02877 if ((state_[nnIdx[i]] == FRINGE) || (state_[nnIdx[i]] == BOUNDARY)) 02878 { 02879 doubleEdge e; 02880 e.index = i; 02881 nr_edge++; 02882 tmp_ = coords_[ffn_[nnIdx[i]]] - proj_qp_; 02883 e.first[0] = tmp_.dot(u_); 02884 e.first[1] = tmp_.dot(v_); 02885 tmp_ = coords_[sfn_[nnIdx[i]]] - proj_qp_; 02886 e.second[0] = tmp_.dot(u_); 02887 e.second[1] = tmp_.dot(v_); 02888 doubleEdges.push_back(e); 02889 } 02890 } 02891 angles_[0].visible = false; 02892 02893 // Verify the visibility of each potential new vertex 02894 for (int i = 1; i < nnn_; i++) // nearest neighbor with index 0 is the query point R_ itself 02895 if ((angles_[i].visible) && (ffn_[R_] != nnIdx[i]) && (sfn_[R_] != nnIdx[i])) 02896 { 02897 bool visibility = true; 02898 for (int j = 0; j < nr_edge; j++) 02899 { 02900 if (ffn_[nnIdx[doubleEdges[j].index]] != nnIdx[i]) 02901 visibility = isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].first, Eigen::Vector2f::Zero()); 02902 if (!visibility) 02903 break; 02904 if (sfn_[nnIdx[doubleEdges[j].index]] != nnIdx[i]) 02905 visibility = isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].second, Eigen::Vector2f::Zero()); 02906 if (!visibility == false) 02907 break; 02908 } 02909 angles_[i].visible = visibility; 02910 } 02911 02912 // Selecting first two visible free neighbors 02913 bool not_found = true; 02914 int left = 1; 02915 do 02916 { 02917 while ((left < nnn_) && ((!angles_[left].visible) || (state_[nnIdx[left]] > FREE))) left++; 02918 if (left >= nnn_) 02919 break; 02920 else 02921 { 02922 int right = left+1; 02923 do 02924 { 02925 while ((right < nnn_) && ((!angles_[right].visible) || (state_[nnIdx[right]] > FREE))) right++; 02926 if (right >= nnn_) 02927 break; 02928 else if ((coords_[nnIdx[left]] - coords_[nnIdx[right]]).squaredNorm () > sqr_max_edge) 02929 right++; 02930 else 02931 { 02932 addFringePoint (nnIdx[right], R_); 02933 addFringePoint (nnIdx[left], nnIdx[right]); 02934 addFringePoint (R_, nnIdx[left]); 02935 state_[R_] = state_[nnIdx[left]] = state_[nnIdx[right]] = FRINGE; 02936 ffn_[R_] = nnIdx[left]; 02937 sfn_[R_] = nnIdx[right]; 02938 ffn_[nnIdx[left]] = nnIdx[right]; 02939 sfn_[nnIdx[left]] = R_; 02940 ffn_[nnIdx[right]] = R_; 02941 sfn_[nnIdx[right]] = nnIdx[left]; 02942 addTriangle (R_, nnIdx[left], nnIdx[right], output); 02943 nr_parts++; 02944 not_found = false; 02945 break; 02946 } 02947 } 02948 while (true); 02949 left++; 02950 } 02951 } 02952 while (not_found); 02953 } 02954 02955 is_free = NONE; 02956 for (unsigned temp = point_size_old; temp < indices_->size (); temp++) 02957 { 02958 if (state_[temp] == FREE) 02959 { 02960 is_free = temp; 02961 break; 02962 } 02963 } 02964 02965 is_fringe = true; 02966 while (is_fringe) 02967 { 02968 is_fringe = false; 02969 02970 int fqSize = fringe_queue_.size(); 02971 while ((fqIdx < fqSize) && (state_[fringe_queue_[fqIdx]] != FRINGE)) 02972 fqIdx++; 02973 02974 // an unfinished fringe point is found 02975 if (fqIdx >= fqSize) 02976 { 02977 continue; 02978 } 02979 02980 R_ = fringe_queue_[fqIdx]; 02981 is_fringe = true; 02982 02983 if (ffn_[R_] == sfn_[R_]) 02984 { 02985 state_[R_] = COMPLETED; 02986 continue; 02987 } 02988 //searchForNeighbors ((*indices_)[R_], nnIdx, sqrDists); 02989 tree_->nearestKSearch ((*indices_)[R_], nnn_, nnIdx, sqrDists); 02990 02991 // Locating FFN and SFN to adapt distance threshold 02992 double sqr_source_dist = (coords_[R_] - coords_[source_[R_]]).squaredNorm (); 02993 double sqr_ffn_dist = (coords_[R_] - coords_[ffn_[R_]]).squaredNorm (); 02994 double sqr_sfn_dist = (coords_[R_] - coords_[sfn_[R_]]).squaredNorm (); 02995 double max_sqr_fn_dist = (std::max)(sqr_ffn_dist, sqr_sfn_dist); 02996 double sqr_dist_threshold = (std::min)(sqr_max_edge, sqr_mu * sqrDists[1]); //sqr_mu * sqr_avg_conn_dist); 02997 if (max_sqr_fn_dist > sqrDists[nnn_-1]) 02998 { 02999 if (0 == increase_nnn4fn) 03000 PCL_WARN("Not enough neighbors are considered: ffn or sfn out of range! Consider increasing nnn_... Setting R=%d to be BOUNDARY!\n", R_); 03001 increase_nnn4fn++; 03002 state_[R_] = BOUNDARY; 03003 continue; 03004 } 03005 double max_sqr_fns_dist = (std::max)(sqr_source_dist, max_sqr_fn_dist); 03006 if (max_sqr_fns_dist > sqrDists[nnn_-1]) 03007 { 03008 if (0 == increase_nnn4s) 03009 PCL_WARN("Not enough neighbors are considered: source of R=%d is out of range! Consider increasing nnn_...\n", R_); 03010 increase_nnn4s++; 03011 } 03012 03013 // Get the normal estimate at the current point 03014 const Eigen::Vector3f nc = input_->points[(*indices_)[R_]].getNormalVector3fMap (); 03015 03016 // Get a coordinate system that lies on a plane defined by its normal 03017 v_ = nc.unitOrthogonal (); 03018 u_ = nc.cross (v_); 03019 03020 // Projecting point onto the surface 03021 double dist = nc.dot(coords_[R_]); 03022 proj_qp_ = coords_[R_] - dist * nc; 03023 03024 // Converting coords, calculating angles and saving the projected near boundary edges 03025 int nr_edge = 0; 03026 std::vector<doubleEdge> doubleEdges; 03027 for (int i = 1; i < nnn_; i++) // nearest neighbor with index 0 is the query point R_ itself 03028 { 03029 tmp_ = coords_[nnIdx[i]] - proj_qp_; 03030 uvn_nn[i][0] = tmp_.dot(u_); 03031 uvn_nn[i][1] = tmp_.dot(v_); 03032 03033 // Computing the angle between each neighboring point and the query point itself 03034 angles_[i].angle = atan2(uvn_nn[i][1], uvn_nn[i][0]); 03035 // initializing angle descriptors 03036 angles_[i].index = nnIdx[i]; 03037 angles_[i].nnIndex = i; 03038 if ( 03039 (state_[nnIdx[i]] == COMPLETED) || (state_[nnIdx[i]] == BOUNDARY) 03040 || (sqrDists[i] > sqr_dist_threshold) 03041 ) 03042 angles_[i].visible = false; 03043 else 03044 angles_[i].visible = true; 03045 if ((ffn_[R_] == nnIdx[i]) || (sfn_[R_] == nnIdx[i])) 03046 angles_[i].visible = true; 03047 bool same_side = true; 03048 const Eigen::Vector3f neighbor_normal = input_->points[(*indices_)[nnIdx[i]]].getNormalVector3fMap (); 03049 double cosine = nc.dot (neighbor_normal); 03050 if (cosine > 1) cosine = 1; 03051 if (cosine < -1) cosine = -1; 03052 double angle = acos (cosine); 03053 if ((!consistent_) && (angle > M_PI/2)) 03054 angle = M_PI - angle; 03055 if (angle > eps_angle_) 03056 { 03057 angles_[i].visible = false; 03058 same_side = false; 03059 } 03060 // Saving the edges between nearby boundary points 03061 if ((i!=0) && (same_side) && ((state_[nnIdx[i]] == FRINGE) || (state_[nnIdx[i]] == BOUNDARY))) 03062 { 03063 doubleEdge e; 03064 e.index = i; 03065 nr_edge++; 03066 tmp_ = coords_[ffn_[nnIdx[i]]] - proj_qp_; 03067 e.first[0] = tmp_.dot(u_); 03068 e.first[1] = tmp_.dot(v_); 03069 tmp_ = coords_[sfn_[nnIdx[i]]] - proj_qp_; 03070 e.second[0] = tmp_.dot(u_); 03071 e.second[1] = tmp_.dot(v_); 03072 doubleEdges.push_back(e); 03073 // Pruning by visibility criterion 03074 if ((state_[nnIdx[i]] == FRINGE) && (ffn_[R_] != nnIdx[i]) && (sfn_[R_] != nnIdx[i])) 03075 { 03076 double angle1 = atan2(e.first[1] - uvn_nn[i][1], e.first[0] - uvn_nn[i][0]); 03077 double angle2 = atan2(e.second[1] - uvn_nn[i][1], e.second[0] - uvn_nn[i][0]); 03078 double angleMin, angleMax; 03079 if (angle1 < angle2) 03080 { 03081 angleMin = angle1; 03082 angleMax = angle2; 03083 } 03084 else 03085 { 03086 angleMin = angle2; 03087 angleMax = angle1; 03088 } 03089 double angleR = angles_[i].angle + M_PI; 03090 if (angleR >= 2*M_PI) 03091 angleR -= 2*M_PI; 03092 if ((source_[nnIdx[i]] == ffn_[nnIdx[i]]) || (source_[nnIdx[i]] == sfn_[nnIdx[i]])) 03093 { 03094 if ((angleMax - angleMin) < M_PI) 03095 { 03096 if ((angleMin < angleR) && (angleR < angleMax)) 03097 angles_[i].visible = false; 03098 } 03099 else 03100 { 03101 if ((angleR < angleMin) || (angleMax < angleR)) 03102 angles_[i].visible = false; 03103 } 03104 } 03105 else 03106 { 03107 tmp_ = coords_[source_[nnIdx[i]]] - proj_qp_; 03108 uvn_s[0] = tmp_.dot(u_); 03109 uvn_s[1] = tmp_.dot(v_); 03110 double angleS = atan2(uvn_s[1] - uvn_nn[i][1], uvn_s[0] - uvn_nn[i][0]); 03111 if ((angleMin < angleS) && (angleS < angleMax)) 03112 { 03113 if ((angleMin < angleR) && (angleR < angleMax)) 03114 angles_[i].visible = false; 03115 } 03116 else 03117 { 03118 if ((angleR < angleMin) || (angleMax < angleR)) 03119 angles_[i].visible = false; 03120 } 03121 } 03122 } 03123 } 03124 } 03125 angles_[0].visible = false; 03126 03127 // Verify the visibility of each potential new vertex 03128 for (int i = 1; i < nnn_; i++) // nearest neighbor with index 0 is the query point R_ itself 03129 if ((angles_[i].visible) && (ffn_[R_] != nnIdx[i]) && (sfn_[R_] != nnIdx[i])) 03130 { 03131 bool visibility = true; 03132 for (int j = 0; j < nr_edge; j++) 03133 { 03134 if (doubleEdges[j].index != i) 03135 { 03136 int f = ffn_[nnIdx[doubleEdges[j].index]]; 03137 if ((f != nnIdx[i]) && (f != R_)) 03138 visibility = isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].first, Eigen::Vector2f::Zero()); 03139 if (visibility == false) 03140 break; 03141 03142 int s = sfn_[nnIdx[doubleEdges[j].index]]; 03143 if ((s != nnIdx[i]) && (s != R_)) 03144 visibility = isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].second, Eigen::Vector2f::Zero()); 03145 if (visibility == false) 03146 break; 03147 } 03148 } 03149 angles_[i].visible = visibility; 03150 } 03151 03152 // Sorting angles 03153 std::sort (angles_.begin (), angles_.end (), GreedyProjectionTriangulation<PointInT>::nnAngleSortAsc); 03154 03155 // Triangulating 03156 if (angles_[2].visible == false) 03157 { 03158 if ( !( (angles_[0].index == ffn_[R_] && angles_[1].index == sfn_[R_]) || (angles_[0].index == sfn_[R_] && angles_[1].index == ffn_[R_]) ) ) 03159 { 03160 state_[R_] = BOUNDARY; 03161 } 03162 else 03163 { 03164 if ((source_[R_] == angles_[0].index) || (source_[R_] == angles_[1].index)) 03165 state_[R_] = BOUNDARY; 03166 else 03167 { 03168 if (sqr_max_edge < (coords_[ffn_[R_]] - coords_[sfn_[R_]]).squaredNorm ()) 03169 { 03170 state_[R_] = BOUNDARY; 03171 } 03172 else 03173 { 03174 tmp_ = coords_[source_[R_]] - proj_qp_; 03175 uvn_s[0] = tmp_.dot(u_); 03176 uvn_s[1] = tmp_.dot(v_); 03177 double angleS = atan2(uvn_s[1], uvn_s[0]); 03178 double dif = angles_[1].angle - angles_[0].angle; 03179 if ((angles_[0].angle < angleS) && (angleS < angles_[1].angle)) 03180 { 03181 if (dif < 2*M_PI - maximum_angle_) 03182 state_[R_] = BOUNDARY; 03183 else 03184 closeTriangle (output); 03185 } 03186 else 03187 { 03188 if (dif >= maximum_angle_) 03189 state_[R_] = BOUNDARY; 03190 else 03191 closeTriangle (output); 03192 } 03193 } 03194 } 03195 } 03196 continue; 03197 } 03198 03199 // Finding the FFN and SFN 03200 int start = -1, end = -1; 03201 for (int i=0; i<nnn_; i++) 03202 { 03203 if (ffn_[R_] == angles_[i].index) 03204 { 03205 start = i; 03206 if (sfn_[R_] == angles_[i+1].index) 03207 end = i+1; 03208 else 03209 if (i==0) 03210 { 03211 for (i = i+2; i < nnn_; i++) 03212 if (sfn_[R_] == angles_[i].index) 03213 break; 03214 end = i; 03215 } 03216 else 03217 { 03218 for (i = i+2; i < nnn_; i++) 03219 if (sfn_[R_] == angles_[i].index) 03220 break; 03221 end = i; 03222 } 03223 break; 03224 } 03225 if (sfn_[R_] == angles_[i].index) 03226 { 03227 start = i; 03228 if (ffn_[R_] == angles_[i+1].index) 03229 end = i+1; 03230 else 03231 if (i==0) 03232 { 03233 for (i = i+2; i < nnn_; i++) 03234 if (ffn_[R_] == angles_[i].index) 03235 break; 03236 end = i; 03237 } 03238 else 03239 { 03240 for (i = i+2; i < nnn_; i++) 03241 if (ffn_[R_] == angles_[i].index) 03242 break; 03243 end = i; 03244 } 03245 break; 03246 } 03247 } 03248 03249 // start and end are always set, as we checked if ffn or sfn are out of range before, but let's check anyways if < 0 03250 if ((start < 0) || (end < 0) || (end == nnn_) || (!angles_[start].visible) || (!angles_[end].visible)) 03251 { 03252 state_[R_] = BOUNDARY; 03253 continue; 03254 } 03255 03256 // Finding last visible nn 03257 int last_visible = end; 03258 while ((last_visible+1<nnn_) && (angles_[last_visible+1].visible)) last_visible++; 03259 03260 // Finding visibility region of R 03261 bool need_invert = false; 03262 int sourceIdx = nnn_; 03263 if ((source_[R_] == ffn_[R_]) || (source_[R_] == sfn_[R_])) 03264 { 03265 if ((angles_[end].angle - angles_[start].angle) < M_PI) 03266 need_invert = true; 03267 } 03268 else 03269 { 03270 for (sourceIdx=0; sourceIdx<nnn_; sourceIdx++) 03271 if (angles_[sourceIdx].index == source_[R_]) 03272 break; 03273 if (sourceIdx == nnn_) 03274 { 03275 int vis_free = NONE, nnCB = NONE; // any free visible and nearest completed or boundary neighbor of R 03276 for (int i = 1; i < nnn_; i++) // nearest neighbor with index 0 is the query point R_ itself 03277 { 03278 // NOTE: nnCB is an index in nnIdx 03279 if ((state_[nnIdx[i]] == COMPLETED) || (state_[nnIdx[i]] == BOUNDARY)) 03280 { 03281 if (nnCB == NONE) 03282 { 03283 nnCB = i; 03284 if (vis_free != NONE) 03285 break; 03286 } 03287 } 03288 // NOTE: vis_free is an index in angles 03289 if (state_[angles_[i].index] <= FREE) 03290 { 03291 if (i <= last_visible) 03292 { 03293 vis_free = i; 03294 if (nnCB != NONE) 03295 break; 03296 } 03297 } 03298 } 03299 // NOTE: nCB is an index in angles 03300 int nCB = 0; 03301 if (nnCB != NONE) 03302 while (angles_[nCB].index != nnIdx[nnCB]) nCB++; 03303 else 03304 nCB = NONE; 03305 03306 if (vis_free != NONE) 03307 { 03308 if ((vis_free < start) || (vis_free > end)) 03309 need_invert = true; 03310 } 03311 else 03312 { 03313 if (nCB != NONE) 03314 { 03315 if ((nCB == start) || (nCB == end)) 03316 { 03317 bool inside_CB = false; 03318 bool outside_CB = false; 03319 for (int i=0; i<nnn_; i++) 03320 { 03321 if ( 03322 ((state_[angles_[i].index] == COMPLETED) || (state_[angles_[i].index] == BOUNDARY)) 03323 && (i != start) && (i != end) 03324 ) 03325 { 03326 if ((angles_[start].angle <= angles_[i].angle) && (angles_[i].angle <= angles_[end].angle)) 03327 { 03328 inside_CB = true; 03329 if (outside_CB) 03330 break; 03331 } 03332 else 03333 { 03334 outside_CB = true; 03335 if (inside_CB) 03336 break; 03337 } 03338 } 03339 } 03340 if (inside_CB && !outside_CB) 03341 need_invert = true; 03342 else if (!(!inside_CB && outside_CB)) 03343 { 03344 if ((angles_[end].angle - angles_[start].angle) < M_PI) 03345 need_invert = true; 03346 } 03347 } 03348 else 03349 { 03350 if ((angles_[nCB].angle > angles_[start].angle) && (angles_[nCB].angle < angles_[end].angle)) 03351 need_invert = true; 03352 } 03353 } 03354 else 03355 { 03356 if (start == end-1) 03357 need_invert = true; 03358 } 03359 } 03360 } 03361 else if ((angles_[start].angle < angles_[sourceIdx].angle) && (angles_[sourceIdx].angle < angles_[end].angle)) 03362 need_invert = true; 03363 } 03364 03365 // switching start and end if necessary 03366 if (need_invert) 03367 { 03368 int tmp = start; 03369 start = end; 03370 end = tmp; 03371 } 03372 03373 // Arranging visible nnAngles in the order they need to be connected and 03374 // compute the maximal angle difference between two consecutive visible angles 03375 bool is_boundary = false, is_skinny = false; 03376 std::vector<bool> gaps (nnn_, false); 03377 std::vector<bool> skinny (nnn_, false); 03378 std::vector<double> dif (nnn_); 03379 std::vector<int> angleIdx; angleIdx.reserve (nnn_); 03380 if (start > end) 03381 { 03382 for (int j=start; j<last_visible; j++) 03383 { 03384 dif[j] = (angles_[j+1].angle - angles_[j].angle); 03385 if (dif[j] < minimum_angle_) 03386 { 03387 skinny[j] = is_skinny = true; 03388 } 03389 else if (maximum_angle_ <= dif[j]) 03390 { 03391 gaps[j] = is_boundary = true; 03392 } 03393 if ((!gaps[j]) && (sqr_max_edge < (coords_[angles_[j+1].index] - coords_[angles_[j].index]).squaredNorm ())) 03394 { 03395 gaps[j] = is_boundary = true; 03396 } 03397 angleIdx.push_back(j); 03398 } 03399 03400 dif[last_visible] = (2*M_PI + angles_[0].angle - angles_[last_visible].angle); 03401 if (dif[last_visible] < minimum_angle_) 03402 { 03403 skinny[last_visible] = is_skinny = true; 03404 } 03405 else if (maximum_angle_ <= dif[last_visible]) 03406 { 03407 gaps[last_visible] = is_boundary = true; 03408 } 03409 if ((!gaps[last_visible]) && (sqr_max_edge < (coords_[angles_[0].index] - coords_[angles_[last_visible].index]).squaredNorm ())) 03410 { 03411 gaps[last_visible] = is_boundary = true; 03412 } 03413 angleIdx.push_back(last_visible); 03414 03415 for (int j=0; j<end; j++) 03416 { 03417 dif[j] = (angles_[j+1].angle - angles_[j].angle); 03418 if (dif[j] < minimum_angle_) 03419 { 03420 skinny[j] = is_skinny = true; 03421 } 03422 else if (maximum_angle_ <= dif[j]) 03423 { 03424 gaps[j] = is_boundary = true; 03425 } 03426 if ((!gaps[j]) && (sqr_max_edge < (coords_[angles_[j+1].index] - coords_[angles_[j].index]).squaredNorm ())) 03427 { 03428 gaps[j] = is_boundary = true; 03429 } 03430 angleIdx.push_back(j); 03431 } 03432 angleIdx.push_back(end); 03433 } 03434 // start < end 03435 else 03436 { 03437 for (int j=start; j<end; j++) 03438 { 03439 dif[j] = (angles_[j+1].angle - angles_[j].angle); 03440 if (dif[j] < minimum_angle_) 03441 { 03442 skinny[j] = is_skinny = true; 03443 } 03444 else if (maximum_angle_ <= dif[j]) 03445 { 03446 gaps[j] = is_boundary = true; 03447 } 03448 if ((!gaps[j]) && (sqr_max_edge < (coords_[angles_[j+1].index] - coords_[angles_[j].index]).squaredNorm ())) 03449 { 03450 gaps[j] = is_boundary = true; 03451 } 03452 angleIdx.push_back(j); 03453 } 03454 angleIdx.push_back(end); 03455 } 03456 03457 // Set the state of the point 03458 state_[R_] = is_boundary ? BOUNDARY : COMPLETED; 03459 03460 std::vector<int>::iterator first_gap_after = angleIdx.end (); 03461 std::vector<int>::iterator last_gap_before = angleIdx.begin (); 03462 int nr_gaps = 0; 03463 for (std::vector<int>::iterator it = angleIdx.begin (); it != angleIdx.end () - 1; it++) 03464 { 03465 if (gaps[*it]) 03466 { 03467 nr_gaps++; 03468 if (first_gap_after == angleIdx.end()) 03469 first_gap_after = it; 03470 last_gap_before = it+1; 03471 } 03472 } 03473 if (nr_gaps > 1) 03474 { 03475 angleIdx.erase(first_gap_after+1, last_gap_before); 03476 } 03477 03478 // Neglecting points that would form skinny triangles (if possible) 03479 if (is_skinny) 03480 { 03481 double angle_so_far = 0, angle_would_be; 03482 double max_combined_angle = (std::min)(maximum_angle_, M_PI-2*minimum_angle_); 03483 Eigen::Vector2f X; 03484 Eigen::Vector2f S1; 03485 Eigen::Vector2f S2; 03486 std::vector<int> to_erase; 03487 for (std::vector<int>::iterator it = angleIdx.begin()+1; it != angleIdx.end()-1; it++) 03488 { 03489 if (gaps[*(it-1)]) 03490 angle_so_far = 0; 03491 else 03492 angle_so_far += dif[*(it-1)]; 03493 if (gaps[*it]) 03494 angle_would_be = angle_so_far; 03495 else 03496 angle_would_be = angle_so_far + dif[*it]; 03497 if ( 03498 (skinny[*it] || skinny[*(it-1)]) && 03499 ((state_[angles_[*it].index] <= FREE) || (state_[angles_[*(it-1)].index] <= FREE)) && 03500 ((!gaps[*it]) || (angles_[*it].nnIndex > angles_[*(it-1)].nnIndex)) && 03501 ((!gaps[*(it-1)]) || (angles_[*it].nnIndex > angles_[*(it+1)].nnIndex)) && 03502 (angle_would_be < max_combined_angle) 03503 ) 03504 { 03505 if (gaps[*(it-1)]) 03506 { 03507 gaps[*it] = true; 03508 to_erase.push_back(*it); 03509 } 03510 else if (gaps[*it]) 03511 { 03512 gaps[*(it-1)] = true; 03513 to_erase.push_back(*it); 03514 } 03515 else 03516 { 03517 std::vector<int>::iterator prev_it; 03518 int erased_idx = to_erase.size()-1; 03519 for (prev_it = it-1; (erased_idx != -1) && (it != angleIdx.begin()); it--) 03520 if (*it == to_erase[erased_idx]) 03521 erased_idx--; 03522 else 03523 break; 03524 bool can_delete = true; 03525 for (std::vector<int>::iterator curr_it = prev_it+1; curr_it != it+1; curr_it++) 03526 { 03527 tmp_ = coords_[angles_[*curr_it].index] - proj_qp_; 03528 X[0] = tmp_.dot(u_); 03529 X[1] = tmp_.dot(v_); 03530 tmp_ = coords_[angles_[*prev_it].index] - proj_qp_; 03531 S1[0] = tmp_.dot(u_); 03532 S1[1] = tmp_.dot(v_); 03533 tmp_ = coords_[angles_[*(it+1)].index] - proj_qp_; 03534 S2[0] = tmp_.dot(u_); 03535 S2[1] = tmp_.dot(v_); 03536 // check for inclusions 03537 if (isVisible(X,S1,S2)) 03538 { 03539 can_delete = false; 03540 angle_so_far = 0; 03541 break; 03542 } 03543 } 03544 if (can_delete) 03545 { 03546 to_erase.push_back(*it); 03547 } 03548 } 03549 } 03550 else 03551 angle_so_far = 0; 03552 } 03553 for (std::vector<int>::iterator it = to_erase.begin(); it != to_erase.end(); it++) 03554 { 03555 for (std::vector<int>::iterator iter = angleIdx.begin(); iter != angleIdx.end(); iter++) 03556 if (*it == *iter) 03557 { 03558 angleIdx.erase(iter); 03559 break; 03560 } 03561 } 03562 } 03563 03564 // Writing edges and updating edge-front 03565 changed_1st_fn_ = false; 03566 changed_2nd_fn_ = false; 03567 new2boundary_ = NONE; 03568 for (std::vector<int>::iterator it = angleIdx.begin()+1; it != angleIdx.end()-1; it++) 03569 { 03570 current_index_ = angles_[*it].index; 03571 03572 is_current_free_ = false; 03573 if (state_[current_index_] <= FREE) 03574 { 03575 state_[current_index_] = FRINGE; 03576 is_current_free_ = true; 03577 } 03578 else if (!already_connected_) 03579 { 03580 prev_is_ffn_ = (ffn_[current_index_] == angles_[*(it-1)].index) && (!gaps[*(it-1)]); 03581 prev_is_sfn_ = (sfn_[current_index_] == angles_[*(it-1)].index) && (!gaps[*(it-1)]); 03582 next_is_ffn_ = (ffn_[current_index_] == angles_[*(it+1)].index) && (!gaps[*it]); 03583 next_is_sfn_ = (sfn_[current_index_] == angles_[*(it+1)].index) && (!gaps[*it]); 03584 if (!prev_is_ffn_ && !next_is_sfn_ && !prev_is_sfn_ && !next_is_ffn_) 03585 { 03586 nr_touched++; 03587 } 03588 } 03589 03590 if (gaps[*it]) 03591 if (gaps[*(it-1)]) 03592 { 03593 if (is_current_free_) 03594 state_[current_index_] = NONE; 03595 } 03596 03597 else // (gaps[*it]) && ^(gaps[*(it-1)]) 03598 { 03599 addTriangle (current_index_, angles_[*(it-1)].index, R_, output); 03600 addFringePoint (current_index_, R_); 03601 new2boundary_ = current_index_; 03602 if (!already_connected_) 03603 connectPoint (output, angles_[*(it-1)].index, R_, 03604 angles_[*(it+1)].index, 03605 uvn_nn[angles_[*it].nnIndex], uvn_nn[angles_[*(it-1)].nnIndex], uvn_nn_qp_zero); 03606 else already_connected_ = false; 03607 if (ffn_[R_] == angles_[*(angleIdx.begin())].index) 03608 { 03609 ffn_[R_] = new2boundary_; 03610 } 03611 else if (sfn_[R_] == angles_[*(angleIdx.begin())].index) 03612 { 03613 sfn_[R_] = new2boundary_; 03614 } 03615 } 03616 03617 else // ^(gaps[*it]) 03618 if (gaps[*(it-1)]) 03619 { 03620 addFringePoint (current_index_, R_); 03621 new2boundary_ = current_index_; 03622 if (!already_connected_) connectPoint(output, R_, angles_[*(it+1)].index, 03623 (it+2) == angleIdx.end() ? -1 : angles_[*(it+2)].index, 03624 uvn_nn[angles_[*it].nnIndex], uvn_nn_qp_zero, uvn_nn[angles_[*(it+1)].nnIndex]); 03625 else already_connected_ = false; 03626 if (ffn_[R_] == angles_[*(angleIdx.end()-1)].index) 03627 { 03628 ffn_[R_] = new2boundary_; 03629 } 03630 else if (sfn_[R_] == angles_[*(angleIdx.end()-1)].index) 03631 { 03632 sfn_[R_] = new2boundary_; 03633 } 03634 } 03635 03636 else // ^(gaps[*it]) && ^(gaps[*(it-1)]) 03637 { 03638 addTriangle (current_index_, angles_[*(it-1)].index, R_, output); 03639 addFringePoint (current_index_, R_); 03640 if (!already_connected_) connectPoint(output, angles_[*(it-1)].index, angles_[*(it+1)].index, 03641 (it+2) == angleIdx.end() ? -1 : gaps[*(it+1)] ? R_ : angles_[*(it+2)].index, 03642 uvn_nn[angles_[*it].nnIndex], uvn_nn[angles_[*(it-1)].nnIndex], uvn_nn[angles_[*(it+1)].nnIndex]); 03643 else already_connected_ = false; 03644 } 03645 } 03646 03647 // Finishing up R_ 03648 if (ffn_[R_] == sfn_[R_]) 03649 { 03650 state_[R_] = COMPLETED; 03651 } 03652 if (!gaps[*(angleIdx.end()-2)]) 03653 { 03654 addTriangle (angles_[*(angleIdx.end()-2)].index, angles_[*(angleIdx.end()-1)].index, R_, output); 03655 addFringePoint (angles_[*(angleIdx.end()-2)].index, R_); 03656 if (R_ == ffn_[angles_[*(angleIdx.end()-1)].index]) 03657 { 03658 if (angles_[*(angleIdx.end()-2)].index == sfn_[angles_[*(angleIdx.end()-1)].index]) 03659 { 03660 state_[angles_[*(angleIdx.end()-1)].index] = COMPLETED; 03661 } 03662 else 03663 { 03664 ffn_[angles_[*(angleIdx.end()-1)].index] = angles_[*(angleIdx.end()-2)].index; 03665 } 03666 } 03667 else if (R_ == sfn_[angles_[*(angleIdx.end()-1)].index]) 03668 { 03669 if (angles_[*(angleIdx.end()-2)].index == ffn_[angles_[*(angleIdx.end()-1)].index]) 03670 { 03671 state_[angles_[*(angleIdx.end()-1)].index] = COMPLETED; 03672 } 03673 else 03674 { 03675 sfn_[angles_[*(angleIdx.end()-1)].index] = angles_[*(angleIdx.end()-2)].index; 03676 } 03677 } 03678 } 03679 if (!gaps[*(angleIdx.begin())]) 03680 { 03681 if (R_ == ffn_[angles_[*(angleIdx.begin())].index]) 03682 { 03683 if (angles_[*(angleIdx.begin()+1)].index == sfn_[angles_[*(angleIdx.begin())].index]) 03684 { 03685 state_[angles_[*(angleIdx.begin())].index] = COMPLETED; 03686 } 03687 else 03688 { 03689 ffn_[angles_[*(angleIdx.begin())].index] = angles_[*(angleIdx.begin()+1)].index; 03690 } 03691 } 03692 else if (R_ == sfn_[angles_[*(angleIdx.begin())].index]) 03693 { 03694 if (angles_[*(angleIdx.begin()+1)].index == ffn_[angles_[*(angleIdx.begin())].index]) 03695 { 03696 state_[angles_[*(angleIdx.begin())].index] = COMPLETED; 03697 } 03698 else 03699 { 03700 sfn_[angles_[*(angleIdx.begin())].index] = angles_[*(angleIdx.begin()+1)].index; 03701 } 03702 } 03703 } 03704 } 03705 } 03706 PCL_DEBUG ("Number of triangles: %d\n", (int)output.polygons.size()); 03707 PCL_DEBUG ("Number of unconnected parts: %d\n", nr_parts); 03708 if (increase_nnn4fn > 0) 03709 PCL_WARN ("Number of neighborhood size increase requests for fringe neighbors: %d\n", increase_nnn4fn); 03710 if (increase_nnn4s > 0) 03711 PCL_WARN ("Number of neighborhood size increase requests for source: %d\n", increase_nnn4s); 03712 if (increase_dist > 0) 03713 PCL_WARN ("Number of automatic maximum distance increases: %d\n", increase_dist); 03714 03715 // sorting and removing doubles from fringe queue 03716 std::sort (fringe_queue_.begin (), fringe_queue_.end ()); 03717 fringe_queue_.erase (std::unique (fringe_queue_.begin (), fringe_queue_.end ()), fringe_queue_.end ()); 03718 PCL_DEBUG ("Number of processed points: %d / %d\n", (int)fringe_queue_.size(), (int)indices_->size ()); 03719 } 03720 03722 template <typename PointInT> void 03723 pcl::GreedyProjectionTriangulation<PointInT>::updateMesh ( 03724 const PointCloudInConstPtr &update, pcl::PolygonMesh &output, 03725 pcl::TextureMesh &tex_mesh) 03726 { 03727 // store old information 03728 size_t point_size1 = input_->points.size (); 03729 03730 // update mesh 03731 updateMesh(update, output); 03732 03733 // update texture mesh 03734 tex_mesh.header = output.header; 03735 tex_mesh.cloud = output.cloud; 03736 03737 // Split to 2 submeshes 03738 std::vector< ::pcl::Vertices> polygon; 03739 03740 // testing:: the first 1/3 faces belong to 1st mesh 03741 for(size_t i =point_size1; i < output.polygons.size(); ++i) 03742 polygon.push_back(output.polygons[i]); 03743 03744 // add new polygon to texture mesh 03745 tex_mesh.tex_polygons.push_back(polygon); 03746 } 03747 03748 #define PCL_INSTANTIATE_GreedyProjectionTriangulation(T) \ 03749 template class PCL_EXPORTS pcl::GreedyProjectionTriangulation<T>; 03750 03751 #endif // PCL_SURFACE_IMPL_GP3_H_ 03752 03753