Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/surface/include/pcl/surface/gp3.h
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2011, Willow Garage, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id$
00037  *
00038  */
00039 
00040 #ifndef PCL_GP3_H_
00041 #define PCL_GP3_H_
00042 
00043 // PCL includes
00044 #include <pcl/surface/reconstruction.h>
00045 #include <pcl/surface/boost.h>
00046 
00047 #include <pcl/conversions.h>
00048 #include <pcl/kdtree/kdtree.h>
00049 #include <pcl/kdtree/kdtree_flann.h>
00050 #include <pcl/PolygonMesh.h>
00051 
00052 #include <fstream>
00053 #include <iostream>
00054 
00055 
00056 
00057 namespace pcl
00058 {
00059   /** \brief Returns if a point X is visible from point R (or the origin)
00060     * when taking into account the segment between the points S1 and S2
00061     * \param X 2D coordinate of the point
00062     * \param S1 2D coordinate of the segment's first point
00063     * \param S2 2D coordinate of the segment's secont point
00064     * \param R 2D coorddinate of the reference point (defaults to 0,0)
00065     * \ingroup surface
00066     */
00067   inline bool 
00068   isVisible (const Eigen::Vector2f &X, const Eigen::Vector2f &S1, const Eigen::Vector2f &S2, 
00069              const Eigen::Vector2f &R = Eigen::Vector2f::Zero ())
00070   {
00071     double a0 = S1[1] - S2[1];
00072     double b0 = S2[0] - S1[0];
00073     double c0 = S1[0]*S2[1] - S2[0]*S1[1];
00074     double a1 = -X[1];
00075     double b1 = X[0];
00076     double c1 = 0;
00077     if (R != Eigen::Vector2f::Zero())
00078     {
00079       a1 += R[1];
00080       b1 -= R[0];
00081       c1 = R[0]*X[1] - X[0]*R[1];
00082     }
00083     double div = a0*b1 - b0*a1;
00084     double x = (b0*c1 - b1*c0) / div;
00085     double y = (a1*c0 - a0*c1) / div;
00086 
00087     bool intersection_outside_XR;
00088     if (R == Eigen::Vector2f::Zero())
00089     {
00090       if (X[0] > 0)
00091         intersection_outside_XR = (x <= 0) || (x >= X[0]);
00092       else if (X[0] < 0)
00093         intersection_outside_XR = (x >= 0) || (x <= X[0]);
00094       else if (X[1] > 0)
00095         intersection_outside_XR = (y <= 0) || (y >= X[1]);
00096       else if (X[1] < 0)
00097         intersection_outside_XR = (y >= 0) || (y <= X[1]);
00098       else
00099         intersection_outside_XR = true;
00100     }
00101     else
00102     {
00103       if (X[0] > R[0])
00104         intersection_outside_XR = (x <= R[0]) || (x >= X[0]);
00105       else if (X[0] < R[0])
00106         intersection_outside_XR = (x >= R[0]) || (x <= X[0]);
00107       else if (X[1] > R[1])
00108         intersection_outside_XR = (y <= R[1]) || (y >= X[1]);
00109       else if (X[1] < R[1])
00110         intersection_outside_XR = (y >= R[1]) || (y <= X[1]);
00111       else
00112         intersection_outside_XR = true;
00113     }
00114     if (intersection_outside_XR)
00115       return true;
00116     else
00117     {
00118       if (S1[0] > S2[0])
00119         return (x <= S2[0]) || (x >= S1[0]);
00120       else if (S1[0] < S2[0])
00121         return (x >= S2[0]) || (x <= S1[0]);
00122       else if (S1[1] > S2[1])
00123         return (y <= S2[1]) || (y >= S1[1]);
00124       else if (S1[1] < S2[1])                                                                                                                     
00125         return (y >= S2[1]) || (y <= S1[1]);
00126       else
00127         return false;
00128     }
00129   }  
00130 
00131   /** \brief GreedyProjectionTriangulation is an implementation of a greedy triangulation algorithm for 3D points
00132     * based on local 2D projections. It assumes locally smooth surfaces and relatively smooth transitions between
00133     * areas with different point densities.
00134     * \author Zoltan Csaba Marton
00135     * \ingroup surface
00136     */
00137   template <typename PointInT>
00138   class GreedyProjectionTriangulation : public MeshConstruction<PointInT>
00139   {
00140     public:
00141       typedef boost::shared_ptr<GreedyProjectionTriangulation<PointInT> > Ptr;
00142       typedef boost::shared_ptr<const GreedyProjectionTriangulation<PointInT> > ConstPtr;
00143 
00144       using MeshConstruction<PointInT>::tree_;
00145       using MeshConstruction<PointInT>::input_;
00146       using MeshConstruction<PointInT>::indices_;
00147 
00148       typedef typename pcl::KdTree<PointInT> KdTree;
00149       typedef typename pcl::KdTree<PointInT>::Ptr KdTreePtr;
00150 
00151       typedef pcl::PointCloud<PointInT> PointCloudIn;
00152       typedef typename PointCloudIn::Ptr PointCloudInPtr;
00153       typedef typename PointCloudIn::ConstPtr PointCloudInConstPtr;
00154 
00155       enum GP3Type
00156       { 
00157         NONE = -1,    // not-defined
00158         FREE = 0,    
00159         FRINGE = 1,  
00160         BOUNDARY = 2,
00161         COMPLETED = 3
00162       };
00163     
00164       /** \brief Empty constructor. */
00165       GreedyProjectionTriangulation () : 
00166         mu_ (0), 
00167         search_radius_ (0), // must be set by user
00168         nnn_ (100),
00169         minimum_angle_ (M_PI/18), // 10 degrees
00170         maximum_angle_ (2*M_PI/3), // 120 degrees
00171         eps_angle_(M_PI/4), //45 degrees,
00172         consistent_(false), 
00173         consistent_ordering_ (false),
00174         triangle_ (),
00175         coords_ (),
00176         angles_ (),
00177         R_ (),
00178         state_ (),
00179         source_ (),
00180         ffn_ (),
00181         sfn_ (),
00182         part_ (),
00183         fringe_queue_ (),
00184         is_current_free_ (false),
00185         current_index_ (),
00186         prev_is_ffn_ (false),
00187         prev_is_sfn_ (false),
00188         next_is_ffn_ (false),
00189         next_is_sfn_ (false),
00190         changed_1st_fn_ (false),
00191         changed_2nd_fn_ (false),
00192         new2boundary_ (),
00193         already_connected_ (false),
00194         proj_qp_ (),
00195         u_ (),
00196         v_ (),
00197         uvn_ffn_ (),
00198         uvn_sfn_ (),
00199         uvn_next_ffn_ (),
00200         uvn_next_sfn_ (),
00201         tmp_ ()
00202       {};
00203 
00204       /** \brief Set the multiplier of the nearest neighbor distance to obtain the final search radius for each point
00205        *  (this will make the algorithm adapt to different point densities in the cloud).
00206         * \param[in] mu the multiplier
00207         */
00208       inline void 
00209       setMu (double mu) { mu_ = mu; }
00210 
00211       /** \brief Get the nearest neighbor distance multiplier. */
00212       inline double 
00213       getMu () const { return (mu_); }
00214 
00215       /** \brief Set the maximum number of nearest neighbors to be searched for.
00216         * \param[in] nnn the maximum number of nearest neighbors
00217         */
00218       inline void 
00219       setMaximumNearestNeighbors (int nnn) { nnn_ = nnn; }
00220 
00221       /** \brief Get the maximum number of nearest neighbors to be searched for. */
00222       inline int 
00223       getMaximumNearestNeighbors () const { return (nnn_); }
00224 
00225       /** \brief Set the sphere radius that is to be used for determining the k-nearest neighbors used for triangulating.
00226         * \param[in] radius the sphere radius that is to contain all k-nearest neighbors
00227         * \note This distance limits the maximum edge length!
00228         */
00229       inline void 
00230       setSearchRadius (double radius) { search_radius_ = radius; }
00231 
00232       /** \brief Get the sphere radius used for determining the k-nearest neighbors. */
00233       inline double 
00234       getSearchRadius () const { return (search_radius_); }
00235 
00236       /** \brief Set the minimum angle each triangle should have.
00237         * \param[in] minimum_angle the minimum angle each triangle should have
00238         * \note As this is a greedy approach, this will have to be violated from time to time
00239         */
00240       inline void 
00241       setMinimumAngle (double minimum_angle) { minimum_angle_ = minimum_angle; }
00242 
00243       /** \brief Get the parameter for distance based weighting of neighbors. */
00244       inline double 
00245       getMinimumAngle () const { return (minimum_angle_); }
00246 
00247       /** \brief Set the maximum angle each triangle can have.
00248         * \param[in] maximum_angle the maximum angle each triangle can have
00249         * \note For best results, its value should be around 120 degrees
00250         */
00251       inline void 
00252       setMaximumAngle (double maximum_angle) { maximum_angle_ = maximum_angle; }
00253 
00254       /** \brief Get the parameter for distance based weighting of neighbors. */
00255       inline double 
00256       getMaximumAngle () const { return (maximum_angle_); }
00257 
00258       /** \brief Don't consider points for triangulation if their normal deviates more than this value from the query point's normal.
00259         * \param[in] eps_angle maximum surface angle
00260         * \note As normal estimation methods usually give smooth transitions at sharp edges, this ensures correct triangulation
00261         *       by avoiding connecting points from one side to points from the other through forcing the use of the edge points.
00262         */
00263       inline void 
00264       setMaximumSurfaceAngle (double eps_angle) { eps_angle_ = eps_angle; }
00265 
00266       /** \brief Get the maximum surface angle. */
00267       inline double 
00268       getMaximumSurfaceAngle () const { return (eps_angle_); }
00269 
00270       /** \brief Set the flag if the input normals are oriented consistently.
00271         * \param[in] consistent set it to true if the normals are consistently oriented
00272         */
00273       inline void 
00274       setNormalConsistency (bool consistent) { consistent_ = consistent; }
00275 
00276       /** \brief Get the flag for consistently oriented normals. */
00277       inline bool 
00278       getNormalConsistency () const { return (consistent_); }
00279 
00280       /** \brief Set the flag to order the resulting triangle vertices consistently (positive direction around normal).
00281         * @note Assumes consistently oriented normals (towards the viewpoint) -- see setNormalConsistency ()
00282         * \param[in] consistent_ordering set it to true if triangle vertices should be ordered consistently
00283         */
00284       inline void 
00285       setConsistentVertexOrdering (bool consistent_ordering) { consistent_ordering_ = consistent_ordering; }
00286 
00287       /** \brief Get the flag signaling consistently ordered triangle vertices. */
00288       inline bool 
00289       getConsistentVertexOrdering () const { return (consistent_ordering_); }
00290 
00291       /** \brief Get the state of each point after reconstruction.
00292         * \note Options are defined as constants: FREE, FRINGE, COMPLETED, BOUNDARY and NONE
00293         */
00294       inline std::vector<int> 
00295       getPointStates () const { return (state_); }
00296 
00297       /** \brief Get the ID of each point after reconstruction.
00298         * \note parts are numbered from 0, a -1 denotes unconnected points
00299         */
00300       inline std::vector<int> 
00301       getPartIDs () const { return (part_); }
00302 
00303 
00304       /** \brief Get the sfn list. */
00305       inline std::vector<int>
00306       getSFN () const { return (sfn_); }
00307 
00308       /** \brief Get the ffn list. */
00309       inline std::vector<int>
00310       getFFN () const { return (ffn_); }
00311 
00312     protected:
00313       /** \brief The nearest neighbor distance multiplier to obtain the final search radius. */
00314       double mu_;
00315 
00316       /** \brief The nearest neighbors search radius for each point and the maximum edge length. */
00317       double search_radius_;
00318 
00319       /** \brief The maximum number of nearest neighbors accepted by searching. */
00320       int nnn_;
00321 
00322       /** \brief The preferred minimum angle for the triangles. */
00323       double minimum_angle_;
00324 
00325       /** \brief The maximum angle for the triangles. */
00326       double maximum_angle_;
00327 
00328       /** \brief Maximum surface angle. */
00329       double eps_angle_;
00330 
00331       /** \brief Set this to true if the normals of the input are consistently oriented. */
00332       bool consistent_;
00333       
00334       /** \brief Set this to true if the output triangle vertices should be consistently oriented. */
00335       bool consistent_ordering_;
00336 
00337      private:
00338       /** \brief Struct for storing the angles to nearest neighbors **/
00339       struct nnAngle
00340       {
00341         double angle;
00342         int index;
00343         int nnIndex;
00344         bool visible;
00345       };
00346 
00347       /** \brief Struct for storing the edges starting from a fringe point **/
00348       struct doubleEdge
00349       {
00350         doubleEdge () : index (0), first (), second () {}
00351         int index;
00352         Eigen::Vector2f first;
00353         Eigen::Vector2f second;
00354       };
00355 
00356       // Variables made global to decrease the number of parameters to helper functions
00357 
00358       /** \brief Temporary variable to store a triangle (as a set of point indices) **/
00359       pcl::Vertices triangle_;
00360       /** \brief Temporary variable to store point coordinates **/
00361       std::vector<Eigen::Vector3f> coords_;
00362 
00363       /** \brief A list of angles to neighbors **/
00364       std::vector<nnAngle> angles_;
00365       /** \brief Index of the current query point **/
00366       int R_;
00367       /** \brief List of point states **/
00368       std::vector<int> state_;
00369       /** \brief List of sources **/
00370       std::vector<int> source_;
00371       /** \brief List of fringe neighbors in one direction **/
00372       std::vector<int> ffn_;
00373       /** \brief List of fringe neighbors in other direction **/
00374       std::vector<int> sfn_;
00375       /** \brief Connected component labels for each point **/
00376       std::vector<int> part_;
00377       /** \brief Points on the outer edge from which the mesh has to be grown **/
00378       std::vector<int> fringe_queue_;
00379 
00380       /** \brief Flag to set if the current point is free **/
00381       bool is_current_free_;
00382       /** \brief Current point's index **/
00383       int current_index_;
00384       /** \brief Flag to set if the previous point is the first fringe neighbor **/
00385       bool prev_is_ffn_;
00386       /** \brief Flag to set if the next point is the second fringe neighbor **/
00387       bool prev_is_sfn_;
00388       /** \brief Flag to set if the next point is the first fringe neighbor **/
00389       bool next_is_ffn_;
00390       /** \brief Flag to set if the next point is the second fringe neighbor **/
00391       bool next_is_sfn_;
00392       /** \brief Flag to set if the first fringe neighbor was changed **/
00393       bool changed_1st_fn_;
00394       /** \brief Flag to set if the second fringe neighbor was changed **/
00395       bool changed_2nd_fn_;
00396       /** \brief New boundary point **/
00397       int new2boundary_;
00398       
00399       /** \brief Flag to set if the next neighbor was already connected in the previous step.
00400         * To avoid inconsistency it should not be connected again.
00401         */
00402       bool already_connected_; 
00403 
00404       /** \brief Point coordinates projected onto the plane defined by the point normal **/
00405       Eigen::Vector3f proj_qp_;
00406       /** \brief First coordinate vector of the 2D coordinate frame **/
00407       Eigen::Vector3f u_;
00408       /** \brief Second coordinate vector of the 2D coordinate frame **/
00409       Eigen::Vector3f v_;
00410       /** \brief 2D coordinates of the first fringe neighbor **/
00411       Eigen::Vector2f uvn_ffn_;
00412       /** \brief 2D coordinates of the second fringe neighbor **/
00413       Eigen::Vector2f uvn_sfn_;
00414       /** \brief 2D coordinates of the first fringe neighbor of the next point **/
00415       Eigen::Vector2f uvn_next_ffn_;
00416       /** \brief 2D coordinates of the second fringe neighbor of the next point **/
00417       Eigen::Vector2f uvn_next_sfn_;
00418 
00419       /** \brief Temporary variable to store 3 coordiantes **/
00420       Eigen::Vector3f tmp_;
00421 
00422       /** \brief The actual surface reconstruction method.
00423         * \param[out] output the resultant polygonal mesh
00424         */
00425       void 
00426       performReconstruction (pcl::PolygonMesh &output);
00427 
00428       /** \brief The actual surface reconstruction method.
00429         * \param[out] polygons the resultant polygons, as a set of vertices. The Vertices structure contains an array of point indices.
00430         */
00431       void 
00432       performReconstruction (std::vector<pcl::Vertices> &polygons);
00433 
00434       /** \brief The actual surface reconstruction method.
00435         * \param[out] polygons the resultant polygons, as a set of vertices. The Vertices structure contains an array of point indices.
00436         */
00437       bool
00438       reconstructPolygons (std::vector<pcl::Vertices> &polygons);
00439 
00440       /** \brief Class get name method. */
00441       std::string 
00442       getClassName () const { return ("GreedyProjectionTriangulation"); }
00443 
00444       /** \brief Forms a new triangle by connecting the current neighbor to the query point 
00445         * and the previous neighbor
00446         * \param[out] polygons the polygon mesh to be updated
00447         * \param[in] prev_index index of the previous point
00448         * \param[in] next_index index of the next point
00449         * \param[in] next_next_index index of the point after the next one
00450         * \param[in] uvn_current 2D coordinate of the current point
00451         * \param[in] uvn_prev 2D coordinates of the previous point
00452         * \param[in] uvn_next 2D coordinates of the next point
00453         */
00454       void 
00455       connectPoint (std::vector<pcl::Vertices> &polygons, 
00456                     const int prev_index, 
00457                     const int next_index, 
00458                     const int next_next_index, 
00459                     const Eigen::Vector2f &uvn_current, 
00460                     const Eigen::Vector2f &uvn_prev, 
00461                     const Eigen::Vector2f &uvn_next);
00462 
00463       /** \brief Whenever a query point is part of a boundary loop containing 3 points, that triangle is created
00464         * (called if angle constraints make it possible)
00465         * \param[out] polygons the polygon mesh to be updated
00466         */
00467       void 
00468       closeTriangle (std::vector<pcl::Vertices> &polygons);
00469 
00470       /** \brief Get the list of containing triangles for each vertex in a PolygonMesh
00471         * \param[in] polygonMesh the input polygon mesh
00472         */
00473       std::vector<std::vector<size_t> >
00474       getTriangleList (const pcl::PolygonMesh &input);
00475 
00476       /** \brief Add a new triangle to the current polygon mesh
00477         * \param[in] a index of the first vertex
00478         * \param[in] b index of the second vertex
00479         * \param[in] c index of the third vertex
00480         * \param[out] polygons the polygon mesh to be updated
00481         */
00482       inline void
00483       addTriangle (int a, int b, int c, std::vector<pcl::Vertices> &polygons)
00484       {
00485         triangle_.vertices.resize (3);
00486         if (consistent_ordering_)
00487         {
00488           const PointInT p = input_->at (indices_->at (a));
00489           const Eigen::Vector3f pv = p.getVector3fMap ();
00490           if (p.getNormalVector3fMap ().dot (
00491                 (pv - input_->at (indices_->at (b)).getVector3fMap ()).cross (
00492                  pv - input_->at (indices_->at (c)).getVector3fMap ()) ) > 0)
00493           {
00494             triangle_.vertices[0] = a;
00495             triangle_.vertices[1] = b;
00496             triangle_.vertices[2] = c;
00497           }
00498           else
00499           {
00500             triangle_.vertices[0] = a;
00501             triangle_.vertices[1] = c;
00502             triangle_.vertices[2] = b;
00503           }
00504         }
00505         else
00506         {
00507           triangle_.vertices[0] = a;
00508           triangle_.vertices[1] = b;
00509           triangle_.vertices[2] = c;
00510         }
00511         polygons.push_back (triangle_);
00512       }
00513 
00514       /** \brief Add a new vertex to the advancing edge front and set its source point
00515         * \param[in] v index of the vertex that was connected
00516         * \param[in] s index of the source point
00517         */
00518       inline void
00519       addFringePoint (int v, int s)
00520       {
00521         source_[v] = s;
00522         part_[v] = part_[s];
00523         fringe_queue_.push_back(v);
00524       }
00525 
00526       /** \brief Function for ascending sort of nnAngle, taking visibility into account
00527         * (angles to visible neighbors will be first, to the invisible ones after).
00528         * \param[in] a1 the first angle
00529         * \param[in] a2 the second angle
00530         */
00531       static inline bool 
00532       nnAngleSortAsc (const nnAngle& a1, const nnAngle& a2)
00533       {
00534         if (a1.visible == a2.visible)
00535           return (a1.angle < a2.angle);
00536         else
00537           return a1.visible;
00538       }
00539   };
00540 
00541 } // namespace pcl
00542 
00543 #ifdef PCL_NO_PRECOMPILE
00544 #include <pcl/surface/impl/gp3.hpp>
00545 #endif
00546 
00547 #endif  //#ifndef PCL_GP3_H_
00548