コード例 #1
0
 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;
   }
 }
コード例 #2
0
  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;
  }