C++ MatrixBase::cols方法代码示例

Eigen::Matrix<typename DerivedA::Scalar, matGradMultMatNumRows(DerivedA::RowsAtCompileTime, DerivedB::ColsAtCompileTime), DerivedDA::ColsAtCompileTime>
matGradMultMat(
    const Eigen::MatrixBase<DerivedA>& A,
    const Eigen::MatrixBase<DerivedB>& B,
    const Eigen::MatrixBase<DerivedDA>& dA,
    const Eigen::MatrixBase<DerivedDB>& dB) {
  assert(dA.cols() == dB.cols());
  const int nq = dA.cols();
  const int nq_at_compile_time = DerivedDA::ColsAtCompileTime;

  Eigen::Matrix<typename DerivedA::Scalar,
  matGradMultMatNumRows(DerivedA::RowsAtCompileTime, DerivedB::ColsAtCompileTime),
  DerivedDA::ColsAtCompileTime> ret(A.rows() * B.cols(), nq);

  for (int col = 0; col < B.cols(); col++) {
    auto block = ret.template block<DerivedA::RowsAtCompileTime, nq_at_compile_time>(col * A.rows(), 0, A.rows(), nq);

    // A * dB part:
    block.noalias() = A * dB.template block<DerivedA::ColsAtCompileTime, nq_at_compile_time>(col * A.cols(), 0, A.cols(), nq);

    for (int row = 0; row < B.rows(); row++) {
      // B * dA part:
      block.noalias() += B(row, col) * dA.template block<DerivedA::RowsAtCompileTime, nq_at_compile_time>(row * A.rows(), 0, A.rows(), nq);
    }
  }
  return ret;

  // much slower and requires eigen/unsupported:
//  return Eigen::kroneckerProduct(Eigen::MatrixXd::Identity(B.cols(), B.cols()), A) * dB + Eigen::kroneckerProduct(B.transpose(), Eigen::MatrixXd::Identity(A.rows(), A.rows())) * dA;
}

IGL_INLINE void igl::cat(
  const int dim,
  const Eigen::MatrixBase<Derived> & A, 
  const Eigen::MatrixBase<Derived> & B,
  MatC & C)
{
  assert(dim == 1 || dim == 2);
  // Special case if B or A is empty
  if(A.size() == 0)
  {
    C = B;
    return;
  }
  if(B.size() == 0)
  {
    C = A;
    return;
  }

  if(dim == 1)
  {
    assert(A.cols() == B.cols());
    C.resize(A.rows()+B.rows(),A.cols());
    C << A,B;
  }else if(dim == 2)
  {
    assert(A.rows() == B.rows());
    C.resize(A.rows(),A.cols()+B.cols());
    C << A,B;
  }else
  {
    fprintf(stderr,"cat.h: Error: Unsupported dimension %d\n",dim);
  }
}

inline Eigen::Matrix<typename Eigen::internal::traits<Derived_T>::Scalar, 3, 3> crossMx(
    Eigen::MatrixBase<Derived_T> const & v)
{
  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived_T>, 3);
  assert((v.cols()==3 && v.rows()==1)||(v.rows()==3 && v.cols()==1));
  return crossMx(v(0, 0), v(1, 0), v(2, 0));
}

void assert_size(const Eigen::MatrixBase<Derived> &X, int rows_expected, int cols_expected)
{
    common_assert_msg_ex(
            X.rows() == rows_expected && X.cols() == cols_expected,
            "matrix [row, col] mismatch" << std::endl << 
                "actual: [" << X.rows() << ", " << X.cols() << "]" << std::endl << 
                "expected: [" << rows_expected << ", " << cols_expected << "]",
            eigen_utilities::assert_error);
}

