C++ VectorXd类代码示例

 MatrixXd infer(const VectorXd& input)
 {
     auto sigmod = [&](double& elem)
     {
         return 1/(1 + exp(-elem));
     };
     
     MatrixXd    output = input.transpose();
     for(auto layer=0; layer<weights.size(); ++layer)
     {
         output = output * weights[layer];
         for(auto i=0; i<output.size(); ++i)
         {
             output(i) = sigmod(output(i));
         }
     }
     
     return output;
 }

void UKF::PredictMeasurement(int n_z, const MatrixXd &Zsig, VectorXd &z_pred, MatrixXd &S, MatrixXd &R) {

  // predict measurement mean
  z_pred.fill(0.0);
  for (int i=0; i < 2 * n_aug_ + 1; i++) {
    z_pred += weights_(i) * Zsig.col(i);
  }

  // measurement covariance matrix S
  S.fill(0.0);
  for (int i = 0; i < 2 * n_aug_ + 1; i++) {
    VectorXd z_diff = Zsig.col(i) - z_pred;
    while (z_diff(1) >  M_PI) z_diff(1) -= 2. * M_PI;
    while (z_diff(1) < -M_PI) z_diff(1) += 2. * M_PI;
    S += weights_(i) * z_diff * z_diff.transpose();
  }

  S += R;
}

int contactPhi(const RigidBodyTree<double>& r,
               const KinematicsCache<double>& cache,
               // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
               SupportStateElement& supp, VectorXd& phi) {
  int nc = static_cast<int>(supp.contact_pts.size());
  phi.resize(nc);

  if (nc < 1) return nc;

  int i = 0;
  for (auto pt_iter = supp.contact_pts.begin();
       pt_iter != supp.contact_pts.end(); pt_iter++) {
    Vector3d contact_pos = r.transformPoints(cache, *pt_iter, supp.body_idx, 0);
    phi(i) = supp.support_surface.head<3>().dot(contact_pos) +
             supp.support_surface(3);
    i++;
  }
  return nc;
}

double Ebend(const vector<vector<int>> &springpairs,
             const vector<spring> &springlist,
             const VectorXd &XY,
             const double g11,
             const double g12,
             const double g22,
             const double kappa)
{
 double Energy=0;
 int num=XY.size()/2;
 double l1,l2,costh;
 double x1,y1,x21,y21,x23,y23,x3,y3;
 for(int i=0;i<springpairs.size();i++){
 int springone=springpairs[i][0];
    int springtwo=springpairs[i][1];
 
    int coordNRone=springlist[springone].one;
    int coordNRtwo=springlist[springone].two;
    int coordNRthree=springlist[springtwo].two;
 
    x1=XY(coordNRone);
    y1=XY(coordNRone+num);
 
    x21=XY(coordNRtwo)+springlist[springone].wlr; //version of (x2,y2) that lies in on spring 1, so possibly outside of the box
    y21=XY(coordNRtwo+num)+springlist[springone].wud;
 
    x23=XY(coordNRtwo);                 //version of (x2,y2) that is on spring 2, so MUST be inside the box
    y23=XY(coordNRtwo+num);
 
    x3=XY(coordNRthree)+springlist[springtwo].wlr;
    y3=XY(coordNRthree+num)+springlist[springtwo].wud;

    l1=Dist(x1,y1,x21,y21,g11,g12,g22);
    l2=Dist(x23,y23,x3,y3,g11,g12,g22);
    costh=(g11*(x21-x1)*(x3-x23)+g12*(x21-x1)*(y3-y23)+g12*(y21-y1)*(x3-x23)+g22*(y21-y1)*(y3-y23))/(l1*l2);
    if(costh>1.0) costh=1.0;
    if(costh<-1.0) costh=-1.0;

    Energy=Energy+kappa*pow(pi-acos(costh),2)/(l1+l2);
}

return Energy; 
}

SparseMatrix<double> spmtimesd(const SparseMatrix<double>& m, const VectorXd& d1, const VectorXd& d2)
{
	assert(m.rows() == d1.rows() || d1.rows() == 0);
	assert(m.cols() == d2.rows() || d2.rows() == 0);

	SparseMatrix<double> result(m.rows(), m.cols());
	result = m;
	if(d1.rows() > 0)
		result = sparseFromDiag(d1) * result;
	if(d2.rows() > 0)
		result = result * sparseFromDiag(d2);

	return result;
}

