incremental_model_registration_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
36 
37 #include <sstream>
40 #include <pcl/common/transforms.h>
41 #include <pcl/filters/extract_indices.h>
42 #include <jsk_recognition_msgs/ICPAlign.h>
43 
44 namespace jsk_pcl_ros
45 {
47  {
48 
49  }
50 
51  CapturedSamplePointCloud::CapturedSamplePointCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,
52  const Eigen::Affine3f& pose):
53  original_cloud_(cloud), original_pose_(pose),
54  refined_cloud_(new pcl::PointCloud<pcl::PointXYZRGB>)
55  {
56 
57  }
58 
59  pcl::PointCloud<pcl::PointXYZRGB>::Ptr
61  {
62  return original_cloud_;
63  }
64 
66  {
67  return original_pose_;
68  }
69 
70  pcl::PointCloud<pcl::PointXYZRGB>::Ptr
72  {
73  return refined_cloud_;
74  }
75 
77  {
78  return refined_pose_;
79  }
80 
82  pcl::PointCloud<pcl::PointXYZRGB> cloud)
83  {
84  *refined_cloud_ = cloud; // copying
85  }
86 
88  Eigen::Affine3f pose)
89  {
90  refined_pose_ = Eigen::Affine3f(pose);
91  }
92 
94  {
95  DiagnosticNodelet::onInit();
96  pnh_->param("frame_id", frame_id_,
97  std::string("multisense/left_camera_optical_frame"));
98  start_registration_srv_
99  = pnh_->advertiseService(
101  this);
102  pub_cloud_non_registered_
103  = pnh_->advertise<sensor_msgs::PointCloud2>("output/non_registered", 1);
104  pub_registered_
105  = pnh_->advertise<sensor_msgs::PointCloud2>("output/registered", 1);
106  sub_cloud_.subscribe(*pnh_, "input", 1);
107  sub_indices_.subscribe(*pnh_, "input/indices", 1);
108  sub_pose_.subscribe(*pnh_, "input/pose", 1);
109  sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
110  sync_->connectInput(sub_cloud_, sub_indices_, sub_pose_);
111  sync_->registerCallback(boost::bind(
113  this, _1, _2, _3));
114  onInitPostProcess();
115  }
116 
118  pcl::PointCloud<pcl::PointXYZRGB>::Ptr input,
119  pcl::PointCloud<pcl::PointXYZRGB>::Ptr output,
120  const geometry_msgs::PoseStamped::ConstPtr& pose_msg)
121  {
122  Eigen::Affine3f posef;
123  tf::poseMsgToEigen(pose_msg->pose, posef);
124  Eigen::Affine3f transform = posef.inverse();
125 
126  pcl::transformPointCloud<pcl::PointXYZRGB>(
127  *input, *output, transform);
128  }
129 
131  const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
132  const pcl_msgs::PointIndices::ConstPtr& indices_msg,
133  const geometry_msgs::PoseStamped::ConstPtr& pose_msg)
134  {
135  boost::mutex::scoped_lock lock(mutex_);
136  pcl::PointCloud<pcl::PointXYZRGB>::Ptr
137  cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
138  pcl::PointCloud<pcl::PointXYZRGB>::Ptr
139  filtered_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
140  pcl::fromROSMsg(*cloud_msg, *cloud);
141  pcl::PointCloud<pcl::PointXYZRGB>::Ptr
142  transformed_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
143  pcl::PointIndices::Ptr
144  indices (new pcl::PointIndices);
145  indices->indices = indices_msg->indices;
146  pcl::ExtractIndices<pcl::PointXYZRGB> ex;
147  ex.setInputCloud(cloud);
148  ex.setIndices(indices);
149  ex.filter(*filtered_cloud);
150  transformPointCloudRepsectedToPose(
151  filtered_cloud, transformed_cloud, pose_msg);
152  Eigen::Affine3f initial_pose;
153  if (samples_.size() == 0) {
154  // first pointcloud, it will be the `origin`
155  tf::poseMsgToEigen(pose_msg->pose, origin_);
156  initial_pose = origin_;
157  }
158  else {
159  // compute transformation from origin_
160  Eigen::Affine3f offset;
161  tf::poseMsgToEigen(pose_msg->pose, offset);
162  initial_pose = origin_.inverse() * offset;
163  }
164  CapturedSamplePointCloud::Ptr sample (new CapturedSamplePointCloud(transformed_cloud, initial_pose));
165  samples_.push_back(sample);
166 
167  all_cloud_ = all_cloud_ + *transformed_cloud;
168  sensor_msgs::PointCloud2 ros_all_cloud;
169  pcl::toROSMsg(all_cloud_, ros_all_cloud);
170  ros_all_cloud.header = cloud_msg->header;
171  pub_cloud_non_registered_.publish(ros_all_cloud);
172  }
173 
175  pcl::PointCloud<pcl::PointXYZRGB>::Ptr reference,
176  pcl::PointCloud<pcl::PointXYZRGB>::Ptr target,
177  Eigen::Affine3f& output_transform)
178  {
180  = pnh_->serviceClient<jsk_recognition_msgs::ICPAlign>("icp_service");
181  sensor_msgs::PointCloud2 reference_ros, target_ros;
182  pcl::toROSMsg(*reference, reference_ros);
183  pcl::toROSMsg(*target, target_ros);
185  reference_ros.header.stamp = target_ros.header.stamp = now;
186  reference_ros.header.frame_id = target_ros.header.frame_id = "map";
187  jsk_recognition_msgs::ICPAlign srv;
188  srv.request.reference_cloud = reference_ros;
189  srv.request.target_cloud = target_ros;
190  icp.call(srv);
191  tf::poseMsgToEigen(srv.response.result.pose,
192  output_transform);
193  }
194 
196  std_srvs::Empty::Request& req,
197  std_srvs::Empty::Response& res)
198  {
199  if (samples_.size() <= 1) {
200  ROS_ERROR("no enough samples");
201  return false;
202  }
203  ROS_INFO("Starting registration %lu samples", samples_.size());
204  // setup initial
205  CapturedSamplePointCloud::Ptr initial_sample = samples_[0];
206  initial_sample->setRefinedPointCloud(
207  *(initial_sample->getOriginalPointCloud()));
208  initial_sample->setRefinedPose(initial_sample->getOriginalPose());
209  for (size_t i = 0; i < samples_.size() - 1; i++) {
210  CapturedSamplePointCloud::Ptr from_sample = samples_[i];
211  CapturedSamplePointCloud::Ptr to_sample = samples_[i+1];
212  pcl::PointCloud<pcl::PointXYZRGB>::Ptr reference_cloud
213  = from_sample->getRefinedPointCloud();
214  pcl::PointCloud<pcl::PointXYZRGB>::Ptr target_cloud
215  = to_sample->getOriginalPointCloud();
216  Eigen::Affine3f transform;
217  callICP(reference_cloud, target_cloud, transform);
218  to_sample->setRefinedPose(to_sample->getOriginalPose() * transform);
219  // transform pointcloud
220  pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed_cloud
221  (new pcl::PointCloud<pcl::PointXYZRGB>);
222  pcl::transformPointCloud<pcl::PointXYZRGB>(
223  *target_cloud, *transformed_cloud, transform);
224  to_sample->setRefinedPointCloud(*transformed_cloud);
225  }
226  pcl::PointCloud<pcl::PointXYZRGB>::Ptr registered_cloud
227  (new pcl::PointCloud<pcl::PointXYZRGB>);
228  pcl::PointCloud<pcl::PointXYZRGB>::Ptr non_registered_cloud
229  (new pcl::PointCloud<pcl::PointXYZRGB>);
230  for (size_t i = 0; i < samples_.size(); i++) {
231  *registered_cloud = *(samples_[i]->getRefinedPointCloud()) + *registered_cloud;
232  *non_registered_cloud = *(samples_[i]->getOriginalPointCloud()) + *non_registered_cloud;
233  }
234  sensor_msgs::PointCloud2 registered_ros_cloud, nonregistered_ros_cloud;
235  pcl::toROSMsg(*registered_cloud, registered_ros_cloud);
236  registered_ros_cloud.header.stamp = ros::Time::now();
237  registered_ros_cloud.header.frame_id = frame_id_;
238  pub_registered_.publish(registered_ros_cloud);
239  pcl::toROSMsg(*non_registered_cloud, nonregistered_ros_cloud);
240  nonregistered_ros_cloud.header.stamp = ros::Time::now();
241  nonregistered_ros_cloud.header.frame_id = frame_id_;
242  pub_cloud_non_registered_.publish(nonregistered_ros_cloud);
243  }
244 
245 }
246 
pcl::PointCloud< pcl::PointXYZRGB >::Ptr original_cloud_
virtual void newsampleCallback(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg, const pcl_msgs::PointIndices::ConstPtr &indices_msg, const geometry_msgs::PoseStamped::ConstPtr &pose_msg)
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::IncrementalModelRegistration, nodelet::Nodelet)
virtual void setRefinedPointCloud(pcl::PointCloud< pcl::PointXYZRGB > cloud)
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
bool call(MReq &req, MRes &res)
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
virtual bool startRegistration(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
virtual void transformPointCloudRepsectedToPose(pcl::PointCloud< pcl::PointXYZRGB >::Ptr input, pcl::PointCloud< pcl::PointXYZRGB >::Ptr output, const geometry_msgs::PoseStamped::ConstPtr &pose_msg)
virtual pcl::PointCloud< pcl::PointXYZRGB >::Ptr getOriginalPointCloud()
sensor_msgs::PointCloud2 PointCloud
#define ROS_INFO(...)
virtual pcl::PointCloud< pcl::PointXYZRGB >::Ptr getRefinedPointCloud()
pcl::PointCloud< pcl::PointXYZRGB >::Ptr refined_cloud_
srv
void toROSMsg(const pcl::PointCloud< T > &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
static Time now()
std::string frame_id_
boost::mutex mutex_
cloud
virtual void callICP(pcl::PointCloud< pcl::PointXYZRGB >::Ptr reference, pcl::PointCloud< pcl::PointXYZRGB >::Ptr target, Eigen::Affine3f &output_transform)
#define ROS_ERROR(...)


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