geometric_consistency_grouping_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
36 #include <pcl/kdtree/kdtree_flann.h>
37 #include <pcl/kdtree/impl/kdtree_flann.hpp>
40 #include <pcl/recognition/cg/geometric_consistency.h>
41 
42 namespace jsk_pcl_ros
43 {
45  {
46  DiagnosticNodelet::onInit();
47  srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
48  typename dynamic_reconfigure::Server<Config>::CallbackType f =
49  boost::bind (&GeometricConsistencyGrouping::configCallback, this, _1, _2);
50  srv_->setCallback (f);
51  pub_output_ = advertise<geometry_msgs::PoseStamped>(*pnh_, "output", 1);
52  pub_output_cloud_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output/cloud", 1);
53 
54  reference_sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
55  sub_reference_cloud_.subscribe(*pnh_, "input/reference", 1);
56  sub_reference_feature_.subscribe(*pnh_, "input/reference/feature", 1);
57  reference_sync_->connectInput(sub_reference_cloud_, sub_reference_feature_);
58  reference_sync_->registerCallback(
60  this, _1, _2));
62  }
63 
65  {
66  sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
67  sub_input_.subscribe(*pnh_, "input", 1);
68  sub_feature_.subscribe(*pnh_, "input/feature", 1);
69  sync_->connectInput(sub_input_, sub_feature_);
70  sync_->registerCallback(boost::bind(&GeometricConsistencyGrouping::recognize,
71  this, _1, _2));
72  }
73 
75  {
78  }
79 
81  Config& config, uint32_t level)
82  {
83  boost::mutex::scoped_lock lock(mutex_);
84  gc_size_ = config.gc_size;
85  gc_thresh_ = config.gc_thresh;
86  }
87 
89  const sensor_msgs::PointCloud2::ConstPtr& model_cloud_msg,
90  const sensor_msgs::PointCloud2::ConstPtr& model_feature_msg)
91  {
92  boost::mutex::scoped_lock lock(mutex_);
93 
94  reference_cloud_.reset(new pcl::PointCloud<pcl::PointNormal>);
95  reference_feature_.reset(new pcl::PointCloud<pcl::SHOT352>);
96 
97  pcl::fromROSMsg(*model_cloud_msg, *reference_cloud_);
98  pcl::fromROSMsg(*model_feature_msg, *reference_feature_);
99  }
100 
102  const sensor_msgs::PointCloud2::ConstPtr& scene_cloud_msg,
103  const sensor_msgs::PointCloud2::ConstPtr& scene_feature_msg)
104  {
105  boost::mutex::scoped_lock lock(mutex_);
107  NODELET_ERROR_THROTTLE(1.0, "Not yet reference is available");
108  return;
109  }
110 
111  pcl::PointCloud<pcl::SHOT352>::Ptr
112  scene_feature (new pcl::PointCloud<pcl::SHOT352>);
113  pcl::PointCloud<pcl::PointNormal>::Ptr
114  scene_cloud (new pcl::PointCloud<pcl::PointNormal>);
115  pcl::fromROSMsg(*scene_cloud_msg, *scene_cloud);
116  pcl::fromROSMsg(*scene_feature_msg, *scene_feature);
117 
118  pcl::CorrespondencesPtr model_scene_corrs (new pcl::Correspondences ());
119  pcl::KdTreeFLANN<pcl::SHOT352> match_search;
120  match_search.setInputCloud (reference_feature_);
121 
122  for (size_t i = 0; i < scene_feature->size(); ++i)
123  {
124  std::vector<int> neigh_indices(1);
125  std::vector<float> neigh_sqr_dists(1);
126  if (!pcl_isfinite (scene_feature->at(i).descriptor[0])) { //skipping NaNs
127  continue;
128  }
129  int found_neighs
130  = match_search.nearestKSearch(scene_feature->at(i), 1,
131  neigh_indices, neigh_sqr_dists);
132  // add match only if the squared descriptor distance is less than 0.25
133  // (SHOT descriptor distances are between 0 and 1 by design)
134  if (found_neighs == 1 && neigh_sqr_dists[0] < 0.25f) {
135  pcl::Correspondence corr (neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]);
136  model_scene_corrs->push_back (corr);
137  }
138  }
139 
140  pcl::GeometricConsistencyGrouping<pcl::PointNormal, pcl::PointNormal> gc_clusterer;
141  gc_clusterer.setGCSize(gc_size_);
142  gc_clusterer.setGCThreshold(gc_thresh_);
143 
144  gc_clusterer.setInputCloud(reference_cloud_);
145  gc_clusterer.setSceneCloud(scene_cloud);
146  gc_clusterer.setModelSceneCorrespondences(model_scene_corrs);
147 
148  //gc_clusterer.cluster (clustered_corrs);
149  std::vector<pcl::Correspondences> clustered_corrs;
150  std::vector<Eigen::Matrix4f,
151  Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations;
152  gc_clusterer.recognize(rototranslations, clustered_corrs);
153  if (rototranslations.size() > 0) {
154  NODELET_INFO("detected %lu objects", rototranslations.size());
155  Eigen::Matrix4f result_mat = rototranslations[0];
156  Eigen::Affine3f affine(result_mat);
157  geometry_msgs::PoseStamped ros_pose;
158  tf::poseEigenToMsg(affine, ros_pose.pose);
159  ros_pose.header = scene_cloud_msg->header;
160  pub_output_.publish(ros_pose);
161  }
162  else {
163  NODELET_WARN("Failed to find object");
164  }
165 
166  }
167 
168 }
169 
170 
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::GeometricConsistencyGrouping, nodelet::Nodelet)
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > reference_sync_
#define NODELET_WARN(...)
void publish(const boost::shared_ptr< M > &message) const
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
pcl::PointCloud< pcl::SHOT352 >::Ptr reference_feature_
#define NODELET_ERROR_THROTTLE(rate,...)
virtual void referenceCallback(const sensor_msgs::PointCloud2::ConstPtr &model_cloud_msg, const sensor_msgs::PointCloud2::ConstPtr &model_feature_msg)
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
pcl::PointCloud< pcl::PointNormal >::Ptr reference_cloud_
boost::shared_ptr< ros::NodeHandle > pnh_
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_feature_
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
#define NODELET_INFO(...)
Nodelet implementation of jsk_pcl/GeometricConsistencyGrouping.
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_reference_cloud_
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)
virtual void recognize(const sensor_msgs::PointCloud2::ConstPtr &scene_cloud_msg, const sensor_msgs::PointCloud2::ConstPtr &scene_feature_msg)
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
virtual void configCallback(Config &config, uint32_t level)
callback for dynamic_reconfigure
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_reference_feature_
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_input_


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