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

IGL_INLINE void igl::per_face_normals(
  const Eigen::MatrixBase<DerivedV>& V,
  const Eigen::MatrixBase<DerivedF>& F,
  const Eigen::MatrixBase<DerivedZ> & Z,
  Eigen::PlainObjectBase<DerivedN> & N)
{
  N.resize(F.rows(),3);
  // loop over faces
  int Frows = F.rows();
#pragma omp parallel for if (Frows>10000)
  for(int i = 0; i < Frows;i++)
  {
    const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> v1 = V.row(F(i,1)) - V.row(F(i,0));
    const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> v2 = V.row(F(i,2)) - V.row(F(i,0));
    N.row(i) = v1.cross(v2);//.normalized();
    typename DerivedV::Scalar r = N.row(i).norm();
    if(r == 0)
    {
      N.row(i) = Z;
    }else
    {
      N.row(i) /= r;
    }
  }
}

 void jacobian(const double t, const std::vector<double>& y,
               Eigen::MatrixBase<Derived1>& dy_dt,
               Eigen::MatrixBase<Derived2>& Jy,
               Eigen::MatrixBase<Derived2>& Jtheta) const {
   using Eigen::Dynamic;
   using Eigen::Map;
   using Eigen::Matrix;
   using Eigen::RowVectorXd;
   using stan::math::var;
   using std::vector;
   vector<double> grad(y.size() + theta_.size());
   Map<RowVectorXd> grad_eig(&grad[0], y.size() + theta_.size());
   try {
     start_nested();
     vector<var> y_var(y.begin(), y.end());
     vector<var> theta_var(theta_.begin(), theta_.end());
     vector<var> z_var;
     z_var.reserve(y.size() + theta_.size());
     z_var.insert(z_var.end(), y_var.begin(), y_var.end());
     z_var.insert(z_var.end(), theta_var.begin(), theta_var.end());
     vector<var> dy_dt_var = f_(t, y_var, theta_var, x_, x_int_, msgs_);
     for (size_t i = 0; i < dy_dt_var.size(); ++i) {
       dy_dt(i) = dy_dt_var[i].val();
       set_zero_all_adjoints_nested();
       dy_dt_var[i].grad(z_var, grad);
       Jy.row(i) = grad_eig.leftCols(y.size());
       Jtheta.row(i) = grad_eig.rightCols(theta_.size());
     }
   } catch (const std::exception& e) {
     recover_memory_nested();
     throw;
   }
   stan::math::recover_memory_nested();
 }

IGL_INLINE void igl::per_face_normals_stable(
  const Eigen::MatrixBase<DerivedV>& V,
  const Eigen::MatrixBase<DerivedF>& F,
  Eigen::PlainObjectBase<DerivedN> & N)
{
  using namespace Eigen;
  typedef Matrix<typename DerivedV::Scalar,1,3> RowVectorV3;
  typedef typename DerivedV::Scalar Scalar;

  const size_t m = F.rows();

  N.resize(F.rows(),3);
  // Grad all points
  for(size_t f = 0;f<m;f++)
  {
    const RowVectorV3 p0 = V.row(F(f,0));
    const RowVectorV3 p1 = V.row(F(f,1));
    const RowVectorV3 p2 = V.row(F(f,2));
    const RowVectorV3 n0 = (p1 - p0).cross(p2 - p0);
    const RowVectorV3 n1 = (p2 - p1).cross(p0 - p1);
    const RowVectorV3 n2 = (p0 - p2).cross(p1 - p2);

    // careful sum
    for(int d = 0;d<3;d++)
    {
      // This is a little _silly_ in terms of complexity, but its recursive
      // implementation is clean looking...
      const std::function<Scalar(Scalar,Scalar,Scalar)> sum3 =
        [&sum3](Scalar a, Scalar b, Scalar c)->Scalar
      {
        if(fabs(c)>fabs(a))
        {
          return sum3(c,b,a);
        }
        // c < a
        if(fabs(c)>fabs(b))
        {
          return sum3(a,c,b);
        }
        // c < a, c < b
        if(fabs(b)>fabs(a))
        {
          return sum3(b,a,c);
        }
        return (a+b)+c;
      };

      N(f,d) = sum3(n0(d),n1(d),n2(d));
    }
    // sum better not be sure, or else NaN
    N.row(f) /= N.row(f).norm();
  }

}

