Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
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
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+=this->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=-this->chi2(e);
00126 PG::refineEdge(e,mean,information);
00127 derr+=this->chi2(e);
00128 _cachedChi+=derr;
00129 }
00130
00131 template <typename PG>
00132 bool HCholOptimizer<PG>::removeEdge(Graph::Edge* e){
00133
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-=this->chi2(eAux);
00146 _lastOptChi-=this->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
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 }