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

bool toEigen(const yarp::sig::Vector & vec_yrp, Eigen::VectorXd & vec_eigen)
{
 if( vec_yrp.size() != vec_eigen.size() ) { vec_eigen.resize(vec_yrp.size()); }
    if( memcpy(vec_eigen.data(),vec_yrp.data(),sizeof(double)*vec_eigen.size()) != NULL ) {
        return true;
    } else {
        return false;
    }
}

void GetBoundingBox(const Eigen::MatrixXd& data, Eigen::VectorXd& minCorner, Eigen::VectorXd& maxCorner)
{
  assert(data.cols() > 0);

  minCorner.resize(data.rows());
  maxCorner.resize(data.rows());

  for(unsigned int coordinate = 0; coordinate < data.rows(); ++coordinate)
  {
    minCorner[coordinate] = std::numeric_limits<double>::max();
    maxCorner[coordinate] = std::numeric_limits<double>::min();

    for(unsigned int pointId = 0; pointId < data.cols(); ++pointId)
    {
      if(data(coordinate, pointId) > maxCorner(coordinate))
      {
        maxCorner(coordinate) = data(coordinate, pointId);
      }

      if(data(coordinate, pointId) < minCorner(coordinate))
      {
        minCorner(coordinate) = data(coordinate, pointId);
      }
    }
  }
}

int main(int argc, char** argv) {

	ros::init(argc, argv, "joint_to_cart");
	ros::NodeHandle nh;
	ros::NodeHandle _nh("~");

	mRobot = new RTKRobotArm(true);
	if(!mRobot->initialize(_nh)) {
		ROS_ERROR("Error while loading robot");
		return 1;
	}

	if(!parseParams(_nh)) {
		ROS_ERROR("Errors while parsing arguments.");
		return 1;
	}

	numdof = mRobot->numdof;
	read_torque.resize(numdof);
	read_jpos.resize(numdof);
	ee_ft.resize(6);

	pub_pose = nh.advertise<geometry_msgs::PoseStamped>(output_cart_pose, 3);
	pub_ft = nh.advertise<geometry_msgs::WrenchStamped>(output_cart_ft, 3);
	ros::Subscriber sub = nh.subscribe<sensor_msgs::JointState>(input_joint_topic, 10, jointStateCallback,ros::TransportHints().tcpNoDelay());
	ros::Subscriber sub_ft = nh.subscribe<geometry_msgs::WrenchStamped>("/right_arm_ft_sensor/wrench", 10, sensorFTCallback, ros::TransportHints().tcpNoDelay());


	ROS_INFO("Node started");
	ros::spin();

	return 0;
}

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

// Parse right hand side arguments for a matlab mex function.
//
// Inputs:
//   nrhs  number of right hand side arguments
//   prhs  pointer to right hand side arguments
// Outputs:
//   V  n by dim list of mesh vertex positions
//   F  m by dim list of mesh face indices
//   s  1 by dim bone source vertex position
//   d  1 by dim bone dest vertex position
// "Throws" matlab errors if dimensions are not sane.
void parse_rhs(
  const int nrhs, 
  const mxArray *prhs[], 
  Eigen::MatrixXd & V,
  Eigen::MatrixXi & F,
  Eigen::VectorXd & s,
  Eigen::VectorXd & d)
{
  using namespace std;
  if(nrhs < 4)
  {
    mexErrMsgTxt("nrhs < 4");
  }
  const int dim = mxGetN(prhs[0]);
  if(dim != 3)
  {
    mexErrMsgTxt("Mesh vertex list must be #V by 3 list of vertex positions");
  }
  if(dim != (int)mxGetN(prhs[1]))
  {
   mexErrMsgTxt("Mesh facet size must equal dimension");
  }
  if(dim != (int)mxGetN(prhs[2]))
  {
   mexErrMsgTxt("Source dim must equal vertex dimension");
  }
  if(dim != (int)mxGetN(prhs[3]))
  {
   mexErrMsgTxt("Dest dim must equal vertex dimension");
  }
  // set number of mesh vertices
  const int n = mxGetM(prhs[0]);
  // set vertex position pointers
  double * Vp = mxGetPr(prhs[0]);
  // set number of faces
  const int m = mxGetM(prhs[1]);
  // set face index list pointer
  double * Fp = mxGetPr(prhs[1]);
  // set source and dest pointers
  double * sp = mxGetPr(prhs[2]);
  double * dp = mxGetPr(prhs[3]);
  // resize output to transpose
  V.resize(n,dim);
  copy(Vp,Vp+n*dim,V.data());
  // resize output to transpose
  F.resize(m,dim);
  // Q: Is this doing a cast?
  // A: Yes.
  copy(Fp,Fp+m*dim,F.data());
  // http://stackoverflow.com/a/4461466/148668
  transform(F.data(),F.data()+m*dim,F.data(),
    bind2nd(std::plus<double>(),-1.0));
  // resize output to transpose
  s.resize(dim);
  copy(sp,sp+dim,s.data());
  d.resize(dim);
  copy(dp,dp+dim,d.data());
}

