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

# /Users/fpy/titan-1-5-MBARI/Mers/Generators/BFIG/BFIGGenerator.cpp

    00001
    00002 //
    00003 // Copyright (c) 2004           Model-based Embedded and Robotic Systems Group
    00004 // All Rights Reserved          Massachusetts Institute of Technology
    00005 //
    00006 // This software is provided as is. Model-based Embedded and Robotic Systems
    00007 // (MERS) group does not assume any responsibility.
    00008 //
    00009 // Organization:        Model-based Embedded and Robotic Systems Group
    00010 //                                      Massachusetts Institute of Technology
    00011 //
    00013 //# CHECKED MJP 08-07-2002
    00014
    00015 #ifdef _MSC_VER
    00016 #pragma warning( disable : 4786 )
    00017 #endif
    00018
    00019 #include "BFIGGenerator.h"
    00020
    00021 #include <cassert>
    00022 #include "BFIGAssignment.h"
    00023 #include "BFIGConstraint.h"
    00024 #include "BFIGImplicant.h"
    00025 #include "BFIGVariable.h"
    00026 #include "BFIGImplicantExpander.h"
    00027 #include "BFIGObjectiveFunction.h"
    00028 //#include "ConflictDB.h"
    00029
    00030 #include <Mers/Models/Base/BaseModelAssignmentArray.h>
    00031 #include <Mers/Models/Base/BaseModelDomain.h>
    00032 #include <Mers/Models/Base/BaseModelVariableArray.h>
    00033 #include <Mers/Models/Base/BaseModelSentence.h>
    00034
    00035 #include <Mers/Utils/Containers/IntegerArray.h>
    00036
    00037 #include <Mers/Utils/Exceptions/Exception.h>
    00038
    00040
    00041 using namespace Mers::Generators::BFIG;
    00042 using Mers::Utils::Containers::IntegerArray;
    00043 using Mers::Models::Base::BaseModelVariableArray;
    00044 using Mers::Models::Base::BaseModelVariable;
    00045 using Mers::Models::Base::BaseModelValue;
    00046 using Mers::Models::Base::BaseModelAssignmentArray;
    00047 using Mers::Models::Base::BaseModelAssignment;
    00048 using Mers::Models::Base::BaseModelDomain;
    00049 using Mers::Models::Base::BaseModelClauseArray;
    00050 using std::ostream;
    00051 using std::vector;
    00052 using std::cout;
    00053 using std::endl;
    00054
    00056
    00057 BFIGGenerator::BFIGGenerator(BFIGFactory &bfigFactory,
    00058         BaseModelVariableArray const & variableSet,
    00059         BFIGObjectiveFunction const * ocspObjectiveFunction,
    00060         BaseModelVariableArray const &allVariables,
    00061         double cutoffCost)
    00062         : mSearchBMVars(false), bmImplicant(true)
    00063 {
    00064         (void) allVariables;
    00065         debug = 0;
    00066
    00067         // Start by sorting our variable set
    00068         sortBMVariables(variableSet, mSearchBMVars);
    00069
    00070         if(variableSet.getLength() != mSearchBMVars.getLength()) {
    00071                 // This works since sortBMVariables eliminates duplicates
    00072                 fprintf(stderr, "BFIGGenerator::BFIGGenerator: Error: Duplicate variables "
    00073                                                 "passed into BFIG\n");
    00074                 // Fatal error.
    00075                 throw 1;
    00076         }
    00077
    00078         compactBFIGVars.resize(mSearchBMVars.getLength());
    00079
    00080         // Create a BFIG variable for every searchable BM Variable.
    00081         for(unsigned int y = 0; y < mSearchBMVars.getLength(); y++)
    00082         {
    00083                 unsigned int index = bfindBMVariable(mSearchBMVars, variableSet[y]->getUID());
    00084
    00085                 vector<double> costs;           // Should set the costs for all values in the domain to 0
    00086                 costs.resize(variableSet[y]->getDomain()->getLength());
    00087
    00088                 BFIGVariable * bfigVariable = new BFIGVariable(index, variableSet[y], costs, false);
    00089                 compactBFIGVars[index] = bfigVariable;
    00090         }
    00091
    00092         bfig = bfigFactory.create(compactBFIGVars, ocspObjectiveFunction, cutoffCost);
    00093 }
    00094
    00096
    00097 BFIGGenerator::BFIGGenerator(BFIGFactory &bfigFactory,
    00098         BaseModelVariableArray const & variableSet,
    00099         BaseModelVariableArray const &allVariables,
    00100         IntegerArray const &valueMap, vector<double> const & probabilities,
    00101         double cutoffCost, bool useProb)
    00102         : mSearchBMVars(false), bmImplicant(true)
    00103 {
    00104         (void) allVariables;
    00105         debug = 0;
    00106
    00107         // Start by sorting our variable set
    00108         sortBMVariables(variableSet, mSearchBMVars);
    00109
    00110         if(variableSet.getLength() != mSearchBMVars.getLength()) {
    00111                 // This works since sortBMVariables eliminates duplicates
    00112                 fprintf(stderr, "BFIGGenerator::BFIGGenerator: Error: Duplicate variables "
    00113                                                 "passed into BFIG\n");
    00114                 // Fatal error.
    00115                 throw 1;
    00116         }
    00117
    00118         compactBFIGVars.resize(mSearchBMVars.getLength());
    00119
    00120         // Create a BFIG variable for every searchable BM Variable.
    00121         for(unsigned int y = 0; y < mSearchBMVars.getLength(); y++)
    00122         {
    00123                 unsigned int index = bfindBMVariable(mSearchBMVars,
    00124                         variableSet[y]->getUID());
    00125                 vector<double> costs = extractVariableProbabilities(*variableSet[y], y,
    00126                         valueMap, probabilities);
    00127                 BFIGVariable * bfigVariable = variableToVariable(index,
    00128                         variableSet[y], costs, useProb);
    00129                 compactBFIGVars[index] = bfigVariable;
    00130         }
    00131
    00132         bfig = bfigFactory.create(compactBFIGVars, cutoffCost);
    00133 }
    00134
    00136
    00137 BFIGGenerator::BFIGGenerator(BFIGFactory &bfigFactory,
    00138         BaseModelVariableArray const & variableSet,
    00139         BaseModelVariableArray const &allVariables,
    00140         IntegerArray const &valueMap, vector<double> const & probabilities,
    00141         bool useProb)
    00142         : mSearchBMVars(false), bmImplicant(true)
    00143 {
    00144         (void) allVariables;
    00145         debug = 0;
    00146
    00147         // Start by sorting our variable set
    00148         sortBMVariables(variableSet, mSearchBMVars);
    00149
    00150         if(variableSet.getLength() != mSearchBMVars.getLength()) {
    00151                 // This works since sortBMVariables eliminates duplicates
    00152                 fprintf(stderr, "BFIGGenerator::BFIGGenerator: Error: Duplicate variables "
    00153                                                 "passed into BFIG\n");
    00154                 // Fatal error.
    00155                 throw 1;
    00156         }
    00157
    00158         compactBFIGVars.resize(mSearchBMVars.getLength());
    00159
    00160         // Create a BFIG variable for every searchable BM Variable.
    00161         for(unsigned int y = 0; y < mSearchBMVars.getLength(); y++)
    00162         {
    00163                 unsigned int index = bfindBMVariable(mSearchBMVars,
    00164                         variableSet[y]->getUID());
    00165                 vector<double> costs = extractVariableProbabilities(*variableSet[y], y,
    00166                         valueMap, probabilities);
    00167                 BFIGVariable * bfigVariable = variableToVariable(index,
    00168                         variableSet[y], costs, useProb);
    00169                 compactBFIGVars[index] = bfigVariable;
    00170         }
    00171
    00172         bfig = bfigFactory.create(compactBFIGVars);
    00173 }
    00174
    00176
    00177 BFIGGenerator::~BFIGGenerator()
    00178 {
    00179 /*#
    00180         //if (debug)
    00181         //      cout << "deleting bfig... " << flush;
    00182 */
    00183         if (bfig)
    00184                 delete bfig;
    00185
    00186         for(unsigned int x = 0; x < compactBFIGVars.size(); x++){
    00187                 if(compactBFIGVars[x] != NULL)
    00188                         delete compactBFIGVars[x];
    00189         }
    00190 /*#
    00191         //if (debug)
    00192         //      cout << "done." << endl;
    00193 */
    00194 }
    00195
    00197
    00198 void
    00199 BFIGGenerator::addConflict(BaseModelAssignmentArray const * newConflict)
    00200 {
    00201         vector<BFIGAssignment const *> inconsistentAssignments;
    00202         unsigned int length = newConflict->getLength();
    00203         for(unsigned int x = 0; x < length; x++){
    00204                 BaseModelAssignment const &bmAsg = *((*newConflict)[x]);
    00205                 BaseModelVariable const &bmVar = *(bmAsg.getVariable());
    00206                 BaseModelValue const &bmVal = *(bmAsg.getValue());
    00207
    00208                 BFIGVariable * bfigVar;
    00209                 if(FAILED(getBFIGVariable(bmVar.getUID(), bfigVar))) {
    00210                         throw 1;
    00211                 }
    00212
    00213                 BFIGAssignment const * bfigAsg =
    00214                         bfigVar->getAssignmentByValue(bmVal.getUID());
    00215                 inconsistentAssignments.push_back(bfigAsg);
    00216         }
    00217
    00218         BFIGConstraint * conflict = new BFIGConstraint(inconsistentAssignments);
    00219         bfig->addConflict(conflict);
    00220 }
    00221
    00223
    00224 void
    00225 BFIGGenerator::addConflict(BaseModelAssignmentArray const & newConflict)
    00226 {
    00227         addConflict(&newConflict);
    00228 }
    00229
    00231
    00232 void
    00233 BFIGGenerator::addConflicts(BaseModelClauseArray const & newConflicts)
    00234 {
    00235         unsigned int length = newConflicts.getLength();
    00236         for(unsigned int x = 0; x < length; x++){
    00237                 addConflict(*(newConflicts[x]));
    00238         }
    00239 }
    00240
    00242
    00243 void
    00244 BFIGGenerator::addConflict(BFIGConstraint const * newConflict)
    00245 {
    00246         //conflictDB->addConflict(newConflict);  // not currently used.
    00247         bfig->addConflict(newConflict);
    00248 }
    00249
    00251
    00252 void
    00253 BFIGGenerator::addConflicts(vector<BFIGConstraint const *> const & newConflicts)
    00254 {
    00255         //conflictDB->addConflict(newConflict);  // not currently used.
    00256         bfig->addConflicts(newConflicts);
    00257 }
    00258
    00260
    00261 void
    00262 BFIGGenerator::addConflict(BFIGConstraint const & newConflict)
    00263 {
    00264         BFIGConstraint * conflictp = new BFIGConstraint(newConflict);
    00265         //unfreeConstraints.push_back(conflictp);  // must free this one ourselves...
    00266         addConflict(conflictp);
    00267 }
    00268
    00270
    00271 BaseModelAssignmentArray const *
    00272 BFIGGenerator::generateBestImplicant(long maximumVisitedStateCount,
    00273                                            long maxSize, bool expand)
    00274 {
    00275         (void) expand;
    00276         bmImplicant.makeEmpty();
    00277
    00278         // non-copied memory:
    00279         BFIGImplicant const * bestImplicant_p =
    00280                 bfig->getBestImplicant(maximumVisitedStateCount, maxSize);
    00281
    00282         if (!bestImplicant_p)  // no best generated!
    00283         {
    00284                 if (debug)
    00285                         cout << "(BFIG found no best implicant)" << endl;
    00286                 return NULL;
    00287         }
    00288
    00289         vector<BFIGAssignment const *> bfigAsgs = bestImplicant_p->getAssignments();
    00290
    00291         unsigned int bfigAsgsLength = bfigAsgs.size();
    00292         for(unsigned int x = 0; x < bfigAsgsLength; x++){
    00293                 BFIGAssignment const &bfigAsg = *(bfigAsgs[x]);
    00294                 BaseModelVariable * bmVar = mSearchBMVars[bfigAsg.getNameInt()];
    00295                 BaseModelAssignment * bmAsg =
    00296                         new BaseModelAssignment(bmVar, bmVar->getValue(bfigAsg.getValueInt()),
    00297                         BaseModelAssignment::Positive);
    00298                 bmImplicant.pushBack(bmAsg);
    00299         }
    00300
    00301         return &bmImplicant;
    00302 }
    00303
    00305
    00306 void
    00307 BFIGGenerator::markBestFeasible()
    00308 {
    00309         bfig->markBestFeasible();
    00310 }
    00311
    00313
    00314 void
    00315 BFIGGenerator::printStatistics(bool verbose) const
    00316 {
    00317         bfig->printStatistics(verbose);
    00318 }
    00319
    00321
    00322 void BFIGGenerator::restart()
    00323 {
    00324         bfig->restart();
    00325 }
    00326
    00328
    00329 void BFIGGenerator::reset()
    00330 {
    00331         bfig->reset();
    00332 }
    00333
    00335
    00336 void
    00337 BFIGGenerator::setDebug(unsigned int state)
    00338 {
    00339         AssignmentGeneratorInterface::setDebug(state);
    00340         bfig->setDebug(state);
    00341 }
    00342
    00344
    00345 /*
    00346 void BFIGGenerator::normalizeCosts()
    00347 {
    00348         // Normalize every variable
    00349         unsigned int varUID;
    00350         for (varUID = 0; varUID != compactBFIGVars.size(); varUID++)
    00351         {
    00352                 BFIGVariable * variable = compactBFIGVars[varUID];
    00353                 variable->normalizeCosts();
    00354         }
    00355 }
    00356 */
    00357
    00359
    00360 int BFIGGenerator::updateProbability(unsigned int variableUID,
    00361         unsigned int valueUID, double prob)
    00362 {
    00363         BFIGVariable * variable;
    00364         if(FAILED(getBFIGVariable(variableUID, variable)))
    00365                 return 1;
    00366
    00367         double cost = variable->probToCost(prob);
    00368         return variable->setCost(valueUID, cost);
    00369 }
    00370
    00372
    00373 int BFIGGenerator::updateProbabilities(unsigned int variableUID, double probs[])
    00374 {
    00375         BFIGVariable * variable;
    00376         if(FAILED(getBFIGVariable(variableUID, variable)))
    00377                 return 1;
    00378
    00379         unsigned int numVals = variable->getDomainTotalSize();
    00380         double * costs = new double[numVals];
    00381         for(unsigned int x = 0; x < numVals; x++) {
    00382                 costs[x] = variable->probToCost(probs[x]);
    00383         }
    00384
    00385         return variable->setCosts(costs);
    00386 }
    00387
    00389
    00390 int
    00391 BFIGGenerator::updateAllProbabilities(BaseModelVariableArray const &decisionVars,
    00392                 IntegerArray const &valueMap, vector<double> const &allProbabilities)
    00393 {
    00394         unsigned int success = 0;
    00395         unsigned int i, j;
    00396         vector<double> variableProbabilities;
    00397         for(i = 0; i < decisionVars.getLength(); i++)
    00398         {
    00399                 BaseModelVariable const * dVar = decisionVars[i];
    00400                 variableProbabilities = extractVariableProbabilities(*dVar, i,
    00401                         valueMap, allProbabilities);
    00402
    00403                 BaseModelDomain const &dDom = *(dVar->getDomain());
    00404                 unsigned int dDomLen = dDom.getLength();
    00405                 for(j = 0; j < dDomLen; j++)
    00406                         success += updateProbability(dVar->getUID(), dDom[j]->getUID(),
    00407                                 variableProbabilities[j]);
    00408         }
    00409         return success;
    00410 }
    00411
    00413
    00414 int BFIGGenerator::updateCost(unsigned int variableUID,
    00415         unsigned int valueUID, double cost)
    00416 {
    00417         BFIGVariable * variable;
    00418         if(FAILED(getBFIGVariable(variableUID, variable)))
    00419                 return 1;
    00420
    00421         return variable->setCost(valueUID, cost);
    00422 }
    00423
    00425
    00426 int BFIGGenerator::updateCosts(unsigned int variableUID, double costs[])
    00427 {
    00428         BFIGVariable * variable;
    00429         if(FAILED(getBFIGVariable(variableUID, variable)))
    00430                 return 1;
    00431
    00432         return variable->setCosts(costs);
    00433 }
    00434
    00436
    00437 unsigned int BFIGGenerator::getFullAssignmentSize()
    00438 {
    00439         return compactBFIGVars.size();
    00440 }
    00441
    00443
    00444 Mers::Generators::BFIG::BFIGVariable *
    00445 BFIGGenerator::variableToVariable(unsigned int bfigVarIndex,
    00446         BaseModelVariable const * varIn, vector<double> &costs, bool useProb) const
    00447 {
    00448
    00449         return new BFIGVariable(bfigVarIndex, varIn, costs, useProb);
    00450
    00451 }
    00452
    00454
    00455 void BFIGGenerator::sortBMVariables(
    00456         BaseModelVariableArray const &unsortedArray,
    00457         BaseModelVariableArray &sortedArray)
    00458 {
    00459         // make sure we're aware of the case where the two arrays are the same.
    00460         bool sameArray = (&unsortedArray) == (&sortedArray);
    00461
    00462         // Since this is likely sorted most of the time, first check if we
    00463         // need to do anything.
    00464         bool sorted = true;
    00465         for(unsigned int i = 1; i < unsortedArray.getLength() && sorted; i++) {
    00466                 sorted = unsortedArray[i - 1]->getUID() < unsortedArray[i]->getUID();
    00467         }
    00468
    00469         // If we're already sorted, just copy the data.
    00470         if(sorted) {
    00471                 if(!sameArray) {
    00472                         sortedArray.makeEmpty();
    00473                         sortedArray.resize(unsortedArray.getLength());
    00474                         for(unsigned int j = 0; j < unsortedArray.getLength(); j++) {
    00475                                 sortedArray[j] = (BaseModelVariable *) unsortedArray[j];
    00476                         }
    00477                 }
    00478                 return;
    00479         }
    00480
    00481         // We presently don't handle the case where the two arrays are the same
    00482         // and we need to sort.
    00483         if(sameArray) {
    00484                 fprintf(stderr, "BFIGGenerator::sortBMVariables: Error: Invalide use:"
    00485                                                 " Same source and destination\n");
    00486                 // Throw instead of return because this is unrecoverable (Code bug).
    00487                 // Probably should throw a real exception.
    00488                 throw 1;
    00489         }
    00490
    00491         sortedArray.makeEmpty();
    00492
    00493         // We need to sort the array.
    00494         for(unsigned int j = 0; j < unsortedArray.getLength(); j++) {
    00495                 BaseModelVariable * var = (BaseModelVariable *) unsortedArray[j];
    00496                 unsigned int i = bfindBMVariable(sortedArray, var->getUID());
    00497                 if(i >= sortedArray.getLength() || sortedArray[i]->getUID() != var->getUID()) {
    00498                         sortedArray.pushBack(NULL);
    00499                         for(unsigned int k = sortedArray.getLength() - 1; k > i; k--) {
    00500                                 sortedArray[k] = sortedArray[k - 1];
    00501                         }
    00502                         sortedArray[i] = var;
    00503                 }
    00504         }
    00505 }
    00506
    00508
    00509 unsigned int BFIGGenerator::bfindBMVariable(BaseModelVariableArray &sortedVars,
    00510         unsigned int uid)
    00511 {
    00512         unsigned int l, r;
    00513         l = 0;
    00514         r = sortedVars.getLength();
    00515         while(l < r) {
    00516                 unsigned int m = (l + r) >> 1;
    00517                 unsigned int varUID = sortedVars[m]->getUID();
    00518                 if(varUID < uid) {
    00519                         l = m + 1;
    00520                 } else if(varUID > uid) {
    00521                         r = m;
    00522                 } else {
    00523                         return m;
    00524                 }
    00525         }
    00526         return l;
    00527 }
    00528
    00530
    00531 unsigned int BFIGGenerator::getBFIGVariable(unsigned int bmVarUID,
    00532         BFIGVariable * &bfigVar)
    00533 {
    00534         unsigned int index = bfindBMVariable(mSearchBMVars, bmVarUID);
    00535         if(index >= mSearchBMVars.getLength() ||
    00536                  mSearchBMVars[index]->getUID() != bmVarUID) {
    00537                 fprintf(stderr, "BFIGGenerator::getBFIGVariable: Warning, variable %d "
    00538                         "was not found\n", bmVarUID);
    00539                 return 1;
    00540         }
    00541         bfigVar = compactBFIGVars[index];
    00542         if(bfigVar == NULL)
    00543         {
    00544                 fprintf(stderr, "BFIGGenerator::getBFIGVariable: Warning, variable %d "
    00545                         "(BFIG Variable %d) was NULL\n", bmVarUID, index);
    00546                 return 1;
    00547         }
    00548         return 0;
    00549 }
    00550
    00552
    00553 vector<double>
    00554 BFIGGenerator::extractVariableProbabilities(BaseModelVariable const &myVariable,
    00555                 unsigned int index, IntegerArray const &valueMap, vector<double> const &allProbabilities) const
    00556 {
    00557         vector<double> myProbabilities;
    00558         unsigned int numValues = myVariable.getDomain()->getLength();
    00559
    00560         unsigned int probsIndex = valueMap[index];                      // Shouldn't have to use index... should be myVariable.getUID()
    00561         unsigned int probsOffs;
    00562
    00563         for(probsOffs = 0; probsOffs < numValues &&
    00564                 probsIndex + probsOffs < allProbabilities.size(); probsOffs++) {
    00565                 myProbabilities.push_back(allProbabilities[probsIndex + probsOffs]);
    00566         }
    00567
    00568         if(probsOffs < numValues) {
    00569                 fprintf(stderr, "Error, the variable %d %s does not have an "
    00570                         "associated probability for %d values\n", myVariable.getUID(),
    00571                         myVariable.getName(), numValues - probsOffs);
    00572         }
    00573
    00574         for(; probsOffs < numValues; probsOffs++) {
    00575                 myProbabilities.push_back(0);
    00576         }
    00577
    00578         return myProbabilities;
    00579 }
    00580
    00581