gp3.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2010-2011, 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: gp3.h 5124 2012-03-16 03:09:41Z rusu $
 *
 */

#ifndef PCL_GP3_H_
#define PCL_GP3_H_

// PCL includes
#include <pcl/surface/reconstruction.h>

#include <pcl/ros/conversions.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/PolygonMesh.h>
#include <pcl/TextureMesh.h>
#include <boost/function.hpp>

#include <fstream>
#include <iostream>

// add by ktran to export update function
#include <pcl/pcl_macros.h>
#include <pcl/point_types.h>


namespace pcl
{
  inline bool 
  isVisible (const Eigen::Vector2f &X, const Eigen::Vector2f &S1, const Eigen::Vector2f &S2, 
             const Eigen::Vector2f &R = Eigen::Vector2f::Zero ())
  {
    double a0 = S1[1] - S2[1];
    double b0 = S2[0] - S1[0];
    double c0 = S1[0]*S2[1] - S2[0]*S1[1];
    double a1 = -X[1];
    double b1 = X[0];
    double c1 = 0;
    if (R != Eigen::Vector2f::Zero())
    {
      a1 += R[1];
      b1 -= R[0];
      c1 = R[0]*X[1] - X[0]*R[1];
    }
    double div = a0*b1 - b0*a1;
    double x = (b0*c1 - b1*c0) / div;
    double y = (a1*c0 - a0*c1) / div;

    bool intersection_outside_XR;
    if (R == Eigen::Vector2f::Zero())
    {
      if (X[0] > 0)
        intersection_outside_XR = (x <= 0) || (x >= X[0]);
      else if (X[0] < 0)
        intersection_outside_XR = (x >= 0) || (x <= X[0]);
      else if (X[1] > 0)
        intersection_outside_XR = (y <= 0) || (y >= X[1]);
      else if (X[1] < 0)
        intersection_outside_XR = (y >= 0) || (y <= X[1]);
      else
        intersection_outside_XR = true;
    }
    else
    {
      if (X[0] > R[0])
        intersection_outside_XR = (x <= R[0]) || (x >= X[0]);
      else if (X[0] < R[0])
        intersection_outside_XR = (x >= R[0]) || (x <= X[0]);
      else if (X[1] > R[1])
        intersection_outside_XR = (y <= R[1]) || (y >= X[1]);
      else if (X[1] < R[1])
        intersection_outside_XR = (y >= R[1]) || (y <= X[1]);
      else
        intersection_outside_XR = true;
    }
    if (intersection_outside_XR)
      return true;
    else
    {
      if (S1[0] > S2[0])
        return (x <= S2[0]) || (x >= S1[0]);
      else if (S1[0] < S2[0])
        return (x >= S2[0]) || (x <= S1[0]);
      else if (S1[1] > S2[1])
        return (y <= S2[1]) || (y >= S1[1]);
      else if (S1[1] < S2[1])                                                                                                                     
        return (y >= S2[1]) || (y <= S1[1]);
      else
        return false;
    }
  }  

  template <typename PointInT>
  class GreedyProjectionTriangulation : public MeshConstruction<PointInT>
  {
    public:
      using MeshConstruction<PointInT>::tree_;
      using MeshConstruction<PointInT>::input_;
      using MeshConstruction<PointInT>::indices_;

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

      typedef pcl::PointCloud<PointInT> PointCloudIn;
      typedef typename PointCloudIn::Ptr PointCloudInPtr;
      typedef typename PointCloudIn::ConstPtr PointCloudInConstPtr;

      // FIXME this enum should have a type.  Not be anonymous. 
      // Otherplaces where consts are used probably should be fixed.
      enum 
      { 
        NONE = -1,    // not-defined
        FREE = 0,    
        FRINGE = 1,  
        BOUNDARY = 2,
        COMPLETED = 3
      };
    
      GreedyProjectionTriangulation () : 
        mu_ (0), 
        search_radius_ (0), // must be set by user
        nnn_ (100),
        minimum_angle_ (M_PI/18), // 10 degrees
        maximum_angle_ (2*M_PI/3), // 120 degrees
        eps_angle_(M_PI/4), //45 degrees,
        consistent_(false), 
        consistent_ordering_ (false),
        triangle_ (),
        coords_ (),
        angles_ (),
        R_ (),
        state_ (),
        source_ (),
        ffn_ (),
        sfn_ (),
        part_ (),
        fringe_queue_ (),
        is_current_free_ (false),
        current_index_ (),
        prev_is_ffn_ (false),
        prev_is_sfn_ (false),
        next_is_ffn_ (false),
        next_is_sfn_ (false),
        changed_1st_fn_ (false),
        changed_2nd_fn_ (false),
        new2boundary_ (),
        already_connected_ (false),
        proj_qp_ (),
        u_ (),
        v_ (),
        uvn_ffn_ (),
        uvn_sfn_ (),
        uvn_next_ffn_ (),
        uvn_next_sfn_ (),
        tmp_ ()
      {};

      inline void 
      setMu (double mu) { mu_ = mu; }

      inline double 
      getMu () { return (mu_); }

      inline void 
      setMaximumNearestNeighbors (int nnn) { nnn_ = nnn; }

