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


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Wed Sep 16 2015 04:36:48