C++ DiagonalMatrix类代码示例

void SpinAdapted::diagonalise(Matrix& sym, DiagonalMatrix& d, Matrix& vec)
{
  int nrows = sym.Nrows();
  int ncols = sym.Ncols();
  assert(nrows == ncols);
  d.ReSize(nrows);
  vec.ReSize(nrows, nrows);

  Matrix workmat;
  workmat = sym;
  vector<double> workquery(1);
  int info = 0;
  double* dptr = d.Store();

  int query = -1;
  DSYEV('V', 'L', nrows, workmat.Store(), nrows, dptr, &(workquery[0]), query, info); // do query to find best size
  
  int optlength = static_cast<int>(workquery[0]);
  vector<double> workspace(optlength);

  DSYEV('V', 'U', nrows, workmat.Store(), nrows, dptr, &(workspace[0]), optlength, info); // do query to find best size


  
  if (info > 0) 
    {
      pout << "failed to converge " << endl;
      abort(); 
    }
  
  for (int i = 0; i < nrows; ++i)
    for (int j = 0; j < ncols; ++j)
      vec(j+1,i+1) = workmat(i+1,j+1);
}

void SpinAdapted::svd(Matrix& M, DiagonalMatrix& d, Matrix& U, Matrix& V)
{
  int nrows = M.Nrows();
  int ncols = M.Ncols();

  assert(nrows >= ncols);

  int minmn = min(nrows, ncols);
  int maxmn = max(nrows, ncols);
  int eigenrows = min(minmn, minmn);
  d.ReSize(minmn);
  Matrix Ut;
  Ut.ReSize(nrows, nrows);
  V.ReSize(ncols, ncols);

  int lwork = maxmn * maxmn + 100;
  double* workspace = new double[lwork];

  // first transpose matrix
  Matrix Mt;
  Mt = M.t();
  int info = 0;
  DGESVD('A', 'A', nrows, ncols, Mt.Store(), nrows, d.Store(), 
	 Ut.Store(), nrows, V.Store(), ncols, workspace, lwork, info);

  U.ReSize(nrows, ncols);
  SpinAdapted::Clear(U);
  for (int i = 0; i < nrows; ++i)
    for (int j = 0; j < ncols; ++j)
      U(i+1,j+1) = Ut(j+1,i+1);
  delete[] workspace;
}

/*!
  @fn Clik::Clik(const mRobot & mrobot_, const DiagonalMatrix & Kp_, const DiagonalMatrix & Ko_,
                 const Real eps_, const Real lambda_max_, const Real dt);
  @brief Constructor.
*/
Clik::Clik(const mRobot & mrobot_, const DiagonalMatrix & Kp_, const DiagonalMatrix & Ko_,
           const Real eps_, const Real lambda_max_, const Real dt_):
      dt(dt_),
      eps(eps_),
      lambda_max(lambda_max_),
      mrobot(mrobot_)
{
   robot_type = CLICK_mDH;
   // Initialize with same joint position (and rates) has the robot.
   q = mrobot.get_q();
   qp = mrobot.get_qp();
   qp_prev = qp;
   Kpep = ColumnVector(3); Kpep = 0;
   Koe0Quat = ColumnVector(3); Koe0Quat = 0;
   v = ColumnVector(6); v = 0;

   if(Kp_.Nrows()==3)
      Kp = Kp_;
   else
   {
      Kp = DiagonalMatrix(3); Kp = 0.0;
      cerr << "Clik::Clik-->mRobot, Kp if not 3x3, set gain to 0." << endl;
   }
   if(Ko_.Nrows()==3)
      Ko = Ko_;
   else
   {
      Ko = DiagonalMatrix(3); Ko = 0.0;
      cerr << "Clik::Cli, Ko if not 3x3, set gain to 0." << endl;
   }
}

