C++ MatrixXd::resize方法代码示例

void color_intersections(
  const Eigen::MatrixXd & V,
  const Eigen::MatrixXi & F,
  const Eigen::MatrixXd & U,
  const Eigen::MatrixXi & G,
  Eigen::MatrixXd & C,
  Eigen::MatrixXd & D)
{
  using namespace igl;
  using namespace igl::cgal;
  using namespace Eigen;
  MatrixXi IF;
  const bool first_only = false;
  intersect_other(V,F,U,G,first_only,IF);
  C.resize(F.rows(),3);
  C.col(0).setConstant(0.4);
  C.col(1).setConstant(0.8);
  C.col(2).setConstant(0.3);
  D.resize(G.rows(),3);
  D.col(0).setConstant(0.4);
  D.col(1).setConstant(0.3);
  D.col(2).setConstant(0.8);
  for(int f = 0;f<IF.rows();f++)
  {
    C.row(IF(f,0)) = RowVector3d(1,0.4,0.4);
    D.row(IF(f,1)) = RowVector3d(0.8,0.7,0.3);
  }
}

/**
 * Generates an artificial topographyGrid of size numRows x numCols if no
 * topographic data is available.  Results are dumped into topographyGrid.
 * @param topographyGrid A pointer to a zero-initialized Grid of size
 * numRows x numCols.
 * @param numRows The desired number of non-border rows in the resulting matrix.
 * @param numCols The desired number of non-border cols in the resulting matrix.
 */
void simulatetopographyGrid(Grid* topographyGrid, int numRows, int numCols) {
    Eigen::VectorXd refx = refx.LinSpaced(numCols, -2*M_PI, 2*M_PI);
    Eigen::VectorXd refy = refx.LinSpaced(numRows, -2*M_PI, 2*M_PI);
    Eigen::MatrixXd X = refx.replicate(1, numRows);
    X.transposeInPlace();
    Eigen::MatrixXd Y = refy.replicate(1, numCols);

    // Eigen can only deal with two matrices at a time,
    // so split the computation:
    // topographyGrid = sin(X) * sin(Y) * abs(X) * abs(Y) -pi
    Eigen::MatrixXd absXY = X.cwiseAbs().cwiseProduct(Y.cwiseAbs());
    Eigen::MatrixXd sins = X.array().sin().cwiseProduct(Y.array().sin());
    Eigen::MatrixXd temp;
    temp.resize(numRows, numCols);
    temp = absXY.cwiseProduct(sins);

    // All this work to create a matrix of pi...
    Eigen::MatrixXd pi;
    pi.resize(numRows, numCols);
    pi.setConstant(M_PI);
    temp = temp - pi;
    // And the final result.
    topographyGrid->data.block(border, border, numRows, numCols) =
                              temp.block(0, 0, numRows, numCols);
    // Ignore positive values.
    topographyGrid->data =
            topographyGrid->data.unaryExpr(std::ptr_fun(validateDepth));
    topographyGrid->clearNA();
}

int main(int argc, char *argv[])
{
  using namespace Eigen;
  using namespace std;

  // Create the boundary of a square
  V.resize(8,2);
  E.resize(8,2);
  H.resize(1,2);

  V << -1,-1, 1,-1, 1,1, -1, 1,
       -2,-2, 2,-2, 2,2, -2, 2;

  E << 0,1, 1,2, 2,3, 3,0,
       4,5, 5,6, 6,7, 7,4;

  H << 0,0;

  // Triangulate the interior
  igl::triangle::triangulate(V,E,H,"a0.005q",V2,F2);

  // Plot the generated mesh
  igl::viewer::Viewer viewer;
  viewer.data.set_mesh(V2,F2);
  viewer.launch();
}

