$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/macros.h> 00024 #include <hogman_minimal/stuff/os_specific.h> 00025 #include <assert.h> 00026 00027 00028 namespace AISNavigation{ 00029 using namespace std; 00030 00031 template <typename PG> 00032 HCholOptimizer<PG>::HCholOptimizer(double maxDistance){ 00033 _lowerOptimizer=0; 00034 _upperOptimizer=0; 00035 _cachedChi=0.; 00036 _lastOptChi=0; 00037 _online=false; 00038 _maxDistance=_maxDistance; 00039 _gnuplot=false; 00040 00041 _translationalPropagationError=0.05; 00042 _rotationalPropagationError=0.05; 00043 00044 _globalIncrementalIterations=5; 00045 _downIncrementalIterations=2; 00046 _edgeAnnotationIncrementalIterations=5; 00047 _propagateDown=false; 00048 } 00049 00050 template <typename PG> 00051 HCholOptimizer<PG>::HCholOptimizer(HCholOptimizer<PG>* ll, double maxDistance){ 00052 _upperOptimizer=0; 00053 _lowerOptimizer=ll; 00054 _cachedChi=1.; 00055 _lastOptChi=1.; 00056 HCholOptimizer<PG> *lho=dynamic_cast<HCholOptimizer<PG> *>(ll); 00057 if (lho) 00058 lho->_upperOptimizer=this; 00059 _maxDistance=maxDistance; 00060 _gnuplot=false; 00061 _online=false; 00062 00063 _translationalPropagationError=0.1; 00064 _rotationalPropagationError=0.05; 00065 _globalIncrementalIterations=3; 00066 _downIncrementalIterations=3; 00067 _edgeAnnotationIncrementalIterations=5; 00068 _propagateDown=false; 00069 } 00070 00071 template <typename PG> 00072 HCholOptimizer<PG>::HCholOptimizer(int nLevels, double maxDistance){ 00073 _lowerOptimizer=0; 00074 _upperOptimizer=0; 00075 _cachedChi=0.; 00076 _lastOptChi=0; 00077 _online=false; 00078 _maxDistance=_maxDistance; 00079 _gnuplot=false; 00080 HCholOptimizer<PG>* lopt=this; 00081 for (int i=1; i<nLevels; i++){ 00082 lopt=new HCholOptimizer<PG>(lopt, maxDistance); 00083 } 00084 00085 _translationalPropagationError=0.1; 00086 _rotationalPropagationError=0.05; 00087 _globalIncrementalIterations=3; 00088 _downIncrementalIterations=3; 00089 _edgeAnnotationIncrementalIterations=5; 00090 _propagateDown=false; 00091 } 00092 00093 template <typename PG> 00094 HCholOptimizer<PG>::~HCholOptimizer(){ 00095 if (_upperOptimizer){ 00096 delete _upperOptimizer; 00097 _upperOptimizer=0; 00098 } 00099 } 00100 00101 template <typename PG> 00102 void HCholOptimizer<PG>::clear(){ 00103 if (_upperOptimizer) 00104 _upperOptimizer->clear(); 00105 PG::clear(); 00106 _rootIDs.clear(); 00107 } 00108 00109 00110 template <typename PG> 00111 int HCholOptimizer<PG>::nLevels() const { 00112 assert(! _lowerOptimizer); 00113 const HCholOptimizer<PG>* opt=this; 00114 int l=0; 00115 while (opt){ 00116 opt=opt->_upperOptimizer; 00117 l++; 00118 } 00119 return l; 00120 } 00121 00122 template <typename PG> 00123 HCholOptimizer<PG>* HCholOptimizer<PG>::level(int i) { 00124 HCholOptimizer<PG>* opt=this; 00125 while (i!=0 && opt){ 00126 if (i<0){ 00127 opt=opt->_lowerOptimizer; 00128 i++; 00129 } else { 00130 opt=opt->_upperOptimizer; 00131 i--; 00132 } 00133 } 00134 return opt; 00135 } 00136 00137 template <typename PG> 00138 void HCholOptimizer<PG>::annotateHiearchicalEdge(typename PG::Edge* e, int iterations, double lambda, bool initWithObservations){ 00139 if (!_lowerOptimizer) 00140 return; 00141 HVertex* from = dynamic_cast< HVertex* >( e->from() ); 00142 HVertex* to = dynamic_cast< HVertex* >( e->to() ); 00143 assert (from && to); 00144 Graph::VertexSet jointSet; 00145 std::set_union(from->children().begin(), 00146 from->children().end(), 00147 to->children().begin(), 00148 to->children().end(), 00149 std::insert_iterator<Graph::VertexSet>(jointSet, jointSet.end())); 00150 typename PG::InformationType covariance = PG::InformationType::eye(1.0); 00151 int otherId=to->id(); 00152 _lowerOptimizer->backupSubset(jointSet); 00153 // cerr << __PRETTY_FUNCTION__ << ": js= "; 00154 // for (VertexSet::iterator it=jointSet.begin(); it!=jointSet.end(); it++){ 00155 // cerr << (*it)->id() << " "; 00156 // } 00157 // cerr << endl; 00158 // cerr << __PRETTY_FUNCTION__ << ": root= " << from->lowerRoot()->id() << endl; 00159 //_lowerOptimizer->transformSubset(from->lowerRoot(), jointSet, typename PG::TransformationType()); 00160 _lowerOptimizer->optimizeSubset(from->lowerRoot(), jointSet, iterations, lambda, initWithObservations, otherId, &covariance); 00161 typename PG::TransformationType mean=from->lowerRoot()->transformation.inverse()*to->lowerRoot()->transformation; 00162 _lowerOptimizer->restoreSubset(jointSet); 00163 if (this->verbose()){ 00164 cerr << "u[" << jointSet.size() << "] "; 00165 } 00166 00167 if (covariance.det()<0){ 00168 cerr << "![" << covariance.det() << "] " << endl; 00169 covariance = typename PG::InformationType().eye(1.)*1e9; 00170 } else 00171 covariance=covariance.inverse(); 00172 00173 refineEdge(e, mean, covariance); 00174 } 00175 00176 template <typename PG> 00177 void HCholOptimizer<PG>::annotateHiearchicalEdgeOnDenseGraph(typename PG::TransformationType& mean, typename PG::InformationType& info, 00178 typename PG::Edge* e, int iterations, double lambda, bool initWithObservations){ 00179 if (!_lowerOptimizer) 00180 return; 00181 assert (! _upperOptimizer); 00182 00183 HVertex* from = dynamic_cast< HVertex* >( e->from() ); 00184 HVertex* to = dynamic_cast< HVertex* >( e->to() ); 00185 00186 assert (from && to); 00187 HVertex* fromAux = dynamic_cast< HVertex* >( e->from() ); 00188 HVertex* toAux = dynamic_cast< HVertex* >( e->to() ); 00189 HCholOptimizer<PG>* optAux=this; 00190 00191 Graph::VertexSet jointSet; 00192 jointSet.insert(fromAux); 00193 jointSet.insert(toAux); 00194 int level=0; 00195 while (optAux->_lowerOptimizer) { 00196 // cerr << "L=" << level << " V="; 00197 Graph::VertexSet jointAuxSet; 00198 for (Graph::VertexSet::iterator it=jointSet.begin(); it!=jointSet.end(); it++){ 00199 HVertex* v = dynamic_cast< HVertex* >(*it); 00200 for (typename HVertexSet::iterator it=v->children().begin(); it!=v->children().end(); it++){ 00201 // cerr << (*it)->id() << " "; 00202 jointAuxSet.insert(*it); 00203 } 00204 } 00205 //cerr << endl; 00206 fromAux=dynamic_cast<HVertex*>(fromAux->lowerRoot()); 00207 toAux=dynamic_cast<HVertex*>(toAux->lowerRoot()); 00208 optAux=optAux->_lowerOptimizer; 00209 jointSet=jointAuxSet; 00210 level++; 00211 } 00212 //cerr << "DONE " << jointSet.size() << ": "; 00213 for (Graph::VertexSet::iterator it=jointSet.begin(); it!=jointSet.end(); it++){ 00214 //cerr << (*it)->id() << " "; 00215 } 00216 //cerr << endl; 00217 assert (toAux->_optimizer==optAux); 00218 00219 typename PG::InformationType covariance; 00220 int otherId=toAux->id(); 00221 optAux->backupSubset(jointSet); 00222 optAux->optimizeSubset(fromAux, jointSet, iterations, lambda, initWithObservations, otherId, &covariance); 00223 mean=fromAux->transformation.inverse()*toAux->transformation; 00224 optAux->restoreSubset(jointSet); 00225 if (covariance.det()<0){ 00226 cerr << "![" << covariance.det() << "] " << endl; 00227 info=typename PG::InformationType::eye(1e9); 00228 } else 00229 info=covariance.inverse(); 00230 } 00231 00232 template <typename PG> 00233 void HCholOptimizer<PG>::computeTopLevelDenseGraph(CholOptimizer<PG>* chol, int iterations, int lambda, int initWithObservations){ 00234 if (_upperOptimizer){ 00235 _upperOptimizer->computeTopLevelDenseGraph(chol, iterations, lambda, initWithObservations); 00236 return; 00237 } 00238 chol->clear(); 00239 for (typename PG::VertexIDMap::iterator it=this->vertices().begin(); it!=this->vertices().end(); it++){ 00240 HVertex* v = dynamic_cast< HVertex* >(it->second); 00241 typename PG::Vertex* vc= chol->addVertex(v->id()); 00242 vc->transformation=v->transformation; 00243 } 00244 for (typename PG::EdgeSet::iterator it=this->edges().begin(); it!=this->edges().end(); it++){ 00245 typename PG::Edge* e=dynamic_cast<typename PG::Edge*>(*it); 00246 typename PG::TransformationType mean; 00247 typename PG::InformationType info; 00248 annotateHiearchicalEdgeOnDenseGraph(mean, info, e, iterations, lambda, initWithObservations); 00249 typename PG::Vertex* from=dynamic_cast<typename PG::Vertex*>(chol->vertex(e->from()->id())); 00250 typename PG::Vertex* to=dynamic_cast<typename PG::Vertex*>(chol->vertex(e->to()->id())); 00251 chol->addEdge(from, to, mean, info); 00252 } 00253 // for (VertexIDMap::iterator it=vertices().begin(); it!=vertices().end(); it++){ 00254 // HVertex* v = dynamic_cast< HVertex* >(it->second); 00255 // Vertex* vc = dynamic_cast< Vertex* >(chol->vertex(v->id())); 00256 // vc->transformation=v->transformation; 00257 // } 00258 } 00259 00260 template <typename PG> 00261 int HCholOptimizer<PG>::optimize(int iterations, bool online){ 00262 if (! _upperOptimizer){ 00263 CholOptimizer<PG>::optimize(iterations, online); 00264 return iterations; 00265 } 00266 assert(!_lowerOptimizer); 00267 if (! online){ 00268 _online=false; 00269 updateStructure(false); 00270 HCholOptimizer<PG>* uopt=this; 00271 while(uopt->_upperOptimizer){ 00272 uopt=uopt->_upperOptimizer; 00273 } 00274 for (int i=0; i<iterations; i++){ 00275 uopt->vCycle(3,0.,1.,false); 00276 } 00277 } else { 00278 optimizePendingIncremental(); 00279 } 00280 return 1; 00281 } 00282 00283 }