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

    /* Solve min||Ax-b|| for a matrix A whose rank is given. */
    VectorXd solve( const VectorXd& b , bool inY=false)
    {
      if( rank==0 ) return VectorXd::Zero(NC);

      /* Approximate solution using no basis transfo (result is meanigless
       * appart from the computation time pov. */
      /*
	VectorXd sol = b.head(rank);
	matrixL().solveInPlace( sol );
	VectorXd res; res.setZero(NC);
	res.head(rank)=sol; return res;
      */

      /* With plain matrices. */
      /*
	VectorXd sol = matrixUr().transpose()*b;
	matrixL().solveInPlace( sol );
	return matrixVr()*sol;
      */

      /* Using the HH representation of V. */
      assert( m_computeThinU || m_computeFullU );
      VectorXd sol; 
      if( inY ) sol.setZero(rank); else sol.setZero(NC); 
      sol.head(rank) = matrixUr().transpose()*b; 
      matrixL().solveInPlace( sol.head(rank) );
      if( ! inY ) sol.applyOnTheLeft(qrv.householderQ());
      return sol;
   }

// 2 dimensional collision check. Return true if point is contained inside rectangle
bool exists_collision(VectorXd x_near, VectorXd x_new) {

	// ONLY WORKS WITH EIGEN3. IF FOR SOME REASON YOU ARE RUNNING THIS SOMEWHERE ELSE,
	// HARD CODE THIS
	x_near = x_near.head(2);
	x_new = x_new.head(2);

	MatrixXd obstacles = setup_values.obstacles;
	int num_obstacles = obstacles.cols();

	for (int n = 0; n < num_obstacles; n++) {

		double x_min = obstacles(0,n) - .5*obstacles(1,n);
		double x_max = obstacles(0,n) + .5*obstacles(1,n);
		double y_min = obstacles(2,n) - .5*obstacles(3,n);
		double y_max = obstacles(2,n) + .5*obstacles(3,n);

		// Determine if point is inside obstacle by discretization
		float dis_num = 20;
		VectorXd direction = (x_new - x_near)/(x_new - x_near).norm();
		double length = (x_new - x_near).norm();
		for (int i = 1;i <= dis_num; i++) {
			Vector2d point = x_near + length * i/dis_num * direction;
			if (inside_rectangular_obs(point, x_min, x_max, y_min, y_max)) {
				return true;
			}
		}

	}
	return false;
}

double whimed_i(
			VectorXd& a,
			VectorXi& w,
			int n,
			VectorXd& a_cand,
			VectorXd& a_psrt,
			VectorXi& w_cand
		){
//Eigen-ized version of whimed_i. See citation("pcaPP")
	int n2,i,k_cand,nn=n;/* sum of weights: `int' do overflow when  n ~>= 1e5 */
	int64_t wleft,wmid,wright,w_tot,wrest=0;
	double trial;
	w_tot=w.head(n).sum();
	do{
		a_psrt.head(nn)=a.head(nn);
		n2=nn/2;
		std::nth_element(a_psrt.data(),a_psrt.data()+n2,a_psrt.data()+nn);
		trial=a_psrt(n2);
		wleft=0;wmid=0;wright=0;
		for (i=0;i<nn;++i){
		    if(a(i)<trial)					wleft+=w(i);
		    else if(a(i)>trial)				wright+=w(i);
		    else wmid+=w(i);
		}
		k_cand=0;
		if((2*(wrest+wleft))>w_tot){
			for (i=0;i<nn;++i){
				if(a(i)<trial){
				    	a_cand(k_cand)=a(i);
				    	w_cand(k_cand)=w(i);
					++k_cand;
				}
			}
		}
		else if((2*(wrest+wleft+wmid))<=w_tot){
			for (i=0;i<nn;++i){
				if (a(i)>trial){
					a_cand(k_cand)=a(i);
					w_cand(k_cand)=w(i);
					++k_cand;
				}
		    	}
			wrest+=wleft+wmid;
		} else {
		        return(trial);
		}
		nn=k_cand;
		a.head(nn)=a_cand.head(nn);
		w.head(nn)=w_cand.head(nn);
	} while(1); 
}

void LCPSolver::transferSolFromODEFormulation(const VectorXd& _x, VectorXd& xOut, int numDir)
{
    int numContacts = _x.size() / 3;
    xOut = VectorXd::Zero(numContacts * (2 + numDir));

    xOut.head(numContacts) = _x.head(numContacts);

    int offset = numDir / 4;
    for (int i = 0; i < numContacts; ++i)
    {
        xOut[numContacts + i * numDir + 0] = _x[numContacts + i * 2 + 0];
        xOut[numContacts + i * numDir + offset] = _x[numContacts + i * 2 + 1];
    }
}

