C++ VectorXd::cwiseProduct方法代码示例

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;
}

//.........这里部分代码省略.........
	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();

//.........这里部分代码省略.........

//.........这里部分代码省略.........

                // 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;

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++)
//.........这里部分代码省略.........

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);
//.........这里部分代码省略.........