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

double cosangle(VectorXd a, VectorXd b) {
    if (a.norm() < 1e-6 || b.norm() < 1e-6) {
        return 1;
    } else {
        return a.dot(b) / (a.norm() * b.norm());
    }
}

VectorXd Optimizer::compute_dog_leg(double alpha, const VectorXd& h_sd,
    const VectorXd& h_gn, double delta, double& gain_ratio_denominator) {
  if (h_gn.norm() <= delta) {
    gain_ratio_denominator = current_SSE_at_linpoint;
    return h_gn;
  }

  double h_sd_norm = h_sd.norm();

  if ((alpha * h_sd_norm) >= delta) {
    gain_ratio_denominator = delta * (2 * alpha * h_sd_norm - delta)
        / (2 * alpha);
    return (delta / h_sd_norm) * h_sd;
  } else {
    // complicated case: calculate intersection of trust region with
    // line between Gauss-Newton and steepest descent solutions
    VectorXd a = alpha * h_sd;
    VectorXd b = h_gn;
    double c = a.dot(b - a);
    double b_a_norm2 = (b - a).squaredNorm();
    double a_norm2 = a.squaredNorm();
    double delta2 = delta * delta;
    double sqrt_term = sqrt(c * c + b_a_norm2 * (delta2 - a_norm2));
    double beta;
    if (c <= 0) {
      beta = (-c + sqrt_term) / b_a_norm2;
    } else {
      beta = (delta2 - a_norm2) / (c + sqrt_term);
    }

    gain_ratio_denominator = .5 * alpha * (1 - beta) * (1 - beta) * h_sd_norm
        * h_sd_norm + beta * (2 - beta) * current_SSE_at_linpoint;
    return (alpha * h_sd + beta * (h_gn - alpha * h_sd));
  }
}

double probutils::eigpower (const MatrixXd& A, VectorXd& eigvec)
{
  // Check if A is square
  if (A.rows() != A.cols())
    throw invalid_argument("Matrix A must be square!");

  // Check if A is a scalar
  if (A.rows() == 1)
  {
    eigvec.setOnes(1);
    return A(0,0);
  }

  // Initialise working vectors
  VectorXd v = VectorXd::LinSpaced(A.rows(), -1, 1);
  VectorXd oeigvec(A.rows());

  // Initialise eigenvalue and eigenvectors etc
  double eigval = v.norm();
  double vdist = numeric_limits<double>::infinity();
  eigvec = v/eigval;

  // Loop until eigenvector converges or we reach max iterations
  for (int i=0; (vdist>EIGCONTHRESH) && (i<MAXITER); ++i)
  {
    oeigvec = eigvec;
    v.noalias() = A * oeigvec;
    eigval = v.norm();
    eigvec = v/eigval;
    vdist = (eigvec - oeigvec).norm();
  }

  return eigval;
}

    List fastLm(Rcpp::NumericMatrix Xs, Rcpp::NumericVector ys, int type) {
        const Map<MatrixXd>  X(as<Map<MatrixXd> >(Xs));
        const Map<VectorXd>  y(as<Map<VectorXd> >(ys));
        Index                n = X.rows();
        if ((Index)y.size() != n) throw invalid_argument("size mismatch");

			    // Select and apply the least squares method
        lm                 ans(do_lm(X, y, type));

			    // Copy coefficients and install names, if any
        NumericVector     coef(wrap(ans.coef()));

        List          dimnames(NumericMatrix(Xs).attr("dimnames"));
        if (dimnames.size() > 1) {
            RObject   colnames = dimnames[1];
            if (!(colnames).isNULL())
                coef.attr("names") = clone(CharacterVector(colnames));
        }
	    
        VectorXd         resid = y - ans.fitted();
        int               rank = ans.rank();
        int                 df = (rank == ::NA_INTEGER) ? n - X.cols() : n - rank;
        double               s = resid.norm() / std::sqrt(double(df));
			    // Create the standard errors
        VectorXd            se = s * ans.se();

        return List::create(_["coefficients"]  = coef,
                            _["se"]            = se,
                            _["rank"]          = rank,
                            _["df.residual"]   = df,
                            _["residuals"]     = resid,
                            _["s"]             = s,
                            _["fitted.values"] = ans.fitted());
    }

