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

Controller::Controller(dynamics::SkeletonDynamics* _skel, const vector<int> &_actuatedDofs,
                       const VectorXd &_kP, const VectorXd &_kD, const vector<int> &_ankleDofs, const VectorXd &_anklePGains, const VectorXd &_ankleDGains) :
    mSkel(_skel),
    mKp(_kP.asDiagonal()),
    mKd(_kD.asDiagonal()),
    mAnkleDofs(_ankleDofs),
    mAnklePGains(_anklePGains),
    mAnkleDGains(_ankleDGains),
    mTrajectory(NULL)
{
    const int nDof = mSkel->getNumDofs();

    mSelectionMatrix = MatrixXd::Zero(nDof, nDof);
    for (int i = 0; i < _actuatedDofs.size(); i++) {
        mSelectionMatrix(_actuatedDofs[i], _actuatedDofs[i]) = 1.0;
    }

    mDesiredDofs.resize(nDof);
    for (int i = 0; i < nDof; i++) {
        mDesiredDofs[i] = mSkel->getDof(i)->getValue();
    }

    Vector3d com = mSkel->getWorldCOM();
    double cop = 0.0;
    mPreOffset = com[0] - cop;
}

VectorXd lsq_weigh_nonneg(const MatrixXd& A, const VectorXd& b, const VectorXd& w)
{
	const unsigned m = A.rows();
	const unsigned n = A.cols();
	if(b.size() != m || w.size() != m || n < 1 || m < 1)
	{
		std::cerr<<"#error in lsq_weigh_nonneg: If A is a m by n matrix, then b and w must have length m. We also want n >= 1 and m >= 1. A is "<< m <<" by "<< n <<", b has length "<< b.size() <<" and w has length "<< w.size() <<"."<<std::endl;
		throw std::invalid_argument("lsq_weigh_nonneg: dimension mismatch");
	}
	
	// initialization
	const MatrixXd H = A.transpose() * w.asDiagonal() * A;
	const VectorXd f = - A.transpose() * w.asDiagonal() * b;
	VectorXd x = VectorXd::Zero(n), mu = f;
	
	// iterate
	double relerr;
	do
	{
		for(unsigned k = 0; k < n; ++k)
		{
			const double xtmp = x(k);
			x(k) -= mu(k) / H(k,k);
			if(x(k) < 0) x(k) = 0;
			const double d = x(k) - xtmp;
			if(d != 0) mu += d * H.col(k);
		}
		
		relerr = 0;
		for(unsigned k = 0; k < n; ++k)
		{
			if(x(k) != 0)
			{
				double err = mu(k) / x(k);
				if(err < 0) err = -err;
				if(err > relerr) relerr = err;
			}
		}
	}
	while(relerr >= 1e-2);
	
	// update non-zero columns with more costly, but also more precise calculation
	const VectorXi p = find(x.array() > 0);
	const VectorXd xp = lscov(pick_col(A, p), b, w);
	if(find(xp.array() >= 0).all()) set_ind(x, p, xp);
	
	return x;
}

VectorXd svdSolve( JacobiSVD<MatrixXd> & svd,VectorXd & ref, double * rankPtr = NULL, double EPS=1e-6 )
{
  typedef JacobiSVD<MatrixXd> SVDType;
  const JacobiSVD<MatrixXd>::MatrixUType &U = svd.matrixU();
  const JacobiSVD<MatrixXd>::MatrixVType &V = svd.matrixV();
  const JacobiSVD<MatrixXd>::SingularValuesType &s = svd.singularValues();
  //std::cout << (MATLAB)U<< (MATLAB)s<<(MATLAB)V << std::endl;

  const int N=V.rows(),M=U.rows(),S=std::min(M,N);

  int rank=0;
  while( rank<S && s[rank]>EPS ) rank++;

  VectorXd sinv = s.head(rank).array().inverse();

  //std::cout << "U = " << (MATLAB)U.leftCols(rank) << std::endl;
  //std::cout << "V = " << (MATLAB)V.leftCols(rank) << std::endl;
  //std::cout << "S = " << (MATLAB)(MatrixXd)sinv.asDiagonal() << std::endl;
  
  VectorXd x;
  x = V.leftCols(rank)
    * sinv.asDiagonal()
    * U.leftCols(rank).transpose()
    * ref;

  if(rankPtr!=NULL) *rankPtr = rank;

  return x;
}

