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


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:24:33