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

Eigen::MatrixXd cIKSolver::BuildConsThetaWorldJacob(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, const tConsDesc& cons_desc)
{
	assert(static_cast<int>(cons_desc(eConsDescType)) == eConsTypeThetaWorld);

	int num_joints = static_cast<int>(joint_mat.rows());
	int joint_id = static_cast<int>(cons_desc(eConsDescParam0));

	int num_dof = cKinTree::GetNumDof(joint_mat);
	int cons_dim = GetConsDim(cons_desc);
	Eigen::MatrixXd J = Eigen::MatrixXd(cons_dim, num_dof);
	J.setZero();

	int curr_id = joint_id;
	while (true)
	{
		J(0, gPosDims + curr_id) = 1;

		int curr_parent_id = cKinTree::GetParent(joint_mat, curr_id);
		if (curr_parent_id == cKinTree::gInvalidJointID)
		{
			break;
		}
		curr_id = curr_parent_id;
	}

	return J;
}

void Model::construct_feedback_matrix(Eigen::MatrixXd &S,
                                      const Eigen::MatrixXd &M,
                                      const Parameter &parameter) {
  S.setZero();

  Eigen::Matrix2d T;
  T.setConstant(1.0 / sqrt(2));
  T(0, 0) = -1 * T(0, 0);

  Eigen::DiagonalMatrix<double, 2> W(1, parameter.wgt);

  Eigen::MatrixXd DV(n_dimensions, 1);

  double s;
  for (unsigned int i = 0; i < n_alternatives; i++) {
    for (unsigned int j = 0; j < n_alternatives; j++) {

      for (unsigned int k = 0; k < n_dimensions; k++) {
        DV(k, 0) = M(i, k) - M(j, k);
      }
      DV = T * DV;
      s = (DV.transpose() * W * DV)(0, 0);
      S(i, j) = parameter.phi2 * exp(-1 * parameter.phi1 * s * s);
    }
  }

  S = Eigen::MatrixXd::Identity(n_alternatives, n_alternatives) - S;
}

void filter(const Eigen::MatrixXd& x, Eigen::MatrixXd& y, const Eigen::MatrixXd& b, const Eigen::MatrixXd& a)
{
  y.setZero();

  for(int c = 0; c < x.rows(); c++)
  {
    for(int t = 0; t < x.cols(); t++)
    {
      const int maxPQ = std::max(b.rows(), a.rows());
      for(int pq = 0; pq < maxPQ; pq++)
      {
        const double tSource = t - pq;
        if(pq < b.rows())
        {
          if(tSource >= 0)
            y(c, t) += b(pq) * x(c, tSource);
          else
            y(c, t) += b(pq) * x(c, -tSource);
        }
        if(pq > 0 && pq < a.rows())
        {
          if(tSource >= 0)
            y(c, t) += a(pq) * x(c, tSource);
          else
            y(c, t) += a(pq) * x(c, -tSource);
        }
      }
      y(c, t) /= a(0);
    }
  }
}

//------------------------------------------------------------------------------
void
LocalAssembler::assemble(Eigen::MatrixXd& A,
                         UFC& ufc,
                         const std::vector<double>& vertex_coordinates,
                         ufc::cell& ufc_cell,
                         const Cell& cell,
                         const MeshFunction<std::size_t>* cell_domains,
                         const MeshFunction<std::size_t>* exterior_facet_domains,
                         const MeshFunction<std::size_t>* interior_facet_domains)
{
    // Clear tensor
    A.setZero();

    cell.get_cell_data(ufc_cell);

    // Assemble contributions from cell integral
    assemble_cell(A, ufc, vertex_coordinates, ufc_cell, cell, cell_domains);

    // Assemble contributions from facet integrals
    for (FacetIterator facet(cell); !facet.end(); ++facet)
    {
        ufc_cell.local_facet = facet.pos();
        if (facet->num_entities(cell.dim()) == 2)
        {
            assemble_interior_facet(A, ufc, vertex_coordinates, ufc_cell, cell,
                                    *facet, facet.pos(), interior_facet_domains);
        }
        else
        {
            assemble_exterior_facet(A, ufc, vertex_coordinates, ufc_cell, cell,
                                    *facet, facet.pos(), exterior_facet_domains);
        }
    }
}

