hinted_plane_detector_nodelet.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, JSK Lab
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/o2r other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the JSK Lab nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 #define BOOST_PARAMETER_MAX_ARITY 7
00035 #include "jsk_pcl_ros/hinted_plane_detector.h"
00036 #include "pcl_ros/transforms.h"
00037 #include <visualization_msgs/Marker.h>
00038 #include <pcl/surface/concave_hull.h>
00039 #include <pcl/sample_consensus/method_types.h>
00040 #include <pcl/sample_consensus/model_types.h>
00041 #include <pcl/segmentation/sac_segmentation.h>
00042 #include <pcl/filters/project_inliers.h>
00043 #include <pcl/filters/extract_indices.h>
00044 #include <pcl/ModelCoefficients.h>
00045 #include <pcl/kdtree/kdtree.h>
00046 #include <pcl/segmentation/extract_clusters.h>
00047 #include <pcl/common/centroid.h>
00048 #include <pluginlib/class_list_macros.h>
00049 
00050 namespace jsk_pcl_ros {
00051   
00052   void HintedPlaneDetector::onInit() {
00053     pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
00054     DiagnosticNodelet::onInit();
00055     srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00056     typename dynamic_reconfigure::Server<Config>::CallbackType f =
00057       boost::bind (&HintedPlaneDetector::configCallback, this, _1, _2);
00058     srv_->setCallback (f);
00059     
00060     pub_hint_polygon_ = advertise<geometry_msgs::PolygonStamped>(
00061       *pnh_, "output/hint/polygon", 1);
00062     pub_hint_polygon_array_ = advertise<jsk_recognition_msgs::PolygonArray>(
00063       *pnh_, "output/hint/polygon_array", 1);
00064     pub_hint_inliers_ = advertise<PCLIndicesMsg>(
00065       *pnh_, "output/hint/inliers", 1);
00066     pub_hint_coefficients_ = advertise<PCLModelCoefficientMsg>(
00067       *pnh_, "output/hint/coefficients", 1);
00068     pub_polygon_ = advertise<geometry_msgs::PolygonStamped>(
00069       *pnh_, "output/polygon", 1);
00070     pub_polygon_array_ = advertise<jsk_recognition_msgs::PolygonArray>(
00071       *pnh_, "output/polygon_array", 1);
00072     pub_hint_filtered_indices_ = advertise<PCLIndicesMsg>(
00073       *pnh_, "output/hint_filtered_indices", 1);
00074     pub_plane_filtered_indices_ = advertise<PCLIndicesMsg>(
00075       *pnh_, "output/plane_filtered_indices", 1);
00076     pub_density_filtered_indices_ = advertise<PCLIndicesMsg>(
00077       *pnh_, "output/density_filtered_indices", 1);
00078     pub_euclidean_filtered_indices_ = advertise<PCLIndicesMsg>(
00079       *pnh_, "output/euclidean_filtered_indices", 1);
00080     pub_inliers_ = advertise<PCLIndicesMsg>(
00081       *pnh_, "output/inliers", 1);
00082     pub_coefficients_ = advertise<PCLModelCoefficientMsg>(
00083       *pnh_, "output/coefficients", 1);
00084     onInitPostProcess();
00085   }
00086 
00087   void HintedPlaneDetector::subscribe()
00088   {
00089     sub_cloud_.subscribe(*pnh_, "input", 1);
00090     sub_hint_cloud_.subscribe(*pnh_, "input/hint/cloud", 1);
00091     sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
00092     sync_->connectInput(sub_cloud_, sub_hint_cloud_);
00093     sync_->registerCallback(boost::bind(&HintedPlaneDetector::detect,
00094                                         this, _1, _2));
00095   }
00096 
00097   void HintedPlaneDetector::unsubscribe()
00098   {
00099     sub_cloud_.unsubscribe();
00100     sub_hint_cloud_.unsubscribe();
00101   }
00102 
00103   void HintedPlaneDetector::configCallback(
00104     Config &config, uint32_t level)
00105   {
00106     boost::mutex::scoped_lock lock(mutex_);
00107     hint_outlier_threashold_ = config.hint_outlier_threashold;
00108     hint_max_iteration_ = config.hint_max_iteration;
00109     hint_min_size_ = config.hint_min_size;
00110     outlier_threashold_ = config.outlier_threashold;
00111     max_iteration_ = config.max_iteration;
00112     min_size_ = config.min_size;
00113     eps_angle_ = config.eps_angle;
00114     normal_filter_eps_angle_ = config.normal_filter_eps_angle;
00115     euclidean_clustering_filter_tolerance_ = config.euclidean_clustering_filter_tolerance;
00116     euclidean_clustering_filter_min_size_ = config.euclidean_clustering_filter_min_size;
00117     density_radius_ = config.density_radius;
00118     density_num_ = config.density_num;
00119     enable_euclidean_filtering_ = config.enable_euclidean_filtering;
00120     enable_normal_filtering_ = config.enable_normal_filtering;
00121     enable_distance_filtering_ = config.enable_distance_filtering;
00122     enable_density_filtering_ = config.enable_density_filtering;
00123   }
00124   
00125   void HintedPlaneDetector::detect(
00126     const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
00127     const sensor_msgs::PointCloud2::ConstPtr& hint_cloud_msg)
00128   {
00129     vital_checker_->poke();
00130     boost::mutex::scoped_lock lock(mutex_);
00131     pcl::PointCloud<pcl::PointNormal>::Ptr
00132       input_cloud (new pcl::PointCloud<pcl::PointNormal>);
00133     pcl::PointCloud<pcl::PointXYZ>::Ptr
00134       hint_cloud (new pcl::PointCloud<pcl::PointXYZ>);
00135     pcl::fromROSMsg(*cloud_msg, *input_cloud);
00136     pcl::fromROSMsg(*hint_cloud_msg, *hint_cloud);
00137     
00138     // estimate plane out of hint_cloud
00139     
00140     jsk_recognition_utils::ConvexPolygon::Ptr convex;
00141     
00142     if (detectHintPlane(hint_cloud, convex) && convex) {
00143       if (detectLargerPlane(input_cloud, convex)) {
00144         NODELET_INFO("success to detect!");
00145       }
00146       else {
00147         NODELET_ERROR("failed to detect larger plane");
00148       }
00149     }
00150   }
00151 
00152   pcl::PointIndices::Ptr HintedPlaneDetector::getBestCluster(
00153     pcl::PointCloud<pcl::PointNormal>::Ptr input_cloud,
00154     const std::vector<pcl::PointIndices>& cluster_indices,
00155     const jsk_recognition_utils::ConvexPolygon::Ptr hint_convex)
00156   {
00157     Eigen::Vector3f center = hint_convex->centroid();
00158     double min_dist = DBL_MAX;
00159     size_t min_index = 0;
00160     for (size_t i = 0; i < cluster_indices.size(); i++) {
00161       Eigen::Vector4f center_cluster4;
00162       pcl::compute3DCentroid<pcl::PointNormal>(*input_cloud,
00163                                                cluster_indices[i].indices,
00164                                                center_cluster4);
00165       Eigen::Vector3f center_cluster3(center_cluster4[0], center_cluster4[1], center_cluster4[2]);
00166       double dist = (center - center_cluster3).norm();
00167       if (dist < min_dist) {
00168         min_dist = dist;
00169         min_index = i;
00170       }
00171     }
00172     pcl::PointIndices::Ptr ret (new pcl::PointIndices);
00173     ret->indices = cluster_indices[min_index].indices;
00174     return ret;
00175   }
00176 
00177   void HintedPlaneDetector::densityFilter(
00178     const pcl::PointCloud<pcl::PointNormal>::Ptr cloud,
00179     const pcl::PointIndices::Ptr indices,
00180     pcl::PointIndices& output)
00181   {
00182     if (enable_density_filtering_) {
00183       pcl::KdTreeFLANN<pcl::PointNormal> kdtree;
00184       pcl::KdTreeFLANN<pcl::PointNormal>::IndicesPtr indices_ptr
00185         (new std::vector<int>);
00186       *indices_ptr = indices->indices;
00187       kdtree.setInputCloud(cloud, indices_ptr);
00188       for (size_t i = 0; i < indices->indices.size(); i++) {
00189         int point_index = indices->indices[i];
00190         std::vector<int> result_indices;
00191         std::vector<float> result_distances;
00192         kdtree.radiusSearch(i, density_radius_,
00193                             result_indices, result_distances);
00194         if (result_distances.size() >= density_num_) {
00195           output.indices.push_back(point_index);
00196         }
00197       }
00198     }
00199     else {
00200       output = *indices;
00201     }
00202     output.header = cloud->header;
00203     PCLIndicesMsg ros_indices;
00204     pcl_conversions::fromPCL(output, ros_indices);
00205     pub_density_filtered_indices_.publish(ros_indices);
00206   }
00207 
00208   void HintedPlaneDetector::euclideanFilter(
00209     const pcl::PointCloud<pcl::PointNormal>::Ptr cloud,
00210     const pcl::PointIndices::Ptr indices,
00211     const jsk_recognition_utils::ConvexPolygon::Ptr hint_convex,
00212     pcl::PointIndices& output)
00213   {
00214     if (enable_density_filtering_) {
00215       pcl::EuclideanClusterExtraction<pcl::PointNormal> ec;
00216       ec.setClusterTolerance(euclidean_clustering_filter_tolerance_);
00217       pcl::search::KdTree<pcl::PointNormal>::Ptr tree
00218         (new pcl::search::KdTree<pcl::PointNormal>);
00219       tree->setInputCloud(cloud);
00220       ec.setSearchMethod(tree);
00221       ec.setIndices(indices);
00222       ec.setInputCloud(cloud);
00223       //ec.setMinClusterSize ();
00224       std::vector<pcl::PointIndices> cluster_indices;
00225       ec.extract(cluster_indices);
00226       if (cluster_indices.size() == 0) {
00227         return;
00228       }
00229       NODELET_INFO("%lu clusters", cluster_indices.size());
00230       pcl::PointIndices::Ptr filtered_indices
00231         = getBestCluster(cloud, cluster_indices, hint_convex);
00232       output = *filtered_indices;
00233     }
00234     else {
00235       output = *indices;
00236     }
00237     output.header = cloud->header;
00238     PCLIndicesMsg ros_indices;
00239     pcl_conversions::fromPCL(output, ros_indices);
00240     pub_euclidean_filtered_indices_.publish(ros_indices);
00241   }
00242 
00243   void HintedPlaneDetector::hintFilter(
00244     const pcl::PointCloud<pcl::PointNormal>::Ptr cloud,
00245     const jsk_recognition_utils::ConvexPolygon::Ptr hint_convex,
00246     pcl::PointIndices& output)
00247   {
00248     for (size_t i = 0; i < cloud->points.size(); i++) {
00249       pcl::PointNormal p = cloud->points[i];
00250       if (!std::isnan(p.x) && !std::isnan(p.y) && !std::isnan(p.y)) {
00251         Eigen::Vector4f v = p.getVector4fMap();
00252         if (!enable_distance_filtering_ || hint_convex->distanceToPoint(v) < outlier_threashold_) {
00253           Eigen::Vector3f n(p.normal_x, p.normal_y, p.normal_z);
00254           if (!enable_normal_filtering_ || hint_convex->angle(n) < normal_filter_eps_angle_) {
00255             output.indices.push_back(i);
00256           }
00257         }
00258       }
00259     }
00260     output.header = cloud->header;
00261     PCLIndicesMsg ros_candidate_inliers;
00262     pcl_conversions::fromPCL(output, ros_candidate_inliers);
00263     pub_hint_filtered_indices_.publish(ros_candidate_inliers);
00264   }
00265 
00266   void HintedPlaneDetector::planeFilter(
00267     const pcl::PointCloud<pcl::PointNormal>::Ptr cloud,
00268     const pcl::PointIndices::Ptr indices,
00269     const Eigen::Vector3f& normal,
00270     pcl::PointIndices& output,
00271     pcl::ModelCoefficients& coefficients)
00272   {
00273     pcl::SACSegmentation<pcl::PointNormal> seg;
00274     seg.setOptimizeCoefficients (true);
00275     seg.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE);
00276     seg.setMethodType (pcl::SAC_RANSAC);
00277     seg.setDistanceThreshold (outlier_threashold_);
00278     seg.setMaxIterations (max_iteration_);
00279     seg.setEpsAngle(eps_angle_);
00280     seg.setAxis(normal);
00281     seg.setInputCloud(cloud);
00282     seg.setIndices(indices);
00283     seg.segment(output, coefficients);
00284     coefficients.header = cloud->header;
00285     output.header = cloud->header;
00286     PCLIndicesMsg ros_indices;
00287     pcl_conversions::fromPCL(output, ros_indices);
00288     pub_plane_filtered_indices_.publish(ros_indices);
00289   }
00290   
00291   bool HintedPlaneDetector::detectLargerPlane(
00292     pcl::PointCloud<pcl::PointNormal>::Ptr input_cloud,
00293     jsk_recognition_utils::ConvexPolygon::Ptr hint_convex)
00294   {
00295     pcl::PointIndices::Ptr candidate_inliers (new pcl::PointIndices);
00296     hintFilter(input_cloud, hint_convex, *candidate_inliers);
00297     // filter points based on hint_convex to get better result...
00298     
00299     pcl::PointIndices::Ptr plane_inliers (new pcl::PointIndices);
00300     pcl::ModelCoefficients::Ptr plane_coefficients(new pcl::ModelCoefficients);
00301     planeFilter(input_cloud, candidate_inliers,
00302                 hint_convex->getNormal(),
00303                 *plane_inliers, *plane_coefficients);
00304     if (plane_inliers->indices.size() < min_size_) { // good!
00305       NODELET_ERROR("failed to detect by plane fitting filtering");
00306       return false;
00307     }
00308     // Check direction of plane_coefficients
00309     Eigen::Vector3f plane_normal(plane_coefficients->values[0], plane_coefficients->values[1], plane_coefficients->values[2]);
00310     if (plane_normal.dot(Eigen::Vector3f::UnitZ()) > 0) {
00311       // flip
00312       plane_coefficients->values[0] = -plane_coefficients->values[0];
00313       plane_coefficients->values[1] = -plane_coefficients->values[1];
00314       plane_coefficients->values[2] = -plane_coefficients->values[2];
00315       plane_coefficients->values[3] = -plane_coefficients->values[3];
00316     }
00317     // filtering by euclidean clustering
00318     pcl::PointIndices::Ptr euclidean_filtered_indices(new pcl::PointIndices);
00319     euclideanFilter(input_cloud, plane_inliers, hint_convex,
00320                     *euclidean_filtered_indices);
00321     if (euclidean_filtered_indices->indices.size() < min_size_) {
00322       NODELET_ERROR("failed to detect by euclidean filtering");
00323       return false;
00324     }
00325     pcl::PointIndices::Ptr density_filtered_indices (new pcl::PointIndices);
00326     densityFilter(
00327       input_cloud, euclidean_filtered_indices, *density_filtered_indices);
00328       
00329     if (density_filtered_indices->indices.size() < min_size_) {
00330       NODELET_ERROR("failed to detect by density filtering");
00331       return false;
00332     }
00333     jsk_recognition_utils::ConvexPolygon::Ptr convex
00334       = jsk_recognition_utils::convexFromCoefficientsAndInliers<pcl::PointNormal>(
00335         input_cloud, density_filtered_indices, plane_coefficients);
00336     // publish to ROS
00337     publishPolygon(convex, pub_polygon_, pub_polygon_array_,
00338                    input_cloud->header);
00339     PCLIndicesMsg ros_inliers;
00340     pcl_conversions::fromPCL(*density_filtered_indices, ros_inliers);
00341     pub_inliers_.publish(ros_inliers);
00342     PCLModelCoefficientMsg ros_coefficients;
00343     pcl_conversions::fromPCL(*plane_coefficients, ros_coefficients);
00344     pub_coefficients_.publish(ros_coefficients);
00345     return true;
00346   }
00347 
00348   void HintedPlaneDetector::publishPolygon(
00349     const jsk_recognition_utils::ConvexPolygon::Ptr convex,
00350     ros::Publisher& pub_polygon,
00351     ros::Publisher& pub_polygon_array,
00352     const pcl::PCLHeader& header)
00353   {
00354     geometry_msgs::PolygonStamped ros_polygon;
00355     ros_polygon.polygon = convex->toROSMsg();
00356     pcl_conversions::fromPCL(header, ros_polygon.header);
00357     jsk_recognition_msgs::PolygonArray ros_polygon_array;
00358     pcl_conversions::fromPCL(header, ros_polygon_array.header);
00359     ros_polygon_array.polygons.push_back(
00360       ros_polygon);
00361     pub_polygon_array.publish(ros_polygon_array);
00362     pub_polygon.publish(ros_polygon);
00363   }
00364   
00365   bool HintedPlaneDetector::detectHintPlane(
00366     pcl::PointCloud<pcl::PointXYZ>::Ptr hint_cloud,
00367     jsk_recognition_utils::ConvexPolygon::Ptr& convex)
00368   {
00369     pcl::PointIndices::Ptr hint_inliers (new pcl::PointIndices);
00370     pcl::ModelCoefficients::Ptr hint_coefficients(new pcl::ModelCoefficients);
00371     pcl::SACSegmentation<pcl::PointXYZ> seg;
00372     seg.setOptimizeCoefficients (true);
00373     seg.setModelType (pcl::SACMODEL_PLANE);
00374     seg.setMethodType (pcl::SAC_RANSAC);
00375     seg.setDistanceThreshold (hint_outlier_threashold_);
00376     seg.setMaxIterations (hint_max_iteration_);
00377     seg.setInputCloud(hint_cloud);
00378     seg.segment(*hint_inliers, *hint_coefficients);
00379     if (hint_inliers->indices.size() > hint_min_size_) { // good!
00380       convex = jsk_recognition_utils::convexFromCoefficientsAndInliers<pcl::PointXYZ>(
00381         hint_cloud, hint_inliers, hint_coefficients);
00382       // publish hint results for debug/visualization
00383       publishPolygon(convex,
00384                      pub_hint_polygon_, pub_hint_polygon_array_,
00385                      hint_cloud->header);
00386       PCLIndicesMsg ros_indices;
00387       PCLModelCoefficientMsg ros_coefficients;
00388       pcl_conversions::fromPCL(*hint_inliers, ros_indices);
00389       pub_hint_inliers_.publish(ros_indices);
00390       pcl_conversions::fromPCL(*hint_coefficients, ros_coefficients);
00391       pub_hint_coefficients_.publish(ros_coefficients);
00392       return true;
00393     }
00394     else {
00395       NODELET_ERROR("Failed to find hint plane");
00396       return false;
00397     }
00398   }
00399 
00400 }
00401 
00402 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::HintedPlaneDetector, nodelet::Nodelet);


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