示例#1
0
double BeliefValue::GetValue(const BeliefInterface &Belief,
                             const VectorSet &v,
                             const vector<bool> mask)
{
    double x,maxVal=-DBL_MAX;
    bool maskValid=false;
    vector<double> values(v.size2());

    if(mask.size()!=v.size1())
    {
        throw(E("BeliefValue::GetValue: mask has incorrect size"));
    }

    for(unsigned int i=0;i!=v.size1();i++)
    {
        if(mask[i])
        {
            maskValid=true;
            for(unsigned int k=0;k!=v.size2();++k)
                values[k]=v(i,k);

            // compute inner product of belief with vector
            x=Belief.InnerProduct(values);

            // keep the maximizing value
            if(x>maxVal)
                maxVal=x;
        }
    }

    return(maxVal);
}
/** Returns a vector<int> which for each vector k in V specifies
 * whether it is unique, in which case vector<int>[k] is set to -1, or
 * whether it is a duplicate of another vector l in V, in which case
 * vector<int>[k] is set to the index of l, where 0 <= l <
 * V.size()). */
vector<int> AlphaVectorPlanning::GetDuplicateIndices(const VectorSet &V)
{
    int nrInV=V.size1(), nrS=V.size2();
    bool equal;

    vector<int> duplicates(nrInV,-1);

    for(int i=1;i!=nrInV;++i) // start at 1, first is never a duplicate
    {
        for(int j=0;j!=i;++j) // loop over all previous vectors
        {
            equal=true;
            for(int s=0;s!=nrS;++s)
            {
                if(abs(V(i,s)-V(j,s))>PROB_PRECISION)
                {
                    equal=false;
                    break; // if 1 number differs, they are not equal
                }
            }
            if(equal)
            {
                duplicates[i]=j;
                break; // we need to find only the first duplicate
            }
        }
    }

#if 0 // reduce verbosity
    PrintVectorCout(duplicates); cout << endl;
#endif
    return(duplicates);
}
VectorSet
AlphaVectorPlanning::Prune(const VectorSet &V) const
{
    int nrInV=V.size1(),nrS=V.size2();
    bool dominated,valuesDominated;
    vector<bool> dominatedVectors(nrInV,false);
    int it,it1;
    vector<int> vectorsToKeep;

#if DEBUG_AlphaVectorPlanning_Prune
    cout << "AlphaVectorPlanning::Prune " << nrInV << " vectors" << endl;
#endif

    for(it=0;it!=nrInV;it++)
    {
        it1=0;
        dominated=false;
        // check whether "it" is dominated by any it1
        while(!dominatedVectors[it] && it1!=nrInV && !dominated)
        {
            valuesDominated=true;
            for(int s=0;s!=nrS;s++)
                if(V(it,s) > V(it1,s))
                    valuesDominated=false;

            if(valuesDominated && it1!=it)
                dominated=true;
            else
                dominated=false;

            it1++;
        }
        if(!dominated)
        {
            vectorsToKeep.push_back(it);
#if DEBUG_AlphaVectorPlanning_Prune
            cout << "AlphaVectorPlanning::Prune added vector " << it << endl;
#endif
        }
        else
            dominatedVectors[it]=true;

    }

    int newNrInV=vectorsToKeep.size();
    VectorSet V1(newNrInV,nrS);
    for(int i=0;i!=newNrInV;i++)
        for(int s=0;s!=nrS;s++)
            V1(i,s)=V(vectorsToKeep[i],s);

#if DEBUG_AlphaVectorPlanning_Prune
    cout << "AlphaVectorPlanning::Prune reduced " << nrInV << " to " 
         << newNrInV << endl;
#endif

    return(V1);
}
/**
 * Implements equation (3.11) of PhD thesis Matthijs.
 */
