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

GTEST_TEST(TestConvexHull, testRandomConvexCombinations) {
  // Generate a set of points, then find a random convex combination of those
  // points, and verify that it's correctly reported as being inside the
  // convex hull
  for (int i = 2; i < 50; ++i) {
    for (int j = 0; j < 500; ++j) {
      MatrixXd pts = MatrixXd::Random(2, i);
      VectorXd weights = VectorXd::Random(i);
      if (weights.minCoeff() < 0) {
        weights = weights.array() -
                  weights.minCoeff();  // make sure they're all nonnegative
      }
      weights = weights.array() / weights.sum();
      Vector2d q = pts * weights;
      EXPECT_TRUE(inConvexHull(pts, q, 1e-8));
    }
  }
}

Matrix<bool, Dynamic, 1> getActiveSupportMask(
    const RigidBodyTree &r, VectorXd q, VectorXd qd,
    std::vector<SupportStateElement,
                Eigen::aligned_allocator<SupportStateElement>> &
        available_supports,
    const Ref<const Matrix<bool, Dynamic, 1>> &contact_force_detected,
    double contact_threshold) {
  KinematicsCache<double> cache = r.doKinematics(q, qd);

  size_t nsupp = available_supports.size();
  Matrix<bool, Dynamic, 1> active_supp_mask =
      Matrix<bool, Dynamic, 1>::Zero(nsupp);
  VectorXd phi;
  SupportStateElement se;
  bool needs_kin_check;
  bool kin_contact;
  bool force_contact;

  for (size_t i = 0; i < nsupp; i++) {
    // mexPrintf("evaluating support: %d\n", i);
    se = available_supports[i];

    force_contact = (contact_force_detected(se.body_idx) != 0);
    // Determine if the body needs to be checked for kinematic contact. We only
    // need to check for kin contact if the logic map indicates that the
    // presence or absence of such contact would  affect the decision about
    // whether to use that body as a support.
    needs_kin_check = (((se.support_logic_map[1] != se.support_logic_map[0]) &&
                        (contact_force_detected(se.body_idx) == 0)) ||
                       ((se.support_logic_map[3] != se.support_logic_map[2]) &&
                        (contact_force_detected(se.body_idx) == 1)));

    if (needs_kin_check) {
      if (contact_threshold == -1) {
        kin_contact = true;
      } else {
        contactPhi(r, cache, se, phi);
        kin_contact = (phi.minCoeff() <= contact_threshold);
      }
    } else {
      kin_contact = false;  // we've determined already that kin contact doesn't
                            // matter for this support element
    }

    active_supp_mask(i) =
        isSupportElementActive(&se, force_contact, kin_contact);
    // mexPrintf("needs check: %d force contact: %d kin_contact: %d is_active:
    // %d\n", needs_kin_check, force_contact, kin_contact, active_supp_mask(i));
  }
  return active_supp_mask;
}

