AlgorithmSelector.hpp
Go to the documentation of this file.
00001 #include "AlgorithmSelector.h"
00002 #include "XMLTag.h"
00003 
00004 
00005 template<typename T>
00006 AlgorithmSelector<T>::AlgorithmSelector (XMLTag* node
00007 #ifdef LOGFILE
00008                                          , LogFile& log
00009 #endif /*LOGFILE*/
00010                                          ):
00011           m_tree(NULL),
00012           m_paLength(0),
00013           m_paAllocatedLength(0),
00014           m_paDim(4)
00015 #ifdef LOGFILE
00016     ,m_logfile(log)
00017 #endif /*LOGFILE*/
00018 
00019 {
00020   m_mutexAlgEval = new boost::mutex();
00021   if(node != NULL)
00022   {
00023     try
00024     {
00025       m_algorithmlist = XMLTag::Load(node, &m_algorithmlist);
00026     }
00027     catch(char* pst)
00028     {
00029       printf("Error loading Algorithm list: %s\n", pst);
00030     }
00031     AlgorithmID_t id = 0;
00032     typename std::vector< AlgorithmEval<T> > mv = GetAlgorithmList(&mv);
00033     for(typename std::vector< class AlgorithmEval<T> >::const_iterator iter = mv.begin();
00034       iter != mv.end(); iter++)
00035     {
00036       (*iter).m_algorithm->m_AlgIndex = id;
00037       id++;
00038     }
00039   }
00040 }
00041 
00042 //
00043 // Methods
00044 //
00045 template <typename T>
00046 XMLTag* AlgorithmSelector<T>::Save(std::string name )
00047 {
00048   return XMLTag::Tag(m_algorithmlist, name);
00049 }
00050 
00051 template <typename T>
00052 int AlgorithmSelector<T>::InsertInList(AlgorithmEval<T> eval)
00053 {
00054   m_algorithmlist.push_back(eval);
00055   eval.m_algorithm->m_AlgIndex = m_algorithmlist.size() - 1;
00056   return (int)m_algorithmlist.size() - 1;
00057 }
00058 
00059 template <typename T>
00060 std::vector<AlgorithmEval<T> >& AlgorithmSelector<T>::GetAlgorithmList(std::vector<AlgorithmEval<T> >* )
00061 {
00062   return m_algorithmlist;
00063 }
00064 
00065 template<typename T>
00066 Algorithm<T>* AlgorithmSelector<T>::getAlgorithm(int index)
00067 {
00068   return m_algorithmlist[index].m_algorithm;
00069 }
00070 
00071 
00072 template<typename T>
00073 Algorithm<T>* AlgorithmSelector<T>::BestAlgorithm(int type, const Signature &sig, const std::vector<Sensor*> &sensors)
00074 {
00075   Algorithm<T>* selAlgorithm = NULL;
00076   double maxEval = -1.0;
00077   typename std::vector< AlgorithmEval<T> > mv = GetAlgorithmList(&mv);
00078   for(typename std::vector< class AlgorithmEval<T> >::const_iterator iter = mv.begin();
00079     iter != mv.end(); iter++)
00080   {
00081     double sig_compatibility = (*iter).m_algorithm->CheckSignature(sig, sensors);
00082     if(CheckTypeCompatibility((*iter).m_algorithmType, type) && sig_compatibility > 0.0)
00083     {
00084       double earlierExperiences = 0.0;
00085       std::pair<double, int> pp = (*iter).GetEval(sig.m_ID);
00086       if(pp.second > 0)
00087       {
00088         earlierExperiences = pp.first / pp.second;
00089         printf("%s: Earlier experience for this object ID (%f, %d)\n",(*iter).m_algorithm->GetName().c_str(), earlierExperiences, pp.second);
00090         if(earlierExperiences == 0.0)
00091         {
00092           ROS_WARN("Reject to perform action on object ID %ld with alg %s", sig.m_ID,  (*iter).m_algorithm->GetName().c_str());
00093         }
00094       }
00095       else
00096         printf("%s: No earlier experience for this object ID %ld (map entries: %ld)\n", (*iter).m_algorithm->GetName().c_str(), sig.m_ID,(*iter).MapLength());
00097 
00098       double currEval = (min(1.0, (*iter).m_eval * sig_compatibility) + earlierExperiences*pp.second) / (1+pp.second);
00099       if(currEval > maxEval)
00100       {
00101 
00102         maxEval = currEval;
00103         selAlgorithm = (*iter).m_algorithm;
00104 #ifdef _DEBUG
00105         printf("The algorithm %s has a good probability for the signature (%f)\n", (*iter).m_algorithm->GetName().c_str(), maxEval);
00106 #endif
00107       }
00108       else
00109       {
00110 #ifdef _DEBUG
00111         printf("The algorithm %s is  worse than others (%f < %f)\n", (*iter).m_algorithm->GetName().c_str(), currEval, maxEval);
00112 #endif
00113       }
00114     }
00115   }
00116   return selAlgorithm;
00117 }
00118 
00119 template<typename T>
00120 std::vector<Algorithm<T>*> AlgorithmSelector<T>::BestAlgorithmList(int type, const Signature &sig, const std::vector<Sensor*> &sensors)
00121 {
00122   std::vector<Algorithm<T>*> selAlgorithm;
00123   double maxEval = -1.0;
00124   typename std::vector< AlgorithmEval<T> > mv = GetAlgorithmList(&mv);
00125   for(typename std::vector< class AlgorithmEval<T> >::const_iterator iter = mv.begin();
00126     iter != mv.end(); iter++)
00127   {
00128     double sig_compatibility = (*iter).m_algorithm->CheckSignature(sig, sensors);
00129     if(CheckTypeCompatibility((*iter).m_algorithmType, type) && sig_compatibility > 0.0)
00130     {
00131       double earlierExperiences = 0.0;
00132 
00133       std::pair<double, int> pp = (*iter).GetEval(sig.m_ID);
00134       if(pp.second > 0)
00135       {
00136         earlierExperiences = pp.first / pp.second;
00137         printf("%s: Earlier experience for this object ID (%f)\n",(*iter).m_algorithm->GetName().c_str(), earlierExperiences);
00138         if(earlierExperiences == 0.0)
00139         {
00140           printf("Reject to go with this object ID\n");
00141           ROS_WARN("Reject to perform action on object ID %ld with alg %s", sig.m_ID,  (*iter).m_algorithm->GetName().c_str());
00142         }
00143       }
00144       else
00145         printf("%s: No earlier experience for this object ID %ld (map entries: %ld)\n", (*iter).m_algorithm->GetName().c_str(), sig.m_ID,(*iter).MapLength());
00146 
00147       double currEval = (min(1.0, (*iter).m_eval * sig_compatibility) + earlierExperiences) / 2.0;
00148       if(currEval > 0.0)
00149       {
00150         if(currEval > maxEval)
00151           maxEval = currEval;
00152         selAlgorithm.push_back((*iter).m_algorithm);
00153 #ifdef _DEBUG
00154         printf("The algorithm %s has a good probability for the signature (%f)\n", (*iter).m_algorithm->GetName().c_str(), currEval);
00155 #endif
00156       }
00157       else
00158       {
00159 #ifdef _DEBUG
00160         printf("The algorithm %s is  worse than others (%f < %f)\n", (*iter).m_algorithm->GetName().c_str(), currEval, maxEval);
00161 #endif
00162       }
00163     }
00164   }
00165   return selAlgorithm;
00166 }
00167 
00168 
00169 template<typename T>
00170 void AlgorithmSelector<T>::EvalAlgorithm(Evaluable* alg, double eval, double time, Signature* relatedElem)
00171 {
00172   lock lk (*m_mutexAlgEval);
00173   typename std::vector<AlgorithmEval<T> > helper;
00174   typename std::vector<AlgorithmEval<T> > &mv = GetAlgorithmList(&helper);
00175   for(typename std::vector<AlgorithmEval<T> >::iterator iter = mv.begin();
00176     iter != mv.end(); iter++)
00177   {
00178 
00179     if((*iter).m_algorithm == alg)
00180     {
00181 #ifdef _DEBUG
00182       printf("Add Evaluation to Algorithm %s: %f\n",alg->GetName().c_str(), eval);
00183 #endif
00184       (*iter).m_eval = ((*iter).m_eval + eval) / 2;    //TODO eval rel situation and object...
00185       (*iter).m_avgRunTime = ((*iter).m_avgRunTime + time) / 2;
00186 
00187       if(relatedElem != NULL)
00188       {
00189         (*iter).SetEval(relatedElem->m_ID, eval);
00190 
00191         printf("Added %ld to the list of length (now %ld) of objects of %s\n", relatedElem->m_ID, (*iter).MapLength(), (*iter).m_algorithm->GetName().c_str());
00192         printf("EvalAlgorithm: Do we have a pose?\n");
00193         RelPose* pose_t = relatedElem->GetObjectPose();
00194         if(pose_t != NULL)
00195         {
00196           try
00197           {
00198             Matrix m = pose_t->GetMatrix(ID_WORLD);
00202             printf("EvalAlgorithm: Taking pose %ld of elem %ld\n", pose_t->m_uniqueID, relatedElem->m_ID);
00203             m_paLength++;
00204             if(m_tree != NULL)
00205             {
00206               delete m_tree;
00207             }
00208 
00209             if(m_paLength > m_paAllocatedLength)
00210             {
00211               bool copy = true;
00212               if(m_paAllocatedLength == 0)
00213                 copy = false;
00214               m_paAllocatedLength += 100;
00215               m_paDim = 4;
00216               ANNpointArray newPA = annAllocPts(m_paAllocatedLength, m_paDim);
00217               double*   newEval = new double[m_paAllocatedLength];
00218               if(copy)
00219               {
00220                 printf("EvalAlgorithm: We have to copy old data\n");
00221                 for(int i = 0; i < (m_paLength - 1); i++)
00222                 {
00223                   memcpy(newPA[i], m_pointArray[i],sizeof(ANNcoord) * m_paDim);
00224                 }
00225                 memcpy(newEval, m_pointEval,sizeof(double)*(m_paLength - 1));
00226                 annDeallocPts(m_pointArray);
00227                 delete m_pointEval;
00228               }
00229               m_pointArray = newPA;
00230               m_pointEval = newEval;
00231             }
00232             m_pointArray[(m_paLength - 1)][0] = (ANNcoord)(*iter).m_algorithm->m_AlgIndex;
00233             m_pointArray[(m_paLength - 1)][1] = (ANNcoord)m.element(0,3);
00234             m_pointArray[(m_paLength - 1)][2] = (ANNcoord)m.element(1,3);
00235             m_pointArray[(m_paLength - 1)][3] = (ANNcoord)m.element(2,3);
00236             m_pointEval[m_paLength - 1] = eval;
00237             m_tree = new ANNkd_tree((ANNpointArray)m_pointArray, m_paLength, m_paDim);
00238             printf("EvalAlgorithm: Added point [ %f ,  %f,  %f,  %f ]\n",  m_pointArray[(m_paLength - 1)][0], m_pointArray[(m_paLength - 1)][1],
00239             m_pointArray[(m_paLength - 1)][2], m_pointArray[(m_paLength - 1)][3]);
00240 
00241           }
00242           catch(const char* text)
00243           {
00244             printf("Error: %s\n", text);
00245           }
00246 
00247         }
00248       }
00249 
00250 #ifdef LOGFILE
00251       m_logfile.Log(alg->GetName(), GetName(), time / 100000000 , eval, relatedElem);
00252 #endif /*LOGFILE*/
00253       break;
00254     }
00255   }
00256 }
00257 
00258 template<typename T>
00259 double AlgorithmSelector<T>::ReEvalKernel(double val)
00260 {
00261   return 2.0 / (1.0 + exp(-val));
00262 }
00263 
00264 template<typename T>
00265 void AlgorithmSelector<T>::ReevaluatePose(Algorithm<T>* alg, RelPose*& pose)
00266 {
00267   lock lk(*m_mutexAlgEval);
00268   Matrix m = pose->GetMatrix(ID_WORLD);
00269   ANNcoord point[4];
00270   point[0] = (ANNcoord)alg->m_AlgIndex;
00271   point[1] = (ANNcoord)m.element(0,3);
00272   point[2] = (ANNcoord)m.element(1,3);
00273   point[3] = (ANNcoord)m.element(2,3);
00274   ANNidx idx[4];
00275   ANNdist dist[4];
00276 
00277   printf("ReevaluatePose %s %ld (number of entries %d, p: [%f %f %f %f ] (qual %f))\n", alg->GetName().c_str(), pose->m_uniqueID, m_paLength, point[0], point[1], point[2], point[3], pose->m_qualityMeasure);
00278 
00279   if(m_paLength > 4)
00280   {
00281     double test;
00282     m_tree->annkSearch(point, 4, idx, dist, 0.0);
00283     if((dist[0] + dist[1] + dist[2] + dist[3]) > 0.0001)
00284     {
00285       test = (m_pointEval[idx[0]] * ReEvalKernel(dist[0]) +
00286       m_pointEval[idx[1]] * ReEvalKernel(dist[1]) +
00287       m_pointEval[idx[2]] * ReEvalKernel(dist[2]) +
00288       m_pointEval[idx[3]] * ReEvalKernel(dist[3])) / (4*(dist[0] + dist[1] + dist[2] + dist[3]));
00289     }
00290     else
00291     {
00292       test = (m_pointEval[idx[0]] * ReEvalKernel(dist[0]) +
00293       m_pointEval[idx[1]] * ReEvalKernel(dist[1]) +
00294       m_pointEval[idx[2]] * ReEvalKernel(dist[2]) +
00295       m_pointEval[idx[3]] * ReEvalKernel(dist[3])) / 4;
00296     }
00297 
00298     double test2 = ReEvalKernel(dist[0]);
00299     printf("Found 4 neighnors: Eval before %f\n", pose->m_qualityMeasure);
00300     //pose->m_qualityMeasure = (pose->m_qualityMeasure + test) / 2;
00301     printf("Eval after                     %f\n", pose->m_qualityMeasure);
00302     printf("test2(%f)                %f\n", dist[0], test2);
00303     printf("test                           %f\n", test);
00304   }
00305 }
00306 
00307 template<typename T>
00308 bool AlgorithmSelector<T>::CheckTypeCompatibility(int listedType, int askedType)
00309 {
00310   if(askedType == listedType)
00311     return true;
00312   if(askedType / 0x100 == listedType / 0x100)
00313   {
00314     if(askedType % 0x100 <= listedType % 0x100)
00315       return true;
00316   }
00317   return false;
00318 }
00319 
00320 template<typename T>
00321 int AlgorithmSelector<T>::AddAlgorithm(Algorithm<T>* alg, int nType, double dEval, double dTime)
00322 {
00323   if(dEval <= -1.0)
00324     dEval = -0.99;
00325   if(alg == NULL)
00326     return -1;
00327   AlgorithmEval<T> pair(alg, nType, dEval, dTime);
00328   return InsertInList(pair);
00329 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


cognitive_perception
Author(s): Ulrich F Klank
autogenerated on Thu May 23 2013 07:38:34