void At_mul_A(Eigen::MatrixXd & out, SparseDoubleFeat & A) {
  if (out.cols() != A.cols()) {
    throw std::runtime_error("At_mul_A(SparseDoubleFeat): out.cols() must equal A.cols()");
  }
  if (out.cols() != out.rows()) {
    throw std::runtime_error("At_mul_A(SparseDoubleFeat): out must be square matrix.)");
  }
  out.setZero();
  const int nfeat = A.M.ncol;

#pragma omp parallel for schedule(dynamic, 8)
  for (int f1 = 0; f1 < nfeat; f1++) {
    // looping over all non-zero rows of f1
    for (int i = A.Mt.row_ptr[f1], end = A.Mt.row_ptr[f1 + 1]; i < end; i++) {
      int Mrow     = A.Mt.cols[i]; /* row in M */
      double val1  = A.Mt.vals[i]; /* value for Mrow */

      for (int j = A.M.row_ptr[Mrow], end2 = A.M.row_ptr[Mrow + 1]; j < end2; j++) {
        int f2 = A.M.cols[j];
        if (f1 <= f2) {
          out(f2, f1) += A.M.vals[j] * val1;
        }
      }
    }
  }
}

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

Eigen::MatrixXd cIKSolver::BuildConsPosJacob(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, const tConsDesc& cons_desc)
{
	assert(static_cast<int>(cons_desc(eConsDescType)) == eConsTypePos);

	int num_joints = static_cast<int>(joint_mat.rows());
	int parent_id = static_cast<int>(cons_desc(eConsDescParam0));
	tVector attach_pt = tVector(cons_desc(eConsDescParam1), cons_desc(eConsDescParam2), 0.f, 0.f);
	tVector end_pos = cKinTree::LocalToWorldPos(joint_mat, pose, parent_id, attach_pt);

	const Eigen::Vector3d rot_axis = Eigen::Vector3d(0, 0, 1);

	int num_dof = cKinTree::GetNumDof(joint_mat);
	Eigen::MatrixXd J = Eigen::MatrixXd(gPosDims, num_dof);
	J.setZero();

	for (int i = 0; i < gPosDims; ++i)
	{
		J(i, i) = 1;
	}
	 
	int curr_id = parent_id;
	while (true)
	{
		tVector joint_pos = cKinTree::CalcJointWorldPos(joint_mat, pose, curr_id);
		tVector delta = end_pos - joint_pos;

		Eigen::Vector3d tangent = rot_axis.cross(Eigen::Vector3d(delta(0), delta(1), delta(2)));
		int curr_parent_id = cKinTree::GetParent(joint_mat, curr_id);

		for (int i = 0; i < gPosDims; ++i)
		{
			J(i, gPosDims + curr_id) = tangent(i);
		}
		if (curr_parent_id == cKinTree::gInvalidJointID)
		{
			// no scaling for root
			break;
		}
		else
		{
#if !defined(DISABLE_LINK_SCALE)
			double attach_x = joint_desc(curr_id, cKinTree::eJointDescAttachX);
			double attach_y = joint_desc(curr_id, cKinTree::eJointDescAttachY);
			double attach_z = joint_desc(curr_id, cKinTree::eJointDescAttachZ);

			double parent_world_theta = cKinTree::CalcJointWorldTheta(joint_desc, curr_parent_id);
			double world_attach_x = std::cos(parent_world_theta) * attach_x - std::sin(parent_world_theta) * attach_y;
			double world_attach_y = std::sin(parent_world_theta) * attach_x + std::cos(parent_world_theta) * attach_y;
			double world_attach_z = attach_z; // hack ignoring z, do this properly

			J(0, gPosDims + num_joints + curr_id) = world_attach_x;
			J(1, gPosDims + num_joints + curr_id) = world_attach_y;
#endif
			curr_id = curr_parent_id;
		}
	}

	return J;
}

void SubSystem::calcJacobi(VEC_pD &params, Eigen::MatrixXd &jacobi)
{
    jacobi.setZero(csize, params.size());
    for (int j=0; j < int(params.size()); j++) {
        MAP_pD_pD::const_iterator
          pmapfind = pmap.find(params[j]);
        if (pmapfind != pmap.end())
            for (int i=0; i < csize; i++)
                jacobi(i,j) = clist[i]->grad(pmapfind->second);
    }
}

void Model::construct_contrast_matrix(Eigen::MatrixXd &C) {

  if (n_alternatives > 1) {
    C.setConstant(-1.0 / (n_alternatives - 1.0));
  } else {
    C.setZero();
  }

  for (unsigned int i = 0; i < n_alternatives; i++) {
    C(i, i) = 1.0;
  }
}