void  UpdaterMean::costsToWeights(const VectorXd& costs, string weighting_method, double eliteness, VectorXd& weights) const
{
  weights.resize(costs.size());
  if (weighting_method.compare("PI-BB")==0)
  {
    // PI^2 style weighting: continuous, cost exponention
    double h = eliteness; // In PI^2, eliteness parameter is known as "h"
    double range = costs.maxCoeff()-costs.minCoeff();
    if (range==0)
      weights.fill(1);
    else
      weights = (-h*(costs.array()-costs.minCoeff())/range).exp();
  } 
  else if (weighting_method.compare("CMA-ES")==0 || weighting_method.compare("CEM")==0 )
  {
    // CMA-ES and CEM are rank-based, so we must first sort the costs, and the assign a weight to 
    // each rank.
    VectorXd costs_sorted = costs; 
    std::sort(costs_sorted.data(), costs_sorted.data()+costs_sorted.size());
    // In Python this is more elegant because we have argsort.
    // indices = np.argsort(costs)
    // It is possible to do this with fancy lambda functions or std::pair in C++ too, but  I don't
    // mind writing two for loops instead ;-)
    
    weights.fill(0.0);
    int mu = eliteness; // In CMA-ES, eliteness parameter is known as "mu"
    assert(mu<costs.size());
    for (int ii=0; ii<mu; ii++)
    {
      double cur_cost = costs_sorted[ii];
      for (int jj=0; jj<costs.size(); jj++)
      {
        if (costs[jj] == cur_cost)
        {
          if (weighting_method.compare("CEM")==0)
            weights[jj] = 1.0/mu; // CEM
          else
            weights[jj] = log(mu+0.5) - log(ii+1); // CMA-ES
          break;
        }
      }
      
    }
    // For debugging
    //MatrixXd print_mat(3,costs.size());
    //print_mat.row(0) = costs_sorted;
    //print_mat.row(1) = costs;
    //print_mat.row(2) = weights;
    //cout << print_mat << endl;
  }
  else
  {
    cout << __FILE__ << ":" << __LINE__ << ":WARNING: Unknown weighting method '" << weighting_method << "'. Calling with PI-BB weighting." << endl; 
    costsToWeights(costs, "PI-BB", eliteness, weights);
    return;
  }
  
  // Relative standard deviation of total costs
  double mean = weights.mean();
  double std = sqrt((weights.array()-mean).pow(2).mean());
  double rel_std = std/mean;
  if (rel_std<1e-10)
  {
      // Special case: all costs are the same
      // Set same weights for all.
      weights.fill(1);
  }

  // Normalize weights
  weights = weights/weights.sum();

}

