region_growing_multiple_plane_segmentation.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_REGION_GROWING_MULTIPLE_PLANE_SEGMENTATION_H_
38 #define JSK_PCL_ROS_REGION_GROWING_MULTIPLE_PLANE_SEGMENTATION_H_
39 
40 #include <dynamic_reconfigure/server.h>
41 #include "jsk_pcl_ros/RegionGrowingMultiplePlaneSegmentationConfig.h"
48 #include "jsk_recognition_msgs/PolygonArray.h"
49 #include "jsk_recognition_msgs/ClusterPointIndices.h"
50 #include "jsk_recognition_msgs/ModelCoefficientsArray.h"
52 #include <std_msgs/Float32.h>
53 
54 namespace jsk_pcl_ros
55 {
58  {
59  public:
60  typedef pcl::PointXYZRGB PointT;
61  typedef RegionGrowingMultiplePlaneSegmentationConfig
64  sensor_msgs::PointCloud2,
65  sensor_msgs::PointCloud2 > NormalSyncPolicy;
67  : DiagnosticNodelet("RegionGrowingMultiplePlaneSegmentation"),
68  timer_(10),
69  done_initialization_(false) {}
70 
71  protected:
73  // methods
75  virtual void onInit();
76  virtual void subscribe();
77  virtual void unsubscribe();
78  virtual void segment(
79  const sensor_msgs::PointCloud2::ConstPtr& msg,
80  const sensor_msgs::PointCloud2::ConstPtr& normal_msg);
81  virtual void configCallback (Config &config, uint32_t level);
83  // static methods
85  // static method
87  const double angular_threshold,
88  const double distance_threshold)
89  {
90  global_angular_threshold = angular_threshold;
91  global_distance_threshold = distance_threshold;
92  }
93  virtual void ransacEstimation(
94  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud,
95  const pcl::PointIndices::Ptr& indices,
96  pcl::PointIndices& inliers,
97  pcl::ModelCoefficients& coefficient);
98 
99  static bool regionGrowingFunction(const pcl::PointXYZRGBNormal& a,
100  const pcl::PointXYZRGBNormal& b,
101  float distance)
102  {
103  if (distance > global_distance_threshold) {
104  return false;
105  }
106  else {
107  Eigen::Vector3f a_normal(a.normal_x, a.normal_y, a.normal_z);
108  Eigen::Vector3f b_normal(b.normal_x, b.normal_y, b.normal_z);
109  double dot = std::abs(a_normal.dot(b_normal));
110  double angle;
111  if (dot > 1.0) {
112  angle = acos(1.0);
113  }
114  else if (dot < -1.0) {
115  angle = acos(-1.0);
116  }
117  else {
118  angle = acos(dot);
119  }
120  //ROS_INFO("angle: %f", angle);
121  if (angle > global_angular_threshold) {
122  return false;
123  }
124  else {
125  return true;
126  }
127  }
128  }
129 
131  // ROS variables
147  // Parameters
154  double min_area_;
155  double max_area_;
161  // static parameters
166  private:
167 
168  };
169 }
170 
171 #endif
boost::shared_ptr< message_filters::Synchronizer< NormalSyncPolicy > > sync_
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_normal_
virtual void segment(const sensor_msgs::PointCloud2::ConstPtr &msg, const sensor_msgs::PointCloud2::ConstPtr &normal_msg)
config
T dot(T *pf1, T *pf2, int length)
DiagnosticNodelet(const std::string &name)
GLfloat angle
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
boost::mutex mutex
global mutex.
virtual void ransacEstimation(const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, const pcl::PointIndices::Ptr &indices, pcl::PointIndices &inliers, pcl::ModelCoefficients &coefficient)
message_filters::sync_policies::ExactTime< sensor_msgs::PointCloud2, sensor_msgs::PointCloud2 > NormalSyncPolicy
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
static void setCondifionFunctionParameter(const double angular_threshold, const double distance_threshold)
cloud
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_input_
static bool regionGrowingFunction(const pcl::PointXYZRGBNormal &a, const pcl::PointXYZRGBNormal &b, float distance)


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