Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

Config.cc

Go to the documentation of this file.
00001 #include "Config.h"
00002 #include <libxml/xmlmemory.h>
00003 #include <libxml/parser.h>
00004 #include <iostream>
00005 #include <fstream>
00006 #include <cstdio>
00007 #include <cerrno>
00008 #include <cstring>
00009 #include <cctype>
00010 #include <cmath>
00011 #include <string>
00012 #include <sys/stat.h>
00013 #ifdef PLATFORM_APERIOS
00014 #  include <OPENR/OPENRAPI.h>
00015 #else
00016 #include <sys/param.h>
00017 #include <unistd.h>
00018 #endif
00019 #include "Shared/ProjectInterface.h"
00020 #include "Shared/string_util.h"
00021 //#include "Behaviors/Mon/RawCam.h"
00022 #include "Vision/RawCameraGenerator.h"
00023 //#include "Vision/SegmentedColorGenerator.h"
00024 //#include "Wireless/Socket.h"
00025 #include "Shared/debuget.h"
00026 
00027 /*! Explicitly declaring these values allows us to relegate storage to a 
00028 *  single translation unit instead of having it reallocated every time
00029 *  the class is referenced.  This actually adds up to megabytes of
00030 *  symbols (especially with debugging info). */
00031 //! @name plist::NamedEnumeration statics
00032 //! provides string names for enumeration values (see plist::NamedEnumeration and #INSTANTIATE_NAMEDENUMERATION_STATICS)
00033 INSTANTIATE_NAMEDENUMERATION_STATICS(Config::main_config::consoleMode_t);
00034 INSTANTIATE_NAMEDENUMERATION_STATICS(Config::sound_config::volume_levels);
00035 INSTANTIATE_NAMEDENUMERATION_STATICS(Config::transports);
00036 INSTANTIATE_NAMEDENUMERATION_STATICS(Config::vision_config::RawCamConfig::compression_t);
00037 INSTANTIATE_NAMEDENUMERATION_STATICS(Config::vision_config::encoding_t);
00038 INSTANTIATE_NAMEDENUMERATION_STATICS(Config::vision_config::DepthCamConfig::compression_t);
00039 INSTANTIATE_NAMEDENUMERATION_STATICS(Config::vision_config::SegCamConfig::compression_t);
00040 INSTANTIATE_NAMEDENUMERATION_STATICS(Config::vision_config::gain_levels);
00041 INSTANTIATE_NAMEDENUMERATION_STATICS(Config::vision_config::shutter_speeds);
00042 INSTANTIATE_NAMEDENUMERATION_STATICS(Config::vision_config::white_balance_levels);
00043 INSTANTIATE_NAMEDENUMERATION_STATICS(J_DCT_METHOD);
00044 //@}
00045 
00046 Config* config=NULL;
00047 const std::locale& Config::curLocale=std::locale::classic();
00048 
00049 const std::string ConfigDictionary::msPrefix="Model";
00050 const std::string ConfigDictionary::msSep=":";
00051 //const std::string ConfigDictionary::msNum="#";
00052 std::string ConfigDictionary::curModel;
00053 
00054 const char * Config::xmlIntro1="<?xml version";
00055 const char * Config::xmlIntro2="<!DOCTYPE ";
00056 const char * Config::xmlIntro3="<plist";
00057 
00058 const char * Config::transport_names[] = { "UDP", "TCP", NULL };
00059 
00060 const char * Config::vision_config::dct_method_names[4] = { "islow", "ifast", "float", NULL };
00061 const char * Config::vision_config::encoding_names[Config::vision_config::NUM_ENCODINGS+1] = { "color", "grayscale", "depth", NULL };
00062 const char * Config::vision_config::RawCamConfig::compression_names[Config::vision_config::RawCamConfig::NUM_COMPRESSIONS+1] = { "none", "jpeg", NULL };
00063 const char * Config::vision_config::DepthCamConfig::compression_names[Config::vision_config::DepthCamConfig::NUM_COMPRESSIONS+1] = { "none", "jpeg", NULL };
00064 const char * Config::vision_config::SegCamConfig::compression_names[Config::vision_config::SegCamConfig::NUM_COMPRESSIONS+1] = { "none", "rle", NULL };
00065 
00066 const char * Config::main_config::consoleModeNames[Config::main_config::NUM_CONSOLE_MODES+1] = { "controller", "textmsg", "auto", NULL };
00067 
00068 bool ConfigDictionary::loadXMLNode(const std::string& key, xmlNode* val, const std::string& comment) {
00069   // if not a model specific key, just load it normally
00070   if(key.substr(0,msPrefix.size())!=msPrefix)
00071     return plist::Dictionary::loadXMLNode(key,val,comment);
00072   
00073   //otherwise, strip off enumeration serial number (if any)
00074   //std::string k=key.substr(0,key.find(msNum));
00075   const std::string& k=key;
00076   
00077   //see if additional parameters in the key
00078   if(k.size()<=msPrefix.size() || k.substr(msPrefix.size(),msSep.size())!=msSep) {
00079     // not a valid model specific string, treat as normal key
00080     return plist::Dictionary::loadXMLNode(key,val,comment);
00081   }
00082   
00083   //prefix plus pattern (at least) -- check pattern
00084   std::string::size_type patStart=msPrefix.size()+msSep.size();
00085   std::string::size_type patEnd=k.find(msSep,patStart);
00086   std::string pattern=k.substr(patStart,patEnd-patStart);
00087   if(!matchNoCase(curModel,pattern))
00088     return false; //doesn't apply to the current model
00089   if(patEnd==std::string::npos) {
00090     // prefix plus pattern only -- load subnodes
00091     loadXML(val);
00092     return true;
00093   } else {
00094     // prefix plus pattern plus single item -- load it
00095     return plist::Dictionary::loadXMLNode(k.substr(patEnd+1),val,comment);
00096   }
00097 }
00098 bool ConfigDictionary::saveOverXMLNode(xmlNode* k, xmlNode* val, const std::string& key, std::string comment, const std::string& indentStr, std::set<std::string>& seen) const {
00099   // if not a model specific key, just load it normally
00100   if(key.substr(0,msPrefix.size())!=msPrefix)
00101     return plist::Dictionary::saveOverXMLNode(k,val,key,comment,indentStr,seen);
00102   
00103   //otherwise, strip off enumeration serial number (if any)
00104   //std::string ks=key.substr(0,key.find(msNum));
00105   const std::string& ks=key;
00106   
00107   //see if additional parameters in the key
00108   if(ks.size()<=msPrefix.size() || ks.substr(msPrefix.size(),msSep.size())!=msSep) {
00109     // not a valid model specific string, treat as normal key
00110     return plist::Dictionary::saveOverXMLNode(k,val,key,comment,indentStr,seen);
00111   }
00112   
00113   //prefix plus pattern (at least) -- check pattern
00114   std::string::size_type patStart=msPrefix.size()+msSep.size();
00115   std::string::size_type patEnd=ks.find(msSep,patStart);
00116   std::string pattern=ks.substr(patStart,patEnd-patStart);
00117   if(!matchNoCase(curModel,pattern))
00118     return true; //doesn't apply to the current model (still return true though because it's not an error, just a no-op)
00119   if(patEnd==std::string::npos) {
00120     // prefix plus pattern only -- save subnodes
00121     saveXML(val,true,seen);
00122     return true;
00123   } else {
00124     // prefix plus pattern plus single item -- save over it
00125     const std::string skey=ks.substr(patEnd+1);
00126     return plist::Dictionary::saveOverXMLNode(k,val,skey,comment,indentStr,seen);
00127   }
00128 }
00129 
00130 bool ConfigDictionary::matchNoCase(const std::string& model, const std::string& pattern) {
00131   unsigned int i=0;
00132   if(i==pattern.size() && i==model.size())
00133     return true;
00134   if(i==pattern.size() || i==model.size())
00135     return false;
00136   while(pattern[i]!='*') {
00137     if(toupper(pattern[i])!=toupper(model[i]))
00138       return false;
00139     i++;
00140     if(i==pattern.size() && i==model.size())
00141       return true;
00142     if(i==pattern.size() || i==model.size())
00143       return false;
00144   }
00145   i=pattern.size()-1;
00146   unsigned int j=model.size()-1;
00147   while(pattern[i]!='*') {
00148     if(toupper(pattern[i])!=toupper(model[j]))
00149       return false;
00150     i--; j--;
00151   }
00152   return true;
00153 }
00154 
00155 void Config::saveXML(xmlNode* node, bool onlyOverwrite, std::set<std::string>& seen) const {
00156   if(node==NULL)
00157     return;
00158   if(node->children==NULL) {
00159     std::string indentStr=getIndentationPrefix(node);
00160     const char* headerComment="\n"
00161       "##################################################################\n"
00162       "######################   Tekkotsu config   #######################\n"
00163       "##################################################################\n"
00164       "\n"
00165       "This is an XML-based format using the Property List (plist) layout.\n"
00166       "\n"
00167       "As a slight extension to standard plists, you can specify\n"
00168       "model-specific settings by prepending a key with:\n"
00169       "   Model:MODEL_PATTERN:KEY_NAME\n"
00170       "For example, to use different 'thresh' settings on the ERS-2xx\n"
00171       "series vs. the ERS-7 model, you would use the keys:\n"
00172       "   Model:ERS-2*:thresh\n"
00173       "   Model:ERS-7:thresh\n"
00174       "You can filter several items in a group by leaving off the second\n"
00175       "':' and name, and providing a dictionary as the value.  If the\n"
00176       "model matches, all entries from the dictionary are imported into\n"
00177       "the parent dictionary.\n"
00178       "\n"
00179       "You can change these settings at run time from the Controller\n"
00180       "by entering the command:\n"
00181       "   !set section_name.variable=value\n"
00182       "or by using the configuration editor found in the \"File Access\"\n"
00183       "menu.  Paths are assumed to be relative to the 'project/ms/'\n"
00184       "directory.  For example, the primary configuration file would be\n"
00185       "referenced as 'config/tekkotsu.xml'\n"
00186       "\n"
00187       "See also the 'simulator.xml' file, which provides you the ability\n"
00188       "to override settings when running in the simulator\n";
00189     xmlAddChild(node,xmlNewComment((const xmlChar*)headerComment));
00190     xmlAddChild(node,xmlNewText((const xmlChar*)("\n"+indentStr).c_str()));
00191   }
00192   ConfigDictionary::saveXML(node,onlyOverwrite,seen);
00193 }
00194 
00195 unsigned int Config::loadBuffer(const char buf[], unsigned int len, const char* filename) {
00196   if(strncmp(buf,xmlIntro1,strlen(xmlIntro1))==0 || strncmp(buf,xmlIntro2,strlen(xmlIntro2))==0 || strncmp(buf,xmlIntro3,strlen(xmlIntro3))==0)
00197     return plist::Dictionary::loadBuffer(buf,len,filename);
00198   return loadOldFormat(buf,len);
00199 }
00200 
00201 unsigned int Config::loadFile(const char* filename) {
00202   std::string path=portPath(filename);
00203   bool isXML;
00204   {
00205     std::ifstream in(path.c_str());
00206     std::string line;
00207     std::getline(in,line);
00208     isXML = (strncmp(line.c_str(),xmlIntro1,strlen(xmlIntro1))==0 || strncmp(line.c_str(),xmlIntro2,strlen(xmlIntro2))==0 || strncmp(line.c_str(),xmlIntro3,strlen(xmlIntro3))==0);
00209   }
00210   if(isXML) {
00211     return plist::Dictionary::loadFile(path.c_str());
00212   }
00213   FILE* fp = fopen(path.c_str(), "r");
00214   if (fp==NULL) {
00215     std::cerr << "ERROR: Config could not open file '" << path.c_str() << "' for loading." << std::endl;
00216     return 0;
00217   }
00218   try {
00219     unsigned int ans=loadOldFormat(fp);
00220     fclose(fp);
00221     return ans;
00222   } catch(...) {
00223     fclose(fp);
00224     throw;
00225   }
00226 }
00227 
00228 unsigned int Config::loadFileStream(FILE* f, const char* filename) {
00229   std::string line;
00230   char c=fgetc(f);
00231   while(c!='\n' && c!=EOF) {
00232     line+=c;
00233     c=fgetc(f);
00234   }
00235   bool isXML = (strncmp(line.c_str(),xmlIntro1,strlen(xmlIntro1))==0 || strncmp(line.c_str(),xmlIntro2,strlen(xmlIntro2))==0 || strncmp(line.c_str(),xmlIntro3,strlen(xmlIntro3))==0);
00236   if(fseek(f,line.size()+1,SEEK_CUR)!=0) {
00237 #ifdef PLATFORM_APERIOS
00238     int merrno=errno;
00239     if (filename)
00240       std::cerr << "File " << filename << ":";
00241     std::cerr << "Warning: error on seek: " << strerror(merrno) << std::endl;
00242 #else
00243     char err[100];
00244     if (strerror_r(errno,err,100) );   // if (...); suppresses "unused return value" warning about strerror_r
00245     if (filename)
00246       std::cerr << "File " << filename << ":";
00247     std::cerr << "Warning: error on seek: " << err << std::endl;
00248 #endif
00249   }
00250   if(isXML)
00251     return plist::Dictionary::loadFileStream(f,filename);
00252   return loadOldFormat(f);
00253 }
00254 
00255 void Config::setFileSystemRoot(const std::string& fsr) {
00256 #ifdef PLATFORM_APERIOS
00257   fsRoot=fsr;
00258 #else
00259   char buf[MAXPATHLEN+2];
00260   if(getcwd(buf,MAXPATHLEN+2)==NULL)
00261     perror("Config::setFileSystemRoot(): getcwd");
00262   buf[MAXPATHLEN+1]='\0';
00263   std::string sbuf(buf);
00264   if(sbuf[sbuf.size()-1]!='/' && fsr[0]!='/')
00265     fsRoot=sbuf+'/'+fsr;
00266   else if(sbuf[sbuf.size()-1]=='/' && fsr[0]=='/')
00267     fsRoot=sbuf+fsr.substr(1);
00268   else
00269     fsRoot=sbuf+fsr;
00270 #endif
00271 }
00272 
00273 
00274 std::string Config::portPath(const std::string& path) const {
00275   if(fsRoot.size()==0)
00276     return path;
00277   if(path.size()==0)
00278     return fsRoot;
00279   if(path[0]=='/')
00280     return path;
00281   if(path.substr(0,fsRoot.size())==fsRoot)
00282     return path;
00283   else if(fsRoot[fsRoot.size()-1]=='/')
00284       return fsRoot+path;
00285     else
00286       return fsRoot+'/'+path;
00287 }
00288 
00289 void* Config::setValue(const std::string& section, std::string key, const std::string& value) {
00290   if(section=="vision") {
00291     if (key=="resolution") {
00292       if (value=="full") {
00293         vision.resolution=1;
00294       } else if (value=="half") {
00295         vision.resolution=2;
00296       } else if (value=="quarter") {
00297         vision.resolution=3;
00298       }
00299       return &vision.resolution;
00300     } else if (key=="rawcam_encoding") {
00301       if (value=="color") {
00302         vision.rawcam.encoding=vision_config::ENCODE_COLOR;
00303         vision.rawcam.channel=RawCameraGenerator::CHAN_Y;
00304       } else if (value=="y_only") {
00305         vision.rawcam.encoding=vision_config::ENCODE_SINGLE_CHANNEL;
00306         vision.rawcam.channel=RawCameraGenerator::CHAN_Y;
00307       } else if (value=="uv_only") {
00308         vision.rawcam.encoding=vision_config::ENCODE_COLOR;
00309         vision.rawcam.channel=-1;
00310       } else if (value=="u_only") {
00311         vision.rawcam.encoding=vision_config::ENCODE_SINGLE_CHANNEL;
00312         vision.rawcam.channel=RawCameraGenerator::CHAN_U;
00313       } else if (value=="v_only") {
00314         vision.rawcam.encoding=vision_config::ENCODE_SINGLE_CHANNEL;
00315         vision.rawcam.channel=RawCameraGenerator::CHAN_V;
00316       } else if (value=="y_dx_only") {
00317         vision.rawcam.encoding=vision_config::ENCODE_SINGLE_CHANNEL;
00318         vision.rawcam.channel=RawCameraGenerator::CHAN_Y_DX;
00319       } else if (value=="y_dy_only") {
00320         vision.rawcam.encoding=vision_config::ENCODE_SINGLE_CHANNEL;
00321         vision.rawcam.channel=RawCameraGenerator::CHAN_Y_DY;
00322       } else if (value=="y_dxdy_only") {
00323         vision.rawcam.encoding=vision_config::ENCODE_SINGLE_CHANNEL;
00324         vision.rawcam.channel=RawCameraGenerator::CHAN_Y_DXDY;
00325       }
00326       return &vision.rawcam.encoding;
00327     } else if (key=="focal_len_x" || key=="focal_len_y"
00328        || key=="principle_point_x" || key=="principle_point_y"
00329        || key=="skew" || key=="kc1_r2" || key=="kc2_r4"
00330        || key=="kc5_r6" || key=="kc3_tan1" || key=="kc4_tan2"
00331        || key=="calibration_res_x" || key=="calibration_res_y" )
00332     {
00333       key="calibration."+key;
00334     } else if (key.compare(0,7,"rawcam_")==0) {
00335       key[6]='.';
00336     } else if (key.compare(0,9,"depthcam_")==0) {
00337       key[8]='.';
00338     } else if (key.compare(0,7,"segcam_")==0) {
00339       key[6]='.';
00340     } else if (key.compare(0,10,"regioncam_")==0) {
00341       key[9]='.';
00342     } else if (key=="region_port" || key=="region_transport") {
00343       key.replace(0,7,"regioncam.");
00344     }
00345   } else if(section=="motion") {
00346     std::string calibratePrefix="calibrate:";
00347     if (key.substr(0,calibratePrefix.size())==calibratePrefix) {
00348       std::string keyval = key.substr(calibratePrefix.size());
00349       OutputConfig<plist::Primitive<float> >::const_iterator it = motion.calibration_scale.findEntry(keyval);
00350       plist::Primitive<float>* prim = NULL;
00351       if(it!=motion.calibration_scale.end())
00352         prim = dynamic_cast<plist::Primitive<float> *>(it->second);
00353       if(prim==NULL) {
00354         std::cout << "WARNING: Could not match '" << key.substr(10) << "' as calibration parameter" << std::endl;
00355         return NULL;
00356       }
00357       //std::cout << "setting " << key.substr(10) << " to " << value << std::endl;
00358       prim->set(value);
00359       *prim = 1 / *(prim);
00360       return prim;
00361     }
00362   } else if(section=="sound") {
00363     if (key=="streaming.mic_bits") {
00364       key="streaming.mic_sample_bits";
00365     } else if (key=="streaming.speaker_frame_len") {
00366       key="streaming.speaker_frame_length";
00367     }
00368   } else if(section=="main") {
00369     if (key=="debug_level" || key=="error_level" || key=="verbose_level")
00370       return NULL; //drop, unused items
00371   }
00372   
00373   plist::Collection * sct=dynamic_cast<plist::Collection*>(resolveEntry(section));
00374   if(sct==NULL)
00375     return NULL;
00376   plist::ObjectBase * entry=sct->resolveEntry(key);
00377   if(entry==NULL) {
00378     std::cerr << "Unknown " << section << " key '" << key << "' for value " << value << std::endl;
00379     return NULL;
00380   } else if(plist::PrimitiveBase * prim=dynamic_cast<plist::PrimitiveBase*>(entry)) {
00381     try {
00382       prim->set(value);
00383     } catch(...) {
00384       std::cerr << "Error setting " << section << " key '" << key << "' to value " << value << std::endl;
00385       return NULL;
00386     }
00387     return prim;
00388   } else if(plist::ArrayBase::StringConversion * arr=dynamic_cast<plist::ArrayBase::StringConversion*>(entry)) {
00389     try {
00390       arr->addValue(value);
00391     } catch(...) {
00392       std::cerr << "Error setting " << section << " key '" << key << "' to value " << value << std::endl;
00393       return NULL;
00394     }
00395     return arr;
00396   } else {
00397     std::cerr << "Unknown type of " << section << " key '" << key << "' for value " << value << std::endl;
00398     return NULL;
00399   }
00400 }
00401 
00402 unsigned int Config::loadOldFormat(const char buf[], unsigned int len) {
00403   if (buf==NULL) return 0;
00404   unsigned int numRead=0, used=0;
00405   char line[256];
00406   unsigned int lineno=0;
00407   std::vector<std::string> modelStack;
00408   bool ignoring=false;
00409   std::string section="invalid";
00410   while (numRead<len && (used=sscanf(&buf[numRead],"%255[^\n]\n", line))!=(unsigned int)EOF) {
00411     numRead+=used;
00412     parseLine(line,++lineno,modelStack,ignoring,section);
00413   }
00414   return numRead;
00415 }
00416 
00417 unsigned int Config::loadOldFormat(FILE* fp) {
00418   if (fp==NULL) return 0;
00419   unsigned int numRead=0;
00420   char line[256];
00421   unsigned int lineno=0;
00422   std::vector<std::string> modelStack;
00423   bool ignoring=false;
00424   std::string section="invalid";
00425   while (fgets(line,256,fp)!=NULL) {
00426     numRead+=strlen(line);
00427     parseLine(line,++lineno,modelStack,ignoring,section);
00428   }
00429   return numRead;
00430 }
00431 
00432 void Config::parseLine(const char buf[], unsigned int lineno, std::vector<std::string>& modelStack, bool& ignoring, std::string& section) {
00433   char key[30], value[50];
00434   while(std::isspace(*buf))
00435     buf++;
00436   if(buf[0]=='#')
00437     return;
00438   if (sscanf(buf,"<%29[^>]>",key)>0) {
00439     if(key[0]=='/') {
00440       if(modelStack.size()==0) {
00441         printf("WARNING: not in a model specific section, line %d\n",lineno);
00442         return;
00443       }
00444       bool subset=matchNoCase(&key[1],modelStack.back());
00445       bool superset=matchNoCase(modelStack.back(),&key[1]);
00446       if(subset && superset) {
00447         //printf("leaving modelsection %s\n",modelStack.back().c_str());
00448         modelStack.pop_back();
00449       } else if(superset) {
00450         while(modelStack.size()>0) {
00451           //printf("leaving modelsection %s (==%s)\n",modelStack.back().c_str(),&key[1]);
00452           modelStack.pop_back();
00453           if(!matchNoCase(modelStack.back(),&key[1]))
00454             break;
00455         }
00456       } else
00457         printf("WARNING: config model mismatch, line %d\n",lineno);
00458       
00459       ignoring=false; //scan through current model selection stack, see if we're still ignoring
00460       for(unsigned int i=0; i<modelStack.size(); i++)
00461         if(!matchNoCase(curModel,modelStack[i])) {
00462           ignoring=true;
00463           break;
00464         }
00465           //printf("ignoring==%d\n",ignoring);
00466           
00467     } else {
00468       modelStack.push_back(key);
00469       //printf("entering section %s\n",modelStack.back().c_str());
00470       ignoring=ignoring || !matchNoCase(curModel,key);
00471       //printf("ignoring==%d\n",ignoring);
00472     }
00473   } else if(!ignoring) {
00474     if (sscanf(buf,"[%29[^]]]",key)>0) {
00475       section=key;
00476       std::transform(section.begin(), section.end(), section.begin(), (int(*)(int)) std::tolower);
00477       //std::cout << "now parsing section " << section << std::endl;
00478       plist::Collection * sct=dynamic_cast<plist::Collection*>(resolveEntry(section));
00479       if(sct==NULL)
00480         std::cerr << "ERROR: Unknown configuration section " << section << std::endl;
00481     } else if (sscanf(buf,"%29[^ =] =%49s",key,value)>1) {
00482       //printf("setValue(%d,'%s','%s');\n",section,key,value);
00483       //void * var=
00484       setValue(section, string_util::trim(key), string_util::trim(value));
00485       /*if(var==NULL) {
00486         std::cerr << "WARNING: Config::setValue(\"" << section << "\", \"" << string_util::trim(key) << "\", \"" << string_util::trim(key) << "\") returned NULL" << std::endl;
00487         std::cerr << "         This probably indicates the section or key is invalid." << std::endl;
00488       }*/
00489     }
00490   }
00491 }
00492 
00493 void Config::vision_config::aspectRatioChanged() {
00494   if(aspectRatio>1) {
00495     x_range=1;
00496     y_range=1/aspectRatio;
00497   } else {
00498     x_range=aspectRatio;
00499     y_range=1;
00500   }
00501   x_focalLen = x_range / std::tan(CameraHorizFOV/2);
00502   y_focalLen = y_range / std::tan(CameraVertFOV/2);
00503 }
00504 
00505 void Config::vision_config::computeRay(float x, float y, float& r_x, float& r_y, float& r_z) {
00506   float correctedX=x, correctedY=y;
00507 #if defined(TGT_HAS_CAMERA) || defined(TGT_HAS_WEBCAM)
00508   correctedX = CameraHomography(0,0)*x + CameraHomography(0,1)*y + CameraHomography(0,2);
00509   correctedY = CameraHomography(1,0)*x + CameraHomography(1,1)*y + CameraHomography(1,2);
00510   float z = CameraHomography(2,0)*x + CameraHomography(2,1)*y + CameraHomography(2,2);
00511   if(z == 0)
00512     z = 1;
00513   
00514   correctedX /= z;
00515   correctedY /= z;
00516 #endif
00517   
00518   r_x = correctedX / x_focalLen;
00519   r_y = correctedY / y_focalLen;
00520   r_z = 1;
00521 }
00522 
00523 void Config::vision_config::computePixel(float r_x, float r_y, float r_z, float& x, float& y) {
00524   if ( r_z == 0 ) {
00525     x = y = 0;
00526     return;
00527   }
00528   x = r_x * (x_focalLen / r_z);
00529   y = r_y * (y_focalLen / r_z) * aspectRatio;
00530 }
00531 
00532 void Config::vision_config::computePixelCorrected(float r_x, float r_y, float r_z, float& x, float &y) {
00533   computePixel(r_x, r_y, r_z, x, y);
00534   
00535 #if defined(TGT_HAS_CAMERA) || defined(TGT_HAS_WEBCAM)
00536   float tX = x, tY = y, z;
00537   //HACK: DO NOT WANT TO COMPUTE INVERSE EVERY RUN
00538   fmat::Matrix<3,3> invH = invert(CameraHomography);
00539   x = invH(0,0)*tX + invH(0,1)*tY + invH(0,2);
00540   y = invH(1,0)*tX + invH(1,1)*tY + invH(1,2);
00541   z = invH(2,0)*tX + invH(2,1)*tY + invH(2,2);
00542   
00543   x /= z;
00544   y /= z;
00545   
00546 #endif
00547 }
00548 
00549 std::string Config::searchPath(const std::string& name,  const std::string& root) const {
00550   if(name[0]=='/')
00551     return portPath(name);
00552   struct stat statbuf;
00553   
00554   std::string tgtDir;
00555   if(root[root.size()-1]=='/')
00556     tgtDir=portPath(root+string_util::makeLower(TargetName));
00557   else
00558     tgtDir=portPath(root+"/"+string_util::makeLower(TargetName));
00559   std::string full = tgtDir+"/"+name;
00560   
00561   std::string candidate=full;
00562   if(::stat(candidate.c_str(),&statbuf)==0)
00563     return candidate;
00564   
00565   if(root[root.size()-1]=='/')
00566     candidate=portPath(root+name);
00567   else
00568     candidate=portPath(root+"/"+name);
00569   if(::stat(candidate.c_str(),&statbuf)==0)
00570     return candidate;
00571   
00572   candidate=name;
00573   if(::stat(candidate.c_str(),&statbuf)==0)
00574     return candidate;
00575   
00576   if(::stat(tgtDir.c_str(),&statbuf)!=0) {
00577     // missing target specific sub-directory, create in case we are about to write
00578     if(mkdir(tgtDir.c_str(),0777)!=0) {
00579       perror(("Could not create target directory '"+tgtDir+"'").c_str());
00580     }
00581   }
00582   return full;
00583 }
00584 
00585 
00586 /*! @file
00587  * @brief Implements Config, which provides global access to system configuration information
00588  * @author ejt (Creator)
00589  * @author alokl (Original configuration system)
00590  */
00591 

Tekkotsu v5.1CVS
Generated Mon May 9 04:58:37 2016 by Doxygen 1.6.3