C++ VectorXd::size方法代码示例

Trajectory Trajectory::generatePolynomialTrajectory(const VectorXd& ts, const VectorXd& y_from, const VectorXd& yd_from, const VectorXd& ydd_from,
        const VectorXd& y_to, const VectorXd& yd_to, const VectorXd& ydd_to)
{
    VectorXd a0 = y_from;
    VectorXd a1 = yd_from;
    VectorXd a2 = ydd_from / 2;

    VectorXd a3 = -10 * y_from - 6 * yd_from - 2.5 * ydd_from + 10 * y_to - 4 * yd_to + 0.5 * ydd_to;
    VectorXd a4 = 15 * y_from + 8 * yd_from + 2 * ydd_from - 15  * y_to  + 7 * yd_to - ydd_to;
    VectorXd a5 = -6 * y_from - 3 * yd_from - 0.5 * ydd_from  + 6 * y_to  - 3 * yd_to + 0.5 * ydd_to;

    int n_time_steps = ts.size();
    int n_dims = y_from.size();

    MatrixXd ys(n_time_steps,n_dims), yds(n_time_steps,n_dims), ydds(n_time_steps,n_dims);

    for (int i = 0; i < ts.size(); i++)
    {
        double t = (ts[i] - ts[0]) / (ts[n_time_steps - 1] - ts[0]);
        ys.row(i) = a0 + a1 * t + a2 * pow(t, 2) + a3 * pow(t, 3) + a4 * pow(t, 4) + a5 * pow(t, 5);
        yds.row(i) = a1 + 2 * a2 * t + 3 * a3 * pow(t, 2) + 4 * a4 * pow(t, 3) + 5 * a5 * pow(t, 4);
        ydds.row(i) = 2 * a2 + 6 * a3 * t + 12 * a4 * pow(t, 2) + 20 * a5 * pow(t, 3);
    }

    yds /= (ts[n_time_steps - 1] - ts[0]);
    ydds /= pow(ts[n_time_steps - 1] - ts[0], 2);

    return Trajectory(ts, ys, yds, ydds);
}

Trajectory Trajectory::generatePolynomialTrajectoryThroughViapoint(const VectorXd& ts, const VectorXd& y_from, const VectorXd& y_yd_ydd_viapoint, double viapoint_time, const VectorXd& y_to)
{

    int n_dims = y_from.size();
    assert(n_dims==y_to.size());
    assert(3*n_dims==y_yd_ydd_viapoint.size()); // Contains y, yd and ydd, so *3

    int n_time_steps = ts.size();

    int viapoint_time_step = 0;
    while (viapoint_time_step<n_time_steps && ts[viapoint_time_step]<viapoint_time)
        viapoint_time_step++;

    if (viapoint_time_step>=n_time_steps)
    {
        cerr << __FILE__ << ":" << __LINE__ << ":";
        cerr << "ERROR: the time vector does not contain any time smaller than " << viapoint_time << ". Returning min-jerk trajectory WITHOUT viapoint." <<  endl;
        return Trajectory();
    }

    VectorXd yd_from        = VectorXd::Zero(n_dims);
    VectorXd ydd_from       = VectorXd::Zero(n_dims);

    VectorXd y_viapoint     = y_yd_ydd_viapoint.segment(0*n_dims,n_dims);
    VectorXd yd_viapoint    = y_yd_ydd_viapoint.segment(1*n_dims,n_dims);
    VectorXd ydd_viapoint   = y_yd_ydd_viapoint.segment(2*n_dims,n_dims);

    VectorXd yd_to          = VectorXd::Zero(n_dims);
    VectorXd ydd_to         = VectorXd::Zero(n_dims);

    Trajectory traj = Trajectory::generatePolynomialTrajectory(ts.segment(0, viapoint_time_step + 1), y_from, yd_from, ydd_from, y_viapoint, yd_viapoint, ydd_viapoint);
    traj.append(Trajectory::generatePolynomialTrajectory(ts.segment(viapoint_time_step, n_time_steps - viapoint_time_step), y_viapoint, yd_viapoint, ydd_viapoint, y_to, yd_to, ydd_to));

    return traj;
}

