C++ VectorXi::size方法代码示例

 aslam::backend::EuclideanExpression EuclideanBSplineDesignVariable::toEuclideanExpression(double time, int order)
 {
     Eigen::VectorXi dvidxs = _bspline.localVvCoefficientVectorIndices(time);
     std::vector<aslam::backend::DesignVariable *> dvs;
     for(int i = 0; i < dvidxs.size(); ++i)
     {
         dvs.push_back(&_designVariables[dvidxs[i]]);
     }
     boost::shared_ptr<aslam::splines::BSplineEuclideanExpressionNode > root( new aslam::splines::BSplineEuclideanExpressionNode(&_bspline, dvs, time, order) );
     
     return aslam::backend::EuclideanExpression(root);
 }

void ExecControl::reachability()
{
	rgraph.clear();
	Eigen::VectorXi trans = marking.transpose()*Dm;

	std::cout<<"Transition matrix:"<<trans<<std::endl;

	int last_m = boost::add_vertex(rgraph);
	rgraph[last_m].marking = marking;

	Eigen::VectorXi fire = Eigen::VectorXi::Zero(trans.size());
	int tnum = -1;
	for (int i=0; i<trans.size(); ++i)
	{
		if (trans(i)>0)
		{
			std::cout<<"Transition "<<i<<" can fire."<<std::endl;
			fire(i) = 1;
			tnum = i;
			break;
		}
	}

	//marking = marking + (Dp-Dm)*fire;

	int new_m = boost::add_vertex(rgraph);
	rgraph[new_m].marking = marking;
	REdgeProperty prop;
	prop.t = tnum;
	prop.weight = 1;
	//Enable edge
	boost::add_edge(last_m, new_m, prop, rgraph);
	prop.t+=1;
	//Disable edge added so we can skip it in next iteration.
	boost::add_edge(new_m, last_m, prop, rgraph);


	std::cout<<"Transition fired, new marking:"<<marking<<std::endl;
}

IGL_INLINE bool igl::copyleft::boolean::mesh_boolean(
  const Eigen::PlainObjectBase<DerivedVA > & VA,
  const Eigen::PlainObjectBase<DerivedFA > & FA,
  const Eigen::PlainObjectBase<DerivedVB > & VB,
  const Eigen::PlainObjectBase<DerivedFB > & FB,
  const MeshBooleanType & type,
  Eigen::PlainObjectBase<DerivedVC > & VC,
  Eigen::PlainObjectBase<DerivedFC > & FC,
  Eigen::PlainObjectBase<DerivedJ > & J)
{
  typedef CGAL::Epeck Kernel;
  typedef Kernel::FT ExactScalar;
  typedef Eigen::Matrix<
    ExactScalar,
    Eigen::Dynamic,
    Eigen::Dynamic,
    DerivedVC::IsRowMajor> MatrixXES;

  std::function<void(
      const Eigen::PlainObjectBase<DerivedVA>&,
      const Eigen::PlainObjectBase<DerivedFA>&,
      Eigen::PlainObjectBase<MatrixXES>&,
      Eigen::PlainObjectBase<DerivedFC>&,
      Eigen::PlainObjectBase<DerivedJ>&)> resolve_func =
    [](const Eigen::PlainObjectBase<DerivedVA>& V,
        const Eigen::PlainObjectBase<DerivedFA>& F,
        Eigen::PlainObjectBase<MatrixXES>& Vo,
        Eigen::PlainObjectBase<DerivedFC>& Fo,
        Eigen::PlainObjectBase<DerivedJ>& J) {
      Eigen::VectorXi I;
      igl::copyleft::cgal::RemeshSelfIntersectionsParam params;

      MatrixXES Vr;
      DerivedFC Fr;
      Eigen::MatrixXi IF;
      igl::copyleft::cgal::remesh_self_intersections(
          V, F, params, Vr, Fr, IF, J, I);
      assert(I.size() == Vr.rows());

      // Merge coinciding vertices into non-manifold vertices.
      std::for_each(Fr.data(), Fr.data()+Fr.size(),
          [&I](typename DerivedFC::Scalar& a) { a=I[a]; });

      // Remove unreferenced vertices.
      Eigen::VectorXi UIM;
      igl::remove_unreferenced(Vr, Fr, Vo, Fo, UIM);
    };

  return mesh_boolean(VA,FA,VB,FB,type,resolve_func,VC,FC,J);
}

