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

    virtual bool initialize() {
        const BodyPtr& io = ioBody();
				qinit.resize(DOF);
        for(int i=0; i<DOF; i++){
            io->joint(i)->u() = 0.0;
						qinit[i] = io->joint(i)->q();
				}
        qref.resize(DOF);
				dqref.resize(DOF);
        return true;
    }

void FunctionApproximator::getParameterVectorSelectedMinMax(VectorXd& min, VectorXd& max) const
{
  if (model_parameters_==NULL)
  {
    cerr << __FILE__ << ":" << __LINE__ << ": Warning: Trying to access model parameters of the function approximator, but it has not been trained yet. Returning empty parameter vector." << endl;
    min.resize(0);
    max.resize(0);
    return;
  }

  model_parameters_->getParameterVectorSelectedMinMax(min,max);
}

void generateBezierCurve()
{
    ts = VectorXd::LinSpaced ( 21,0,1. );
    t_x.resize ( ts.size() );
    t_y.resize ( ts.size() );
    t_z.resize ( ts.size() );

    for ( int idx=0; idx< ts.size(); ++idx )
    {
        t_x ( idx ) = getValue ( ts ( idx ), pt_x );
        t_y ( idx ) = getValue ( ts ( idx ), pt_y );
        t_z ( idx ) = getValue ( ts ( idx ), pt_z );
    }
}

void parameters::Likelihood(const VectorXd & eff){
	 VectorXd loc;
	 loc.resize(m_proba.rows());
	 loc=(m_proba.rowwise().sum().array().log());
	 m_loglikelihood=eff.transpose()*loc;
	 m_bic=m_loglikelihood - 0.5*m_nbparam*log(eff.sum());
}

int
main() {
   vector<Triplet<double>> trips(4);
   trips.push_back(Triplet<double>(0, 0, .435));
   trips.push_back(Triplet<double>(1, 1, .435));
   trips.push_back(Triplet<double>(2, 2, .435));
   trips.push_back(Triplet<double>(3, 3, .435));

   SparseMatrix<double> A;
   A.resize(4, 4);
   A.setFromTriplets(trips.begin(), trips.end());
   SparseQR<SparseMatrix<double>, COLAMDOrdering<int>> solverA;
   solverA.compute(A);

   VectorXd B;
   B.resize(4);
   B(0) = .435;
   B(1) = .435;
   B(2) = .435;
   B(3) = .435;

   VectorXd X = solverA.solve(B);

   for (int i = 0; i < 4; i++) cout << X(i) << endl;
}

void pca_small(MatrixXd &B, int method, MatrixXd& U, VectorXd &d, bool verbose)
{
   if(method == METHOD_SVD)
   {
      verbose && std::cout << timestamp() << " SVD begin" << std::endl;
      JacobiSVD<MatrixXd> svd(B, ComputeThinU | ComputeThinV);
      U = svd.matrixU();
      MatrixXd V = svd.matrixV();
      d = svd.singularValues().array().pow(2);
      verbose && std::cout << timestamp() << " SVD done" << std::endl;
   }
   else if(method == METHOD_EIGEN)
   {
      verbose && std::cout << timestamp() << " Eigen-decomposition begin" << std::endl;
      MatrixXd BBT = B * B.transpose();
      verbose && std::cout << timestamp() << " dim(BBT): " << dim(BBT) << std::endl;
      SelfAdjointEigenSolver<MatrixXd> eig(BBT);

      // The eigenvalues come out sorted in *increasing* order,
      // but we need decreasing order
      VectorXd eval = eig.eigenvalues();
      MatrixXd evec = eig.eigenvectors();
      d.resize(eval.size());
      U.resize(BBT.rows(), BBT.rows());

      unsigned int k = 0, s = d.size();
      for(unsigned int i = d.size() - 1 ; i != -1 ; --i)
      {
	 // we get eigenvalues, which are the squared singular values
	 d(k) = eval(i);
	 U.col(k) = evec.col(i);
	 k++;
      }
   }
}