void test1(Real* y, Real* x1, Real* x2, int nobs, int npred)
{
   cout << "\n\nTest 1 - traditional, bad\n";

   // traditional sum of squares and products method of calculation
   // but not adjusting means; maybe subject to round-off error

   // make matrix of predictor values with 1s into col 1 of matrix
   int npred1 = npred+1;        // number of cols including col of ones.
   Matrix X(nobs,npred1);
   X.Column(1) = 1.0;

   // load x1 and x2 into X
   //    [use << rather than = when loading arrays]
   X.Column(2) << x1;  X.Column(3) << x2;

   // vector of Y values
   ColumnVector Y(nobs); Y << y;

   // form sum of squares and product matrix
   //    [use << rather than = for copying Matrix into SymmetricMatrix]
   SymmetricMatrix SSQ; SSQ << X.t() * X;

   // calculate estimate
   //    [bracket last two terms to force this multiplication first]
   //    [ .i() means inverse, but inverse is not explicity calculated]
   ColumnVector A = SSQ.i() * (X.t() * Y);

   // Get variances of estimates from diagonal elements of inverse of SSQ
   // get inverse of SSQ - we need it for finding D
   DiagonalMatrix D; D << SSQ.i();
   ColumnVector V = D.AsColumn();

   // Calculate fitted values and residuals
   ColumnVector Fitted = X * A;
   ColumnVector Residual = Y - Fitted;
   Real ResVar = Residual.SumSquare() / (nobs-npred1);

   // Get diagonals of Hat matrix (an expensive way of doing this)
   DiagonalMatrix Hat;  Hat << X * (X.t() * X).i() * X.t();

   // print out answers
   cout << "\nEstimates and their standard errors\n\n";

   // make vector of standard errors
   ColumnVector SE(npred1);
   for (int i=1; i<=npred1; i++) SE(i) = sqrt(V(i)*ResVar);
   // use concatenation function to form matrix and use matrix print
   // to get two columns
   cout << setw(11) << setprecision(5) << (A | SE) << endl;

   cout << "\nObservations, fitted value, residual value, hat value\n";

   // use concatenation again; select only columns 2 to 3 of X
   cout << setw(9) << setprecision(3) <<
     (X.Columns(2,3) | Y | Fitted | Residual | Hat.AsColumn());
   cout << "\n\n";
}

void test3(Real* y, Real* x1, Real* x2, int nobs, int npred)
{
   cout << "\n\nTest 3 - Cholesky\n";

   // traditional sum of squares and products method of calculation
   // with subtraction of means - using Cholesky decomposition

   Matrix X(nobs,npred);
   X.Column(1) << x1;  X.Column(2) << x2;
   ColumnVector Y(nobs); Y << y;
   ColumnVector Ones(nobs); Ones = 1.0;
   RowVector M = Ones.t() * X / nobs;
   Matrix XC(nobs,npred);
   XC = X - Ones * M;
   ColumnVector YC(nobs);
   Real m = Sum(Y) / nobs;  YC = Y - Ones * m;
   SymmetricMatrix SSQ; SSQ << XC.t() * XC;

   // Cholesky decomposition of SSQ
   LowerTriangularMatrix L = Cholesky(SSQ);

   // calculate estimate
   ColumnVector A = L.t().i() * (L.i() * (XC.t() * YC));

   // calculate estimate of constant term
   Real a = m - (M * A).AsScalar();

   // Get variances of estimates from diagonal elements of invoice of SSQ
   DiagonalMatrix D; D << L.t().i() * L.i();
   ColumnVector V = D.AsColumn();
   Real v = 1.0/nobs + (L.i() * M.t()).SumSquare();

   // Calculate fitted values and residuals
   int npred1 = npred+1;
   ColumnVector Fitted = X * A + a;
   ColumnVector Residual = Y - Fitted;
   Real ResVar = Residual.SumSquare() / (nobs-npred1);

   // Get diagonals of Hat matrix (an expensive way of doing this)
   Matrix X1(nobs,npred1); X1.Column(1)<<Ones; X1.Columns(2,npred1)<<X;
   DiagonalMatrix Hat;  Hat << X1 * (X1.t() * X1).i() * X1.t();

   // print out answers
   cout << "\nEstimates and their standard errors\n\n";
   cout.setf(ios::fixed, ios::floatfield);
   cout << setw(11) << setprecision(5)  << a << " ";
   cout << setw(11) << setprecision(5)  << sqrt(v*ResVar) << endl;
   ColumnVector SE(npred);
   for (int i=1; i<=npred; i++) SE(i) = sqrt(V(i)*ResVar);
   cout << setw(11) << setprecision(5) << (A | SE) << endl;
   cout << "\nObservations, fitted value, residual value, hat value\n";
   cout << setw(9) << setprecision(3) <<
      (X | Y | Fitted | Residual | Hat.AsColumn());
   cout << "\n\n";
}