void CGppe::Make_Predictions_New_User(const VectorXd & theta_x, const VectorXd& theta_t, double& sigma, const MatrixXd& train_t, const MatrixXd &x, const TypePair & train_pairs,
                                     const VectorXd & idx_global, const VectorXd& idx_global_1, const VectorXd& idx_global_2,
                                     const VectorXd& ind_t, const VectorXd& ind_x, const MatrixXd & test_t, const MatrixXd& idx_pairs, const VectorXd& ftrue, const VectorXd& ytrue)
{
    int N = x.rows();
    int Mtrain = train_t.rows();
    int Npairs = idx_pairs.rows();
    VectorXd fstar;
    MatrixXd pair;
    VectorXd P = VectorXd::Zero(Npairs);
    VectorXd ypred = VectorXd::Zero(Npairs);
    VectorXd sum = VectorXd::Zero(N);
    VectorXd count = VectorXd::Zero(N);

    Approx_CGppe_Laplace( theta_x, theta_t, sigma,
                         train_t, x, train_pairs, idx_global, idx_global_1, idx_global_2, ind_t, ind_x, Mtrain, N);

    for (int i = 0;i < Npairs;i++)
    {
        pair = idx_pairs.row(i);


        Predict_CGppe_Laplace(sigma, train_t, x, idx_global, ind_t, ind_x,
                             test_t, pair);
        P(i) = p;
        sum(pair(0)) += mustar(0);
        count(pair(0)) += 1;
        sum(pair(1)) += mustar(1);
        count(pair(1)) += 1;

    }

    for (int i = 0;i < P.rows();i++)
    {
        if (P(i) > 0.5000001)
            ypred(i) = 1;
        else
        	ypred(i)=0;
    }

    fstar = sum.array() / count.array();
	dsp(fstar,"fstar");
    cout << endl << endl << "error =  " << (GetDiff(ytrue, ypred)).sum() / ytrue.rows()<<endl;
    // need for a plot function here ?
}

void generateBezierCurve()
{
    ts = VectorXd::LinSpaced ( 21,0,1. );
    t_x.resize ( ts.size() );
    t_y.resize ( ts.size() );
    t_z.resize ( ts.size() );

    for ( int idx=0; idx< ts.size(); ++idx )
    {
        t_x ( idx ) = getValue ( ts ( idx ), pt_x );
        t_y ( idx ) = getValue ( ts ( idx ), pt_y );
        t_z ( idx ) = getValue ( ts ( idx ), pt_z );
    }
}

VectorXd build_l_mode_a1etaa3(const VectorXd x_l, double H_l, double fc_l, double f_s, double eta, double a3, double asym, double gamma_l, const int l, VectorXd V){
/*
 * This model includes:
 *      - Asymetry of Lorentzian asym
 *      - splitting a1
 *      - an Asphericity parameter eta
 *      - latitudinal effect a3
*/
	const long Nxl=x_l.size();
    VectorXd profile(Nxl), tmp(Nxl), tmp2(Nxl), result(Nxl), asymetry(Nxl);
	double Qlm, clm;

	result.setZero();
	for(int m=-l; m<=l; m++){
		if(l != 0){
			Qlm=(l*(l+1) - 3*pow(m,2))/((2*l - 1)*(2*l + 3)); // accounting for eta
			if(l == 1){
				clm=m; // a3 for l=1
			}
			if(l == 2){
				clm=(5*pow(m,3) - 17*m)/3.; // a3 for l=2
			}
			if(l == 3){
				clm=0; // a3 NOT YET IMPLEMENTED FOR l=3
			}
			profile=(x_l - tmp.setConstant(fc_l*(1. + eta*Qlm) + m*f_s + clm*a3)).array().square();
			profile=4*profile/pow(gamma_l,2);
		} else{
			profile=(x_l - tmp.setConstant(fc_l)).array().square();
			profile=4*profile/pow(gamma_l,2);
		}
		if(asym == 0){ //Model with no asymetry
			result=result+ H_l*V(m+l)* ((tmp.setConstant(1) + profile)).cwiseInverse();
		} else{
			tmp.setConstant(1);
			asymetry=(tmp + asym*(x_l/fc_l - tmp)).array().square() + (tmp2.setConstant(0.5*gamma_l*asym/fc_l)).array().square();
			result=result+ H_l*V(m+l)*asymetry.cwiseProduct(((tmp.setConstant(1) + profile)).cwiseInverse());
		}
	}

return result;
}

