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 <string>
00011 #ifdef PLATFORM_APERIOS
00012 #  include <OPENR/OPENRAPI.h>
00013 #else
00014 #include <sys/param.h>
00015 #include <unistd.h>
00016 #endif
00017 #include "Shared/ProjectInterface.h"
00018 #include "Shared/string_util.h"
00019 //#include "Behaviors/Mon/RawCamBehavior.h"
00020 #include "Vision/RawCameraGenerator.h"
00021 //#include "Vision/SegmentedColorGenerator.h"
00022 //#include "Wireless/Socket.h"
00023 #include "Shared/debuget.h"
00024 
00025 /*! Explicitly declaring these values allows us to relegate storage to a 
00026 *  single translation unit instead of having it reallocated every time
00027 *  the class is referenced.  This actually adds up to megabytes of
00028 *  symbols (especially with debugging info). */
00029 //! @name plist::NamedEnumeration statics
00030 //! provides string names for enumeration values (see plist::NamedEnumeration and #INSTANTIATE_NAMEDENUMERATION_STATICS)
00031 INSTANTIATE_NAMEDENUMERATION_STATICS(Config::main_config::consoleMode_t);
00032 INSTANTIATE_NAMEDENUMERATION_STATICS(Config::sound_config::volume_levels);
00033 INSTANTIATE_NAMEDENUMERATION_STATICS(Config::transports);
00034 INSTANTIATE_NAMEDENUMERATION_STATICS(Config::vision_config::RawCamConfig::compression_t);
00035 INSTANTIATE_NAMEDENUMERATION_STATICS(Config::vision_config::RawCamConfig::encoding_t);
00036 INSTANTIATE_NAMEDENUMERATION_STATICS(Config::vision_config::SegCamConfig::compression_t);
00037 INSTANTIATE_NAMEDENUMERATION_STATICS(Config::vision_config::gain_levels);
00038 INSTANTIATE_NAMEDENUMERATION_STATICS(Config::vision_config::shutter_speeds);
00039 INSTANTIATE_NAMEDENUMERATION_STATICS(Config::vision_config::white_balance_levels);
00040 INSTANTIATE_NAMEDENUMERATION_STATICS(J_DCT_METHOD);
00041 //@}
00042 
00043 Config* config=NULL;
00044 const std::locale& Config::curLocale=std::locale::classic();
00045 
00046 const std::string ConfigDictionary::msPrefix="Model";
00047 const std::string ConfigDictionary::msSep=":";
00048 //const std::string ConfigDictionary::msNum="#";
00049 std::string ConfigDictionary::curModel;
00050 
00051 const char * Config::xmlIntro1="<?xml version";
00052 const char * Config::xmlIntro2="<!DOCTYPE ";
00053 const char * Config::xmlIntro3="<plist";
00054 
00055 const char * Config::transport_names[] = { "UDP", "TCP", "" };
00056 
00057 const char * Config::vision_config::dct_method_names[4] = { "islow", "ifast", "float", "" };
00058 const char * Config::vision_config::RawCamConfig::encoding_names[Config::vision_config::RawCamConfig::NUM_ENCODINGS+1] = { "color", "grayscale", "" };
00059 const char * Config::vision_config::RawCamConfig::compression_names[Config::vision_config::RawCamConfig::NUM_COMPRESSIONS+1] = { "none", "jpeg", "" };
00060 const char * Config::vision_config::SegCamConfig::compression_names[Config::vision_config::SegCamConfig::NUM_COMPRESSIONS+1] = { "none", "rle", "" };
00061 
00062 const char * Config::main_config::consoleModeNames[Config::main_config::NUM_CONSOLE_MODES+1] = { "controller", "textmsg", "auto", "" };
00063 
00064 bool ConfigDictionary::loadXMLNode(const std::string& key, xmlNode* val, const std::string& comment) {
00065   // if not a model specific key, just load it normally
00066   if(key.substr(0,msPrefix.size())!=msPrefix)
00067     return plist::Dictionary::loadXMLNode(key,val,comment);
00068   
00069   //otherwise, strip off enumeration serial number (if any)
00070   //std::string k=key.substr(0,key.find(msNum));
00071   const std::string& k=key;
00072   
00073   //see if additional parameters in the key
00074   if(k.size()<=msPrefix.size() || k.substr(msPrefix.size(),msSep.size())!=msSep) {
00075     // not a valid model specific string, treat as normal key
00076     return plist::Dictionary::loadXMLNode(key,val,comment);
00077   }
00078   
00079   //prefix plus pattern (at least) -- check pattern
00080   std::string::size_type patStart=msPrefix.size()+msSep.size();
00081   std::string::size_type patEnd=k.find(msSep,patStart);
00082   std::string pattern=k.substr(patStart,patEnd-patStart);
00083   if(!matchNoCase(curModel,pattern))
00084     return false; //doesn't apply to the current model
00085   if(patEnd==std::string::npos) {
00086     // prefix plus pattern only -- load subnodes
00087     loadXML(val);
00088     return true;
00089   } else {
00090     // prefix plus pattern plus single item -- load it
00091     return plist::Dictionary::loadXMLNode(k.substr(patEnd+1),val,comment);
00092   }
00093 }
00094 bool ConfigDictionary::saveOverXMLNode(xmlNode* k, xmlNode* val, const std::string& key, std::string comment, const std::string& indentStr, std::set<std::string>& seen) const {
00095   // if not a model specific key, just load it normally
00096   if(key.substr(0,msPrefix.size())!=msPrefix)
00097     return plist::Dictionary::saveOverXMLNode(k,val,key,comment,indentStr,seen);
00098   
00099   //otherwise, strip off enumeration serial number (if any)
00100   //std::string ks=key.substr(0,key.find(msNum));
00101   const std::string& ks=key;
00102   
00103   //see if additional parameters in the key
00104   if(ks.size()<=msPrefix.size() || ks.substr(msPrefix.size(),msSep.size())!=msSep) {
00105     // not a valid model specific string, treat as normal key
00106     return plist::Dictionary::saveOverXMLNode(k,val,key,comment,indentStr,seen);
00107   }
00108   
00109   //prefix plus pattern (at least) -- check pattern
00110   std::string::size_type patStart=msPrefix.size()+msSep.size();
00111   std::string::size_type patEnd=ks.find(msSep,patStart);
00112   std::string pattern=ks.substr(patStart,patEnd-patStart);
00113   if(!matchNoCase(curModel,pattern))
00114     return true; //doesn't apply to the current model (still return true though because it's not an error, just a no-op)
00115   if(patEnd==std::string::npos) {
00116     // prefix plus pattern only -- save subnodes
00117     saveXML(val,true,seen);
00118     return true;
00119   } else {
00120     // prefix plus pattern plus single item -- save over it
00121     const std::string skey=ks.substr(patEnd+1);
00122     return plist::Dictionary::saveOverXMLNode(k,val,skey,comment,indentStr,seen);
00123   }
00124 }
00125 
00126 bool ConfigDictionary::matchNoCase(const std::string& model, const std::string& pattern) {
00127   unsigned int i=0;
00128   if(i==pattern.size() && i==model.size())
00129     return true;
00130   if(i==pattern.size() || i==model.size())
00131     return false;
00132   while(pattern[i]!='*') {
00133     if(toupper(pattern[i])!=toupper(model[i]))
00134       return false;
00135     i++;
00136     if(i==pattern.size() && i==model.size())
00137       return true;
00138     if(i==pattern.size() || i==model.size())
00139       return false;
00140   }
00141   i=pattern.size()-1;
00142   unsigned int j=model.size()-1;
00143   while(pattern[i]!='*') {
00144     if(toupper(pattern[i])!=toupper(model[j]))
00145       return false;
00146     i--; j--;
00147   }
00148   return true;
00149 }
00150 
00151 void Config::saveXML(xmlNode* node, bool onlyOverwrite, std::set<std::string>& seen) const {
00152   if(node==NULL)
00153     return;
00154   if(node->children==NULL) {
00155     std::string indentStr=getIndentationPrefix(node);
00156     const char* headerComment="\n"
00157       "##################################################################\n"
00158       "######################   Tekkotsu config   #######################\n"
00159       "##################################################################\n"
00160       //note the use of "s to break the string to avoid keyword substitution in the header...
00161       "#####################   $" "Revision:  $   ######################\n"
00162       "################   $" "Date:  $   ################\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) {
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);
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   FILE* fp = fopen(path.c_str(), "r");
00213   if (fp==NULL) {
00214     std::cerr << "ERROR: Config could not open file '" << path.c_str() << "' for loading." << std::endl;
00215     return 0;
00216   }
00217   try {
00218     unsigned int ans=loadOldFormat(fp);
00219     fclose(fp);
00220     return ans;
00221   } catch(...) {
00222     fclose(fp);
00223     throw;
00224   }
00225 }
00226 
00227 unsigned int Config::loadFileStream(FILE* f) {
00228   std::string line;
00229   char c=fgetc(f);
00230   while(c!='\n' && c!=EOF) {
00231     line+=c;
00232     c=fgetc(f);
00233   }
00234   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);
00235   if(fseek(f,line.size()+1,SEEK_CUR)!=0) {
00236 #ifdef PLATFORM_APERIOS
00237     int merrno=errno;
00238     std::cerr << "Warning: error on seek: " << strerror(merrno) << std::endl;
00239 #else
00240     char err[100];
00241     strerror_r(errno,err,100);
00242     std::cerr << "Warning: error on seek: " << err << std::endl;
00243 #endif
00244   }
00245   if(isXML)
00246     return plist::Dictionary::loadFileStream(f);
00247   return loadOldFormat(f);
00248 }
00249 
00250 void Config::setFileSystemRoot(const std::string& fsr) {
00251 #ifdef PLATFORM_APERIOS
00252   fsRoot=fsr;
00253 #else
00254   char buf[MAXPATHLEN+2];
00255   if(getcwd(buf,MAXPATHLEN+2)==NULL)
00256     perror("Config::setFileSystemRoot(): getcwd");
00257   buf[MAXPATHLEN+1]='\0';
00258   std::string sbuf(buf);
00259   if(sbuf[sbuf.size()-1]!='/' && fsr[0]!='/')
00260     fsRoot=sbuf+'/'+fsr;
00261   else if(sbuf[sbuf.size()-1]=='/' && fsr[0]=='/')
00262     fsRoot=sbuf+fsr.substr(1);
00263   else
00264     fsRoot=sbuf+fsr;
00265 #endif
00266 }
00267 
00268 
00269 std::string Config::portPath(const std::string& path) const {
00270   if(fsRoot.size()==0)
00271     return path;
00272   if(path.size()==0)
00273     return fsRoot;
00274   if(path.substr(0,fsRoot.size())==fsRoot)
00275     return path;
00276   else if(fsRoot[fsRoot.size()-1]=='/') {
00277     if(path[0]!='/')
00278       return fsRoot+path;
00279     else
00280       return fsRoot+path.substr(1);
00281   } else {
00282     if(path[0]!='/')
00283       return fsRoot+'/'+path;
00284     else
00285       return fsRoot+path;
00286   }
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::RawCamConfig::ENCODE_COLOR;
00303         vision.rawcam.channel=RawCameraGenerator::CHAN_Y;
00304       } else if (value=="y_only") {
00305         vision.rawcam.encoding=vision_config::RawCamConfig::ENCODE_SINGLE_CHANNEL;
00306         vision.rawcam.channel=RawCameraGenerator::CHAN_Y;
00307       } else if (value=="uv_only") {
00308         vision.rawcam.encoding=vision_config::RawCamConfig::ENCODE_COLOR;
00309         vision.rawcam.channel=-1;
00310       } else if (value=="u_only") {
00311         vision.rawcam.encoding=vision_config::RawCamConfig::ENCODE_SINGLE_CHANNEL;
00312         vision.rawcam.channel=RawCameraGenerator::CHAN_U;
00313       } else if (value=="v_only") {
00314         vision.rawcam.encoding=vision_config::RawCamConfig::ENCODE_SINGLE_CHANNEL;
00315         vision.rawcam.channel=RawCameraGenerator::CHAN_V;
00316       } else if (value=="y_dx_only") {
00317         vision.rawcam.encoding=vision_config::RawCamConfig::ENCODE_SINGLE_CHANNEL;
00318         vision.rawcam.channel=RawCameraGenerator::CHAN_Y_DX;
00319       } else if (value=="y_dy_only") {
00320         vision.rawcam.encoding=vision_config::RawCamConfig::ENCODE_SINGLE_CHANNEL;
00321         vision.rawcam.channel=RawCameraGenerator::CHAN_Y_DY;
00322       } else if (value=="y_dxdy_only") {
00323         vision.rawcam.encoding=vision_config::RawCamConfig::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,7,"segcam_")==0) {
00337       key[6]='.';
00338     } else if (key.compare(0,10,"regioncam_")==0) {
00339       key[9]='.';
00340     } else if (key=="region_port" || key=="region_transport") {
00341       key.replace(0,7,"regioncam.");
00342     }
00343   } else if(section=="motion") {
00344     std::string calibratePrefix="calibrate:";
00345     if (key.substr(0,calibratePrefix.size())==calibratePrefix) {
00346       std::string keyval = key.substr(calibratePrefix.size());
00347       OutputConfig<plist::Primitive<float> >::const_iterator it = motion.calibration_scale.findEntry(keyval);
00348       plist::Primitive<float>* prim = NULL;
00349       if(it!=motion.calibration_scale.end())
00350         prim = dynamic_cast<plist::Primitive<float> *>(it->second);
00351       if(prim==NULL) {
00352         std::cout << "WARNING: Could not match '" << key.substr(10) << "' as calibration parameter" << std::endl;
00353         return NULL;
00354       }
00355       //std::cout << "setting " << key.substr(10) << " to " << value << std::endl;
00356       prim->set(value);
00357       *prim = 1 / *(prim);
00358       return prim;
00359     }
00360   } else if(section=="sound") {
00361     if (key=="streaming.mic_bits") {
00362       key="streaming.mic_sample_bits";
00363     } else if (key=="streaming.speaker_frame_len") {
00364       key="streaming.speaker_frame_length";
00365     }
00366   } else if(section=="main") {
00367     if (key=="debug_level" || key=="error_level" || key=="verbose_level")
00368       return NULL; //drop, unused items
00369   }
00370   
00371   plist::Collection * sct=dynamic_cast<plist::Collection*>(resolveEntry(section));
00372   if(sct==NULL)
00373     return NULL;
00374   plist::ObjectBase * entry=sct->resolveEntry(key);
00375   if(entry==NULL) {
00376     std::cerr << "Unknown " << section << " key '" << key << "' for value " << value << std::endl;
00377     return NULL;
00378   } else if(plist::PrimitiveBase * prim=dynamic_cast<plist::PrimitiveBase*>(entry)) {
00379     try {
00380       prim->set(value);
00381     } catch(...) {
00382       std::cerr << "Error setting " << section << " key '" << key << "' to value " << value << std::endl;
00383       return NULL;
00384     }
00385     return prim;
00386   } else if(plist::ArrayBase::StringConversion * arr=dynamic_cast<plist::ArrayBase::StringConversion*>(entry)) {
00387     try {
00388       arr->addValue(value);
00389     } catch(...) {
00390       std::cerr << "Error setting " << section << " key '" << key << "' to value " << value << std::endl;
00391       return NULL;
00392     }
00393     return arr;
00394   } else {
00395     std::cerr << "Unknown type of " << section << " key '" << key << "' for value " << value << std::endl;
00396     return NULL;
00397   }
00398 }
00399 
00400 unsigned int Config::loadOldFormat(const char buf[], unsigned int len) {
00401   if (buf==NULL) return 0;
00402   unsigned int numRead=0, used=0;
00403   char line[256];
00404   unsigned int lineno=0;
00405   std::vector<std::string> modelStack;
00406   bool ignoring=false;
00407   std::string section="invalid";
00408   while (numRead<len && (used=sscanf(&buf[numRead],"%255[^\n]\n", line))!=(unsigned int)EOF) {
00409     numRead+=used;
00410     parseLine(line,++lineno,modelStack,ignoring,section);
00411   }
00412   return numRead;
00413 }
00414 
00415 unsigned int Config::loadOldFormat(FILE* fp) {
00416   if (fp==NULL) return 0;
00417   unsigned int numRead=0;
00418   char line[256];
00419   unsigned int lineno=0;
00420   std::vector<std::string> modelStack;
00421   bool ignoring=false;
00422   std::string section="invalid";
00423   while (fgets(line,256,fp)!=NULL) {
00424     numRead+=strlen(line);
00425     parseLine(line,++lineno,modelStack,ignoring,section);
00426   }
00427   return numRead;
00428 }
00429 
00430 void Config::parseLine(const char buf[], unsigned int lineno, std::vector<std::string>& modelStack, bool& ignoring, std::string& section) {
00431   char key[30], value[50];
00432   while(std::isspace(*buf))
00433     buf++;
00434   if(buf[0]=='#')
00435     return;
00436   if (sscanf(buf,"<%29[^>]>",key)>0) {
00437     if(key[0]=='/') {
00438       if(modelStack.size()==0) {
00439         printf("WARNING: not in a model specific section, line %d\n",lineno);
00440         return;
00441       }
00442       bool subset=matchNoCase(&key[1],modelStack.back());
00443       bool superset=matchNoCase(modelStack.back(),&key[1]);
00444       if(subset && superset) {
00445         //printf("leaving modelsection %s\n",modelStack.back().c_str());
00446         modelStack.pop_back();
00447       } else if(superset) {
00448         while(modelStack.size()>0) {
00449           //printf("leaving modelsection %s (==%s)\n",modelStack.back().c_str(),&key[1]);
00450           modelStack.pop_back();
00451           if(!matchNoCase(modelStack.back(),&key[1]))
00452             break;
00453         }
00454       } else
00455         printf("WARNING: config model mismatch, line %d\n",lineno);
00456       
00457       ignoring=false; //scan through current model selection stack, see if we're still ignoring
00458       for(unsigned int i=0; i<modelStack.size(); i++)
00459         if(!matchNoCase(curModel,modelStack[i])) {
00460           ignoring=true;
00461           break;
00462         }
00463           //printf("ignoring==%d\n",ignoring);
00464           
00465     } else {
00466       modelStack.push_back(key);
00467       //printf("entering section %s\n",modelStack.back().c_str());
00468       ignoring=ignoring || !matchNoCase(curModel,key);
00469       //printf("ignoring==%d\n",ignoring);
00470     }
00471   } else if(!ignoring) {
00472     if (sscanf(buf,"[%29[^]]]",key)>0) {
00473       section=key;
00474       std::transform(section.begin(), section.end(), section.begin(), (int(*)(int)) std::tolower);
00475       //std::cout << "now parsing section " << section << std::endl;
00476       plist::Collection * sct=dynamic_cast<plist::Collection*>(resolveEntry(section));
00477       if(sct==NULL)
00478         std::cerr << "ERROR: Unknown configuration section " << section << std::endl;
00479     } else if (sscanf(buf,"%29[^ =] =%49s",key,value)>1) {
00480       //printf("setValue(%d,'%s','%s');\n",section,key,value);
00481       //void * var=
00482       setValue(section, string_util::trim(key), string_util::trim(value));
00483       /*if(var==NULL) {
00484         std::cerr << "WARNING: Config::setValue(\"" << section << "\", \"" << string_util::trim(key) << "\", \"" << string_util::trim(key) << "\") returned NULL" << std::endl;
00485         std::cerr << "         This probably indicates the section or key is invalid." << std::endl;
00486       }*/
00487     }
00488   }
00489 }     
00490 
00491 void Config::vision_config::computeRay(float x, float y, float& r_x, float& r_y, float& r_z) {
00492   x=(x+1)*calibration.calibration_res_x/2;
00493   y=(y+1)*calibration.calibration_res_y/2;
00494   float yd=(y-calibration.principle_point_y)/calibration.focal_len_y;
00495   float xd=(x-calibration.principle_point_x)/calibration.focal_len_x-calibration.skew*yd;
00496   float r2=xd*xd+yd*yd; //estimate of original
00497   float radial=(1 + calibration.kc1_r2*r2 + calibration.kc2_r4*r2*r2 + calibration.kc5_r6*r2*r2*r2);
00498   r_x=(xd - 2*calibration.kc3_tan1*xd*yd - calibration.kc4_tan2*(r2+2*xd*xd))/radial; //estimating tangential component
00499   r_y=(yd - calibration.kc3_tan1*(r2+2*yd*yd) - 2*calibration.kc4_tan2*xd*yd)/radial; //estimating tangential component
00500   r_z=1;
00501 }
00502 
00503 //!provides a pixel hit in image by a ray going through the camera frame
00504 /*! @param[in] r_x x value of the ray
00505 *  @param[in] r_y y value of the ray
00506 *  @param[in] r_z z value of the ray
00507 *  @param[out] x x position in range [-1,1]
00508 *  @param[out] y y position in range [-1,1] */
00509 void Config::vision_config::computePixel(float r_x, float r_y, float r_z, float& x, float& y) {
00510   if(r_z==0) {
00511     x=y=0;
00512     return;
00513   }
00514   r_x/=r_z;
00515   r_y/=r_z;
00516   float r2 = r_x*r_x + r_y*r_y; //'r2' for 'radius squared', not 'ray number 2'
00517   float radial=(1 + calibration.kc1_r2*r2 + calibration.kc2_r4*r2*r2 + calibration.kc5_r6*r2*r2*r2);
00518   float xd = radial*r_x + 2*calibration.kc3_tan1*r_x*r_y + calibration.kc4_tan2*(r2+2*r_x*r_x);
00519   float yd = radial*r_y + calibration.kc3_tan1*(r2+2*r_y*r_y) + 2*calibration.kc4_tan2*r_x*r_y;
00520   x=calibration.focal_len_x*(xd+calibration.skew*yd)+calibration.principle_point_x;
00521   y=calibration.focal_len_y*yd+calibration.principle_point_y;
00522   x=2*x/calibration.calibration_res_x-1;
00523   y=2*y/calibration.calibration_res_y-1;
00524 }
00525 
00526 /*! @file
00527  * @brief Implements Config, which provides global access to system configuration information
00528  * @author ejt (Creator)
00529  * @author alokl (Original configuration system)
00530  *
00531  * $Author: ejt $
00532  * $Name: tekkotsu-4_0 $
00533  * $Revision: 1.71 $
00534  * $State: Exp $
00535  * $Date: 2007/11/16 15:53:49 $
00536  */
00537 

Tekkotsu v4.0
Generated Thu Nov 22 00:54:52 2007 by Doxygen 1.5.4