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

void Model::construct_preference_and_covariance(
    Eigen::VectorXd &Eta, Eigen::MatrixXd &Omega, const Eigen::MatrixXd &C,
    const Eigen::MatrixXd &M, const Eigen::VectorXd &W,
    const Eigen::MatrixXd &S, const Parameter &parameter) {

  Eigen::VectorXd Mu = C * M * W;
  Eta = Mu;

  Eigen::MatrixXd Psi = W.asDiagonal();
  Psi -= (W * W.transpose());

  Eigen::MatrixXd I(n_alternatives, n_alternatives);
  I.setIdentity();
  Eigen::MatrixXd Phi = C * M * Psi * M.transpose() * C.transpose() +
                        parameter.sig2 * C * I * C.transpose();

  Omega = Phi;
  double rt;
  Eigen::MatrixXd Si = I;

  for (int i = 2; i <= parameter.stopping_time; i++) {
    rt = 1.0 / (1 + exp(i - 202.0) / 25); // [Takao] Not sure what this is.
    Si = S * Si;
    Eta += rt * Si * Mu;
    Omega += pow(rt, 2) * Si * Phi * Si.transpose();
  }
}

void projectorFromSvd(const Eigen::MatrixXd& jac,
                      Eigen::JacobiSVD<Eigen::MatrixXd>& svd,
                      Eigen::VectorXd& svdSingular,
                      Eigen::MatrixXd& preResult,
                      Eigen::MatrixXd& result,
                      double epsilon=std::numeric_limits<double>::epsilon(),
                      double minTol=1e-8)
{
  // we are force to compute the Full matrix because of
  // the nullspace matrix computation
  svd.compute(jac, Eigen::ComputeFullU | Eigen::ComputeFullV);

  double tolerance =
      epsilon*double(std::max(jac.cols(), jac.rows()))*
      std::abs(svd.singularValues()[0]);
  tolerance = std::max(tolerance, minTol);

  svdSingular.setOnes();
  for(int i = 0; i < svd.singularValues().rows(); ++i)
  {
    svdSingular[i] = svd.singularValues()[i] > tolerance ? 0. : 1.;
  }

  preResult.noalias() = svd.matrixV()*svdSingular.asDiagonal();
  result.noalias() = preResult*svd.matrixV().adjoint();
}

void constMatrix::gradient_add( const Eigen::VectorXd & X, const Eigen::VectorXd & iV)
{
  Eigen::VectorXd vtmp = Q * X;

  double xtQx =  vtmp.dot(iV.asDiagonal() * vtmp);
  dtau +=  (d - xtQx)/ tau;
  ddtau -=  (d + xtQx)/ pow(tau, 2);
}

      Eigen::VectorXd dphi_dq(
        softabs_point& z,
        interface_callbacks::writer::base_writer& info_writer,
        interface_callbacks::writer::base_writer& error_writer) {
          Eigen::VectorXd a
            = z.softabs_lambda_inv.cwiseProduct(z.pseudo_j.diagonal());
          Eigen::MatrixXd A =  a.asDiagonal()
                             * z.eigen_deco.eigenvectors().transpose();
          Eigen::MatrixXd B = z.eigen_deco.eigenvectors() * A;

          stan::math::grad_tr_mat_times_hessian(
            softabs_fun<Model>(this->model_, 0), z.q, B, a);

          return - 0.5 * a + z.g;
      }

      Eigen::VectorXd dtau_dq(
        softabs_point& z,
        interface_callbacks::writer::base_writer& info_writer,
        interface_callbacks::writer::base_writer& error_writer) {
        Eigen::VectorXd a =    z.softabs_lambda_inv.cwiseProduct(
                               z.eigen_deco.eigenvectors().transpose() * z.p);
        Eigen::MatrixXd A =   a.asDiagonal()
                            * z.eigen_deco.eigenvectors().transpose();
        Eigen::MatrixXd B = z.pseudo_j.selfadjointView<Eigen::Lower>() * A;
        Eigen::MatrixXd C = A.transpose() * B;

         Eigen::VectorXd b(z.q.size());
         stan::math::grad_tr_mat_times_hessian(
           softabs_fun<Model>(this->model_, 0), z.q, C, b);

         return 0.5 * b;
      }

