parallel_edge_finder_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/parallel_edge_finder.h"
00037 #include "jsk_recognition_utils/pcl_util.h"
00038 #include <jsk_recognition_msgs/ParallelEdgeArray.h>
00039 
00040 namespace jsk_pcl_ros
00041 {
00042   void ParallelEdgeFinder::onInit()
00043   {
00044     ConnectionBasedNodelet::onInit();
00045     
00047     // publishers
00049     pub_ = advertise<jsk_recognition_msgs::ParallelEdgeArray>(*pnh_, "output_edges_groups", 1);
00050     pub_clusters_ = advertise<jsk_recognition_msgs::ClusterPointIndices>(*pnh_, "output_clusters", 1);
00051 
00053     // dynamic reconfigure
00055     srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00056     dynamic_reconfigure::Server<Config>::CallbackType f =
00057       boost::bind (&ParallelEdgeFinder::configCallback, this, _1, _2);
00058     srv_->setCallback (f);
00059     onInitPostProcess();
00060   }
00061 
00062   void ParallelEdgeFinder::subscribe()
00063   {
00065     // subscription
00067     sub_indices_.subscribe(*pnh_, "input_indices", 1);
00068     sub_coefficients_.subscribe(*pnh_, "input_coefficients", 1);
00069     sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
00070     sync_->connectInput(sub_indices_, sub_coefficients_);
00071     sync_->registerCallback(boost::bind(&ParallelEdgeFinder::estimate,
00072                                         this, _1, _2));
00073   }
00074 
00075   void ParallelEdgeFinder::unsubscribe()
00076   {
00077     sub_indices_.unsubscribe();
00078     sub_coefficients_.unsubscribe();
00079   }
00080 
00081   void ParallelEdgeFinder::configCallback(
00082     Config &config, uint32_t level)
00083   {
00084     boost::mutex::scoped_lock lock(mutex_);
00085     angle_threshold_ = config.angular_threshold;
00086   }
00087   
00088   void ParallelEdgeFinder::estimate(
00089     const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& input_indices,
00090     const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr& input_coefficients)
00091   {
00092     boost::mutex::scoped_lock lock(mutex_);
00094     // first, build Line instances
00096     std::vector<jsk_recognition_utils::Line::Ptr> lines;
00097     if (input_coefficients->coefficients.size() == 0) {
00098       return;
00099     }
00100     for (size_t i = 0; i < input_coefficients->coefficients.size(); i++) {
00101       std::vector<float> the_coefficients
00102         = input_coefficients->coefficients[i].values;
00103       lines.push_back(jsk_recognition_utils::Line::fromCoefficients(the_coefficients));
00104     }
00105 
00106     std::map<int, std::vector<int> > parallel_map;
00107     for (size_t i = 0; i < lines.size() - 1; i++) {
00108       parallel_map[i] = std::vector<int>();
00109       jsk_recognition_utils::Line::Ptr the_line = lines[i];
00110       for (size_t j = i + 1; j < lines.size(); j++) {
00111         jsk_recognition_utils::Line::Ptr candidate_line = lines[j];
00112         if (the_line->isParallel(*candidate_line, angle_threshold_)) {
00113           parallel_map[i].push_back(j);
00114         }
00115       }
00116     }
00117     
00118     // build 'Group' recursively
00119     // list of set of the indices of parallel edges
00120     std::vector<std::set<int> > parallel_groups_list;
00121     
00122     // set of the indices which are already regarded as one of
00123     // parallel edges
00124     std::set<int> listed_indices;
00125 
00126     for (size_t i = 0; i < lines.size() - 1; i++) {
00127       std::vector<int> parallel_indices = parallel_map[i];
00128       if (listed_indices.find(i) == listed_indices.end()) {
00129         if (parallel_indices.size() == 0) {
00130           // nothing to do
00131         }
00132         else {
00133           std::set<int> new_parallel_set;
00134           jsk_recognition_utils::buildGroupFromGraphMap(parallel_map,
00135                                  i,
00136                                  parallel_indices,
00137                                  new_parallel_set);
00138           parallel_groups_list.push_back(new_parallel_set);
00139           jsk_recognition_utils::addSet(listed_indices, new_parallel_set);
00140         }
00141       }
00142     }
00143     // publish the result
00144     publishResult(parallel_groups_list, input_indices, input_coefficients);
00145     publishResultAsCluser(parallel_groups_list, input_indices,
00146                           input_coefficients);
00147   }
00148 
00149   void ParallelEdgeFinder::publishResult(
00150     const std::vector<std::set<int> >& parallel_groups_list,
00151     const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& input_indices,
00152     const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr& input_coefficients)
00153   {
00154     jsk_recognition_msgs::ParallelEdgeArray ros_output_msg;
00155     ros_output_msg.header = input_indices->header;
00156     for (size_t i = 0; i < parallel_groups_list.size(); i++) {
00157       std::set<int> parallel_groups = parallel_groups_list[i];
00158       jsk_recognition_msgs::ParallelEdge edge_group;
00159       edge_group.header = input_indices->header;
00160       for (std::set<int>::iterator it = parallel_groups.begin();
00161            it != parallel_groups.end();
00162            ++it) {
00163         int the_index = *it;
00164         std::vector<int> indices
00165           = input_indices->cluster_indices[the_index].indices;
00166         std::vector<float> coefficients
00167           = input_coefficients->coefficients[the_index].values;
00168         PCLIndicesMsg indices_msg;
00169         indices_msg.header = input_indices->header;
00170         indices_msg.indices = indices;
00171         PCLModelCoefficientMsg coefficients_msg;
00172         coefficients_msg.header = input_coefficients->header;
00173         coefficients_msg.values = coefficients;
00174         edge_group.cluster_indices.push_back(indices_msg);
00175         edge_group.coefficients.push_back(coefficients_msg);
00176       }
00177       ros_output_msg.edge_groups.push_back(edge_group);
00178     }
00179     pub_.publish(ros_output_msg);
00180   }
00181 
00182   void ParallelEdgeFinder::publishResultAsCluser(
00183       const std::vector<std::set<int> >& parallel_groups_list,
00184       const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& input_indices,
00185       const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr& input_coefficients)
00186   {
00187     jsk_recognition_msgs::ClusterPointIndices ros_output_msg;
00188     ros_output_msg.header = input_indices->header;
00189 
00190     for (size_t i = 0; i < parallel_groups_list.size(); i++) {
00191       std::set<int> parallel_groups = parallel_groups_list[i];
00192       PCLIndicesMsg indices_msg;
00193       indices_msg.header = input_indices->header;
00194       for (std::set<int>::iterator it = parallel_groups.begin();
00195            it != parallel_groups.end();
00196            ++it) {
00197         indices_msg.indices = jsk_recognition_utils::addIndices(
00198           indices_msg.indices,
00199           input_indices->cluster_indices[*it].indices);
00200       }
00201       ros_output_msg.cluster_indices.push_back(indices_msg);
00202     }
00203     pub_clusters_.publish(ros_output_msg);
00204   }
00205   
00206 }
00207 
00208 #include <pluginlib/class_list_macros.h>
00209 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::ParallelEdgeFinder, nodelet::Nodelet);
00210 


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