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

/**
* Performs eigenvector decomposition of an affinity matrix
*
* @param data 		the affinity matrix
* @param numDims	the number of dimensions to consider when clustering
*/
SpectralClustering::SpectralClustering(Eigen::MatrixXd& data, int numDims):
	mNumDims(numDims),
	mNumClusters(0)
{
	Eigen::MatrixXd Deg = Eigen::MatrixXd::Zero(data.rows(),data.cols());

	// calc normalised laplacian 
	for ( int i=0; i < data.cols(); i++) {
		Deg(i,i)=1/(sqrt((data.row(i).sum())) );
	}
	Eigen::MatrixXd Lapla = Deg * data * Deg;

	Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> s(Lapla, true);
	Eigen::VectorXd val = s.eigenvalues();
	Eigen::MatrixXd vec = s.eigenvectors();

	//sort eigenvalues/vectors
	int n = data.cols();
	for (int i = 0; i < n - 1; ++i) {
		int k;
		val.segment(i, n - i).maxCoeff(&k);
		if (k > 0) {
			std::swap(val[i], val[k + i]);
			vec.col(i).swap(vec.col(k + i));
		}
	}

	//choose the number of eigenvectors to consider
	if (mNumDims < vec.cols()) {
		mEigenVectors = vec.block(0,0,vec.rows(),mNumDims);
	} else {
		mEigenVectors = vec;
	}
}

/**
 * Generates an artificial topographyGrid of size numRows x numCols if no
 * topographic data is available.  Results are dumped into topographyGrid.
 * @param topographyGrid A pointer to a zero-initialized Grid of size
 * numRows x numCols.
 * @param numRows The desired number of non-border rows in the resulting matrix.
 * @param numCols The desired number of non-border cols in the resulting matrix.
 */
void simulatetopographyGrid(Grid* topographyGrid, int numRows, int numCols) {
    Eigen::VectorXd refx = refx.LinSpaced(numCols, -2*M_PI, 2*M_PI);
    Eigen::VectorXd refy = refx.LinSpaced(numRows, -2*M_PI, 2*M_PI);
    Eigen::MatrixXd X = refx.replicate(1, numRows);
    X.transposeInPlace();
    Eigen::MatrixXd Y = refy.replicate(1, numCols);

    // Eigen can only deal with two matrices at a time,
    // so split the computation:
    // topographyGrid = sin(X) * sin(Y) * abs(X) * abs(Y) -pi
    Eigen::MatrixXd absXY = X.cwiseAbs().cwiseProduct(Y.cwiseAbs());
    Eigen::MatrixXd sins = X.array().sin().cwiseProduct(Y.array().sin());
    Eigen::MatrixXd temp;
    temp.resize(numRows, numCols);
    temp = absXY.cwiseProduct(sins);

    // All this work to create a matrix of pi...
    Eigen::MatrixXd pi;
    pi.resize(numRows, numCols);
    pi.setConstant(M_PI);
    temp = temp - pi;
    // And the final result.
    topographyGrid->data.block(border, border, numRows, numCols) =
                              temp.block(0, 0, numRows, numCols);
    // Ignore positive values.
    topographyGrid->data =
            topographyGrid->data.unaryExpr(std::ptr_fun(validateDepth));
    topographyGrid->clearNA();
}

double GenericCell<dim, spacedim>::get_error_in_cell(
  const TimeFunction<dim, dealii::Tensor<1, func_out_dim> > &func,
  const Eigen::MatrixXd &modal_vector,
  const double &time)
{
  double error = 0;
  const std::vector<dealii::Point<dim> > &points_loc =
    cell_quad_fe_vals->get_quadrature_points();
  const std::vector<double> &JxWs = cell_quad_fe_vals->get_JxW_values();
  unsigned n_unknowns = the_elem_basis->n_polys;
  assert(points_loc.size() == JxWs.size());
  assert(modal_vector.rows() == func_out_dim * n_unknowns);
  //  assert((long int)points_loc.size() == mode_to_Qpoint_matrix.rows());
  std::vector<Eigen::MatrixXd> values_at_Nodes(func_out_dim);
  for (unsigned i_dim = 0; i_dim < func_out_dim; ++i_dim)
    values_at_Nodes[i_dim] = the_elem_basis->get_dof_vals_at_quads(
      modal_vector.block(i_dim * n_unknowns, 0, n_unknowns, 1));
  for (unsigned i_point = 0; i_point < JxWs.size(); ++i_point)
  {
    dealii::Tensor<1, func_out_dim> temp_val;
    for (unsigned i_dim = 0; i_dim < func_out_dim; ++i_dim)
    {
      temp_val[i_dim] = values_at_Nodes[i_dim](i_point, 0);
    }
    dealii::Tensor<1, func_out_dim> func_val =
      func.value(points_loc[i_point], points_loc[i_point], time);
    error += (func_val - temp_val) * (func_val - temp_val) * JxWs[i_point];
  }
  return error;
}

