linemod_nodelet.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2014, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 #define BOOST_PARAMETER_MAX_ARITY 7
00036 
00037 #include <limits>
00038 #include "jsk_pcl_ros/linemod.h"
00039 #include <pcl_conversions/pcl_conversions.h>
00040 #include <boost/filesystem.hpp>
00041 #include <boost/format.hpp>
00042 #include <pcl/recognition/color_gradient_modality.h>
00043 #include <pcl/recognition/surface_normal_modality.h>
00044 #include <pcl/filters/extract_indices.h>
00045 #include <pcl/io/pcd_io.h>
00046 #include <jsk_topic_tools/rosparam_utils.h>
00047 #include <glob.h>
00048 #include <boost/algorithm/string/predicate.hpp>
00049 #include <pcl/common/transforms.h>
00050 #include <pcl/range_image/range_image_planar.h>
00051 #include "jsk_pcl_ros/viewpoint_sampler.h"
00052 #include <cv_bridge/cv_bridge.h>
00053 #include <sensor_msgs/image_encodings.h>
00054 #include <pcl/kdtree/kdtree_flann.h>
00055 #include <yaml-cpp/yaml.h>
00056 #include <pcl/common/common.h>
00057 #include "jsk_recognition_utils/pcl_util.h"
00058 #include "jsk_recognition_utils/geo_util.h"
00059 
00060 namespace jsk_pcl_ros
00061 {
00062   void LINEMODTrainer::onInit()
00063   {
00064     PCLNodelet::onInit();
00065     pnh_->param("output_file", output_file_, std::string("template"));
00066     pnh_->param("sample_viewpoint", sample_viewpoint_, true);
00067     pnh_->param("sample_viewpoint_angle_step", sample_viewpoint_angle_step_,
00068                 40.0);
00069     pnh_->param("sample_viewpoint_angle_min", sample_viewpoint_angle_min_,
00070                 -80.0);
00071     pnh_->param("sample_viewpoint_angle_max", sample_viewpoint_angle_max_,
00072                 80.0);
00073     pnh_->param("sample_viewpoint_radius_step", sample_viewpoint_radius_step_,
00074                 0.2);
00075     pnh_->param("sample_viewpoint_radius_min", sample_viewpoint_radius_min_,
00076                 0.4);
00077     pnh_->param("sample_viewpoint_radius_max", sample_viewpoint_radius_max_,
00078                 0.8);
00079     if (!sample_viewpoint_) {
00080       sub_input_.subscribe(*pnh_, "input", 1);
00081       sub_indices_.subscribe(*pnh_, "input/indices", 1);
00082       sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
00083       sync_->connectInput(sub_input_, sub_indices_);
00084       sync_->registerCallback(boost::bind(&LINEMODTrainer::store,
00085                                           this, _1, _2));
00086     }
00087     else {
00088       sub_input_nonsync_ = pnh_->subscribe("input", 1,
00089                                            &LINEMODTrainer::subscribeCloud,
00090                                            this);
00091       sub_camera_info_nonsync_ = pnh_->subscribe(
00092         "input/info", 1,
00093         &LINEMODTrainer::subscribeCameraInfo,
00094         this);
00095       pub_range_image_ = pnh_->advertise<sensor_msgs::Image>(
00096         "output/range_image", 1);
00097       pub_colored_range_image_ = pnh_->advertise<sensor_msgs::Image>(
00098           "output/colored_range_image", 1);
00099       pub_sample_cloud_ = pnh_->advertise<sensor_msgs::PointCloud2>(
00100         "output/sample_cloud", 1);
00101     }
00102     start_training_srv_
00103       = pnh_->advertiseService(
00104         "start_training", &LINEMODTrainer::startTraining,
00105         this);
00106     clear_data_srv_
00107       = pnh_->advertiseService(
00108         "clear", &LINEMODTrainer::clearData,
00109         this);
00110   }
00111 
00112   void LINEMODTrainer::store(
00113     const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
00114     const PCLIndicesMsg::ConstPtr& indices_msg)
00115   {
00116     boost::mutex::scoped_lock lock(mutex_);
00117     pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud
00118       (new pcl::PointCloud<pcl::PointXYZRGBA>);
00119     pcl::fromROSMsg(*cloud_msg, *cloud);
00120     pcl::PointIndices::Ptr indices(new pcl::PointIndices);
00121     pcl_conversions::toPCL(*indices_msg, *indices);
00122     samples_.push_back(cloud);
00123     sample_indices_.push_back(indices);
00124     NODELET_INFO("%lu samples", samples_.size());
00125   }
00126 
00127   void LINEMODTrainer::subscribeCloud(
00128     const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
00129   {
00130     boost::mutex::scoped_lock lock(mutex_);
00131     pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud
00132       (new pcl::PointCloud<pcl::PointXYZRGBA>);
00133     pcl::fromROSMsg(*cloud_msg, *cloud);
00134     samples_before_sampling_.push_back(cloud);
00135     NODELET_INFO("%lu samples", samples_.size());
00136   }
00137 
00138   void LINEMODTrainer::subscribeCameraInfo(
00139     const sensor_msgs::CameraInfo::ConstPtr& info_msg)
00140   {
00141     boost::mutex::scoped_lock lock(mutex_);
00142     camera_info_ = info_msg;
00143   }
00144   
00145   bool LINEMODTrainer::clearData(
00146     std_srvs::Empty::Request& req,
00147     std_srvs::Empty::Response& res)
00148   {
00149     boost::mutex::scoped_lock lock(mutex_);
00150     NODELET_INFO("clearing %lu samples", samples_.size());
00151     samples_.clear();
00152     sample_indices_.clear();
00153     return true;
00154   }
00155 
00156   void LINEMODTrainer::organizedPointCloudWithViewPoint(
00157     const Eigen::Affine3f& transform,
00158     pcl::PointCloud<pcl::PointXYZRGBA>::Ptr raw_cloud,
00159     const image_geometry::PinholeCameraModel& model,
00160     pcl::PointCloud<pcl::PointXYZRGBA>::Ptr colored_cloud,
00161     pcl::PointIndices& mask)
00162   {
00163     int width = model.fullResolution().width;
00164     int height = model.fullResolution().height;
00165     double fx = model.fx();
00166     double fy = model.fy();
00167     double cx = model.cx();
00168     double cy = model.cy();
00169     Eigen::Affine3f viewpoint_transform = transform;
00170     Eigen::Affine3f object_transform = viewpoint_transform.inverse();
00171     pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud
00172       (new pcl::PointCloud<pcl::PointXYZRGBA>);
00173     pcl::transformPointCloud<pcl::PointXYZRGBA>(
00174       *raw_cloud, *cloud, object_transform);
00175     pcl::RangeImagePlanar range_image;
00176     Eigen::Affine3f dummytrans;
00177     dummytrans.setIdentity();
00178     range_image.createFromPointCloudWithFixedSize(
00179       *cloud, width, height,
00180       cx, cy, fx, fy, dummytrans);
00181     cv::Mat mat(range_image.height, range_image.width, CV_32FC1);
00182     float *tmpf = (float *)mat.ptr();
00183     for(unsigned int i = 0; i < range_image.height * range_image.width; i++) {
00184       tmpf[i] = range_image.points[i].z;
00185     }
00186     std_msgs::Header dummy_header;
00187     dummy_header.stamp = ros::Time::now();
00188     dummy_header.frame_id = camera_info_->header.frame_id;
00189     cv_bridge::CvImage range_bridge(dummy_header,
00190                                     "32FC1",
00191                                     mat);
00192     pub_range_image_.publish(range_bridge.toImageMsg());
00193     pcl::KdTreeFLANN<pcl::PointXYZRGBA>::Ptr
00194       kdtree (new pcl::KdTreeFLANN<pcl::PointXYZRGBA>);
00195     kdtree->setInputCloud(cloud);
00196 
00197     colored_cloud->width = range_image.width;
00198     colored_cloud->height = range_image.height;
00199     colored_cloud->is_dense = range_image.is_dense;
00200     pcl::PointXYZRGBA nan_point;
00201     nan_point.x = nan_point.y = nan_point.z
00202       = std::numeric_limits<float>::quiet_NaN();
00203     pcl::PointIndices::Ptr mask_indices (new pcl::PointIndices);
00204     colored_cloud->points.resize(range_image.points.size());
00205     
00206     cv::Mat colored_image = cv::Mat::zeros(
00207       range_image.height, range_image.width, CV_8UC3);
00208     for (size_t pi = 0; pi < range_image.points.size(); pi++) {
00209       if (std::isnan(range_image.points[pi].x) ||
00210           std::isnan(range_image.points[pi].y) ||
00211           std::isnan(range_image.points[pi].z)) {
00212         // nan
00213         colored_cloud->points[pi] = nan_point;
00214       }
00215       else {
00216         pcl::PointXYZRGBA input_point;
00217         input_point.x = range_image.points[pi].x;
00218         input_point.y = range_image.points[pi].y;
00219         input_point.z = range_image.points[pi].z;
00220         std::vector<int> indices;
00221         std::vector<float> distances;
00222         kdtree->nearestKSearch(input_point, 1, indices, distances);
00223         if (indices.size() > 0) {
00224           input_point.rgba = cloud->points[indices[0]].rgba;
00225         }
00226         colored_image.at<cv::Vec3b>(pi / range_image.width,
00227                                     pi % range_image.width)
00228           = cv::Vec3b(cloud->points[indices[0]].r,
00229                       cloud->points[indices[0]].g,
00230                       cloud->points[indices[0]].b);
00231         colored_cloud->points[pi] = input_point;
00232       }
00233     }
00234     for (size_t pi = 0; pi < range_image.points.size(); pi++) {
00235       if (!std::isnan(range_image.points[pi].x) &&
00236           !std::isnan(range_image.points[pi].y) &&
00237           !std::isnan(range_image.points[pi].z)) {
00238         // nan
00239         mask.indices.push_back(pi);
00240       }
00241     }
00242     cv_bridge::CvImage colored_range_bridge(dummy_header,
00243                                             sensor_msgs::image_encodings::RGB8,
00244                                             colored_image);
00245     pub_colored_range_image_.publish(colored_range_bridge.toImageMsg());
00246     // // trick, rgba -> rgb
00247     sensor_msgs::PointCloud2 ros_sample_cloud;
00248     pcl::toROSMsg(*colored_cloud, ros_sample_cloud);
00249     pcl::PointCloud<pcl::PointXYZRGB> rgb_cloud;
00250     pcl::fromROSMsg(ros_sample_cloud, rgb_cloud);
00251     pcl::toROSMsg(rgb_cloud, ros_sample_cloud);
00252     ros_sample_cloud.header = dummy_header;
00253     pub_sample_cloud_.publish(ros_sample_cloud);
00254   }
00255   
00256   void LINEMODTrainer::trainWithViewpointSampling()
00257   {
00258     NODELET_INFO("Start LINEMOD training from %lu samples", samples_before_sampling_.size());
00259     if (!camera_info_) {
00260       NODELET_FATAL("no camera info is available");
00261       return;
00262     }
00263     if (samples_before_sampling_.size() != 1) {
00264       NODELET_FATAL("we expect only one training pointcloud, but it has %lu pointclouds",
00265                     samples_before_sampling_.size());
00266       return;
00267     }
00268     
00269     image_geometry::PinholeCameraModel model;
00270     model.fromCameraInfo(camera_info_);
00271     
00272     if (samples_before_sampling_.size() != 1) {
00273       NODELET_FATAL("we expect only one sample data");
00274       return;
00275     }
00276     ViewpointSampler sampler(sample_viewpoint_angle_step_,
00277                                sample_viewpoint_angle_min_,
00278                                sample_viewpoint_angle_max_,
00279                                sample_viewpoint_radius_step_,
00280                                sample_viewpoint_radius_min_,
00281                                sample_viewpoint_radius_max_,
00282                                150);
00283     std::vector<Eigen::Affine3f> transforms;
00284     transforms.resize(sampler.sampleNum());
00285     for (size_t i = 0; i < sampler.sampleNum(); i++) {
00286       Eigen::Affine3f transform;
00287       sampler.get(transform);
00288       transforms[i] = transform;
00289       sampler.next();
00290     }
00291     
00292     // NB:
00293     // This line is super important.
00294     // dummy Range Image to avoid static method initialization in
00295     // multi-thread environment. In detail,
00296     // pcl::RangeImagePlanar::createLookupTables is not thread-safe.
00297     pcl::RangeImagePlanar dummy_range_image;
00298     
00299     pcl::PointCloud<pcl::PointXYZRGBA>::Ptr raw_cloud
00300       = samples_before_sampling_[0];
00301 
00302     // prepare for multi-threaded optimization
00303     std::vector<pcl::ColorGradientModality<pcl::PointXYZRGBA> >
00304       color_modalities (sampler.sampleNum());
00305     std::vector<pcl::SurfaceNormalModality<pcl::PointXYZRGBA> >
00306       surface_norm_modalities (sampler.sampleNum());
00307     std::vector<pcl::MaskMap> mask_maps (sampler.sampleNum());
00308     std::vector<pcl::RegionXY> regions (sampler.sampleNum());
00309     boost::mutex train_mutex;
00310     pcl::LINEMOD linemod;
00311     int counter = 0;
00312     std::vector<Eigen::Affine3f> pose_in_order_of_training;
00313 #ifdef _OPENMP
00314 #pragma omp parallel for
00315 #endif
00316     for (size_t j = 0; j < sampler.sampleNum(); j++) {
00317       
00318       pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBA>);
00319       pcl::PointIndices::Ptr indices(new pcl::PointIndices);
00320       organizedPointCloudWithViewPoint(transforms[j], raw_cloud, model,
00321                                        cloud, *indices);
00322       // generate mask and modalities
00323       pcl::ColorGradientModality<pcl::PointXYZRGBA> color_modality;
00324       pcl::SurfaceNormalModality<pcl::PointXYZRGBA> surface_norm_mod;
00325       pcl::MaskMap mask_map(model.fullResolution().width,
00326                             model.fullResolution().height);
00327       pcl::RegionXY region;
00328       generateLINEMODTrainingData(cloud, indices,
00329                                   color_modality, surface_norm_mod,
00330                                   mask_map, region);
00331       std::vector<pcl::QuantizableModality*> modalities(2);
00332       modalities[0] = &color_modality;
00333       modalities[1] = &surface_norm_mod;
00334       std::vector<pcl::MaskMap*> masks(2);
00335       masks[0] = &mask_map;
00336       masks[1] = &mask_map;
00337       {
00338         boost::mutex::scoped_lock lock(train_mutex);
00339         ++counter;
00340         NODELET_INFO("training: %d/%lu", counter, sampler.sampleNum());
00341         linemod.createAndAddTemplate(modalities, masks, region);
00342         pose_in_order_of_training.push_back(transforms[j]);
00343       }
00344     }
00345     // dump result into file
00346     // 1. linemod file
00347     // 2. original pointcloud into .pcd file
00348     // 3. pose yaml about the template
00349     std::ofstream linemod_file;
00350     const std::string linemod_file_name = output_file_ + ".linemod";
00351     NODELET_INFO("writing to %s", linemod_file_name.c_str());
00352     linemod_file.open(linemod_file_name.c_str(),
00353                       std::ofstream::out | std::ofstream::binary);
00354     linemod.serialize(linemod_file);
00355     linemod_file.close();
00356     const std::string pcd_file_name = output_file_ + ".pcd";
00357     NODELET_INFO("writing to %s", pcd_file_name.c_str());
00358     pcl::PCDWriter writer;
00359     writer.writeBinaryCompressed(pcd_file_name, *raw_cloud);
00360     // pose yaml
00361     std::ofstream pose_file;
00362     const std::string pose_file_name = output_file_ + "_poses.yaml";
00363     pose_file.open(pose_file_name.c_str(), std::ofstream::out);
00364     pose_file << "template_poses: [" << std::endl;
00365     for (size_t i = 0; i < pose_in_order_of_training.size(); i++) {
00366       Eigen::Affine3f pose = pose_in_order_of_training[i];
00367       Eigen::Vector3f pos(pose.translation());
00368       Eigen::Quaternionf rot(pose.rotation());
00369       pose_file << "["
00370                 << pos[0] << ", " << pos[1] << ", " << pos[2] << ", "
00371                 << rot.x() << ", " << rot.y() << ", " << rot.z() << ", " << rot.w() << "]";
00372       if (i != pose_in_order_of_training.size() - 1) {
00373         pose_file << ", " << std::endl;
00374       }
00375       else {
00376         pose_file << "]" << std::endl;
00377       }
00378     }
00379     pose_file.close();
00380     NODELET_INFO("done");
00381   }
00382 
00383   void LINEMODTrainer::generateLINEMODTrainingData(
00384     pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud,
00385     pcl::PointIndices::Ptr mask,
00386     pcl::ColorGradientModality<pcl::PointXYZRGBA>& color_grad_mod,
00387     pcl::SurfaceNormalModality<pcl::PointXYZRGBA>& surface_norm_mod,
00388     pcl::MaskMap& mask_map,
00389     pcl::RegionXY& region)
00390   {
00391     pcl::PointCloud<pcl::PointXYZRGBA>::Ptr masked_cloud
00392       (new pcl::PointCloud<pcl::PointXYZRGBA>);
00393     pcl::ExtractIndices<pcl::PointXYZRGBA> ex;
00394     ex.setKeepOrganized(true);
00395     ex.setInputCloud(cloud);
00396     ex.setIndices(mask);
00397     ex.filter(*masked_cloud);
00398     color_grad_mod.setInputCloud(masked_cloud);
00399     color_grad_mod.processInputData();
00400     surface_norm_mod.setInputCloud(cloud);
00401     surface_norm_mod.processInputData();
00402     size_t min_x(masked_cloud->width), min_y(masked_cloud->height), max_x(0), max_y(0);
00403     for (size_t j = 0; j < masked_cloud->height; ++j) {
00404       for (size_t i = 0; i < masked_cloud->width; ++i) {
00405         pcl::PointXYZRGBA p
00406           = masked_cloud->points[j * masked_cloud->width + i];
00407         if (!std::isnan(p.x) && !std::isnan(p.y) && !std::isnan(p.z)) {
00408           mask_map(i, j) = 1;
00409           min_x = std::min(min_x, i);
00410           max_x = std::max(max_x, i);
00411           min_y = std::min(min_y, j);
00412           max_y = std::max(max_y, j);
00413         }
00414         else {
00415           mask_map(i, j) = 0;
00416         }
00417       }
00418     }
00419     region.x = static_cast<int>(min_x);
00420     region.y = static_cast<int>(min_y);
00421     region.width = static_cast<int>(max_x - min_x + 1);
00422     region.height = static_cast<int>(max_y - min_y + 1);
00423   }
00424                                    
00425 
00426   std::vector<std::string> LINEMODTrainer::trainOneData(
00427     pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud,
00428     pcl::PointIndices::Ptr mask,
00429     std::string& tempstr,
00430     int i)
00431   {
00432     pcl::LINEMOD linemod;
00433     pcl::PointCloud<pcl::PointXYZRGBA>::Ptr masked_cloud
00434       (new pcl::PointCloud<pcl::PointXYZRGBA>);
00435     pcl::ExtractIndices<pcl::PointXYZRGBA> ex;
00436     ex.setKeepOrganized(true);
00437     ex.setInputCloud(cloud);
00438     ex.setIndices(mask);
00439     ex.filter(*masked_cloud);
00440     pcl::ColorGradientModality<pcl::PointXYZRGBA> color_grad_mod;
00441     color_grad_mod.setInputCloud(masked_cloud);
00442     color_grad_mod.processInputData();
00443     pcl::SurfaceNormalModality<pcl::PointXYZRGBA> surface_norm_mod;
00444     surface_norm_mod.setInputCloud(masked_cloud);
00445     surface_norm_mod.processInputData();
00446     std::vector<pcl::QuantizableModality*> modalities(2);
00447     modalities[0] = &color_grad_mod;
00448     modalities[1] = &surface_norm_mod;
00449     size_t min_x(masked_cloud->width), min_y(masked_cloud->height), max_x(0), max_y(0);
00450     pcl::MaskMap mask_map(masked_cloud->width, masked_cloud->height);
00451     for (size_t j = 0; j < masked_cloud->height; ++j) {
00452       for (size_t i = 0; i < masked_cloud->width; ++i) {
00453         pcl::PointXYZRGBA p
00454           = masked_cloud->points[j * masked_cloud->width + i];
00455         if (!std::isnan(p.x) && !std::isnan(p.y) && !std::isnan(p.z)) {
00456           mask_map(i, j) = 1;
00457           min_x = std::min(min_x, i);
00458           max_x = std::max(max_x, i);
00459           min_y = std::min(min_y, j);
00460           max_y = std::max(max_y, j);
00461         }
00462         else {
00463           mask_map(i, j) = 0;
00464         }
00465       }
00466     }
00467     std::vector<pcl::MaskMap*> masks(2);
00468     masks[0] = &mask_map;
00469     masks[1] = &mask_map;
00470     pcl::RegionXY region;
00471     region.x = static_cast<int>(min_x);
00472     region.y = static_cast<int>(min_y);
00473     region.width = static_cast<int>(max_x - min_x + 1);
00474     region.height = static_cast<int>(max_y - min_y + 1);
00475     linemod.createAndAddTemplate(modalities, masks, region);
00476     
00477     std::vector<std::string> ret;
00478     {
00479       // sqmmt
00480       std::stringstream filename_stream;
00481       filename_stream << boost::format("%s/%05d_template.sqmmt") % tempstr % i;
00482       std::string filename = filename_stream.str();
00483       std::cerr << "writing " << filename << std::endl;
00484       std::ofstream file_stream;
00485       file_stream.open(filename.c_str(),
00486                        std::ofstream::out | std::ofstream::binary);
00487       linemod.getTemplate(0).serialize(file_stream);
00488       file_stream.close();
00489       ret.push_back(filename);
00490     }
00491     {
00492       // pcd
00493       std::stringstream filename_stream;
00494       filename_stream << boost::format("%s/%05d_template.pcd") % tempstr % i;
00495       std::string filename = filename_stream.str();
00496       std::cerr << "writing " << filename << std::endl;
00497       pcl::PCDWriter writer;
00498       writer.writeBinaryCompressed(filename, *masked_cloud);
00499       ret.push_back(filename);
00500     }
00501     return ret;
00502   }
00503   
00504   void LINEMODTrainer::trainWithoutViewpointSampling()
00505   {
00506     NODELET_INFO("Start LINEMOD training from %lu samples", samples_.size());
00507     boost::filesystem::path temp = boost::filesystem::unique_path();
00508     boost::filesystem::create_directory(temp);
00509     std::string tempstr = temp.native();
00510     NODELET_INFO("mkdir %s", tempstr.c_str());
00511     std::vector<std::string> all_files;
00512     for (size_t i = 0; i < samples_.size(); i++) {
00513       NODELET_INFO("Processing %lu-th data", i);
00514       pcl::PointIndices::Ptr mask = sample_indices_[i];
00515       pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud = samples_[i];
00516       //std::vector<std::string> files = trainOneData(cloud, mask, tempstr, i);
00517       // for (size_t i = 0; i < files.size(); i++) {
00518       //   all_files.push_back(files[i]);
00519       // }
00520     }
00521     tar(tempstr, output_file_);
00522     NODELET_INFO("done");
00523   }
00524 
00525   void LINEMODTrainer::tar(const std::string& directory, const std::string& output)
00526   {
00527     std::stringstream command_stream;
00528     command_stream << "tar --format=ustar -cf " << output << " " << directory << "/*";
00529     NODELET_INFO("executing %s", command_stream.str().c_str());
00530     int ret = system(command_stream.str().c_str());
00531   }
00532   
00533   bool LINEMODTrainer::startTraining(
00534     std_srvs::Empty::Request& req,
00535     std_srvs::Empty::Response& res)
00536   {
00537     boost::mutex::scoped_lock lock(mutex_);
00538     if (sample_viewpoint_) {
00539       trainWithViewpointSampling();
00540     }
00541     else {
00542       trainWithoutViewpointSampling();
00543     }
00544     return true;
00545   }
00546 
00547   void LINEMODDetector::onInit()
00548   {
00549     DiagnosticNodelet::onInit();
00550     pnh_->param("template_file", template_file_, std::string("template"));
00551     // load original point and poses
00552     template_cloud_.reset(new pcl::PointCloud<pcl::PointXYZRGBA>);
00553     pcl::PCDReader reader;
00554     reader.read(template_file_ + ".pcd", *template_cloud_);
00555     const std::string pose_yaml = template_file_ + "_poses.yaml";
00556     YAML::Node doc;
00557 #ifdef USE_OLD_YAML
00558     std::ifstream pose_fin;
00559     pose_fin.open(pose_yaml.c_str(), std::ifstream::in);
00560     YAML::Parser parser(pose_fin);
00561     while (parser.GetNextDocument(doc)) {
00562       const YAML::Node& template_pose_yaml = doc["template_poses"];
00563       for (size_t i = 0; i < template_pose_yaml.size(); i++) {
00564         const YAML::Node& pose = template_pose_yaml[i];
00565         Eigen::Affine3f trans = jsk_recognition_utils::affineFromYAMLNode(pose);
00566         template_poses_.push_back(trans);
00567         // set template_bboxes
00568         pcl::PointCloud<pcl::PointXYZRGBA> transformed_cloud;
00569         pcl::transformPointCloud<pcl::PointXYZRGBA>(
00570           *template_cloud_, transformed_cloud, trans);
00571         // compute size of bounding box
00572         Eigen::Vector4f minpt, maxpt;
00573         pcl::getMinMax3D<pcl::PointXYZRGBA>(transformed_cloud, minpt, maxpt);
00574         jsk_recognition_msgs::BoundingBox bbox = jsk_recognition_utils::boundingBoxFromPointCloud(transformed_cloud);
00575         //ROS_INFO("bounding box size: [%f, %f, %f]", bbox.dimensions.x, bbox.dimensions.y, bbox.dimensions.z);
00576         template_bboxes_.push_back(bbox);
00577       }
00578     }
00579     pose_fin.close();
00580 #else
00581      // yaml-cpp is greater than 0.5.0
00582      doc = YAML::LoadFile(pose_yaml);
00583 #endif
00584 
00585     
00586     srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00587     dynamic_reconfigure::Server<Config>::CallbackType f =
00588       boost::bind (
00589         &LINEMODDetector::configCallback, this, _1, _2);
00590     srv_->setCallback (f);
00591     
00592     pub_cloud_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output", 1);
00593     pub_detect_mask_ = advertise<sensor_msgs::Image>(*pnh_, "output/mask", 1);
00594     pub_pose_ = advertise<geometry_msgs::PoseStamped>(*pnh_, "output/pose", 1);
00595     pub_original_template_cloud_ = advertise<sensor_msgs::PointCloud2>(
00596       *pnh_, "output/template", 1);
00597   }
00598 
00599   void LINEMODDetector::subscribe()
00600   {
00601     sub_cloud_ = pnh_->subscribe("input", 1, &LINEMODDetector::detect, this);
00602   }
00603 
00604   void LINEMODDetector::unsubscribe()
00605   {
00606     sub_cloud_.shutdown();
00607   }
00608   
00609   void LINEMODDetector::configCallback(Config &config, uint32_t level)
00610   {
00611     boost::mutex::scoped_lock lock(mutex_);
00612     gradient_magnitude_threshold_ = config.gradient_magnitude_threshold;
00613     detection_threshold_ = config.detection_threshold;
00614     color_gradient_mod_.setGradientMagnitudeThreshold(gradient_magnitude_threshold_);
00615     linemod_.setDetectionThreshold(detection_threshold_);
00616 
00617     // desearlize
00618     const std::string linemod_file = template_file_ + ".linemod";
00619     std::ifstream linemod_in;
00620     linemod_in.open(linemod_file.c_str(), std::ifstream::in);
00621     linemod_.deserialize(linemod_in);
00622     linemod_in.close();
00623   }
00624 
00625   void LINEMODDetector::updateDiagnostic(
00626     diagnostic_updater::DiagnosticStatusWrapper &stat)
00627   {
00628     if (vital_checker_->isAlive()) {
00629       stat.summary(diagnostic_msgs::DiagnosticStatus::OK,
00630                    "LINEMODDetector running");
00631     }
00632     else {
00633       jsk_topic_tools::addDiagnosticErrorSummary(
00634         "LINEMODDetector", vital_checker_, stat);
00635     }
00636   }
00637 
00638   void LINEMODDetector::computeCenterOfTemplate(
00639     pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud,
00640     const pcl::SparseQuantizedMultiModTemplate& linemod_template,
00641     const pcl::LINEMODDetection& linemod_detection,
00642     Eigen::Vector3f& center)
00643   {
00644     const size_t start_x = std::max(linemod_detection.x, 0);
00645     const size_t start_y = std::max(linemod_detection.y, 0);
00646     const size_t end_x = std::min(
00647       static_cast<size_t> (start_x + linemod_template.region.width * linemod_detection.scale),
00648       static_cast<size_t> (cloud->width));
00649     const size_t end_y = std::min(
00650       static_cast<size_t> (start_y + linemod_template.region.height * linemod_detection.scale),
00651       static_cast<size_t> (cloud->height));
00652     size_t counter = 0;
00653     for (size_t row_index = start_y; row_index < end_y; ++row_index) {
00654       for (size_t col_index = start_x; col_index < end_x; ++col_index) {
00655         const pcl::PointXYZRGBA & point = (*cloud) (col_index, row_index);
00656         if (pcl_isfinite (point.x) &&
00657             pcl_isfinite (point.y) &&
00658             pcl_isfinite (point.z)) {
00659           center = center + point.getVector3fMap();
00660           ++counter;
00661         }
00662       }
00663     }
00664     center = center / counter;
00665   }
00666   
00667   void LINEMODDetector::detect(
00668     const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
00669   {
00670     NODELET_INFO("detect");
00671     vital_checker_->poke();
00672     boost::mutex::scoped_lock lock(mutex_);
00673     pcl::PointCloud<pcl::PointXYZRGBA>::Ptr
00674       cloud (new pcl::PointCloud<pcl::PointXYZRGBA>);
00675     pcl::fromROSMsg(*cloud_msg, *cloud);
00676 
00677     surface_normal_mod_.setInputCloud(cloud);
00678     surface_normal_mod_.processInputData ();
00679     color_gradient_mod_.setInputCloud (cloud);
00680     color_gradient_mod_.processInputData ();
00681     std::vector<pcl::LINEMODDetection> linemod_detections;
00682     std::vector<pcl::QuantizableModality*> modalities;
00683     modalities.push_back(&color_gradient_mod_);
00684     modalities.push_back(&surface_normal_mod_);
00685     linemod_.detectTemplatesSemiScaleInvariant(modalities, linemod_detections,
00686                                                0.6944444f, 1.44f, 1.2f);
00687     NODELET_INFO("detected %lu result", linemod_detections.size());
00688     // lookup the best result
00689     if (linemod_detections.size() > 0) {
00690       double max_score = 0;
00691       size_t max_template_id = 0;
00692       double max_scale = 0;
00693       pcl::LINEMODDetection linemod_detection;
00694       for (size_t i = 0; i < linemod_detections.size(); i++) {
00695         const pcl::LINEMODDetection& detection = linemod_detections[i];
00696         if (max_score < detection.score) {
00697           linemod_detection = detection;
00698           max_score = detection.score;
00699         }
00700       }
00701       
00702       const pcl::SparseQuantizedMultiModTemplate& linemod_template = 
00703         linemod_.getTemplate(linemod_detection.template_id);
00704       Eigen::Vector3f center(0, 0, 0);
00705       computeCenterOfTemplate(
00706         cloud, linemod_template, linemod_detection, center);
00707       // publish mask image here
00708       cv::Mat detect_mask = cv::Mat::zeros(cloud->width, cloud->height, CV_8UC1);
00709       int scaled_template_width
00710         = linemod_template.region.width * linemod_detection.scale;
00711       int scaled_template_height
00712         = linemod_template.region.height * linemod_detection.scale;
00713       cv::rectangle(
00714         detect_mask,
00715         cv::Point(linemod_detection.x, linemod_detection.y),
00716         cv::Point(linemod_detection.x + scaled_template_width,
00717                   linemod_detection.y + scaled_template_height),
00718         cv::Scalar(255), CV_FILLED);
00719       pub_detect_mask_.publish(cv_bridge::CvImage(cloud_msg->header,
00720                                                   "8UC1",
00721                                                   detect_mask).toImageMsg());
00722       // compute translation
00723       jsk_recognition_msgs::BoundingBox bbox = template_bboxes_[linemod_detection.template_id];
00724       pcl::PointCloud<pcl::PointXYZRGBA>::Ptr 
00725         result (new pcl::PointCloud<pcl::PointXYZRGBA>);
00726       pcl::transformPointCloud<pcl::PointXYZRGBA>(
00727         *template_cloud_, *result, template_poses_[linemod_detection.template_id]);
00728       Eigen::Vector4f minpt, maxpt;
00729       pcl::getMinMax3D<pcl::PointXYZRGBA>(*result, minpt, maxpt);
00730       Eigen::Vector4f template_center = (minpt + maxpt) / 2;
00731       Eigen::Vector3f translation = center - Eigen::Vector3f(template_center[0],
00732                                                              template_center[1],
00733                                                              template_center[2]);
00734       Eigen::Affine3f pose = template_poses_[linemod_detection.template_id] * Eigen::Translation3f(translation);
00735       geometry_msgs::PoseStamped ros_pose;
00736       tf::poseEigenToMsg(pose, ros_pose.pose);
00737       ros_pose.header = cloud_msg->header;
00738       pub_pose_.publish(ros_pose);
00739       for (size_t i = 0; i < result->points.size(); i++) {
00740         result->points[i].getVector3fMap()
00741           = result->points[i].getVector3fMap() + translation;
00742       }
00743       sensor_msgs::PointCloud2 ros_result;
00744       pcl::toROSMsg(*result, ros_result);
00745       ros_result.header = cloud_msg->header;
00746       pub_cloud_.publish(ros_result);
00747       sensor_msgs::PointCloud2 ros_template;
00748       pcl::toROSMsg(*template_cloud_, ros_template);
00749       ros_template.header = cloud_msg->header;
00750       pub_original_template_cloud_.publish(ros_template);
00751     }
00752   }
00753 }
00754 
00755 #include <pluginlib/class_list_macros.h>
00756 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::LINEMODTrainer, nodelet::Nodelet);
00757 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::LINEMODDetector, nodelet::Nodelet);


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Sun Oct 8 2017 02:43:50