graph_optimizer_hchol_batch.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   void HCholOptimizer<PG>::annotateHiearchicalEdges(int iterations, double lambda, bool initWithObservations){
00033     if (this->verbose())
00034       cerr <<  "refining edges" << endl; 
00035     for (typename PG::EdgeSet::iterator it=this->edges().begin(); it!=this->edges().end(); it++){
00036       typename PG::Edge* e=dynamic_cast<typename PG::Edge*>(*it);
00037       annotateHiearchicalEdge(e,iterations, lambda, initWithObservations);
00038     }
00039     if (this->verbose())
00040       cerr << "done";
00041   }
00042 
00043   template <typename PG>
00044   void HCholOptimizer<PG>::bottomToTop(int iterations, double lambda, bool initWithObservations){
00045     if (! _lowerOptimizer)
00046       return;
00047     if (_lowerOptimizer){
00048       _lowerOptimizer->bottomToTop(iterations, lambda,initWithObservations);
00049     }
00050     annotateHiearchicalEdges(iterations, lambda, initWithObservations);
00051     if (this->verbose())
00052       cerr << "_upperOptimizer="  << _upperOptimizer << endl;
00053     if (! _upperOptimizer){
00054       if (this->verbose())
00055         cerr << "Optimizing the top level" << endl;
00056       HVertex* rootVertex=_MY_CAST_<HVertex*>(this->vertices().begin()->second);
00057       initialize(rootVertex->id());
00058       //ofstream oss("topLevel.graph");
00059       //save(oss);
00060       //oss.close();
00061       Graph::VertexSet vset;
00062       for (Graph::VertexIDMap::const_iterator it=this->vertices().begin(); it!=this->vertices().end(); it++){
00063         vset.insert(it->second);
00064       }
00065       optimizeSubset(rootVertex, vset, iterations*3, lambda, true);
00066       if (this->verbose()) {
00067         cerr << "Done";
00068         //ofstream os("topLevel.dat");
00069         //saveAsGnuplot(os);
00070         //os.close();
00071         cerr << "done" << endl;
00072       }
00073 
00074     }
00075   }
00076 
00077   template <typename PG>
00078   void HCholOptimizer<PG>::topToBottom(int iterations, double lambda){
00079     if (! _lowerOptimizer)
00080       return;
00081 
00082     int total=0;
00083     // first step, project the nodes according to the parent
00084     for (Graph::VertexIDMap::iterator it=this->vertices().begin(); it!=this->vertices().end(); it++){
00085       HVertex* v=dynamic_cast<HVertex*>(it->second);
00086       assert(v);
00087       _lowerOptimizer->transformSubset(v->lowerRoot(), *(Graph::VertexSet*)(&v->children()), v->transformation);
00088       _lowerOptimizer->optimizeSubset (v->lowerRoot(), *(Graph::VertexSet*)(&v->children()), 0, 0., true);
00089       total+=v->children().size();
00090     }
00091     if((int)_lowerOptimizer->vertices().size()!=total){
00092       cerr << "fatal error in partitioning: " << _lowerOptimizer->vertices().size()  << " != " << total << endl;  
00093     }
00094     
00095     // second step, optimize the nodes, by keeping the neighbors fixed (relaxation)
00096     for (Graph::VertexIDMap::iterator it=this->vertices().begin(); it!=this->vertices().end(); it++){
00097 
00098       HVertex* v=dynamic_cast<HVertex*>(it->second);
00099       assert(v);
00100       if (this->verbose()) cerr << "d";
00101       _lowerOptimizer->optimizeSubset (v->lowerRoot(), *(Graph::VertexSet*)(&v->children()), iterations, lambda, true);
00102     }
00103 
00104     HCholOptimizer<PG>* lowerHOpt=dynamic_cast<HCholOptimizer<PG>*>(_lowerOptimizer);
00105     if (lowerHOpt){
00106       lowerHOpt->topToBottom(iterations, lambda);
00107     }
00108    
00109   }
00110 
00111   template <typename PG>
00112   void HCholOptimizer<PG>::vCycle(int iterations, double lambdaUp, double lambdaDown, bool initWithObservations){
00113     if (_upperOptimizer){
00114       cerr << "# ERROR, a V-Cycle can be only started from the topmost optimizer in the hierarchy" << endl;
00115       return;
00116     }
00117     bottomToTop(iterations, lambdaUp, initWithObservations);
00118     topToBottom(iterations, lambdaDown);
00119   }
00120 
00121 }


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