コード例 #1
0
ファイル: MatrixBLAS.C プロジェクト: i-maruyama/Block
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);
}
コード例 #2
0
ファイル: MatrixBLAS.C プロジェクト: i-maruyama/Block
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;
}
コード例 #3
0
ファイル: clik.cpp プロジェクト: JakaCikac/katana_300_ros
/*!
  @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;
   }
}
コード例 #4
0
ファイル: example.cpp プロジェクト: 151706061/sofa
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";
}
コード例 #5
0
ファイル: example.cpp プロジェクト: 151706061/sofa
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";
}
コード例 #6
0
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;
}
コード例 #7
0
ファイル: operatorfunctions.C プロジェクト: chrinide/Block
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]);
	      }
	  }
}
コード例 #8
0
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];
            }
        }
    }
}
コード例 #9
0
ファイル: nl_ex.cpp プロジェクト: 151706061/sofa
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;
}
コード例 #10
0
ファイル: main_test_cholesky.cpp プロジェクト: Aldrog/corecvs
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;
}
コード例 #11
0
ファイル: hesstrans.C プロジェクト: VictorMion/vmd-cvs-github
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;
}
コード例 #12
0
ファイル: mvn.cpp プロジェクト: cran/Boom
 //======================================================================
 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;
 }
コード例 #13
0
ファイル: Matrix3x2.cpp プロジェクト: Haider-BA/geode
//#####################################################################
// 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());
}
コード例 #14
0
 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));
         }
       }
     }
   }
 }
コード例 #15
0
ファイル: example.cpp プロジェクト: 151706061/sofa
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";
}
コード例 #16
0
ファイル: example.cpp プロジェクト: 151706061/sofa
void test5(Real* y, Real* x1, Real* x2, int nobs, int npred)
{
   cout << "\n\nTest 5 - singular value\n";

   // Singular value decomposition 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 SVD
   Matrix U, V; DiagonalMatrix D;
   SVD(X,D,U,V);                              // X = U * D * V.t()
   ColumnVector Fitted = U.t() * Y;
   ColumnVector A = V * ( D.i() * Fitted );
   Fitted = U * Fitted;
   ColumnVector Residual = Y - Fitted;
   Real ResVar = Residual.SumSquare() / (nobs-npred1);

   // get variances of estimates
   D << V * (D * D).i() * V.t();

   // Get diagonals of Hat matrix
   DiagonalMatrix Hat;  Hat << U * U.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 | Residual | Hat.AsColumn());
   cout << "\n\n";
}
コード例 #17
0
int my_main()                  // called by main()
{
   Tracer tr("my_main ");      // for tracking exceptions
   
   int n = 7;                 // this is the order we will work with
   int i, j;

   // declare a matrix
   SymmetricMatrix H(n);
   
   // load values for Hilbert matrix
   for (i = 1; i <= n; ++i) for (j = 1; j <= i; ++j)
      H(i, j) = 1.0 / (i + j - 1);

   // print the matrix
   cout << "SymmetricMatrix H" << endl;
   cout << setw(10) << setprecision(7) << H << endl;
   
   // calculate its eigenvalues and eigenvectors and print them
   Matrix U; DiagonalMatrix D;
   EigenValues(H, D, U);
   cout << "Eigenvalues of H" << endl;
   cout << setw(17) << setprecision(14) << D.AsColumn() << endl;
   cout << "Eigenvector matrix, U" << endl;
   cout << setw(10) << setprecision(7) << U << endl;

   // check orthogonality
   cout << "U * U.t() (should be near identity)" << endl;   
   cout << setw(10) << setprecision(7) << (U * U.t()) << endl;
   
   // check decomposition
   cout << "U * D * U.t() (should be near H)" << endl;   
   cout << setw(10) << setprecision(7) << (U * D * U.t()) << endl;
   
   return 0;
}
コード例 #18
0
ファイル: tmt.cpp プロジェクト: Jornason/DieHard
void Print(const DiagonalMatrix& X)
{
   ++PCN;
   cout << "\nMatrix type: " << X.Type().Value() << " (";
   cout << X.Nrows() << ", ";
   cout << X.Ncols() << ")\n\n";
   if (X.IsZero()) { cout << "All elements are zero\n" << flush; return; }
   int nr=X.Nrows(); int nc=X.Ncols();
   for (int i=1; i<=nr; i++)
   {
      for (int j=1; j<i; j++) cout << "\t";
      if (i<=nc) cout << X(i,i) << "\t";
      cout << "\n";
   }
   cout << flush; ++PCZ;
}
コード例 #19
0
ファイル: MathTypes.cpp プロジェクト: doubaoatthu/clothsim
void SymMatrixBlocks::SetDiag(DiagonalMatrix &src, DiagonalMatrix &ret)
{
	// this function treats DiagonalMatrix src as a column vector with
	// the diagonal entries as the vector components.
	// the result is another diagonal matrix with the diagonal entries
	// treates as the components of the resulting vector from the 
	// matrix-vector multiply.

	ret.Zero();
	// these really must be of the same size to make sense.
	if( m_iSize != src.m_iSize)
		return;

	// if all entries are still zero, we're already done
	if(m_bAllZero) return;

	for(int i=0; i<m_iSize; i++ )
	{
		int start = m_rowstart[i];
		int length = m_rowstart[i+1] - start;
		for(int j = 0; j<length; j++ )
		{
			// perform multiplication for nonzero matrix blocks
			int iCol = m_col_lookup[start+j];
			Matrix3x3 *temp = m_matBlock[start+j];
			ret.m_block[i].x += temp->elt[0] * src.m_block[iCol].x + temp->elt[1] * src.m_block[iCol].y + temp->elt[2] * src.m_block[iCol].z;
			ret.m_block[i].y += temp->elt[3] * src.m_block[iCol].x + temp->elt[4] * src.m_block[iCol].y + temp->elt[5] * src.m_block[iCol].z;
			ret.m_block[i].z += temp->elt[6] * src.m_block[iCol].x + temp->elt[7] * src.m_block[iCol].y + temp->elt[8] * src.m_block[iCol].z;
			// since we stored stuff in upper triangular form, we need to use the transpose of the appropriate matrix block for the complementary matrix-vector mult
			// NOTE: symmetry does not hold for the supermatrix or the submatrices necessarily, but for the matrix obtained by embedding the submatrices into the supermatrix structure
			if( i != iCol )
			{
				ret.m_block[iCol].x += temp->elt[0] * src.m_block[i].x + temp->elt[3] * src.m_block[i].y + temp->elt[6] * src.m_block[i].z;
				ret.m_block[iCol].y += temp->elt[1] * src.m_block[i].x + temp->elt[4] * src.m_block[i].y + temp->elt[7] * src.m_block[i].z;
				ret.m_block[iCol].z += temp->elt[2] * src.m_block[i].x + temp->elt[5] * src.m_block[i].y + temp->elt[8] * src.m_block[i].z;
			}
		}
	}
}
コード例 #20
0
ファイル: linear.C プロジェクト: junyang4711/Block
void SpinAdapted::Linear::block_davidson(vector<Wavefunction>& b, DiagonalMatrix& h_diag, double normtol, const bool &warmUp, Davidson_functor& h_multiply, bool& useprecond, int currentRoot, std::vector<Wavefunction> &lowerStates)
{

    pout.precision (12);
#ifndef SERIAL
    mpi::communicator world;
#endif
    int iter = 0;
    double levelshift = 0.0;
    int nroots = b.size();
    //normalise all the guess roots
    if(mpigetrank() == 0) {
        for(int i=0; i<nroots; ++i)
        {
            for(int j=0; j<i; ++j)
            {
                double overlap = DotProduct(b[j], b[i]);
                ScaleAdd(-overlap, b[j], b[i]);
            }
            Normalise(b[i]);
        }


        //if we are doing state specific, lowerstates has lower energy states
        if (lowerStates.size() != 0) {
            for (int i=0; i<lowerStates.size(); i++) {
                double overlap = DotProduct(b[0], lowerStates[i]);
                ScaleAdd(-overlap/DotProduct(lowerStates[i], lowerStates[i]), lowerStates[i], b[0]);
            }
            Normalise(b[0]);
        }

    }
    vector<Wavefunction> sigma;
    int converged_roots = 0;
    int maxiter = h_diag.Ncols() - lowerStates.size();
    while(true)
    {
        if (dmrginp.outputlevel() > 0)
            pout << "\t\t\t Davidson Iteration :: " << iter << endl;

        ++iter;
        dmrginp.hmultiply -> start();

        int sigmasize, bsize;

        if (mpigetrank() == 0) {
            sigmasize = sigma.size();
            bsize = b.size();
        }
#ifndef SERIAL
        mpi::broadcast(world, sigmasize, 0);
        mpi::broadcast(world, bsize, 0);
#endif
        //multiply all guess vectors with hamiltonian c = Hv

        for(int i=sigmasize; i<bsize; ++i) {
            Wavefunction sigmai, bi;
            Wavefunction* sigmaptr=&sigmai, *bptr = &bi;

            if (mpigetrank() == 0) {
                sigma.push_back(b[i]);
                sigma[i].Clear();
                sigmaptr = &sigma[i];
                bptr = &b[i];
            }

#ifndef SERIAL
            mpi::broadcast(world, *bptr, 0);
#endif
            if (mpigetrank() != 0) {
                sigmai = bi;
                sigmai.Clear();
            }

            h_multiply(*bptr, *sigmaptr);
            //if (mpigetrank() == 0) {
            //  cout << *bptr << endl;
            //  cout << *sigmaptr << endl;
            //}
        }
        dmrginp.hmultiply -> stop();

        Wavefunction r;
        DiagonalMatrix subspace_eigenvalues;

        if (mpigetrank() == 0) {
            Matrix subspace_h(b.size(), b.size());
            for (int i = 0; i < b.size(); ++i)
                for (int j = 0; j <= i; ++j) {
                    subspace_h.element(i, j) = DotProduct(b[i], sigma[j]);
                    subspace_h.element(j, i) = subspace_h.element(i, j);
                }

            Matrix alpha;
            diagonalise(subspace_h, subspace_eigenvalues, alpha);

            if (dmrginp.outputlevel() > 0) {
                for (int i = 1; i <= subspace_eigenvalues.Ncols (); ++i)
                    pout << "\t\t\t " << i << " ::  " << subspace_eigenvalues(i,i)+dmrginp.get_coreenergy() << endl;
            }

            //now calculate the ritz vectors which are approximate eigenvectors
            vector<Wavefunction> btmp = b;
            vector<Wavefunction> sigmatmp = sigma;
            for (int i = 0; i < b.size(); ++i)
            {
                Scale(alpha.element(i, i), b[i]);
                Scale(alpha.element(i, i), sigma[i]);
            }
            for (int i = 0; i < b.size(); ++i)
                for (int j = 0; j < b.size(); ++j)
                {
                    if (i != j)
                    {
                        ScaleAdd(alpha.element(i, j), btmp[i], b[j]);
                        ScaleAdd(alpha.element(i, j), sigmatmp[i], sigma[j]);
                    }
                }


            // build residual
            for (int i=0; i<converged_roots; i++) {
                r = sigma[i];
                ScaleAdd(-subspace_eigenvalues(i+1), b[i], r);
                double rnorm = DotProduct(r,r);
                if (rnorm > normtol) {
                    converged_roots = i;
                    if (dmrginp.outputlevel() > 0)
                        pout << "\t\t\t going back to converged root "<<i<<"  "<<rnorm<<" > "<<normtol<<endl;
                    continue;
                }
            }
            r = sigma[converged_roots];
            ScaleAdd(-subspace_eigenvalues(converged_roots+1), b[converged_roots], r);

            if (lowerStates.size() != 0) {
                for (int i=0; i<lowerStates.size(); i++) {
                    double overlap = DotProduct(r, lowerStates[i]);
                    ScaleAdd(-overlap/DotProduct(lowerStates[i], lowerStates[i]), lowerStates[i], r);
                    //ScaleAdd(-overlap, lowerStates[i], r);
                }
            }
        }


        double rnorm;
        if (mpigetrank() == 0)
            rnorm = DotProduct(r,r);

#ifndef SERIAL
        mpi::broadcast(world, converged_roots, 0);
        mpi::broadcast(world, rnorm, 0);
#endif

        if (useprecond && mpigetrank() == 0)
            olsenPrecondition(r, b[converged_roots], subspace_eigenvalues(converged_roots+1), h_diag, levelshift);


        if (dmrginp.outputlevel() > 0)
            pout << "\t \t \t residual :: " << rnorm << endl;
        if (rnorm < normtol)
        {
            if (dmrginp.outputlevel() > 0)
                pout << "\t\t\t Converged root " << converged_roots << endl;

            ++converged_roots;
            if (converged_roots == nroots)
            {
                if (mpigetrank() == 0) {

                    for (int i = 0; i < min((int)(b.size()), h_diag.Ncols()); ++i)
                        h_diag.element(i) = subspace_eigenvalues.element(i);
                }
                break;
            }
        }
        else if (mpigetrank() == 0)
        {
            if(b.size() >= dmrginp.deflation_max_size())
            {
                if (dmrginp.outputlevel() > 0)
                    pout << "\t\t\t Deflating block Davidson...\n";
                b.resize(dmrginp.deflation_min_size());
                sigma.resize(dmrginp.deflation_min_size());
            }
            for (int j = 0; j < b.size(); ++j)
            {
                //Normalize
                double normalization = DotProduct(r, r);
                Scale(1./sqrt(normalization), r);

                double overlap = DotProduct(r, b[j]);
                ScaleAdd(-overlap, b[j], r);
            }

            //if we are doing state specific, lowerstates has lower energy states
            if (lowerStates.size() != 0) {
                for (int i=0; i<lowerStates.size(); i++) {
                    double overlap = DotProduct(r, lowerStates[i]);
                    ScaleAdd(-overlap/DotProduct(lowerStates[i], lowerStates[i]), lowerStates[i], r);
                    //ScaleAdd(-overlap, lowerStates[i], r);
                }
            }
            //double tau2 = DotProduct(r,r);

            Normalise(r);
            b.push_back(r);

        }


    }
}
コード例 #21
0
ファイル: tmt.cpp プロジェクト: Jornason/DieHard
void Clean(DiagonalMatrix& A, Real c)
{
   int nr = A.Nrows();
   for (int i=1; i<=nr; i++)
   { Real a = A(i,i); if ((a < c) && (a > -c)) A(i,i) = 0.0; }
}
コード例 #22
0
ファイル: tmt5.cpp プロジェクト: AsherBond/MondocosmOS
ReturnMatrix Returner7(const GenericMatrix& GM)
{ DiagonalMatrix M = GM*7; M.Release(); return M; }
コード例 #23
0
ファイル: tmte.cpp プロジェクト: Vaa3D/v3d_external
void trymate()
{


    Tracer et("Fourteenth test of Matrix package");
    Tracer::PrintTrace();

    {
        Tracer et1("Stage 1");
        Matrix A(8,5);
        {
            Real   a[] = { 22, 10,  2,  3,  7,
                           14,  7, 10,  0,  8,
                           -1, 13, -1,-11,  3,
                           -3, -2, 13, -2,  4,
                           9,  8,  1, -2,  4,
                           9,  1, -7,  5, -1,
                           2, -6,  6,  5,  1,
                           4,  5,  0, -2,  2
                         };
            int   ai[] = { 22, 10,  2,  3,  7,
                           14,  7, 10,  0,  8,
                           -1, 13, -1,-11,  3,
                           -3, -2, 13, -2,  4,
                           9,  8,  1, -2,  4,
                           9,  1, -7,  5, -1,
                           2, -6,  6,  5,  1,
                           4,  5,  0, -2,  2
                         };
            A << a;

            Matrix AI(8,5);
            AI << ai;
            AI -= A;
            Print(AI);
            int b[] = { 13, -1,-11,
                        -2, 13, -2,
                        8,  1, -2,
                        1, -7,  5
                      };
            Matrix B(8, 5);
            B = 23;
            B.SubMatrix(3,6,2,4) << b;
            AI = A;
            AI.Rows(1,2) = 23;
            AI.Rows(7,8) = 23;
            AI.Column(1) = 23;
            AI.Column(5) = 23;
            AI -= B;
            Print(AI);
        }
        DiagonalMatrix D;
        Matrix U;
        Matrix V;
        int anc = A.Ncols();
        IdentityMatrix I(anc);
        SymmetricMatrix S1;
        S1 << A.t() * A;
        SymmetricMatrix S2;
        S2 << A * A.t();
        Real zero = 0.0;
        SVD(A+zero,D,U,V);
        CheckIsSorted(D);
        DiagonalMatrix D1;
        SVD(A,D1);
        CheckIsSorted(D1);
        D1 -= D;
        Clean(D1,0.000000001);
        Print(D1);
        Matrix W;
        SVD(A, D1, W, W, true, false);
        D1 -= D;
        W -= U;
        Clean(W,0.000000001);
        Print(W);
        Clean(D1,0.000000001);
        Print(D1);
        Matrix WX;
        SVD(A, D1, WX, W, false, true);
        D1 -= D;
        W -= V;
        Clean(W,0.000000001);
        Print(W);
        Clean(D1,0.000000001);
        Print(D1);
        Matrix SU = U.t() * U - I;
        Clean(SU,0.000000001);
        Print(SU);
        Matrix SV = V.t() * V - I;
        Clean(SV,0.000000001);
        Print(SV);
        Matrix B = U * D * V.t() - A;
        Clean(B,0.000000001);
        Print(B);
        D1=0.0;
        SVD(A,D1,A);
        CheckIsSorted(D1);
        A -= U;
        Clean(A,0.000000001);
        Print(A);
        D(1) -= sqrt(1248.0);
        D(2) -= 20;
        D(3) -= sqrt(384.0);
        Clean(D,0.000000001);
        Print(D);

        Jacobi(S1, D, V);
        CheckIsSorted(D, true);
        V = S1 - V * D * V.t();
        Clean(V,0.000000001);
        Print(V);
        D = D.Reverse();
        D(1)-=1248;
        D(2)-=400;
        D(3)-=384;
        Clean(D,0.000000001);
        Print(D);

        Jacobi(S1, D);
        CheckIsSorted(D, true);
        D = D.Reverse();
        D(1)-=1248;
        D(2)-=400;
        D(3)-=384;
        Clean(D,0.000000001);
        Print(D);

        SymmetricMatrix JW(5);
        Jacobi(S1, D, JW);
        CheckIsSorted(D, true);
        D = D.Reverse();
        D(1)-=1248;
        D(2)-=400;
        D(3)-=384;
        Clean(D,0.000000001);
        Print(D);

        Jacobi(S2, D, V);
        CheckIsSorted(D, true);
        V = S2 - V * D * V.t();
        Clean(V,0.000000001);
        Print(V);
        D = D.Reverse();
        D(1)-=1248;
        D(2)-=400;
        D(3)-=384;
        Clean(D,0.000000001);
        Print(D);

        EigenValues(S1, D, V);
        CheckIsSorted(D, true);
        V = S1 - V * D * V.t();
        Clean(V,0.000000001);
        Print(V);
        D(5)-=1248;
        D(4)-=400;
        D(3)-=384;
        Clean(D,0.000000001);
        Print(D);

        EigenValues(S2, D, V);
        CheckIsSorted(D, true);
        V = S2 - V * D * V.t();
        Clean(V,0.000000001);
        Print(V);
        D(8)-=1248;
        D(7)-=400;
        D(6)-=384;
        Clean(D,0.000000001);
        Print(D);

        EigenValues(S1, D);
        CheckIsSorted(D, true);
        D(5)-=1248;
        D(4)-=400;
        D(3)-=384;
        Clean(D,0.000000001);
        Print(D);

        SymmetricMatrix EW(S2);
        EigenValues(S2, D, EW);
        CheckIsSorted(D, true);
        D(8)-=1248;
        D(7)-=400;
        D(6)-=384;
        Clean(D,0.000000001);
        Print(D);

    }

    {
        Tracer et1("Stage 2");
        Matrix A(20,21);
        int i,j;
        for (i=1; i<=20; i++) for (j=1; j<=21; j++)
            {
                if (i>j) A(i,j) = 0;
                else if (i==j) A(i,j) = 21-i;
                else A(i,j) = -1;
            }
        A = A.t();
        SymmetricMatrix S1;
        S1 << A.t() * A;
        SymmetricMatrix S2;
        S2 << A * A.t();
        DiagonalMatrix D;
        Matrix U;
        Matrix V;
        DiagonalMatrix I(A.Ncols());
        I=1.0;
        SVD(A,D,U,V);
        CheckIsSorted(D);
        Matrix SU = U.t() * U - I;
        Clean(SU,0.000000001);
        Print(SU);
        Matrix SV = V.t() * V - I;
        Clean(SV,0.000000001);
        Print(SV);
        Matrix B = U * D * V.t() - A;
        Clean(B,0.000000001);
        Print(B);
        for (i=1; i<=20; i++)  D(i) -= sqrt((22.0-i)*(21.0-i));
        Clean(D,0.000000001);
        Print(D);
        Jacobi(S1, D, V);
        CheckIsSorted(D, true);
        V = S1 - V * D * V.t();
        Clean(V,0.000000001);
        Print(V);
        D = D.Reverse();
        for (i=1; i<=20; i++)  D(i) -= (22-i)*(21-i);
        Clean(D,0.000000001);
        Print(D);
        Jacobi(S2, D, V);
        CheckIsSorted(D, true);
        V = S2 - V * D * V.t();
        Clean(V,0.000000001);
        Print(V);
        D = D.Reverse();
        for (i=1; i<=20; i++)  D(i) -= (22-i)*(21-i);
        Clean(D,0.000000001);
        Print(D);

        EigenValues(S1, D, V);
        CheckIsSorted(D, true);
        V = S1 - V * D * V.t();
        Clean(V,0.000000001);
        Print(V);
        for (i=1; i<=20; i++)  D(i) -= (i+1)*i;
        Clean(D,0.000000001);
        Print(D);
        EigenValues(S2, D, V);
        CheckIsSorted(D, true);
        V = S2 - V * D * V.t();
        Clean(V,0.000000001);
        Print(V);
        for (i=2; i<=21; i++)  D(i) -= (i-1)*i;
        Clean(D,0.000000001);
        Print(D);

        EigenValues(S1, D);
        CheckIsSorted(D, true);
        for (i=1; i<=20; i++)  D(i) -= (i+1)*i;
        Clean(D,0.000000001);
        Print(D);
        EigenValues(S2, D);
        CheckIsSorted(D, true);
        for (i=2; i<=21; i++)  D(i) -= (i-1)*i;
        Clean(D,0.000000001);
        Print(D);
    }

    {
        Tracer et1("Stage 3");
        Matrix A(30,30);
        int i,j;
        for (i=1; i<=30; i++) for (j=1; j<=30; j++)
            {
                if (i>j) A(i,j) = 0;
                else if (i==j) A(i,j) = 1;
                else A(i,j) = -1;
            }
        Real d1 = A.LogDeterminant().Value();
        DiagonalMatrix D;
        Matrix U;
        Matrix V;
        DiagonalMatrix I(A.Ncols());
        I=1.0;
        SVD(A,D,U,V);
        CheckIsSorted(D);
        Matrix SU = U.t() * U - I;
        Clean(SU,0.000000001);
        Print(SU);
        Matrix SV = V.t() * V - I;
        Clean(SV,0.000000001);
        Print(SV);
        Real d2 = D.LogDeterminant().Value();
        Matrix B = U * D * V.t() - A;
        Clean(B,0.000000001);
        Print(B);
        Real d3 = D.LogDeterminant().Value();
        ColumnVector Test(3);
        Test(1) = d1 - 1;
        Test(2) = d2 - 1;
        Test(3) = d3 - 1;
        Clean(Test,0.00000001);
        Print(Test); // only 8 decimal figures
        A.ReSize(2,2);
        Real a = 1.5;
        Real b = 2;
        Real c = 2 * (a*a + b*b);
        A << a << b << a << b;
        I.ReSize(2);
        I=1;
        SVD(A,D,U,V);
        CheckIsSorted(D);
        SU = U.t() * U - I;
        Clean(SU,0.000000001);
        Print(SU);
        SV = V.t() * V - I;
        Clean(SV,0.000000001);
        Print(SV);
        B = U * D * V.t() - A;
        Clean(B,0.000000001);
        Print(B);
        D = D*D;
        SortDescending(D);
        DiagonalMatrix D50(2);
        D50 << c << 0;
        D = D - D50;
        Clean(D,0.000000001);
        Print(D);
        A << a << a << b << b;
        SVD(A,D,U,V);
        CheckIsSorted(D);
        SU = U.t() * U - I;
        Clean(SU,0.000000001);
        Print(SU);
        SV = V.t() * V - I;
        Clean(SV,0.000000001);
        Print(SV);
        B = U * D * V.t() - A;
        Clean(B,0.000000001);
        Print(B);
        D = D*D;
        SortDescending(D);
        D = D - D50;
        Clean(D,0.000000001);
        Print(D);
    }

    {
        Tracer et1("Stage 4");

        // test for bug found by Olof Runborg,
        // Department of Numerical Analysis and Computer Science (NADA),
        // KTH, Stockholm

        Matrix A(22,20);

        A = 0;

        int a=1;

        A(a+0,a+2) = 1;
        A(a+0,a+18) = -1;
        A(a+1,a+9) = 1;
        A(a+1,a+12) = -1;
        A(a+2,a+11) = 1;
        A(a+2,a+12) = -1;
        A(a+3,a+10) = 1;
        A(a+3,a+19) = -1;
        A(a+4,a+16) = 1;
        A(a+4,a+19) = -1;
        A(a+5,a+17) = 1;
        A(a+5,a+18) = -1;
        A(a+6,a+10) = 1;
        A(a+6,a+4) = -1;
        A(a+7,a+3) = 1;
        A(a+7,a+2) = -1;
        A(a+8,a+14) = 1;
        A(a+8,a+15) = -1;
        A(a+9,a+13) = 1;
        A(a+9,a+16) = -1;
        A(a+10,a+8) = 1;
        A(a+10,a+9) = -1;
        A(a+11,a+1) = 1;
        A(a+11,a+15) = -1;
        A(a+12,a+16) = 1;
        A(a+12,a+4) = -1;
        A(a+13,a+6) = 1;
        A(a+13,a+9) = -1;
        A(a+14,a+5) = 1;
        A(a+14,a+4) = -1;
        A(a+15,a+0) = 1;
        A(a+15,a+1) = -1;
        A(a+16,a+14) = 1;
        A(a+16,a+0) = -1;
        A(a+17,a+7) = 1;
        A(a+17,a+6) = -1;
        A(a+18,a+13) = 1;
        A(a+18,a+5) = -1;
        A(a+19,a+7) = 1;
        A(a+19,a+8) = -1;
        A(a+20,a+17) = 1;
        A(a+20,a+3) = -1;
        A(a+21,a+6) = 1;
        A(a+21,a+11) = -1;


        Matrix U, V;
        DiagonalMatrix S;

        SVD(A, S, U, V, true, true);
        CheckIsSorted(S);

        DiagonalMatrix D(20);
        D = 1;

        Matrix tmp = U.t() * U - D;
        Clean(tmp,0.000000001);
        Print(tmp);

        tmp = V.t() * V - D;
        Clean(tmp,0.000000001);
        Print(tmp);

        tmp = U * S * V.t() - A ;
        Clean(tmp,0.000000001);
        Print(tmp);

    }

    {
        Tracer et1("Stage 5");
        Matrix A(10,10);

        A.Row(1)  <<  1.00 <<  0.07 <<  0.05 <<  0.00 <<  0.06
                  <<  0.09 <<  0.03 <<  0.02 <<  0.02 << -0.03;
        A.Row(2)  <<  0.07 <<  1.00 <<  0.05 <<  0.05 << -0.03
                  <<  0.07 <<  0.00 <<  0.07 <<  0.00 <<  0.02;
        A.Row(3)  <<  0.05 <<  0.05 <<  1.00 <<  0.05 <<  0.02
                  <<  0.01 << -0.05 <<  0.04 <<  0.05 << -0.03;
        A.Row(4)  <<  0.00 <<  0.05 <<  0.05 <<  1.00 << -0.05
                  <<  0.04 <<  0.01 <<  0.02 << -0.05 <<  0.00;
        A.Row(5)  <<  0.06 << -0.03 <<  0.02 << -0.05 <<  1.00
                  << -0.03 <<  0.02 << -0.02 <<  0.04 <<  0.00;
        A.Row(6)  <<  0.09 <<  0.07 <<  0.01 <<  0.04 << -0.03
                  <<  1.00 << -0.06 <<  0.08 << -0.02 << -0.10;
        A.Row(7)  <<  0.03 <<  0.00 << -0.05 <<  0.01 <<  0.02
                  << -0.06 <<  1.00 <<  0.09 <<  0.12 << -0.03;
        A.Row(8)  <<  0.02 <<  0.07 <<  0.04 <<  0.02 << -0.02
                  <<  0.08 <<  0.09 <<  1.00 <<  0.00 << -0.02;
        A.Row(9)  <<  0.02 <<  0.00 <<  0.05 << -0.05 <<  0.04
                  << -0.02 <<  0.12 <<  0.00 <<  1.00 <<  0.02;
        A.Row(10) << -0.03 <<  0.02 << -0.03 <<  0.00 <<  0.00
                  << -0.10 << -0.03 << -0.02 <<  0.02 <<  1.00;

        SymmetricMatrix AS;
        AS << A;
        Matrix V;
        DiagonalMatrix D, D1;
        ColumnVector Check(6);
        EigenValues(AS,D,V);
        CheckIsSorted(D, true);
        Check(1) = MaximumAbsoluteValue(A - V * D * V.t());
        DiagonalMatrix I(10);
        I = 1;
        Check(2) = MaximumAbsoluteValue(V * V.t() - I);
        Check(3) = MaximumAbsoluteValue(V.t() * V - I);

        EigenValues(AS, D1);
        CheckIsSorted(D1, true);
        D -= D1;
        Clean(D,0.000000001);
        Print(D);

        Jacobi(AS,D,V);
        Check(4) = MaximumAbsoluteValue(A - V * D * V.t());
        Check(5) = MaximumAbsoluteValue(V * V.t() - I);
        Check(6) = MaximumAbsoluteValue(V.t() * V - I);

        SortAscending(D);
        D -= D1;
        Clean(D,0.000000001);
        Print(D);

        Clean(Check,0.000000001);
        Print(Check);

        // Check loading rows

        SymmetricMatrix B(10);

        B.Row(1)  <<  1.00;
        B.Row(2)  <<  0.07 <<  1.00;
        B.Row(3)  <<  0.05 <<  0.05 <<  1.00;
        B.Row(4)  <<  0.00 <<  0.05 <<  0.05 <<  1.00;
        B.Row(5)  <<  0.06 << -0.03 <<  0.02 << -0.05 <<  1.00;
        B.Row(6)  <<  0.09 <<  0.07 <<  0.01 <<  0.04 << -0.03
                  <<  1.00;
        B.Row(7)  <<  0.03 <<  0.00 << -0.05 <<  0.01 <<  0.02
                  << -0.06 <<  1.00;
        B.Row(8)  <<  0.02 <<  0.07 <<  0.04 <<  0.02 << -0.02
                  <<  0.08 <<  0.09 <<  1.00;
        B.Row(9)  <<  0.02 <<  0.00 <<  0.05 << -0.05 <<  0.04
                  << -0.02 <<  0.12 <<  0.00 <<  1.00;
        B.Row(10) << -0.03 <<  0.02 << -0.03 <<  0.00 <<  0.00
                  << -0.10 << -0.03 << -0.02 <<  0.02 <<  1.00;

        B -= AS;
        Print(B);

    }

    {
        Tracer et1("Stage 6");
        // badly scaled matrix
        Matrix A(9,9);

        A.Row(1) << 1.13324e+012 << 3.68788e+011 << 3.35163e+009
                 << 3.50193e+011 << 1.25335e+011 << 1.02212e+009
                 << 3.16602e+009 << 1.02418e+009 << 9.42959e+006;
        A.Row(2) << 3.68788e+011 << 1.67128e+011 << 1.27449e+009
                 << 1.25335e+011 << 6.05413e+010 << 4.34573e+008
                 << 1.02418e+009 << 4.69192e+008 << 3.61098e+006;
        A.Row(3) << 3.35163e+009 << 1.27449e+009 << 1.25571e+007
                 << 1.02212e+009 << 4.34573e+008 << 3.69769e+006
                 << 9.42959e+006 << 3.61098e+006 << 3.59450e+004;
        A.Row(4) << 3.50193e+011 << 1.25335e+011 << 1.02212e+009
                 << 1.43514e+011 << 5.42310e+010 << 4.15822e+008
                 << 1.23068e+009 << 4.31545e+008 << 3.58714e+006;
        A.Row(5) << 1.25335e+011 << 6.05413e+010 << 4.34573e+008
                 << 5.42310e+010 << 2.76601e+010 << 1.89102e+008
                 << 4.31545e+008 << 2.09778e+008 << 1.51083e+006;
        A.Row(6) << 1.02212e+009 << 4.34573e+008 << 3.69769e+006
                 << 4.15822e+008 << 1.89102e+008 << 1.47143e+006
                 << 3.58714e+006 << 1.51083e+006 << 1.30165e+004;
        A.Row(7) << 3.16602e+009 << 1.02418e+009 << 9.42959e+006
                 << 1.23068e+009 << 4.31545e+008 << 3.58714e+006
                 << 1.12335e+007 << 3.54778e+006 << 3.34311e+004;
        A.Row(8) << 1.02418e+009 << 4.69192e+008 << 3.61098e+006
                 << 4.31545e+008 << 2.09778e+008 << 1.51083e+006
                 << 3.54778e+006 << 1.62552e+006 << 1.25885e+004;
        A.Row(9) << 9.42959e+006 << 3.61098e+006 << 3.59450e+004
                 << 3.58714e+006 << 1.51083e+006 << 1.30165e+004
                 << 3.34311e+004 << 1.25885e+004 << 1.28000e+002;


        SymmetricMatrix AS;
        AS << A;
        Matrix V;
        DiagonalMatrix D, D1;
        ColumnVector Check(6);
        EigenValues(AS,D,V);
        CheckIsSorted(D, true);
        Check(1) = MaximumAbsoluteValue(A - V * D * V.t()) / 100000;
        DiagonalMatrix I(9);
        I = 1;
        Check(2) = MaximumAbsoluteValue(V * V.t() - I);
        Check(3) = MaximumAbsoluteValue(V.t() * V - I);

        EigenValues(AS, D1);
        D -= D1;
        Clean(D,0.001);
        Print(D);

        Jacobi(AS,D,V);
        Check(4) = MaximumAbsoluteValue(A - V * D * V.t()) / 100000;
        Check(5) = MaximumAbsoluteValue(V * V.t() - I);
        Check(6) = MaximumAbsoluteValue(V.t() * V - I);

        SortAscending(D);
        D -= D1;
        Clean(D,0.001);
        Print(D);

        Clean(Check,0.0000001);
        Print(Check);
    }

    {
        Tracer et1("Stage 7");
        // matrix with all singular values close to 1
        Matrix A(8,8);
        A.Row(1)<<-0.4343<<-0.0445<<-0.4582<<-0.1612<<-0.3191<<-0.6784<<0.1068<<0;
        A.Row(2)<<0.5791<<0.5517<<0.2575<<-0.1055<<-0.0437<<-0.5282<<0.0442<<0;
        A.Row(3)<<0.5709<<-0.5179<<-0.3275<<0.2598<<-0.196<<-0.1451<<-0.4143<<0;
        A.Row(4)<<0.2785<<-0.5258<<0.1251<<-0.4382<<0.0514<<-0.0446<<0.6586<<0;
        A.Row(5)<<0.2654<<0.3736<<-0.7436<<-0.0122<<0.0376<<0.3465<<0.3397<<0;
        A.Row(6)<<0.0173<<-0.0056<<-0.1903<<-0.7027<<0.4863<<-0.0199<<-0.4825<<0;
        A.Row(7)<<0.0434<<0.0966<<0.1083<<-0.4576<<-0.7857<<0.3425<<-0.1818<<0;
        A.Row(8)<<0.0<<0.0<<0.0<<0.0<<0.0<<0.0<<0.0<<-1.0;
        Matrix U,V;
        DiagonalMatrix D;
        SVD(A,D,U,V);
        CheckIsSorted(D);
        Matrix B = U * D * V.t() - A;
        Clean(B,0.000000001);
        Print(B);
        DiagonalMatrix I(8);
        I = 1;
        D -= I;
        Clean(D,0.0001);
        Print(D);
        U *= U.t();
        U -= I;
        Clean(U,0.000000001);
        Print(U);
        V *= V.t();
        V -= I;
        Clean(V,0.000000001);
        Print(V);

    }

    {
        Tracer et1("Stage 8");
        // check SortSV functions

        Matrix A(15, 10);
        int i, j;
        for (i = 1; i <= 15; ++i) for (j = 1; j <= 10; ++j)
                A(i, j) = i + j / 1000.0;
        DiagonalMatrix D(10);
        D << 0.2 << 0.5 << 0.1 << 0.7 << 0.8 << 0.3 << 0.4 << 0.7 << 0.9 << 0.6;
        Matrix U = A;
        Matrix V = 10 - 2 * A;
        Matrix Prod = U * D * V.t();

        DiagonalMatrix D2 = D;
        SortDescending(D2);
        DiagonalMatrix D1 = D;
        SortSV(D1, U, V);
        Matrix X = D1 - D2;
        Print(X);
        X = Prod - U * D1 * V.t();
        Clean(X,0.000000001);
        Print(X);
        U = A;
        V = 10 - 2 * A;
        D1 = D;
        SortSV(D1, U);
        X = D1 - D2;
        Print(X);
        D1 = D;
        SortSV(D1, V);
        X = D1 - D2;
        Print(X);
        X = Prod - U * D1 * V.t();
        Clean(X,0.000000001);
        Print(X);

        D2 = D;
        SortAscending(D2);
        U = A;
        V = 10 - 2 * A;
        D1 = D;
        SortSV(D1, U, V, true);
        X = D1 - D2;
        Print(X);
        X = Prod - U * D1 * V.t();
        Clean(X,0.000000001);
        Print(X);
        U = A;
        V = 10 - 2 * A;
        D1 = D;
        SortSV(D1, U, true);
        X = D1 - D2;
        Print(X);
        D1 = D;
        SortSV(D1, V, true);
        X = D1 - D2;
        Print(X);
        X = Prod - U * D1 * V.t();
        Clean(X,0.000000001);
        Print(X);
    }

    {
        Tracer et1("Stage 9");
        // Tom William's example
        Matrix A(10,10);
        Matrix U;
        Matrix V;
        DiagonalMatrix Sigma;
        Real myVals[] =
        {
            1,    1,    1,    1,    1,    1,    1,    1,    1,    1,
            1,    1,    1,    1,    1,    1,    1,    1,    1,    1,
            1,    1,    1,    1,    1,    1,    1,    1,    1,    1,
            1,    1,    1,    1,    1,    1,    1,    1,    1,    1,
            1,    1,    1,    1,    1,    1,    1,    1,    1,    1,
            1,    1,    1,    1,    1,    1,    1,    1,    1,    0,
            1,    1,    1,    1,    1,    1,    1,    1,    1,    0,
            1,    1,    1,    1,    1,    1,    1,    1,    0,    0,
            1,    1,    1,    1,    1,    1,    1,    0,    0,    0,
            1,    1,    1,    1,    1,    0,    0,    0,    0,    0,
        };

        A << myVals;
        SVD(A, Sigma, U, V);
        CheckIsSorted(Sigma);
        A -= U * Sigma * V.t();
        Clean(A, 0.000000001);
        Print(A);
    }



}
コード例 #24
0
ファイル: tmt8.cpp プロジェクト: Hendranus/cvg_ardrone2_ibvs
void trymat8()
{
//   cout << "\nEighth test of Matrix package\n";
   Tracer et("Eighth test of Matrix package");
   Tracer::PrintTrace();

   int i;


   DiagonalMatrix D(6);
   for (i=1;i<=6;i++)  D(i,i)=i*i+i-10;
   DiagonalMatrix D2=D;
   Matrix MD=D;

   DiagonalMatrix D1(6); for (i=1;i<=6;i++) D1(i,i)=-100+i*i*i;
   Matrix MD1=D1;
   Print(Matrix(D*D1-MD*MD1));
   Print(Matrix((-D)*D1+MD*MD1));
   Print(Matrix(D*(-D1)+MD*MD1));
   DiagonalMatrix DX=D;
   {
      Tracer et1("Stage 1");
      DX=(DX+D1)*DX; Print(Matrix(DX-(MD+MD1)*MD));
      DX=D;
      DX=-DX*DX+(DX-(-D1))*((-D1)+DX);
      // Matrix MX = Matrix(MD1);
      // MD1=DX+(MX.t())*(MX.t()); Print(MD1);
      MD1=DX+(Matrix(MD1).t())*(Matrix(MD1).t()); Print(MD1);
      DX=D; DX=DX; DX=D2-DX; Print(DiagonalMatrix(DX));
      DX=D;
   }
   {
      Tracer et1("Stage 2");
      D.Release(2);
      D1=D; D2=D;
      Print(DiagonalMatrix(D1-DX));
      Print(DiagonalMatrix(D2-DX));
      MD1=1.0;
      Print(Matrix(MD1-1.0));
   }
   {
      Tracer et1("Stage 3");
      //GenericMatrix
      LowerTriangularMatrix LT(4);
      LT << 1 << 2 << 3 << 4 << 5 << 6  << 7 << 8 << 9 << 10;
      UpperTriangularMatrix UT = LT.t() * 2.0;
      GenericMatrix GM1 = LT;
      LowerTriangularMatrix LT1 = GM1-LT; Print(LT1);
      GenericMatrix GM2 = GM1; LT1 = GM2; LT1 = LT1-LT; Print(LT1);
      GM2 = GM1; LT1 = GM2; LT1 = LT1-LT; Print(LT1);
      GM2 = GM1*2; LT1 = GM2; LT1 = LT1-LT*2; Print(LT1);
      GM1.Release();
      GM1=GM1; LT1=GM1-LT; Print(LT1); LT1=GM1-LT; Print(LT1);
      GM1.Release();
      GM1=GM1*4; LT1=GM1-LT*4; Print(LT1);
      LT1=GM1-LT*4; Print(LT1); GM1.CleanUp();
      GM1=LT; GM2=UT; GM1=GM1*GM2; Matrix M=GM1; M=M-LT*UT; Print(M);
      Transposer(LT,GM2); LT1 = LT - GM2.t(); Print(LT1);
      GM1=LT; Transposer(GM1,GM2); LT1 = LT - GM2.t(); Print(LT1);
      GM1 = LT; GM1 = GM1 + GM1; LT1 = LT*2-GM1; Print(LT1);
      DiagonalMatrix D; D << LT; GM1 = D; LT1 = GM1; LT1 -= D; Print(LT1);
      UpperTriangularMatrix UT1 = GM1; UT1 -= D; Print(UT1);
   }
   {
      Tracer et1("Stage 4");
      // Another test of SVD
      Matrix M(12,12); M = 0;
      M(1,1) = M(2,2) = M(4,4) = M(6,6) =
         M(7,7) = M(8,8) = M(10,10) = M(12,12) = -1;
      M(1,6) = M(1,12) = -5.601594;
      M(3,6) = M(3,12) = -0.000165;
      M(7,6) = M(7,12) = -0.008294;
      DiagonalMatrix D;
      SVD(M,D);
      SortDescending(D);
      // answer given by matlab
      DiagonalMatrix DX(12);
      DX(1) = 8.0461;
      DX(2) = DX(3) = DX(4) = DX(5) = DX(6) = DX(7) = 1;
      DX(8) = 0.1243;
      DX(9) = DX(10) = DX(11) = DX(12) = 0;
      D -= DX; Clean(D,0.0001); Print(D);
   }
#ifndef DONT_DO_NRIC
   {
      Tracer et1("Stage 5");
      // test numerical recipes in C interface
      DiagonalMatrix D(10);
      D << 1 << 4 << 6 << 2 << 1 << 6 << 4 << 7 << 3 << 1;
      ColumnVector C(10);
      C << 3 << 7 << 5 << 1 << 4 << 2 << 3 << 9 << 1 << 3;
      RowVector R(6);
      R << 2 << 3 << 5 << 7 << 11 << 13;
      nricMatrix M(10, 6);
      DCR( D.nric(), C.nric(), 10, R.nric(), 6, M.nric() );
      M -= D * C * R;  Print(M);

      D.ReSize(5);
      D << 1.25 << 4.75 << 9.5 << 1.25 << 3.75;
      C.ReSize(5);
      C << 1.5 << 7.5 << 4.25 << 0.0 << 7.25;
      R.ReSize(9);
      R << 2.5 << 3.25 << 5.5 << 7 << 11.25 << 13.5 << 0.0 << 1.5 << 3.5;
      Matrix MX = D * C * R;
      M.ReSize(MX);
      DCR( D.nric(), C.nric(), 5, R.nric(), 9, M.nric() );
      M -= MX;  Print(M);
      
      // test swap
      nricMatrix A(3,4); nricMatrix B(4,5);
      A.Row(1) << 2 << 7 << 3 << 6;
      A.Row(2) << 6 << 2 << 5 << 9;
      A.Row(3) << 1 << 0 << 1 << 6;
      B.Row(1) << 2 << 8 << 4 << 5 << 3;
      B.Row(2) << 1 << 7 << 5 << 3 << 9;
      B.Row(3) << 7 << 8 << 2 << 1 << 6;
      B.Row(4) << 5 << 2 << 9 << 0 << 9;
      nricMatrix A1(1,2); nricMatrix B1;
      nricMatrix X(3,5); Matrix X1 = A * B;
      swap(A, A1); swap(B1, B);
      for (int i = 1; i <= 3; ++i) for (int j = 1; j <= 5; ++j)
      {
         X.nric()[i][j] = 0.0;
         for (int k = 1; k <= 4; ++k)
            X.nric()[i][j] += A1.nric()[i][k] * B1.nric()[k][j];
      }
      X1 -= X; Print(X1); 
   }
#endif
   {
      Tracer et1("Stage 6");
      // test dotproduct
      DiagonalMatrix test(5); test = 1;
      ColumnVector C(10);
      C << 3 << 7 << 5 << 1 << 4 << 2 << 3 << 9 << 1 << 3;
      RowVector R(10);
      R << 2 << 3 << 5 << 7 << 11 << 13 << -3 << -4 << 2 << 4;
      test(1) = (R * C).AsScalar() - DotProduct(C, R);
      test(2) = C.SumSquare() - DotProduct(C, C);
      test(3) = 6.0 * (C.t() * R.t()).AsScalar() - DotProduct(2.0 * C, 3.0 * R);
      Matrix MC = C.AsMatrix(2,5), MR = R.AsMatrix(5,2);
      test(4) = DotProduct(MC, MR) - (R * C).AsScalar();
      UpperTriangularMatrix UT(5);
      UT << 3 << 5 << 2 << 1 << 7
              << 1 << 1 << 8 << 2
                   << 7 << 0 << 1
                        << 3 << 5
                             << 6;
      LowerTriangularMatrix LT(5);
      LT << 5
         << 2 << 3
         << 1 << 0 << 7
         << 9 << 8 << 1 << 2
         << 0 << 2 << 1 << 9 << 2;
      test(5) = DotProduct(UT, LT) - Sum(SP(UT, LT));
      Print(test);
      // check row-wise load;
      LowerTriangularMatrix LT1(5);
      LT1.Row(1) << 5;
      LT1.Row(2) << 2   << 3;
      LT1.Row(3) << 1   << 0   << 7;
      LT1.Row(4) << 9   << 8   << 1   << 2;
      LT1.Row(5) << 0   << 2   << 1   << 9   << 2;
      Matrix M = LT1 - LT; Print(M);
      // check solution with identity matrix
      IdentityMatrix IM(5); IM *= 2;
      LinearEquationSolver LES1(IM);
      LowerTriangularMatrix LTX = LES1.i() * LT;
      M = LTX * 2 - LT; Print(M);
      DiagonalMatrix D = IM;
      LinearEquationSolver LES2(IM);
      LTX = LES2.i() * LT;
      M = LTX * 2 - LT; Print(M);
      UpperTriangularMatrix UTX = LES1.i() * UT;
      M = UTX * 2 - UT; Print(M);
      UTX = LES2.i() * UT;
      M = UTX * 2 - UT; Print(M);
   }

   {
      Tracer et1("Stage 7");
      // Some more GenericMatrix stuff with *= |= &=
      // but don't any additional checks
      BandMatrix BM1(6,2,3);
      BM1.Row(1) << 3 << 8 << 4 << 1;
      BM1.Row(2) << 5 << 1 << 9 << 7 << 2;
      BM1.Row(3) << 1 << 0 << 6 << 3 << 1 << 3;
      BM1.Row(4)      << 4 << 2 << 5 << 2 << 4;
      BM1.Row(5)           << 3 << 3 << 9 << 1;
      BM1.Row(6)                << 4 << 2 << 9;
      BandMatrix BM2(6,1,1);
      BM2.Row(1) << 2.5 << 7.5;
      BM2.Row(2) << 1.5 << 3.0 << 8.5;
      BM2.Row(3)        << 6.0 << 6.5 << 7.0;
      BM2.Row(4)               << 2.5 << 2.0 << 8.0;
      BM2.Row(5)                      << 0.5 << 4.5 << 3.5;
      BM2.Row(6)                             << 9.5 << 7.5;
      Matrix RM1 = BM1, RM2 = BM2;
      Matrix X;
      GenericMatrix GRM1 = RM1, GBM1 = BM1, GRM2 = RM2, GBM2 = BM2;
      Matrix Z(6,0); Z = 5; Print(Z);
      GRM1 |= Z; GBM1 |= Z; GRM2 &= Z.t(); GBM2 &= Z.t();
      X = GRM1 - BM1; Print(X); X = GBM1 - BM1; Print(X);
      X = GRM2 - BM2; Print(X); X = GBM2 - BM2; Print(X);

      GRM1 = RM1; GBM1 = BM1; GRM2 = RM2; GBM2 = BM2;
      GRM1 *= GRM2; GBM1 *= GBM2;
      X = GRM1 - BM1 * BM2; Print(X);
      X = RM1 * RM2 - GBM1; Print(X);

      GRM1 = RM1; GBM1 = BM1; GRM2 = RM2; GBM2 = BM2;
      GRM1 *= GBM2; GBM1 *= GRM2;          // Bs and Rs swapped on LHS
      X = GRM1 - BM1 * BM2; Print(X);
      X = RM1 * RM2 - GBM1; Print(X);

      X = BM1.t(); BandMatrix BM1X = BM1.t();
      GRM1 = RM1; X -= GRM1.t(); Print(X); X = BM1X - BM1.t(); Print(X);

      // check that linear equation solver works with Identity Matrix
      IdentityMatrix IM(6); IM *= 2;
      GBM1 = BM1; GBM1 *= 4; GRM1 = RM1; GRM1 *= 4;
      DiagonalMatrix D = IM;
      LinearEquationSolver LES1(D);
      BandMatrix BX;
      BX = LES1.i() * GBM1; BX -= BM1 * 2; X = BX; Print(X);
      LinearEquationSolver LES2(IM);
      BX = LES2.i() * GBM1; BX -= BM1 * 2; X = BX; Print(X);
      BX = D.i() * GBM1; BX -= BM1 * 2; X = BX; Print(X);
      BX = IM.i() * GBM1; BX -= BM1 * 2; X = BX; Print(X);
      BX = IM.i(); BX *= GBM1; BX -= BM1 * 2; X = BX; Print(X);

      // try symmetric band matrices
      SymmetricBandMatrix SBM; SBM << SP(BM1, BM1.t());
      SBM << IM.i() * SBM;
      X = 2 * SBM - SP(RM1, RM1.t()); Print(X);

      // Do this again with more general D
      D << 2.5 << 7.5 << 2 << 5 << 4.5 << 7.5;
      BX = D.i() * BM1; X = BX - D.i() * RM1;
      Clean(X,0.00000001); Print(X);
      BX = D.i(); BX *= BM1; X = BX - D.i() * RM1;
      Clean(X,0.00000001); Print(X);
      SBM << SP(BM1, BM1.t());
      BX = D.i() * SBM; X = BX - D.i() * SP(RM1, RM1.t());
      Clean(X,0.00000001); Print(X);

      // test return
      BX = TestReturn(BM1); X = BX - BM1;
      if (BX.BandWidth() != BM1.BandWidth()) X = 5;
      Print(X);
   }

//   cout << "\nEnd of eighth test\n";
}
コード例 #25
0
ファイル: example.cpp プロジェクト: 151706061/sofa
void test2(Real* y, Real* x1, Real* x2, int nobs, int npred)
{
   cout << "\n\nTest 2 - traditional, OK\n";

   // traditional sum of squares and products method of calculation
   // with subtraction of means - less subject to round-off error
   // than test1

   // make matrix of predictor values
   Matrix X(nobs,npred);

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

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

   // make vector of 1s
   ColumnVector Ones(nobs); Ones = 1.0;

   // calculate means (averages) of x1 and x2 [ .t() takes transpose]
   RowVector M = Ones.t() * X / nobs;

   // and subtract means from x1 and x1
   Matrix XC(nobs,npred);
   XC = X - Ones * M;

   // do the same to Y [use Sum to get sum of elements]
   ColumnVector YC(nobs);
   Real m = Sum(Y) / nobs;  YC = Y - Ones * m;

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

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

   // calculate estimate of constant term
   //    [AsScalar converts 1x1 matrix to Real]
   Real a = m - (M * A).AsScalar();

   // Get variances of estimates from diagonal elements of inverse of SSQ
   //    [ we are taking inverse of SSQ - we need it for finding D ]
   Matrix ISSQ = SSQ.i(); DiagonalMatrix D; D << ISSQ;
   ColumnVector V = D.AsColumn();
   Real v = 1.0/nobs + (M * ISSQ * M.t()).AsScalar();
					    // for calc variance of const

   // 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;
   // make vector of standard errors
   ColumnVector SE(npred);
   for (int i=1; i<=npred; 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";
   cout << setw(9) << setprecision(3) <<
     (X | Y | Fitted | Residual | Hat.AsColumn());
   cout << "\n\n";
}
コード例 #26
0
LP_InteriorPointSolver::Result LP_InteriorPointSolver::Solve()
{
  if (verbose>=1) cout << "Solving LP_InteriorPoint:" << endl;
  
  if (x0.n == 0) {
    if (! FindFeasiblePoint()) {
      if(verbose>=1) cout << "Couldn't find an initial feasible point in LP_InteriorPointSolver::solve()" << endl;
      return Infeasible;
    }
  }
	
  if (verbose>=2) { cout << "x0 = "<<VectorPrinter(x0)<<endl; }
	
  // Make sure starting xopt is always set to x0 when we begin to solve.
  xopt = x0;
    
  // Already checked that the initial point is feasible.
	
  //
  // Algorithm parameters
  //
  
  //	t	- indicates how far the search is along the "central path"; basically,
  //		  it just trades off how much we weight the goal vs. the inequality barriers.
  double t=0.01;
  //	mu	- multiple by which we increase t at each outer iteration.
  double mu=15;
  //	alpha	- ? - in backtracking line search, I think this indicates a goal decrement or something.
  //	beta	- in backtracking line search, how much we back off at each iteration
  double alpha=0.2;
  double beta=0.5;
	
  //
  // Outer Loop
  //

  DiagonalMatrix d;
  Matrix Hsub,H;
  Vector g;
  Vector dx;
  Vector xcur;
  //cout<<"A is "<<endl<<MatrixPrinter(A)<<endl;
  //cout<<"b is "<<VectorPrinter(b)<<endl;
  //getchar();
	
  double gap=1.0;
  int iter_outer=0;
  while (gap>tol_outer) {
    if(verbose >= 2) { cout << "Current xopt = "<<VectorPrinter(xopt)<<endl; }
		
    ++iter_outer;
    if (iter_outer > kMaxIters_Outer) {
      cout << "WARNING: LP_InteriorPoint outer loop did not converge within max iters" << endl;
      return MaxItersReached;
    }
		
    // Update position along the central path
    t *= mu;
		
    if (verbose>=2) cout << " Outer iter " << iter_outer << ", t=" << t << endl;
		
    //
    // Inner Loop
    //
		
    double dec=1.0;
    int iter_inner=0;
    while ((fabs(dec)>tol_inner) && (iter_inner <= kMaxIters_Inner)) {
      ++iter_inner;
      /*
	if (iter_inner > kMaxIters_Inner) {
	cout << "WARNING: LP_InteriorPoint inner loop did not converge within max iters" << endl;
	return MaxItersReached;
	}
      */
			
      if (verbose>=3) cout << "  Inner iter " << iter_inner << ", dec=" << dec << endl;
			
      // (1) Compute the direction to descend
      //		- Newton direction is dx = -(Hessian^-1)*(gradient)
      //		- H = (Aineq'*(diag(d.^2))*Aineq)
      //		- g = (t*f) + (Aineq'*d)
			
      //cout << "Size of Aineq is " << A.m << " x " << A.n << endl;

      //d = (bineq-Aineq*xopt)^-1 component-wise
      A.mul(xopt,d);  d.inplaceNegative();  d += p;
      d.inplacePseudoInverse();
      //Hsub = diag(d)*Aineq
      d.preMultiply(A,Hsub);
      //H = Hsub'*Hsub
      H.mulTransposeA(Hsub,Hsub);		

      //g = Aineq'*d+t*c	
      A.mulTranspose(d,g);
      g.madd(c,t);
      Matrix Htemp=H;
      Vector gtemp=g;
      // Solve the system to find Newton direction
      // Try using a better numerically conditioned matrix
      HConditioner Hinv(Htemp,gtemp);
      //if(!Hinv.Solve_SVD(dx)) {
      if(!Hinv.Solve_Cholesky(dx)) {
	RobustSVD<Real> svd;
	if(svd.set(Hinv.A)) {
	  svd.backSub(Hinv.b,dx);
	  if(verbose >= 1) cout<<"Solved by SVD"<<endl;
	  if(verbose >= 2) {
	    cout<<"Singular values "<<svd.svd.W<<endl;
	    cout<<"b "<<Hinv.b<<endl;
	    getchar();
	  }
	}
	else {
	  if(verbose >= 1) {
	    //cout<<"H Matrix "<<endl; H.print();
	    //cout<<"Scale is "; Hinv.S.print();
	    cout<<"Scaled H Matrix "<<endl<<MatrixPrinter(Hinv.A)<<endl;
	    cout<<"LP: Error performing H^-1*g!"<<endl;
	  }
	  return Error;
	}
      }
      Hinv.Post(dx);
      //getchar();

      dx.inplaceNegative();

      /*			
      // TEMPORARY!!!
      cout << "H" << endl; H.print();
      cout << "d" << endl; d.print();
      cout << "g" << endl; g.print();
      cout << "dx" << endl; dx.print();
      */
			
      // (2) Compute decrement (a scalar) -> dec=abs(-g'*dx)
      dec = dot(g,dx);

      // (3) Line search along dx, using backtracking
      double s=1.0;
      Real obji_xopt = Objective_Ineq(xopt,t);
      //cout<<"Starting search at "<< Objective(xopt)<<", "; xopt.print();
      //cout<<"   direction "; dx.print();
      //can find max on s outside of the loop
      {
	Real smax = 2;
	for(int i=0;i<A.m;i++) {
	  Real r = p(i) - A.dotRow(i,xopt);
	  Real q = A.dotRow(i,dx);
	  Assert(r >= Zero);
	  if(q > Zero)
	    smax = Min(smax,r/q);
	}
	Assert(smax >= 0.0);
	if(smax <= 1.0)
	  s = smax*0.99;
      }

      int iter_backtrack=0;
      bool done_backtrack=false;
      while (! done_backtrack) {
	++iter_backtrack;
	if (verbose>=4) cout << "   Backtrack iter " << iter_backtrack << ", s=" << s << ", dec=" << dec << endl;
	xcur = xopt; xcur.madd(dx,s);
	bool sat_ineq=SatisfiesInequalities(xcur);
	//inequalities will be satisfied unless there's some numerical error
	/*
	if(!sat_ineq) {
	  Real maxViolation=0;
	  for(int i=0;i<A.m;i++) {
	    Real r = b(i) - A.dotRow(i,xcur);
	    maxViolation = Max(maxViolation,-r);
	  }
	  if(!FuzzyZero(maxViolation)) {
	    cout<<"WHAT?!?! s is "<<s<<" on iter "<<iter_backtrack<<endl;
	    cout<<"Maximum violation is "<<maxViolation<<endl;  
	    cout<<"xcur "; xcur.print();
	    cout<<"dx "; dx.print();
	    getchar();
	  }
	}
	*/
	if (sat_ineq && (Objective_Ineq(xcur,t)<=(obji_xopt+(alpha*s*dec)))) {
	  xopt=xcur;
	  done_backtrack = true;
	} else {
	  s *= beta;
	  if(s < 1e-10) {
	    if(verbose >= 2) cout<<"s is really small, breaking"<<endl;
	    done_backtrack = true;
	  }
	}
      }

      if (verbose>=3) cout << "  Inner iter " << iter_inner << ", obj=" << Objective(xopt) << ", s=" << s << ", dec=" << dec << ", gap=" << ((double) p.n) / t << endl;
      //if (verbose>=4) cout << "   Backtrack iter " << iter_backtrack << ", s=" << s << ", dec=" << dec << endl;
			
      /*
	while ((f_objective(x+(s*dx),f,Aineq,bineq,t) > (f_objective(x,f,Aineq,bineq,t)+(alpha*s*g'*dx))) | (max((Aineq*(x+(s*dx)))-bineq) > 0))
	iter_bt = iter_bt + 1;
	if (diagnostics) disp(['   Backtrack iter ' num2str(iter_bt)]); end
	s = beta*s;
	%if (max((A*(x+(s*dx)))>b) > 0)
	%    disp('Error: current iteration of x is not feasible when backtracking.');
	%    return;
	%end
	end
	% (4) Update x with the value obtained from backtracking.
	x = x + (s*dx);
      */
			
      if (!IsInf(objectiveBreak) && (Objective(xopt) < objectiveBreak)) return SubOptimal;
      if(s < 1e-10) break;
    }
    
    gap = ((double) p.n) / t;
  }
   
  if (verbose>=1) cout << "Optimization was successful." << endl;

  // If we were going to stop when the objective is negative, check that the
  // objective actually _became_ negative!
  if (!IsInf(objectiveBreak) && (Objective(xopt) >= objectiveBreak)) {
    if(verbose>=1) cout<<"The max objective is not achieved! "<<Objective(xopt)<<endl;
    return OptimalNoBreak;
  }
  else return Optimal;
}
コード例 #27
0
ファイル: ImplicitUtils.C プロジェクト: 99731/GoTools
//==========================================================================
void make_implicit_svd(vector<vector<double> >& mat,
		       vector<double>& b, double& sigma_min)
