$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 "graph_optimizer2d_hchol.h" 00018 #include <iostream> 00019 #include <iomanip> 00020 #include <algorithm> 00021 #include <iterator> 00022 #include <sys/time.h> 00023 #include <hogman_minimal/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 }