$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 #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 }