int contactConstraintsBV(
    const RigidBodyTree<double>& r,
    const KinematicsCache<double>& cache, int nc,
    std::vector<double> support_mus,
    // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
    drake::eigen_aligned_std_vector<SupportStateElement>& supp, MatrixXd& B,
    // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
    MatrixXd& JB, MatrixXd& Jp, VectorXd& Jpdotv, MatrixXd& normals) {
  int j, k = 0, nq = r.get_num_positions();

  B.resize(3, nc * 2 * m_surface_tangents);
  JB.resize(nq, nc * 2 * m_surface_tangents);
  Jp.resize(3 * nc, nq);
  Jpdotv.resize(3 * nc);
  normals.resize(3, nc);

  Vector3d contact_pos, pos, normal;
  MatrixXd J(3, nq);
  Matrix<double, 3, m_surface_tangents> d;

  for (auto iter = supp.begin(); iter != supp.end(); iter++) {
    double mu = support_mus[iter - supp.begin()];
    double norm = sqrt(1 + mu * mu);  // because normals and ds are orthogonal,
                                      // the norm has a simple form
    if (nc > 0) {
      for (auto pt_iter = iter->contact_pts.begin();
           pt_iter != iter->contact_pts.end(); pt_iter++) {
        contact_pos = r.transformPoints(cache, *pt_iter, iter->body_idx, 0);
        J = r.transformPointsJacobian(cache, *pt_iter, iter->body_idx, 0, true);

        normal = iter->support_surface.head(3);
        surfaceTangents(normal, d);
        for (j = 0; j < m_surface_tangents; j++) {
          B.col(2 * k * m_surface_tangents + j) =
              (normal + mu * d.col(j)) / norm;
          B.col((2 * k + 1) * m_surface_tangents + j) =
              (normal - mu * d.col(j)) / norm;

          JB.col(2 * k * m_surface_tangents + j) =
              J.transpose() * B.col(2 * k * m_surface_tangents + j);
          JB.col((2 * k + 1) * m_surface_tangents + j) =
              J.transpose() * B.col((2 * k + 1) * m_surface_tangents + j);
        }

        // store away kin sols into Jp and Jpdotv
        // NOTE: I'm cheating and using a slightly different ordering of J and
        // Jdot here
        Jp.block(3 * k, 0, 3, nq) = J;
        Vector3d pt = (*pt_iter).head(3);
        Jpdotv.block(3 * k, 0, 3, 1) =
            r.transformPointsJacobianDotTimesV(cache, pt, iter->body_idx, 0);
        normals.col(k) = normal;

        k++;
      }
    }
  }

  return k;
}

/** 
 * reconstruct the displacements u in Euler space with RS coordinates y
 * provided.
 * 
 * @param y the RS coordinates.
 * @param u the constructed Euler coordinates represents the displacements.
 * 
 * @return true if construction is success.
 */
bool RS2Euler::reconstruct(const VectorXd &y, VectorXd &u){

  assert (tetmesh != NULL);
  const int node_numx3 = tetmesh->nodes().size()*3;
  bool succ = true;

  // assemble the right_side
  VectorXd b;
  assemble_b(y,b);
  assert_eq(VG_t.cols(),b.size());
  assert_eq(VG_t.rows(),node_numx3);

  VectorXd right_side(node_numx3 + numFixedDofs());
  right_side.segment(0, node_numx3) = VG_t*b;
  right_side.segment(node_numx3,numFixedDofs()).setZero();
  right_side.segment(node_numx3,barycenter_uc.size()) = barycenter_uc;

  // solve A*u = right_side
  assert_eq (right_side.size(), A_solver.rows());
  u = A_solver.solve(right_side);

  // get the result 
  if(A_solver.info()!=Eigen::Success) {
	
  	succ = false;
  	u.resize(node_numx3);
  	u.setZero();
  	ERROR_LOG("failed to solve for A X = P.");
  }else{
	assert_gt(u.size(), node_numx3);
	const VectorXd x = u.head(node_numx3);
	u = x;
  }
  return succ;
}

