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

# /Users/fpy/titan-1-5-MBARI/Mers/Estimation/Discrete/ApproximateBeliefState/StaticOCSPEstimateEnumeration/StaticOCSPEstimateEnumerator.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 "StaticOCSPEstimateEnumerator.h"
    00010
    00011 #ifdef _MSC_VER
    00012 #pragma warning( disable : 4786 )
    00013 #endif
    00014
    00015
    00016 #include <math.h>
    00017 #include <Mers/Estimation/Discrete/Estimate.h>
    00018 #include <Mers/Estimation/Discrete/Trigger.h>
    00019 #include <Mers/Estimation/Discrete/Trajectories.h>
    00020 #include <Mers/Estimation/Discrete/EstimateHeap.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 using namespace Mers::Estimation::Discrete::ApproximateBeliefState::StaticOCSPEstimateEnumeration;
    00030 using namespace Mers::Estimation::Discrete::ModeEstimationPerformance;
    00031 using namespace Mers::Estimation::Discrete::ModeEstimation;
    00032 using namespace Mers::Estimation::Discrete;
    00033 using Mers::Models::Base::BaseModelAssignment;
    00034 using Mers::Models::Base::BaseModelAssignmentArray;
    00035 using Mers::Models::Base::BaseModelClause;
    00036 using Mers::Models::Base::BaseModelClauseArray;
    00037 using Mers::Generators::BFIG::BFIGGenerator;
    00038 using Mers::Solvers::SAT::GATSAT::OpSAT::Opsat;
    00039 using Mers::Solvers::SAT::AssignmentTesterInterface;
    00040 using Mers::Models::CCA::CCAModel;
    00041 using std::vector;
    00042 using std::cout;
    00043 using std::cerr;
    00044 using std::endl;
    00045
    00046         //-----------------------------------------------
    00047         //      CONSTRUCTOR
    00048         //-----------------------------------------------
    00049
    00050 StaticOCSPEstimateEnumerator::StaticOCSPEstimateEnumerator(const CCAModel &cca, Trajectories * initialTrajectories)
    00051 : ModeEstimatorDebug(cca, initialTrajectories)
    00052 {
    00053 }
    00054
    00055
    00056         //-----------------------------------------------
    00057         //      DESTRUCTOR
    00058         //-----------------------------------------------
    00059
    00060
    00061 StaticOCSPEstimateEnumerator::~StaticOCSPEstimateEnumerator()
    00062 {
    00063 }
    00064
    00065
    00066         //-----------------------------------------------
    00067         //      PUBLIC METHODS
    00068         //-----------------------------------------------
    00069
    00070         //-----------------------------------------------
    00071         //      PRIVATE METHODS
    00072         //-----------------------------------------------
    00073
    00074 bool
    00075 StaticOCSPEstimateEnumerator::generateTrajectories(void)
    00076 {
    00077         unsigned int i, j;
    00078         unsigned int obsValueUID;
    00079         unsigned int numInvalidCandidates = 0;
    00080         double estimateProbability = 0.0;
    00081         double trajectoryProbability = 0.0;
    00082         double heuristicProbability = 0.0;
    00083         double nroot = 1.0/(double)decisionVariables->getLength();
    00084         Estimate * anEst;
    00085         EstimateHeap bestFirstHeap = EstimateHeap(true);
    00086         BaseModelAssignmentArray * res;
    00087         BaseModelAssignmentArray * estAssign;
    00088         BaseModelClauseArray * currentObservationAssignments = new BaseModelClauseArray(true);
    00089         BaseModelClause * aObservationClause;
    00090         vector<double> allProbabilities = compiledObjectTrigger->getNormalizedEnabledTransitionProbabilities();
    00091         vector<double> probabilities;
    00092
    00093         if(currentTrajectories->getNumberEstimates() < 1)
    00094         {
    00095                 delete currentObservationAssignments;
    00096                 cerr << "<ModeEstimator> ERROR: There are no current trajectories!" << endl;
    00097                 return false;
    00098         }
    00099
    00100         if(debug > 2)
    00101         {
    00102                 cout << *compiledObjectTrigger << endl << endl;
    00103
    00104                 for(i=0; i<allProbabilities.size(); i++)
    00105                 {
    00106                         if(i%(allProbabilities.size()/currentTrajectories->getNumberEstimates()) == 0)
    00107                                 cout << endl << "Probabilities for Trajectory " <<
    00108                                 i/(allProbabilities.size()/currentTrajectories->getNumberEstimates()) << ": " << endl;
    00109                         cout << allProbabilities[i] << ", ";
    00110                 }
    00111                 cout << endl << endl;
    00112         }
    00113
    00114         // Set the size of the vectors
    00115         probabilities.resize(allProbabilities.size()/currentTrajectories->getNumberEstimates());
    00116
    00117         // Combine the transition probabilities into mode probability hueristics
    00118         for(i=0; i < probabilities.size(); i++)
    00119                 probabilities[i] = 0.0;
    00120         for(i=0; i < currentTrajectories->getNumberEstimates(); i++)
    00121         {
    00122                 for(j=0; j < probabilities.size(); j++)
    00123                 {       // Sum of the product of transition probability and n-th root of source estimate probability
    00124                         probabilities[j] += allProbabilities[i * probabilities.size() + j]
    00125                                 * pow(currentTrajectories->getEstimate(i)->getProbability(), nroot);
    00126                 }
    00127         }
    00128
    00129         // Create clause constraints for all the current observations
    00130         for(i=0; i<observationVariables->getLength(); i++)
    00131         {
    00132                 obsValueUID = compiledObjectTrigger->getCurrentCommandObservationValueUID((*observationVariables)[i]->getUID());
    00133                 if(obsValueUID != (unsigned int)Trigger::UNDEFINED)
    00134                 {
    00135                         aObservationClause = new BaseModelClause(BaseModelClause::DISJUNCTIVE, true);
    00136                         aObservationClause->pushBack(new BaseModelAssignment((*observationVariables)[i], (*observationVariables)[i]->getDomain()->getValue(obsValueUID)));
    00137                         currentObservationAssignments->pushBack(aObservationClause);
    00138                 }
    00139         }
    00140
    00141         // Initialize OpSat if one hasn't already been declared
    00142         if(opsats.size() < 1)
    00143         {
    00144                 opsats.resize(1);
    00145                 opsats[0] = new Opsat(*decisionVariables, *probabilityMap, probabilities, *myCCAmodel.getVariables(), *globalConstraints);
    00146                 setDebug(debug);
    00147         }
    00148
    00149         // Insert the current probabilities into BFIG
    00150         ((BFIGGenerator *)opsats[0]->getGenerator())->updateAllProbabilities(*decisionVariables, *probabilityMap, probabilities);
    00151
    00152
    00153         // Add all the observation constraints
    00154         for(j=0; j<currentObservationAssignments->getLength(); j++)
    00155         {
    00156                 if(debug > 2)
    00157                         cout << "Adding " << (*currentObservationAssignments)[j] << " constraint to opsat." << endl;
    00158                 opsats[0]->getTester()->addTemporaryConstraint((*currentObservationAssignments)[j]);            // Assumes that clearTemporaryConstraint() does
    00159                                                                                                                                                                                                         // not physically delete contraints
    00160         }
    00161
    00162         // Initialize next trajectories object
    00163         nextTrajectories = new Trajectories(currentTrajectories->getAccumulatedUncertainty(), debug);
    00164
    00165         // Intialize estimate counter (timing requirements)
    00166         numEstimatesGenerated = 0;
    00167
    00168         while(nextTrajectories->getNumberEstimates() < maxNumberEstimates)
    00169         {
    00170                 // Get next best estimate using Opsat
    00171                 res = opsats[0]->findSatisfyingSolution((unsigned int)1e10,(unsigned int)1e10);         // Not sure what to use for these #s
    00172                 numEstimatesGenerated++;
    00173                 if(res != NULL) {
    00174
    00175                         estimateProbability = 0.0;
    00176                         for(i=0; i < currentTrajectories->getNumberEstimates(); i++)
    00177                         {
    00178                                 trajectoryProbability = currentTrajectories->getEstimate(i)->getProbability();
    00179                                 for(j=0; j < res->getLength(); j++)
    00180                                 {
    00181                                         // Assumes that assignment order is the same as the decision variable order
    00182                                         trajectoryProbability *= allProbabilities[i * probabilities.size() + (*probabilityMap)[j] + (*res)[j]->getValue()->getUID()];
    00183                                         if((*res)[j]->getVariable()->getUID() != (*decisionVariables)[j]->getUID())
    00184                                         {
    00185                                                 cerr << "<ModeEstimator> ERROR: Probabilities aren't in expected order!" << endl;
    00186                                                 cout << "j: " << j << "  res uid: " << (*res)[j]->getVariable()->getUID() << "  decision UID: " << (*decisionVariables)[j]->getUID() << endl;
    00187                                                 resetOpsats();
    00188                                                 delete currentObservationAssignments;
    00189                                                 delete res;
    00190                                                 delete nextTrajectories;
    00191                                                 nextTrajectories = NULL;
    00192                                                 return false;
    00193                                         }
    00194                                 }
    00195                                 estimateProbability += trajectoryProbability;
    00196                         }
    00197
    00198                         heuristicProbability = 1.0;
    00199                         for(j=0; j < res->getLength(); j++)
    00200                                 heuristicProbability *= probabilities[(*probabilityMap)[j] + (*res)[j]->getValue()->getUID()];
    00201
    00202                         if(res->getLength() > 0 && estimateProbability > 0.0)
    00203                         {
    00204                                 estAssign = new BaseModelAssignmentArray(true);
    00205                                 for(j=0; j<res->getLength(); j++)
    00206                                         estAssign->pushBack(new BaseModelAssignment((*res)[j]->getVariable(), (*res)[j]->getValue()));
    00207                                 anEst = new Estimate(estAssign, estimateProbability, 0);        // always opsat[0]
    00208                                 bestFirstHeap.insert(anEst);
    00209
    00210                                 if(debug > 1)
    00211                                 {
    00212                                         cout << "<StaticOCSPEstimateEnumerator> Added candidate to heap." << endl;
    00213                                         cout << " " << *anEst << endl;
    00214                                         cout << " estimate probability: " << anEst->getProbability() << endl;
    00215                                         cout << " heuristic value: " << heuristicProbability << endl;
    00216                                 }
    00217                         }
    00218                         else
    00219                         {
    00220                                 numInvalidCandidates++;
    00221                                 if(debug > 0)
    00222                                 {
    00223                                         cout << "<StaticOCSPEstimateEnumerator> WARNING: Zero probability estimate was generated." << endl;
    00224                                         cout << "               res length: " << res->getLength() << endl;
    00225                                         cout << "               est prob: " << estimateProbability << endl;
    00226                                         cout << "               heuristic prob: " << heuristicProbability << endl;
    00227                                         cout << "Candidate: " << endl << *res << endl;
    00228                                 }
    00229                         }
    00230                         delete res;
    00231                 }
    00232                 else            // No more estimates left to generate in opsat
    00233                 {
    00234                         break;
    00235                 }
    00236
    00237                 do {
    00238                         bestFirstHeap.peekFirst(anEst);
    00239                         if(anEst->getProbability() >= heuristicProbability)             // This comparision has some serious consequences
    00240                                                                                                                                         // when running into machine error problems. If the
    00241                                                                                                                                         // two values are close to the same but slightly off,
    00242                                                                                                                                         // this condition will fail and not pop any estimates
    00243                                                                                                                                         // off the heap; leading to an explosion in the number
    00244                                                                                                                                         // of consistent states that opsat will be asked to
    00245                                                                                                                                         // generate. For now, we will normalize the approximate
    00246                                                                                                                                         // belief state to avoid this problem.
    00247                         {
    00248                                 bestFirstHeap.getFirst(anEst);
    00249                                 if(!nextTrajectories->addEstimate(anEst))       // Add it to the next trajectories list
    00250                                 {
    00251                                         cerr << "<StaticOCSPEstimateEnumerator> ERROR: Couldn't add estimate to next trajectories!" << endl;
    00252                                         resetOpsats();
    00253                                         delete currentObservationAssignments;
    00254                                         delete nextTrajectories;
    00255                                         nextTrajectories = NULL;
    00256                                         return false;
    00257                                 }
    00258                         }
    00259                 } while(anEst->getProbability() >= heuristicProbability && nextTrajectories->getNumberEstimates() < maxNumberEstimates
    00260                         && !bestFirstHeap.isEmpty());
    00261         }
    00262
    00263         // If all consistent estimates have been generated from opsat but more there are more approximate
    00264         // belief states to fill, start popping them off
    00265         while(!bestFirstHeap.isEmpty() && nextTrajectories->getNumberEstimates() < maxNumberEstimates)
    00266         {
    00267                 bestFirstHeap.getFirst(anEst);
    00268                 if(!nextTrajectories->addEstimate(anEst))       // Add it to the next trajectories list
    00269                 {
    00270                         cerr << "<StaticOCSPEstimateEnumerator> ERROR: Couldn't add estimate to next trajectories!" << endl;
    00271                         resetOpsats();
    00272                         delete currentObservationAssignments;
    00273                         delete nextTrajectories;
    00274                         nextTrajectories = NULL;
    00275                         return false;
    00276                 }
    00277         }
    00278
    00279         if(nextTrajectories->getNumberEstimates() < 1)
    00280         {
    00281                         cerr << "<StaticOCSPEstimateEnumerator> ERROR: Couldn't find any consistent estimates for next trajectories!" << endl;
    00282                         resetOpsats();
    00283                         delete currentObservationAssignments;
    00284                         delete nextTrajectories;
    00285                         nextTrajectories = NULL;
    00286                         return false;
    00287         }
    00288
    00289         if(numInvalidCandidates > 0 && debug > 1)
    00290                 cout << "Number of zero probability candidates: " << numInvalidCandidates << endl;
    00291
    00292         numConflictsGenerated = ((BFIGGenerator *)opsats[0]->getGenerator())->getNumberOfConflicts();
    00293
    00294         // Count up the number of elements expanded in all the queues;
    00295         numElementsExpanded = 0;
    00296         maxQueueSizeInBFIG = 0;
    00297         numCandidatesGenerated = 0;
    00298         for(i=0; i<opsats.size(); i++)
    00299         {
    00300                 numElementsExpanded += ((BFIGGenerator *)opsats[i]->getGenerator())->getTotalNumberNodesExpanded();
    00301                 maxQueueSizeInBFIG += ((BFIGGenerator *)opsats[i]->getGenerator())->getLargestQueueSinceReset();
    00302                 numCandidatesGenerated += ((BFIGGenerator *)opsats[i]->getGenerator())->getNumberCandidatesSinceReset();
    00303         }
    00304         numElementsExpanded -= numElementsPreviouslyExpanded;
    00305         numElementsPreviouslyExpanded += numElementsExpanded;
    00306
    00307         // Release Heap Memory
    00308         bestFirstHeap.makeEmpty();
    00309
    00310         // Reset Opsat Objects (deletion only occures when ModeEstimator is deleted)
    00311         resetOpsats();
    00312
    00313         // Clear observation constraints
    00314         delete currentObservationAssignments;
    00315
    00316         // Normalize new trajectories to avoid machine error
    00317         nextTrajectories->normalize();
    00318
    00319         return true;
    00320 }
    00321