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

 /** Constructor
  *
  * returns a Particle filter object.
  *
  * @param process The process model object that the PF is defined for
  * @param resampler The resampling algorithm
  * @param particles_num Number of particles \f$M\f$
  */
 Particle(Process process, Resampler resampler, unsigned long particles_num)
     : process_{process}, resampler_{resampler}, num_{particles_num} {
   // initialized w_ and state_par_
   w_.resize(num_);
   // take one sample to find out dimension
   auto tmp = process_.template getProcess<0>().getInitialPDF().random();
   state_par_.resize(tmp.size(), num_);
 }

void Koornwinder3D::EvaluatePolynomial(const int p, const arma::mat& xin, 
				       arma::mat &f, arma::cube &df){

  using namespace arma;

  const int ndof = (p+1)*(p+2)*(p+3)/6;
  const int npts = xin.n_rows;
  
  f.resize(ndof,npts);
  df.resize(ndof,npts,3);



  const mat x = 2.*xin-1.;
  const vec xi = x.unsafe_col(0);
  const vec eta = x.unsafe_col(1);
  const vec zeta = x.unsafe_col(2);

  vec r = -2.*(1.+xi)/(eta+zeta) - 1.;
  vec s = 2.*(1.+eta)/(1.-zeta) - 1.;
  vec t = zeta;

  for(int i=0; i<r.n_rows; i++){
    if(std::abs(zeta(i)-1.0) < 1.0e-10) s(i) = -1.0;
    if(std::abs(eta(i)+zeta(i)) < 1.0e-10) r(i) = -1.0;
 
  }


  const vec dr_dxi = -2./(eta+zeta);
  const vec dr_deta = 2.*(1.+xi)/pow(eta+zeta,2.0);
  const vec dr_dzeta = 2.*(1.+xi)/pow(eta+zeta,2.0);
  
  const vec ds_deta = 2./(1. - zeta);
  const vec ds_dzeta = -2.*(1. + eta)/pow(1. - zeta,2.0);


  const imat mnl = tetindex(p);



  const vec2 pm_half = {0.5, -0.5}; 
  vec pm, pn, pl;
  vec dpm, dpn, dpl;
  vec mval(npts), nval(npts), lval(npts);
  vec dmval(npts), dnval(npts), dlval(npts);

  for(int i=0; i<ndof; i++){
    const int m = mnl(i,0);
    const int n = mnl(i,1);
    const int l = mnl(i,2);
    pm = jacobi(m,0.,0.);
    pn = jacobi(n,2.*m+1.,0.);
    pl = jacobi(l,2.*(m+n+1.),0.);
    for(int j=0; j<m; j++){
      pn = conv(pm_half,pn);
    }
    for(int j=0; j<(m+n); j++){
      pl = conv(pm_half,pl);
    }

    polyder(pm,dpm);
    polyder(pn,dpn);
    polyder(pl,dpl);

    polyval(pm,r,mval);
    polyval(pn,s,nval);
    polyval(pl,t,lval);

    polyval(dpm,r,dmval);
    polyval(dpn,s,dnval);
    polyval(dpl,t,dlval);
    

    double fc = sqrt((2.*m+1.)*(2.*n+1+2.*m+1.)*(2.*l+1+2.*(m+n+1.)));

    for(int j=0; j<npts; j++){
      f(i,j) = fc*mval(j)*nval(j)*lval(j);
      df(i,j,0) = 2.0*fc*dmval(j)*dr_dxi(j)*nval(j)*lval(j);
      df(i,j,1) = 2.0*fc*(dmval(j)*dr_deta(j)*nval(j)*lval(j) + mval(j)*
			  dnval(j)*ds_deta(j)*lval(j));
      df(i,j,2) = 2.0*fc*(dmval(j)*dr_dzeta(j)*nval(j)*lval(j) + mval(j)*
			  (dnval(j)*ds_dzeta(j)*lval(j) + nval(j)*dlval(j)));
									
    }
  }
}

