hinted_stick_finder_nodelet.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2015, 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 #define BOOST_PARAMETER_MAX_ARITY 7
00036 #include "jsk_pcl_ros/hinted_stick_finder.h"
00037 #include "jsk_recognition_utils/pcl_conversion_util.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/project_inliers.h>
00042 #include <pcl/filters/extract_indices.h>
00043 #include <pcl/features/normal_3d_omp.h>
00044 #include <visualization_msgs/Marker.h>
00045 #include <geometry_msgs/PoseStamped.h>
00046 
00047 namespace jsk_pcl_ros
00048 {
00049   void HintedStickFinder::onInit()
00050   {
00051     DiagnosticNodelet::onInit();
00052     srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00053     typename dynamic_reconfigure::Server<Config>::CallbackType f =
00054       boost::bind (&HintedStickFinder::configCallback, this, _1, _2);
00055     srv_->setCallback (f);
00056     pnh_->param("use_normal", use_normal_, false);
00057     pnh_->param("not_synchronize", not_synchronize_, false);
00058     
00059     pub_line_filtered_indices_ = advertise<PCLIndicesMsg>(
00060       *pnh_, "debug/line_filtered_indices", 1);
00061     pub_line_filtered_normal_ = advertise<sensor_msgs::PointCloud2>(
00062       *pnh_, "debug/line_filtered_normal", 1);
00063     pub_cylinder_marker_ = advertise<visualization_msgs::Marker>(
00064       *pnh_, "debug/cylinder_marker", 1);
00065     pub_cylinder_pose_ = advertise<geometry_msgs::PoseStamped>(
00066       *pnh_, "output/cylinder_pose", 1);
00067     pub_inliers_ = advertise<PCLIndicesMsg>(
00068       *pnh_, "output/inliers", 1);
00069     pub_coefficients_ = advertise<PCLModelCoefficientMsg>(
00070       *pnh_, "output/coefficients", 1);
00071     onInitPostProcess();
00072   }
00073 
00074   void HintedStickFinder::subscribe()
00075   {
00076     if (!not_synchronize_) {
00077       sub_polygon_.subscribe(*pnh_, "input/hint/line", 1);
00078       sub_info_.subscribe(*pnh_, "input/camera_info", 1);
00079       sub_cloud_.subscribe(*pnh_, "input", 1);
00080       sync_ = boost::make_shared<message_filters::Synchronizer<ASyncPolicy> >(100);
00081       sync_->connectInput(sub_polygon_, sub_info_, sub_cloud_);
00082       sync_->registerCallback(boost::bind(&HintedStickFinder::detect, this,
00083                                           _1, _2, _3));
00084     }
00085     else {
00086       sub_no_sync_cloud_ = pnh_->subscribe(
00087         "input", 1, &HintedStickFinder::cloudCallback, this);
00088       sub_no_sync_camera_info_ = pnh_->subscribe(
00089         "input/camera_info", 1, &HintedStickFinder::infoCallback, this);
00090       sub_no_sync_polygon_ = pnh_->subscribe(
00091         "input/hint/line", 1, &HintedStickFinder::hintCallback, this);
00092     }
00093   }
00094 
00095 
00096   void HintedStickFinder::cloudCallback(
00097       const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
00098   {
00099     {
00100       boost::mutex::scoped_lock lock(mutex_);
00101       if (!latest_hint_ || !latest_camera_info_) {
00102         // not yet ready
00103         NODELET_WARN_THROTTLE(1, "~input/hint/lline or ~input/camera_info is not ready");
00104         return;
00105       }
00106     }
00107     detect(latest_hint_, latest_camera_info_, cloud_msg);
00108   }
00109 
00110 
00111   void HintedStickFinder::hintCallback(
00112     const geometry_msgs::PolygonStamped::ConstPtr& hint_msg)
00113   {
00114     boost::mutex::scoped_lock lock(mutex_);
00115     latest_hint_ = hint_msg;
00116   }
00117 
00118   void HintedStickFinder::infoCallback(
00119     const sensor_msgs::CameraInfo::ConstPtr& info_msg)
00120   {
00121     boost::mutex::scoped_lock lock(mutex_);
00122     latest_camera_info_ = info_msg;
00123   }
00124   
00125   
00126   void HintedStickFinder::unsubscribe()
00127   {
00128     if (!not_synchronize_) {
00129       sub_polygon_.unsubscribe();
00130       sub_info_.unsubscribe();
00131       sub_cloud_.unsubscribe();
00132     }
00133     else {
00134       sub_no_sync_cloud_.shutdown();
00135       sub_no_sync_camera_info_.shutdown();
00136       sub_no_sync_polygon_.shutdown();
00137     }
00138   }
00139 
00140   void HintedStickFinder::detect(
00141     const geometry_msgs::PolygonStamped::ConstPtr& polygon_msg,
00142     const sensor_msgs::CameraInfo::ConstPtr& camera_info_msg,
00143     const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
00144   {
00145     boost::mutex::scoped_lock lock(mutex_);
00146     NODELET_WARN("starting detection");
00147     ros::Time start_time = ros::Time::now();
00148     image_geometry::PinholeCameraModel model;
00149     model.fromCameraInfo(camera_info_msg);
00150     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud
00151       (new pcl::PointCloud<pcl::PointXYZ>);
00152     pcl::fromROSMsg(*cloud_msg, *cloud);
00153     // convert 2-D point into 3-D ray
00154     Eigen::Vector3f a, b;
00155     jsk_recognition_utils::ConvexPolygon::Ptr polygon = polygonFromLine(polygon_msg, model, a, b);
00156     pcl::PointIndices::Ptr candidate_indices
00157       (new pcl::PointIndices);
00158     
00159     filterPointCloud(cloud, polygon, *candidate_indices);
00160     pcl::PointCloud<pcl::Normal>::Ptr normals
00161       (new pcl::PointCloud<pcl::Normal>);
00162     pcl::PointCloud<pcl::PointXYZ>::Ptr normals_cloud
00163       (new pcl::PointCloud<pcl::PointXYZ>);
00164     if (!use_normal_) {
00165       normalEstimate(cloud, candidate_indices, *normals, *normals_cloud);
00166     }
00167     else {
00168       // we don't need to compute normal
00169       pcl::PointCloud<pcl::Normal>::Ptr all_normals
00170         (new pcl::PointCloud<pcl::Normal>);
00171       pcl::fromROSMsg(*cloud_msg, *all_normals);
00172       pcl::ExtractIndices<pcl::PointXYZ> xyz_extract;
00173       xyz_extract.setInputCloud(cloud);
00174       xyz_extract.setIndices(candidate_indices);
00175       xyz_extract.filter(*normals_cloud);
00176       
00177       pcl::ExtractIndices<pcl::Normal> normal_extract;
00178       normal_extract.setInputCloud(all_normals);
00179       normal_extract.setIndices(candidate_indices);
00180       normal_extract.filter(*normals);
00181     }
00182     fittingCylinder(normals_cloud, normals, a, b);
00183     ros::Time end_time = ros::Time::now();
00184     NODELET_WARN("detection time: %f", (end_time - start_time).toSec());
00185   }
00186 
00187   void HintedStickFinder::normalEstimate(
00188     const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
00189     const pcl::PointIndices::Ptr indices,
00190     pcl::PointCloud<pcl::Normal>& normals,
00191     pcl::PointCloud<pcl::PointXYZ>& normals_cloud)
00192   {
00193     pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> ne;
00194     ne.setInputCloud(cloud);
00195     ne.setIndices(indices);
00196     pcl::search::KdTree<pcl::PointXYZ>::Ptr tree
00197       (new pcl::search::KdTree<pcl::PointXYZ> ());
00198     ne.setSearchMethod(tree);
00199     ne.setRadiusSearch (0.03);
00200     ne.compute (normals);
00201     pcl::ExtractIndices<pcl::PointXYZ> ex;
00202     ex.setInputCloud(cloud);
00203     ex.setIndices(indices);
00204     ex.filter(normals_cloud);
00205   }
00206 
00207   bool HintedStickFinder::rejected2DHint(
00208     const jsk_recognition_utils::Cylinder::Ptr& cylinder,
00209     const Eigen::Vector3f& a,
00210     const Eigen::Vector3f& b)
00211   {
00212     Eigen::Vector3f hint_dir((b - a));
00213     hint_dir[2] = 0;
00214     hint_dir.normalize();
00215     Eigen::Vector3f cylinder_dir(cylinder->getDirection());
00216     cylinder_dir[2] = 0;
00217     cylinder_dir.normalize();
00218     double ang = acos(cylinder_dir.dot(hint_dir));
00219     NODELET_INFO("angle: %f", ang);
00220     return !(ang < eps_2d_angle_ || (M_PI - ang) < eps_2d_angle_);
00221   }
00222   
00223   void HintedStickFinder::fittingCylinder(
00224     const pcl::PointCloud<pcl::PointXYZ>::Ptr& filtered_cloud,
00225     const pcl::PointCloud<pcl::Normal>::Ptr& cloud_normals,
00226     const Eigen::Vector3f& a,
00227     const Eigen::Vector3f& b)
00228   {
00229     pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg;
00230     pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
00231     pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
00232     Eigen::Vector3f normal = (a - b).normalized();
00233     seg.setOptimizeCoefficients (true);
00234     seg.setModelType (pcl::SACMODEL_CYLINDER);
00235     seg.setMethodType (pcl::SAC_RANSAC);
00236     seg.setDistanceThreshold (outlier_threshold_);
00237     seg.setMaxIterations (max_iteration_);
00238     seg.setNormalDistanceWeight (0.1);
00239     seg.setRadiusLimits(min_radius_, max_radius_);
00240     // seg.setEpsAngle(eps_angle_);
00241     // seg.setAxis(normal);
00242     seg.setProbability(min_probability_);
00243     seg.setInputCloud(filtered_cloud);
00244     seg.setInputNormals(cloud_normals);
00245     for (size_t i = 0; i < cylinder_fitting_trial_; i++) {
00246       seg.segment(*inliers, *coefficients);
00247       if (inliers->indices.size() > min_inliers_) {
00248         Eigen::Vector3f dir(coefficients->values[3],
00249                             coefficients->values[4],
00250                             coefficients->values[5]);
00251         if (dir.dot(Eigen::Vector3f(0, -1, 0)) < 0) {
00252           dir = -dir;
00253         }
00254         jsk_recognition_utils::Cylinder::Ptr cylinder(new jsk_recognition_utils::Cylinder(Eigen::Vector3f(
00255                                                                                             coefficients->values[0],
00256                                                                                             coefficients->values[1],
00257                                                                                             coefficients->values[2]),
00258                                                                                           dir,
00259                                                                                           coefficients->values[6]));
00260         pcl::PointIndices::Ptr cylinder_indices
00261           (new pcl::PointIndices);
00262         cylinder->filterPointCloud(*filtered_cloud,
00263                                    outlier_threshold_,
00264                                    *cylinder_indices);
00265         double height = 0;
00266         Eigen::Vector3f center;
00267         cylinder->estimateCenterAndHeight(
00268           *filtered_cloud, *cylinder_indices,
00269           center, height);
00270         if (!rejected2DHint(cylinder, a, b)) {
00271           Eigen::Vector3f uz = Eigen::Vector3f(
00272             dir).normalized();
00273           // build maker
00274           visualization_msgs::Marker marker;
00275           cylinder->toMarker(marker, center, uz, height);
00276           pcl_conversions::fromPCL(filtered_cloud->header, marker.header);
00277           pub_cylinder_marker_.publish(marker);
00278           geometry_msgs::PoseStamped pose;
00279           pose.header = marker.header;
00280           pose.pose = marker.pose;
00281           pub_cylinder_pose_.publish(pose);
00282           
00283           PCLIndicesMsg ros_inliers;
00284           pcl_conversions::fromPCL(*inliers, ros_inliers);
00285           pub_inliers_.publish(ros_inliers);
00286           PCLModelCoefficientMsg ros_coefficients;
00287           ros_coefficients.header = pcl_conversions::fromPCL(coefficients->header);
00288           ros_coefficients.values.push_back(center[0]);
00289           ros_coefficients.values.push_back(center[1]);
00290           ros_coefficients.values.push_back(center[2]);
00291           ros_coefficients.values.push_back(dir[0]);
00292           ros_coefficients.values.push_back(dir[1]);
00293           ros_coefficients.values.push_back(dir[2]);
00294           ros_coefficients.values.push_back(coefficients->values[6]);
00295           ros_coefficients.values.push_back(height);
00296           pub_coefficients_.publish(ros_coefficients);
00297         return;
00298         }
00299       }
00300       NODELET_WARN("failed to detect cylinder [%lu/%d]", i, cylinder_fitting_trial_);
00301     }
00302   }
00303   
00304   void HintedStickFinder::filterPointCloud(
00305     const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
00306     const jsk_recognition_utils::ConvexPolygon::Ptr polygon,
00307     pcl::PointIndices& output_indices)
00308   {
00309     output_indices.indices.clear();
00310     for (size_t i = 0; i < cloud->points.size(); i++) {
00311       pcl::PointXYZ p = cloud->points[i];
00312       if (!std::isnan(p.x) && !std::isnan(p.y) && !std::isnan(p.z)) {
00313         if (polygon->isProjectableInside(p.getVector3fMap())) {
00314           if (polygon->distanceSmallerThan(p.getVector3fMap(), filter_distance_)) {
00315             output_indices.indices.push_back(i);
00316           }
00317         }
00318       }
00319     }
00320     output_indices.header = cloud->header;
00321     PCLIndicesMsg ros_indices;
00322     pcl_conversions::fromPCL(output_indices, ros_indices);
00323     pub_line_filtered_indices_.publish(ros_indices);
00324   }
00325 
00326 
00327   jsk_recognition_utils::ConvexPolygon::Ptr HintedStickFinder::polygonFromLine(
00328     const geometry_msgs::PolygonStamped::ConstPtr& polygon_msg,
00329     const image_geometry::PinholeCameraModel& model,
00330     Eigen::Vector3f& a,
00331     Eigen::Vector3f& b)
00332   {
00333     cv::Point2d point_a(polygon_msg->polygon.points[0].x,
00334                         polygon_msg->polygon.points[0].y);
00335     cv::Point2d point_b(polygon_msg->polygon.points[1].x,
00336                         polygon_msg->polygon.points[1].y);
00337     cv::Point3d ray_a = model.projectPixelTo3dRay(point_a);
00338     cv::Point3d ray_b = model.projectPixelTo3dRay(point_b);
00339     a = Eigen::Vector3f(ray_a.x, ray_a.y, ray_a.z);
00340     b = Eigen::Vector3f(ray_b.x, ray_b.y, ray_b.z);
00341     // 20m is far enough??
00342     Eigen::Vector3f far_a = 20.0 * a;
00343     Eigen::Vector3f far_b = 20.0 * b;
00344     Eigen::Vector3f O(0, 0, 0);
00345     jsk_recognition_utils::Vertices vertices;
00346     vertices.push_back(O);
00347     vertices.push_back(far_a);
00348     vertices.push_back(far_b);
00349     jsk_recognition_utils::ConvexPolygon::Ptr polygon (new jsk_recognition_utils::ConvexPolygon(vertices));
00350     return polygon;
00351   }
00352 
00353   void HintedStickFinder::configCallback(Config &config, uint32_t level)
00354   {
00355     boost::mutex::scoped_lock lock(mutex_);
00356     min_radius_ = config.min_radius;
00357     max_radius_ = config.max_radius;
00358     filter_distance_ = config.filter_distance;
00359     outlier_threshold_ = config.outlier_threshold;
00360     max_iteration_ = config.max_iteration;
00361     eps_angle_ = config.eps_angle;
00362     min_probability_ = config.min_probability;
00363     cylinder_fitting_trial_ = config.cylinder_fitting_trial;
00364     min_inliers_ = config.min_inliers;
00365     eps_2d_angle_ = config.eps_2d_angle;
00366   }
00367 }
00368 
00369 #include <pluginlib/class_list_macros.h>
00370 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::HintedStickFinder, nodelet::Nodelet);


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