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

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

    00001 //==============================================================================
    00002 // Copyright (c) 2005           Model-based Embedded and Robotic Systems Group
    00003 // All Rights Reserved          Massachusetts Institute of Technology
    00004 //
    00005 // This software is provided as is. Model-based Embedded and Robotic Systems
    00006 // (MERS) group does not assume any responsibility.
    00007 //==============================================================================
    00008
    00009 #include "BFBSE.h"
    00010
    00011 #ifdef _MSC_VER
    00012 #pragma warning( disable : 4786 )
    00013 #endif
    00014
    00015
    00016 #include <Mers/Estimation/Discrete/Estimate.h>
    00017 #include <Mers/Estimation/Discrete/Trigger.h>
    00018 #include <Mers/Estimation/Discrete/Trajectories.h>
    00019 #include <Mers/Estimation/Discrete/EstimateHeap.h>
    00020 #include <Mers/Estimation/Discrete/ApproximateBeliefState/BSEObjectiveFunction.h>
    00021 #include <Mers/Solvers/SAT/GATSAT/OpSAT/Opsat.h>
    00022 #include <Mers/Solvers/SAT/AssignmentTesterInterface.h>
    00023 #include <Mers/Generators/BFIG/BFIGGenerator.h>
    00024 #include <Mers/Models/Base/BaseModelAssignment.h>
    00025 #include <Mers/Models/Base/BaseModelAssignmentArray.h>
    00026 #include <Mers/Models/Base/BaseModelClause.h>
    00027 #include <Mers/Models/Base/BaseModelClauseArray.h>
    00028
    00029 #include <math.h>
    00030 #include <map>
    00031
    00032 using Mers::Estimation::Discrete::ApproximateBeliefState::BFBSE::BFBSE;
    00033 using Mers::Estimation::Discrete::ApproximateBeliefState::BSEObjectiveFunction;
    00034 using namespace Mers::Estimation::Discrete::ModeEstimationPerformance;
    00035 using namespace Mers::Estimation::Discrete::ModeEstimation;
    00036 using namespace Mers::Estimation::Discrete;
    00037 using Mers::Models::Base::BaseModelAssignment;
    00038 using Mers::Models::Base::BaseModelAssignmentArray;
    00039 using Mers::Models::Base::BaseModelClause;
    00040 using Mers::Models::Base::BaseModelClauseArray;
    00041 using Mers::Generators::BFIG::BFIGGenerator;
    00042 using Mers::Solvers::SAT::GATSAT::OpSAT::Opsat;
    00043 using Mers::Solvers::SAT::AssignmentTesterInterface;
    00044 using Mers::Models::CCA::CCAModel;
    00045 using std::vector;
    00046 using std::map;
    00047 using std::cout;
    00048 using std::cerr;
    00049 using std::endl;
    00050
    00051         //-----------------------------------------------
    00052         //      CONSTRUCTOR
    00053         //-----------------------------------------------
    00054
    00055 BFBSE::BFBSE(const CCAModel &cca, Trajectories * initialTrajectories)
    00056 : ModeEstimatorDebug(cca, initialTrajectories)
    00057 {
    00058         if(objectiveFunction != NULL)
    00059                 delete objectiveFunction;
    00060
    00061         // Probability map that maps UIDs instead of just array indices
    00062         map<unsigned int, unsigned int> varUIDtoProbMap;
    00063
    00064         unsigned int nextVarPos = 0;
    00065         for(unsigned int i=0; i < decisionVariables->getLength(); i++)
    00066         {
    00067                 varUIDtoProbMap[(*decisionVariables)[i]->getUID()] = nextVarPos;
    00068                 nextVarPos += (*decisionVariables)[i]->getDomain()->getLength();
    00069         }
    00070
    00071         vector<double> emptyTransProbs;
    00072         map<unsigned int,double> emptyStateProbs;
    00073
    00074         objectiveFunction = new BSEObjectiveFunction(decisionVariables, varUIDtoProbMap, emptyTransProbs, emptyStateProbs);
    00075 }
    00076
    00077
    00078         //-----------------------------------------------
    00079         //      DESTRUCTOR
    00080         //-----------------------------------------------
    00081
    00082
    00083 BFBSE::~BFBSE()
    00084 {
    00085         if(objectiveFunction != NULL)
    00086         {
    00087                 delete objectiveFunction;
    00088                 objectiveFunction = NULL;
    00089         }
    00090 }
    00091
    00092
    00093         //-----------------------------------------------
    00094         //      PUBLIC METHODS
    00095         //-----------------------------------------------
    00096
    00097         //-----------------------------------------------
    00098         //      PRIVATE METHODS
    00099         //-----------------------------------------------
    00100
    00101 bool
    00102 BFBSE::generateTrajectories(void)
    00103 {
    00104         unsigned int i, j;
    00105         unsigned int obsValueUID;
    00106         unsigned int numEstimatesBeforeRequest = 0;
    00107         BaseModelClauseArray * currentObservationAssignments = new BaseModelClauseArray(true);
    00108         BaseModelClause * aObservationClause;
    00109         vector<double> allProbabilities = compiledObjectTrigger->getNormalizedEnabledTransitionProbabilities();
    00110         map<unsigned int, double> currentStateProbabilities;
    00111
    00112         if(currentTrajectories->getNumberEstimates() < 1)
    00113         {
    00114                 cerr << "<BFBSE> ERROR: There are no current estimates!" << endl;
    00115                 cleanAfterFailure();
    00116                 return false;
    00117         }
    00118
    00119         if(debug > 2)
    00120         {
    00121                 cout << *compiledObjectTrigger << endl << endl;
    00122
    00123                 for(i=0; i<allProbabilities.size(); i++)
    00124                 {
    00125                         if(i%(allProbabilities.size()/currentTrajectories->getNumberEstimates()) == 0)
    00126                                 cout << endl << "Probabilities from source state " <<
    00127                                 i/(allProbabilities.size()/currentTrajectories->getNumberEstimates()) << ": " << endl;
    00128                         cout << allProbabilities[i] << ", ";
    00129                 }
    00130                 cout << endl << endl;
    00131         }
    00132
    00133
    00134         // Gather all the current state probabilities
    00135         for(i=0; i < currentTrajectories->getNumberEstimates(); i++)
    00136                 currentStateProbabilities[i] = currentTrajectories->getEstimate(i)->getProbability();
    00137
    00138         // Update the objective function with current probability data
    00139         objectiveFunction->updateProbabilities(allProbabilities, currentStateProbabilities);
    00140
    00141         // Create clause constraints for all the current observations
    00142         for(i=0; i<observationVariables->getLength(); i++)
    00143         {
    00144                 obsValueUID = compiledObjectTrigger->getCurrentCommandObservationValueUID((*observationVariables)[i]->getUID());
    00145                 if(obsValueUID != (unsigned int)Trigger::UNDEFINED)
    00146                 {
    00147                         aObservationClause = new BaseModelClause(BaseModelClause::DISJUNCTIVE, true);
    00148                         aObservationClause->pushBack(new BaseModelAssignment((*observationVariables)[i], (*observationVariables)[i]->getDomain()->getValue(obsValueUID)));
    00149                         currentObservationAssignments->pushBack(aObservationClause);
    00150                 }
    00151         }
    00152
    00153         // Initialize OpSat if one hasn't already been declared
    00154         if(opsats.size() < 1)
    00155         {
    00156                 opsats.resize(1);
    00157                 opsats[0] = new Opsat(*decisionVariables, objectiveFunction, *myCCAmodel.getVariables(), *globalConstraints);
    00158                 setDebug(debug);
    00159         }
    00160
    00161         // Add all the observation constraints
    00162         for(j=0; j<currentObservationAssignments->getLength(); j++)
    00163         {
    00164                 if(debug > 2)
    00165                         cout << "Adding " << (*currentObservationAssignments)[j] << " constraint to opsat." << endl;
    00166                 opsats[0]->getTester()->addTemporaryConstraint((*currentObservationAssignments)[j]);
    00167         }
    00168
    00169         // Initialize next trajectories object
    00170         if(nextTrajectories != NULL)
    00171                 delete nextTrajectories;
    00172         nextTrajectories = new Trajectories(currentTrajectories->getAccumulatedUncertainty(), debug);
    00173
    00174         // Intialize estimate counter (timing requirements)
    00175         numEstimatesGenerated = 0;
    00176
    00177         do {
    00178                 numEstimatesBeforeRequest = nextTrajectories->getNumberEstimates();
    00179                 // Get next best estimates using Opsat
    00180                 if(!findNextBestStateAndPutInHeap(0, allProbabilities, NULL, false))
    00181                 {
    00182                         cleanAfterFailure();
    00183                         return false;
    00184                 }
    00185         // Exits if k estimates have been generated or the search is exhausted
    00186         } while(nextTrajectories->getNumberEstimates() < maxNumberEstimates &&
    00187                                 nextTrajectories->getNumberEstimates() != numEstimatesBeforeRequest);
    00188
    00189         if(nextTrajectories->getNumberEstimates() < 1)
    00190         {
    00191                         cerr << "<BFBSE> ERROR: Couldn't find any consistent estimates for next trajectories!" << endl;
    00192                         cleanAfterFailure();
    00193                         return false;
    00194         }
    00195
    00196         releaseMemoryAndNormalize(currentObservationAssignments);
    00197
    00198         return true;
    00199 }
    00200
    00202
    00203 bool
    00204 BFBSE::addEstimate(Estimate * theEst, EstimateHeap * nextTrajsEstHeap)
    00205 {
    00206         (void)nextTrajsEstHeap;
    00207         if(nextTrajectories == NULL)
    00208         {
    00209                 cerr << "<ModeEstimatorBase> ERROR: Next trajectories memory not allocated!" << endl;
    00210                 return false;
    00211         }
    00212         if(!nextTrajectories->addEstimate(theEst))      // Add it to the next trajectories list
    00213         {
    00214                 cerr << "<ModeEstimatorBase> ERROR: Couldn't add estimate to next trajectories!" << endl;
    00215                 return false;
    00216         }
    00217         return true;
    00218 }
    00219
    00221
    00222 bool
    00223 BFBSE::calculateEstimateProbability(double & estProbability, const BaseModelAssignmentArray * state,
    00224                                                                                                 unsigned int generatorNum, const vector<double> &allProbs) const
    00225 // This function really belongs in a general BFBSE class
    00226 {
    00227         (void) generatorNum;
    00228         (void) allProbs;
    00229
    00230         estProbability = objectiveFunction->getUtility(state);
    00231
    00232         return true;
    00233 }