bool allGreaterEquals(const Eigen::MatrixBase<DerivedA> & A,
                      const Eigen::MatrixBase<DerivedB> & B,
                      double tolerance = math::NUMERICAL_ZERO_DIFFERENCE)
{
  assertion(A.rows() == B.rows(), "Matrices with different number of rows can't be compared.");
  assertion(A.cols() == B.cols(), "Matrices with different number of cols can't be compared.");
  
  return ((A-B).array() >= tolerance).all();
}

    static void run( float a, const Eigen::MatrixBase<Derived2> & A, const Eigen::MatrixBase<Derived1> & x, float  b,  Eigen::MatrixBase<Derived1>  &y) {

        EIGEN_STATIC_ASSERT(sizeof(PREC) == sizeof(typename Derived1::Scalar), YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)

        ASSERTMSG(A.cols() == x.rows() && A.rows() == y.rows() ,"ERROR: Vector/Matrix wrong dimension");

        // b_dev = alpha * A_dev * x_old_dev + beta *b_dev
        //Derived2 C = A;

#if USE_INTEL_BLAS == 1

        CBLAS_ORDER     order;
        CBLAS_TRANSPOSE trans;

        if(Derived1::Flags & Eigen::RowMajorBit) {
            order = CblasRowMajor;
        } else {
            order = CblasColMajor;
        }

        trans = CblasNoTrans;


        mkl_set_dynamic(false);
        mkl_set_num_threads(BLAS_NUM_THREADS);
        //cout << "Threads:" << mkl_get_max_threads();
        cblas_sgemv(order, trans, A.rows(), A.cols(), a, const_cast<double*>(&(A.operator()(0,0))), A.outerStride(), const_cast<double*>(&(x.operator()(0,0))), 1, b, &(y.operator()(0,0)), 1);
        //cblas_dgemm(order,trans,trans, A.rows(), A.cols(), A.cols(), 1.0,  const_cast<double*>(&(A.operator()(0,0))), A.rows(), const_cast<double*>(&(A.operator()(0,0))), A.rows(), 1.0 , &(C.operator()(0,0)), C.rows() );
#else

#if USE_GOTO_BLAS == 1
        /* static DGEMVFunc DGEMV = NULL;
        if (DGEMV == NULL) {
          HINSTANCE hInstLibrary = LoadLibrary("libopenblasp-r0.1alpha2.2.dll");
          DGEMV = (DGEMVFunc)GetProcAddress(hInstLibrary, "DGEMV");
        }*/

        char trans = 'N';
        BLAS_INT idx = 1;
        BLAS_INT m = A.rows();
        BLAS_INT n = A.cols();

        sgemv(&trans, &m, &n, &a, &(A.operator()(0,0)), &m, &(x.operator()(0,0)), &idx, &b, &(y.operator()(0,0)), &idx);

        //   FreeLibrary(hInstLibrary);
#else
        ASSERTMSG(false,"No implementation for BLAS defined!");
#endif

#endif

    }

      void least_squares_eqcon(Eigen::MatrixBase<data1__>  &x, const Eigen::MatrixBase<data2__> &A, const Eigen::MatrixBase<data3__> &b, const Eigen::MatrixBase<data4__> &B, const Eigen::MatrixBase<data5__> &d)
      {
        typedef Eigen::Matrix<typename Eigen::MatrixBase<data4__>::Scalar, Eigen::Dynamic, Eigen::Dynamic> B_matrixD;
        B_matrixD Q, R, temp_B, temp_R;
        typedef Eigen::Matrix<typename Eigen::MatrixBase<data2__>::Scalar, Eigen::Dynamic, Eigen::Dynamic> A_matrixD;
        A_matrixD A1, A2, temp_A;
        typedef Eigen::Matrix<typename Eigen::MatrixBase<data5__>::Scalar, Eigen::Dynamic, Eigen::Dynamic> d_matrixD;
        d_matrixD y;
        typedef Eigen::Matrix<typename Eigen::MatrixBase<data3__>::Scalar, Eigen::Dynamic, Eigen::Dynamic> b_matrixD;
        b_matrixD z, rhs;
        typedef Eigen::Matrix<typename Eigen::MatrixBase<data1__>::Scalar, Eigen::Dynamic, Eigen::Dynamic> x_matrixD;
        x_matrixD x_temp(A.cols(), b.cols());
        typename A_matrixD::Index p(B.rows()), n(A.cols());
  #ifdef DEBUG
        typename A_matrixD::Index m(b.rows());
  #endif

        // build Q and R
        Eigen::HouseholderQR<B_matrixD> qr(B.transpose());
        Q=qr.householderQ();
        temp_B=qr.matrixQR();
        temp_R=Eigen::TriangularView<B_matrixD, Eigen::Upper>(temp_B);
        R=temp_R.topRows(p);
        assert((R.rows()==p) && (R.cols()==p));
        assert((Q.rows()==n) && (Q.cols()==n));

        // build A1 and A2
        temp_A=A*Q;
        A1=temp_A.leftCols(p);
        A2=temp_A.rightCols(n-p);
#ifdef DEBUG
        assert((A1.rows()==m) && (A1.cols()==p));
        assert((A2.rows()==m) && (A2.cols()==n-p));
#endif
        assert(A1.cols()==p);
        assert(A2.cols()==n-p);

        // solve for y
        y=R.transpose().lu().solve(d);

        // setup the unconstrained optimization
        rhs=b-A1*y;
        least_squares_uncon(z, A2, rhs);

        // build the solution
        x_temp.topRows(p)=y;
        x_temp.bottomRows(n-p)=z;
        x=Q*x_temp;
      }

