C++ Vertex::push方法代码示例

void OptimizableGraph::push(HyperGraph::VertexSet& vset)
{
  for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) {
    OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
    v->push();
  }
}

  void SparseOptimizer::push(HyperGraph::VertexSet& vlist)
  {
    for (HyperGraph::VertexSet::iterator it = vlist.begin(); it != vlist.end(); ++it) {
      OptimizableGraph::Vertex* v = dynamic_cast<OptimizableGraph::Vertex*>(*it);
      if (v)
	v->push();
      else 
	cerr << __FUNCTION__ << ": FATAL PUSH SET" << endl;
    }
  }

  void SparseOptimizer::computeInitialGuess(EstimatePropagatorCost& costFunction)
  {
    OptimizableGraph::VertexSet emptySet;
    std::set<Vertex*> backupVertices;
    HyperGraph::VertexSet fixedVertices; // these are the root nodes where to start the initialization
    for (EdgeContainer::iterator it = _activeEdges.begin(); it != _activeEdges.end(); ++it) {
      OptimizableGraph::Edge* e = *it;
      for (size_t i = 0; i < e->vertices().size(); ++i) {
        OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(e->vertex(i));
	if (!v)
	  continue;
        if (v->fixed())
          fixedVertices.insert(v);
        else { // check for having a prior which is able to fully initialize a vertex
          for (EdgeSet::const_iterator vedgeIt = v->edges().begin(); vedgeIt != v->edges().end(); ++vedgeIt) {
            OptimizableGraph::Edge* vedge = static_cast<OptimizableGraph::Edge*>(*vedgeIt);
            if (vedge->vertices().size() == 1 && vedge->initialEstimatePossible(emptySet, v) > 0.) {
              //cerr << "Initialize with prior for " << v->id() << endl;
              vedge->initialEstimate(emptySet, v);
              fixedVertices.insert(v);
            }
          }
        }
        if (v->hessianIndex() == -1) {
          std::set<Vertex*>::const_iterator foundIt = backupVertices.find(v);
          if (foundIt == backupVertices.end()) {
            v->push();
            backupVertices.insert(v);
          }
        }
      }
    }

    EstimatePropagator estimatePropagator(this);
    estimatePropagator.propagate(fixedVertices, costFunction);

    // restoring the vertices that should not be initialized
    for (std::set<Vertex*>::iterator it = backupVertices.begin(); it != backupVertices.end(); ++it) {
      Vertex* v = *it;
      v->pop();
    }
    if (verbose()) {
      computeActiveErrors();
      cerr << "iteration= -1\t chi2= " << activeChi2()
          << "\t time= 0.0"
          << "\t cumTime= 0.0"
          << "\t (using initial guess from " << costFunction.name() << ")" << endl;
    }
  }

