MultiAgentDecisionProcess  Release 0.2.1
JointBeliefSparse.cpp
Go to the documentation of this file.
1 
28 #include "JointBeliefSparse.h"
31 #include "TGet.h"
32 #include <float.h>
33 
34 using namespace std;
35 
36 //Necessary as header file contains a forward declaration:
38 
39 #define JointBeliefSparse_doSanityCheckAfterEveryUpdate 0
40 
42 {
43 }
44 
46  BeliefSparse(size)
47 {
48 }
49 
50 JointBeliefSparse::JointBeliefSparse(const vector<double> &belief) :
51  BeliefSparse(belief)
52 {
53 }
54 
56  BeliefSparse(belief)
57 {
58 }
59 
61  BeliefSparse(belief)
62 {
63 }
64 //Destructor
66 {
67 }
68 
71 {
72  if (this == &o) return *this; // Gracefully handle self assignment
73  // Put the normal assignment duties here...
75  return *this;
76 }
77 
80 {
81  if (this == &o) return *this; // Gracefully handle self assignment
82  const JointBeliefSparse& casted_o =
83  dynamic_cast<const JointBeliefSparse&>(o);
84  return(operator=(casted_o));// call the operator= for JointBeliefSparse
85 }
86 
88  Index lastJAI, Index newJOI)
89 {
90  //const TransitionModelDiscrete* T=pu.GetTransitionModelDiscretePtr();
92 
93  //pointer to the transition probability Get funtion:
94  TGet* T = pu.GetTGet();
95  if(T==0)
96  {
97  return(UpdateSlow(pu,lastJAI,newJOI));
98  }
99 
100  double Po_ba = 0.0; // P(o|b,a) with o=newJO
101  double Ps_ba, Po_as, Pso_ba;
102  size_t nrS = _m_b.size();
103  BS newJB_unnorm(nrS);
104  for(Index sI=0; sI < nrS; sI++)
105  {
106  //P(sI | b, a) = sum_(prec_s) P(sI | prec_s, a)*JB(prec_s)
107  Ps_ba = 0.0;
108 
109  for(BScit it=_m_b.begin(); it!=_m_b.end(); ++it)
110  Ps_ba += T->Get(it.index(), lastJAI, sI) * *it;
111 
112  if(Ps_ba>0) // if it is zero, Pso_ba will be zero anyway
113  {
114  //P(newJOI | lastJAI, sI) :
115  Po_as = O->Get(lastJAI, sI, newJOI);
116 
117  //the new (unormalized) belief P(s,o|b,a)
118  Pso_ba = Po_as * Ps_ba;
119 
120  if(Pso_ba>PROB_PRECISION) // we don't want to store very
121  // small probabilities in a
122  // sparse representation
123  {
124  newJB_unnorm[sI]=Pso_ba; //unnormalized new belief
125  Po_ba += Pso_ba; //running sum of P(o|b,a)
126  }
127  }
128  }
129 
130  //normalize:
131  if(Po_ba>0)
132  for(BSit it=newJB_unnorm.begin(); it!=newJB_unnorm.end(); ++it)
133  *it/=Po_ba;
134 
135  _m_b=newJB_unnorm;
136 
137 #if JointBeliefSparse_doSanityCheckAfterEveryUpdate
138  if(!SanityCheck())
139  throw(E("JointBeliefSparse::Update SanityCheck failed"));
140 #endif
141 
142  return(Po_ba);
143 }
144 
147  Index lastJAI, Index newJOI)
148 {
149  double Po_ba = 0.0; // P(o|b,a) with o=newJO
150  double Ps_ba, Po_as, Pso_ba;
151  size_t nrS = _m_b.size();
152  BS newJB_unnorm(nrS);
153  for(Index sI=0; sI < nrS; sI++)
154  {
155  //P(sI | b, a) = sum_(prec_s) P(sI | prec_s, a)*JB(prec_s)
156  Ps_ba = 0.0;
157 
158  for(BScit it=_m_b.begin(); it!=_m_b.end(); ++it)
159  Ps_ba += pu.GetTransitionProbability(it.index(),
160  lastJAI, sI) *
161  *it;
162 
163  if(Ps_ba>0) // if it is zero, Pso_ba will be zero anyway
164  {
165  //P(newJOI | lastJAI, sI) :
166  Po_as = pu.GetObservationProbability(lastJAI, sI, newJOI);
167 
168  //the new (unormalized) belief P(s,o|b,a)
169  Pso_ba = Po_as * Ps_ba;
170 
171  if(Pso_ba>PROB_PRECISION) // we don't want to store very
172  // small probabilities in a
173  // sparse representation
174  {
175  newJB_unnorm[sI]=Pso_ba; //unnormalized new belief
176  Po_ba += Pso_ba; //running sum of P(o|b,a)
177  }
178  }
179  }
180 
181  //normalize:
182  if(Po_ba>0)
183  for(BSit it=newJB_unnorm.begin(); it!=newJB_unnorm.end(); ++it)
184  *it/=Po_ba;
185 
186  _m_b=newJB_unnorm;
187 
188 #if JointBeliefSparse_doSanityCheckAfterEveryUpdate
189  if(!SanityCheck())
190  throw(E("JointBeliefSparse::UpdateSlow SanityCheck failed"));
191 #endif
192 
193  return(Po_ba);
194 }