Eigen::Matrix<typename DerivedDA::Scalar, matGradMultNumRows(DerivedDA::RowsAtCompileTime, Derivedb::RowsAtCompileTime), DerivedDA::ColsAtCompileTime>
matGradMult(const Eigen::MatrixBase<DerivedDA>& dA, const Eigen::MatrixBase<Derivedb>& b) {
  const int nq = dA.cols();
  assert(b.cols() == 1);
  assert(dA.rows() % b.rows() == 0);
  const int A_rows = dA.rows() / b.rows();

  Eigen::Matrix<typename DerivedDA::Scalar, matGradMultNumRows(DerivedDA::RowsAtCompileTime, Derivedb::RowsAtCompileTime), DerivedDA::ColsAtCompileTime> ret(A_rows, nq);

  ret.setZero();
  for (int row = 0; row < b.rows(); row++) {
    ret += b(row, 0) * dA.block(row * A_rows, 0, A_rows, nq);
  }
  return ret;
}

         bool CameraModel::_project_extrinsic( const Eigen::MatrixBase<Derived1>& mP3D,
                                               Eigen::MatrixBase<Derived2>& mP2D,
                                               Eigen::MatrixBase<Derived3>& mdP2DE
                                               ) const {
     if( !project( mP3D, mP2D ) ) {
         return false;
     }
     const unsigned int nNum3DDims = 3;
     const unsigned int nNumPoints = mP3D.cols();
     // Compute finite-diff Jacobians
     Derived3 mdP2Dfd( 2, nNumPoints );
     Derived3 mP2Dfd( 2, nNumPoints );
     typename Eigen::internal::traits<Derived1>::Scalar dEps = 1e-4;
     Derived2 mP2DP = mP2D;
     Derived2 mP2DM = mP2D;
     { 
         // Compute extrinsic Jacobian
         for( size_t kk=0; kk<nNum3DDims; kk++ ) {
             Derived1 mP3DP = mP3D;
             Derived1 mP3DM = mP3D;
             mP3DP.row( kk ).array() += dEps;
             mP3DM.row( kk ).array() -= dEps;
             project( mP3DP, mP2DP );
             project( mP3DM, mP2DM );
             Derived2 mdP2Dfd = ( mP2DP - mP2DM ) / (2*dEps);
             for( size_t ll=0; ll<nNumPoints; ll++ ) {
                 mdP2DE.col( nNum3DDims*ll + kk ) = mdP2Dfd.col( ll );
             }
         }
     }
     return true;
 }

bool checkRange(const Eigen::MatrixBase<DerivedA>& mm,
  typename DerivedA::Scalar minVal, typename DerivedA::Scalar maxVal)
{
    assert(minVal < maxVal);

    // Go through each element in the matrix and ensure they are not
    // NaN and fall within the specified min and max values.
    for (int ii = 0; ii < mm.rows(); ii++)
    {
        for (int jj = 0; jj < mm.cols(); jj++)
        {
            if (std::isnan(mm(ii, jj)))
            {
                std::stringstream ss;
                ss << "NaN detected at index (" << ii << ", " << jj << "), returning false!";
                ROS_WARN_STREAM(ss.str());
                return false;
            }
            if (mm(ii, jj) > maxVal || mm(ii, jj) < minVal)
            {
                std::stringstream ss;
                ss << "Value of out range at index (" << ii << ", " << jj << "), returning false.\n"
                     " - value = " << mm(ii, jj) << "\n"
                     " - minVal = " << minVal << "\n"
                     " - maxVal = " << maxVal;
                ROS_WARN_STREAM(ss.str());
                return false;
            }
        }
    }

    return true;
}

