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 #ifndef INCLUDED_Vision_h
00030 #define INCLUDED_Vision_h
00031
00032 #include <math.h>
00033
00034 #include "cmvision.h"
00035 #include "VisionInterface.h"
00036
00037 using namespace VisionInterface;
00038
00039 #include "Shared/Config.h"
00040 #include "Visiondefines.h"
00041 #include "Wireless/Wireless.h"
00042 #include "Events/VisionEvent.h"
00043
00044 #define MAX_TMAPS 16
00045
00046
00047 typedef uchar pixel;
00048 typedef uchar cmap_t;
00049 typedef CMVision::image<uchar> image;
00050 typedef CMVision::color_class_state color_class_state;
00051 typedef CMVision::run<cmap_t> run;
00052 typedef CMVision::region region;
00053
00054 const int bits_y = 4;
00055 const int bits_u = 6;
00056 const int bits_v = 6;
00057
00058
00059
00060
00061
00062
00063
00064 static const double FocalDist =(176.0/2)/tan(HorzFOV/2);
00065 static const double FocalDistV=(144.0/2)/tan(VertFOV/2);
00066
00067 static const double YPixelSize=(FocalDistV/FocalDist);
00068
00069 static const double RobotArea = 13711;
00070
00071 #define MIN_EXP_REGION_SIZE 16
00072 #define MIN_EXP_RUN_LENGTH 8
00073
00074
00075 struct VisionObjectInfo {
00076 region *reg,*reg2;
00077 };
00078
00079 struct Marker {
00080 region* regs[3];
00081 int cen_x;
00082 int cen_y;
00083 int marker;
00084 };
00085
00086 struct VisionEventSpec {
00087 int listeners;
00088 int filter;
00089 int count;
00090 double confidence;
00091 float cx, cy;
00092 bool present;
00093 };
00094
00095 class VisionSerializer;
00096
00097 class Vision{
00098 friend class VisionSerializer;
00099 public:
00100 Vision();
00101 ~Vision() { }
00102
00103 unsigned long frameTimestamp;
00104 int frame_count;
00105
00106 int num_tmaps;
00107 int cur_tmap;
00108 cmap_t *cmap,*tmap[MAX_TMAPS];
00109 run *rmap,*rmap2;
00110 region *reg;
00111 int yindex[144];
00112
00113 VisionObjectInfo vobj_info[NUM_VISION_OBJECTS];
00114 VisionEventSpec vevent_spec[NUM_VEVENTS];
00115
00116 color_class_state color[MAX_COLORS];
00117
00118 int width,height;
00119 int max_width,max_height;
00120 int max_runs,max_regions;
00121 int num_colors,num_runs,num_regions;
00122
00123 double body_angle, body_height;
00124 double head_angles[3];
00125
00126 vector3d camera_loc,camera_dir,camera_up,camera_right;
00127
00128
00129 double sq_distort_coeff,lin_distort_coeff;
00130
00131 int setThreshold(int threshold_id);
00132
00133 const vector3d &get_camera_loc() {return camera_loc;}
00134 const vector3d &get_camera_dir() {return camera_dir;}
00135 int getColor(int x,int y)
00136 {return((x>=0 && y>=0 && x<width && y<height)?
00137 cmap[y*width+x] : COLOR_BACKGROUND);}
00138 int getWidth() {return width;}
00139 int getHeight() {return height;}
00140 void initialize();
00141 void setCameraParam();
00142 void initializeEventSpecs();
00143
00144 void enableEvents(int vevent);
00145 void enableEvents(int vevent, int noise);
00146 void disableEvents(int vevent);
00147
00148 void setNoiseThreshold(int vevent, int noise);
00149
00150 bool close();
00151
00152 bool processFrame(const uchar *data_y, const uchar *data_u, const uchar *data_v, int width, int height);
00153 bool saveThresholdImage(char *filename);
00154
00155 void sendRawImage ();
00156 void sendRLEImage ();
00157 void sendColorArea();
00158
00159 bool calcTotalArea;
00160
00161 Marker markers[3];
00162 int vis_markers;
00163 ObjectInfo *obj_info;
00164
00165 private:
00166
00167 int outCountAvgColor;
00168 int outCountColorArea;
00169 int outCountRaw;
00170 int outCountRLE;
00171
00172 VisionSerializer* vser;
00173
00174
00175 CMVision::image_yuv<const uchar> img;
00176
00177 bool thresholdImage(CMVision::image_idx<rgb> &img);
00178 bool thresholdImage(CMVision::image_yuv<const uchar> &img);
00179
00180 template<class image>
00181 bool runLowLevelVision(image &img);
00182
00183 int getColorUnsafe(int x,int y)
00184 {return(cmap[y*width+x]);}
00185 int getNearColor(int x,int y)
00186 {return(cmap[bound(y,0,height-1)*width+bound(x,0,width-1)]);}
00187
00188 int addToHistHorizStrip(int y, int x1, int x2, int *color_cnt);
00189 int addToHistVertStrip (int x, int y1, int y2, int *color_cnt);
00190 void createEvent(unsigned int tid, unsigned int sid, float cenX, float cenY);
00191
00192
00193 vector3d getPixelDirection(double x, double y);
00194
00195 void findSpan(double &left, double &right, double x1, double x2, double y1, double y2);
00196
00197 int calcEdgeMask(double x1, double x2, double y1, double y2);
00198 int calcEdgeMask(int x1,int x2,int y1,int y2);
00199 int calcEdgeMask(region *tmpreg)
00200 {return(calcEdgeMask(tmpreg->x1,tmpreg->x2,tmpreg->y1,tmpreg->y2));}
00201
00202 int isIn(region *r1, region *r2);
00203 int isAdjacent(region *r1, region *r2);
00204
00205 bool findHand(VObject *hand,VisionObjectInfo *hand_info);
00206 bool findBall(int ball_color, VObject *ball,VisionObjectInfo *ball_info);
00207 bool findThing(VObject *thing,VisionObjectInfo *thing_info);
00208 bool findMarkers();
00209 bool findGesture(VisionObjectInfo *hand_info);
00210 int identifyMarker(int color1, int color2, int color3);
00211 bool generateEvent(int vevent, double conf, int cenX, int cenY);
00212
00213 bool runHighLevelVision(ObjectInfo *obj_info);
00214
00215 Vision(const Vision&);
00216 Vision operator=(const Vision&);
00217 };
00218
00219 extern Vision* vision;
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234 #endif