IGL_INLINE void igl::internal_angles(
  const Eigen::MatrixBase<DerivedV>& V,
  const Eigen::MatrixBase<DerivedF>& F,
  Eigen::PlainObjectBase<DerivedK> & K)
{
  using namespace Eigen;
  using namespace std;
  typedef typename DerivedV::Scalar Scalar;
  if(F.cols() == 3)
  {
    // Edge lengths
    Matrix<
      Scalar,
      DerivedF::RowsAtCompileTime,
      DerivedF::ColsAtCompileTime> L_sq;
      igl::squared_edge_lengths(V,F,L_sq);

    assert(F.cols() == 3 && "F should contain triangles");
    igl::internal_angles_using_squared_edge_lengths(L_sq,K);
  }else
  {
    assert(V.cols() == 3 && "If F contains non-triangle facets, V must be 3D");
    K.resizeLike(F);
    auto corner = [](
      const typename DerivedV::ConstRowXpr & x, 
      const typename DerivedV::ConstRowXpr & y, 
      const typename DerivedV::ConstRowXpr & z)
    {
      typedef Eigen::Matrix<Scalar,1,3> RowVector3S;
      RowVector3S v1 = (x-y).normalized();
      RowVector3S v2 = (z-y).normalized();
      // http://stackoverflow.com/questions/10133957/signed-angle-between-two-vectors-without-a-reference-plane
      Scalar s = v1.cross(v2).norm();
      Scalar c = v1.dot(v2);
      return atan2(s, c);
    };
    for(unsigned i=0; i<F.rows(); ++i)
    {
      for(unsigned j=0; j<F.cols(); ++j)
      {
        K(i,j) = corner(
            V.row(F(i,int(j-1+F.cols())%F.cols())),
            V.row(F(i,j)),
            V.row(F(i,(j+1+F.cols())%F.cols()))
            );
      }
    }
  }
}

IGL_INLINE void igl::project_to_line_segment(
  const Eigen::MatrixBase<DerivedP> & P,
  const Eigen::MatrixBase<DerivedS> & S,
  const Eigen::MatrixBase<DerivedD> & D,
  Eigen::PlainObjectBase<Derivedt> & t,
  Eigen::PlainObjectBase<DerivedsqrD> & sqrD)
{
  project_to_line(P,S,D,t,sqrD);
  const int np = P.rows();
  // loop over points and fix those that projected beyond endpoints
#pragma omp parallel for if (np>10000)
  for(int p = 0;p<np;p++)
  {
    const DerivedP Pp = P.row(p);
    if(t(p)<0)
    {
      sqrD(p) = (Pp-S).squaredNorm();
      t(p) = 0;
    }else if(t(p)>1)
    {
      sqrD(p) = (Pp-D).squaredNorm();
      t(p) = 1;
    }
  }
}

void angularvel2quatdotMatrix(const Eigen::MatrixBase<DerivedQ>& q,
    Eigen::MatrixBase<DerivedM>& M,
    typename Gradient<DerivedM, QUAT_SIZE, 1>::type* dM)
{
  // note: not normalizing to match MATLAB implementation
  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<DerivedQ>, QUAT_SIZE);
  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Eigen::MatrixBase<DerivedM>, QUAT_SIZE, SPACE_DIMENSION);
  M.row(0) << -q(1), -q(2), -q(3);
  M.row(1) << q(0), q(3), -q(2);
  M.row(2) << -q(3), q(0), q(1);
  M.row(3) << q(2), -q(1), q(0);
  M *= 0.5;

  if (dM) {
  (*dM) << 0.0, -0.5, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.5, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, -0.5, 0.0, 0.0, 0.0, 0.0, 0.5, 0.5, 0.0, 0.0, 0.0, 0.0, -0.5, 0.0, 0.0, 0.0, 0.0, 0.0, -0.5, 0.0, 0.0, -0.5, 0.0, 0.0, 0.5, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0;
 }
}

