agast_2d.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2010-2011, Willow Garage, Inc.
 *  Copyright (c) 2012-, Open Perception, 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.
 *
 */

#ifndef PCL_KEYPOINTS_AGAST_KEYPOINT_2D_H_
#define PCL_KEYPOINTS_AGAST_KEYPOINT_2D_H_

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/keypoints/keypoint.h>
#include <pcl/common/intensity.h>

namespace pcl
{
  namespace keypoints
  {
    namespace agast
    {

      class PCL_EXPORTS AbstractAgastDetector
      {
        public:
          typedef boost::shared_ptr<AbstractAgastDetector> Ptr;
          typedef boost::shared_ptr<const AbstractAgastDetector> ConstPtr;

          AbstractAgastDetector (const size_t width, 
                                 const size_t height, 
                                 const double threshold,
                                 const double bmax) 
            : width_ (width)
            , height_ (height)
            , threshold_ (threshold)
            , nr_max_keypoints_ (std::numeric_limits<unsigned int>::max ())
            , bmax_ (bmax)
          {}

          virtual ~AbstractAgastDetector () {}

          void 
          detectKeypoints (const std::vector<unsigned char> &intensity_data, 
                           pcl::PointCloud<pcl::PointUV> &output);

          void 
          detectKeypoints (const std::vector<float> &intensity_data, 
                           pcl::PointCloud<pcl::PointUV> &output);

          void
          applyNonMaxSuppression (const std::vector<unsigned char>& intensity_data, 
                                  const pcl::PointCloud<pcl::PointUV> &input, 
                                  pcl::PointCloud<pcl::PointUV> &output);

          void
          applyNonMaxSuppression (const std::vector<float>& intensity_data, 
                                  const pcl::PointCloud<pcl::PointUV> &input, 
                                  pcl::PointCloud<pcl::PointUV> &output);

          virtual int 
          computeCornerScore (const unsigned char* im) const = 0;

          virtual int 
          computeCornerScore (const float* im) const = 0;

          inline void
          setThreshold (const double threshold)
          {
            threshold_ = threshold;
          }

          inline double
          getThreshold ()
          {
            return (threshold_);
          }

          inline void
          setMaxKeypoints (const unsigned int nr_max_keypoints)
          {
            nr_max_keypoints_ = nr_max_keypoints;
          }

          inline unsigned int 
          getMaxKeypoints ()
          {
            return (nr_max_keypoints_);
          }

          virtual void 
          detect (const unsigned char* im, 
                  std::vector<pcl::PointUV, Eigen::aligned_allocator<pcl::PointUV> > &corners_all) const = 0;

          virtual void 
          detect (const float* im, 
                  std::vector<pcl::PointUV, Eigen::aligned_allocator<pcl::PointUV> > &) const = 0;

        protected:

          struct ScoreIndex
          {
            int idx;
            int score;
          };

          struct CompareScoreIndex
          {
            inline bool
            operator() (const ScoreIndex &i1, const ScoreIndex &i2)
            {
              return (i1.score > i2.score);
            }
          };

          virtual void
          initPattern () = 0;

          void
          applyNonMaxSuppression (const pcl::PointCloud<pcl::PointUV> &input, 
                                  const std::vector<ScoreIndex>& scores, 
                                  pcl::PointCloud<pcl::PointUV> &output);

          void 
          computeCornerScores (const unsigned char* im, 
                               const std::vector<pcl::PointUV, Eigen::aligned_allocator<pcl::PointUV> > & corners_all, 
                               std::vector<ScoreIndex> & scores);

          void 
          computeCornerScores (const float* im, 
                               const std::vector<pcl::PointUV, Eigen::aligned_allocator<pcl::PointUV> > & corners_all, 
                               std::vector<ScoreIndex> & scores);

          size_t width_;
          size_t height_;

          double threshold_;

          unsigned int nr_max_keypoints_;

          double bmax_;
      };

