superpixel_smoother_node.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <sensor_msgs/Image.h>
00003 #include <image_transport/image_transport.h>
00004 #include <cv_bridge/CvBridge.h>
00005 #include "opencv2/highgui/highgui.hpp"
00006 #include <string>
00007 #include <vector>
00008 
00009 #include <cpl_superpixels/segment/segment.h>
00010 #include <cpl_superpixels/SmoothClutter.h>
00011 
00012 using cpl_superpixels::SmoothClutter;
00013 // using cpl_superpixels::getSuperpixelImage;
00014 
00015 class SuperpixelSmootherNode
00016 {
00017  public:
00018   SuperpixelSmootherNode(ros::NodeHandle n): n_(n), it_(n), sigma_(0.3),
00019                                              k_(500), min_size_(10)
00020 
00021   {
00022     ros::NodeHandle n_private("~");
00023     n_private.param("superpixel_sigma", sigma_, 0.3);
00024     n_private.param("superpixel_k", k_, 500.0);
00025     n_private.param("min_patch_size", min_size_, 10);
00026 
00027     image_sub_ = it_.subscribe("image_topic", 1,
00028                                &SuperpixelSmootherNode::imageCallback, this);
00029     label_pub_ = it_.advertise("sp_labeled_image", 1);
00030 
00031     // Setup service server
00032     smoother_srv_ = n_.advertiseService(
00033         "smooth_clutter", &SuperpixelSmootherNode::smoothClutterService, this);
00034 
00035   }
00036 
00037   cv::Mat smoothClutter(cv::Mat& sp_img, cv::Mat& label_img, int num_regions)
00038   {
00039     ROS_INFO("Creating raw label array.");
00040     // Vector to hold the label results per region index
00041     std::vector<std::vector<int> > raw_region_labels;
00042     for (int i = 0; i < num_regions; ++i)
00043     {
00044       std::vector<int> labels;
00045       raw_region_labels.push_back(labels);
00046     }
00047     ROS_INFO("Populating raw label array.");
00048     ROS_INFO_STREAM("Num indexes: " << raw_region_labels.size());
00049 
00050     // Accumulate the pixel labels for each superpixel
00051     for (int r = 0; r < sp_img.rows; ++r)
00052     {
00053       for (int c = 0; c < sp_img.cols; ++c)
00054       {
00055         int idx = static_cast<int>(sp_img.at<float>(r,c));
00056         raw_region_labels[idx].push_back(label_img.at<uchar>(r,c));
00057       }
00058     }
00059 
00060     // Perform the majority vote for each region
00061     const int SURFACE_LABEL_VAL = 120;
00062     const int CLUTTER_LABEL_VAL = 255;
00063     const int UNLABELLED_VAL = 0;
00064 
00065     ROS_INFO("Calculating majority votes.");
00066     std::vector<float> maj_region_labels;
00067     for(unsigned int i = 0; i < raw_region_labels.size(); ++i)
00068     {
00069       std::vector<int> labels = raw_region_labels[i];
00070       unsigned int unlabelled_count = 0;
00071       unsigned int clutter_count = 0;
00072       unsigned int surface_count = 0;
00073 
00074       for (unsigned int j = 0; j < labels.size(); ++j)
00075       {
00076         if (labels[j] == SURFACE_LABEL_VAL)
00077         {
00078           surface_count++;
00079         }
00080         else if (labels[j] == CLUTTER_LABEL_VAL)
00081         {
00082           clutter_count++;
00083         }
00084         else
00085         {
00086           unlabelled_count++;
00087         }
00088       }
00089       // TODO: Maybe change how we deal with unlabelled stuff
00090       if (unlabelled_count > clutter_count &&
00091           unlabelled_count > surface_count)
00092       {
00093         maj_region_labels.push_back(UNLABELLED_VAL);
00094       }
00095       else if ( clutter_count >= surface_count)
00096       {
00097         maj_region_labels.push_back(CLUTTER_LABEL_VAL);
00098       }
00099       else
00100       {
00101         maj_region_labels.push_back(SURFACE_LABEL_VAL);
00102       }
00103     }
00104 
00105     // Propogate superpixel majority vote values to all members of the region
00106     ROS_INFO("Propogatting results.");
00107     cv::Mat smooth_labels(label_img.size(), label_img.type());
00108     for (int r = 0; r < sp_img.rows; ++r)
00109     {
00110       for (int c = 0; c < sp_img.cols; ++c)
00111       {
00112         int idx = static_cast<int>(sp_img.at<float>(r,c));
00113         smooth_labels.at<uchar>(r,c) = maj_region_labels[idx];
00114       }
00115     }
00116     return smooth_labels;
00117   }
00118 
00119   //
00120   // ROS Stuff
00121   //
00122 
00123   bool smoothClutterService(SmoothClutter::Request &req,
00124                             SmoothClutter::Response &res)
00125   {
00126     sensor_msgs::ImageConstPtr raw_img_msg =
00127         boost::shared_ptr<sensor_msgs::Image const>(
00128             new sensor_msgs::Image(req.raw_img));
00129     sensor_msgs::ImageConstPtr label_img_msg =
00130         boost::shared_ptr<sensor_msgs::Image const>(
00131             new sensor_msgs::Image(req.clutter_labels));
00132     cv::Mat raw_img = bridge_.imgMsgToCv(raw_img_msg);
00133     cv::Mat label_img = bridge_.imgMsgToCv(label_img_msg);
00134     int num_regions = 0;
00135     cv::Mat sp_img = cpl_superpixels::getSuperpixelImage(raw_img, num_regions,
00136                                                          sigma_, k_, min_size_);
00137     ROS_INFO("Performing majority vote.");
00138     cv::Mat smooth_labels = smoothClutter(sp_img, label_img, num_regions);
00139 
00140     ROS_INFO("Converting result for transport.");
00141     IplImage smooth_labels_ipl = smooth_labels;
00142     sensor_msgs::Image::ConstPtr label_out = bridge_.cvToImgMsg(&smooth_labels_ipl);
00143     res.smooth_clutter_labels = *label_out;
00144     return true;
00145   }
00146 
00153   void imageCallback(const sensor_msgs::ImageConstPtr& msg_ptr)
00154   {
00155     cv::Mat input_img;
00156     input_img = bridge_.imgMsgToCv(msg_ptr);
00157 
00158     int num_regions = 0;
00159     cv::Mat label_img = cpl_superpixels::getSuperpixelImage(input_img,
00160                                                             num_regions, sigma_,
00161                                                             k_, min_size_);
00162 
00163     // Publish the label data
00164     sensor_msgs::ImageConstPtr label_msg;
00165     IplImage label_ipl = label_img;
00166     label_msg = bridge_.cvToImgMsg(&label_ipl);
00167     label_pub_.publish(label_msg);
00168   }
00169 
00173   void spin()
00174   {
00175     while(n_.ok())
00176     {
00177       ros::spinOnce();
00178     }
00179   }
00180 
00181  protected:
00182   ros::NodeHandle n_;
00183   ros::ServiceServer smoother_srv_;
00184   image_transport::ImageTransport it_;
00185   image_transport::Subscriber image_sub_;
00186   image_transport::Publisher label_pub_;
00187   sensor_msgs::CvBridge bridge_;
00188   double sigma_;
00189   double k_;
00190   int min_size_;
00191 };
00192 
00193 int main(int argc, char** argv)
00194 {
00195   ros::init(argc, argv, "");
00196   ros::NodeHandle n;
00197   SuperpixelSmootherNode smoother_node(n);
00198   smoother_node.spin();
00199 }


cpl_superpixels
Author(s): Tucker Hermans
autogenerated on Wed Nov 27 2013 11:38:57