Point Cloud Library (PCL)
1.7.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$ 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