int ComputationGraph::matrix(Eigen::VectorXi vector) {
    std::vector<int> output;
    for (int i = 0; i < vector.size(); i++) {
        output.push_back(vector(i));
    }

    nodes.push_back({
        -1,
        {},
        output
    });

    return nodes.size() - 1;
}

IGL_INLINE void igl::n_polyvector(const Eigen::MatrixXd &V,
                             const Eigen::MatrixXi &F,
                             const Eigen::VectorXi& b,
                             const Eigen::MatrixXd& bc,
                             Eigen::MatrixXd &output)
{
  Eigen::VectorXi isConstrained = Eigen::VectorXi::Constant(F.rows(),0);
  Eigen::MatrixXd cfW = Eigen::MatrixXd::Constant(F.rows(),bc.cols(),0);

  for(unsigned i=0; i<b.size();++i)
  {
    isConstrained(b(i)) = 1;
    cfW.row(b(i)) << bc.row(i);
  }
  if (b.size() == F.rows())
  {
    output = cfW;
    return;
  }

  int n = cfW.cols()/3;
  igl::PolyVectorFieldFinder<Eigen::MatrixXd, Eigen::MatrixXi> pvff(V,F,n);
  pvff.solve(isConstrained, cfW, output);
}

void cDrawKinTree::DrawTree(const Eigen::MatrixXd& joint_desc, const Eigen::VectorXd& pose, int joint_id, double link_width,
						const tVector& fill_col, const tVector& line_col)
{
	const double node_radius = 0.02;

	if (joint_id != cKinTree::gInvalidJointID)
	{
		bool has_parent = cKinTree::HasParent(joint_desc, joint_id);
		if (has_parent)
		{
			tVector attach_pt = cKinTree::GetScaledAttachPt(joint_desc, joint_id);
			attach_pt[2] = 0; // hack ignore z
			double len = attach_pt.norm();

			glPushMatrix();
			tVector pos = tVector(len / 2, 0, 0, 0);
			tVector size = tVector(len, link_width, 0, 0);

			double theta = std::atan2(attach_pt[1], attach_pt[0]);
			cDrawUtil::Rotate(theta, tVector(0, 0, 1, 0));

			cDrawUtil::SetColor(tVector(fill_col[0], fill_col[1], fill_col[2], fill_col[3]));
			cDrawUtil::DrawRect(pos, size, cDrawUtil::eDrawSolid);
			cDrawUtil::SetColor(tVector(line_col[0], line_col[1], line_col[2], line_col[3]));
			cDrawUtil::DrawRect(pos, size, cDrawUtil::eDrawWire);
			
			// draw node
			cDrawUtil::SetColor(tVector(fill_col[0] * 0.25, fill_col[1] * 0.25, fill_col[2] * 0.25, fill_col[3]));
			cDrawUtil::DrawDisk(node_radius, 16);
			
			glPopMatrix();
		}

		glPushMatrix();
		tMatrix m = cKinTree::ChildParentTrans(joint_desc, pose, joint_id);
		cDrawUtil::GLMultMatrix(m);

		Eigen::VectorXi children;
		cKinTree::FindChildren(joint_desc, joint_id, children);
		for (int i = 0; i < children.size(); ++i)
		{
			int child_id = children[i];
			DrawTree(joint_desc, pose, child_id, link_width, fill_col, line_col);
		}

		glPopMatrix();
	}
}

int main(int argc, char *argv[])
{
  using namespace Eigen;
  using namespace std;
  igl::readOFF("../shared/decimated-knight.off",V,F);
  U=V;
  igl::readDMAT("../shared/decimated-knight-selection.dmat",S);

  // vertices in selection
  igl::colon<int>(0,V.rows()-1,b);
  b.conservativeResize(stable_partition( b.data(), b.data()+b.size(), 
   [](int i)->bool{return S(i)>=0;})-b.data());
  // Centroid
  mid = 0.5*(V.colwise().maxCoeff() + V.colwise().minCoeff());
  // Precomputation
  arap_data.max_iter = 100;
  igl::arap_precomputation(V,F,V.cols(),b,arap_data);

  // Set color based on selection
  MatrixXd C(F.rows(),3);
  RowVector3d purple(80.0/255.0,64.0/255.0,255.0/255.0);
  RowVector3d gold(255.0/255.0,228.0/255.0,58.0/255.0);
  for(int f = 0;f<F.rows();f++)
  {
    if( S(F(f,0))>=0 && S(F(f,1))>=0 && S(F(f,2))>=0)
    {
      C.row(f) = purple;
    }else
    {
      C.row(f) = gold;
    }
  }

  // Plot the mesh with pseudocolors
  igl::viewer::Viewer viewer;
  viewer.data.set_mesh(U, F);
  viewer.data.set_colors(C);
  viewer.callback_pre_draw = &pre_draw;
  viewer.callback_key_down = &key_down;
  viewer.core.is_animating = false;
  viewer.core.animation_max_fps = 30.;
  cout<<
    "Press [space] to toggle animation"<<endl;
  viewer.launch();
}

