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
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
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
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
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
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
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
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
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
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 }