示例#1
0
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);
}
示例#3
0
//	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);
}
示例#5
0
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;
}
示例#6
0
文件: CGppe.cpp 项目: cwebers/C-GPPE
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;
}
示例#7
0
/*@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;
}
示例#8
0
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;
}
示例#9
0
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;
}
示例#10
0
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;
}
示例#12
0
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;

}
示例#13
0
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;

}