// Plots the mesh with an N-RoSy field and its singularities on top
// The constrained faces (b) are colored in red.
void plot_mesh_nrosy(
    igl::viewer::Viewer& viewer,
    Eigen::MatrixXd& V,
    Eigen::MatrixXi& F,
    int N,
    Eigen::MatrixXd& PD1,
    Eigen::VectorXd& S,
    Eigen::VectorXi& b)
{
    using namespace Eigen;
    using namespace std;
    // Clear the mesh
    viewer.data.clear();
    viewer.data.set_mesh(V,F);

    // Expand the representative vectors in the full vector set and plot them as lines
    double avg = igl::avg_edge_length(V, F);
    MatrixXd Y;
    representative_to_nrosy(V, F, PD1, N, Y);

    MatrixXd B;
    igl::barycenter(V,F,B);

    MatrixXd Be(B.rows()*N,3);
    for(unsigned i=0; i<B.rows(); ++i)
        for(unsigned j=0; j<N; ++j)
            Be.row(i*N+j) = B.row(i);

    viewer.data.add_edges(Be,Be+Y*(avg/2),RowVector3d(0,0,1));

    // Plot the singularities as colored dots (red for negative, blue for positive)
    for (unsigned i=0; i<S.size(); ++i)
    {
        if (S(i) < -0.001)
            viewer.data.add_points(V.row(i),RowVector3d(1,0,0));
        else if (S(i) > 0.001)
            viewer.data.add_points(V.row(i),RowVector3d(0,1,0));
    }

    // Highlight in red the constrained faces
    MatrixXd C = MatrixXd::Constant(F.rows(),3,1);
    for (unsigned i=0; i<b.size(); ++i)
        C.row(b(i)) << 1, 0, 0;
    viewer.data.set_colors(C);
}

IGL_INLINE void igl::polyvector_field_singularities_from_matchings(
                                                                   const Eigen::PlainObjectBase<DerivedV> &V,
                                                                   const Eigen::PlainObjectBase<DerivedF> &F,
                                                                   const std::vector<bool> &V_border,
                                                                   const std::vector<std::vector<VFType> > &VF,
                                                                   const Eigen::MatrixXi &TT,
                                                                   const Eigen::MatrixXi &E2F,
                                                                   const Eigen::MatrixXi &F2E,
                                                                   const Eigen::PlainObjectBase<DerivedM> &match_ab,
                                                                   const Eigen::PlainObjectBase<DerivedM> &match_ba,
                                                                   Eigen::PlainObjectBase<DerivedS> &singularities)
{

  int numV = V.rows();

  std::vector<int> singularities_v;
  int half_degree = match_ab.cols();
  for (int vi =0; vi<numV; ++vi)
  {
    ///check that is on border..
    if (V_border[vi])
      continue;
    Eigen::VectorXi fi;
    Eigen::MatrixXi mvi;
    igl::polyvector_field_one_ring_matchings(V, F, VF, E2F, F2E, TT, match_ab, match_ba, vi, mvi, fi);

    int num = fi.size();
    //pick one of the vectors to check for singularities
    for (int vector_to_match = 0; vector_to_match < half_degree; ++vector_to_match)
    {
      if(mvi(num-1,vector_to_match) != mvi(0,vector_to_match))
      {
        singularities_v.push_back(vi);
        break;
      }
    }
  }
  std::sort(singularities_v.begin(), singularities_v.end());
  auto last = std::unique(singularities_v.begin(), singularities_v.end());
  singularities_v.erase(last, singularities_v.end());

  igl::list_to_matrix(singularities_v, singularities);
}

