disparity_segmentation.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (Modified BSD License)
00003  *
00004  *  Copyright (c) 2012, PAL Robotics, S.L.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of PAL Robotics, S.L. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00042 // PAL headers
00043 #include <pal_vision_segmentation/DisparitySegmentConfig.h>
00044 
00045 // ROS headers
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 // OpenCV headers
00054 #include <opencv2/core/core.hpp>
00055 #include <opencv2/imgproc/imgproc.hpp>
00056 #include <opencv2/highgui/highgui.hpp>
00057 
00058 
00059 /***Variables used in callbacks***/
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 /***end of callback section***/
00073 
00074 void disparityCb(const stereo_msgs::DisparityImageConstPtr& msg)
00075 {
00076     if(cam_pub.getNumSubscribers() > 0 ||
00077        mask_pub.getNumSubscribers() > 0)
00078     {
00079         //    double ticksBefore = cv::getTickCount();
00080         // Check for common errors in input
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         // code taken from image_view / disparity_view
00094         // Colormap and display the disparity image
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                 //index = std::min(255, std::max(0, index));
00110                 // pushing it into the interval does not matter because of the threshold afterwards
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             //if we want rescale then rescale
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             // to provide a synchronized output we publish both topics with the same timestamp
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         //    ROS_INFO("disparityCb runtime: %f ms",
00158         //             1000*(cv::getTickCount() - ticksBefore)/cv::getTickFrequency());
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         //if we want rescale then rescale
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     //    ROS_INFO("disparityCb runtime: %f ms",
00184     //             1000*(cv::getTickCount() - ticksBefore)/cv::getTickFrequency());
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     // the image is rectified so by default there's no distortion
00216     for(unsigned int i=0; i<camera_info.D.size(); ++i)
00217         camera_info.D[i] = 0.0;
00218 
00219     //rescale the parameters if the image is rescaled
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     // Copy the top 3x3 matrix of P into K
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 


pal_vision_segmentation
Author(s): Bence Magyar
autogenerated on Fri Aug 28 2015 11:57:00