//==========================================================================
{
    int rows = (int)mat.size();
    int cols = (int)mat[0].size();
//     cout << "Rows = " << rows << endl
// 	 << "Cols = " << cols << endl;

    Matrix nmat;
    nmat.ReSize(rows, cols);
    for (int i = 0; i < rows; ++i) {
	for (int j = 0; j < cols; ++j) {
	    nmat.element(i, j) = mat[i][j];
	}
    }

    // Check if mat has enough rows. If not fill out with zeros.
    if (rows < cols) {
	RowVector zero(cols);
	zero = 0.0; // Initializes zero to a null-vector.
	for (int i = rows; i < cols; ++i) {
	    nmat &= zero; // & means horizontal concatenation in newmat
	}
    }

    // Perform SVD.
//     cout << "Running SVD..." << endl;
    static DiagonalMatrix diag;
    static Matrix V;
    Try {
	SVD(nmat, diag, nmat, V);
    } CatchAll {
	cout << Exception::what() << endl;
	b = vector<double>(cols, 0.0);
	sigma_min = -1.0;
	return;
    }

//     // Write out singular values.
//     cout << "Singular values:" << endl;
//     for (int ik = 0; ik < cols; ik++)
//  	cout << ik << "\t" << diag.element(ik, ik) << endl;

//     // Write out info about singular values
//     double s_min = diag.element(cols-1, cols-1);
//     double s_max = diag.element(0, 0);
//     cout << "Implicitization:" << endl
// 	 << "s_min = " << s_min << endl
// 	 << "s_max = " << s_max << endl
// 	 << "Ratio of s_min/s_max = " << s_min/s_max << endl;

//     // Find square sum of singular values
//     double sum = 0.0;
//     for (int i = 0; i < cols; i++)
//  	sum += diag.element(i, i) * diag.element(i, i);
//     sum = sqrt(sum);
//     cout << "Square sum = " << sum << endl;

    // Get the appropriate null-vector and corresponding singular value
    const double eps = 1.0e-15;
    double tol = cols * fabs(diag.element(0, 0)) * eps;
    int nullvec = 0;
    for (int i = 0; i < cols-1; ++i) {
	if (fabs(diag.element(i, i)) > tol) {
	    ++nullvec;
	}
    }
    sigma_min = diag.element(nullvec, nullvec);
//     cout << "Null-vector: " << nullvec << endl
// 	 << "sigma_min = " << sigma_min << endl;

    // Set the coefficients
    b.resize(cols);
    for (int jk = 0; jk < cols; ++jk)
	b[jk] = V.element(jk, nullvec);

    return;
}
コード例 #28
0
ファイル: jacobi.cpp プロジェクト: 151706061/Slicer3
void Jacobi(const SymmetricMatrix& X, DiagonalMatrix& D, SymmetricMatrix& A,
   Matrix& V, bool eivec)
{
   Real epsilon = FloatingPointPrecision::Epsilon();
   Tracer et("Jacobi");
   REPORT
   int n = X.Nrows(); DiagonalMatrix B(n), Z(n); D.resize(n); A = X;
   if (eivec) { REPORT V.resize(n,n); D = 1.0; V = D; }
   B << A; D = B; Z = 0.0; A.Inject(Z);
   bool converged = false;
   for (int i=1; i<=50; i++)
   {
      Real sm=0.0; Real* a = A.Store(); int p = A.Storage();
      while (p--) sm += fabs(*a++);            // have previously zeroed diags
      if (sm==0.0) { REPORT converged = true; break; }
      Real tresh = (i<4) ? 0.2 * sm / square(n) : 0.0; a = A.Store();
      for (p = 0; p < n; p++)
      {
         Real* ap1 = a + (p*(p+1))/2;
         Real& zp = Z.element(p); Real& dp = D.element(p);
         for (int q = p+1; q < n; q++)
         {
            Real* ap = ap1; Real* aq = a + (q*(q+1))/2;
            Real& zq = Z.element(q); Real& dq = D.element(q);
            Real& apq = A.element(q,p);
            Real g = 100 * fabs(apq); Real adp = fabs(dp); Real adq = fabs(dq);

            if (i>4 && g < epsilon*adp && g < epsilon*adq) { REPORT apq = 0.0; }
            else if (fabs(apq) > tresh)
            {
               REPORT
               Real t; Real h = dq - dp; Real ah = fabs(h);
               if (g < epsilon*ah) { REPORT t = apq / h; }
               else
               {
                  REPORT
                  Real theta = 0.5 * h / apq;
                  t = 1.0 / ( fabs(theta) + sqrt(1.0 + square(theta)) );
                  if (theta<0.0) { REPORT t = -t; }
               }
               Real c = 1.0 / sqrt(1.0 + square(t)); Real s = t * c;
               Real tau = s / (1.0 + c); h = t * apq;
               zp -= h; zq += h; dp -= h; dq += h; apq = 0.0;
               int j = p;
               while (j--)
               {
                  g = *ap; h = *aq;
                  *ap++ = g-s*(h+g*tau); *aq++ = h+s*(g-h*tau);
               }
               int ip = p+1; j = q-ip; ap += ip++; aq++;
               while (j--)
               {
                  g = *ap; h = *aq;
                  *ap = g-s*(h+g*tau); *aq++ = h+s*(g-h*tau);
                  ap += ip++;
               }
               if (q < n-1)             // last loop is non-empty
               {
                  int iq = q+1; j = n-iq; ap += ip++; aq += iq++;
                  for (;;)
                  {
                     g = *ap; h = *aq;
                     *ap = g-s*(h+g*tau); *aq = h+s*(g-h*tau);
                     if (!(--j)) break;
                     ap += ip++; aq += iq++;
                  }
               }
               if (eivec)
               {
                  REPORT
                  RectMatrixCol VP(V,p); RectMatrixCol VQ(V,q);
                  Rotate(VP, VQ, tau, s);
               }
            }
         }
      }
      B = B + Z; D = B; Z = 0.0;
   }
   if (!converged) Throw(ConvergenceException(X));
   if (eivec) SortSV(D, V, true);
   else SortAscending(D);
}
コード例 #29
0
/*
 * Fits a weighted cubic regression on predictor(s)
 *
 * @param contrast - want to predict this value per snp
 * @param strength - covariate of choice
 * @param weights - weight of data points for this genotype
 * @param Predictor - output, prediction function coefficients
 * @param Predicted - output, predicted contrast per snp
 */
