C++ JacobiSVD::singularValues方法代码示例

bool MatrixXr_pseudoInverse(const MatrixXr &a, MatrixXr &a_pinv, double epsilon) {

    // see : http://en.wikipedia.org/wiki/Moore-Penrose_pseudoinverse#The_general_case_and_the_SVD_method
    if ( a.rows()<a.cols() ) return false;

    // SVD
    Eigen::JacobiSVD<MatrixXr> svdA;
    svdA.compute(a,Eigen::ComputeThinU|Eigen::ComputeThinV);
    MatrixXr vSingular = svdA.singularValues();

    // Build a diagonal matrix with the Inverted Singular values
    // The pseudo inverted singular matrix is easy to compute :
    // is formed by replacing every nonzero entry by its reciprocal (inversing).
    VectorXr vPseudoInvertedSingular(svdA.matrixV().cols(),1);

    for (int iRow =0; iRow<vSingular.rows(); iRow++) {
        if(fabs(vSingular(iRow))<=epsilon) vPseudoInvertedSingular(iRow,0)=0.;
        else vPseudoInvertedSingular(iRow,0)=1./vSingular(iRow);
    }

    // A little optimization here
    MatrixXr mAdjointU = svdA.matrixU().adjoint().block(0,0,vSingular.rows(),svdA.matrixU().adjoint().cols());

    // Pseudo-Inversion : V * S * U'
    a_pinv = (svdA.matrixV() *  vPseudoInvertedSingular.asDiagonal()) * mAdjointU;

    return true;
}

bool pseudoInverse(
    const _Matrix_Type_ &a, _Matrix_Type_ &result,
    double epsilon =
        std::numeric_limits<typename _Matrix_Type_::Scalar>::epsilon()) {
  if (a.rows() < a.cols())
    return false;

  Eigen::JacobiSVD<_Matrix_Type_> svd = a.jacobiSvd();

  typename _Matrix_Type_::Scalar tolerance =
      epsilon * std::max(a.cols(), a.rows()) *
      svd.singularValues().array().abs().maxCoeff();

  result = svd.matrixV() *
           _Matrix_Type_(
               _Matrix_Type_((svd.singularValues().array().abs() > tolerance)
                                 .select(svd.singularValues().array().inverse(),
                                         0)).diagonal()) *
           svd.matrixU().adjoint();
}

int main() {

  std::ifstream file;
  file.open("SVD_benchmark");
  if (!file) 
  {
    CGAL_TRACE_STREAM << "Error loading file!\n";
    return 0;
  }

  int ite = 200000;
  Eigen::JacobiSVD<Eigen::Matrix3d> svd;
  Eigen::Matrix3d u, v, cov, r;         
  Eigen::Vector3d w;   

  int matrix_idx = rand()%200;
  for (int i = 0; i < matrix_idx; i++)
  {
    for (int j = 0; j < 3; j++)
    {
      for (int k = 0; k < 3; k++)
      {
        file >> cov(j, k);
      }
    }
  }


  CGAL::Timer task_timer; 

  CGAL_TRACE_STREAM << "Start SVD decomposition...";
  task_timer.start();
  for (int i = 0; i < ite; i++)
  {
    
    svd.compute( cov, Eigen::ComputeFullU | Eigen::ComputeFullV );
    u = svd.matrixU(); v = svd.matrixV(); w = svd.singularValues();
    r = v*u.transpose();
  }
  task_timer.stop();
  file.close();

  CGAL_TRACE_STREAM << "done: " << task_timer.time() << "s\n";

  return 0;
}

// Derived from code by Yohann Solaro ( http://listengine.tuxfamily.org/lists.tuxfamily.org/eigen/2010/01/msg00187.html )
// see : http://en.wikipedia.org/wiki/Moore-Penrose_pseudoinverse#The_general_case_and_the_SVD_method
Eigen::MatrixXd pinv( const Eigen::MatrixXd &b, double rcond )
{
  // TODO: Figure out why it wants fewer rows than columns
  // if ( a.rows()<a.cols() )
  // return false;
  bool flip = false;
  Eigen::MatrixXd a;
  if( a.rows() < a.cols() )
  {
    a = b.transpose();
    flip = true;
  }
  else
    a = b;
  // SVD
  Eigen::JacobiSVD<Eigen::MatrixXd> svdA;
  svdA.compute( a, Eigen::ComputeFullU | Eigen::ComputeThinV );
  Eigen::JacobiSVD<Eigen::MatrixXd>::SingularValuesType vSingular = svdA.singularValues();
  // Build a diagonal matrix with the Inverted Singular values
  // The pseudo inverted singular matrix is easy to compute :
  // is formed by replacing every nonzero entry by its reciprocal (inversing).
  Eigen::VectorXd vPseudoInvertedSingular( svdA.matrixV().cols() );

  for (int iRow=0; iRow<vSingular.rows(); iRow++)
  {
    if ( fabs(vSingular(iRow)) <= rcond )
    {
      vPseudoInvertedSingular(iRow)=0.;
    }
    else
      vPseudoInvertedSingular(iRow)=1./vSingular(iRow);
  }
  // A little optimization here
  Eigen::MatrixXd mAdjointU = svdA.matrixU().adjoint().block( 0, 0, vSingular.rows(), svdA.matrixU().adjoint().cols() );
  // Pseudo-Inversion : V * S * U'
  Eigen::MatrixXd a_pinv = (svdA.matrixV() * vPseudoInvertedSingular.asDiagonal()) * mAdjointU;
  if( flip )
  {
    a = a.transpose();
    a_pinv = a_pinv.transpose();
  }
  return a_pinv;
}

// typename DerivedA::Scalar
void pseudo_inverse_svd(const Eigen::MatrixBase<DerivedA>& M,
  Eigen::MatrixBase<OutputMatrixType>& Minv,
  typename DerivedA::Scalar epsilon = 1e-6)//std::numeric_limits<typename DerivedA::Scalar>::epsilon())
{
  // CONTROLIT_INFO << "Method called!\n  epsilon = " << epsilon << ", M = \n" << M;

  // Ensure matrix Minv has the correct size.  Its size should be equal to M.transpose().
  assert_msg(M.rows() == Minv.cols(), "Minv has invalid number of columns.  Expected " << M.rows() << " got " << Minv.cols());
  assert_msg(M.cols() == Minv.rows(), "Minv has invalid number of rows.  Expected " << M.cols() << " got " << Minv.rows());

  // According to Eigen documentation, "If the input matrix has inf or nan coefficients, the result of the
  // computation is undefined, but the computation is guaranteed to terminate in finite (and reasonable) time."
  Eigen::JacobiSVD<DerivedA> svd = M.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);

  // Get the max singular value
  typename DerivedA::Scalar maxSingularValue = svd.singularValues().array().abs().maxCoeff();

  // Use Minv to temporarily hold sigma
  Minv.setZero();

  typename DerivedA::Scalar tolerance = 0;

  // Only compute sigma if the max singular value is greater than zero.
  if (maxSingularValue > epsilon)
  {
    tolerance = epsilon * std::max(M.cols(), M.rows()) * maxSingularValue;

    // For each singular value of matrix M's SVD decomposition, check if it is greater than
    // the tolerance value.  If it is, save 1/(singular value) in the sigma vector.
    // Otherwise save zero in the sigma vector.
    DerivedA sigmaVector = DerivedA( (svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0) );
    // DerivedA zeroSVs = DerivedA( (svd.singularValues().array().abs() <= tolerance).select(svd.singularValues().array().inverse(), 0) );

    // CONTROLIT_INFO << "epsilon: " << epsilon << ", std::max(M.cols(), M.rows()): " << std::max(M.cols(), M.rows()) << ", maxSingularValue: " << maxSingularValue << ", tolerance: " << tolerance;
    // CONTROLIT_INFO << "sigmaVector = " << sigmaVector.transpose();
    // CONTROLIT_INFO << "zeroSigmaVector : "<< zeroSVs.transpose();

    Minv.block(0, 0, sigmaVector.rows(), sigmaVector.rows()) = sigmaVector.asDiagonal();
  }

  // Double check to make sure the matrices have the correct dimensions
  assert_msg(svd.matrixV().cols() == Minv.rows(),
    "Matrix dimension mismatch, svd.matrixV().cols() = " << svd.matrixV().cols() << ", Minv.rows() = " << Minv.rows() << ".");
  assert_msg(Minv.cols() == svd.matrixU().adjoint().rows(),
    "Matrix dimension mismatch, Minv.cols() = " << Minv.cols() << ", svd.matrixU().adjoint().rows() = " << svd.matrixU().adjoint().rows() << ".");

  Minv = svd.matrixV() *
         Minv *
         svd.matrixU().adjoint(); // take the transpose of matrix U

  // CONTROLIT_INFO << "Done method call! Minv = " << Minv;

  // typename DerivedA::Scalar errorNorm = std::abs((M * Minv - DerivedA::Identity(M.rows(), Minv.cols())).norm());

  // if (tolerance != 0 && errorNorm > tolerance * 10)
  // {
  //   CONTROLIT_WARN << "Problems computing pseudoinverse.  Perhaps the tolerance is too high?\n"
  //     << "  - epsilon: " << epsilon << "\n"
  //     << "  - tolerance: " << tolerance << "\n"
  //     << "  - maxSingularValue: " << maxSingularValue << "\n"
  //     << "  - errorNorm: " << errorNorm << "\n"
  //     << "  - M:\n" << M << "\n"
  //     << "  - Minv:\n" << Minv;
  // }

  // return errorNorm;
}

IGL_INLINE void igl::min_quad_dense_precompute(
  const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& A,
  const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& Aeq,    
  const bool use_lu_decomposition,
  Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& S)
{
  typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Mat;
        // This threshold seems to matter a lot but I'm not sure how to
        // set it
  const T treshold = igl::FLOAT_EPS;
  //const T treshold = igl::DOUBLE_EPS;

  const int n = A.rows();
  assert(A.cols() == n);
  const int m = Aeq.rows();
  assert(Aeq.cols() == n);

  // Lagrange multipliers method:
  Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> LM(n + m, n + m);
  LM.block(0, 0, n, n) = A;
  LM.block(0, n, n, m) = Aeq.transpose();
  LM.block(n, 0, m, n) = Aeq;
  LM.block(n, n, m, m).setZero();

  Mat LMpinv;
  if(use_lu_decomposition)
  {
    // if LM is close to singular, use at your own risk :)
    LMpinv = LM.inverse();
  }else
  {
    // use SVD
    typedef Eigen::Matrix<T, Eigen::Dynamic, 1> Vec; 
    Vec singValues;
    Eigen::JacobiSVD<Mat> svd;
    svd.compute(LM, Eigen::ComputeFullU | Eigen::ComputeFullV );
    const Mat& u = svd.matrixU();
    const Mat& v = svd.matrixV();
    const Vec& singVals = svd.singularValues();

    Vec pi_singVals(n + m);
    int zeroed = 0;
    for (int i=0; i<n + m; i++)
    {
      T sv = singVals(i, 0);
      assert(sv >= 0);      
                 // printf("sv: %lg ? %lg\n",(double) sv,(double)treshold);
      if (sv > treshold) pi_singVals(i, 0) = T(1) / sv;
      else 
      {
        pi_singVals(i, 0) = T(0);
        zeroed++;
      }
    }

    printf("min_quad_dense_precompute: %i singular values zeroed (threshold = %e)\n", zeroed, treshold);
    Eigen::DiagonalMatrix<T, Eigen::Dynamic> pi_diag(pi_singVals);

    LMpinv = v * pi_diag * u.transpose();
  }
  S = LMpinv.block(0, 0, n, n + m);

  //// debug:
  //mlinit(&g_pEngine);
  //
  //mlsetmatrix(&g_pEngine, "A", A);
  //mlsetmatrix(&g_pEngine, "Aeq", Aeq);
  //mlsetmatrix(&g_pEngine, "LM", LM);
  //mlsetmatrix(&g_pEngine, "u", u);
  //mlsetmatrix(&g_pEngine, "v", v);
  //MatrixXd svMat = singVals;
  //mlsetmatrix(&g_pEngine, "singVals", svMat);
  //mlsetmatrix(&g_pEngine, "LMpinv", LMpinv);
  //mlsetmatrix(&g_pEngine, "S", S);

  //int hu = 1;
}

