edge_depth_refinement.h
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2014, JSK Lab
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/o2r other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 
37 #ifndef JSK_PCL_ROS_EDGE_DEPTH_REFINEMENT_H_
38 #define JSK_PCL_ROS_EDGE_DEPTH_REFINEMENT_H_
39 
40 #include <pcl_ros/pcl_nodelet.h>
41 #include <sensor_msgs/PointCloud2.h>
42 #include <jsk_recognition_msgs/ClusterPointIndices.h>
43 #include <jsk_recognition_msgs/ModelCoefficientsArray.h>
44 #include <jsk_recognition_msgs/SegmentArray.h>
45 
49 
52 #include <jsk_pcl_ros/EdgeDepthRefinementConfig.h>
53 #include <dynamic_reconfigure/server.h>
54 #include <boost/tuple/tuple.hpp>
55 
57 
58 namespace jsk_pcl_ros
59 {
61  {
62  public:
64  sensor_msgs::PointCloud2,
65  jsk_recognition_msgs::ClusterPointIndices > SyncPolicy;
66  typedef pcl::PointXYZRGB PointT;
67  typedef jsk_pcl_ros::EdgeDepthRefinementConfig Config;
68  protected:
70  // methods
72  virtual void onInit();
73 
74  virtual void refine(
75  const sensor_msgs::PointCloud2ConstPtr &point,
76  const jsk_recognition_msgs::ClusterPointIndicesConstPtr &indices);
77 
78  virtual void removeOutliersByLine(
79  const pcl::PointCloud<PointT>::Ptr& cloud,
80  const std::vector<int>& indices,
81  pcl::PointIndices& inliers,
82  pcl::ModelCoefficients& coefficients);
83 
84  virtual void removeOutliers(
85  const pcl::PointCloud<PointT>::Ptr& cloud,
86  const std::vector<PCLIndicesMsg>& indices,
87  std::vector<pcl::PointIndices::Ptr>& output_inliers,
88  std::vector<pcl::ModelCoefficients::Ptr>& output_coefficients);
89 
90  virtual void removeDuplicatedEdges(
91  const pcl::PointCloud<PointT>::Ptr& cloud,
92  const std::vector<pcl::PointIndices::Ptr> inliers,
93  const std::vector<pcl::ModelCoefficients::Ptr> coefficients,
94  std::vector<pcl::PointIndices::Ptr>& output_inliers,
95  std::vector<pcl::ModelCoefficients::Ptr>& output_coefficients);
96 
98  const pcl::ModelCoefficients::Ptr coefficients);
99 
101  const pcl::PointCloud<PointT>::Ptr& cloud,
102  const std::vector<int>& indices,
104 
105  virtual void publishIndices(
107  ros::Publisher& pub_coefficients,
108  ros::Publisher& pub_edges,
109  const std::vector<pcl::PointIndices::Ptr> inliers,
110  const std::vector<pcl::ModelCoefficients::Ptr> coefficients,
111  const std_msgs::Header& header);
112 
113  virtual boost::tuple<int, int> findMinMaxIndex(
114  const int width, const int height,
115  const std::vector<int>& indices);
116 
117  virtual void integrateDuplicatedIndices(
118  const pcl::PointCloud<PointT>::Ptr& cloud,
119  const std::set<int>& duplicated_set,
120  const std::vector<pcl::PointIndices::Ptr> all_inliers,
121  pcl::PointIndices::Ptr& output_indices);
122 
123  virtual void configCallback (Config &config, uint32_t level);
124 
125  virtual void subscribe();
126  virtual void unsubscribe();
127 
129  // ROS variables
140  // outlier removal
145  // duplication removal
149  private:
150 
151  };
152 }
153 
154 #endif
virtual void removeDuplicatedEdges(const pcl::PointCloud< PointT >::Ptr &cloud, const std::vector< pcl::PointIndices::Ptr > inliers, const std::vector< pcl::ModelCoefficients::Ptr > coefficients, std::vector< pcl::PointIndices::Ptr > &output_inliers, std::vector< pcl::ModelCoefficients::Ptr > &output_coefficients)
message_filters::Subscriber< jsk_recognition_msgs::ClusterPointIndices > sub_indices_
virtual jsk_recognition_utils::Line::Ptr lineFromCoefficients(const pcl::ModelCoefficients::Ptr coefficients)
message_filters::sync_policies::ExactTime< sensor_msgs::PointCloud2, jsk_recognition_msgs::ClusterPointIndices > SyncPolicy
virtual boost::tuple< int, int > findMinMaxIndex(const int width, const int height, const std::vector< int > &indices)
virtual void removeOutliers(const pcl::PointCloud< PointT >::Ptr &cloud, const std::vector< PCLIndicesMsg > &indices, std::vector< pcl::PointIndices::Ptr > &output_inliers, std::vector< pcl::ModelCoefficients::Ptr > &output_coefficients)
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_input_
virtual void publishIndices(ros::Publisher &pub, ros::Publisher &pub_coefficients, ros::Publisher &pub_edges, const std::vector< pcl::PointIndices::Ptr > inliers, const std::vector< pcl::ModelCoefficients::Ptr > coefficients, const std_msgs::Header &header)
virtual void configCallback(Config &config, uint32_t level)
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
virtual void removeOutliersByLine(const pcl::PointCloud< PointT >::Ptr &cloud, const std::vector< int > &indices, pcl::PointIndices &inliers, pcl::ModelCoefficients &coefficients)
jsk_pcl_ros::EdgeDepthRefinementConfig Config
boost::mutex mutex
global mutex.
virtual void refine(const sensor_msgs::PointCloud2ConstPtr &point, const jsk_recognition_msgs::ClusterPointIndicesConstPtr &indices)
virtual void integrateDuplicatedIndices(const pcl::PointCloud< PointT >::Ptr &cloud, const std::set< int > &duplicated_set, const std::vector< pcl::PointIndices::Ptr > all_inliers, pcl::PointIndices::Ptr &output_indices)
virtual jsk_recognition_utils::Segment::Ptr segmentFromIndices(const pcl::PointCloud< PointT >::Ptr &cloud, const std::vector< int > &indices, const jsk_recognition_utils::Line::Ptr &line)


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Mon May 3 2021 03:03:46