cob_3d_segmentation: simple_segmentation

Go to the documentation of this file.
00001 
00063 #include <sstream>
00064 #include <ros/ros.h>
00065 #include <pluginlib/class_list_macros.h>
00066 #include <eigen_conversions/eigen_msg.h>
00067 #include <pcl/common/transforms.h>
00068 #include <pcl/filters/extract_indices.h>
00069 #include <pcl/surface/organized_fast_mesh.h>
00070 #include <pcl/PolygonMesh.h>
00071 #include <pcl/io/ply_io.h>
00072 #include <pcl/io/vtk_io.h>
00073 #include <pcl/io/pcd_io.h>
00074 #include <pcl_conversions/pcl_conversions.h>
00075 #include <opencv2/opencv.hpp>
00076 
00077 #include <cob_3d_mapping_msgs/ShapeArray.h>
00078 #include <cob_3d_mapping_common/stop_watch.h>
00079 #include <cob_3d_mapping_common/ros_msg_conversions.h>
00080 #include "cob_3d_segmentation/simple_segmentation_nodelet.h"
00081 #include "cob_3d_mapping_filters/downsample_filter.h"
00082 
00083 void
00084 cob_3d_segmentation::SimpleSegmentationNodelet::onInit()
00085 {
00086   //PCLNodelet::onInit();
00087   nh_ = getNodeHandle();
00088   config_server_.reset(
00089     new dynamic_reconfigure::Server<segmentation_nodeletConfig>(
00090       getPrivateNodeHandle()));
00091   config_server_->setCallback(
00092     boost::bind(&SimpleSegmentationNodelet::configCallback, this, _1, _2));
00093 
00094   one_.setOutputLabels(labels_);
00095   one_.setPixelSearchRadius(8,2,2);
00096   one_.setSkipDistantPointThreshold(8);
00097 
00098   seg_.setNormalCloudIn(normals_);
00099   seg_.setLabelCloudInOut(labels_);
00100   seg_.setSeedMethod(SEED_RANDOM);
00101 
00102   pub_segmented_ = nh_.advertise<PointCloud>("segmented_cloud", 1);
00103   pub_classified_ = nh_.advertise<PointCloud>("classified_cloud", 1);
00104   pub_shape_array_ = nh_.advertise<cob_3d_mapping_msgs::ShapeArray>("shape_array",1);
00105   as_ = new actionlib::SimpleActionServer<cob_3d_mapping_msgs::TriggerAction>(
00106     nh_, "segmentation/trigger",
00107     boost::bind(&SimpleSegmentationNodelet::actionCallback, this, _1), false);
00108   as_->start();
00109 }
00110 
00111 
00112 void
00113 cob_3d_segmentation::SimpleSegmentationNodelet::configCallback(
00114   cob_3d_segmentation::segmentation_nodeletConfig& config,
00115     uint32_t level)
00116 {
00117   NODELET_INFO("[segmentation]: received new parameters");
00118   centroid_passthrough_ = config.centroid_passthrough;
00119   filter_ = config.filter;
00120   min_cluster_size_ = config.min_cluster_size;
00121   downsample_ = config.downsample;
00122   colorize_ = config.colorize;
00123   enable_action_mode_ = config.enable_action_mode;
00124   if(!is_running_ && !enable_action_mode_)
00125   { // Start immediately
00126     ROS_INFO("Starting segmentation...");
00127     sub_points_ = nh_.subscribe("point_cloud", 1, &SimpleSegmentationNodelet::topicCallback, this);
00128     is_running_ = true;
00129   }
00130 }
00131 
00132 void
00133 cob_3d_segmentation::SimpleSegmentationNodelet::actionCallback(
00134   const cob_3d_mapping_msgs::TriggerGoalConstPtr& goal)
00135 {
00136   //boost::lock_guard<boost::mutex> guard(mutex_);
00137   cob_3d_mapping_msgs::TriggerResult result;
00138   if(goal->start && !is_running_)
00139   {
00140     ROS_INFO("Starting segmentation...");
00141     sub_points_ = nh_.subscribe("point_cloud", 1, &SimpleSegmentationNodelet::topicCallback, this);
00142     is_running_ = true;
00143   }
00144   else if(!goal->start && is_running_)
00145   {
00146     ROS_INFO("Stopping segmentation...");
00147     sub_points_.shutdown();
00148     is_running_ = false;
00149   }
00150 
00151   as_->setSucceeded(result);
00152 }
00153 
00154 void
00155 cob_3d_segmentation::SimpleSegmentationNodelet::topicCallback(
00156   const PointCloud::ConstPtr& cloud)
00157 {
00158   //boost::lock_guard<boost::mutex> guard(mutex_);
00159   PrecisionStopWatch t;
00160   NODELET_INFO("Received PointCloud. Start downsampling .... ");
00161 
00162   //t.precisionStart();
00163   if(downsample_)
00164   {
00165     cob_3d_mapping_filters::DownsampleFilter<pcl::PointXYZRGB> down;
00166     down_->header = cloud->header;
00167     down.setInputCloud(cloud);
00168     down.filter(*down_);
00169     *segmented_ = *down_;
00170   }
00171   else
00172   {
00173     *segmented_ = *down_ = *cloud;
00174   }
00175   //std::cout << "Downsampling took " << t.precisionStop() << " s." << std::endl;
00176   computeAndPublish();
00177 }
00178 
00179 void
00180 cob_3d_segmentation::SimpleSegmentationNodelet::computeAndPublish()
00181 {
00182   PrecisionStopWatch t, t2;
00183   t2.precisionStart();
00184   NODELET_INFO("Start with segmentation .... ");
00185   one_.setInputCloud(down_);
00186   one_.compute(*normals_);
00187 
00188   seg_.setInputCloud(down_);
00189   //t.precisionStart();
00190   seg_.compute();
00191   //std::cout << "segmentation took " << t.precisionStop() << " s." << std::endl;
00192   seg_.mapSegmentColor(segmented_);
00193 
00194   pub_segmented_.publish(segmented_);
00195 
00196   // --- start shape array publisher: ---
00197   PointCloud::Ptr hull(new PointCloud);
00198   cob_3d_mapping_msgs::ShapeArray sa;
00199   pcl_conversions::fromPCL(down_->header, sa.header);
00200   //sa.header = down_->header;
00201   //sa.header.frame_id = down_->header.frame_id.c_str();
00202   //unsigned int id = 0;
00203   for (ClusterPtr c = seg_.clusters()->begin(); c != seg_.clusters()->end(); ++c)
00204   {
00205     if(c->size() < min_cluster_size_) {continue;} //segment too small
00206     if(c->getCentroid()(2) > centroid_passthrough_) {continue;} //segment too far away
00207     if(c->border_points.size() > ceil(0.9f * (float)c->size()))  {continue;} //ratio of border points too high => long and thin segment
00208 
00209     seg_.clusters()->computeClusterComponents(c);
00210     if(filter_ && !c->is_planar_) {continue;} //segment is not a plane
00211 
00212     cob_3d_mapping::Polygon::Ptr p;
00213     //std::cout << "cluster id " << c->id() << std::endl;
00214     conv_.setColor(colorize_);
00215     conv_.setInputCloud(down_);
00216     if(!conv_.clusterToPolygon(c, p)) continue;
00217 
00218     sa.shapes.push_back(cob_3d_mapping_msgs::Shape());
00219     cob_3d_mapping_msgs::Shape* s = &sa.shapes.back();
00220     pcl_conversions::fromPCL(down_->header, s->header);
00221     //s->header = down_->header;
00222 
00223     //computeTexture(c, p->pose_, p->id_);
00224     cob_3d_mapping::toROSMsg(*p, *s);
00225     //id++;
00226   }
00227 
00228   pub_shape_array_.publish(sa);
00229   publishClassifiedCloud();
00230   std::cout << "total nodelet time: " << t2.precisionStop() << std::endl;
00231 }
00232 
00233 
00234 void
00235 cob_3d_segmentation::SimpleSegmentationNodelet::publishClassifiedCloud()
00236 {
00237   seg_.clusters()->mapTypeColor(down_);
00238   seg_.clusters()->mapClusterBorders(down_);
00239 //  for (ClusterPtr c = seg_.clusters()->begin(); c != seg_.clusters()->end(); ++c)
00240 //  {
00241 //    Eigen::Vector3f centroid = c->getCentroid();
00242 //    //if(centroid(2) > centroid_passthrough_) {continue;}
00243 //    //seg_.clusters()->computeClusterComponents(c);
00244 //    uint32_t color = LBL_BORDER;
00245 //    // not a plane
00246 //    //if(/*c->size() < min_cluster_size_ || c->size() <= ceil(1.1f * (float)c->border_points.size()) ||*/ (filter_ && !c->is_save_plane))
00247 //    /*{
00248 //      color = LBL_SPH;
00249 //    }
00250 //    else
00251 //    {
00252 //      color = LBL_PLANE;
00253 //    }*/
00254 //    if(c->id() == I_NAN) color = LBL_BORDER;
00255 //    /*for(unsigned int i = 0; i < c->indices_.size(); i++)
00256 //    {
00257 //      down_->points[c->indices_[i]].rgb = color;
00258 //    }*/
00259 //    PolygonContours<PolygonPoint> poly;
00260 //    pe_.outline(down_->width, down_->height, c->border_points, poly);
00261 //    for (int i = 0; i < (int)poly.polys_.size(); ++i)
00262 //    {
00263 //      std::vector<PolygonPoint>::iterator it = poly.polys_[i].begin();
00264 //      for ( ; it != poly.polys_[i].end(); ++it)
00265 //        down_->points[ it->x + it->y * down_->width ].rgb = LBL_BORDER;;
00266 //    }
00267 //    for(std::vector<PolygonPoint>::iterator bp = c->border_points.begin(); bp != c->border_points.end(); ++bp)
00268 //    {
00269 //      down_->points[PolygonPoint::getInd(bp->x,bp->y)].rgb = LBL_BORDER;
00270 //    }
00271 //    /*for(unsigned int i = 0; i < c->border_points.size(); i++)
00272 //    {
00273 //      down_->points[PolygonPoint::getInd(c->border_points[i].x,c->border_points[i].y)].r = 255;
00274 //      down_->points[PolygonPoint::getInd(c->border_points[i].x,c->border_points[i].y)].g = 0;
00275 //      down_->points[PolygonPoint::getInd(c->border_points[i].x,c->border_points[i].y)].b = 0;
00276 //    }*/
00277 //  }
00278   pub_classified_.publish(down_);
00279 }
00280 
00281 void
00282 cob_3d_segmentation::SimpleSegmentationNodelet::computeTexture(
00283   ClusterPtr &c, Eigen::Affine3f &trf, unsigned int id)
00284 {
00285   pcl::ExtractIndices<pcl::PointXYZRGB> ei;
00286   ei.setInputCloud(down_);
00287   pcl::IndicesPtr ind_ptr(new std::vector<int>);
00288   for(unsigned int i=0; i<c->indices_.size(); i++)
00289     ind_ptr->push_back(c->indices_[i]);
00290   ei.setIndices(ind_ptr);
00291   PointCloud::Ptr segment(new PointCloud);
00292   ei.filter(*segment);
00293   PointCloud::Ptr segment_tr(new PointCloud);
00294   pcl::transformPointCloud(*segment, *segment_tr, trf.inverse());
00295   std::stringstream ss;
00296   ss << "/tmp/seg_" << id << ".pcd";
00297   pcl::io::savePCDFileBinary(ss.str(), *segment_tr);
00298   double mToPx = 500;
00299   int min_u = std::numeric_limits<int>::max(), max_u = std::numeric_limits<int>::min(), min_v = std::numeric_limits<int>::max(), max_v  = std::numeric_limits<int>::min();
00300   for (unsigned int i=0; i<segment_tr->size(); i++)
00301   {
00302     int u = round(segment_tr->points[i].x * mToPx);
00303     int v = round(segment_tr->points[i].y * mToPx);
00304     if(u < min_u) min_u = u;
00305     if(u > max_u) max_u = u;
00306     if(v < min_v) min_v = v;
00307     if(v > max_v) max_v = v;
00308     //std::cout << segment_tr->points[i].x << "," << segment_tr->points[i].y << ":" << u << "," << v << std::endl;
00309   }
00310   //TODO: save min/max u v for transforming back
00311   //std::cout << "u(min, max): " << min_u << "," << max_u << " v(min,max): " << min_v << "," << max_v << std::endl;
00312   cv::Mat img(abs(max_u - min_u) +1 , abs(max_v - min_v) + 1, CV_8UC3, cv::Scalar(-1));
00313   //std::cout << "im size: " << img.rows << "," << img.cols << std::endl;
00314   for (unsigned int i=0; i<segment_tr->size(); i++)
00315   {
00316     //TODO: handle case that both u_min and u-Max are negative (same for v)
00317     int u = round(segment_tr->points[i].x * mToPx) - min_u;
00318     int v = round(segment_tr->points[i].y * mToPx) - min_v;
00319     //std::cout << "u,v" << u << "," << v << std::endl;
00320     img.at<cv::Vec3b>(u,v)[2] = segment_tr->points[i].r;
00321     img.at<cv::Vec3b>(u,v)[1] = segment_tr->points[i].g;
00322     img.at<cv::Vec3b>(u,v)[0] = segment_tr->points[i].b;
00323   }
00324   std::stringstream ss1;
00325   ss1 << "/tmp/seg_" << id << ".png";
00326   cv::imwrite(ss1.str(), img);
00327 
00328   cv::Mat img_dil(abs(max_u - min_u) +1 , abs(max_v - min_v) + 1, CV_8UC3);
00329   cv::dilate(img, img_dil, cv::Mat(), cv::Point(-1,-1), 2);
00330   std::stringstream ss2;
00331   ss2 << "/tmp/seg_" << id << "_dil.png";
00332   cv::imwrite(ss2.str(), img_dil);
00333 
00334   PointCloud::Ptr segm(new PointCloud);
00335   //pcl::copyPointCloud(*down_, copy);
00336   pcl::ExtractIndices<pcl::PointXYZRGB> ei2;
00337   ei2.setInputCloud(down_);
00338   ei2.setIndices(ind_ptr);
00339   ei2.setKeepOrganized(true);
00340   ei2.filter(*segm);
00341 
00342   pcl::OrganizedFastMesh<pcl::PointXYZRGB> ofm;
00343   pcl::PolygonMesh pcl_mesh;
00344   ofm.setInputCloud(segm);
00345   //ofm.setIndices(ind_ptr);
00346   ofm.setTriangulationType(pcl::OrganizedFastMesh<pcl::PointXYZRGB>::TRIANGLE_ADAPTIVE_CUT);
00347   ofm.reconstruct(pcl_mesh);
00348   /*try
00349   {
00350     std::stringstream ss3;
00351     ss3 << "/tmp/mesh_" << id << ".ply";
00352     //pcl::io::saveVTKFile(ss3.str(), pcl_mesh);
00353     pcl::io::savePLYFile(ss3.str(), pcl_mesh);
00354   }
00355   catch( std::exception& x )
00356   {
00357     std::cerr << x.what() << std::endl;
00358     return;
00359   }*/
00360 
00361 }
00362 
00363 PLUGINLIB_DECLARE_CLASS(cob_3d_segmentation, SimpleSegmentationNodelet,
00364                         cob_3d_segmentation::SimpleSegmentationNodelet,
00365                         nodelet::Nodelet);