MultivariateFNormalSufficient::MultivariateFNormalSufficient(
        const VectorXd& Fbar, double JF, const VectorXd& FM, int Nobs,
        const MatrixXd& W, const MatrixXd& Sigma, double factor)
{
        reset_flags();
        N_=Nobs;
        M_=Fbar.rows();
        LOG( "MVN: sufficient statistics init with N=" << N_
                << " and M=" << M_ << std::endl);
        CHECK( N_ > 0,
            "please provide at least one observation per dimension");
        CHECK( M_ > 0,
            "please provide at least one variable");
        set_factor(factor);
        set_FM(FM);
        set_Fbar(Fbar);
        set_W(W);
        set_jacobian(JF);
        set_Sigma(Sigma);
}

static void writeMesh(const char *filename, const VectorXd &verts, const Matrix3Xi &faces)
{
	ofstream ofs(filename);

	for(int i=0; i<verts.size()/3; i++)
	{
		ofs << "v ";
		for(int j=0; j<3; j++)
			ofs << verts[3*i+j] << " ";
		ofs << endl;
	}

	for(int i=0; i<faces.cols(); i++)
	{
		ofs << "f ";
		for(int j=0; j<3; j++)
			ofs << faces.coeff(j, i)+1 << " ";
		ofs << endl;
	}
}

MatrixXd MotionModel::dqomegadt_by_domega(VectorXd omega, double delta_t)
{
	// This function calculates dq((w_k^C + omega^C) * delta_t)/domega_k^C. Javier book P130. A.17
	// omega Modulus
	double omega_norm = 0;
	size_t omega_size = omega.size(), i;
	MatrixXd dqomegadt_by_domegaRES(4,3);
	for (i = 0; i < omega_size; ++i)
	{
		omega_norm += omega(i) * omega(i);
	}
	omega_norm = sqrt(omega_norm);

	// Use generic ancillary functions to calculate components of Jacobian
	dqomegadt_by_domegaRES << dq0_by_domegaA(omega(0), omega_norm, delta_t), dq0_by_domegaA(omega(1), omega_norm, delta_t), dq0_by_domegaA(omega(2), omega_norm, delta_t),
		dqA_by_domegaA(omega(0), omega_norm, delta_t), dqA_by_domegaB(omega(0), omega(1), omega_norm, delta_t), dqA_by_domegaB(omega(0), omega(2), omega_norm, delta_t),
		dqA_by_domegaB(omega(1), omega(0), omega_norm, delta_t), dqA_by_domegaA(omega(1), omega_norm, delta_t), dqA_by_domegaB(omega(1), omega(2), omega_norm, delta_t), 
		dqA_by_domegaB(omega(2), omega(0), omega_norm, delta_t), dqA_by_domegaB(omega(2), omega(1), omega_norm, delta_t), dqA_by_domegaA(omega(2), omega_norm, delta_t);
	return dqomegadt_by_domegaRES;
}

double line_search(fcn_Rn_to_R f, VectorXd x0, VectorXd v0, VectorXd Dfx0 ) {
	// Performs a line search. Takes in a function (f), initial point (x), and a
	// direction (v). Tries to find a scalar value (a) such that f(x + av) is
	// minimized.
	//
	double alpha = 1;
	double beta =  0.2;
	double tau = 0.5;

	double fval = f(x0);
	double stepsize = beta * Dfx0.dot(v0);

	while ( f(x0 + alpha*v0) > fval + alpha*stepsize ) {
		alpha = tau*alpha;
		//cout << "Step size : " << alpha << endl;
	}


	return alpha;
}

