graph_optimizer_chol.hpp
Go to the documentation of this file.
00001 // HOG-Man - Hierarchical Optimization for Pose Graphs on Manifolds
00002 // Copyright (C) 2010 G. Grisetti, R. K├╝mmerle, C. Stachniss
00003 // 
00004 // HOG-Man is free software: you can redistribute it and/or modify
00005 // it under the terms of the GNU Lesser General Public License as published
00006 // by the Free Software Foundation, either version 3 of the License, or
00007 // (at your option) any later version.
00008 // 
00009 // HOG-Man is distributed in the hope that it will be useful,
00010 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00011 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012 // GNU Lesser General Public License for more details.
00013 // 
00014 // You should have received a copy of the GNU Lesser General Public License
00015 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
00016 
00017 #include <iostream>
00018 #include <iomanip>
00019 #include <algorithm>
00020 #include <iterator>
00021 #include <sys/time.h>
00022 #include <hogman_minimal/stuff/os_specific.h>
00023 #include <hogman_minimal/stuff/color_macros.h>
00024 #include <assert.h>
00025 #include "csparse_helper.h"
00026 
00027 namespace AISNavigation{
00028   using namespace std;
00029 
00030   template <typename PG>
00031   bool CholOptimizer<PG>::initialize(int rootNode){
00032     if (this->verbose())
00033       cerr << "# init " << rootNode << endl;
00034     if (this->vertex(rootNode)){
00035       _rootNode=rootNode;
00036       return true;
00037     }
00038     _rootNode=-1;
00039     _ivMap.clear();
00040     _activeEdges.clear();
00041     return false;
00042   }
00043   
00044   template <typename PG>
00045   int CholOptimizer<PG>::optimize(int iterations, bool online){
00046     Graph::VertexSet vset;
00047     for (Graph::VertexIDMap::const_iterator it=this->vertices().begin(); it!=this->vertices().end(); it++){
00048       vset.insert(it->second);
00049     }
00050     
00051     typename PG::Vertex* root=dynamic_cast<typename PG::Vertex*>(this->vertex(_rootNode));
00052     if (! root)
00053       root=_MY_CAST_<typename PG::Vertex*>(this->vertices().begin()->second);
00054     if (this->verbose())
00055       cerr << "# root id " << root->id() << endl;
00056     bool initFromObservations = _guessOnEdges;
00057     optimizeSubset(root, vset, iterations, 0., initFromObservations);
00058     return iterations;
00059   }
00060 
00061   template <typename PG>
00062   CholOptimizer<PG>::~CholOptimizer<PG>(){
00063     if (_sparseB)
00064       delete [] _sparseB;
00065     if (_sparseMatrix)
00066       delete [] _sparseMatrix;
00067     delete[]_sparseMatrixPtr; _sparseMatrixPtr = 0;
00068     cs_sfree(_symbolicCholesky); _symbolicCholesky = 0;
00069     delete[] _csWorkspace; _csWorkspace = 0;
00070     delete[] _csInvWorkB; _csInvWorkB = 0;
00071     delete[] _csInvWorkTemp; _csInvWorkTemp = 0;
00072     delete[] _csIntWorkspace; _csIntWorkspace = 0;
00073   }
00074 
00075   template <typename PG>
00076   int CholOptimizer<PG>::optimizeSubset(typename PG::Vertex* rootVertex, Graph::VertexSet& vset, int iterations, double lambda, bool initFromObservations,
00077       int otherNode, typename PG::InformationType* otherCovariance)
00078   {
00079     if (vset.size() <= 1) {
00080       return 0;
00081     }
00082     if (!buildIndexMapping(rootVertex,vset)){
00083       return 0;
00084     }
00085     
00086     // clean up from last call
00087     if (_symbolicCholesky) {
00088       cs_sfree(_symbolicCholesky);
00089       _symbolicCholesky = 0;
00090     }
00091 
00092     computeActiveEdges(rootVertex,vset);
00093 
00094     if (initFromObservations){
00095       initializeActiveSubsetWithObservations(rootVertex);
00096       if (this->verbose()){
00097         cerr << "iteration= " << -1 
00098           << "\t chi2= " << this->chi2() 
00099           << "\t time= " << 0.0
00100           << "\t cumTime= " << 0.0
00101           << endl;
00102       }
00103     }
00104 
00105     int dim = PG::TransformationVectorType::TemplateSize;
00106     int cjIterations=0;
00107     double cumTime=0;
00108     for (int i=0; i<iterations; i++){
00109       struct timeval ts, te;
00110       gettimeofday(&ts,0);
00111       buildLinearSystem(rootVertex,lambda);
00112 
00113       if (i == 0) {
00114         // we have to sort the matrix structure only within the first iteration, it stays the same for the following iterations
00115         SparseMatrixEntry* entry = _sparseMatrix;
00116         for (int j = 0; j < _sparseNz; ++j) { // store the pointer to the array
00117           _sparseMatrixPtr[j] = entry;
00118           ++entry;
00119         }
00120         std::sort(_sparseMatrixPtr, _sparseMatrixPtr + _sparseNz, SparseMatrixEntryPtrCmp());
00121       }
00122 
00123       if (otherNode==-1 || i!=iterations-1){
00124         solveAndUpdate();
00125       } else {
00126         double** pblock = new double*[dim];
00127         for (int k = 0; k < dim; ++k)
00128           pblock[k] = (*otherCovariance)[k];
00129         typename PG::Vertex* otherVertex=_MY_CAST_<typename PG::Vertex*>(this->vertex(otherNode));
00130         int j=otherVertex->tempIndex()*dim;
00131         solveAndUpdate(pblock, j,j,j+dim,j+dim);
00132         static TransformCovariance<PG> tCov;
00133         tCov(*otherCovariance, rootVertex->transformation, otherVertex->transformation);
00134         assert(otherCovariance->det()>0.);
00135       }
00136       gettimeofday(&te,0);
00137       double dts=(te.tv_sec-ts.tv_sec)+1e-6*(te.tv_usec-ts.tv_usec);
00138       cumTime+=dts;
00139       if (this->verbose()){
00140         cerr << "iteration= " << i 
00141           << "\t chi2= " << this->chi2() 
00142           << "\t time= " << dts 
00143           << "\t cumTime= " << cumTime
00144           << endl;
00145       }
00146       if (this->visualizeToStdout())
00147         this->visualizeToStream(cout);
00148       ++cjIterations;
00149 
00150     }
00151     clearIndexMapping();
00152 
00153     return cjIterations;
00154   }
00155 
00156 
00157   template <typename PG>
00158   CholOptimizer<PG>::CholOptimizer(){
00159     _sparseB=0;
00160     _sparseDim=0;
00161     _sparseNz=0;
00162     _nBlocks=0;
00163     _sparseMatrix=0;
00164     _sparseMatrixPtr = 0;
00165     _sparseDimMax=0;
00166     _sparseNzMax=0;
00167     _symbolicCholesky = 0;
00168     _csWorkspace = 0;
00169     _csIntWorkspace = 0;
00170     _csWorkspaceSize = -1;
00171     _csInvWorkspaceSize = -1;
00172     _csInvWorkB = 0;
00173     _csInvWorkTemp = 0;
00174     _useRelativeError=true;
00175     _rootNode=-1;
00176     _addDuplicateEdgeIterations = 3;
00177   }
00178 
00179   template <typename PG>
00180   bool CholOptimizer<PG>::buildIndexMapping(typename PG::Vertex* rootVertex, Graph::VertexSet& vset){
00181     _ivMap.resize(vset.size());
00182     int i=0;
00183     for (Graph::VertexSet::iterator it=vset.begin(); it!=vset.end(); it++){
00184       typename PG::Vertex* v=_MY_CAST_<typename PG::Vertex*>(*it);
00185       if (v!=rootVertex && ! v->fixed()){
00186         v->tempIndex()=i;
00187         _ivMap[i]=v;
00188         i++;
00189       } 
00190     }
00191     _ivMap.resize(i);
00192     return true;
00193   }
00194 
00195   template <typename PG>
00196   void CholOptimizer<PG>::clearIndexMapping(){
00197     for (int i=0; i<(int)_ivMap.size(); i++){
00198       _ivMap[i]->tempIndex()=-1;
00199       _ivMap[i]=0;
00200     }
00201   }
00202 
00203   template <typename PG>
00204   void CholOptimizer<PG>::computeActiveEdges(typename PG::Vertex* rootVertex, Graph::VertexSet& vset){
00205     _activeEdges.clear();
00206     for (int i=0; i<(int)_ivMap.size(); i++){
00207       typename PG::Vertex* v=_ivMap[i];
00208       const typename PG::EdgeSet& vEdges=v->edges();
00209       for (typename PG::EdgeSet::const_iterator it=vEdges.begin(); it!=vEdges.end(); it++){
00210         _activeEdges.insert(reinterpret_cast<typename PG::Edge*>(*it));
00211       }
00212     }
00213     const typename PG::EdgeSet& vEdges=rootVertex->edges();
00214     for (typename PG::EdgeSet::const_iterator it=vEdges.begin(); it!=vEdges.end(); it++){
00215       _activeEdges.insert(reinterpret_cast<typename PG::Edge*>(*it));
00216     }
00217   }
00218 
00219 
00220   template <typename PG>
00221   int CholOptimizer<PG>::linearizeConstraint(const typename PG::Edge* e, double lambda){
00222       typename PG::TransformationVectorType f;
00223       typename PG::InformationType A, B;
00224       if (_useRelativeError){
00225         static ManifoldGradient<PG> gradient;
00226         gradient(f,A,B,*e);
00227       } else {
00228         static Gradient<PG> gradient;
00229         gradient(f,A,B,*e);
00230       }
00231       typename PG::InformationType omega=e->information();
00232       typename PG::TransformationVectorType r=f*(-1.);
00233 
00234       const typename PG::Vertex* from=_MY_CAST_<const typename PG::Vertex*>(e->from());
00235       const typename PG::Vertex* to=_MY_CAST_<const typename PG::Vertex*>(e->to());
00236 
00237       int i=from->tempIndex();
00238       if(i==-1){
00239         A = PG::InformationType::eye(1.);
00240       }
00241 
00242       int j=to->tempIndex();
00243       if(j==-1){
00244         B = PG::InformationType::eye(1.);
00245       }
00246 
00247       if (from->fixed() || to->fixed())
00248         lambda=1.;
00249       if (i==-1 || j==-1)
00250         omega=omega*lambda;
00251       if (i!=-1){
00252         typename PG::TransformationVectorType bi=A.transpose()*(omega*r);
00253         typename PG::InformationType Aii = A.transpose()*omega*A;
00254         from->b()+=bi;
00255         from->A()+=Aii;
00256       }
00257       if (j!=-1){
00258         typename PG::TransformationVectorType bj=B.transpose()*(omega*r);      
00259         typename PG::InformationType Ajj = B.transpose()*omega*B;
00260         to->b()+=bj;
00261         to->A()+=Ajj;
00262       }
00263       if (i!=-1 && j!=-1){
00264         typename PG::InformationType Aij = A.transpose()*omega*B;
00265         e->AFromTo()+=Aij;
00266         return 2;
00267       }
00268       return 0;
00269   }
00270 
00271   template <typename PG>
00272   typename PG::Edge* CholOptimizer<PG>::addEdge(typename PG::Vertex* from, typename PG::Vertex* to, const typename PG::TransformationType& mean, const typename PG::InformationType& information){
00273     Graph::EdgeSet eset1=this->connectingEdges(from, to);
00274     Graph::EdgeSet eset2=this->connectingEdges(to, from);
00275     Graph::EdgeSet eset;
00276     std::set_union(eset1.begin(),
00277                    eset1.end(),
00278                    eset2.begin(),
00279                    eset2.end(), 
00280                    std::insert_iterator<Graph::EdgeSet>(eset, eset.end()));
00281 
00282     if (eset.empty()){
00283       typename PG::Edge* e = PG::addEdge(from, to, mean, information);
00284       if (_guessOnEdges && to->edges().size()==1 && ! to->fixed()){
00285         to->transformation=from->transformation*mean;
00286       }
00287       return e;
00288     }
00289     assert(eset.size()==1);
00290 
00291     // least square estimate of the new and the old edge to get one edge between the two nodes
00292     typename PG::Edge*   origEdge = dynamic_cast<typename PG::Edge*>(*eset.begin());
00293     typename PG::Vertex* origTo   = dynamic_cast<typename PG::Vertex*>(origEdge->to());
00294     typename PG::Vertex* origFrom = dynamic_cast<typename PG::Vertex*>(origEdge->from());
00295     CholEdge auxEdge(from, to, mean, information);
00296     // backup the two vertices and reset the transformation to the old mean
00297     origFrom->backup();
00298     origTo->backup();
00299     origFrom->transformation = typename PG::TransformationType();
00300     origTo->transformation = origFrom->transformation * origEdge->mean();
00301 
00302     static ManifoldGradient<PG> gradient;
00303     typename PG::TransformationVectorType f;
00304     typename PG::InformationType A, B;
00305     typename PG::InformationType sysMat;
00306     typename PG::TransformationVectorType rightHand;
00307     for (int iteration = 0; iteration < _addDuplicateEdgeIterations; ++iteration) {
00308       sysMat.fill(0.);
00309       rightHand.fill(0.);
00310 
00311       // original edge
00312       gradient(f, A, B, *origEdge); f *= -1;
00313       rightHand += B.transpose() * (origEdge->information() * f);
00314       sysMat    += B.transpose() * origEdge->information() * B;
00315 
00316       // new edge
00317       gradient(f, A, B, auxEdge); f *= -1;
00318       if (origTo == to) {
00319         rightHand += B.transpose() * (auxEdge.information() * f);
00320         sysMat    += B.transpose() * auxEdge.information() * B;
00321       } else {
00322         rightHand += A.transpose() * (auxEdge.information() * f);
00323         sysMat    += A.transpose() * auxEdge.information() * A;
00324       }
00325       
00326       // solving the system and updating
00327       sysMat = sysMat.inverse(); // sysmat is now the covariance
00328       rightHand = sysMat * rightHand;
00329       //cerr << "update is " << rightHand << endl;
00330       static PoseUpdate<PG> poseUpdate;
00331       poseUpdate(origTo->transformation, &rightHand[0]);
00332     }
00333 
00334     typename PG::TransformationType newMean = origFrom->transformation.inverse() * origTo->transformation;
00335     static TransformCovariance<PG> tCov;
00336     tCov(sysMat, origFrom->transformation, origTo->transformation);
00337     typename PG::InformationType newInfo = sysMat.inverse();
00338     origFrom->restore();
00339     origTo->restore();
00340 
00341     if (0 && this->verbose()) {
00342       cerr << CL_GREEN(__PRETTY_FUNCTION__ << ": updating old edge with new measurement") << endl;
00343       cerr << "old mean: " << origEdge->mean().toVector() << endl;
00344       cerr << "new mean: " << (to==origTo ? mean.toVector() : mean.inverse().toVector()) << endl;
00345       cerr << " -> mean: " << newMean.toVector() << endl << endl;
00346       cerr << "old information:\n" << origEdge->information() << endl;
00347       cerr << "new information:\n" << information << endl;
00348       cerr << " -> information:\n" << newInfo << endl;
00349     }
00350 
00351     this->refineEdge(origEdge, newMean, newInfo);
00352     return origEdge;
00353 
00354   }
00355 
00356 
00357   template <typename PG>
00358   void CholOptimizer<PG>::buildLinearSystem(typename PG::Vertex* rootVertex, double lambda){
00359     for (int i=0; i<(int)_ivMap.size(); i++){
00360       typename PG::Vertex* v=_ivMap[i];
00361       assert(v);
00362       assert(v!=rootVertex);
00363       v->b().fill(0.0);
00364       v->A().fill(0.0);
00365     }
00366     // compute the terms for the pairwise constraints
00367     for (typename std::set<typename PG::Edge*>::const_iterator it=_activeEdges.begin(); it!=_activeEdges.end(); it++){
00368       const typename PG::Edge* e=*it;
00369       e->AFromTo().fill(0.0);
00370     }
00371     int blockCount=0;
00372     for (typename set<typename PG::Edge*>::const_iterator it=_activeEdges.begin(); it!=_activeEdges.end(); it++){
00373       const typename PG::Edge* e=*it;
00374       double l=lambda;
00375       if (e->from()==rootVertex || e->to()==rootVertex)
00376         l=1;
00377       blockCount+= linearizeConstraint(e, l);
00378     }
00379 
00380     int dim = PG::TransformationVectorType::TemplateSize;
00381     assert(dim > 0);
00382 
00383     blockCount+=_ivMap.size();
00384 
00385     _sparseDim=_ivMap.size()*dim;
00386     _nBlocks=blockCount;
00387     _sparseNz=0;
00388 
00389     int sparseNzCurr=_nBlocks*dim*dim;
00390     if (_sparseDim>_sparseDimMax){
00391       if (_sparseB ) {
00392         delete [] _sparseB;
00393         _sparseB = 0;
00394       }
00395       _sparseDimMax=_sparseDim*2;
00396       _sparseB = new double [_sparseDimMax];
00397       for (int i = 0; i < _sparseDimMax; ++i)
00398         _sparseB[i] = 0;
00399     }
00400     if (sparseNzCurr>_sparseNzMax){
00401       if (_sparseMatrix)
00402         delete [] _sparseMatrix;
00403       _sparseNzMax=2*sparseNzCurr;
00404       _sparseMatrix = new SparseMatrixEntry[_sparseNzMax];
00405       delete[] _sparseMatrixPtr;
00406       _sparseMatrixPtr = new SparseMatrixEntry*[_sparseNzMax];
00407     }
00408 
00409     
00410     SparseMatrixEntry* entry=_sparseMatrix;
00411     int nz=0;
00412     for (int i=0; i<(int)_ivMap.size(); i++){
00413       typename PG::Vertex* v=_ivMap[i];
00414       assert(v);
00415       int iBase=i*dim;
00416       for (int j=0; j<dim; j++)
00417         _sparseB[iBase+j]=v->b()[j];
00418       typename PG::InformationType& Ai=v->A();
00419       for (int j=0; j<dim; j++)
00420         for (int k=0; k<dim; k++){
00421           int r=iBase+j;
00422           int c=iBase+k;
00423           double v=Ai[j][k];
00424           entry->set(r,c,v);
00425           nz++;
00426           entry++;
00427         }
00428     }
00429     for (typename std::set<typename PG::Edge*>::const_iterator it=_activeEdges.begin();
00430         it!=_activeEdges.end();
00431         it++){
00432       const typename PG::Edge* e=*it;
00433       const typename PG::Vertex* from=_MY_CAST_<const typename PG::Vertex*>(e->from());
00434       const typename PG::Vertex* to=_MY_CAST_<const typename PG::Vertex*>(e->to());
00435       typename PG::InformationType Aij=e->AFromTo();
00436 
00437       int i=from->tempIndex();
00438       int j=to->tempIndex();
00439       if (i==-1 || j==-1)
00440         continue;
00441 
00442       for (int symm=0; symm<2; symm++){
00443         int iBase=i*dim;
00444         int jBase=j*dim;
00445         if (symm){
00446           Aij.transposeInPlace();
00447           iBase=j*dim;
00448           jBase=i*dim;
00449         }
00450         for (int q=0; q<dim; q++)
00451           for (int k=0; k<dim; k++){
00452             int r=iBase+q;
00453             int c=jBase+k;
00454             double v=Aij[q][k];
00455             entry->set(r,c,v);
00456             nz++;
00457             entry++;
00458           }
00459       }
00460     }
00461     _sparseNz=nz;
00462     //if (_sparseNz!=_nBlocks*dim*dim){
00463       //cerr << "FATAL: _sparseNz=" <<  _sparseNz << " nBlocks*9=" << _nBlocks*9 << "sparseNzCurr" << sparseNzCurr << endl;
00464     //}
00465     assert (_sparseNz==_nBlocks*dim*dim);
00466   }
00467 
00468 
00469   template <typename PG>
00470   void CholOptimizer<PG>::solveAndUpdate(double** block, int r1, int c1, int r2, int c2){
00471     //cerr << "csparse_create"<< endl;
00472     struct cs_sparse *_csA=SparseMatrixEntryPtrVector2CSparse(_sparseMatrixPtr, _sparseDim, _sparseDim, _sparseNz);
00473     //cs_print(_csA, 0);
00474     struct cs_sparse *_ccsA=cs_compress(_csA);
00475     //cerr << "solving";
00476     
00477     // perform symbolic cholesky once
00478     if (_symbolicCholesky == 0) {
00479       _symbolicCholesky = cs_schol (1, _ccsA) ;
00480       if (!_symbolicCholesky) {
00481         cerr << "Symbolic cholesky failed" << endl;
00482       }
00483     }
00484     // re-allocate the temporary workspace for cholesky
00485     if (_csWorkspaceSize < _ccsA->n) {
00486       _csWorkspaceSize = 2 * _ccsA->n;
00487       delete[] _csWorkspace;
00488       _csWorkspace = new double[_csWorkspaceSize];
00489       delete[] _csIntWorkspace;
00490       _csIntWorkspace = new int[2*_csWorkspaceSize];
00491     }
00492 
00493     int ok=0;
00494     if (! block){
00495       ok = cs_cholsolsymb(_ccsA, _sparseB, _symbolicCholesky, _csWorkspace, _csIntWorkspace);
00496     } else {
00497       // re-allocate the temporary workspace for cholesky
00498       if (_csInvWorkspaceSize < _ccsA->n) {
00499         _csInvWorkspaceSize = 2 * _ccsA->n;
00500         delete[] _csInvWorkB;
00501         _csInvWorkB = new double[_csInvWorkspaceSize];
00502         delete[] _csInvWorkTemp;
00503         _csInvWorkTemp = new double[_csInvWorkspaceSize];
00504       }
00505       ok = cs_cholsolinvblocksymb(_ccsA, block, r1, c1, r2, c2, _sparseB,
00506           _symbolicCholesky, _csWorkspace, _csInvWorkB, _csInvWorkTemp, _csIntWorkspace);
00507     }
00508     
00509     if (! ok) {
00510       cerr << "***** FAILURE *****" << endl;
00511       ofstream failed("failed.graph");
00512       this->save(failed);
00513       abort();
00514     }
00515     cs_spfree(_ccsA);
00516     cs_spfree(_csA);
00517 
00518     int dim = PG::TransformationVectorType::TemplateSize;
00519     int position=0;
00520     double* update = _sparseB;
00521     static PoseUpdate<PG> poseUpdate;
00522     for (int i=0; i<_sparseDim; i += dim) {
00523       typename PG::Vertex* v= _ivMap[position];
00524       poseUpdate(v->transformation, update);
00525       update += dim;
00526       position++;
00527     }
00528   }
00529 
00530 
00531   template <typename PG>
00532   void CholOptimizer<PG>::transformSubset(typename PG::Vertex* rootVertex, Graph::VertexSet& vset, const typename PG::TransformationType& newRootPose){
00533     typename PG::TransformationType t=newRootPose*rootVertex->transformation.inverse();
00534     for (Graph::VertexSet::iterator it=vset.begin(); it!=vset.end(); it++){
00535       typename PG::Vertex* v=_MY_CAST_<typename PG::Vertex*>(*it);
00536       v->transformation=t*v->transformation;
00537     }
00538   }
00539  
00540 
00541   template <typename PG>
00542   struct ActivePathUniformCostFunction: public PG::PathLengthCostFunction{
00543     ActivePathUniformCostFunction(CholOptimizer<PG>* optimizer);
00544     virtual double operator()(Graph::Edge* edge, Graph::Vertex* from, Graph::Vertex* to);
00545   protected:
00546     CholOptimizer<PG>* _optimizer;
00547   };
00548   
00549   template <typename PG>
00550   ActivePathUniformCostFunction<PG>::ActivePathUniformCostFunction(CholOptimizer<PG>* optimizer){
00551     _optimizer=optimizer;
00552   }
00553     
00554   template <typename PG>
00555   double ActivePathUniformCostFunction<PG>::operator()(Graph::Edge* edge, Graph::Vertex* from, Graph::Vertex* to){
00556     typename PG::Edge* e = dynamic_cast<typename PG::Edge*>(edge);
00557     typename std::set<typename PG::Edge*>::iterator it=_optimizer->_activeEdges.find(e);
00558     if (it==_optimizer->_activeEdges.end())
00559       return std::numeric_limits<double>::max();
00560     return 1.;
00561     typename PG::TransformationType::TranslationType t=e->mean().translation();
00562     return sqrt(t*t);
00563   }
00564 
00565   template <typename PG>
00566   void CholOptimizer<PG>::initializeActiveSubsetWithObservations(typename PG::Vertex* root, double maxDistance){
00567     assert(root);
00568     Dijkstra dv(this);
00569     std::map<const typename PG::Vertex*, typename PG::TransformationType> tempT;
00570     for (typename std::set<typename PG::Edge*>::iterator it=_activeEdges.begin(); it!=_activeEdges.end(); it++){
00571       typename PG::Vertex* from=static_cast<typename PG::Vertex*>((*it)->from());
00572       typename PG::Vertex* to=static_cast<typename PG::Vertex*>((*it)->to());
00573       tempT[from]=from->transformation;
00574       tempT[to]=to->transformation;
00575     }
00576     ActivePathUniformCostFunction<PG> apl(this);
00577     dv.shortestPaths(root,&apl,maxDistance);
00578     this->propagateAlongDijkstraTree(root, dv.adjacencyMap(), false, true); 
00579     for (typename std::set<typename PG::Edge*>::iterator it=_activeEdges.begin(); it!=_activeEdges.end(); it++){
00580       typename PG::Vertex* from=static_cast<typename PG::Vertex*>((*it)->from());
00581       typename PG::Vertex* to=static_cast<typename PG::Vertex*>((*it)->to());
00582       if (from->tempIndex()==-1)
00583         from->transformation=tempT[from];
00584       if (to->tempIndex()==-1)
00585         to->transformation=tempT[to];
00586     }
00587   }
00588 
00589 template <typename PG>
00590 void CholOptimizer<PG>::storeVertices()
00591 {
00592   for (typename std::vector<typename PG::Vertex*>::iterator it = _ivMap.begin(); it != _ivMap.end(); ++it) {
00593     typename PG::Vertex* v = *it;
00594     v->storedTransformation = v->transformation;
00595   }
00596 }
00597 
00598 template <typename PG>
00599 void CholOptimizer<PG>::restoreVertices()
00600 {
00601   for (typename std::vector<typename PG::Vertex*>::iterator it = _ivMap.begin(); it != _ivMap.end(); ++it) {
00602     typename PG::Vertex* v = *it;
00603     v->transformation = v->storedTransformation;
00604   }
00605 }
00606 
00607 } // end namespace


hogman_minimal
Author(s): Maintained by Juergen Sturm
autogenerated on Mon Oct 6 2014 00:06:58