edge_depth_refinement_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 
00036 #include "jsk_pcl_ros/edge_depth_refinement.h"
00037 
00038 #include <pcl/segmentation/sac_segmentation.h>
00039 #include <pcl/filters/extract_indices.h>
00040 #include <pcl/io/io.h>
00041 #include "jsk_recognition_utils/pcl_util.h"
00042 #include <sstream>
00043 
00044 namespace jsk_pcl_ros
00045 {
00046   void EdgeDepthRefinement::onInit()
00047   {
00048     ConnectionBasedNodelet::onInit();
00049 
00051     // publishers
00053     pub_indices_ = advertise<jsk_recognition_msgs::ClusterPointIndices>(
00054       *pnh_, "output", 1);
00055     pub_coefficients_ = advertise<jsk_recognition_msgs::ModelCoefficientsArray>(
00056       *pnh_, "output_coefficients", 1);
00057     pub_edges_ = advertise<jsk_recognition_msgs::SegmentArray>(
00058       *pnh_, "output_edges", 1);
00059     pub_outlier_removed_indices_ = advertise<jsk_recognition_msgs::ClusterPointIndices>(
00060       *pnh_, "output_outlier_removed", 1);
00061     pub_outlier_removed_coefficients_ = advertise<jsk_recognition_msgs::ModelCoefficientsArray>(
00062       *pnh_, "output_outlier_removed_coefficients", 1);
00063     pub_outlier_removed_edges_= advertise<jsk_recognition_msgs::SegmentArray>(
00064       *pnh_, "output_outlier_removed_edges", 1);
00066     // dynamic reconfigure
00068     srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00069     dynamic_reconfigure::Server<Config>::CallbackType f =
00070       boost::bind (&EdgeDepthRefinement::configCallback, this, _1, _2);
00071     srv_->setCallback (f);
00072 
00073     onInitPostProcess();
00074   }
00075 
00076   void EdgeDepthRefinement::subscribe()
00077   {
00079     // subscribesrs
00081     sub_input_.subscribe(*pnh_, "input", 1);
00082     sub_indices_.subscribe(*pnh_, "input_indices", 1);
00083     sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
00084     sync_->connectInput(sub_input_, sub_indices_);
00085     sync_->registerCallback(boost::bind(&EdgeDepthRefinement::refine,
00086                                         this, _1, _2));
00087   }
00088 
00089   void EdgeDepthRefinement::unsubscribe()
00090   {
00091     sub_input_.unsubscribe();
00092     sub_indices_.unsubscribe();
00093   }
00094   
00096   // line coefficients are:
00097   // 0: point.x
00098   // 1: point.y
00099   // 2. point.z
00100   // 3. direction.x
00101   // 4: direction.y
00102   // 5. direction.z
00104   jsk_recognition_utils::Line::Ptr EdgeDepthRefinement::lineFromCoefficients(
00105     const pcl::ModelCoefficients::Ptr coefficients)
00106   {
00107     Eigen::Vector3f p(coefficients->values[0],
00108                       coefficients->values[1],
00109                       coefficients->values[2]);
00110     Eigen::Vector3f d(coefficients->values[3],
00111                       coefficients->values[4],
00112                       coefficients->values[5]);
00113     jsk_recognition_utils::Line::Ptr ret (new jsk_recognition_utils::Line(d, p));
00114     return ret;
00115   }
00116 
00117   boost::tuple<int, int> EdgeDepthRefinement::findMinMaxIndex(
00118     const int width, const int height,
00119     const std::vector<int>& indices)
00120   {
00121     
00122     int min_y_index, max_y_index, min_x_index, max_x_index;
00123     int min_y = INT_MAX;
00124     int min_x = INT_MAX;
00125     int max_y = INT_MIN;
00126     int max_x = INT_MIN;
00127     for (size_t i = 0; i < indices.size(); i++) {
00128       int index = indices[i];
00129       int x = index % width;
00130       int y = index / width;
00131 
00132       if (x > max_x) {
00133         max_x = x;
00134         max_x_index = index;
00135       }
00136       if (x < min_x) {
00137         min_x = x;
00138         min_x_index = index;
00139       }
00140       if (y > max_y) {
00141         max_y = y;
00142         max_y_index = index;
00143       }
00144       if (y < min_y) {
00145         min_y = y;
00146         min_y_index = index;
00147       }
00148     }
00149 
00150     if (min_x_index != max_x_index) {
00151       return boost::make_tuple(
00152         min_x_index, max_x_index);
00153     }
00154     else {
00155       return boost::make_tuple(
00156         min_y_index, max_y_index);
00157     }
00158   }
00159 
00160   
00161   jsk_recognition_utils::Segment::Ptr EdgeDepthRefinement::segmentFromIndices(
00162     const pcl::PointCloud<PointT>::Ptr& cloud,
00163     const std::vector<int>& indices,
00164     const jsk_recognition_utils::Line::Ptr& line)
00165   {
00166     boost::tuple<int, int> min_max
00167       = findMinMaxIndex(cloud->width, cloud->height, indices);
00168     PointT min_point = cloud->points[min_max.get<0>()];
00169     PointT max_point = cloud->points[min_max.get<1>()];
00170     Eigen::Vector3f min_point_f = min_point.getVector3fMap();
00171     Eigen::Vector3f max_point_f = max_point.getVector3fMap();
00172     Eigen::Vector3f min_foot, max_foot;
00173     line->foot(min_point_f, min_foot);
00174     line->foot(max_point_f, max_foot);
00175     jsk_recognition_utils::Segment::Ptr segment (new jsk_recognition_utils::Segment(min_foot, max_foot));
00176     return segment;
00177   }
00178 
00179   void EdgeDepthRefinement::integrateDuplicatedIndices(
00180     const pcl::PointCloud<PointT>::Ptr& cloud,
00181     const std::set<int>& duplicated_set,
00182     const std::vector<pcl::PointIndices::Ptr> all_inliers,
00183     pcl::PointIndices::Ptr& output_indices)
00184   {
00185     std::vector<int> integrated_indices;
00186     for (std::set<int>::iterator it = duplicated_set.begin();
00187          it != duplicated_set.end();
00188          ++it) {
00189       integrated_indices = jsk_recognition_utils::addIndices(all_inliers[*it]->indices,
00190                                       integrated_indices);
00191     }
00192     output_indices->indices = integrated_indices;
00193   }
00194   
00195   void EdgeDepthRefinement::removeDuplicatedEdges(
00196     const pcl::PointCloud<PointT>::Ptr& cloud,
00197     const std::vector<pcl::PointIndices::Ptr> all_inliers,
00198     const std::vector<pcl::ModelCoefficients::Ptr> all_coefficients,
00199     std::vector<pcl::PointIndices::Ptr>& output_inliers,
00200     std::vector<pcl::ModelCoefficients::Ptr>& output_coefficients)
00201   {
00202     if (all_inliers.size() == 0) {
00203       NODELET_ERROR("no edges are specified");
00204       return;
00205     }
00206 
00207     // buildup Lines and Segments
00208     std::vector<jsk_recognition_utils::Line::Ptr> lines;
00209     std::vector<jsk_recognition_utils::Segment::Ptr> segments;
00210     
00211     for (size_t i = 0; i < all_inliers.size(); i++) {
00212       pcl::PointIndices::Ptr the_inliers = all_inliers[i];
00213       pcl::ModelCoefficients::Ptr the_coefficients = all_coefficients[i];
00214       jsk_recognition_utils::Line::Ptr the_line = lineFromCoefficients(the_coefficients);
00215       jsk_recognition_utils::Segment::Ptr the_segment
00216         = segmentFromIndices(cloud, the_inliers->indices, the_line);
00217       lines.push_back(the_line);
00218       segments.push_back(the_segment);
00219     }
00220 
00222     // build duplication map
00223     // duplication map is a hash map from int to a list of int
00225     std::map<int, std::vector<int> > duplication_map;
00226     for (size_t i = 0; i < all_inliers.size() - 1; i++) {
00227       duplication_map[i] = std::vector<int>(); // add empty map
00228       jsk_recognition_utils::Line::Ptr the_line = lines[i];
00229       jsk_recognition_utils::Segment::Ptr the_segment = segments[i];
00230       for (size_t j = i + 1; j < all_inliers.size(); j++) {
00231         jsk_recognition_utils::Line::Ptr candidate_line = lines[j];
00232         jsk_recognition_utils::Segment::Ptr candidate_segment = segments[j];
00233         Eigen::Vector3f candidate_midpoint;
00234         candidate_segment->midpoint(candidate_midpoint);
00235 
00236         double angle_diff = the_line->angle(*candidate_line);
00237         if (duplication_angle_threshold_ > angle_diff) {
00238           double distance_diff = the_segment->distance(candidate_midpoint);
00239           if (duplication_distance_threshold_ > distance_diff) {
00240             duplication_map[i].push_back(j);
00241           }
00242         }
00243       }
00244     }
00245 
00247     // convert duplication map into set
00249     std::vector<std::set<int> > duplication_set_list;
00250     std::set<int> duplicated_indices;
00251     for (size_t i = 0; i < all_inliers.size(); i++) {
00252       std::vector<int> duplication_list;
00253       if (i < all_inliers.size() - 1) {
00254         duplication_list = duplication_map[i];
00255       }
00256       if (duplicated_indices.find(i) == duplicated_indices.end()) {
00257         if (i == all_inliers.size() - 1 || duplication_list.size() == 0) { // no duplication found
00258           std::set<int> no_duplication_set;
00259           no_duplication_set.insert(i);
00260           duplication_set_list.push_back(no_duplication_set);
00261           jsk_recognition_utils::addSet<int>(duplicated_indices, no_duplication_set);
00262         }
00263         else { // some duplication found
00264           std::set<int> new_duplication_set;
00265           jsk_recognition_utils::buildGroupFromGraphMap(duplication_map,
00266                                  i,
00267                                  duplication_list,
00268                                  new_duplication_set);
00269           duplication_set_list.push_back(new_duplication_set);
00270           // add new_duplication_set to duplicated_indices
00271           jsk_recognition_utils::addSet<int>(duplicated_indices, new_duplication_set);
00272         }
00273       }
00274     }
00275 
00276 
00277     for (size_t i = 0; i < duplication_set_list.size(); i++) {
00278       pcl::PointIndices::Ptr integrated_indices (new pcl::PointIndices);
00279       integrateDuplicatedIndices(cloud, duplication_set_list[i],
00280                                  all_inliers,
00281                                  integrated_indices);
00282       output_inliers.push_back(integrated_indices);
00283       
00284       // use the first one,,, ok?
00285       pcl::ModelCoefficients::Ptr integrated_coefficients 
00286         = all_coefficients[(*duplication_set_list[i].begin())];
00287       output_coefficients.push_back(integrated_coefficients);
00288     }
00289 
00290     // print result for debug
00291     // NODELET_INFO("%lu duplication set", duplication_set_list.size());
00292     // for (size_t i = 0; i < duplication_set_list.size(); i++) {
00293     //   std::stringstream ss;
00294     //   ss << "[";
00295     //   for (std::set<int>::iterator it = duplication_set_list[i].begin();
00296     //        it != duplication_set_list[i].end();
00297     //        ++it)
00298     //   {
00299     //     ss << *it << ", ";
00300     //   }
00301     //   NODELET_INFO("%s", ss.str().c_str());
00302     // }
00303     
00304     // for (size_t i = 0; i < all_inliers.size() - 1; i++) {
00305     //   std::stringstream ss;
00306     //   std::vector<int> similar_indices = duplication_map[i];
00307     //   ss << i << " -> ";
00308     //   for (size_t j = 0; j < similar_indices.size(); j++) {
00309     //     ss << similar_indices[j] << ", ";
00310     //   }
00311     //   NODELET_INFO("%s", ss.str().c_str());
00312     // }
00313   }
00314   
00315   void EdgeDepthRefinement::removeOutliers(
00316     const pcl::PointCloud<PointT>::Ptr& cloud,
00317     const std::vector<PCLIndicesMsg>& indices,
00318     std::vector<pcl::PointIndices::Ptr>& output_inliers,
00319     std::vector<pcl::ModelCoefficients::Ptr>& output_coefficients)
00320   {
00321     // output_inliers.resize(indices.size());
00322     // output_coefficients.resize(indices.size());
00323     for (size_t i = 0; i < indices.size(); i++) {
00324       std::vector<int> cluster_indices = indices[i].indices;
00325       pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
00326       pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
00327       removeOutliersByLine(cloud, cluster_indices, *inliers, *coefficients);
00328       if (inliers->indices.size() > min_inliers_) {
00329         output_inliers.push_back(inliers);
00330         output_coefficients.push_back(coefficients);
00331       }
00332     }
00333   }
00334   
00335   void EdgeDepthRefinement::removeOutliersByLine(
00336     const pcl::PointCloud<PointT>::Ptr& cloud,
00337     const std::vector<int>& indices,
00338     pcl::PointIndices& inliers,
00339     pcl::ModelCoefficients& coefficients)
00340   {
00341     // one line shoud have ONE line at most...?
00342     // in that case, we can estimate the true line by RANSAC
00343     pcl::SACSegmentation<PointT> seg;
00344     seg.setOptimizeCoefficients (true);
00345     seg.setModelType (pcl::SACMODEL_LINE);
00346     seg.setMethodType (pcl::SAC_RANSAC);
00347     seg.setDistanceThreshold (outlier_distance_threshold_);
00348     seg.setInputCloud(cloud);
00349     pcl::PointIndices::Ptr indices_ptr (new pcl::PointIndices);
00350     indices_ptr->indices = indices;
00351     seg.setIndices(indices_ptr);
00352     seg.segment (inliers, coefficients);
00353   }
00354 
00355   void EdgeDepthRefinement::publishIndices(
00356     ros::Publisher& pub,
00357     ros::Publisher& pub_coefficients,
00358     ros::Publisher& pub_edges,
00359     const std::vector<pcl::PointIndices::Ptr> inliers,
00360     const std::vector<pcl::ModelCoefficients::Ptr> coefficients,
00361     const std_msgs::Header& header)
00362   {
00363     jsk_recognition_msgs::ClusterPointIndices output_ros_msg;
00364     jsk_recognition_msgs::ModelCoefficientsArray output_ros_coefficients_msg;
00365     jsk_recognition_msgs::SegmentArray output_ros_edges_msg;
00366     output_ros_msg.header = header;
00367     output_ros_coefficients_msg.header = header;
00368     output_ros_edges_msg.header = header;
00369     for (size_t i = 0; i < inliers.size(); i++) {
00370       PCLIndicesMsg output_indices_msg;
00371       PCLModelCoefficientMsg output_coefficients_msg;
00372       jsk_recognition_msgs::Segment output_edge_msg;
00373       output_indices_msg.header = header;
00374       output_indices_msg.indices = inliers[i]->indices;
00375       output_ros_msg.cluster_indices.push_back(output_indices_msg);
00376 
00377       output_coefficients_msg.header = header;
00378       output_coefficients_msg.values = coefficients[i]->values;
00379       output_ros_coefficients_msg.coefficients.push_back(output_coefficients_msg);
00380 
00381       output_edge_msg.start_point.x = coefficients[i]->values[0] - coefficients[i]->values[3];
00382       output_edge_msg.start_point.y = coefficients[i]->values[1] - coefficients[i]->values[4];
00383       output_edge_msg.start_point.z = coefficients[i]->values[2] - coefficients[i]->values[5];
00384       output_edge_msg.end_point.x = coefficients[i]->values[0] + coefficients[i]->values[3];
00385       output_edge_msg.end_point.y = coefficients[i]->values[1] + coefficients[i]->values[4];
00386       output_edge_msg.end_point.z = coefficients[i]->values[2] + coefficients[i]->values[5];
00387       output_ros_edges_msg.segments.push_back(output_edge_msg);
00388     }
00389     pub.publish(output_ros_msg);
00390     pub_coefficients.publish(output_ros_coefficients_msg);
00391     pub_edges.publish(output_ros_edges_msg);
00392   }
00393 
00394   void EdgeDepthRefinement::configCallback (Config &config, uint32_t level)
00395   {
00396     boost::mutex::scoped_lock lock(mutex_);
00397     outlier_distance_threshold_ = config.outlier_distance_threshold;
00398     min_inliers_ = config.min_inliers;
00399     duplication_angle_threshold_ = config.duplication_angle_threshold;
00400     duplication_distance_threshold_ = config.duplication_distance_threshold;
00401   }
00402   
00403   void EdgeDepthRefinement::refine(
00404     const sensor_msgs::PointCloud2ConstPtr &input,
00405     const jsk_recognition_msgs::ClusterPointIndicesConstPtr &indices)
00406   {
00407     boost::mutex::scoped_lock lock(mutex_);
00408     pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
00409     pcl::fromROSMsg(*input, *cloud);
00410     
00411     std::vector<pcl::PointIndices::Ptr> inliers;
00412     std::vector<pcl::ModelCoefficients::Ptr> coefficients;
00413 
00414     removeOutliers(cloud, indices->cluster_indices, inliers, coefficients);
00415     std::vector<pcl::PointIndices::Ptr> non_duplicated_inliers;
00416     std::vector<pcl::ModelCoefficients::Ptr> non_duplicated_coefficients;
00417     removeDuplicatedEdges(cloud, inliers, coefficients,
00418                           non_duplicated_inliers,
00419                           non_duplicated_coefficients);
00420     publishIndices(pub_outlier_removed_indices_,
00421                    pub_outlier_removed_coefficients_,
00422                    pub_outlier_removed_edges_,
00423                    inliers, coefficients,
00424                    input->header);
00425     publishIndices(pub_indices_,
00426                    pub_coefficients_,
00427                    pub_edges_,
00428                    non_duplicated_inliers,
00429                    non_duplicated_coefficients,
00430                    input->header);
00431   }
00432   
00433 }
00434 
00435 #include <pluginlib/class_list_macros.h>
00436 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::EdgeDepthRefinement, nodelet::Nodelet);


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