int main() {
        int m   =       40;
        int n   =       20;
        
        Eigen::MatrixXd A       =       Eigen::MatrixXd(m,n);
        Eigen::MatrixXd B       =       Eigen::MatrixXd(m,n);
        
        double tolerance        =       1e-14;
        
        Eigen::MatrixXd U, V1, V2;
        Eigen::VectorXd S;

        int rank;

        Compress_Cols(A, B, tolerance, U, S, V1, V2, rank);
        std::cout << (A-U*S.asDiagonal()*V1.transpose()).cwiseAbs().maxCoeff() << std::endl;
        std::cout << (B-U*S.asDiagonal()*V2.transpose()).cwiseAbs().maxCoeff() << std::endl;
}

double polyfit(size_t n, size_t deg, const double* xp, const double* yp,
               const double* wp, double* pp)
{
    ConstMappedVector x(xp, n);
    Eigen::VectorXd y = ConstMappedVector(yp, n);
    MappedVector p(pp, deg+1);

    if (deg >= n) {
        throw CanteraError("polyfit", "Polynomial degree ({}) must be less "
            "than number of input data points ({})", deg, n);
    }

    // Construct A such that each row i of A has the elements
    // 1, x[i], x[i]^2, x[i]^3 ... + x[i]^deg
    Eigen::MatrixXd A(n, deg+1);
    A.col(0).setConstant(1.0);

    if (deg > 0) {
        A.col(1) = x;
    }
    for (size_t i = 1; i < deg; i++) {
        A.col(i+1) = A.col(i).array() * x.array();
    }

    if (wp != nullptr && wp[0] > 0) {
        // For compatibility with old Fortran dpolft, input weights are the
        // squares of the weight vector used in this algorithm
        Eigen::VectorXd w = ConstMappedVector(wp, n).cwiseSqrt().eval();

        // Multiply by the weights on both sides
        A = w.asDiagonal() * A;
        y.array() *= w.array();
    }

    // Solve W*A*p = W*y to find the polynomial coefficients
    p = A.colPivHouseholderQr().solve(y);

    // Evaluate the computed polynomial at the input x coordinates to compute
    // the RMS error as the return value
    return (A*p - y).eval().norm() / sqrt(n);
}

void pseudoInverse(const Eigen::MatrixXd& jac,
                   Eigen::JacobiSVD<Eigen::MatrixXd>& svd,
                   Eigen::VectorXd& svdSingular,
                   Eigen::MatrixXd& prePseudoInv,
                   Eigen::MatrixXd& result,
                   double epsilon=std::numeric_limits<double>::epsilon(),
                   double minTol=1e-8)
{
  svd.compute(jac, Eigen::ComputeThinU | Eigen::ComputeThinV);

  // singular values are sorted in decreasing order
  // so the first one is the max one
  double tolerance =
      epsilon*double(std::max(jac.cols(), jac.rows()))*
      std::abs(svd.singularValues()[0]);
  tolerance = std::max(tolerance, minTol);

  svdSingular = ((svd.singularValues().array().abs() > tolerance).
      select(svd.singularValues().array().inverse(), 0.));

  prePseudoInv.noalias() = svd.matrixV()*svdSingular.asDiagonal();
  result.noalias() = prePseudoInv*svd.matrixU().adjoint();
}

