Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/surface/include/pcl/surface/grid_projection.h
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 &center) 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