C++ MatrixXd::trace方法代码示例

// Kinetic energy integral
double kinetic(int N, Eigen::MatrixXd& A, Eigen::MatrixXd& A_p,
			   Eigen::MatrixXd& A_dp, Eigen::MatrixXd& A_dp_inv,
			   Eigen::MatrixXd& k, Eigen::MatrixXd& k_p,
			   Eigen::MatrixXd& k_dp, double S, Eigen::MatrixXd& L2)
{
	// Calculate the T factor
	// Start with A''^{-1} A' L2 A A''^{-1}
	Eigen::MatrixXd tempMat = A_dp_inv*A_p*L2*A*A_dp_inv;
	
	// Determine k''. A''^{-1} A' L2  A A''^{-1} . k''
	double T = 0.0;
  	for (int i = 0; i < N; i++){
		for (int j = 0; j < N; j++){
			double temp = k_dp(i, 0) * k_dp(j, 0) + k_dp(i, 1) * k_dp(j, 1);
			temp += k_dp(i, 2) * k_dp(j, 2);
			T += tempMat(i, j) * temp;
		}
	}

	// Then k . L2 .  k'
	for (int i = 0; i < N; i++){
		T += k(i, 0) * L2(i, i) *  k_p(i, 0);
		T += k(i, 1) * L2(i, i) * k_p(i, 1);
		T += k(i, 2) * L2(i, i) * k_p(i, 2);
	}

	// And -k'.A L2 A''^{-1}.k''
	tempMat = A*L2*A_dp_inv;
	for (int i = 0; i < N; i++){
		for (int j = 0; j < N; j++){
			double temp = k_p(i, 0) * k_dp(j, 0);
			temp += k_p(i, 1) * k_dp(j, 1) + k_p(i, 2) * k_dp(j, 2);
			T -= tempMat(i, j) * temp;
		}
	}
	
	// And -k''.A''^-1 L2 A'.k
	tempMat = A_dp_inv * L2 *  A_p;
	for (int i = 0; i < N; i++){
		for (int j = 0; j < N; j++){
			double temp = k_dp(i, 0) * k(j, 0);
			temp += k_dp(i, 1) * k(j, 1) + k_dp(i, 2) * k(j, 2);
			T -= tempMat(i, j) * temp;
		}
	}

	
	// This just leaves 6Tr{A A''^-1 A' L2} 
	tempMat = A * A_dp_inv * A_p*L2;
	T += 6.0*tempMat.trace(); 

	T = 0.5*T*S;
	
	return T;
}

double weight_unsymkl_gauss(PCObject &o1, PCObject &o2)
{
    int dim = o1.gaussian.dim;
    Eigen::MatrixXd multicov = Eigen::MatrixXd(3,3);
    multicov = o2.gaussian.cov_inverse * o1.gaussian.covariance;
    Eigen::VectorXd mean = o2.gaussian.mean-o1.gaussian.mean;
    double unsymkl_12 = (multicov.trace()
                         + mean.transpose()*o2.gaussian.cov_inverse*mean
                         + log(o1.gaussian.cov_determinant/o2.gaussian.cov_determinant)-dim) / 2.;
//    cout<<"kl: "<<unsymkl_12<<endl;
    return unsymkl_12;
}

//.........这里部分代码省略.........
        J(k,j) = t1 * cc + t2 * ss;
        J(k,j + 1) = xny * (J(k,j) + t1) - t2;
      }
    }
  };

  int i, j, k, l; /* indices */
  int ip, me, mi;
  int n=g0.size();  int p=ce0.size();  int m=ci0.size();  
  MatrixXd R(G.rows(),G.cols()), J(G.rows(),G.cols());
  
  LLT<MatrixXd,Lower> chol(G.cols());
 
  VectorXd s(m+p), z(n), r(m + p), d(n),  np(n), u(m + p);
  VectorXd x_old(n), u_old(m + p);
  double f_value, psi, c1, c2, sum, ss, R_norm;
  const double inf = std::numeric_limits<double>::infinity();
  double t, t1, t2; /* t is the step length, which is the minimum of the partial step length t1 
    * and the full step length t2 */
  VectorXi A(m + p), A_old(m + p), iai(m + p);
  int q;
  int iq, iter = 0;
  std::vector<bool> iaexcl(m + p);
 	
  me = p; /* number of equality constraints */
  mi = m; /* number of inequality constraints */
  q = 0;  /* size of the active set A (containing the indices of the active constraints) */
  
  /*
   * Preprocessing phase
   */
	
  /* compute the trace of the original matrix G */
  c1 = G.trace();
	
	/* decompose the matrix G in the form LL^T */
  chol.compute(G);
 
  /* initialize the matrix R */
  d.setZero();
  R.setZero();
	R_norm = 1.0; /* this variable will hold the norm of the matrix R */
  
	/* compute the inverse of the factorized matrix G^-1, this is the initial value for H */
  // J = L^-T
  J.setIdentity();
  J = chol.matrixU().solve(J);
	c2 = J.trace();
#ifdef TRACE_SOLVER
 print_matrix("J", J, n);
#endif
  
	/* c1 * c2 is an estimate for cond(G) */
  
	/* 
   * Find the unconstrained minimizer of the quadratic form 0.5 * x G x + g0 x 
   * this is a feasible point in the dual space
	 * x = G^-1 * g0
   */
  x = chol.solve(g0);
  x = -x;
	/* and compute the current solution value */ 
	f_value = 0.5 * g0.dot(x);
#ifdef TRACE_SOLVER
  std::cerr << "Unconstrained solution: " << f_value << std::endl;
  print_vector("x", x, n);