void
FitWeightedCubic(const std::vector<double> &contrast,
                 const std::vector<double> &strength,
                 const std::vector<double> &weights,
                 std::vector<double> &Predictor,
                 std::vector<double> &Predicted) {

  	// Singular value decomposition method
	unsigned int i;
	unsigned int nobs;
	unsigned int npred;
	npred = 3+1;
	nobs= contrast.size();

	// convert double into doubles to match newmat
  vector<Real> tmp_vec(nobs);
  Real* tmp_ptr = &tmp_vec[0];
	vector<Real> obs_vec(nobs);
	Real *obs_ptr = &obs_vec[0];
	vector<Real> weight_vec(nobs);

  Matrix covarMat(nobs,npred);
	ColumnVector observedVec(nobs);

	// fill in the data
	// modified by weights
	for (i=0; i<nobs; i++)
		weight_vec[i] = sqrt(weights[i]);

  	// load data - 1s into col 1 of matrix
	for (i=0; i<nobs; i++)
		tmp_vec[i] = weight_vec[i];
  	covarMat.Column(1) << tmp_ptr;
	for (i=0; i<nobs; i++)
		tmp_vec[i] *= strength[i];
  	covarMat.Column(2) << tmp_ptr;
	for (i=0; i<nobs; i++)
		tmp_vec[i] *= strength[i];
  	covarMat.Column(3) << tmp_ptr;
	for (i=0; i<nobs; i++)
		tmp_vec[i] *= strength[i];
  	covarMat.Column(4) << tmp_ptr;

  	for (i=0; i<nobs; i++)
		obs_vec[i] = contrast[i]*weight_vec[i];
  	observedVec << obs_ptr;

  	// do SVD
  	Matrix U, V;
  	DiagonalMatrix D;
    ColumnVector Fitted(nobs);
    ColumnVector A(npred);

  	SVD(covarMat,D,U,V);

  	Fitted = U.t() * observedVec;
  	A = V * ( D.i() * Fitted );

	// this predicts "0" for low weights
	// because of weighted regression
  	Fitted = U * Fitted;

	// this is the predictor
	Predictor.resize(npred);
	for (i=0; i<npred; i++)
		Predictor[i] = A.element(i);


  // export data back to doubles
	// and therefore this predicts "0" for low-weighted points
	// which is >not< the desired outcome!!!!
	// instead we need to predict all points at once
	// >unweighted< as output
	vector<double> Goofy;
	Predicted.resize(nobs);
  	for (i = 0; i < nobs; ++i) {
      Goofy.resize(npred);
      Goofy[0] = 1;
      Goofy[1] = strength[i];
      Goofy[2] = strength[i]*Goofy[1];
      Goofy[3] = strength[i]*Goofy[2];
      Predicted[i] = vprod(Goofy,Predictor);
  	}
}
コード例 #30
0
ファイル: newmatnl.cpp プロジェクト: 151706061/sofa
void NonLinearLeastSquares::GetHatDiagonal(DiagonalMatrix& Hat) const
{
   Hat.ReSize(n_obs);
   for (int i = 1; i<=n_obs; i++) Hat(i) = X.Row(i).SumSquare();
}