cylinder.h

Go to the documentation of this file.

#ifndef CYLINDER_H_
#define CYLINDER_H_

//general includes
#include <math.h>
#include <sstream>

//cob includes
#include "cob_3d_mapping_common/shape.h"
#include "cob_3d_mapping_common/polygon.h"
extern "C"
{
#include "gpc/gpc.h"
}
//boost includes
#include <boost/shared_ptr.hpp>
#include <boost/enable_shared_from_this.hpp>
#include <boost/lexical_cast.hpp>
//pcl includes
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/centroid.h>
#include <pcl/common/eigen.h>
#ifdef PCL_VERSION_COMPARE
#include <pcl/common/transforms.h>
#else
#include <pcl/common/transform.h>
#endif
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/registration/transforms.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/exceptions.h>
#include <pcl/common/common.h>

namespace cob_3d_mapping
{

  double
  radiusAndOriginFromCloud (pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr in_cloud, std::vector<int>& indices,
                            Eigen::Vector3f& origin, const Eigen::Vector3f& sym_axis);

  class Cylinder : public Polygon
  {

  public:
    typedef boost::shared_ptr<Cylinder> Ptr;

    Cylinder () :
        Polygon ()
    {
    }

    Cylinder (unsigned int id, Eigen::Vector3f origin, Eigen::Vector3f sym_axis, double radius,
              std::vector<std::vector<Eigen::Vector3f> >& contours_3d, std::vector<bool> holes,
              std::vector<float> color);

    Cylinder (unsigned int id, Eigen::Vector3f origin, Eigen::Vector3f sym_axis, double radius,
              std::vector<pcl::PointCloud<pcl::PointXYZ> >& contours_3d, std::vector<bool> holes,
              std::vector<float> color);

    //##############Methods to initialize cylinder and its paramers#########

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

    virtual std::vector<std::vector<Eigen::Vector3f> >
    getContours3D ();

    virtual void
    updateAttributes (const Eigen::Vector3f &sym_axis, const Eigen::Vector3f &origin, const Eigen::Vector3f &z_axis);

    virtual void
    transform (Eigen::Affine3f & tf);

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

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

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

    void
    computePose (Eigen::Vector3f origin, Eigen::Vector3f z_axis);

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

    void
    computeHeight ();

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

    //############## debugging methods ####################
    void
    dbgOut (pcl::PointCloud<pcl::PointXYZRGB>::Ptr points, std::string& name);

    void
    printAttributes (std::string & name);

    void
    dumpParams (std::string name);

    //################# member variables########################
    double r_; 
    double h_min_; 
    double h_max_; 
    Eigen::Vector3f sym_axis_; 
    //Eigen::Vector3f origin_;         /**< Origin of cylinder. */

  protected:
    /*
     * \brief Compute average cylinder from a set of cylinders.
     *
     * \param[in] poly_vec The set of cylinders.
     * \param[out] p_average The average cylinder.
     */
    virtual void
    computeAverage (const std::vector<Polygon::Ptr>& poly_vec, Polygon::Ptr& p_average);

  };
}

#endif