Trajectory Trajectory::generateMinJerkTrajectory(const VectorXd& ts, const VectorXd& y_from, const VectorXd& y_to)
{
    int n_time_steps = ts.size();
    int n_dims = y_from.size();

    MatrixXd ys(n_time_steps,n_dims), yds(n_time_steps,n_dims), ydds(n_time_steps,n_dims);

    double D =  ts[n_time_steps-1];
    ArrayXd tss = (ts/D).array();


    ArrayXd A = y_to.array()-y_from.array();

    for (int i_dim=0; i_dim<n_dims; i_dim++)
    {

        // http://noisyaccumulation.blogspot.fr/2012/02/how-to-decompose-2d-trajectory-data.html

        ys.col(i_dim)   = y_from[i_dim] + A[i_dim]*(  6*tss.pow(5)  -15*tss.pow(4) +10*tss.pow(3));

        yds.col(i_dim)  =             (A[i_dim]/D)*( 30*tss.pow(4)  -60*tss.pow(3) +30*tss.pow(2));

        ydds.col(i_dim) =         (A[i_dim]/(D*D))*(120*tss.pow(3) -180*tss.pow(2) +60*tss       );
    }

    return Trajectory(ts,ys,yds,ydds);


}

double Mesh::triangleInequalityLineSearch(const VectorXd &g, const VectorXd &dg) const
{
    assert(g.size() == numedges());
    assert(dg.size() == numedges());

    double maxt = std::numeric_limits<double>::infinity();

    for(OMMesh::FaceIter fi = mesh_->faces_begin(); fi != mesh_->faces_end(); ++fi)
    {
        vector<double> gs;
        vector<double> dgs;
        for(OMMesh::FaceEdgeIter fei = mesh_->fe_iter(fi.handle()); fei; ++fei)
        {
            gs.push_back(g[fei.handle().idx()]);
            dgs.push_back(dg[fei.handle().idx()]);
        }
        assert(gs.size() == 3);
        for(int i=0; i<3; i++)
        {
            int idx[3];
            for(int j=0; j<3; j++)
            {
                idx[j] = (i+j)%3;
            }
            double thismax = triangleInequalityLineSearch(gs[idx[0]], gs[idx[1]], gs[idx[2]],
                    dgs[idx[0]], dgs[idx[1]], dgs[idx[2]]);

            maxt = std::min(maxt, thismax);
        }
    }

    return maxt;
}

VectorXd HarmonicGradient(const vector<spring> &springlist,
                  const VectorXd &XY,
                  const double g11,
                  const double g12,
                  const double g22)
{
  VectorXd gradE(XY.size());
  for(int i=0;i<gradE.size();i++){
    gradE(i)=0;
  }
  VectorXd X(XY.size()/2);
  VectorXd Y(XY.size()/2);
  X=XY.head(XY.size()/2);
  Y=XY.tail(XY.size()/2);

  for(int i=0;i<springlist.size();i++){
    int one=springlist[i].one;
    int two=springlist[i].two;
    int num=XY.size()/2;
    double dx=X(one)-(X(two)+springlist[i].wlr);
    double dy=Y(one)-(Y(two)+springlist[i].wud);
    double k=springlist[i].k;
    double L=springlist[i].rlen;
    double dist=sqrt( g11*dx*dx+ 2*g12*dx*dy+ g22*dy*dy );
   
    double gradx= k*(dist-L)*(g11*dx+g12*dy)/dist;
    double grady= k*(dist-L)*(g22*dy+g12*dx)/dist;

    gradE(one) += gradx;
    gradE(two) -= gradx;
    gradE(one+num) += grady;
    gradE(two+num) -= grady;
  }
return gradE;
}

  int operator()(const VectorXd &x, VectorXd &fvec) const
  {
    assert( x.size() == _dim );
    assert( (unsigned int) fvec.size() == 6);

    Eigen::Matrix<double,12,1> solution = _a;
    for(size_t i = 0; i < _dim; i++)
      solution += x[i]*_V.col(12-_dim+i);

    points_t ccam;
    for(size_t i = 0; i<4; i++)
      ccam.push_back(solution.block<3,1>(i*3,0));

    Eigen::Vector3d diffw;
    Eigen::Vector3d diffc;
    size_t index = 0;

    for(size_t i = 0; i<3; i++)
    {
      for(size_t j = i+1; j < 4; j++)
      {
        diffw = _c[i]-_c[j];
        diffc = ccam[i]-ccam[j];
        fvec[index++] = diffw.dot(diffw)-diffc.dot(diffc);
      }
    }

    return 0;
  }