Matrix BaseController::pseudoInverse(const Matrix M)
{
   Matrix result;
   //int rows = this->rows();
   //int cols = this->columns();
   // calculate SVD decomposition
   Matrix U,V;
   DiagonalMatrix D;
   NEWMAT::SVD(M,D,U,V, true, true);
   Matrix Dinv = D.i();
   result = V * Dinv * U.t();
   return result;
}

void SpinAdapted::operatorfunctions::TensorProduct (const SpinBlock *ablock, const Baseoperator<Matrix>& a, const Baseoperator<Matrix>& b, const SpinBlock* cblock, const StateInfo* cstateinfo, DiagonalMatrix& cDiagonal, double scale)
{
  if (fabs(scale) < TINY) return;
  const int aSz = a.nrows();
  const int bSz = b.nrows();
  const char conjC = (cblock->get_leftBlock() == ablock) ? 'n' : 't';
  const SpinBlock* bblock = (cblock->get_leftBlock() == ablock) ? cblock->get_rightBlock() : cblock->get_leftBlock();
  const StateInfo& s = cblock->get_stateInfo();
  const StateInfo* lS = s.leftStateInfo, *rS = s.rightStateInfo;

  for (int aQ = 0; aQ < aSz; ++aQ)
    if (a.allowed(aQ, aQ))
      for (int bQ = 0; bQ < bSz; ++bQ)
	if (b.allowed(bQ, bQ))
	  if (s.allowedQuanta (aQ, bQ, conjC))
	  {
	    int cQ = s.quantaMap (aQ, bQ, conjC)[0];
	    Real scaleA = scale;
	    Real scaleB = 1;
	    if (conjC == 'n')
	      {
		scaleB *= dmrginp.get_ninej()(lS->quanta[aQ].get_s().getirrep() , rS->quanta[bQ].get_s().getirrep(), cstateinfo->quanta[cQ].get_s().getirrep(), 
					      a.get_spin().getirrep(), b.get_spin().getirrep(), 0,
					      lS->quanta[aQ].get_s().getirrep() , rS->quanta[bQ].get_s().getirrep(), cstateinfo->quanta[cQ].get_s().getirrep());
		scaleB *= Symmetry::spatial_ninej(lS->quanta[aQ].get_symm().getirrep() , rS->quanta[bQ].get_symm().getirrep(), cstateinfo->quanta[cQ].get_symm().getirrep(), 
				     a.get_symm().getirrep(), b.get_symm().getirrep(), 0,
				     lS->quanta[aQ].get_symm().getirrep() , rS->quanta[bQ].get_symm().getirrep(), cstateinfo->quanta[cQ].get_symm().getirrep());
		
		if (b.get_fermion() && IsFermion (lS->quanta [aQ])) scaleB *= -1.0;
		for (int aQState = 0; aQState < lS->quantaStates[aQ] ; aQState++)
		  MatrixDiagonalScale(a.operator_element(aQ, aQ)(aQState+1, aQState+1)*scaleA*scaleB, b.operator_element(bQ, bQ), 
				      cDiagonal.Store()+s.unBlockedIndex[cQ]+aQState*rS->quantaStates[bQ]);

	      }
	    else
	      {
		scaleB *= dmrginp.get_ninej()(lS->quanta[bQ].get_s().getirrep() , rS->quanta[aQ].get_s().getirrep(), cstateinfo->quanta[cQ].get_s().getirrep(), 
					      b.get_spin().getirrep(), a.get_spin().getirrep(), 0,
					      lS->quanta[bQ].get_s().getirrep() , rS->quanta[aQ].get_s().getirrep(), cstateinfo->quanta[cQ].get_s().getirrep());
		scaleB *= Symmetry::spatial_ninej(lS->quanta[bQ].get_symm().getirrep() , rS->quanta[aQ].get_symm().getirrep(), cstateinfo->quanta[cQ].get_symm().getirrep(), 
				     b.get_symm().getirrep(), a.get_symm().getirrep(), 0,
				     lS->quanta[bQ].get_symm().getirrep() , rS->quanta[aQ].get_symm().getirrep(), cstateinfo->quanta[cQ].get_symm().getirrep());
		
		if (a.get_fermion()&& IsFermion(lS->quanta[bQ])) scaleB *= -1.0;
		for (int bQState = 0; bQState < lS->quantaStates[bQ] ; bQState++)
		  MatrixDiagonalScale(b.operator_element(bQ, bQ)(bQState+1, bQState+1)*scaleA*scaleB, a.operator_element(aQ, aQ), 
				      cDiagonal.Store()+s.unBlockedIndex[cQ]+bQState*rS->quantaStates[aQ]);
	      }
	  }
}

