plane_concatenator_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 #define BOOST_PARAMETER_MAX_ARITY 7
00036 #include "jsk_pcl_ros/plane_concatenator.h"
00037 #include <pcl/kdtree/kdtree_flann.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 "jsk_pcl_ros/pcl_conversion_util.h"
00042 #include "jsk_pcl_ros/pcl_util.h"
00043 
00044 namespace jsk_pcl_ros
00045 {
00046   void PlaneConcatenator::onInit()
00047   {
00048     DiagnosticNodelet::onInit();
00049 
00050     srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00051     dynamic_reconfigure::Server<Config>::CallbackType f =
00052       boost::bind (
00053         &PlaneConcatenator::configCallback, this, _1, _2);
00054     srv_->setCallback (f);
00055     
00056     pub_indices_ = advertise<jsk_recognition_msgs::ClusterPointIndices>(
00057       *pnh_, "output/indices", 1);
00058     pub_polygon_ = advertise<jsk_recognition_msgs::PolygonArray>(
00059       *pnh_, "output/polygons", 1);
00060     pub_coefficients_ = advertise<jsk_recognition_msgs::ModelCoefficientsArray>(
00061       *pnh_, "output/coefficients", 1);
00062   }
00063 
00064   void PlaneConcatenator::subscribe()
00065   {
00066     sub_cloud_.subscribe(*pnh_, "input", 1);
00067     sub_indices_.subscribe(*pnh_, "input/indices", 1);
00068     sub_polygon_.subscribe(*pnh_, "input/polygons", 1);
00069     sub_coefficients_.subscribe(*pnh_, "input/coefficients", 1);
00070     sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> > (100);
00071     sync_->connectInput(sub_cloud_, sub_indices_,
00072                         sub_polygon_, sub_coefficients_);
00073     sync_->registerCallback(boost::bind(&PlaneConcatenator::concatenate, this,
00074                                         _1, _2, _3, _4));
00075   }
00076 
00077   void PlaneConcatenator::unsubscribe()
00078   {
00079     sub_cloud_.unsubscribe();
00080     sub_indices_.unsubscribe();
00081     sub_polygon_.unsubscribe();
00082     sub_coefficients_.unsubscribe();
00083   }
00084 
00085   void PlaneConcatenator::concatenate(
00086     const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
00087     const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& indices_msg,
00088     const jsk_recognition_msgs::PolygonArray::ConstPtr& polygon_array_msg,
00089     const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr& coefficients_array_msg)
00090   {
00091     boost::mutex::scoped_lock lock(mutex_);
00092     vital_checker_->poke();
00093     
00094     size_t nr_cluster = polygon_array_msg->polygons.size();
00095     pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
00096     pcl::fromROSMsg(*cloud_msg, *cloud);
00097     // first convert to polygon instance
00098     std::vector<pcl::ModelCoefficients::Ptr> all_coefficients
00099       = pcl_conversions::convertToPCLModelCoefficients(
00100         coefficients_array_msg->coefficients);
00101     std::vector<pcl::PointIndices::Ptr> all_indices
00102       = pcl_conversions::convertToPCLPointIndices(indices_msg->cluster_indices);
00103     std::vector<pcl::PointCloud<PointT>::Ptr> all_clouds
00104       = convertToPointCloudArray<PointT>(cloud, all_indices);
00105     std::vector<Plane::Ptr> planes = convertToPlanes(all_coefficients);
00106     // build connection map
00107     IntegerGraphMap connection_map;
00108     for (size_t i = 0; i < nr_cluster; i++) {
00109       connection_map[i] = std::vector<int>();
00110       connection_map[i].push_back(i);
00111       // build kdtree
00112       pcl::KdTreeFLANN<PointT> kdtree;
00113       pcl::PointCloud<PointT>::Ptr focused_cloud = all_clouds[i];
00114       kdtree.setInputCloud(focused_cloud);
00115       // look up near polygon
00116       Plane::Ptr focused_plane = planes[i];
00117       for (size_t j = i + 1; j < nr_cluster; j++) {
00118         Plane::Ptr target_plane = planes[j];
00119         if (focused_plane->angle(*target_plane) < connect_angular_threshold_) {
00120           // second, check distance
00121           bool is_near_enough = isNearPointCloud(kdtree, all_clouds[j], target_plane);
00122           if (is_near_enough) {
00123             connection_map[i].push_back(j);
00124           }
00125         }
00126       }
00127     }
00128     std::vector<std::set<int> > cloud_sets;
00129     buildAllGroupsSetFromGraphMap(connection_map, cloud_sets);
00130     // build new indices
00131     std::vector<pcl::PointIndices::Ptr> new_indices;
00132     std::vector<pcl::ModelCoefficients::Ptr> new_coefficients;
00133     for (size_t i = 0; i < cloud_sets.size(); i++) {
00134       pcl::PointIndices::Ptr new_one_indices (new pcl::PointIndices);
00135       new_coefficients.push_back(all_coefficients[*cloud_sets[i].begin()]);
00136       for (std::set<int>::iterator it = cloud_sets[i].begin();
00137            it != cloud_sets[i].end();
00138            ++it) {
00139         new_one_indices = addIndices(*new_one_indices, *all_indices[*it]);
00140       }
00141       new_indices.push_back(new_one_indices);
00142     }
00143     
00144     // refinement
00145     std::vector<pcl::ModelCoefficients::Ptr> new_refined_coefficients;
00146     for (size_t i = 0; i < new_indices.size(); i++) {
00147       pcl::ModelCoefficients::Ptr refined_coefficients
00148         = refinement(cloud, new_indices[i], new_coefficients[i]);
00149       new_refined_coefficients.push_back(refined_coefficients);
00150     }
00151 
00152     // publish
00153     jsk_recognition_msgs::ClusterPointIndices new_ros_indices;
00154     jsk_recognition_msgs::ModelCoefficientsArray new_ros_coefficients;
00155     jsk_recognition_msgs::PolygonArray new_ros_polygons;
00156     new_ros_indices.header = cloud_msg->header;
00157     new_ros_coefficients.header = cloud_msg->header;
00158     new_ros_polygons.header = cloud_msg->header;
00159     new_ros_indices.cluster_indices
00160       = pcl_conversions::convertToROSPointIndices(new_indices, cloud_msg->header);
00161     new_ros_coefficients.coefficients
00162       = pcl_conversions::convertToROSModelCoefficients(
00163         new_refined_coefficients, cloud_msg->header);
00164     
00165     for (size_t i = 0; i < new_indices.size(); i++) {
00166       ConvexPolygon::Ptr convex
00167         = convexFromCoefficientsAndInliers<PointT>(
00168           cloud, new_indices[i], new_refined_coefficients[i]);
00169       geometry_msgs::PolygonStamped polygon;
00170       polygon.polygon = convex->toROSMsg();
00171       polygon.header = cloud_msg->header;
00172       new_ros_polygons.polygons.push_back(polygon);
00173     }
00174 
00175     pub_indices_.publish(new_ros_indices);
00176     pub_polygon_.publish(new_ros_polygons);
00177     pub_coefficients_.publish(new_ros_coefficients);
00178   }
00179 
00180   pcl::ModelCoefficients::Ptr PlaneConcatenator::refinement(
00181     pcl::PointCloud<PointT>::Ptr cloud,
00182     pcl::PointIndices::Ptr indices,
00183     pcl::ModelCoefficients::Ptr original_coefficients)
00184   {
00185     pcl::SACSegmentation<PointT> seg;
00186     seg.setOptimizeCoefficients (true);
00187     seg.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE);
00188     seg.setMethodType (pcl::SAC_RANSAC);
00189     seg.setDistanceThreshold (ransac_refinement_outlier_threshold_);
00190     
00191     seg.setInputCloud(cloud);
00192     
00193     seg.setIndices(indices);
00194     seg.setMaxIterations (ransac_refinement_max_iteration_);
00195     Eigen::Vector3f normal (original_coefficients->values[0],
00196                             original_coefficients->values[1],
00197                             original_coefficients->values[2]);
00198     seg.setAxis(normal);
00199     // seg.setInputNormals(cloud);
00200     // seg.setDistanceFromOrigin(original_coefficients->values[3]);
00201     // seg.setEpsDist(ransac_refinement_eps_distance_);
00202     seg.setEpsAngle(ransac_refinement_eps_angle_);
00203     pcl::PointIndices::Ptr refined_inliers (new pcl::PointIndices);
00204     pcl::ModelCoefficients::Ptr refined_coefficients(new pcl::ModelCoefficients);
00205     seg.segment(*refined_inliers, *refined_coefficients);
00206     if (refined_inliers->indices.size() > 0) {
00207       return refined_coefficients;
00208     }
00209     else {
00210       return original_coefficients;
00211     }
00212   }
00213   
00214   bool PlaneConcatenator::isNearPointCloud(
00215     pcl::KdTreeFLANN<PointT>& kdtree,
00216     pcl::PointCloud<PointT>::Ptr cloud,
00217     Plane::Ptr target_plane)
00218   {
00219     pcl::PointCloud<PointT>::ConstPtr input_cloud = kdtree.getInputCloud();
00220     for (size_t i = 0; i < cloud->points.size(); i++) {
00221       PointT p = cloud->points[i];
00222       std::vector<int> k_indices;
00223       std::vector<float> k_sqr_distances;
00224       if (kdtree.radiusSearch(p, connect_distance_threshold_,
00225                               k_indices, k_sqr_distances, 1) > 0) {
00226         // Decompose distance into pependicular distance and parallel distance
00227         const PointT near_p = input_cloud->points[k_indices[0]];
00228         Eigen::Affine3f plane_coordinates = target_plane->coordinates();
00229         Eigen::Vector3f plane_local_p = plane_coordinates.inverse() * p.getVector3fMap();
00230         Eigen::Vector3f plane_local_near_p = plane_coordinates.inverse() * near_p.getVector3fMap();
00231         Eigen::Vector3f plane_local_diff = plane_local_near_p - plane_local_p;
00232         double perpendicular_distance = std::abs(plane_local_diff[2]);
00233         if (perpendicular_distance < connect_perpendicular_distance_threshold_) {
00234           return true;
00235         }
00236       }
00237     }
00238     return false;
00239   }
00240     
00241   void PlaneConcatenator::configCallback(
00242     Config &config, uint32_t level)
00243   {
00244     boost::mutex::scoped_lock lock(mutex_);
00245     connect_angular_threshold_ = config.connect_angular_threshold;
00246     connect_distance_threshold_ = config.connect_distance_threshold;
00247     connect_perpendicular_distance_threshold_ = config.connect_perpendicular_distance_threshold;
00248     ransac_refinement_max_iteration_ = config.ransac_refinement_max_iteration;
00249     ransac_refinement_outlier_threshold_
00250       = config.ransac_refinement_outlier_threshold;
00251     ransac_refinement_eps_angle_ = config.ransac_refinement_eps_angle;
00252     min_size_ = config.min_size;
00253   }
00254 
00255   void PlaneConcatenator::updateDiagnostic(
00256     diagnostic_updater::DiagnosticStatusWrapper &stat)
00257   {
00258     if (vital_checker_->isAlive()) {
00259       stat.summary(diagnostic_msgs::DiagnosticStatus::OK,
00260                    "PlaneConcatenator running");
00261     }
00262     else {
00263       jsk_topic_tools::addDiagnosticErrorSummary(
00264         "PlaneConcatenator", vital_checker_, stat);
00265     }
00266   }
00267 
00268 }
00269 
00270 #include <pluginlib/class_list_macros.h>
00271 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::PlaneConcatenator, nodelet::Nodelet);


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Wed Sep 16 2015 04:36:48