      class PCL_EXPORTS AgastDetector7_12s : public AbstractAgastDetector
      {
        public:
          typedef boost::shared_ptr<AgastDetector7_12s> Ptr;
          typedef boost::shared_ptr<const AgastDetector7_12s> ConstPtr;

          AgastDetector7_12s (const size_t width, 
                              const size_t height, 
                              const double threshold,
                              const double bmax = 255) 
            : AbstractAgastDetector (width, height, threshold, bmax)
          {
            initPattern ();
          }

          ~AgastDetector7_12s () {}

          int 
          computeCornerScore (const unsigned char* im) const;

          int 
          computeCornerScore (const float* im) const;

          void 
          detect (const unsigned char* im, std::vector<pcl::PointUV, Eigen::aligned_allocator<pcl::PointUV> > &corners_all) const;

          void 
          detect (const float* im, std::vector<pcl::PointUV, Eigen::aligned_allocator<pcl::PointUV> > &corners_all) const;

        protected:
          void 
          initPattern ();

        private:
          static const int border_width_ = 2;

          // offsets defining the sample pattern
          int_fast16_t s_offset0_;
          int_fast16_t s_offset1_;
          int_fast16_t s_offset2_;
          int_fast16_t s_offset3_;
          int_fast16_t s_offset4_;
          int_fast16_t s_offset5_;
          int_fast16_t s_offset6_;
          int_fast16_t s_offset7_;
          int_fast16_t s_offset8_;
          int_fast16_t s_offset9_;
          int_fast16_t s_offset10_;
          int_fast16_t s_offset11_;
      };

      class PCL_EXPORTS AgastDetector5_8 : public AbstractAgastDetector
      {
        public:
          typedef boost::shared_ptr<AgastDetector5_8> Ptr;
          typedef boost::shared_ptr<const AgastDetector5_8> ConstPtr;

          AgastDetector5_8 (const size_t width, 
                            const size_t height, 
                            const double threshold,
                            const double bmax = 255) 
            : AbstractAgastDetector (width, height, threshold, bmax)
          {
            initPattern ();
          }

          ~AgastDetector5_8 () {}

          int 
          computeCornerScore (const unsigned char* im) const;

          int 
          computeCornerScore (const float* im) const;

          void 
          detect (const unsigned char* im, std::vector<pcl::PointUV, Eigen::aligned_allocator<pcl::PointUV> > &corners_all) const;

          void 
          detect (const float* im, std::vector<pcl::PointUV, Eigen::aligned_allocator<pcl::PointUV> > &corners_all) const;

        protected:
          void 
          initPattern ();

        private:
          static const int border_width_ = 1;

          // offsets defining the sample pattern
          int_fast16_t s_offset0_;
          int_fast16_t s_offset1_;
          int_fast16_t s_offset2_;
          int_fast16_t s_offset3_;
          int_fast16_t s_offset4_;
          int_fast16_t s_offset5_;
          int_fast16_t s_offset6_;
          int_fast16_t s_offset7_;
      };

      class PCL_EXPORTS OastDetector9_16 : public AbstractAgastDetector
      {
        public:
          typedef boost::shared_ptr<OastDetector9_16> Ptr;
          typedef boost::shared_ptr<const OastDetector9_16> ConstPtr;

          OastDetector9_16 (const size_t width, 
                            const size_t height, 
                            const double threshold,
                            const double bmax = 255) 
            : AbstractAgastDetector (width, height, threshold, bmax)
          {
            initPattern ();
          }

          ~OastDetector9_16 () {}

          int 
          computeCornerScore (const unsigned char* im) const;

          int 
          computeCornerScore (const float* im) const;

          void 
          detect (const unsigned char* im, std::vector<pcl::PointUV, Eigen::aligned_allocator<pcl::PointUV> > &corners_all) const;

          void 
          detect (const float* im, std::vector<pcl::PointUV, Eigen::aligned_allocator<pcl::PointUV> > &corners_all) const;

        protected:
          void 
          initPattern ();

        private:
          static const int border_width_ = 3;

          // offsets defining the sample pattern
          int_fast16_t s_offset0_;
          int_fast16_t s_offset1_;
          int_fast16_t s_offset2_;
          int_fast16_t s_offset3_;
          int_fast16_t s_offset4_;
          int_fast16_t s_offset5_;
          int_fast16_t s_offset6_;
          int_fast16_t s_offset7_;
          int_fast16_t s_offset8_;
          int_fast16_t s_offset9_;
          int_fast16_t s_offset10_;
          int_fast16_t s_offset11_;
          int_fast16_t s_offset12_;
          int_fast16_t s_offset13_;
          int_fast16_t s_offset14_;
          int_fast16_t s_offset15_;
      };
    } // namespace agast
  } // namespace keypoints

  namespace keypoints
  {
    namespace internal
    {
      template <typename Out> 
      struct AgastApplyNonMaxSuppresion
      {
        AgastApplyNonMaxSuppresion (
            const std::vector<unsigned char> &image_data, 
            const pcl::PointCloud<pcl::PointUV> &tmp_cloud,
            const pcl::keypoints::agast::AbstractAgastDetector::Ptr &detector,
            pcl::PointCloud<Out> &output)
        {
          pcl::PointCloud<pcl::PointUV> output_temp;
          detector->applyNonMaxSuppression (image_data, tmp_cloud, output_temp);
          pcl::copyPointCloud<pcl::PointUV, Out> (output_temp, output);
        }
      };

      template <>
      struct AgastApplyNonMaxSuppresion<pcl::PointUV>
      {
        AgastApplyNonMaxSuppresion (
            const std::vector<unsigned char> &image_data, 
            const pcl::PointCloud<pcl::PointUV> &tmp_cloud,
            const pcl::keypoints::agast::AbstractAgastDetector::Ptr &detector,
            pcl::PointCloud<pcl::PointUV> &output)
        {
          detector->applyNonMaxSuppression (image_data, tmp_cloud, output);
        }
      };
      template <typename Out> 
      struct AgastDetector
      {
        AgastDetector (
            const std::vector<unsigned char> &image_data, 
            const pcl::keypoints::agast::AbstractAgastDetector::Ptr &detector,
            pcl::PointCloud<Out> &output)
        {
          pcl::PointCloud<pcl::PointUV> output_temp;
          detector->detectKeypoints (image_data, output_temp);
          pcl::copyPointCloud<pcl::PointUV, Out> (output_temp, output);
        }
      };

      template <>
      struct AgastDetector<pcl::PointUV>
      {
        AgastDetector (
            const std::vector<unsigned char> &image_data, 
            const pcl::keypoints::agast::AbstractAgastDetector::Ptr &detector,
            pcl::PointCloud<pcl::PointUV> &output)
        {
          detector->detectKeypoints (image_data, output);
        }
      };
    } // namespace agast
  } // namespace keypoints


  template <typename PointInT, typename PointOutT, typename IntensityT = pcl::common::IntensityFieldAccessor<PointInT> >
  class AgastKeypoint2DBase : public Keypoint<PointInT, PointOutT>
  {
    public:
      typedef typename Keypoint<PointInT, PointOutT>::PointCloudIn PointCloudIn;
      typedef typename Keypoint<PointInT, PointOutT>::PointCloudOut PointCloudOut;
      typedef typename Keypoint<PointInT, PointOutT>::KdTree KdTree;
      typedef typename PointCloudIn::ConstPtr PointCloudInConstPtr;

      typedef pcl::keypoints::agast::AbstractAgastDetector::Ptr AgastDetectorPtr;
     
      using Keypoint<PointInT, PointOutT>::name_;
      using Keypoint<PointInT, PointOutT>::input_;
      using Keypoint<PointInT, PointOutT>::indices_;
      using Keypoint<PointInT, PointOutT>::k_;

      AgastKeypoint2DBase ()
        : threshold_ (10)
        , apply_non_max_suppression_ (true)
        , bmax_ (255)
        , detector_ ()
        , nr_max_keypoints_ (std::numeric_limits<unsigned int>::max ())
      {
        k_ = 1;
      }

      virtual ~AgastKeypoint2DBase ()
      {
      }

      inline void
      setThreshold (const double threshold)
      {
        threshold_ = threshold;
      }

      inline double
      getThreshold ()
      {
        return (threshold_);
      }

      inline void
      setMaxKeypoints (const unsigned int nr_max_keypoints)
      {
        nr_max_keypoints_ = nr_max_keypoints;
      }

      inline unsigned int 
      getMaxKeypoints ()
      {
        return (nr_max_keypoints_);
      }

      inline void
      setMaxDataValue (const double bmax)
      {
        bmax_ = bmax;
      }

      inline double
      getMaxDataValue ()
      {
        return (bmax_);
      }

      inline void
      setNonMaxSuppression (const bool enabled)
      {
        apply_non_max_suppression_ = enabled;
      }

      inline bool
      getNonMaxSuppression ()
      {
        return (apply_non_max_suppression_);
      }

      inline void
      setAgastDetector (const AgastDetectorPtr &detector)
      {
        detector_ = detector;
      }

      inline AgastDetectorPtr
      getAgastDetector ()
      {
        return (detector_);
      }
    protected:

      bool 
      initCompute ();
      
      virtual void 
      detectKeypoints (PointCloudOut &output) = 0;

      IntensityT intensity_;
      
      double threshold_;

      bool apply_non_max_suppression_;

      double bmax_;

      AgastDetectorPtr detector_;

      unsigned int nr_max_keypoints_;
  };

  template <typename PointInT, typename PointOutT = pcl::PointUV>
  class AgastKeypoint2D : public AgastKeypoint2DBase<PointInT, PointOutT, pcl::common::IntensityFieldAccessor<PointInT> >
  {
    public:
      typedef typename Keypoint<PointInT, PointOutT>::PointCloudOut PointCloudOut;

      using Keypoint<PointInT, PointOutT>::name_;
      using Keypoint<PointInT, PointOutT>::input_;
      using Keypoint<PointInT, PointOutT>::indices_;
      using Keypoint<PointInT, PointOutT>::k_;
      using AgastKeypoint2DBase<PointInT, PointOutT, pcl::common::IntensityFieldAccessor<PointInT> >::intensity_;
      using AgastKeypoint2DBase<PointInT, PointOutT, pcl::common::IntensityFieldAccessor<PointInT> >::threshold_;
      using AgastKeypoint2DBase<PointInT, PointOutT, pcl::common::IntensityFieldAccessor<PointInT> >::bmax_;
      using AgastKeypoint2DBase<PointInT, PointOutT, pcl::common::IntensityFieldAccessor<PointInT> >::apply_non_max_suppression_;
      using AgastKeypoint2DBase<PointInT, PointOutT, pcl::common::IntensityFieldAccessor<PointInT> >::detector_;
      using AgastKeypoint2DBase<PointInT, PointOutT, pcl::common::IntensityFieldAccessor<PointInT> >::nr_max_keypoints_;

      AgastKeypoint2D ()
      {
        name_ = "AgastKeypoint2D";
      }

      virtual ~AgastKeypoint2D ()
      {
      }

    protected:
      virtual void 
      detectKeypoints (PointCloudOut &output);
  };

  template <>
  class AgastKeypoint2D<pcl::PointXYZ, pcl::PointUV>
    : public AgastKeypoint2DBase<pcl::PointXYZ, pcl::PointUV, pcl::common::IntensityFieldAccessor<pcl::PointXYZ> > 
  {
    public:
      AgastKeypoint2D ()
      {
        name_ = "AgastKeypoint2D";
        bmax_ = 4;    // max data value for an OpenNI camera
      }

      virtual ~AgastKeypoint2D ()
      {
      }

    protected:
      virtual void 
      detectKeypoints (pcl::PointCloud<pcl::PointUV> &output);
  };

}

#include <pcl/keypoints/impl/agast_2d.hpp>

#endif