Point Cloud Library (PCL)
1.7.0
|
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$ 00035 * 00036 */ 00037 00038 #ifndef PCL_SURFACE_GRID_PROJECTION_H_ 00039 #define PCL_SURFACE_GRID_PROJECTION_H_ 00040 00041 #include <pcl/surface/boost.h> 00042 #include <pcl/surface/reconstruction.h> 00043 00044 namespace pcl 00045 { 00046 /** \brief The 12 edges of a cell. */ 00047 const int I_SHIFT_EP[12][2] = { 00048 {0, 4}, {1, 5}, {2, 6}, {3, 7}, 00049 {0, 1}, {1, 2}, {2, 3}, {3, 0}, 00050 {4, 5}, {5, 6}, {6, 7}, {7, 4} 00051 }; 00052 00053 const int I_SHIFT_PT[4] = { 00054 0, 4, 5, 7 00055 }; 00056 00057 const int I_SHIFT_EDGE[3][2] = { 00058 {0,1}, {1,3}, {1,2} 00059 }; 00060 00061 00062 /** \brief Grid projection surface reconstruction method. 00063 * \author Rosie Li 00064 * 00065 * \note If you use this code in any academic work, please cite: 00066 * - Ruosi Li, Lu Liu, Ly Phan, Sasakthi Abeysinghe, Cindy Grimm, Tao Ju. 00067 * Polygonizing extremal surfaces with manifold guarantees. 00068 * In Proceedings of the 14th ACM Symposium on Solid and Physical Modeling, 2010. 00069 * \ingroup surface 00070 */ 00071 template <typename PointNT> 00072 class GridProjection : public SurfaceReconstruction<PointNT> 00073 { 00074 public: 00075 typedef boost::shared_ptr<GridProjection<PointNT> > Ptr; 00076 typedef boost::shared_ptr<const GridProjection<PointNT> > ConstPtr; 00077 00078 using SurfaceReconstruction<PointNT>::input_; 00079 using SurfaceReconstruction<PointNT>::tree_; 00080 00081 typedef typename pcl::PointCloud<PointNT>::Ptr PointCloudPtr; 00082 00083 typedef typename pcl::KdTree<PointNT> KdTree; 00084 typedef typename pcl::KdTree<PointNT>::Ptr KdTreePtr; 00085 00086 /** \brief Data leaf. */ 00087 struct Leaf 00088 { 00089 Leaf () : data_indices (), pt_on_surface (), vect_at_grid_pt () {} 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 00098 /** \brief Constructor. */ 00099 GridProjection (); 00100 00101 /** \brief Constructor. 00102 * \param in_resolution set the resolution of the grid 00103 */ 00104 GridProjection (double in_resolution); 00105 00106 /** \brief Destructor. */ 00107 ~GridProjection (); 00108 00109 /** \brief Set the size of the grid cell 00110 * \param resolution the size of the grid cell 00111 */ 00112 inline void 00113 setResolution (double resolution) 00114 { 00115 leaf_size_ = resolution; 00116 } 00117 00118 inline double 00119 getResolution () const 00120 { 00121 return (leaf_size_); 00122 } 00123 00124 /** \brief When averaging the vectors, we find the union of all the input data 00125 * points within the padding area,and do a weighted average. Say if the padding 00126 * size is 1, when we process cell (x,y,z), we will find union of input data points 00127 * from (x-1) to (x+1), (y-1) to (y+1), (z-1) to (z+1)(in total, 27 cells). In this 00128 * way, even the cells itself doesnt contain any data points, we will stil process it 00129 * because there are data points in the padding area. This can help us fix holes which 00130 * is smaller than the padding size. 00131 * \param padding_size The num of padding cells we want to create 00132 */ 00133 inline void 00134 setPaddingSize (int padding_size) 00135 { 00136 padding_size_ = padding_size; 00137 } 00138 inline int 00139 getPaddingSize () const 00140 { 00141 return (padding_size_); 00142 } 00143 00144 /** \brief Set this only when using the k nearest neighbors search 00145 * instead of finding the point union 00146 * \param k The number of nearest neighbors we are looking for 00147 */ 00148 inline void 00149 setNearestNeighborNum (int k) 00150 { 00151 k_ = k; 00152 } 00153 inline int 00154 getNearestNeighborNum () const 00155 { 00156 return (k_); 00157 } 00158 00159 /** \brief Binary search is used in projection. given a point x, we find another point 00160 * which is 3*cell_size_ far away from x. Then we do a binary search between these 00161 * two points to find where the projected point should be. 00162 */ 00163 inline void 00164 setMaxBinarySearchLevel (int max_binary_search_level) 00165 { 00166 max_binary_search_level_ = max_binary_search_level; 00167 } 00168 inline int 00169 getMaxBinarySearchLevel () const 00170 { 00171 return (max_binary_search_level_); 00172 } 00173 00174 /////////////////////////////////////////////////////////// 00175 inline const HashMap& 00176 getCellHashMap () const 00177 { 00178 return (cell_hash_map_); 00179 } 00180 00181 inline const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> >& 00182 getVectorAtDataPoint () const 00183 { 00184 return (vector_at_data_point_); 00185 } 00186 00187 inline const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& 00188 getSurface () const 00189 { 00190 return (surface_); 00191 } 00192 00193 protected: 00194 /** \brief Get the bounding box for the input data points, also calculating the 00195 * cell size, and the gaussian scale factor 00196 */ 00197 void 00198 getBoundingBox (); 00199 00200 /** \brief The actual surface reconstruction method. 00201 * \param[out] polygons the resultant polygons, as a set of vertices. The Vertices structure contains an array of point indices. 00202 */ 00203 bool 00204 reconstructPolygons (std::vector<pcl::Vertices> &polygons); 00205 00206 /** \brief Create the surface. 00207 * 00208 * The 1st step is filling the padding, so that all the cells in the padding 00209 * area are in the hash map. The 2nd step is store the vector, and projected 00210 * point. The 3rd step is finding all the edges intersects the surface, and 00211 * creating surface. 00212 * 00213 * \param[out] output the resultant polygonal mesh 00214 */ 00215 void 00216 performReconstruction (pcl::PolygonMesh &output); 00217 00218 /** \brief Create the surface. 00219 * 00220 * The 1st step is filling the padding, so that all the cells in the padding 00221 * area are in the hash map. The 2nd step is store the vector, and projected 00222 * point. The 3rd step is finding all the edges intersects the surface, and 00223 * creating surface. 00224 * 00225 * \param[out] points the resultant points lying on the surface 00226 * \param[out] polygons the resultant polygons, as a set of vertices. The Vertices structure contains an array of point indices. 00227 */ 00228 void 00229 performReconstruction (pcl::PointCloud<PointNT> &points, 00230 std::vector<pcl::Vertices> &polygons); 00231 00232 /** \brief When the input data points don't fill into the 1*1*1 box, 00233 * scale them so that they can be filled in the unit box. Otherwise, 00234 * it will be some drawing problem when doing visulization 00235 * \param scale_factor scale all the input data point by scale_factor 00236 */ 00237 void 00238 scaleInputDataPoint (double scale_factor); 00239 00240 /** \brief Get the 3d index (x,y,z) of the cell based on the location of 00241 * the cell 00242 * \param p the coordinate of the input point 00243 * \param index the output 3d index 00244 */ 00245 inline void 00246 getCellIndex (const Eigen::Vector4f &p, Eigen::Vector3i& index) const 00247 { 00248 for (int i = 0; i < 3; ++i) 00249 index[i] = static_cast<int> ((p[i] - min_p_(i)) / leaf_size_); 00250 } 00251 00252 /** \brief Given the 3d index (x, y, z) of the cell, get the 00253 * coordinates of the cell center 00254 * \param index the output 3d index 00255 * \param center the resultant cell center 00256 */ 00257 inline void 00258 getCellCenterFromIndex (const Eigen::Vector3i &index, Eigen::Vector4f ¢er) const 00259 { 00260 for (int i = 0; i < 3; ++i) 00261 center[i] = 00262 min_p_[i] + static_cast<float> (index[i]) * 00263 static_cast<float> (leaf_size_) + 00264 static_cast<float> (leaf_size_) / 2.0f; 00265 } 00266 00267 /** \brief Given cell center, caluate the coordinates of the eight vertices of the cell 00268 * \param cell_center the coordinates of the cell center 00269 * \param pts the coordinates of the 8 vertices 00270 */ 00271 void 00272 getVertexFromCellCenter (const Eigen::Vector4f &cell_center, 00273 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &pts) const; 00274 00275 /** \brief Given an index (x, y, z) in 3d, translate it into the index 00276 * in 1d 00277 * \param index the index of the cell in (x,y,z) 3d format 00278 */ 00279 inline int 00280 getIndexIn1D (const Eigen::Vector3i &index) const 00281 { 00282 //assert(data_size_ > 0); 00283 return (index[0] * data_size_ * data_size_ + 00284 index[1] * data_size_ + index[2]); 00285 } 00286 00287 /** \brief Given an index in 1d, translate it into the index (x, y, z) 00288 * in 3d 00289 * \param index_1d the input 1d index 00290 * \param index_3d the output 3d index 00291 */ 00292 inline void 00293 getIndexIn3D (int index_1d, Eigen::Vector3i& index_3d) const 00294 { 00295 //assert(data_size_ > 0); 00296 index_3d[0] = index_1d / (data_size_ * data_size_); 00297 index_1d -= index_3d[0] * data_size_ * data_size_; 00298 index_3d[1] = index_1d / data_size_; 00299 index_1d -= index_3d[1] * data_size_; 00300 index_3d[2] = index_1d; 00301 } 00302 00303 /** \brief For a given 3d index of a cell, test whether the cells within its 00304 * padding area exist in the hash table, if no, create an entry for that cell. 00305 * \param index the index of the cell in (x,y,z) format 00306 */ 00307 void 00308 fillPad (const Eigen::Vector3i &index); 00309 00310 /** \brief Obtain the index of a cell and the pad size. 00311 * \param index the input index 00312 * \param pt_union_indices the union of input data points within the cell and padding cells 00313 */ 00314 void 00315 getDataPtsUnion (const Eigen::Vector3i &index, std::vector <int> &pt_union_indices); 00316 00317 /** \brief Given the index of a cell, exam it's up, left, front edges, and add 00318 * the vectices to m_surface list.the up, left, front edges only share 4 00319 * points, we first get the vectors at these 4 points and exam whether those 00320 * three edges are intersected by the surface \param index the input index 00321 * \param pt_union_indices the union of input data points within the cell and padding cells 00322 */ 00323 void 00324 createSurfaceForCell (const Eigen::Vector3i &index, std::vector <int> &pt_union_indices); 00325 00326 00327 /** \brief Given the coordinates of one point, project it onto the surface, 00328 * return the projected point. Do a binary search between p and p+projection_distance 00329 * to find the projected point 00330 * \param p the coordinates of the input point 00331 * \param pt_union_indices the union of input data points within the cell and padding cells 00332 * \param projection the resultant point projected 00333 */ 00334 void 00335 getProjection (const Eigen::Vector4f &p, std::vector<int> &pt_union_indices, Eigen::Vector4f &projection); 00336 00337 /** \brief Given the coordinates of one point, project it onto the surface, 00338 * return the projected point. Find the plane which fits all the points in 00339 * pt_union_indices, projected p to the plane to get the projected point. 00340 * \param p the coordinates of the input point 00341 * \param pt_union_indices the union of input data points within the cell and padding cells 00342 * \param projection the resultant point projected 00343 */ 00344 void 00345 getProjectionWithPlaneFit (const Eigen::Vector4f &p, 00346 std::vector<int> &pt_union_indices, 00347 Eigen::Vector4f &projection); 00348 00349 00350 /** \brief Given the location of a point, get it's vector 00351 * \param p the coordinates of the input point 00352 * \param pt_union_indices the union of input data points within the cell and padding cells 00353 * \param vo the resultant vector 00354 */ 00355 void 00356 getVectorAtPoint (const Eigen::Vector4f &p, 00357 std::vector <int> &pt_union_indices, Eigen::Vector3f &vo); 00358 00359 /** \brief Given the location of a point, get it's vector 00360 * \param p the coordinates of the input point 00361 * \param k_indices the k nearest neighbors of the query point 00362 * \param k_squared_distances the squared distances of the k nearest 00363 * neighbors to the query point 00364 * \param vo the resultant vector 00365 */ 00366 void 00367 getVectorAtPointKNN (const Eigen::Vector4f &p, 00368 std::vector<int> &k_indices, 00369 std::vector<float> &k_squared_distances, 00370 Eigen::Vector3f &vo); 00371 00372 /** \brief Get the magnitude of the vector by summing up the distance. 00373 * \param p the coordinate of the input point 00374 * \param pt_union_indices the union of input data points within the cell and padding cells 00375 */ 00376 double 00377 getMagAtPoint (const Eigen::Vector4f &p, const std::vector <int> &pt_union_indices); 00378 00379 /** \brief Get the 1st derivative 00380 * \param p the coordinate of the input point 00381 * \param vec the vector at point p 00382 * \param pt_union_indices the union of input data points within the cell and padding cells 00383 */ 00384 double 00385 getD1AtPoint (const Eigen::Vector4f &p, const Eigen::Vector3f &vec, 00386 const std::vector <int> &pt_union_indices); 00387 00388 /** \brief Get the 2nd derivative 00389 * \param p the coordinate of the input point 00390 * \param vec the vector at point p 00391 * \param pt_union_indices the union of input data points within the cell and padding cells 00392 */ 00393 double 00394 getD2AtPoint (const Eigen::Vector4f &p, const Eigen::Vector3f &vec, 00395 const std::vector <int> &pt_union_indices); 00396 00397 /** \brief Test whether the edge is intersected by the surface by 00398 * doing the dot product of the vector at two end points. Also test 00399 * whether the edge is intersected by the maximum surface by examing 00400 * the 2nd derivative of the intersection point 00401 * \param end_pts the two points of the edge 00402 * \param vect_at_end_pts 00403 * \param pt_union_indices the union of input data points within the cell and padding cells 00404 */ 00405 bool 00406 isIntersected (const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &end_pts, 00407 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts, 00408 std::vector <int> &pt_union_indices); 00409 00410 /** \brief Find point where the edge intersects the surface. 00411 * \param level binary search level 00412 * \param end_pts the two end points on the edge 00413 * \param vect_at_end_pts the vectors at the two end points 00414 * \param start_pt the starting point we use for binary search 00415 * \param pt_union_indices the union of input data points within the cell and padding cells 00416 * \param intersection the resultant intersection point 00417 */ 00418 void 00419 findIntersection (int level, 00420 const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &end_pts, 00421 const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts, 00422 const Eigen::Vector4f &start_pt, 00423 std::vector<int> &pt_union_indices, 00424 Eigen::Vector4f &intersection); 00425 00426 /** \brief Go through all the entries in the hash table and update the 00427 * cellData. 00428 * 00429 * When creating the hash table, the pt_on_surface field store the center 00430 * point of the cell.After calling this function, the projection operator will 00431 * project the center point onto the surface, and the pt_on_surface field will 00432 * be updated using the projected point.Also the vect_at_grid_pt field will be 00433 * updated using the vector at the upper left front vertex of the cell. 00434 * 00435 * \param index_1d the index of the cell after flatting it's 3d index into a 1d array 00436 * \param index_3d the index of the cell in (x,y,z) 3d format 00437 * \param pt_union_indices the union of input data points within the cell and pads 00438 * \param cell_data information stored in the cell 00439 */ 00440 void 00441 storeVectAndSurfacePoint (int index_1d, const Eigen::Vector3i &index_3d, 00442 std::vector<int> &pt_union_indices, const Leaf &cell_data); 00443 00444 /** \brief Go through all the entries in the hash table and update the cellData. 00445 * When creating the hash table, the pt_on_surface field store the center point 00446 * of the cell.After calling this function, the projection operator will project the 00447 * center point onto the surface, and the pt_on_surface field will be updated 00448 * using the projected point.Also the vect_at_grid_pt field will be updated using 00449 * the vector at the upper left front vertex of the cell. When projecting the point 00450 * and calculating the vector, using K nearest neighbors instead of using the 00451 * union of input data point within the cell and pads. 00452 * 00453 * \param index_1d the index of the cell after flatting it's 3d index into a 1d array 00454 * \param index_3d the index of the cell in (x,y,z) 3d format 00455 * \param cell_data information stored in the cell 00456 */ 00457 void 00458 storeVectAndSurfacePointKNN (int index_1d, const Eigen::Vector3i &index_3d, const Leaf &cell_data); 00459 00460 private: 00461 /** \brief Map containing the set of leaves. */ 00462 HashMap cell_hash_map_; 00463 00464 /** \brief Min and max data points. */ 00465 Eigen::Vector4f min_p_, max_p_; 00466 00467 /** \brief The size of a leaf. */ 00468 double leaf_size_; 00469 00470 /** \brief Gaussian scale. */ 00471 double gaussian_scale_; 00472 00473 /** \brief Data size. */ 00474 int data_size_; 00475 00476 /** \brief Max binary search level. */ 00477 int max_binary_search_level_; 00478 00479 /** \brief Number of neighbors (k) to use. */ 00480 int k_; 00481 00482 /** \brief Padding size. */ 00483 int padding_size_; 00484 00485 /** \brief The point cloud input (XYZ+Normals). */ 00486 PointCloudPtr data_; 00487 00488 /** \brief Store the surface normal(vector) at the each input data point. */ 00489 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > vector_at_data_point_; 00490 00491 /** \brief An array of points which lay on the output surface. */ 00492 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > surface_; 00493 00494 /** \brief Bit map which tells if there is any input data point in the cell. */ 00495 boost::dynamic_bitset<> occupied_cell_list_; 00496 00497 /** \brief Class get name method. */ 00498 std::string getClassName () const { return ("GridProjection"); } 00499 00500 public: 00501 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00502 }; 00503 } 00504 00505 #endif // PCL_SURFACE_GRID_PROJECTION_H_ 00506