Eigen::Matrix<Scalar, HOMOGENEOUS_TRANSFORM_SIZE, DerivedDT::ColsAtCompileTime> dHomogTransInv(
    const Eigen::Transform<Scalar, 3, Eigen::Isometry>& T,
    const Eigen::MatrixBase<DerivedDT>& dT) {
  const int nq_at_compile_time = DerivedDT::ColsAtCompileTime;
  int nq = dT.cols();

  const auto& R = T.linear();
  const auto& p = T.translation();

  std::array<int, 3> rows {0, 1, 2};
  std::array<int, 3> R_cols {0, 1, 2};
  std::array<int, 1> p_cols {3};

  auto dR = getSubMatrixGradient(dT, rows, R_cols, T.Rows);
  auto dp = getSubMatrixGradient(dT, rows, p_cols, T.Rows);

  auto dinvT_R = transposeGrad(dR, R.rows());
  auto dinvT_p = (-R.transpose() * dp - matGradMult(dinvT_R, p)).eval();

  const int numel = HOMOGENEOUS_TRANSFORM_SIZE;
  Eigen::Matrix<Scalar, numel, nq_at_compile_time> ret(numel, nq);
  setSubMatrixGradient(ret, dinvT_R, rows, R_cols, T.Rows);
  setSubMatrixGradient(ret, dinvT_p, rows, p_cols, T.Rows);

  // zero out gradient of elements in last row:
  const int last_row = 3;
  for (int col = 0; col < T.HDim; col++) {
    ret.row(last_row + col * T.Rows).setZero();
  }

  return ret;
}

IGL_INLINE void igl::internal_angles_using_edge_lengths(
  const Eigen::MatrixBase<DerivedL>& L,
  Eigen::PlainObjectBase<DerivedK> & K)
{
  // Note:
  //   Usage of internal_angles_using_squared_edge_lengths() is preferred to internal_angles_using_squared_edge_lengths()
  //   This function is deprecated and probably will be removed in future versions
  typedef typename DerivedL::Index Index;
  assert(L.cols() == 3 && "Edge-lengths should come from triangles");
  const Index m = L.rows();
  K.resize(m,3);
  parallel_for(
    m,
    [&L,&K](const Index f)
    {
      for(size_t d = 0;d<3;d++)
      {
        const auto & s1 = L(f,d);
        const auto & s2 = L(f,(d+1)%3);
        const auto & s3 = L(f,(d+2)%3);
        K(f,d) = acos((s3*s3 + s2*s2 - s1*s1)/(2.*s3*s2));
      }
    },
    1000l);
}

typename Gradient<Eigen::Matrix<typename DerivedR::Scalar, RPY_SIZE, 1>, DerivedDR::ColsAtCompileTime>::type drotmat2rpy(
    const Eigen::MatrixBase<DerivedR>& R,
    const Eigen::MatrixBase<DerivedDR>& dR)
{
  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Eigen::MatrixBase<DerivedR>, SPACE_DIMENSION, SPACE_DIMENSION);
  EIGEN_STATIC_ASSERT(Eigen::MatrixBase<DerivedDR>::RowsAtCompileTime == RotmatSize, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);

  const int nq = dR.cols();
  typedef typename DerivedR::Scalar Scalar;
  typedef typename Gradient<Eigen::Matrix<Scalar, RPY_SIZE, 1>, DerivedDR::ColsAtCompileTime>::type ReturnType;
  ReturnType drpy(RPY_SIZE, nq);

  auto dR11_dq = getSubMatrixGradient(dR, 0, 0, R.rows());
  auto dR21_dq = getSubMatrixGradient(dR, 1, 0, R.rows());
  auto dR31_dq = getSubMatrixGradient(dR, 2, 0, R.rows());
  auto dR32_dq = getSubMatrixGradient(dR, 2, 1, R.rows());
  auto dR33_dq = getSubMatrixGradient(dR, 2, 2, R.rows());

  Scalar sqterm = R(2,1) * R(2,1) + R(2,2) * R(2,2);

  using namespace std;
  // droll_dq
  drpy.row(0) = (R(2, 2) * dR32_dq - R(2, 1) * dR33_dq) / sqterm;

  // dpitch_dq
  Scalar sqrt_sqterm = sqrt(sqterm);
  drpy.row(1) = (-sqrt_sqterm * dR31_dq + R(2, 0) / sqrt_sqterm * (R(2, 1) * dR32_dq + R(2, 2) * dR33_dq)) / (R(2, 0) * R(2, 0) + R(2, 1) * R(2, 1) + R(2, 2) * R(2, 2));

  // dyaw_dq
  sqterm = R(0, 0) * R(0, 0) + R(1, 0) * R(1, 0);
  drpy.row(2) = (R(0, 0) * dR21_dq - R(1, 0) * dR11_dq) / sqterm;
  return drpy;
}