void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray *prhs[])
{
  if(nrhs!=3 || nlhs != 8)
  {
    mexErrMsgIdAndTxt("Drake:testMultipleTimeLinearPostureConstrainttmex:BadInputs","Usage [num_cnst,cnst_val,iAfun,jAvar,A,cnst_name,lb,ub] = testMultipleTimeLinearPostureConstraintmex(kinCnst,q,t)");
  }
  MultipleTimeLinearPostureConstraint* cnst = (MultipleTimeLinearPostureConstraint*) getDrakeMexPointer(prhs[0]);
  int n_breaks = mxGetNumberOfElements(prhs[2]);
  double* t_ptr = new double[n_breaks];
  memcpy(t_ptr,mxGetPrSafe(prhs[2]),sizeof(double)*n_breaks);
  int nq = cnst->getRobotPointer()->num_positions;
  MatrixXd q(nq,n_breaks);
  if(mxGetM(prhs[1]) != nq || mxGetN(prhs[1]) != n_breaks)
  {
    mexErrMsgIdAndTxt("Drake:testMultipleTimeLinearPostureConstraintmex:BadInputs","Argument 2 must be of size nq*n_breaks");
  }
  memcpy(q.data(),mxGetPrSafe(prhs[1]),sizeof(double)*nq*n_breaks); 
  int num_cnst = cnst->getNumConstraint(t_ptr,n_breaks); 
  VectorXd c(num_cnst);
  cnst->feval(t_ptr,n_breaks,q,c);
  VectorXi iAfun;
  VectorXi jAvar;
  VectorXd A;
  cnst->geval(t_ptr,n_breaks,iAfun,jAvar,A);
  std::vector<std::string> cnst_names;
  cnst->name(t_ptr,n_breaks,cnst_names);
  VectorXd lb(num_cnst);
  VectorXd ub(num_cnst);
  cnst->bounds(t_ptr,n_breaks,lb,ub);
  VectorXd iAfun_tmp(iAfun.size());
  VectorXd jAvar_tmp(jAvar.size());
  for(int i = 0;i<iAfun.size();i++)
  {
    iAfun_tmp(i) = (double) iAfun(i)+1;
    jAvar_tmp(i) = (double) jAvar(i)+1;
  }
  plhs[0] = mxCreateDoubleScalar((double) num_cnst);
  plhs[1] = mxCreateDoubleMatrix(num_cnst,1,mxREAL);
  memcpy(mxGetPrSafe(plhs[1]),c.data(),sizeof(double)*num_cnst);
  plhs[2] = mxCreateDoubleMatrix(iAfun_tmp.size(),1,mxREAL);
  memcpy(mxGetPrSafe(plhs[2]),iAfun_tmp.data(),sizeof(double)*iAfun_tmp.size());
  plhs[3] = mxCreateDoubleMatrix(jAvar_tmp.size(),1,mxREAL);
  memcpy(mxGetPrSafe(plhs[3]),jAvar_tmp.data(),sizeof(double)*jAvar_tmp.size());
  plhs[4] = mxCreateDoubleMatrix(A.size(),1,mxREAL);
  memcpy(mxGetPrSafe(plhs[4]),A.data(),sizeof(double)*A.size());
  int name_ndim = 1;
  mwSize name_dims[] = {(mwSize) num_cnst};
  plhs[5] = mxCreateCellArray(name_ndim,name_dims);
  mxArray *name_ptr;
  for(int i = 0;i<num_cnst;i++)
  {
    name_ptr = mxCreateString(cnst_names[i].c_str());
    mxSetCell(plhs[5],i,name_ptr);
  }
  plhs[6] = mxCreateDoubleMatrix(num_cnst,1,mxREAL);
  plhs[7] = mxCreateDoubleMatrix(num_cnst,1,mxREAL);
  memcpy(mxGetPrSafe(plhs[6]),lb.data(),sizeof(double)*num_cnst);
  memcpy(mxGetPrSafe(plhs[7]),ub.data(),sizeof(double)*num_cnst);
  delete[] t_ptr;
}

