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

/**
 * @function heuristicCost
 * @brief
 */
double M_RRT::heuristicCost( Eigen::VectorXd node )
{

  Eigen::Transform<double, 3, Eigen::Affine> T;

  // Calculate the EE position
  robinaLeftArm_fk( node, TWBase, Tee, T );

  Eigen::VectorXd trans_ee = T.translation();
  Eigen::VectorXd x_ee = T.rotation().col(0);
  Eigen::VectorXd y_ee = T.rotation().col(1);
  Eigen::VectorXd z_ee = T.rotation().col(2);

  Eigen::VectorXd GH = ( goalPosition - trans_ee );

  double fx1 = GH.norm() ;

  GH = GH/GH.norm();

  double fx2 = abs( GH.dot( x_ee ) - 1 );

  double fx3 = abs( GH.dot( z_ee ) );

  double heuristic = w1*fx1 + w2*fx2 + w3*fx3;     

  return heuristic;
}

/**
 * Returns the expected return of the given portfolio structure.
 *
 * @param portfolio  the portfolio structure
 *
 * @return the expected portfolio return
 */
double EfficientPortfolioWithRisklessAsset::portfolioReturn(const Eigen::VectorXd& portfolio)
{
	Eigen::VectorXd riskyAssets = portfolio.head(dim - 1);

	double riskReturn = riskyAssets.dot(mean);
	double risklessReturn = portfolio(dim - 1) * _risklessRate;

	return riskyAssets.dot(mean) + portfolio(dim - 1) * _risklessRate;
}

  bool FilterBase::checkMahalanobisThreshold(const Eigen::VectorXd &innovation,
                                             const Eigen::MatrixXd &invCovariance,
                                             const double nsigmas)
  {
    double sqMahalanobis = innovation.dot(invCovariance * innovation);
    double threshold = nsigmas*nsigmas;

    if (sqMahalanobis >= threshold)
    {
      if (getDebug())
      {
        *debugStream_ << "Innovation mahalanobis distance test failed. Squared Mahalanobis is\n";
        *debugStream_ << sqMahalanobis << "\n";
        *debugStream_ << "threshold was:\n";
        *debugStream_ << threshold << "\n";
        *debugStream_ << "Innovation:\n";
        *debugStream_ << innovation << "\n";
        *debugStream_ << "Inv covariance:\n";
        *debugStream_ << invCovariance << "\n";
      }
      return false;
    }

    return true;
  }

void constMatrix::gradient_add( const Eigen::VectorXd & X, const Eigen::VectorXd & iV)
{
  Eigen::VectorXd vtmp = Q * X;

  double xtQx =  vtmp.dot(iV.asDiagonal() * vtmp);
  dtau +=  (d - xtQx)/ tau;
  ddtau -=  (d + xtQx)/ pow(tau, 2);
}

double ObjectiveMLSNoCopy::eval(const Eigen::VectorXd& x) const
{
  double obj = 0.0;
  for(int i = 0; i < A_->cols(); ++i)
    obj -= logsig(-x.dot(A_->col(i)) - b_->coeff(i));
  
  obj /= (double)A_->cols();
  return obj;
}

	CircularPathSegment(const Eigen::VectorXd &start, const Eigen::VectorXd &intersection, const Eigen::VectorXd &end, double maxDeviation) {
		if((intersection - start).norm() < 0.000001 || (end - intersection).norm() < 0.000001) {
			length = 0.0;
			radius = 1.0;
			center = intersection;
			x = Eigen::VectorXd::Zero(start.size());
			y = Eigen::VectorXd::Zero(start.size());
			return;
		}

		const Eigen::VectorXd startDirection = (intersection - start).normalized();
		const Eigen::VectorXd endDirection = (end - intersection).normalized();

		if((startDirection - endDirection).norm() < 0.000001) {
			length = 0.0;
			radius = 1.0;
			center = intersection;
			x = Eigen::VectorXd::Zero(start.size());
			y = Eigen::VectorXd::Zero(start.size());
			return;
		}

    // const double startDistance = (start - intersection).norm();
    // const double endDistance = (end - intersection).norm();

		double distance = std::min((start - intersection).norm(), (end - intersection).norm());
		const double angle = acos(startDirection.dot(endDirection));

		distance = std::min(distance, maxDeviation * sin(0.5 * angle) / (1.0 - cos(0.5 * angle)));  // enforce max deviation

		radius = distance / tan(0.5 * angle);
		length = angle * radius;

		center = intersection + (endDirection - startDirection).normalized() * radius / cos(0.5 * angle);
		x = (intersection - distance * startDirection - center).normalized();
		y = startDirection;

		//debug
		double dotStart = startDirection.dot((intersection - getConfig(0.0)).normalized());
		double dotEnd = endDirection.dot((getConfig(length) - intersection).normalized());
    if(std::abs(dotStart - 1.0) > 0.0001 || std::abs(dotEnd - 1.0) > 0.0001) {
			std::cout << "Error\n";
		}
	}

