hinted_stick_finder_nodelet.cpp
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2015, 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
38 #include <pcl/sample_consensus/method_types.h>
39 #include <pcl/sample_consensus/model_types.h>
40 #include <pcl/segmentation/sac_segmentation.h>
41 #include <pcl/filters/project_inliers.h>
42 #include <pcl/filters/extract_indices.h>
43 #include <pcl/features/normal_3d_omp.h>
44 #include <visualization_msgs/Marker.h>
45 #include <geometry_msgs/PoseStamped.h>
46 
47 namespace jsk_pcl_ros
48 {
50  {
51  DiagnosticNodelet::onInit();
52  srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
53  typename dynamic_reconfigure::Server<Config>::CallbackType f =
54  boost::bind (&HintedStickFinder::configCallback, this, _1, _2);
55  srv_->setCallback (f);
56  pnh_->param("use_normal", use_normal_, false);
57  pnh_->param("not_synchronize", not_synchronize_, false);
58 
59  pub_line_filtered_indices_ = advertise<PCLIndicesMsg>(
60  *pnh_, "debug/line_filtered_indices", 1);
61  pub_line_filtered_normal_ = advertise<sensor_msgs::PointCloud2>(
62  *pnh_, "debug/line_filtered_normal", 1);
63  pub_cylinder_marker_ = advertise<visualization_msgs::Marker>(
64  *pnh_, "debug/cylinder_marker", 1);
65  pub_cylinder_pose_ = advertise<geometry_msgs::PoseStamped>(
66  *pnh_, "output/cylinder_pose", 1);
67  pub_inliers_ = advertise<PCLIndicesMsg>(
68  *pnh_, "output/inliers", 1);
69  pub_coefficients_ = advertise<PCLModelCoefficientMsg>(
70  *pnh_, "output/coefficients", 1);
72  }
73 
75  {
76  if (!not_synchronize_) {
77  sub_polygon_.subscribe(*pnh_, "input/hint/line", 1);
78  sub_info_.subscribe(*pnh_, "input/camera_info", 1);
79  sub_cloud_.subscribe(*pnh_, "input", 1);
80  sync_ = boost::make_shared<message_filters::Synchronizer<ASyncPolicy> >(100);
81  sync_->connectInput(sub_polygon_, sub_info_, sub_cloud_);
82  sync_->registerCallback(boost::bind(&HintedStickFinder::detect, this,
83  _1, _2, _3));
84  }
85  else {
86  sub_no_sync_cloud_ = pnh_->subscribe(
87  "input", 1, &HintedStickFinder::cloudCallback, this);
88  sub_no_sync_camera_info_ = pnh_->subscribe(
89  "input/camera_info", 1, &HintedStickFinder::infoCallback, this);
90  sub_no_sync_polygon_ = pnh_->subscribe(
91  "input/hint/line", 1, &HintedStickFinder::hintCallback, this);
92  }
93  }
94 
95 
97  const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
98  {
99  {
100  boost::mutex::scoped_lock lock(mutex_);
102  // not yet ready
103  NODELET_WARN_THROTTLE(1, "~input/hint/lline or ~input/camera_info is not ready");
104  return;
105  }
106  }
108  }
109 
110 
112  const geometry_msgs::PolygonStamped::ConstPtr& hint_msg)
113  {
114  boost::mutex::scoped_lock lock(mutex_);
115  latest_hint_ = hint_msg;
116  }
117 
119  const sensor_msgs::CameraInfo::ConstPtr& info_msg)
120  {
121  boost::mutex::scoped_lock lock(mutex_);
122  latest_camera_info_ = info_msg;
123  }
124 
125 
127  {
128  if (!not_synchronize_) {
132  }
133  else {
137  }
138  }
139 
141  const geometry_msgs::PolygonStamped::ConstPtr& polygon_msg,
142  const sensor_msgs::CameraInfo::ConstPtr& camera_info_msg,
143  const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
144  {
145  boost::mutex::scoped_lock lock(mutex_);
146  NODELET_WARN("starting detection");
147  ros::Time start_time = ros::Time::now();
149  model.fromCameraInfo(camera_info_msg);
150  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud
151  (new pcl::PointCloud<pcl::PointXYZ>);
152  pcl::fromROSMsg(*cloud_msg, *cloud);
153  // convert 2-D point into 3-D ray
154  Eigen::Vector3f a, b;
156  pcl::PointIndices::Ptr candidate_indices
157  (new pcl::PointIndices);
158 
159  filterPointCloud(cloud, polygon, *candidate_indices);
160  pcl::PointCloud<pcl::Normal>::Ptr normals
161  (new pcl::PointCloud<pcl::Normal>);
162  pcl::PointCloud<pcl::PointXYZ>::Ptr normals_cloud
163  (new pcl::PointCloud<pcl::PointXYZ>);
164  if (!use_normal_) {
165  normalEstimate(cloud, candidate_indices, *normals, *normals_cloud);
166  }
167  else {
168  // we don't need to compute normal
169  pcl::PointCloud<pcl::Normal>::Ptr all_normals
170  (new pcl::PointCloud<pcl::Normal>);
171  pcl::fromROSMsg(*cloud_msg, *all_normals);
172  pcl::ExtractIndices<pcl::PointXYZ> xyz_extract;
173  xyz_extract.setInputCloud(cloud);
174  xyz_extract.setIndices(candidate_indices);
175  xyz_extract.filter(*normals_cloud);
176 
177  pcl::ExtractIndices<pcl::Normal> normal_extract;
178  normal_extract.setInputCloud(all_normals);
179  normal_extract.setIndices(candidate_indices);
180  normal_extract.filter(*normals);
181  }
182  fittingCylinder(normals_cloud, normals, a, b);
183  ros::Time end_time = ros::Time::now();
184  NODELET_WARN("detection time: %f", (end_time - start_time).toSec());
185  }
186 
188  const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
189  const pcl::PointIndices::Ptr indices,
190  pcl::PointCloud<pcl::Normal>& normals,
191  pcl::PointCloud<pcl::PointXYZ>& normals_cloud)
192  {
193  pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> ne;
194  ne.setInputCloud(cloud);
195  ne.setIndices(indices);
196  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree
197  (new pcl::search::KdTree<pcl::PointXYZ> ());
198  ne.setSearchMethod(tree);
199  ne.setRadiusSearch (0.03);
200  ne.compute (normals);
201  pcl::ExtractIndices<pcl::PointXYZ> ex;
202  ex.setInputCloud(cloud);
203  ex.setIndices(indices);
204  ex.filter(normals_cloud);
205  }
206 
208  const jsk_recognition_utils::Cylinder::Ptr& cylinder,
209  const Eigen::Vector3f& a,
210  const Eigen::Vector3f& b)
211  {
212  Eigen::Vector3f hint_dir((b - a));
213  hint_dir[2] = 0;
214  hint_dir.normalize();
215  Eigen::Vector3f cylinder_dir(cylinder->getDirection());
216  cylinder_dir[2] = 0;
217  cylinder_dir.normalize();
218  double ang = acos(cylinder_dir.dot(hint_dir));
219  NODELET_INFO("angle: %f", ang);
220  return !(ang < eps_2d_angle_ || (M_PI - ang) < eps_2d_angle_);
221  }
222 
224  const pcl::PointCloud<pcl::PointXYZ>::Ptr& filtered_cloud,
225  const pcl::PointCloud<pcl::Normal>::Ptr& cloud_normals,
226  const Eigen::Vector3f& a,
227  const Eigen::Vector3f& b)
228  {
229  pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg;
230  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
231  pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
232  Eigen::Vector3f normal = (a - b).normalized();
233  seg.setOptimizeCoefficients (true);
234  seg.setModelType (pcl::SACMODEL_CYLINDER);
235  seg.setMethodType (pcl::SAC_RANSAC);
236  seg.setDistanceThreshold (outlier_threshold_);
237  seg.setMaxIterations (max_iteration_);
238  seg.setNormalDistanceWeight (0.1);
239  seg.setRadiusLimits(min_radius_, max_radius_);
240  // seg.setEpsAngle(eps_angle_);
241  // seg.setAxis(normal);
242  seg.setProbability(min_probability_);
243  seg.setInputCloud(filtered_cloud);
244  seg.setInputNormals(cloud_normals);
245  for (size_t i = 0; i < cylinder_fitting_trial_; i++) {
246  seg.segment(*inliers, *coefficients);
247  if (inliers->indices.size() > min_inliers_) {
248  Eigen::Vector3f dir(coefficients->values[3],
249  coefficients->values[4],
250  coefficients->values[5]);
251  if (dir.dot(Eigen::Vector3f(0, -1, 0)) < 0) {
252  dir = -dir;
253  }
255  coefficients->values[0],
256  coefficients->values[1],
257  coefficients->values[2]),
258  dir,
259  coefficients->values[6]));
260  pcl::PointIndices::Ptr cylinder_indices
261  (new pcl::PointIndices);
262  cylinder->filterPointCloud(*filtered_cloud,
264  *cylinder_indices);
265  double height = 0;
266  Eigen::Vector3f center;
267  cylinder->estimateCenterAndHeight(
268  *filtered_cloud, *cylinder_indices,
269  center, height);
270  if (!rejected2DHint(cylinder, a, b)) {
271  Eigen::Vector3f uz = Eigen::Vector3f(
272  dir).normalized();
273  // build maker
274  visualization_msgs::Marker marker;
275  cylinder->toMarker(marker, center, uz, height);
276  pcl_conversions::fromPCL(filtered_cloud->header, marker.header);
278  geometry_msgs::PoseStamped pose;
279  pose.header = marker.header;
280  pose.pose = marker.pose;
282 
283  PCLIndicesMsg ros_inliers;
284  pcl_conversions::fromPCL(*inliers, ros_inliers);
285  pub_inliers_.publish(ros_inliers);
286  PCLModelCoefficientMsg ros_coefficients;
287  ros_coefficients.header = pcl_conversions::fromPCL(coefficients->header);
288  ros_coefficients.values.push_back(center[0]);
289  ros_coefficients.values.push_back(center[1]);
290  ros_coefficients.values.push_back(center[2]);
291  ros_coefficients.values.push_back(dir[0]);
292  ros_coefficients.values.push_back(dir[1]);
293  ros_coefficients.values.push_back(dir[2]);
294  ros_coefficients.values.push_back(coefficients->values[6]);
295  ros_coefficients.values.push_back(height);
296  pub_coefficients_.publish(ros_coefficients);
297  return;
298  }
299  }
300  NODELET_WARN("failed to detect cylinder [%lu/%d]", i, cylinder_fitting_trial_);
301  }
302  }
303 
305  const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
307  pcl::PointIndices& output_indices)
308  {
309  output_indices.indices.clear();
310  for (size_t i = 0; i < cloud->points.size(); i++) {
311  pcl::PointXYZ p = cloud->points[i];
312  if (!std::isnan(p.x) && !std::isnan(p.y) && !std::isnan(p.z)) {
313  if (polygon->isProjectableInside(p.getVector3fMap())) {
314  if (polygon->distanceSmallerThan(p.getVector3fMap(), filter_distance_)) {
315  output_indices.indices.push_back(i);
316  }
317  }
318  }
319  }
320  output_indices.header = cloud->header;
321  PCLIndicesMsg ros_indices;
322  pcl_conversions::fromPCL(output_indices, ros_indices);
323  pub_line_filtered_indices_.publish(ros_indices);
324  }
325 
326 
328  const geometry_msgs::PolygonStamped::ConstPtr& polygon_msg,
330  Eigen::Vector3f& a,
331  Eigen::Vector3f& b)
332  {
333  cv::Point2d point_a(polygon_msg->polygon.points[0].x,
334  polygon_msg->polygon.points[0].y);
335  cv::Point2d point_b(polygon_msg->polygon.points[1].x,
336  polygon_msg->polygon.points[1].y);
337  cv::Point3d ray_a = model.projectPixelTo3dRay(point_a);
338  cv::Point3d ray_b = model.projectPixelTo3dRay(point_b);
339  a = Eigen::Vector3f(ray_a.x, ray_a.y, ray_a.z);
340  b = Eigen::Vector3f(ray_b.x, ray_b.y, ray_b.z);
341  // 20m is far enough??
342  Eigen::Vector3f far_a = 20.0 * a;
343  Eigen::Vector3f far_b = 20.0 * b;
344  Eigen::Vector3f O(0, 0, 0);
346  vertices.push_back(O);
347  vertices.push_back(far_a);
348  vertices.push_back(far_b);
350  return polygon;
351  }
352 
353  void HintedStickFinder::configCallback(Config &config, uint32_t level)
354  {
355  boost::mutex::scoped_lock lock(mutex_);
356  min_radius_ = config.min_radius;
357  max_radius_ = config.max_radius;
358  filter_distance_ = config.filter_distance;
359  outlier_threshold_ = config.outlier_threshold;
360  max_iteration_ = config.max_iteration;
361  eps_angle_ = config.eps_angle;
362  min_probability_ = config.min_probability;
363  cylinder_fitting_trial_ = config.cylinder_fitting_trial;
364  min_inliers_ = config.min_inliers;
365  eps_2d_angle_ = config.eps_2d_angle;
366  }
367 }
368 
virtual void hintCallback(const geometry_msgs::PolygonStamped::ConstPtr &hint_msg)
Non synchronized message callback for ~input/hint/line.
virtual void normalEstimate(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::PointIndices::Ptr indices, pcl::PointCloud< pcl::Normal > &normals, pcl::PointCloud< pcl::PointXYZ > &normals_cloud)
bool use_normal_
True if use ~input has normal fields.
#define NODELET_WARN(...)
void publish(const boost::shared_ptr< M > &message) const
cv::Point3d projectPixelTo3dRay(const cv::Point2d &uv_rect) const
geometry_msgs::PolygonStamped::ConstPtr latest_hint_
coefficients
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
virtual void cloudCallback(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg)
Non synchronized message callback for ~input pointcloud.
pose
message_filters::Subscriber< sensor_msgs::CameraInfo > sub_info_
virtual void infoCallback(const sensor_msgs::CameraInfo::ConstPtr &info_msg)
Non synchronized message callback for ~input/camera_info.
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
TFSIMD_FORCE_INLINE Vector3 normalized() const
boost::shared_ptr< message_filters::Synchronizer< ASyncPolicy > > sync_
#define M_PI
virtual void fittingCylinder(const pcl::PointCloud< pcl::PointXYZ >::Ptr &filtered_cloud, const pcl::PointCloud< pcl::Normal >::Ptr &cloud_nromals, const Eigen::Vector3f &a, const Eigen::Vector3f &b)
#define NODELET_WARN_THROTTLE(rate,...)
HintedStickFinderConfig Config
bool fromCameraInfo(const sensor_msgs::CameraInfo &msg)
boost::shared_ptr< ros::NodeHandle > pnh_
virtual jsk_recognition_utils::ConvexPolygon::Ptr polygonFromLine(const geometry_msgs::PolygonStamped::ConstPtr &polygon_msg, const image_geometry::PinholeCameraModel &model, Eigen::Vector3f &a, Eigen::Vector3f &b)
sensor_msgs::CameraInfo::ConstPtr latest_camera_info_
bool not_synchronize_
Run in continuous mode. continuous mode means this nodelet does not synchronize hint and input messag...
message_filters::Subscriber< geometry_msgs::PolygonStamped > sub_polygon_
virtual void filterPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const jsk_recognition_utils::ConvexPolygon::Ptr polygon, pcl::PointIndices &output_indices)
std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > Vertices
#define NODELET_INFO(...)
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
height
p
virtual void configCallback(Config &config, uint32_t level)
static Time now()
virtual void detect(const geometry_msgs::PolygonStamped::ConstPtr &polygon_msg, const sensor_msgs::CameraInfo::ConstPtr &camera_info_msg, const sensor_msgs::PointCloud2::ConstPtr &cloud_msg)
Synchronized message callback.
pcl::PointIndices PCLIndicesMsg
cloud
pcl::ModelCoefficients PCLModelCoefficientMsg
virtual bool rejected2DHint(const jsk_recognition_utils::Cylinder::Ptr &cylinder, const Eigen::Vector3f &a, const Eigen::Vector3f &b)
Check direction of cylinder in 2-D image coordinate system and if it is larger than eps_2d_angle_...
std_msgs::Header fromPCL(const pcl::PCLHeader &pcl_header)
char a[26]
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::HintedStickFinder, nodelet::Nodelet)
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_cloud_


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