void Foam::multiply
(
    scalarRectangularMatrix& ans,         // value changed in return
    const scalarRectangularMatrix& A,
    const DiagonalMatrix<scalar>& B,
    const scalarRectangularMatrix& C
)
{
    if (A.m() != B.size())
    {
        FatalErrorIn
        (
            "multiply("
            "const scalarRectangularMatrix& A, "
            "const DiagonalMatrix<scalar>& B, "
            "const scalarRectangularMatrix& C, "
            "scalarRectangularMatrix& answer)"
        )   << "A and B must have identical inner dimensions but A.m = "
            << A.m() << " and B.n = " << B.size()
            << abort(FatalError);
    }

    if (B.size() != C.n())
    {
        FatalErrorIn
        (
            "multiply("
            "const scalarRectangularMatrix& A, "
            "const DiagonalMatrix<scalar>& B, "
            "const scalarRectangularMatrix& C, "
            "scalarRectangularMatrix& answer)"
        )   << "B and C must have identical inner dimensions but B.m = "
            << B.size() << " and C.n = " << C.n()
            << abort(FatalError);
    }

    ans = scalarRectangularMatrix(A.n(), C.m(), scalar(0));

    for(register label i = 0; i < A.n(); i++)
    {
        for(register label g = 0; g < C.m(); g++)
        {
            for(register label l = 0; l < C.n(); l++)
            {
                ans[i][g] += C[l][g] * A[i][l]*B[l];
            }
        }
    }
}

