Program Listing for File sac_segmentation.hpp

↰ Return to documentation for file (include/pcl_ros/segmentation/sac_segmentation.hpp)

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2009, 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: sac_segmentation.h 35564 2011-01-27 07:32:12Z rusu $
 *
 */

#ifndef PCL_ROS__SEGMENTATION__SAC_SEGMENTATION_HPP_
#define PCL_ROS__SEGMENTATION__SAC_SEGMENTATION_HPP_

#include <message_filters/pass_through.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <dynamic_reconfigure/server.h>
#include <string>
#include "pcl_ros/pcl_nodelet.hpp"
#include "pcl_ros/SACSegmentationConfig.hpp"
#include "pcl_ros/SACSegmentationFromNormalsConfig.hpp"

namespace pcl_ros
{
namespace sync_policies = message_filters::sync_policies;


class SACSegmentation : public PCLNodelet
{
  typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
  typedef boost::shared_ptr<PointCloud> PointCloudPtr;
  typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;

public:
  SACSegmentation()
  : min_inliers_(0) {}

  inline void setInputTFframe(std::string tf_frame) {tf_input_frame_ = tf_frame;}

  inline std::string getInputTFframe() {return tf_input_frame_;}

  inline void setOutputTFframe(std::string tf_frame) {tf_output_frame_ = tf_frame;}

  inline std::string getOutputTFframe() {return tf_output_frame_;}

protected:
  // The minimum number of inliers a model must have in order to be considered valid.
  int min_inliers_;

  // ROS nodelet attributes
  ros::Publisher pub_indices_;

  ros::Publisher pub_model_;

  ros::Subscriber sub_input_;

  boost::shared_ptr<dynamic_reconfigure::Server<SACSegmentationConfig>> srv_;

  std::string tf_input_frame_;

  std::string tf_input_orig_frame_;

  std::string tf_output_frame_;

  message_filters::PassThrough<pcl_msgs::PointIndices> nf_pi_;

  virtual void onInit();

  virtual void subscribe();
  virtual void unsubscribe();

  void config_callback(SACSegmentationConfig & config, uint32_t level);

  void input_indices_callback(
    const PointCloudConstPtr & cloud,
    const PointIndicesConstPtr & indices);

  PointIndices indices_;

  inline void
  indices_callback(const PointIndicesConstPtr & indices)
  {
    indices_ = *indices;
  }

  inline void
  input_callback(const PointCloudConstPtr & input)
  {
    indices_.header = fromPCL(input->header);
    PointIndicesConstPtr indices;
    indices.reset(new PointIndices(indices_));
    nf_pi_.add(indices);
  }

private:
  boost::mutex mutex_;

  pcl::SACSegmentation<pcl::PointXYZ> impl_;

  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
    PointIndices>>> sync_input_indices_e_;
  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
    PointIndices>>> sync_input_indices_a_;

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};


class SACSegmentationFromNormals : public SACSegmentation
{
  typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
  typedef boost::shared_ptr<PointCloud> PointCloudPtr;
  typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;

  typedef pcl::PointCloud<pcl::Normal> PointCloudN;
  typedef boost::shared_ptr<PointCloudN> PointCloudNPtr;
  typedef boost::shared_ptr<const PointCloudN> PointCloudNConstPtr;

public:
  inline void setInputTFframe(std::string tf_frame) {tf_input_frame_ = tf_frame;}

  inline std::string getInputTFframe() {return tf_input_frame_;}

  inline void setOutputTFframe(std::string tf_frame) {tf_output_frame_ = tf_frame;}

  inline std::string getOutputTFframe() {return tf_output_frame_;}

protected:
  // ROS nodelet attributes
  message_filters::Subscriber<PointCloudN> sub_normals_filter_;

  ros::Subscriber sub_axis_;

  boost::shared_ptr<dynamic_reconfigure::Server<SACSegmentationFromNormalsConfig>> srv_;

  inline void
  input_callback(const PointCloudConstPtr & cloud)
  {
    PointIndices indices;
    indices.header.stamp = fromPCL(cloud->header).stamp;
    nf_.add(boost::make_shared<PointIndices>(indices));
  }

  message_filters::PassThrough<PointIndices> nf_;

  std::string tf_input_frame_;
  std::string tf_input_orig_frame_;
  std::string tf_output_frame_;

  virtual void onInit();

  virtual void subscribe();
  virtual void unsubscribe();

  void axis_callback(const pcl_msgs::ModelCoefficientsConstPtr & model);

  void config_callback(SACSegmentationFromNormalsConfig & config, uint32_t level);

  void input_normals_indices_callback(
    const PointCloudConstPtr & cloud,
    const PointCloudNConstPtr & cloud_normals,
    const PointIndicesConstPtr & indices);

private:
  boost::mutex mutex_;

  pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> impl_;

  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
    PointCloudN, PointIndices>>> sync_input_normals_indices_a_;
  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloudN,
    PointIndices>>> sync_input_normals_indices_e_;

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}  // namespace pcl_ros

#endif  // PCL_ROS__SEGMENTATION__SAC_SEGMENTATION_HPP_