line_segment_detector_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 #include "jsk_recognition_utils/pcl_conversion_util.h"
00036 #include "jsk_pcl_ros/line_segment_detector.h"
00037 #include <visualization_msgs/Marker.h>
00038 #include <pcl/sample_consensus/method_types.h>
00039 #include <pcl/sample_consensus/model_types.h>
00040 #include <pcl/segmentation/sac_segmentation.h>
00041 #include <pcl/filters/extract_indices.h>
00042 #include <jsk_topic_tools/color_utils.h>
00043 
00044 namespace jsk_pcl_ros
00045 {
00046 
00047   LineSegment::LineSegment(
00048     pcl::PointIndices::Ptr indices,
00049     pcl::ModelCoefficients::Ptr coefficients):
00050     indices_(indices), coefficients_(coefficients)
00051   {
00052   }
00053   
00054   LineSegment::LineSegment(
00055     const std_msgs::Header& input_header,
00056     pcl::PointIndices::Ptr indices,
00057     pcl::ModelCoefficients::Ptr coefficients,
00058     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud):
00059     header(input_header),
00060     indices_(indices), coefficients_(coefficients),
00061     points_(new pcl::PointCloud<pcl::PointXYZ>),
00062     raw_points_(new pcl::PointCloud<pcl::PointXYZ>)
00063   {
00064     pcl::ProjectInliers<pcl::PointXYZ> proj;
00065     proj.setInputCloud(cloud);
00066     proj.setIndices(indices);
00067     proj.setModelType(pcl::SACMODEL_LINE);
00068     proj.setModelCoefficients(coefficients);
00069     proj.filter(*points_);
00070     pcl::ExtractIndices<pcl::PointXYZ> ex;
00071     ex.setInputCloud(cloud);
00072     ex.setIndices(indices);
00073     ex.filter(*raw_points_);
00074   }
00075 
00076   LineSegment::~LineSegment()
00077   {
00078   }
00079 
00080   jsk_recognition_utils::Line::Ptr LineSegment::toSegment()
00081   {
00082     // we suppose the first and the last point should be the end points
00083     // return Segment::Ptr(
00084     //   new Segment(
00085     //     points_->points[0].getVector3fMap(),
00086     //     points_->points[points_->points.size() - 1].getVector3fMap()));
00087     Eigen::Vector3f direction;
00088     direction[0] = coefficients_->values[3];
00089     direction[1] = coefficients_->values[4];
00090     direction[2] = coefficients_->values[5];
00091     return jsk_recognition_utils::Line::Ptr(new jsk_recognition_utils::Line(direction,
00092                                                                             points_->points[0].getVector3fMap()));
00093     // return Segment::Ptr(
00094     //   new Segment(
00095     //     points_->points[0].getVector3fMap(),
00096     //     points_->points[points_->points.size() - 1].getVector3fMap()));
00097   }
00098   
00099 
00100   bool LineSegment::addMarkerLine(
00101     visualization_msgs::Marker& marker,
00102     const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
00103     const double minimum_line_length)
00104   {
00105     // lookup minimum and max index
00106     int min_index = INT_MAX;
00107     int max_index = - INT_MAX;
00108     for (size_t i = 0; i < indices_->indices.size(); i++) {
00109       int index = indices_->indices[i];
00110       if (min_index > index) {
00111         min_index = index;
00112       }
00113       if (max_index < index) {
00114         max_index = index;
00115       }
00116     }
00117     geometry_msgs::Point a, b;
00118     jsk_recognition_utils::pointFromXYZToXYZ<pcl::PointXYZ, geometry_msgs::Point>(
00119       cloud->points[min_index], a);
00120     jsk_recognition_utils::pointFromXYZToXYZ<pcl::PointXYZ, geometry_msgs::Point>(
00121       cloud->points[max_index], b);
00122     if (std::sqrt((a.x - b.x) * (a.x - b.x) +
00123                   (a.y - b.y) * (a.y - b.y) +
00124                   (a.z - b.z) * (a.z - b.z)) < minimum_line_length) {
00125       return false;
00126     }
00127     marker.points.push_back(a);
00128     marker.points.push_back(b);
00129     return true;
00130   }
00131   
00132   void LineSegmentDetector::onInit()
00133   {
00134     DiagnosticNodelet::onInit();
00135 
00136     pnh_->param("approximate_sync", approximate_sync_, false);
00137 
00138     srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (config_mutex_, *pnh_);
00139     dynamic_reconfigure::Server<Config>::CallbackType f =
00140       boost::bind (&LineSegmentDetector::configCallback, this, _1, _2);
00141     srv_->setCallback (f);
00142 
00144     // setup publishers
00146     pub_line_marker_ = advertise<visualization_msgs::Marker>(
00147       *pnh_, "debug/line_marker", 1);
00148     pub_indices_ = advertise<jsk_recognition_msgs::ClusterPointIndices>(
00149       *pnh_, "output/inliers", 1);
00150     pub_coefficients_ = advertise<jsk_recognition_msgs::ModelCoefficientsArray>(
00151       *pnh_, "output/coefficients", 1);
00152 
00153     onInitPostProcess();
00154   }
00155 
00156   void LineSegmentDetector::configCallback(Config &config, uint32_t level)
00157   {
00158     boost::mutex::scoped_lock lock(mutex_);
00159     outlier_threshold_ = config.outlier_threshold;
00160     max_iterations_ = config.max_iterations;
00161     min_indices_ = config.min_indices;
00162     min_length_ = config.min_length;
00163     line_width_ = config.line_width;
00164 
00165 
00166     // update segmentation parameters
00167     seg_.setOptimizeCoefficients (true);
00168     seg_.setModelType(pcl::SACMODEL_LINE);
00169     int segmentation_method;
00170     {
00171       boost::lock_guard<boost::recursive_mutex> lock(config_mutex_);
00172       segmentation_method = config.method_type;
00173     }
00174     seg_.setMethodType(segmentation_method);
00175     seg_.setDistanceThreshold (outlier_threshold_);
00176     seg_.setMaxIterations (max_iterations_);
00177   }
00178   
00179   void LineSegmentDetector::subscribe()
00180   {
00181     sub_input_.subscribe(*pnh_, "input", 1);
00182     sub_indices_.subscribe(*pnh_, "input_indices", 1);
00183     if (approximate_sync_) {
00184       async_ = boost::make_shared<message_filters::Synchronizer<ApproximateSyncPolicy> >(100);
00185       async_->connectInput(sub_input_, sub_indices_);
00186       async_->registerCallback(boost::bind(&LineSegmentDetector::segment,
00187                                            this, _1, _2));
00188     }
00189     else {
00190       sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
00191       sync_->connectInput(sub_input_, sub_indices_);
00192       sync_->registerCallback(boost::bind(&LineSegmentDetector::segment,
00193                                           this, _1, _2));
00194     }
00195   }
00196 
00197   void LineSegmentDetector::unsubscribe()
00198   {
00199     sub_input_.unsubscribe();
00200     sub_indices_.unsubscribe();
00201   }
00202 
00203   void LineSegmentDetector::updateDiagnostic(
00204     diagnostic_updater::DiagnosticStatusWrapper &stat)
00205   {
00206   }
00207 
00208   void LineSegmentDetector::publishResult(
00209     const std_msgs::Header& header,
00210     const pcl::PointCloud<PointT>::Ptr& cloud,
00211     const std::vector<LineSegment::Ptr>& segments)
00212   {
00213     std::vector<pcl::PointIndices::Ptr> indices;
00214     std::vector<pcl::ModelCoefficients::Ptr> coefficients;
00215     visualization_msgs::Marker marker;
00216     marker.header = header;
00217     marker.pose.orientation.w = 1.0;
00218     marker.scale.x = line_width_;
00219     marker.type = visualization_msgs::Marker::LINE_LIST;
00220     marker.color.a = 1.0;
00221     marker.color.r = 1.0;
00222     for (size_t i = 0; i < segments.size(); i++) {
00223       if (segments[i]->addMarkerLine(marker, cloud, min_length_) == false)
00224         continue;
00225       indices.push_back(segments[i]->getIndices());
00226       coefficients.push_back(segments[i]->getCoefficients());
00227       std_msgs::ColorRGBA color = jsk_topic_tools::colorCategory20(i);
00228       color.a = 1.0;
00229       marker.colors.push_back(color);
00230     }
00231 
00232     jsk_recognition_msgs::ModelCoefficientsArray ros_coefficients;
00233     jsk_recognition_msgs::ClusterPointIndices ros_indices;
00234     ros_coefficients.header = header;
00235     ros_indices.header = header;
00236     ros_coefficients.coefficients
00237       = pcl_conversions::convertToROSModelCoefficients(
00238         coefficients, header);
00239     ros_indices.cluster_indices
00240       = pcl_conversions::convertToROSPointIndices(
00241         indices, header);
00242     pub_indices_.publish(ros_indices);
00243     pub_coefficients_.publish(ros_coefficients);
00244     pub_line_marker_.publish(marker);
00245   }
00246   
00247   void LineSegmentDetector::segmentLines(
00248     const pcl::PointCloud<PointT>::Ptr& cloud,
00249     const pcl::PointIndices::Ptr& indices,
00250     std::vector<pcl::PointIndices::Ptr>& line_indices,
00251     std::vector<pcl::ModelCoefficients::Ptr>& line_coefficients)
00252   {
00253     boost::mutex::scoped_lock lock(mutex_);
00254     pcl::PointIndices::Ptr rest_indices (new pcl::PointIndices);
00255     rest_indices->indices = indices->indices;
00256     // use RANSAC to segment lines
00257     seg_.setInputCloud(cloud);
00258     while (true) {
00259       if (rest_indices->indices.size() > min_indices_) {
00260         pcl::PointIndices::Ptr
00261           result_indices (new pcl::PointIndices);
00262         pcl::ModelCoefficients::Ptr
00263           result_coefficients (new pcl::ModelCoefficients);
00264         seg_.setIndices(rest_indices);
00265         seg_.segment(*result_indices, *result_coefficients);
00266         if (result_indices->indices.size() > min_indices_) {
00267           line_indices.push_back(result_indices);
00268           line_coefficients.push_back(result_coefficients);
00269           rest_indices = jsk_recognition_utils::subIndices(*rest_indices, *result_indices);
00270         }
00271         else {
00272           break;
00273         }
00274       }
00275       else {
00276         break;
00277       }
00278     }
00279     
00280   }
00281   
00282   void LineSegmentDetector::segment(
00283     const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
00284     const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& cluster_msg)
00285   {
00286     pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
00287     pcl::fromROSMsg(*cloud_msg, *cloud);
00288     std::vector<LineSegment::Ptr> segments;
00289     std::vector<pcl::PointIndices::Ptr> input_indices
00290       = pcl_conversions::convertToPCLPointIndices(cluster_msg->cluster_indices);
00291     // for each cluster
00292     for (size_t i = 0; i < cluster_msg->cluster_indices.size(); i++) {
00293       std::vector<pcl::PointIndices::Ptr> line_indices;
00294       std::vector<pcl::ModelCoefficients::Ptr> line_coefficients;
00295       segmentLines(cloud, input_indices[i],
00296                    line_indices, line_coefficients);
00297       if (line_indices.size() > 0) {
00298         // update lines
00299         for (size_t j = 0; j < line_indices.size(); j++) {
00300           segments.push_back(
00301             LineSegment::Ptr(new LineSegment(line_indices[j],
00302                                              line_coefficients[j])));
00303         }
00304       }
00305     }
00306     // publish result
00307     publishResult(cloud_msg->header, cloud, segments);
00308   }
00309   
00310 }
00311 
00312 #include <pluginlib/class_list_macros.h>
00313 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::LineSegmentDetector,
00314                         nodelet::Nodelet);


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