hogman_minimal: hogman_minimal: graph_optimizer.hpp Source File

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 using namespace std;
00018 
00019 template <typename PG>
00020 GraphOptimizer<PG>::GraphOptimizer() :
00021   PG(),
00022   _verbose(false), _visualizeToStdout(false), _guessOnEdges(false)
00023 {
00024 }
00025 
00026 template <typename PG>
00027 GraphOptimizer<PG>::~GraphOptimizer()
00028 {
00029 }
00030 
00031 template <typename PG>
00032 double GraphOptimizer<PG>::chi2(const typename PG::Edge* e_)
00033 {
00034   const typename PG::Edge* e = _MY_CAST_<const typename PG::Edge*>(e_);
00035   const typename PG::Vertex* v1 = _MY_CAST_<const typename PG::Vertex*>(e->from());
00036   const typename PG::Vertex* v2 = _MY_CAST_<const typename PG::Vertex*>(e->to());
00037   typename PG::TransformationType delta = e->mean(false) * (v1->transformation.inverse() * v2->transformation);
00038   typename PG::TransformationVectorType dp = delta.toVector();
00039   typename PG::TransformationVectorType partial = e->information() * dp;
00040   return dp * partial;
00041 }
00042 
00043 template <typename PG>
00044 double GraphOptimizer<PG>::chi2() const
00045 {
00046   double chi = 0.0;
00047   for (typename PG::EdgeSet::const_iterator it = this->edges().begin(); it != this->edges().end(); it++){
00048     typename PG::Edge* e = _MY_CAST_<typename PG::Edge*>(*it);
00049     chi += chi2(e);
00050   }
00051   return chi;
00052 }
00053 
00054 template <typename PG>
00055 void GraphOptimizer<PG>::absChi(double& rotationalError, double& translationalError, typename PG::Edge* e_)
00056 {
00057   typename PG::Edge* e = _MY_CAST_<typename PG::Edge*>(e_);
00058   typename PG::Vertex* v1 = _MY_CAST_<typename PG::Vertex*>(e->from());
00059   typename PG::Vertex* v2 = _MY_CAST_<typename PG::Vertex*>(e->to());
00060   typename PG::TransformationType delta = (v1->transformation.inverse() * v2->transformation) * e->mean().inverse();
00061   _Vector< PG::TransformationType::RotationType::Angles, double > anglesDelta = delta.rotation().angles();
00062   rotationalError = sqrt(anglesDelta * anglesDelta);
00063   translationalError = sqrt(delta.translation() * delta.translation());
00064 }
00065 
00066 template <typename PG>
00067 void  GraphOptimizer<PG>::chiStat(ChiStatMap& emap)
00068 {
00069   emap.clear();
00070   for (typename PG::EdgeSet::iterator it= this->edges().begin(); it!= this->edges().end(); ++it)
00071     emap.insert(make_pair(_MY_CAST_<typename PG::Edge*>(*it), chi2(*it)));
00072 }
00073 
00074 template <typename PG>
00075 void GraphOptimizer<PG>::sqError(double& are, double& ate, double& mte, double& mre, const typename PG::EdgeSet* eset) const
00076 {
00077   if (! eset)
00078     eset = &this->edges();
00079   double cumTe=0, cumRe=0;
00080   double maxTe=0, maxRe=0;
00081   int count=0;
00082   for (typename PG::EdgeSet::const_iterator it = eset->begin(); it != eset->end(); ++it){
00083     double te=0, re=0;
00084     typename PG::Edge* e = _MY_CAST_<typename PG::Edge*>(*it);
00085     absChi(re, te, e);
00086     maxTe = max(maxTe, te);
00087     maxRe = max(maxRe, re);
00088     cumTe += te;
00089     cumRe += re;
00090     count++;
00091   }
00092   mte = maxTe;
00093   mre = maxRe;
00094   ate = cumTe/(double)count;
00095   are = cumRe/(double)count;
00096 }
00097 
00098 template <typename PG>
00099 void GraphOptimizer<PG>::backup()
00100 {
00101   for (typename PG::VertexIDMap::iterator it=_vertices.begin(); it!=_vertices.end(); it++)
00102   {
00103     typename PG::Vertex* v=_MY_CAST_<typename PG::Vertex*>(it->second);
00104     v->backup();
00105   }
00106 }
00107 
00108 template <typename PG>
00109 void GraphOptimizer<PG>::restore()
00110 {
00111   for (typename PG::VertexIDMap::iterator it=_vertices.begin(); it!=_vertices.end(); it++)
00112   {
00113     typename PG::Vertex* v=_MY_CAST_<typename PG::Vertex*>(it->second);
00114     v->restore();
00115   }
00116 }
00117 
00118 template <typename PG>
00119 void GraphOptimizer<PG>::backupSubset(typename PG::VertexSet& vset){
00120   for (typename PG::VertexSet::iterator it=vset.begin(); it!=vset.end(); it++) {
00121     (*it)->backup();
00122   }
00123 }
00124 
00125 template <typename PG>
00126 void GraphOptimizer<PG>::restoreSubset(typename PG::VertexSet& vset)
00127 {
00128   for (typename PG::VertexSet::iterator it=vset.begin(); it!=vset.end(); it++) {
00129     (*it)->restore();
00130   }
00131 }
00132 
00133 template <typename PG>
00134 void GraphOptimizer<PG>::backupSubset(Graph::VertexSet& vset)
00135 {
00136   for (Graph::VertexSet::iterator it=vset.begin(); it!=vset.end(); it++) {
00137     typename PG::Vertex* v = dynamic_cast<typename PG::Vertex*>(*it);
00138     if (v)
00139       v->backup();
00140   }
00141 }
00142 
00143 template <typename PG>
00144 void GraphOptimizer<PG>::restoreSubset(Graph::VertexSet& vset)
00145 {
00146   for (Graph::VertexSet::iterator it=vset.begin(); it!=vset.end(); it++) {
00147     typename PG::Vertex* v = dynamic_cast<typename PG::Vertex*>(*it);
00148     if (v)
00149       v->restore();
00150   }
00151 }