void FunctionApproximator::getParameterVectorSelected(VectorXd& values, bool normalized) const
{
  if (checkModelParametersInitialized())
    model_parameters_->getParameterVectorSelected(values, normalized);
  else
    values.resize(0);
}

void ModelParametersLWPR::getParameterVectorAll(VectorXd& values) const
{
  values.resize(getParameterVectorAllSize());
  int ii=0;
  
  for (int iDim = 0; iDim < lwpr_object_->model.nOut; iDim++)
    for (int iRF = 0; iRF < lwpr_object_->model.sub[iDim].numRFS; iRF++)
      for (int j = 0; j < lwpr_object_->nIn(); j++)
       values[ii++] = lwpr_object_->model.sub[iDim].rf[iRF]->c[j];

  for (int iDim = 0; iDim < lwpr_object_->model.nOut; iDim++)
    for (int iRF = 0; iRF < lwpr_object_->model.sub[iDim].numRFS; iRF++)
      for (int j = 0; j < lwpr_object_->nIn(); j++)
        values[ii++] = lwpr_object_->model.sub[iDim].rf[iRF]->D[j*lwpr_object_->model.nInStore+j];

  for (int iDim = 0; iDim < lwpr_object_->model.nOut; iDim++)
    for (int iRF = 0; iRF < lwpr_object_->model.sub[iDim].numRFS; iRF++)
      for (int j = 0; j < lwpr_object_->model.sub[iDim].rf[iRF]->nReg; j++)
        values[ii++] = lwpr_object_->model.sub[iDim].rf[iRF]->beta[j];
      
  for (int iDim = 0; iDim < lwpr_object_->model.nOut; iDim++)
    for (int iRF = 0; iRF < lwpr_object_->model.sub[iDim].numRFS; iRF++)
      values[ii++] = lwpr_object_->model.sub[iDim].rf[iRF]->beta0;
  
  assert(ii == getParameterVectorAllSize()); 
  
};

void Reservoir::linear_activation(VectorXd &inputs, VectorXd &results) {
    unsigned int num_elems = (inputs.rows() > inputs.cols()) ? inputs.rows() : inputs.cols();
    results.resize(num_elems, 1);
    for (unsigned int i=0; i<num_elems; i++) {
        results(i) = inputs(i);
    }
}

int contactPhi(RigidBodyManipulator* r, SupportStateElement& supp, void *map_ptr, VectorXd &phi, double terrain_height)
{
  std::unique_ptr<RigidBody>& b = r->bodies[supp.body_idx];
  int nc = supp.contact_pt_inds.size();
  phi.resize(nc);

  if (nc<1) return nc;

  Vector3d contact_pos,pos,posB,normal; Vector4d tmp;

  int i=0;
  for (std::set<int>::iterator pt_iter=supp.contact_pt_inds.begin(); pt_iter!=supp.contact_pt_inds.end(); pt_iter++) {
    if (*pt_iter<0 || *pt_iter>=b->contact_pts.cols()) 
      mexErrMsgIdAndTxt("DRC:ControlUtil:BadInput","requesting contact pt %d but body only has %d pts",*pt_iter,b->contact_pts.cols());
    
    tmp = b->contact_pts.col(*pt_iter);
    r->forwardKin(supp.body_idx,tmp,0,contact_pos);
    collisionDetect(map_ptr,contact_pos,pos,NULL,terrain_height);
    pos -= contact_pos;  // now -rel_pos in matlab version
    
    phi(i) = pos.norm();
    if (pos.dot(normal)>0)
      phi(i)=-phi(i);
    i++;
  }
  return nc;
}

