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