gp3.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: gp3.h 35810 2011-02-08 00:03:46Z 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/PolygonMesh.h>
#include <boost/function.hpp>

#include "pcl/io/io.h"
#include <fstream>
#include <iostream>

namespace pcl
{
  template <typename PointInT>
  class GreedyProjectionTriangulation : public SurfaceReconstruction<PointInT>
  {
    public:
      using SurfaceReconstruction<PointInT>::tree_;
      using SurfaceReconstruction<PointInT>::input_;
      using SurfaceReconstruction<PointInT>::indices_;
      using SurfaceReconstruction<PointInT>::search_method_;

      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 () : nnn_ (0), mu_ (0), search_radius_ (0), 
                                         minimum_angle_ (0), maximum_angle_ (0), 
                                         eps_angle_(0), consistent_(false) 
      {};

      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 
      setMaximumSurfaceAgle (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 std::vector<int> 
      getPointStates () { return (state_); }

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

    protected:
      int nnn_;

      double mu_;

      double search_radius_;

      double minimum_angle_;

      double maximum_angle_;

      double eps_angle_;

      bool consistent_;

      inline int
      searchForNeighbors (int index, std::vector<int> &indices, std::vector<float> &distances)
      {
        return (search_method_ (index, nnn_, indices, distances));
      }

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

      struct doubleEdge
      {
        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);

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

      void 
      connectPoint (pcl::PolygonMesh &output, 
                    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 (pcl::PolygonMesh &output);

      inline void
      addTriangle (int a, int b, int c, pcl::PolygonMesh &output)
      {
        triangle_.vertices.clear ();
        triangle_.vertices.push_back (a);
        triangle_.vertices.push_back (b);
        triangle_.vertices.push_back (c);
        output.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;
      }

      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;
        }
      }  
  };

} // namespace pcl

#endif  //#ifndef PCL_GP3_H_

---

pcl
Author(s): See http://pcl.ros.org/authors for the complete list of authors.
autogenerated on Fri Jan 11 09:55:22 2013