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

void LiftingLine::solve_for_best_gamma(double cL)
{
    int matsize = this->segments.size() + 1;
    Eigen::MatrixXd matrix;
    Eigen::VectorXd rhs;
    Eigen::VectorXd result;
    matrix.resize(matsize, matsize);
    matrix.setZero();
    rhs.resize(matsize);
    rhs.setZero();
    result.resize(matsize);
    result.setZero();
    //   adding the main min-function
    for (int i = 0; i < (matsize - 1); i++)
    {
        for (int j = 0; j < (matsize - 1); j++)
        {
            matrix(i, j) += this->segments[i].b() * this->segments[j].ind_influence(this->segments[i]);
            matrix(i, j) += this->segments[j].b() * this->segments[i].ind_influence(this->segments[j]);
        }
    //     adding lagrange multiplicator
        matrix(i, matsize - 1) += this->segments[i].lift_factor;
    }
    for (int i = 0; i < (matsize -1); i++)
    {
        matrix(matsize - 1, i) += this->segments[i].lift_factor;
    }
    rhs(matsize - 1) += cL;
    
    result = matrix.fullPivHouseholderQr().solve(rhs);
    for (int i = 0; i < matsize - 1; i++)
    {
        this->segments[i].best_gamma = result[i];
    }
}

 segment_info(unsigned int nc) {
     E.resize(6, nc);
     E_tilde.resize(6, nc);
     G.resize(nc);
     M.resize(nc, nc);
     EZ.resize(nc);
     E.setZero();
     E_tilde.setZero();
     M.setZero();
     G.setZero();
     EZ.setZero();
 };

 segment_info(unsigned int nc):
     D(0),nullspaceAccComp(0),constAccComp(0),biasAccComp(0),totalBias(0),u(0)
 {
     E.resize(6, nc);
     E_tilde.resize(6, nc);
     G.resize(nc);
     M.resize(nc, nc);
     EZ.resize(nc);
     E.setZero();
     E_tilde.setZero();
     M.setZero();
     G.setZero();
     EZ.setZero();
 };

//==============================================================================
void Spline::evaluateDerivative(
    double _t, int _derivative, Eigen::VectorXd& _tangentVector) const
{
  if (mSegments.empty())
    throw std::logic_error("Unable to evaluate empty trajectory.");
  if (_derivative < 1)
    throw std::logic_error("Derivative must be positive.");

  const auto targetSegmentInfo = getSegmentForTime(_t);
  const auto& targetSegment = mSegments[targetSegmentInfo.first];
  const auto evaluationTime = _t - targetSegmentInfo.second;

  // Return zero for higher-order derivatives.
  if (_derivative < targetSegment.mCoefficients.cols())
  {
    // TODO: We should transform this into the body frame using the adjoint
    // transformation.
    _tangentVector = evaluatePolynomial(
        targetSegment.mCoefficients, evaluationTime, _derivative);
  }
  else
  {
    _tangentVector.resize(mStateSpace->getDimension());
    _tangentVector.setZero();
  }
}