void Static_grid_filter::create_cube(arma::mat& points,float l, float w, float h, float bin_w,
                                         const arma::colvec3 position, const arma::vec3& rpy){

    std::cout<< "-1" << std::endl;

    std::size_t num_l = l/bin_w;
    std::size_t num_w = w/bin_w;
    std::size_t num_h = h/bin_w;

    if(num_h == 0){
        num_h = 1;
    }
    if(num_w == 0){
        num_w = 1;
    }
    if(num_l == 0){
        num_l = 1;
    }

    tf::Matrix3x3 R;
    tf::Vector3 p;
    std::cout<< "-2" << std::endl;
    std::cout<< "num_l: " << num_l << std::endl;
    std::cout<< "num_w: " << num_w << std::endl;
    std::cout<< "nu_h:  " << num_h << std::endl;

    //R.setRPY(rpy(0),rpy(1),rpy(2));
    R.setIdentity();

    tf::Vector3 t(-l/2.0 + position(0),-w/2.0 + position(1),-h/2.0 + position(2));
    std::cout<< "-3" << std::endl;

    std::cout<< "before resize" << std::endl;
    std::cout<< "total num points: " << num_l * num_w * num_h << std::endl;
    points.resize(num_l * num_w * num_h,3);

    std::cout<< "after resize" << std::endl;

    std::size_t index = 0;
    float step = bin_w/2;

    for(std::size_t i = 0; i < num_l; i++){
        for(std::size_t j = 0; j < num_w; j++){
            for(std::size_t k=0; k < num_h; k++){

                p.setX((float)i * bin_w + step);
                p.setY((float)j * bin_w + step);
                p.setZ((float)k * bin_w + step);
                p = R * p + t;
                points(index,0) = p.x();
                points(index,1) = p.y();
                points(index,2) = p.z();
                index++;
            }
        }
    }

    std::cout<< "finished creating cube" << std::endl;


}

void Koornwinder2D::EvaluatePolynomial(const int p, const arma::mat& xin, 
				       arma::mat &f, arma::cube &df){

  using namespace arma;

 const int ndof = (p+1)*(p+2)/2;
 const int npt = xin.n_rows;

 arma::mat x = 2.*xin-1.;
 vec xi = x.col(0);
 vec eta = x.col(1);

 f.resize(ndof,npt);
 df.resize(ndof,npt,2);


 const umat pind = pascalindex(p);
 arma::vec r = 2.*(1.0+xi)/(1.-eta)-1.;
 const arma::vec s = eta;
  for(int i=0; i<npt; i++){
    if(x(i,1)==1){
      r(i) = -1.0;
    }
  }


  const arma::vec dr_dxi = 2./(1.0-x.col(1));
  const arma::vec dr_deta = 2.*(1.0+x.col(0))/pow((1.-x.col(1)),2.0);
  const arma::vec2 cv = {0.5,-0.5};

  arma::vec pval(npt), qval(npt), dpval(npt), dqval(npt);
  arma::vec dpp,dqp;
  arma::vec pp, qp;
  for(int i=0; i<ndof; i++){
    pp = jacobi(pind(i,0),0.0,0.0);
    qp = jacobi(pind(i,1),2.*pind(i,0)+1.,0.0);

    for(int j=0; j<pind(i,0); j++){
      qp = conv(cv,qp);
    }
    
    
    polyval(pp,r,pval);
    polyval(qp,s,qval);

    const double fc = sqrt((2.*pind(i,0)+1.)*2.*(pind(i,0)+pind(i,1)+1.));

  
    polyder(pp,dpp);
    polyder(qp,dqp);

    polyval(dpp,r,dpval);
    polyval(dqp,s,dqval);

    for(int j=0; j<npt; j++){
      f(i,j) = fc*pval(j)*qval(j);
      df(i,j,0) = 2.0*fc*dpval(j)*qval(j)*dr_dxi(j);
      df(i,j,1) = 2.0*fc*(dpval(j)*qval(j)*dr_deta(j) + pval(j)*dqval(j));
    }
  }
  


 
}