void TaskSolverDmpArm2D::performRollout(const Eigen::VectorXd& sample, const Eigen::VectorXd& task_parameters, Eigen::MatrixXd& cost_vars) const
{
  TaskSolverDmp::performRollout(sample, task_parameters, cost_vars);

  // cost_vars_angles structure is (without the link positions!)
  //
  // time  joint angles (e.g. n_dofs = 3)     forcing term  link positions (e.g. 3+1) 
  // ____  __________________________________  __________  _________________________    
  // t     | a a a | ad ad ad | add add add |  f f f       | x y | x y | x y | x y  |    
  //
  // We now compute the link positions and add them to the end
  
  int n_dofs = dmp_->dim_orig();
  int n_time_steps = cost_vars.rows();
  assert(cost_vars.cols() == 1+4*n_dofs);
  
  // Make room for link positions, i.e. 2 * (n_links+1)
  cost_vars.conservativeResize(n_time_steps, 1 + 4*n_dofs + 2*(n_dofs+1));
  
  MatrixXd angles = cost_vars.block(0,1,n_time_steps,n_dofs);
  MatrixXd link_positions;
  anglesToLinkPositions(angles,link_positions);
  
  // Add the link positions to the right side of the cost_vars matrix
  cost_vars.rightCols(2*(n_dofs+1)) = link_positions;
}

/**
 * @brief hqp_wrapper::updObstacle Updates obstacles' matrices
 * @param levelName                 Level's ID
 * @param Jac                       Jacobian (6xNc)
 * @param n                         Normalised Vector between end effector and obstacles' position
 * @param b                         scalar b, n*J < b
 */
void hqp_wrapper::updObstacle(std::string levelName, const Eigen::MatrixXd Jac, const Eigen::Vector3d n, const double b ){
    int indx = leveNameMap[levelName];
    this->B[indx][0] = soth::Bound(b, soth::Bound::BOUND_INF);
    this->J[indx] = n.transpose()*Jac.block(0,0,3,this->NC);
    //  std::cout << "J[indx]:\n" <<J[indx] <<"\n";

}

IGL_INLINE void igl::ViewerData::set_uv(const Eigen::MatrixXd& UV_V, const Eigen::MatrixXi& UV_F)
{
  set_face_based(true);
  V_uv = UV_V.block(0,0,UV_V.rows(),2);
  F_uv = UV_F;
  dirty |= DIRTY_UV;
}

void BallJointConstraint::updateDynamics(Eigen::MatrixXd & _J1, Eigen::VectorXd & _C, Eigen::VectorXd & _CDot, int _rowIndex) {
    getJacobian();
    _J1.block(_rowIndex, 0, 3, mBodyNode1->getSkeleton()->getNumGenCoords()) = mJ1;
    Eigen::Vector3d worldP1 = mBodyNode1->getWorldTransform() * mOffset1;
    Eigen::VectorXd qDot1 = mBodyNode1->getSkeleton()->get_dq();
    _C.segment(_rowIndex, 3) = worldP1 - mOffset2;
    _CDot.segment(_rowIndex, 3) = mJ1 * qDot1;
}

/**
 * @brief hqp_wrapper::addObstacle
 * @param levelName         String to be used as ID
 * @param Jac               Jacobian (6xNc), only the first 3 rows (position) is used currently
 * @param n                 Normalised Vector between end effector and obstacles' position
 * @param b                 scalar b, n*J < b
 */
void hqp_wrapper::addObstacle(std::string levelName, const Eigen::MatrixXd Jac, const Eigen::Vector3d n, double b){
    Eigen::MatrixXd Jtmp;
    Jtmp = n.transpose()*Jac.block(0,0,3,this->NC);
    Eigen::Matrix<double,1,1>  B;
    B(0) = b;
    addStage(levelName,Jtmp,B,soth::Bound::BOUND_INF);
    std::cout <<"Adding new element:" << levelName <<", at:" << leveNameMap[levelName]<<"\n";
}

void addMatrixBlock( Eigen::MatrixXd &matrix, const int block_length ){
  assert( matrix.rows()==matrix.cols() );

  int old_size = matrix.rows();
  matrix.conservativeResize( old_size+block_length, old_size+block_length );
  /*
    O: old
    Nx: newly initalized
    O   N1
    N2  N1
  */
  // N1
  matrix.block( 0, old_size, old_size+block_length, block_length ) = Eigen::MatrixXd::Zero( old_size+block_length, block_length);
  // N2
  matrix.block( old_size, 0, block_length, old_size ) = Eigen::MatrixXd::Zero( block_length, old_size);

}