//TODO: Compute the joint impedance here. Conversion from cart_stiffness to joint_stiffness not implemented yet.
void computeJointImpedance(Eigen::VectorXd& joint_stiff, Eigen::VectorXd& joint_damp) {
	if(joint_stiff.size() != numdof) {
		joint_stiff.resize(numdof);
	}
	if(joint_damp.size() != numdof) {
		joint_damp.resize(numdof);
	}
	for(int i=0; i<numdof; ++i) {
		joint_stiff[i] = DEFAULT_JSTIFF;
		joint_damp[i] = DEFAULT_JDAMP;
	}
}

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

int main(int argc, char *argv[])
{
  using namespace Eigen;
  using namespace std;
  MatrixXd V;
  MatrixXi F;
  igl::readOFF(TUTORIAL_SHARED_PATH "/cheburashka.off",V,F);

  // Plot the mesh
  igl::opengl::glfw::Viewer viewer;
  viewer.data().set_mesh(V, F);
  viewer.data().show_lines = false;
  viewer.callback_key_down = &key_down;

  // One fixed point
  b.resize(1,1);
  // point on belly.
  b<<2556;
  bc.resize(1,1);
  bc<<1;

  // Construct Laplacian and mass matrix
  SparseMatrix<double> L,M,Minv;
  igl::cotmatrix(V,F,L);
  igl::massmatrix(V,F,igl::MASSMATRIX_TYPE_VORONOI,M);
  //M = (M/M.diagonal().maxCoeff()).eval();
  igl::invert_diag(M,Minv);
  // Bi-Laplacian
  Q = L.transpose() * (Minv * L);
  // Zero linear term
  B = VectorXd::Zero(V.rows(),1);

  // Lower and upper bound
  lx = VectorXd::Zero(V.rows(),1);
  ux = VectorXd::Ones(V.rows(),1);

  // Equality constraint constrain solution to sum to 1
  Beq.resize(1,1);
  Beq(0) = 0.08;
  Aeq = M.diagonal().sparseView().transpose();
  // (Empty inequality constraints)
  solve(viewer);
  cout<<
    "Press '.' to increase scale and resolve."<<endl<<
    "Press ',' to decrease scale and resolve."<<endl;

  viewer.launch();
}

/** Return the vector from a back-substitution step that solves: Ux=b   */
void CSparseMatrix::CholeskyDecomp::backsub(
	const Eigen::VectorXd& b, Eigen::VectorXd& sol) const
{
	ASSERT_(b.size() > 0);
	sol.resize(b.size());
	this->backsub(&b[0], &sol[0], b.size());
}

void Locator::layoutByWord(const QHash<SymbolPath, SymbolData>& symbolWord, 
								 QVector<QVector2D>& pos2D,
								 float sparseFactor, 
								 float* radius,
								 QVector<float>* projRadius)
{
	// get doc term matrix and radius of each data
	SparseMatrix docTermMat;
	Eigen::VectorXd radiusVec;
	buildDocTermMat(symbolWord, docTermMat, radiusVec);

	// use external radius data
	if (projRadius)
	{
		radiusVec.resize(projRadius->size());
		for (int i = 0; i < projRadius->size(); ++i)
			radiusVec[i] = (*projRadius)[i];
	}

	// compute 2D position
	m_wordLocator.setDocTermMat(docTermMat, radiusVec);
	m_wordLocator.setUseTfIdfMeasure(false);
	m_wordLocator.compute(sparseFactor); 
	//m_wordLocator.saveMatsToFile("H:/Programs/QtCreator/qt-creator_master/src/plugins/MyPlugin/CodeAtlas/codeData.m");
	pos2D = m_wordLocator.getOri2DPositions();
	if (radius)
		*radius = m_wordLocator.getOri2DRadius() + 0.1f;
}