int main(int argc, char* argv[])
{
  //Misc. initialization
  StartTime = (unsigned)time(0); //Time the program starts
  cout.precision(12);
  cout << fixed;
  //End of section

  //Initialize local variables
  fstream vcidata,spectfile; //File streams for the input and output files
  MatrixXd VCIHam; //Full Hamiltonian matrix
  MatrixXd CIVec; //CI eigenvectors (wavefunction)
  VectorXd CIFreq; //CI eigenvalues (frequencies
  double Ezpe = 0; //CI zero-point energy
  double RunTime; //Run time for the calculations
  string TimeUnits; //Seconds, minutes, or hours for RunTime
  //End of section

  //Print title and compile date
  PrintFancyTitle();
  cout << "Last modification: ";
  cout << __TIME__ << " on ";
  cout << __DATE__ << '\n';
  cout << '\n';
  cout.flush();
  //End of section

  //Gather input and check for errors
  cout << "Reading input..." << '\n';
  ReadCIArgs(argc,argv,vcidata,spectfile); //Read arguments
  ReadCIInput(VCIHam,vcidata); //Read input files
  //End of section

  //Adjust force constants
  ScaleFC();
  //End of section

  //Calculate spectrum
  cout << "Constructing the CI Hamiltonian...";
  cout << '\n';
  cout << "  Zeroth-order Hamiltionian";
  cout.flush(); //Print progress
  ZerothHam(VCIHam); //Harmonic terms
  cout << "; Done." << '\n';
  cout << "  Adding anharmonic potential";
  cout.flush(); //Print progress
  AnharmHam(VCIHam); //Anharmonic terms
  cout << "; Done." << '\n';
  cout << '\n';
  cout << "Diagonalizing the Hamiltonian...";
  cout << '\n' << '\n';
  cout.flush(); //Print progress
  VCIDiagonalize(VCIHam,CIVec,CIFreq); //Diagonalization wrapper
  //End of section

  //Free memory
  VCIHam = MatrixXd(1,1);
  //End of section

  //Calculate and remove ZPE
  VectorXd UnitVec(BasisSet.size()); //Create a unit vector
  UnitVec.setOnes(); //Subtracting a constant requires a vector
  Ezpe = CIFreq.minCoeff(); //Find ZPE
  if (Ezpe < 0)
  {
    //Print error
    cout << "Error: The zero-point energy is negative.";
    cout << " Something is wrong.";
    cout << '\n';
    cout << "Try reducing the number of quanta or removing";
    cout << " large anharmonic";
    cout << '\n';
    cout << "force constants.";
    cout << '\n' << '\n';
    cout.flush();
    //Quit
    exit(0);
  }
  CIFreq -= (Ezpe*UnitVec); //Remove ZPE from all frequencies
  //End of section

  //Free memory
  UnitVec = VectorXd(1);
  //End of section

  //Print results
  cout << "Printing the spectrum...";
  cout << '\n' << '\n';
  cout << "Results:" << '\n';
  cout << "  Zero-point energy: ";
  cout.precision(2); //Truncate energy
  cout << Ezpe << " (1/cm)"; //Print energy
  cout.precision(12); //Replace settings
  cout << '\n';
  cout.flush(); //Print progress
  PrintSpectrum(CIFreq,CIVec,spectfile); //Convert the frequencies to a spectrum
  EndTime = (unsigned)time(0); //Time the calculation stops
  RunTime = (double)(EndTime-StartTime); //Total run time
  if (RunTime >= 3600)
  {
//.........这里部分代码省略.........

void mexFunction(int nlhs, mxArray *plhs[], 
    int nrhs, const mxArray *prhs[])
{
  // This is useful for debugging whether Matlab is caching the mex binary
  //mexPrintf("%s %s\n",__TIME__,__DATE__);
  igl::matlab::MexStream mout;
  std::streambuf *outbuf = std::cout.rdbuf(&mout);

  using namespace std;
  using namespace Eigen;
  using namespace igl;
  using namespace igl::matlab;
  using namespace igl::copyleft::cgal;

  MatrixXd V;
  MatrixXi F;
  igl::copyleft::cgal::RemeshSelfIntersectionsParam params;

  string prefix;
  bool use_obj_format = false;
  if(nrhs < 2)
  {
    mexErrMsgTxt("nrhs < 2");
  }
  parse_rhs_double(prhs,V);
  parse_rhs_index(prhs+1,F);
  mexErrMsgTxt(V.cols()==3,"V must be #V by 3");
  mexErrMsgTxt(F.cols()==3,"F must be #F by 3");

  if(nrhs>2)
  {
    int i = 2;
    while(i<nrhs)
    {
      if(!mxIsChar(prhs[i]))
      {
        mexErrMsgTxt("Parameter names should be char strings");
      }
      // Cast to char
      const char * name = mxArrayToString(prhs[i]);
      if(strcmp("DetectOnly",name) == 0)
      {
        validate_arg_scalar(i,nrhs,prhs,name);
        validate_arg_logical(i,nrhs,prhs,name);
        mxLogical * v = (mxLogical *)mxGetData(prhs[++i]);
        params.detect_only = *v;
      }else if(strcmp("FirstOnly",name) == 0)
      {
        validate_arg_scalar(i,nrhs,prhs,name);
        validate_arg_logical(i,nrhs,prhs,name);
        mxLogical * v = (mxLogical *)mxGetData(prhs[++i]);
        params.first_only = *v;
      }else if(strcmp("StitchAll",name) == 0)
      {
        validate_arg_scalar(i,nrhs,prhs,name);
        validate_arg_logical(i,nrhs,prhs,name);
        mxLogical * v = (mxLogical *)mxGetData(prhs[++i]);
        params.stitch_all = *v;
      }else
      {
        mexErrMsgTxt(C_STR("Unsupported parameter: "<<name));
      }
      i++;
    }
  }
  MatrixXi IF;
  VectorXi J,IM;
  if(F.rows()>0)
  {
    // Check that there aren't any combinatorially or geometrically degenerate triangles
    VectorXd A;
    doublearea(V,F,A);
    if(A.minCoeff()<=0)
    {
      mexErrMsgTxt("Geometrically degenerate face found.");
    }
    if(
       (F.array().col(0) == F.array().col(1)).any() ||
       (F.array().col(1) == F.array().col(2)).any() ||
       (F.array().col(2) == F.array().col(0)).any())
    {
      mexErrMsgTxt("Combinatorially degenerate face found.");
    }

    // Now mesh self intersections
    {
      MatrixXd tempV;
      MatrixXi tempF;
      remesh_self_intersections(V,F,params,tempV,tempF,IF,J,IM);
      //cout<<BLUEGIN("Found and meshed "<<IF.rows()<<" pair"<<(IF.rows()==1?"":"s")
      //  <<" of self-intersecting triangles.")<<endl;
      V=tempV;
      F=tempF;
    }

  // Double-check output

#ifdef DEBUG
    // There should be *no* combinatorial duplicates
    {
//.........这里部分代码省略.........

//.........这里部分代码省略.........
    memcpy(mxGetData(plhs[0]),&pdata,sizeof(pdata));
     
    return;
  }
  
  // first get the ptr back from matlab
  if (!mxIsNumeric(prhs[0]) || mxGetNumberOfElements(prhs[0])!=1)
    mexErrMsgIdAndTxt("DRC:supportDetectmex:BadInputs","the first argument should be the ptr");
  memcpy(&pdata,mxGetData(prhs[0]),sizeof(pdata));

  int nq = pdata->r->num_positions;
  int nv = pdata->r->num_velocities;

  int narg=1;  
  double *q = mxGetPr(prhs[narg++]);
  double *qd = &q[nq];
  
  Map<VectorXd> qvec(q,nq);
  Map<VectorXd> qdvec(qd, nv);

  int desired_support_argid = narg++;

  double* double_contact_sensor = mxGetPr(prhs[narg]); int len = static_cast<int>(mxGetNumberOfElements(prhs[narg++]));
  VectorXi contact_sensor(len);  
  for (i=0; i<len; i++)
    contact_sensor(i)=(int)double_contact_sensor[i];
  double contact_threshold = mxGetScalar(prhs[narg++]);
  double terrain_height = mxGetScalar(prhs[narg++]); // nonzero if we're using DRCFlatTerrainMap
  
  int contact_logic_AND = (int) mxGetScalar(prhs[narg++]); // true if we should AND plan and sensor, false if we should OR them

  KinematicsCache<double> cache = pdata->r->doKinematics(qvec, qdvec); // FIXME: pass this into the function.

  //---------------------------------------------------------------------
  // Compute active support from desired supports -----------------------
  vector<SupportStateElement,Eigen::aligned_allocator<SupportStateElement>> active_supports;
  set<int> contact_bodies; // redundant, clean up later
  int num_active_contact_pts=0;
  if (!mxIsEmpty(prhs[desired_support_argid])) {
    VectorXd phi;
    mxArray* mxBodies;
    mxBodies = mxGetProperty(prhs[desired_support_argid],0,"bodies");
    if (!mxBodies) mxBodies = mxGetField(prhs[desired_support_argid],0,"bodies");
    if (!mxBodies) mexErrMsgTxt("couldn't get bodies");
    double* pBodies = mxGetPr(mxBodies);

    mxArray* mxContactPts;
    // We may have gotten a RigidBodySupportState object (in which case we need mxGetProperty...
    mxContactPts = mxGetProperty(prhs[desired_support_argid],0,"contact_pts");
    // ...or a struct array, in which case we need mxGetField
    if (!mxContactPts) mxContactPts = mxGetField(prhs[desired_support_argid],0,"contact_pts");
    if (!mxContactPts) mexErrMsgTxt("couldn't get contact points");
    
    for (i=0; i<mxGetNumberOfElements(mxBodies);i++) {
      mxArray* mxBodyContactPts = mxGetCell(mxContactPts,i);
      assert(mxGetM(mxBodyContactPts) == 3);
      int nc = static_cast<int>(mxGetN(mxBodyContactPts));
      if (nc<1) continue;
      
      Map<MatrixXd> all_body_contact_pts(mxGetPr(mxBodyContactPts), mxGetM(mxBodyContactPts), mxGetN(mxBodyContactPts));

      SupportStateElement se;
      se.body_idx = (int) pBodies[i]-1;
      for (j=0; j<nc; j++) {
        se.contact_pts.push_back(all_body_contact_pts.col(j));
      }
      
      if (contact_threshold == -1) { // ignore terrain
        if (contact_sensor(i)!=0) { // no sensor info, or sensor says yes contact
          active_supports.push_back(se);
          num_active_contact_pts += nc;
          contact_bodies.insert((int)se.body_idx); 
        }
      } 
      else {
        contactPhi(pdata->r, cache, se, pdata->map_ptr, phi, terrain_height);
        bool in_contact = true;
        if (contact_logic_AND) { // plan is true, now check contact sensor/kinematics
          in_contact =  (phi.minCoeff()<=contact_threshold || contact_sensor(i)==1); // any contact below threshold (kinematically) OR contact sensor says yes contact
        }
        // else: (plan) OR (sensor), so in_contact = true

        if (in_contact) { 
          active_supports.push_back(se);
          num_active_contact_pts += nc;
          contact_bodies.insert((int)se.body_idx);
        }
      }
    }
  }

  if (nlhs>0) {
    plhs[0] = mxCreateDoubleMatrix(1,static_cast<int>(active_supports.size()),mxREAL);
    pr = mxGetPr(plhs[0]);
    int i=0;
    for (vector<SupportStateElement,Eigen::aligned_allocator<SupportStateElement>>::iterator iter = active_supports.begin(); iter!=active_supports.end(); iter++) {
      pr[i++] = (double) (iter->body_idx + 1);
    }
  }
} 

//.........这里部分代码省略.........
    //cout<<REDRUM("Reading "<<filename<<" failed.")<<endl;
    return false;
  }
  cout<<GREENGIN("Read "<<filename<<" successfully.")<<endl;
  {
    // dirname, basename, extension and filename
    string dirname,b,ext;
    pathinfo(filename,dirname,b,ext,prefix);
    prefix = dirname + "/" + prefix;
    transform(ext.begin(), ext.end(), ext.begin(), ::tolower);
    use_obj_format = ext == "obj";
  }
  if(argc>2)
  {
    //http://stackoverflow.com/a/9748431/148668
    char *p;
    long d = strtol(argv[2], &p, 10);
    if (errno != 0 || *p != '\0')
    {
      cerr<<"detect only param should be 0 or 1"<<endl;
    }else
    {
      params.detect_only = d!=0;
    }
  }
#endif
  MatrixXi IF;
  VectorXi J,IM;
  if(F.rows()>0)
  {
    // Check that there aren't any combinatorially or geometrically degenerate triangles
    VectorXd A;
    doublearea(V,F,A);
    if(A.minCoeff()<=0)
    {
#ifdef MEX
      mexErrMsgTxt("Geometrically degenerate face found.");
#else
      cerr<<"Geometrically degenerate face found."<<endl;
      return 1;
#endif
    }
    VectorXi F12,F23,F31;
    F12 = F.col(0)-F.col(1);
    F23 = F.col(1)-F.col(2);
    F31 = F.col(2)-F.col(0);
    if(
      F12.minCoeff() == 0 || 
      F23.minCoeff() == 0 || 
      F31.minCoeff() == 0)
    {
#ifdef MEX
      mexErrMsgTxt("Combinatorially degenerate face found.");
#else
      cerr<<"Geometrically degenerate face found."<<endl;
      return 1;
#endif
    }

    // Now mesh self intersections
    {
      MatrixXd tempV;
      MatrixXi tempF;
      remesh_self_intersections(V,F,params,tempV,tempF,IF,J,IM);
      //cout<<BLUEGIN("Found and meshed "<<IF.rows()<<" pair"<<(IF.rows()==1?"":"s")
      //  <<" of self-intersecting triangles.")<<endl;

//#define IGL_LINPROG_VERBOSE
IGL_INLINE bool igl::linprog(
  const Eigen::VectorXd & c,
  const Eigen::MatrixXd & _A,
  const Eigen::VectorXd & b,
  const int k,
  Eigen::VectorXd & x)
{
  // This is a very literal translation of
  // http://www.mathworks.com/matlabcentral/fileexchange/2166-introduction-to-linear-algebra/content/strang/linprog.m
  using namespace Eigen;
  using namespace std;
  bool success = true;
  // number of constraints
  const int m = _A.rows();
  // number of original variables
  const int n = _A.cols();
  // number of iterations
  int it = 0;
  // maximum number of iterations
  //const int MAXIT = 10*m;
  const int MAXIT = 100*m;
  // residual tolerance
  const double tol = 1e-10;
  const auto & sign = [](const Eigen::VectorXd & B) -> Eigen::VectorXd
  {
    Eigen::VectorXd Bsign(B.size());
    for(int i = 0;i<B.size();i++)
    {
      Bsign(i) = B(i)>0?1:(B(i)<0?-1:0);
    }
    return Bsign;
  };
  // initial (inverse) basis matrix
  VectorXd Dv = sign(sign(b).array()+0.5);
  Dv.head(k).setConstant(1.);
  MatrixXd D = Dv.asDiagonal();
  // Incorporate slack variables
  MatrixXd A(_A.rows(),_A.cols()+D.cols());
  A<<_A,D;
  // Initial basis
  VectorXi B = igl::colon<int>(n,n+m-1);
  // non-basis, may turn out that vector<> would be better here
  VectorXi N = igl::colon<int>(0,n-1);
  int j;
  double bmin = b.minCoeff(&j);
  int phase;
  VectorXd xb;
  VectorXd s;
  VectorXi J;
  if(k>0 && bmin<0)
  {
    phase = 1;
    xb = VectorXd::Ones(m);
    // super cost
    s.resize(n+m+1);
    s<<VectorXd::Zero(n+k),VectorXd::Ones(m-k+1);
    N.resize(n+1);
    N<<igl::colon<int>(0,n-1),B(j);
    J.resize(B.size()-1);
    // [0 1 2 3 4]
    //      ^
    // [0 1]
    //      [3 4]
    J.head(j) = B.head(j);
    J.tail(B.size()-j-1) = B.tail(B.size()-j-1);
    B(j) = n+m;
    MatrixXd AJ;
    igl::slice(A,J,2,AJ);
    const VectorXd a = b - AJ.rowwise().sum();
    {
      MatrixXd old_A = A;
      A.resize(A.rows(),A.cols()+a.cols());
      A<<old_A,a;
    }
    D.col(j) = -a/a(j);
    D(j,j) = 1./a(j);
  }else if(k==m)
  {
    phase = 2;
    xb = b;
    s.resize(c.size()+m);
    // cost function
    s<<c,VectorXd::Zero(m);
  }else //k = 0 or bmin >=0
  {
    phase = 1;
    xb = b.array().abs();
    s.resize(n+m);
    // super cost
    s<<VectorXd::Zero(n+k),VectorXd::Ones(m-k);
  }
  while(phase<3)
  {
    double df = -1;
    int t = std::numeric_limits<int>::max();
    // Lagrange mutipliers fro Ax=b
    VectorXd yb = D.transpose() * igl::slice(s,B);
    while(true)
    {
//.........这里部分代码省略.........

bool ClassLayouter::computeNormally()
{
    FuzzyDependAttr::Ptr fuzzyAttr = m_parent->getAttr<FuzzyDependAttr>();
    if (!fuzzyAttr)
        return false;

    Graph G;
    GraphAttributes GA(G,
                       GraphAttributes::nodeGraphics | GraphAttributes::edgeGraphics |
                       GraphAttributes::nodeLabel    | GraphAttributes::nodeTemplate |
                       GraphAttributes::edgeDoubleWeight);

    SparseMatrix& veMat = fuzzyAttr->vtxEdgeMatrix();
    VectorXd	  edgeWeight	= fuzzyAttr->edgeWeightVector();
    if (edgeWeight.size() != veMat.cols())
    {
        m_status |= WARNING_USE_DEFAULT_EDGE_WEIGHT;
        edgeWeight.setOnes(veMat.cols());
    }
    const int nNodes = veMat.rows();
    const int nEdges = veMat.cols();
    if (nNodes <= 0 || nNodes != m_childList.size() || nEdges < 1)
    {
        m_status |= WARNING_NO_EDGE;
        return false;
    }

    bool isUseOrthoLayout = nEdges < 50;

    vector<node> nodeArray;
    vector<edge> edgeArray;
    NodeArray<float> nodeSize(G);
    EdgeArray<double> edgeLength(G);
    for (int i = 0; i < nNodes; ++i)
    {
        nodeArray.push_back(G.newNode());
        float r = m_nodeRadius[i];
        GA.width(nodeArray.back()) = r*2;
        GA.height(nodeArray.back()) = r*2;
        nodeSize[nodeArray.back()] = r * 2;
    }

    float maxEdgeWeight = edgeWeight.maxCoeff();
    float minEdgeWeight = edgeWeight.minCoeff();
    for (int ithEdge = 0; ithEdge < nEdges; ++ithEdge)
    {
        int src, dst;
        GraphUtility::getVtxFromEdge(veMat, ithEdge, src, dst);
        edgeArray.push_back(G.newEdge(nodeArray[src], nodeArray[dst]));
        //GA.doubleWeight(edgeArray.back()) = edgeWeight[ithEdge];
        edgeLength[edgeArray.back()] = 1;//log(edgeWeight[ithEdge] + 1);
    }


    // add dummy vertices and edges in order to merge parallel edge segments attached to the same node
    VectorXi compVec;
    int nComp = 1;
    const float dummyNodeRadius = 0.01;
    if(m_isMergeEdgeEnd && isUseOrthoLayout && GraphUtility::findConnectedComponentsVE(	veMat, compVec, nComp ))
    {
        bool* isCompSet = new bool[nComp];
        for (int i = 0; i < nComp; ++i)
            isCompSet[i] = false;

        // add a dummy node and edge for each connect component
        for (int ithVtx = 0; ithVtx < compVec.size(); ++ithVtx)
        {
            int ithCmp = compVec[ithVtx];
            if (isCompSet[ithCmp] == false)
            {
                // add dummy node and set its radius
                nodeArray.push_back(G.newNode());
                GA.width(nodeArray.back()) = dummyNodeRadius;
                GA.height(nodeArray.back()) = dummyNodeRadius;
                // add dummy edge
                edgeArray.push_back(G.newEdge(nodeArray[ithVtx], nodeArray.back()));
                isCompSet[ithCmp] = true;
            }
        }
        delete[] isCompSet;
    }


    MatrixXd pos;
    pos.resize(nNodes, 2);
    try
    {
        if (isUseOrthoLayout)
        {
            PlanarizationLayout layouter;
            OrthoLayout *ol = new OrthoLayout;
            float sep = max(m_nodeRadius.sum() / nNodes, LayoutSetting::s_baseRadius * 12);
            ol->separation(sep);
            ol->cOverhang(0.1);
            ol->setOptions(1+4);
            layouter.setPlanarLayouter(ol);
            layouter.call(GA);
            for (int v = 0; v < nNodes; ++v)
            {
                double x = GA.x(nodeArray[v]);
//.........这里部分代码省略.........

//.........这里部分代码省略.........
    seed = (sal_->getIndxSalient())->at(si);
    if (!isSeedValid())
      continue;
    seeds.push_back(seed);
    ns++;

    // Step 4: fetch and validate neighborhood
    //double last_nn = pcl::getTime ();
    search.radiusSearch(filtered_cloud_->points[seed], radius_, nn_indices, nn_sqr_distances);
    nn_cloud_->points.resize (0);
    for (int i=0; i<nn_indices.size(); i++)
      nn_cloud_->points.push_back(filtered_cloud_->points[nn_indices[i]]);
    //double now_nn = pcl::getTime ();
    //cerr << "Time for nn: " << now_nn-last_nn << endl;
   
    if (do_vis_ && show_nn_ && viewer_)
      AutoSeg::showNN();

    // Step 5: fit the patch
    //double last_pf = pcl::getTime ();
    PatchFit *patchFit;
    patchFit = new PatchFit(nn_cloud_);
    patchFit->setSSmax (50);
    patchFit->fit();
    //double now_pf = pcl::getTime ();
    //cerr << "Time for fitting " << nn_cloud_->points.size() <<
    //        " points: " << now_pf-last_pf << endl;
  

    // Step 6: validate patch
    //double last_val = pcl::getTime ();
    if (do_validation_)
    {
      (*patchFit->p_).computeResidual (nn_cloud_);
      if ((*patchFit->p_).getResidual() > t_residual_)
        continue;

      VectorXd k;
      k = (*patchFit->p_).getK ();
      if (k.minCoeff () < t_min_curv_ || k.maxCoeff ()>t_max_curv_)
        continue;
    }
    num_patches++;
    //double now_val = pcl::getTime ();
    //cerr << "Time for validation: " << now_val-last_val << endl;

    // Visualize patch
    //double last_pp = pcl::getTime ();
    if (do_vis_ && show_patches_ && viewer_)
    {
      PatchPlot *patch_plot;
      (*patchFit->p_).setID (ns);
      (*patchFit->p_).setSS (0.025);
      (*patchFit->p_).infer_params();
      (*patchFit->p_).gs();

      //PatchPlot patch_plot (*patchFit->p_);
      patch_plot = new PatchPlot (*patchFit->p_);
    
      patch_plot->showPatch (viewer_, 0, 1, 0, boost::to_string(ns) + "_patch");
    
      delete (patch_plot);
    }
    //double now_pp = pcl::getTime ();
    //cerr << "Time for ploting patch: " << now_pp-last_pp << endl;

    if (!no_stats_)
    {
      // Save normal vector
      (*patchFit->p_).setCnn (sal_->getNormalNormalAngle (seed));
      if (use_gravity_)
        (*patchFit->p_).setCng (sal_->getNormalGravityAngle (seed));

      // Step 7: save statistics
      AutoSeg::saveStat (*patchFit->p_);
    
      // Step 8: print statistics
      Vector2d sk = (*patchFit->p_).getSK();
      cout << "patch stats" << " "
           << sk (0) << " "
           << sk (1) << " "
           << (*patchFit->p_).getCnn () << " "
           << (*patchFit->p_).getCng () << endl;
    }
    // delete objects
    delete (patchFit);

  } // for each seed

  if(do_vis_ && show_patches_) max_patch_plot_id_ = ns-1;
  else max_patch_plot_id_ = 0;

  // Total timing
  double now = getTime();
  cerr << "Total autoseg for " << num_patches << " patches out of " << ns <<
          " seed points: " << now-last << " sec." << endl;

  // destroy objects
  delete (sal_);
}

double DataGenerator::evaluate(MatrixXd estimate) {
    
    if (estimate.rows()!=mCenters.rows()) {
        cout << endl <<"ERROR:DataGenerator evaluate"<< endl;
        cout << "resultEvaluation dimension mismatch "<< endl;
        return 0;
    }
    
    //MatrixXd finalEstimate(mPara.dimension(),   mPara.cluster());
    unsigned long nEstimate = estimate.cols();
    
    //cout << "finalEstimate row:"<< finalEstimate.rows() << endl;
    //cout << "estimate row:"<< estimate.rows() << endl;
    //cout << "nEstimate:"<< nEstimate <<"      mPara.cluster():"<<  mPara.cluster() << endl;
    
    
    //finalEstimate.leftCols(nEstimate) = estimate;
    
    MatrixXd finalEstimate = estimate;
    
    /*
     if (nEstimate !=   mPara.cluster()) {
     unsigned long sizeDiff =   mPara.cluster() - nEstimate;
     
     if (sizeDiff>0) {
     finalEstimate.rightCols(sizeDiff) = MatrixXd::Zero(mPara.dimension(), sizeDiff);
     cout << "# of decomposed element is less than expected!! expect:" <<   mPara.cluster() << "  only:" << nEstimate << endl;
     nEstimate = nEstimate + sizeDiff;
     } else {
     cout << endl << "ERROR:DataGenerator evaluate" << endl;
     cout <<"Decompsed center is more than expected. We already pick dominant centers!!" << endl;
     
     }
     }
     */
    
    // Assign all the real center to the nearest estimater center
    
    MatrixXd bestMatch(mPara.dimension(),   mPara.cluster());
    
    double error = 0;
    
    for (unsigned long i=0; i<  mPara.cluster(); i++) {
        MatrixXd currentRep = mCenters.col(i).replicate(1,nEstimate);
        MatrixXd diff = finalEstimate - currentRep;
        diff = diff.array().pow(2);
        VectorXd dist = diff.colwise().sum();
        
        MatrixXf::Index minRow, minCol;
        error += pow(dist.minCoeff(&minRow, &minCol),0.5);
        bestMatch.col(i) = finalEstimate.col(minRow);
    }
    
    //cout << endl <<" ===== Best match ===== " << endl;
    //cout << bestMatch << endl;
    //cout << " ===== Original ===== " << endl;
    //cout << mCenters << endl;
    // cout << endl << "Avg RMSE=" << error/  mPara.cluster() << endl;
    
    return (error/  mPara.cluster());
}