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

# /Users/fpy/titan-1-5-MBARI/Mers/Estimation/Discrete/CompiledME/CompiledModeEstimator.cpp

    00001 //==============================================================================
    00002 // Copyright (c) 2004           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 "CompiledModeEstimator.h"
    00010
    00011 #ifdef _MSC_VER
    00012 #pragma warning( disable : 4786 )
    00013 #endif
    00014
    00015 #include <Mers/Models/Base/BaseModel.h>
    00016 #include <Mers/Models/Base/BaseModelAssignment.h>
    00017 #include <Mers/Models/Base/BaseModelAssignmentArray.h>
    00018
    00019 #include <Mers/Generators/BFIG/CDBFIG/CDBFIGFactory.h>
    00020 #include <Mers/Generators/BFIG/BFIGGenerator.h>
    00021
    00022 #include <Mers/Estimation/Discrete/BFTEObjectiveFunction.h>
    00023 #include "../Estimate.h"
    00024 #include "../EstimateHeap.h"
    00025 #include "../Trajectories.h"
    00026 #include "../Trigger.h"
    00027 #include "../Dissent.h"
    00028 #include "../CompiledTransition.h"
    00029
    00030
    00031 using namespace Mers::Estimation::Discrete::CompiledME;
    00032 using namespace Mers::Estimation::Discrete;
    00033 using Mers::Estimation::Discrete::BFTEObjectiveFunction;
    00034 using Mers::Models::Base::BaseModel;
    00035 using Mers::Models::Base::BaseModelAssignment;
    00036 using Mers::Models::Base::BaseModelAssignmentArray;
    00037 using Mers::Generators::BFIG::BFIGGenerator;
    00038 using Mers::Generators::BFIG::CDBFIG::CDBFIGFactory;
    00039 using std::vector;
    00040 using std::map;
    00041 using std::cout;
    00042 using std::cerr;
    00043 using std::endl;
    00044
    00045
    00046         //-----------------------------------------------
    00047         //      CONSTRUCTOR
    00048         //-----------------------------------------------
    00049
    00050 CompiledModeEstimator::CompiledModeEstimator(const BaseModel &bm, vector<const Dissent *> dissents,
    00051                 vector<const CompiledTransition *> compiledTransitions,
    00052                 Trajectories * initialTrajectories)
    00053 : ModeEstimatorBase(bm, dissents, compiledTransitions, initialTrajectories)
    00054 {
    00055         // -------------- Set up the objective function ----------------
    00056         // Probability map that maps UIDs instead of just array indices
    00057         map<unsigned int, unsigned int> varUIDtoProbMap;
    00058
    00059         unsigned int nextVarPos = 0;
    00060         for(unsigned int i=0; i < decisionVariables->getLength(); i++)
    00061         {
    00062                 varUIDtoProbMap[(*decisionVariables)[i]->getUID()] = nextVarPos;
    00063                 nextVarPos += (*decisionVariables)[i]->getDomain()->getLength();
    00064         }
    00065
    00066         vector<double> emptyTransProbs;
    00067         map<unsigned int,double> emptyStateProbs;
    00068
    00069         objectiveFunction = new BFTEObjectiveFunction(decisionVariables, varUIDtoProbMap, emptyTransProbs, emptyStateProbs);
    00070 }
    00071
    00072
    00073         //-----------------------------------------------
    00074         //      DESTRUCTOR
    00075         //-----------------------------------------------
    00076
    00077
    00078 CompiledModeEstimator::~CompiledModeEstimator()
    00079 {
    00080         // Release BFIG Memory
    00081         BFIGGenerator * oldBFIGgen;
    00082         while(bfigGen.size() != 0)
    00083         {
    00084                 oldBFIGgen = bfigGen.back();
    00085                 bfigGen.pop_back();
    00086                 delete oldBFIGgen;
    00087         }
    00088 }
    00089
    00090
    00091         //-----------------------------------------------
    00092         //      PUBLIC METHODS
    00093         //-----------------------------------------------
    00094
    00095 void
    00096 CompiledModeEstimator::setDebug(const unsigned short db)
    00097 {
    00098         unsigned short reducedDB = 0;
    00099         if(db > 1)
    00100                 reducedDB = db-2;
    00101         for(unsigned int i=0; i<bfigGen.size(); i++)
    00102                 bfigGen[i]->setDebug(reducedDB);
    00103
    00104         ModeEstimatorBase::setDebug(db);
    00105         return;
    00106 }
    00107
    00108
    00109         //-----------------------------------------------
    00110         //      PRIVATE METHODS
    00111         //-----------------------------------------------
    00112
    00113 bool
    00114 CompiledModeEstimator::generateTrajectories(void)
    00115 {
    00116         unsigned int i, j;
    00117         Estimate * anEst;
    00118         EstimateHeap bestFirstHeap = EstimateHeap(true);
    00119         vector<BaseModelAssignmentArray const *> conflicts = compiledObjectTrigger->getConflicts();
    00120         vector<double> allProbabilities = compiledObjectTrigger->getNormalizedEnabledTransitionProbabilities();
    00121         vector<double> probabilities;
    00122         map<unsigned int, double> currentStateProbabilities;
    00123
    00124         if(currentTrajectories->getNumberEstimates() < 1)
    00125         {
    00126                 cerr << "<ModeEstimator> ERROR: There are no current trajectories!" << endl;
    00127                 cleanAfterFailure(&bestFirstHeap);
    00128                 return false;
    00129         }
    00130
    00131         if(debug > 1)
    00132         {
    00133                 cout << *compiledObjectTrigger << endl << endl;
    00134
    00135                 cout << endl << "Conflicts: " << endl;
    00136                 for(i=0; i<conflicts.size(); i++)
    00137                         cout << *conflicts[i] << endl;
    00138
    00139                 for(i=0; i<allProbabilities.size(); i++)
    00140                 {
    00141                         if(i%(allProbabilities.size()/currentTrajectories->getNumberEstimates()) == 0)
    00142                                 cout << endl << "Probabilities for Trajectory " <<
    00143                                 i/(allProbabilities.size()/currentTrajectories->getNumberEstimates()) << ": " << endl;
    00144                         cout << allProbabilities[i] << ", ";
    00145                 }
    00146                 cout << endl << endl;
    00147         }
    00148
    00149         // Gather all the current state probabilities
    00150         for(i=0; i < currentTrajectories->getNumberEstimates(); i++)
    00151                 currentStateProbabilities[i] = currentTrajectories->getEstimate(i)->getProbability();
    00152
    00153         // Update the objective function with current probability data
    00154         objectiveFunction->updateProbabilities(allProbabilities, currentStateProbabilities);
    00155
    00156         // Set the size of the vectors
    00157         probabilities.resize(allProbabilities.size()/currentTrajectories->getNumberEstimates());
    00158
    00159         // Get the best transition from each initial state in the current trajectories
    00160         for(i=0; i<currentTrajectories->getNumberEstimates(); i++)
    00161         {
    00162                 // Seperate out probabilities
    00163                 for(j=0; j<allProbabilities.size()/currentTrajectories->getNumberEstimates(); j++)
    00164                         probabilities[j] = allProbabilities[i * (allProbabilities.size()/currentTrajectories->getNumberEstimates()) + j];
    00165
    00166                 // Allocate BFIG memory if necessary
    00167                 if(i == bfigGen.size())
    00168                 {
    00169                         CDBFIGFactory factory;
    00170                         bfigGen.resize(bfigGen.size()+1);
    00171                         bfigGen[i] = new BFIGGenerator(factory, *decisionVariables, *model.getVariables(), *probabilityMap, probabilities, (long int)1e10, true);
    00172                 } else
    00173                         // Insert the current probabilities into BFIG
    00174                         bfigGen[i]->updateAllProbabilities(*decisionVariables, *probabilityMap, probabilities);
    00175
    00176                 // Add all the conflicts. The same conflicts apply to all trajectories
    00177                 for(j=0; j<conflicts.size(); j++)
    00178                         bfigGen[i]->addConflict(conflicts[j]);
    00179
    00180
    00181                 // Get best estimate given orignal state using BFIG
    00182                 if(!findNextBestStateAndPutInHeap(i, allProbabilities, &bestFirstHeap, true))
    00183                 {
    00184                         cleanAfterFailure(&bestFirstHeap);
    00185                         return false;
    00186                 }
    00187         }
    00188
    00189         if(bestFirstHeap.isEmpty())
    00190         {
    00191                 cout << "<CompiledModeEstimator> ERROR: No consistent states found!" << endl;
    00192                 cleanAfterFailure(&bestFirstHeap);
    00193                 return false;
    00194         }
    00195         else
    00196                 nextTrajectories = new Trajectories(currentTrajectories->getAccumulatedUncertainty(), debug);
    00197
    00198         // Keep popping off and adding new esimtates to the heap until k best trajectories
    00199         // have been found
    00200
    00201         while(!bestFirstHeap.isEmpty())
    00202         {
    00203                 bestFirstHeap.getFirst(anEst);                  // Pop next most likely off the top of heap
    00204                 if(!nextTrajectories->addEstimate(anEst))       // Add it to the next trajectories list
    00205                 {
    00206                         cerr << "<ModeEstimator> ERROR: Couldn't add estimate to next trajectories!" << endl;
    00207                         cleanAfterFailure(&bestFirstHeap);
    00208                         return false;
    00209                 }
    00210                 if(nextTrajectories->getNumberEstimates() == maxNumberEstimates)
    00211                         break;
    00212
    00213                 // Get best estimate given orignal state using Opsat
    00214                 if(!findNextBestStateAndPutInHeap(anEst->getOriginatedTrajectoryNumber(), allProbabilities, &bestFirstHeap))
    00215                 {
    00216                         cleanAfterFailure(&bestFirstHeap);
    00217                         return false;
    00218                 }
    00219         }
    00220
    00221         // Clean up unused memory (bestFirstHeap, bfigs) and normalize trajectories
    00222         releaseMemoryAndNormalize(&bestFirstHeap);
    00223
    00224         return true;
    00225 }
    00226
    00228
    00229 const BaseModelAssignmentArray *
    00230 CompiledModeEstimator::generateBestSolution(unsigned int generatorNum, long int maximumVisitedNodeCount,
    00231                                                                         long int maxQueueSize)
    00232 {
    00233         return bfigGen[generatorNum]->generateBestImplicant(maximumVisitedNodeCount, maxQueueSize, true);
    00234         // The third parameter (true) is to tell BFIG to generate full solutions (full assignment to mode vars)
    00235 }
    00236
    00238
    00239 bool
    00240 CompiledModeEstimator::addEstimate(Estimate * theEst, EstimateHeap * nextTrajsEstHeap)
    00241 {
    00242         nextTrajsEstHeap->insert(theEst);
    00243         return true;
    00244 }
    00245
    00247
    00248 bool
    00249 CompiledModeEstimator::releaseMemoryAndNormalize(EstimateHeap * nextTrajsEstHeap)
    00250 {
    00251         // Reset BFIG Objects (deletion only occures when ModeEstimator is deleted)
    00252         resetBFIGs();
    00253
    00254         // Call the base function to release the heap memory and normalize
    00255         return ModeEstimatorBase::releaseMemoryAndNormalize(nextTrajsEstHeap);
    00256 }
    00257
    00258
    00259 void
    00260 CompiledModeEstimator::resetBFIGs(void)
    00261 {
    00262         // Reset BFIG Objects (deletion only occures when ModeEstimator is deleted)
    00263         for(unsigned int i=0; i<bfigGen.size(); i++)
    00264                 bfigGen[i]->reset();
    00265         return;
    00266 }