Point Cloud Library (PCL)  1.3.1
gp3.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: gp3.h 3031 2011-11-01 04:25:15Z rusu $
00035  *
00036  */
00037 
00038 #ifndef PCL_GP3_H_
00039 #define PCL_GP3_H_
00040 
00041 // PCL includes
00042 #include "pcl/surface/reconstruction.h"
00043 
00044 #include "pcl/ros/conversions.h"
00045 #include "pcl/kdtree/kdtree.h"
00046 #include <pcl/kdtree/kdtree_flann.h>
00047 #include <pcl/PolygonMesh.h>
00048 #include <pcl/TextureMesh.h>
00049 #include <boost/function.hpp>
00050 
00051 #include <fstream>
00052 #include <iostream>
00053 
00054 // add by ktran to export update function
00055 #include <pcl/pcl_macros.h>
00056 #include <pcl/point_types.h>
00057 
00058 
00059 namespace pcl
00060 {
00061   // add by ktran for Kdtree_flaan search
00062   struct SearchPoint : public PointXYZ
00063   {
00064     SearchPoint(float x, float y, float z) {this->x=x; this->y=y; this->z=z;}
00065   };
00066 
00075   inline bool 
00076   isVisible (const Eigen::Vector2f &X, const Eigen::Vector2f &S1, const Eigen::Vector2f &S2, 
00077              const Eigen::Vector2f &R = Eigen::Vector2f::Zero ())
00078   {
00079     double a0 = S1[1] - S2[1];
00080     double b0 = S2[0] - S1[0];
00081     double c0 = S1[0]*S2[1] - S2[0]*S1[1];
00082     double a1 = -X[1];
00083     double b1 = X[0];
00084     double c1 = 0;
00085     if (R != Eigen::Vector2f::Zero())
00086     {
00087       a1 += R[1];
00088       b1 -= R[0];
00089       c1 = R[0]*X[1] - X[0]*R[1];
00090     }
00091     double div = a0*b1 - b0*a1;
00092     double x = (b0*c1 - b1*c0) / div;
00093     double y = (a1*c0 - a0*c1) / div;
00094 
00095     bool intersection_outside_XR;
00096     if (R == Eigen::Vector2f::Zero())
00097     {
00098       if (X[0] > 0)
00099         intersection_outside_XR = (x <= 0) || (x >= X[0]);
00100       else if (X[0] < 0)
00101         intersection_outside_XR = (x >= 0) || (x <= X[0]);
00102       else if (X[1] > 0)
00103         intersection_outside_XR = (y <= 0) || (y >= X[1]);
00104       else if (X[1] < 0)
00105         intersection_outside_XR = (y >= 0) || (y <= X[1]);
00106       else
00107         intersection_outside_XR = true;
00108     }
00109     else
00110     {
00111       if (X[0] > R[0])
00112         intersection_outside_XR = (x <= R[0]) || (x >= X[0]);
00113       else if (X[0] < R[0])
00114         intersection_outside_XR = (x >= R[0]) || (x <= X[0]);
00115       else if (X[1] > R[1])
00116         intersection_outside_XR = (y <= R[1]) || (y >= X[1]);
00117       else if (X[1] < R[1])
00118         intersection_outside_XR = (y >= R[1]) || (y <= X[1]);
00119       else
00120         intersection_outside_XR = true;
00121     }
00122     if (intersection_outside_XR)
00123       return true;
00124     else
00125     {
00126       if (S1[0] > S2[0])
00127         return (x <= S2[0]) || (x >= S1[0]);
00128       else if (S1[0] < S2[0])
00129         return (x >= S2[0]) || (x <= S1[0]);
00130       else if (S1[1] > S2[1])
00131         return (y <= S2[1]) || (y >= S1[1]);
00132       else if (S1[1] < S2[1])                                                                                                                     
00133         return (y >= S2[1]) || (y <= S1[1]);
00134       else
00135         return false;
00136     }
00137   }  
00138 
00145   template <typename PointInT>
00146   class GreedyProjectionTriangulation : public SurfaceReconstruction<PointInT>
00147   {
00148     public:
00149       using SurfaceReconstruction<PointInT>::tree_;
00150       using SurfaceReconstruction<PointInT>::input_;
00151       using SurfaceReconstruction<PointInT>::indices_;
00152 
00153       typedef typename pcl::KdTree<PointInT> KdTree;
00154       typedef typename pcl::KdTree<PointInT>::Ptr KdTreePtr;
00155 
00156       typedef pcl::PointCloud<PointInT> PointCloudIn;
00157       typedef typename PointCloudIn::Ptr PointCloudInPtr;
00158       typedef typename PointCloudIn::ConstPtr PointCloudInConstPtr;
00159 
00160       // FIXME this enum should have a type.  Not be anonymous. 
00161       // Otherplaces where consts are used probably should be fixed.
00162       enum { 
00163         NONE = -1,    // not-defined
00164         FREE = 0,    
00165         FRINGE = 1,  
00166         BOUNDARY = 2,
00167         COMPLETED = 3,
00168       };
00169     
00171       GreedyProjectionTriangulation () : nnn_ (0), mu_ (0), search_radius_ (0), 
00172                                          minimum_angle_ (0), maximum_angle_ (0), 
00173                                          eps_angle_(0), consistent_(false) 
00174       {};
00175 
00180       inline void 
00181       setMu (double mu) { mu_ = mu; }
00182 
00184       inline double 
00185       getMu () { return (mu_); }
00186 
00190       inline void 
00191       setMaximumNearestNeighbors (int nnn) { nnn_ = nnn; }
00192 
00194       inline int 
00195       getMaximumNearestNeighbors () { return (nnn_); }
00196 
00201       inline void 
00202       setSearchRadius (double radius) { search_radius_ = radius; }
00203 
00205       inline double 
00206       getSearchRadius () { return (search_radius_); }
00207 
00212       inline void 
00213       setMinimumAngle (double minimum_angle) { minimum_angle_ = minimum_angle; }
00214 
00216       inline double 
00217       getMinimumAngle () { return (minimum_angle_); }
00218 
00223       inline void 
00224       setMaximumAngle (double maximum_angle) { maximum_angle_ = maximum_angle; }
00225 
00227       inline double 
00228       getMaximumAngle () { return (maximum_angle_); }
00229 
00235       inline void 
00236       setMaximumSurfaceAngle (double eps_angle) { eps_angle_ = eps_angle; }
00237 
00239       inline double 
00240       getMaximumSurfaceAngle () { return (eps_angle_); }
00241 
00245       inline void 
00246       setNormalConsistency (bool consistent) { consistent_ = consistent; }
00247 
00249       inline bool 
00250       getNormalConsistency () { return (consistent_); }
00251 
00255       inline std::vector<int> 
00256       getPointStates () { return (state_); }
00257 
00261       inline std::vector<int> 
00262       getPartIDs () { return (part_); }
00263 
00264 
00265       // add by ktran for update & merge meshes
00268       inline std::vector<int>
00269       getSFN () { return (sfn_); }
00270 
00273       inline std::vector<int>
00274       getFFN () { return (ffn_); }
00275 
00279       void
00280       updateMesh (const PointCloudInConstPtr &update, pcl::PolygonMesh &output);
00281 
00285       void
00286       updateMesh (const PointCloudInConstPtr &update, pcl::PolygonMesh &output, pcl::TextureMesh &tex_mesh);
00287 
00291       void
00292       merge2Meshes (pcl::PolygonMesh &mesh1, pcl::PolygonMesh &mesh2, std::vector<int> state2, std::vector<int> sfn2, std::vector<int> ffn2);
00293 
00297       void
00298       removeOverlapTriangles (pcl::PolygonMesh &mesh1, pcl::PolygonMesh &mesh2);
00299 
00300     protected:
00302       int nnn_;
00303 
00305       double mu_;
00306 
00308       double search_radius_;
00309 
00311       double minimum_angle_;
00312 
00314       double maximum_angle_;
00315 
00317       double eps_angle_;
00318 
00320       bool consistent_;
00321 
00322     private:
00324       struct nnAngle
00325       {
00326         double angle;
00327         int index;
00328         int nnIndex;
00329         bool visible;
00330       };
00331 
00333       struct doubleEdge
00334       {
00335         int index;
00336         Eigen::Vector2f first;
00337         Eigen::Vector2f second;
00338       };
00339 
00340       // Variables made global to decrease the number of parameters to helper functions
00341 
00343       pcl::Vertices triangle_;
00345       std::vector<Eigen::Vector3f> coords_;
00346 
00348       std::vector<nnAngle> angles_;
00350       int R_;
00352       std::vector<int> state_;
00354       std::vector<int> source_;
00356       std::vector<int> ffn_;
00358       std::vector<int> sfn_;
00360       std::vector<int> part_;
00362       std::vector<int> fringe_queue_;
00363 
00365       bool is_current_free_;
00367       int current_index_;
00369       bool prev_is_ffn_;
00371       bool prev_is_sfn_;
00373       bool next_is_ffn_;
00375       bool next_is_sfn_;
00377       bool changed_1st_fn_;
00379       bool changed_2nd_fn_;
00381       int new2boundary_;
00382       
00386       bool already_connected_; 
00387 
00389       Eigen::Vector3f proj_qp_;
00391       Eigen::Vector3f u_;
00393       Eigen::Vector3f v_;
00395       Eigen::Vector2f uvn_ffn_;
00397       Eigen::Vector2f uvn_sfn_;
00399       Eigen::Vector2f uvn_next_ffn_;
00401       Eigen::Vector2f uvn_next_sfn_;
00402 
00404       Eigen::Vector3f tmp_;
00405 
00409       void 
00410       performReconstruction (pcl::PolygonMesh &output);
00411 
00413       std::string 
00414       getClassName () const { return ("GreedyProjectionTriangulation"); }
00415 
00426       void 
00427       connectPoint (pcl::PolygonMesh &output, 
00428                     const int prev_index, 
00429                     const int next_index, 
00430                     const int next_next_index, 
00431                     const Eigen::Vector2f &uvn_current, 
00432                     const Eigen::Vector2f &uvn_prev, 
00433                     const Eigen::Vector2f &uvn_next);
00434 
00439       void 
00440       closeTriangle (pcl::PolygonMesh &output);
00441 
00442 
00443       // add by ktran
00447       std::vector<std::vector<size_t> >
00448       getTriangleList (pcl::PolygonMesh input);
00449 
00450 
00457       inline void
00458       addTriangle (int a, int b, int c, pcl::PolygonMesh &output)
00459       {
00460         triangle_.vertices.clear ();
00461         triangle_.vertices.push_back (a);
00462         triangle_.vertices.push_back (b);
00463         triangle_.vertices.push_back (c);
00464         output.polygons.push_back(triangle_);
00465       }
00466 
00471       inline void
00472       addFringePoint (int v, int s)
00473       {
00474         source_[v] = s;
00475         part_[v] = part_[s];
00476         fringe_queue_.push_back(v);
00477       }
00478 
00484       static inline bool 
00485       nnAngleSortAsc (const nnAngle& a1, const nnAngle& a2)
00486       {
00487         if (a1.visible == a2.visible)
00488           return (a1.angle < a2.angle);
00489         else
00490           return a1.visible;
00491       }
00492   };
00493 
00494 } // namespace pcl
00495 
00496 #endif  //#ifndef PCL_GP3_H_
00497 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines