hough_3d.h

Go to the documentation of this file.

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

#ifndef PCL_RECOGNITION_HOUGH_3D_H_
#define PCL_RECOGNITION_HOUGH_3D_H_

#include <pcl/recognition/cg/correspondence_grouping.h>
#include <pcl/recognition/boost.h>

namespace pcl
{
  namespace recognition
  {
    class PCL_EXPORTS HoughSpace3D
    {

      public:
      
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
      
        HoughSpace3D (const Eigen::Vector3d &min_coord, const Eigen::Vector3d &bin_size, const Eigen::Vector3d &max_coord);

        void
        reset ();

        int
        vote (const Eigen::Vector3d &single_vote_coord, double weight, int voter_id);

        int
        voteInt (const Eigen::Vector3d &single_vote_coord, double weight, int voter_id);

        double
        findMaxima (double min_threshold, std::vector<double> & maxima_values, std::vector<std::vector<int> > &maxima_voter_ids);

      protected:

        Eigen::Vector3d min_coord_;

        Eigen::Vector3d bin_size_;

        Eigen::Vector3i bin_count_;

        int partial_bin_products_[4];

        int total_bins_count_;

        std::vector<double> hough_space_;
        //boost::unordered_map<int, double> hough_space_;

        boost::unordered_map<int, std::vector<int> > voter_ids_;
    };
  }

  template<typename PointModelT, typename PointSceneT, typename PointModelRfT = pcl::ReferenceFrame, typename PointSceneRfT = pcl::ReferenceFrame>
  class Hough3DGrouping : public CorrespondenceGrouping<PointModelT, PointSceneT>
  {
    public:
      typedef pcl::PointCloud<PointModelRfT> ModelRfCloud;
      typedef typename ModelRfCloud::Ptr ModelRfCloudPtr;
      typedef typename ModelRfCloud::ConstPtr ModelRfCloudConstPtr;

      typedef pcl::PointCloud<PointSceneRfT> SceneRfCloud;
      typedef typename SceneRfCloud::Ptr SceneRfCloudPtr;
      typedef typename SceneRfCloud::ConstPtr SceneRfCloudConstPtr;

      typedef pcl::PointCloud<PointModelT> PointCloud;
      typedef typename PointCloud::Ptr PointCloudPtr;
      typedef typename PointCloud::ConstPtr PointCloudConstPtr;

      typedef typename pcl::CorrespondenceGrouping<PointModelT, PointSceneT>::SceneCloudConstPtr SceneCloudConstPtr;

      Hough3DGrouping () 
        : input_rf_ ()
        , scene_rf_ ()
        , needs_training_ (true)
        , model_votes_ ()
        , hough_threshold_ (-1)
        , hough_bin_size_ (1.0)
        , use_interpolation_ (true)
        , use_distance_weight_ (false)
        , local_rf_normals_search_radius_ (0.0f)
        , local_rf_search_radius_ (0.0f)
        , hough_space_ ()
        , found_transformations_ ()
        , hough_space_initialized_ (false)
      {}

      inline void
      setInputCloud (const PointCloudConstPtr &cloud)
      {
        PCLBase<PointModelT>::setInputCloud (cloud);
        needs_training_ = true;
        hough_space_initialized_ = false;
        input_rf_.reset();
      }

      inline void
      setInputRf (const ModelRfCloudConstPtr &input_rf)
      {
        input_rf_ = input_rf;
        needs_training_ = true;
        hough_space_initialized_ = false;
      }

      inline ModelRfCloudConstPtr
      getInputRf () const
      {
        return (input_rf_);
      }
      
      inline void
      setSceneCloud (const SceneCloudConstPtr &scene)
      {
        scene_ = scene;
        hough_space_initialized_ = false;
        scene_rf_.reset();
      }

      inline void
      setSceneRf (const SceneRfCloudConstPtr &scene_rf)
      {
        scene_rf_ = scene_rf;
        hough_space_initialized_ = false;
      }

      inline SceneRfCloudConstPtr
      getSceneRf () const
      {
        return (scene_rf_);
      }

      inline void
      setModelSceneCorrespondences (const CorrespondencesConstPtr &corrs)
      {
        model_scene_corrs_ = corrs;
        hough_space_initialized_ = false;
      }

      inline void
      setHoughThreshold (double threshold)
      {
        hough_threshold_ = threshold;
      }

      inline double
      getHoughThreshold () const
      {
        return (hough_threshold_);
      }

      inline void
      setHoughBinSize (double bin_size)
      {
        hough_bin_size_ = bin_size;
        hough_space_initialized_ = false;
      }

      inline double
      getHoughBinSize () const
      {
        return (hough_bin_size_);
      }

      inline void
      setUseInterpolation (bool use_interpolation)
      {
        use_interpolation_ = use_interpolation;
        hough_space_initialized_ = false;
      }

      inline bool
      getUseInterpolation () const
      {
        return (use_interpolation_);
      }

      inline void
      setUseDistanceWeight (bool use_distance_weight)
      {
        use_distance_weight_ = use_distance_weight;
        hough_space_initialized_ = false;
      }

      inline bool
      getUseDistanceWeight () const
      {
        return (use_distance_weight_);
      } 

      inline void
      setLocalRfNormalsSearchRadius (float local_rf_normals_search_radius)
      {
        local_rf_normals_search_radius_ = local_rf_normals_search_radius;
        needs_training_ = true;
        hough_space_initialized_ = false;
      }

      inline float
      getLocalRfNormalsSearchRadius () const
      {
        return (local_rf_normals_search_radius_);
      }

      inline void
      setLocalRfSearchRadius (float local_rf_search_radius)
      {
        local_rf_search_radius_ = local_rf_search_radius;
        needs_training_ = true;
        hough_space_initialized_ = false;
      }

      inline float
      getLocalRfSearchRadius () const
      {
        return (local_rf_search_radius_);
      }

      bool
      train ();

      bool
      recognize (std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > &transformations);

      bool
      recognize (std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > &transformations, std::vector<pcl::Correspondences> &clustered_corrs);

    protected:
      using CorrespondenceGrouping<PointModelT, PointSceneT>::input_;
      using CorrespondenceGrouping<PointModelT, PointSceneT>::scene_;
      using CorrespondenceGrouping<PointModelT, PointSceneT>::model_scene_corrs_;

      ModelRfCloudConstPtr input_rf_;

      SceneRfCloudConstPtr scene_rf_;

      bool needs_training_;

      std::vector<Eigen::Vector3f> model_votes_;

      double hough_threshold_;

      double hough_bin_size_;

      bool use_interpolation_;

      bool use_distance_weight_;

      float local_rf_normals_search_radius_;

      float local_rf_search_radius_;

      boost::shared_ptr<pcl::recognition::HoughSpace3D> hough_space_;

      std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > found_transformations_;

      bool hough_space_initialized_;

      void
      clusterCorrespondences (std::vector<Correspondences> &model_instances);

      //  * 
      //  * \param[in] the scene cloud in which the PointSceneT has been converted to PointModelT.
      //  * \param[in] corrs a set of correspondences.
      //  * \param[out] transform the transformation matrix between the input cloud and the scene cloud that aligns the found correspondences.
      //  * \return true if the recognition had been successful or false if errors have occurred.
      //  */
      //bool
      //getTransformMatrix (const PointCloudConstPtr &scene_cloud, const Correspondences &corrs, Eigen::Matrix4f &transform);

      bool
      houghVoting ();

      template<typename PointType, typename PointRfType> void
      computeRf (const boost::shared_ptr<const pcl::PointCloud<PointType> > &input, pcl::PointCloud<PointRfType> &rf);
  };
}

#ifdef PCL_NO_PRECOMPILE
#include <pcl/recognition/impl/cg/hough_3d.hpp>
#endif

#endif // PCL_RECOGNITION_HOUGH_3D_H_