//.........这里部分代码省略.........
		}
	}
	
	/// STEP #4: Compute new grid velocities

	//Temporary variables for plasticity and force calculation
	//We need one set of variables for each thread that will be running
	eigen_matrix3 def_elastic, def_plastic, energy, svd_u, svd_v;
	Eigen::JacobiSVD<eigen_matrix3, Eigen::NoQRPreconditioner> svd;
	eigen_vector3 svd_e;
	matrix3  HDK_def_plastic, HDK_def_elastic, HDK_energy;
	freal* data_dp = HDK_def_plastic.data();
	freal* data_de = HDK_def_elastic.data();
	freal* data_energy = HDK_energy.data();
	//Map Eigen matrices to HDK matrices
	Eigen::Map<eigen_matrix3> data_dp_map(data_dp);
	Eigen::Map<eigen_matrix3> data_de_map(data_de);
	Eigen::Map<eigen_matrix3> data_energy_map(data_energy);	

	//Compute force at each particle and transfer to Eulerian grid
	//We use "nvel" to hold the grid force, since that variable is not in use
	for (GA_Iterator it(gdp_in->getPointRange()); !it.atEnd(); it.advance()){
		int pid = it.getOffset();
		
		//Apply plasticity to deformation gradient, before computing forces
		//We need to use the Eigen lib to do the SVD; transfer houdini matrices to Eigen matrices
		HDK_def_plastic = p_Fp.get(pid);
		HDK_def_elastic = p_Fe.get(pid);
		def_plastic = Eigen::Map<eigen_matrix3>(data_dp);
		def_elastic = Eigen::Map<eigen_matrix3>(data_de);
		
		//Compute singular value decomposition (uev*)
		svd.compute(def_elastic, Eigen::ComputeFullV | Eigen::ComputeFullU);
		svd_e = svd.singularValues();
		svd_u = svd.matrixU();
		svd_v = svd.matrixV();
		//Clamp singular values
		for (int i=0; i<3; i++){
			if (svd_e[i] < CRIT_COMPRESS) 
				svd_e[i] = CRIT_COMPRESS;
			else if (svd_e[i] > CRIT_STRETCH)
				svd_e[i] = CRIT_STRETCH;
		}
		//Put SVD back together for new elastic and plastic gradients
		def_plastic = svd_v * svd_e.asDiagonal().inverse() * svd_u.transpose() * def_elastic * def_plastic;
		svd_v.transposeInPlace();
		def_elastic = svd_u * svd_e.asDiagonal() * svd_v;
		
		//Now compute the energy partial derivative (which we use to get force at each grid node)
		energy = 2*mu*(def_elastic - svd_u*svd_v)*def_elastic.transpose();
		//Je is the determinant of def_elastic (equivalent to svd_e.prod())
		freal Je = svd_e.prod(),
			contour = lambda*Je*(Je-1),
			jp = def_plastic.determinant(),
			particle_vol = p_volume.get(pid);
		for (int i=0; i<3; i++)
			energy(i,i) += contour;
		energy *=  particle_vol * exp(HARDENING*(1-jp));
		
		//Transfer Eigen matrices back to HDK
		data_dp_map = def_plastic;
		data_de_map = def_elastic;
		data_energy_map = energy;
		
		p_Fp.set(pid,HDK_def_plastic);
		p_Fe.set(pid,HDK_def_elastic);