Eigen::VectorXd KronProdSPMat2(
    Eigen::SparseMatrix<double, Eigen::RowMajor> a0,
    Eigen::SparseMatrix<double, Eigen::RowMajor> a1,
    Eigen::VectorXd y) {

    signal(SIGSEGV, handler);   // install our handler
    Eigen::VectorXd retvec;
    retvec.setZero( a0.rows() * a1.rows()  );

    //loop rows a0
    for (int row_idx0=0; row_idx0<a0.outerSize(); ++row_idx0) {
        int row_offset1 = row_idx0;
        row_offset1    *= a1.rows();

        // loop rows a1
        for (int row_idx1=0; row_idx1<a1.outerSize(); ++row_idx1) {

            // loop cols a0 (non-zero elements only)
            for (Eigen::SparseMatrix<double,RowMajor>::InnerIterator it0(a0,row_idx0); it0; ++it0) {
                int col_offset1 = it0.index();
                col_offset1    *= a1.innerSize();
                double factor1 = it0.value();

                for (Eigen::SparseMatrix<double,RowMajor>::InnerIterator it1(a1,row_idx1); it1; ++it1) {
                    retvec( row_offset1 + row_idx1 ) += factor1 * it1.value() * y( col_offset1 + it1.index() );
                }
            }
        }
    }
    return retvec;
}

	void log_gradient_and_output(size_t variable_idx, const Eigen::VectorXd& inputs, const Eigen::VectorXd& parameters, Eigen::VectorXd& outputs, Eigen::VectorXd& gradient_vector)
	{
		// TODO: Check concept for InputIterator

		//double N = std::distance(first_input, last_input);
		//double scaling_factor = 1. / view.size();
		gradient_vector.setZero();

		// DEBUG
		//std::cout << gradient_ << std::endl;

		//for (unsigned int i = 0; i < view.size(); i++) {
		forward_propagation(parameters, inputs, outputs, true);

		// DEBUG: Look for NaN
		/*for (size_t idx = 0; idx < static_cast<size_t>(outputs.size()); idx++) {
		if (std::isnan(outputs[idx]))
		std::cout << "NaN: Output[" << idx << "] from forward propagation" << std::endl;
		}*/

		// TODO: Check that outputs isn't overwritten by back propagation!
		// back_propagation_output(size_t variable_idx, const Eigen::VectorXd& parameters, const Eigen::MatrixBase<T>& inputs, Eigen::VectorXd& outputs, Eigen::VectorXd& gradient, double scaling_factor)
		back_propagation_log_output(variable_idx, parameters, inputs, inputs_to_output_activation_, gradient_vector, 1.);
		//}

		// DEBUG
		//std::cout << gradient_ << std::endl;
	}

void SubSystem::getParams(Eigen::VectorXd &xOut)
{
    if (xOut.size() != psize)
        xOut.setZero(psize);

    for (int i=0; i < psize; i++)
        xOut[i] = pvals[i];
}

template <typename PointSource, typename PointTarget> void
pcl::registration::TransformationEstimationLM<PointSource, PointTarget>::estimateRigidTransformation (
    const pcl::PointCloud<PointSource> &cloud_src,
    const pcl::PointCloud<PointTarget> &cloud_tgt,
    Eigen::Matrix4f &transformation_matrix)
{

  // <cloud_src,cloud_src> is the source dataset
  if (cloud_src.points.size () != cloud_tgt.points.size ())
  {
    PCL_ERROR ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] ");
    PCL_ERROR ("Number or points in source (%zu) differs than target (%zu)!\n", 
               cloud_src.points.size (), cloud_tgt.points.size ());
    return;
  }
  if (cloud_src.points.size () < 4)     // need at least 4 samples
  {
    PCL_ERROR ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] ");
    PCL_ERROR ("Need at least 4 points to estimate a transform! Source and target have %zu points!\n", 
               cloud_src.points.size ());
    return;
  }

  // If no warp function has been set, use the default (WarpPointRigid6D)
  if (!warp_point_)
    warp_point_.reset (new WarpPointRigid6D<PointSource, PointTarget>);

  int n_unknowns = warp_point_->getDimension ();
  Eigen::VectorXd x (n_unknowns);
  x.setZero ();
  
  // Set temporary pointers
  tmp_src_ = &cloud_src;
  tmp_tgt_ = &cloud_tgt;

  OptimizationFunctor functor (static_cast<int> (cloud_src.points.size ()), this);
  Eigen::NumericalDiff<OptimizationFunctor> num_diff (functor);
  Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, double> lm (num_diff);
  int info = lm.minimize (x);

  // Compute the norm of the residuals
  PCL_DEBUG ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation]");
  PCL_DEBUG ("LM solver finished with exit code %i, having a residual norm of %g. \n", info, lm.fvec.norm ());
  PCL_DEBUG ("Final solution: [%f", x[0]);
  for (int i = 1; i < n_unknowns; ++i) 
    PCL_DEBUG (" %f", x[i]);
  PCL_DEBUG ("]\n");

  // Return the correct transformation
  Eigen::VectorXf params = x.cast<float> ();
  warp_point_->setParam (params);
  transformation_matrix = warp_point_->getTransform ();

  tmp_src_ = NULL;
  tmp_tgt_ = NULL;
}

void buildProblem(std::vector<T>& coefficients, Eigen::VectorXd& b, int n)
{
  b.setZero();
  Eigen::ArrayXd boundary = Eigen::ArrayXd::LinSpaced(n, 0,M_PI).sin().pow(2);
  for(int j=0; j<n; ++j)
  {
    for(int i=0; i<n; ++i)
    {
      int id = i+j*n;
      insertCoefficient(id, i-1,j, -1, coefficients, b, boundary);
      insertCoefficient(id, i+1,j, -1, coefficients, b, boundary);
      insertCoefficient(id, i,j-1, -1, coefficients, b, boundary);
      insertCoefficient(id, i,j+1, -1, coefficients, b, boundary);
      insertCoefficient(id, i,j,    4, coefficients, b, boundary);
    }
  }
}