Eigen::VectorXd get_symmetric_point(
    const Eigen::VectorXd& _normal,
    const Eigen::VectorXd& _center,
    const Eigen::VectorXd& _point)
{
    // Assume that '_normal' is normalized.
    Eigen::VectorXd plane_to_point = _normal * _normal.dot(_point - _center);
    Eigen::VectorXd symmetric_point = _point - (plane_to_point * 2);
    return symmetric_point;
}

double cMathUtil::EvalGaussianLogp(const Eigen::VectorXd& mean, const Eigen::VectorXd& covar, const Eigen::VectorXd& sample)
{
	int data_size = static_cast<int>(covar.size());

	Eigen::VectorXd diff = sample - mean;
	double logp = -0.5 * diff.dot(diff.cwiseQuotient(covar));
	double det = covar.prod();
	logp += -0.5 * (data_size * std::log(2 * M_PI) + std::log(det));

	return logp;
}

double cMathUtil::EvalGaussian(const Eigen::VectorXd& mean, const Eigen::VectorXd& covar, const Eigen::VectorXd& sample)
{
	assert(mean.size() == covar.size());
	assert(sample.size() == covar.size());

	Eigen::VectorXd diff = sample - mean;
	double exp_val = diff.dot(diff.cwiseQuotient(covar));
	double likelihood = std::exp(-0.5 * exp_val);

	double partition = CalcGaussianPartition(covar);
	likelihood /= partition;
	return likelihood;
}

  bool FilterBase::checkMahalanobisThreshold(const Eigen::VectorXd &innovation,
                                             const Eigen::MatrixXd &invCovariance,
                                             const double nsigmas)
  {
    double sqMahalanobis = innovation.dot(invCovariance * innovation);
    double threshold = nsigmas * nsigmas;

    if (sqMahalanobis >= threshold)
    {
      FB_DEBUG("Innovation mahalanobis distance test failed. Squared Mahalanobis is: " << sqMahalanobis << "\n" <<
               "Threshold is: " << threshold << "\n" <<
               "Innovation is: " << innovation << "\n" <<
               "Innovation covariance is:\n" << invCovariance << "\n");

      return false;
    }

    return true;
  }