//.........这里部分代码省略.........

    {
        trace.beginBlock("forced oscillations");

        const Z2i::Domain domain(Z2i::Point(0,0), Z2i::Point(50,50));
        const Calculus calculus = CalculusFactory::createFromDigitalSet(generateDiskSet(domain), false);

        const Calculus::DualIdentity0 laplace = calculus.laplace<DUAL>();
        trace.info() << "laplace = " << laplace << endl;

        trace.beginBlock("finding eigen pairs");
        typedef Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> EigenSolverMatrix;
        const EigenSolverMatrix eigen_solver(laplace.myContainer);

        const Eigen::VectorXd laplace_eigen_values = eigen_solver.eigenvalues();
        const Eigen::MatrixXd laplace_eigen_vectors = eigen_solver.eigenvectors();
        trace.info() << "eigen_values_range = " << laplace_eigen_values.minCoeff() << "/" << laplace_eigen_values.maxCoeff() << endl;
        trace.endBlock();

        Calculus::DualForm0 concentration(calculus);
        {
            const Z2i::Point point(25,25);
            const Calculus::Cell cell = calculus.myKSpace.uSpel(point);
            const Calculus::Index index = calculus.getCellIndex(cell);
            concentration.myContainer(index) = 1;
        }

        {
            Board2D board;
            board << domain;
            board << concentration;
            board.saveSVG("propagation_forced_concentration.svg");
        }

        for (int ll=0; ll<6; ll++)
        {
            //! [forced_lambda]
            const Calculus::Scalar lambda = 4*20.75/(1+2*ll);
            //! [forced_lambda]
            trace.info() << "lambda = " << lambda << endl;

            //! [forced_dalembert_eigen]
            const Eigen::VectorXd dalembert_eigen_values = (laplace_eigen_values.array() - (2*M_PI/lambda)*(2*M_PI/lambda)).array().inverse();
            const Eigen::MatrixXd concentration_to_wave = laplace_eigen_vectors * dalembert_eigen_values.asDiagonal() * laplace_eigen_vectors.transpose();
            //! [forced_dalembert_eigen]

            //! [forced_wave]
            Calculus::DualForm0 wave(calculus, concentration_to_wave * concentration.myContainer);
            //! [forced_wave]
            wave.myContainer /= wave.myContainer(calculus.getCellIndex(calculus.myKSpace.uSpel(Z2i::Point(25,25))));

            {
                trace.info() << "saving samples" << endl;
                const Calculus::Properties properties = calculus.getProperties();
                {
                    std::stringstream ss;
                    ss << "propagation_forced_samples_hv_" << ll << ".dat";
                    std::ofstream handle(ss.str().c_str());
                    for (int kk=0; kk<=50; kk++)
                    {
                        const Z2i::Point point_horizontal(kk,25);
                        const Z2i::Point point_vertical(25,kk);
                        const Calculus::Scalar xx = 2 * (kk/50. - .5);
                        handle << xx << " ";
                        handle << sample_dual_0_form<Calculus>(properties, wave, point_horizontal) << " ";
                        handle << sample_dual_0_form<Calculus>(properties, wave, point_vertical) << endl;
                    }
                }

                {
                    std::stringstream ss;
                    ss << "propagation_forced_samples_diag_" << ll << ".dat";
                    std::ofstream handle(ss.str().c_str());
                    for (int kk=0; kk<=50; kk++)
                    {
                        const Z2i::Point point_diag_pos(kk,kk);
                        const Z2i::Point point_diag_neg(kk,50-kk);
                        const Calculus::Scalar xx = 2 * sqrt(2) * (kk/50. - .5);
                        handle << xx << " ";
                        handle << sample_dual_0_form<Calculus>(properties, wave, point_diag_pos) << " ";
                        handle << sample_dual_0_form<Calculus>(properties, wave, point_diag_neg) << endl;
                    }
                }
            }

            {
                std::stringstream ss;
                ss << "propagation_forced_wave_" << ll << ".svg";
                Board2D board;
                board << domain;
                board << CustomStyle("KForm", new KFormStyle2D(-.5, 1));
                board << wave;
                board.saveSVG(ss.str().c_str());
            }
        }

        trace.endBlock();
    }
    trace.endBlock();
}