VectorXd MotionModel::V2Q(VectorXd V)
{
	// Converts from rotation vector to quaternion representation
	// Javier Civera book P130. A.15
	size_t V_size;
	double norm = 0, v_norm = 0;
	VectorXd Q(4), V_norm;
	V_size = V.size();
	// Norm of the V
	//for (i = 0; i < V_size; ++i)
	//{
	//	norm += V(i) * V(i);
	//}
	//norm = sqrt(norm);

	norm = V.norm();
	if (norm < eps)
		Q << 1, 0, 0, 0;
	else
	{
		V_norm = V / norm;
		//for (i = 0; i < V_size; ++i)
		//{
		//	v_norm += V_norm(i) * V_norm(i);
		//}
		//v_norm = sqrt(v_norm);

		v_norm = V_norm.norm();
		// Quaternion cos(theta/2)+sin(theta/2)uxI+sin(theta/2)uyJ+sin(theta/2)uzK
		Q << cos(norm / 2), sin(norm / 2) * V_norm / v_norm;
	}
	return Q; // Q is a 4x1 vector
}

void SolveAmpere()
{
    int i,j;

    // First allocate the matrix and vectors for the finite difference scheme, source, and solution
    MatrixXd AmMatrix = MatrixXd::Zero((M)*(N),(M)*(N));
    VectorXd Source = VectorXd::Zero((M)*(N));
    VectorXd Soln = VectorXd::Zero((M)*(N));

    // The finite difference scheme is applied
    createAmpereMatrix(AmMatrix,Source);

    // Now call LinAlgebra package to invert the matrix and obtain the new potential values
    Soln = AmMatrix.colPivHouseholderQr().solve(Source);

    // Test to make sure solution is actually a solution (Uses L2 norm)
    double L2error = (AmMatrix*Soln - Source).norm() / Source.norm();

    //cout << (AmMatrix) << endl;
    //cout << Source << endl;
    //cout << (AmMatrix*Soln - Source) << endl;

    cout << "Ampere Matrix L2 error = " << L2error << endl;

    // Copy the solution to the newState vector
    for(i=0;i<M;i++) for(j=0;j<N;j++) newState[Apot][i][j] = Soln(Sidx(i,j));

    // Apot has been updated, we don't need to renormalize
};

 virtual double getEnergy(const VectorXd &q) const
 {
     VectorXd gradq;
     SparseMatrix<double> dgdq, hessq;
     int derivs = ElasticEnergy::DR_DQ;
     double energyB, energyS;
     m_.elasticEnergy(q_, q, energyB, energyS, gradq, hessq, dgdq, derivs);
     return gradq.norm();
 }

 virtual double getEnergyAndDerivatives(const VectorXd &q, VectorXd &grad, SparseMatrix<double> &hess) const
 {
     SparseMatrix<double> dgdq, hessq;
     VectorXd gradq;
     double energyB, energyS;
     int derivs = ElasticEnergy::DR_DQ | ElasticEnergy::DR_DGDQ;
     m_.elasticEnergy(q_, q, energyB, energyS, gradq, hessq, dgdq, derivs);
     grad = dgdq*gradq;
     hess = dgdq*dgdq.transpose();
     return gradq.norm();
 }

// [[Rcpp::export]]
double R2pred(MatrixXd Xt, VectorXd yt, MatrixXd Xv, VectorXd yv) {
    const VectorXd coefHat(betaHat(Xt, yt));
    const VectorXd fitted(Xv * coefHat);
    const VectorXd resid(yv - fitted);
    const VectorXd residNull(dev(yv));
    const double SSerr(pow(resid.norm(), 2));
    const double SStot(pow(residNull.norm(), 2));
    // Rcpp::Rcout << SSerr << "\n\n" << SStot << std::endl;
    const double out(1 - (SSerr/SStot));
    return out;
}

// [[Rcpp::export]]
double R2(MatrixXd X, VectorXd y) {
    // const double n(X.rows());  // FIXME:  not needed?
    const VectorXd coefHat(betaHat(X, y));
    const VectorXd fitted(X * coefHat);
    const VectorXd resid(y - fitted);
    const VectorXd residNull(dev(y));
    const double SSerr(pow(resid.norm(), 2));
    const double SStot(pow(residNull.norm(), 2));
    // Rcpp::Rcout << SSerr << "\n\n" << SStot << std::endl;
    const double out(1 - (SSerr/SStot));
    return out;
}

VectorXd LowRankRepresentation::solve_l2(VectorXd& w, double lambda)
{
    double nw;
    VectorXd x;

    nw = w.norm();
    if(nw > lambda)
        x = (nw - lambda) * w / nw;
    else
        x = VectorXd::Zero(w.size(),1);
    return x;
}

