primitive_shape_classifier_nodelet.cpp
Go to the documentation of this file.
00001 // -*- mode: C++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2017, 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  * primitive_shape_classifier_nodelet.cpp
00037  * Author: Yuki Furuta <furushchev@jsk.imi.i.u-tokyo.ac.jp>
00038  */
00039 
00040 #include <jsk_pcl_ros/primitive_shape_classifier.h>
00041 #include <algorithm>
00042 #include <iterator>
00043 #include <pcl/common/centroid.h>
00044 #include <pcl/features/boundary.h>
00045 #include <pcl/filters/extract_indices.h>
00046 #include <pcl/filters/project_inliers.h>
00047 #include <pcl/sample_consensus/method_types.h>
00048 #include <pcl/sample_consensus/model_types.h>
00049 #include <pcl/segmentation/sac_segmentation.h>
00050 #include <pcl_conversions/pcl_conversions.h>
00051 
00052 namespace jsk_pcl_ros
00053 {
00054   void PrimitiveShapeClassifier::onInit()
00055   {
00056     DiagnosticNodelet::onInit();
00057 
00058     srv_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);
00059     dynamic_reconfigure::Server<Config>::CallbackType f =
00060       boost::bind(&PrimitiveShapeClassifier::configCallback, this, _1, _2);
00061     srv_->setCallback(f);
00062 
00063     pub_class_ = advertise<jsk_recognition_msgs::ClassificationResult>(*pnh_, "output", 1);
00064     pub_boundary_indices_ =
00065       advertise<jsk_recognition_msgs::ClusterPointIndices>(*pnh_, "debug/boundary_indices", 1);
00066     pub_projected_cloud_ =
00067       advertise<sensor_msgs::PointCloud2>(*pnh_, "debug/projected_cloud", 1);
00068 
00069     onInitPostProcess();
00070   }
00071 
00072   void PrimitiveShapeClassifier::subscribe()
00073   {
00074     sub_cloud_.subscribe(*pnh_, "input", 1);
00075     sub_normal_.subscribe(*pnh_, "input/normal", 1);
00076     sub_indices_.subscribe(*pnh_, "input/indices", 1);
00077     sub_polygons_.subscribe(*pnh_, "input/polygons", 1);
00078 
00079     sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(queue_size_);
00080     sync_->connectInput(sub_cloud_, sub_normal_, sub_indices_, sub_polygons_);
00081     sync_->registerCallback(boost::bind(&PrimitiveShapeClassifier::process, this, _1, _2, _3, _4));
00082   }
00083 
00084   void PrimitiveShapeClassifier::unsubscribe()
00085   {
00086     sub_cloud_.unsubscribe();
00087     sub_normal_.unsubscribe();
00088     sub_indices_.unsubscribe();
00089     sub_polygons_.unsubscribe();
00090   }
00091 
00092   void PrimitiveShapeClassifier::configCallback(Config& config, uint32_t level)
00093   {
00094     boost::mutex::scoped_lock lock(mutex_);
00095     min_points_num_ = config.min_points_num;
00096 
00097     sac_max_iterations_ = config.sac_max_iterations;
00098     sac_distance_threshold_ = config.sac_distance_threshold;
00099     if (config.sac_radius_limit_min < config.sac_radius_limit_max) {
00100       sac_radius_limit_min_ = config.sac_radius_limit_min;
00101       sac_radius_limit_max_ = config.sac_radius_limit_max;
00102     } else {
00103       config.sac_radius_limit_min = sac_radius_limit_min_;
00104       config.sac_radius_limit_max = sac_radius_limit_max_;
00105     }
00106 
00107     box_threshold_ = config.box_threshold;
00108     circle_threshold_ = config.circle_threshold;
00109 
00110     if (queue_size_ = config.queue_size) {
00111       queue_size_ = config.queue_size;
00112       if (isSubscribed()) {
00113         unsubscribe();
00114         subscribe();
00115       }
00116     }
00117   }
00118 
00119   bool
00120   PrimitiveShapeClassifier::checkFrameId(const sensor_msgs::PointCloud2::ConstPtr& ros_cloud,
00121                                          const sensor_msgs::PointCloud2::ConstPtr& ros_normal,
00122                                          const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& ros_indices,
00123                                          const jsk_recognition_msgs::PolygonArray::ConstPtr& ros_polygons)
00124   {
00125     std::string cloud_topic = ros::names::resolve(sub_cloud_.getTopic());
00126     std::string normal_topic = ros::names::resolve(sub_normal_.getTopic());
00127     std::string indices_topic = ros::names::resolve(sub_indices_.getTopic());
00128     std::string polygons_topic = ros::names::resolve(sub_polygons_.getTopic());
00129     if (!jsk_recognition_utils::isSameFrameId(ros_cloud->header, ros_normal->header)) {
00130       NODELET_ERROR_STREAM(cloud_topic << " and " << normal_topic << " must have same frame_id");
00131       return false;
00132     }
00133     if (!jsk_recognition_utils::isSameFrameId(ros_cloud->header, ros_indices->header)) {
00134       NODELET_ERROR_STREAM(cloud_topic << " and " << indices_topic << " must have same frame_id");
00135       return false;
00136     }
00137     if (!jsk_recognition_utils::isSameFrameId(ros_cloud->header, ros_polygons->header)) {
00138       NODELET_ERROR_STREAM(cloud_topic << " and " << polygons_topic << " must have same frame_id");
00139       return false;
00140     }
00141     NODELET_DEBUG_STREAM("Frame id is ok: " << ros_cloud->header.frame_id);
00142     return true;
00143   }
00144 
00145   void
00146   PrimitiveShapeClassifier::process(const sensor_msgs::PointCloud2::ConstPtr& ros_cloud,
00147                                     const sensor_msgs::PointCloud2::ConstPtr& ros_normal,
00148                                     const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& ros_indices,
00149                                     const jsk_recognition_msgs::PolygonArray::ConstPtr& ros_polygons)
00150   {
00151     boost::mutex::scoped_lock lock(mutex_);
00152 
00153     if (!checkFrameId(ros_cloud, ros_normal, ros_indices, ros_polygons)) return;
00154 
00155     pcl::PointCloud<PointT>::Ptr input(new pcl::PointCloud<PointT>);
00156     pcl::fromROSMsg(*ros_cloud, *input);
00157 
00158     pcl::PointCloud<pcl::Normal>::Ptr normal(new pcl::PointCloud<pcl::Normal>);
00159     pcl::fromROSMsg(*ros_normal, *normal);
00160 
00161     pcl::ExtractIndices<PointT> ext_input;
00162     ext_input.setInputCloud(input);
00163     pcl::ExtractIndices<pcl::Normal> ext_normal;
00164     ext_normal.setInputCloud(normal);
00165 
00166     std::vector<jsk_recognition_utils::Polygon::Ptr> polygons
00167       = jsk_recognition_utils::Polygon::fromROSMsg(*ros_polygons);
00168 
00169     jsk_recognition_msgs::ClassificationResult result;
00170     result.header = ros_cloud->header;
00171     result.classifier = "primitive_shape_classifier";
00172     result.target_names.push_back("box");
00173     result.target_names.push_back("circle");
00174     result.target_names.push_back("other");
00175 
00176     pcl::PointCloud<PointT>::Ptr projected_cloud(new pcl::PointCloud<PointT>);
00177     std::vector<pcl::PointIndices::Ptr> boundary_indices;
00178 
00179     NODELET_DEBUG_STREAM("Cluster num: " << ros_indices->cluster_indices.size());
00180     for (size_t i = 0; i < ros_indices->cluster_indices.size(); ++i)
00181     {
00182       pcl::PointIndices::Ptr indices(new pcl::PointIndices);
00183       indices->indices = ros_indices->cluster_indices[i].indices;
00184       NODELET_DEBUG_STREAM("Estimating cluster #" << i << " (" << indices->indices.size() << " points)");
00185 
00186       pcl::PointCloud<PointT>::Ptr cluster_cloud(new pcl::PointCloud<PointT>);
00187       ext_input.setIndices(indices);
00188       ext_input.filter(*cluster_cloud);
00189 
00190       pcl::PointCloud<pcl::Normal>::Ptr cluster_normal(new pcl::PointCloud<pcl::Normal>);
00191       ext_normal.setIndices(indices);
00192       ext_normal.filter(*cluster_normal);
00193 
00194       pcl::ModelCoefficients::Ptr support_plane(new pcl::ModelCoefficients);
00195       if (!getSupportPlane(cluster_cloud, polygons, support_plane))
00196       {
00197         NODELET_ERROR_STREAM("cloud " << i << " has no support plane. skipped");
00198         continue;
00199       }
00200 
00201       pcl::PointIndices::Ptr b(new pcl::PointIndices);
00202       pcl::PointCloud<PointT>::Ptr pc(new pcl::PointCloud<PointT>);
00203       float circle_likelihood, box_likelihood;
00204       estimate(cluster_cloud, cluster_normal, support_plane,
00205                b, pc,
00206                circle_likelihood, box_likelihood);
00207       boundary_indices.push_back(std::move(b));
00208       *projected_cloud += *pc;
00209 
00210       if (circle_likelihood > circle_threshold_) {
00211         // circle
00212         result.labels.push_back(1);
00213         result.label_names.push_back("circle");
00214         result.label_proba.push_back(circle_likelihood);
00215       } else if (box_likelihood > box_threshold_) {
00216         // box
00217         result.labels.push_back(0);
00218         result.label_names.push_back("box");
00219         result.label_proba.push_back(box_likelihood);
00220       } else {
00221         // other
00222         result.labels.push_back(3);
00223         result.label_names.push_back("other");
00224         result.label_proba.push_back(0.0);
00225       }
00226     }
00227 
00228     // publish results
00229     sensor_msgs::PointCloud2 ros_projected_cloud;
00230     pcl::toROSMsg(*projected_cloud, ros_projected_cloud);
00231     ros_projected_cloud.header = ros_cloud->header;
00232     pub_projected_cloud_.publish(ros_projected_cloud);
00233 
00234     jsk_recognition_msgs::ClusterPointIndices ros_boundary_indices;
00235     ros_boundary_indices.header = ros_cloud->header;
00236     for (size_t i = 0; i < boundary_indices.size(); ++i)
00237     {
00238       pcl_msgs::PointIndices ri;
00239       pcl_conversions::moveFromPCL(*boundary_indices[i], ri);
00240       ros_boundary_indices.cluster_indices.push_back(ri);
00241     }
00242     pub_boundary_indices_.publish(ros_boundary_indices);
00243 
00244     pub_class_.publish(result);
00245   }
00246 
00247   bool
00248   PrimitiveShapeClassifier::estimate(const pcl::PointCloud<PointT>::Ptr& cloud,
00249                                      const pcl::PointCloud<pcl::Normal>::Ptr& normal,
00250                                      const pcl::ModelCoefficients::Ptr& plane,
00251                                      pcl::PointIndices::Ptr& boundary_indices,
00252                                      pcl::PointCloud<PointT>::Ptr& projected_cloud,
00253                                      float& circle_likelihood,
00254                                      float& box_likelihood)
00255   {
00256     // estimate boundaries
00257     pcl::PointCloud<pcl::Boundary>::Ptr boundaries(new pcl::PointCloud<pcl::Boundary>);
00258     pcl::BoundaryEstimation<PointT, pcl::Normal, pcl::Boundary> b;
00259     b.setInputCloud(cloud);
00260     b.setInputNormals(normal);
00261     b.setSearchMethod(typename pcl::search::KdTree<PointT>::Ptr(new pcl::search::KdTree<PointT>));
00262     b.setAngleThreshold(DEG2RAD(70));
00263     b.setRadiusSearch(0.03);
00264     b.compute(*boundaries);
00265 
00266     // set boundaries as indices
00267     for (size_t i = 0; i < boundaries->points.size(); ++i)
00268       if ((int)boundaries->points[i].boundary_point)
00269         boundary_indices->indices.push_back(i);
00270 
00271     // extract boundaries
00272     pcl::PointCloud<PointT>::Ptr boundary_cloud(new pcl::PointCloud<PointT>);
00273     pcl::ExtractIndices<PointT> ext;
00274     ext.setInputCloud(cloud);
00275     ext.setIndices(boundary_indices);
00276     ext.filter(*boundary_cloud);
00277 
00278     // thresholding with min points num
00279     if (boundary_indices->indices.size() < min_points_num_)
00280       return false;
00281 
00282     // project to supporting plane
00283     pcl::ProjectInliers<PointT> proj;
00284     proj.setModelType(pcl::SACMODEL_PLANE);
00285     proj.setInputCloud(boundary_cloud);
00286     proj.setModelCoefficients(plane);
00287     proj.filter(*projected_cloud);
00288 
00289     // estimate circles
00290     pcl::PointIndices::Ptr      circle_inliers(new pcl::PointIndices);
00291     pcl::ModelCoefficients::Ptr circle_coeffs(new pcl::ModelCoefficients);
00292     pcl::PointIndices::Ptr      line_inliers(new pcl::PointIndices);
00293     pcl::ModelCoefficients::Ptr line_coeffs(new pcl::ModelCoefficients);
00294 
00295     pcl::SACSegmentation<PointT> seg;
00296     seg.setInputCloud(projected_cloud);
00297 
00298     seg.setOptimizeCoefficients(true);
00299     seg.setMethodType(pcl::SAC_RANSAC);
00300     seg.setMaxIterations(sac_max_iterations_);
00301     seg.setModelType(pcl::SACMODEL_CIRCLE3D);
00302     seg.setDistanceThreshold(sac_distance_threshold_);
00303     seg.setRadiusLimits(sac_radius_limit_min_, sac_radius_limit_max_);
00304     seg.segment(*circle_inliers, *circle_coeffs);
00305 
00306     seg.setOptimizeCoefficients(true);
00307     seg.setMethodType(pcl::SAC_RANSAC);
00308     seg.setMaxIterations(sac_max_iterations_);
00309     seg.setModelType(pcl::SACMODEL_LINE);
00310     seg.setDistanceThreshold(sac_distance_threshold_);
00311     seg.segment(*line_inliers, *line_coeffs);
00312 
00313     // compute likelihood
00314     circle_likelihood =
00315       1.0f * circle_inliers->indices.size() / projected_cloud->points.size();
00316     box_likelihood =
00317       1.0f * line_inliers->indices.size() / projected_cloud->points.size();
00318 
00319     NODELET_DEBUG_STREAM("Projected cloud has " << projected_cloud->points.size() << " points");
00320     NODELET_DEBUG_STREAM(circle_inliers->indices.size() << " circle inliers found");
00321     NODELET_DEBUG_STREAM("circle confidence: " << circle_likelihood);
00322     NODELET_DEBUG_STREAM(line_inliers->indices.size() << " line inliers found");
00323     NODELET_DEBUG_STREAM("box confidence: " << box_likelihood);
00324 
00325     return true;
00326   }
00327 
00328   bool
00329   PrimitiveShapeClassifier::getSupportPlane(const pcl::PointCloud<PointT>::Ptr& cloud,
00330                                             const std::vector<jsk_recognition_utils::Polygon::Ptr>& polygons,
00331                                             pcl::ModelCoefficients::Ptr& coeff)
00332   {
00333     Eigen::Vector4f c;
00334     pcl::compute3DCentroid(*cloud, c);
00335     Eigen::Vector3f centroid(c[0], c[1], c[2]);
00336     Eigen::Vector3f projected;
00337     for (size_t i = 0; i < polygons.size(); ++i)
00338     {
00339       jsk_recognition_utils::Polygon::Ptr p = polygons[i];
00340       p->project(centroid, projected);
00341       if (p->isInside(projected)) {
00342         p->toCoefficients(coeff->values);
00343         return true;
00344       }
00345     }
00346     return false;
00347   }
00348 }
00349 
00350 #include <pluginlib/class_list_macros.h>
00351 PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::PrimitiveShapeClassifier, nodelet::Nodelet);


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