00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00042
00043 #include <pal_vision_segmentation/DisparitySegmentConfig.h>
00044
00045
00046 #include <ros/ros.h>
00047 #include <image_transport/image_transport.h>
00048 #include <cv_bridge/cv_bridge.h>
00049 #include <sensor_msgs/image_encodings.h>
00050 #include <stereo_msgs/DisparityImage.h>
00051 #include <dynamic_reconfigure/server.h>
00052
00053
00054 #include <opencv2/core/core.hpp>
00055 #include <opencv2/imgproc/imgproc.hpp>
00056 #include <opencv2/highgui/highgui.hpp>
00057
00058
00059
00060 image_transport::Publisher mask_pub;
00061 int threshold;
00062 int dilate_iterations;
00063 int dilate_size;
00064 int erode_iterations;
00065 int erode_size;
00066 int scale_factor;
00067 cv::Mat image_rect;
00068 image_transport::CameraPublisher cam_pub;
00069 sensor_msgs::CameraInfo camera_info;
00070 ros::Subscriber cam_info_sub;
00071 cv::Mat mask;
00072
00073
00074 void disparityCb(const stereo_msgs::DisparityImageConstPtr& msg)
00075 {
00076 if(cam_pub.getNumSubscribers() > 0 ||
00077 mask_pub.getNumSubscribers() > 0)
00078 {
00079
00080
00081 if (msg->min_disparity == 0.0 && msg->max_disparity == 0.0)
00082 {
00083 ROS_ERROR("Disparity image fields min_disparity and max_disparity are not set");
00084 return;
00085 }
00086 if (msg->image.encoding != sensor_msgs::image_encodings::TYPE_32FC1)
00087 {
00088 ROS_ERROR("Disparity image must be 32-bit floating point (encoding '32FC1'), but has encoding '%s'",
00089 msg->image.encoding.c_str());
00090 return;
00091 }
00092
00093
00094
00095 float min_disparity = msg->min_disparity;
00096 float max_disparity = msg->max_disparity;
00097 float multiplier = 255.0f / (max_disparity - min_disparity);
00098
00099 const cv::Mat_<float> dmat(msg->image.height, msg->image.width,
00100 (float*)&msg->image.data[0], msg->image.step);
00101 cv::Mat disparity_greyscale;
00102 disparity_greyscale.create(msg->image.height, msg->image.width, CV_8UC1);
00103
00104 for (int row = 0; row < disparity_greyscale.rows; ++row) {
00105 const float* d = dmat[row];
00106 for (int col = 0; col < disparity_greyscale.cols; ++col) {
00107 int index = (d[col] - min_disparity) * multiplier + 0.5;
00108
00109
00110
00111 if(index >= threshold)
00112 disparity_greyscale.at<uchar>(row, col) = 255;
00113 else
00114 disparity_greyscale.at<uchar>(row, col) = 0;
00115 }
00116 }
00117
00118 cv::Mat tmp1, mask;
00119 cv::erode(disparity_greyscale, tmp1,
00120 cv::Mat::ones(erode_size, erode_size, CV_8UC1),
00121 cv::Point(-1, -1), erode_iterations);
00122 cv::dilate(tmp1, mask,
00123 cv::Mat::ones(dilate_size, dilate_size, CV_8UC1),
00124 cv::Point(-1, -1), dilate_iterations);
00125
00126 if(mask_pub.getNumSubscribers() > 0)
00127 {
00128 cv_bridge::CvImage mask_msg;
00129 mask_msg.header = msg->header;
00130 mask_msg.encoding = sensor_msgs::image_encodings::MONO8;
00131 mask_msg.image = mask;
00132 mask_pub.publish(mask_msg.toImageMsg());
00133 }
00134
00135 if(!image_rect.empty() && cam_pub.getNumSubscribers() > 0)
00136 {
00137 cv::Mat masked;
00138 image_rect.copyTo(masked, mask);
00139 cv_bridge::CvImage masked_msg;
00140 masked_msg.header = msg->header;
00141 masked_msg.encoding = sensor_msgs::image_encodings::BGR8;
00142
00143
00144 if(scale_factor > 1)
00145 {
00146 cv::Mat masked_tmp = masked;
00147 cv::resize(masked_tmp, masked,
00148 cv::Size(masked.cols*scale_factor, masked.rows*scale_factor));
00149 }
00150 masked_msg.image = masked;
00151
00152 masked_msg.header.stamp = msg->header.stamp;
00153 camera_info.header.stamp = msg->header.stamp;
00154 cam_pub.publish(*masked_msg.toImageMsg(), camera_info);
00155 }
00156
00157
00158
00159 }
00160 }
00161
00162 void constMaskCb(const stereo_msgs::DisparityImageConstPtr& msg)
00163 {
00164 if(!image_rect.empty())
00165 {
00166 cv::Mat masked;
00167 image_rect.copyTo(masked, mask);
00168 cv_bridge::CvImage masked_msg;
00169 masked_msg.header = msg->header;
00170 masked_msg.encoding = sensor_msgs::image_encodings::BGR8;
00171
00172
00173 if(scale_factor > 1)
00174 {
00175 cv::Mat masked_tmp = masked;
00176 cv::resize(masked_tmp, masked,
00177 cv::Size(masked.cols*scale_factor, masked.rows*scale_factor));
00178 }
00179 masked_msg.image = masked;
00180 cam_pub.publish(*masked_msg.toImageMsg(), camera_info);
00181 }
00182
00183
00184
00185 }
00186
00187 void imageCb(const sensor_msgs::ImageConstPtr& msg)
00188 {
00189 cv_bridge::CvImagePtr cv_ptr;
00190 try
00191 {
00192 cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
00193 }
00194 catch (cv_bridge::Exception& e)
00195 {
00196 ROS_ERROR("cv_bridge exception: %s", e.what());
00197 return;
00198 }
00199
00200 image_rect = cv_ptr->image;
00201 }
00202
00203 void reconf_callback(pal_vision_segmentation::DisparitySegmentConfig &config, uint32_t level)
00204 {
00205 threshold = config.threshold;
00206 dilate_iterations = config.dilate_iterations;
00207 dilate_size = config.dilate_size;
00208 erode_iterations = config.erode_iterations;
00209 erode_size = config.erode_size;
00210 }
00211
00212 void camInfoCallback(const sensor_msgs::CameraInfo &msg)
00213 {
00214 camera_info = msg;
00215
00216 for(unsigned int i=0; i<camera_info.D.size(); ++i)
00217 camera_info.D[i] = 0.0;
00218
00219
00220 if(scale_factor > 1)
00221 {
00222 camera_info.width *= scale_factor;
00223 camera_info.height *= scale_factor;
00224 for(int i=0; i<12; i++)
00225 camera_info.P[i] *= scale_factor;
00226 camera_info.P[10] = 1.0;
00227 }
00228
00229
00230 for(int i=0; i<3; ++i)
00231 for(int j=0; j<3; ++j)
00232 camera_info.K[i*3+j] = camera_info.P[i*4+j];
00233 }
00234
00235 int main(int argc, char *argv[] )
00236 {
00237 ros::init(argc, argv, "disparity_segmentation");
00238 ros::NodeHandle nh("disparity_segmentation");
00239 nh.param<int>("threshold", threshold, 89);
00240 nh.param<int>("dilate_iterations", dilate_iterations, 9);
00241 nh.param<int>("dilate_size", dilate_size, 7);
00242 nh.param<int>("erode_iterations", erode_iterations, 1);
00243 nh.param<int>("erode_size", erode_size, 3);
00244 nh.param<int>("scale_factor", scale_factor, 1);
00245
00246 std::string mask_path;
00247 nh.param<std::string>("mask_path", mask_path, "");
00248
00249 image_transport::ImageTransport it(nh);
00250 ros::Subscriber disp_sub;
00251 if(mask_path == "")
00252 {
00253 disp_sub = nh.subscribe<stereo_msgs::DisparityImage>("/disparity_image", 1, &disparityCb);
00254 } else {
00255 mask = cv::imread(mask_path);
00256 disp_sub = nh.subscribe<stereo_msgs::DisparityImage>("/disparity_image", 1, &constMaskCb);
00257 }
00258 image_transport::Subscriber rect_sub = it.subscribe("/image", 1, &imageCb);
00259 cam_pub = it.advertiseCamera("image_masked", 1);
00260 cam_info_sub = nh.subscribe("/camera_info", 1, &camInfoCallback);
00261 mask_pub = it.advertise("mask", 1);
00262
00263 dynamic_reconfigure::Server<pal_vision_segmentation::DisparitySegmentConfig> server;
00264 dynamic_reconfigure::Server<pal_vision_segmentation::DisparitySegmentConfig>::CallbackType f;
00265 f = boost::bind(&reconf_callback, _1, _2);
00266 server.setCallback(f);
00267
00268 ros::spin();
00269 }
00270