//! Function to filter tidal corrections based on RSS amplitude criteria.
std::pair< Eigen::MatrixXd, Eigen::MatrixXd > filterRawDataForAmplitudes(
        const Eigen::MatrixXd rawAmplitudes,
        const Eigen::MatrixXd rawFundamentalArgumentMultipliers,
        const double minimumAmplitude )
{
    // Get number of amplitudes per entry.
    int numberOfAmplitudes = rawAmplitudes.cols( );

    // Initialize return matrices
    int numberOfAcceptedAmplitudes = 0;
    Eigen::MatrixXd filteredAmplitudes;
    filteredAmplitudes.resize( numberOfAcceptedAmplitudes, numberOfAmplitudes );
    Eigen::MatrixXd filteredFundamentalArgumentMultipliers;
    filteredFundamentalArgumentMultipliers.resize( numberOfAcceptedAmplitudes, 6 );

    // Iterate over all entries, calculate RSS amplitude and accept or reject entry.
    double rssAmplitude = 0.0;
    for( int i = 0; i < rawAmplitudes.rows( ); i++ )
    {
        // Calculate RSS of amplitude.
        rssAmplitude = 0.0;
        for( int j =0; j < numberOfAmplitudes; j++ )
        {
            rssAmplitude += rawAmplitudes( i, j ) * rawAmplitudes( i, j );
        }
        rssAmplitude = std::sqrt( rssAmplitude );

        // If RSS amplitude is sufficiently large, accept value.
        if( rssAmplitude > minimumAmplitude )
        {
            if( numberOfAcceptedAmplitudes == 0 )
            {
                numberOfAcceptedAmplitudes++;
                filteredAmplitudes.resize( 1, numberOfAmplitudes );
                filteredAmplitudes = rawAmplitudes.block( i, 0, 1, numberOfAmplitudes );

                filteredFundamentalArgumentMultipliers.resize( 1, 6 );
                filteredFundamentalArgumentMultipliers = rawFundamentalArgumentMultipliers.block( i, 0, 1, 6 );
            }
            else
            {
                numberOfAcceptedAmplitudes++;
                filteredAmplitudes.conservativeResize( numberOfAcceptedAmplitudes, numberOfAmplitudes );
                filteredAmplitudes.block( numberOfAcceptedAmplitudes - 1, 0, 1, numberOfAmplitudes ) =
                        rawAmplitudes.block( i, 0, 1, numberOfAmplitudes );

                filteredFundamentalArgumentMultipliers.conservativeResize( numberOfAcceptedAmplitudes, 6 );
                filteredFundamentalArgumentMultipliers.block( numberOfAcceptedAmplitudes - 1, 0, 1, 6 )  =
                        rawFundamentalArgumentMultipliers.block( i, 0, 1, 6 );
            }
        }
    }

    return std::pair< Eigen::MatrixXd, Eigen::MatrixXd >( filteredAmplitudes, filteredFundamentalArgumentMultipliers );
}

int MNIST_Parser::read_MNIST_format(
    std::ifstream &file,
    Eigen::MatrixXd &storage,
    unsigned int header_size)
{
  if (!file.is_open())
    return -1;
  unsigned char *buffer = new unsigned char [header_size];
  int *header = new int [header_size];
  for (unsigned int i = 0; i < header_size; ++i) {
    file.read((char*)buffer, 4);
    //convert big-endian to little-endian
    header[i] = buffer[0] << 24 | buffer[1] << 16 | buffer[2] << 8 | buffer[3] << 0;
    //for (int i = 0; i < 4; ++i) {
      //std::cout << std::hex << (int)buffer[i] << std::endl;
    //}
    //if (i==0) std::cout << std::hex;
    //else std::cout << std::dec;
    //std::cout << header[i] << std::endl;
  }
  if (header_size == 4)
  {
    int img_size = header[2]*header[3];
    storage.resize(img_size, header[1]);
    unsigned char *img = new unsigned char [img_size];
    for (int i = 0; i < header[1]; ++i) {
      file.read((char*)img, img_size);
      for (int j = 0; j < img_size; ++j) {
        storage.col(i).row(j) << (double)(img[j]);
      }
    }
    storage /= 255.0;
    delete[] img;
  }
  else if (header_size == 2)
  {
    int column_size = 10;
    storage.resize(column_size, header[1]);
    storage = MatrixXd::Zero(column_size, header[1]);
    unsigned char *digit = new unsigned char [header[1]];
    //std::cout << storage.cols() << std::endl;
    //std::cout << storage.rows() << std::endl;
    file.read((char*)digit, header[1]);
    for (int i = 0; i < header[1]; ++i) {
      //std::cout << (int)digit[i] << std::endl;
      storage.col(i).row(digit[i]) << 1.0;
    }
    delete[] digit;
  }

  delete[] buffer;
  delete[] header;
  return 0;
}

  EReturn getMatrix(const tinyxml2::XMLElement & xml_matrix,
      Eigen::MatrixXd & eigen_matrix)
  {
    int dimension = 0;

    if (xml_matrix.QueryIntAttribute("dim", &dimension)
        != tinyxml2::XML_NO_ERROR)
    {
      INDICATE_FAILURE
      ;
      eigen_matrix = Eigen::MatrixXd(); //!< Null matrix again
      return PAR_ERR;
    }

    if (dimension < 1)
    {
      INDICATE_FAILURE
      ;
      eigen_matrix = Eigen::MatrixXd(); //!< Null matrix again
      return PAR_ERR;
    }
    eigen_matrix.resize(dimension, dimension);

    if (!xml_matrix.GetText())
    {
      INDICATE_FAILURE
      ;
      eigen_matrix = Eigen::MatrixXd(); //!< Null matrix again
      return PAR_ERR;
    }

    std::istringstream text_parser(xml_matrix.GetText());
    double temp_entry;
    for (int i = 0; i < dimension; i++) //!< Note that this does not handle comments!
    {
      for (int j = 0; j < dimension; j++)
      {
        text_parser >> temp_entry;
        if (text_parser.fail() || text_parser.bad()) //!< If a failure other than end of file
        {
          INDICATE_FAILURE
          ;
          eigen_matrix.resize(0, 0);
          return PAR_ERR;
        }
        else
        {
          eigen_matrix(i, j) = temp_entry;
        }
      }
    }

    return SUCCESS;
  }

