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 camerasensor that calculates the optical flow of the image * 00027 * * 00028 * * 00029 ***************************************************************************/ 00030 00031 #ifndef __OPTICALFLOW_H 00032 #define __OPTICALFLOW_H 00033 00034 #include "camerasensor.h" 00035 #include <selforg/controller_misc.h> 00036 00037 namespace lpzrobots { 00038 00039 /// configuration object for OpticalFlow 00040 struct OpticalFlowConf { 00041 /// dimensions to return the flow (X means horizonal, Y vertical) 00042 Sensor::Dimensions dims; 00043 /** points to measure optical flow in normalized coordinates [-1,1] 00044 however the points are placed sufficiently away from the border to have no 00045 boundary effects.*/ 00046 std::list<Pos> points; 00047 /** maximum fraction of the image dimension to consider for 00048 a possible flow. Larger values increase the computational demand and move the 00049 points more to the center (since we don't want edge effects)*/ 00050 double maxFlow; 00051 /** size (edge length) of the measurement field (block) in pixel 00052 (if 0 then 1/12th of width) */ 00053 int fieldSize; 00054 /** verbosity level 00055 (0: quite, 1: initialization values, 2: warnings, 3: info, 4: debug) */ 00056 int verbose; 00057 }; 00058 00059 /** This CameraSensor calculates the optical flow at few points of the image 00060 based on a box matching technique. 00061 This can be applied directly to the camera image. 00062 */ 00063 class OpticalFlow : public CameraSensor { 00064 public: 00065 00066 struct Vec2i { 00067 Vec2i() : x(0), y(0) {} 00068 Vec2i(int x, int y) : x(x), y(y) {} 00069 int x; 00070 int y; 00071 Vec2i operator + (const Vec2i& v) const; 00072 Vec2i operator * (int i) const; 00073 Vec2i operator / (int i) const; 00074 }; 00075 00076 typedef std::list< std::pair<Vec2i,int> > FlowDelList; 00077 00078 /** @see CameraSensor for further parameter explanation. 00079 */ 00080 OpticalFlow(OpticalFlowConf conf = getDefaultConf()); 00081 00082 virtual ~OpticalFlow(); 00083 00084 /** calculates default positions for optical flow detection. 00085 The points are in aranged horizontally in a line at the vertical center. 00086 For num 2 the points are at the border, 00087 3 points there is additioanlly one is the center and so on. 00088 */ 00089 static std::list<Pos> getDefaultPoints(int num); 00090 00091 /// the default config has 2 points in and calculates the flow in X and Y 00092 static OpticalFlowConf getDefaultConf(){ 00093 OpticalFlowConf c; 00094 c.dims = XY; 00095 c.points = getDefaultPoints(2); 00096 c.fieldSize = 24; 00097 c.maxFlow = 0.15; 00098 c.verbose = 1; 00099 return c; 00100 } 00101 00102 virtual void intern_init(); 00103 00104 /// Performs the calculations 00105 virtual bool sense(const GlobalData& globaldata); 00106 00107 virtual int getSensorNumber() const { 00108 return num; 00109 }; 00110 00111 virtual int get(sensor* sensors, int length) const; 00112 00113 00114 protected: 00115 /** 00116 compares a small part of two given images 00117 and returns the average absolute difference. 00118 Field center, size and shift have to be choosen, 00119 so that no clipping is required! 00120 \param field specifies position(center) of subimage to use for comparison 00121 \param size specifies the size of the field edged in pixels 00122 \param d_x shift in x direction 00123 \param d_y shift in y direction 00124 */ 00125 static double compareSubImg(const unsigned char* const I1, 00126 const unsigned char* const I2, 00127 const Vec2i& field, int size, int width, int height, 00128 int bytesPerPixel, int d_x,int d_y); 00129 00130 /** calculates the optimal transformation for one field in RGB 00131 * using all three color channels 00132 * @param minerror (is to return the minimum error achieved during the matching) 00133 */ 00134 Vec2i calcFieldTransRGB(const Vec2i& field, const osg::Image* current, 00135 const osg::Image* last, double& minerror) const; 00136 00137 protected: 00138 OpticalFlowConf conf; 00139 int num; 00140 std::list<Vec2i> fields; // fields in image coordinates 00141 sensor* data; 00142 osg::Image* lasts[4]; 00143 std::vector<Vec2i> oldFlows; 00144 int maxShiftX; 00145 int maxShiftY; 00146 int width; 00147 int height; 00148 int cnt; 00149 double avgerror; // average minimum matching error 00150 }; 00151 00152 00153 } 00154 00155 #endif