Пример #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;
}
Пример #2
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;
}
VectorXd sample_coefs_single(
    VectorXd UtEta,
    MatrixXd UtW,
    VectorXd prior_mean,
    VectorXd prior_prec,
    double h2,
    double tot_Eta_prec,
    VectorXd randn_theta,
    VectorXd randn_e,
    VectorXd s,
    int b,
    int n
) {

  VectorXd R_sq_diag = ((h2 * s.array() + (1.0-h2))/tot_Eta_prec).sqrt();
  VectorXd theta_star = randn_theta.array()/prior_prec.array().sqrt();
  theta_star += prior_mean;
  VectorXd e_star = randn_e.array() * R_sq_diag.array();
  MatrixXd UtW_theta_star = UtW * theta_star;
  VectorXd eta_resid = UtEta - UtW_theta_star - e_star;
  MatrixXd RinvSqUtW = R_sq_diag.cwiseInverse().asDiagonal() * UtW;
  VectorXd eta_std = eta_resid.array()/R_sq_diag.array();
  VectorXd WtURinvy = RinvSqUtW.transpose() * eta_std;

  VectorXd theta_tilda;
  if(b < n) {
    MatrixXd C = RinvSqUtW.transpose() * RinvSqUtW;
    C.diagonal() = C.diagonal() + prior_prec;
    theta_tilda = C.householderQr().solve(WtURinvy);
  } else{
    MatrixXd VAi = UtW * prior_prec.cwiseInverse().asDiagonal();
    MatrixXd inner = VAi*UtW.transpose();
    for(int i = 0; i < n; i++) {
      inner(i,i) += (h2 * s(i) + (1.0-h2)) / tot_Eta_prec;
    }
    VectorXd VAiWtURinvy = VAi * WtURinvy;
    VectorXd outerWtURinvy = VAi.transpose() * inner.householderQr().solve(VAiWtURinvy);
    theta_tilda = WtURinvy.array() / prior_prec.array();
    theta_tilda -= outerWtURinvy;
  }

  VectorXd coefs = theta_tilda + theta_star;
  return coefs;
}
Пример #4
0
Matrix4d MinEnFilter::iterate(MatrixXd& XY, VectorXd& Depth, MatrixXd& Flow) {
	cout << "Iterate: " << iteration << endl;
	iteration ++;
	
	LieAlgebra grad, KE;
	MatrixXd Hessian, KP;
	
	
	// get and check dimensions
	nPoints = XY.rows();
	
	if (nPoints!= Depth.rows()) {
		cerr << "Dimensions of XY do not coincide with Dimensions of Depth." << endl;
	}
	if (nPoints!= Flow.rows()) {
		cerr << "Dimensions of XY do not coincide with Dimensions of Flow." << endl;
	}
	
	MatrixXd XYZ_inhom  = inhomCoordinates(XY, false);		// inhomogenous Coordinatestes
	MatrixXd Flow_inhom = inhomCoordinates(Flow, true);		// inhomogenous (relative) flow
	MatrixXd Disps_inhom = XYZ_inhom + Flow_inhom;			// inhomogenous Disparities
	MatrixXd temp(nPoints, 4);
	
	temp << XYZ_inhom, Depth.cwiseInverse();
	MatrixXd Gk = (Depth*MatrixXd::Ones(1,4)).cwiseProduct(temp);

	
	// compute Gradient of Hamiltonian
	grad = computeGradient(currG , Gk, Disps_inhom);
	// compute Hessian of Hamiltonian
	Hessian = computeHessian(Gk, Disps_inhom);

	
	// Numerical integration of state and second order information
	for (size_t i = 0; i < numSteps; i++ ){
		KE = integrateGImplicit(grad, Gk, Disps_inhom);
		currG = currG * KE.exp_g();
		KP = integratePExplicit(grad ,Hessian);
		currP = currP + delta * KP;
	}
	
	return currG.E;
}
MatrixXd sample_MME_multiple_diagR(
    MatrixXd Y,
    SpMat W,
    SpMat chol_C,
    VectorXd pe,
    MSpMat chol_K_inv,
    VectorXd tot_Eta_prec,
    MatrixXd randn_theta,
    MatrixXd randn_e
){
  MatrixXd theta_star = chol_K_inv.triangularView<Upper>().solve(randn_theta);
  MatrixXd e_star = randn_e * pe.cwiseSqrt().cwiseInverse().asDiagonal();
  MatrixXd W_theta_star = W * theta_star;

  MatrixXd Y_resid = Y - W_theta_star - e_star;
  MatrixXd WtRiy = W.transpose() * (Y_resid * pe.asDiagonal());

  MatrixXd theta_tilda = chol_C.transpose().triangularView<Upper>().solve(chol_C.triangularView<Lower>().solve(WtRiy));

  MatrixXd theta = theta_tilda * tot_Eta_prec.cwiseInverse().asDiagonal() + theta_star;

  return theta;
}
Пример #6
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;
}