int main()
{
   {
      // Get the data
      ColumnVector X(6);
      ColumnVector Y(6);
      X << 1   << 2   <<  3   <<  4   <<  6   <<  8;
      Y << 3.2 << 7.9 << 11.1 << 14.5 << 16.7 << 18.3;


      // Do the fit
      Model_3pe model(X);                // the model object
      NonLinearLeastSquares NLLS(model); // the non-linear least squares
                                         // object
      ColumnVector Para(3);              // for the parameters
      Para << 9 << -6 << .5;             // trial values of parameters
      cout << "Fitting parameters\n";
      NLLS.Fit(Y,Para);                  // do the fit

      // Inspect the results
      ColumnVector SE;                   // for the standard errors
      NLLS.GetStandardErrors(SE);
      cout << "\n\nEstimates and standard errors\n" <<
         setw(10) << setprecision(2) << (Para | SE) << endl;
      Real ResidualSD = sqrt(NLLS.ResidualVariance());
      cout << "\nResidual s.d. = " << setw(10) << setprecision(2) <<
         ResidualSD << endl;
      SymmetricMatrix Correlations;
      NLLS.GetCorrelations(Correlations);
      cout << "\nCorrelationMatrix\n" <<
         setw(10) << setprecision(2) << Correlations << endl;
      ColumnVector Residuals;
      NLLS.GetResiduals(Residuals);
      DiagonalMatrix Hat;
      NLLS.GetHatDiagonal(Hat);
      cout << "\nX, Y, Residual, Hat\n" << setw(10) << setprecision(2) <<
         (X | Y | Residuals | Hat.AsColumn()) << endl;
      // recover var/cov matrix
      SymmetricMatrix D;
      D << SE.AsDiagonal() * Correlations * SE.AsDiagonal();
      cout << "\nVar/cov\n" << setw(14) << setprecision(4) << D << endl;
   }

#ifdef DO_FREE_CHECK
   FreeCheck::Status();
#endif
 
   return 0;
}

TEST(CholeskyTest, testCholesky)
{
    Matrix A = Matrix(3,3,false);
    A.fillWithArgs(
             2.0,-1.0, 1.0,
            -1.0, 2.0,-1.0,
             1.0,-1.0, 2.0 );

/*    A.fillWithArgs(
      2.0, 0.0, 0.0,
      0.0, 2.0, 0.0,
      0.0, 0.0, 2.0 );*/


    cout << "Input Matrix:" << endl << A << endl;

    Matrix *U;
    DiagonalMatrix *D;
    Cholesky::udutDecompose(&A, &U, &D);
    cout << *U;

    cout << endl << "[";
    for (int i = 0; i < D->size(); i++)
    {
        cout << D->at(i) << " ";
    }
    cout << "]" << endl;

    UpperUnitaryMatrix *U1;
    DiagonalMatrix *D1;
    Cholesky::udutDecompose(&A,&U1,&D1);

    cout << *U1;

    cout << endl << "[";
    for (int i = 0; i < D->size(); i++)
    {
        cout << D->at(i) << " ";
    }
    cout << "]" << endl;

    delete U;
    delete U1;
    delete D;
    delete D1;
}

void getGeneralizedInverse(Matrix& G, Matrix& Gi) {
#ifdef DEBUG
  cout << "\n\ngetGeneralizedInverse - Singular Value\n";
#endif  

  // Singular value decomposition method
  
  // do SVD
  Matrix U, V;
  DiagonalMatrix D;
  SVD(G,D,U,V);            // X = U * D * V.t()
  
#ifdef DEBUG
  cout << "D:\n";
  cout << setw(9) << setprecision(6) << (D);
  cout << "\n\n";
#endif
  
  DiagonalMatrix Di;
  Di << D.i();
  
#ifdef DEBUG
  cout << "Di:\n";
  cout << setw(9) << setprecision(6) << (Di);
  cout << "\n\n";
#endif
  

  int i=Di.Nrows();
  for (; i>=1; i--) {
    if (Di(i) > 1000.0) {
      Di(i) = 0.0;
    }
  }
  
#ifdef DEBUG
  cout << "Di with biggies zeroed out:\n";
  cout << setw(9) << setprecision(6) << (Di);
  cout << "\n\n";
#endif
  
  //Matrix Gi;
  Gi << (U * (Di * V.t()));
  
  return;
}

 //======================================================================
 Vector rmvn_mt(RNG &rng, const Vector &mu, const DiagonalMatrix &V) {
   Vector ans(mu);
   const ConstVectorView variances(V.diag());
   for (int i = 0; i < mu.size(); ++i) {
     ans[i] += rnorm_mt(rng, 0, sqrt(variances[i]));
   }
   return ans;
 }