//! Function to get the concatenated state transition matrices for each arc and sensitivity matrix at a given time.
Eigen::MatrixXd MultiArcCombinedStateTransitionAndSensitivityMatrixInterface::getFullCombinedStateTransitionAndSensitivityMatrix(
        const double evaluationTime )
{
    Eigen::MatrixXd combinedStateTransitionMatrix = Eigen::MatrixXd::Zero(
                stateTransitionMatrixSize_, numberOfStateArcs_ * stateTransitionMatrixSize_ + sensitivityMatrixSize_ );

    int currentArc = lookUpscheme_->findNearestLowerNeighbour( evaluationTime );

    // Set Phi and S matrices of current arc.
    combinedStateTransitionMatrix.block( 0, currentArc * stateTransitionMatrixSize_, stateTransitionMatrixSize_, stateTransitionMatrixSize_ ) =
            stateTransitionMatrixInterpolators_.at( currentArc )->interpolate( evaluationTime );
    combinedStateTransitionMatrix.block(
                0, numberOfStateArcs_ * stateTransitionMatrixSize_, stateTransitionMatrixSize_, sensitivityMatrixSize_ ) =
            sensitivityMatrixInterpolators_.at( currentArc )->interpolate( evaluationTime );

    return combinedStateTransitionMatrix;
}

void calc()
{
  Eigen::MatrixXd M_inv1 = M1.inverse();
  Eigen::MatrixXd M_inv2 = M2.inverse();
  Eigen::MatrixXd Jv1 = J1.block(0, 0, 3, J1.cols());
  Eigen::MatrixXd Jv2 = J2.block(0, 0, 3, J2.cols());
  Eigen::MatrixXd Lambda_inv1 = Jv1 * M_inv1 * Jv1.transpose();
  Eigen::MatrixXd Lambda_inv2 = Jv2 * M_inv2 * Jv2.transpose();
  Eigen::MatrixXd Lambda1 = Lambda_inv1.inverse();
  Eigen::MatrixXd Lambda2 = Lambda_inv2.inverse();
  Eigen::MatrixXd J_dyn_inv1 = M_inv1 * Jv1.transpose() * Lambda1;
  Eigen::MatrixXd J_dyn_inv2 = M_inv2 * Jv2.transpose() * Lambda2;
  Eigen::MatrixXd I = Eigen::MatrixXd::Identity(M_inv1.rows(), M_inv1.rows());
  Eigen::MatrixXd N1 = I - J_dyn_inv1 * Jv1;
  Eigen::MatrixXd N2 = I - J_dyn_inv2 * Jv2;

  //std::cout << N.transpose() << std::endl << std::endl;
}

 Eigen::MatrixXd grad(const Eigen::VectorXd& x, const GP& gp) const
 {
     Eigen::MatrixXd grad = Eigen::MatrixXd::Zero(_tr.rows(), _h_params.size());
     Eigen::VectorXd m = _mean_function(x, gp);
     for (int i = 0; i < _tr.rows(); i++) {
         grad.block(i, i * _tr.cols(), 1, _tr.cols() - 1) = m.transpose();
         grad(i, (i + 1) * _tr.cols() - 1) = 1;
     }
     return grad;
 }

/*! \fn inline void symmetryPacking(std::vector<Eigen::MatrixXd> & blockedMatrix, const Eigen::MatrixXd & fullMatrix, int nrBlocks, int dimBlock)
 *  \param[out] blockedMatrix the result of packing fullMatrix
 *  \param[in]  fullMatrix the matrix to be packed
 *  \param[in]  dimBlock the dimension of the square blocks
 *  \param[in]  nrBlocks the number of square blocks
 */
inline void symmetryPacking(std::vector<Eigen::MatrixXd> & blockedMatrix,
                            const Eigen::MatrixXd & fullMatrix, int dimBlock, int nrBlocks)
{
    // This function packs the square block diagonal fullMatrix with nrBlocks of dimension dimBlock
    // into a std::vector<Eigen::MatrixXd> containing nrBlocks square matrices of dimenion dimBlock.
    int j = 0;
    for (int i = 0; i < nrBlocks; ++i) {
        blockedMatrix.push_back(fullMatrix.block(j, j, dimBlock, dimBlock));
        j += dimBlock;
    }
}

void drawField(igl::viewer::Viewer &viewer,
               const Eigen::MatrixXd &field,
               const Eigen::RowVector3d &color)
{
  for (int n=0; n<2; ++n)
  {
    Eigen::MatrixXd VF = field.block(0,n*3,F.rows(),3);
    Eigen::VectorXd c = VF.rowwise().norm();
    viewer.data.add_edges(B - global_scale*VF, B + global_scale*VF , color);
  }
}

Eigen::Quaterniond MainWindow::rotation2quaternion( Eigen::MatrixXd rotation )
{
  Eigen::Matrix3d _rotation3d;

  _rotation3d = rotation.block( 0 , 0 , 3 , 3 );

  Eigen::Quaterniond _quaternion;
  _quaternion = _rotation3d;

  return _quaternion;
}