polygon.h

Go to the documentation of this file.

#ifndef POLYGON_H_
#define POLYGON_H_

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include "cob_3d_mapping_common/shape.h"
#include <boost/shared_ptr.hpp>
#include <boost/enable_shared_from_this.hpp>
extern "C" {
#include "gpc/gpc.h"
}
#include <polypartition/polypartition.h>
#include <cob_3d_mapping_common/dominant_color.h>
#include <fstream>
#include <iostream>

//#include "cob_3d_mapping_common/stop_watch.h"

namespace cob_3d_mapping
{
  struct MergeConfig
  {
    MergeConfig()
    : angle_thresh(0.97),
      d_thresh(0.1),
      weighting_method("AREA")
    {};

    double angle_thresh;
    float  d_thresh;
    std::string weighting_method;
  };


  class Polygon : public Shape
  {
  public:
    typedef boost::shared_ptr<Polygon> Ptr;
    typedef boost::shared_ptr<const Polygon> ConstPtr;

    Polygon()
      : Shape(),
        normal_(Eigen::Vector3f::Zero()),
        d_(0.0),
        merge_weight_(1.0)
    { }

    Polygon(unsigned int id,
            Eigen::Vector3f normal,
            Eigen::Vector3f centroid,
            std::vector<std::vector<Eigen::Vector3f> >& contours_3d,
            std::vector<bool> holes,
            std::vector<float> color);

    Polygon(unsigned int id,
            Eigen::Vector3f normal,
            Eigen::Vector3f centroid,
            std::vector<pcl::PointCloud<pcl::PointXYZ> >& contours_3d,
            std::vector<bool> holes,
            std::vector<float> color);

    //Polygon(Polygon::Ptr polygon);


    inline size_t outerContourIndex() const
    {
      for(size_t i = 0; i < contours_.size(); ++i) { if(!holes_[i]) { return i; } };
      return 0;
    }

    virtual void setContours3D(std::vector<std::vector<Eigen::Vector3f> >& contours_3d);
    virtual std::vector<std::vector<Eigen::Vector3f> > getContours3D();


    void setContours2D(std::vector<std::vector<Eigen::Vector2f> >& contours_2d) {contours_ = contours_2d;};
    std::vector<std::vector<Eigen::Vector2f> > getContours2D() {return contours_;};

    //##########methods for instantiation##############

    virtual void updateAttributes(const Eigen::Vector3f &new_normal, const Eigen::Vector3f & new_centroid);


    virtual void transform(const Eigen::Affine3f& trafo);
    //virtual void transform2tf(const Eigen::Affine3f& trafo);


    void smoothContours();


    //###########methods for merging##################
    virtual void getMergeCandidates(const std::vector<Polygon::Ptr>& poly_vec, std::vector<int>& intersections) const;


    virtual bool isIntersectedWith(const Polygon::Ptr& poly) const;



    bool getContourOverlap(const Polygon::Ptr& poly, float& rel_overlap, int& abs_overlap) const;


    float computeSimilarity(const Polygon::Ptr& poly) const;


    virtual void merge(std::vector<Polygon::Ptr>& poly_vec);


    void mergeDifference(Polygon::Ptr& p_merge);

    virtual void projectContour(const Polygon& p, std::vector<std::vector<Eigen::Vector2f> >& contours) const;

    void assignWeight();


    virtual void setParamsFrom(Polygon::Ptr& p);


    inline bool hasSimilarParametersWith(const Polygon::Ptr& poly) const
    {
      Eigen::Vector3f d = (this->pose_.translation() - poly->pose_.translation());
      return ( fabs(poly->normal_.dot(this->normal_)) > this->merge_settings_.angle_thresh &&
               fabs( d.dot(this->normal_) ) < this->merge_settings_.d_thresh &&
               fabs( d.dot(poly->normal_) ) < this->merge_settings_.d_thresh );
    }

    //#######methods for calculation#####################


    //void computePose();

    void computePose(std::vector<std::vector<Eigen::Vector3f> >& contours_3d);

    Eigen::Vector3f computeCentroid();

    Eigen::Vector3f computeCentroid(std::vector<std::vector<Eigen::Vector3f> >& contours_3d);


    //double computeArea() const;  // http://paulbourke.net/geometry/polyarea/


    double computeArea3d() const;


    virtual void triangulate(list<TPPLPoly>& tri_list) const;

    //void computePoseAndBoundingBox(Eigen::Affine3f& pose, Eigen::Vector4f& min_pt, Eigen::Vector4f& max_pt);



    //#############debugging methods#######################


    void debugOutput(std::string name);


    //########## member variables#################
    //needed for 32-bit systems: see http://eigen.tuxfamily.org/dox/TopicStructHavingEigenMembers.html
    //EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    std::vector<std::vector<Eigen::Vector2f> > contours_;
    Eigen::Vector3f normal_;
    double d_;
    //Eigen::Affine3f transform_from_world_to_plane;/**< Transformation from world coordinate system to coordinate system on polygon. */
    std::vector<bool> holes_;
    double merge_weight_;
    MergeConfig merge_settings_;
  protected:
    virtual void computeAverage(const std::vector<Polygon::Ptr>& poly_vec, Polygon::Ptr& p_average);

    void getIntersection(const Polygon::Ptr& poly, gpc_polygon* gpc_intersection) const;

    void mergeUnion(std::vector<Polygon::Ptr>& poly_vec, Polygon::Ptr&  p_average);

    void getGpcStructure(gpc_polygon* gpc_p, const std::vector<std::vector<Eigen::Vector2f> >& contours) const;


    void applyGpcStructure(const gpc_polygon* gpc_p);

    void assignID(const std::vector<Polygon::Ptr>& poly_vec);

    DominantColor d_color_;


  };
}
#endif /* POLYGON_H_ */