void place::removeMinimumConnectedComponents(cv::Mat &image) {
  std::list<std::pair<int, int>> toLabel;
  Eigen::RowMatrixXi labeledImage =
      Eigen::RowMatrixXi::Zero(image.rows, image.cols);
  int currentLabel = 1;
  for (int j = 0; j < image.rows; ++j) {
    const uchar *src = image.ptr<uchar>(j);
    for (int i = 0; i < image.cols; ++i) {
      if (src[i] != 255 && labeledImage(j, i) == 0) {
        labeledImage(j, i) = currentLabel;
        toLabel.emplace_front(j, i);

        labelNeighbours(image, currentLabel, labeledImage, toLabel);

        ++currentLabel;
      }
    }
  }

  Eigen::VectorXi countPerLabel = Eigen::VectorXi::Zero(currentLabel);
  const int *labeledImagePtr = labeledImage.data();
  for (int i = 0; i < labeledImage.size(); ++i)
    ++countPerLabel[*(labeledImagePtr + i)];

  double average = 0.0, sigma = 0.0;
  const int *countPerLabelPtr = countPerLabel.data();
  std::tie(average, sigma) = place::aveAndStdev(
      countPerLabelPtr + 1, countPerLabelPtr + countPerLabel.size());

  double threshHold = average;
  for (int j = 0; j < image.rows; ++j) {
    uchar *src = image.ptr<uchar>(j);
    for (int i = 0; i < image.cols; ++i) {
      if (src[i] != 255) {
        const int label = labeledImage(j, i);
        const int count = countPerLabel[label];
        if (count < threshHold)
          src[i] = 255;
      }
    }
  }
}

void addMotionErrorTerms(OptimizationProblemBase& problem, SplineDv& splineDv, const Eigen::MatrixXd& W, unsigned int errorTermOrder) {
  // Add one error term for each segment
  auto spline = splineDv.spline();

  for(int i = 0; i < spline.numValidTimeSegments(); ++i) {
    Eigen::MatrixXd R = spline.segmentIntegral(i, W, errorTermOrder);
    Eigen::VectorXd c = spline.segmentCoefficientVector(i);
    // These indices tell us the order of R and c.
    Eigen::VectorXi idxs = spline.segmentVvCoefficientVectorIndices(i);

    std::vector< DesignVariable * > dvs;
    for(unsigned i = 0; i < idxs.size(); ++i) {
      dvs.push_back(splineDv.designVariable(idxs[i]));
    }
    MarginalizationPriorErrorTerm::Ptr err( 
        new MarginalizationPriorErrorTerm( dvs, R*c, R ));
    problem.addErrorTerm(err);
            
  }
}

/**
 * @function getLeftArmIds
 * @brief Get DOF's IDs for ROBINA's left arm
 */
Eigen::VectorXi NSTab::GetLeftArmIds() {

  mEENode = mWorld->mRobots[mRobotId]->getNode( mEEName.c_str() );
  mEEId = mEENode->getSkelIndex();
  
  string LINK_NAMES[7] = {"LJ0", "LJ1", "LJ2", "LJ3", "LJ4", "LJ5", "LJ6" };
  
  Eigen::VectorXi linksAll = mWorld->mRobots[mRobotId]->getQuickDofsIndices(); 

  Eigen::VectorXi linksLeftArm(7);
  for( unsigned int i = 0; i < 7; i++ ) {
    for( unsigned int j = 0; j < linksAll.size(); j++ ) {      
      if( mWorld->mRobots[mRobotId]->getDof( linksAll[j] )->getJoint()->getChildNode()->getName() == LINK_NAMES[i] ) {
	linksLeftArm[i] = linksAll[j]; 
	break;   
      }
    }
  }
  
  return linksLeftArm;
}

/**
 * @function GetLinksId
 * @brief Get DOF's IDs for Manipulator
 */
Eigen::VectorXi ConfigTab::GetLinksId() {

    string LINK_NAMES[sNumLinks];
    if( gRobotName.compare("Robina") == 0 ) {
        LINK_NAMES[0] = "LJ0";
        LINK_NAMES[1] = "LJ1";
        LINK_NAMES[2] = "LJ2";
        LINK_NAMES[3] ="LJ3";
        LINK_NAMES[4] = "LJ4";
        LINK_NAMES[5] = "LJ5";
        LINK_NAMES[6] = "LJ6";
    }
    else if( gRobotName.compare("LWA3") == 0 ) {
        LINK_NAMES[0] = "L1";
        LINK_NAMES[1] = "L2";
        LINK_NAMES[2] = "L3";
        LINK_NAMES[3] = "L4";
        LINK_NAMES[4] = "L5";
        LINK_NAMES[5] = "L6";
        LINK_NAMES[6] = "FT";
    }

    Eigen::VectorXi linksAll = mWorld->getRobot(gRobotId)->getQuickDofsIndices();

    Eigen::VectorXi links( sNumLinks );
    for( unsigned int i = 0; i < sNumLinks; i++ ) {
        for( unsigned int j = 0; j < linksAll.size(); j++ ) {
            if( mWorld->getRobot(gRobotId)->getDof( linksAll[j] )->getJoint()->getChildNode()->getName() == LINK_NAMES[i] ) {
                links[i] = linksAll[j];
                break;
            }
        }
    }

    return links;
}

