linemod_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 <limits>
38 #include "jsk_pcl_ros/linemod.h"
40 #include <boost/filesystem.hpp>
41 #include <boost/format.hpp>
42 #include <pcl/recognition/color_gradient_modality.h>
43 #include <pcl/recognition/surface_normal_modality.h>
44 #include <pcl/filters/extract_indices.h>
45 #include <pcl/io/pcd_io.h>
47 #include <glob.h>
48 #include <boost/algorithm/string/predicate.hpp>
49 #include <pcl/common/transforms.h>
50 #include <pcl/range_image/range_image_planar.h>
52 #include <cv_bridge/cv_bridge.h>
54 #include <pcl/kdtree/kdtree_flann.h>
55 #include <yaml-cpp/yaml.h>
56 #include <pcl/common/common.h>
59 
60 #if ( CV_MAJOR_VERSION >= 4)
61 #include <opencv2/imgproc/imgproc_c.h>
62 #endif
63 
64 namespace jsk_pcl_ros
65 {
67  {
68  PCLNodelet::onInit();
69  pnh_->param("output_file", output_file_, std::string("template"));
70  pnh_->param("sample_viewpoint", sample_viewpoint_, true);
71  pnh_->param("sample_viewpoint_angle_step", sample_viewpoint_angle_step_,
72  40.0);
73  pnh_->param("sample_viewpoint_angle_min", sample_viewpoint_angle_min_,
74  -80.0);
75  pnh_->param("sample_viewpoint_angle_max", sample_viewpoint_angle_max_,
76  80.0);
77  pnh_->param("sample_viewpoint_radius_step", sample_viewpoint_radius_step_,
78  0.2);
79  pnh_->param("sample_viewpoint_radius_min", sample_viewpoint_radius_min_,
80  0.4);
81  pnh_->param("sample_viewpoint_radius_max", sample_viewpoint_radius_max_,
82  0.8);
83  pnh_->param("n_points", n_points_, 150);
84  if (!sample_viewpoint_) {
85  sub_input_.subscribe(*pnh_, "input", 1);
86  sub_indices_.subscribe(*pnh_, "input/indices", 1);
87  sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
88  sync_->connectInput(sub_input_, sub_indices_);
89  sync_->registerCallback(boost::bind(&LINEMODTrainer::store,
90  this, _1, _2));
91  }
92  else {
93  sub_input_nonsync_ = pnh_->subscribe("input", 1,
95  this);
96  sub_camera_info_nonsync_ = pnh_->subscribe(
97  "input/info", 1,
99  this);
100  pub_range_image_ = pnh_->advertise<sensor_msgs::Image>(
101  "output/range_image", 1);
102  pub_colored_range_image_ = pnh_->advertise<sensor_msgs::Image>(
103  "output/colored_range_image", 1);
104  pub_sample_cloud_ = pnh_->advertise<sensor_msgs::PointCloud2>(
105  "output/sample_cloud", 1);
106  }
108  = pnh_->advertiseService(
109  "start_training", &LINEMODTrainer::startTraining,
110  this);
112  = pnh_->advertiseService(
113  "clear", &LINEMODTrainer::clearData,
114  this);
115  }
116 
118  const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
119  const PCLIndicesMsg::ConstPtr& indices_msg)
120  {
121  boost::mutex::scoped_lock lock(mutex_);
122  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud
123  (new pcl::PointCloud<pcl::PointXYZRGBA>);
124  pcl::fromROSMsg(*cloud_msg, *cloud);
125  pcl::PointIndices::Ptr indices(new pcl::PointIndices);
126  pcl_conversions::toPCL(*indices_msg, *indices);
127  samples_.push_back(cloud);
128  sample_indices_.push_back(indices);
129  NODELET_INFO("%lu samples", samples_.size());
130  }
131 
133  const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
134  {
135  boost::mutex::scoped_lock lock(mutex_);
136  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud
137  (new pcl::PointCloud<pcl::PointXYZRGBA>);
138  pcl::fromROSMsg(*cloud_msg, *cloud);
139  samples_before_sampling_.push_back(cloud);
140  NODELET_INFO("%lu samples", samples_.size());
141  }
142 
144  const sensor_msgs::CameraInfo::ConstPtr& info_msg)
145  {
146  boost::mutex::scoped_lock lock(mutex_);
147  camera_info_ = info_msg;
148  }
149 
151  std_srvs::Empty::Request& req,
152  std_srvs::Empty::Response& res)
153  {
154  boost::mutex::scoped_lock lock(mutex_);
155  NODELET_INFO("clearing %lu samples", samples_.size());
156  samples_.clear();
157  sample_indices_.clear();
158  return true;
159  }
160 
162  const Eigen::Affine3f& transform,
163  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr raw_cloud,
165  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr colored_cloud,
166  pcl::PointIndices& mask)
167  {
168  int width = model.fullResolution().width;
169  int height = model.fullResolution().height;
170  double fx = model.fx();
171  double fy = model.fy();
172  double cx = model.cx();
173  double cy = model.cy();
174  Eigen::Affine3f viewpoint_transform = transform;
175  Eigen::Affine3f object_transform = viewpoint_transform.inverse();
176  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud
177  (new pcl::PointCloud<pcl::PointXYZRGBA>);
178  pcl::transformPointCloud<pcl::PointXYZRGBA>(
179  *raw_cloud, *cloud, object_transform);
180  pcl::RangeImagePlanar range_image;
181  Eigen::Affine3f dummytrans;
182  dummytrans.setIdentity();
183  range_image.createFromPointCloudWithFixedSize(
184  *cloud, width, height,
185  cx, cy, fx, fy, dummytrans);
186  cv::Mat mat(range_image.height, range_image.width, CV_32FC1);
187  float *tmpf = (float *)mat.ptr();
188  for(unsigned int i = 0; i < range_image.height * range_image.width; i++) {
189  tmpf[i] = range_image.points[i].z;
190  }
191  std_msgs::Header dummy_header;
192  dummy_header.stamp = ros::Time::now();
193  dummy_header.frame_id = camera_info_->header.frame_id;
194  cv_bridge::CvImage range_bridge(dummy_header,
195  "32FC1",
196  mat);
197  pub_range_image_.publish(range_bridge.toImageMsg());
198  pcl::KdTreeFLANN<pcl::PointXYZRGBA>::Ptr
199  kdtree (new pcl::KdTreeFLANN<pcl::PointXYZRGBA>);
200  kdtree->setInputCloud(cloud);
201 
202  colored_cloud->width = range_image.width;
203  colored_cloud->height = range_image.height;
204  colored_cloud->is_dense = range_image.is_dense;
205  pcl::PointXYZRGBA nan_point;
206  nan_point.x = nan_point.y = nan_point.z
207  = std::numeric_limits<float>::quiet_NaN();
208  pcl::PointIndices::Ptr mask_indices (new pcl::PointIndices);
209  colored_cloud->points.resize(range_image.points.size());
210 
211  cv::Mat colored_image = cv::Mat::zeros(
212  range_image.height, range_image.width, CV_8UC3);
213  for (size_t pi = 0; pi < range_image.points.size(); pi++) {
214  if (std::isnan(range_image.points[pi].x) ||
215  std::isnan(range_image.points[pi].y) ||
216  std::isnan(range_image.points[pi].z)) {
217  // nan
218  colored_cloud->points[pi] = nan_point;
219  }
220  else {
221  pcl::PointXYZRGBA input_point;
222  input_point.x = range_image.points[pi].x;
223  input_point.y = range_image.points[pi].y;
224  input_point.z = range_image.points[pi].z;
225  std::vector<int> indices;
226  std::vector<float> distances;
227  kdtree->nearestKSearch(input_point, 1, indices, distances);
228  if (indices.size() > 0) {
229  input_point.rgba = cloud->points[indices[0]].rgba;
230  }
231  colored_image.at<cv::Vec3b>(pi / range_image.width,
232  pi % range_image.width)
233  = cv::Vec3b(cloud->points[indices[0]].r,
234  cloud->points[indices[0]].g,
235  cloud->points[indices[0]].b);
236  colored_cloud->points[pi] = input_point;
237  }
238  }
239  for (size_t pi = 0; pi < range_image.points.size(); pi++) {
240  if (!std::isnan(range_image.points[pi].x) &&
241  !std::isnan(range_image.points[pi].y) &&
242  !std::isnan(range_image.points[pi].z)) {
243  // nan
244  mask.indices.push_back(pi);
245  }
246  }
247  cv_bridge::CvImage colored_range_bridge(dummy_header,
249  colored_image);
250  pub_colored_range_image_.publish(colored_range_bridge.toImageMsg());
251  // // trick, rgba -> rgb
252  sensor_msgs::PointCloud2 ros_sample_cloud;
253  pcl::toROSMsg(*colored_cloud, ros_sample_cloud);
254  pcl::PointCloud<pcl::PointXYZRGB> rgb_cloud;
255  pcl::fromROSMsg(ros_sample_cloud, rgb_cloud);
256  pcl::toROSMsg(rgb_cloud, ros_sample_cloud);
257  ros_sample_cloud.header = dummy_header;
258  pub_sample_cloud_.publish(ros_sample_cloud);
259  }
260 
262  {
263  NODELET_INFO("Start LINEMOD training from %lu samples", samples_before_sampling_.size());
264  if (!camera_info_) {
265  NODELET_FATAL("no camera info is available");
266  return;
267  }
268  if (samples_before_sampling_.size() != 1) {
269  NODELET_FATAL("we expect only one training pointcloud, but it has %lu pointclouds",
270  samples_before_sampling_.size());
271  return;
272  }
273 
276 
277  if (samples_before_sampling_.size() != 1) {
278  NODELET_FATAL("we expect only one sample data");
279  return;
280  }
287  n_points_);
288  std::vector<Eigen::Affine3f> transforms;
289  transforms.resize(sampler.sampleNum());
290  for (size_t i = 0; i < sampler.sampleNum(); i++) {
291  Eigen::Affine3f transform;
292  sampler.get(transform);
293  transforms[i] = transform;
294  sampler.next();
295  }
296 
297  // NB:
298  // This line is super important.
299  // dummy Range Image to avoid static method initialization in
300  // multi-thread environment. In detail,
301  // pcl::RangeImagePlanar::createLookupTables is not thread-safe.
302  pcl::RangeImagePlanar dummy_range_image;
303 
304  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr raw_cloud
306 
307  // prepare for multi-threaded optimization
308  std::vector<pcl::ColorGradientModality<pcl::PointXYZRGBA> >
309  color_modalities (sampler.sampleNum());
310  std::vector<pcl::SurfaceNormalModality<pcl::PointXYZRGBA> >
311  surface_norm_modalities (sampler.sampleNum());
312  std::vector<pcl::MaskMap> mask_maps (sampler.sampleNum());
313  std::vector<pcl::RegionXY> regions (sampler.sampleNum());
314  boost::mutex train_mutex;
315  pcl::LINEMOD linemod;
316  int counter = 0;
317  std::vector<Eigen::Affine3f> pose_in_order_of_training;
318 #ifdef _OPENMP
319 #pragma omp parallel for
320 #endif
321  for (size_t j = 0; j < sampler.sampleNum(); j++) {
322 
323  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBA>);
324  pcl::PointIndices::Ptr indices(new pcl::PointIndices);
325  organizedPointCloudWithViewPoint(transforms[j], raw_cloud, model,
326  cloud, *indices);
327  // generate mask and modalities
328  pcl::ColorGradientModality<pcl::PointXYZRGBA> color_modality;
329  pcl::SurfaceNormalModality<pcl::PointXYZRGBA> surface_norm_mod;
330  pcl::MaskMap mask_map(model.fullResolution().width,
331  model.fullResolution().height);
332  pcl::RegionXY region;
333  generateLINEMODTrainingData(cloud, indices,
334  color_modality, surface_norm_mod,
335  mask_map, region);
336  std::vector<pcl::QuantizableModality*> modalities(2);
337  modalities[0] = &color_modality;
338  modalities[1] = &surface_norm_mod;
339  std::vector<pcl::MaskMap*> masks(2);
340  masks[0] = &mask_map;
341  masks[1] = &mask_map;
342  {
343  boost::mutex::scoped_lock lock(train_mutex);
344  ++counter;
345  NODELET_INFO("training: %d/%lu", counter, sampler.sampleNum());
346  linemod.createAndAddTemplate(modalities, masks, region);
347  pose_in_order_of_training.push_back(transforms[j]);
348  }
349  }
350  // dump result into file
351  // 1. linemod file
352  // 2. original pointcloud into .pcd file
353  // 3. pose yaml about the template
354  std::ofstream linemod_file;
355  const std::string linemod_file_name = output_file_ + ".linemod";
356  NODELET_INFO("writing to %s", linemod_file_name.c_str());
357  linemod_file.open(linemod_file_name.c_str(),
358  std::ofstream::out | std::ofstream::binary);
359  linemod.serialize(linemod_file);
360  linemod_file.close();
361  const std::string pcd_file_name = output_file_ + ".pcd";
362  NODELET_INFO("writing to %s", pcd_file_name.c_str());
363  pcl::PCDWriter writer;
364  writer.writeBinaryCompressed(pcd_file_name, *raw_cloud);
365  // pose yaml
366  std::ofstream pose_file;
367  const std::string pose_file_name = output_file_ + "_poses.yaml";
368  NODELET_INFO("writing to %s", pose_file_name.c_str());
369  pose_file.open(pose_file_name.c_str(), std::ofstream::out);
370  pose_file << "template_poses: [" << std::endl;
371  for (size_t i = 0; i < pose_in_order_of_training.size(); i++) {
372  Eigen::Affine3f pose = pose_in_order_of_training[i];
373  Eigen::Vector3f pos(pose.translation());
374  Eigen::Quaternionf rot(pose.rotation());
375  pose_file << "["
376  << pos[0] << ", " << pos[1] << ", " << pos[2] << ", "
377  << rot.x() << ", " << rot.y() << ", " << rot.z() << ", " << rot.w() << "]";
378  if (i != pose_in_order_of_training.size() - 1) {
379  pose_file << ", " << std::endl;
380  }
381  else {
382  pose_file << "]" << std::endl;
383  }
384  }
385  pose_file.close();
386  NODELET_INFO("done");
387  }
388 
390  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud,
391  pcl::PointIndices::Ptr mask,
392  pcl::ColorGradientModality<pcl::PointXYZRGBA>& color_grad_mod,
393  pcl::SurfaceNormalModality<pcl::PointXYZRGBA>& surface_norm_mod,
394  pcl::MaskMap& mask_map,
395  pcl::RegionXY& region)
396  {
397  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr masked_cloud
398  (new pcl::PointCloud<pcl::PointXYZRGBA>);
399  pcl::ExtractIndices<pcl::PointXYZRGBA> ex;
400  ex.setKeepOrganized(true);
401  ex.setInputCloud(cloud);
402  ex.setIndices(mask);
403  ex.filter(*masked_cloud);
404  color_grad_mod.setInputCloud(masked_cloud);
405  color_grad_mod.processInputData();
406  surface_norm_mod.setInputCloud(cloud);
407  surface_norm_mod.processInputData();
408  size_t min_x(masked_cloud->width), min_y(masked_cloud->height), max_x(0), max_y(0);
409  for (size_t j = 0; j < masked_cloud->height; ++j) {
410  for (size_t i = 0; i < masked_cloud->width; ++i) {
411  pcl::PointXYZRGBA p
412  = masked_cloud->points[j * masked_cloud->width + i];
413  if (!std::isnan(p.x) && !std::isnan(p.y) && !std::isnan(p.z)) {
414  mask_map(i, j) = 1;
415  min_x = std::min(min_x, i);
416  max_x = std::max(max_x, i);
417  min_y = std::min(min_y, j);
418  max_y = std::max(max_y, j);
419  }
420  else {
421  mask_map(i, j) = 0;
422  }
423  }
424  }
425  region.x = static_cast<int>(min_x);
426  region.y = static_cast<int>(min_y);
427  region.width = static_cast<int>(max_x - min_x + 1);
428  region.height = static_cast<int>(max_y - min_y + 1);
429  }
430 
431 
432  std::vector<std::string> LINEMODTrainer::trainOneData(
433  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud,
434  pcl::PointIndices::Ptr mask,
435  std::string& tempstr,
436  int i)
437  {
438  pcl::LINEMOD linemod;
439  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr masked_cloud
440  (new pcl::PointCloud<pcl::PointXYZRGBA>);
441  pcl::ExtractIndices<pcl::PointXYZRGBA> ex;
442  ex.setKeepOrganized(true);
443  ex.setInputCloud(cloud);
444  ex.setIndices(mask);
445  ex.filter(*masked_cloud);
446  pcl::ColorGradientModality<pcl::PointXYZRGBA> color_grad_mod;
447  color_grad_mod.setInputCloud(masked_cloud);
448  color_grad_mod.processInputData();
449  pcl::SurfaceNormalModality<pcl::PointXYZRGBA> surface_norm_mod;
450  surface_norm_mod.setInputCloud(masked_cloud);
451  surface_norm_mod.processInputData();
452  std::vector<pcl::QuantizableModality*> modalities(2);
453  modalities[0] = &color_grad_mod;
454  modalities[1] = &surface_norm_mod;
455  size_t min_x(masked_cloud->width), min_y(masked_cloud->height), max_x(0), max_y(0);
456  pcl::MaskMap mask_map(masked_cloud->width, masked_cloud->height);
457  for (size_t j = 0; j < masked_cloud->height; ++j) {
458  for (size_t i = 0; i < masked_cloud->width; ++i) {
459  pcl::PointXYZRGBA p
460  = masked_cloud->points[j * masked_cloud->width + i];
461  if (!std::isnan(p.x) && !std::isnan(p.y) && !std::isnan(p.z)) {
462  mask_map(i, j) = 1;
463  min_x = std::min(min_x, i);
464  max_x = std::max(max_x, i);
465  min_y = std::min(min_y, j);
466  max_y = std::max(max_y, j);
467  }
468  else {
469  mask_map(i, j) = 0;
470  }
471  }
472  }
473  std::vector<pcl::MaskMap*> masks(2);
474  masks[0] = &mask_map;
475  masks[1] = &mask_map;
476  pcl::RegionXY region;
477  region.x = static_cast<int>(min_x);
478  region.y = static_cast<int>(min_y);
479  region.width = static_cast<int>(max_x - min_x + 1);
480  region.height = static_cast<int>(max_y - min_y + 1);
481  linemod.createAndAddTemplate(modalities, masks, region);
482 
483  std::vector<std::string> ret;
484  {
485  // sqmmt
486  std::stringstream filename_stream;
487  filename_stream << boost::format("%s/%05d_template.sqmmt") % tempstr % i;
488  std::string filename = filename_stream.str();
489  std::cerr << "writing " << filename << std::endl;
490  std::ofstream file_stream;
491  file_stream.open(filename.c_str(),
492  std::ofstream::out | std::ofstream::binary);
493  linemod.getTemplate(0).serialize(file_stream);
494  file_stream.close();
495  ret.push_back(filename);
496  }
497  {
498  // pcd
499  std::stringstream filename_stream;
500  filename_stream << boost::format("%s/%05d_template.pcd") % tempstr % i;
501  std::string filename = filename_stream.str();
502  std::cerr << "writing " << filename << std::endl;
503  pcl::PCDWriter writer;
504  writer.writeBinaryCompressed(filename, *masked_cloud);
505  ret.push_back(filename);
506  }
507  return ret;
508  }
509 
511  {
512  NODELET_INFO("Start LINEMOD training from %lu samples", samples_.size());
513  boost::filesystem::path temp = boost::filesystem::unique_path();
514  boost::filesystem::create_directory(temp);
515  std::string tempstr = temp.native();
516  NODELET_INFO("mkdir %s", tempstr.c_str());
517  std::vector<std::string> all_files;
518  for (size_t i = 0; i < samples_.size(); i++) {
519  NODELET_INFO("Processing %lu-th data", i);
520  pcl::PointIndices::Ptr mask = sample_indices_[i];
521  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud = samples_[i];
522  //std::vector<std::string> files = trainOneData(cloud, mask, tempstr, i);
523  // for (size_t i = 0; i < files.size(); i++) {
524  // all_files.push_back(files[i]);
525  // }
526  }
527  tar(tempstr, output_file_);
528  NODELET_INFO("done");
529  }
530 
531  void LINEMODTrainer::tar(const std::string& directory, const std::string& output)
532  {
533  std::stringstream command_stream;
534  command_stream << "tar --format=ustar -cf " << output << " " << directory << "/*";
535  NODELET_INFO("executing %s", command_stream.str().c_str());
536  int ret = system(command_stream.str().c_str());
537  }
538 
540  std_srvs::Empty::Request& req,
541  std_srvs::Empty::Response& res)
542  {
543  boost::mutex::scoped_lock lock(mutex_);
544  if (sample_viewpoint_) {
546  }
547  else {
549  }
550  return true;
551  }
552 
554  {
555  DiagnosticNodelet::onInit();
556  pnh_->param("template_file", template_file_, std::string("template"));
557  // load original point and poses
558  template_cloud_.reset(new pcl::PointCloud<pcl::PointXYZRGBA>);
559  pcl::PCDReader reader;
560  reader.read(template_file_ + ".pcd", *template_cloud_);
561  const std::string pose_yaml = template_file_ + "_poses.yaml";
562  YAML::Node doc;
563 #ifdef USE_OLD_YAML
564  std::ifstream pose_fin;
565  pose_fin.open(pose_yaml.c_str(), std::ifstream::in);
566  YAML::Parser parser(pose_fin);
567  while (parser.GetNextDocument(doc)) {
568  setTemplate(doc);
569  }
570  pose_fin.close();
571 #else
572  // yaml-cpp is greater than 0.5.0
573  doc = YAML::LoadFile(pose_yaml);
574  setTemplate(doc);
575 #endif
576 
577 
578  srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
579  dynamic_reconfigure::Server<Config>::CallbackType f =
580  boost::bind (
581  &LINEMODDetector::configCallback, this, _1, _2);
582  srv_->setCallback (f);
583 
584  pub_cloud_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output", 1);
585  pub_detect_mask_ = advertise<sensor_msgs::Image>(*pnh_, "output/mask", 1);
586  pub_pose_ = advertise<geometry_msgs::PoseStamped>(*pnh_, "output/pose", 1);
587  pub_original_template_cloud_ = advertise<sensor_msgs::PointCloud2>(
588  *pnh_, "output/template", 1);
589 
591  }
592 
594  {
595  sub_cloud_ = pnh_->subscribe("input", 1, &LINEMODDetector::detect, this);
596  }
597 
599  {
600  sub_cloud_.shutdown();
601  }
602 
603  void LINEMODDetector::setTemplate(YAML::Node doc)
604  {
605  const YAML::Node& template_pose_yaml = doc["template_poses"];
606  for (size_t i = 0; i < template_pose_yaml.size(); i++) {
607  const YAML::Node& pose = template_pose_yaml[i];
608  Eigen::Affine3f trans = jsk_recognition_utils::affineFromYAMLNode(pose);
609  template_poses_.push_back(trans);
610  // set template_bboxes
611  pcl::PointCloud<pcl::PointXYZRGBA> transformed_cloud;
612  pcl::transformPointCloud<pcl::PointXYZRGBA>(*template_cloud_, transformed_cloud, trans);
613  // compute size of bounding box
614  Eigen::Vector4f minpt, maxpt;
615  pcl::getMinMax3D<pcl::PointXYZRGBA>(transformed_cloud, minpt, maxpt);
616  jsk_recognition_msgs::BoundingBox bbox = jsk_recognition_utils::boundingBoxFromPointCloud(transformed_cloud);
617  //ROS_INFO("bounding box size: [%f, %f, %f]", bbox.dimensions.x, bbox.dimensions.y, bbox.dimensions.z);
618  template_bboxes_.push_back(bbox);
619  }
620  }
621 
622  void LINEMODDetector::configCallback(Config &config, uint32_t level)
623  {
624  boost::mutex::scoped_lock lock(mutex_);
625  gradient_magnitude_threshold_ = config.gradient_magnitude_threshold;
626  detection_threshold_ = config.detection_threshold;
627  color_gradient_mod_.setGradientMagnitudeThreshold(gradient_magnitude_threshold_);
628  linemod_.setDetectionThreshold(detection_threshold_);
629 
630  // desearlize
631  const std::string linemod_file = template_file_ + ".linemod";
632  std::ifstream linemod_in;
633  linemod_in.open(linemod_file.c_str(), std::ifstream::in);
634  linemod_.deserialize(linemod_in);
635  linemod_in.close();
636  }
637 
639  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud,
640  const pcl::SparseQuantizedMultiModTemplate& linemod_template,
641  const pcl::LINEMODDetection& linemod_detection,
642  Eigen::Vector3f& center)
643  {
644  const size_t start_x = std::max(linemod_detection.x, 0);
645  const size_t start_y = std::max(linemod_detection.y, 0);
646  const size_t end_x = std::min(
647  static_cast<size_t> (start_x + linemod_template.region.width * linemod_detection.scale),
648  static_cast<size_t> (cloud->width));
649  const size_t end_y = std::min(
650  static_cast<size_t> (start_y + linemod_template.region.height * linemod_detection.scale),
651  static_cast<size_t> (cloud->height));
652  size_t counter = 0;
653  for (size_t row_index = start_y; row_index < end_y; ++row_index) {
654  for (size_t col_index = start_x; col_index < end_x; ++col_index) {
655  const pcl::PointXYZRGBA & point = (*cloud) (col_index, row_index);
656  if (pcl_isfinite (point.x) &&
657  pcl_isfinite (point.y) &&
658  pcl_isfinite (point.z)) {
659  center = center + point.getVector3fMap();
660  ++counter;
661  }
662  }
663  }
664  center = center / counter;
665  }
666 
668  const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
669  {
670  NODELET_INFO("detect");
671  vital_checker_->poke();
672  boost::mutex::scoped_lock lock(mutex_);
673  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr
674  cloud (new pcl::PointCloud<pcl::PointXYZRGBA>);
675  pcl::fromROSMsg(*cloud_msg, *cloud);
676 
677  surface_normal_mod_.setInputCloud(cloud);
678  surface_normal_mod_.processInputData ();
679  color_gradient_mod_.setInputCloud (cloud);
680  color_gradient_mod_.processInputData ();
681  std::vector<pcl::LINEMODDetection> linemod_detections;
682  std::vector<pcl::QuantizableModality*> modalities;
683  modalities.push_back(&color_gradient_mod_);
684  modalities.push_back(&surface_normal_mod_);
685  linemod_.detectTemplatesSemiScaleInvariant(modalities, linemod_detections,
686  0.6944444f, 1.44f, 1.2f);
687  NODELET_INFO("detected %lu result", linemod_detections.size());
688  // lookup the best result
689  if (linemod_detections.size() > 0) {
690  double max_score = 0;
691  size_t max_template_id = 0;
692  double max_scale = 0;
693  pcl::LINEMODDetection linemod_detection;
694  for (size_t i = 0; i < linemod_detections.size(); i++) {
695  const pcl::LINEMODDetection& detection = linemod_detections[i];
696  if (max_score < detection.score) {
697  linemod_detection = detection;
698  max_score = detection.score;
699  }
700  }
701 
702  const pcl::SparseQuantizedMultiModTemplate& linemod_template =
703  linemod_.getTemplate(linemod_detection.template_id);
704  Eigen::Vector3f center(0, 0, 0);
705  computeCenterOfTemplate(
706  cloud, linemod_template, linemod_detection, center);
707  // publish mask image here
708  cv::Mat detect_mask = cv::Mat::zeros(cloud->height, cloud->width, CV_8UC1);
709  int scaled_template_width
710  = linemod_template.region.width * linemod_detection.scale;
711  int scaled_template_height
712  = linemod_template.region.height * linemod_detection.scale;
713  cv::rectangle(
714  detect_mask,
715  cv::Point(linemod_detection.x, linemod_detection.y),
716  cv::Point(linemod_detection.x + scaled_template_width,
717  linemod_detection.y + scaled_template_height),
718  cv::Scalar(255), CV_FILLED);
719  pub_detect_mask_.publish(cv_bridge::CvImage(cloud_msg->header,
720  "8UC1",
721  detect_mask).toImageMsg());
722  // compute translation
723  jsk_recognition_msgs::BoundingBox bbox = template_bboxes_[linemod_detection.template_id];
724  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr
725  result (new pcl::PointCloud<pcl::PointXYZRGBA>);
726  pcl::transformPointCloud<pcl::PointXYZRGBA>(
727  *template_cloud_, *result, template_poses_[linemod_detection.template_id]);
728  Eigen::Vector4f minpt, maxpt;
729  pcl::getMinMax3D<pcl::PointXYZRGBA>(*result, minpt, maxpt);
730  Eigen::Vector4f template_center = (minpt + maxpt) / 2;
731  Eigen::Vector3f translation = center - Eigen::Vector3f(template_center[0],
732  template_center[1],
733  template_center[2]);
734  Eigen::Affine3f pose = template_poses_[linemod_detection.template_id] * Eigen::Translation3f(translation);
735  geometry_msgs::PoseStamped ros_pose;
736  tf::poseEigenToMsg(pose, ros_pose.pose);
737  ros_pose.header = cloud_msg->header;
738  pub_pose_.publish(ros_pose);
739  for (size_t i = 0; i < result->points.size(); i++) {
740  result->points[i].getVector3fMap()
741  = result->points[i].getVector3fMap() + translation;
742  }
743  sensor_msgs::PointCloud2 ros_result;
744  pcl::toROSMsg(*result, ros_result);
745  ros_result.header = cloud_msg->header;
746  pub_cloud_.publish(ros_result);
747  sensor_msgs::PointCloud2 ros_template;
748  pcl::toROSMsg(*template_cloud_, ros_template);
749  ros_template.header = cloud_msg->header;
750  pub_original_template_cloud_.publish(ros_template);
751  }
752  }
753 }
754 
virtual void subscribeCloud(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg)
ros::ServiceServer clear_data_srv_
Definition: linemod.h:167
std::vector< pcl::PointIndices::Ptr > sample_indices_
Definition: linemod.h:176
double min(const OneDataStat &d)
wrapper function for min method for boost::function
Definition: one_data_stat.h:94
filename
double sample_viewpoint_radius_max_
Definition: linemod.h:185
virtual void setTemplate(YAML::Node doc)
pos
void publish(const boost::shared_ptr< M > &message) const
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
sensor_msgs::CameraInfo::ConstPtr camera_info_
Definition: linemod.h:173
double max(const OneDataStat &d)
wrapper function for max method for boost::function
virtual void trainWithViewpointSampling()
double sample_viewpoint_angle_min_
Definition: linemod.h:182
virtual void trainWithoutViewpointSampling()
std::vector< pcl::PointCloud< pcl::PointXYZRGBA >::Ptr > samples_before_sampling_
Definition: linemod.h:174
double sample_viewpoint_radius_min_
Definition: linemod.h:183
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
ros::Publisher pub_range_image_
Definition: linemod.h:168
virtual void computeCenterOfTemplate(pcl::PointCloud< pcl::PointXYZRGBA >::Ptr cloud, const pcl::SparseQuantizedMultiModTemplate &linemod_template, const pcl::LINEMODDetection &linemod_detection, Eigen::Vector3f &center)
pose
virtual bool startTraining(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
message_filters::Subscriber< PCLIndicesMsg > sub_indices_
Definition: linemod.h:165
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_input_
Definition: linemod.h:164
virtual std::vector< std::string > trainOneData(pcl::PointCloud< pcl::PointXYZRGBA >::Ptr cloud, pcl::PointIndices::Ptr mask, std::string &tempstr, int i)
void toPCL(const pcl_msgs::PointIndices &pi, pcl::PointIndices &pcl_pi)
std::vector< pcl::PointCloud< pcl::PointXYZRGBA >::Ptr > samples_
Definition: linemod.h:175
ros::Subscriber sub_input_nonsync_
Definition: linemod.h:171
result
boost::shared_ptr< ros::NodeHandle > pnh_
virtual void subscribeCameraInfo(const sensor_msgs::CameraInfo::ConstPtr &info_msg)
ros::ServiceServer start_training_srv_
Definition: linemod.h:166
virtual void store(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg, const PCLIndicesMsg::ConstPtr &indices_msg)
Eigen::Affine3f affineFromYAMLNode(const YAML::Node &pose)
bool fromCameraInfo(const sensor_msgs::CameraInfo &msg)
virtual void tar(const std::string &directory, const std::string &output)
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
Definition: linemod.h:163
virtual void detect(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg)
doc
rot
ros::Publisher pub_sample_cloud_
Definition: linemod.h:170
void toROSMsg(const pcl::PointCloud< T > &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
ros::Subscriber sub_camera_info_nonsync_
Definition: linemod.h:172
ros::Publisher pub_pose_
std::string output_file_
Definition: linemod.h:178
boost::mutex mutex
global mutex.
LINEMODDetectorConfig Config
Definition: linemod.h:69
#define NODELET_INFO(...)
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
virtual void generateLINEMODTrainingData(pcl::PointCloud< pcl::PointXYZRGBA >::Ptr cloud, pcl::PointIndices::Ptr mask, pcl::ColorGradientModality< pcl::PointXYZRGBA > &color_grad_mod, pcl::SurfaceNormalModality< pcl::PointXYZRGBA > &surface_norm_mod, pcl::MaskMap &mask_map, pcl::RegionXY &region)
p
jsk_recognition_msgs::BoundingBox boundingBoxFromPointCloud(const pcl::PointCloud< PointT > &cloud)
double sample_viewpoint_angle_step_
Definition: linemod.h:180
static Time now()
double sample_viewpoint_angle_max_
Definition: linemod.h:184
virtual void configCallback(Config &config, uint32_t level)
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::LINEMODTrainer, nodelet::Nodelet)
ros::Publisher pub_colored_range_image_
Definition: linemod.h:169
#define NODELET_FATAL(...)
cloud
virtual void get(Eigen::Affine3f &transform)
double sample_viewpoint_radius_step_
Definition: linemod.h:181
sensor_msgs::ImagePtr toImageMsg() const
virtual void organizedPointCloudWithViewPoint(const Eigen::Affine3f &transform, pcl::PointCloud< pcl::PointXYZRGBA >::Ptr raw_cloud, const image_geometry::PinholeCameraModel &model, pcl::PointCloud< pcl::PointXYZRGBA >::Ptr output, pcl::PointIndices &mask)
virtual bool clearData(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
width


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