void parse_rhs(
  const int nrhs, 
  const mxArray *prhs[], 
  Eigen::MatrixXd & V,
  Eigen::MatrixXi & F,
  Eigen::MatrixXd & P,
  Eigen::MatrixXd & N,
  int & num_samples)
{
  using namespace std;
  if(nrhs < 5)
  {
    mexErrMsgTxt("nrhs < 5");
  }

  const int dim = mxGetN(prhs[0]);
  if(dim != 3)
  {
    mexErrMsgTxt("Mesh vertex list must be #V by 3 list of vertex positions");
  }
  if(dim != (int)mxGetN(prhs[1]))
  {
   mexErrMsgTxt("Mesh facet size must be 3");
  }
  if(mxGetN(prhs[2]) != dim)
  {
    mexErrMsgTxt("Point list must be #P by 3 list of origin locations");
  }
  if(mxGetN(prhs[3]) != dim)
  {
    mexErrMsgTxt("Normal list must be #P by 3 list of origin normals");
  }
  if(mxGetN(prhs[4]) != 1 || mxGetM(prhs[4]) != 1)
  {
    mexErrMsgTxt("Number of samples must be scalar.");
  }


  V.resize(mxGetM(prhs[0]),mxGetN(prhs[0]));
  copy(mxGetPr(prhs[0]),mxGetPr(prhs[0])+V.size(),V.data());
  F.resize(mxGetM(prhs[1]),mxGetN(prhs[1]));
  copy(mxGetPr(prhs[1]),mxGetPr(prhs[1])+F.size(),F.data());
  F.array() -= 1;
  P.resize(mxGetM(prhs[2]),mxGetN(prhs[2]));
  copy(mxGetPr(prhs[2]),mxGetPr(prhs[2])+P.size(),P.data());
  N.resize(mxGetM(prhs[3]),mxGetN(prhs[3]));
  copy(mxGetPr(prhs[3]),mxGetPr(prhs[3])+N.size(),N.data());
  if(*mxGetPr(prhs[4]) != (int)*mxGetPr(prhs[4]))
  {
    mexErrMsgTxt("Number of samples should be non negative integer.");
  }
  num_samples = (int) *mxGetPr(prhs[4]);
}