IGL_INLINE void igl::map_vertices_to_circle(
  const Eigen::MatrixXd& V,
  const Eigen::VectorXi& bnd,
  Eigen::MatrixXd& UV)
{
  // Get sorted list of boundary vertices
  std::vector<int> interior,map_ij;
  map_ij.resize(V.rows());

  std::vector<bool> isOnBnd(V.rows(),false);
  for (int i = 0; i < bnd.size(); i++)
  {
    isOnBnd[bnd[i]] = true;
    map_ij[bnd[i]] = i;
  }

  for (int i = 0; i < (int)isOnBnd.size(); i++)
  {
    if (!isOnBnd[i])
    {
      map_ij[i] = interior.size();
      interior.push_back(i);
    }
  }

  // Map boundary to unit circle
  std::vector<double> len(bnd.size());
  len[0] = 0.;

  for (int i = 1; i < bnd.size(); i++)
  {
    len[i] = len[i-1] + (V.row(bnd[i-1]) - V.row(bnd[i])).norm();
  }
  double total_len = len[len.size()-1] + (V.row(bnd[0]) - V.row(bnd[bnd.size()-1])).norm();

  UV.resize(bnd.size(),2);
  for (int i = 0; i < bnd.size(); i++)
  {
    double frac = len[i] * 2. * M_PI / total_len;
    UV.row(map_ij[bnd[i]]) << cos(frac), sin(frac);
  }

}

int main(int argc, char *argv[])
{
  using namespace Eigen;
  using namespace std;
  igl::readOBJ(TUTORIAL_SHARED_PATH "/decimated-max.obj",V,F);
  U=V;
  // S(i) = j: j<0 (vertex i not in handle), j >= 0 (vertex i in handle j)
  VectorXi S;
  igl::readDMAT(TUTORIAL_SHARED_PATH "/decimated-max-selection.dmat",S);
  igl::colon<int>(0,V.rows()-1,b);
  b.conservativeResize(stable_partition( b.data(), b.data()+b.size(),
   [&S](int i)->bool{return S(i)>=0;})-b.data());

  // Boundary conditions directly on deformed positions
  U_bc.resize(b.size(),V.cols());
  V_bc.resize(b.size(),V.cols());
  for(int bi = 0;bi<b.size();bi++)
  {
    V_bc.row(bi) = V.row(b(bi));
    switch(S(b(bi)))
    {
      case 0:
        // Don't move handle 0
        U_bc.row(bi) = V.row(b(bi));
        break;
      case 1:
        // move handle 1 down
        U_bc.row(bi) = V.row(b(bi)) + RowVector3d(0,-50,0);
        break;
      case 2:
      default:
        // move other handles forward
        U_bc.row(bi) = V.row(b(bi)) + RowVector3d(0,0,-25);
        break;
    }
  }

  // Pseudo-color based on selection
  MatrixXd C(F.rows(),3);
  RowVector3d purple(80.0/255.0,64.0/255.0,255.0/255.0);
  RowVector3d gold(255.0/255.0,228.0/255.0,58.0/255.0);
  for(int f = 0;f<F.rows();f++)
  {
    if( S(F(f,0))>=0 && S(F(f,1))>=0 && S(F(f,2))>=0)
    {
      C.row(f) = purple;
    }else
    {
      C.row(f) = gold;
    }
  }

  // Plot the mesh with pseudocolors
  igl::opengl::glfw::Viewer viewer;
  viewer.data().set_mesh(U, F);
  viewer.data().show_lines = false;
  viewer.data().set_colors(C);
  viewer.core().trackball_angle = Eigen::Quaternionf(sqrt(2.0),0,sqrt(2.0),0);
  viewer.core().trackball_angle.normalize();
  viewer.callback_pre_draw = &pre_draw;
  viewer.callback_key_down = &key_down;
  //viewer.core().is_animating = true;
  viewer.core().animation_max_fps = 30.;
  cout<<
    "Press [space] to toggle deformation."<<endl<<
    "Press 'd' to toggle between biharmonic surface or displacements."<<endl;
  viewer.launch();
}