void Locator::buildDocTermMat(const QHash<SymbolPath, SymbolData>& symbolWordList,
									SparseMatrix& docTermMat,
									Eigen::VectorXd& radiusVec)
{
	int nSymbols = symbolWordList.size();
	int nWords   = SymbolWordAttr::totalWords();
	QVector<float> wordCountPerDoc(nSymbols,0.f);	// total number of words for each doc
	QVector<float> docCountPerWord(nWords  ,0.f);	// total number of doc   for each word

	docTermMat = SparseMatrix(nSymbols, nWords);
	radiusVec.resize(nSymbols);
	QHash<SymbolPath, SymbolData>::ConstIterator pSymbol;
	int ithSymbol = 0;
	for (pSymbol =  symbolWordList.begin(); 
		 pSymbol != symbolWordList.end(); ++pSymbol, ++ithSymbol)
	{
		const SymbolData& item = pSymbol.value();
		wordCountPerDoc[ithSymbol] = item.getTotalWordCount();

		QMap<int,float>::ConstIterator pWord;
		for (pWord = item.m_wordWeightMap.begin(); 
			 pWord != item.m_wordWeightMap.end(); ++pWord)
		{
			int wordId = pWord.key();
			float wordCount = pWord.value();
 			docCountPerWord[wordId]    += 1;
			docTermMat.insert(ithSymbol, wordId) = wordCount;
		}
		radiusVec(ithSymbol) = item.getRadius();
	}
	docTermMat.makeCompressed();
}

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

void Parameterizable::getParameterVectorSelected(Eigen::VectorXd& values, bool normalized) const
{
  Eigen::VectorXd all_values;
  getParameterVectorAll(all_values);
  
  values.resize(getParameterVectorSelectedSize());
  // We cannot do this with Block, because regions might not be contiguous
  int ii = 0;
  for (int all_ii=0; all_ii<selected_mask_.size(); all_ii++)
    if (selected_mask_[all_ii]>0)
      values[ii++] = all_values[all_ii];
    
  if (normalized)
  {
    VectorXd min_vec, max_vec;
    getParameterVectorSelectedMinMax(min_vec, max_vec);
    
    VectorXd range =  (max_vec.array()-min_vec.array());
    for (int ii=0; ii<values.size(); ii++)
    {
      if (range[ii]>0)
      {
        values[ii] = (values[ii]-min_vec[ii])/range[ii];
      }
      else
      {
        if (abs(max_vec[ii])>0)
          values[ii] = values[ii]/abs(2*max_vec[ii]);
      }
    }
    
  }
    
}

bool BlockSparseMatrix::ConjugateGradientSolve(const Eigen::VectorXd& rhs, Eigen::VectorXd& sol)
{
	//sol = mMatrix.llt().solve(rhs);
	return LltSolve(rhs, sol);

	MatrixSolver* solver = new PCGSolver();
	int vectorLength = static_cast<int>(rhs.size());
	double* result = new double[vectorLength];
	double* rhsData = new double[vectorLength];
	memcpy(rhsData, rhs.data(), vectorLength * sizeof(double));

	if (mbCSRDirty)
	{
		mCSREquivalent.ConvertFromBlockSparseMatrix(*this);
		mbCSRDirty = false;
	}

	solver->SetMatrix(&mCSREquivalent);
	solver->SetRHS(rhsData);
	solver->Solve(result, true);
	sol.resize(vectorLength);
	for (int i = 0; i < vectorLength; ++i)
		sol[i] = result[i];

	delete[] rhsData;
	delete[] result;
	delete solver;
	return true;
}