graph_optimizer_hchol_aux.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 
00026 
00027 namespace AISNavigation{
00028 
00029   using namespace std;
00030 
00031   template <typename PG>
00032   HCholOptimizer<PG>::HVertex::HVertex(int id):
00033     PG::Vertex(id){
00034     _root=0;
00035     _lowerRoot=0;
00036     _parentVertex=0;
00037     _edgeToRoot=0;
00038     _distanceToRoot=0;
00039     _tainted=false;
00040   }
00041 
00042   template <typename PG>
00043   HCholOptimizer<PG>::HVertex::~HVertex(){
00044     if (_parentVertex){
00045       typename HVertexSet::iterator selfIt=_parentVertex->_children.find(this);
00046       assert(selfIt!=_parentVertex->_children.end());
00047       if (selfIt!=_parentVertex->_children.end()){
00048         _parentVertex->_children.erase(selfIt);
00049       }
00050     }    
00051     detachChildren();
00052   }
00053 
00054   template <typename PG>
00055   void HCholOptimizer<PG>::HVertex::detachChildren(){
00056     for (typename HVertexSet::iterator it=children().begin(); it!=children().end(); it++){
00057         HVertex* v=*it;
00058         if (v->_root==v)
00059           v->_optimizer->_rootIDs.insert(v->id());
00060         v->detachChildren();
00061         v->_root=0;
00062         v->_lowerRoot=0;
00063         v->_parentVertex=0;
00064         v->_edgeToRoot=0;
00065         v->_distanceToRoot=0;
00066     }
00067     _lowerRoot=0;
00068     _children.clear();
00069   }
00070 
00071   template <typename PG>
00072   void HCholOptimizer<PG>::HVertex::taint(){
00073     // std::cerr << __PRETTY_FUNCTION__ << this << " id:" << _id << " opt:" << _optimizer <<  " parent=" << _parentVertex << endl;
00074     if (_optimizer->_lowerOptimizer)
00075       _tainted=true;
00076     if (_parentVertex)
00077       _parentVertex->taint();
00078   }
00079 
00080   template <typename PG>
00081   typename PG::Vertex* HCholOptimizer<PG>::addVertex(const int& k){
00082     HVertex* v=new HVertex(k);
00083     v->_optimizer=this;
00084     HVertex* vresult=dynamic_cast<HVertex*>(Graph::addVertex(v));
00085     if (!vresult){
00086       delete v;
00087     }
00088     return vresult;
00089   }
00090   
00091   template <typename PG>
00092   typename PG::Vertex* HCholOptimizer<PG>::addVertex(int id, const typename PG::TransformationType& pose, const typename PG::InformationType& information){
00093     typename PG::Vertex* v=new HVertex(id);
00094     HVertex* vresult=dynamic_cast<HVertex*>(Graph::addVertex(v));
00095     vresult->_optimizer=this;
00096     if (!vresult){
00097       delete v;
00098       return vresult;
00099     }
00100     vresult->transformation=pose;
00101     vresult->covariance=information.inverse();
00102     return vresult;
00103   }
00104 
00105   template <typename PG>
00106   typename PG::Edge*   HCholOptimizer<PG>::addEdge(typename PG::Vertex* from, typename PG::Vertex* to,
00107       const typename PG::TransformationType& mean, const typename PG::InformationType& information){
00108     typename PG::Edge* e=0;
00109     if (!_lowerOptimizer){
00110       e=CholOptimizer<PG>::addEdge(from, to, mean, information);
00111       if (to->edges().size()==1){
00112         to->transformation=from->transformation*mean;
00113       }
00114       HVertex* hFrom=dynamic_cast<HVertex*>(from);
00115       hFrom->taint();
00116     } else
00117       e=PG::addEdge(from, to, mean, information);
00118     _cachedChi+=this->chi2(e);
00119     return e;
00120   };
00121 
00122 
00123   template <typename PG>
00124   void HCholOptimizer<PG>::refineEdge(typename PG::Edge* e, const typename PG::TransformationType& mean, const typename PG::InformationType& information){
00125     double derr=-this->chi2(e);
00126     PG::refineEdge(e,mean,information);
00127     derr+=this->chi2(e);
00128     _cachedChi+=derr;
00129   }
00130 
00131   template <typename PG>
00132   bool HCholOptimizer<PG>::removeEdge(Graph::Edge* e){
00133     //cerr << __PRETTY_FUNCTION__ << " this:" << this << "edge: " << e->from()->id() << " " << e->to()->id() << endl;
00134     HVertex* v1=dynamic_cast<HVertex*>(e->from());
00135     HVertex* v2=dynamic_cast<HVertex*>(e->to());
00136     assert(this->vertex(e->from()->id())==v1);
00137     assert(this->vertex(e->to()->id())==v2);
00138     assert(v1->_optimizer==this);
00139     assert(v2->_optimizer==this);
00140     if (!_lowerOptimizer){
00141       v1->taint();
00142       v2->taint();
00143     }
00144     typename PG::Edge* eAux = reinterpret_cast<typename PG::Edge*>(e);
00145     _cachedChi-=this->chi2(eAux);
00146     _lastOptChi-=this->chi2(eAux);
00147     return Graph::removeEdge(e);
00148   }
00149   
00150 
00151   template <typename PG>
00152   bool HCholOptimizer<PG>::removeVertex(Graph::Vertex* _v){
00153     HVertex* v=dynamic_cast<HVertex*>(_v);
00154     assert (v->_optimizer==this);
00155     if (! v) {
00156       cerr << __PRETTY_FUNCTION__ << ": attempting to remove node " << _v->id()  << " which is not present in the graph" << endl;
00157       assert(0);
00158       return false;
00159     }
00160     //cerr << __PRETTY_FUNCTION__ << " " << this << ": removing " << _v->id() << endl;
00161     assert(v);
00162     if (!_lowerOptimizer)
00163       v->taint();
00164     return Graph::removeVertex(v);
00165   }
00166 
00167 
00168   template <typename PG>
00169   void HCholOptimizer<PG>::addVertexToUpperLevels(HVertex* v){
00170     if (! _upperOptimizer)
00171       return;
00172     assert(v);
00173     v->_edgeToRoot=0;
00174     v->_distanceToRoot=0;
00175     v->_root=v;
00176     HVertex* vup=dynamic_cast<HVertex*>(_upperOptimizer->addVertex(v->id(), v->transformation, v->covariance, 0));
00177     v->_parentVertex=vup;
00178     vup->_children.insert(v);
00179     vup->_lowerRoot=v;
00180     _upperOptimizer->addVertexToUpperLevels(vup);
00181   }
00182 
00183   template <typename PG>
00184   void HCholOptimizer<PG>::cleanupTainted(){
00185     if (_upperOptimizer)
00186       _upperOptimizer->cleanupTainted();
00187     if (_lowerOptimizer){
00188       HVertexSet removed;
00189       for (typename PG::VertexIDMap::iterator it=this->vertices().begin(); it!=this->vertices().end(); it++){
00190         HVertex* v=dynamic_cast<HVertex*>(it->second);
00191         if (v->_tainted || ! v->_lowerRoot){
00192           removed.insert(v);
00193         }
00194       }
00195       for (typename HVertexSet::iterator it=removed.begin(); it!=removed.end(); it++){
00196         HVertex* v=*it;
00197         removeVertex(v);
00198       }
00199     }
00200   }
00201 
00202 
00203 #if 0
00204   template <typename PG>
00205   void HCholOptimizer<PG>::visualizeWithGnuplot(){
00206     if (_lowerOptimizer)
00207       return;
00208     int l=0;
00209     HCholOptimizer<PG>* opt=this;
00210     string command="plot ";
00211     int activeLevels=0;
00212     bool first=false;
00213     while (opt){
00214       if (opt && opt->_gnuplot){
00215         char chlev='0'+(char)l;
00216         if (first)
00217           command =command+ ", ";
00218         command = command + "\'-\' w l title \"level"+ chlev + "\"";
00219         if (opt->_upperOptimizer && opt->_upperOptimizer->_gnuplot)
00220         activeLevels++;
00221         first=true;
00222       }
00223       opt=opt->_upperOptimizer;
00224       l++;
00225     }
00226     if (activeLevels){
00227       cout  << command << endl;
00228     }
00229     opt=this;
00230     while (opt){
00231       if (opt && opt->_gnuplot){
00232         opt->saveAsGnuplot(cout, !_lowerOptimizer);
00233         cout << "e" << endl;
00234       }
00235       opt=opt->_upperOptimizer;
00236     }
00237     if (activeLevels)
00238       cout << endl;
00239   }
00240 #endif
00241 
00242 } // end namespace


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