camerasensors.h

Go to the documentation of this file.
00001 /***************************************************************************
00002  *   Copyright (C) 2007 by Robot Group Leipzig                             *
00003  *    georg@nld.ds.mpg.de                                                  *
00004  *    fhesse@informatik.uni-leipzig.de                                     *
00005  *    der@informatik.uni-leipzig.de                                        *
00006  *                                                                         *
00007  *   This program is free software; you can redistribute it and/or modify  *
00008  *   it under the terms of the GNU General Public License as published by  *
00009  *   the Free Software Foundation; either version 2 of the License, or     *
00010  *   (at your option) any later version.                                   *
00011  *                                                                         *
00012  *   This program is distributed in the hope that it will be useful,       *
00013  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00014  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
00015  *   GNU General Public License for more details.                          *
00016  *                                                                         *
00017  *   You should have received a copy of the GNU General Public License     *
00018  *   along with this program; if not, write to the                         *
00019  *   Free Software Foundation, Inc.,                                       *
00020  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
00021  ***************************************************************************
00022  ***************************************************************************
00023  *                                                                         *
00024  *  DESCRIPTION                                                            *
00025  *                                                                         *
00026  *  A collection of typical camerasensor implementations.                  *
00027  *                                                                         *
00028  ***************************************************************************/
00029 
00030 #ifndef __CAMERASENSORS_H
00031 #define __CAMERASENSORS_H
00032 
00033 #include "camerasensor.h"
00034 #include <selforg/controller_misc.h>
00035 
00036 namespace lpzrobots {
00037 
00038 
00039   /** This CameraSensor implements a direct conversion from pixels to sensors.
00040       Probably you want to use an image processor like LineImgProc before.      
00041    */
00042   class DirectCameraSensor : public CameraSensor {
00043   public:  
00044 
00045     /** the camera image should be black and white (e.g. @see BWImageProcessor)
00046         @see CameraSensor for further parameter explanation.
00047         @param minValue pixel value that corresponds to -1
00048         @param maxValue pixel value that corresponds to 1
00049         (for minValue =-256 and maxValue=256 the sensor values are in [0,1)
00050      */
00051     DirectCameraSensor(int minValue=-256, int maxValue=256)
00052       : minValue(minValue), maxValue(maxValue) {
00053     }
00054 
00055 
00056     virtual ~DirectCameraSensor(){
00057       delete[] data;
00058     }
00059 
00060     virtual void intern_init(){
00061       assert(camera->isInitialized());
00062       const osg::Image* img = camera->getImage();
00063       assert(img && img->getPixelFormat()==GL_LUMINANCE  && img->getDataType()==GL_UNSIGNED_BYTE);
00064       num = img->s() * img->t();
00065       data = new sensor[num]; 
00066     };
00067 
00068     
00069     /// Performs the calculations
00070     virtual bool sense(const GlobalData& globaldata){
00071       const osg::Image* img = camera->getImage();
00072       const unsigned char* pixel = img->data();
00073       if(img->s() * img->t() < num) return false;
00074       int center = (maxValue+minValue)/2;
00075       for(int k=0; k< num; k++){
00076         data[k]=(pixel[k]-center)*2.0/double(maxValue-minValue);        
00077 }
00078       return true;
00079     }
00080      
00081     virtual int getSensorNumber() const {
00082       return num;
00083     };
00084 
00085     /// overload this function and return the sensor values
00086     virtual int get(sensor* sensors, int length) const {
00087       assert(length>=num);
00088       memcpy(sensors, data, num * sizeof(sensor));
00089       return num;
00090     }
00091     
00092 
00093   protected:
00094     int num;
00095     int minValue;
00096     int maxValue;
00097     sensor* data;
00098   };  
00099 
00100 
00101   /** This CameraSensor calculates the position of the visible object(s) that
00102       is essentially the center of gravity of the image.      
00103       The position in normalized to -1 to 1.
00104       Probably you want to use an image processor like ColorFilterImgProc before.      
00105    */
00106   class PositionCameraSensor : public CameraSensor {
00107   public:  
00108     /** additional sensor values. Size is the size of the object (only one value,
00109         independent of the dimensions */
00110     enum ValueTypes { None = 0, Position = 1, Size = 2, SizeChange = 4 };
00111     typedef short Values; ///< combination of ValueTypes
00112     
00113     /** The camera image should be black and white 
00114         (e.g. @see BWImageProcessor or ColorFilterImgProc)
00115         @see CameraSensor for further parameter explanation.
00116         @param values sensor values to compute (@see PositionCameraSensor::ValueTypes)
00117         @param dims dimensions to return the position (X means horizonal, Y vertical)
00118      */
00119     PositionCameraSensor(Values values = Position, Dimensions dims = XY )
00120       : dims(dims), values(values), oldsize(0) {
00121       num = (bool(dims & X) + bool(dims & Y))*bool(values & Position) +
00122         bool(values & Size) + bool(values & SizeChange);
00123       data = new sensor[num]; 
00124       memset(data,0,sizeof(sensor)*num);
00125     }
00126 
00127     virtual ~PositionCameraSensor(){
00128       if(data) delete[] data;
00129     }
00130 
00131     virtual void intern_init(){
00132       assert(camera->isInitialized());
00133       const osg::Image* img = camera->getImage();
00134       assert(img && img->getPixelFormat()==GL_LUMINANCE  && 
00135              img->getDataType()==GL_UNSIGNED_BYTE);
00136     };
00137 
00138     
00139     /// Performs the calculations
00140     virtual bool sense(const GlobalData& globaldata){
00141       const osg::Image* img = camera->getImage();
00142       double x;
00143       double y;      
00144       double size;      
00145       calcImgCOG(img, x, y, size);
00146       int k=0;
00147       if(values & Position){
00148         if(dims & X) data[k++] = x;
00149         if(dims & Y) data[k++] = y;
00150       }
00151       if(values & Size) data[k++] = size;
00152       if(values & SizeChange) data[k++] = (size - oldsize)*10;
00153       oldsize = size;
00154       return true;
00155     }
00156 
00157     /** calculates the Center of Gravity of an image normalized to -1 to 1
00158         @return false if image is image
00159     */
00160     static bool calcImgCOG(const osg::Image* img, double& x, double& y, double& size, 
00161                            int threshold = 1){
00162       int w = img->s();
00163       int h = img->t();
00164       double centerX = w/2.0;
00165       double centerY = h/2.0;
00166       if(threshold<1) threshold = 1;
00167       x = y = 0;
00168       double sum= 0;
00169       for(int r=0; r< h; r++){
00170         const unsigned char* row = img->data(0, r);
00171         double pY = ((double)r-centerY);
00172         for(int c=0; c < w; c++){
00173           sum += row[c];
00174           x  += row[c] * ((double)c-centerX);
00175           y  += row[c] * pY;
00176         }
00177       }
00178       if(sum<threshold){
00179         x = y = size = 0;
00180         return false;
00181       }else{
00182         x /= sum * centerX; // normalize to -1 to 1 
00183         y /= sum * centerY; // normalize to -1 to 1 
00184         // the /255 would be correct, but then the values is so small
00185         size = double(sum) / (w*h) / 100; 
00186         return true;
00187       }
00188     }
00189 
00190      
00191     virtual int getSensorNumber() const {
00192       return num;
00193     };
00194 
00195     /// overload this function and return the sensor values
00196     virtual int get(sensor* sensors, int length) const {
00197       assert(length>=num);
00198       memcpy(sensors, data, num * sizeof(sensor));
00199       return num;
00200     }
00201   protected:
00202     int num;
00203     Dimensions dims;
00204     Values values;
00205     sensor* data;
00206     double oldsize;
00207   }; 
00208 
00209   /** This CameraSensor calculates the global optical flow of the camera image
00210       using the center of gravity method. It requires objects to be bright on black ground.
00211       The velocity is by default windowed to avoid step-like artefacts at the border.
00212       Probably you want to use an image processor like ColorFilterImgProc before. 
00213    */
00214   class MotionCameraSensor : public PositionCameraSensor {
00215   public:  
00216 
00217     /** The camera image should be black and white 
00218         (e.g. @see BWImageProcessor or ColorFilterImgProc)
00219         @see CameraSensor for further parameter explanation.
00220         @param dims dimensions to return the position (X means horizonal, Y vertical)
00221         @param values additional sensor values, @see PositionCameraSensor::Values
00222         @param factor factor for measured velocity (velocity is in framesize/frame
00223         @param avg averaging time window (1: no averaging)
00224         @param window whether to apply a windowing function to avoid edge effects
00225         @param clipsize value at which the values are clipped, e.g. [-1.5,1.5] 
00226      */
00227     MotionCameraSensor(int avg = 2, Values values=None, Dimensions dims = X, 
00228                        double factor = 5.0, bool window = true, double clipsize = 1.5)
00229       : PositionCameraSensor(values, dims), factor(factor), 
00230         window(true), clipsize(clipsize), last(false), lastX(0), lastY(0)
00231     {
00232       lambda = 1/(double)avg;
00233       num += bool(dims & X) + bool(dims & Y);
00234       delete[] data;
00235       data = new sensor[num]; 
00236       memset(data,0,sizeof(sensor)*num); 
00237     }
00238 
00239     virtual ~MotionCameraSensor(){
00240     }
00241     
00242     /// Performs the calculations
00243     virtual bool sense(const GlobalData& globaldata){
00244       const osg::Image* img = camera->getImage();
00245       double x;
00246       double y;      
00247       double size;      
00248       bool success = calcImgCOG(img, x, y, size);
00249       int k=0;
00250       if(last && success){
00251         // check if the apparent shift is infeasible, then leave the old sensor value.
00252         if(fabs(x - lastX) < 0.4 && fabs(y - lastY) < 0.4) {
00253           if(dims & X) { 
00254             data[k] = lambda*(x - lastX)*factor* (window ? windowfunc(x) : 1) 
00255               + (1- lambda)*data[k]; k++; 
00256           }
00257           if(dims & Y) { 
00258             data[k] = lambda*(y - lastY)*factor* (window ? windowfunc(y) : 1)
00259               + (1- lambda)*data[k]; k++; 
00260           }
00261         }
00262       }else{
00263         if(dims & X) data[k++] = 0;
00264         if(dims & Y) data[k++] = 0;     
00265       }
00266       if(values & Position){
00267         if(dims & X) data[k++] = x;
00268         if(dims & Y) data[k++] = y;
00269       }
00270       if(values & Size) data[k++] = size;       
00271       if(values & SizeChange) data[k++] = (size - oldsize)*10;
00272         
00273       if(clipsize!=0){
00274         for(int i=0; i<num; i++){
00275           data[i] = clip(data[i],-clipsize,clipsize);
00276         }
00277       }
00278         
00279       lastX = x;
00280       lastY = y;
00281       last  = success;
00282       oldsize = size;
00283       return true;
00284     }
00285     
00286     /// window function for the interval -1 to 1, with ramps from 0.5 off center
00287     double windowfunc(double x){
00288       if(x>-0.5 && x<0.5) return 1.0;
00289       if(x<-0.5) return 2+ 2*x;
00290       else return 2- 2*x; // (x>0.5)
00291     }
00292 
00293   protected:
00294     double factor;
00295     double lambda;
00296     bool window;
00297     double clipsize;
00298     bool   last;  ///< whether last image had a valid position 
00299     double lastX;
00300     double lastY;
00301 
00302   };  
00303 
00304 }
00305 
00306 #endif
Generated on Fri Nov 4 10:59:38 2011 for Robot Simulator of the Robotics Group for Self-Organization of Control by  doxygen 1.6.3