adjust_exposure.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 <ros/ros.h>
00032 #include <unistd.h>
00033 #include <opencv/cv.h>
00034 #include <opencv/highgui.h>
00035 #include <sensor_msgs/Image.h>
00036 #include <sensor_msgs/RegionOfInterest.h>
00037 #include <cv_bridge/CvBridge.h>
00038 #include "ias_projected_light/cp.h"
00039 #include <math.h>
00040 
00041 //#include <find_roi.h>
00042 
00043 using namespace std;
00044 
00045 class AdjExposure
00046 {
00047 private:
00048   ros::NodeHandle n;
00049   ros::Subscriber sub;
00050   ros::Subscriber sub_roi;
00051   ros::ServiceClient cl;
00052 
00053   string prefix;
00054   string input_topic;
00055   string roi_input_topic;
00056 
00057   IplImage* image;
00058   IplImage* non_image; //Image without pattern
00059 
00060   CvRect roi;
00061 
00062   sensor_msgs::CvBridge bridge;
00063 
00064   int exposure_min, exposure_max, exposure;
00065   int mode;
00066 
00067   bool roi_set;
00068 
00069   //This constant equates to the average brightness reached by automatic exposure adjustion:
00070   const static int avg_brightness_auto = 120;
00071 
00072   //Pattern number (as in create_pattern), 4 is the random pattern:
00073   const static int pattern_number = 4;
00074 
00075   //Sets the camera exposure time to the value in exposure
00076   // Because a API for C++ doesn't exist, yet, this has to be done with a system call:
00077   void setExposure()
00078   {
00079     stringstream ss;
00080     ss << "rosrun dynamic_reconfigure dynparam set " << prefix << " exposure " << exposure;
00081     int s = system(ss.str().c_str());
00082     if (s != 0)
00083     {
00084       exit(s);
00085     }
00086 
00087   }
00088 
00089   void changeROI(const sensor_msgs::RegionOfInterestConstPtr& r)
00090   {
00091     CvRect roi_new;
00092     roi_new.x = r->x_offset;
00093     roi_new.y = r->y_offset;
00094     roi_new.width = r->width;
00095     roi_new.height = r->height;
00096     roi = roi_new;
00097 
00098     roi_set = true;
00099   }
00100 
00101   void adjust(const sensor_msgs::ImageConstPtr& ms)
00102   {
00103     if (!roi_set)
00104       return;
00105 
00106     image = bridge.imgMsgToCv(ms);
00107 
00108     switch (mode)
00109     {
00110       case 0: //Project black pattern (no pattern)
00111       {
00112         ias_projected_light::cp srv;
00113         srv.request.pattern = 0;
00114         srv.request.block_size = 1;
00115         while (ros::ok() && !cl.call(srv))
00116         {
00117           ROS_INFO("Failed to show black pattern!");
00118           cvWaitKey(50);
00119         }
00120         ROS_INFO("Switched to black image!");
00121         sleep(1);
00122       }
00123         break;
00124 
00125       case 1: //Save image without pattern and project the real pattern:
00126       {
00127         non_image = cvCreateImage(cvSize(image->width, image->height), image->depth, 1);
00128         cvCopy(image, non_image);
00129         ias_projected_light::cp srv;
00130         srv.request.pattern = pattern_number;
00131         srv.request.block_size = 15;
00132         while (ros::ok() && !cl.call(srv))
00133         {
00134           ROS_INFO("Failed to show pattern!");
00135 
00136           cvWaitKey(50);
00137         }
00138         ROS_INFO("Switched to pattern!");
00139         sleep(1);
00140       }
00141         break;
00142 
00143       default: //Compute the exposure time:
00144       {
00145         int w = roi.width;
00146         int h = roi.height;
00147 
00148         //Compute sum of brightnesses for the whole image and the part that is not ROI, together with
00149         //the average brightness in the ROI:
00150         int avg_image = 0;
00151         int avg_roi = 0;
00152 
00153         for (int i = 0; i < image->width; i++)
00154         {
00155           for (int j = 0; j < image->height; j++)
00156           {
00157             if (i >= roi.x && i <= roi.x + roi.width && j >= roi.y && j <= roi.y + roi.height)
00158             {
00159               avg_roi += cvGet2D(image, j, i).val[0];
00160             }
00161             avg_image += cvGet2D(image, j, i).val[0];
00162           }
00163         }
00164         avg_roi /= (w * h);
00165         avg_image /= (image->width * image->height);
00166 
00167         int avg_brightness_wanted = avg_image + avg_brightness_auto - avg_roi;
00168 
00169         exposure = max(1, avg_brightness_wanted * exposure / avg_image);
00170         setExposure();
00171         ROS_INFO("Wanted Avg: %d Real Avg: %d, Avg ROI (should be %d):, %d", avg_brightness_wanted, avg_image,
00172             avg_brightness_auto, avg_roi);
00173 
00174       }
00175     }
00176     if (mode < 3)
00177       mode++;
00178   }
00179 
00180 public:
00181   AdjExposure(ros::NodeHandle nh) :
00182     n(nh), roi_set(false)
00183   {
00184     prefix = nh.resolveName("prefix");
00185     if (strcmp(prefix.c_str(), "/prefix") == 0)
00186     {
00187       prefix = "/stereo/stereodcam2701/";
00188     }
00189 
00190     input_topic = nh.resolveName("input");
00191     if (strcmp(input_topic.c_str(), "/input") == 0)
00192     {
00193       input_topic = "/stereo/left/image_mono";
00194     }
00195     roi_input_topic = nh.resolveName("roi_input");
00196     if (strcmp(roi_input_topic.c_str(), "/roi_input") == 0)
00197     {
00198       roi_input_topic = "/stereo/left/roi";
00199     }
00200     mode = 0;
00201 
00202     //Get ROS Parameters for camera exposure:
00203     if (!nh.getParam(prefix + "exposure_min", exposure_min))
00204     {
00205       ROS_ERROR("Parameter %sexposure_min not found!", prefix.c_str());
00206       exit(-1);
00207     }
00208     if (!nh.getParam(prefix + "exposure_max", exposure_max))
00209     {
00210       ROS_ERROR("Parameter %sexposure_max not found!", prefix.c_str());
00211       exit(-1);
00212     }
00213     if (!nh.getParam(prefix + "exposure", exposure))
00214     {
00215       ROS_ERROR("Parameter %sexposure not found!", prefix.c_str());
00216       exit(-1);
00217     }
00218 
00219     ROS_INFO("Exposure parameters found!");
00220 
00221     exposure = (exposure_max - exposure_min) / 2; //Set a initial value for exposure time
00222     setExposure();
00223 
00224     sleep(2);
00225 
00226     sub = nh.subscribe(input_topic, 1, &AdjExposure::adjust, this);
00227     sub_roi = nh.subscribe(roi_input_topic, 1, &AdjExposure::changeROI, this);
00228     cl = nh.serviceClient<ias_projected_light::cp> ("change_pattern");
00229 
00230   }
00231 };
00232 
00233 int main(int argc, char** argv)
00234 {
00235   ros::init(argc, argv, "adjust_exposure");
00236   ros::NodeHandle nh;
00237 
00238   AdjExposure a(nh);
00239 
00240   ros::spin();
00241 }
 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