organized_curvature_estimation.h

Go to the documentation of this file.

#ifndef __ORGANIZED_CURVATURE_ESTIMATION_H__
#define __ORGANIZED_CURVATURE_ESTIMATION_H__

#include "cob_3d_features/organized_features.h"
#include "cob_3d_mapping_common/label_defines.h"

// number of currently used labels:
#define I_SIZE 3

namespace cob_3d_features
{
  template <typename PointInT, typename PointNT, typename PointLabelT, typename PointOutT>
    class OrganizedCurvatureEstimation : public OrganizedFeatures<PointInT,PointOutT>
  {
    public:

    using OrganizedFeatures<PointInT,PointOutT>::pixel_search_radius_;
    using OrganizedFeatures<PointInT,PointOutT>::pixel_steps_;
    using OrganizedFeatures<PointInT,PointOutT>::circle_steps_;
    using OrganizedFeatures<PointInT,PointOutT>::n_points_;
    using OrganizedFeatures<PointInT,PointOutT>::mask_;
    using OrganizedFeatures<PointInT,PointOutT>::input_;
    using OrganizedFeatures<PointInT,PointOutT>::indices_;
    using OrganizedFeatures<PointInT,PointOutT>::surface_;
    using OrganizedFeatures<PointInT,PointOutT>::skip_distant_point_threshold_;
    using OrganizedFeatures<PointInT,PointOutT>::feature_name_;

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

    typedef pcl::PointCloud<PointNT> NormalCloudIn;
    typedef typename NormalCloudIn::Ptr NormalCloudInPtr;
    typedef typename NormalCloudIn::ConstPtr NormalCloudInConstPtr;

    typedef pcl::PointCloud<PointLabelT> LabelCloudOut;
    typedef typename LabelCloudOut::Ptr LabelCloudOutPtr;
    typedef typename LabelCloudOut::ConstPtr LabelCloudOutConstPtr;

    typedef pcl::PointCloud<PointOutT> PointCloudOut;
    typedef typename PointCloudOut::Ptr PointCloudOutPtr;
    typedef typename PointCloudOut::ConstPtr PointCloudOutConstPtr;

    OrganizedCurvatureEstimation ()
    {
      feature_name_ = "OrganizedCurvatureEstimation";
    };

    inline void
      setInputNormals(const NormalCloudInConstPtr &normals) { normals_ = normals; }

    inline void
      setOutputLabels(LabelCloudOutPtr labels) {labels_ = labels; }

    inline void
      setEdgeClassThreshold(float th)
    {
      edge_curvature_threshold_ = th;
    }

    void
      computePointCurvatures(
        const NormalCloudIn &normals,
        const int index,
        const std::vector<int> &indices,
        float &pcx,
        float &pcy,
        float &pcz,
        float &pc1,
        float &pc2,
        int &label_out);

    void
      computePointCurvatures(
        const NormalCloudIn &normals,
        const int index,
        const std::vector<int> &indices,
        const std::vector<float> &sqr_distances,
        float &pcx,
        float &pcy,
        float &pcz,
        float &pc1,
        float &pc2,
        int &label_out);

    protected:

    void
      computeFeature (PointCloudOut &output);

    NormalCloudInConstPtr normals_;
    LabelCloudOutPtr labels_;

    float edge_curvature_threshold_;

  };
}

#endif