//#####################################################################
// Function Fast_Singular_Value_Decomposition
//#####################################################################
template<class T> void Matrix<T,3,2>::
fast_singular_value_decomposition(Matrix<T,3,2>& U,DiagonalMatrix<T,2>& singular_values,Matrix<T,2>& V) const
{
    if(!is_same<T,double>::value){
        Matrix<double,3,2> U_double;DiagonalMatrix<double,2> singular_values_double;Matrix<double,2> V_double;
        Matrix<double,3,2>(*this).fast_singular_value_decomposition(U_double,singular_values_double,V_double);
        U=Matrix<T,3,2>(U_double);singular_values=DiagonalMatrix<T,2>(singular_values_double);V=Matrix<T,2>(V_double);return;}
    // now T is double

    DiagonalMatrix<T,2> lambda;normal_equations_matrix().solve_eigenproblem(lambda,V);
    if(lambda.x11<0) lambda=lambda.clamp_min(0);
    singular_values=lambda.sqrt();
    U.set_column(0,(*this*V.column(0)).normalized());
    Vector<T,3> other=cross(weighted_normal(),U.column(0));
    T other_magnitude=other.magnitude();
    U.set_column(1,other_magnitude?other/other_magnitude:U.column(0).unit_orthogonal_vector());
}

 void PrincipalComponentsAnalysis::_sortEigens(Matrix& eigenVectors, 
   DiagonalMatrix& eigenValues)
 {
   // simple bubble sort, slow, but I don't expect very large matrices. Lots of room for 
   // improvement.
   bool change = true;
   while (change)
   {
     change = false;
     for (int c = 0; c < eigenVectors.Ncols() - 1; c++)
     {
       if (eigenValues.element(c) < eigenValues.element(c + 1))
       {
         std::swap(eigenValues.element(c), eigenValues.element(c + 1));
         for (int r = 0; r < eigenVectors.Nrows(); r++)
         {
           std::swap(eigenVectors.element(r, c), eigenVectors.element(r, c + 1));
         }
       }
     }
   }
 }

void test4(Real* y, Real* x1, Real* x2, int nobs, int npred)
{
   cout << "\n\nTest 4 - QR triangularisation\n";

   // QR triangularisation method
 
   // load data - 1s into col 1 of matrix
   int npred1 = npred+1;
   Matrix X(nobs,npred1); ColumnVector Y(nobs);
   X.Column(1) = 1.0;  X.Column(2) << x1;  X.Column(3) << x2;  Y << y;

   // do Householder triangularisation
   // no need to deal with constant term separately
   Matrix X1 = X;                 // Want copy of matrix
   ColumnVector Y1 = Y;
   UpperTriangularMatrix U; ColumnVector M;
   QRZ(X1, U); QRZ(X1, Y1, M);    // Y1 now contains resids
   ColumnVector A = U.i() * M;
   ColumnVector Fitted = X * A;
   Real ResVar = Y1.SumSquare() / (nobs-npred1);

   // get variances of estimates
   U = U.i(); DiagonalMatrix D; D << U * U.t();

   // Get diagonals of Hat matrix
   DiagonalMatrix Hat;  Hat << X1 * X1.t();

   // print out answers
   cout << "\nEstimates and their standard errors\n\n";
   ColumnVector SE(npred1);
   for (int i=1; i<=npred1; i++) SE(i) = sqrt(D(i)*ResVar);
   cout << setw(11) << setprecision(5) << (A | SE) << endl;
   cout << "\nObservations, fitted value, residual value, hat value\n";
   cout << setw(9) << setprecision(3) << 
      (X.Columns(2,3) | Y | Fitted | Y1 | Hat.AsColumn());
   cout << "\n\n";
}