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

# /Users/fpy/titan-1-5-MBARI/Mers/Estimation/Discrete/ApproximateBeliefState/DynamicModeEstimateGeneration/DynamicModeEstimateGenerator.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 #include "DynamicModeEstimateGenerator.h"
    00009
    00010 #ifdef _MSC_VER
    00011 #pragma warning( disable : 4786 )
    00012 #endif
    00013
    00014 #include <math.h>
    00015 #include <Mers/Estimation/Discrete/Estimate.h>
    00016 #include <Mers/Estimation/Discrete/Trigger.h>
    00017 #include <Mers/Estimation/Discrete/Trajectories.h>
    00018 #include <Mers/Estimation/Discrete/EstimateHeap.h>
    00019 #include <Mers/Solvers/SAT/GATSAT/OpSAT/Opsat.h>
    00020 #include <Mers/Solvers/SAT/AssignmentTesterInterface.h>
    00021 #include <Mers/Generators/BFIG/BFIGGenerator.h>
    00022 #include <Mers/Models/Base/BaseModelAssignment.h>
    00023 #include <Mers/Models/Base/BaseModelAssignmentArray.h>
    00024 #include <Mers/Models/Base/BaseModelClause.h>
    00025 #include <Mers/Models/Base/BaseModelClauseArray.h>
    00026
    00027 using namespace Mers::Estimation::Discrete::ApproximateBeliefState::DynamicModeEstimateGenertation;
    00028 using namespace Mers::Estimation::Discrete::ModeEstimationPerformance;
    00029 using namespace Mers::Estimation::Discrete::ModeEstimation;
    00030 using namespace Mers::Estimation::Discrete;
    00031 using Mers::Models::Base::BaseModelAssignment;
    00032 using Mers::Models::Base::BaseModelAssignmentArray;
    00033 using Mers::Models::Base::BaseModelClause;
    00034 using Mers::Models::Base::BaseModelClauseArray;
    00035 using Mers::Generators::BFIG::BFIGGenerator;
    00036 using Mers::Solvers::SAT::GATSAT::OpSAT::Opsat;
    00037 using Mers::Solvers::SAT::AssignmentTesterInterface;
    00038 using Mers::Models::CCA::CCAModel;
    00039 //using Mers::Utils::Containers::IntegerArray;
    00040 //using Mers::Utils::Containers::DynamicArray;
    00041 using std::vector;
    00042 using std::cout;
    00043 using std::cerr;
    00044 using std::endl;
    00045
    00046
    00047         //-----------------------------------------------
    00048         //      CONSTRUCTOR
    00049         //-----------------------------------------------
    00050
    00051 DynamicModeEstimateGenerator::DynamicModeEstimateGenerator(const CCAModel &cca, Trajectories * initialTrajectories)
    00052 : ModeEstimatorDebug(cca, initialTrajectories), numberExternalConflictsAdded(0)
    00053 {
    00054 }
    00055
    00056
    00057         //-----------------------------------------------
    00058         //      DESTRUCTOR
    00059         //-----------------------------------------------
    00060
    00061
    00062 DynamicModeEstimateGenerator::~DynamicModeEstimateGenerator()
    00063 {
    00064 }
    00065
    00066
    00067         //-----------------------------------------------
    00068         //      PUBLIC METHODS
    00069         //-----------------------------------------------
    00070
    00071 unsigned int
    00072 DynamicModeEstimateGenerator::getNumConflictsGenerated(void)
    00073 {
    00074         if(numberExternalConflictsAdded > numConflictsGenerated)
    00075         {
    00076                 cout << "<DynamicModeEstimateGenerator> ERROR: number of conflicts less than the number added!" << endl;
    00077                 return 0;
    00078         }
    00079         return numConflictsGenerated-numberExternalConflictsAdded;
    00080 }
    00081
    00082
    00083         //-----------------------------------------------
    00084         //      PRIVATE METHODS
    00085         //-----------------------------------------------
    00086
    00087 bool
    00088 DynamicModeEstimateGenerator::generateTrajectories(void)
    00089 {
    00090         unsigned int i, j, k;
    00091         unsigned int obsValueUID, bestIndex;
    00092         double estimateProbability = 0.0;
    00093         double totalRemainingProbabilitySpace = 1.0;
    00094         double trajectoryProbability = 0.0;
    00095         Estimate * anEst;
    00096         EstimateHeap bestFirstHeap = EstimateHeap(true);
    00097         BaseModelAssignmentArray * res;
    00098         BaseModelAssignmentArray * estAssign;
    00099         BaseModelClauseArray * currentObservationAssignments = new BaseModelClauseArray(true);
    00100         BaseModelClause * aObservationClause;
    00101         vector<double> allProbabilities = compiledObjectTrigger->getNormalizedEnabledTransitionProbabilities();
    00102         vector<double> probabilities;
    00103         vector<double> initEstimateProbabilitySpaceLeft;                // The amount of probability space left for each
    00104         bool foundNextMostLikely = false;                                               // estimate in the current trajectories list
    00105
    00106         numberExternalConflictsAdded = 0;
    00107
    00108         if(currentTrajectories->getNumberEstimates() < 1)
    00109         {
    00110                 delete currentObservationAssignments;
    00111                 cerr << "<DynamicModeEstimateGenerator> ERROR: There are no current trajectories!" << endl;
    00112                 return false;
    00113         }
    00114
    00115         if(debug > 1)
    00116         {
    00117                 cout << *compiledObjectTrigger << endl << endl;
    00118
    00119                 for(i=0; i<allProbabilities.size(); i++)
    00120                 {
    00121                         if(i%(allProbabilities.size()/currentTrajectories->getNumberEstimates()) == 0)
    00122                                 cout << endl << "Probabilities for Trajectory " <<
    00123                                 i/(allProbabilities.size()/currentTrajectories->getNumberEstimates()) << ": " << endl;
    00124                         cout << allProbabilities[i] << ", ";
    00125                 }
    00126                 cout << endl << endl;
    00127         }
    00128
    00129         // Set the size of the vectors
    00130         probabilities.resize(allProbabilities.size()/currentTrajectories->getNumberEstimates());
    00131         initEstimateProbabilitySpaceLeft.resize(currentTrajectories->getNumberEstimates());
    00132
    00133         // Create clause constraints for all the current observations
    00134         for(i=0; i<observationVariables->getLength(); i++)
    00135         {
    00136                 obsValueUID = compiledObjectTrigger->getCurrentCommandObservationValueUID((*observationVariables)[i]->getUID());
    00137                 if(obsValueUID != (unsigned int)Trigger::UNDEFINED)
    00138                 {
    00139                         aObservationClause = new BaseModelClause(BaseModelClause::DISJUNCTIVE, true);
    00140                         aObservationClause->pushBack(new BaseModelAssignment((*observationVariables)[i], (*observationVariables)[i]->getDomain()->getValue(obsValueUID)));
    00141                         currentObservationAssignments->pushBack(aObservationClause);
    00142                 }
    00143         }
    00144
    00145         // Intialize estimate counter (timing requirements)
    00146         numEstimatesGenerated = 0;
    00147
    00148         // Get the best transition from each initial state in the current trajectories
    00149         for(i=0; i<currentTrajectories->getNumberEstimates(); i++)
    00150         {
    00151                 initEstimateProbabilitySpaceLeft[i] = currentTrajectories->getEstimate(i)->getProbability();
    00152
    00153                 // Seperate out probabilities
    00154                 for(j=0; j<allProbabilities.size()/currentTrajectories->getNumberEstimates(); j++)
    00155                         probabilities[j] = allProbabilities[i * (allProbabilities.size()/currentTrajectories->getNumberEstimates()) + j];
    00156
    00157                 // Allocate opsat memory if necessary
    00158                 if(i == opsats.size())
    00159                 {
    00160                         opsats.resize(opsats.size()+1);
    00161                         opsats[i] = new Opsat(*decisionVariables, *probabilityMap, probabilities, *myCCAmodel.getVariables(), *globalConstraints);
    00162                         setDebug(debug);
    00163                 } else
    00164                         // Insert the current probabilities into BFIG
    00165                         ((BFIGGenerator *)opsats[i]->getGenerator())->updateAllProbabilities(*decisionVariables, *probabilityMap, probabilities);
    00166
    00167                 // Add all the observation constraints. The same constraints apply to all trajectories
    00168                 for(j=0; j<currentObservationAssignments->getLength(); j++)
    00169                 {
    00170                         if(debug > 1)
    00171                                 cout << "Adding " << (*currentObservationAssignments)[j] << " constraint to opsat." << endl;
    00172                         opsats[i]->getTester()->addTemporaryConstraint((*currentObservationAssignments)[j]);            // Assumes that clearTemporaryConstraint() does
    00173                                                                                                                                                                 // not physically delete contraints
    00174                 }
    00175
    00176                 // Get best estimate given orignal state using Opsat
    00177                 res = opsats[i]->findSatisfyingSolution((unsigned int)1e10,(unsigned int)1e10);         // Not sure what to use for these #s
    00178                 numEstimatesGenerated++;
    00179                 if(res != NULL) {
    00180                         estimateProbability = 0.0;
    00181                         for(k=0; k < currentTrajectories->getNumberEstimates(); k++)
    00182                         {
    00183                                 trajectoryProbability = currentTrajectories->getEstimate(k)->getProbability();
    00184                                 for(j=0; j < res->getLength(); j++)
    00185                                 {
    00186                                         // Assumes that assignment order is the same as the decision variable order
    00187                                         trajectoryProbability *= allProbabilities[k * probabilities.size() + (*probabilityMap)[j] + (*res)[j]->getValue()->getUID()];
    00188                                         if((*res)[j]->getVariable()->getUID() != (*decisionVariables)[j]->getUID())
    00189                                         {
    00190                                                 cerr << "<DynamicModeEstimateGenerator> ERROR: Probabilities aren't in expected order!" << endl;
    00191                                                 cout << "j: " << j << "  res uid: " << (*res)[j]->getVariable()->getUID() << "  decision UID: " << (*decisionVariables)[j]->getUID() << endl;
    00192                                                 resetOpsats();
    00193                                                 delete currentObservationAssignments;
    00194                                                 delete res;
    00195                                                 return false;
    00196                                         }
    00197                                 }
    00198                                 initEstimateProbabilitySpaceLeft[k] -= trajectoryProbability;
    00199                                 estimateProbability += trajectoryProbability;
    00200                         }
    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, i);
    00208
    00209                                 for(j=0; j<opsats.size(); j++)
    00210                                         opsats[j]->getGenerator()->addConflict(estAssign);
    00211                                 numberExternalConflictsAdded++;
    00212 // ******** Probably should add this estimate to the conflict list of each bfig instantiation so no other bfigs return
    00213 //                      the same estimate... deal with this later
    00214
    00215                                 totalRemainingProbabilitySpace -= estimateProbability;
    00216                                 bestFirstHeap.insert(anEst);
    00217                         }
    00218                         else
    00219                                 cout << "<DynamicModeEstimateGenerator> WARNING: No consistent states found for trajectory " << i << "!" << endl;
    00220                         delete res;
    00221                 }
    00222         }
    00223
    00224
    00225         if(bestFirstHeap.isEmpty())
    00226         {
    00227                 cout << "<DynamicModeEstimateGenerator> ERROR: No consistent states found!" << endl;
    00228                 resetOpsats();
    00229                 delete currentObservationAssignments;
    00230                 return false;
    00231         }
    00232         else
    00233                 nextTrajectories = new Trajectories(currentTrajectories->getAccumulatedUncertainty(), debug);
    00234
    00235
    00236         while(!bestFirstHeap.isEmpty())
    00237         {
    00238                 // Add estimates to the next trajectories if their probability is greater than the total amount of
    00239                 // probability space left
    00240                 foundNextMostLikely = false;
    00241                 do {
    00242                         bestFirstHeap.peekFirst(anEst);
    00243                         if(anEst->getProbability() >= totalRemainingProbabilitySpace)
    00244                         {
    00245                                 foundNextMostLikely = true;
    00246                                 bestFirstHeap.getFirst(anEst);
    00247                                 if(!nextTrajectories->addEstimate(anEst))       // Add it to the next trajectories list
    00248                                 {
    00249                                         cerr << "<DynamicModeEstimateGenerator> ERROR: Couldn't add estimate to next trajectories!" << endl;
    00250                                         resetOpsats();
    00251                                         delete currentObservationAssignments;
    00252                                         delete nextTrajectories;
    00253                                         nextTrajectories = NULL;
    00254                                         return false;
    00255                                 }
    00256                         }
    00257                 } while(anEst->getProbability() >= totalRemainingProbabilitySpace && nextTrajectories->getNumberEstimates() < maxNumberEstimates
    00258                         && !bestFirstHeap.isEmpty());
    00259
    00260                 if(!foundNextMostLikely)                // Take the best out of the heap anyway... cuz Vanny's algorithm SUCKS ASS and doesn't work at all
    00261                 {
    00262                                 bestFirstHeap.getFirst(anEst);
    00263                                 if(!nextTrajectories->addEstimate(anEst))       // Add it to the next trajectories list
    00264                                 {
    00265                                         cerr << "<DynamicModeEstimateGenerator> ERROR: Couldn't add estimate to next trajectories!" << endl;
    00266                                         resetOpsats();
    00267                                         delete currentObservationAssignments;
    00268                                         delete nextTrajectories;
    00269                                         nextTrajectories = NULL;
    00270                                         return false;
    00271                                 }
    00272                 }
    00273
    00274
    00275                 if(nextTrajectories->getNumberEstimates() == maxNumberEstimates)
    00276                         break;
    00277
    00278                 // Figure our which OpSat engine has the most probability space left and expand that one
    00279                 bestIndex = 0;
    00280                 for(i=0; i<initEstimateProbabilitySpaceLeft.size(); i++)
    00281                 {
    00282                         if(initEstimateProbabilitySpaceLeft[i] > initEstimateProbabilitySpaceLeft[bestIndex])
    00283                                 bestIndex = i;
    00284                 }
    00285
    00286                 res = opsats[bestIndex]->findSatisfyingSolution((unsigned int)1e10,(unsigned int)1e10); // Not sure what to use for these #s
    00287 //              cout << "Conflicts Gen: " << ((BFIGGenerator *)opsats[anEst->getOriginatedTrajectoryNumber()]->getGenerator())->bfig->conflicts.size() << endl;
    00288 //              for(j = 0; j < ((BFIGGenerator *)opsats[anEst->getOriginatedTrajectoryNumber()]->getGenerator())->bfig->conflicts.size(); j++)
    00289 //                      cout << ((BFIGGenerator *)opsats[anEst->getOriginatedTrajectoryNumber()]->getGenerator())->bfig->conflicts[j]->getAssignments() << endl;
    00290                 numEstimatesGenerated++;
    00291                 if(res != NULL) {
    00292
    00293                         estimateProbability = 0.0;
    00294                         for(k=0; k < currentTrajectories->getNumberEstimates(); k++)
    00295                         {
    00296                                 trajectoryProbability = currentTrajectories->getEstimate(k)->getProbability();
    00297                                 for(j=0; j < res->getLength(); j++)
    00298                                 {
    00299                                         // Assumes that assignment order is the same as the decision variable order
    00300                                         trajectoryProbability *= allProbabilities[k * probabilities.size() + (*probabilityMap)[j] + (*res)[j]->getValue()->getUID()];
    00301                                         if((*res)[j]->getVariable()->getUID() != (*decisionVariables)[j]->getUID())
    00302                                         {
    00303                                                 cerr << "<DynamicModeEstimateGenerator> ERROR: Probabilities aren't in expected order!" << endl;
    00304                                                 cout << "j: " << j << "  res uid: " << (*res)[j]->getVariable()->getUID() << "  decision UID: " << (*decisionVariables)[j]->getUID() << endl;
    00305                                                 resetOpsats();
    00306                                                 delete currentObservationAssignments;
    00307                                                 delete nextTrajectories;
    00308                                                 nextTrajectories = NULL;
    00309                                                 delete res;
    00310                                                 return false;
    00311                                         }
    00312                                 }
    00313                                 initEstimateProbabilitySpaceLeft[k] -= trajectoryProbability;
    00314                                 estimateProbability += trajectoryProbability;
    00315                         }
    00316
    00317                         if(res->getLength() > 0 && estimateProbability > 0.0)
    00318                         {
    00319                                 estAssign = new BaseModelAssignmentArray(true);
    00320                                 for(j=0; j<res->getLength(); j++)
    00321                                         estAssign->pushBack(new BaseModelAssignment((*res)[j]->getVariable(), (*res)[j]->getValue()));
    00322                                 anEst = new Estimate(estAssign, estimateProbability, bestIndex);
    00323                                 for(j=0; j<opsats.size(); j++)
    00324                                         opsats[j]->getGenerator()->addConflict(estAssign);
    00325                                 numberExternalConflictsAdded++;
    00326 // ******** Probably should add this estimate to the conflict list of each bfig instantiation so no other bfigs return
    00327 //                      the same estimate... deal with this later
    00328
    00329                                 totalRemainingProbabilitySpace -= estimateProbability;
    00330                                 bestFirstHeap.insert(anEst);
    00331                         }
    00332                         delete res;
    00333                 }
    00334         }
    00335
    00336         // Number of conflicts generated w/o double counting them
    00337         numConflictsGenerated = ((BFIGGenerator *)opsats[0]->getGenerator())->getNumberOfConflicts();
    00338
    00339         // Count up the number of elements expanded in all the queues;
    00340         numElementsExpanded = 0;
    00341         maxQueueSizeInBFIG = 0;
    00342         numCandidatesGenerated = 0;
    00343         for(i=0; i<opsats.size(); i++)
    00344         {
    00345                 numElementsExpanded += ((BFIGGenerator *)opsats[i]->getGenerator())->getTotalNumberNodesExpanded();
    00346                 maxQueueSizeInBFIG += ((BFIGGenerator *)opsats[i]->getGenerator())->getLargestQueueSinceReset();
    00347                 numCandidatesGenerated += ((BFIGGenerator *)opsats[i]->getGenerator())->getNumberCandidatesSinceReset();
    00348         }
    00349         numElementsExpanded -= numElementsPreviouslyExpanded;
    00350         numElementsPreviouslyExpanded += numElementsExpanded;
    00351
    00352         // Release Heap Memory
    00353         bestFirstHeap.makeEmpty();
    00354
    00355         // Reset Opsat Objects (deletion only occures when ModeEstimator is deleted)
    00356         resetOpsats();
    00357
    00358         // Clear observation constraints
    00359         delete currentObservationAssignments;
    00360
    00361         // Normalize new trajectories to avoid machine error
    00362         nextTrajectories->normalize();
    00363
    00364         return true;
    00365 }
    00366