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

# /Users/fpy/titan-1-5-MBARI/Mers/Estimation/Discrete/ApproximateBeliefState/CompiledBFBSE/CompiledBFBSE.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 "CompiledBFBSE.h"
    00010
    00011 #ifdef _MSC_VER
    00012 #pragma warning( disable : 4786 )
    00013 #endif
    00014
    00015
    00016 #include <math.h>
    00017 #include <map>
    00018
    00019 #include <Mers/Estimation/Discrete/CompiledME/CompiledMEDebug/CBFTEDebug.h>
    00020 #include <Mers/Estimation/Discrete/Estimate.h>
    00021 #include <Mers/Estimation/Discrete/Trigger.h>
    00022 #include <Mers/Estimation/Discrete/Trajectories.h>
    00023
    00024 #include <Mers/Estimation/Discrete/ApproximateBeliefState/BSEObjectiveFunction.h>
    00025 #include <Mers/Generators/BFIG/CDBFIG/CDBFIGFactory.h>
    00026 #include <Mers/Generators/BFIG/BFIGGenerator.h>
    00027
    00028 #include <Mers/Models/Base/BaseModel.h>
    00029 #include <Mers/Models/Base/BaseModelAssignment.h>
    00030 #include <Mers/Models/Base/BaseModelAssignmentArray.h>
    00031
    00032 using namespace Mers::Estimation::Discrete::ModeEstimationPerformance;
    00033 using namespace Mers::Estimation::Discrete;
    00034 using Mers::Estimation::Discrete::ApproximateBeliefState::CompiledBFBSE::CompiledBFBSE;
    00035 using Mers::Estimation::Discrete::CompiledME::CompiledMEDebug::CBFTEDebug;
    00036 using Mers::Models::Base::BaseModel;
    00037 using Mers::Models::Base::BaseModelAssignment;
    00038 using Mers::Models::Base::BaseModelAssignmentArray;
    00039 using Mers::Generators::BFIG::BFIGGenerator;
    00040 using Mers::Generators::BFIG::CDBFIG::CDBFIGFactory;
    00041 using std::vector;
    00042 using std::map;
    00043 using std::cout;
    00044 using std::cerr;
    00045 using std::endl;
    00046
    00047         //-----------------------------------------------
    00048         //      CONSTRUCTOR
    00049         //-----------------------------------------------
    00050
    00051 CompiledBFBSE::CompiledBFBSE(const BaseModel &bm, vector<const Dissent *> dissents,
    00052                         vector<const CompiledTransition *> compiledTransitions, Trajectories * initialTrajectories)
    00053 : CBFTEDebug(bm, dissents, compiledTransitions, initialTrajectories)
    00054 {
    00055         // Probability map that maps UIDs instead of just array indices
    00056         map<unsigned int, unsigned int> varUIDtoProbMap;
    00057
    00058         unsigned int nextVarPos = 0;
    00059         for(unsigned int i=0; i < decisionVariables->getLength(); i++)
    00060         {
    00061                 varUIDtoProbMap[(*decisionVariables)[i]->getUID()] = nextVarPos;
    00062                 nextVarPos += (*decisionVariables)[i]->getDomain()->getLength();
    00063         }
    00064
    00065         vector<double> emptyTransProbs;
    00066         map<unsigned int,double> emptyStateProbs;
    00067
    00068         objectiveFunction = new BSEObjectiveFunction(decisionVariables, varUIDtoProbMap, emptyTransProbs, emptyStateProbs);
    00069 }
    00070
    00071
    00072         //-----------------------------------------------
    00073         //      DESTRUCTOR
    00074         //-----------------------------------------------
    00075
    00076
    00077 CompiledBFBSE::~CompiledBFBSE()
    00078 {
    00079         delete objectiveFunction;
    00080 }
    00081
    00082
    00083         //-----------------------------------------------
    00084         //      PUBLIC METHODS
    00085         //-----------------------------------------------
    00086
    00087         //-----------------------------------------------
    00088         //      PRIVATE METHODS
    00089         //-----------------------------------------------
    00090
    00091 bool
    00092 CompiledBFBSE::generateTrajectories(void)
    00093 {
    00094         unsigned int i, j;
    00095         unsigned int numInvalidCandidates = 0;
    00096         double estimateProbability = 0.0;
    00097         Estimate * anEst;
    00098         BaseModelAssignmentArray * res;
    00099         BaseModelAssignmentArray * estAssign;
    00100         vector<BaseModelAssignmentArray const *> conflicts = compiledObjectTrigger->getConflicts();
    00101         vector<double> allProbabilities = compiledObjectTrigger->getNormalizedEnabledTransitionProbabilities();
    00102         map<unsigned int, double> currentStateProbabilities;
    00103
    00104         if(currentTrajectories->getNumberEstimates() < 1)
    00105         {
    00106                 cerr << "<CompiledBFBSE> ERROR: There are no current estimates!" << endl;
    00107                 return false;
    00108         }
    00109
    00110         if(debug > 2)
    00111         {
    00112                 cout << *compiledObjectTrigger << endl << endl;
    00113
    00114                 cout << endl << "Conflicts: " << endl;
    00115                 for(i=0; i<conflicts.size(); i++)
    00116                         cout << *conflicts[i] << endl;
    00117
    00118                 for(i=0; i<allProbabilities.size(); i++)
    00119                 {
    00120                         if(i%(allProbabilities.size()/currentTrajectories->getNumberEstimates()) == 0)
    00121                                 cout << endl << "Probabilities from source state " <<
    00122                                 i/(allProbabilities.size()/currentTrajectories->getNumberEstimates()) << ": " << endl;
    00123                         cout << allProbabilities[i] << ", ";
    00124                 }
    00125                 cout << endl << endl;
    00126         }
    00127
    00128
    00129         // Gather all the current state probabilities
    00130         for(i=0; i < currentTrajectories->getNumberEstimates(); i++)
    00131                 currentStateProbabilities[i] = currentTrajectories->getEstimate(i)->getProbability();
    00132
    00133         // Update the objective function with current probability data
    00134         objectiveFunction->updateProbabilities(allProbabilities, currentStateProbabilities);
    00135
    00136         // Allocate BFIG memory if one hasn't already been initialized
    00137         if(bfigGen.size() < 1)
    00138         {
    00139                 CDBFIGFactory factory;
    00140                 bfigGen.resize(1);
    00141                 bfigGen[0] = new BFIGGenerator(factory, *decisionVariables, objectiveFunction, *model.getVariables(), (long int)1e10);
    00142         }
    00143         //else
    00144                 // Insert the current probabilities into BFIG
    00145         //      bfigGen[i]->updateAllProbabilities(*decisionVariables, *probabilityMap, probabilities);
    00146
    00147         // Add all the conflicts. The same conflicts apply to all trajectories
    00148         for(j=0; j<conflicts.size(); j++)
    00149                 bfigGen[0]->addConflict(conflicts[j]);
    00150
    00151
    00152         // Initialize next trajectories object
    00153         nextTrajectories = new Trajectories(currentTrajectories->getAccumulatedUncertainty(), debug);
    00154
    00155         // Intialize estimate counter (timing requirements)
    00156         numEstimatesGenerated = 0;
    00157
    00158         while(nextTrajectories->getNumberEstimates() < maxNumberEstimates)
    00159         {
    00160                 // Get next best estimate using BFIG
    00161                 const BaseModelAssignmentArray * tempRes = bfigGen[0]->generateBestImplicant((long int)1e10,(long int)1e10,false);
    00162                 numEstimatesGenerated++;
    00163                 if(tempRes != NULL) {
    00164
    00165                         res = new BaseModelAssignmentArray(*tempRes);
    00166                         estimateProbability = objectiveFunction->getUtility(res);
    00167
    00168                         if(res->getLength() > 0 && estimateProbability > 0.0)
    00169                         {
    00170                                 estAssign = new BaseModelAssignmentArray(true);
    00171                                 for(j=0; j<res->getLength(); j++)
    00172                                         estAssign->pushBack(new BaseModelAssignment((*res)[j]->getVariable(), (*res)[j]->getValue()));
    00173                                 anEst = new Estimate(estAssign, estimateProbability, 0);        // always bfigGen[0]
    00174                                 if(!nextTrajectories->addEstimate(anEst))       // Add it to the next trajectories list
    00175                                 {
    00176                                         cerr << "<CompiledBFBSE> ERROR: Couldn't add estimate to next trajectories!" << endl;
    00177                                         resetBFIGs();
    00178 //                                      delete currentObservationAssignments;
    00179                                         delete nextTrajectories;
    00180                                         nextTrajectories = NULL;
    00181                                         return false;
    00182                                 }
    00183
    00184                                 if(debug > 1)
    00185                                 {
    00186                                         cout << "<CompiledBFBSE> Added an estimate to the belief state." << endl;
    00187                                         cout << " " << *anEst << endl;
    00188                                         cout << " estimate probability: " << anEst->getProbability() << endl;
    00189                                 }
    00190                         }
    00191                         else
    00192                         {
    00193                                 numInvalidCandidates++;
    00194                                 if(debug >= 0)
    00195                                 {
    00196                                         cout << "<CompiledBFBSE> WARNING: Zero probability estimate was generated." << endl;
    00197                                         cout << "               res length: " << res->getLength() << endl;
    00198                                         cout << "               est prob: " << estimateProbability << endl;
    00199                                         cout << "Candidate: " << endl << *res << endl;
    00200                                 }
    00201                         }
    00202                         delete res;
    00203                 }
    00204                 else            // No more estimates left to generate in opsat
    00205                 {
    00206                         break;
    00207                 }
    00208
    00209         }
    00210
    00211         if(nextTrajectories->getNumberEstimates() < 1)
    00212         {
    00213                         cerr << "<CompiledBFBSE> ERROR: Couldn't find any consistent estimates for next trajectories!" << endl;
    00214                         resetBFIGs();
    00215 //                      delete currentObservationAssignments;
    00216                         delete nextTrajectories;
    00217                         nextTrajectories = NULL;
    00218                         return false;
    00219         }
    00220
    00221         if(numInvalidCandidates > 0 && debug > 1)
    00222                 cout << "Number of zero probability candidates: " << numInvalidCandidates << endl;
    00223
    00224         numConflictsGenerated = bfigGen[0]->getNumberOfConflicts();
    00225
    00226         // Count up the number of elements expanded in all the queues;
    00227         numElementsExpanded = 0;
    00228         maxQueueSizeInBFIG = 0;
    00229         numCandidatesGenerated = 0;
    00230         for(i=0; i<bfigGen.size(); i++)
    00231         {
    00232                 numElementsExpanded += bfigGen[i]->getTotalNumberNodesExpanded();
    00233                 maxQueueSizeInBFIG += bfigGen[i]->getLargestQueueSinceReset();
    00234                 numCandidatesGenerated += bfigGen[i]->getNumberCandidatesSinceReset();
    00235         }
    00236         numElementsExpanded -= numElementsPreviouslyExpanded;
    00237         numElementsPreviouslyExpanded += numElementsExpanded;
    00238
    00239         // Reset BFIG Objects (deletion only occures when ModeEstimator is deleted)
    00240         resetBFIGs();
    00241
    00242         // Clear observation constraints
    00243 //      delete currentObservationAssignments;
    00244
    00245         // Normalize new trajectories to avoid machine error
    00246         nextTrajectories->normalize();
    00247
    00248         return true;
    00249 }
    00250