camerasensors.h
Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
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
00040
00041
00042 class DirectCameraSensor : public CameraSensor {
00043 public:
00044
00045
00046
00047
00048
00049
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
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
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
00102
00103
00104
00105
00106 class PositionCameraSensor : public CameraSensor {
00107 public:
00108
00109
00110 enum ValueTypes { None = 0, Position = 1, Size = 2, SizeChange = 4 };
00111 typedef short Values;
00112
00113
00114
00115
00116
00117
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
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
00158
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;
00183 y /= sum * centerY;
00184
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
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
00210
00211
00212
00213
00214 class MotionCameraSensor : public PositionCameraSensor {
00215 public:
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225
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
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
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
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;
00291 }
00292
00293 protected:
00294 double factor;
00295 double lambda;
00296 bool window;
00297 double clipsize;
00298 bool last;
00299 double lastX;
00300 double lastY;
00301
00302 };
00303
00304 }
00305
00306 #endif