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

# /Users/fpy/titan-1-5-MBARI/Mers/Estimation/Discrete/ApproximateBeliefState/BSEObjectiveFunction.cpp

    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 #include "BSEObjectiveFunction.h"
    00015
    00016 #ifdef _MSC_VER
    00017 #pragma warning( disable : 4786 )
    00018 #endif
    00019
    00020 #include <Mers/Estimation/Discrete/Trajectories.h>
    00021 #include <Mers/Estimation/Discrete/Estimate.h>
    00022
    00023 #include <Mers/Generators/BFIG/BFIGAssignment.h>
    00024 #include <Mers/Generators/BFIG/BFIGVariable.h>
    00025 #include <Mers/Models/Base/BaseModelVariableArray.h>
    00026 #include <Mers/Models/Base/BaseModelVariable.h>
    00027 #include <Mers/Models/Base/BaseModelValue.h>
    00028 #include <Mers/Models/Base/BaseModelAssignment.h>
    00029 #include <Mers/Models/Base/BaseModelAssignmentArray.h>
    00030
    00031 using namespace Mers::Estimation::Discrete::ApproximateBeliefState;
    00032 using Mers::Estimation::Discrete::Trajectories;
    00033 using Mers::Estimation::Discrete::Estimate;
    00034 using Mers::Generators::BFIG::BFIGAssignment;
    00035 using Mers::Generators::BFIG::BFIGVariable;
    00036 using Mers::Models::Base::BaseModelVariableArray;
    00037 using Mers::Models::Base::BaseModelVariable;
    00038 using Mers::Models::Base::BaseModelValue;
    00039 using Mers::Models::Base::BaseModelAssignment;
    00040 using Mers::Models::Base::BaseModelAssignmentArray;
    00041 using std::vector;
    00042 using std::map;
    00043 using std::pair;
    00044 using std::cout;
    00045 using std::endl;
    00046
    00048 //      CONSTRUCTOR
    00050
    00051 BSEObjectiveFunction::BSEObjectiveFunction(BaseModelVariableArray const * dVs,
    00052                 const map<unsigned int,unsigned int> pMap, vector<double> allTransProbs, map<unsigned int,double> sProbs)
    00053 : MEObjectiveFunction(dVs, pMap, allTransProbs, sProbs)
    00054 {}
    00055
    00057
    00058 BSEObjectiveFunction::~BSEObjectiveFunction()
    00059 {
    00060 }
    00061
    00062
    00064 //      PUBLIC METHODS
    00066
    00067
    00068
    00069
    00071 //      PROTECTED METHODS
    00073
    00074 double
    00075 BSEObjectiveFunction::calculateUtility(vector<BaseModelAssignment const *> & bmAssignments,
    00076                                                                            int sourceStateIndex) const
    00077 {
    00078         (void) sourceStateIndex; // Not used by BSEObjectiveFunction
    00079
    00080         double fX = 0.0;
    00081         double trajectoryProb = 1.0;
    00082         unsigned int i, j, varUID;
    00083         unsigned int numSourceStates = (unsigned int) sourceStateProbabilities.size();
    00084
    00085         unsigned int numNotAssigned = 0;
    00086         unsigned int numAssigned = 0;
    00087
    00088         map<unsigned int, bool> wasAssigned;
    00089         map<unsigned int, unsigned int> assignedValue;
    00090         for(i=0; i<decisionVariables->getLength(); i++)
    00091                 wasAssigned[(*decisionVariables)[i]->getUID()] = false;
    00092
    00093         for(i=0; i<bmAssignments.size(); i++)
    00094         {
    00095                 varUID = bmAssignments[i]->getVariable()->getUID();
    00096                 wasAssigned[varUID] = true;
    00097                 assignedValue[varUID] = bmAssignments[i]->getValue()->getUID();                 // Assumes probabilities are still listed in UID order
    00098         }
    00099
    00100         for(i=0; i<numSourceStates; i++)
    00101         {
    00102                 map<unsigned int, double>::const_iterator sProbItr = sourceStateProbabilities.find(i);
    00103                 trajectoryProb = (*sProbItr).second;    // Direct access isn't a constant function so apparently this is required
    00104                 for(j=0; j<decisionVariables->getLength(); j++)
    00105                 {
    00106                         varUID = (*decisionVariables)[j]->getUID();
    00107                         if(wasAssigned[varUID])
    00108                         {
    00109                                 map<unsigned int, unsigned int>::const_iterator pMapItr = probabilityMap.find(varUID);
    00110                                 trajectoryProb *= allTransitionProbabilitiesForAllStates[i*(allTransitionProbabilitiesForAllStates.size() /
    00111                                                 numSourceStates) + (*pMapItr).second + assignedValue[varUID]];
    00112                                 numAssigned++;
    00113                         }
    00114                         else
    00115                         {
    00116                                 map<pair<unsigned int, unsigned int>, double>::const_iterator opItr;
    00117                                 opItr = optimisticProbabilities.find(pair<unsigned int, unsigned int>(varUID, i));
    00118                                 trajectoryProb *= (*opItr).second;
    00119                                 numNotAssigned++;
    00120                         }
    00121                 }
    00122                 fX += trajectoryProb;
    00123         }
    00124
    00125 //      cout << "# assigned: " << numAssigned << ", # not assigned: " << numNotAssigned << ", prob: " << fX << endl;
    00126
    00127         return fX;
    00128 }
    00129
    00131 //      PRIVATE METHODS
    00133
    00134
    00135 double
    00136 BSEObjectiveFunction::calculatePropagationUtility(vector<BaseModelAssignment const *> & bmAssignments,
    00137                                                                                                   int sourceStateIndex) const
    00138 {
    00139         return BSEObjectiveFunction::calculateUtility(bmAssignments, sourceStateIndex);
    00140 }
    00141
    00143