$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 <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+=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=-chi2(e); 00126 PG::refineEdge(e,mean,information); 00127 derr+=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-=chi2(eAux); 00146 _lastOptChi-=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