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 #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
00044
00045
00046
00047
00048
00049
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;
00078
00079
00080
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
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
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
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
00170 default:
00171 {
00172
00173 IplImage* image = bridge.imgMsgToCv(ms);
00174
00175
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
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
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
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
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
00215 if (y == 0)
00216 {
00217 derv[y][x] = 0;
00218 }
00219
00220 else if (isnan(norm[y][x]) || isnan(norm[y - 1][x]))
00221 {
00222 derv[y][x] = 0;
00223 }
00224 else
00225 {
00226
00227 derv[y][x] = abs(norm[y][x] - norm[y - 1][x]);
00228 }
00229 if (blocks)
00230 {
00231
00232 if (x == 0)
00233 {
00234 hderv[y][x] = 0;
00235 }
00236
00237 else if (isnan(norm[y][x]) || isnan(norm[y ][x - 1]))
00238 {
00239 hderv[y][x] = 0;
00240 }
00241 else
00242 {
00243
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
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
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
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
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