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

  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;
    }
  }

  bool SparseOptimizer::gaugeFreedom()
  {
    if (vertices().empty())
      return false;

    int maxDim=0;
    for (HyperGraph::VertexIDMap::iterator
         it  = vertices().begin();
         it != vertices().end();
         it++)
    {
      OptimizableGraph::Vertex* v =
         static_cast<OptimizableGraph::Vertex*>(it->second);
      maxDim = std::max(maxDim,v->dimension());
    }

    for (HyperGraph::VertexIDMap::iterator
         it  = vertices().begin();
         it != vertices().end();
         it++)
    {
      OptimizableGraph::Vertex* v =
         static_cast<OptimizableGraph::Vertex*>(it->second);
      if (v->dimension() == maxDim)
      {
        // test for full dimension prior
        for (HyperGraph::EdgeSet::const_iterator
             eit  = v->edges().begin();
             eit != v->edges().end();
             ++eit)
        {
          OptimizableGraph::Edge* e =
             static_cast<OptimizableGraph::Edge*>(*eit);
          if (e->vertices().size() == 1 && e->dimension() == maxDim)
            return false;
        }
      }
    }
    return true;
  }

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

  // temporary structures for building the pattern of the Schur complement
  SparseBlockMatrixHashMap<PoseMatrixType>* schurMatrixLookup = 0;
  if (_doSchur) {
    schurMatrixLookup = new SparseBlockMatrixHashMap<PoseMatrixType>(_Hschur->rowBlockIndices(), _Hschur->colBlockIndices());
    schurMatrixLookup->blockCols().resize(_Hschur->blockCols().size());
  }

  // here we assume that the landmark indices start after the pose ones
  // create the structure in Hpp, Hll and in Hpl
  for (SparseOptimizer::EdgeContainer::const_iterator it=_optimizer->activeEdges().begin(); it!=_optimizer->activeEdges().end(); ++it){
    OptimizableGraph::Edge* e = *it;

    for (size_t viIdx = 0; viIdx < e->vertices().size(); ++viIdx) {
      OptimizableGraph::Vertex* v1 = (OptimizableGraph::Vertex*) e->vertex(viIdx);
      int ind1 = v1->hessianIndex();
      if (ind1 == -1)
        continue;
      int indexV1Bak = ind1;
      for (size_t vjIdx = viIdx + 1; vjIdx < e->vertices().size(); ++vjIdx) {
        OptimizableGraph::Vertex* v2 = (OptimizableGraph::Vertex*) e->vertex(vjIdx);
        int ind2 = v2->hessianIndex();
        if (ind2 == -1)
          continue;
        ind1 = indexV1Bak;
        bool transposedBlock = ind1 > ind2;
        if (transposedBlock){ // make sure, we allocate the upper triangle block
          std::swap(ind1, ind2);
        }
        if (! v1->marginalized() && !v2->marginalized()){
          PoseMatrixType* m = _Hpp->block(ind1, ind2, true);
          if (zeroBlocks)
            m->setZero();
          e->mapHessianMemory(m->data(), viIdx, vjIdx, transposedBlock);
          if (_Hschur) {// assume this is only needed in case we solve with the schur complement
            schurMatrixLookup->addBlock(ind1, ind2);
          }
        } else if (v1->marginalized() && v2->marginalized()){
          // RAINER hmm.... should we ever reach this here????
          LandmarkMatrixType* m = _Hll->block(ind1-_numPoses, ind2-_numPoses, true);
          if (zeroBlocks)
            m->setZero();
          e->mapHessianMemory(m->data(), viIdx, vjIdx, false);
        } else { 
          if (v1->marginalized()){ 
            PoseLandmarkMatrixType* m = _Hpl->block(v2->hessianIndex(),v1->hessianIndex()-_numPoses, true);
            if (zeroBlocks)
              m->setZero();
            e->mapHessianMemory(m->data(), viIdx, vjIdx, true); // transpose the block before writing to it
          } else {
            PoseLandmarkMatrixType* m = _Hpl->block(v1->hessianIndex(),v2->hessianIndex()-_numPoses, true);
            if (zeroBlocks)
              m->setZero();
            e->mapHessianMemory(m->data(), viIdx, vjIdx, false); // directly the block
          }
        }
      }
    }
  }

  if (! _doSchur)
    return true;

  _DInvSchur->diagonal().resize(landmarkIdx);
  _Hpl->fillSparseBlockMatrixCCS(*_HplCCS);

  for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
    OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];
    if (v->marginalized()){
      const HyperGraph::EdgeSet& vedges=v->edges();
      for (HyperGraph::EdgeSet::const_iterator it1=vedges.begin(); it1!=vedges.end(); ++it1){
        for (size_t i=0; i<(*it1)->vertices().size(); ++i)
        {
          OptimizableGraph::Vertex* v1= (OptimizableGraph::Vertex*) (*it1)->vertex(i);
          if (v1->hessianIndex()==-1 || v1==v)
            continue;
          for  (HyperGraph::EdgeSet::const_iterator it2=vedges.begin(); it2!=vedges.end(); ++it2){
            for (size_t j=0; j<(*it2)->vertices().size(); ++j)
            {
              OptimizableGraph::Vertex* v2= (OptimizableGraph::Vertex*) (*it2)->vertex(j);
              if (v2->hessianIndex()==-1 || v2==v)
                continue;
              int i1=v1->hessianIndex();
              int i2=v2->hessianIndex();
              if (i1<=i2) {
                schurMatrixLookup->addBlock(i1, i2);
              }
            }
          }
        }
      }
    }
  }

  _Hschur->takePatternFromHash(*schurMatrixLookup);
  delete schurMatrixLookup;
  _Hschur->fillSparseBlockMatrixCCSTransposed(*_HschurTransposedCCS);

  return true;
}

  void EstimatePropagator::propagate(OptimizableGraph::VertexSet& vset, 
      const EstimatePropagator::PropagateCost& cost, 
       const EstimatePropagator::PropagateAction& action,
       double maxDistance, 
       double maxEdgeCost)
  {
    reset();

    PriorityQueue frontier;
    for (OptimizableGraph::VertexSet::iterator vit=vset.begin(); vit!=vset.end(); ++vit){
      OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*vit);
      AdjacencyMap::iterator it = _adjacencyMap.find(v);
      assert(it != _adjacencyMap.end());
      it->second._distance = 0.;
      it->second._parent.clear();
      it->second._frontierLevel = 0;
      frontier.push(&it->second);
    }

    while(! frontier.empty()){
      AdjacencyMapEntry* entry = frontier.pop();
      OptimizableGraph::Vertex* u = entry->child();
      double uDistance = entry->distance();
      //cerr << "uDistance " << uDistance << endl;

      // initialize the vertex
      if (entry->_frontierLevel > 0) {
        action(entry->edge(), entry->parent(), u);
      }

      /* std::pair< OptimizableGraph::VertexSet::iterator, bool> insertResult = */ _visited.insert(u);
      OptimizableGraph::EdgeSet::iterator et = u->edges().begin();
      while (et != u->edges().end()){
        OptimizableGraph::Edge* edge = static_cast<OptimizableGraph::Edge*>(*et);
        ++et;

        int maxFrontier = -1;
        OptimizableGraph::VertexSet initializedVertices;
        for (size_t i = 0; i < edge->vertices().size(); ++i) {
          OptimizableGraph::Vertex* z = static_cast<OptimizableGraph::Vertex*>(edge->vertex(i));
          AdjacencyMap::iterator ot = _adjacencyMap.find(z);
          if (ot->second._distance != numeric_limits<double>::max()) {
            initializedVertices.insert(z);
            maxFrontier = (max)(maxFrontier, ot->second._frontierLevel);
          }
        }
        assert(maxFrontier >= 0);

        for (size_t i = 0; i < edge->vertices().size(); ++i) {
          OptimizableGraph::Vertex* z = static_cast<OptimizableGraph::Vertex*>(edge->vertex(i));
          if (z == u)
            continue;

          size_t wasInitialized = initializedVertices.erase(z);

          double edgeDistance = cost(edge, initializedVertices, z);
          if (edgeDistance > 0. && edgeDistance != std::numeric_limits<double>::max() && edgeDistance < maxEdgeCost) {
            double zDistance = uDistance + edgeDistance;
            //cerr << z->id() << " " << zDistance << endl;

            AdjacencyMap::iterator ot = _adjacencyMap.find(z);
            assert(ot!=_adjacencyMap.end());

            if (zDistance < ot->second.distance() && zDistance < maxDistance){
              //if (ot->second.inQueue)
                //cerr << "Updating" << endl;
              ot->second._distance = zDistance;
              ot->second._parent = initializedVertices;
              ot->second._edge = edge;
              ot->second._frontierLevel = maxFrontier + 1;
              frontier.push(&ot->second);
            }
          }

          if (wasInitialized > 0)
            initializedVertices.insert(z);

        }
      }
    }

    // writing debug information like cost for reaching each vertex and the parent used to initialize
#ifdef DEBUG_ESTIMATE_PROPAGATOR
    cerr << "Writing cost.dat" << endl;
    ofstream costStream("cost.dat");
    for (AdjacencyMap::const_iterator it = _adjacencyMap.begin(); it != _adjacencyMap.end(); ++it) {
      HyperGraph::Vertex* u = it->second.child();
      costStream << "vertex " << u->id() << "  cost " << it->second._distance << endl;
    }
    cerr << "Writing init.dat" << endl;
    ofstream initStream("init.dat");
    vector<AdjacencyMapEntry*> frontierLevels;
    for (AdjacencyMap::iterator it = _adjacencyMap.begin(); it != _adjacencyMap.end(); ++it) {
      if (it->second._frontierLevel > 0)
        frontierLevels.push_back(&it->second);
    }
    sort(frontierLevels.begin(), frontierLevels.end(), FrontierLevelCmp());
    for (vector<AdjacencyMapEntry*>::const_iterator it = frontierLevels.begin(); it != frontierLevels.end(); ++it) {
      AdjacencyMapEntry* entry       = *it;
      OptimizableGraph::Vertex* to   = entry->child();
//.........这里部分代码省略.........