multi_plane_sac_segmentation_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/multi_plane_sac_segmentation.h"
00037 #include <pcl/sample_consensus/method_types.h>
00038 #include <pcl/sample_consensus/model_types.h>
00039 #include <pcl/segmentation/sac_segmentation.h>
00040 #include <pcl/filters/extract_indices.h>
00041 #include "jsk_recognition_utils/pcl_util.h"
00042 #include <eigen_conversions/eigen_msg.h>
00043 
00044 namespace jsk_pcl_ros
00045 {
00046   void MultiPlaneSACSegmentation::onInit()
00047   {
00048     ConnectionBasedNodelet::onInit();
00049     srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00050     dynamic_reconfigure::Server<Config>::CallbackType f =
00051       boost::bind (&MultiPlaneSACSegmentation::configCallback, this, _1, _2);
00052     srv_->setCallback (f);
00053     pnh_->param("use_normal", use_normal_, false);
00054     pnh_->param("use_clusters", use_clusters_, false);
00055     pnh_->param("use_imu_parallel", use_imu_parallel_, false);
00056     pnh_->param("use_imu_perpendicular", use_imu_perpendicular_, false);
00057     
00058     if (use_imu_perpendicular_ && use_imu_parallel_) {
00059       NODELET_ERROR("Cannot use ~use_imu_perpendicular and ~use_imu_parallel at the same time");
00060       return;
00061     }
00062     if (use_imu_perpendicular_ || use_imu_parallel_) {
00063       tf_listener_ = TfListenerSingleton::getInstance();
00064     }
00066     // publishers
00068     pub_inliers_ = advertise<jsk_recognition_msgs::ClusterPointIndices>(*pnh_, "output_indices", 1);
00069     pub_coefficients_
00070       = advertise<jsk_recognition_msgs::ModelCoefficientsArray>(*pnh_, "output_coefficients", 1);
00071     pub_polygons_ = advertise<jsk_recognition_msgs::PolygonArray>(*pnh_, "output_polygons", 1);
00072     onInitPostProcess();
00073   }
00074 
00075   void MultiPlaneSACSegmentation::subscribe()
00076   {
00078     // subscriber
00080     if (use_imu_perpendicular_ || use_imu_parallel_) {
00081       sub_input_.subscribe(*pnh_, "input", 1);
00082       sub_imu_.subscribe(*pnh_, "input_imu", 1);
00083       if (use_normal_) {
00084         sub_normal_.subscribe(*pnh_, "input_normal", 1);
00085         sync_normal_imu_ = boost::make_shared<NormalImuSynchronizer>(100);
00086         sync_normal_imu_->connectInput(sub_input_, sub_normal_, sub_imu_);
00087         sync_normal_imu_->registerCallback(
00088           boost::bind(
00089             &MultiPlaneSACSegmentation::segmentWithImu,
00090             this, _1, _2, _3));
00091       }
00092       else {
00093         sync_imu_ = boost::make_shared<message_filters::Synchronizer<SyncImuPolicy> >(100);
00094         sync_imu_->connectInput(sub_input_, sub_imu_);
00095         sync_imu_->registerCallback(boost::bind(
00096                                       &MultiPlaneSACSegmentation::segmentWithImu,
00097                                       this, _1, _2));
00098       }
00099     }
00100     else if (use_normal_) {
00101       sub_input_.subscribe(*pnh_, "input", 1);
00102       sub_normal_.subscribe(*pnh_, "input_normal", 1);
00103       sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
00104       sync_->connectInput(sub_input_, sub_normal_);
00105       sync_->registerCallback(boost::bind(&MultiPlaneSACSegmentation::segment,
00106                                           this, _1, _2));
00107     }
00108     else if (use_clusters_) {
00109       NODELET_INFO("use clusters");
00110       sub_input_.subscribe(*pnh_, "input", 1);
00111       sub_clusters_.subscribe(*pnh_, "input_clusters", 1);
00112       sync_cluster_
00113         = boost::make_shared<message_filters::Synchronizer<SyncClusterPolicy> >(100);
00114       sync_cluster_->connectInput(sub_input_, sub_clusters_);
00115       sync_cluster_->registerCallback(
00116         boost::bind(&MultiPlaneSACSegmentation::segmentWithClusters,
00117                     this, _1, _2));
00118     }
00119     else {
00120       sub_ = pnh_->subscribe("input", 1, &MultiPlaneSACSegmentation::segment, this);
00121     }
00122   }
00123 
00124   void MultiPlaneSACSegmentation::unsubscribe()
00125   {
00127     // subscriber
00129     if (use_normal_) {
00130       sub_input_.unsubscribe();
00131       sub_normal_.unsubscribe();
00132     }
00133     else if (use_clusters_) {
00134       sub_input_.unsubscribe();
00135       sub_clusters_.unsubscribe();
00136     }
00137     else {
00138       sub_.shutdown();
00139     }
00140   }
00141 
00142   void MultiPlaneSACSegmentation::configCallback (Config &config, uint32_t level)
00143   {
00144     boost::mutex::scoped_lock lock(mutex_);
00145     outlier_threshold_ = config.outlier_threshold;
00146     min_inliers_ = config.min_inliers;
00147     min_points_ = config.min_points;
00148     max_iterations_ = config.max_iterations;
00149     eps_angle_ = config.eps_angle;
00150     normal_distance_weight_ = config.normal_distance_weight;
00151     min_trial_ = config.min_trial;
00152   }
00153   
00154   void MultiPlaneSACSegmentation::applyRecursiveRANSAC(
00155       const pcl::PointCloud<PointT>::Ptr& input,
00156       const pcl::PointCloud<pcl::Normal>::Ptr& normal,
00157       const Eigen::Vector3f& imu_vector,
00158       std::vector<pcl::PointIndices::Ptr>& output_inliers,
00159       std::vector<pcl::ModelCoefficients::Ptr>& output_coefficients,
00160       std::vector<jsk_recognition_utils::ConvexPolygon::Ptr>& output_polygons)
00161   {
00162     pcl::PointCloud<PointT>::Ptr rest_cloud (new pcl::PointCloud<PointT>);
00163     pcl::PointCloud<pcl::Normal>::Ptr rest_normal (new pcl::PointCloud<pcl::Normal>);
00164     *rest_cloud = *input;
00165     *rest_normal = *normal;
00166     int counter = 0;
00167     while (true) {
00168       NODELET_INFO("apply RANSAC: %d", counter);
00169       ++counter;
00170       pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
00171       pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
00172       if (!use_normal_) {
00173         pcl::SACSegmentation<PointT> seg;
00174         seg.setOptimizeCoefficients (true);
00175         seg.setRadiusLimits(0.3, std::numeric_limits<double>::max ());
00176         seg.setMethodType (pcl::SAC_RANSAC);
00177         seg.setDistanceThreshold (outlier_threshold_);
00178         seg.setInputCloud(rest_cloud);
00179         seg.setMaxIterations (max_iterations_);
00180         if (use_imu_parallel_) {
00181           seg.setModelType(pcl::SACMODEL_PARALLEL_PLANE);
00182           seg.setAxis(imu_vector);
00183           seg.setEpsAngle(eps_angle_);
00184         }
00185         else if (use_imu_perpendicular_) {
00186           seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
00187           seg.setAxis(imu_vector);
00188           seg.setEpsAngle(eps_angle_);
00189         }
00190         else {
00191           seg.setModelType (pcl::SACMODEL_PLANE);
00192         }
00193         seg.segment (*inliers, *coefficients);
00194       }
00195       else {
00196         pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg;
00197         seg.setOptimizeCoefficients (true);
00198         
00199         seg.setMethodType (pcl::SAC_RANSAC);
00200         seg.setDistanceThreshold (outlier_threshold_);
00201         seg.setNormalDistanceWeight(normal_distance_weight_);
00202         seg.setInputCloud(rest_cloud);
00203         seg.setInputNormals(rest_normal);
00204         seg.setMaxIterations (max_iterations_);
00205         if (use_imu_parallel_) {
00206           seg.setModelType (pcl::SACMODEL_NORMAL_PARALLEL_PLANE);
00207           seg.setAxis(imu_vector);
00208           seg.setEpsAngle(eps_angle_);
00209         }
00210         else if (use_imu_perpendicular_) {
00211           seg.setModelType (pcl::SACMODEL_NORMAL_PLANE);
00212           seg.setAxis(imu_vector);
00213           seg.setEpsAngle(eps_angle_);
00214         }
00215         else {
00216           seg.setModelType (pcl::SACMODEL_NORMAL_PLANE);  
00217         }
00218         seg.segment (*inliers, *coefficients);
00219       }
00220       NODELET_INFO("inliers: %lu", inliers->indices.size());
00221       if (inliers->indices.size() >= min_inliers_) {
00222         output_inliers.push_back(inliers);
00223         output_coefficients.push_back(coefficients);
00224         jsk_recognition_utils::ConvexPolygon::Ptr convex = jsk_recognition_utils::convexFromCoefficientsAndInliers<PointT>(
00225           input, inliers, coefficients);
00226         output_polygons.push_back(convex);
00227       }
00228       else {
00229         if (min_trial_ <= counter) {
00230           return;
00231         }
00232       }
00233       
00234       // prepare for next loop
00235       pcl::PointCloud<PointT>::Ptr next_rest_cloud
00236         (new pcl::PointCloud<PointT>);
00237       pcl::PointCloud<pcl::Normal>::Ptr next_rest_normal
00238         (new pcl::PointCloud<pcl::Normal>);
00239       pcl::ExtractIndices<PointT> ex;
00240       ex.setInputCloud(rest_cloud);
00241       ex.setIndices(inliers);
00242       ex.setNegative(true);
00243       ex.setKeepOrganized(true);
00244       ex.filter(*next_rest_cloud);
00245       if (use_normal_) {
00246         pcl::ExtractIndices<pcl::Normal> ex_normal;
00247         ex_normal.setInputCloud(rest_normal);
00248         ex_normal.setIndices(inliers);
00249         ex_normal.setNegative(true);
00250         ex_normal.setKeepOrganized(true);
00251         ex_normal.filter(*next_rest_normal);
00252       }
00253       if (next_rest_cloud->points.size() < min_points_) {
00254         NODELET_INFO("no more enough points are left");
00255         return;
00256       }
00257       rest_cloud = next_rest_cloud;
00258       rest_normal = next_rest_normal;
00259     }
00260   }
00261 
00262   void MultiPlaneSACSegmentation::segmentWithClusters(
00263     const sensor_msgs::PointCloud2::ConstPtr& msg,
00264     const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& clusters)
00265   {
00266     boost::mutex::scoped_lock lock(mutex_);
00267     pcl::PointCloud<PointT>::Ptr input (new pcl::PointCloud<PointT>);
00268     pcl::fromROSMsg(*msg, *input);
00269     std::vector<pcl::PointIndices::Ptr> cluster_indices
00270       = pcl_conversions::convertToPCLPointIndices(clusters->cluster_indices);
00271     pcl::ExtractIndices<PointT> ex;
00272     ex.setInputCloud(input);
00273     ex.setKeepOrganized(true);
00274     std::vector<pcl::PointIndices::Ptr> all_inliers;
00275     std::vector<pcl::ModelCoefficients::Ptr> all_coefficients;
00276     std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> all_convexes;
00277     for (size_t i = 0; i < cluster_indices.size(); i++) {
00278       pcl::PointIndices::Ptr indices = cluster_indices[i];
00279       pcl::PointCloud<PointT>::Ptr cluster_cloud (new pcl::PointCloud<PointT>);
00280       pcl::PointCloud<pcl::Normal>::Ptr normal (new pcl::PointCloud<pcl::Normal>);
00281       ex.setIndices(indices);
00282       
00283       ex.filter(*cluster_cloud);
00284       std::vector<pcl::PointIndices::Ptr> inliers;
00285       std::vector<pcl::ModelCoefficients::Ptr> coefficients;
00286       std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> convexes;
00287       Eigen::Vector3f dummy_imu_vector;
00288       applyRecursiveRANSAC(cluster_cloud, normal, dummy_imu_vector, inliers, coefficients, convexes);
00289       appendVector(all_inliers, inliers);
00290       appendVector(all_coefficients, coefficients);
00291       appendVector(all_convexes, convexes);
00292     }
00293     publishResult(msg->header, all_inliers, all_coefficients, all_convexes);
00294   }
00295 
00296   void MultiPlaneSACSegmentation::publishResult(
00297     const std_msgs::Header& header,
00298     const std::vector<pcl::PointIndices::Ptr>& inliers,
00299     const std::vector<pcl::ModelCoefficients::Ptr>& coefficients,
00300     const std::vector<jsk_recognition_utils::ConvexPolygon::Ptr>& convexes)
00301   {
00302     jsk_recognition_msgs::ClusterPointIndices ros_indices_output;
00303     jsk_recognition_msgs::ModelCoefficientsArray ros_coefficients_output;
00304     jsk_recognition_msgs::PolygonArray ros_polygon_output;
00305     ros_indices_output.header = header;
00306     ros_coefficients_output.header = header;
00307     ros_polygon_output.header = header;
00308     ros_indices_output.cluster_indices
00309       = pcl_conversions::convertToROSPointIndices(inliers, header);
00310     ros_coefficients_output.coefficients
00311       = pcl_conversions::convertToROSModelCoefficients(coefficients, header);
00312     pub_inliers_.publish(ros_indices_output);
00313     pub_coefficients_.publish(ros_coefficients_output);
00314     for (size_t i = 0; i < convexes.size(); i++) {
00315       geometry_msgs::PolygonStamped polygon;
00316       polygon.header = header;
00317       polygon.polygon = convexes[i]->toROSMsg();
00318       ros_polygon_output.polygons.push_back(polygon);
00319     }
00320     pub_polygons_.publish(ros_polygon_output);
00321   }
00322 
00323   void MultiPlaneSACSegmentation::segmentWithImu(
00324     const sensor_msgs::PointCloud2::ConstPtr& msg,
00325     const sensor_msgs::Imu::ConstPtr& imu_msg)
00326   {
00327     segmentWithImu(msg, sensor_msgs::PointCloud2::ConstPtr(), imu_msg);
00328   }
00329   
00330   void MultiPlaneSACSegmentation::segmentWithImu(
00331     const sensor_msgs::PointCloud2::ConstPtr& msg,
00332     const sensor_msgs::PointCloud2::ConstPtr& normal_msg,
00333     const sensor_msgs::Imu::ConstPtr& imu_msg)
00334   {
00335     boost::mutex::scoped_lock lock(mutex_);
00336     pcl::PointCloud<PointT>::Ptr input (new pcl::PointCloud<PointT>);
00337     pcl::PointCloud<pcl::Normal>::Ptr
00338       normal (new pcl::PointCloud<pcl::Normal>);
00339     pcl::fromROSMsg(*msg, *input);
00340     if (normal_msg) {
00341       pcl::fromROSMsg(*normal_msg, *normal);
00342     }
00343     geometry_msgs::Vector3Stamped stamped_imu, transformed_stamped_imu;
00344     stamped_imu.header = imu_msg->header;
00345     stamped_imu.vector = imu_msg->linear_acceleration;
00346     try {
00347       tf_listener_->waitForTransform(
00348         msg->header.frame_id, imu_msg->header.frame_id, imu_msg->header.stamp,
00349         ros::Duration(0.1));
00350       tf_listener_->transformVector(
00351         msg->header.frame_id, stamped_imu, transformed_stamped_imu); 
00352       Eigen::Vector3d imu_vectord;
00353       Eigen::Vector3f imu_vector;
00354       tf::vectorMsgToEigen(transformed_stamped_imu.vector, imu_vectord);
00355       jsk_recognition_utils::pointFromVectorToVector<Eigen::Vector3d, Eigen::Vector3f>(
00356         imu_vectord, imu_vector);
00357       imu_vector = - imu_vector;
00358       
00359       std::vector<pcl::PointIndices::Ptr> inliers;
00360       std::vector<pcl::ModelCoefficients::Ptr> coefficients;
00361       std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> convexes;
00362       applyRecursiveRANSAC(input, normal, imu_vector,
00363                            inliers, coefficients, convexes);
00364       publishResult(msg->header, inliers, coefficients, convexes);                     
00365     }
00366     catch (tf2::ConnectivityException &e) {
00367       NODELET_ERROR("[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what());
00368     }
00369     catch (tf2::InvalidArgumentException &e) {
00370       NODELET_ERROR("[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what());
00371     }
00372     catch (tf2::ExtrapolationException& e) {
00373       NODELET_ERROR("[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what());
00374     }
00375     
00376   }
00377   
00378   void MultiPlaneSACSegmentation::segment(
00379     const sensor_msgs::PointCloud2::ConstPtr& msg,
00380     const sensor_msgs::PointCloud2::ConstPtr& msg_normal)
00381   {
00382     boost::mutex::scoped_lock lock(mutex_);
00383     pcl::PointCloud<PointT>::Ptr input (new pcl::PointCloud<PointT>);
00384     pcl::PointCloud<pcl::Normal>::Ptr normal (new pcl::PointCloud<pcl::Normal>);
00385     pcl::fromROSMsg(*msg, *input);
00386     if (use_normal_) {
00387       pcl::fromROSMsg(*msg_normal, *normal);
00388     }
00389     std::vector<pcl::PointIndices::Ptr> inliers;
00390     std::vector<pcl::ModelCoefficients::Ptr> coefficients;
00391     std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> convexes;
00392     Eigen::Vector3f dummy_imu_vector;
00393     applyRecursiveRANSAC(input, normal, dummy_imu_vector,
00394                          inliers, coefficients, convexes);
00395     publishResult(msg->header, inliers, coefficients, convexes); 
00396   }
00397   
00398   void MultiPlaneSACSegmentation::segment(
00399     const sensor_msgs::PointCloud2::ConstPtr& msg)
00400   {
00401     segment(msg, sensor_msgs::PointCloud2::ConstPtr());
00402   }
00403 }
00404 
00405 #include <pluginlib/class_list_macros.h>
00406 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::MultiPlaneSACSegmentation, nodelet::Nodelet);


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Tue Jul 2 2019 19:41:43