void CholOptimizer<PG>::transformSubset(typename PG::Vertex* rootVertex, Graph::VertexSet& vset, const typename PG::TransformationType& newRootPose){
typename PG::TransformationType t=newRootPose*rootVertex->transformation.inverse();
for (Graph::VertexSet::iterator it=vset.begin(); it!=vset.end(); it++){
typename PG::Vertex* v=_MY_CAST_<typename PG::Vertex*>(*it);
v->transformation=t*v->transformation;
}
}
bool CholOptimizer<PG>::buildIndexMapping(typename PG::Vertex* rootVertex, Graph::VertexSet& vset){
_ivMap.resize(vset.size());
int i=0;
for (Graph::VertexSet::iterator it=vset.begin(); it!=vset.end(); it++){
typename PG::Vertex* v=_MY_CAST_<typename PG::Vertex*>(*it);
if (v!=rootVertex && ! v->fixed()){
v->tempIndex()=i;
_ivMap[i]=v;
i++;
}
}
_ivMap.resize(i);
return true;
}