void f_u(VectorXd &val, double * x_array, double * u_array, VectorXd const &y, double K, double S0, int n){
        
        gsl_interp_accel *my_accel_ptr = gsl_interp_accel_alloc ();
        gsl_spline *my_spline_ptr = gsl_spline_alloc (gsl_interp_cspline, n);
        gsl_spline_init (my_spline_ptr, x_array, u_array, n);
        
        val=VectorXd(y.size());
        int j=0;
        int k=0;
        while (y(j)<x_array[0]) {
                val(k)=payoff(y(k),K,S0);
                ++k;
                ++j;
        }
        while (j<y.size() && y(j)<x_array[n-1]) {
                val(k)=gsl_spline_eval(my_spline_ptr, y(k) , my_accel_ptr);
                ++j;
                ++k;
        }
        for (int i=k; i<y.size(); ++i) {
                val(i)=0;
        }
        
        gsl_spline_free(my_spline_ptr);
        gsl_interp_accel_free(my_accel_ptr);
        
        return;
}

VectorXd math_ext::interpolate_y(const VectorXd &x_in, const VectorXd &y_in, const VectorXd &y_out)
{
    const int n=x_in.size(), m=y_out.size();
    VectorXd x_out(m);
    for(int n1=0;n1<m;++n1)
    {
        if(y_out(n1)<=y_in(0))
        {
            //x_out(n1)=x_in(0)+(y_out(n1)-y_in(0))*(x_in(1)-x_in(0))/(y_in(1)-y_in(0));
            x_out(n1)=x_in(0);
            continue;
        }
        else if(y_out(n1)>=y_in(n-1))
        {
            //x_out(n1)=x_in(n-1)+(y_out(n1)-y_in(n-1))*(x_in(n-2)-x_in(n-1))/(y_in(n-2)-y_in(n-1));
            x_out(n1)=x_in(n-1);
            continue;
        }
        for(int n2=1;n2<n;++n2)
        {
            if(y_out(n1)>y_in(n2-1) && y_out(n1)<=y_in(n2))
            {
                x_out(n1)=x_in(n2-1)+(x_in(n2)-x_in(n2-1))*(y_out(n1)-y_in(n2-1))/(y_in(n2)-y_in(n2-1));
                break;
            }
            else
                continue;
        }
    }
    return(x_out);
}

	int operator()(const VectorXd &b, VectorXd &fvec) const {
		ASSERT(b.size() == unknowns);
		ASSERT(fvec.size() == datasetLen);
		for(int i = 0; i < 35; i++)
			fvec[i] = b[0]/b[1] * exp(-0.5*(x[i]-b[2])*(x[i]-b[2])/(b[1]*b[1])) - y[i];
		return 0;
	}

/** 
 * reconstruct the displacements u in Euler space with RS coordinates y
 * provided.
 * 
 * @param y the RS coordinates.
 * @param u the constructed Euler coordinates represents the displacements.
 * 
 * @return true if construction is success.
 */
bool RS2Euler::reconstruct(const VectorXd &y, VectorXd &u){

  assert (tetmesh != NULL);
  const int node_numx3 = tetmesh->nodes().size()*3;
  bool succ = true;

  // assemble the right_side
  VectorXd b;
  assemble_b(y,b);
  assert_eq(VG_t.cols(),b.size());
  assert_eq(VG_t.rows(),node_numx3);

  VectorXd right_side(node_numx3 + numFixedDofs());
  right_side.segment(0, node_numx3) = VG_t*b;
  right_side.segment(node_numx3,numFixedDofs()).setZero();
  right_side.segment(node_numx3,barycenter_uc.size()) = barycenter_uc;

  // solve A*u = right_side
  assert_eq (right_side.size(), A_solver.rows());
  u = A_solver.solve(right_side);

  // get the result 
  if(A_solver.info()!=Eigen::Success) {
	
  	succ = false;
  	u.resize(node_numx3);
  	u.setZero();
  	ERROR_LOG("failed to solve for A X = P.");
  }else{
	assert_gt(u.size(), node_numx3);
	const VectorXd x = u.head(node_numx3);
	u = x;
  }
  return succ;
}

//The unvectorized version
void fresnelApprox(const VectorXd &t, VectorXd *s, VectorXd *c)
{
    s->resize(t.size());
    c->resize(t.size());
    for(int i = 0; i < t.size(); ++i)
        fresnelApprox(t[i], &((*s)[i]), &((*c)[i]));
}

