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