line_segment_collector_nodelet.cpp
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 #define BOOST_PARAMETER_MAX_ARITY 7
39 #include <pcl/sample_consensus/method_types.h>
40 #include <pcl/sample_consensus/model_types.h>
41 #include <pcl/segmentation/sac_segmentation.h>
42 
43 namespace jsk_pcl_ros
44 {
46  delta_(Eigen::Vector3f(0, 0, 0)),
47  points_(new pcl::PointCloud<pcl::PointXYZ>),
48  raw_points_(new pcl::PointCloud<pcl::PointXYZ>)
49  {
50 
51  }
52 
54  LineSegment::Ptr segment, const double tau)
55  {
56  segments_.push_back(segment);
57  Eigen::Vector3f new_delta = segment->toSegment()->getDirection();
58  if (new_delta.dot(delta_) < 0) {
59  new_delta = - new_delta;
60  }
61  delta_ = ((1 - tau) * delta_ + tau * new_delta).normalized();
62 
63  // update points_
64  pcl::PointCloud<pcl::PointXYZ>::Ptr new_cloud = segment->getPoints();
65  for (size_t i = 0; i < new_cloud->points.size(); i++) {
66  points_->points.push_back(new_cloud->points[i]);
67  }
68  // update raw_points_
69  pcl::PointCloud<pcl::PointXYZ>::Ptr new_raw_cloud = segment->getRawPoints();
70  for (size_t i = 0; i < new_raw_cloud->points.size(); i++) {
71  raw_points_->points.push_back(new_raw_cloud->points[i]);
72  }
73  }
74 
75  pcl::PointCloud<pcl::PointXYZ>::Ptr LineSegmentCluster::getPoints()
76  {
77  return points_;
78  }
79 
80  pcl::PointCloud<pcl::PointXYZ>::Ptr LineSegmentCluster::getRawPoints()
81  {
82  return raw_points_;
83  }
84 
86  {
87  bool removed = false;
88  for (std::vector<LineSegment::Ptr>::iterator it = segments_.begin();
89  it != segments_.end(); ) {
90  if (((*it)->header.stamp - stamp).toSec() < 0) {
91  it = segments_.erase(it);
92  removed = true;
93  }
94  else {
95  ++it;
96  }
97  }
98  if (removed) {
99  // reconstruct pointcloud
100  points_.reset(new pcl::PointCloud<pcl::PointXYZ>);
101 
102  raw_points_.reset(new pcl::PointCloud<pcl::PointXYZ>);
103  for (std::vector<LineSegment::Ptr>::iterator it = segments_.begin();
104  it != segments_.end(); ++it) {
105  {
106  pcl::PointCloud<pcl::PointXYZ>::Ptr segment_points = (*it)->getPoints();
107  for (size_t i = 0; i < segment_points->points.size(); i++) {
108  points_->points.push_back(segment_points->points[i]);
109  }
110  }
111  {
112  pcl::PointCloud<pcl::PointXYZ>::Ptr segment_points = (*it)->getRawPoints();
113  for (size_t i = 0; i < segment_points->points.size(); i++) {
114  raw_points_->points.push_back(segment_points->points[i]);
115  }
116  }
117  }
118  }
119  }
120 
122  {
123  return segments_.size() == 0;
124  }
125 
127  {
128  DiagnosticNodelet::onInit();
129  srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
130  dynamic_reconfigure::Server<Config>::CallbackType f =
131  boost::bind (&LineSegmentCollector::configCallback, this, _1, _2);
132  srv_->setCallback (f);
133 
134  std::string rotate_type_str;
135  pnh_->param("rotate_type", rotate_type_str, std::string("tilt_two_way"));
136  if (rotate_type_str == "tilt") {
137  rotate_type_ = ROTATION_TILT;
138  }
139  else if (rotate_type_str == "tilt_two_way") {
140  rotate_type_ = ROTATION_TILT_TWO_WAY;
141  }
142  else if (rotate_type_str == "spindle") {
143  rotate_type_ = ROTATION_SPINDLE;
144  }
145  else {
146  NODELET_ERROR("unknown ~rotate_type: %s", rotate_type_str.c_str());
147  return;
148  }
149 
150  pub_point_cloud_
151  = advertise<sensor_msgs::PointCloud2>(*pnh_, "output/cloud", 1);
152  pub_inliers_ = advertise<jsk_recognition_msgs::ClusterPointIndices>(*pnh_, "output/inliers", 1);
153  pub_coefficients_
154  = advertise<jsk_recognition_msgs::ModelCoefficientsArray>(*pnh_, "output/coefficients", 1);
155  pub_polygons_
156  = advertise<jsk_recognition_msgs::PolygonArray>(*pnh_, "output/polygons", 1);
157  debug_pub_inliers_before_plane_
158  = advertise<jsk_recognition_msgs::ClusterPointIndices>(
159  *pnh_, "debug/connect_segments/inliers", 1);
160 
161  onInitPostProcess();
162  }
163 
164  void LineSegmentCollector::configCallback(Config &config, uint32_t level)
165  {
166  boost::mutex::scoped_lock lock(mutex_);
167  ewma_tau_ = config.ewma_tau;
168  segment_connect_normal_threshold_ = config.segment_connect_normal_threshold;
169  outlier_threshold_ = config.outlier_threshold;
170  }
171 
173  {
174  sub_input_.subscribe(*pnh_, "input", 1);
175  sub_indices_.subscribe(*pnh_, "input_indices", 1);
176  sub_coefficients_.subscribe(*pnh_, "input_coefficients", 1);
177  sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
178  sync_->connectInput(sub_input_, sub_indices_, sub_coefficients_);
179  sync_->registerCallback(boost::bind(&LineSegmentCollector::collect,
180  this, _1, _2, _3));
181  sub_trigger_ = pnh_->subscribe("trigger", 1,
183  }
184 
186  {
187  sub_input_.unsubscribe();
188  sub_indices_.unsubscribe();
189  sub_coefficients_.unsubscribe();
190  sub_trigger_.shutdown();
191  }
192 
194  const ros::Time& stamp)
195  {
196  pointclouds_buffer_.removeBefore(stamp);
197  indices_buffer_.removeBefore(stamp);
198  coefficients_buffer_.removeBefore(stamp);
199  segments_buffer_.removeBefore(stamp);
200  for (std::vector<LineSegmentCluster::Ptr>::iterator it = segment_clusters_.begin();
201  it != segment_clusters_.end();) {
202  (*it)->removeBefore(stamp);
203  if ((*it)->isEmpty()) {
204  it = segment_clusters_.erase(it);
205  }
206  else {
207  ++it;
208  }
209  }
210  }
211 
213  const jsk_recognition_msgs::TimeRange::ConstPtr& trigger)
214  {
215  boost::mutex::scoped_lock lock(mutex_);
216  time_range_ = trigger;
217  cleanupBuffers(time_range_->start);
218  }
219 
221  const std_msgs::Header& header,
222  const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
223  const std::vector<pcl::PointIndices::Ptr>& connected_indices)
224  {
225  sensor_msgs::PointCloud2 ros_cloud;
226  pcl::toROSMsg(*cloud, ros_cloud);
227  ros_cloud.header = header;
228  pub_point_cloud_.publish(ros_cloud);
229  jsk_recognition_msgs::ClusterPointIndices ros_indices;
230  ros_indices.header = header;
231  ros_indices.cluster_indices
232  = pcl_conversions::convertToROSPointIndices(connected_indices, header);
233  debug_pub_inliers_before_plane_.publish(ros_indices);
234  }
235 
237  LineSegment::Ptr segment)
238  {
239  int max_index = -1;
240  double max_dot = - DBL_MAX;
241  for (size_t i = 0; i < segment_clusters_.size(); i++) {
242  LineSegmentCluster::Ptr cluster = segment_clusters_[i];
243  Eigen::Vector3f delta_cluster = cluster->getDelta();
244  Eigen::Vector3f delta = segment->toSegment()->getDirection();
245  double delta_dot = std::abs(delta_cluster.dot(delta));
246  if (delta_dot > segment_connect_normal_threshold_) {
247  if (max_dot < delta_dot) {
248  max_dot = delta_dot;
249  max_index = i;
250  }
251  }
252  // else {
253  // if (segment_clusters_.size() != 0) {
254  // NODELET_INFO("dot: %f", delta_dot);
255  // }
256  // }
257  }
258  if (max_index == -1) {
259 
260  return LineSegmentCluster::Ptr();
261  }
262  else {
263  //ROS_INFO("max angle: %f", acos(max_dot) * 180.0 / M_PI);
264  return segment_clusters_[max_index];
265  }
266  }
267 
269  const std_msgs::Header& header,
270  std::vector<LineSegment::Ptr> new_segments)
271  {
272  for (size_t i = 0; i < new_segments.size(); i++) {
273  LineSegment::Ptr segment = new_segments[i];
274  LineSegmentCluster::Ptr cluster = lookupNearestSegment(segment);
275  if (cluster) {
276  cluster->addLineSegmentEWMA(segment, ewma_tau_);
277  }
278  else {
279  cluster.reset(new LineSegmentCluster());
280  cluster->addLineSegmentEWMA(segment, 1.0);
281  segment_clusters_.push_back(cluster);
282  }
283  }
284 
285  pcl::PointCloud<pcl::PointXYZ>::Ptr
286  connected_cloud(new pcl::PointCloud<pcl::PointXYZ>);
287  std::vector<pcl::PointIndices::Ptr> connected_indices;
288  std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr > clouds_list;
289  for (size_t i = 0; i < segment_clusters_.size(); i++) {
290  LineSegmentCluster::Ptr cluster = segment_clusters_[i];
291  pcl::PointIndices::Ptr current_indices (new pcl::PointIndices);
292  pcl::PointCloud<pcl::PointXYZ>::Ptr current_cloud
293  = cluster->getRawPoints();
294  for (size_t j = 0; j < current_cloud->points.size(); j++) {
295  current_indices->indices.push_back(connected_cloud->points.size() + j);
296  }
297  connected_indices.push_back(current_indices);
298  clouds_list.push_back(current_cloud);
299  *connected_cloud = *connected_cloud + *current_cloud;
300  }
301  // publish debug information
302  publishBeforePlaneSegmentation(
303  header,
304  connected_cloud,
305  connected_indices);
306  }
307 
309  const std_msgs::Header& header,
310  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
311  std::vector<pcl::ModelCoefficients::Ptr> all_coefficients,
312  std::vector<pcl::PointIndices::Ptr> all_indices)
313  {
314  jsk_recognition_msgs::ClusterPointIndices ros_indices;
315  ros_indices.header = header;
316  ros_indices.cluster_indices
318  header);
319  pub_inliers_.publish(ros_indices);
320  jsk_recognition_msgs::ModelCoefficientsArray ros_coefficients;
321  ros_coefficients.header = header;
322  ros_coefficients.coefficients
324  all_coefficients,
325  header);
326  pub_coefficients_.publish(ros_coefficients);
327  jsk_recognition_msgs::PolygonArray ros_polygon;
328  ros_polygon.header = header;
329  for (size_t i = 0; i < all_indices.size(); i++) {
331  = jsk_recognition_utils::convexFromCoefficientsAndInliers<pcl::PointXYZ>(
332  cloud, all_indices[i], all_coefficients[i]);
333  geometry_msgs::PolygonStamped polygon_stamped;
334  polygon_stamped.header = header;
335  polygon_stamped.polygon = convex->toROSMsg();
336  ros_polygon.polygons.push_back(polygon_stamped);
337  }
338  pub_polygons_.publish(ros_polygon);
339  }
340 
342  const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
343  const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& indices_msg,
344  const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr& coefficients_msg)
345  {
346  boost::mutex::scoped_lock lock(mutex_);
347  //NODELET_INFO("buffer length: %lu", pointclouds_buffer_.size());
348  pointclouds_buffer_.push_back(cloud_msg);
349  indices_buffer_.push_back(indices_msg);
350  coefficients_buffer_.push_back(coefficients_msg);
351  pcl::PointCloud<pcl::PointXYZ>::Ptr
352  input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
353  pcl::fromROSMsg(*cloud_msg, *input_cloud);
354  // buildup segments
355  std::vector<pcl::PointIndices::Ptr> input_indices
356  = pcl_conversions::convertToPCLPointIndices(indices_msg->cluster_indices);
357  std::vector<pcl::ModelCoefficients::Ptr> input_coefficients
359  coefficients_msg->coefficients);
360  std::vector<LineSegment::Ptr> new_segments;
361  for (size_t i = 0; i < indices_msg->cluster_indices.size(); i++) {
362  LineSegment::Ptr segment (new LineSegment(cloud_msg->header,
363  input_indices[i],
364  input_coefficients[i],
365  input_cloud));
366  segments_buffer_.push_back(segment);
367  new_segments.push_back(segment);
368  }
369  collectFromBuffers(cloud_msg->header, new_segments);
370  }
371 
372 }
373 
virtual LineSegmentCluster::Ptr lookupNearestSegment(LineSegment::Ptr segment)
virtual pcl::PointCloud< pcl::PointXYZ >::Ptr getRawPoints()
virtual void addLineSegmentEWMA(LineSegment::Ptr segment, const double tau)
#define NODELET_ERROR(...)
virtual void publishResult(const std_msgs::Header &header, pcl::PointCloud< pcl::PointXYZ >::Ptr cloud, std::vector< pcl::ModelCoefficients::Ptr > all_coefficients, std::vector< pcl::PointIndices::Ptr > all_indices)
virtual void removeBefore(const ros::Time &stamp)
virtual void collect(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg, const jsk_recognition_msgs::ClusterPointIndices::ConstPtr &indices_msg, const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr &coefficients_msg)
std::vector< PCLModelCoefficientMsg > convertToROSModelCoefficients(const std::vector< pcl::ModelCoefficients::Ptr > &coefficients, const std_msgs::Header &header)
std::vector< LineSegment::Ptr > segments_
std::vector< pcl::ModelCoefficients::Ptr > convertToPCLModelCoefficients(const std::vector< PCLModelCoefficientMsg > &coefficients)
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
virtual void cleanupBuffers(const ros::Time &stamp)
jsk_pcl_ros::LineSegmentCollectorConfig Config
std::vector< pcl::PointIndices::Ptr > convertToPCLPointIndices(const std::vector< PCLIndicesMsg > &cluster_indices)
virtual void configCallback(Config &config, uint32_t level)
sensor_msgs::PointCloud2 PointCloud
int max_index
pcl::PointCloud< pcl::PointXYZ >::Ptr points_
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::LineSegmentCollector, nodelet::Nodelet)
virtual void collectFromBuffers(const std_msgs::Header &header, std::vector< LineSegment::Ptr > new_segments)
void toROSMsg(const pcl::PointCloud< T > &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
std::vector< PCLIndicesMsg > convertToROSPointIndices(const std::vector< pcl::PointIndices::Ptr > cluster_indices, const std_msgs::Header &header)
virtual void publishBeforePlaneSegmentation(const std_msgs::Header &header, const pcl::PointCloud< pcl::PointXYZ >::Ptr cloud, const std::vector< pcl::PointIndices::Ptr > &connected_indices)
boost::mutex mutex_
boost::shared_ptr< LineSegmentCluster > Ptr
pcl::PointCloud< pcl::PointXYZ >::Ptr raw_points_
virtual void triggerCallback(const jsk_recognition_msgs::TimeRange::ConstPtr &trigger)
virtual pcl::PointCloud< pcl::PointXYZ >::Ptr getPoints()


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