pair<MatrixXd*,MatrixXd*> CutbySphere(MatrixXd &Atoms, double Radius, double x0,double y0,double z0 ){


	//return ;
	VectorXd Dx = Atoms.col(0).array() - x0;

	VectorXd Dy = Atoms.col(1).array() - y0;
	VectorXd Dz = Atoms.col(2).array() - z0;
	//d2space=Dx.^2+Dy.^2+Dz.^2;
	MatrixXd d2space = Dx.cwiseAbs2()+ Dy.cwiseAbs2() + Dz.cwiseAbs2();
	MatrixXd D2space = d2space.array() - Radius*Radius;
	MatrixXd d2plane = Dx.cwiseAbs2() + Dy.cwiseAbs2();
	MatrixXd D2plane = d2plane.array() - Radius*Radius;

	int range=Atoms.rows();
	int j = 0, k = 0;
	//need to improve here
	MatrixXd* PickedAtoms = new MatrixXd(range,Atoms.cols());
	MatrixXd* Unpicked = new MatrixXd(range,Atoms.cols());
	
	for ( int i = 0 ; i < range; i ++){
		//cout<<i<<" "<<Atoms(i,2)<<" "<<" "<<D2space(i,0)<<endl;
		if (Atoms(i,2) > z0 && D2space(i,0) <= 0){
			PickedAtoms->row(j) = Atoms.row(i);
			j++;
		}else if (Atoms(i,2) <= z0 && D2plane(i,0) <= 0){
			PickedAtoms->row(j) = Atoms.row(i);
			j++;
		}else{
			Unpicked->row(k) = Atoms.row(i);
			k++;
		}
	}
	PickedAtoms->conservativeResize(j,Atoms.cols());
	Unpicked->conservativeResize(k,Atoms.cols());
	return pair<MatrixXd*,MatrixXd*> (PickedAtoms,Unpicked);

}

void addJointSoftLimits(const JointSoftLimitParams &params, const DrakeRobotState &robot_state, const VectorXd &q_des, std::vector<SupportStateElement,Eigen::aligned_allocator<SupportStateElement>> &supports, std::vector<drake::lcmt_joint_pd_override> &joint_pd_override) {
  Matrix<bool, Dynamic, 1> has_joint_override = Matrix<bool, Dynamic, 1>::Zero(q_des.size());
  for (std::vector<drake::lcmt_joint_pd_override>::iterator it = joint_pd_override.begin(); it != joint_pd_override.end(); ++it) {
    has_joint_override(it->position_ind - 1) = true;
  }
  for (int i=0; i < params.lb.size(); i++) {
    if (!has_joint_override(i) && params.enabled(i)) {
      int disable_body_1idx = params.disable_when_body_in_support(i);
      if (disable_body_1idx == 0 || !inSupport(supports, disable_body_1idx - 1)) {
        double w_lb = 0;
        double w_ub = 0;
        if (!std::isinf(params.lb(i))) {
          w_lb = logisticSigmoid(params.weight(i), params.k_logistic(i), params.lb(i), robot_state.q(i));
        }
        if (!std::isinf(params.ub(i))) {
          w_ub = logisticSigmoid(params.weight(i), params.k_logistic(i), robot_state.q(i), params.ub(i));
        }
        double weight = std::max(w_ub, w_lb);
        drake::lcmt_joint_pd_override override;
        override.position_ind = i + 1;
        override.qi_des = q_des(i);

int contactPhi(RigidBodyManipulator* r, SupportStateElement& supp, void *map_ptr, VectorXd &phi, double terrain_height)
{
  int nc = static_cast<int>(supp.contact_pts.size());
  phi.resize(nc);

  if (nc<1) return nc;

  Vector3d contact_pos,pos,normal;

  int i=0;
  for (std::vector<Vector4d,aligned_allocator<Vector4d>>::iterator pt_iter=supp.contact_pts.begin(); pt_iter!=supp.contact_pts.end(); pt_iter++) {
    r->forwardKin(supp.body_idx,*pt_iter,0,contact_pos);
    collisionDetect(map_ptr,contact_pos,pos,&normal,terrain_height);
    pos -= contact_pos;  // now -rel_pos in matlab version
    phi(i) = pos.norm();
    if (pos.dot(normal)>0)
      phi(i)=-phi(i);
    i++;
  }
  return nc;
}