GaoVectorSet AlphaVectorPlanning::BackProjectFull(const VectorSet &v) const
{
    unsigned int nrA=GetPU()->GetNrJointActions(),
        nrO=GetPU()->GetNrJointObservations(),
        nrS=GetPU()->GetNrStates(),
        nrInV=v.size1();

    if(nrInV==0)
        throw(E("AlphaVectorPlanning::BackProjectFull attempting to backproject empty value function"));

#if DEBUG_AlphaVectorPlanning_BackProject
    tms timeStruct;
    clock_t ticks_before, ticks_after;
    ticks_before = times(&timeStruct);
#endif

    StartTimer("BackProjectFull");
    
    GaoVectorSet G(boost::extents[nrA][nrO]);
    VectorSet v1(nrInV,nrS);
#if AlphaVectorPlanning_UseUBLASinBackProject
    VectorSet vv=v;
#endif
    double x;

#if AlphaVectorPlanning_CheckForDuplicates
    vector<int> duplicates=GetDuplicateIndices(v);
#else
    vector<int> duplicates(nrInV,-1);
#endif
    int dup;

    using namespace boost::numeric::ublas;

    for(unsigned int a=0;a!=nrA;a++)
        for(unsigned int o=0;o!=nrO;o++)
        {
#if AlphaVectorPlanning_UseUBLASinBackProject
            matrix_column<const ObservationModelMapping::Matrix> mO(*_m_O[a],o);
#endif
            for(unsigned int k=0;k!=nrInV;k++)
            {
                if(duplicates[k]==-1)
                {
#if AlphaVectorPlanning_UseUBLASinBackProject
                    const matrix_row<VectorSet> mV(vv,k);
#endif
                    for(unsigned int s=0;s!=nrS;s++)
                    {
#if AlphaVectorPlanning_UseUBLASinBackProject
                        matrix_row<const TransitionModelMapping::Matrix>
                            mT(*_m_T[a],s);
                        x=inner_prod(element_prod(mT,mO),mV);
#if AlphaVectorPlanning_VerifyUBLASinBackProject
                        double x1=0;
                        for(unsigned int s1=0;s1!=nrS;s1++)
                            x1+=(*_m_O[a])(s1,o)*(*_m_T[a])(s,s1)*v(k,s1);
                        if(abs(x-x1)>1e-14)
                        {
                            cerr << x << " " << x1 << " " << x-x1 << endl;
                            abort();
                        }
#endif
#else // AlphaVectorPlanning_UseUBLASinBackProject
                        x=0;
                        for(unsigned int s1=0;s1!=nrS;s1++)
                            x+=(*_m_O[a])(s1,o)*(*_m_T[a])(s,s1)*v(k,s1);
#endif
                        v1(k,s)=x;
                    }
                }
                else
                {
                    dup=duplicates[k];
                    for(unsigned int s=0;s!=nrS;s++)
                        v1(k,s)=v1(dup,s);
                }
            }
            G[a][o]=new VectorSet(v1);
        }

    StopTimer("BackProjectFull");

#if DEBUG_AlphaVectorPlanning_BackProjectFullPrintout
    cout << "BackProjectFull of:" << endl;
    for(unsigned int k=0;k!=nrInV;k++)
    {
        for(unsigned int s=0;s!=nrS;s++)
            cout << v(k,s) << " ";
        cout << endl;
    }

    VectorSet *VS;
    for(unsigned int a=0;a!=nrA;a++)
        for(unsigned int o=0;o!=nrO;o++)
        {
            cout << "Gao a " << a << " o " << o << endl;

            VS=G[a][o];
            for(unsigned int k=0;k!=VS->size1();k++)
            {
                for(unsigned int s=0;s!=VS->size2();s++)
                    cout << (*VS)(k,s) << " ";
                cout << endl;
            }
        }
#endif

#if DEBUG_AlphaVectorPlanning_BackProjectFullSanityCheck
    double maxInV=-DBL_MAX;
    for(unsigned int k=0;k!=nrInV;k++)
        for(unsigned int s=0;s!=nrS;s++)
            maxInV=max(maxInV,v(k,s));

    double maxInGao=-DBL_MAX;
    for(unsigned int a=0;a!=nrA;a++)
        for(unsigned int o=0;o!=nrO;o++)
            for(unsigned int k=0;k!=nrInV;k++)
                for(unsigned int s=0;s!=nrS;s++)
                    maxInGao=max(maxInGao,(*G[a][o])(k,s));

    if(maxInGao>maxInV)
    {
        cout << "Max value in V is " << maxInV << ", max in Gao is "
             << maxInGao << endl;
        abort();
    }
#endif

#if DEBUG_AlphaVectorPlanning_BackProject
    ticks_after = times(&timeStruct);
    cout << "AlphaVectorPlanning::BackProject done in " 
         << ticks_after - ticks_before << " clock ticks, "
         << static_cast<double>((ticks_after - ticks_before))
        / sysconf(_SC_CLK_TCK) 
         << "s" << endl;
    // test results on 22-12-2006
    // using dense Matrix is about 2 faster than calling the Get*Prob()
    // using sparse Matrix is almost 3 times slower then dense Matrix
#endif
    return(G);
}