- Main Page
- Namespaces
- Classes
- Files
- Related Pages

# /Users/fpy/titan-1-5-MBARI/Mers/Estimation/Discrete/MEObjectiveFunction.h

    00001
    00002 //
    00003 // Copyright (c) 2005           Model-based Embedded and Robotic Systems Group
    00004 // All Rights Reserved          Massachusetts Institute of Technology
    00005 //
    00006 // This software is provided as is. Model-based Embedded and Robotic Systems
    00007 // (MERS) group does not assume any responsibility.
    00008 //
    00009 // Organization:        Model-based Embedded and Robotic Systems Group
    00010 //                      Massachusetts Institute of Technology
    00011 //
    00013
    00014 #ifndef _MEOBJECTIVEFUNCTION_H_
    00015 #define _MEOBJECTIVEFUNCTION_H_
    00016
    00017 #include <vector>
    00018 #include <map>
    00019
    00020 #include <Mers/Generators/BFIG/BFIGObjectiveFunction.h>
    00021
    00022 #include <Mers/Estimation/Discrete/Trajectories.h>
    00023 #include <Mers/Estimation/Discrete/Estimate.h>
    00024 #include <Mers/Models/Base/BaseModelAssignment.h>
    00025 #include <Mers/Models/Base/BaseModelVariableArray.h>
    00026
    00027 namespace Mers {
    00028         namespace Models {
    00029                 namespace Base {
    00030                         class BaseModelVariableArray;
    00031                         class BaseModelAssignment;
    00032                 }
    00033         }
    00034         namespace Estimation {
    00035                 namespace Discrete {
    00036                         class Estimate;
    00037                         class Trajectories;
    00038
    00039
    00049                         class MEObjectiveFunction : public Mers::Generators::BFIG::BFIGObjectiveFunction
    00050                         {
    00051
    00052                         public:
    00053                                 //--------------------------------------------
    00054                                 //      CONSTRUCTOR
    00055                                 //--------------------------------------------
    00056
    00072                                 MEObjectiveFunction(Mers::Models::Base::BaseModelVariableArray const *  dVs,
    00073                                                 const std::map<unsigned int, unsigned int> pMap,
    00074                                                 std::vector<double> allTransProbs,
    00075                                                 std::map<unsigned int,double> sProbs)
    00076
    00077                                                 : BFIGObjectiveFunction(false), decisionVariables(dVs),
    00078                                                         allTransitionProbabilitiesForAllStates(allTransProbs),
    00079                                                         sourceStateProbabilities(sProbs), probabilityMap(pMap)
    00080                                                 {
    00081                                                         determineOptimisticAssignmentValues();
    00082                                                 };
    00083
    00087                                 virtual ~MEObjectiveFunction() {};
    00088
    00089                                 //--------------------------------------------
    00090                                 //      PUBLIC FUNCTION METHODS
    00091                                 //--------------------------------------------
    00092
    00103                                  double getBeliefStateUncertainty(const Mers::Estimation::Discrete::Trajectories * beliefState) const
    00104                                  {
    00105                                         double uncertainty = 1.0;
    00106                                         std::vector<Mers::Models::Base::BaseModelAssignment const *> bmAssignmentVector;
    00107                                         for(unsigned int stateNum=0; stateNum < beliefState->getNumberEstimates(); stateNum++)
    00108                                         {
    00109                                                 Mers::Models::Base::BaseModelAssignmentArray const * bmAssignments =
    00110                                                         beliefState->getEstimate(stateNum)->getComponentAssignments();
    00111                                                 for(unsigned int i=0; i<bmAssignments->getLength(); i++)
    00112                                                         bmAssignmentVector.push_back((*bmAssignments)[i]);
    00113                                                 uncertainty -= calculatePropagationUtility(bmAssignmentVector,
    00114                                                         beliefState->getEstimate(stateNum)->getOriginatedTrajectoryNumber());
    00115                                                 bmAssignmentVector.clear();
    00116                                         }
    00117                                         return uncertainty;
    00118                                  };
    00119
    00130                                  double calculatePropagationUtility(Mers::Models::Base::BaseModelAssignmentArray const * bmAssignments,
    00131                                                                         int sourceStateIndex = -1) const
    00132                                  {
    00133                                         double propagationUtility;
    00134                                         std::vector<Mers::Models::Base::BaseModelAssignment const *> bmAssignmentVector;
    00135                                         for(unsigned int i=0; i<bmAssignments->getLength(); i++)
    00136                                                         bmAssignmentVector.push_back((*bmAssignments)[i]);
    00137                                         propagationUtility = calculatePropagationUtility(bmAssignmentVector, sourceStateIndex);
    00138                                         bmAssignmentVector.clear();
    00139                                         return propagationUtility;
    00140                                  };
    00141
    00142
    00152                                 void updateProbabilities(std::vector<double> & allTransProbs, std::map<unsigned int,double> & sProbs)
    00153                                 {
    00154                                         allTransitionProbabilitiesForAllStates.clear();
    00155                                         sourceStateProbabilities.clear();
    00156
    00157                                         allTransitionProbabilitiesForAllStates = allTransProbs;
    00158                                         sourceStateProbabilities = sProbs;
    00159
    00160                                         determineOptimisticAssignmentValues();
    00161                                 };
    00162
    00169                                 void updateStateProbabilities(const std::map<unsigned int,double> & sProbs)
    00170                                 {
    00171                                         sourceStateProbabilities.clear();
    00172                                         sourceStateProbabilities = sProbs;
    00173                                         determineOptimisticAssignmentValues();
    00174                                 };
    00175
    00176                         protected:
    00177                                 //--------------------------------------------
    00178                                 //      PROTECTED FUNCTION METHODS
    00179                                 //--------------------------------------------
    00180
    00192                                  virtual double calculateUtility(
    00193                                                  std::vector<Mers::Models::Base::BaseModelAssignment const *> & bmAssignments,
    00194                                                                                 int sourceStateIndex = -1) const = 0;
    00195
    00196
    00207                                 virtual void    determineOptimisticAssignmentValues(void)
    00208                                 {
    00209                                         optimisticProbabilities.clear();
    00210
    00211                                         double bestValue = 0.0;
    00212                                         double nextValue = 0.0;
    00213                                         unsigned int domainSize = 0;
    00214                                         unsigned int numSourceStates = (unsigned int) sourceStateProbabilities.size();
    00215
    00216                                         if(numSourceStates == 0)        // Nothing to compute for optimisticProbabilities
    00217                                                 return;
    00218
    00219                                         unsigned int thisVarUID;
    00220                                         for(unsigned int dvIndex=0; dvIndex < decisionVariables->getLength(); dvIndex++)
    00221                                         {
    00222                                                 thisVarUID = (*decisionVariables)[dvIndex]->getUID();
    00223                                                 std::map<unsigned int, unsigned int>::const_iterator thisPMIter = probabilityMap.find(thisVarUID);
    00224                                                 if(dvIndex+1 < decisionVariables->getLength())
    00225                                                 {
    00226                                                         std::map<unsigned int, unsigned int>::const_iterator nextPMIter = probabilityMap.find(
    00227                                                                 (*decisionVariables)[dvIndex+1]->getUID());
    00228                                                         domainSize = (*nextPMIter).second - (*thisPMIter).second;
    00229                                                 }
    00230                                                 else
    00231                                                         domainSize = ((unsigned int)allTransitionProbabilitiesForAllStates.size())/numSourceStates -
    00232                                                                                         (*thisPMIter).second;
    00233
    00234                                                 for(unsigned int stateNum=0; stateNum < numSourceStates; stateNum++)
    00235                                                 {
    00236
    00237                                                         bestValue = 0.0;
    00238                                                         for(unsigned int i=0; i<domainSize; i++)
    00239                                                         {
    00240                                                                 nextValue = allTransitionProbabilitiesForAllStates[ stateNum *
    00241                                                                         (allTransitionProbabilitiesForAllStates.size()/numSourceStates) + (*thisPMIter).second + i];
    00242                                                                 if(bestValue < nextValue)
    00243                                                                         bestValue = nextValue;
    00244                                                         }
    00245                                                         optimisticProbabilities[std::pair<unsigned int,unsigned int>(thisVarUID, stateNum)] = bestValue;
    00246                                                 }
    00247                                         }
    00248                                         return;
    00249                                 };
    00250
    00251
    00262                                  virtual double calculatePropagationUtility(
    00263                                                  std::vector<Mers::Models::Base::BaseModelAssignment const *> & bmAssignments,
    00264                                                  int sourceStateIndex = -1) const = 0;
    00265
    00266
    00267                                 //--------------------------------------------
    00268                                 //      PROTECTED DATA MEMBERS
    00269                                 //--------------------------------------------
    00270
    00272                                 Mers::Models::Base::BaseModelVariableArray const *      decisionVariables;
    00273
    00277                                 std::vector<double>             allTransitionProbabilitiesForAllStates;
    00278
    00281                                 std::map<unsigned int,double>           sourceStateProbabilities;
    00282
    00285                                 const std::map<unsigned int, unsigned int>      probabilityMap;
    00286
    00290                                 std::map<std::pair<unsigned int, unsigned int>,double>  optimisticProbabilities;
    00291
    00292                         };
    00293                 }
    00294         }
    00295 }
    00296
    00297 #endif //ndef _MEOBJECTIVEFUNCTION_H_
    00298