/** 
 * 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;
}

void MSCKF::addSlideState()
{
    SlideState newState;
    int nominalStateLength = (int)fullNominalState.size();
    int errorStateLength = (int)fullErrorCovariance.rows();
    
    VectorXd tmpNominal = VectorXd::Zero(nominalStateLength + NOMINAL_POSE_STATE_SIZE);
    MatrixXd tmpCovariance = MatrixXd::Zero(errorStateLength + ERROR_POSE_STATE_SIZE, errorStateLength + ERROR_POSE_STATE_SIZE);
    MatrixXd Jpi = MatrixXd::Zero(9, errorStateLength);
    
    tmpNominal.head(nominalStateLength) = fullNominalState;
    tmpNominal.segment(nominalStateLength, NOMINAL_POSE_STATE_SIZE) = fullNominalState.head(NOMINAL_POSE_STATE_SIZE);
    fullNominalState = tmpNominal;
    
    newState.q = fullNominalState.head(4);
    newState.p = fullNominalState.segment(4, 3);
    newState.v = fullNominalState.segment(7, 3);
    
    slidingWindow.push_back(newState);
    
    Jpi.block<3,3>(0, 0) = Matrix3d::Identity(3, 3);
    Jpi.block<3,3>(3, 3) = Matrix3d::Identity(3, 3);
    Jpi.block<3,3>(6, 6) = Matrix3d::Identity(3, 3);
    tmpCovariance.block(0, 0, errorStateLength, errorStateLength) = fullErrorCovariance;
    tmpCovariance.block(errorStateLength, 0, 9, errorStateLength) = Jpi * fullErrorCovariance;
    tmpCovariance.block(0, errorStateLength, errorStateLength, 9) = fullErrorCovariance * Jpi.transpose();
    tmpCovariance.block<ERROR_POSE_STATE_SIZE, ERROR_POSE_STATE_SIZE>(errorStateLength, errorStateLength) =
        Jpi * fullErrorCovariance * Jpi.transpose();
    
    fullErrorCovariance = tmpCovariance;
}

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;
}

 double Model::score(const VectorXd& psi) const
 {
   ROS_ASSERT(psi.rows() == eweights_.rows() + nweights_.rows());
   double val = psi.head(eweights_.rows()).dot(eweights_);
   val += psi.tail(nweights_.rows()).dot(nweights_);
   return val;
 }

void Optimizer::augment_sparse_linear_system(SparseSystem& W,
    const Properties& prop) {
  if (prop.method == DOG_LEG) {
    // We're using the incremental version of Powell's Dog-Leg, so we need
    // to form the updated gradient.
    const VectorXd& f_new = W.rhs();

    // Augment the running count for the sum-of-squared errors at the current
    // linearization point.
    current_SSE_at_linpoint += f_new.squaredNorm();

    // Allocate the new gradient vector
    VectorXd g_new(W.num_cols());

    // Compute W^T \cdot f_new
    VectorXd increment = mul_SparseMatrixTrans_Vector(W, f_new);

    // Set g_new = (g_old 0)^T + W^T f_new.
    g_new.head(gradient.size()) = gradient + increment.head(gradient.size());
    g_new.tail(W.num_cols() - gradient.size()) = increment.tail(
        W.num_cols() - gradient.size());

    // Cache the new gradient vector
    gradient = g_new;
  }

  // Apply Givens to QR factorize the newly augmented sparse system.
  for (int i = 0; i < W.num_rows(); i++) {
    SparseVector new_row = W.get_row(i);
    function_system._R.add_row_givens(new_row, W.rhs()(i));
  }
}

int unimcd(
		VectorXd& y,
		const int& n,
		const int& h,
		const int& len,
		double& initmean,
		double& initcov
	){
	//does NOT accept ties in the y's!
	std::sort(y.data(),y.data()+y.size());
	VectorXd ay(len);
	ay(0)=y.head(h).sum();	
	for(int samp=1;samp<len;samp++) ay(samp)=ay(samp-1)-y(samp-1)+y(samp+h-1);
	VectorXd ay2(len);
	ay2=ay.array().square()/(double)h;
	VectorXd y2(n);
	y2=y.array().square();
	VectorXd sq(len);
	sq(0)=y2.head(h).sum()-ay2(0);
	for(int samp=1;samp<len;samp++) sq(samp)=sq(samp-1)-y2(samp-1)+y2(samp+h-1)-ay2(samp)+ay2(samp-1);
	int minone;
	initcov=sq.minCoeff(&minone);
	initcov/=(double)(h-1);
	initmean=ay(minone)/(double)h;
	return(minone);
}

// Implements Equation S12 in the Supplementary 
MatrixXd expected_dissimilarities(const MatrixXd &J, const MatrixXd &M, const VectorXd &W) {
  int n = J.rows();
  int o = J.cols();
  VectorXd JW = J*W.head(o);
  VectorXd u_n = VectorXd::Ones(n);
  MatrixXd Delta = J*resistance_distance(M,o)*J.transpose();
  Delta.noalias() += 0.5*JW*u_n.transpose();
  Delta.noalias() += 0.5*u_n*JW.transpose();
  Delta.diagonal() -= JW;
  return (Delta);
}

bool inBounds(VectorXd state) {
	Vector2d pos, vel;
	pos = state.head(2);
	vel = state.tail(2);
	if (inside_rectangular_obs(pos, setup_values.x_min, setup_values.x_max,
			setup_values.x_min, setup_values.x_max) &&
		inside_rectangular_obs(vel, setup_values.v_min, setup_values.v_max,
			setup_values.v_min, setup_values.v_max)) {
		return true;
	}
	return false;
}

VectorXd CurvatureError::operator()(const VectorXd& a) const {
    DblVec curvatures;
    DblVec Deltas;
    curvatures = toDblVec(a.head(pi->curvature_vars.size()));
    Deltas = DblVec(pi->curvature_vars.size(), a(a.size() - 1));
    VectorXd ret(1);
    ret(0) = -this->total_curvature_limit;
    for (int i = 0; i < curvatures.size(); ++i) {
        ret(0) += fabs(curvatures[i]) * Deltas[i];
    }
    return ret;
}

IGL_INLINE bool igl::linprog(
  const Eigen::VectorXd & f,
  const Eigen::MatrixXd & A,
  const Eigen::VectorXd & b,
  const Eigen::MatrixXd & B,
  const Eigen::VectorXd & c,
  Eigen::VectorXd & x)
{
  using namespace Eigen;
  using namespace std;
  const int m = A.rows();
  const int n = A.cols();
  const int p = B.rows();
  MatrixXd Im = MatrixXd::Identity(m,m);
  MatrixXd AS(m,n+m);
  AS<<A,Im;
  MatrixXd bS = b.array().abs();
  for(int i = 0;i<m;i++)
  {
    const auto & sign = [](double x)->double
    {
      return (x<0?-1:(x>0?1:0));
    };
    AS.row(i) *= sign(b(i));
  }
  MatrixXd In = MatrixXd::Identity(n,n);
  MatrixXd P(n+m,2*n+m);
  P<<              In, -In, MatrixXd::Zero(n,m),
     MatrixXd::Zero(m,2*n), Im;
  MatrixXd ASP = AS*P;
  MatrixXd BSP(0,2*n+m);
  if(p>0)
  {
    MatrixXd BS(p,2*n);
    BS<<B,MatrixXd::Zero(p,n);
    BSP = BS*P;
  }

  VectorXd fSP = VectorXd::Ones(2*n+m);
  fSP.head(2*n) = P.block(0,0,n,2*n).transpose()*f;
  const VectorXd & cc = fSP;

  MatrixXd AA(m+p,2*n+m);
  AA<<ASP,BSP;
  VectorXd bb(m+p);
  bb<<bS,c;

  VectorXd xxs;
  bool ret = linprog(cc,AA,bb,0,xxs);
  x = P.block(0,0,n,2*n+m)*xxs;
  return ret;
}

void MotionModel::predict_state_and_covariance(VectorXd x_k_k, MatrixXd p_k_k, string type, double std_a,
	double std_alpha, VectorXd *X_km1_k, MatrixXd *P_km1_k)
{
	size_t x_k_size = x_k_k.size(), p_k_size = p_k_k.rows();
	double delta_t = 1, linear_acceleration_noise_covariance, angular_acceleration_noise_covariance;
	VectorXd Xv_km1_k(18), Pn_diag(6);
	MatrixXd F = MatrixXd::Identity(18,18), // The camera state size is assumed to be 18
		Pn, Q, G;

	// Camera motion prediction
	fv(x_k_k.head(18), delta_t, type, std_a, std_alpha, &Xv_km1_k);

	// Feature prediction
	(*X_km1_k).resize(x_k_size);
	// Add the feature points to the state vector after cam motion prediction
	(*X_km1_k) << Xv_km1_k, x_k_k.tail(x_k_size - 18);

	// State transition equation derivatives
	dfv_by_dxv(x_k_k.head(18), delta_t, type, &F);

	// State noise
	linear_acceleration_noise_covariance = (std_a * delta_t) * (std_a * delta_t);
	angular_acceleration_noise_covariance = (std_alpha * delta_t) * (std_alpha * delta_t);
	Pn_diag << linear_acceleration_noise_covariance, linear_acceleration_noise_covariance, linear_acceleration_noise_covariance, 
		angular_acceleration_noise_covariance, angular_acceleration_noise_covariance, angular_acceleration_noise_covariance;
	Pn = Pn_diag.asDiagonal();
	func_Q(x_k_k.head(18), Pn, delta_t, type, &G, &Q);

	// P_km1_k resize and update 
	// With the property of symmetry for the covariance matrix, only the Pxx and Pxy 
	// which means the camera state covariance and camera feature points covariance can be calculated
	(*P_km1_k).resize(p_k_size,p_k_size);
	(*P_km1_k).block(0,0,18,18) = F * p_k_k.block(0,0,18,18) * F.transpose() + Q;
	(*P_km1_k).block(0,18,18,p_k_size - 18) = F * p_k_k.block(0,18,18,p_k_size - 18);
	(*P_km1_k).block(18,0,p_k_size - 18,18) = p_k_k.block(18,0,p_k_size - 18,18) * F.transpose();
	(*P_km1_k).block(18,18,p_k_size - 18,p_k_size - 18) = p_k_k.block(18,18,p_k_size - 18,p_k_size - 18);

}