void CompressionMatrixFactory::createCompressionMatrix(Eigen::MatrixXd& cm)
{
  if(cm.rows() < paramDim || cm.cols() < inputDim)
    cm.resize(paramDim, inputDim);
  if(compress)
  {
    cm.setZero();
    fillCompressionMatrix(cm);
  }
  else
    cm = Eigen::MatrixXd::Identity(cm.rows(), cm.cols());
}

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

void dynamicsRegressorLoop(const UndirectedTree & ,
                         const KDL::JntArray &q,
                         const Traversal & traversal,
                         const std::vector<Frame>& X_b,
                         const std::vector<Twist>& v,
                         const std::vector<Twist>& a,
                         Eigen::MatrixXd & dynamics_regressor)
{
        dynamics_regressor.setZero();

        Eigen::Matrix<double, 6, 10> netWrenchRegressor_i;

        // Store the base_world translational transform in world orientation
        KDL::Frame world_base_X_world_world = KDL::Frame(-(X_b[traversal.getBaseLink()->getLinkIndex()].p));

        for(int l =(int)traversal.getNrOfVisitedLinks()-1; l >= 0; l-- ) {
            LinkMap::const_iterator link = traversal.getOrderedLink(l);
            int i = link->getLinkIndex();

            //Each link affects the dynamics of the joints from itself to the base
            netWrenchRegressor_i = netWrenchRegressor(v[i],a[i]);

            //Base dynamics
            // The base dynamics is expressed with the orientation of the world but
            // with respect to the base origin
            dynamics_regressor.block(0,(int)(10*i),6,10) = WrenchTransformationMatrix(world_base_X_world_world*X_b[i])*netWrenchRegressor_i;

            //dynamics_regressor.block(0,(int)(10*i),6,10) = WrenchTransformationMatrix(X_b[i])*netWrenchRegressor_i;

            LinkMap::const_iterator child_link = link;
            LinkMap::const_iterator parent_link=traversal.getParentLink(link);
            while( child_link != traversal.getOrderedLink(0) ) {
                if( child_link->getAdjacentJoint(parent_link)->getNrOfDOFs() == 1 ) {
                    #ifndef NDEBUG
                    //std::cerr << "Calculating regressor columns for link " << link->getName() << " and joint " << child_link->getAdjacentJoint(parent_link)->getName() << std::endl;
                    #endif
                    int dof_index = child_link->getAdjacentJoint(parent_link)->getDOFIndex();
                    int child_index = child_link->getLinkIndex();
                    Frame X_j_i = X_b[child_index].Inverse()*X_b[i];
                    dynamics_regressor.block(6+dof_index,10*i,1,10) =
                            toEigen(parent_link->S(child_link,q(dof_index))).transpose()*WrenchTransformationMatrix(X_j_i)*netWrenchRegressor_i;
                }
                child_link = parent_link;
                #ifndef NDEBUG
                //std::cout << "Getting parent link of link of index " << child_link->getName() << " " << child_link->getLinkIndex() << std::endl;
                //std::cout << "Current base " << traversal.order[0]->getName() << " " << traversal.order[0]->getLinkIndex() << std::endl;
                #endif
                parent_link = traversal.getParentLink(child_link);
            }
        }
}

void ALLModel::calculateEuclDistanceMatrix(Eigen::MatrixXd& m1, Eigen::MatrixXd& m2, Eigen::MatrixXd& output)
{
    output.resize(m1.rows(), m2.rows());
    output.setZero();
    Statistics n;
    for (int i = 0; i < m1.rows(); i++)
    {
        for (int j = 0; j < m2.rows(); j++)
        {
            //get euclidean distances of the two current rows
            output(i, j) = n.euclDistance(m1, m2, i, j);
        }
    }
}

 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 Model::construct_L_matrix(Eigen::MatrixXd &L, const unsigned int i) {

  L.setZero();
  for (unsigned int row = 0; row < n_alternatives - 1; row++) {
    for (unsigned int col = 0; col < n_alternatives; col++) {
      if (col == i) {
        L(row, col) = 1;
      } else if ((col < i) && (row == col)) {
        L(row, col) = -1;
      } else if ((col > i) && (row == col - 1)) {
        L(row, col) = -1;
      }
    }
  }
}