void Spectral::generate_kernel_matrix(){

	// Fill kernel matrix
	K.resize(X.rows(),X.rows());
	for(unsigned int i = 0; i < X.rows(); i++){
		for(unsigned int j = i; j < X.rows(); j++){
			K(i,j) = K(j,i) = kernel(X.row(i),X.row(j));
			//if(i == 0) cout << K(i,j) << " ";
		}	
	}

	// Normalise kernel matrix	
	VectorXd d = K.rowwise().sum();
	for(unsigned int i = 0; i < d.rows(); i++){
		d(i) = 1.0/sqrt(d(i));
	}
	auto F = d.asDiagonal();
	MatrixXd l = (K * F);
	for(unsigned int i = 0; i < l.rows(); i++){
		for(unsigned int j = 0; j < l.cols(); j++){
			l(i,j) = l(i,j) * d(i);
		}
	}		
	K = l;

}

void eigs_sym_Cpp(MatrixXd &M, VectorXd &init_resid, int k, int m,
                  double &time_used, double &prec_err)
{
    double start, end;
    start = get_wall_time();

    DenseGenMatProd<double> op(M);
    SymEigsSolver< double, LARGEST_MAGN, DenseGenMatProd<double> > eigs(&op, k, m);
    eigs.init(init_resid.data());

    int nconv = eigs.compute();
    int niter = eigs.num_iterations();
    int nops = eigs.num_operations();

    VectorXd evals = eigs.eigenvalues();
    MatrixXd evecs = eigs.eigenvectors();

/*
    std::cout << "computed eigenvalues D = \n" << evals.transpose() << std::endl;
    std::cout << "first 5 rows of computed eigenvectors U = \n" << evecs.topRows<5>() << std::endl;
    std::cout << "nconv = " << nconv << std::endl;
    std::cout << "niter = " << niter << std::endl;
    std::cout << "nops = " << nops << std::endl;
*/

    end = get_wall_time();
    time_used = (end - start) * 1000;

    MatrixXd err = M * evecs - evecs * evals.asDiagonal();
    prec_err = err.cwiseAbs().maxCoeff();
}

SparseMatrix<double> compute_M2_topic( SparseMatrix<double> data ){
	cout << "Computing M2 topic..." << endl;
	int n = data.rows();
	int na = data.cols();
	SparseMatrix<double> M2(na,na);
	for (int t = 0; t < n; ++t){
		cout << t << endl;

		VectorXd c = data.row(t);
		SparseVector<double> c_sp = data.row(t);
		MatrixXd temp = c.asDiagonal();
		M2 += (c_sp*c_sp.transpose() - temp.sparseView());
	}
	M2 *= (alpha0+1.)/n;

	SparseVector<double> M1 = compute_M1_topic(data).sparseView();

	cout << M2.cols() << endl;
	cout << M2.rows() << endl;
	cout << M1.size() << endl;
	SparseMatrix<double> aux = alpha0 * ( M1 * M1.transpose() );
	cout << aux.cols() << endl;
	cout << aux.rows() << endl;
	M2 = M2 - aux;
	return M2;
}

// ZCA of genotypes
void RandomPCA::zca_whiten(bool transpose)
{
   verbose && std::cout << timestamp() << " Whitening begin" << std::endl;
   VectorXd s = 1 / d.array();
   MatrixXd Dinv = s.asDiagonal();

   if(transpose)
      W.noalias() = U * Dinv * U.transpose() * X.transpose();
   else
      W.noalias() = U * Dinv * U.transpose() * X;
   verbose && std::cout << timestamp() << " Whitening done (" << dim(W) << ")" << std::endl;
}

	void pseudoinverse(const MatrixXd& A, MatrixXd& P)
	{
		JacobiSVD<MatrixXd> svd(A, ComputeThinU | ComputeThinV);
		VectorXd invSingularVals = svd.singularValues();
		const double tolerance = 1e-6;
		for (int i = 0; i < invSingularVals.size(); i++)
			if (fabs(invSingularVals(i)) > tolerance)
				invSingularVals(i) = 1.0/invSingularVals(i);
			else
				invSingularVals(i) = 0.0;
		P = svd.matrixV() * invSingularVals.asDiagonal() * svd.matrixU().transpose();
	}

MatrixXd CMT::pInverse(const MatrixXd& matrix) {
	if(matrix.size() == 0)
		return matrix.transpose();

	JacobiSVD<MatrixXd> svd(matrix, ComputeThinU | ComputeThinV);

	VectorXd svInv = svd.singularValues();

	for(int i = 0; i < svInv.size(); ++i)
		if(svInv[i] > 1e-8)
			svInv[i] = 1. / svInv[i];

	return svd.matrixV() * svInv.asDiagonal() * svd.matrixU().transpose();
}

