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: grid_projection.h 2745 2011-10-13 05:57:12Z rusu $ 00035 * 00036 */ 00037 00038 #ifndef PCL_SURFACE_GRID_PROJECTION_H_ 00039 #define PCL_SURFACE_GRID_PROJECTION_H_ 00040 00041 #include <pcl/surface/reconstruction.h> 00042 #include <boost/dynamic_bitset/dynamic_bitset.hpp> 00043 #include <boost/unordered_map.hpp> 00044 00045 namespace pcl 00046 { 00048 const int I_SHIFT_EP[12][2] = { 00049 {0, 4}, {1, 5}, {2, 6}, {3, 7}, 00050 {0, 1}, {1, 2}, {2, 3}, {3, 0}, 00051 {4, 5}, {5, 6}, {6, 7}, {7, 4} 00052 }; 00053 00054 const int I_SHIFT_PT[4] = { 00055 0, 4, 5, 7 00056 }; 00057 00058 const int I_SHIFT_EDGE[3][2] = { 00059 {0,1}, {1,3}, {1,2} 00060 }; 00061 00062 00076 template <typename PointNT> 00077 class GridProjection : public SurfaceReconstruction<PointNT> 00078 { 00079 public: 00080 using SurfaceReconstruction<PointNT>::input_; 00081 using SurfaceReconstruction<PointNT>::tree_; 00082 00083 typedef typename pcl::PointCloud<PointNT>::Ptr PointCloudPtr; 00084 00085 typedef typename pcl::KdTree<PointNT> KdTree; 00086 typedef typename pcl::KdTree<PointNT>::Ptr KdTreePtr; 00087 00089 struct Leaf 00090 { 00091 std::vector<int> data_indices; 00092 Eigen::Vector4f pt_on_surface; 00093 Eigen::Vector3f vect_at_grid_pt; 00094 }; 00095 00096 typedef boost::unordered_map<int, Leaf, boost::hash<int>, std::equal_to<int>, Eigen::aligned_allocator<int> > HashMap; 00097 00099 GridProjection (); 00100 00104 GridProjection (double in_resolution); 00105 00107 ~GridProjection (); 00108 00118 void 00119 performReconstruction (pcl::PolygonMesh &output); 00120 00124 inline void 00125 setResolution (double resolution) 00126 { 00127 leaf_size_ = resolution; 00128 } 00129 00130 inline double 00131 getResolution () const 00132 { 00133 return (leaf_size_); 00134 } 00135 00145 inline void 00146 setPaddingSize (int padding_size) 00147 { 00148 padding_size_ = padding_size; 00149 } 00150 inline int 00151 getPaddingSize () const 00152 { 00153 return (padding_size_); 00154 } 00155 00160 inline void 00161 setNearestNeighborNum (int k) 00162 { 00163 k_ = k; 00164 } 00165 inline int 00166 getNearestNeighborNum () const 00167 { 00168 return (k_); 00169 } 00170 00175 inline void 00176 setMaxBinarySearchLevel (int max_binary_search_level) 00177 { 00178 max_binary_search_level_ = max_binary_search_level; 00179 } 00180 inline int 00181 getMaxBinarySearchLevel () const 00182 { 00183 return (max_binary_search_level_); 00184 } 00185 00187 inline const HashMap& 00188 getCellHashMap () const 00189 { 00190 return (cell_hash_map_); 00191 } 00192 00193 inline const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> >& 00194 getVectorAtDataPoint () const 00195 { 00196 return (vector_at_data_point_); 00197 } 00198 00199 inline const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& 00200 getSurface () const 00201 { 00202 return (surface_); 00203 } 00204 00208 void 00209 getBoundingBox (); 00210 00211 protected: 00217 void 00218 scaleInputDataPoint (double scale_factor); 00219 00225 inline void 00226 getCellIndex (const Eigen::Vector4f &p, Eigen::Vector3i& index) const 00227 { 00228 for (int i = 0; i < 3; ++i) 00229 index[i] = (p[i] - min_p_(i))/leaf_size_; 00230 } 00231 00237 inline void 00238 getCellCenterFromIndex (const Eigen::Vector3i &index, Eigen::Vector4f ¢er) const 00239 { 00240 for (int i = 0; i < 3; ++i) 00241 center[i] = min_p_[i] + index[i] * leaf_size_ + leaf_size_/2; 00242 } 00243 00248 void 00249 getVertexFromCellCenter (const Eigen::Vector4f &cell_center, 00250 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &pts) const; 00251 00256 inline int 00257 getIndexIn1D (const Eigen::Vector3i &index) const 00258 { 00259 //assert(data_size_ > 0); 00260 return (index[0] * data_size_ * data_size_ + 00261 index[1] * data_size_ + index[2]); 00262 } 00263 00269 inline void 00270 getIndexIn3D (int index_1d, Eigen::Vector3i& index_3d) const 00271 { 00272 //assert(data_size_ > 0); 00273 index_3d[0] = index_1d / (data_size_ * data_size_); 00274 index_1d -= index_3d[0] * data_size_ * data_size_; 00275 index_3d[1] = index_1d / data_size_; 00276 index_1d -= index_3d[1] * data_size_; 00277 index_3d[2] = index_1d; 00278 } 00279 00284 void 00285 fillPad (const Eigen::Vector3i &index); 00286 00291 void 00292 getDataPtsUnion (const Eigen::Vector3i &index, std::vector <int> &pt_union_indices); 00293 00300 void 00301 createSurfaceForCell (const Eigen::Vector3i &index, std::vector <int> &pt_union_indices); 00302 00303 00311 void 00312 getProjection (const Eigen::Vector4f &p, std::vector<int> &pt_union_indices, Eigen::Vector4f &projection); 00313 00321 void 00322 getProjectionWithPlaneFit (const Eigen::Vector4f &p, 00323 std::vector<int> &pt_union_indices, 00324 Eigen::Vector4f &projection); 00325 00326 00332 void 00333 getVectorAtPoint (const Eigen::Vector4f &p, 00334 std::vector <int> &pt_union_indices, Eigen::Vector3f &vo); 00335 00343 void 00344 getVectorAtPointKNN (const Eigen::Vector4f &p, 00345 std::vector<int> &k_indices, 00346 std::vector<float> &k_squared_distances, 00347 Eigen::Vector3f &vo); 00348 00353 double 00354 getMagAtPoint (const Eigen::Vector4f &p, const std::vector <int> &pt_union_indices); 00355 00361 double 00362 getD1AtPoint (const Eigen::Vector4f &p, const Eigen::Vector3f &vec, 00363 const std::vector <int> &pt_union_indices); 00364 00370 double 00371 getD2AtPoint (const Eigen::Vector4f &p, const Eigen::Vector3f &vec, 00372 const std::vector <int> &pt_union_indices); 00373 00382 bool 00383 isIntersected (const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &end_pts, 00384 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts, 00385 std::vector <int> &pt_union_indices); 00386 00395 void 00396 findIntersection (int level, 00397 const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &end_pts, 00398 const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts, 00399 const Eigen::Vector4f &start_pt, 00400 std::vector<int> &pt_union_indices, 00401 Eigen::Vector4f &intersection); 00402 00417 void 00418 storeVectAndSurfacePoint (int index_1d, const Eigen::Vector3i &index_3d, 00419 std::vector<int> &pt_union_indices, const Leaf &cell_data); 00420 00434 void 00435 storeVectAndSurfacePointKNN (int index_1d, const Eigen::Vector3i &index_3d, const Leaf &cell_data); 00436 00437 private: 00439 HashMap cell_hash_map_; 00440 00442 Eigen::Vector4f min_p_, max_p_; 00443 00445 double leaf_size_; 00446 00448 double gaussian_scale_; 00449 00451 int data_size_; 00452 00454 int max_binary_search_level_; 00455 00457 int k_; 00458 00460 int padding_size_; 00461 00463 PointCloudPtr data_; 00464 00466 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > vector_at_data_point_; 00467 00469 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > surface_; 00470 00472 boost::dynamic_bitset<> occupied_cell_list_; 00473 00475 std::string getClassName () const { return ("GridProjection"); } 00476 00477 public: 00478 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00479 }; 00480 } 00481 00482 #endif // PCL_SURFACE_GRID_PROJECTION_H_ 00483