void pca_small(MatrixXd &B, int method, MatrixXd& U, VectorXd &d, bool verbose)
{
   if(method == METHOD_SVD)
   {
      verbose && std::cout << timestamp() << " SVD begin" << std::endl;
      JacobiSVD<MatrixXd> svd(B, ComputeThinU | ComputeThinV);
      U = svd.matrixU();
      MatrixXd V = svd.matrixV();
      d = svd.singularValues().array().pow(2);
      verbose && std::cout << timestamp() << " SVD done" << std::endl;
   }
   else if(method == METHOD_EIGEN)
   {
      verbose && std::cout << timestamp() << " Eigen-decomposition begin" << std::endl;
      MatrixXd BBT = B * B.transpose();
      verbose && std::cout << timestamp() << " dim(BBT): " << dim(BBT) << std::endl;
      SelfAdjointEigenSolver<MatrixXd> eig(BBT);

      // The eigenvalues come out sorted in *increasing* order,
      // but we need decreasing order
      VectorXd eval = eig.eigenvalues();
      MatrixXd evec = eig.eigenvectors();
      d.resize(eval.size());
      U.resize(BBT.rows(), BBT.rows());

      unsigned int k = 0, s = d.size();
      for(unsigned int i = d.size() - 1 ; i != -1 ; --i)
      {
	 // we get eigenvalues, which are the squared singular values
	 d(k) = eval(i);
	 U.col(k) = evec.col(i);
	 k++;
      }
   }
}

double EEMS::test_prior(const MatrixXd &mSeeds, const VectorXd &mEffcts, const double mrateMu,
			const MatrixXd &qSeeds, const VectorXd &qEffcts,
			const double df, const double sigma2, const double mrateS2, const double qrateS2) const {
  bool inrange = true;
  int qtiles = qEffcts.size();
  int mtiles = mEffcts.size();
  // First check that all parameters fall into their support range
  for ( int i = 0 ; i < qtiles ; i++ ) {
    if (!habitat.in_point(qSeeds(i,0),qSeeds(i,1))) { inrange = false; }
  }
  for ( int i = 0 ; i < mtiles ; i++ ) {
    if (!habitat.in_point(mSeeds(i,0),mSeeds(i,1))) { inrange = false; }
  }
  if (qEffcts.cwiseAbs().minCoeff()>params.qEffctHalfInterval) { inrange = false; }
  if (mEffcts.cwiseAbs().minCoeff()>params.mEffctHalfInterval) { inrange = false; }
  if (abs(mrateMu)>params.mrateMuHalfInterval) { inrange = false; }
  if ((df<params.dfmin) || (df>params.dfmax)) { inrange = false; }
  if (!inrange) { return (-Inf); }
  // Then compute the prior, on the log scale
  double logpi = - log(df)
    + dnegbinln(mtiles,params.negBiSize,params.negBiProb)
    + dnegbinln(qtiles,params.negBiSize,params.negBiProb)
    + dinvgamln(mrateS2,params.mrateShape_2,params.mrateScale_2)
    + dinvgamln(qrateS2,params.qrateShape_2,params.qrateScale_2)
    + dmvnormln(mEffcts,VectorXd::Zero(mtiles),mrateS2*MatrixXd::Identity(mtiles,mtiles))
    + dmvnormln(qEffcts,VectorXd::Zero(qtiles),qrateS2*MatrixXd::Identity(qtiles,qtiles))
    + dinvgamln(sigma2,params.sigmaShape_2,params.sigmaScale_2);
  return (logpi);
}

Eigen::VectorXd normProbVector(VectorXd P_vec)
{
	VectorXd P_norm = P_vec;
	if (P_vec == Eigen::VectorXd::Zero(P_vec.size())){
	}
	else{
		double P_positive = 0; double P_negative = 0;

		for (int row_idx = 0; row_idx < P_vec.size(); row_idx++){
			P_positive = (P_vec(row_idx) > 0) ? (P_positive + P_vec(row_idx)) : P_positive;
			P_negative = (P_vec(row_idx) > 0) ? P_negative : (P_negative + P_vec(row_idx));
		}
		if (fabs(P_positive) < fabs(P_negative)){
			P_norm = -P_vec / fabs(P_negative);
		}
		else{
			P_norm = P_vec / fabs(P_positive);
		}

		for (int row_idx = 0; row_idx < P_vec.size(); row_idx++){
			P_norm(row_idx) = (P_norm(row_idx)<0) ? 0 : P_norm(row_idx);
		}
	}
	return P_norm;
}