$search
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 <stuff/os_specific.h> 00023 #include <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 (verbose()) 00033 cerr << "# init " << rootNode << endl; 00034 if (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=vertices().begin(); it!=vertices().end(); it++){ 00048 vset.insert(it->second); 00049 } 00050 00051 typename PG::Vertex* root=dynamic_cast<typename PG::Vertex*>(vertex(_rootNode)); 00052 if (! root) 00053 root=_MY_CAST_<typename PG::Vertex*>(vertices().begin()->second); 00054 if (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 (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*>(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 (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=connectingEdges(from, to); 00274 Graph::EdgeSet eset2=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 && 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 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 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