TEST(advi_test, multivar_no_constraint_meanfield) {
  // Create mock data_var_context
  static const std::string DATA = "";
  std::stringstream data_stream(DATA);
  stan::io::dump dummy_context(data_stream);

  // Instantiate model
  Model my_model(dummy_context);

  // RNG
  rng_t base_rng(0);

  // Other params
  int n_monte_carlo_grad = 10;
  std::stringstream output;
  stan::interface_callbacks::writer::stream_writer message_writer(output);

  // Dummy input
  Eigen::VectorXd cont_params = Eigen::VectorXd::Zero(2);
  cont_params(0) = 0.75;
  cont_params(1) = 0.75;

  // ADVI
  stan::variational::advi<Model, stan::variational::normal_meanfield, rng_t>
    test_advi(my_model,
              cont_params,
              base_rng,
              n_monte_carlo_grad,
              1e4, // absurdly high!
              100,
              1);

  // Create some arbitrary variational q() family to calculate the ELBO over
  Eigen::VectorXd mu  = Eigen::VectorXd::Constant(my_model.num_params_r(),
                                                     2.5);
  Eigen::VectorXd sigma_tilde  = Eigen::VectorXd::Constant(
                                          my_model.num_params_r(),
                                          0.0); // initializing sigma_tilde = 0
                                                // means sigma = 1
  stan::variational::normal_meanfield musigmatilde =
    stan::variational::normal_meanfield(mu, sigma_tilde);

  double elbo = 0.0;
  elbo = test_advi.calc_ELBO(musigmatilde, message_writer);

  // Can calculate ELBO analytically
  double zeta = -0.5 * ( 3*2*log(2.0*stan::math::pi()) + 18.5 + 25 + 13 );
  Eigen::VectorXd mu_J = Eigen::VectorXd::Zero(2);
  mu_J(0) = 10.5;
  mu_J(1) =  7.5;

  double elbo_true = 0.0;
  elbo_true += zeta;
  elbo_true += mu_J.dot(mu);
  elbo_true += -0.5 * ( 3*mu.dot(mu) + 3*2 );
  elbo_true += 1 + log(2.0*stan::math::pi());

  double const EPSILON = 0.1;
  EXPECT_NEAR(elbo_true, elbo, EPSILON);

  Eigen::VectorXd mu_grad = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd st_grad = Eigen::VectorXd::Zero(my_model.num_params_r());

  std::string error = "stan::variational::normal_meanfield: "
                      "Dimension of mean vector (3) and "
                      "Dimension of log std vector (2) must match in size";
  EXPECT_THROW_MSG(stan::variational::normal_meanfield(mu_grad, st_grad),
                   std::invalid_argument, error);

  mu_grad = Eigen::VectorXd::Zero(0);

  error = "stan::variational::normal_meanfield: "
          "Dimension of mean vector (0) and "
          "Dimension of log std vector (2) must match in size";
  EXPECT_THROW_MSG(stan::variational::normal_meanfield(mu_grad, st_grad),
                   std::invalid_argument, error);

  mu_grad = Eigen::VectorXd::Zero(my_model.num_params_r());
  st_grad  = Eigen::VectorXd::Zero(3);

  error = "stan::variational::normal_meanfield: "
          "Dimension of mean vector (2) and "
          "Dimension of log std vector (3) must match in size";
  EXPECT_THROW_MSG(stan::variational::normal_meanfield(mu_grad, st_grad),
                   std::invalid_argument, error);

  mu_grad = Eigen::VectorXd::Zero(my_model.num_params_r());
  st_grad  = Eigen::VectorXd::Zero(0);

  error = "stan::variational::normal_meanfield: "
          "Dimension of mean vector (2) and "
          "Dimension of log std vector (0) must match in size";
  EXPECT_THROW_MSG(stan::variational::normal_meanfield(mu_grad, st_grad),
                   std::invalid_argument, error);

  mu_grad = Eigen::VectorXd::Zero(3);
  st_grad  = Eigen::VectorXd::Zero(3);
  stan::variational::normal_meanfield elbo_grad = stan::variational::normal_meanfield(mu_grad, st_grad);

  error = "stan::variational::normal_meanfield::calc_grad: "
//.........这里部分代码省略.........

//.........这里部分代码省略.........
   * Preprocessing phase
   */
	
  /* compute the trace of the original matrix G */
  c1 = G.trace();
	
	/* decompose the matrix G in the form LL^T */
  chol.compute(G);
 
  /* initialize the matrix R */
  d.setZero();
  R.setZero();
	R_norm = 1.0; /* this variable will hold the norm of the matrix R */
  
	/* compute the inverse of the factorized matrix G^-1, this is the initial value for H */
  // J = L^-T
  J.setIdentity();
  J = chol.matrixU().solve(J);
	c2 = J.trace();
#ifdef TRACE_SOLVER
 print_matrix("J", J, n);
#endif
  
	/* c1 * c2 is an estimate for cond(G) */
  
	/* 
   * Find the unconstrained minimizer of the quadratic form 0.5 * x G x + g0 x 
   * this is a feasible point in the dual space
	 * x = G^-1 * g0
   */
  x = chol.solve(g0);
  x = -x;
	/* and compute the current solution value */ 
	f_value = 0.5 * g0.dot(x);
#ifdef TRACE_SOLVER
  std::cerr << "Unconstrained solution: " << f_value << std::endl;
  print_vector("x", x, n);
#endif
  
	/* Add equality constraints to the working set A */
  iq = 0;
	for (i = 0; i < me; i++)
	{
    np = CE.col(i);
    compute_d(d, J, np);
		update_z(z, J, d,  iq);
		update_r(R, r, d,  iq);
#ifdef TRACE_SOLVER
		print_matrix("R", R, iq);
		print_vector("z", z, n);
		print_vector("r", r, iq);
		print_vector("d", d, n);
#endif
    
    /* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint 
      becomes feasible */
    t2 = 0.0;
    if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
      t2 = (-np.dot(x) - ce0(i)) / z.dot(np);
    
    x += t2 * z;

    /* set u = u+ */
    u(iq) = t2;
    u.head(iq) -= t2 * r.head(iq);
    

//.........这里部分代码省略.........
                if (vertex_flag[index_j[p]]) {
                    for (int k=0; k<3; ++k) 
                        for (int dim=0; dim < 3; ++dim)
                            Y[dim](row + k) +=  weight_smooth*Q_j_hat(p, k)*dst.vertex_coord[vertex_real_col[index_j[p]]][dim];
                } else {
                    for (int k=0; k<3; ++k) 
                        A.coeffRef(row+k, src.poly_num + vertex_real_col[index_j[p]]) += -weight_smooth*Q_j_hat(p, k);
                }
            }
            row += 3;
        }
    }
    energy_size[0] = row;

    double weight_identity = weights[1];
    for (int i=0; i<src.poly_num; ++i) {
        Eigen::Matrix3d& Q_i_hat = Q_hat_inverse[i];
        unsigned int* index_i = src.polyIndex[i].vert_index;
        Y[0](row) = weight_identity;    Y[0](row+1) = 0.0;              Y[0](row+2) = 0.0; 
        Y[1](row) = 0.0;                Y[1](row+1) = weight_identity;  Y[1](row+2) = 0.0; 
        Y[2](row) = 0.0;                Y[2](row+1) = 0.0;              Y[2](row+2) = weight_identity; 

        for (int k=0; k<3; ++k)    
            A.coeffRef(row+k, i) = weight_identity*Q_i_hat(0, k);   //n

        if (vertex_flag[index_i[0]]) {
            for (int k=0; k<3; ++k) 
                for (int dim = 0; dim < 3; ++dim)
                    Y[dim](row+k) += weight_identity*(Q_i_hat(1, k)+Q_i_hat(2,k))*dst.vertex_coord[vertex_real_col[index_i[0]]][dim];
        } else {
            for (int k=0; k<3; ++k) 
                A.coeffRef(row+k, src.poly_num+vertex_real_col[index_i[0]]) = -weight_identity*(Q_i_hat(1, k)+Q_i_hat(2,k));
        }

        for (int p=1; p<3; ++p) {
            if (vertex_flag[index_i[p]]) {
                for (int k=0; k<3; ++k)
                    for (int dim=0; dim<3; ++dim)
                        Y[dim](row + k) += -weight_identity*Q_i_hat(p, k)*dst.vertex_coord[vertex_real_col[index_i[p]]][dim];
            } else {
                for (int k=0; k<3; ++k) 
                    A.coeffRef(row+k, src.poly_num + vertex_real_col[index_i[p]]) = weight_identity*Q_i_hat(p, k);
            }
        }

        row += 3;
    }
    energy_size[1] = row;

    double weight_soft_constraint = weights[2];
    for (int i=0; i<soft_corres.size(); ++i) {
        if (vertex_flag[soft_corres[i].first]) continue;
        A.coeffRef(row, src.poly_num + vertex_real_col[soft_corres[i].first]) = weight_soft_constraint;
        for (int dim=0; dim<3; ++dim)
            Y[dim](row) += weight_soft_constraint*dst.vertex_coord[soft_corres[i].second][dim];
        ++row;
    }
    energy_size[2] = row;

    assert (row == rows);
    
       //start solving the least-square problem
    fprintf(stdout, "finished filling matrix\n");
    Eigen::SparseMatrix<double> At = A.transpose();
    Eigen::SparseMatrix<double> AtA = At*A;

    Eigen::SimplicialCholesky<SparseMatrix<double>> solver;
    solver.compute(AtA);
    if (solver.info() != Eigen::Success) {
        fprintf(stdout, "unable to defactorize AtA\n");
        exit(-1);
    }

	VectorXd X[3];
	for (int i=0; i<3; ++i) {
		VectorXd AtY = At*Y[i];
		X[i] = solver.solve(AtY);
		Eigen::VectorXd Energy = A*X[i] - Y[i];
		Eigen::VectorXd smoothEnergy = Energy.head(energy_size[0]);
		Eigen::VectorXd identityEnergy = Energy.segment(energy_size[0], energy_size[1]-energy_size[0]);
		Eigen::VectorXd softRegularEnergy = Energy.tail(energy_size[2]-energy_size[1]);
		fprintf(stdout, "\t%lf = %lf + %lf + %lf\n", 
			Energy.dot(Energy), smoothEnergy.dot(smoothEnergy), 
			identityEnergy.dot(identityEnergy), softRegularEnergy.dot(softRegularEnergy));
	}
    
    //fill data back to src
    for (int i=0; i<src.poly_num; ++i)
        for (int d=0; d<3; ++d)
            src.face_norm[i][d] = X[d](i);
    for (size_t i=0; i<corres.size(); ++i) src.vertex_coord[corres[i].first] = dst.vertex_coord[corres[i].second];
    int p = 0;
    for (int i=0; i<src.vert_num; ++i) {
        if (vertex_flag[i]) continue;
        for (int d=0; d<3; ++d)
            src.vertex_coord[i][d] = X[d](src.poly_num+p);
        ++p;
    }
    return;
}

/**
 * Returns the variance of the given portfolio structure.
 *
 * @param portfolio  the portfolio structure
 *
 * @return the portfolio variance
 */
double EfficientPortfolioWithRisklessAsset::portfolioVariance(const Eigen::VectorXd& portfolio)
{
	Eigen::VectorXd riskyAssets = portfolio.head(dim - 1);

	return riskyAssets.dot(variance * riskyAssets);
}

		double Statistics::scalarProduct(const Eigen::VectorXd& cv)
		{
			return cv.dot(cv);
		}