00001 #include "VisionObjectEvent.h"
00002 #include "Shared/RobotInfo.h"
00003 #include <sstream>
00004 #include <libxml/tree.h>
00005
00006 const EventBase::classTypeID_t VisionObjectEvent::autoRegisterVisionObjectEvent=getTypeRegistry().registerType<VisionObjectEvent>(makeClassTypeID("VISO"));
00007
00008 std::string
00009 VisionObjectEvent::getDescription(bool showTypeSpecific, unsigned int verbosity) const {
00010 if(!showTypeSpecific)
00011 return EventBase::getDescription(showTypeSpecific,verbosity);
00012 std::ostringstream logdata;
00013 logdata << EventBase::getDescription(showTypeSpecific,verbosity) << '\t' << _x1 << '\t' <<_x2 <<'\t' <<_y1 <<'\t' <<_y2<<'\t'<<_frame ;
00014 return logdata.str();
00015 }
00016
00017 float
00018 VisionObjectEvent::getDistanceEstimate(float diaMajor, float diaMinor) const {
00019 float diaX,diaY;
00020 float w=getWidth();
00021 float h=getHeight();
00022 if(w>h) {
00023 diaX=diaMajor;
00024 diaY=diaMinor;
00025 } else {
00026 diaX=diaMinor;
00027 diaY=diaMajor;
00028 }
00029 float xest=diaX>0?calcDistance(getWidth()/2*CameraFOV,diaX):0;
00030 float yest=diaY>0?calcDistance(getHeight()/2*CameraFOV,diaY):0;
00031 if(xest>0 && yest>0) {
00032 return (xest+yest)/2;
00033 } else if(xest>0) {
00034 return xest;
00035 } else if(yest>0) {
00036 return yest;
00037 }
00038 return 0;
00039 }
00040
00041 float
00042 VisionObjectEvent::calcDistance(float visArc, float physDia) {
00043 float r=tan(visArc/2);
00044 if(r==0)
00045 return 0;
00046 return physDia/r;
00047 }
00048
00049 unsigned int
00050 VisionObjectEvent::getBinSize() const {
00051 unsigned int used=EventBase::getBinSize();
00052 if(saveFormat==XML)
00053 return used;
00054
00055 used+=creatorSize("EventBase::VisionObjectEvent");
00056 used+=getSerializedSize(_x1);
00057 used+=getSerializedSize(_x2);
00058 used+=getSerializedSize(_y1);
00059 used+=getSerializedSize(_y2);
00060 return used;
00061 }
00062
00063 unsigned int
00064 VisionObjectEvent::loadBinaryBuffer(const char buf[], unsigned int len) {
00065 unsigned int origlen=len;
00066 if(!checkInc(EventBase::loadBinaryBuffer(buf,len),buf,len)) return 0;
00067 if(!checkCreatorInc("EventBase::VisionObjectEvent",buf,len,true)) return 0;
00068 if(!decodeInc(_x1,buf,len)) return 0;
00069 if(!decodeInc(_x2,buf,len)) return 0;
00070 if(!decodeInc(_y1,buf,len)) return 0;
00071 if(!decodeInc(_y2,buf,len)) return 0;
00072 return origlen-len;
00073 }
00074
00075 unsigned int
00076 VisionObjectEvent::saveBinaryBuffer(char buf[], unsigned int len) const {
00077 unsigned int origlen=len;
00078 if(!checkInc(EventBase::saveBinaryBuffer(buf,len),buf,len)) return 0;
00079 if(!saveCreatorInc("EventBase::VisionObjectEvent",buf,len)) return 0;
00080 if(!encodeInc(_x1,buf,len)) return 0;
00081 if(!encodeInc(_x2,buf,len)) return 0;
00082 if(!encodeInc(_y1,buf,len)) return 0;
00083 if(!encodeInc(_y2,buf,len)) return 0;
00084 return origlen-len;
00085 }
00086
00087 void VisionObjectEvent::loadXML(xmlNode* node) {
00088 if(node==NULL)
00089 return;
00090
00091 EventBase::loadXML(node);
00092
00093 for(xmlNode* cur = skipToElement(node->children); cur!=NULL; cur = skipToElement(cur->next)) {
00094 if(xmlStrcmp(cur->name, (const xmlChar *)"param"))
00095 continue;
00096
00097 xmlChar * name = xmlGetProp(cur,(const xmlChar*)"name");
00098 if(name==NULL)
00099 throw bad_format(cur,"property missing name");
00100
00101 xmlChar * val = xmlGetProp(cur,(const xmlChar*)"value");
00102 if(val==NULL)
00103 throw bad_format(cur,"property missing value");
00104
00105 if(xmlStrcmp(name, (const xmlChar *)"x1")==0)
00106 _x1=atof((const char*)val);
00107 else if(xmlStrcmp(name, (const xmlChar *)"x2")==0)
00108 _x2=atof((const char*)val);
00109 else if(xmlStrcmp(name, (const xmlChar *)"y1")==0)
00110 _y1=atof((const char*)val);
00111 else if(xmlStrcmp(name, (const xmlChar *)"y2")==0)
00112 _y2=atof((const char*)val);
00113 else if(xmlStrcmp(name, (const xmlChar *)"clipLeft")==0)
00114 _clipLeft=atoi((const char*)val);
00115 else if(xmlStrcmp(name, (const xmlChar *)"clipRight")==0)
00116 _clipRight=atoi((const char*)val);
00117 else if(xmlStrcmp(name, (const xmlChar *)"clipTop")==0)
00118 _clipTop=atoi((const char*)val);
00119 else if(xmlStrcmp(name, (const xmlChar *)"clipBottom")==0)
00120 _clipBottom=atoi((const char*)val);
00121
00122 xmlFree(val);
00123 xmlFree(name);
00124 }
00125 }
00126
00127
00128
00129 #define SAVE_PARAM(strname,varname,format) {\
00130 xmlNode* cur=xmlNewChild(node,NULL,(const xmlChar*)"param",NULL); \
00131 if(cur==NULL) \
00132 throw bad_format(node,"Error: VisionObjectEvent xml error on saving param"); \
00133 xmlSetProp(cur,(const xmlChar*)"name",(const xmlChar*)strname); \
00134 char valbuf[20]; \
00135 snprintf(valbuf,20,format,varname); \
00136 xmlSetProp(cur,(const xmlChar*)"value",(const xmlChar*)valbuf); }
00137
00138 void VisionObjectEvent::saveXML(xmlNode * node) const {
00139 if(node==NULL)
00140 return;
00141 EventBase::saveXML(node);
00142
00143
00144 for(xmlNode* cur = skipToElement(node->children); cur!=NULL; ) {
00145 if(xmlStrcmp(cur->name, (const xmlChar *)"param")==0) {
00146 xmlUnlinkNode(cur);
00147 xmlFreeNode(cur);
00148 cur = skipToElement(node->children);
00149 } else
00150 cur = skipToElement(cur->next);
00151 }
00152
00153 SAVE_PARAM("x1",_x1,"%g");
00154 SAVE_PARAM("y1",_y1,"%g");
00155 SAVE_PARAM("x2",_x2,"%g");
00156 SAVE_PARAM("y2",_y2,"%g");
00157 SAVE_PARAM("clipLeft",_clipLeft,"%d");
00158 SAVE_PARAM("clipRight",_clipRight,"%d");
00159 SAVE_PARAM("clipTop",_clipTop,"%d");
00160 SAVE_PARAM("clipBottom",_clipBottom,"%d");
00161 }
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173