convex_connected_voxels_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/convex_connected_voxels.h>
00037 
00038 namespace jsk_pcl_ros
00039 {   
00040     void ConvexConnectedVoxels::onInit()
00041     {
00042        DiagnosticNodelet::onInit();
00043        pub_indices_ = advertise<
00044           jsk_recognition_msgs::ClusterPointIndices>(
00045              *pnh_, "output/indices", 1);
00046        onInitPostProcess();
00047    }
00048 
00049     void ConvexConnectedVoxels::subscribe()
00050     {
00051        sub_indices_ = pnh_->subscribe(
00052           "input/indices", 10,
00053           &ConvexConnectedVoxels::indices_cb, this);
00054        sub_cloud_ = pnh_->subscribe(
00055           "input/cloud", 10,
00056           &ConvexConnectedVoxels::cloud_cb, this);
00057     }
00058    
00059     void ConvexConnectedVoxels::unsubscribe()
00060     {
00061        sub_cloud_.shutdown();
00062        sub_indices_.shutdown();
00063     }
00064 
00065     void ConvexConnectedVoxels::updateDiagnostic(
00066        diagnostic_updater::DiagnosticStatusWrapper &stat)
00067     {
00068        if (vital_checker_->isAlive()) {
00069           stat.summary(diagnostic_msgs::DiagnosticStatus::OK,
00070                        "ConvexConnectedVoxels running");
00071        } else {
00072           jsk_topic_tools::addDiagnosticErrorSummary(
00073              "ConvexConnectedVoxels", vital_checker_, stat);
00074        }
00075     }
00076    
00077     void ConvexConnectedVoxels::cloud_cb(
00078        const sensor_msgs::PointCloud2::ConstPtr &cloud_msg)
00079     {
00080        // ROS_INFO("PROCESSING CLOUD CALLBACK");
00081        
00082        // boost::mutex::scoped_lock lock(this->mutex_);
00083        // vital_checker_->poke();
00084        pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
00085        pcl::fromROSMsg(*cloud_msg, *cloud);
00086        std::vector<pcl::PointCloud<PointT>::Ptr> cloud_clusters;
00087        std::vector<pcl::PointCloud<pcl::Normal>::Ptr> normal_clusters;
00088        pcl::PointCloud<pcl::PointXYZ>::Ptr centroids(
00089           new pcl::PointCloud<pcl::PointXYZ>);
00090        this->segmentCloud(
00091           cloud, this->indices_, cloud_clusters, normal_clusters, centroids);
00092        std::vector<std::vector<int> > neigbour_idx;
00093        this->nearestNeigborSearch(centroids, neigbour_idx, 4);
00094        boost::shared_ptr<jsk_pcl_ros::RegionAdjacencyGraph> rag(
00095           new jsk_pcl_ros::RegionAdjacencyGraph);
00096        rag->generateRAG(
00097           cloud_clusters,
00098           normal_clusters,
00099           centroids,
00100           neigbour_idx,
00101           rag->RAG_EDGE_WEIGHT_CONVEX_CRITERIA);
00102        rag->splitMergeRAG(0.0);
00103        std::vector<int> labelMD;
00104        rag->getCloudClusterLabels(labelMD);
00105        std::map<int, pcl::PointIndices> _indices;
00106        this->getConvexLabelCloudIndices(
00107           cloud_clusters, cloud, labelMD, _indices);
00108        std::vector<pcl::PointIndices> all_indices;
00109        for (std::map<int, pcl::PointIndices>::iterator it = _indices.begin();
00110             it != _indices.end(); it++) {
00111           all_indices.push_back((*it).second);
00112        }
00113 
00114        // ROS_INFO("Size: %ld", _indices.size());
00115 
00116        jsk_recognition_msgs::ClusterPointIndices ros_indices;
00117        ros_indices.cluster_indices = pcl_conversions::convertToROSPointIndices(
00118           all_indices, cloud_msg->header);
00119        ros_indices.header = cloud_msg->header;
00120        pub_indices_.publish(ros_indices);
00121     }
00122 
00123     void ConvexConnectedVoxels::indices_cb(
00124        const jsk_recognition_msgs::ClusterPointIndices &indices_msg)
00125     {
00126        // boost::mutex::scoped_lock lock(this->mutex_);
00127        vital_checker_->poke();
00128        this->indices_.clear();
00129        std::vector<pcl_msgs::PointIndices> indices =
00130           indices_msg.cluster_indices;
00131        for (int i = 0; i < indices.size(); i++) {
00132           pcl::PointIndices _index;
00133           pcl_conversions::toPCL(indices[i], _index);
00134           this->indices_.push_back(_index);
00135        }
00136     }
00137    
00138     void ConvexConnectedVoxels::segmentCloud(
00139         const pcl::PointCloud<PointT>::Ptr cloud,
00140         const std::vector<pcl::PointIndices> &indices,
00141         std::vector<pcl::PointCloud<PointT>::Ptr> &cloud_clusters,
00142         std::vector<pcl::PointCloud<pcl::Normal>::Ptr> &normal_clusters,
00143         pcl::PointCloud<pcl::PointXYZ>::Ptr centroids)
00144     {
00145         boost::mutex::scoped_lock lock(this->mutex_);
00146         pcl::ExtractIndices<PointT>::Ptr eifilter(
00147            new pcl::ExtractIndices<PointT>);
00148         eifilter->setInputCloud(cloud);
00149         for (int i = 0; i < indices.size(); i++) {
00150            pcl::PointIndices::Ptr index(new pcl::PointIndices());
00151            index->indices = indices[i].indices;
00152            eifilter->setIndices(index);
00153            pcl::PointCloud<PointT>::Ptr tmp_cloud(
00154               new pcl::PointCloud<PointT>);
00155            eifilter->filter(*tmp_cloud);
00156            if (tmp_cloud->width > 0) {
00157              Eigen::Vector4f centroid;
00158              pcl::compute3DCentroid<PointT, float>(*cloud, *index, centroid);
00159              float ct_x = static_cast<float>(centroid[0]);
00160              float ct_y = static_cast<float>(centroid[1]);
00161              float ct_z = static_cast<float>(centroid[2]);
00162              if (!std::isnan(ct_x) && !std::isnan(ct_y) && !std::isnan(ct_z)) {
00163                 pcl::PointCloud<pcl::Normal>::Ptr s_normal(
00164                    new pcl::PointCloud<pcl::Normal>);
00165                 this->estimatePointCloudNormals(
00166                    tmp_cloud, s_normal, 40, 0.05, false);
00167                 normal_clusters.push_back(s_normal);
00168                 centroids->push_back(pcl::PointXYZ(ct_x, ct_y, ct_z));
00169                 cloud_clusters.push_back(tmp_cloud);
00170              }
00171           }
00172         }
00173     }
00174    
00175     void ConvexConnectedVoxels::estimatePointCloudNormals(
00176        const pcl::PointCloud<PointT>::Ptr cloud,
00177        pcl::PointCloud<pcl::Normal>::Ptr s_normal,
00178        const int k, const double radius, bool ksearch)
00179     {
00180        pcl::NormalEstimationOMP<PointT, pcl::Normal> ne;
00181        ne.setInputCloud(cloud);
00182        ne.setNumberOfThreads(8);
00183        pcl::search::KdTree<PointT>::Ptr tree(
00184           new pcl::search::KdTree<PointT> ());
00185        ne.setSearchMethod(tree);
00186        if (ksearch) {
00187           ne.setKSearch(k);
00188        } else {
00189           ne.setRadiusSearch(radius);
00190        }
00191        ne.compute(*s_normal);
00192     }
00193 
00194     void ConvexConnectedVoxels::nearestNeigborSearch(
00195       pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
00196       std::vector<std::vector<int> > &pointIndices,
00197       const int k, const double radius, bool isneigbour)
00198     {
00199       pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
00200       kdtree.setInputCloud(cloud);
00201       std::vector<std::vector<float> > pointSquaredDistance;
00202       for (int i = 0; i < cloud->size(); i++) {
00203          std::vector<int>pointIdx;
00204          std::vector<float> pointSqDist;
00205          pcl::PointXYZ searchPoint = cloud->points[i];
00206          if (isneigbour) {
00207             kdtree.nearestKSearch(searchPoint, k, pointIdx, pointSqDist);
00208          } else {
00209             kdtree.radiusSearch(searchPoint, radius, pointIdx, pointSqDist);
00210          }
00211          pointIndices.push_back(pointIdx);
00212          pointSquaredDistance.push_back(pointSqDist);
00213          pointIdx.clear();
00214          pointSqDist.clear();
00215       }
00216     }
00217 
00218    void ConvexConnectedVoxels::getConvexLabelCloudIndices(
00219        const std::vector<pcl::PointCloud<PointT>::Ptr> &cloud_clusters,
00220        pcl::PointCloud<PointT>::Ptr cloud,
00221        const std::vector<int> &labelMD,
00222        std::map<int, pcl::PointIndices> &all_indices)
00223    {
00224        int icounter = 0;
00225        for (int i = 0; i < cloud_clusters.size(); i++) {
00226           int _idx = labelMD.at(i); 
00227           pcl::PointIndices _ind;
00228           for (int j = 0; j < cloud_clusters[i]->size(); j++) {
00229              _ind.indices.push_back(icounter++);
00230           }
00231           std::map<int, pcl::PointIndices>::iterator
00232              iter = all_indices.find(_idx);
00233           if (iter == all_indices.end()) {
00234              all_indices.insert(
00235                 std::map<int, pcl::PointIndices>::value_type(_idx, _ind));
00236           } else {
00237              pcl::PointIndices prev_ind = (*iter).second;
00238              prev_ind.indices.insert(prev_ind.indices.end(),
00239                                      _ind.indices.begin(),
00240                                      _ind.indices.end());
00241              (*iter).second = prev_ind;
00242           }
00243        }
00244     }
00245 }  // namespace jsk_pcl_ros
00246 
00247 #include <pluginlib/class_list_macros.h>
00248 PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::ConvexConnectedVoxels, nodelet::Nodelet);


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