|
Point Cloud Library (PCL)
1.6.0
|
|
Point Cloud Library (PCL)
1.6.0
|
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: gp3.h 5124 2012-03-16 03:09:41Z rusu $ 00037 * 00038 */ 00039 00040 #ifndef PCL_GP3_H_ 00041 #define PCL_GP3_H_ 00042 00043 // PCL includes 00044 #include <pcl/surface/reconstruction.h> 00045 00046 #include <pcl/ros/conversions.h> 00047 #include <pcl/kdtree/kdtree.h> 00048 #include <pcl/kdtree/kdtree_flann.h> 00049 #include <pcl/PolygonMesh.h> 00050 #include <pcl/TextureMesh.h> 00051 #include <boost/function.hpp> 00052 00053 #include <fstream> 00054 #include <iostream> 00055 00056 // add by ktran to export update function 00057 #include <pcl/pcl_macros.h> 00058 #include <pcl/point_types.h> 00059 00060 00061 namespace pcl 00062 { 00071 inline bool 00072 isVisible (const Eigen::Vector2f &X, const Eigen::Vector2f &S1, const Eigen::Vector2f &S2, 00073 const Eigen::Vector2f &R = Eigen::Vector2f::Zero ()) 00074 { 00075 double a0 = S1[1] - S2[1]; 00076 double b0 = S2[0] - S1[0]; 00077 double c0 = S1[0]*S2[1] - S2[0]*S1[1]; 00078 double a1 = -X[1]; 00079 double b1 = X[0]; 00080 double c1 = 0; 00081 if (R != Eigen::Vector2f::Zero()) 00082 { 00083 a1 += R[1]; 00084 b1 -= R[0]; 00085 c1 = R[0]*X[1] - X[0]*R[1]; 00086 } 00087 double div = a0*b1 - b0*a1; 00088 double x = (b0*c1 - b1*c0) / div; 00089 double y = (a1*c0 - a0*c1) / div; 00090 00091 bool intersection_outside_XR; 00092 if (R == Eigen::Vector2f::Zero()) 00093 { 00094 if (X[0] > 0) 00095 intersection_outside_XR = (x <= 0) || (x >= X[0]); 00096 else if (X[0] < 0) 00097 intersection_outside_XR = (x >= 0) || (x <= X[0]); 00098 else if (X[1] > 0) 00099 intersection_outside_XR = (y <= 0) || (y >= X[1]); 00100 else if (X[1] < 0) 00101 intersection_outside_XR = (y >= 0) || (y <= X[1]); 00102 else 00103 intersection_outside_XR = true; 00104 } 00105 else 00106 { 00107 if (X[0] > R[0]) 00108 intersection_outside_XR = (x <= R[0]) || (x >= X[0]); 00109 else if (X[0] < R[0]) 00110 intersection_outside_XR = (x >= R[0]) || (x <= X[0]); 00111 else if (X[1] > R[1]) 00112 intersection_outside_XR = (y <= R[1]) || (y >= X[1]); 00113 else if (X[1] < R[1]) 00114 intersection_outside_XR = (y >= R[1]) || (y <= X[1]); 00115 else 00116 intersection_outside_XR = true; 00117 } 00118 if (intersection_outside_XR) 00119 return true; 00120 else 00121 { 00122 if (S1[0] > S2[0]) 00123 return (x <= S2[0]) || (x >= S1[0]); 00124 else if (S1[0] < S2[0]) 00125 return (x >= S2[0]) || (x <= S1[0]); 00126 else if (S1[1] > S2[1]) 00127 return (y <= S2[1]) || (y >= S1[1]); 00128 else if (S1[1] < S2[1]) 00129 return (y >= S2[1]) || (y <= S1[1]); 00130 else 00131 return false; 00132 } 00133 } 00134 00141 template <typename PointInT> 00142 class GreedyProjectionTriangulation : public MeshConstruction<PointInT> 00143 { 00144 public: 00145 using MeshConstruction<PointInT>::tree_; 00146 using MeshConstruction<PointInT>::input_; 00147 using MeshConstruction<PointInT>::indices_; 00148 00149 typedef typename pcl::KdTree<PointInT> KdTree; 00150 typedef typename pcl::KdTree<PointInT>::Ptr KdTreePtr; 00151 00152 typedef pcl::PointCloud<PointInT> PointCloudIn; 00153 typedef typename PointCloudIn::Ptr PointCloudInPtr; 00154 typedef typename PointCloudIn::ConstPtr PointCloudInConstPtr; 00155 00156 // FIXME this enum should have a type. Not be anonymous. 00157 // Otherplaces where consts are used probably should be fixed. 00158 enum 00159 { 00160 NONE = -1, // not-defined 00161 FREE = 0, 00162 FRINGE = 1, 00163 BOUNDARY = 2, 00164 COMPLETED = 3 00165 }; 00166 00168 GreedyProjectionTriangulation () : 00169 mu_ (0), 00170 search_radius_ (0), // must be set by user 00171 nnn_ (100), 00172 minimum_angle_ (M_PI/18), // 10 degrees 00173 maximum_angle_ (2*M_PI/3), // 120 degrees 00174 eps_angle_(M_PI/4), //45 degrees, 00175 consistent_(false), 00176 consistent_ordering_ (false), 00177 triangle_ (), 00178 coords_ (), 00179 angles_ (), 00180 R_ (), 00181 state_ (), 00182 source_ (), 00183 ffn_ (), 00184 sfn_ (), 00185 part_ (), 00186 fringe_queue_ (), 00187 is_current_free_ (false), 00188 current_index_ (), 00189 prev_is_ffn_ (false), 00190 prev_is_sfn_ (false), 00191 next_is_ffn_ (false), 00192 next_is_sfn_ (false), 00193 changed_1st_fn_ (false), 00194 changed_2nd_fn_ (false), 00195 new2boundary_ (), 00196 already_connected_ (false), 00197 proj_qp_ (), 00198 u_ (), 00199 v_ (), 00200 uvn_ffn_ (), 00201 uvn_sfn_ (), 00202 uvn_next_ffn_ (), 00203 uvn_next_sfn_ (), 00204 tmp_ () 00205 {}; 00206 00211 inline void 00212 setMu (double mu) { mu_ = mu; } 00213 00215 inline double 00216 getMu () { return (mu_); } 00217 00221 inline void 00222 setMaximumNearestNeighbors (int nnn) { nnn_ = nnn; } 00223 00225 inline int 00226 getMaximumNearestNeighbors () { return (nnn_); } 00227 00232 inline void 00233 setSearchRadius (double radius) { search_radius_ = radius; } 00234 00236 inline double 00237 getSearchRadius () { return (search_radius_); } 00238 00243 inline void 00244 setMinimumAngle (double minimum_angle) { minimum_angle_ = minimum_angle; } 00245 00247 inline double 00248 getMinimumAngle () { return (minimum_angle_); } 00249 00254 inline void 00255 setMaximumAngle (double maximum_angle) { maximum_angle_ = maximum_angle; } 00256 00258 inline double 00259 getMaximumAngle () { return (maximum_angle_); } 00260 00266 inline void 00267 setMaximumSurfaceAngle (double eps_angle) { eps_angle_ = eps_angle; } 00268 00270 inline double 00271 getMaximumSurfaceAngle () { return (eps_angle_); } 00272 00276 inline void 00277 setNormalConsistency (bool consistent) { consistent_ = consistent; } 00278 00280 inline bool 00281 getNormalConsistency () { return (consistent_); } 00282 00287 inline void 00288 setConsistentVertexOrdering (bool consistent_ordering) { consistent_ordering_ = consistent_ordering; } 00289 00291 inline bool 00292 getConsistentVertexOrdering () { return (consistent_ordering_); } 00293 00297 inline std::vector<int> 00298 getPointStates () { return (state_); } 00299 00303 inline std::vector<int> 00304 getPartIDs () { return (part_); } 00305 00306 00308 inline std::vector<int> 00309 getSFN () { return (sfn_); } 00310 00312 inline std::vector<int> 00313 getFFN () { return (ffn_); } 00314 00315 protected: 00317 double mu_; 00318 00320 double search_radius_; 00321 00323 int nnn_; 00324 00326 double minimum_angle_; 00327 00329 double maximum_angle_; 00330 00332 double eps_angle_; 00333 00335 bool consistent_; 00336 00338 bool consistent_ordering_; 00339 00340 private: 00342 struct nnAngle 00343 { 00344 double angle; 00345 int index; 00346 int nnIndex; 00347 bool visible; 00348 }; 00349 00351 struct doubleEdge 00352 { 00353 doubleEdge () : index (0), first (), second () {} 00354 int index; 00355 Eigen::Vector2f first; 00356 Eigen::Vector2f second; 00357 }; 00358 00359 // Variables made global to decrease the number of parameters to helper functions 00360 00362 pcl::Vertices triangle_; 00364 std::vector<Eigen::Vector3f> coords_; 00365 00367 std::vector<nnAngle> angles_; 00369 int R_; 00371 std::vector<int> state_; 00373 std::vector<int> source_; 00375 std::vector<int> ffn_; 00377 std::vector<int> sfn_; 00379 std::vector<int> part_; 00381 std::vector<int> fringe_queue_; 00382 00384 bool is_current_free_; 00386 int current_index_; 00388 bool prev_is_ffn_; 00390 bool prev_is_sfn_; 00392 bool next_is_ffn_; 00394 bool next_is_sfn_; 00396 bool changed_1st_fn_; 00398 bool changed_2nd_fn_; 00400 int new2boundary_; 00401 00405 bool already_connected_; 00406 00408 Eigen::Vector3f proj_qp_; 00410 Eigen::Vector3f u_; 00412 Eigen::Vector3f v_; 00414 Eigen::Vector2f uvn_ffn_; 00416 Eigen::Vector2f uvn_sfn_; 00418 Eigen::Vector2f uvn_next_ffn_; 00420 Eigen::Vector2f uvn_next_sfn_; 00421 00423 Eigen::Vector3f tmp_; 00424 00428 void 00429 performReconstruction (pcl::PolygonMesh &output); 00430 00434 void 00435 performReconstruction (std::vector<pcl::Vertices> &polygons); 00436 00440 bool 00441 reconstructPolygons (std::vector<pcl::Vertices> &polygons); 00442 00444 std::string 00445 getClassName () const { return ("GreedyProjectionTriangulation"); } 00446 00457 void 00458 connectPoint (std::vector<pcl::Vertices> &polygons, 00459 const int prev_index, 00460 const int next_index, 00461 const int next_next_index, 00462 const Eigen::Vector2f &uvn_current, 00463 const Eigen::Vector2f &uvn_prev, 00464 const Eigen::Vector2f &uvn_next); 00465 00470 void 00471 closeTriangle (std::vector<pcl::Vertices> &polygons); 00472 00476 std::vector<std::vector<size_t> > 00477 getTriangleList (const pcl::PolygonMesh &input); 00478 00485 inline void 00486 addTriangle (int a, int b, int c, std::vector<pcl::Vertices> &polygons) 00487 { 00488 triangle_.vertices.resize (3); 00489 if (consistent_ordering_) 00490 { 00491 const PointInT p = input_->at (indices_->at (a)); 00492 const Eigen::Vector3f pv = p.getVector3fMap (); 00493 if (p.getNormalVector3fMap ().dot ( 00494 (pv - input_->at (indices_->at (b)).getVector3fMap ()).cross ( 00495 pv - input_->at (indices_->at (c)).getVector3fMap ()) ) > 0) 00496 { 00497 triangle_.vertices[0] = a; 00498 triangle_.vertices[1] = b; 00499 triangle_.vertices[2] = c; 00500 } 00501 else 00502 { 00503 triangle_.vertices[0] = a; 00504 triangle_.vertices[1] = c; 00505 triangle_.vertices[2] = b; 00506 } 00507 } 00508 else 00509 { 00510 triangle_.vertices[0] = a; 00511 triangle_.vertices[1] = b; 00512 triangle_.vertices[2] = c; 00513 } 00514 polygons.push_back (triangle_); 00515 } 00516 00521 inline void 00522 addFringePoint (int v, int s) 00523 { 00524 source_[v] = s; 00525 part_[v] = part_[s]; 00526 fringe_queue_.push_back(v); 00527 } 00528 00534 static inline bool 00535 nnAngleSortAsc (const nnAngle& a1, const nnAngle& a2) 00536 { 00537 if (a1.visible == a2.visible) 00538 return (a1.angle < a2.angle); 00539 else 00540 return a1.visible; 00541 } 00542 }; 00543 00544 } // namespace pcl 00545 00546 #endif //#ifndef PCL_GP3_H_ 00547
1.7.6.1