void function1_grad(const real_1d_array &x, double &func, real_1d_array &grad, void *ptr){

  CasADi::SXFunction &fun = ((CasADi::SXFunction*)ptr)[0];
  CasADi::SXFunction &grad_fun = ((CasADi::SXFunction*)ptr)[1];

  VectorXd eig_x(x.length());
  for (int i = 0; i < eig_x.size(); ++i)
	eig_x[i] = x[i];
  
  static VectorXd f,g;
  CASADI::evaluate(fun, eig_x, f);
  CASADI::evaluate(grad_fun, eig_x, g);
  assert_eq(f.size(),1);
  assert_eq(g.size(), grad.length());
  assert_eq(g.norm(), g.norm());
  assert_eq(f[0], f[0]);

  func = f[0];
  for (int i = 0; i < g.size(); ++i)
    grad[i] = g[i];

  DEBUG_LOG("f = "<<func);
}

// Method performs a step towards target; such target could  
// be a 3D vector (X,Y,Z) or a 6D vector (X,Y,Z,R,P,Y)
VectorXd JointMover::OneStepTowardsXYZRPY( VectorXd _q, VectorXd _targetXYZRPY) {
  assert(_targetXYZRPY.size() >= 3);
  assert(_q.size() > 3);
  bool both = (_targetXYZRPY.size() == 6);
  //GetXYZ also updates the config to _q, so Jaclin use an updated value
  VectorXd delta = _targetXYZRPY - GetXYZRPY(_q, both); 
  //if target also specifies roll, pitch and yaw compute entire Jacobian; otherwise, just translational Jacobian
  VectorXd dConfig = GetPseudoInvJac(both)*delta;
  
  // Constant to not let vector to be larger than mConfigStep
  double alpha = min((mConfigStep/dConfig.norm()), 1.0);
  dConfig = alpha*dConfig;
  return _q + dConfig;
}

/* Computes the minimum of a function f:: R^n -> R using the BFGS method */
VectorXd minimize_bfgs(fcn_Rn_to_R f, VectorXd x0, fcn_Rn_to_Rn Df ) {

	cout << "Begin BFGS minimization";

	// Initial checks

	VectorXd p, s, x, Dfx , Dfx_next, y;

	// Initial guess of Hessian
	int N = x0.size();
	MatrixXd B = x0.norm() * MatrixXd::Identity(N, N);
	MatrixXd term3;
	double alpha = 1;

	x = x0;  // Should this be done in place?
	Dfx = Df(x0);
	int MAX_ITER = 5000;
	int iter = 0;

	double tol = 1e-5;


	while( (Dfx.norm() > tol) && (iter < MAX_ITER)) {
		cout << ".";
		p = -B.ldlt().solve(Dfx);
		p.normalize();

		alpha = line_search(f, x, p, Dfx);
		s = alpha*p;
		x += s;

		Dfx_next = Df(x);
		y = Dfx_next - Dfx;

		term3 = B*s*s.transpose()*B/(s.transpose()*B*s);
		B +=  (y*y.transpose())/(y.transpose()*s) - term3;

		Dfx = Dfx_next;
		
		iter++;
	}
	cout << endl;

	if (iter >= MAX_ITER){
		cout << "WARNING: Reached max iterations before converging" << endl;
	}

	return x;
}

// Method performs a Jacobian towards specified target; such target could  
// be a 3D vector (X,Y,Z) or a 6D vector (X,Y,Z,R,P,Y)
bool JointMover::GoToXYZRPY( VectorXd _qStart, VectorXd _targetXYZRPY, VectorXd &_qResult, std::list<Eigen::VectorXd> &path) {
  _qResult = _qStart;
  bool both = (_targetXYZRPY.size() == 6);
  // GetXYZ also updates the config to _qResult, so Jaclin use an updated value
  VectorXd delta = _targetXYZRPY - GetXYZRPY(_qResult, both); 
  
  int iter = 0;
  while( delta.norm() > mWorkspaceThresh && iter < mMaxIter ) {
    _qResult = OneStepTowardsXYZRPY(_qResult, _targetXYZRPY);
    path.push_back(_qResult);
    delta = (_targetXYZRPY - GetXYZRPY(_qResult, both) );
    //PRINT(delta.norm());
    iter++;
  }
  return iter < mMaxIter;
}