void SubSystem::calcGrad(VEC_pD &params, Eigen::VectorXd &grad)
{
    assert(grad.size() == int(params.size()));

    grad.setZero();
    for (int j=0; j < int(params.size()); j++) {
        MAP_pD_pD::const_iterator
          pmapfind = pmap.find(params[j]);
        if (pmapfind != pmap.end()) {
            // assert(p2c.find(pmapfind->second) != p2c.end());
            std::vector<Constraint *> constrs=p2c[pmapfind->second];
            for (std::vector<Constraint *>::const_iterator constr = constrs.begin();
                 constr != constrs.end(); ++constr)
                grad[j] += (*constr)->error() * (*constr)->grad(pmapfind->second);
        }
    }
}

	void gradient(View& view, const Eigen::VectorXd& parameters, Eigen::VectorXd& gradient_vector)
	{
		// TODO: Check concept for InputIterator

		//double N = std::distance(first_input, last_input);
		double scaling_factor = 1. / view.size();
		gradient_vector.setZero();

		// DEBUG
		//std::cout << gradient_ << std::endl;

		for (unsigned int i = 0; i < view.size(); i++) {
			forward_propagation(parameters, view.first(i), outputs());
			back_propagation_error(parameters, view.first(i), outputs(), view.second(i), gradient_vector, scaling_factor);
		}

		// DEBUG
		//std::cout << gradient_ << std::endl;
	}

 SimuEuro(const option & o, long path, const std::vector<double> & RN){
     opt=o;
     N= path;
     asset_price.resize(N);
     asset_price.setZero();
     option_value= asset_price;
     
     for (long i=0; i< N; i++) {
         asset_price(i)=opt.S* exp((opt.r- opt.q)*opt.T-.5*opt.sigma*opt.sigma*opt.T+ opt.sigma* sqrt(opt.T)* RN[i]);
         
         if(opt.Call) option_value(i)= fmax(asset_price(i)- opt.K,0.0);
         else option_value(i)= fmax(-asset_price(i)+opt.K, 0.0);
     }
     
     mean= option_value.sum()/ option_value.size() * exp(-opt.T*opt.r);
     stdiv= option_value.squaredNorm()/ option_value.size()* exp(-opt.r*opt.T *2);
     stdiv= stdiv- pow(mean,2.0);
     stdiv= sqrt(stdiv/ N);
 };

// Computes the difference vector of all design variables between the linearization point at marginalization and the current guess, on the tangent space (i.e. log(x_bar - x))
Eigen::VectorXd MarginalizationPriorErrorTerm::getDifferenceSinceMarginalization()
{
  Eigen::VectorXd diff = Eigen::VectorXd(_dimensionDesignVariables);
  diff.setZero();
  int index = 0;
  std::vector<Eigen::MatrixXd>::iterator it_marg = _designVariableValuesAtMarginalization.begin();
  std::vector<aslam::backend::DesignVariable*>::iterator it_current = _designVariables.begin();
  for(;it_current != _designVariables.end(); ++it_current, ++it_marg)
  {
	  // retrieve current value (xbar) and value at marginalization(xHat)
      Eigen::MatrixXd xHat = *it_marg;
      //get minimal difference in tangent space
      Eigen::VectorXd diffVector;
      (*it_current)->minimalDifference(xHat, diffVector);
      //int base = index;
      int dim = diffVector.rows();
      diff.segment(index, dim) = diffVector;
      index += dim;
  }
  return diff;
}

	void KernelBand<NLSSystemObject>::CalculatesNormOfJacobianRows(Eigen::VectorXd& row_norms)
	{
		row_norms.setZero();

		for (int group = 1; group <= ngroups_; group++)
		{
			for (int j = group - 1; j < static_cast<int>(this->ne_); j += width_)
			{
				double* col_j = BAND_COL(J_, j);
				int i1 = std::max(0, j - J_->nUpper());
				int i2 = std::min(j + J_->nLower(), static_cast<int>(this->ne_ - 1));

				for (int i = i1; i <= i2; i++)
				{
					const double J = BAND_COL_ELEM(col_j, i, j);
					row_norms(i) += J*J;
				}
			}
		}

		for (unsigned int i = 0; i < this->ne_; i++)
			row_norms(i) = std::sqrt(row_norms(i));
	}

