コード例 #1
0
  bool CholOptimizer<PG>::buildIndexMapping(typename PG::Vertex* rootVertex, Graph::VertexSet& vset){
    _ivMap.resize(vset.size());
    int i=0;
    for (Graph::VertexSet::iterator it=vset.begin(); it!=vset.end(); it++){
      typename PG::Vertex* v=_MY_CAST_<typename PG::Vertex*>(*it);
      if (v!=rootVertex && ! v->fixed()){
	v->tempIndex()=i;
	_ivMap[i]=v;
	i++;
      } 
    }
    _ivMap.resize(i);
    return true;
  }
コード例 #2
0
  int CholOptimizer<PG>::optimizeSubset(typename PG::Vertex* rootVertex, Graph::VertexSet& vset, int iterations, double lambda, bool initFromObservations,
      int otherNode, typename PG::InformationType* otherCovariance)
  {
    if (vset.size() <= 1) {
      return 0;
    }
    if (!buildIndexMapping(rootVertex,vset)){
      return 0;
    }
    
    // clean up from last call
    if (_symbolicCholesky) {
      cs_sfree(_symbolicCholesky);
      _symbolicCholesky = 0;
    }

    computeActiveEdges(rootVertex,vset);

    if (initFromObservations){
      initializeActiveSubsetWithObservations(rootVertex);
      if (verbose()){
        cerr << "iteration= " << -1 
          << "\t chi2= " << this->chi2() 
          << "\t time= " << 0.0
          << "\t cumTime= " << 0.0
          << endl;
      }
    }

    int dim = PG::TransformationVectorType::TemplateSize;
    int cjIterations=0;
    double cumTime=0;
    for (int i=0; i<iterations; i++){
      struct timeval ts, te;
      gettimeofday(&ts,0);
      buildLinearSystem(rootVertex,lambda);

      if (i == 0) {
        // we have to sort the matrix structure only within the first iteration, it stays the same for the following iterations
        SparseMatrixEntry* entry = _sparseMatrix;
        for (int j = 0; j < _sparseNz; ++j) { // store the pointer to the array
          _sparseMatrixPtr[j] = entry;
          ++entry;
        }
        std::sort(_sparseMatrixPtr, _sparseMatrixPtr + _sparseNz, SparseMatrixEntryPtrCmp());
      }

      if (otherNode==-1 || i!=iterations-1){
	solveAndUpdate();
      } else {
	double** pblock = new double*[dim];
        for (int k = 0; k < dim; ++k)
          pblock[k] = (*otherCovariance)[k];
	typename PG::Vertex* otherVertex=_MY_CAST_<typename PG::Vertex*>(vertex(otherNode));
	int j=otherVertex->tempIndex()*dim;
	solveAndUpdate(pblock, j,j,j+dim,j+dim);
        static TransformCovariance<PG> tCov;
        tCov(*otherCovariance, rootVertex->transformation, otherVertex->transformation);
	assert(otherCovariance->det()>0.);
      }
      gettimeofday(&te,0);
      double dts=(te.tv_sec-ts.tv_sec)+1e-6*(te.tv_usec-ts.tv_usec);
      cumTime+=dts;
      if (verbose()){
        cerr << "iteration= " << i 
          << "\t chi2= " << this->chi2() 
          << "\t time= " << dts 
          << "\t cumTime= " << cumTime
          << endl;
      }
      if (this->visualizeToStdout())
	this->visualizeToStream(cout);
      ++cjIterations;

    }
    clearIndexMapping();

    return cjIterations;
  }