graph_optimizer_hchol_incremental.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 "graph_optimizer2d_hchol.h"
00018 #include <iostream>
00019 #include <iomanip>
00020 #include <algorithm>
00021 #include <iterator>
00022 #include <sys/time.h>
00023 #include <stuff/os_specific.h>
00024 #include <assert.h>
00025 #include <list>
00026 
00027 namespace AISNavigation{
00028   using namespace std;
00029 
00030   template <typename PG>
00031   bool HCholOptimizer<PG>::optimizePendingIncremental(){
00032     if (! _lowerOptimizer){
00033       HVertexSet updatedSet;
00034       for (typename PG::VertexIDMap::iterator it=this->vertices().begin(); it!=this->vertices().end(); it++){
00035         HVertex* v=dynamic_cast<HVertex*>(it->second);
00036         if (! v->_root){
00037           updatedSet.insert(v);
00038         }
00039       }
00040       updateStructure(true);
00041       optimizeLevels(_propagateDown);
00042       std::set<HVertex*> topLevelSet;
00043       for (typename HVertexSet::iterator it =updatedSet.begin(); it!=updatedSet.end(); it++){
00044         HVertex* v=*it;
00045         postprocessIncremental(v);
00046         while (v){
00047           if (! v->parentVertex())
00048             topLevelSet.insert(v);
00049           v=v->parentVertex();
00050         }
00051       }
00052       for (typename HVertexSet::iterator it =topLevelSet.begin(); it!=topLevelSet.end(); it++){
00053         HVertex* v=*it;
00054         v->_optimizer->propagateDownIncremental(v,1.);
00055       }
00056 
00057       HCholOptimizer<PG>* opt=this;
00058       int l=0;
00059       while(opt){
00060         opt=opt->_upperOptimizer;
00061         l++;
00062       }
00063       return true;
00064     }
00065     return false;
00066   }
00067  
00068   template <typename PG>
00069   void HCholOptimizer<PG>::postprocessIncremental(HVertex* v){
00070     //annotateHierarchicalEdgesIncremental(v);
00071     if (!_lowerOptimizer)
00072       propagateDownIncremental(v);
00073   }
00074   
00075   template <typename PG>
00076   void HCholOptimizer<PG>::propagateDownIncremental(HVertex* to, double lambda){
00077     if (! _lowerOptimizer)
00078       return;
00079     transformSubset(to->lowerRoot(), *(Graph::VertexSet*)(&to->children()),to->transformation);
00080     optimizeSubset(to->lowerRoot(),  *(Graph::VertexSet*)(&to->children()), _downIncrementalIterations, lambda, false);
00081     for (typename HVertexSet::iterator it=to->children().begin(); it!=to->children().end(); it++){
00082       HVertex* v=*it;
00083       _lowerOptimizer->propagateDownIncremental(v,lambda);
00084     }
00085   }
00086 
00087   template <typename PG>
00088   void HCholOptimizer<PG>::propagateDownIncremental(HVertex* to){
00089     if (! _upperOptimizer)
00090       return;
00091     _upperOptimizer->propagateDownIncremental(to->parentVertex());
00092     if (_upperOptimizer){
00093       HVertex* toParent=to->parentVertex();
00094       std::set<HVertex*> upperRegion;
00095       for (std::set<Graph::Edge*>::iterator it =toParent->edges().begin(); it!=toParent->edges().end(); it++){
00096         HVertex* fpv=dynamic_cast<HVertex*>((*it)->from());
00097         HVertex* tpv=dynamic_cast<HVertex*>((*it)->to());
00098         upperRegion.insert(fpv);
00099         upperRegion.insert(tpv);
00100       }
00101       Graph::VertexSet region;
00102       for (typename HVertexSet::iterator it=upperRegion.begin(); it!=upperRegion.end(); it++){
00103         HVertex* pv=*it;
00104         for(typename HVertexSet::iterator ft=pv->children().begin(); ft!=pv->children().end(); ft++){
00105           region.insert(*ft);
00106         }
00107         transformSubset(pv->lowerRoot(), *(Graph::VertexSet*)(&pv->children()), pv->transformation);
00108       }
00109       optimizeSubset(toParent->lowerRoot(), region, _downIncrementalIterations, 1., false);
00110     }
00111   }
00112 
00113   template <typename PG>
00114   bool HCholOptimizer<PG>::optimizeLevels(bool propagateDown){
00115     if (! _upperOptimizer){
00116 
00117       double tGlobalOptimization=0;
00118       if (cachedChi()-_lastOptChi>0.){
00119         struct timeval ts,te;
00120         gettimeofday(&ts,0);
00121         if (this->verbose()) cerr <<"o";
00122         bool v=this->verbose();
00123         this->verbose()=false;
00124         CholOptimizer<PG>::optimize(_globalIncrementalIterations,false);
00125         _lastOptChi=this->chi2();
00126         _cachedChi=_lastOptChi;
00127         this->verbose()=v;
00128 
00129         gettimeofday(&te,0);
00130         tGlobalOptimization=1e-6*(te.tv_usec-ts.tv_usec)+te.tv_sec-ts.tv_sec;
00131         return true;
00132       }
00133       return false;
00134     }
00135     bool ok=_upperOptimizer->optimizeLevels(propagateDown);
00136     if (! ok){
00137       if (this->verbose()) cerr << "n";
00138       return false;
00139     }
00140     HVertexSet changed;
00141     int touched=0;
00142     for (typename PG::VertexIDMap::iterator it=_upperOptimizer->vertices().begin(); it!=_upperOptimizer->vertices().end(); it++){
00143       HVertex* parentVertex=dynamic_cast<HVertex*>(it->second);
00144       assert (!parentVertex->_tainted);
00145       typename PG::TransformationType delta=parentVertex->transformation.inverse()*parentVertex->lowerRoot()->transformation;
00146       typename PG::TransformationType::TranslationType pdeltaTrans = delta.translation();
00147       _Vector<PG::TransformationType::RotationType::Angles, double> pdeltaRot = delta.rotation().angles();
00148       double maxTrans = 0.0;
00149       for (int i = 0; i < pdeltaTrans.size(); ++i)
00150         if (fabs(pdeltaTrans[i]) > maxTrans)
00151           maxTrans = fabs(pdeltaTrans[i]);
00152       double maxRot = 0.0;
00153       for (int i = 0; i < pdeltaRot.size(); ++i)
00154         if (fabs(pdeltaRot[i]) > maxRot)
00155             maxRot = fabs(pdeltaRot[i]);
00156       if (maxTrans < _translationalPropagationError && maxRot < _rotationalPropagationError)
00157         continue;
00158       transformSubset(parentVertex->lowerRoot(), *(Graph::VertexSet*)(&parentVertex->children()), parentVertex->transformation);
00159       touched+=parentVertex->children().size();
00160       changed.insert(parentVertex);
00161     }
00162     _nVerticesPropagatedDownIncremental=changed.size();
00163     if (propagateDown){
00164       for (typename HVertexSet::iterator it=changed.begin(); it!=changed.end(); it++){
00165         HVertex* parentVertex=*it;
00166         optimizeSubset(parentVertex->lowerRoot(), *(Graph::VertexSet*)(&parentVertex->children()), _downIncrementalIterations, 1., false);
00167       } 
00168     }
00169     return true;
00170   }
00171 
00172 
00173   template <typename PG>
00174   void HCholOptimizer<PG>::annotateHierarchicalEdgesIncremental(HVertex* v){
00175     if (! _upperOptimizer || _lowerOptimizer)
00176       return;
00177   // update the hierarchy of edges up;
00178     HVertex* vParent=v->parentVertex();
00179     HCholOptimizer<PG>* opt=_upperOptimizer;
00180     while (opt) {
00181       const typename PG::EdgeSet& eset=vParent->edges();
00182       for (typename PG::EdgeSet::iterator it=eset.begin(); it!=eset.end(); it++){
00183         typename PG::Edge* e=dynamic_cast<typename PG::Edge*>(*it);     
00184         opt->_cachedChi-=chi2(e);
00185         opt->annotateHiearchicalEdge(e, _edgeAnnotationIncrementalIterations, 0., false);
00186         opt->_cachedChi+=chi2(e);
00187       }
00188       vParent=vParent->parentVertex();
00189       opt=opt->_upperOptimizer;
00190     }
00191   }
00192 
00193 
00194   template <typename PG>
00195   bool HCholOptimizer<PG>::optimizeUpperLevelIncremental(){
00196     if (! _upperOptimizer || _lowerOptimizer)
00197       return false;
00198     HCholOptimizer<PG>* upperOpt=this;
00199     while (upperOpt->_upperOptimizer){
00200       upperOpt=upperOpt->_upperOptimizer;
00201     }
00202     if (upperOpt->cachedChi()-upperOpt->_lastOptChi>1.){
00203       if (this->verbose()) cerr <<"o";
00204       bool v=upperOpt->verbose();
00205       upperOpt->verbose()=false;
00206       upperOpt->optimize(_globalIncrementalIterations,false);
00207       upperOpt->_lastOptChi=upperOpt->chi2();
00208       upperOpt->_cachedChi=upperOpt->_lastOptChi;
00209       upperOpt->verbose()=v;
00210       return true;
00211     }
00212     return false;
00213   }
00214 
00215   template <typename PG>
00216   void HCholOptimizer<PG>::updateStructure(bool incremental){
00217     typedef  std::set<HVertex*, Graph::VertexIDCompare> HVertexSetID;
00218 
00219     if (!_lowerOptimizer && _upperOptimizer){ 
00220       if (! incremental) {
00221         HCholOptimizer<PG> * opt=_upperOptimizer;
00222         while (opt){
00223           opt->clear();
00224           opt=opt->_upperOptimizer;
00225         }
00226         for (typename PG::VertexIDMap::iterator it=this->vertices().begin(); it!=this->vertices().end(); it++){
00227           HVertex* v=dynamic_cast<HVertex*>(it->second);
00228           v->_root=0;
00229           v->_parentVertex=0;
00230           v->_edgeToRoot=0;
00231           v->_distanceToRoot=0;
00232           v->_lowerRoot=0;
00233         }
00234       }
00235       cleanupTainted();
00236       _upperOptimizer->updateStructure(incremental);
00237       return;
00238     }
00239     
00240     typedef std::multimap<double, HVertex*> ProgressiveMap;
00241     // phase1 construct an associationMap
00242     HVertexSetID openSet;
00243     HVertexSetID openOldRoots;
00244     if (this->verbose()) cerr << "openSet: ";
00245 
00246     for (typename PG::VertexIDMap::iterator it=_lowerOptimizer->vertices().begin(); it!=_lowerOptimizer->vertices().end(); it++){
00247       HVertex* v=dynamic_cast<HVertex*>(it->second);
00248       if (v->_root==0){
00249         openSet.insert(v);
00250         if (_rootIDs.find(v->id())!=_rootIDs.end()){
00251           //if (v->_wasRoot) {
00252           openOldRoots.insert(v);
00253           if (this->verbose()) cerr << v->id() << "!";
00254         }
00255         if (this->verbose()) cerr << v->id() << " ";
00256       }
00257     }
00258 
00259 
00260     if (this->verbose()) cerr << endl;
00261     if (this->verbose()) cerr << "openSet.size=" << openSet.size()<< endl;
00262     std::list<HVertex*> newVertices;
00263     if (this->verbose()) cerr << "Island Creation" << endl;
00264     // create the non-overlapping sets
00265     Dijkstra dv(_lowerOptimizer);
00266     UniformCostFunction cost;
00267     while (! openSet.empty()){
00268       bool resumedRoot=false;
00269       typename HVertexSetID::iterator openIt=openSet.begin();
00270       typename HVertexSetID::iterator openOldRootIt=openOldRoots.begin();
00271       if (! openOldRoots.empty()){
00272         openIt=openSet.find(*openOldRootIt);
00273         resumedRoot=true;
00274         if (openIt == openSet.end()) {
00275           cerr << "fatal, vertex in the openRootSet" << (*openOldRootIt)->id() << " not in the openSet" << endl;
00276         }
00277       } else {
00278         openIt=openSet.begin();
00279       }
00280       HVertex* root = *openIt;
00281 
00282       if (root->_root){
00283         openSet.erase(openIt);
00284         if (resumedRoot)
00285           openOldRoots.erase(openOldRootIt);
00286         continue;
00287       }
00288 
00289       HVertex* hv=dynamic_cast<HVertex*>(addVertex(root->id()));
00290       assert(hv);
00291       newVertices.push_back(hv);
00292       hv->_lowerRoot=root;
00293       hv->transformation=root->transformation;
00294       hv->covariance=root->covariance;
00295       hv->_children.insert(root);
00296 
00297       root->_parentVertex=hv;
00298       root->_distanceToRoot=0;
00299       root->_edgeToRoot=0;
00300       root->_root=root;
00301 
00302       openSet.erase(openIt);
00303       if (resumedRoot)
00304         openOldRoots.erase(openOldRootIt);
00305 
00306       dv.shortestPaths(root,&cost,_maxDistance);
00307       ProgressiveMap progressiveMap;
00308       for (Graph::VertexSet::const_iterator it=dv.visited().begin(); it!=dv.visited().end(); it++){
00309         HVertex* v=dynamic_cast<HVertex*>(*it);
00310         double d=dv.adjacencyMap().find(v)->second.distance();
00311         progressiveMap.insert(make_pair(d,v));
00312       }
00313 
00314       for (typename ProgressiveMap::const_iterator it=progressiveMap.begin(); it!=progressiveMap.end(); it++){
00315         HVertex* v=it->second;
00316         typename PG::Edge* edgeToRoot=dynamic_cast<typename PG::Edge*>(dv.adjacencyMap().find(v)->second.edge());
00317         HVertex* previousV=dynamic_cast<HVertex*>(dv.adjacencyMap().find(v)->second.parent());
00318         double d=it->first;
00319         if ((v->_root==0/* || v->_distanceToRoot>d*/) && previousV && previousV->_root==root){
00320           typename HVertexSet::iterator ot=openSet.find(v);
00321 
00322           assert (ot!=openSet.end());
00323           openSet.erase(ot);
00324           typename HVertexSet::iterator oldRootIt=openOldRoots.find(v);
00325           if (oldRootIt!=openOldRoots.end())
00326             openOldRoots.erase(oldRootIt);
00327 
00328           v->_parentVertex=hv;
00329           v->_distanceToRoot=d;
00330           v->_edgeToRoot=edgeToRoot;
00331           v->_root=root;
00332           std::set<int>::iterator v_it=_rootIDs.find(v->id());
00333           if (v_it!=_rootIDs.end())
00334             _rootIDs.erase(v_it);
00335           hv->_children.insert(v);
00336           if (this->verbose()) cerr << v->id() << " vparent= " << hv->id() << " vprevious=" << previousV->id() << endl;
00337         }
00338       }
00339     }
00340 
00341 #ifndef DNDEBUG
00342     if (this->verbose()) cerr << "CONSISTENCY_CHECK " << endl;
00343     // check that every children has a parent and that each parent contains the children in its children set;
00344     for (typename PG::VertexIDMap::iterator it=_lowerOptimizer->vertices().begin(); it!=_lowerOptimizer->vertices().end(); it++){
00345       HVertex * lv= dynamic_cast<HVertex*>(it->second);
00346       if (this->verbose()) cerr << "n:" << lv->id() << " p:" << lv->parentVertex()->id() << endl;
00347       assert(lv->parentVertex());
00348       assert(lv->parentVertex()->children().find(lv)!=lv->parentVertex()->children().end());
00349     }
00350 #endif
00351 
00352     if (this->verbose())  {
00353       cerr << "Edges Creation" << endl;
00354       cerr << "newVertices.size=" << newVertices.size()<< endl;
00355     }
00356 
00357     // connect the sets
00358     for (typename list<HVertex*>::iterator it=newVertices.begin(); it!=newVertices.end(); it++){
00359       HVertex* hv=dynamic_cast<HVertex*>(*it);
00360       for (typename HVertexSet::iterator vt=hv->_children.begin(); vt!=hv->_children.end(); vt++){
00361         HVertex* cv=dynamic_cast<HVertex*>(*vt);
00362         for (typename PG::EdgeSet::iterator et=cv->edges().begin(); et!=cv->edges().end(); et++){
00363           typename PG::Edge* e=dynamic_cast<typename PG::Edge*>(*et);
00364           HVertex* cv1=dynamic_cast<HVertex*>(e->from());
00365           HVertex* cv2=dynamic_cast<HVertex*>(e->to());
00366           HVertex* pv1=cv1->parentVertex();
00367           HVertex* pv2=cv2->parentVertex();
00368           assert(pv1 && pv2 && (pv1==hv || pv2==hv) );
00369           if (pv1!=pv2 && connectingEdges(pv1,pv2).empty() && connectingEdges(pv2,pv1).empty()){
00370             typename PG::InformationType info = PG::InformationType::eye(1.);
00371             int rotDim = PG::TransformationType::RotationType::Dimension;
00372             assert(rotDim + PG::TransformationType::RotationType::Angles == info.rows());
00373             for (int i = rotDim; i < info.rows(); ++i)
00374               info[i][i]=5;
00375             typename PG::TransformationType mean=pv1->transformation.inverse()*pv2->transformation;
00376             typename PG::Edge* en=addEdge( pv1, pv2,  mean, info);
00377             annotateHiearchicalEdge(en, _edgeAnnotationIncrementalIterations, 0., false);
00378           }
00379         }
00380       }
00381     }
00382     if (_upperOptimizer)
00383       _upperOptimizer->updateStructure(incremental);
00384   }
00385 
00386 }


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