grid_projection.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2010, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * $Id: grid_projection.h 5036 2012-03-12 08:54:15Z rusu $
 *
 */

#ifndef PCL_SURFACE_GRID_PROJECTION_H_
#define PCL_SURFACE_GRID_PROJECTION_H_

#include <pcl/surface/reconstruction.h>
#include <boost/dynamic_bitset/dynamic_bitset.hpp>
#include <boost/unordered_map.hpp>

namespace pcl
{
  const int I_SHIFT_EP[12][2] = {
    {0, 4}, {1, 5}, {2, 6}, {3, 7}, 
    {0, 1}, {1, 2}, {2, 3}, {3, 0},
    {4, 5}, {5, 6}, {6, 7}, {7, 4}
  };

  const int I_SHIFT_PT[4] = {
    0, 4, 5, 7
  };

  const int I_SHIFT_EDGE[3][2] = {
    {0,1}, {1,3}, {1,2}
  };


  template <typename PointNT>
  class GridProjection : public SurfaceReconstruction<PointNT>
  {
    public:
      using SurfaceReconstruction<PointNT>::input_;
      using SurfaceReconstruction<PointNT>::tree_;

      typedef typename pcl::PointCloud<PointNT>::Ptr PointCloudPtr;

      typedef typename pcl::KdTree<PointNT> KdTree;
      typedef typename pcl::KdTree<PointNT>::Ptr KdTreePtr;

      struct Leaf
      {
        Leaf () : data_indices (), pt_on_surface (), vect_at_grid_pt () {}

        std::vector<int> data_indices;
        Eigen::Vector4f pt_on_surface; 
        Eigen::Vector3f vect_at_grid_pt;
      };

      typedef boost::unordered_map<int, Leaf, boost::hash<int>, std::equal_to<int>, Eigen::aligned_allocator<int> > HashMap;

      GridProjection ();

      GridProjection (double in_resolution);

      ~GridProjection ();

      inline void 
      setResolution (double resolution)
      {
        leaf_size_ = resolution;
      }

      inline double 
      getResolution () const
      {
        return (leaf_size_);
      }

      inline void 
      setPaddingSize (int padding_size)
      {
        padding_size_ = padding_size;
      }
      inline int 
      getPaddingSize () const
      {
        return (padding_size_);
      }

      inline void 
      setNearestNeighborNum (int k)
      {
        k_ = k;
      }
      inline int 
      getNearestNeighborNum () const
      {
        return (k_);
      }

      inline void 
      setMaxBinarySearchLevel (int max_binary_search_level)
      {
        max_binary_search_level_ = max_binary_search_level;
      }
      inline int 
      getMaxBinarySearchLevel () const
      {
        return (max_binary_search_level_);
      }

      inline const HashMap& 
      getCellHashMap () const
      {
        return (cell_hash_map_);
      }

      inline const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> >& 
      getVectorAtDataPoint () const
      {
        return (vector_at_data_point_);
      }
      
      inline const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& 
      getSurface () const
      {
        return (surface_);
      }

    protected:
      void 
      getBoundingBox ();

      bool
      reconstructPolygons (std::vector<pcl::Vertices> &polygons);

      void 
      performReconstruction (pcl::PolygonMesh &output);

      void 
      performReconstruction (pcl::PointCloud<PointNT> &points, 
                             std::vector<pcl::Vertices> &polygons);

      void 
      scaleInputDataPoint (double scale_factor);

      inline void 
      getCellIndex (const Eigen::Vector4f &p, Eigen::Vector3i& index) const
      {
        for (int i = 0; i < 3; ++i)
          index[i] = static_cast<int> ((p[i] - min_p_(i)) / leaf_size_);
      }

      inline void
      getCellCenterFromIndex (const Eigen::Vector3i &index, Eigen::Vector4f &center) const
      {
        for (int i = 0; i < 3; ++i)
          center[i] = 
            min_p_[i] + static_cast<float> (index[i]) * 
            static_cast<float> (leaf_size_) + 
            static_cast<float> (leaf_size_) / 2.0f;
      }

      void 
      getVertexFromCellCenter (const Eigen::Vector4f &cell_center, 
                               std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &pts) const;

      inline int 
      getIndexIn1D (const Eigen::Vector3i &index) const
      {
        //assert(data_size_ > 0);
        return (index[0] * data_size_ * data_size_ + 
                index[1] * data_size_ + index[2]);
      }

      inline void 
      getIndexIn3D (int index_1d, Eigen::Vector3i& index_3d) const
      {
        //assert(data_size_ > 0);
        index_3d[0] = index_1d / (data_size_ * data_size_);
        index_1d -= index_3d[0] * data_size_ * data_size_;
        index_3d[1] = index_1d / data_size_;
        index_1d -= index_3d[1] * data_size_;
        index_3d[2] = index_1d;
      }

      void 
      fillPad (const Eigen::Vector3i &index);

      void 
      getDataPtsUnion (const Eigen::Vector3i &index, std::vector <int> &pt_union_indices);

      void 
      createSurfaceForCell (const Eigen::Vector3i &index, std::vector <int> &pt_union_indices);


      void
      getProjection (const Eigen::Vector4f &p, std::vector<int> &pt_union_indices, Eigen::Vector4f &projection);

      void 
      getProjectionWithPlaneFit (const Eigen::Vector4f &p, 
                                 std::vector<int> &pt_union_indices, 
                                 Eigen::Vector4f &projection);


      void
      getVectorAtPoint (const Eigen::Vector4f &p, 
                        std::vector <int> &pt_union_indices, Eigen::Vector3f &vo);

      void
      getVectorAtPointKNN (const Eigen::Vector4f &p, 
                           std::vector<int> &k_indices, 
                           std::vector<float> &k_squared_distances,
                           Eigen::Vector3f &vo);

      double 
      getMagAtPoint (const Eigen::Vector4f &p, const std::vector <int> &pt_union_indices);

      double 
      getD1AtPoint (const Eigen::Vector4f &p, const Eigen::Vector3f &vec, 
                    const std::vector <int> &pt_union_indices);

      double 
      getD2AtPoint (const Eigen::Vector4f &p, const Eigen::Vector3f &vec, 
                    const std::vector <int> &pt_union_indices);

      bool 
      isIntersected (const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &end_pts, 
                     std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts, 
                     std::vector <int> &pt_union_indices);

      void
      findIntersection (int level, 
                        const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &end_pts, 
                        const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts, 
                        const Eigen::Vector4f &start_pt, 
                        std::vector<int> &pt_union_indices,
                        Eigen::Vector4f &intersection);

      void
      storeVectAndSurfacePoint (int index_1d, const Eigen::Vector3i &index_3d, 
                                std::vector<int> &pt_union_indices, const Leaf &cell_data);

      void 
      storeVectAndSurfacePointKNN (int index_1d, const Eigen::Vector3i &index_3d, const Leaf &cell_data);

    private:
      HashMap cell_hash_map_;

      Eigen::Vector4f min_p_, max_p_;

      double leaf_size_;

      double gaussian_scale_;

      int data_size_;

      int max_binary_search_level_;

      int k_;

      int padding_size_;

      PointCloudPtr data_;

      std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > vector_at_data_point_;
      
      std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > surface_;

      boost::dynamic_bitset<> occupied_cell_list_;

      std::string getClassName () const { return ("GridProjection"); }

    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  };
}

#endif  // PCL_SURFACE_GRID_PROJECTION_H_