IGL_INLINE void igl::average_onto_vertices(const Eigen::MatrixBase<DerivedV> &V,
  const Eigen::MatrixBase<DerivedF> &F,
  const Eigen::MatrixBase<DerivedS> &S,
  Eigen::MatrixBase<DerivedS> &SV)
{
  SV = DerivedS::Zero(V.rows(),S.cols());
  Eigen::Matrix<typename DerivedF::Scalar,Eigen::Dynamic,1> COUNT(V.rows());
  COUNT.setZero();
  for (int i = 0; i <F.rows(); ++i)
  {
    for (int j = 0; j<F.cols(); ++j)
    {
      SV.row(F(i,j)) += S.row(i);
      COUNT[F(i,j)] ++;
    }
  }
  for (int i = 0; i <V.rows(); ++i)
    SV.row(i) /= COUNT[i];
};

void doSomeRankPreservingOperations(Eigen::MatrixBase<Derived>& m)
{
  typedef typename Derived::RealScalar RealScalar;
  for(int a = 0; a < 3*(m.rows()+m.cols()); a++)
  {
    RealScalar d = Eigen::ei_random<RealScalar>(-1,1);
    int i = Eigen::ei_random<int>(0,m.rows()-1); // i is a random row number
    int j;
    do {
      j = Eigen::ei_random<int>(0,m.rows()-1);
    } while (i==j); // j is another one (must be different)
    m.row(i) += d * m.row(j);

    i = Eigen::ei_random<int>(0,m.cols()-1); // i is a random column number
    do {
      j = Eigen::ei_random<int>(0,m.cols()-1);
    } while (i==j); // j is another one (must be different)
    m.col(i) += d * m.col(j);
  }
}

void setSubMatrixGradient(Eigen::MatrixBase<DerivedA>& dM, const Eigen::MatrixBase<DerivedB>& dM_submatrix,
    const std::vector<int>& rows, const std::vector<int>& cols, typename DerivedA::Index M_rows, typename DerivedA::Index q_start, typename DerivedA::Index q_subvector_size) {
  if (q_subvector_size < 0) {
    q_subvector_size = dM.cols() - q_start;
  }
  int index = 0;
  for (typename std::vector<int>::const_iterator col = cols.begin(); col != cols.end(); ++col) {
    for (typename std::vector<int>::const_iterator row = rows.begin(); row != rows.end(); ++row) {
      dM.block((*row) + (*col) * M_rows, q_start, 1, q_subvector_size) = dM_submatrix.row(index++);
    }
  }
}

void setSubMatrixGradient(Eigen::MatrixBase<DerivedA>& dM, const Eigen::MatrixBase<DerivedB>& dM_submatrix,
    const std::vector<int>& rows, const std::vector<int>& cols, int M_rows, int q_start, int q_subvector_size) {
  if (q_subvector_size < 0) {
    q_subvector_size = dM.cols() - q_start;
  }
  int index = 0;
  for (int col : cols) {
    for (int row : rows) {
      dM.block(row + col * M_rows, q_start, 1, q_subvector_size) = dM_submatrix.row(index++);
    }
  }
}

IGL_INLINE void igl::normalize_row_sums(
  const Eigen::MatrixBase<DerivedA>& A,
  Eigen::MatrixBase<DerivedB> & B)
{
#ifndef NDEBUG
  // loop over rows
  for(int i = 0; i < A.rows();i++)
  {
    typename DerivedB::Scalar sum = A.row(i).sum();
    assert(sum != 0);
  }
#endif
  B = (A.array().colwise() / A.rowwise().sum().array()).eval();
}

