blur_estimation.cpp
Go to the documentation of this file.
00001 /* 
00002  * Copyright (c) 2010, Florian Zacherl <Florian.Zacherl1860@mytum.de>, Dejan Pangercic <dejan.pangercic@cs.tum.edu>
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Intelligent Autonomous Systems Group/
00014  *       Technische Universitaet Muenchen nor the names of its contributors 
00015  *       may be used to endorse or promote products derived from this software 
00016  *       without specific prior written permission.
00017  * 
00018  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00022  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00028  * POSSIBILITY OF SUCH DAMAGE.
00029  */
00030 
00031 #include <iostream>
00032 #include <math.h>
00033 
00034 #include <opencv/cv.h>
00035 #include <opencv/highgui.h>
00036 #include <ros/ros.h>
00037 #include <sensor_msgs/Image.h>
00038 #include <sensor_msgs/RegionOfInterest.h>
00039 #include <cv_bridge/CvBridge.h>
00040 
00041 #include "ias_projected_light/cp.h"
00042 
00043 /*If TEST_MODE is defined a test image is produced and published instead of the normal one
00044  * If NORMALIZED is defined to, it's the normalized image with non-roi illuminated pixels in green
00045  * Otherwise it shows the vertical derivations
00046  */
00047 
00048 //#define TEST_MODE
00049 //#define NORMALIZED
00050 
00051 using namespace std;
00052 
00053 class BlurEst
00054 {
00055 protected:
00056   ros::NodeHandle nh;
00057   ros::Subscriber sub;
00058   ros::Subscriber sub_roi;
00059   ros::Publisher pub;
00060   ros::ServiceClient cl;
00061   sensor_msgs::CvBridge bridge;
00062 
00063   IplImage* white_image;
00064   IplImage* gray_image;
00065 
00066   string input_topic;
00067   string roi_input_topic;
00068   string output_topic;
00069 
00070   CvRect roi;
00071   bool roi_set;
00072 
00073   float max_blur;
00074   bool blocks;
00075   const static int threshold = 10;
00076 
00077   int pattern_number; //0 = white pattern, 1 = black pattern, 2 = normal pattern
00078 
00079   /*How long to wait while switching through the patterns
00080    * Should be be bigger than publishing delay of the image topic
00081    */
00082   const static int wait_time = 1000;
00083 
00084   void changeROI(const sensor_msgs::RegionOfInterestConstPtr& r)
00085   {
00086     CvRect roi_new;
00087     roi_new.x = r->x_offset;
00088     roi_new.y = r->y_offset;
00089     roi_new.width = r->width;
00090     roi_new.height = r->height;
00091     roi = roi_new;
00092 
00093     roi_set = true;
00094   }
00095 
00096   void detect_sharp_area(const sensor_msgs::ImageConstPtr& ms)
00097   {
00098     if (!roi_set)
00099       return;
00100 
00101 
00102     switch (pattern_number)
00103     {
00104       //Project white image:
00105       case 0:
00106       {
00107         ias_projected_light::cp srv;
00108         srv.request.pattern = 1;
00109         srv.request.block_size = 1;
00110         while (ros::ok() && !cl.call(srv))
00111         {
00112           ROS_INFO("Failed to show white pattern!");
00113           cvWaitKey(50);
00114         }
00115         ROS_INFO("%s pattern ready!", srv.response.name.c_str());
00116         cvWaitKey(wait_time);
00117       }
00118         break;
00119 
00120         //Save white image and project black image:
00121       case 1:
00122       {
00123         IplImage* image = bridge.imgMsgToCv(ms);
00124         white_image = cvCloneImage(image);
00125 
00126         ias_projected_light::cp srv;
00127         srv.request.pattern = 0;
00128         srv.request.block_size = 1;
00129         while (ros::ok() && !cl.call(srv))
00130         {
00131           ROS_INFO("Failed to show black pattern!");
00132           cvWaitKey(50);
00133         }
00134 
00135         ROS_INFO("%s pattern ready!", srv.response.name.c_str());
00136         cvWaitKey(wait_time);
00137       }
00138         break;
00139 
00140         //Save black image, project stripe/block pattern and compute ROI out of black and white image
00141       case 2:
00142       {
00143         IplImage* image = bridge.imgMsgToCv(ms);
00144         gray_image = cvCloneImage(image);
00145 
00146         ias_projected_light::cp srv;
00147         if (blocks)
00148         {
00149           srv.request.pattern = 2;
00150         }
00151         else
00152         {
00153           srv.request.pattern = 3;
00154         }
00155         srv.request.block_size = 40;
00156 
00157         while (ros::ok() && !cl.call(srv))
00158         {
00159           ROS_INFO("Failed to show %s pattern", (blocks ? "block" : "stripe"));
00160           cvWaitKey(50);
00161         }
00162         ROS_INFO("%s pattern ready!", srv.response.name.c_str());
00163 
00164         cvSetImageROI(white_image, roi);
00165         cvSetImageROI(gray_image, roi);
00166       }
00167         break;
00168 
00169         //Blur Estimation:
00170       default:
00171       {
00172         //Original image (without color):
00173         IplImage* image = bridge.imgMsgToCv(ms);
00174 
00175         //Color image to accent Region of Interest and sharp edges in color:
00176         IplImage* col = cvCreateImage(cvSize(image->width, image->height), IPL_DEPTH_8U, 4);
00177         cvCvtColor(image, col, CV_GRAY2BGR);
00178 
00179         cvSetImageROI(image, roi);
00180         cvSetImageROI(col, roi);
00181 
00182         unsigned int width = roi.width;
00183         unsigned int height = roi.height;
00184 
00185         //Normalized image and derivations in both directions:
00186         float norm[height][width];
00187         float derv[height][width];
00188         float hderv[height][width];
00189 
00190         for (unsigned int y = 0; y < height; y++)
00191         {
00192           for (unsigned int x = 0; x < width; x++)
00193           {
00194             //Compute normalized image:
00195             float c = cvGet2D(image, y, x).val[0] - cvGet2D(gray_image, y, x).val[0];
00196             float d = cvGet2D(white_image, y, x).val[0] - cvGet2D(gray_image, y, x).val[0];
00197 
00198             //NaN-value for pixels that are in the ROI but NOT illuminated:
00199             if (d < threshold)
00200             {
00201               norm[y][x] = numeric_limits<float>::quiet_NaN();
00202             }
00203             else
00204             {
00205               norm[y][x] = c / d;
00206 
00207               //Values should be between 0 and 1, all others are measurement errors:
00208               if (norm[y][x] < 0)
00209                 norm[y][x] = 0;
00210               if (norm[y][x] > 1)
00211                 norm[y][x] = 1;
00212             }
00213 
00214             //No derivations possible in the first row
00215             if (y == 0)
00216             {
00217               derv[y][x] = 0;
00218             }
00219             //No dervations for non-illuminated pixels:
00220             else if (isnan(norm[y][x]) || isnan(norm[y - 1][x]))
00221             {
00222               derv[y][x] = 0;
00223             }
00224             else
00225             {
00226               //Compute first derivative in vertical direction
00227               derv[y][x] = abs(norm[y][x] - norm[y - 1][x]);
00228             }
00229             if (blocks) //Block pattern
00230             {
00231               //No derivations possible in the first column
00232               if (x == 0)
00233               {
00234                 hderv[y][x] = 0;
00235               }
00236               //No derivations for non-illuminated pixels:
00237               else if (isnan(norm[y][x]) || isnan(norm[y ][x - 1]))
00238               {
00239                 hderv[y][x] = 0;
00240               }
00241               else
00242               {
00243                 //Compute first derivative in horizontal direction
00244                 hderv[y][x] = abs(norm[y][x] - norm[y][x - 1]);
00245               }
00246 
00247             }
00248           }
00249         }
00250 
00251 #ifdef TEST_MODE
00252         IplImage* test = cvCreateImage(cvSize(width, height), IPL_DEPTH_8U, 3);
00253 
00254         for (unsigned int y = 0; y < height; y++)
00255         {
00256           for (unsigned int x = 0; x < width; x++)
00257           {
00258 #ifdef NORMALIZED
00259             if (isnan(norm[y][x]))
00260             {
00261               cvSet2D(test, y, x, cvScalar(0, 255, 0));
00262             }
00263             else
00264             {
00265               cvSet2D(test, y, x, cvScalar(norm[y][x] * 255, norm[y][x] * 255, norm[y][x] * 255));
00266             }
00267 #else
00268             cvSet2D(test, y, x, cvScalar(derv[y][x] * 255,derv[y][x] * 255,derv[y][x] * 255));
00269 #endif
00270           }
00271         }
00272 #endif
00273 
00274         float blur[height][width];
00275         float hblur[height][width];
00276 
00277         //Blur in vertical direction:
00278         for (unsigned int x = 0; x < width; x++)
00279         {
00280           for (unsigned int y = 0; y < height; y++)
00281           {
00282             blur[y][x] = 1 / (derv[y][x] * sqrt(2 * M_PI));
00283 
00284             //Edge is sharp:
00285             if (blur[y][x] < max_blur)
00286             {
00287               cvSet2D(col, y, x, cvScalar(255, 0, 0, 0));
00288             }
00289           }
00290 
00291           if (blocks)
00292           {
00293             //Blur in horzontal direction:
00294             for (unsigned int x = 0; x < width; x++)
00295             {
00296               for (unsigned int y = 0; y < height; y++)
00297               {
00298                 hblur[y][x] = 1 / (hderv[y][x] * sqrt(2 * M_PI));
00299 
00300                 //Edge is sharp:
00301                 if (hblur[y][x] < max_blur)
00302                   cvSet2D(col, y, x, cvScalar(0, 0, 255, 0));
00303               }
00304             }
00305           }
00306         }
00307 
00308         cvResetImageROI(col);
00309         cvRectangle(col, cvPoint(roi.x, roi.y), cvPoint(roi.x + roi.width, roi.y + roi.height), cvScalar(0, 255, 0), 2);
00310 
00311 #ifdef TEST_MODE
00312         pub.publish(bridge.cvToImgMsg(test));
00313         cvReleaseImage(&test);
00314 #else
00315         pub.publish(bridge.cvToImgMsg(col));
00316 #endif
00317 
00318         ROS_INFO("Image published on topic  %s", output_topic.c_str());
00319 
00320         cvReleaseImage(&col);
00321       }
00322         break;
00323     }
00324 
00325     if (pattern_number < 3)
00326       pattern_number++;
00327 
00328   }
00329 
00330 public:
00331 
00332   BlurEst(ros::NodeHandle& n, float m_blur, bool bls, string input_topic, string roi_topic) :
00333     nh(n), max_blur(m_blur),  input_topic(input_topic), output_topic("/sharp_region"), pattern_number(0),roi_set(false),roi_input_topic(roi_topic), blocks(bls)
00334   {
00335     sub = nh.subscribe(input_topic, 1, &BlurEst::detect_sharp_area, this);
00336     sub_roi = nh.subscribe(roi_input_topic, 1, &BlurEst::changeROI, this);
00337     pub = nh.advertise<sensor_msgs::Image> (output_topic, 1);
00338     cl = nh.serviceClient<ias_projected_light::cp> ("change_pattern");
00339     ROS_INFO("Parameter: %f , %s", max_blur, (blocks ? "true" : "false"));
00340   }
00341 
00342   ~BlurEst()
00343   {
00344     cvReleaseImage(&white_image);
00345     cvReleaseImage(&gray_image);
00346   }
00347 };
00348 
00349 int main(int argc, char** argv)
00350 {
00351   if (argc < 4)
00352   {
00353     ROS_INFO(
00354              "This function expects three parameters:\n\t-Float value for the maximal blur value that should be treated as sharp (e.g. 1.0)\n\t-Bool value that declares if a block or a stripe pattern is used (Zero is stripe pattern)\n\t-Image input topic");
00355   }
00356   else
00357   {
00358     string roi_topic = "stereo/left/roi";
00359     if(argc > 4)
00360     {
00361       roi_topic = argv[4];
00362     }
00363     ros::init(argc, argv, "blur_estimation");
00364     ros::NodeHandle n;
00365     BlurEst be(n, atof(argv[1]), atoi(argv[2]), argv[3], roi_topic);
00366     ros::spin();
00367   }
00368 }
00369 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


ias_projected_light
Author(s): Florian Zacherl, Dejan Pangercic
autogenerated on Sun Oct 6 2013 12:09:47