Point Cloud Library (PCL)  1.3.1
gp3.hpp
Go to the documentation of this file.
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 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines