C++ Matrix::array方法代码示例

    static bool testApply()
    {
        bool b, ret;
        // apply delta:
        Eigen::Matrix<double, 6, 1> delta = Eigen::Matrix<double, 6, 1>::Zero();
        Eigen::Matrix<double, 4, 4> expectedT = Eigen::Matrix<double, 4, 4>::Identity();
        Eigen::Matrix<double, 4, 4> diff;

        SE3<double> pose;
        pose.set( delta );
        delta[ 0 ] = Math::deg2Rad( 1.5 );
        delta[ 1 ] = Math::deg2Rad( 1.1 );
        delta[ 2 ] = Math::deg2Rad( 1.6 );
        delta[ 3 ] = 1;
        delta[ 4 ] = 1;
        delta[ 5 ] = 1;
        pose.apply( delta );

        expectedT( 0, 3 ) = delta[ 3 ];
        expectedT( 1, 3 ) = delta[ 4 ];
        expectedT( 2, 3 ) = delta[ 5 ];

        Eigen::Matrix<double, 3, 1> axis = delta.segment<3>( 0 );
        double angle = axis.norm();	axis /= angle;

        expectedT.block<3, 3>( 0, 0 ) = Eigen::AngleAxis<double>( angle, axis ).toRotationMatrix();
        diff = expectedT - pose.transformation();

        ret = b = ( diff.array().abs().sum() / 12 < 0.001 );

        if( !b ){
            std::cout << expectedT << std::endl;
            std::cout << pose.transformation() << std::endl;
            std::cout << "avg SAD: " << diff.array().abs().sum() / 12 << std::endl;
        }

        pose.apply( -delta );
        expectedT.setIdentity();

        b &= ( ( expectedT - pose.transformation() ).array().abs().sum() / 12 < 0.0001 );
        CVTTEST_PRINT( "apply", b );
        ret &= b;

        return ret;
    }

IGL_INLINE void igl::repdiag(
  const Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> & A,
  const int d,
  Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> & B)
{
  int m = A.rows();
  int n = A.cols();
  B.resize(m*d,n*d);
  B.array() *= 0;
  for(int i = 0;i<d;i++)
  {
    B.block(i*m,i*n,m,n) = A;
  }
}

void SignedDistanceField::calculateSignedDistanceField(const GridMap& gridMap, const std::string& layer,
                                                       const double heightClearance)
{
  data_.clear();
  resolution_ = gridMap.getResolution();
  position_ = gridMap.getPosition();
  size_ = gridMap.getSize();
  Matrix map = gridMap.get(layer); // Copy!

  float minHeight = map.minCoeffOfFinites();
  if (!std::isfinite(minHeight)) minHeight = lowestHeight_;
  float maxHeight = map.maxCoeffOfFinites();
  if (!std::isfinite(maxHeight)) maxHeight = lowestHeight_;

  const float valueForEmptyCells = lowestHeight_; // maxHeight, minHeight (TODO Make this an option).
  for (size_t i = 0; i < map.size(); ++i) {
    if (std::isnan(map(i))) map(i) = valueForEmptyCells;
  }

  // Height range of the signed distance field is higher than the max height.
  maxHeight += heightClearance;

  Matrix sdfElevationAbove = Matrix::Ones(map.rows(), map.cols()) * maxDistance_;
  Matrix sdfLayer = Matrix::Zero(map.rows(), map.cols());
  std::vector<Matrix> sdf;
  zIndexStartHeight_ = minHeight;

  // Calculate signed distance field from bottom.
  for (float h = minHeight; h < maxHeight; h += resolution_) {
    Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> obstacleFreeField = map.array() < h;
    Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> obstacleField = obstacleFreeField.array() < 1;
    Matrix sdfObstacle = getPlanarSignedDistanceField(obstacleField);
    Matrix sdfObstacleFree = getPlanarSignedDistanceField(obstacleFreeField);
    Matrix sdf2d;
    // If 2d sdfObstacleFree calculation failed, neglect this SDF
    // to avoid extreme small distances (-INF).
    if ((sdfObstacleFree.array() >= INF).any()) sdf2d = sdfObstacle;
    else sdf2d = sdfObstacle - sdfObstacleFree;
    sdf2d *= resolution_;
    for (size_t i = 0; i < sdfElevationAbove.size(); ++i) {
      if(sdfElevationAbove(i) == maxDistance_ && map(i) <= h) sdfElevationAbove(i) = h - map(i);
      else if(sdfElevationAbove(i) != maxDistance_ && map(i) <= h) sdfElevationAbove(i) = sdfLayer(i) + resolution_;
      if (sdf2d(i) == 0) sdfLayer(i) = h - map(i);
      else if (sdf2d(i) < 0) sdfLayer(i) = -std::min(fabs(sdf2d(i)), fabs(map(i) - h));
      else sdfLayer(i) = std::min(sdf2d(i), sdfElevationAbove(i));
    }
    data_.push_back(sdfLayer);
  }
}

IGL_INLINE Eigen::Matrix<Scalar,3,1> igl::unproject(
  const    Eigen::Matrix<Scalar,3,1>&  win,
  const    Eigen::Matrix<Scalar,4,4>& model,
  const    Eigen::Matrix<Scalar,4,4>& proj,
  const    Eigen::Matrix<Scalar,4,1>&  viewport)
{
  Eigen::Matrix<Scalar,4,4> Inverse = (proj * model).inverse();

  Eigen::Matrix<Scalar,4,1> tmp;
  tmp << win, 1;
  tmp(0) = (tmp(0) - viewport(0)) / viewport(2);
  tmp(1) = (tmp(1) - viewport(1)) / viewport(3);
  tmp = tmp.array() * 2.0f - 1.0f;

  Eigen::Matrix<Scalar,4,1> obj = Inverse * tmp;
  obj /= obj(3);

  return obj.head(3);
}

Eigen::Matrix<Scalar,3,1> igl::project(
  const    Eigen::Matrix<Scalar,3,1>&  obj,
  const    Eigen::Matrix<Scalar,4,4>& model,
  const    Eigen::Matrix<Scalar,4,4>& proj,
  const    Eigen::Matrix<Scalar,4,1>&  viewport)
{
  Eigen::Matrix<Scalar,4,1> tmp;
  tmp << obj,1;

  tmp = model * tmp;

  tmp = proj * tmp;

  tmp = tmp.array() / tmp(3);
  tmp = tmp.array() * 0.5f + 0.5f;
  tmp(0) = tmp(0) * viewport(2) + viewport(0);
  tmp(1) = tmp(1) * viewport(3) + viewport(1);

  return tmp.head(3);
}

	PointCoordinateMatrices	ConverterPlaneFromTo3d::projectTo3d(Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> image)
	{
		PointCoordinateMatrices reshapedCoordinates;

		//std::cout << "image: " << std::endl << image << std::endl << std::endl;

		//As defined for the example data sets to get the distance in meters Z = depth / 5000 TODO change for kinect data (maybe is the same...)
		Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> z = image.array() / 1;// 5000;
		Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> x = z.cwiseProduct(xAdjustment_);
		Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> y = z.cwiseProduct(yAdjustment_);

		//std::cout << "x: " << std::endl << x << std::endl << std::endl;
		//std::cout << "y: " << std::endl << y << std::endl << std::endl;
		//std::cout << "z: " << std::endl << z << std::endl << std::endl;

		reshapedCoordinates.x = Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(x.data(), 1, image.size());
		reshapedCoordinates.y = Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(y.data(), 1, image.size());
		reshapedCoordinates.z = Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(z.data(), 1, image.size());

		return reshapedCoordinates;
	}

    static bool testHessian()
    {
        Eigen::Matrix<double, 6, 1> delta = Eigen::Matrix<double, 6, 1>::Zero();
        Eigen::Matrix<double, 24, 6> hN, hA;

        SE3<double> pose;
        pose.set( Math::deg2Rad( 10.0 ), Math::deg2Rad( 40.0 ), Math::deg2Rad( -120.0 ), -100.0, 200.0, 300.0 );

        Eigen::Matrix<double, 3, 3> K( Eigen::Matrix<double, 3, 3>::Zero() );
        K( 0, 0 ) = 650.0; K( 0, 2 ) = 320.0;
        K( 1, 1 ) = 650.0; K( 1, 2 ) = 240.0;
        K( 2, 2 ) = 1.0;

        Eigen::Matrix<double, 3, 1> point;
        Eigen::Matrix<double, 3, 1> p, ff, fb, bf, bb, xxf, xxb, hess;
        point[ 0 ] = 16;
        point[ 1 ] = 80;
        point[ 2 ] = 13;

        pose.transform( p, point );

        double h = 0.0001;
        for( size_t i = 0; i < 6; i++ ){
            for( size_t j = 0; j < 6; j++ ){
                delta.setZero();
                if( i == j ){
                    // +
                    delta[ j ] = h;
                    pose.apply( delta );
                    pose.transform( xxf, point );
                    pose.apply( -delta );

                    delta[ j ] = -h;
                    pose.apply( delta );
                    pose.transform( xxb, point );
                    pose.apply( -delta );

                    hess = ( xxb - 2 * p + xxf ) / ( h*h );
                } else {
                    delta[ i ] = h;
                    delta[ j ] = h;
                    pose.apply( delta );
                    pose.transform( ff, point );
                    pose.apply( -delta );

                    delta[ i ] = h;
                    delta[ j ] = -h;
                    pose.apply( delta );
                    pose.transform( fb, point );
                    pose.apply( -delta );

                    delta[ i ] = -h;
                    delta[ j ] =  h;
                    pose.apply( delta );
                    pose.transform( bf, point );
                    pose.apply( -delta );

                    delta[ i ] = -h;
                    delta[ j ] = -h;
                    pose.apply( delta );
                    pose.transform( bb, point );
                    pose.apply( -delta );

                    hess = ( ff - bf - fb + bb ) / ( 4 * h * h );
                }

                hN( 4 * i , j ) = hess[ 0 ];
                hN( 4 * i + 1 , j ) = hess[ 1 ];
                hN( 4 * i + 2 , j ) = hess[ 2 ];
                hN( 4 * i + 3 , j ) = 0.0;
            }
        }

        pose.hessian( hA, p );

        bool b, ret = true;
        Eigen::Matrix<double, 24, 6> jDiff;
        jDiff = hN - hA;
		b = ( jDiff.array().abs().sum() / ( double )( jDiff.rows() * jDiff.cols() ) ) < 0.00001;

        CVTTEST_PRINT( "Pose Hessian", b );
        if( !b ){
            std::cout << "Analytic:\n" << hA << std::endl;
            std::cout << "Numeric:\n" << hN << std::endl;
            std::cout << "Difference:\n" << jDiff << std::endl;
        }
        ret &= b;

        return ret;
    }

    static bool testScreenHessian()
    {
        Eigen::Matrix<double, 6, 1> delta = Eigen::Matrix<double, 6, 1>::Zero();
        Eigen::Matrix<double, 6, 6> shNumericX, shNumericY, shX, shY;


        SE3<double> pose;
        pose.set( Math::deg2Rad( 10.0 ), Math::deg2Rad( 40.0 ), Math::deg2Rad( -120.0 ), -100.0, 200.0, 300.0 );

        Eigen::Matrix<double, 3, 3> K( Eigen::Matrix<double, 3, 3>::Zero() );
        K( 0, 0 ) = 650.0; K( 0, 2 ) = 320.0;
        K( 1, 1 ) = 650.0; K( 1, 2 ) = 240.0;
        K( 2, 2 ) = 1.0;

        Eigen::Matrix<double, 3, 1> point, ptrans;
        Eigen::Matrix<double, 2, 1> sp, ff, fb, bf, bb, xxf, xxb, hess;
        point[ 0 ] = 100; point[ 1 ] = 200; point[ 2 ] = 300;

        // project the point with current parameters
        pose.transform( ptrans, point );
        projectWithCam( sp, ptrans, K );

        double h = 0.001;
        for( size_t i = 0; i < 6; i++ ){
            for( size_t j = 0; j < 6; j++ ){

                if( i == j ){
                    // +
                    delta[ j ] = h;
                    pose.apply( delta );
                    pose.transform( ptrans, point );
                    projectWithCam( xxf, ptrans, K );
                    delta[ j ] = -2 * h;
                    pose.apply( delta );
                    pose.transform( ptrans, point );
                    projectWithCam( xxb, ptrans, K );

                    hess = ( xxb - 2 * sp + xxf ) / ( h*h );

                    // back to start
                    delta[ j ] = h;
                    pose.apply( delta );
                    delta[ j ] = 0;
                } else {
                    delta[ i ] = h;
                    delta[ j ] = h;
                    pose.apply( delta );
                    pose.transform( ptrans, point );
                    projectWithCam( ff, ptrans, K );
                    pose.apply( -delta );

                    delta[ i ] = h;
                    delta[ j ] = -h;
                    pose.apply( delta );
                    pose.transform( ptrans, point );
                    projectWithCam( fb, ptrans, K );
                    pose.apply( -delta );

                    delta[ i ] = -h;
                    delta[ j ] =  h;
                    pose.apply( delta );
                    pose.transform( ptrans, point );
                    projectWithCam( bf, ptrans, K );
                    pose.apply( -delta );

                    delta[ i ] = -h;
                    delta[ j ] = -h;
                    pose.apply( delta );
                    pose.transform( ptrans, point );
                    projectWithCam( bb, ptrans, K );
                    pose.apply( -delta );

                    hess = ( ff - bf - fb + bb ) / ( 4 * h * h );
                    delta.setZero();
                }

                shNumericX( i, j ) = hess[ 0 ];
                shNumericY( i, j ) = hess[ 1 ];

            }
        }

        pose.transform( ptrans, point );
        pose.screenHessian( shX, shY, ptrans, K );

        bool b, ret = true;
        Eigen::Matrix<double, 6, 6> jDiff;
		jDiff = shNumericX - shX;
		b = ( jDiff.array().abs().sum() / ( double )( jDiff.rows() * jDiff.cols() ) ) < 0.0001;

        CVTTEST_PRINT( "Pose ScreenHessian X", b );
        if( !b ){
            std::cout << "Analytic:\n" << shX << std::endl;
            std::cout << "Numeric:\n" << shNumericX << std::endl;
            std::cout << "Difference:\n" << jDiff << std::endl;
        }
        ret &= b;

        jDiff = shNumericY - shY;
		b = ( jDiff.array().abs().sum() / ( double )( jDiff.rows() * jDiff.cols() ) ) < 0.0001;
//.........这里部分代码省略.........

    static bool testScreenJacobian()
    {
        Eigen::Matrix<double, 6, 1> delta = Eigen::Matrix<double, 6, 1>::Zero();
        Eigen::Matrix<double, 2, 6> shNumeric, sh;

        SE3<double> pose;
        pose.set( Math::deg2Rad( 10.0 ), Math::deg2Rad( 40.0 ), Math::deg2Rad( -120.0 ), -100.0, 200.0, 300.0 );

        Eigen::Matrix<double, 3, 3> K( Eigen::Matrix<double, 3, 3>::Zero() );
        K( 0, 0 ) = 650.0; K( 0, 2 ) = 320.0;
        K( 1, 1 ) = 650.0; K( 1, 2 ) = 240.0;
        K( 2, 2 ) = 1.0;

        Eigen::Matrix<double, 3, 1> point, ptrans;
        Eigen::Matrix<double, 2, 1> sp, ff, bb, jac;
        point[ 0 ] = 100; point[ 1 ] = 200; point[ 2 ] = 300;

        // project the point with current parameters
        pose.transform( ptrans, point );
        projectWithCam( sp, ptrans, K );

        double h = 0.001;
        for( size_t i = 0; i < 6; i++ ){
            delta[ i ] = h;
            pose.apply( delta );
            pose.transform( ptrans, point );
            projectWithCam( ff, ptrans, K );
            pose.apply( -delta );

            delta[ i ] = -h;
            pose.apply( delta );
            pose.transform( ptrans, point );
            projectWithCam( bb, ptrans, K );
            pose.apply( -delta );

            jac = ( ff - bb ) / ( 2 * h );
            delta.setZero();

            shNumeric( 0, i ) = jac[ 0 ];
            shNumeric( 1, i ) = jac[ 1 ];

        }

        pose.transform( ptrans, point );
        pose.screenJacobian( sh, ptrans, K );

        bool b, ret = true;
        Eigen::Matrix<double, 2, 6> jDiff;
        jDiff = shNumeric - sh;
		b = ( jDiff.array().abs().sum() / ( double )( jDiff.rows() * jDiff.cols() ) ) < 0.0001;

        CVTTEST_PRINT( "Pose ScreenJacobian", b );
        if( !b ){
            std::cout << "Analytic:\n" << sh << std::endl;
            std::cout << "Numeric:\n" << shNumeric << std::endl;
            std::cout << "Difference:\n" << jDiff << std::endl;
        }
        ret &= b;

        return ret;
    }

IGL_INLINE igl::SolverStatus igl::active_set(
  const Eigen::SparseMatrix<AT>& A,
  const Eigen::PlainObjectBase<DerivedB> & B,
  const Eigen::PlainObjectBase<Derivedknown> & known,
  const Eigen::PlainObjectBase<DerivedY> & Y,
  const Eigen::SparseMatrix<AeqT>& Aeq,
  const Eigen::PlainObjectBase<DerivedBeq> & Beq,
  const Eigen::SparseMatrix<AieqT>& Aieq,
  const Eigen::PlainObjectBase<DerivedBieq> & Bieq,
  const Eigen::PlainObjectBase<Derivedlx> & p_lx,
  const Eigen::PlainObjectBase<Derivedux> & p_ux,
  const igl::active_set_params & params,
  Eigen::PlainObjectBase<DerivedZ> & Z
  )
{
//#define ACTIVE_SET_CPP_DEBUG
#ifdef ACTIVE_SET_CPP_DEBUG
#  warning "ACTIVE_SET_CPP_DEBUG"
#endif
  using namespace Eigen;
  using namespace std;
  SolverStatus ret = SOLVER_STATUS_ERROR;
  const int n = A.rows();
  assert(n == A.cols() && "A must be square");
  // Discard const qualifiers
  //if(B.size() == 0)
  //{
  //  B = Eigen::PlainObjectBase<DerivedB>::Zero(n,1);
  //}
  assert(n == B.rows() && "B.rows() must match A.rows()");
  assert(B.cols() == 1 && "B must be a column vector");
  assert(Y.cols() == 1 && "Y must be a column vector");
  assert((Aeq.size() == 0 && Beq.size() == 0) || Aeq.cols() == n);
  assert((Aeq.size() == 0 && Beq.size() == 0) || Aeq.rows() == Beq.rows());
  assert((Aeq.size() == 0 && Beq.size() == 0) || Beq.cols() == 1);
  assert((Aieq.size() == 0 && Bieq.size() == 0) || Aieq.cols() == n);
  assert((Aieq.size() == 0 && Bieq.size() == 0) || Aieq.rows() == Bieq.rows());
  assert((Aieq.size() == 0 && Bieq.size() == 0) || Bieq.cols() == 1);
  Eigen::Matrix<typename Derivedlx::Scalar,Eigen::Dynamic,1> lx;
  Eigen::Matrix<typename Derivedux::Scalar,Eigen::Dynamic,1> ux;
  if(p_lx.size() == 0)
  {
    lx = Eigen::PlainObjectBase<Derivedlx>::Constant(
      n,1,-numeric_limits<typename Derivedlx::Scalar>::max());
  }else
  {
    lx = p_lx;
  }
  if(ux.size() == 0)
  {
    ux = Eigen::PlainObjectBase<Derivedux>::Constant(
      n,1,numeric_limits<typename Derivedux::Scalar>::max());
  }else
  {
    ux = p_ux;
  }
  assert(lx.rows() == n && "lx must have n rows");
  assert(ux.rows() == n && "ux must have n rows");
  assert(ux.cols() == 1 && "lx must be a column vector");
  assert(lx.cols() == 1 && "ux must be a column vector");
  assert((ux.array()-lx.array()).minCoeff() > 0 && "ux(i) must be > lx(i)");
  if(Z.size() != 0)
  {
    // Initial guess should have correct size
    assert(Z.rows() == n && "Z must have n rows");
    assert(Z.cols() == 1 && "Z must be a column vector");
  }
  assert(known.cols() == 1 && "known must be a column vector");
  // Number of knowns
  const int nk = known.size();

  // Initialize active sets
  typedef int BOOL;
#define TRUE 1
#define FALSE 0
  Matrix<BOOL,Dynamic,1> as_lx = Matrix<BOOL,Dynamic,1>::Constant(n,1,FALSE);
  Matrix<BOOL,Dynamic,1> as_ux = Matrix<BOOL,Dynamic,1>::Constant(n,1,FALSE);
  Matrix<BOOL,Dynamic,1> as_ieq = Matrix<BOOL,Dynamic,1>::Constant(Aieq.rows(),1,FALSE);

  // Keep track of previous Z for comparison
  PlainObjectBase<DerivedZ> old_Z;
  old_Z = PlainObjectBase<DerivedZ>::Constant(
      n,1,numeric_limits<typename DerivedZ::Scalar>::max());

  int iter = 0;
  while(true)
  {
#ifdef ACTIVE_SET_CPP_DEBUG
    cout<<"Iteration: "<<iter<<":"<<endl;
    cout<<"  pre"<<endl;
#endif
    // FIND BREACHES OF CONSTRAINTS
    int new_as_lx = 0;
    int new_as_ux = 0;
    int new_as_ieq = 0;
    if(Z.size() > 0)
    {
      for(int z = 0;z < n;z++)
      {
        if(Z(z) < lx(z))
//.........这里部分代码省略.........

//.........这里部分代码省略.........
  // Iterate over outside
  for(int k=0; k<C.outerSize(); ++k)
  {
    // Iterate over inside
    for(typename Eigen::SparseMatrix<T>::InnerIterator it (C,k); it; ++it)
    {
      has_entry[it.col()] = true;
    }
  }
  for(int i=0;i<(int)has_entry.size();i++)
  {
    if(!has_entry[i])
    {
      // See note above about empty columns in C
      fprintf(stderr,"Error: lu_lagrange() C(:,%d) has no entries\n",i);
      return false;
    }
  }



  // Cholesky factorization of ATA
  //// Eigen fails if you give a full view of the matrix like this:
  //Eigen::SparseLLT<SparseMatrix<T> > ATA_LLT(ATA);
  Eigen::SparseMatrix<T> ATA_LT = ATA.template triangularView<Eigen::Lower>();
  Eigen::SparseLLT<Eigen::SparseMatrix<T> > ATA_LLT(ATA_LT);

  Eigen::SparseMatrix<T> J = ATA_LLT.matrixL();

  //if(!ATA_LLT.succeeded())
  if(!((J*0).eval().nonZeros() == 0))
  {
    fprintf(stderr,"Error: lu_lagrange() failed to factor ATA\n");
    return false;
  }

  if(m == 0)
  {
    // If there are no constraints (C is empty) then LU decomposition is just L
    // and L' from cholesky decomposition
    L = J;
    U = J.transpose();
  }else
  {
    // Construct helper matrix M
    Eigen::SparseMatrix<T> M = C;
    J.template triangularView<Eigen::Lower>().solveInPlace(M);

    // Compute cholesky factorizaiton of M'*M
    Eigen::SparseMatrix<T> MTM = M.transpose() * M;

    Eigen::SparseLLT<Eigen::SparseMatrix<T> > MTM_LLT(MTM.template triangularView<Eigen::Lower>());

    Eigen::SparseMatrix<T> K = MTM_LLT.matrixL();

    //if(!MTM_LLT.succeeded())
    if(!((K*0).eval().nonZeros() == 0))
    {
      fprintf(stderr,"Error: lu_lagrange() failed to factor MTM\n");
      return false;
    }

    // assemble LU decomposition of Q
    Eigen::Matrix<int,Eigen::Dynamic,1> MI;
    Eigen::Matrix<int,Eigen::Dynamic,1> MJ;
    Eigen::Matrix<T,Eigen::Dynamic,1> MV;
    igl::find(M,MI,MJ,MV);

    Eigen::Matrix<int,Eigen::Dynamic,1> KI;
    Eigen::Matrix<int,Eigen::Dynamic,1> KJ;
    Eigen::Matrix<T,Eigen::Dynamic,1> KV;
    igl::find(K,KI,KJ,KV);

    Eigen::Matrix<int,Eigen::Dynamic,1> JI;
    Eigen::Matrix<int,Eigen::Dynamic,1> JJ;
    Eigen::Matrix<T,Eigen::Dynamic,1> JV;
    igl::find(J,JI,JJ,JV);

    int nnz = JV.size()  + MV.size() + KV.size();

    Eigen::Matrix<int,Eigen::Dynamic,1> UI(nnz);
    Eigen::Matrix<int,Eigen::Dynamic,1> UJ(nnz);
    Eigen::Matrix<T,Eigen::Dynamic,1> UV(nnz);
    UI << JJ,                        MI, (KJ.array() + n).matrix();
    UJ << JI, (MJ.array() + n).matrix(), (KI.array() + n).matrix(); 
    UV << JV,                        MV,                     KV*-1;
    igl::sparse(UI,UJ,UV,U);

    Eigen::Matrix<int,Eigen::Dynamic,1> LI(nnz);
    Eigen::Matrix<int,Eigen::Dynamic,1> LJ(nnz);
    Eigen::Matrix<T,Eigen::Dynamic,1> LV(nnz);
    LI << JI, (MJ.array() + n).matrix(), (KI.array() + n).matrix();
    LJ << JJ,                        MI, (KJ.array() + n).matrix(); 
    LV << JV,                        MV,                        KV;
    igl::sparse(LI,LJ,LV,L);
  }

  return true;
  #endif
}

Eigen::Matrix<Scalar, Eigen::Dynamic, 1> squareVector(Eigen::Matrix<Scalar, Eigen::Dynamic, 1> x) {
  return x.array() * x.array();
}

Eigen::Matrix<Scalar, Eigen::Dynamic, ColsAtCompileTime> squareMatrix(Eigen::Matrix<Scalar, Eigen::Dynamic, ColsAtCompileTime> x) {
  return x.array() * x.array();
}