Point Cloud Library (PCL)  1.3.1
grid_projection.h
Go to the documentation of this file.
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 &center) 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  
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines