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

IGL_INLINE void igl::sparse(
  const IndexVector & I,
  const IndexVector & J,
  const ValueVector & V,
  const size_t m,
  const size_t n,
  Eigen::SparseMatrix<T>& X)
{
  using namespace std;
  using namespace Eigen;
  assert((int)I.maxCoeff() < (int)m);
  assert((int)I.minCoeff() >= 0);
  assert((int)J.maxCoeff() < (int)n);
  assert((int)J.minCoeff() >= 0);
  assert(I.size() == J.size());
  assert(J.size() == V.size());
  // Really we just need .size() to be the same, but this is safer
  assert(I.rows() == J.rows());
  assert(J.rows() == V.rows());
  assert(I.cols() == J.cols());
  assert(J.cols() == V.cols());

  vector<Triplet<T> > IJV;
  IJV.reserve(I.size());
  for(int x = 0;x<I.size();x++)
  {
    IJV.push_back(Triplet<T >(I(x),J(x),V(x)));
  }
  X.resize(m,n);
  X.setFromTriplets(IJV.begin(),IJV.end());
}

  bool AlignmentAlgorithmSE2::operator()(AlignmentAlgorithmSE2::TransformType& transform, const CorrespondenceVector& correspondences, const IndexVector& indices){
    if ((int)indices.size()<minimalSetSize())
      return false;

    SE2 x0;
    SE2 ix0;
    double deltaRSum=0.;
    Vector2d mean1(0.,0.);
    Vector2d mean2(0.,0.);
    for (size_t i=0; i<indices.size(); i++){
      const Correspondence& c = correspondences[indices[i]];
      const EdgeSE2* edge = static_cast<const g2o::EdgeSE2*>(c.edge());
      const VertexSE2* v1 = static_cast<const g2o::VertexSE2*>(edge->vertex(0));
      const VertexSE2* v2 = static_cast<const g2o::VertexSE2*>(edge->vertex(1));
      SE2 xi = v2->estimate()*v1->estimate().inverse();
      mean1 += v1->estimate().translation();
      mean2 += v2->estimate().translation();
      if (i==0){
	x0 = xi;
	ix0 = x0.inverse();
      } else {
	SE2 delta=ix0*xi;
	deltaRSum += delta.rotation().angle();
      }
    }
    int count  = indices.size();
    double icount = 1./double(count);
    deltaRSum*=icount;
    mean1*=icount;
    mean2*=icount;
    Rotation2Dd R = x0.rotation()*Rotation2Dd(deltaRSum);
    transform.setRotation(R);
    transform.setTranslation(mean2 - R*mean1);
    transform = transform.inverse();
    return true;
  }

    void _computeReverseMap() {

        _genesToTranscripts.resize( _geneNames.size(), {});

        Index geneID;
        Index transcriptID = 0;
        size_t maxNumTrans = 0;
        Index maxGene;
        for ( size_t transcriptID = 0; transcriptID < _transcriptsToGenes.size(); ++transcriptID ) {
            _genesToTranscripts[ _transcriptsToGenes[transcriptID] ].push_back( transcriptID );
            if ( maxNumTrans < _genesToTranscripts[ _transcriptsToGenes[transcriptID] ].size() ) {
                maxNumTrans = _genesToTranscripts[ _transcriptsToGenes[transcriptID] ].size();
                maxGene = _transcriptsToGenes[transcriptID];
            }
        }
        std::cerr << "max # of transcripts in a gene was " << maxNumTrans << " in gene " << _geneNames[maxGene] << "\n";
    }

  bool IdCorrespondenceValidator::operator()(const CorrespondenceVector& correspondences, const IndexVector& indices, int k){
    if (k>minimalSetSize())
      return true;
    
    assert(indices.size()>=k && "VALIDATION_INDEX_OUT_OF_BOUND");
    assert(correspondences.size()>=indices[k] && "VALIDATION_CORRESPONDENCE_INDEX_OUT_OF_BOUND");
    const g2o::OptimizableGraph::Edge* edgek = correspondences[indices[k]].edge();
    int idk1=edgek->vertex(0)->id();
    int idk2=edgek->vertex(1)->id();
    
    for (int i=0; i<k-1; i++){
      const g2o::OptimizableGraph::Edge* edge = correspondences[indices[i]].edge();
      int id1=edge->vertex(0)->id();
      int id2=edge->vertex(1)->id();
      if (idk1==id1)
	return false;
      if (idk2==id2)
	return false;
    }
    return true;
  }

//.........这里部分代码省略.........
        }
      } // array sequence iter

      // to be utilized later for more detailed output
#if 0
      for(IndexToReadWriteDataMap::iterator imap=indexToReadWriteDataMap.begin();
          imap!=indexToReadWriteDataMap.end();
          ++imap) {
        //        cout<<"DEBUG: INDEX: "<<(*imap).first<<" R-SET: ";
        IndexVector index=(*imap).first;

        cout<<"DEBUG: INDEX: ";
        for(IndexVector::iterator iv=index.begin();iv!=index.end();++iv) {
          if(iv!=index.begin())
            cout<<",";
          cout<<*iv;
        }
        cout<<" R-SET: ";
        for(ArrayElementAccessDataSet::const_iterator i=indexToReadWriteDataMap[index].readArrayAccessSet.begin();i!=indexToReadWriteDataMap[index].readArrayAccessSet.end();++i) {
          cout<<(*i).toString(variableIdMapping)<<" ";
        }
        cout<<endl;
        cout<<"DEBUG: INDEX: ";
        for(IndexVector::iterator iv=index.begin();iv!=index.end();++iv) {
          if(iv!=index.begin())
            cout<<",";
          cout<<*iv;
        }
        cout<<" W-SET: ";
        for(ArrayElementAccessDataSet::const_iterator i=indexToReadWriteDataMap[index].writeArrayAccessSet.begin();i!=indexToReadWriteDataMap[index].writeArrayAccessSet.end();++i) {
          cout<<(*i).toString(variableIdMapping)<<" ";
        }
        cout<<endl;
        cout<<"DEBUG: read-array-access:"<<indexToReadWriteDataMap[index].readArrayAccessSet.size()<<" read-var-access:"<<indexToReadWriteDataMap[index].readVarIdSet.size()<<endl;
        cout<<"DEBUG: write-array-access:"<<indexToReadWriteDataMap[index].writeArrayAccessSet.size()<<" write-var-access:"<<indexToReadWriteDataMap[index].writeVarIdSet.size()<<endl;
      } // imap
#endif

      // perform the check now
      // 1) compute vector if index-vectors for each outer-var-vector
      // 2) check each index-vector. For each iteration of each par-loop iteration then.
      
      //typedef set<int> ParVariableValueSet;
      //ParVariableValueSet parVariableValueSet;
      // MAP: par-variable-val -> vector of IndexVectors with this par-variable-val
      typedef vector<IndexVector> ThreadVector;
      typedef map<IndexVector,ThreadVector > CheckMapType;
      CheckMapType checkMap;
      for(IndexToReadWriteDataMap::iterator imap=indexToReadWriteDataMap.begin();
          imap!=indexToReadWriteDataMap.end();
          ++imap) {
        IndexVector index=(*imap).first;
        IndexVector outVarIndex;
        // if index.size()==0, it will analyze the loop independet of outer loops
        if(index.size()>0) {
          ROSE_ASSERT(index.size()>0);
          for(size_t iv1=0;iv1<index.size()-1;iv1++) {
            outVarIndex.push_back(index[iv1]);
          }
          ROSE_ASSERT(outVarIndex.size()<index.size());
        } else {
          // nothing to check
          continue;
        }
        // last index of index of par-variable
        //int parVariableValue=index[index.size()-1];

bool AlignmentAlgorithmPlaneLinear::operator()(AlignmentAlgorithmPlaneLinear::TransformType& transform, const CorrespondenceVector& correspondences, const IndexVector& indices)
{
    double err=0;
    //If my correspondaces indices are less then a minimum threshold, stop it please
    if ((int)indices.size()<minimalSetSize()) return false;

    //My initial guess for the transformation it's the identity matrix
    //maybe in the future i could have a less rough guess
    transform = Isometry3d::Identity();
    //Unroll the matrix to a vector
    Vector12d x=homogeneous2vector(transform.matrix());
    Matrix9x1d rx=x.block<9,1>(0,0);

    //Initialization of variables, melting fat, i've so much space
    Matrix9d H;
    H.setZero();
    Vector9d b;
    b.setZero();
    Matrix3x9d A;
    //iteration for each correspondace
    for (size_t i=0; i<indices.size(); i++)
    {

        A.setZero();
        const Correspondence& c = correspondences[indices[i]];
        const EdgePlane* edge = static_cast<const EdgePlane*>(c.edge());

        //estraggo i vertici dagli edge
        const VertexPlane* v1 = static_cast<const VertexPlane*>(edge->vertex(0));
        const VertexPlane* v2 = static_cast<const VertexPlane*>(edge->vertex(1));

        //recupero i dati dei piani
        const AlignmentAlgorithmPlaneLinear::PointEstimateType& pi= v1->estimate();
        const AlignmentAlgorithmPlaneLinear::PointEstimateType& pj= v2->estimate();

        //recupeo le normali, mi servono per condizionare la parte rotazionale
        Vector3d ni;
        Vector3d nj;
        Vector4d coeffs_i=pi.toVector();
        Vector4d coeffs_j=pj.toVector();

        ni=coeffs_i.head(3);
        nj=coeffs_j.head(3);

        //inizializzo lo jacobiano, solo la parte rotazionale (per ora)
        A.block<1,3>(0,0)=nj.transpose();
        A.block<1,3>(1,3)=nj.transpose();
        A.block<1,3>(2,6)=nj.transpose();

        if(DEBUG) {
            cerr << "[DEBUG] PI"<<endl;
            cerr << coeffs_i<<endl;
            cerr << "[DEBUG] PJ "<<endl;
            cerr << coeffs_j<<endl;
            cerr << "[ROTATION JACOBIAN][PLANE "<<i<<"]"<<endl;
            cerr << A<<endl;
        }
        //errore
        //inizializzo errore
        Vector3d ek;
        ek.setZero();
        ek=A*x.block<9,1>(0,0)-coeffs_i.head(3);
        H+=A.transpose()*A;

        b+=A.transpose()*ek;

        err+=abs(ek.dot(ek));

        if(DEBUG)
            cerr << "[ROTATIONAL Hessian for plane "<<i<<"]"<<endl<<H<<endl;
        if(DEBUG)
            cerr << "[ROTATIONAL B for plane "<<i<<"]"<<endl<<b<<endl;
    }
    LDLT<Matrix9d> ldlt(H);
    if (ldlt.isNegative()) return false;
    rx=ldlt.solve(-b); // using a LDLT factorizationldlt;

    x.block<3,1>(0,0)+=rx.block<3,1>(0,0);
    x.block<3,1>(3,0)+=rx.block<3,1>(3,0);
    x.block<3,1>(6,0)+=rx.block<3,1>(6,0);
    if(DEBUG) {
        cerr << "Solving the linear system"<<endl;
        cerr << "------------>H"<<endl;
        cerr << H<<endl;
        cerr << "------------>b"<<endl;
        cerr << b<<endl;
        cerr << "------------>rx"<<endl;
        cerr << rx<<endl;
        cerr << "------------>xTEMP"<<endl;
        cerr << vector2homogeneous(x)<<endl<<endl;

        cerr << "łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł"<<endl;
        cerr << "łłłłłłłłłłł Ringraziamo Cthulhu la parte rotazionale è finitałłłłłłłłłłł"<<endl;
        cerr << "łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł"<<endl;
    }
    Matrix4d Xtemp=vector2homogeneous(x);

    //now the problem si solved but i could have (and probably i have!)
    //a not orthogonal rotation matrix, so i've to recondition it

//.........这里部分代码省略.........