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

# camerasensors.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
    00025 #ifndef __CAMERASENSORS_H
    00026 #define __CAMERASENSORS_H
    00027
    00028 #include "camerasensor.h"
    00029 #include <selforg/controller_misc.h>
    00030
    00031 namespace lpzrobots {
    00032
    00033
    00034   /** This CameraSensor implements a direct conversion from pixels to sensors.
    00035       Probably you want to use an image processor like LineImgProc before.
    00036    */
    00037   class DirectCameraSensor : public CameraSensor {
    00038   public:
    00039
    00040     /** the camera image should be black and white (e.g. @see BWImageProcessor)
    00041         @see CameraSensor for further parameter explanation.
    00042         @param minValue pixel value that corresponds to -1
    00043         @param maxValue pixel value that corresponds to 1
    00044         (for minValue =-256 and maxValue=256 the sensor values are in [0,1)
    00045      */
    00046     DirectCameraSensor(int minValue=-256, int maxValue=256)
    00047       : minValue(minValue), maxValue(maxValue) {
    00048     }
    00049
    00050
    00051     virtual ~DirectCameraSensor(){
    00052       delete[] data;
    00053     }
    00054
    00055     virtual void intern_init(){
    00056       assert(camera->isInitialized());
    00057       const osg::Image* img = camera->getImage();
    00058       assert(img && img->getPixelFormat()==GL_LUMINANCE  && img->getDataType()==GL_UNSIGNED_BYTE);
    00059       num = img->s() * img->t();
    00060       data = new sensor[num];
    00061     };
    00062
    00063
    00064     /// Performs the calculations
    00065     virtual bool sense(const GlobalData& globaldata){
    00066       const osg::Image* img = camera->getImage();
    00067       const unsigned char* pixel = img->data();
    00068       if(img->s() * img->t() < num) return false;
    00069       int center = (maxValue+minValue)/2;
    00070       for(int k=0; k< num; k++){
    00071         data[k]=(pixel[k]-center)*2.0/double(maxValue-minValue);
    00072 }
    00073       return true;
    00074     }
    00075
    00076     virtual int getSensorNumber() const {
    00077       return num;
    00078     };
    00079
    00080     /// overload this function and return the sensor values
    00081     virtual int get(sensor* sensors, int length) const {
    00082       assert(length>=num);
    00083       memcpy(sensors, data, num * sizeof(sensor));
    00084       return num;
    00085     }
    00086
    00087
    00088   protected:
    00089     int num;
    00090     int minValue;
    00091     int maxValue;
    00092     sensor* data;
    00093   };
    00094
    00095   struct PositionCameraSensorConf {
    00096     typedef short Values; ///< combination of PositionCameraSensor::ValueTypes
    00097
    00098     /// values additional sensor values, @see PositionCameraSensor::Values
    00099     Values     values   ;
    00100     /// dims dimensions to return the position (X means horizonal, Y vertical)
    00101     Sensor::Dimensions dims     ;
    00102     /** exponent for the measured size. A sqrt (0.5) make small sizes larger, so
    00103         that changes in the distance can be measured better */
    00104     double     sizeExponent;
    00105     /// factor for measured size change (velocity is in framesize/frame)
    00106     double     factorSizeChange;
    00107     /// clipsize value at which the values are clipped, e.g. [-1.5,1.5]
    00108     double     clipsize ;
    00109     /** if >0 then the size and sizechange are zero if position is that far (border) away from the image border @see PositionCameraSensor */
    00110     double     border   ;
    00111   };
    00112
    00113
    00114   /** This CameraSensor calculates the position of the visible object(s) that
    00115       is essentially the center of gravity of the image.
    00116       The position in normalized to -1 to 1.
    00117       Probably you want to use an image processor like ColorFilterImgProc before.
    00118   */
    00119   class PositionCameraSensor : public CameraSensor {
    00120   public:
    00121     /** additional sensor values. Size is the size of the object (only one value,
    00122         independent of the dimensions */
    00123     enum ValueTypes { None = 0, Position = 1, Size = 2, SizeChange = 4 };
    00124
    00125     /** The camera image should be black and white
    00126         (e.g. @see BWImageProcessor or ColorFilterImgProc)
    00127         @see CameraSensor for further parameter explanation.
    00128         @param values sensor values to compute (@see PositionCameraSensor::ValueTypes)
    00129         @param dims dimensions to return the position (X means horizonal, Y vertical)
    00130         @param border if >0 then the size and sizechange are zero if position is that far (border) away from
    00131         image border
    00132      */
    00133     PositionCameraSensor(PositionCameraSensorConf conf = getDefaultConf())
    00134       : conf(conf), oldsize(0) {
    00135       num = (bool(conf.dims & X) + bool(conf.dims & Y))* bool(conf.values & Position) +
    00136         bool(conf.values & Size) + bool(conf.values & SizeChange);
    00137     }
    00138
    00139     static PositionCameraSensorConf getDefaultConf(){
    00140       PositionCameraSensorConf c;
    00141       c.values           = Position;
    00142       c.dims             = XY;
    00143       c.factorSizeChange = 10.0;
    00144       c.sizeExponent     = 1;
    00145       c.clipsize         = 1.5;
    00146       c.border           = 0;
    00147       return c;
    00148     }
    00149
    00150     virtual ~PositionCameraSensor(){
    00151       if(data) delete[] data;
    00152     }
    00153
    00154     virtual void intern_init(){
    00155       assert(camera->isInitialized());
    00156
    00157       data = new sensor[num];
    00158       memset(data,0,sizeof(sensor)*num);
    00159
    00160       const osg::Image* img = camera->getImage();
    00161       img=img; // to avoid unused variable in NDEBUG mode
    00162       assert(img && img->getPixelFormat()==GL_LUMINANCE  &&
    00163              img->getDataType()==GL_UNSIGNED_BYTE);
    00164     };
    00165
    00166
    00167     /// Performs the calculations
    00168     virtual bool sense(const GlobalData& globaldata){
    00169       const osg::Image* img = camera->getImage();
    00170       double x;
    00171       double y;
    00172       double size;
    00173       calcImgCOG(img, x, y, size);
    00174       int k=0;
    00175       return processAndFillData(x, y, size, k );
    00176     }
    00177
    00178     virtual int getSensorNumber() const {
    00179       return num;
    00180     };
    00181
    00182     /// overload this function and return the sensor values
    00183     virtual int get(sensor* sensors, int length) const {
    00184       assert(length>=num);
    00185       memcpy(sensors, data, num * sizeof(sensor));
    00186       return num;
    00187     }
    00188
    00189   protected:
    00190     /// process sensor information and fills
    00191     //   the data buffer starting at intex k
    00192     virtual bool processAndFillData(double& x, double& y, double& size, int& k){
    00193       int kstart = k;
    00194       if(conf.sizeExponent!=1)
    00195         size = pow(size,conf.sizeExponent);
    00196
    00197       if(conf.values & Position){
    00198         if(conf.dims & X) data[k++] = x;
    00199         if(conf.dims & Y) data[k++] = y;
    00200       }
    00201       double sizeChange = (size - oldsize)*conf.factorSizeChange;
    00202       oldsize = size;
    00203
    00204       // check border effects
    00205       if(conf.border>0){
    00206         if((x==0 && y==0) || ((conf.dims & X) && (fabs(x) > (1-conf.border))) ||
    00207            ((conf.dims & Y) && (fabs(y) > (1-conf.border))) ){
    00208           size=0;
    00209           sizeChange=0;
    00210         }
    00211       }
    00212       if(conf.values & Size)       data[k++] = size;
    00213       if(conf.values & SizeChange) data[k++] = sizeChange;
    00214
    00215       // clip values
    00216       if(conf.clipsize!=0){
    00217         for(int i=kstart; i<k; i++){
    00218           data[i] = clip(data[i],-conf.clipsize,conf.clipsize);
    00219         }
    00220       }
    00221       return true;
    00222     }
    00223
    00224     /** calculates the Center of Gravity (normalized to -1 to 1) of an image.
    00225         As a bonus it also calculates the sum of all pixels (normalizes to 0-2.5) in size
    00226         @return false if image is too dark
    00227     */
    00228     static bool calcImgCOG(const osg::Image* img, double& x, double& y, double& size,
    00229                            int threshold = 1){
    00230       int w = img->s();
    00231       int h = img->t();
    00232       double centerX = w/2.0;
    00233       double centerY = h/2.0;
    00234       if(threshold<1) threshold = 1;
    00235       x = y = 0;
    00236       double sum= 0;
    00237       for(int r=0; r< h; r++){
    00238         const unsigned char* row = img->data(0, r);
    00239         double pY = ((double)r-centerY);
    00240         for(int c=0; c < w; c++){
    00241           sum += row[c];
    00242           x  += row[c] * ((double)c-centerX);
    00243           y  += row[c] * pY;
    00244         }
    00245       }
    00246       if(sum<threshold){
    00247         x = y = size = 0;
    00248         return false;
    00249       }else{
    00250         x /= sum * centerX; // normalize to -1 to 1
    00251         y /= sum * centerY; // normalize to -1 to 1
    00252         // the /255 would be correct, but then the values is so small
    00253         size = double(sum) / (w*h) / 128;
    00254         return true;
    00255       }
    00256     }
    00257
    00258
    00259     protected:
    00260     PositionCameraSensorConf conf;
    00261     int num;
    00262     sensor* data;
    00263     double oldsize;
    00264   };
    00265
    00266
    00267   struct MotionCameraSensorConf : public PositionCameraSensorConf{
    00268     /// averaging time window (1: no averaging)
    00269     int        avg      ;
    00270     /// factor for measured velocity (velocity is in framesize/frame)
    00271     double     factorMotion ;
    00272     /// window whether to apply a windowing function to motion data to avoid edge effects
    00273     bool       window   ;
    00274
    00275   };
    00276
    00277
    00278   /** This CameraSensor calculates the global optical flow of the camera image
    00279       using the center of gravity method. It requires objects to be bright on black ground.
    00280       The velocity is by default windowed to avoid step-like artefacts at the border.
    00281       Probably you want to use an image processor like ColorFilterImgProc before.
    00282    */
    00283   class MotionCameraSensor : public PositionCameraSensor {
    00284   public:
    00285
    00286     /** The camera image should be black and white
    00287         (e.g. @see BWImageProcessor or ColorFilterImgProc)
    00288         @see CameraSensor for further parameter explanation.
    00289         @param mconf configuration object @see MotionCameraSensorConf
    00290          and @see PositionCameraSensorConf
    00291      */
    00292     MotionCameraSensor(const MotionCameraSensorConf& mconf = getDefaultConf())
    00293       : PositionCameraSensor(mconf), mconf(mconf),
    00294         last(false), lastX(0), lastY(0)
    00295     {
    00296       if(this->mconf.avg<1) this->mconf.avg=1;
    00297       lambda = 1/(double)this->mconf.avg;
    00298       num   += bool(this->mconf.dims & X) + bool(this->mconf.dims & Y);
    00299     }
    00300
    00301     static MotionCameraSensorConf getDefaultConf(){
    00302       MotionCameraSensorConf c;
    00303       c.avg              = 2;
    00304       c.values           = None;
    00305       c.dims             = X;
    00306       c.factorSizeChange = 10.0;
    00307       c.sizeExponent     = 1;
    00308       c.clipsize         = 1.5;
    00309       c.border           = 0;
    00310       c.factorMotion     = 5.0;
    00311       c.window           = true;
    00312       return c;
    00313     }
    00314
    00315     virtual ~MotionCameraSensor(){
    00316     }
    00317
    00318     /// Performs the calculations
    00319     virtual bool sense(const GlobalData& globaldata){
    00320       const osg::Image* img = camera->getImage();
    00321       double x;
    00322       double y;
    00323       double size;
    00324       bool success = calcImgCOG(img, x, y, size);
    00325       int k=0;
    00326       // check if the apparent shift is feasible, otherwise set to no motion.
    00327       if(last && success && fabs(x - lastX) < 0.4 && fabs(y - lastY) < 0.4){
    00328         if(mconf.dims & X) {
    00329           data[k] = lambda*(x - lastX)*mconf.factorMotion* (mconf.window ? windowfunc(x) : 1)
    00330             + (1- lambda)*data[k];
    00331           k++;
    00332         }
    00333         if(mconf.dims & Y) {
    00334           data[k] = lambda*(y - lastY)*mconf.factorMotion* (mconf.window ? windowfunc(y) : 1)
    00335             + (1- lambda)*data[k]; k++;
    00336         }
    00337         // clip values
    00338         if(conf.clipsize!=0){
    00339           for(int i=0; i<k; i++){
    00340             data[i] = clip(data[i],-mconf.clipsize,mconf.clipsize);
    00341           }
    00342         }
    00343       }else{
    00344         if(mconf.dims & X) data[k++]=0;
    00345         if(mconf.dims & Y) data[k++]=0;
    00346       }
    00347       lastX = x;
    00348       lastY = y;
    00349       last  = success;
    00350       // add all other sensor values
    00351       return processAndFillData(x,y,size,k);
    00352     }
    00353
    00354     /// window function for the interval -1 to 1, with ramps from 0.5 off center
    00355     double windowfunc(double x){
    00356       if(x>-0.5 && x<0.5) return 1.0;
    00357       if(x<= -0.5) return 2+ 2*x;
    00358       else return 2- 2*x; // (x>0.5)
    00359     }
    00360
    00361   protected:
    00362     MotionCameraSensorConf mconf;
    00363     double lambda;
    00364     bool   last;  ///< whether last image had a valid position
    00365     double lastX;
    00366     double lastY;
    00367
    00368   };
    00369
    00370 }
    00371
    00372 #endif