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