IGL_INLINE double igl::polyvector_field_poisson_reconstruction(
  const Eigen::PlainObjectBase<DerivedV> &Vcut,
  const Eigen::PlainObjectBase<DerivedF> &Fcut,
  const Eigen::PlainObjectBase<DerivedS> &sol3D_combed,
  Eigen::PlainObjectBase<DerivedSF> &scalars,
  Eigen::PlainObjectBase<DerivedS> &sol3D_recon,
  Eigen::PlainObjectBase<DerivedE> &max_error )
  {
    Eigen::SparseMatrix<typename DerivedV::Scalar> gradMatrix;
    igl::grad(Vcut, Fcut, gradMatrix);

    Eigen::VectorXd FAreas;
    igl::doublearea(Vcut, Fcut, FAreas);
    FAreas = FAreas.array() * .5;

    int nf = FAreas.rows();
    Eigen::SparseMatrix<typename DerivedV::Scalar> M,M1;
    Eigen::VectorXi II = igl::colon<int>(0, nf-1);

    igl::sparse(II, II, FAreas, M1);
    igl::repdiag(M1, 3, M) ;

    int half_degree = sol3D_combed.cols()/3;

    sol3D_recon.setZero(sol3D_combed.rows(),sol3D_combed.cols());

    int numF = Fcut.rows();
    scalars.setZero(Vcut.rows(),half_degree);

    Eigen::SparseMatrix<typename DerivedV::Scalar> Q = gradMatrix.transpose()* M *gradMatrix;

    //fix one point at Ik=fix, value at fixed xk=0
    int fix = 0;
    Eigen::VectorXi Ik(1);Ik<<fix;
    Eigen::VectorXd xk(1);xk<<0;

    //unknown indices
    Eigen::VectorXi Iu(Vcut.rows()-1,1);
    Iu<<igl::colon<int>(0, fix-1),  igl::colon<int>(fix+1,Vcut.rows()-1);

    Eigen::SparseMatrix<typename DerivedV::Scalar> Quu, Quk;
    igl::slice(Q, Iu, Iu, Quu);
    igl::slice(Q, Iu, Ik, Quk);
    Eigen::SimplicialLDLT<Eigen::SparseMatrix<typename DerivedV::Scalar> > solver;
    solver.compute(Quu);


    Eigen::VectorXd vec; vec.setZero(3*numF,1);
    for (int i =0; i<half_degree; ++i)
    {
      vec<<sol3D_combed.col(i*3+0),sol3D_combed.col(i*3+1),sol3D_combed.col(i*3+2);
      Eigen::VectorXd b = gradMatrix.transpose()* M * vec;
      Eigen::VectorXd bu = igl::slice(b, Iu);

      Eigen::VectorXd rhs = bu-Quk*xk;
      Eigen::MatrixXd yu = solver.solve(rhs);

      Eigen::VectorXi index = i*Eigen::VectorXi::Ones(Iu.rows(),1);
      igl::slice_into(yu, Iu, index, scalars);scalars(Ik[0],i)=xk[0];
    }

    //    evaluate gradient of found scalar function
    for (int i =0; i<half_degree; ++i)
    {
      Eigen::VectorXd vec_poisson = gradMatrix*scalars.col(i);
      sol3D_recon.col(i*3+0) = vec_poisson.segment(0*numF, numF);
      sol3D_recon.col(i*3+1) = vec_poisson.segment(1*numF, numF);
      sol3D_recon.col(i*3+2) = vec_poisson.segment(2*numF, numF);
    }

    max_error.setZero(numF,1);
    for (int i =0; i<half_degree; ++i)
    {
      Eigen::VectorXd diff = (sol3D_recon.block(0, i*3, numF, 3)-sol3D_combed.block(0, i*3, numF, 3)).rowwise().norm();
      diff = diff.array() / sol3D_combed.block(0, i*3, numF, 3).rowwise().norm().array();
      max_error = max_error.cwiseMax(diff.cast<typename DerivedE::Scalar>());
    }

    return max_error.mean();
  }