posegraph.hpp
Go to the documentation of this 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 #include <assert.h>
00018 #include <queue>
00019 #include <iostream>
00020 #include <iomanip>
00021 #include <math.h>
00022 //#include "posegraph2d.hh"
00023 
00024 #define LINESIZE (4096*4)
00025 
00026 namespace AISNavigation{
00027   using namespace std;
00028 
00029   template <typename T, typename I>
00030   PoseGraph<T,I>::Vertex::Vertex(int i) : Graph::Vertex(i){
00031     _tempIndex=-1;
00032     _isBackup=false;
00033     _fixed=false;
00034   }
00035 
00036   template <typename T, typename I>
00037   PoseGraph<T,I>::Vertex::~Vertex(){
00038   }
00039 
00040   template <typename T, typename I>
00041   PoseGraph<T,I>::Edge::Edge(PoseGraph<T,I>::Vertex* from, PoseGraph<T,I>::Vertex* to, const PoseGraph<T,I>::TransformationType& m, const PoseGraph<T,I>::InformationType& i) : Graph::Edge(from, to){
00042     setAttributes(m,i);
00043   }
00044 
00045   template <typename T, typename I>
00046   bool PoseGraph<T,I>::Edge::direction(Vertex* from_, Vertex* to_) const {
00047     if ((to_==_to)&&(from_==_from))
00048       return true;
00049     if ((to_==_from)&&(from_==_to))
00050       return false;
00051     assert(0);
00052     return false;
00053   }
00054 
00055   template <typename T, typename I>
00056   const typename PoseGraph<T,I>::TransformationType& PoseGraph<T,I>::Edge::mean(bool direct) const {
00057     return direct ? _mean : _rmean;
00058   }
00059 
00060   template <typename T, typename I>
00061   const typename PoseGraph<T,I>::InformationType& PoseGraph<T,I>::Edge::information(bool direct) const {
00062     return direct ? _information :_rinformation;
00063   }
00064 
00065   template <typename T, typename I>
00066   const typename PoseGraph<T,I>::InformationType& PoseGraph<T,I>::Edge::covariance(bool direct) const {
00067     return direct ? _covariance : _rcovariance;
00068   }
00069 
00070   template <typename T, typename I>
00071   const double&    PoseGraph<T,I>::Edge::informationDet(bool direct) const{
00072     return direct ? _infoDet : _rinfoDet;
00073   }
00074 
00075   template <typename T, typename I>
00076   const double&    PoseGraph<T,I>::Edge::covarianceDet(bool direct) const {
00077     return direct ? _covDet : _rcovDet;
00078   }
00079 
00080   template <typename T, typename I>
00081   bool PoseGraph<T,I>::Edge::revert(){
00082     typename PoseGraph<T,I>::TransformationType t_ap(_mean);
00083     _mean=_rmean;
00084     _rmean=t_ap;
00085     typename PoseGraph<T,I>::InformationType i_ap(_information);
00086     _information=_rinformation;
00087     _rinformation=i_ap;
00088     typename PoseGraph<T,I>::InformationType c_ap(_covariance);
00089     _covariance=_rcovariance;
00090     _rcovariance=c_ap;
00091     double id_ap=_infoDet;
00092     _infoDet=_rinfoDet;
00093     _rinfoDet=id_ap;
00094     double ic_ap=_covDet;
00095     _covDet=_rcovDet;
00096     _rcovDet=ic_ap;
00097     return Graph::Edge::revert();
00098   }
00099 
00100   template <typename T, typename I>
00101   void PoseGraph<T,I>::Edge::setAttributes(const PoseGraph<T,I>::TransformationType& mean_, const PoseGraph<T,I>::InformationType& information_){
00102     _mean=mean_;
00103     _information=information_;
00104 
00105     _covariance=information_.inverse();
00106     _infoDet=information_.det();
00107     _covDet=1./_infoDet;
00108     _rmean=mean_.inverse();
00109     //HACK, should use jacobians to project the information matrices and
00110     //the covariances
00111     _rinformation=_information;
00112     _rcovariance=_covariance;
00113     _rinfoDet=_infoDet;
00114     _rcovDet=_covDet;
00115   }
00116 
00117   template <typename T, typename I>
00118   double PoseGraph<T,I>::Edge::chi2() const{
00119     typename PoseGraph<T,I>::Vertex* v1 = reinterpret_cast<typename PoseGraph<T,I>::Vertex*>(_from);
00120     typename PoseGraph<T,I>::Vertex* v2 = reinterpret_cast<typename PoseGraph<T,I>::Vertex*>(_to);
00121     typename PoseGraph<T,I>::TransformationType delta=_rmean * (v1->transformation.inverse()*v2->transformation);
00122     typename PoseGraph<T,I>::TransformationVectorType dp=delta.toVector();
00123     typename PoseGraph<T,I>::TransformationVectorType partial=_information*dp;
00124     return dp*partial;
00125   }
00126 
00127   template <typename T, typename I>
00128   double PoseGraph<T,I>::PathLengthCostFunction::operator()(Graph::Edge* edge, Graph::Vertex* from, Graph::Vertex* to){
00129     const typename PoseGraph<T,I>::Edge* e=dynamic_cast<const typename PoseGraph<T,I>::Edge*>(edge);
00130     typename T::TranslationType t=e->mean().translation();
00131     return sqrt(t*t);
00132   }
00133 
00134   template <typename T, typename I>
00135   double PoseGraph<T,I>::CovarianceDetCostFunction::operator()(Graph::Edge* edge, Graph::Vertex* from, Graph::Vertex* to){
00136     const typename PoseGraph<T,I>::Edge* e=dynamic_cast<const typename PoseGraph<T,I>::Edge*>(edge);
00137     return e->covarianceDet();
00138   }
00139 
00140 
00141   template <typename T, typename I>
00142   typename PoseGraph<T,I>::Vertex* PoseGraph<T,I>::addVertex(const int& k){
00143     Vertex* v=new typename PoseGraph<T,I>::Vertex(k);
00144     Vertex* vresult=dynamic_cast<typename PoseGraph<T,I>::Vertex*>(Graph::addVertex(v));
00145     if (!vresult){
00146       delete v;
00147     }
00148     return vresult;
00149   }
00150 
00151   template <typename T, typename I>
00152   typename PoseGraph<T,I>::Vertex* PoseGraph<T,I>::addVertex(int id, const PoseGraph<T,I>::TransformationType& pose, const PoseGraph<T,I>::InformationType& information){
00153     typename PoseGraph<T,I>::Vertex* v=new typename PoseGraph<T,I>::Vertex(id);
00154     typename PoseGraph<T,I>::Vertex* vresult=dynamic_cast< typename PoseGraph<T,I>::Vertex*>(Graph::addVertex(v));
00155     if (!vresult){
00156       delete v;
00157       return vresult;
00158     }
00159     vresult->transformation=pose;
00160     vresult->covariance=information.inverse();
00161     return vresult;
00162   }
00163 
00164   template <typename T, typename I>
00165   typename PoseGraph<T,I>::Edge*   PoseGraph<T,I>::addEdge(Vertex* from, Vertex* to, const PoseGraph<T,I>::TransformationType& mean, const PoseGraph<T,I>::InformationType& information) {
00166     typename PoseGraph<T,I>::Edge* e=new typename PoseGraph<T,I>::Edge(from, to, mean, information);
00167     typename PoseGraph<T,I>::Edge* eresult=dynamic_cast<typename PoseGraph<T,I>::Edge*>(Graph::addEdge(e));
00168     if (! eresult)
00169       delete e;
00170     return eresult;
00171   }
00172 
00173 
00174   template <typename PG>
00175   struct CovariancePropagator: public Dijkstra::TreeAction{
00176     virtual double perform(Graph::Vertex* v_, Graph::Vertex* vParent_, Graph::Edge* e_){
00177       typename PG::Vertex* v =dynamic_cast<typename PG::Vertex*>(v_);
00178       typename PG::Vertex* vParent =dynamic_cast<typename PG::Vertex*>(vParent_);
00179       typename PG::Edge* e =dynamic_cast<typename PG::Edge*>(e_);
00180       assert(v);
00181       typename PG::InformationType& cov(v->covariance);
00182       if (! vParent){
00183         cov=PG::InformationType::eye(0.);
00184         return 0;
00185       }
00186       assert(vParent);
00187       assert(e);
00188       bool direction=e->direction(vParent, v);
00189       MotionJacobian<PG> jacobian;
00190       typename PG::InformationType Jx=jacobian.state(vParent->transformation, e->mean(direction));
00191       typename PG::InformationType Ju = jacobian.measurement(vParent->transformation, e->mean(direction));
00192       v->covariance=Jx*vParent->covariance*Jx.transpose() + Ju*e->covariance()*Ju.transpose();
00193       return v->covariance.det();
00194     }
00195   };
00196 
00197   template <typename PG>
00198   struct PosePropagator: public Dijkstra::TreeAction{
00199     virtual double perform(Graph::Vertex* v_, Graph::Vertex* vParent_, Graph::Edge* e_){
00200       typename PG::Vertex* v =dynamic_cast<typename PG::Vertex*>(v_);
00201       typename PG::Vertex* vParent =dynamic_cast<typename PG::Vertex*>(vParent_);
00202       typename PG::Edge* e =dynamic_cast<typename PG::Edge*>(e_);
00203       assert(v);
00204       if (v->fixed())
00205         return 1;
00206       if (! vParent){
00207         return 0;
00208       }
00209       assert(vParent);
00210       assert(e);
00211       bool direction=e->direction(vParent, v);
00212       v->transformation=vParent->transformation*e->mean(direction);
00213       return 1;
00214     }
00215   };
00216 
00217 
00218   template <typename T, typename I>
00219   void PoseGraph<T,I>::propagateAlongDijkstraTree(typename PoseGraph<T,I>::Vertex* v, Dijkstra::AdjacencyMap& amap, bool covariance, bool transformation){
00220     Dijkstra::computeTree(v,amap);
00221     if (covariance){
00222       CovariancePropagator<PoseGraph<T,I> > cp;
00223       Dijkstra::visitAdjacencyMap(v,amap,&cp);
00224     }
00225     if (transformation){
00226       PosePropagator< PoseGraph<T,I> > tp;
00227       Dijkstra::visitAdjacencyMap(v,amap,&tp);
00228     }
00229   }
00230 
00231   template <typename T, typename I>
00232   void PoseGraph<T,I>::refineEdge(typename PoseGraph<T,I>::Edge* e, const typename PoseGraph<T,I>::TransformationType& mean, const typename PoseGraph<T,I>::InformationType& information){
00233     e->setAttributes(mean, information);
00234   }
00235 
00236 } // end namespace


hogman_minimal
Author(s): Maintained by Juergen Sturm
autogenerated on Mon Oct 6 2014 00:06:59