void parse_rhs(
  const int nrhs, 
  const mxArray *prhs[], 
  Eigen::MatrixXd & V,
  Eigen::MatrixXi & Ele,
  Eigen::MatrixXd & Q,
  Eigen::MatrixXd & bb_mins,
  Eigen::MatrixXd & bb_maxs,
  Eigen::VectorXi & elements)
{
  using namespace std;
  using namespace igl;
  using namespace igl::matlab;
  mexErrMsgTxt(nrhs >= 3, "The number of input arguments must be >=3.");

  const int dim = mxGetN(prhs[0]);
  mexErrMsgTxt(dim == 3 || dim == 2,
    "Mesh vertex list must be #V by 2 or 3 list of vertex positions");

  mexErrMsgTxt(dim+1 == mxGetN(prhs[1]),
    "Mesh \"face\" simplex size must equal dimension+1");

  parse_rhs_double(prhs,V);
  parse_rhs_index(prhs+1,Ele);
  parse_rhs_double(prhs+2,Q);
  mexErrMsgTxt(Q.cols() == dim,"Dimension of Q should match V");
  if(nrhs > 3)
  {
    mexErrMsgTxt(nrhs >= 6, "The number of input arguments must be 3 or >=6.");
    parse_rhs_double(prhs+3,bb_mins);
    if(bb_mins.size()>0)
    {
      mexErrMsgTxt(bb_mins.cols() == dim,"Dimension of bb_mins should match V");
      mexErrMsgTxt(bb_mins.rows() >= Ele.rows(),"|bb_mins| should be > |Ele|");
    }
    parse_rhs_double(prhs+4,bb_maxs);
    mexErrMsgTxt(bb_maxs.cols() == bb_mins.cols(),
      "|bb_maxs| should match |bb_mins|");
    mexErrMsgTxt(bb_mins.rows() == bb_maxs.rows(),
      "|bb_mins| should match |bb_maxs|");
    parse_rhs_index(prhs+5,elements);
    mexErrMsgTxt(elements.cols() == 1,"Elements should be column vector");
    mexErrMsgTxt(bb_mins.rows() == elements.rows(),
      "|bb_mins| should match |elements|");
  }else
  {
    // Defaults
    bb_mins.resize(0,dim);
    bb_maxs.resize(0,dim);
    elements.resize(0,1);
  }
}

IGL_INLINE void igl::signed_distance_pseudonormal(
  const Eigen::MatrixXd & P,
  const Eigen::MatrixXd & V,
  const Eigen::MatrixXi & F,
  const AABB<Eigen::MatrixXd,3> & tree,
  const Eigen::MatrixXd & FN,
  const Eigen::MatrixXd & VN,
  const Eigen::MatrixXd & EN,
  const Eigen::VectorXi & EMAP,
  Eigen::VectorXd & S,
  Eigen::VectorXi & I,
  Eigen::MatrixXd & C,
  Eigen::MatrixXd & N)
{
  using namespace Eigen;
  const size_t np = P.rows();
  S.resize(np,1);
  I.resize(np,1);
  N.resize(np,3);
  C.resize(np,3);
# pragma omp parallel for if(np>1000)
  for(size_t p = 0;p<np;p++)
  {
    double s,sqrd;
    RowVector3d n,c;
    int i = -1;
    RowVector3d q = P.row(p);
    signed_distance_pseudonormal(tree,V,F,FN,VN,EN,EMAP,q,s,sqrd,i,c,n);
    S(p) = s*sqrt(sqrd);
    I(p) = i;
    N.row(p) = n;
    C.row(p) = c;
  }
//  igl::AABB<MatrixXd,3> tree_P;
//  MatrixXi J = VectorXi::LinSpaced(P.rows(),0,P.rows()-1);
//  tree_P.init(P,J);
//  tree.squared_distance(V,F,tree_P,P,J,S,I,C);
//# pragma omp parallel for if(np>1000)
//  for(size_t p = 0;p<np;p++)
//  {
//    RowVector3d c = C.row(p);
//    RowVector3d q = P.row(p);
//    const int f = I(p);
//    double s;
//    RowVector3d n;
//    pseudonormal_test(V,F,FN,VN,EN,EMAP,q,f,c,s,n);
//    N.row(p) = n;
//    S(p) = s*sqrt(S(p));
//  }

}

  // fvec case, only support row decomposition
  void create_matrix(const paracel::list_type<paracel::str_type> & linelst,
                     Eigen::MatrixXd & blk_dense_mtx,
                     paracel::dict_type<size_t, paracel::str_type> & rm) {

    int csz = 0;
    size_t indx = 0;
    bool flag = true;
    paracel::list_type<Eigen::VectorXd> mtx_llst;
    for(auto & line : linelst) {
      auto stf = parserfunc(line);
      if(flag) { csz = stf.size() - 1; flag = true; }
      rm[indx] = stf[0];
      indx += 1;
      Eigen::VectorXd tmp(csz);
      for(int i = 0; i < csz; ++i) {
        tmp[i] = std::stod(stf[i + 1]);
      }
      mtx_llst.push_back(tmp);
    } 
    // create dense block matrix
    blk_dense_mtx.resize(rm.size(), csz);
    for(size_t i = 0; i < rm.size(); ++i) {
      blk_dense_mtx.row(i) = mtx_llst[i];
    }
  }

  EReturn OMPLsolver::convertPath(const ompl::geometric::PathGeometric &pg,
      Eigen::MatrixXd & traj)
  {
    traj.resize(pg.getStateCount(), state_space_->getDimension());
    Eigen::VectorXd tmp(state_space_->getDimension());

    for (int i = 0; i < (int) pg.getStateCount(); ++i)
    {
      if (!ok(
          compound_ ?
              boost::static_pointer_cast<OMPLSE3RNCompoundStateSpace>(
                  state_space_)->OMPLStateToEigen(pg.getState(i), tmp) :
              boost::static_pointer_cast<OMPLStateSpace>(state_space_)->copyFromOMPLState(
                  pg.getState(i), tmp)))
      {
        ERROR("Can't copy state "<<i);
        return FAILURE;
      }
      else
      {
        traj.row(i) = tmp;
      }
    }
    return SUCCESS;
  }

