roi_diff_image.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 <opencv/cv.h>
00032 #include <math.h>
00033 #include <iostream>
00034 #include <opencv/highgui.h>
00035 #include <ros/ros.h>
00036 #include <sensor_msgs/Image.h>
00037 #include <sensor_msgs/RegionOfInterest.h>
00038 #include <cv_bridge/CvBridge.h>
00039 #include "ias_projected_light/cp.h"
00040 
00041 using namespace std;
00042 
00043 float percentage = 0.3; //Percentage of maximal difference that is used to filter out weak reflections
00044 bool use_partitions = false;
00045 bool recompute = false; //If false the roi is only computed once => the same roi is published every time
00046 
00047 int threshold = 2; //Threshold for the equals function for partitioning
00048 
00049 string input_topic;
00050 string output_topic;
00051 
00052 ros::Subscriber sub;
00053 ros::Publisher pub;
00054 ros::ServiceClient cl;
00055 sensor_msgs::CvBridge bridge;
00056 sensor_msgs::RegionOfInterest r;
00057 
00058 IplImage* black;
00059 IplImage* white;
00060 
00061 int pattern = 0;
00062 
00063 //Equality function for partitioning:
00064 bool equals(CvPoint p1, CvPoint p2)
00065 {
00066   return sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y)) < threshold;
00067 }
00068 
00069 void getImageRoi(const sensor_msgs::ImageConstPtr& ms)
00070 {
00071   IplImage* image = bridge.imgMsgToCv(ms);
00072   switch (pattern)
00073   {
00074 
00075 
00076   case 0: //Project black pattern (no pattern)
00077   {
00078     ias_projected_light::cp srv;
00079     srv.request.pattern = 0;
00080     srv.request.block_size = 1;
00081     while (ros::ok() && !cl.call(srv))
00082     {
00083       ROS_INFO("Failed to show black pattern!");
00084       cvWaitKey(50);
00085     }
00086     ROS_INFO("Switched to black image!");
00087     sleep(1);
00088   }
00089     break;
00090 
00091   case 1: //Save image without pattern and project white pattern:
00092   {
00093     black = cvCreateImage(cvSize(image->width, image->height), image->depth, 1);
00094     cvCopy(image, black);
00095     ias_projected_light::cp srv;
00096     srv.request.pattern = 1;
00097     srv.request.block_size = 1;
00098     while (ros::ok() && !cl.call(srv))
00099     {
00100       ROS_INFO("Failed to show white pattern!");
00101 
00102       cvWaitKey(50);
00103     }
00104     ROS_INFO("Switched to white image!");
00105     sleep(1);
00106   }
00107     break;
00108 
00109   default: //Compute the exposure time:
00110   {
00111     if (pattern == 2 || recompute)
00112     {
00113       white = cvCreateImage(cvSize(image->width, image->height), image->depth, 1);
00114       cvCopy(image, white);
00115 
00116 
00117     IplImage* diff = cvCreateImage(cvSize(white->width, white->height), IPL_DEPTH_8U, 1);
00118 
00119     //Get differences between image with black and white pattern:
00120     int max_diff = 0;
00121     for (int x = 0; x < white->width; x++)
00122     {
00123       for (int y = 0; y < white->height; y++)
00124       {
00125         float d = max(0.0, cvGet2D(white, y, x).val[0] - cvGet2D(black, y, x).val[0]);
00126         cvSet2D(diff, y, x, cvScalar(d));
00127 
00128         if (abs(d) > max_diff)
00129           max_diff = abs(d);
00130       }
00131     }
00132 
00133     //Find all points that have a difference bigger then a certain percentage of the maximal difference
00134     vector<CvPoint> pv;
00135     for (int x = 0; x < white->width; x++)
00136     {
00137       for (int y = 0; y < white->height; y++)
00138       {
00139         if (abs(cvGet2D(diff, y, x).val[0]) > max_diff * percentage)
00140         {
00141           CvPoint p;
00142           p.x = x;
00143           p.y = y;
00144           pv.push_back(p);
00145         }
00146       }
00147     }
00148 
00149     cvReleaseImage(&diff);
00150 
00151     vector<int> labels;
00152     int pmax = 0;
00153 
00154     if (use_partitions)
00155     {
00156       //Find all partitions:
00157 
00158       unsigned int partition_count = cv::partition(pv, labels, equals);
00159 
00160       //Find biggest partition:
00161       unsigned int number_of_points[partition_count];
00162       for (unsigned int i = 0; i < partition_count; i++)
00163       {
00164         number_of_points[i] = 0;
00165       }
00166       for (unsigned int i = 0; i < pv.size(); i++)
00167       {
00168         number_of_points[labels[i]]++;
00169       }
00170 
00171       for (unsigned int i = 1; i < partition_count; i++)
00172       {
00173         if (number_of_points[i] > number_of_points[pmax])
00174           pmax = i;
00175       }
00176     }
00177 
00178     CvMemStorage* stor = cvCreateMemStorage(0);
00179     CvSeq* seq = cvCreateSeq(CV_SEQ_ELTYPE_POINT, sizeof(CvSeq), sizeof(CvPoint), stor);
00180 
00181     //Find bounding box:
00182     for (unsigned int i = 0; i < pv.size(); i++)
00183     {
00184       if (!use_partitions || (labels[i] == pmax))
00185       {
00186         cvSeqPush(seq, &pv[i]);
00187       }
00188     }
00189     CvRect roi = cvBoundingRect(seq);
00190 
00191     r.x_offset = roi.x;
00192     r.y_offset = roi.y;
00193     r.height = roi.height;
00194     r.width = roi.width;
00195 
00196     }
00197 
00198     pub.publish(r);
00199   }
00200 
00201   }
00202   if (pattern < 3)
00203     pattern++;
00204 }
00205 
00206 int main(int argc, char** argv)
00207 {
00208   if (argc > 1)
00209   {
00210     percentage = atof(argv[1]);
00211   }
00212   if (argc > 2)
00213   {
00214     use_partitions = atoi(argv[2]);
00215   }
00216   if (argc > 3)
00217   {
00218     recompute = atoi(argv[3]);
00219   }
00220 
00221   ros::init(argc, argv, "find_roi_diff_image");
00222   ros::NodeHandle n;
00223   n.param("input_topic", input_topic, std::string("/stereo/left/image_rect"));
00224   n.param("output_topic", output_topic, std::string("/stereo/left/roi"));
00225   sub = n.subscribe(input_topic, 1, &getImageRoi);
00226   pub = n.advertise<sensor_msgs::RegionOfInterest> (output_topic, 1);
00227   cl = n.serviceClient<ias_projected_light::cp> ("change_pattern");
00228 
00229   ros::spin();
00230 }
00231 
 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