organized_normal_estimation.h

Go to the documentation of this file.

#ifndef __ORGANIZED_NORMAL_ESTIMATION_H__
#define __ORGANIZED_NORMAL_ESTIMATION_H__

#include "cob_3d_features/organized_features.h"

namespace cob_3d_features
{
  namespace OrganizedNormalEstimationHelper
  {
    template <typename PointT, typename LabelT> void
    computeSegmentNormal( Eigen::Vector3f& normal_out, int index,
                          boost::shared_ptr<const pcl::PointCloud<PointT> > surface,
                          boost::shared_ptr<const pcl::PointCloud<LabelT> > labels,
                          int r, int steps);
  }

  template <typename PointInT, typename PointOutT, typename LabelOutT>
    class OrganizedNormalEstimation : 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>::inv_width_;
    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<PointOutT> PointCloudOut;
    typedef typename PointCloudOut::Ptr PointCloudOutPtr;
    typedef typename PointCloudOut::ConstPtr PointCloudOutConstPtr;

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

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

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

    void
    computePointNormal(
      const PointCloudIn &cloud,
      int index,
      float &n_x,
      float &n_y,
      float &n_z,
      int &label_out);

    void
    recomputeSegmentNormal(PointCloudInConstPtr cloud_in,
                           LabelCloudOutConstPtr label_in,
                           int index,
                           float& n_x,
                           float& n_y,
                           float& n_z);

    protected:

    void
      computeFeature (PointCloudOut &output);

    LabelCloudOutPtr labels_;

  };
}

#endif