int main(){
    DerivedA A;
    DerivedB B;
    
    A.foo();
    B.foo();

    Base *bA = new DerivedA;
    Base *bB = new DerivedB;
    bA->foo();
    bB->foo();

    delete bA;
    delete bB;

    return 0;
}
// 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;
}
Example #3
0
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));
  }
}