Point Cloud Library (PCL)
1.3.1
|
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