VectorXd linear::field_to_vctr(const kind::f scalarf ) {

  std::vector<int> indices;

  std::vector<FT> ff;

  for(F_v_it fv=T.finite_vertices_begin();
      fv!=T.finite_vertices_end();
      fv++)  {

    indices.push_back( fv->idx.val() );
    ff.push_back( fv->sf(scalarf).val() );

  }

  int N=indices.size();
  VectorXd vctr;

  vctr.resize(N);

  for(int nn=0; nn<indices.size(); nn++)  {
    vctr(indices[nn])=ff[nn];

    // cout << indices[nn] << " : "
    //  	 << ff[nn] << endl;
  }

  return vctr;

}

 bool BulletModel::collisionRaycast(const Matrix3Xd &origins, const Matrix3Xd &ray_endpoints, bool use_margins, VectorXd &distances)
 {
   
   distances.resize(origins.cols());
   BulletCollisionWorldWrapper& bt_world = getBulletWorld(use_margins);
   
   for (int i = 0; i < origins.cols(); i ++)
   {
   
       btVector3 ray_from_world(origins(0,i), origins(1,i), origins(2,i));
       btVector3 ray_to_world(ray_endpoints(0,i), ray_endpoints(1,i), ray_endpoints(2,i));
       
       btCollisionWorld::ClosestRayResultCallback ray_callback(ray_from_world, ray_to_world);
       
       bt_world.bt_collision_world->rayTest(ray_from_world, ray_to_world, ray_callback);
       
       if (ray_callback.hasHit()) {
           
           // compute distance to hit
           
           btVector3 end = ray_callback.m_hitPointWorld;
           
           Vector3d end_eigen(end.getX(), end.getY(), end.getZ());
           
           distances(i) = (end_eigen - origins.col(i)).norm();
         
       } else {
           distances(i) = -1;
       }
   }
   
   return true;
 } 

bool EEGoSportsDriver::getSampleMatrixValue(Eigen::MatrixXd &sampleMatrix)
{
    //Check if device was initialised and connected correctly
    if(!m_bInitDeviceSuccess) {
        std::cout << "Plugin EEGoSports - ERROR - getSampleMatrixValue() - Cannot start to get samples from device because device was not initialised correctly" << std::endl;
        return false;
    }

    sampleMatrix = MatrixXd::Zero(m_uiNumberOfChannels, m_uiSamplesPerBlock);

    uint iSampleIterator, iReceivedSamples, iChannelCount, i, j;

    iSampleIterator = 0;

    buffer buf;
    VectorXd vec;

    //get samples from device until the complete matrix is filled, i.e. the samples per block size is met
    while(iSampleIterator < m_uiSamplesPerBlock) {
        //Get sample block from device
        buf = m_pDataStream->getData();

        iReceivedSamples = buf.getSampleCount();
        iChannelCount = buf.getChannelCount();

         //Write the received samples to an extra buffer, so that they are not getting lost if too many samples were received. These are then written to the next matrix (block)
        for(i = 0; i < iReceivedSamples; ++i) {
            vec.resize(iChannelCount);

            for(j = 0; j < iChannelCount; ++j) {
                vec(j) = buf.getSample(j,i);
                //std::cout<<vec(j)<<std::endl;
            }

            m_lSampleBlockBuffer.push_back(vec);
        }

        //Fill matrix with data from buffer
        while(!m_lSampleBlockBuffer.isEmpty()) {
            if(iSampleIterator >= m_uiSamplesPerBlock) {
                break;
            }

            sampleMatrix.col(iSampleIterator) = m_lSampleBlockBuffer.takeFirst();

            iSampleIterator++;
        }

        if(m_outputFileStream.is_open() && m_bWriteDriverDebugToFile) {
            m_outputFileStream << "buf.getSampleCount(): " << buf.getSampleCount() << std::endl;
            m_outputFileStream << "buf.getChannelCount(): " << buf.getChannelCount() << std::endl;
            m_outputFileStream << "buf.size(): " << buf.size() << std::endl;
            m_outputFileStream << "iSampleIterator: " << iSampleIterator << std::endl;
            m_outputFileStream << "m_lSampleBlockBuffer.size(): " << m_lSampleBlockBuffer.size() << std::endl << std::endl;
        }
    }

    return true;
}