void BaseMultiEdge<D, E>::linearizeOplus()
{
#ifdef G2O_OPENMP
  for (size_t i = 0; i < _vertices.size(); ++i) {
    OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(_vertices[i]);
    v->lockQuadraticForm();
  }
#endif

  const double delta = 1e-9;
  const double scalar = 1.0 / (2*delta);
  ErrorVector errorBak;
  ErrorVector errorBeforeNumeric = _error;

  for (size_t i = 0; i < _vertices.size(); ++i) {
    //Xi - estimate the jacobian numerically
    OptimizableGraph::Vertex* vi = static_cast<OptimizableGraph::Vertex*>(_vertices[i]);

    if (vi->fixed())
      continue;

    const int vi_dim = vi->dimension();
#ifdef _MSC_VER
    double* add_vi = new double[vi_dim];
#else
    double add_vi[vi_dim];
#endif
    std::fill(add_vi, add_vi + vi_dim, 0.0);
    if (_jacobianOplus[i].rows() != _dimension || _jacobianOplus[i].cols() != vi_dim)
      _jacobianOplus[i].resize(_dimension, vi_dim);
    // add small step along the unit vector in each dimension
    for (int d = 0; d < vi_dim; ++d) {
      vi->push();
      add_vi[d] = delta;
      vi->oplus(add_vi);
      computeError();
      errorBak = _error;
      vi->pop();
      vi->push();
      add_vi[d] = -delta;
      vi->oplus(add_vi);
      computeError();
      errorBak -= _error;
      vi->pop();
      add_vi[d] = 0.0;

      _jacobianOplus[i].col(d) = scalar * errorBak;
    } // end dimension
#ifdef _MSC_VER
    delete[] add_vi;
#endif
  }
  _error = errorBeforeNumeric;

#ifdef G2O_OPENMP
  for (int i = (int)(_vertices.size()) - 1; i >= 0; --i) {
    OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(_vertices[i]);
    v->unlockQuadraticForm();
  }
#endif

}

  bool Star::labelStarEdges(int iterations, EdgeLabeler* labeler){
    // mark all vertices in the lowLevelEdges as floating
    bool ok=true;
    std::set<OptimizableGraph::Vertex*> vset;
    for (HyperGraph::EdgeSet::iterator it=_lowLevelEdges.begin(); it!=_lowLevelEdges.end(); it++){
      HyperGraph::Edge* e=*it;
      for (size_t i=0; i<e->vertices().size(); i++){
        OptimizableGraph::Vertex* v=(OptimizableGraph::Vertex*)e->vertices()[i];
        v->setFixed(false);
        vset.insert(v);
      }
    }
    for (std::set<OptimizableGraph::Vertex*>::iterator it=vset.begin(); it!=vset.end(); it++){
      OptimizableGraph::Vertex* v = *it;
      v->push();
    }

    // fix all vertices in the gauge
    //cerr << "fixing gauge: ";
    for (HyperGraph::VertexSet::iterator it = _gauge.begin(); it!=_gauge.end(); it++){
      OptimizableGraph::Vertex* v=(OptimizableGraph::Vertex*)*it;
      //cerr << v->id() << " ";
      v->setFixed(true);
    }
    //cerr << endl;
    if (iterations>0){
      _optimizer->initializeOptimization(_lowLevelEdges);
      _optimizer->computeInitialGuess();
      int result=_optimizer->optimize(iterations);
      if (result<1){
        cerr << "Vertices num: " << _optimizer->activeVertices().size() << "ids: ";
        for (size_t i=0; i<_optimizer->indexMapping().size(); i++){
          cerr << _optimizer->indexMapping()[i]->id() << " " ;
        }
        cerr << endl;
        cerr << "!!! optimization failure" << endl;
        cerr << "star size=" << _lowLevelEdges.size() << endl;
        cerr << "gauge: ";
        for (HyperGraph::VertexSet::iterator it=_gauge.begin(); it!=_gauge.end(); it++){
          OptimizableGraph::Vertex* v = (OptimizableGraph::Vertex*)*it;
          cerr << "[" << v->id() << " " << v->hessianIndex() << "] ";
        }
        cerr << endl;
        ok=false;
      }
    }  else {
      optimizer()->initializeOptimization(_lowLevelEdges);
      // cerr << "guess" << endl;
      //optimizer()->computeInitialGuess();
      // cerr << "solver init" << endl;
      optimizer()->solver()->init();
      // cerr << "structure" << endl;
      OptimizationAlgorithmWithHessian* solverWithHessian = dynamic_cast<OptimizationAlgorithmWithHessian*> (optimizer()->solver());
      if (!solverWithHessian->buildLinearStructure())
        cerr << "FATAL: failure while building linear structure" << endl;
      // cerr << "errors" << endl;
      optimizer()->computeActiveErrors();
      // cerr << "system" << endl;
      solverWithHessian->updateLinearSystem();
    }

    std::set<OptimizableGraph::Edge*> star;
    for(HyperGraph::EdgeSet::iterator it=_starEdges.begin(); it!=_starEdges.end(); it++){
      star.insert((OptimizableGraph::Edge*)*it);
    }
    if (ok) {
      int result = labeler->labelEdges(star);
      if (result < 0)
        ok=false;
    }
    // release all vertices in the gauge
    for (std::set<OptimizableGraph::Vertex*>::iterator it=vset.begin(); it!=vset.end(); it++){
      OptimizableGraph::Vertex* v = *it;
      v->pop();
    }
    for (HyperGraph::VertexSet::iterator it=_gauge.begin(); it!=_gauge.end(); it++){
      OptimizableGraph::Vertex* v=(OptimizableGraph::Vertex*)*it;
      v->setFixed(false);
    }

    return ok;
  }