edgebased_cube_finder.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_BASED_CUBE_FINDER_H_
38 #define JSK_PCL_ROS_EDGE_BASED_CUBE_FINDER_H_
39 
40 #include <pcl_ros/pcl_nodelet.h>
41 
43 // messages
45 #include <sensor_msgs/PointCloud2.h>
46 #include <jsk_recognition_msgs/ParallelEdge.h>
47 #include <jsk_recognition_msgs/PolygonArray.h>
48 #include <jsk_recognition_msgs/BoundingBoxArray.h>
49 #include <jsk_recognition_msgs/ParallelEdgeArray.h>
50 
54 
58 
59 #include <boost/tuple/tuple.hpp>
60 #include <jsk_pcl_ros/EdgebasedCubeFinderConfig.h>
61 #include <dynamic_reconfigure/server.h>
62 
64 
65 namespace jsk_pcl_ros
66 {
67  typedef boost::tuple<pcl::PointIndices::Ptr, pcl::PointIndices::Ptr>
69  typedef boost::tuple<pcl::ModelCoefficients::Ptr, pcl::ModelCoefficients::Ptr>
71 
72  typedef boost::tuple<pcl::PointIndices::Ptr, pcl::PointIndices::Ptr, pcl::PointIndices::Ptr>
74  typedef boost::tuple<pcl::ModelCoefficients::Ptr, pcl::ModelCoefficients::Ptr, pcl::ModelCoefficients::Ptr>
76  typedef boost::tuple<IndicesTriple, CoefficientsTriple>
78 
80  // class to represent a hypothesis
81  // value_ := 0.0 ~ 1.0
84  {
85  public:
87  CubeHypothesis(const IndicesPair& pair,
88  const CoefficientsPair& coefficients_pair,
89  const double outlier_threshold);
90  virtual ~CubeHypothesis();
91  virtual double getValue() { return value_; };
93  virtual void estimate(const pcl::PointCloud<pcl::PointXYZRGB>& cloud) = 0;
94 
95  virtual double evaluatePointOnPlanes(
96  const pcl::PointCloud<pcl::PointXYZRGB>& cloud,
100  const jsk_recognition_utils::Line& axis,
101  const jsk_recognition_utils::PointPair& a_candidates,
102  const jsk_recognition_utils::PointPair& b_candidates);
103  protected:
105  // methods
107  virtual void computeCentroid(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud,
108  const pcl::PointIndices::Ptr& indices,
109  Eigen::Vector3f& output);
110  virtual void getLinePoints(const jsk_recognition_utils::Line& line,
111  const pcl::PointCloud<pcl::PointXYZRGB>& cloud,
112  const pcl::PointIndices::Ptr indices,
115  const jsk_recognition_utils::PointPair& a_edge_pair, const jsk_recognition_utils::PointPair& b_edge_pair);
116 
118  // variables
120  double value_;
125  private:
126  };
127 
129  {
130  public:
132  PlanarCubeHypothesis(const IndicesPair& pair,
133  const CoefficientsPair& coefficients_pair,
134  const double outlier_threshold);
135  virtual void estimate(const pcl::PointCloud<pcl::PointXYZRGB>& cloud)
136  {
137  }
138 
139  protected:
140  private:
141  };
142 
144  {
145  public:
148  const CoefficientsPair& coefficients_pair,
149  const double outlier_threshold);
150  virtual void estimate(const pcl::PointCloud<pcl::PointXYZRGB>& cloud);
151  protected:
153  double min_angle_;
154  private:
155  };
156 
158  {
159  public:
161  sensor_msgs::PointCloud2,
162  jsk_recognition_msgs::ParallelEdgeArray > SyncPolicy;
163  typedef pcl::PointXYZRGB PointT;
164  typedef jsk_pcl_ros::EdgebasedCubeFinderConfig Config;
166  {
170  C_PERPENDICULAR
171  };
172 
173  protected:
174 
175 
177  // methods
179  virtual void onInit();
180 
181  virtual void estimate(
182  const sensor_msgs::PointCloud2::ConstPtr& input_cloud,
183  const jsk_recognition_msgs::ParallelEdgeArray::ConstPtr& input_edges);
184  virtual void estimate2(
185  const sensor_msgs::PointCloud2::ConstPtr& input_cloud,
186  const jsk_recognition_msgs::ParallelEdgeArray::ConstPtr& input_edges);
187 
189  // combinateIndices
190  // indices := list of pcl::PointIndices. the length of the
191  // list should be greater equal than 2.
193  virtual std::vector<IndicesPair> combinateIndices(
194  const std::vector<pcl::PointIndices::Ptr>& indices);
195 
196  virtual std::vector<CoefficientsPair> combinateCoefficients(
197  const std::vector<pcl::ModelCoefficients::Ptr>& coefficients);
198 
199  virtual bool isPerpendicularVector(
200  const Eigen::Vector3f& a,
201  const Eigen::Vector3f& b);
202 
203  virtual std::vector<IndicesCoefficientsTriple>
204  tripleIndicesAndCoefficients(
205  const std::vector<pcl::PointIndices::Ptr>& indices,
206  const std::vector<pcl::ModelCoefficients::Ptr>& coefficients);
207 
208  virtual EdgeRelation perpendicularEdgeTriple(
209  const jsk_recognition_utils::Line& edge_a,
210  const jsk_recognition_utils::Line& edge_b,
211  const jsk_recognition_utils::Line& edge_c);
212 
213 
214  virtual std::vector<IndicesCoefficientsTriple>
215  filterPerpendicularEdgeTriples(
216  const std::vector<IndicesCoefficientsTriple>& triples);
217 
218  virtual jsk_recognition_utils::Line::Ptr midLineFromCoefficientsPair(
219  const CoefficientsPair& pair);
220 
221  virtual jsk_recognition_utils::ConvexPolygon::Ptr convexFromPairs(
222  const pcl::PointCloud<PointT>::Ptr cloud,
223  const CoefficientsPair& coefficients_pair,
224  const IndicesPair& indices_pair);
225  virtual int countInliers(
226  const pcl::PointCloud<PointT>::Ptr cloud,
228  virtual void filterBasedOnConvex(
229  const pcl::PointCloud<PointT>::Ptr cloud,
230  const std::vector<jsk_recognition_utils::ConvexPolygon::Ptr>& convexes,
231  std::vector<int>& output_indices);
232  virtual void filterPairsBasedOnParallelEdgeDistances(
233  const std::vector<IndicesPair>& pairs,
234  const std::vector<CoefficientsPair>& coefficients_pair,
235  std::vector<IndicesPair>& filtered_indices_pairs,
236  std::vector<CoefficientsPair>& filtered_coefficients_pairs);
237 
238  virtual jsk_recognition_utils::Cube::Ptr cubeFromIndicesAndCoefficients(
239  const pcl::PointCloud<PointT>::Ptr cloud,
240  const IndicesCoefficientsTriple& indices_coefficients_triple,
241  pcl::PointCloud<EdgebasedCubeFinder::PointT>::Ptr points_on_edge);
242 
243  virtual pcl::PointCloud<PointT>::Ptr extractPointCloud(
244  const pcl::PointCloud<PointT>::Ptr cloud,
245  const pcl::PointIndices::Ptr indices);
246 
247  virtual jsk_recognition_utils::PointPair minMaxPointOnLine(
249  const pcl::PointCloud<PointT>::Ptr cloud);
250 
251  virtual void estimateParallelPlane(
253  const pcl::PointCloud<PointT>::Ptr filtered_cloud,
254  pcl::PointIndices::Ptr output_inliers,
255  pcl::ModelCoefficients::Ptr output_coefficients);
256 
257  virtual void estimatePerpendicularPlane(
259  const CoefficientsPair& edge_coefficients,
260  const pcl::PointCloud<PointT>::Ptr filtered_cloud,
261  pcl::PointIndices::Ptr output_inliers,
262  pcl::ModelCoefficients::Ptr output_coefficients);
263 
264  //virtual pcl::PointCloud<EdgebasedCubeFinder::PointT>::Ptr
265  virtual pcl::PointIndices::Ptr
266  preparePointCloudForRANSAC(
268  const CoefficientsPair& edge_coefficients_pair,
269  const pcl::PointCloud<PointT>::Ptr cloud);
270 
272  estimateConvexPolygon(
273  const pcl::PointCloud<PointT>::Ptr cloud,
274  const pcl::PointIndices::Ptr indices,
275  pcl::ModelCoefficients::Ptr coefficients,
276  pcl::PointIndices::Ptr inliers);
277 
278  virtual void configCallback (Config &config, uint32_t level);
279 
280  virtual void subscribe();
281  virtual void unsubscribe();
283  // ROS variables
291  ros::Publisher pub_debug_marker_, pub_debug_filtered_cloud_,
292  pub_debug_polygons_, pub_debug_clusers_;
295  // parameters for cube finnding
298  double min_inliers_;
301  double parallel_edge_distance_min_threshold_, parallel_edge_distance_max_threshold_;
302  private:
303 
304  };
305 }
306 
307 #endif
virtual void getLinePoints(const jsk_recognition_utils::Line &line, const pcl::PointCloud< pcl::PointXYZRGB > &cloud, const pcl::PointIndices::Ptr indices, jsk_recognition_utils::Vertices &output)
virtual void estimate(const pcl::PointCloud< pcl::PointXYZRGB > &cloud)=0
boost::shared_ptr< PlanarCubeHypothesis > Ptr
virtual jsk_recognition_utils::PointPair computeAxisEndPoints(const jsk_recognition_utils::Line &axis, const jsk_recognition_utils::PointPair &a_candidates, const jsk_recognition_utils::PointPair &b_candidates)
boost::tuple< pcl::ModelCoefficients::Ptr, pcl::ModelCoefficients::Ptr > CoefficientsPair
virtual jsk_recognition_utils::ConvexPolygon::Ptr buildConvexPolygon(const jsk_recognition_utils::PointPair &a_edge_pair, const jsk_recognition_utils::PointPair &b_edge_pair)
boost::tuple< pcl::ModelCoefficients::Ptr, pcl::ModelCoefficients::Ptr, pcl::ModelCoefficients::Ptr > CoefficientsTriple
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_input_
virtual double evaluatePointOnPlanes(const pcl::PointCloud< pcl::PointXYZRGB > &cloud, jsk_recognition_utils::ConvexPolygon &polygon_a, jsk_recognition_utils::ConvexPolygon &polygon_b)
virtual void estimate(const pcl::PointCloud< pcl::PointXYZRGB > &cloud)
boost::tuple< pcl::PointIndices::Ptr, pcl::PointIndices::Ptr > IndicesPair
message_filters::Subscriber< jsk_recognition_msgs::ParallelEdgeArray > sub_edges_
void subscribe()
void output(int index, double value)
message_filters::sync_policies::ExactTime< sensor_msgs::PointCloud2, jsk_recognition_msgs::ParallelEdgeArray > SyncPolicy
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
jsk_pcl_ros::EdgebasedCubeFinderConfig Config
const CoefficientsPair coefficients_pair_
boost::mutex mutex
global mutex.
std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > Vertices
virtual jsk_recognition_utils::Cube::Ptr getCube()
virtual void computeCentroid(const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, const pcl::PointIndices::Ptr &indices, Eigen::Vector3f &output)
boost::shared_ptr< DiagnoalCubeHypothesis > Ptr
boost::tuple< Point, Point > PointPair
boost::tuple< IndicesTriple, CoefficientsTriple > IndicesCoefficientsTriple
boost::tuple< pcl::PointIndices::Ptr, pcl::PointIndices::Ptr, pcl::PointIndices::Ptr > IndicesTriple
CubeHypothesis(const IndicesPair &pair, const CoefficientsPair &coefficients_pair, const double outlier_threshold)
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
cloud
jsk_recognition_utils::Cube::Ptr cube_
boost::shared_ptr< CubeHypothesis > Ptr


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