void CLWSamplingInfEngine::
EnterEvidenceProbability( floatVecVector *pEvidenceProbIn )
{
PNL_CHECK_IS_NULL_POINTER(pEvidenceProbIn);
int i, j;
float w;
int iSamples = pEvidenceProbIn->size();
int iSampleSize = m_currentEvVec.size();
if(iSamples != iSampleSize) return;
for( i = 0; i < iSamples; i++)
{
w = 1;
for( j = 0; j < pEvidenceProbIn[i].size(); j++)
{
w = w * (*pEvidenceProbIn)[i][j];
}
m_particleWeight[i] = w;
}
NormalizeWeight();
}
void CLWSamplingInfEngine::
EnterEvidence( const CEvidence *pEvidenceIn , int maximize , int sumOnMixtureNode )
{
PNL_CHECK_IS_NULL_POINTER(pEvidenceIn);
LWSampling(pEvidenceIn);
// Given evidencs, calculate particle weight by CPD
float w;
int iSampleSize = m_currentEvVec.size();
const int* ObsNodes = pEvidenceIn->GetAllObsNodes();
int NumberObsNodes = pEvidenceIn->GetNumberObsNodes();
int i, j;
for( i = 0; i < iSampleSize; i++)
{
w = 0;
for( j = 0; j < NumberObsNodes; j++)
{
if(pEvidenceIn->IsNodeObserved(ObsNodes[j]))
{
CFactor* pFactor = m_pGraphicalModel->GetFactor(ObsNodes[j]);
w = w + pFactor->GetLogLik( m_currentEvVec[i]);
}
}
m_particleWeight[i] = (float)exp(w);
}
NormalizeWeight();
}
float CLWSamplingInfEngine::GetNeff()
{
if( m_bNormalized == false) NormalizeWeight();
int i;
float totalWeight2 = 0;
for(i = 0; i < m_particleCount; i++)
{
totalWeight2 += m_particleWeight[i]*m_particleWeight[i];
}
if(totalWeight2 <= 0) return -1;
return 1/totalWeight2;
}
/*
* This is the most important function in localization. go through the poles in pole list
* and calculate weight and make decisions...
*/
void Algorithm::ParticleFilter(list<PoleData> & poleList) //i have array of single poles
{
/*
* if there is only one or two poles we can't trust that. so wait for more than 1.
* but if the oldest pole is very old, make an iteration.
*/
if (/*poleList.front().time-(*m_currTimePtr) > 50000 && */poleList.size()<=2) return;
//reset counter
m_stepsWithoutPolePicture=0;
#ifdef LOCALIZATION_DEBUG
printf("Algorithm::ParticleFilter(list<PoleData> & poleList):\n");
printf("have %d poles to go through\n",poleList.size());
#endif
//this was a try to make a low pass filter to throw poles with noise. not tested.
//TODO try to do this. not so important.
// bool DoneLPF = poleList.size() < 3 ? true:false; // lpf only for 3 or more poles;
// PoleData temp[3];
// int i;
// list<PoleData>::iterator it;
// list<PoleData>::iterator itErase;
double w;
while (poleList.size()>0)
{
//LPF stuff
/*
// cout<<"START 1 "<<poleList.size()<<endl;
// it = poleList.begin();
// PoleId ID = it->id;
// i=0;
// while((!DoneLPF) && it!= poleList.end()) //LPF
// {
// cout<<"START 2 "<<poleList.size()<<"\ti= "<<i<<endl;
// if(it->id == ID)
// {
// if(it->distFromMe < 0)
// {
// DoneLPF = true;
// continue;
// }
// else
// {
// temp[i] = *it;
// if(i == 1)
// itErase = it;// the pole to erase
// it++;
// i++;
// }
// }
// if(i == 3)
// {
// cout<<"IF 2 "<<poleList.size()<<"\ti= "<<i<<endl;
// DoneLPF = true;
// if((fabs(temp[0].distFromMe - temp[2].distFromMe) < fabs(temp[0].distFromMe - temp[1].distFromMe )) && (temp[2].time - temp[0].time < 1000000))
// {
// poleList.erase(itErase);
// }
// }
// }
*/
PoleData pole=poleList.front();
poleList.pop_front();
#ifdef LOCALIZATION_DEBUG
cout<<"\t#poles: "<<poleList.size()<<" \t";
cout<<"Dist: "<<pole.distFromMe<<"\tID: "<<pole.id<<endl;
//if(DoneLPF && i==3)
// cout<<"Dist: "<<temp[1].distFromMe<<"\tID: "<<temp[1].id<<"\tTHROWED"<<endl;
#endif
//go through each particle and calculate the wieght
for (State_iterator it = m_currState.begin(); it != m_currState.end() ;it++)//update weight
{
/*
* TODO: OMG found a bug!!
* what happens when the pole.id==GOAL_UNKNOWN_BACK ???!!!
* TODO FIXME
*/
if (pole.id==GOAL_UNKNOWN_FRONT) // either left or right front pole, give maximum weight.
{
double w0,w1;
pole.id=GOAL_LEFT_FRONT;
w0=getWeightFromPole(pole,*it);
pole.id=GOAL_RIGHT_FRONT;
w1=getWeightFromPole(pole,*it);
pole.id=GOAL_UNKNOWN_FRONT;
if (w0>w1) w=w0; else w=w1;
}
else //We know what pole it is.
{
w=getWeightFromPole(pole,*it);
}
//give new weight for the particle, while considering old weight.
(it->weight)*=w;
}
}
double sumOfWeight=0;
for (State_iterator it = m_currState.begin(); it != m_currState.end() ;it++) {
sumOfWeight+=it->weight;
}
sumOfWeight = 1 / sumOfWeight;
//if all the sum of weight is very low, it means all the particles are out
if (sumOfWeight<0.001)
m_outOfLocation=true;
else
m_outOfLocation=false;
//TODO use the sumOfWeight we calculated before and send to this function
//normalize weight for the monte carlo
NormalizeWeight(sumOfWeight); //sum of weights = 1
//choose new set
UpdateStates();
}
//This function will eventually replace the above function to handle
// POIs instead of poles
//TODO compare this function to the previous and see if we can upgrade some of the things
// they were unable to do last year
void Algorithm::ParticleFilter(list<PoiData>& poiList)
{
/*
* if there is only one or two poles we can't trust that. so wait for more than 1.
* but if the oldest pole is very old, make an iteration.
*/
//if (/*poleList.front().time-(*m_currTimePtr) > 50000 && */poleList.size()<=2) return;
//TODO verify this line!!
if(/**m_currTimePtr - poiList.front().time < 50000 ||*/ poiList.size() <= 2)
return;
//reset counter
m_stepsWithoutPolePicture=0;
#ifdef LOCALIZATION_DEBUG
cout << "[Algorithm::ParticleFilter]: have " << poiList.size() << " POIs to go through" << endl;
#endif
//TODO attempt to create a LPF for the POIs
double w, maxW;
while (poiList.size()>0)
{
PoiData poi = poiList.front();
poiList.pop_front();
#ifdef LOCALIZATION_DEBUG
cout<<"\t#POIs: "<<poiList.size()<<" \t";
cout<<"Dist: "<<poi.distFromMe<<"\tID: "<<poi.id<<endl;
#endif
//go through each particle and calculate the weight
for (State_iterator it = m_currState.begin(); it != m_currState.end() ;it++)//update weight
{
if (poi.id==UNKNOWN)
{
w = 0;
maxW = 0;
for(int i=0;i<NUMBER_OF_POIS;i++)
{
poi.id = (PoiId)i;
w = GetWeightFromPoi(poi,*it);
maxW = w > maxW ? w : maxW;
}
w = maxW;
poi.id = UNKNOWN;
}
else //We know what poi it is.
{
w = GetWeightFromPoi(poi,*it);
}
//give new weight for the particle, while considering old weight.
(it->weight)*=w;
}
}
double sumOfWeight=0;
for (State_iterator it = m_currState.begin(); it != m_currState.end() ;it++) {
sumOfWeight += it->weight;
}
//if all the sum of weight is very low, it means all the particles are out
if ((sumOfWeight/m_currState.size()) < 0.001)
m_outOfLocation=true;
else
m_outOfLocation=false;
NormalizeWeight(sumOfWeight);
//choose new set
UpdateStates();
}
void CLWSamplingInfEngine::
MarginalNodes( const int *queryIn, int querySz, int notExpandJPD)
{
PNL_CHECK_IS_NULL_POINTER(queryIn);
if( m_bNormalized == false ) NormalizeWeight();
int i, j, k;
int offset;
int nsamples = m_particleCount;
const CBNet *pBNet = static_cast<const CBNet *>( m_pGraphicalModel );
int type = 0;
int totalnodesizes = 0;
intVector nodesize(querySz);
intVector mulnodesize(querySz, 1);
for( i = querySz; --i >=0; )
{
const CNodeType* pNodeType = pBNet->GetNodeType(queryIn[i]);
nodesize[i] = pNodeType->GetNodeSize();
if(i == querySz-1)
mulnodesize[i] = 1;
else
mulnodesize[i] *= nodesize[i+1];
if(pNodeType->IsDiscrete())
{
type++;
if(totalnodesizes == 0)
totalnodesizes = nodesize[i];
else
totalnodesizes = totalnodesizes * nodesize[i];
}
else
{
totalnodesizes = totalnodesizes + nodesize[i];
}
}
if(type == querySz)
{
//all query nodes are discrete
float *tab = new float[totalnodesizes];
for(i = 0; i < totalnodesizes; i++) tab[i] = 0;
for( i = 0; i < nsamples; i++)
{
CEvidence* pEvidence = m_currentEvVec[i];
int index = 0;
for(j = 0; j < querySz; j++)
{
Value* pValue = pEvidence->GetValue(queryIn[j]);
index += pValue->GetInt() * mulnodesize[j];
}
tab[index] += m_particleWeight[i];
}
m_pQueryJPD = CTabularPotential::Create(queryIn, querySz, pBNet->GetModelDomain () );
m_pQueryJPD->AllocMatrix( tab, matTable );
delete []tab;
}
else if(type == 0)
{
//all query nodes are gaussian
float* val = new float[totalnodesizes];
float* mean = new float[totalnodesizes];
float* cov = new float[totalnodesizes * totalnodesizes];
for( i = 0; i < totalnodesizes; i++) mean[i] = 0;
for( i = 0; i < totalnodesizes * totalnodesizes; i++) cov[i] = 0;
// mean
for( i = 0; i < nsamples; i++)
{
CEvidence* pEvidence = m_currentEvVec[i];
for(j = 0, offset = 0; j < querySz; j++)
{
Value* pValue = pEvidence->GetValue(queryIn[j]);
for(k = 0; k < nodesize[j]; k++)
{
mean[offset] += m_particleWeight[i] * pValue[k].GetFlt();
offset++;
}
}
}
// covariance
for( i = 0; i < nsamples; i++)
{
CEvidence* pEvidence = m_currentEvVec[i];
for(j = 0, offset = 0; j < querySz; j++)
{
Value* pValue = pEvidence->GetValue(queryIn[j]);
for(k = 0; k < nodesize[j]; k++)
{
val[offset] = pValue[k].GetFlt();
offset++;
}
}
for(j = 0; j < totalnodesizes; j++)
{
for(k = j; k < totalnodesizes; k++)
{
cov[k*totalnodesizes+j] += ( m_particleWeight[i] * ( val[j]- mean[j]) * (val[k] - mean[k]) );
cov[j*totalnodesizes+k] = cov[k*totalnodesizes+j];
}
}
}
m_pQueryJPD = CGaussianPotential::Create( queryIn, querySz, pBNet->GetModelDomain () );
m_pQueryJPD->AllocMatrix( mean, matMean );
m_pQueryJPD->AllocMatrix( cov, matCovariance);
delete []val;
delete []mean;
delete []cov;
}
//Get MPE
delete m_pEvidenceMPE;
m_pEvidenceMPE = NULL;
m_pEvidenceMPE = m_pQueryJPD->GetMPE();
}
void CLWSamplingInfEngine::Estimate(CEvidence * pEstimate)
{
if( pEstimate == NULL ) return;
if( m_bNormalized == false ) NormalizeWeight();
int i, j, k, offset;
int iNodes = pEstimate->GetNumberObsNodes ();
const int* obsNodes = pEstimate->GetAllObsNodes();
const CNodeType *const* pNodeTypes = pEstimate->GetNodeTypes();
int totalNodeSize =0;
intVector nodeSize(iNodes);
for( j = 0, totalNodeSize = 0; j < iNodes; j++)
{
nodeSize[j] = pNodeTypes[j]->GetNodeSize();
totalNodeSize += nodeSize[j];
}
floatVector pFloatValue;
pFloatValue.resize(totalNodeSize, 0);
for( j = 0, offset = 0; j < iNodes; j++)
{
if( pNodeTypes[j]->IsDiscrete())
{
for( i = 0; i < m_particleCount; i++)
{
Value* pValue = (m_currentEvVec[i])->GetValue(obsNodes[j]);
// for discrete value type
pFloatValue[offset + pValue->GetInt()] = pFloatValue[offset + pValue->GetInt()] + m_particleWeight[i];
}
}
else
{
for( i = 0; i < m_particleCount; i++)
{
Value* pValue = (m_currentEvVec[i])->GetValue(obsNodes[j]);
// for continuous value type
for( k =0; k < nodeSize[j]; k++)
{
pFloatValue[offset + k] = pFloatValue[offset + k] + m_particleWeight[i] * (pValue+k)->GetFlt();
}
}
}
offset += nodeSize[j];
}
// set pFloatValue data to pEstimate
for( j = 0, offset = 0; j < iNodes; j++)
{
Value* pValue = pEstimate->GetValue(obsNodes[j]);
if( pNodeTypes[j]->IsDiscrete())
{
float maxweight = -99; int itmax = 0;
for( k =0; k < nodeSize[j]; k++)
{
if(maxweight < pFloatValue[offset + k])
{
maxweight = pFloatValue[offset + k];
itmax = k;
}
pValue->SetInt(itmax);
}
}
else
{
for( k =0; k < nodeSize[j]; k++)
(pValue+k)->SetFlt( pFloatValue[offset + k] );
}
offset += nodeSize[j];
}
}
