// 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::ismember_rows( const Eigen::MatrixBase<DerivedA> & A, const Eigen::MatrixBase<DerivedB> & B, Eigen::PlainObjectBase<DerivedIA> & IA, Eigen::PlainObjectBase<DerivedLOCB> & LOCB) { using namespace Eigen; using namespace std; assert(A.cols() == B.cols() && "number of columns must match"); IA.resize(A.rows(),1); IA.setConstant(false); LOCB.resize(A.rows(),1); LOCB.setConstant(-1); // boring base cases if(A.size() == 0) { return; } if(B.size() == 0) { return; } // Get rid of any duplicates DerivedA uA; DerivedB uB; Eigen::Matrix<typename DerivedA::Index,Dynamic,1> uIA,uIuA,uIB,uIuB; unique_rows(A,uA,uIA,uIuA); unique_rows(B,uB,uIB,uIuB); // Sort both DerivedA sA; DerivedB sB; Eigen::Matrix<typename DerivedA::Index,Dynamic,1> sIA,sIB; sortrows(uA,true,sA,sIA); sortrows(uB,true,sB,sIB); Eigen::Matrix<bool,Eigen::Dynamic,1> uF = Eigen::Matrix<bool,Eigen::Dynamic,1>::Zero(sA.size(),1); Eigen::Matrix<typename DerivedLOCB::Scalar, Eigen::Dynamic,1> uLOCB = Eigen::Matrix<typename DerivedLOCB::Scalar,Eigen::Dynamic,1>:: Constant(sA.size(),1,-1); const auto & row_greater_than = [&sA,&sB](const int a, const int b) { for(int c = 0;c<sA.cols();c++) { if(sA(a,c) > sB(b,c)) return true; if(sA(a,c) < sB(b,c)) return false; } return false; }; { int bi = 0; // loop over sA bool past = false; for(int a = 0;a<sA.rows();a++) { assert(bi < sB.rows()); while(!past && row_greater_than(a,bi)) { bi++; past = bi>=sB.rows(); } if(!past && (sA.row(a).array()==sB.row(bi).array()).all() ) { uF(sIA(a)) = true; uLOCB(sIA(a)) = uIB(sIB(bi)); } } } for(int a = 0;a<A.rows();a++) { IA(a) = uF(uIuA(a)); LOCB(a) = uLOCB(uIuA(a)); } }