//**************************************************************************************************
Eigen::MatrixRXd wholeBodyReach::pinvDampedEigen(const Eigen::Ref<Eigen::MatrixRXd> &A, double damp)
{
    // allocate memory
    int m = A.rows(), n = A.cols(), k = m<n?m:n;
    VectorXd SpinvD = VectorXd::Zero(k);
    // compute decomposition
    JacobiSVD<MatrixRXd> svd(A, ComputeThinU | ComputeThinV);    // default Eigen SVD
    VectorXd sv = svd.singularValues();
    // compute pseudoinverse of singular value matrix
    double damp2 = damp*damp;
    for (int c=0;c<k; c++)
        SpinvD(c) = sv(c) / (sv(c)*sv(c) + damp2);
    // compute damped pseudoinverse
    return svd.matrixV() * SpinvD.asDiagonal() * svd.matrixU().transpose();
}

MatrixXd compute_w( SparseMatrix<double> M2, int KHID ){
	cout << "Computing the whitening matrix..." << endl;
	int m = M2.rows();
	cout << "Calculating SVD..." << endl;
	JacobiSVD<MatrixXd> svd( M2, ComputeThinU );
	cout << "SVD successfully calculated" << endl;
	VectorXd E = svd.singularValues();
	E = E.head(KHID);
	E = E.array().sqrt().inverse().matrix();
	MatrixXd E_diag = E.asDiagonal();
	MatrixXd U = svd.matrixU();
	U = U.block(0,0,m,KHID);
	cout << U.rows() << endl;
	cout << U.cols() << endl;
	return U*E_diag;
}

void LAR::train(const MatrixXd& data )
{ 
	LINE; LINE;
	cout << "Data Dimension : " << data.rows() << " X " << data.cols() << endl;
	LINE;
	cout << endl;

	//Training data
	double trainRatio = 2.8 / 3;
	int breakPoint = (int)(data.rows() * trainRatio), col = data.cols(), row = data.rows();

	MatrixXd X_(breakPoint, col);
	X_ << data.block(0, 0, breakPoint, col - 1), MatrixXd::Ones(breakPoint, 1);
	MatrixXd Y = data.block(0, col - 1, breakPoint, 1);

	//testing
	MatrixXd Xts(row - breakPoint, col);
	Xts = data.block(breakPoint, 0, row - breakPoint, col - 1), MatrixXd::Ones(row - breakPoint, 1);
	MatrixXd Yts = data.block(breakPoint, col - 1, row - breakPoint, 1);

	//Least absolute error regression 

	//initialize the parameter 
	VectorXd beta_new = VectorXd::Zero(col);
	VectorXd beta_old = VectorXd::Ones(col);

	cout << "Iteratively weighted least square regression:" << endl << endl;
	int i = 0;
	while ((beta_new - beta_old).norm()>0.01)
	{
		beta_old = beta_new;
		//Parameter estiamtion -- iteratively weighted least square
		VectorXd weight = 1 / (Y - X_*beta_old).array().abs();
		MatrixXd C = weight.asDiagonal();
		beta_new = (X_.transpose()*C*X_).inverse()*X_.transpose()*C*Y;
		cout << "Training iteration: " << setw(3) << ++i << " , current coefficients: " << beta_new.transpose() << endl;
	} 
	//prediction on test set 

	LINE;
	cout << "Result: " << endl << endl;
	beta = beta_new.head(beta_new.size()-1);
	cout << " Coefficients for X are: " << beta << endl;
	offset = beta_new(beta_new.size() - 1);
	cout << " Offset term is: " << offset << endl;
	LINE; LINE;
}

MatrixXd rbf_kernel(MatrixXd& X, const double sigma, bool rbf_center,
   bool verbose)
{
   unsigned int n = X.rows();
   VectorXd norms = X.array().square().rowwise().sum();
   VectorXd ones = VectorXd::Ones(n);
   MatrixXd R = norms * ones.transpose();
   MatrixXd D = R + R.transpose() - 2 * X * X.transpose();
   D = D.array() / (-1 * sigma * sigma);
   MatrixXd K = D.array().exp();

   if(rbf_center)
   {
      verbose && std::cout << timestamp() << " Centering RBF kernel" << std::endl;
      MatrixXd M = ones * ones.transpose() / n;
      MatrixXd I = ones.asDiagonal();
      K = (I - M) * K * (I - M);
   }
   return K;
}

VectorXd
PatchFit::lls(MatrixXd J, VectorXd b)
{
  // check input dimensions
  if (J.rows() != b.rows())
    cerr << "Wrong dimensions" << endl;

  VectorXd a (J.cols()); //output parameters

  JacobiSVD<MatrixXd> svd(J, ComputeThinU | ComputeThinV);
  VectorXd sv = svd.singularValues();
  MatrixXd V = svd.matrixV();

  VectorXd pv = sv.array().inverse();
  VectorXd pvSq = pv.array()*pv.array();

  if (b.any())
    a = V * pvSq.asDiagonal() * V.adjoint() * J.adjoint() * b;
  else
    a = V.rightCols(1);
  
  return a;
}

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