Robot Simulator of the Robotics Group for Self-Organization of Control: imageprocessors.h Source File

# imageprocessors.h

Go to the documentation of this file.

    00001 /***************************************************************************
    00002  *   Copyright (C) 2005-2011 LpzRobots development team                    *
    00003  *    Georg Martius  <georg dot martius at web dot de>                     *
    00004  *    Frank Guettler <guettler at informatik dot uni-leipzig dot de        *
    00005  *    Frank Hesse    <frank at nld dot ds dot mpg dot de>                  *
    00006  *    Ralf Der       <ralfder at mis dot mpg dot de>                       *
    00007  *                                                                         *
    00008  *   This program is free software; you can redistribute it and/or modify  *
    00009  *   it under the terms of the GNU General Public License as published by  *
    00010  *   the Free Software Foundation; either version 2 of the License, or     *
    00011  *   (at your option) any later version.                                   *
    00012  *                                                                         *
    00013  *   This program is distributed in the hope that it will be useful,       *
    00014  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
    00015  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
    00016  *   GNU General Public License for more details.                          *
    00017  *                                                                         *
    00018  *   You should have received a copy of the GNU General Public License     *
    00019  *   along with this program; if not, write to the                         *
    00020  *   Free Software Foundation, Inc.,                                       *
    00021  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
    00022  *                                                                         *
    00023  ***************************************************************************/
    00024 #ifndef __IMAGEPROCESSORS
    00025 #define __IMAGEPROCESSORS
    00026
    00027 #include "imageprocessor.h"
    00028
    00029 #include <selforg/stl_adds.h>
    00030
    00031 #define MIN3(x,y,z) x<y ? (x<z ? x : z) : (y<z ? y : z)
    00032 #define MAX3(x,y,z) x>y ? (x>z ? x : z) : (y>z ? y : z)
    00033
    00034 namespace lpzrobots {
    00035
    00036   /** Standard image processor - convenience class for 1 to 1 image processing.
    00037       The last image of the stack is the source (output of last processor).
    00038       Simpler to implement than ImageProcessor.
    00039       @param show whether do display the resulting image on the screen
    00040       @param scale how to scale the display
    00041       @see ImageProcessor
    00042    */
    00043   struct StdImageProcessor : public ImageProcessor {
    00044     StdImageProcessor(bool show, float scale) {
    00045       _dest.show  = show;
    00046       _dest.scale = scale;
    00047     };
    00048     virtual ~StdImageProcessor() {
    00049     };
    00050
    00051     /// overload this function and initialise the dest.img and the dest.name
    00052     virtual void initDestImage(Camera::CameraImage& dest, const Camera::CameraImage& src) = 0;
    00053
    00054     /// overload this function and do processing here
    00055     virtual void process(const osg::Image* src, osg::Image* dest) = 0;
    00056
    00057     /*   substituded generic interface */
    00058     virtual Camera::CameraImage init(const Camera::CameraImages& imgs){
    00059       assert(imgs.size()>0);
    00060       _src = imgs.back();
    00061       // printf("source picture: %s, %i x %i\n",src.name.c_str(), src.img->s(), src.img->t());
    00062       _dest.img = new osg::Image;
    00063       initDestImage(_dest, _src);
    00064       return _dest;
    00065     }
    00066
    00067     virtual void process(){
    00068       process(_src.img, _dest.img);
    00069       _dest.img->dirty();
    00070     }
    00071
    00072   protected:
    00073     Camera::CameraImage _dest;
    00074     Camera::CameraImage _src;
    00075   };
    00076
    00077
    00078
    00079   /// black and white image @see StdImageProcessor
    00080   struct BWImageProcessor : public StdImageProcessor {
    00081     enum ChannelMask {Red = 1, Green = 2, Blue = 4, Hue = 1, Saturation = 2, Value = 4};
    00082
    00083     /// @param channelmask which channels to consider, @see BWImageProcessor::ChannelMask
    00084     BWImageProcessor(bool show, float scale, char channelmask = 7)
    00085       : StdImageProcessor(show,scale), channelmask(channelmask) {}
    00086
    00087     virtual ~BWImageProcessor() {}
    00088
    00089     virtual void initDestImage(Camera::CameraImage& dest, const Camera::CameraImage& src){
    00090       dest.img->allocateImage(src.img->s(), src.img->t(), 1, GL_LUMINANCE, GL_UNSIGNED_BYTE);
    00091       dest.name  = "bw(" + src.name + ")";
    00092       red   = (channelmask & 1);
    00093       green = (channelmask & 2);
    00094       blue  = (channelmask & 4);
    00095       numchannels = red+green+blue;
    00096       printf("BWImageProcessor: Select: Red %i, Green %i, Blue %i : numchannels %i\n",
    00097              red, green, blue,numchannels);
    00098       if(numchannels==0) numchannels=1; // avoid division by 0
    00099     }
    00100
    00101     virtual void process(const osg::Image* src, osg::Image* dest){
    00102       assert(src && src->getPixelFormat()==GL_RGB && src->getDataType()==GL_UNSIGNED_BYTE);
    00103       for(int r=0; r < src->t(); ++r) {
    00104         const unsigned char* sdata = src->data(0, r);
    00105         unsigned char* ddata = dest->data(0, r);
    00106         for(int c=0; c < src->s(); ++c) {
    00107           (*ddata) =  (*(sdata)*red + *(sdata+1)*green + *(sdata+2)*blue)/numchannels;
    00108           sdata+=3;
    00109           ddata++;
    00110         }
    00111       }
    00112     }
    00113     bool red, green, blue;
    00114     char numchannels;
    00115     char channelmask;
    00116   };
    00117
    00118   /** converts the image to a HSV coded image @see StdImageProcessor.
    00119       If this image is shown the h,s,v is displayed by r,g,b!
    00120       The h (hue) values are given by HSVImgProc::Colors
    00121   */
    00122
    00123   struct HSVImgProc : public StdImageProcessor {
    00124     enum Colors {Red=0, Yellow=30, Green=60, Cyan=90,
    00125                  Blue=120, Magenta=150, Red2=180, Gray=255, Span=30};
    00126
    00127     HSVImgProc(bool show, float scale)
    00128       : StdImageProcessor(show,scale) {
    00129     }
    00130
    00131     virtual ~HSVImgProc() {}
    00132
    00133     virtual void initDestImage(Camera::CameraImage& dest, const Camera::CameraImage& src){
    00134       dest.img->allocateImage(src.img->s(), src.img->t(), 1, GL_RGB, GL_UNSIGNED_BYTE);
    00135       dest.name  = "hsv(" + src.name + ")";
    00136     }
    00137
    00138     virtual void process(const osg::Image* src, osg::Image* dest){
    00139       assert(src && src->getPixelFormat()==GL_RGB && src->getDataType()==GL_UNSIGNED_BYTE);
    00140       for(int r=0; r < src->t(); ++r) {
    00141         const unsigned char* sdata = src->data(0, r);
    00142         unsigned char* ddata = dest->data(0, r);
    00143         for(int c=0; c < src->s(); ++c) {
    00144           RGBtoHSV(*(sdata),*(sdata+1),*(sdata+2),
    00145                    (*ddata), *(ddata+1), *(ddata+2));
    00146           sdata+=3;
    00147           ddata+=3;
    00148         }
    00149       }
    00150     }
    00151
    00152     /** converts RGB to HSV color model;
    00153         r,g,b values are from 0 to 255;
    00154         h = [0,180]+255, s = [0,255], v = [0,255];
    00155         h is the standard hue value/2 (since 360 cannot be represented)
    00156         and 255 if undefined (gray)
    00157     */
    00158     void RGBtoHSV( unsigned char r, unsigned char g, unsigned char b,
    00159                    unsigned char& h, unsigned char& s, unsigned char& v ) {
    00160       unsigned char min, max;
    00161       float delta;
    00162       float hue;
    00163       min = MIN3( r, g, b );
    00164       max = MAX3( r, g, b );
    00165       v = max;                               // v
    00166       delta = max - min;
    00167       if( max != 0 ){
    00168         s = (unsigned char)(255.0*delta / max); // s
    00169       }
    00170       if( max == 0 || delta == 0){
    00171         // r = g = b                         // s = 0, h is undefined
    00172         s = 0;
    00173         h = 255;
    00174         return;
    00175       }
    00176
    00177       if( r == max )
    00178         hue = float( g - b ) / delta;        // between yellow & magenta
    00179       else if( g == max )
    00180         hue = 2.0 + float( b - r ) / delta;     // between cyan & yellow
    00181       else
    00182         hue = 4.0 + float( r - g ) / delta;     // between magenta & cyan
    00183       hue *=30; // this is 60 in full range
    00184       if( hue < 0 )
    00185         hue += 180;
    00186       h = int(hue);
    00187     }
    00188   };
    00189
    00190
    00191   /** filters for a specific color (requires HSV, so use HSVImgProc before)
    00192       @param minhue minimal hue value to pass through @see HSVImgProc::Colors
    00193       @param maxhue maximal hue value to pass through @see HSVImgProc::Colors
    00194       @param satThreshold minimal saturation required to be considered as a color
    00195       @param valThreshold minimal "value" required to be considered as a color
    00196       @see HSVImgProc
    00197       @see StdImageProcessor
    00198   */
    00199   struct ColorFilterImgProc : public StdImageProcessor {
    00200     ColorFilterImgProc(bool show, float scale, int minhue, int maxhue,
    00201                        int sat_threshold=100, int val_threshold=50)
    00202       : StdImageProcessor(show,scale),
    00203         minhue(minhue), maxhue(maxhue), sat_threshold(sat_threshold), val_threshold(val_threshold) {
    00204     }
    00205
    00206     virtual ~ColorFilterImgProc() {}
    00207
    00208     virtual void initDestImage(Camera::CameraImage& dest, const Camera::CameraImage& src){
    00209       dest.img->allocateImage(src.img->s(), src.img->t(), 1, GL_LUMINANCE, GL_UNSIGNED_BYTE);
    00210           //      dest.img->allocateImage(16, 1, 1, GL_LUMINANCE, GL_UNSIGNED_BYTE);
    00211       dest.name  = "spots(" + src.name + ")";
    00212     }
    00213
    00214     virtual void process(const osg::Image* src, osg::Image* dest){
    00215       // actually we need HSV but there is no coding for it
    00216       assert(src && src->getPixelFormat()==GL_RGB && src->getDataType()==GL_UNSIGNED_BYTE);
    00217       for(int r=0; r < src->t(); ++r) {
    00218         const unsigned char* sdata = src->data(0, r);
    00219         unsigned char* ddata = dest->data(0, r);
    00220         for(int c=0; c < src->s(); ++c) {
    00221           if(*(sdata) >= minhue && *(sdata) < maxhue
    00222              && *(sdata+1) > sat_threshold && *(sdata+2) > val_threshold){
    00223             (*ddata) = *(sdata+2);
    00224           } else{
    00225             (*ddata) = 0;
    00226           }
    00227           sdata+=3;
    00228           ddata++;
    00229         }
    00230       }
    00231     }
    00232     int minhue;
    00233     int maxhue;
    00234     int sat_threshold;
    00235     int val_threshold;
    00236   };
    00237
    00238
    00239
    00240   /** creates a lightsensitive sensorline. It requires a black and white source,
    00241       e.g. provided by BWImageProcessor, ColorFilterImgProc
    00242       @param num number of segments of the sensor line
    00243       @param factor factor for average pixel value (rescaling)
    00244       @see StdImageProcessor
    00245   */
    00246   struct LineImgProc : public StdImageProcessor {
    00247     LineImgProc(bool show, float scale, int num, double factor = 20.0)
    00248       : StdImageProcessor(show,scale), num(num), factor(factor) {
    00249     }
    00250
    00251     virtual ~LineImgProc() {}
    00252
    00253     virtual void initDestImage(Camera::CameraImage& dest, const Camera::CameraImage& src){
    00254       dest.img->allocateImage(num, 1, 1, GL_LUMINANCE, GL_UNSIGNED_BYTE);
    00255       dest.name  = "line(" + src.name + ")";
    00256     }
    00257
    00258     virtual void process(const osg::Image* src, osg::Image* dest){
    00259       // actually we need HSV but there is no coding for it
    00260       assert(src && src->getPixelFormat()==GL_LUMINANCE  && src->getDataType()==GL_UNSIGNED_BYTE);
    00261
    00262       int w = src->s();
    00263       int h = src->t();
    00264       int size = w/num; // size of one segment
    00265       int numpixel_per_segm = size*h;
    00266       int segmentvalue;
    00267       unsigned char* destdata = dest->data();
    00268       for(int k=0; k<num; k++){
    00269         int sum = 0;
    00270         for(int j=0; j<h; j++){
    00271           const unsigned char* pixel = src->data(k*size, j);
    00272           for(int i=0; i< size; i++){
    00273             sum += *pixel;
    00274             pixel++;
    00275           }
    00276         }
    00277         segmentvalue = int((double)sum*factor/(double)numpixel_per_segm);
    00278         destdata[k] = std::min(segmentvalue,255);
    00279       }
    00280     }
    00281     int num;
    00282     double factor;
    00283   };
    00284
    00285   /** time average of image @see StdImageProcessor.
    00286   */
    00287
    00288   struct AvgImgProc : public StdImageProcessor {
    00289
    00290     /// @param time length of averageing (time=1: no averaging)
    00291     AvgImgProc(bool show, float scale, int time)
    00292       : StdImageProcessor(show,scale), time(time) {
    00293       if(time<1) time =1;
    00294       factor = 1.0/(float)time;
    00295     }
    00296
    00297     virtual ~AvgImgProc() {}
    00298
    00299     virtual void initDestImage(Camera::CameraImage& dest, const Camera::CameraImage& src){
    00300       assert(src.img && src.img->getDataType()==GL_UNSIGNED_BYTE);
    00301       dest.img->allocateImage(src.img->s(), src.img->t(), 1, src.img->getPixelFormat(),
    00302                               GL_UNSIGNED_BYTE);
    00303       dest.name  = "avg(" + std::itos(time) +  "," + src.name + ")";
    00304     }
    00305
    00306     virtual void process(const osg::Image* src, osg::Image* dest){
    00307       for(int r=0; r < src->t(); ++r) {
    00308         const unsigned char* sdata = src->data(0, r);
    00309         unsigned char* ddata = dest->data(0, r);
    00310         for(unsigned int c=0; c < src->getRowSizeInBytes(); ++c) {
    00311           *ddata = (unsigned char)(((float)*sdata)*factor + ((float)*ddata)*(1-factor));
    00312           sdata++;
    00313           ddata++;
    00314         }
    00315       }
    00316     }
    00317
    00318     int time;
    00319     float factor;
    00320   };
    00321
    00322
    00323 //   /** Testing code
    00324 //   */
    00325 //   struct TestLineImgProc : public StdImageProcessor {
    00326 //     TestLineImgProc(bool show, float scale, int num)
    00327 //       : StdImageProcessor(show,scale), num(num) {
    00328 //     }
    00329
    00330 //     virtual ~TestLineImgProc() {}
    00331
    00332 //     virtual void initDestImage(Camera::CameraImage& dest, const Camera::CameraImage& src){
    00333 //       dest.img->allocateImage(src.img->s(), src.img->t(), 1, GL_LUMINANCE, GL_UNSIGNED_BYTE);
    00334 //       dest.name  = "testline(" + src.name + ")";
    00335 //     }
    00336
    00337 //     virtual void process(const osg::Image* src, osg::Image* dest){
    00338 //       // actually we need HSV but there is no coding for it
    00339 //       assert(src && src->getPixelFormat()==GL_LUMINANCE  && src->getDataType()==GL_UNSIGNED_BYTE);
    00340
    00341 //       int w = src->s();
    00342 //       int h = src->t();
    00343 //       int size = w/num; // size of one segment
    00344 //       int numpixel_per_segm = size*h;
    00345 //       for(int k=0; k<num; k++){
    00346 //         int sum = 0;
    00347 //         for(int j=0; j<h; j++){
    00348 //           const unsigned char* pixel = src->data(k*size, j);
    00349 //           unsigned char* destdata = dest->data(k*size, j);
    00350 //           for(int i=0; i< size; i++){
    00351 //             *destdata= (k*111+*pixel)%256;
    00352 //             pixel++;
    00353 //             destdata++;
    00354 //           }
    00355 //         }
    00356 //       }
    00357 //     }
    00358 //     int num;
    00359 //   };
    00360
    00361
    00362
    00363
    00364 }
    00365
    00366 #endif