typename Derived::PlainObject transposeGrad(
    const Eigen::MatrixBase<Derived>& dX, int rows_X)
{
  typename Derived::PlainObject dX_transpose(dX.rows(), dX.cols());
  int numel = dX.rows();
  int index = 0;
  for (int i = 0; i < numel; i++) {
    dX_transpose.row(i) = dX.row(index);
    index += rows_X;
    if (index >= numel) {
      index = (index % numel) + 1;
    }
  }
  return dX_transpose;
}

IGL_INLINE void igl::barycenter(
    const Eigen::MatrixBase<DerivedV> & V,
    const Eigen::MatrixBase<DerivedF> & F,
    Eigen::PlainObjectBase<DerivedBC> & BC)
{
  BC.setZero(F.rows(),V.cols());
  // Loop over faces
  for(int i = 0;i<F.rows();i++)
  {
    // loop around face
    for(int j = 0;j<F.cols();j++)
    {
      // Accumulate
      BC.row(i) += V.row(F(i,j));
    }
    // average
    BC.row(i) /= double(F.cols());
  }
}

    void filter(const Eigen::MatrixBase<Derived>& data,
            const std::vector<bool>& to_keep,
            Derived& out) {
        const size_t num_data = data.rows();
        const size_t dim = data.cols();
        assert(num_data == to_keep.size());

        size_t num_kept= 0;
        for (size_t i=0; i<num_data; i++) {
            if (to_keep[i]) num_kept++;
        }

        out.resize(num_kept, dim);
        size_t count=0;
        for (size_t i=0; i<num_data; i++) {
            if (to_keep[i]) {
                out.row(count) = data.row(i);
                count++;
            }
        }
    }

        bool CameraModel::_lift( const Eigen::MatrixBase<Derived1>& mP2D,
                                 Eigen::MatrixBase<Derived2>& mP3D ) const {
        typedef typename Eigen::internal::traits<Derived2>::Scalar ScalarType;
        const unsigned int nNumPoints = mP2D.cols();
#if 0
        typedef Eigen::Matrix<ScalarType,1,Eigen::Dynamic> RowVect;
        const int nWidth = 1000;
        const int nHeight = 1000;
        const int nWidthSize = nWidth/10;
        const int nHeightSize = nHeight/10;
        Derived1 mP2DUndist( 3, nWidthSize*nHeightSize );
        Derived1 m1 = RowVect::LinSpaced( nWidthSize, 0, nWidth-1 ).replicate( nHeightSize, 1 );
        mP2DUndist.row( 0 ) = Eigen::Map<Derived1>( m1.data(), 1, nWidthSize*nHeightSize );
        mP2DUndist.row( 1 ) = RowVect::LinSpaced( nHeightSize, 0, nHeight-1 ).replicate( 1, nWidthSize );
        mP2DUndist.row( 2 ) = RowVect::Ones( 1, nWidthSize*nHeightSize );
        Derived1 mP2DDist( 2, nWidthSize*nHeightSize );
        project( mP2DUndist, mP2DDist );

        // Initialise with closest values
        for( int ii=0; ii<mP2D.cols(); ii++ ) {
            //Derived1::Index nMinIndex;
            int nMinIndex;
            Derived1 mDiff = mP2DDist;
            mDiff.colwise() -= mP2D.col( ii );
            mDiff.colwise().norm().minCoeff( &nMinIndex );
            mP3D.col( ii ) = mP2DUndist.col( nMinIndex );
        }
#else
        // Start from distortionless points if possible
        if( get( "fx" ) != "" && get( "fy" ) != "" && 
            get( "cx" ) != "" && get( "cy" ) != "" ) {
            double fx = get<double>( "fx" ); double fy = get<double>( "fy" );
            double cx = get<double>( "cx" ); double cy = get<double>( "cy" );
            mP3D.row( 0 ) = ( mP2D.row( 0 ) - cx*Derived2::Ones( 1, nNumPoints ) ) / fx;
            mP3D.row( 1 ) = ( mP2D.row( 1 ) - cy*Derived2::Ones( 1, nNumPoints ) ) / fy;
            mP3D.row( 2 ) = Derived2::Ones( 1, nNumPoints );            
        }
        else {
            mP3D.row( 0 ) = mP2D.row( 0 );
            mP3D.row( 1 ) = mP2D.row( 1 );
            mP3D.row( 2 ) = Derived2::Ones( 1, nNumPoints );
        }
#endif
        for( size_t nIndex=0; nIndex<nNumPoints; nIndex++ ) {
            mP3D.col( nIndex ) /= mP3D.col( nIndex ).norm();
        }

        Derived1 mP2DEst( 2, nNumPoints );
        Derived2 mdP2DE( 2, 3*nNumPoints );
        Derived2 mdP2DI( 2, get_num_parameters()*nNumPoints );

        double dMaxErr = 0.001;
        double dLastMaxErr = 10;
        const unsigned int nMaxIter = 30;
        const unsigned int nMaxOuterIter = 5;
        for( size_t nOuterIter=0; nOuterIter<nMaxOuterIter; nOuterIter++ ) {
            for( size_t nIter=0; nIter<nMaxIter; nIter++ ) {
                project( mP3D, mP2DEst, mdP2DE, mdP2DI );
                for( size_t nIndex=0; nIndex<nNumPoints; nIndex++ ) {
                    Eigen::Matrix<ScalarType,2,3> mJ = mdP2DE.block( 0, 3*nIndex, 2, 3 );
                    Eigen::Matrix<ScalarType,3,3> mJtJ = mJ.transpose()*mJ + 0.1*Eigen::Matrix<ScalarType,3,3>::Identity();
                    Eigen::Matrix<ScalarType,3,1> mJte = mJ.transpose() * ( mP2DEst.col( nIndex ) - mP2D.col( nIndex ) );
                    mP3D.col( nIndex ) -= mJtJ.ldlt().solve( mJte );
                
                    double dErr = ( mP2DEst.col( nIndex ) - mP2D.col( nIndex ) ).norm();
                    if( nIndex > 0 ) {
                        if( std::isnan( dErr ) ) {
                            mP3D.col( nIndex ) =  mP3D.col( nIndex-1 );
                        }
                    }
                    mP3D.col( nIndex ) /= mP3D.col( nIndex ).norm();
                }
                dLastMaxErr = ( mP2DEst - mP2D ).colwise().norm().maxCoeff();
                if( dLastMaxErr < dMaxErr ) {
                    break;
                }
            }

            if( dLastMaxErr >= dMaxErr ) {
                Derived1 mErrors = ( mP2DEst - mP2D ).colwise().norm();
                for( int ii=0; ii<mErrors.cols(); ii++ ) {
                    if( mErrors(0,ii) > dMaxErr ) {
                        // Find closest value
                        double dMinDiff = 1e10;
                        int nBestIndex = -1;
                        for( int jj=0; jj<mErrors.cols(); jj++ ) {
                            if( jj != ii && mErrors(0,jj) < dMaxErr ) {
                                double dDiff = ( mP2D.col( ii ) - mP2D.col( jj ) ).norm();
                                if( dDiff <  dMinDiff ) { 
                                    dMinDiff = dDiff;
                                    nBestIndex = jj;
                                }
                            }
                        }
                        if( nBestIndex == -1 ) {
                            // All is lost, could not find an index 
                            // that passes the error test
                            return false;
                        }
                        mP3D.col( ii ) = mP3D.col( nBestIndex ) ;
//.........这里部分代码省略.........