MatrixXd MinEnFilter::DAk(const MatrixXd& Gk, const MatrixXd& Disps_inhom, int direction) {
MatrixXd Ones = MatrixXd::Ones(1,4);
MatrixXd Eaux = MatrixXd::Identity(6,6);
Matrix4d eta = MEFcommon::matSE(Eaux.col(direction-1));
MatrixXd iEg = (currG.E.lu().solve(Gk.transpose())).transpose();
VectorXd kappa = iEg.col(2);
MatrixXd h = iEg.leftCols(2).cwiseQuotient(kappa*MatrixXd::Ones(1,2));
MatrixXd z = Disps_inhom.leftCols(2) - h;
VectorXd kappaM1 = kappa.cwiseInverse();
VectorXd kappaM2 = (kappa.cwiseProduct(kappa)).cwiseInverse();
VectorXd kappaM3 = (kappa.cwiseProduct(kappa.cwiseProduct(kappa))).cwiseInverse();
MatrixXd lambda = ((currG.E.lu().solve(eta))*(iEg.transpose())).transpose();
VectorXd xi = (lambda.col(2)).cwiseProduct(kappaM2);
// first part of Hessian
VectorXd zeta1 = -2* kappaM3.cwiseProduct((lambda.col(2)).cwiseProduct(iEg.col(0)));
VectorXd zeta2 = -2 * kappaM3.cwiseProduct((lambda.col(2)).cwiseProduct(iEg.col(1)));
VectorXd eta1 = kappaM2.cwiseProduct(lambda.col(0));
VectorXd eta2 = kappaM2.cwiseProduct(lambda.col(1));
VectorXd a31 = zeta1 + eta1;
VectorXd a32 = zeta2 + eta2;
MatrixXd b1row = q1/nPoints * ((z.col(0))*Ones).cwiseProduct(iEg);
MatrixXd b2row = q2/nPoints * ((z.col(1))*Ones).cwiseProduct(iEg);
MatrixXd c1row = ((xi * Ones).cwiseProduct(b1row)).colwise().sum();
MatrixXd c2row = ((xi * Ones).cwiseProduct(b2row)).colwise().sum();
MatrixXd aux1(nPoints,4);
aux1 << (a31.cwiseProduct(b1row.col(0)) + a32.cwiseProduct(b2row.col(0))) , (a31.cwiseProduct(b1row.col(1)) + a32.cwiseProduct(b2row.col(1))), (a31.cwiseProduct(b1row.col(2)) + a32.cwiseProduct(b2row.col(2))), (a31.cwiseProduct(b1row.col(3)) + a32.cwiseProduct(b2row.col(3)));
MatrixXd c3row = aux1.colwise().sum();
MatrixXd C(4,4);
C << c1row,
c2row,
c3row,
MatrixXd::Zero(1,4);
// % second part of Hessian
VectorXd rho1 = -q1/nPoints * kappaM2.cwiseProduct(iEg.col(0));
VectorXd rho2 = -q2/nPoints * kappaM2.cwiseProduct(iEg.col(1));
MatrixXd Frow1 = kappaM1.cwiseProduct(lambda.col(0)) - kappaM2.cwiseProduct((lambda.col(2)).cwiseProduct(iEg.col(0)));
MatrixXd Frow2 = kappaM1.cwiseProduct(lambda.col(1)) - kappaM2.cwiseProduct((lambda.col(2)).cwiseProduct(iEg.col(1)));
MatrixXd G1row1 = (Frow1*Ones).cwiseProduct(iEg);
MatrixXd G1row2 = (Frow2*Ones).cwiseProduct(iEg);
MatrixXd G2row1 = ((z.col(0))*Ones).cwiseProduct(lambda);
MatrixXd G2row2 = ((z.col(1))*Ones).cwiseProduct(lambda);
MatrixXd Grow1 = G1row1 - G2row1;
MatrixXd Grow2 = G1row2 - G2row2;
MatrixXd h1row = (q1/nPoints * (kappaM1*Ones).cwiseProduct(Grow1)).colwise().sum();
MatrixXd h2row = (q2/nPoints * (kappaM1*Ones).cwiseProduct(Grow2)).colwise().sum();
MatrixXd aux2(nPoints,4);
aux2 << rho1.cwiseProduct(Grow1.col(0)) + rho2.cwiseProduct(Grow2.col(0)), rho1.cwiseProduct(Grow1.col(1)) + rho2.cwiseProduct(Grow2.col(1)), rho1.cwiseProduct(Grow1.col(2)) + rho2.cwiseProduct(Grow2.col(2)), rho1.cwiseProduct(Grow1.col(3)) + rho2.cwiseProduct(Grow2.col(3));
MatrixXd h3row = aux2.colwise().sum();
MatrixXd H(4,4);
H << h1row,
h2row,
h3row,
MatrixXd::Zero(1,4);
return C+H;
}
inline double EvaluateLogMvn_Cholesky(VectorXd& Z_vec, VectorXd& C_vec, VectorXd& Lam_vec, MatrixXd& upper_chol){
VectorXd elm_wise = C_vec.cwiseProduct(Lam_vec);
VectorXd mu = upper_chol*elm_wise;
VectorXd Z_mu = Z_vec-mu;
double prob = (-.5)*(Z_mu.dot(Z_mu));
return(prob);
}
// Obtains standard Chebyshev polynomials evaluated at given set of Points;
void H2_2D_Tree::get_Standard_Chebyshev_Polynomials(const unsigned short nChebPoly, const unsigned long N, const VectorXd& x, MatrixXd& T){
T = MatrixXd::Zero(N,nChebPoly);
T.col(0)= VectorXd::Ones(N);
if(nChebPoly>1){
T.col(1) = x;
for(unsigned short k=2; k<nChebPoly; ++k){
T.col(k)= 2.0*x.cwiseProduct(T.col(k-1))-T.col(k-2);
}
}
}
inline double EvaluateLogMvn(VectorXd& Z_vec, VectorXd C_vec, VectorXd& Lam_vec, MatrixXd& Inv_Sig, MatrixXd& Sig){
VectorXd elm_wise = C_vec.cwiseProduct(Lam_vec);
VectorXd mu = Sig*elm_wise;
VectorXd Z_mu = Z_vec-mu;
VectorXd RHSeval = Inv_Sig*Z_mu;
double prob = (-.5)*(Z_mu.dot(RHSeval));
return(prob);
}
LieAlgebra MinEnFilter::computeGradient(const LieGroup& S, const MatrixXd& Gk, const MatrixXd& Disps_inhom) {
/* DO NOT CHANGE THIS FUNCTION */
LieAlgebra L;
MatrixXd iEg = ((S.E).lu().solve(Gk.transpose())).transpose();
VectorXd kappa = iEg.col(2);
MatrixXd h = iEg.leftCols(2).cwiseQuotient(kappa*MatrixXd::Ones(1,2));
MatrixXd z = Disps_inhom.leftCols(2) - h;
MatrixXd Qvec(1,2);
Qvec << q1/nPoints,q2/nPoints;
MatrixXd qvec = MatrixXd::Ones(nPoints,1) * Qvec;
MatrixXd b12 = (kappa.cwiseInverse()*MatrixXd::Ones(1,2)).cwiseProduct(qvec.cwiseProduct(z));
MatrixXd kappa2 = kappa.cwiseProduct(kappa);
MatrixXd aux1 = (iEg.leftCols(2)).cwiseProduct(z.cwiseProduct(qvec));
MatrixXd b3 = -((kappa2.cwiseInverse()*MatrixXd::Ones(1,2)).cwiseProduct(aux1)).rowwise().sum();
MatrixXd B(nPoints,4);
B << b12, b3, MatrixXd::Zero(nPoints,1);
MatrixXd Ones = MatrixXd::Ones(1,4);
MatrixXd A1 = iEg.col(0) * Ones;
MatrixXd A2 = iEg.col(1) * Ones;
MatrixXd A3 = iEg.col(2) * Ones;
MatrixXd A4 = iEg.col(3) * Ones;
MatrixXd G1 = (B.cwiseProduct(A1)).colwise().sum();
MatrixXd G2 = (B.cwiseProduct(A2)).colwise().sum();
MatrixXd G3 = (B.cwiseProduct(A3)).colwise().sum();
MatrixXd G4 = (B.cwiseProduct(A4)).colwise().sum();
Matrix4d G;
G << G1,
G2,
G3,
G4;
G = MEFcommon::prSE(G.transpose());
if (order == 1) {
L = LieAlgebra(G);
} else if (order >=2) {
L = LieAlgebra(G, VectorXd::Zero((order-1)*6),order);
} else {
cerr << "Parameter order must be greater than zero." << endl;
exit(1);
}
return L;
}
double CGppe::maximum_expected_improvement(const VectorXd & theta_t, const VectorXd& theta_x, const double& sigma,
const MatrixXd& t, const MatrixXd & x, const VectorXd& idx_global, const VectorXd& ind_t, const VectorXd& ind_x, MatrixXd tstar, int N, double fbest)
{
VectorXd idx_xstar=Nfirst(N);
int Kt_ss = 1;
double mei;
MatrixXd Kx_star, Kx_star_star, kstar, Kss, Css;
MatrixXd Kt_star = covfunc_t->Compute(t, tstar);
//dsp(GetKinv(),"Kinv");
Kx_star = GetMatRow(Kx, idx_xstar.transpose()); //maybe need some transpose?
Kx_star_star = GetMat(Kx, idx_xstar.transpose(), idx_xstar.transpose()); // test to test
kstar = Kron(Kt_star, Kx_star);
kstar = GetMatRow(kstar, idx_global);
Kss = Kt_ss * Kx_star_star;
mustar = kstar.transpose() * Kinv * GetVec(f, idx_global);
Css = Kss - kstar.transpose() * W * llt.solve(Kinv * kstar);
varstar = Css.diagonal();
VectorXd sigmastar = sqrt(varstar.array());
VectorXd z = (fbest - mustar.array()) / sigmastar.array();
VectorXd pdfval = normpdf(z);
VectorXd cdfval = normcdf(z);
VectorXd inter = z.array() * (1 - cdfval.array());
VectorXd el = sigmastar.cwiseProduct(inter - pdfval);
el=-1*el;
mei = el.maxCoeff();
//dsp(mei,"mei");
return mei;
}
/*@Input:
- cableScanner, the Pointers of CableScanner
- mps, the Pointers of MPS
- Map, the [ynum]x[znum] map data will be return through the memory of this pointer.
should be pre-malloc memory.
- yPtr zPtr, will be returned including the estimated y, z distance.
should be pre-malloc memory.
- freq, the memory for returning frequency domain data.
- current, the value of current
- [ymin, ymax]x[zmin, zmax], define the area to be scanned.
- ynum znum, the sample num of area
*/
bool fnCableScan(
CableScanner *cableScanner, MPS *mps,
double *Map, double *yPtr, double *zPtr, double *freq, double *measuedVolt,
double *params)
//double current, double ymin, double ymax, double zmin, double zmax, int ynum, int znum)
{
double current = params[0];
double ymin = params[1];
double ymax = params[2];
double zmin = params[3];
double zmax = params[4];
int ynum = (int)params[5];
int znum = (int)params[6];
//cout << ymin << ymax << zmin << zmax << "ynum " << ynum << ", znum " << znum << endl;
if (!Map || !yPtr || !zPtr || !freq || !mps->data)
{
cout << "NULL pointers" << endl;
#ifdef _TEST_MSG_
if (!Map)
cout << "Map is NULL" << endl;
if (!yPtr)
cout << "yPtr is NULL" << endl;
if (!zPtr)
cout << "zPtr is NULL" << endl;
if (!freq)
cout << "freq is NULL" << endl;
if (!mps->data)
cout << "mps->data is NULL" << endl;
#endif
exit(-1);
}
VectorXd e0 = VectorXd::Zero(cableScanner->n);
/* ------ data getter and digital filter ------*/
#ifdef _TEST_MSG_
cout << "getdata..." << endl;
#endif
double *signals = mps->data;//mps->GetData(false); // FIXME:内存不应该这么分配
#ifdef _TEST_MSG_
cout << "getdata...ok" << endl;
#endif
int N = mps->samples*mps->bulks;
double *sig = signals;
//double *freq;
if (freq == NULL)
//freq = new double[N*cableScanner->n]; // FIXME
{
cout << "the freq pointer has not been initialized, you may call 'fnFreqPtrMalloc' firstly. exit..." << endl;
return false;
}
#ifdef _TEST_MSG_
cout << "init filter..." << endl;
#endif
FFTWFilter filter(N);
#ifdef _TEST_MSG_
cout << "init filter...ok" << endl;
#endif
int minFreq = 30;
int maxFreq = 70;
double time = N / (double)mps->sampleRate;
double maxvalue;
int win_min = (int)(0.1*N);
int win_max = (int)(0.9*N);
// Here, we use the cableScanner->n but not the mps->channels, since the
// former may be smaller than the latter
for (int i = 0; i < cableScanner->n; i++)
{
#ifdef _TEST_MSG_
cout << "BandpassFilt..." << i << endl;
#endif
filter.BandpassFilt(sig, freq, minFreq, maxFreq, time);
maxvalue = 0;
for (int j = win_min; j < win_max; j++)
{
if (sig[j] > maxvalue)
maxvalue = sig[j];
}
e0(i) = maxvalue>0.09?maxvalue:0;
measuedVolt[i] = e0(i)/AMP_SCALE;
sig += N;
freq += N;
}
cout << "measured volt" << (int)measuedVolt << endl;
e0 /= AMP_SCALE;
cout << "measured voltages: " << e0.adjoint() << endl;
#ifdef _WRITE_DATA_TO_FILE
ofstream outfile;
outfile.open("data/CableScnnerDLL.txt", ios::app);
outfile << e0.adjoint() << " ";
#endif
/* ------ data getter and digital filter end ------*/
/* ------- scan probmap --------- */
Vector3d pos(0, 0, -0.2);
Vector3d cur(current, 0, 0);
VectorXd cK = VectorXd::Zero(cableScanner->n);
cK << 0.7783,
41.6144,
36.5788,
28.0842,
43.4910,
36.4128,
38.9824,
1.0000; // this should be updated by new train result.
clock_t t1 = clock();
MatrixXd MXd = cableScanner->scan(pos, cur, cK.cwiseProduct(e0), ymin, ymax, zmin, zmax, ynum, znum);
cout << "cable scan used time: " << (clock() - t1) << " ms" << endl;
cout << "cable scan area : " << "[" << ymin << ", " << ymax << "]x[" << zmin << ", " << zmax << "]" << endl;
double *mapPtr = Map;
int i, j;
for (/*int */i = 0; i < ynum; i++)
{
for (/*int */j = 0; j < znum; j++)
{
mapPtr[j] = MXd(i,j);
//cout << "mapPtr " << mapPtr[j] << endl; // TEST
}
mapPtr += znum;
}
*yPtr = cableScanner->maxy;
*zPtr = cableScanner->maxz;
cout << "cable position: (" << *yPtr << ", " << *zPtr << ")" << "@" << cableScanner->maxprob << endl;
#ifdef _WRITE_DATA_TO_FILE
outfile << *yPtr << " " << *zPtr << endl;
outfile.close();
#endif
#ifdef _TEST_MSG_
//cout << "i: " << i << endl;
//cout << "j: " << j << endl;
//cout << "*yPtr = " << *yPtr << endl;
//cout << "*zPtr = " << *zPtr << endl;
cout << "return from cable scanner dll" << endl;
#endif // _TEST_MSG_
if (e0(0) > 1e-4)
{
cout << "WARNING: the car may not in correct direction, for coil 1 get value at " << e0(0) << endl;
}
// Check if the signals is large enough for finding an cable
for (i = 0; i < cableScanner->n; i++)
{
if (e0(i) > 2e-4)
{
cout << "** Found Cable!! **" << endl;
break;
}
}
if (i == cableScanner->n) {
cout << "** not found cable **" << endl;
return false;
}
return true;
}
bool
CableScanner::test()
{
Vector3d pos(0, 0, -0.2);
Vector3d cur(100, 0, 0);
VectorXd e0 = VectorXd::Zero(n);
e0 << 0.000107550000000000,
0.000894042000000000 ,
0.000414297000000000 ,
0.000376473000000000 ,
0.000840930000000000 ,
0.00105442000000000 ,
0.00103013000000000 ,
0;//-1m
//0,
//0.000473595000000000 ,
//0.000915808000000000 ,
//0.00158678000000000 ,
//0.000510788000000000 ,
//0.000413732000000000 ,
//0.000841858000000000 ,
//0; // 0.5m
//0,
//0.000485222000000000,
//0.000469724000000000,
//0.000602988000000000,
//0.000218011000000000,
//0.000144855000000000,
//0.000447740000000000,
//0;// 1m
VectorXd cK = VectorXd::Zero(n);
//cK << 0.7578,
// 378.6616,
// 124.7508,
// 75.3842,
// 216.4860,
// 249.0531,
// 2.5615,
// 1.0000;
/*
cK << 0.9985,
1.0385,
1.0069,
0.9691,
1.0242,
1.0061,
1.0273,
1.0000;
this->k = k*(-0.024384);/*/
cK << 0.7783,
41.6144,
36.5788,
28.0842,
43.4910,
36.4128,
38.9824,
1.0000;/*/
/*0.3269,
36.3118,
29.1515,
15.4095,
47.2785,
37.4407,
0.0026,
1.0000;*/
//ofstream file("D:\\Docs\\code\\gitlab\\pmf\\MPS_Acquire_matlab_package\\errmap.txt");
ofstream file("errmap.txt");
clock_t t1 = clock();
MatrixXd MXd = scan(pos, cur, cK.cwiseProduct(e0), -2, 2, -2, 0.2, 100, 100);
cout << (clock() - t1) << " ms" << endl;
if (file.is_open())
{
file << MXd << endl;
}
else
cout << MXd << endl;
cout << "(" << maxy << "," << maxz << ")" << endl;
return true;
}
CFeatures* CJade::apply(CFeatures* features)
{
ASSERT(features);
SG_REF(features);
SGMatrix<float64_t> X = ((CDenseFeatures<float64_t>*)features)->get_feature_matrix();
int n = X.num_rows;
int T = X.num_cols;
int m = n;
Map<MatrixXd> EX(X.matrix,n,T);
// Mean center X
VectorXd mean = (EX.rowwise().sum() / (float64_t)T);
MatrixXd SPX = EX.colwise() - mean;
MatrixXd cov = (SPX * SPX.transpose()) / (float64_t)T;
#ifdef DEBUG_JADE
std::cout << "cov" << std::endl;
std::cout << cov << std::endl;
#endif
// Whitening & Projection onto signal subspace
SelfAdjointEigenSolver<MatrixXd> eig;
eig.compute(cov);
#ifdef DEBUG_JADE
std::cout << "eigenvectors" << std::endl;
std::cout << eig.eigenvectors() << std::endl;
std::cout << "eigenvalues" << std::endl;
std::cout << eig.eigenvalues().asDiagonal() << std::endl;
#endif
// Scaling
VectorXd scales = eig.eigenvalues().cwiseSqrt();
MatrixXd B = scales.cwiseInverse().asDiagonal() * eig.eigenvectors().transpose();
#ifdef DEBUG_JADE
std::cout << "whitener" << std::endl;
std::cout << B << std::endl;
#endif
// Sphering
SPX = B * SPX;
// Estimation of the cumulant matrices
int dimsymm = (m * ( m + 1)) / 2; // Dim. of the space of real symm matrices
int nbcm = dimsymm; // number of cumulant matrices
m_cumulant_matrix = SGMatrix<float64_t>(m,m*nbcm); // Storage for cumulant matrices
Map<MatrixXd> CM(m_cumulant_matrix.matrix,m,m*nbcm);
MatrixXd R(m,m); R.setIdentity();
MatrixXd Qij = MatrixXd::Zero(m,m); // Temp for a cum. matrix
VectorXd Xim = VectorXd::Zero(m); // Temp
VectorXd Xjm = VectorXd::Zero(m); // Temp
VectorXd Xijm = VectorXd::Zero(m); // Temp
int Range = 0;
for (int im = 0; im < m; im++)
{
Xim = SPX.row(im);
Xijm = Xim.cwiseProduct(Xim);
Qij = SPX.cwiseProduct(Xijm.replicate(1,m).transpose()) * SPX.transpose() / (float)T - R - 2*R.col(im)*R.col(im).transpose();
CM.block(0,Range,m,m) = Qij;
Range = Range + m;
for (int jm = 0; jm < im; jm++)
{
Xjm = SPX.row(jm);
Xijm = Xim.cwiseProduct(Xjm);
Qij = SPX.cwiseProduct(Xijm.replicate(1,m).transpose()) * SPX.transpose() / (float)T - R.col(im)*R.col(jm).transpose() - R.col(jm)*R.col(im).transpose();
CM.block(0,Range,m,m) = sqrt(2)*Qij;
Range = Range + m;
}
}
#ifdef DEBUG_JADE
std::cout << "cumulatant matrices" << std::endl;
std::cout << CM << std::endl;
#endif
// Stack cumulant matrix into ND Array
index_t * M_dims = SG_MALLOC(index_t, 3);
M_dims[0] = m;
M_dims[1] = m;
M_dims[2] = nbcm;
SGNDArray< float64_t > M(M_dims, 3);
for (int i = 0; i < nbcm; i++)
{
Map<MatrixXd> EM(M.get_matrix(i),m,m);
EM = CM.block(0,i*m,m,m);
}
// Diagonalize
SGMatrix<float64_t> Q = CJADiagOrth::diagonalize(M, m_mixing_matrix, tol, max_iter);
Map<MatrixXd> EQ(Q.matrix,m,m);
EQ = -1 * EQ.inverse();
#ifdef DEBUG_JADE
std::cout << "diagonalizer" << std::endl;
std::cout << EQ << std::endl;
#endif
// Separating matrix
SGMatrix<float64_t> sep_matrix = SGMatrix<float64_t>(m,m);
Map<MatrixXd> C(sep_matrix.matrix,m,m);
C = EQ.transpose() * B;
// Sort
VectorXd A = (B.inverse()*EQ).cwiseAbs2().colwise().sum();
bool swap = false;
do
{
swap = false;
for (int j = 1; j < n; j++)
{
if ( A(j) < A(j-1) )
{
std::swap(A(j),A(j-1));
C.col(j).swap(C.col(j-1));
swap = true;
}
}
} while(swap);
for (int j = 0; j < m/2; j++)
C.row(j).swap( C.row(m-1-j) );
// Fix Signs
VectorXd signs = VectorXd::Zero(m);
for (int i = 0; i < m; i++)
{
if( C(i,0) < 0 )
signs(i) = -1;
else
signs(i) = 1;
}
C = signs.asDiagonal() * C;
#ifdef DEBUG_JADE
std::cout << "un mixing matrix" << std::endl;
std::cout << C << std::endl;
#endif
// Unmix
EX = C * EX;
m_mixing_matrix = SGMatrix<float64_t>(m,m);
Map<MatrixXd> Emixing_matrix(m_mixing_matrix.matrix,m,m);
Emixing_matrix = C.inverse();
return features;
}
size_t UPGMpp::messagesLBP(CGraph &graph,
TInferenceOptions &options,
vector<vector<VectorXd> > &messages ,
bool maximize,
const vector<size_t> &tree)
{
const vector<CNodePtr> nodes = graph.getNodes();
const vector<CEdgePtr> edges = graph.getEdges();
multimap<size_t,CEdgePtr> edges_f = graph.getEdgesF();
size_t N_nodes = nodes.size();
size_t N_edges = edges.size();
bool is_tree = (tree.size()>0) ? true : false;
//graph.computePotentials();
//
// Build the messages structure
//
double totalSumOfMsgs = 0;
if ( !messages.size() )
messages.resize( N_edges);
for ( size_t i = 0; i < N_edges; i++ )
{
if ( !messages[i].size() )
{
messages[i].resize(2);
size_t ID1, ID2;
edges[i]->getNodesID(ID1,ID2);
// Messages from first node of the edge to the second one, so the size of
// the message has to be the same as the number of classes of the second node.
double N_classes = graph.getNodeWithID( ID2 )->getPotentials( options.considerNodeFixedValues ).rows();
messages[i][0].resize( N_classes );
messages[i][0].fill(1.0/N_classes);
// Just the opposite as before.
N_classes = graph.getNodeWithID( ID1 )->getPotentials( options.considerNodeFixedValues ).rows();
messages[i][1].resize( N_classes );
messages[i][1].fill(1.0/N_classes);
}
totalSumOfMsgs += messages[i][0].rows() + messages[i][1].rows();
}
// cout << "Initial Messages:" << endl;
// for ( size_t i=0; i < messages.size(); i++)
// for ( size_t j=0; j < messages[i].size(); j++)
// for ( size_t k=0; k < messages[i][j].size(); k++ )
// cout << messages[i][j][k] << " ";
vector<vector<VectorXd> > previousMessages;
if ( options.particularS["order"] == "RBP" )
{
previousMessages = messages;
for ( size_t i = 0; i < previousMessages.size(); i++ )
{
previousMessages[i][0].fill(0);
previousMessages[i][1].fill(0);
}
}
//
// Iterate until convergence or a certain maximum number of iterations is reached
//
size_t iteration;
// cout << endl;
for ( iteration = 0; iteration < options.maxIterations; iteration++ )
{
// cout << "Messages " << iteration << ":" << endl;
// for ( size_t i=0; i < messages.size(); i++)
// for ( size_t j=0; j < messages[i].size(); j++)
// for ( size_t k=0; k < messages[i][j].size(); k++ )
// cout << messages[i][j][k] << " ";
// cout << endl;
// Variables used by Residual Belief Propagation
int edgeWithMaxDiffIndex = -1;
VectorXd associatedMessage;
bool from1to2;
double maxDifference = -1;
//
// Iterate over all the nodes
//
for ( size_t nodeIndex = 0; nodeIndex < N_nodes; nodeIndex++ )
{
const CNodePtr nodePtr = graph.getNode( nodeIndex );
size_t nodeID = nodePtr->getID();
// Check if we are calibrating a tree, and so if the node is not member of the tree,
// so we dont have to update its messages
if ( is_tree && ( std::find(tree.begin(), tree.end(), nodeID ) == tree.end() ) )
continue;
NEIGHBORS_IT neighbors;
neighbors = edges_f.equal_range(nodeID);
//cout << " Sending messages ... " << endl;
//
// Send a message to each neighbor
//
for ( multimap<size_t,CEdgePtr>::iterator itNeigbhor = neighbors.first;
itNeigbhor != neighbors.second;
itNeigbhor++ )
{
// cout << "sending msg to neighbor..." << endl;
VectorXd nodePotPlusIncMsg = nodePtr->getPotentials( options.considerNodeFixedValues );
// cout << "nodePotPlusIncMsg Orig: " << nodePotPlusIncMsg.transpose() << endl;
size_t neighborID;
size_t ID1, ID2;
CEdgePtr edgePtr( (*itNeigbhor).second );
edgePtr->getNodesID(ID1,ID2);
( ID1 == nodeID ) ? neighborID = ID2 : neighborID = ID1;
// cout << "all ready" << endl;
// Check if we are calibrating a tree, and so if the neighbor node
// is not member of the tree, so we dont have to update its messages
if ( is_tree && ( std::find(tree.begin(), tree.end(), neighborID ) == tree.end() ))
continue;
//
// Compute the message from current node as a product of all the
// incoming messages less the one from the current neighbor
// plus the node potential of the current node.
//
for ( multimap<size_t,CEdgePtr>::iterator itNeigbhor2 = neighbors.first;
itNeigbhor2 != neighbors.second;
itNeigbhor2++ )
{
size_t ID11, ID12;
CEdgePtr edgePtr2( (*itNeigbhor2).second );
edgePtr2->getNodesID(ID11,ID12);
size_t edgeIndex = graph.getEdgeIndex( edgePtr2->getID() );
// cout << "Edge index: " << edgeIndex << endl << "node pot" << nodePotPlusIncMsg << endl;
// cout << "Node ID: " << nodeID << " node11 " << ID11 << " node12 " << ID12 << endl;
CNodePtr n1,n2;
edgePtr2->getNodes(n1,n2);
// cout << "Node 1 type: " << n1->getType()->getID() << " label " << n1->getType()->getLabel() << endl;
// cout << "Node 2 type: " << n2->getType()->getID() << " label " << n2->getType()->getLabel() << endl;
// Check if the current neighbor appears in the edge
if ( ( neighborID != ID11 ) && ( neighborID != ID12 ) )
{
if ( nodeID == ID11 )
{
// cout << "nodePotPlusIncMsg Prod: " << messages[ edgeIndex ][ 1 ].transpose() << endl;
// cout << "nodePotPlusIncMsg Bis : " << messages[ edgeIndex ][ 0 ].transpose() << endl;
nodePotPlusIncMsg =
nodePotPlusIncMsg.cwiseProduct(messages[ edgeIndex ][ 1 ]);
// cout << "nodePotPlusIncMsg Prod2: " << nodePotPlusIncMsg.transpose() << endl;
}
else // nodeID == ID2
{
// cout << "nodePotPlusIncMsg Prod: " << messages[ edgeIndex ][ 0 ].transpose() << endl;
// cout << "nodePotPlusIncMsg Bis : " << messages[ edgeIndex ][ 1 ].transpose() << endl;
nodePotPlusIncMsg =
nodePotPlusIncMsg.cwiseProduct(messages[ edgeIndex ][ 0 ]);
// cout << "nodePotPlusIncMsg Prod2: " << nodePotPlusIncMsg.transpose() << endl;
}
}
}
// cout << "Node pot" << endl;
//cout << "Node pot" << nodePotPlusIncMsg << endl;
//
// Take also the potential between the two nodes
//
MatrixXd edgePotentials;
if ( nodeID != ID1 )
edgePotentials = edgePtr->getPotentials();
else
edgePotentials = edgePtr->getPotentials().transpose();
VectorXd newMessage;
size_t edgeIndex = graph.getEdgeIndex( edgePtr->getID() );
// cout << "get new message" << endl;
if ( !maximize )
{
// Multiply both, and update the potential
// cout << "Edge potentials:" << edgePotentials.transpose() << endl;
// cout << "nodePotPlusIncMsg:" << nodePotPlusIncMsg.transpose() << endl;
newMessage = edgePotentials * nodePotPlusIncMsg;
// Normalize new message
if (newMessage.sum())
newMessage = newMessage / newMessage.sum();
//cout << "New message 3:" << newMessage.transpose() << endl;
}
else
{
if ( nodeID == ID1 )
newMessage.resize(messages[ edgeIndex ][0].rows());
else
newMessage.resize(messages[ edgeIndex ][1].rows());
for ( size_t row = 0; row < edgePotentials.rows(); row++ )
{
double maxRowValue = std::numeric_limits<double>::min();
for ( size_t col = 0; col < edgePotentials.cols(); col++ )
{
double value = edgePotentials(row,col)*nodePotPlusIncMsg(col);
if ( value > maxRowValue )
maxRowValue = value;
}
newMessage(row) = maxRowValue;
}
// Normalize new message
if (newMessage.sum())
newMessage = newMessage / newMessage.sum();
//cout << "New message: " << endl << newMessage << endl;
}
//
// Set the message!
//
VectorXd smoothedOldMessage(newMessage.rows());
smoothedOldMessage.setZero();
double smoothing = options.particularD["smoothing"];
if ( smoothing != 0 )
if ( nodeID == ID1 )
newMessage = newMessage + (1-smoothing) * messages[ edgeIndex ][0];
else
newMessage = newMessage + (1-smoothing) * messages[ edgeIndex ][1];
//cout << "New message:" << endl << newMessage << endl << "Smoothed" << endl << smoothedOldMessage << endl;
// If residual belief propagation is activated, just check if the
// newMessage is the one with the higest residual till the
// moment. Otherwise, set the new message as the current one
if ( options.particularS["order"] == "RBP" )
{
if ( nodeID == ID1 )
{
VectorXd differences = messages[edgeIndex][0] - newMessage;
double difference = differences.cwiseAbs().sum();
if ( difference > maxDifference )
{
from1to2 = true;
edgeWithMaxDiffIndex = edgeIndex;
maxDifference = difference;
associatedMessage = newMessage;
}
}
else
{
VectorXd differences = messages[edgeIndex][1] - newMessage;
double difference = differences.cwiseAbs().sum();
if ( difference > maxDifference )
{
from1to2 = false;
edgeWithMaxDiffIndex = edgeIndex;
maxDifference = difference;
associatedMessage = newMessage;
}
}
}
else
{
// cout << newMessage.cols() << " " << newMessage.rows() << endl;
// cout << "edgeIndex" << edgeIndex << endl;
if ( nodeID == ID1 )
{
// cout << messages[ edgeIndex ][0].cols() << " " << messages[ edgeIndex ][0].rows() << endl;
messages[ edgeIndex ][0] = newMessage;
}
else
{
// cout << messages[ edgeIndex ][1].cols() << " " << messages[ edgeIndex ][1].rows() << endl;
messages[ edgeIndex ][1] = newMessage;
}
// cout << "Wop " << endl;
}
}
} // Nodes
if ( options.particularS["order"] == "RBP" && ( edgeWithMaxDiffIndex =! -1 ))
{
if ( from1to2 )
messages[ edgeWithMaxDiffIndex ][0] = associatedMessage;
else
messages[ edgeWithMaxDiffIndex ][1] = associatedMessage;
}
//
// Check convergency!!
//
double newTotalSumOfMsgs = 0;
for ( size_t i = 0; i < N_edges; i++ )
{
newTotalSumOfMsgs += messages[i][0].sum() + messages[i][1].sum();
}
//printf("%4.10f\n",std::abs( totalSumOfMsgs - newTotalSumOfMsgs ));
if ( std::abs( totalSumOfMsgs - newTotalSumOfMsgs ) <
options.convergency )
break;
totalSumOfMsgs = newTotalSumOfMsgs;
// Show messages
/*cout << "Iteration:" << iteration << endl;
for ( size_t i = 0; i < messages.size(); i++ )
{
cout << messages[i][0] << " " << messages[i][1] << endl;
}*/
} // Iterations
return 1;
}
CMap<TParameter*, SGVector<float64_t> > CLaplacianInferenceMethod::
get_marginal_likelihood_derivatives(CMap<TParameter*,
CSGObject*>& para_dict)
{
check_members();
if(update_parameter_hash())
update_all();
MatrixXd Z(m_L.num_rows, m_L.num_cols);
Map<VectorXd> eigen_dlp(dlp.vector, dlp.vlen);
for (index_t i = 0; i < m_L.num_rows; i++)
{
for (index_t j = 0; j < m_L.num_cols; j++)
Z(i,j) = m_L(i,j);
}
MatrixXd sW_temp(sW.vlen, m_ktrtr.num_cols);
VectorXd sum(1);
sum[0] = 0;
for (index_t i = 0; i < sW.vlen; i++)
{
for (index_t j = 0; j < m_ktrtr.num_cols; j++)
sW_temp(i,j) = sW[i];
}
VectorXd g;
Map<VectorXd> eigen_W(W.vector, W.vlen);
Map<MatrixXd> eigen_temp_kernel(temp_kernel.matrix,
temp_kernel.num_rows, temp_kernel.num_cols);
if (eigen_W.minCoeff() < 0)
{
Z = -Z;
MatrixXd A = MatrixXd::Identity(m_ktrtr.num_rows, m_ktrtr.num_cols);
MatrixXd temp_diagonal(sW.vlen, sW.vlen);
temp_diagonal.setZero(sW.vlen, sW.vlen);
for (index_t s = 0; s < temp_diagonal.rows(); s++)
{
for (index_t r = 0; r < temp_diagonal.cols(); r++)
temp_diagonal(r,s) = W[s];
}
A = A + eigen_temp_kernel*m_scale*m_scale*temp_diagonal;
FullPivLU<MatrixXd> lu(A);
MatrixXd temp_matrix =
lu.inverse().cwiseProduct(eigen_temp_kernel*m_scale*m_scale);
VectorXd temp_sum(temp_matrix.rows());
for (index_t i = 0; i < temp_matrix.rows(); i++)
{
for (index_t j = 0; j < temp_matrix.cols(); j++)
temp_sum[i] += temp_matrix(j,i);
}
g = temp_sum/2.0;
}
else
{
MatrixXd C = Z.transpose().colPivHouseholderQr().solve(
sW_temp.cwiseProduct(eigen_temp_kernel*m_scale*m_scale));
MatrixXd temp_diagonal(sW.vlen, sW.vlen);
temp_diagonal.setZero(sW.vlen, sW.vlen);
for (index_t s = 0; s < sW.vlen; s++)
temp_diagonal(s,s) = sW[s];
MatrixXd temp = Z.transpose();
Z = Z.transpose().colPivHouseholderQr().solve(temp_diagonal);
Z = temp.transpose().colPivHouseholderQr().solve(Z);
for (index_t s = 0; s < Z.rows(); s++)
{
for (index_t r = 0; r < Z.cols(); r++)
Z(s,r) *= sW[s];
}
VectorXd temp_sum(C.rows());
temp_sum.setZero(C.rows());
for (index_t i = 0; i < C.rows(); i++)
{
for (index_t j = 0; j < C.cols(); j++)
temp_sum[i] += C(j,i)*C(j,i);
}
g = (eigen_temp_kernel.diagonal()*m_scale*m_scale-temp_sum)/2.0;
}
Map<VectorXd> eigen_d3lp(d3lp.vector, d3lp.vlen);
VectorXd dfhat = g.cwiseProduct(eigen_d3lp);
m_kernel->build_parameter_dictionary(para_dict);
m_mean->build_parameter_dictionary(para_dict);
m_model->build_parameter_dictionary(para_dict);
CMap<TParameter*, SGVector<float64_t> > gradient(
3+para_dict.get_num_elements(),
3+para_dict.get_num_elements());
for (index_t i = 0; i < para_dict.get_num_elements(); i++)
{
shogun::CMapNode<TParameter*, CSGObject*>* node =
para_dict.get_node_ptr(i);
TParameter* param = node->key;
CSGObject* obj = node->data;
index_t length = 1;
if ((param->m_datatype.m_ctype== CT_VECTOR ||
param->m_datatype.m_ctype == CT_SGVECTOR) &&
param->m_datatype.m_length_y != NULL)
length = *(param->m_datatype.m_length_y);
SGVector<float64_t> variables(length);
bool deriv_found = false;
Map<VectorXd> eigen_temp_alpha(temp_alpha.vector,
temp_alpha.vlen);
for (index_t h = 0; h < length; h++)
{
SGMatrix<float64_t> deriv;
SGVector<float64_t> mean_derivatives;
VectorXd mean_dev_temp;
SGVector<float64_t> lik_first_deriv;
SGVector<float64_t> lik_second_deriv;
if (param->m_datatype.m_ctype == CT_VECTOR ||
param->m_datatype.m_ctype == CT_SGVECTOR)
{
deriv = m_kernel->get_parameter_gradient(param, obj);
lik_first_deriv = m_model->get_first_derivative(
(CRegressionLabels*)m_labels, param, obj, function);
lik_second_deriv = m_model->get_second_derivative(
(CRegressionLabels*)m_labels, param, obj, function);
mean_derivatives = m_mean->get_parameter_derivative(
param, obj, m_feature_matrix, h);
for (index_t d = 0; d < mean_derivatives.vlen; d++)
mean_dev_temp[d] = mean_derivatives[d];
}
else
{
mean_derivatives = m_mean->get_parameter_derivative(
param, obj, m_feature_matrix);
for (index_t d = 0; d < mean_derivatives.vlen; d++)
mean_dev_temp[d] = mean_derivatives[d];
deriv = m_kernel->get_parameter_gradient(param, obj);
lik_first_deriv = m_model->get_first_derivative(
(CRegressionLabels*)m_labels, param, obj, function);
lik_second_deriv = m_model->get_second_derivative(
(CRegressionLabels*)m_labels, param, obj, function);
}
if (deriv.num_cols*deriv.num_rows > 0)
{
MatrixXd dK(deriv.num_cols, deriv.num_rows);
for (index_t d = 0; d < deriv.num_rows; d++)
{
for (index_t s = 0; s < deriv.num_cols; s++)
dK(d,s) = deriv(d,s);
}
sum[0] = (Z.cwiseProduct(dK)).sum()/2.0;
sum = sum - eigen_temp_alpha.transpose()*dK*eigen_temp_alpha/2.0;
VectorXd b = dK*eigen_dlp;
sum = sum -
dfhat.transpose()*(b-eigen_temp_kernel*(Z*b)*m_scale*m_scale);
variables[h] = sum[0];
deriv_found = true;
}
else if (mean_derivatives.vlen > 0)
{
sum = -eigen_temp_alpha.transpose()*mean_dev_temp;
sum = sum - dfhat.transpose()*(mean_dev_temp-eigen_temp_kernel*
(Z*mean_dev_temp)*m_scale*m_scale);
variables[h] = sum[0];
deriv_found = true;
}
else if (lik_first_deriv[0]+lik_second_deriv[0] != CMath::INFTY)
{
Map<VectorXd> eigen_fd(lik_first_deriv.vector, lik_first_deriv.vlen);
Map<VectorXd> eigen_sd(lik_second_deriv.vector, lik_second_deriv.vlen);
sum[0] = -g.dot(eigen_sd);
sum[0] = sum[0] - eigen_fd.sum();
variables[h] = sum[0];
deriv_found = true;
}
}
if (deriv_found)
gradient.add(param, variables);
}
TParameter* param;
index_t index = get_modsel_param_index("scale");
param = m_model_selection_parameters->get_parameter(index);
MatrixXd dK(m_ktrtr.num_cols, m_ktrtr.num_rows);
for (index_t d = 0; d < m_ktrtr.num_rows; d++)
{
for (index_t s = 0; s < m_ktrtr.num_cols; s++)
dK(d,s) = m_ktrtr(d,s)*m_scale*2.0;;
}
Map<VectorXd> eigen_temp_alpha(temp_alpha.vector,
temp_alpha.vlen);
sum[0] = (Z.cwiseProduct(dK)).sum()/2.0;
sum = sum - eigen_temp_alpha.transpose()*dK*eigen_temp_alpha/2.0;
VectorXd b = dK*eigen_dlp;
sum = sum - dfhat.transpose()*(b-eigen_temp_kernel*(Z*b)*m_scale*m_scale);
SGVector<float64_t> scale(1);
scale[0] = sum[0];
gradient.add(param, scale);
para_dict.add(param, this);
return gradient;
}
void CLBPInference::infer(CGraph &graph,
map<size_t,VectorXd> &nodeBeliefs,
map<size_t,MatrixXd> &edgeBeliefs,
double &logZ)
{
//
// Algorithm workflow:
// 1. Compute the messages passed
// 2. Compute node beliefs
// 3. Compute edge beliefs
// 4. Compute logZ
//
nodeBeliefs.clear();
edgeBeliefs.clear();
const vector<CNodePtr> nodes = graph.getNodes();
const vector<CEdgePtr> edges = graph.getEdges();
multimap<size_t,CEdgePtr> edges_f = graph.getEdgesF();
size_t N_nodes = nodes.size();
size_t N_edges = edges.size();
//
// 1. Compute the messages passed
//
vector<vector<VectorXd> > messages;
bool maximize = false;
messagesLBP( graph, m_options, messages, maximize );
//
// 2. Compute node beliefs
//
for ( size_t nodeIndex = 0; nodeIndex < N_nodes; nodeIndex++ )
{
const CNodePtr nodePtr = graph.getNode( nodeIndex );
size_t nodeID = nodePtr->getID();
VectorXd nodePotPlusIncMsg = nodePtr->getPotentials( m_options.considerNodeFixedValues );
NEIGHBORS_IT neighbors = edges_f.equal_range(nodeID);
//
// Get the messages for all the neighbors, and multiply them with the node potential
//
for ( multimap<size_t,CEdgePtr>::iterator itNeigbhor = neighbors.first;
itNeigbhor != neighbors.second;
itNeigbhor++ )
{
CEdgePtr edgePtr( (*itNeigbhor).second );
size_t edgeIndex = graph.getEdgeIndex( edgePtr->getID() );
if ( !edgePtr->getNodePosition( nodeID ) ) // nodeID is the first node in the edge
nodePotPlusIncMsg = nodePotPlusIncMsg.cwiseProduct(messages[ edgeIndex ][ 1 ]);
else // nodeID is the second node in the dege
nodePotPlusIncMsg = nodePotPlusIncMsg.cwiseProduct(messages[ edgeIndex ][ 0 ]);
}
// Normalize
nodePotPlusIncMsg = nodePotPlusIncMsg / nodePotPlusIncMsg.sum();
nodeBeliefs[ nodeID ] = nodePotPlusIncMsg;
//cout << "Beliefs of node " << nodeIndex << endl << nodePotPlusIncMsg << endl;
}
//
// 3. Compute edge beliefs
//
for ( size_t edgeIndex = 0; edgeIndex < N_edges; edgeIndex++ )
{
CEdgePtr edgePtr = edges[edgeIndex];
size_t edgeID = edgePtr->getID();
size_t ID1, ID2;
edgePtr->getNodesID( ID1, ID2 );
MatrixXd edgePotentials = edgePtr->getPotentials();
MatrixXd edgeBelief = edgePotentials;
VectorXd &message1To2 = messages[edgeIndex][0];
VectorXd &message2To1 = messages[edgeIndex][1];
//cout << "----------------------" << endl;
//cout << nodeBeliefs[ ID1 ] << endl;
//cout << "----------------------" << endl;
//cout << message2To1 << endl;
VectorXd node1Belief = nodeBeliefs[ ID1 ].cwiseQuotient( message2To1 );
VectorXd node2Belief = nodeBeliefs[ ID2 ].cwiseQuotient( message1To2 );
//cout << "----------------------" << endl;
MatrixXd node1BeliefMatrix ( edgePotentials.rows(), edgePotentials.cols() );
for ( size_t row = 0; row < edgePotentials.rows(); row++ )
for ( size_t col = 0; col < edgePotentials.cols(); col++ )
node1BeliefMatrix(row,col) = node1Belief(row);
//cout << "Node 1 belief matrix: " << endl << node1BeliefMatrix << endl;
edgeBelief = edgeBelief.cwiseProduct( node1BeliefMatrix );
MatrixXd node2BeliefMatrix ( edgePotentials.rows(), edgePotentials.cols() );
for ( size_t row = 0; row < edgePotentials.rows(); row++ )
for ( size_t col = 0; col < edgePotentials.cols(); col++ )
node2BeliefMatrix(row,col) = node2Belief(col);
//cout << "Node 2 belief matrix: " << endl << node2BeliefMatrix << endl;
edgeBelief = edgeBelief.cwiseProduct( node2BeliefMatrix );
//cout << "Edge potentials" << endl << edgePotentials << endl;
//cout << "Edge beliefs" << endl << edgeBelief << endl;
// Normalize
edgeBelief = edgeBelief / edgeBelief.sum();
edgeBeliefs[ edgeID ] = edgeBelief;
}
//
// 4. Compute logZ
//
double energyNodes = 0;
double energyEdges = 0;
double entropyNodes = 0;
double entropyEdges = 0;
// Compute energy and entropy from nodes
for ( size_t nodeIndex = 0; nodeIndex < nodes.size(); nodeIndex++ )
{
CNodePtr nodePtr = nodes[ nodeIndex ];
size_t nodeID = nodePtr->getID();
size_t N_Neighbors = graph.getNumberOfNodeNeighbors( nodeID );
// Useful computations and shorcuts
VectorXd &nodeBelief = nodeBeliefs[nodeID];
VectorXd logNodeBelief = nodeBeliefs[nodeID].array().log();
VectorXd nodePotentials = nodePtr->getPotentials( m_options.considerNodeFixedValues );
VectorXd logNodePotentials = nodePotentials.array().log();
// Entropy from the node
energyNodes += N_Neighbors*( nodeBelief.cwiseProduct( logNodeBelief ).sum() );
// Energy from the node
entropyNodes += N_Neighbors*( nodeBelief.cwiseProduct( logNodePotentials ).sum() );
}
// Compute energy and entropy from nodes
for ( size_t edgeIndex = 0; edgeIndex < N_edges; edgeIndex++ )
{
CEdgePtr edgePtr = edges[ edgeIndex ];
size_t edgeID = edgePtr->getID();
// Useful computations and shorcuts
MatrixXd &edgeBelief = edgeBeliefs[ edgeID ];
MatrixXd logEdgeBelief = edgeBelief.array().log();
MatrixXd &edgePotentials = edgePtr->getPotentials();
MatrixXd logEdgePotentials = edgePotentials.array().log();
// Entropy from the edge
energyEdges += edgeBelief.cwiseProduct( logEdgeBelief ).sum();
// Energy from the edge
entropyEdges += edgeBelief.cwiseProduct( logEdgePotentials ).sum();
}
// Final Bethe free energy
double BethefreeEnergy = ( energyNodes - energyEdges ) - ( entropyNodes - entropyEdges );
// Compute logZ
logZ = - BethefreeEnergy;
}
void CTRPBPInference::infer(CGraph &graph,
map<size_t,VectorXd> &nodeBeliefs,
map<size_t,MatrixXd> &edgeBeliefs,
double &logZ)
{
//
// Algorithm workflow:
// 1. Compute the messages passed
// 2. Compute node beliefs
// 3. Compute edge beliefs
// 4. Compute logZ
//
nodeBeliefs.clear();
edgeBeliefs.clear();
const vector<CNodePtr> nodes = graph.getNodes();
const vector<CEdgePtr> edges = graph.getEdges();
multimap<size_t,CEdgePtr> edges_f = graph.getEdgesF();
size_t N_nodes = nodes.size();
size_t N_edges = edges.size();
//
// 1. Create spanning trees
//
bool allNodesAdded = false;
vector<vector<size_t > > v_trees;
vector<bool> v_addedNodes(N_nodes,false);
map<size_t,size_t> addedNodesMap;
for (size_t i = 0; i < N_nodes; i++)
addedNodesMap[ nodes[i]->getID() ] = i;
while (!allNodesAdded)
{
allNodesAdded = true;
vector<size_t> tree;
getSpanningTree( graph, tree );
// Check that the tree is not empty
if ( tree.size() )
v_trees.push_back( tree );
cout << "Tree: ";
for ( size_t i_node = 0; i_node < tree.size(); i_node++ )
{
v_addedNodes[ addedNodesMap[tree[i_node]] ] = true;
cout << tree[i_node] << " ";
}
cout << endl;
for ( size_t i_node = 0; i_node < N_nodes; i_node++ )
if ( !v_addedNodes[i_node] )
{
allNodesAdded = false;
break;
}
}
//
// 1. Compute messages passed in each tree until convergence
//
vector<vector<VectorXd> > messages;
bool maximize = false;
double totalSumOfMsgs = std::numeric_limits<double>::max();
size_t iteration;
for ( iteration = 0; iteration < m_options.maxIterations; iteration++ )
{
for ( size_t i_tree=0; i_tree < v_trees.size(); i_tree++ )
messagesLBP( graph, m_options, messages, maximize, v_trees[i_tree] );
double newTotalSumOfMsgs = 0;
for ( size_t i = 0; i < N_edges; i++ )
{
newTotalSumOfMsgs += messages[i][0].sum() + messages[i][1].sum();
}
if ( std::abs( totalSumOfMsgs - newTotalSumOfMsgs ) <
m_options.convergency )
break;
totalSumOfMsgs = newTotalSumOfMsgs;
}
//
// 2. Compute node beliefs
//
for ( size_t nodeIndex = 0; nodeIndex < N_nodes; nodeIndex++ )
{
const CNodePtr nodePtr = graph.getNode( nodeIndex );
size_t nodeID = nodePtr->getID();
VectorXd nodePotPlusIncMsg = nodePtr->getPotentials( m_options.considerNodeFixedValues );
NEIGHBORS_IT neighbors = edges_f.equal_range(nodeID);
//
// Get the messages for all the neighbors, and multiply them with the node potential
//
for ( multimap<size_t,CEdgePtr>::iterator itNeigbhor = neighbors.first;
itNeigbhor != neighbors.second;
itNeigbhor++ )
{
CEdgePtr edgePtr( (*itNeigbhor).second );
size_t edgeIndex = graph.getEdgeIndex( edgePtr->getID() );
if ( !edgePtr->getNodePosition( nodeID ) ) // nodeID is the first node in the edge
nodePotPlusIncMsg = nodePotPlusIncMsg.cwiseProduct(messages[ edgeIndex ][ 1 ]);
else // nodeID is the second node in the dege
nodePotPlusIncMsg = nodePotPlusIncMsg.cwiseProduct(messages[ edgeIndex ][ 0 ]);
}
// Normalize
nodePotPlusIncMsg = nodePotPlusIncMsg / nodePotPlusIncMsg.sum();
nodeBeliefs[ nodeID ] = nodePotPlusIncMsg;
//cout << "Beliefs of node " << nodeIndex << endl << nodePotPlusIncMsg << endl;
}
//
// 3. Compute edge beliefs
//
for ( size_t edgeIndex = 0; edgeIndex < N_edges; edgeIndex++ )
{
CEdgePtr edgePtr = edges[edgeIndex];
size_t edgeID = edgePtr->getID();
size_t ID1, ID2;
edgePtr->getNodesID( ID1, ID2 );
MatrixXd edgePotentials = edgePtr->getPotentials();
MatrixXd edgeBelief = edgePotentials;
VectorXd &message1To2 = messages[edgeIndex][0];
VectorXd &message2To1 = messages[edgeIndex][1];
//cout << "----------------------" << endl;
//cout << nodeBeliefs[ ID1 ] << endl;
//cout << "----------------------" << endl;
//cout << message2To1 << endl;
VectorXd node1Belief = nodeBeliefs[ ID1 ].cwiseQuotient( message2To1 );
VectorXd node2Belief = nodeBeliefs[ ID2 ].cwiseQuotient( message1To2 );
//cout << "----------------------" << endl;
MatrixXd node1BeliefMatrix ( edgePotentials.rows(), edgePotentials.cols() );
for ( size_t row = 0; row < edgePotentials.rows(); row++ )
for ( size_t col = 0; col < edgePotentials.cols(); col++ )
node1BeliefMatrix(row,col) = node1Belief(row);
//cout << "Node 1 belief matrix: " << endl << node1BeliefMatrix << endl;
edgeBelief = edgeBelief.cwiseProduct( node1BeliefMatrix );
MatrixXd node2BeliefMatrix ( edgePotentials.rows(), edgePotentials.cols() );
for ( size_t row = 0; row < edgePotentials.rows(); row++ )
for ( size_t col = 0; col < edgePotentials.cols(); col++ )
node2BeliefMatrix(row,col) = node2Belief(col);
//cout << "Node 2 belief matrix: " << endl << node2BeliefMatrix << endl;
edgeBelief = edgeBelief.cwiseProduct( node2BeliefMatrix );
//cout << "Edge potentials" << endl << edgePotentials << endl;
//cout << "Edge beliefs" << endl << edgeBelief << endl;
// Normalize
edgeBelief = edgeBelief / edgeBelief.sum();
edgeBeliefs[ edgeID ] = edgeBelief;
}
//
// 4. Compute logZ
//
double energyNodes = 0;
double energyEdges = 0;
double entropyNodes = 0;
double entropyEdges = 0;
// Compute energy and entropy from nodes
for ( size_t nodeIndex = 0; nodeIndex < nodes.size(); nodeIndex++ )
{
CNodePtr nodePtr = nodes[ nodeIndex ];
size_t nodeID = nodePtr->getID();
size_t N_Neighbors = graph.getNumberOfNodeNeighbors( nodeID );
// Useful computations and shorcuts
VectorXd &nodeBelief = nodeBeliefs[nodeID];
VectorXd logNodeBelief = nodeBeliefs[nodeID].array().log();
VectorXd nodePotentials = nodePtr->getPotentials( m_options.considerNodeFixedValues );
VectorXd logNodePotentials = nodePotentials.array().log();
// Entropy from the node
energyNodes += N_Neighbors*( nodeBelief.cwiseProduct( logNodeBelief ).sum() );
// Energy from the node
entropyNodes += N_Neighbors*( nodeBelief.cwiseProduct( logNodePotentials ).sum() );
}
// Compute energy and entropy from nodes
for ( size_t edgeIndex = 0; edgeIndex < N_edges; edgeIndex++ )
{
CEdgePtr edgePtr = edges[ edgeIndex ];
size_t edgeID = edgePtr->getID();
// Useful computations and shorcuts
MatrixXd &edgeBelief = edgeBeliefs[ edgeID ];
MatrixXd logEdgeBelief = edgeBelief.array().log();
MatrixXd &edgePotentials = edgePtr->getPotentials();
MatrixXd logEdgePotentials = edgePotentials.array().log();
// Entropy from the edge
energyEdges += edgeBelief.cwiseProduct( logEdgeBelief ).sum();
// Energy from the edge
entropyEdges += edgeBelief.cwiseProduct( logEdgePotentials ).sum();
}
// Final Bethe free energy
double BethefreeEnergy = ( energyNodes - energyEdges ) - ( entropyNodes - entropyEdges );
// Compute logZ
logZ = - BethefreeEnergy;
}
