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);
}
VectorSet AlphaVectorPlanning::CrossSum(const VectorSet &A,
const VectorSet &B) const
{
int nrInA=A.size1(),
nrInB=B.size1(),
nrS=A.size1();
#if DEBUG_AlphaVectorPlanning_CrossSum
cout << "AlphaVectorPlanning::CrossSum of " << nrInA
<< " times " << nrInB << endl;
#endif
VectorSet C(nrInA*nrInB,nrS);
int k=-1;
for(int i=0;i!=nrInA;i++)
for(int j=0;j!=nrInB;j++)
{
k++;
for(int s=0;s!=nrS;s++)
C(k,s)=A(i,s)+B(j,s);
}
return(C);
}
/** 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);
}
int
BeliefValue::GetMaximizingVectorIndexAndValue(const BeliefInterface &b,
const VectorSet &v,
const vector<bool> &mask,
double &value)
{
//ugly, copy of JointBelief version..
int nrInV=v.size1();
int maximizingVectorI=INT_MAX;
bool maskValid=false;
if(static_cast<int>(mask.size())!=nrInV)
{
throw(E("BeliefValue::GetMaximizingVectorIndex: mask has incorrect size"));
}
for(int k=0;k!=nrInV;k++)
if(mask[k])
{
maskValid=true;
break;
}
// no vector was enabled in the mask, so there is no maximizing vector
if(!maskValid)
return(-1);
// compute value of b for every vector in VS which has its mask
// set to true
vector<double> Vb=b.InnerProduct(v,mask);
// find maximizing vector, ignores effects of duplicate values
value=-DBL_MAX;
for(int k=0;k!=nrInV;k++)
{
if(mask[k] && Vb[k]>value)
{
value=Vb[k];
maximizingVectorI=k;
}
}
if(value==-DBL_MAX)
{
throw(E("BeliefValue::GetMaximizingVectorIndex: no maximizing vector found"));
}
return(maximizingVectorI);
}
int
BeliefValue::GetMaximizingVectorIndex(const BeliefInterface &b,
const VectorSet &v)
{
int nrInV=v.size1();
int maximizingVectorI=0;
double maxVal;
// compute value of b for every vector in VS
vector<double> Vb=b.InnerProduct(v);
// find maximizing vector, ignores effects of duplicate values
maxVal=-DBL_MAX;
for(int k=0;k!=nrInV;k++)
{
if(Vb[k]>maxVal)
{
maxVal=Vb[k];
maximizingVectorI=k;
}
}
return(maximizingVectorI);
}
/**
* Implements equation (3.11) of PhD thesis Matthijs.
*/
GaoVectorSet AlphaVectorPlanning::BackProjectSparse(const VectorSet &v) const
{
unsigned int nrA=GetPU()->GetNrJointActions(),
nrO=GetPU()->GetNrJointObservations(),
nrS=GetPU()->GetNrStates(),
nrInV=v.size1();
if(nrInV==0)
throw(E("AlphaVectorPlanning::BackProjectSparse attempting to backproject empty value function"));
StartTimer("BackProjectSparse");
GaoVectorSet G(boost::extents[nrA][nrO]);
VectorSet v1(nrInV,nrS);
VectorSet vv=v;
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_UseFastSparseBackup
matrix_column<const ObservationModelMappingSparse::SparseMatrix>
mO(*_m_Os[a],o);
#endif
for(unsigned int k=0;k!=nrInV;k++)
{
if(duplicates[k]==-1)
{
const matrix_row<VectorSet> mV(vv,k);
for(unsigned int s=0;s!=nrS;s++)
{
#if AlphaVectorPlanning_UseFastSparseBackup
x=inner_prod(*_m_TsOsForBackup[a][s][o],mV);
#else
matrix_row<const TransitionModelMappingSparse::
SparseMatrix> mT(*_m_Ts[a],s);
x=inner_prod(element_prod(mT,mO),mV);
#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("BackProjectSparse");
return(G);
}
/**
* 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);
}