IGL_INLINE void igl::face_areas(
  const Eigen::MatrixBase<DerivedL>& L,
  const typename DerivedL::Scalar doublearea_nan_replacement,
  Eigen::PlainObjectBase<DerivedA>& A)
{
  using namespace Eigen;
  assert(L.cols() == 6);
  const int m = L.rows();
  // (unsigned) face Areas (opposite vertices: 1 2 3 4)
  Matrix<typename DerivedA::Scalar,Dynamic,1> 
    A0(m,1), A1(m,1), A2(m,1), A3(m,1);
  Matrix<typename DerivedA::Scalar,Dynamic,3> 
    L0(m,3), L1(m,3), L2(m,3), L3(m,3);
  L0<<L.col(1),L.col(2),L.col(3);
  L1<<L.col(0),L.col(2),L.col(4);
  L2<<L.col(0),L.col(1),L.col(5);
  L3<<L.col(3),L.col(4),L.col(5);
  doublearea(L0,doublearea_nan_replacement,A0);
  doublearea(L1,doublearea_nan_replacement,A1);
  doublearea(L2,doublearea_nan_replacement,A2);
  doublearea(L3,doublearea_nan_replacement,A3);
  A.resize(m,4);
  A.col(0) = 0.5*A0;
  A.col(1) = 0.5*A1;
  A.col(2) = 0.5*A2;
  A.col(3) = 0.5*A3;
}

         bool CameraModel::_project_intrinsic( const Eigen::MatrixBase<Derived1>& mP3D,
                                               Eigen::MatrixBase<Derived2>& mP2D,
                                               Eigen::MatrixBase<Derived3>& mdP2DI
                                               ) const { 
     if( !project( mP3D, mP2D ) ) {
         return false;
     }
     const unsigned int nNumPoints = mP3D.cols();
     const unsigned int nNumCamParams = get_num_parameters();
     // Compute finite-diff Jacobians
     Derived3 mdP2Dfd( 2, nNumPoints );
     Derived3 mP2Dfd( 2, nNumPoints );
     double dEps = 1e-4;
     Derived2 mP2DP = mP2D;
     Derived2 mP2DM = mP2D;
     { 
         // Compute intrinsic Jacobian
         // Horrible const_cast ... difficult to avoid
         for( size_t kk=0; kk<nNumCamParams; kk++ ) {
             std::string sVarName = parameter_index_to_name( kk );
             double dVal = get<double>( sVarName );
             const_cast<CameraModel*>( this )->set<double>( sVarName, dVal + dEps );
             project( mP3D, mP2DP );
             const_cast<CameraModel*>( this )->set<double>( sVarName, dVal - dEps );
             project( mP3D, mP2DM );
             mdP2Dfd = ( mP2DP - mP2DM ) / (2*dEps);
             const_cast<CameraModel*>( this )->set<double>( sVarName, dVal );
             for( size_t ll=0; ll<nNumPoints; ll++ ) {
                 mdP2DI.col( nNumCamParams*ll + kk ) = mdP2Dfd.col( ll );
             }
         }
     }
     return true;
 }