      inline int 
      getMaximumNearestNeighbors () { return (nnn_); }

      inline void 
      setSearchRadius (double radius) { search_radius_ = radius; }

      inline double 
      getSearchRadius () { return (search_radius_); }

      inline void 
      setMinimumAngle (double minimum_angle) { minimum_angle_ = minimum_angle; }

      inline double 
      getMinimumAngle () { return (minimum_angle_); }

      inline void 
      setMaximumAngle (double maximum_angle) { maximum_angle_ = maximum_angle; }

      inline double 
      getMaximumAngle () { return (maximum_angle_); }

      inline void 
      setMaximumSurfaceAngle (double eps_angle) { eps_angle_ = eps_angle; }

      inline double 
      getMaximumSurfaceAngle () { return (eps_angle_); }

      inline void 
      setNormalConsistency (bool consistent) { consistent_ = consistent; }

      inline bool 
      getNormalConsistency () { return (consistent_); }

      inline void 
      setConsistentVertexOrdering (bool consistent_ordering) { consistent_ordering_ = consistent_ordering; }

      inline bool 
      getConsistentVertexOrdering () { return (consistent_ordering_); }

      inline std::vector<int> 
      getPointStates () { return (state_); }

      inline std::vector<int> 
      getPartIDs () { return (part_); }


      inline std::vector<int>
      getSFN () { return (sfn_); }

      inline std::vector<int>
      getFFN () { return (ffn_); }

    protected:
      double mu_;

      double search_radius_;

      int nnn_;

      double minimum_angle_;

      double maximum_angle_;

      double eps_angle_;

      bool consistent_;
      
      bool consistent_ordering_;

     private:
      struct nnAngle
      {
        double angle;
        int index;
        int nnIndex;
        bool visible;
      };

      struct doubleEdge
      {
        doubleEdge () : index (0), first (), second () {}
        int index;
        Eigen::Vector2f first;
        Eigen::Vector2f second;
      };

      // Variables made global to decrease the number of parameters to helper functions

      pcl::Vertices triangle_;
      std::vector<Eigen::Vector3f> coords_;

      std::vector<nnAngle> angles_;
      int R_;
      std::vector<int> state_;
      std::vector<int> source_;
      std::vector<int> ffn_;
      std::vector<int> sfn_;
      std::vector<int> part_;
      std::vector<int> fringe_queue_;

      bool is_current_free_;
      int current_index_;
      bool prev_is_ffn_;
      bool prev_is_sfn_;
      bool next_is_ffn_;
      bool next_is_sfn_;
      bool changed_1st_fn_;
      bool changed_2nd_fn_;
      int new2boundary_;
      
      bool already_connected_; 

      Eigen::Vector3f proj_qp_;
      Eigen::Vector3f u_;
      Eigen::Vector3f v_;
      Eigen::Vector2f uvn_ffn_;
      Eigen::Vector2f uvn_sfn_;
      Eigen::Vector2f uvn_next_ffn_;
      Eigen::Vector2f uvn_next_sfn_;

      Eigen::Vector3f tmp_;

      void 
      performReconstruction (pcl::PolygonMesh &output);

      void 
      performReconstruction (std::vector<pcl::Vertices> &polygons);

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

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

      void 
      connectPoint (std::vector<pcl::Vertices> &polygons, 
                    const int prev_index, 
                    const int next_index, 
                    const int next_next_index, 
                    const Eigen::Vector2f &uvn_current, 
                    const Eigen::Vector2f &uvn_prev, 
                    const Eigen::Vector2f &uvn_next);

      void 
      closeTriangle (std::vector<pcl::Vertices> &polygons);

      std::vector<std::vector<size_t> >
      getTriangleList (const pcl::PolygonMesh &input);

      inline void
      addTriangle (int a, int b, int c, std::vector<pcl::Vertices> &polygons)
      {
        triangle_.vertices.resize (3);
        if (consistent_ordering_)
        {
          const PointInT p = input_->at (indices_->at (a));
          const Eigen::Vector3f pv = p.getVector3fMap ();
          if (p.getNormalVector3fMap ().dot (
                (pv - input_->at (indices_->at (b)).getVector3fMap ()).cross (
                 pv - input_->at (indices_->at (c)).getVector3fMap ()) ) > 0)
          {
            triangle_.vertices[0] = a;
            triangle_.vertices[1] = b;
            triangle_.vertices[2] = c;
          }
          else
          {
            triangle_.vertices[0] = a;
            triangle_.vertices[1] = c;
            triangle_.vertices[2] = b;
          }
        }
        else
        {
          triangle_.vertices[0] = a;
          triangle_.vertices[1] = b;
          triangle_.vertices[2] = c;
        }
        polygons.push_back (triangle_);
      }

      inline void
      addFringePoint (int v, int s)
      {
        source_[v] = s;
        part_[v] = part_[s];
        fringe_queue_.push_back(v);
      }

      static inline bool 
      nnAngleSortAsc (const nnAngle& a1, const nnAngle& a2)
      {
        if (a1.visible == a2.visible)
          return (a1.angle < a2.angle);
        else
          return a1.visible;
      }
  };

} // namespace pcl

#endif  //#ifndef PCL_GP3_H_