bool SparseOptimizer::buildIndexMapping
     (SparseOptimizer::VertexContainer& vlist)
  {
    if (! vlist.size())
    {
      _ivMap.clear();
      return false;
    }

    _ivMap.resize(vlist.size());
    size_t i = 0;
    // Recorre todos los vertices dandoles un indice.
    // Si el vertice es fijo, su indice sera -1
    // Para los vertices no fijos, les da un indice incremental.
    // Primero se les da a los vertices no marginalizables y luego a los que si.
    // Al final _ivMap contendra todos los vertices no fijos con los vertices
    // no marginalizables en las primeras posiciones de _ivMap
    for (int k=0; k<2; k++)
      for (VertexContainer::iterator it=vlist.begin(); it!=vlist.end(); it++)
      {
         OptimizableGraph::Vertex* v = *it;
         if (! v->fixed())
         {
            if (static_cast<int>(v->marginalized()) == k)
            {
               v->setTempIndex(i);
               _ivMap[i]=v;
               i++;
            }
         }else   v->setTempIndex(-1);
      }

    _ivMap.resize(i);
    return true;
  }
Пример #2
0
  bool SparseOptimizer::buildIndexMapping(SparseOptimizer::VertexContainer& vlist){
    if (! vlist.size()){
      _ivMap.clear();
      return false;
    }

    _ivMap.resize(vlist.size());
    size_t i = 0;
    for (int k=0; k<2; k++)
      for (VertexContainer::iterator it=vlist.begin(); it!=vlist.end(); ++it){
      OptimizableGraph::Vertex* v = *it;
      if (! v->fixed()){
        if (static_cast<int>(v->marginalized()) == k){
          v->setHessianIndex(i);
          _ivMap[i]=v;
          i++;
        }
      }
      else {
        v->setHessianIndex(-1);
      }
    }
    _ivMap.resize(i);
    return true;
  }
 void SparseOptimizer::discardTop(SparseOptimizer::VertexContainer& vlist)
 {
   for (VertexContainer::iterator it = vlist.begin(); it != vlist.end(); ++it)
     (*it)->discardTop();
 }
 void SparseOptimizer::push(SparseOptimizer::VertexContainer& vlist)
 {
   for (VertexContainer::iterator it = vlist.begin(); it != vlist.end(); ++it)
     (*it)->push();
 }