//==============================================================================
TEST(Lemke, Lemke_4D)
{
  Eigen::MatrixXd A;
  Eigen::VectorXd b;
  Eigen::VectorXd* f;
  int err;

  f =  new Eigen::VectorXd(4);
  A.resize(4,4);
  A <<
           3.999,0.9985, 1.001,    -2,
          0.9985, 3.998,    -2,0.9995,
           1.001,    -2, 4.002, 1.001,
              -2,0.9995, 1.001, 4.001;

  b.resize(4);
  b <<
           -0.01008,
          -0.009494,
           -0.07234,
           -0.07177;

  err = dart::lcpsolver::Lemke(A,b,f);

  EXPECT_EQ(err, 0);
  EXPECT_TRUE(dart::lcpsolver::validate(A,(*f),b));
}

void CTRCalibration::ComputeErrorJacobian(::Eigen::MatrixXd& error_jacobian)
{
	error_jacobian.resize(m_params.size(),1);
	::Eigen::VectorXd params_original(m_params);

	double epsilon = 0.00001;
	double invEpsilon = 1.0/epsilon;
	double error_perturbed = 0;
	double error_perturbed2 = 0;
	double dummy = 0;
	for(int i = 0; i < m_params.size(); ++i)
	{
		double cur_epsilon = epsilon/m_scaleFactor[i];
		m_params[i] += cur_epsilon;
		//*(robot->GetFreeParameters()[i]) = params[i];
		UpdateParamsInAllCTR();
		error_perturbed = ComputeErrorOnTrainingSet(dummy);
		//error_perturbed = ComputeErrorOnDatasetShape(robot, kinematics, dataset, dummy);
		m_params[i] = params_original[i];
		
		m_params[i] -= cur_epsilon;
		//*(robot->GetFreeParameters()[i]) = params[i];
		UpdateParamsInAllCTR();
		error_perturbed2 = ComputeErrorOnTrainingSet(dummy);
		//error_perturbed2 = ComputeErrorOnDatasetShape(robot, kinematics, dataset, dummy);
		error_jacobian(i) = (error_perturbed - error_perturbed2) / (2*cur_epsilon);
		//jacobian(i) = (error_perturbed - error_original) / cur_epsilon;
		m_params[i] = params_original[i];
		//*(robot->GetFreeParameters()[i]) = m_params[i];
	}

}

void FixedJoint::qdot2v(const Eigen::Ref<const Eigen::VectorXd>& q, Eigen::MatrixXd& qdot_to_v, Eigen::MatrixXd* dqdot_to_v) const
{
  qdot_to_v.resize(getNumVelocities(), getNumPositions());
  if (dqdot_to_v) {
    dqdot_to_v->setZero(qdot_to_v.size(), getNumPositions());
  }
}

void FunctionApproximatorGMR::predictVariance(const Eigen::Ref<const Eigen::MatrixXd>& inputs, Eigen::MatrixXd& variances)
{
  ENTERING_REAL_TIME_CRITICAL_CODE
  variances.resize(inputs.rows(),getExpectedOutputDim());
  predict(inputs,empty_prealloc_,variances);
  EXITING_REAL_TIME_CRITICAL_CODE
}